po_hi_driver_leon_eth.c 23.5 KB
Newer Older
jdelange's avatar
jdelange committed
1
2
3
4
5
/*
 * This is a part of PolyORB-HI-C distribution, a minimal
 * middleware written for generated code from AADL models.
 * You should use it with the Ocarina toolsuite.
 *
yoogx's avatar
yoogx committed
6
 * For more informations, please visit http://taste.tuxfamily.org/wiki
jdelange's avatar
jdelange committed
7
 *
yoogx's avatar
yoogx committed
8
 * Copyright (C) 2011-2018 ESA & ISAE.
jdelange's avatar
jdelange committed
9
10
11
12
13
14
15
16
17
 */

#include <deployment.h>

#if defined (__PO_HI_NEED_DRIVER_ETH_LEON) || \
    defined (__PO_HI_NEED_DRIVER_ETH_LEON_SENDER) || \
    defined (__PO_HI_NEED_DRIVER_ETH_LEON_RECEIVER)

#include <po_hi_debug.h>
18
19
#include <po_hi_task.h>
#include <po_hi_types.h>
jdelange's avatar
jdelange committed
20
21
22
#include <po_hi_utils.h>
#include <po_hi_messages.h>
#include <po_hi_transport.h>
23
#include <po_hi_returns.h>
jdelange's avatar
jdelange committed
24
#include <po_hi_gqueue.h>
yoogx's avatar
yoogx committed
25
26
27
#include <po_hi_main.h>
#include <drivers/po_hi_driver_rasta_common.h>
#include <drivers/po_hi_rtems_utils.h>
jdelange's avatar
jdelange committed
28
#include <drivers/po_hi_driver_leon_eth.h>
29
#include <drivers/po_hi_driver_sockets.h>
jdelange's avatar
jdelange committed
30
31
32
33
34
35
36
37
38
39
40
41
#include <drivers/configuration/ip.h>
/* po-hi-c related files */

#include <activity.h>
#include <marshallers.h>
#include <deployment.h>
/* generated files */

#include <termios.h>
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
42
43
44
45
46
47
48
49
50
51
52
53
54
55
#include <signal.h>
#include <netdb.h>
#include <arpa/inet.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/select.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <netinet/in.h>

__po_hi_inetnode_t nodes[__PO_HI_NB_DEVICES];
__po_hi_inetnode_t rnodes[__PO_HI_NB_DEVICES];

__po_hi_device_id leon_eth_device_id;
yoogx's avatar
yoogx committed
56
extern void rtems_stack_checker_report_usage(void);
jdelange's avatar
jdelange committed
57
58
59
60

#if defined (__PO_HI_NEED_DRIVER_ETH_LEON) || \
    defined (__PO_HI_NEED_DRIVER_ETH_LEON_RECEIVER)

61
62
63
64
   #define __PO_HI_SET_SOCKET_TIMEOUT(mysocket,nsec) { struct timeval timeout; \
                                            timeout.tv_sec = nsec; \
                                            timeout.tv_usec = 0; \
                                            setsockopt (mysocket, SOL_SOCKET, SO_RCVTIMEO, (char *)&timeout,sizeof (timeout)); }
jdelange's avatar
jdelange committed
65

yoogx's avatar
yoogx committed
66
67
68
69
70
71
72
73
74

#if ((defined GRLEON3)||((defined GRLEON2)&&(RTEMS412)))

#define RTEMS_BSP_NETWORK_DRIVER_ATTACH RTEMS_BSP_NETWORK_DRIVER_ATTACH_GRETH

#define RTEMS_BSP_NETWORK_DRIVER_NAME RTEMS_BSP_NETWORK_DRIVER_NAME_GRETH

#elif GRLEON2

75
#define RTEMS_BSP_NETWORK_DRIVER_ATTACH RTEMS_BSP_NETWORK_DRIVER_ATTACH_SMC91111
76
77
78

#define RTEMS_BSP_NETWORK_DRIVER_NAME "open_eth1"

yoogx's avatar
yoogx committed
79
80
#endif

81
82
#include <bsp.h>
#include <rtems/rtems_bsdnet.h>
jdelange's avatar
jdelange committed
83

yoogx's avatar
yoogx committed
84
#if defined RTEMS48 || defined RTEMS410
85
extern void rtems_bsdnet_loopattach();
86
#else
87
extern void rtems_bsdnet_initialize_loop();
88
#endif
89

