po_hi_driver_leon_eth.c 24.2 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
 *
8
 * Copyright (C) 2011-2020 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;
56

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

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

62
63
64
65
   #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
66

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

#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

76
#define RTEMS_BSP_NETWORK_DRIVER_ATTACH RTEMS_BSP_NETWORK_DRIVER_ATTACH_SMC91111
77
78
79

#define RTEMS_BSP_NETWORK_DRIVER_NAME "open_eth1"

yoogx's avatar
yoogx committed
80
81
#endif

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

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

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

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

yoogx's avatar
yoogx committed
104
#if defined RTEMS48 || defined RTEMS410
105
106
107
108
/*
 * Default network interface
 */
static struct rtems_bsdnet_ifconfig netdriver_config = {
yoogx's avatar
yoogx committed
109
110
111
   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
112
113
   loopback_config,

114
   /*
115
116
117
#ifdef RTEMS48
   loopback_config,
#else
118
   0,
119
#endif
120
   */
121
122
123
124
125
   "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
126
#elif defined RTEMS412
127
128
129
130
131
132
133
134
135

// Things are always moving around in RTEMS - adapt.
// The latest RTEMS (2019/07) has restructured Leon/AMBA
// headers under grlib. Detect this by a combination of checks,
// that depends on the fact that our custom cross build in TASTE
// enabled Ada (which Gaisler's RCC doesn't).
#if ((__RTEMS_ADA__ != 0) && (((__RTEMS_MAJOR__ << 8) | (__RTEMS_MINOR__ << 0)) >= 0x0500))
#include <grlib/network_interface_add.h>
#else
yoogx's avatar
yoogx committed
136
#include <bsp/network_interface_add.h>
137
#endif
yoogx's avatar
yoogx committed
138
139
140

struct ethernet_config interface_configs[]=
{
yoogx's avatar
yoogx committed
141
        { "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
142
143
144
145
        {NULL, NULL, {0,0,0,0,0,0}}  // NULL - used for BOOTP
};

#endif
146
147
148

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

182
/******************************************************************************/
183
184
__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;
185
__po_hi_mutex_t            __po_hi_c_leon_eth_send_mutex;
jdelange's avatar
jdelange committed
186

187
188
189
190
191
192
193
194
195
196
197
198
199
200
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;
201
   int                        established = 0;
202
203
204
205
206
207
208
209
210
   __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
211
   {
212
      rnodes[dev].socket = -1;
jdelange's avatar
jdelange committed
213
214
   }

yoogx's avatar
yoogx committed
215
216
217
   __po_hi_bus_id bus_current_node, bus_connect_node;
   bus_current_node = *__po_hi_transport_get_accessed_buses(leon_eth_device_id);

218
219
220
221
   /*
    * 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
222
   {
223
224
      if (dev != leon_eth_device_id)
      {
yoogx's avatar
yoogx committed
225
226
227
         bus_connect_node = *__po_hi_transport_get_accessed_buses(dev);
         if (bus_current_node == bus_connect_node)
         {
jdelange's avatar
jdelange committed
228
         __PO_HI_SET_SOCKET_TIMEOUT(nodes[leon_eth_device_id].socket,500000);
229
230
231
232
233

         established = 0;

         while (established == 0)
         {
jdelange's avatar
jdelange committed
234
235

            __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);
236
237
            sock = accept (nodes[leon_eth_device_id].socket, (struct sockaddr*) &sa, &socklen);

jdelange's avatar
jdelange committed
238
239
240
241
242
243
244
            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);
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264

#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;
265
         }
yoogx's avatar
yoogx committed
266
267
      } /* check the bus */
      } /* check the device */
jdelange's avatar
jdelange committed
268
   }
269
270
271
272
273
   __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");

   /*
274
275
276
    * 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.
277
    */
278

279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
   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);
294
#endif
295
296
297
      }
#ifdef __PO_HI_DEBUG
      __DEBUGMSG ("[DRIVER ETH] Receive message\n");
jdelange's avatar
jdelange committed
298
299
#endif

300
301
302
303
      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))
          {
304
            __DEBUGMSG ("[DRIVER ETH] Receive message from dev %d\n", dev);
305

306
307
308
309
310
311
312
313
314
315
316
317
318
#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;
               }
319
320
               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;
321
322
323
            }

#else
324
325
326
327
328
329
330
            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;
331
            len = recv (rnodes[dev].socket,
332
333
334
335
336
337
338
339
340
                        &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);
341
            __po_hi_c_driver_eth_leon_poller_msg.length = len;
342
343
            __DEBUGMSG ("[DRIVER ETH] Message received len=%d\n",(int)len);

344
345
346
347
            if (len <= 0) {
              __DEBUGMSG ("[DRIVER ETH] Zero size from device %d\n",dev);
              rnodes[dev].socket = -1;
              continue;
348
349
            }

350
351
352
            __po_hi_unmarshall_request
              (& __po_hi_c_driver_eth_leon_poller_received_request,
               &__po_hi_c_driver_eth_leon_poller_msg);
353
354
#endif

355
            __po_hi_main_deliver (& __po_hi_c_driver_eth_leon_poller_received_request);
356
357
         }
      }
358
   }
jdelange's avatar
jdelange committed
359
360
361
362
363
364
365
366
}
#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)
{
367
368
369
370
371
372
373
374
375
376
377
378
379
   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
380
   unsigned short ip_port;
381
382
383
384
   int node;

   ipconf = (__po_hi_c_ip_conf_t*)__po_hi_get_device_configuration (id);

yoogx's avatar
yoogx committed
385
#if defined RTEMS48 || defined RTEMS410
386
387
388
389
390
391
   netdriver_config.ip_address = ipconf->address;

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

yoogx's avatar
yoogx committed
395
396
397
398
399
   if (ipconf->exist.netmask == 1)
   {
      interface_configs[0].ip_netmask= ipconf->netmask;
   }
#endif
400
401
402
403
404
405
406
407
408
409
410

   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
411
  __po_hi_c_driver_rasta_common_init();
yoogx's avatar
yoogx committed
412

yoogx's avatar
yoogx committed
413
  rtems_bsdnet_initialize_network();
yoogx's avatar
yoogx committed
414

415
#if __PO_HI_DEBUG_LEVEL >= __PO_HI_DEBUG_LEVEL_DEBUG
416
417
418
   rtems_bsdnet_show_if_stats ();
   rtems_bsdnet_show_inet_routes ();
   rtems_bsdnet_show_ip_stats ();
yoogx's avatar
yoogx committed
419
   rtems_bsdnet_show_mbuf_stats ();
420
421
#endif

yoogx's avatar
yoogx committed
422
   leon_eth_device_id = id;
423
   __po_hi_mutex_init (&__po_hi_c_leon_eth_send_mutex,__PO_HI_MUTEX_REGULAR, 0);
jdelange's avatar
jdelange committed
424
425
   __po_hi_transport_set_sending_func (leon_eth_device_id, __po_hi_c_driver_eth_leon_sender);

426

427
428
429
430
431
   for (node = 0 ; node < __PO_HI_NB_DEVICES ; node++)
   {
      nodes[node].socket = -1;
   }

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

jdelange's avatar
jdelange committed
434
   __DEBUGMSG ("My configuration, addr=%s, port=%d\n", ipconf->address, ip_port);
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452

   /*
    * 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
453
      __DEBUGMSG ("[DRIVER ETH] Receiving socket for device %d created, value=%d\n", id, nodes[id].socket);
454
455

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

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

jdelange's avatar
jdelange committed
464
      if( listen( nodes[id].socket , __PO_HI_NB_ENTITIES ) == -1)
465
466
467
468
      {
         __DEBUGMSG ("Cannot listen on socket %d\n", nodes[id].socket);
      }

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

471
      /*
472
473
474
475
476
477
478
       * 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
479
480
481
//       __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);
482
483
484
485
486
487
   }

   /*
    * 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
488
489
490
491
492
493
   __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);

494
495
496
497
498
499
   for (dev = 0 ; dev < __PO_HI_NB_DEVICES ; dev++ )
   {
      if (dev == id)
      {
         continue;
      }
yoogx's avatar
yoogx committed
500

yoogx's avatar
yoogx committed
501
502
503
      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
504

yoogx's avatar
yoogx committed
505
506
507
508
      if (bus_current_node != bus_connect_node)
      {
         continue;
      }
509
510
511
512
513
514
515

      __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
516
      ip_port = (unsigned short)ipconf->port;
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544

      __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
545
546
         __DEBUGMSG ("[DRIVER ETH] Got the following information for device %d\n", dev);

547
548
549
550
551
552
553
554
555
556
         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
557
558
559
          *
          *
          memcpy( (char*) &( sa.sin_addr ) , (char*)hostinfo->h_addr , hostinfo->h_length );
560
561
562
563
564
565
566
          */
         tmp = (char*) &(sa.sin_addr);
         for (i=0 ; i<hostinfo->h_length ; i++)
         {
            tmp[i] = hostinfo->h_addr[i];
         }

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

569
570
571
572
573
         /*
          * We try to connect on the remote host. We try every
          * second to connect on.
          */

574
         ret = connect (nodes[dev].socket,
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
                        (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
594
595
596
            __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))
597
            {
jdelange's avatar
jdelange committed
598
               __DEBUGMSG ("[DRIVER ETH] Device %d cannot send his id (ret=%d)\n", id, ret);
599
            }
jdelange's avatar
jdelange committed
600
            __DEBUGMSG ("[DRIVER ETH] Connection established with device %d, socket=%d (ret=%d)\n", dev, nodes[dev].socket, ret);
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
            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
627
   __DEBUGMSG ("[DRIVER ETH] INITIALIZATION DONE\n");
628

jdelange's avatar
jdelange committed
629
630
631
632
}
#endif