90
91
static struct rtems_bsdnet_ifconfig loopback_config = {
   "lo0",            /* name */
yoogx's avatar
yoogx committed
92
#if defined RTEMS48 || defined RTEMS410
93
   rtems_bsdnet_loopattach,   /* attach function */
94
95
96
#else
   rtems_bsdnet_initialize_loop,
#endif
97
   NULL,          /* link to next interface */
jdelange's avatar
jdelange committed
98

99
100
101
102
   "127.0.0.1",         /* IP address */
   "255.0.0.0",         /* IP net mask */
};

yoogx's avatar
yoogx committed
103
#if defined RTEMS48 || defined RTEMS410
104
105
106
107
/*
 * Default network interface
 */
static struct rtems_bsdnet_ifconfig netdriver_config = {
yoogx's avatar
yoogx committed
108
109
110
   RTEMS_BSP_NETWORK_DRIVER_NAME,      /* name  */
   RTEMS_BSP_NETWORK_DRIVER_ATTACH, /* attach function  greth_interface_driver_attach */
//   0, /* link to next interface */
yoogx's avatar
yoogx committed
111
112
   loopback_config,

113
   /*
114
115
116
#ifdef RTEMS48
   loopback_config,
#else
117
   0,
118
#endif
119
   */
120
121
122
123
124
   "255.255.255.255",       /* IP address */
   "255.255.255.255",     /* IP net mask */
   NULL,                           /* Driver supplies hardware address */
   0           /* Use default driver parameters */
};
yoogx's avatar
yoogx committed
125
126
127
128
129
#elif defined RTEMS412
#include <bsp/network_interface_add.h>

struct ethernet_config interface_configs[]=
{
yoogx's avatar
yoogx committed
130
        { "255.255.255.255", "255.255.255.255", {0x80,0x80,0x80,0x80,0x80,0x80}},// NULL - take PHY address and IP from device
yoogx's avatar
yoogx committed
131
132
133
134
        {NULL, NULL, {0,0,0,0,0,0}}  // NULL - used for BOOTP
};

#endif
135
136
137

/*
 * Network configuration
yoogx's avatar
yoogx committed
138
139
140
141
142
 *
 * See
 * https://docs.rtems.org/doc-current/share/rtems/html/networking/Network-Configuration.html
 * for details on each field
 *
143
144
 */
struct rtems_bsdnet_config rtems_bsdnet_config = {
yoogx's avatar
yoogx committed
145
#ifdef RTEMS412
yoogx's avatar
yoogx committed
146
  NULL,
yoogx's avatar
yoogx committed
147
148
149
#else
  &netdriver_config,
#endif
yoogx's avatar
yoogx committed
150
   NULL,        /* Bootp */
yoogx's avatar
yoogx committed
151
#if defined RTEMS48 || defined RTEMS410
152
   100,        /* Default network task priority */
153
154
155
#else
   150,
#endif
yoogx's avatar
yoogx committed
156
157
158
159
   128*1024,             /* mbuf capacity */
   256*1024,             /* mbuf cluster capacity */
   "rtems_host",         /* Host name */
   "localnet",           /* Domain name */
160
   "255.255.255.255",    /* Gateway */
yoogx's avatar
yoogx committed
161
   "10.1.1.1",           /* Log host */
162
163
   {"255.255.255.255" }, /* Name server(s) */
   {"255.155.255.255" }, /* NTP server(s) */
yoogx's avatar
yoogx committed
164
165
166
167
168
   2,                    /* sb_efficiency */
   9216,                 /* UDP TX */
   40 * 1024,            /* UDP RX */
   128 * 1024,           /* TCP TX */
   128 * 1024            /* TCP RX */
169
};
jdelange's avatar
jdelange committed
170

171
/******************************************************************************/
172
173
__po_hi_request_t          __po_hi_c_driver_eth_leon_poller_received_request;
__po_hi_msg_t              __po_hi_c_driver_eth_leon_poller_msg;
jdelange's avatar
jdelange committed
174

175
176
177
178
179
180
181
182
183
184
185
186
187
188
void __po_hi_c_driver_eth_leon_poller (const __po_hi_device_id dev_id)
{
   (void)                     dev_id;
   __DEBUGMSG ("Poller launched, device-id=%d\n", leon_eth_device_id);
   socklen_t                  socklen = sizeof (struct sockaddr);
   /* See ACCEPT (2) for details on initial value of socklen */

   __po_hi_uint32_t           len;
   int                        sock;
   int                        max_socket;
   fd_set                     selector;
   struct sockaddr_in         sa;
   __po_hi_device_id          dev;
   __po_hi_node_t             dev_init;
189
   int                        established = 0;
190
191
192
193
194
195
196
197
198
   __po_hi_protocol_conf_t*   protocol_conf;

   max_socket = 0; /* Used to compute the max socket number, useful for listen() call */

   /*
    * We initialize each node socket with -1 value.  This value means
    * that the socket is not active.
    */
   for (dev = 0 ; dev < __PO_HI_NB_DEVICES ; dev++)
jdelange's avatar
jdelange committed
199
   {
200
      rnodes[dev].socket = -1;
jdelange's avatar
jdelange committed
201
202
   }

yoogx's avatar
yoogx committed
203
204
205
   __po_hi_bus_id bus_current_node, bus_connect_node;
   bus_current_node = *__po_hi_transport_get_accessed_buses(leon_eth_device_id);

206
207
208
209
   /*
    * Create a socket for each node that will communicate with us.
    */
   for (dev = 0; dev < __PO_HI_NB_DEVICES ; dev++)
jdelange's avatar
jdelange committed
210
   {
211
212
      if (dev != leon_eth_device_id)
      {
yoogx's avatar
yoogx committed
213
214
215
         bus_connect_node = *__po_hi_transport_get_accessed_buses(dev);
         if (bus_current_node == bus_connect_node)
         {
jdelange's avatar
jdelange committed
216
         __PO_HI_SET_SOCKET_TIMEOUT(nodes[leon_eth_device_id].socket,500000);
217
218
219
220
221

         established = 0;

         while (established == 0)
         {
jdelange's avatar
jdelange committed
222
223

            __DEBUGMSG ("[DRIVER ETH] Poller waits for connection with device %d on socket %d (waiting device %d)\n", dev, nodes[leon_eth_device_id].socket, leon_eth_device_id);
224
225
            sock = accept (nodes[leon_eth_device_id].socket, (struct sockaddr*) &sa, &socklen);

jdelange's avatar
jdelange committed
226
227
228
229
230
231
232
            if (sock == -1)
            {
               __DEBUGMSG ("[DRIVER ETH] accept() error for device %d on device %d (socket=%d)\n", dev, leon_eth_device_id, nodes[leon_eth_device_id].socket);
               continue;
            }

            __PO_HI_SET_SOCKET_TIMEOUT(sock,100000);
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252

#ifndef __PO_HI_USE_PROTOCOL_MYPROTOCOL_I
            if (read (sock, &dev_init, sizeof (__po_hi_device_id)) != sizeof (__po_hi_device_id))
            {
               established = 0;
               __DEBUGMSG ("[DRIVER ETH] Cannot read device-id for device %d, socket=%d\n", dev, sock);
            }
            else
            {
               __DEBUGMSG ("[DRIVER ETH] read device-id %d from socket=%d\n", dev_init, sock);
               established = 1;
            }
#else
            established = 1;
#endif
         }
         rnodes[dev].socket = sock;
         if (sock > max_socket )
         {
            max_socket = sock;
253
         }
yoogx's avatar
yoogx committed
254
255
      } /* check the bus */
      } /* check the device */
jdelange's avatar
jdelange committed
256
   }
257
258
259
260
261
   __DEBUGMSG ("[DRIVER ETH] Poller initialization finished, waiting for other tasks\n");
   __po_hi_wait_initialization ();
   __DEBUGMSG ("[DRIVER ETH] Other tasks are initialized, let's start the polling !\n");

   /*
262
263
264
    * Main body of the poller function: listen and receive data on the
    * socket, identify the node which send the data and put it in its
    * message queue.
265
    */
266

267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
   while (1)
   {
      FD_ZERO( &selector );
      for (dev = 0; dev < __PO_HI_NB_DEVICES ; dev++)
      {
         if ( (dev != leon_eth_device_id ) && ( rnodes[dev].socket != -1 ) )
         {
            FD_SET( rnodes[dev].socket , &selector );
         }
      }

      if (select (max_socket + 1, &selector, NULL, NULL, NULL) == -1 )
      {
#ifdef __PO_HI_DEBUG
         __DEBUGMSG ("[DRIVER ETH] Error on select for node %d\n", __po_hi_mynode);
282
#endif
283
284
285
      }
#ifdef __PO_HI_DEBUG
      __DEBUGMSG ("[DRIVER ETH] Receive message\n");
jdelange's avatar
jdelange committed
286
287
#endif

288
289
290
291
      for (dev = 0; dev < __PO_HI_NB_DEVICES ; dev++) {
        __DEBUGMSG ("[DRIVER ETH] Try to watch if it comes from device %d (socket=%d)\n", dev, rnodes[dev].socket);
        if ( (rnodes[dev].socket != -1 ) && FD_ISSET(rnodes[dev].socket, &selector))
          {
292
            __DEBUGMSG ("[DRIVER ETH] Receive message from dev %d\n", dev);
293

294
295
296
297
298
299
300
301
302
303
304
305
306
#ifdef __PO_HI_USE_PROTOCOL_MYPROTOCOL_I
            {
               protocol_conf = __po_hi_transport_get_protocol_configuration (virtual_bus_myprotocol_i);

               int datareceived;
               len = recv (rnodes[dev].socket, &datareceived, sizeof (int), MSG_WAITALL);
               __DEBUGMSG ("[DRIVER ETH] Message received len=%d\n",(int)len);
               if (len == 0)
               {
                  __DEBUGMSG ("[DRIVER ETH] Zero size from device %d\n",dev);
                  rnodes[dev].socket = -1;
                  continue;
               }
307
308
               protocol_conf->unmarshaller (& __po_hi_c_driver_eth_leon_poller_received_request, &datareceived, len);
                __po_hi_c_driver_eth_leon_poller_received_request.port = 1;
309
310
311
            }

#else
312
313
314
315
316
317
318
            memset (__po_hi_c_driver_eth_leon_poller_msg.content, '\0',
                    __PO_HI_MESSAGES_MAX_SIZE);

            /* In the following, we first retrieve the size of the
               payload, then the payload itself */

            int datareceived;
319
            len = recv (rnodes[dev].socket,
320
321
322
323
324
325
326
327
328
                        &datareceived, sizeof (int),
                        MSG_WAITALL);
            __DEBUGMSG ("[DRIVER SOCKETS] Waiting for a message of size=%d\n",
                        (int)datareceived);

            len = recv (rnodes[dev].socket,
                        __po_hi_c_driver_eth_leon_poller_msg.content,
                        datareceived,
                        MSG_WAITALL);
329
            __po_hi_c_driver_eth_leon_poller_msg.length = len;
330
331
            __DEBUGMSG ("[DRIVER ETH] Message received len=%d\n",(int)len);

332
333
334
335
            if (len <= 0) {
              __DEBUGMSG ("[DRIVER ETH] Zero size from device %d\n",dev);
              rnodes[dev].socket = -1;
              continue;
336
337
            }

338
339
340
            __po_hi_unmarshall_request
              (& __po_hi_c_driver_eth_leon_poller_received_request,
               &__po_hi_c_driver_eth_leon_poller_msg);
341
342
#endif

343
            __po_hi_main_deliver (& __po_hi_c_driver_eth_leon_poller_received_request);
344
345
         }
      }
346
   }
jdelange's avatar
jdelange committed
347
348
349
350
351
352
353
354
}
#endif