633
#if defined (__PO_HI_NEED_DRIVER_ETH_LEON)
jdelange's avatar
jdelange committed
634

635
636
__po_hi_msg_t              __po_hi_c_driver_eth_leon_sender_msg;

jdelange's avatar
jdelange committed
637
638
int  __po_hi_c_driver_eth_leon_sender (__po_hi_task_id task, __po_hi_port_t port)
{
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
   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
660

661
662
663
664
665
#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
666

667
668
669
670
671
   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
672
      return __PO_HI_ERROR_TRANSPORT_SEND;
673
   }
jdelange's avatar
jdelange committed
674

675
676
677
678
   /*
    * After sending the entity identifier, we send the message which
    * contains the request.
    */
jdelange's avatar
jdelange committed
679

680
   size_to_write = __PO_HI_MESSAGES_MAX_SIZE;
jdelange's avatar
jdelange committed
681

682
683
684
685
686
   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;
687
      return __PO_HI_ERROR_TRANSPORT_SEND;
688
   }
jdelange's avatar
jdelange committed
689

690
691
692
693
694
   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;
695
      return __PO_HI_ERROR_TRANSPORT_SEND;
696
   }
jdelange's avatar
jdelange committed
697

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

700
   if (signal (SIGPIPE, SIG_IGN) == SIG_ERR)
jdelange's avatar
jdelange committed
701
   {
702
703
704
705
      __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
706
   }
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722

   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;
723
            return __PO_HI_ERROR_TRANSPORT_SEND;
724
725
726
727
         }
         break;
      }
#endif
728
      default:
729
730
      {
         request->port = destination_port;
731
         __po_hi_mutex_lock (&__po_hi_c_leon_eth_send_mutex);
732

733
         __po_hi_msg_reallocate (&__po_hi_c_driver_eth_leon_sender_msg);
734
735
736
         __po_hi_marshall_request
           (request, &__po_hi_c_driver_eth_leon_sender_msg);

737
738
         size_to_write =  __po_hi_msg_length
           (&__po_hi_c_driver_eth_leon_sender_msg);
739
740

#ifdef __PO_HI_DEBUG
741
         __po_hi_messages_debug (&__po_hi_c_driver_eth_leon_sender_msg);
jdelange's avatar
jdelange committed
742
743
#endif

744
745
746
747
         /* Note: in the following, we send first the size of the
            message, then the subset of the message buffer we
            actually used. */

748
         int msg_size_network = size_to_write;
749
750
751
752

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

yoogx's avatar
yoogx committed
753
         len = write (nodes[associated_device].socket,
754
755
                      &(__po_hi_c_driver_eth_leon_sender_msg.content),
                      size_to_write);
756
757
758

         if (len != size_to_write)
         {
759
            __po_hi_mutex_unlock (&__po_hi_c_leon_eth_send_mutex);
yoogx's avatar
yoogx committed
760
761
            __DEBUGMSG (" [error write() length in file %s, line%d ]\n",
                        __FILE__, __LINE__);
762
763
            close (nodes[associated_device].socket);
            nodes[associated_device].socket = -1;
764
            return __PO_HI_ERROR_TRANSPORT_SEND;
765
         }
766
         __po_hi_mutex_unlock (&__po_hi_c_leon_eth_send_mutex);
767
         request->port = __PO_HI_GQUEUE_INVALID_PORT;
768
769
770
771
772
         break;
      }
   }

   return __PO_HI_SUCCESS;
jdelange's avatar
jdelange committed
773
774
775
776
}
#endif

#endif