#if (defined (__PO_HI_NEED_DRIVER_ETH_LEON) || \
     defined (__PO_HI_NEED_DRIVER_ETH_LEON_SENDER))

void __po_hi_c_driver_eth_leon_init (__po_hi_device_id id)
{
355
356
357
358
359
360
361
362
363
364
365
366
367
   int                i;
   int                ret;
   int                reuse;
   char               *tmp;
   __po_hi_uint16_t   dev;
   __po_hi_time_t     mytime;
   __po_hi_time_t     tmptime;
   __po_hi_time_t     current_time;
   struct sockaddr_in sa;
   struct hostent*    hostinfo;

   __po_hi_c_ip_conf_t* ipconf;
   char ip_addr[16];
jdelange's avatar
jdelange committed
368
   unsigned short ip_port;
369
370
371
372
   int node;

   ipconf = (__po_hi_c_ip_conf_t*)__po_hi_get_device_configuration (id);

yoogx's avatar
yoogx committed
373
#if defined RTEMS48 || defined RTEMS410
374
375
376
377
378
379
   netdriver_config.ip_address = ipconf->address;

   if (ipconf->exist.netmask == 1)
   {
      netdriver_config.ip_netmask = ipconf->netmask;
   }
yoogx's avatar
yoogx committed
380
381
#elif defined RTEMS412
   interface_configs[0].ip_addr = ipconf->address;
yoogx's avatar
yoogx committed
382

yoogx's avatar
yoogx committed
383
384
385
386
387
   if (ipconf->exist.netmask == 1)
   {
      interface_configs[0].ip_netmask= ipconf->netmask;
   }
#endif
388
389
390
391
392
393
394
395
396
397
398

   if (ipconf->exist.gateway == 1)
   {
      rtems_bsdnet_config.gateway = ipconf->gateway;
   }

   if (ipconf->exist.dns == 1)
   {
      rtems_bsdnet_config.name_server[0] = ipconf->dns;
   }

yoogx's avatar
yoogx committed
399
  __po_hi_c_driver_rasta_common_init();
yoogx's avatar
yoogx committed
400

yoogx's avatar
yoogx committed
401
  rtems_bsdnet_initialize_network();
yoogx's avatar
yoogx committed
402

403
#if __PO_HI_DEBUG_LEVEL >= __PO_HI_DEBUG_LEVEL_DEBUG
404
405
406
   rtems_bsdnet_show_if_stats ();
   rtems_bsdnet_show_inet_routes ();
   rtems_bsdnet_show_ip_stats ();
yoogx's avatar
yoogx committed
407
   rtems_bsdnet_show_mbuf_stats ();
408
409
#endif

yoogx's avatar
yoogx committed
410
   leon_eth_device_id = id;
jdelange's avatar
jdelange committed
411
412
   __po_hi_transport_set_sending_func (leon_eth_device_id, __po_hi_c_driver_eth_leon_sender);

413
414
415
416
417
   for (node = 0 ; node < __PO_HI_NB_DEVICES ; node++)
   {
      nodes[node].socket = -1;
   }

jdelange's avatar
jdelange committed
418
   ip_port = (unsigned short)ipconf->port;
419

jdelange's avatar
jdelange committed
420
   __DEBUGMSG ("My configuration, addr=%s, port=%d\n", ipconf->address, ip_port);
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438

   /*
    * If the current node port has a port number, then it has to
    * listen to other nodes. So, we create a socket, bind it and
    * listen to other nodes.
    */
   if (ip_port != 0)
   {
      nodes[id].socket = socket (AF_INET, SOCK_STREAM, 0);

      if (nodes[id].socket == -1 )
      {
#ifdef __PO_HI_DEBUG
         __DEBUGMSG ("Cannot create socket for device %d\n", id);
#endif
         return;
      }

jdelange's avatar
jdelange committed
439
      __DEBUGMSG ("[DRIVER ETH] Receiving socket for device %d created, value=%d\n", id, nodes[id].socket);
440
441

      sa.sin_addr.s_addr = htonl (INADDR_ANY);   /* We listen on all adresses */
442
      sa.sin_family = AF_INET;
443
444
      sa.sin_port = htons (ip_port);   /* Port provided by the generated code */

jdelange's avatar
jdelange committed
445
      if( bind (nodes[id].socket , (struct sockaddr *) &sa , sizeof (struct sockaddr_in) ) == -1 )
446
447
448
449
      {
         __DEBUGMSG ("Unable to bind socket and port on socket %d\n", nodes[id].socket);
      }

jdelange's avatar
jdelange committed
450
      if( listen( nodes[id].socket , __PO_HI_NB_ENTITIES ) == -1)
451
452
453
454
      {
         __DEBUGMSG ("Cannot listen on socket %d\n", nodes[id].socket);
      }

jdelange's avatar
jdelange committed
455
456
      __DEBUGMSG ("[DRIVER ETH] Receiving socket listen on any address on port %d\n", sa.sin_port);

457
      /*
458
459
460
461
462
463
464
       * Create the thread which receive all data from other
       * nodes. This thread will execute the function
       * __po_hi_receiver_task
       */

      __po_hi_initialize_add_task ();

yoogx's avatar
yoogx committed
465
466
467
//       __po_hi_create_generic_task
//         (-1, 0,__PO_HI_MAX_PRIORITY, 0, 0, (void* (*)(void)) __po_hi_c_driver_eth_leon_poller, NULL);
      __po_hi_create_generic_task(-1, 0, 2, 2*RTEMS_MINIMUM_STACK_SIZE, 0, (void* (*)(void)) __po_hi_c_driver_eth_leon_poller, NULL);
468
469
470
471
472
473
   }

   /*
    * For each node in the sytem that may communicate with the current
    * node we create a socket. This socket will be used to send data.
    */
yoogx's avatar
yoogx committed
474
475
476
477
478
479
   __po_hi_bus_id bus_current_node, bus_connect_node;

   bus_current_node = *__po_hi_transport_get_accessed_buses(id);

   __DEBUGMSG("[DRIVER ETH] Device %d is connected to bus: %d\n", id,bus_current_node);

480
481
482
483
484
485
   for (dev = 0 ; dev < __PO_HI_NB_DEVICES ; dev++ )
   {
      if (dev == id)
      {
         continue;
      }
yoogx's avatar
yoogx committed
486

yoogx's avatar
yoogx committed
487
488
489
      bus_connect_node= *__po_hi_transport_get_accessed_buses(dev);

      __DEBUGMSG("[DRIVER ETH] Device %d is connected to bus: %d\n", dev, bus_connect_node);
yoogx's avatar
yoogx committed
490

yoogx's avatar
yoogx committed
491
492
493
494
      if (bus_current_node != bus_connect_node)
      {
         continue;
      }
495
496
497
498
499
500
501

      __DEBUGMSG ("[DRIVER ETH] Will initialize connection with device %d\n", dev);

      memset (ip_addr, '\0', 16);
      ip_port = 0;

      ipconf = (__po_hi_c_ip_conf_t*) __po_hi_get_device_configuration (dev);
jdelange's avatar
jdelange committed
502
      ip_port = (unsigned short)ipconf->port;
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530

      __DEBUGMSG ("[DRIVER ETH] Configuration for device %d, addr=%s, port=%d\n", dev, ipconf->address, ip_port);

      if (ip_port == 0)
      {
         __DEBUGMSG ("[DRIVER ETH] Invalid remote port\n");
         continue;
      }

      while (1)
      {
         nodes[dev].socket = socket (AF_INET, SOCK_STREAM, 0);

         if (nodes[dev].socket == -1 )
         {
            __DEBUGMSG ("[DRIVER ETH] Socket for dev %d is not created\n", dev);
            return;
         }

         __DEBUGMSG ("[DRIVER ETH] Socket for dev %d created, value=%d\n", dev, nodes[dev].socket);

         hostinfo = gethostbyname ((char*)ipconf->address);

         if (hostinfo == NULL )
         {
            __DEBUGMSG ("[DRIVER ETH] Error while getting host informations for device %d\n", dev);
         }

jdelange's avatar
jdelange committed
531
532
         __DEBUGMSG ("[DRIVER ETH] Got the following information for device %d\n", dev);

533
534
535
536
537
538
539
540
541
542
         sa.sin_port = htons (ip_port);
         sa.sin_family = AF_INET;

         /* The following lines are used to copy the
          * hostinfo->h_length to the sa.sin_addr member. Most
          * of program use the memcpy to do that, but the
          * RT-POSIX profile we use forbid the use of this
          * function.  We use a loop instead to perform the
          * copy.  So, these lines replace the code :
          *
jdelange's avatar
jdelange committed
543
544
545
          *
          *
          memcpy( (char*) &( sa.sin_addr ) , (char*)hostinfo->h_addr , hostinfo->h_length );
546
547
548
549
550
551
552
          */
         tmp = (char*) &(sa.sin_addr);
         for (i=0 ; i<hostinfo->h_length ; i++)
         {
            tmp[i] = hostinfo->h_addr[i];
         }

jdelange's avatar
jdelange committed
553
554
         __PO_HI_SET_SOCKET_TIMEOUT(nodes[dev].socket,100000);

555
556
557
558
559
         /*
          * We try to connect on the remote host. We try every
          * second to connect on.
          */

560
         ret = connect (nodes[dev].socket,
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
                        (struct sockaddr*) &sa ,
                        sizeof (struct sockaddr_in));

#ifdef __PO_HI_USE_PROTOCOL_MYPROTOCOL_I
         if (ret == 0)
         {
            __DEBUGMSG ("[DRIVER ETH] Connection established with device %d, socket=%d\n", dev, nodes[dev].socket);

            break;
         }
         else
         {
            __DEBUGMSG ("connect() failed, return=%d\n", ret);
         }

#else
         if (ret == 0)
         {

jdelange's avatar
jdelange committed
580
581
582
            __DEBUGMSG ("[DRIVER ETH] Send my id (%d) using socket %d (node %d)\n", id, nodes[dev].socket, dev);
            ret = write (nodes[dev].socket, &id, sizeof (__po_hi_device_id));
            if (ret != sizeof (__po_hi_device_id))
583
            {
jdelange's avatar
jdelange committed
584
               __DEBUGMSG ("[DRIVER ETH] Device %d cannot send his id (ret=%d)\n", id, ret);
585
            }
jdelange's avatar
jdelange committed
586
            __DEBUGMSG ("[DRIVER ETH] Connection established with device %d, socket=%d (ret=%d)\n", dev, nodes[dev].socket, ret);
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
            break;
         }
         else
         {
            __DEBUGMSG ("connect() failed, return=%d\n", ret);
         }
#endif

         if (close (nodes[dev].socket))
         {
            __DEBUGMSG ("[DRIVER ETH] Cannot close socket %d\n", nodes[dev].socket);
         }

         /*
          * We wait 500ms each time we try to connect on the
          * remote host
          */

         __po_hi_get_time (&current_time);
         __po_hi_milliseconds (&tmptime, 500);
         __po_hi_add_times (&mytime, &current_time, &tmptime);
         __DEBUGMSG ("[DRIVER ETH] Cannot connect on device %d, wait 500ms\n", dev);
         __po_hi_delay_until (&mytime);
      }
   }

jdelange's avatar
jdelange committed
613
   __DEBUGMSG ("[DRIVER ETH] INITIALIZATION DONE\n");
614

jdelange's avatar
jdelange committed
615
616
617
618
}
#endif


619
#if defined (__PO_HI_NEED_DRIVER_ETH_LEON)
jdelange's avatar
jdelange committed
620

621
622
__po_hi_msg_t              __po_hi_c_driver_eth_leon_sender_msg;

jdelange's avatar
jdelange committed
623
624
int  __po_hi_c_driver_eth_leon_sender (__po_hi_task_id task, __po_hi_port_t port)
{
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
   int                        len;
   int                        size_to_write;
   int                        optval = 0;
   socklen_t                  optlen = 0;

   __po_hi_device_id          associated_device;
   __po_hi_local_port_t       local_port;
   __po_hi_request_t*         request;
   __po_hi_port_t             destination_port;
   __po_hi_protocol_t         protocol_id;
   __po_hi_protocol_conf_t*   protocol_conf;

   local_port              = __po_hi_get_local_port_from_global_port (port);
   request                 = __po_hi_gqueue_get_most_recent_value (task, local_port);
   destination_port        = __po_hi_gqueue_get_destination (task, local_port, 0);
   associated_device       = __po_hi_get_device_from_port (destination_port);
   protocol_id             = __po_hi_transport_get_protocol (port, destination_port);
   protocol_conf           = __po_hi_transport_get_protocol_configuration (protocol_id);

   if (request->port == -1)
   {
jdelange's avatar
jdelange committed
646

647
648
649
650
651
#ifdef __PO_HI_DEBUG
      __DEBUGMSG (" [DRIVER SOCKETS] No data to write on port %d\n", port);
#endif
      return __PO_HI_ERROR_TRANSPORT_SEND;
   }
jdelange's avatar
jdelange committed
652

653
654
655
656
657
   if (nodes[associated_device].socket == -1 )
   {
#ifdef __PO_HI_DEBUG
      __DEBUGMSG (" [DRIVER SOCKETS] Invalid socket for port-id %d, device-id %d\n", destination_port, associated_device);
#endif
658
      return __PO_HI_ERROR_TRANSPORT_SEND;
659
   }
jdelange's avatar
jdelange committed
660

661
662
663
664
   /*
    * After sending the entity identifier, we send the message which
    * contains the request.
    */
jdelange's avatar
jdelange committed
665

666
   size_to_write = __PO_HI_MESSAGES_MAX_SIZE;
jdelange's avatar
jdelange committed
667

668
669
670
671
672
   if (getsockopt (nodes[associated_device].socket, SOL_SOCKET, SO_ERROR, &optval, &optlen) == -1)
   {
      __DEBUGMSG (" [error getsockopt() in file %s, line%d ]\n", __FILE__, __LINE__);
      close (nodes[associated_device].socket);
      nodes[associated_device].socket = -1;
673
      return __PO_HI_ERROR_TRANSPORT_SEND;
674
   }
jdelange's avatar
jdelange committed
675

676
677
678
679
680
   if (optval != 0)
   {
      __DEBUGMSG (" [error getsockopt() return code in file %s, line%d ]\n", __FILE__, __LINE__);
      close (nodes[associated_device].socket);
      nodes[associated_device].socket = -1;
681
      return __PO_HI_ERROR_TRANSPORT_SEND;
682
   }
jdelange's avatar
jdelange committed
683

684
   /* Ignore SIGPIPE to be able to recover from errors instead of crashing the node */
jdelange's avatar
jdelange committed
685

686
   if (signal (SIGPIPE, SIG_IGN) == SIG_ERR)
jdelange's avatar
jdelange committed
687
   {
688
689
690
691
      __DEBUGMSG (" [error signal() return code in file %s, line%d ]\n", __FILE__, __LINE__);
      close (nodes[associated_device].socket);
      nodes[associated_device].socket = -1;
      return __PO_HI_ERROR_TRANSPORT_SEND;
jdelange's avatar
jdelange committed
692
   }
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708

   switch (protocol_id)
   {
#ifdef __PO_HI_USE_PROTOCOL_MYPROTOCOL_I
      case virtual_bus_myprotocol_i:
      {
         size_to_write = sizeof (int);
         int datawritten;
         protocol_conf->marshaller(request, &datawritten, &size_to_write);
         len = write (nodes[associated_device].socket, &datawritten, size_to_write);

         if (len != size_to_write)
         {
            __DEBUGMSG (" [error write() length in file %s, line%d ]\n", __FILE__, __LINE__);
            close (nodes[associated_device].socket);
            nodes[associated_device].socket = -1;
709
            return __PO_HI_ERROR_TRANSPORT_SEND;
710
711
712
713
         }
         break;
      }
#endif
714
      default:
715
716
      {
         request->port = destination_port;
717

718
         __po_hi_msg_reallocate (&__po_hi_c_driver_eth_leon_sender_msg);
719
720
721
         __po_hi_marshall_request
           (request, &__po_hi_c_driver_eth_leon_sender_msg);

722
723
         size_to_write =  __po_hi_msg_length
           (&__po_hi_c_driver_eth_leon_sender_msg);
724
725

#ifdef __PO_HI_DEBUG
726
         __po_hi_messages_debug (&__po_hi_c_driver_eth_leon_sender_msg);
jdelange's avatar
jdelange committed
727
728
#endif

729
730
731
732
         /* Note: in the following, we send first the size of the
            message, then the subset of the message buffer we
            actually used. */

733
         int msg_size_network = size_to_write;
734
735
736
737

         len = write (nodes[associated_device].socket,
                      &msg_size_network, sizeof (int));

yoogx's avatar
yoogx committed
738
         len = write (nodes[associated_device].socket,
739
740
                      &(__po_hi_c_driver_eth_leon_sender_msg.content),
                      size_to_write);
741
742
743

         if (len != size_to_write)
         {
yoogx's avatar
yoogx committed
744
745
            __DEBUGMSG (" [error write() length in file %s, line%d ]\n",
                        __FILE__, __LINE__);
746
747
            close (nodes[associated_device].socket);
            nodes[associated_device].socket = -1;
748
            return __PO_HI_ERROR_TRANSPORT_SEND;
749
         }
750
         request->port = __PO_HI_GQUEUE_INVALID_PORT;
751
752
753
754
755
         break;
      }
   }

   return __PO_HI_SUCCESS;
jdelange's avatar
jdelange committed
756
757
758
759
}
#endif

#endif