po_hi_driver_leon_eth.c 23 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-2017 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

yoogx's avatar
yoogx committed
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
112
113
   RTEMS_BSP_NETWORK_DRIVER_NAME,      /* name  */
   RTEMS_BSP_NETWORK_DRIVER_ATTACH, /* attach function  greth_interface_driver_attach */
//   0, /* link to next interface */
   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
127
128
129
130
131
132
133
134
135
#elif defined RTEMS412
#include <bsp/network_interface_add.h>

struct ethernet_config interface_configs[]=
{
        { "255.255.255.255", "255.255.255.255", {0x80,0x80,0x80,0x80,0x80,0x80}},// NULL - take PHY address and IP from device 
        {NULL, NULL, {0,0,0,0,0,0}}  // NULL - used for BOOTP
};

#endif
136
137
138

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

   unsigned long* swap_pointer;
   unsigned long swap_value;


   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
203
   {
204
      rnodes[dev].socket = -1;
jdelange's avatar
jdelange committed
205
206
   }

yoogx's avatar
yoogx committed
207
208
209
   __po_hi_bus_id bus_current_node, bus_connect_node;
   bus_current_node = *__po_hi_transport_get_accessed_buses(leon_eth_device_id);

210
211
212
213
   /*
    * 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
214
   {
215
216
      if (dev != leon_eth_device_id)
      {
yoogx's avatar
yoogx committed
217
218
219
         bus_connect_node = *__po_hi_transport_get_accessed_buses(dev);
         if (bus_current_node == bus_connect_node)
         {
jdelange's avatar
jdelange committed
220
         __PO_HI_SET_SOCKET_TIMEOUT(nodes[leon_eth_device_id].socket,500000);
221
222
223
224
225

         established = 0;

         while (established == 0)
         {
jdelange's avatar
jdelange committed
226
227

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

jdelange's avatar
jdelange committed
230
231
232
233
234
235
236
            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);
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256

#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;
257
         }
yoogx's avatar
yoogx committed
258
259
      } /* check the bus */
      } /* check the device */
jdelange's avatar
jdelange committed
260
   }
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
   __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");

   /*
    * Then, listen and receive data on the socket, identify the node
    * which send the data and put it in its message queue
    */
   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);
284
#endif
285
286
287
      }
#ifdef __PO_HI_DEBUG
      __DEBUGMSG ("[DRIVER ETH] Receive message\n");
jdelange's avatar
jdelange committed
288
289
#endif

290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
      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))
         {
            __DEBUGMSG ("[DRIVER ETH] Receive message from dev %d\n", dev);
#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;
               }
311
312
               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;
313
314
315
            }

#else
316
317
318
            memset (__po_hi_c_driver_eth_leon_poller_msg.content, '\0', __PO_HI_MESSAGES_MAX_SIZE);
            len = recv (rnodes[dev].socket, __po_hi_c_driver_eth_leon_poller_msg.content, __PO_HI_MESSAGES_MAX_SIZE, MSG_WAITALL);
            __po_hi_c_driver_eth_leon_poller_msg.length = len;
319
320
321
322
323
            __DEBUGMSG ("[DRIVER ETH] Message received len=%d\n",(int)len);

#ifdef __PO_HI_DEBUG
   __po_hi_messages_debug (&msg);
#endif
jdelange's avatar
jdelange committed
324
325


326
327
328
329
330
331
332
            if (len == 0)
            {

               __DEBUGMSG ("[DRIVER ETH] Zero size from device %d\n",dev);
               rnodes[dev].socket = -1;
               continue;
            }
333
            swap_pointer  = (unsigned long*) &__po_hi_c_driver_eth_leon_poller_msg.content[0];
334
335
336
            swap_value    = *swap_pointer;
            *swap_pointer = __po_hi_swap_byte (swap_value);

337
            __po_hi_unmarshall_request (& __po_hi_c_driver_eth_leon_poller_received_request, &__po_hi_c_driver_eth_leon_poller_msg);
338
339
340

#endif

341
            __po_hi_main_deliver (& __po_hi_c_driver_eth_leon_poller_received_request);
342
343
         }
      }
344
   }
jdelange's avatar
jdelange committed
345
346
347
348
349
350
351
352
}
#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)
{
353
354
355
356
357
358
359
360
361
362
363
364
365
366
   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
367
   unsigned short ip_port;
368
369
370
371
   int node;

   ipconf = (__po_hi_c_ip_conf_t*)__po_hi_get_device_configuration (id);

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

   if (ipconf->exist.netmask == 1)
   {
      netdriver_config.ip_netmask = ipconf->netmask;
   }
yoogx's avatar
yoogx committed
379
380
381
382
383
384
385
386
387
#elif defined RTEMS412
   interface_configs[0].ip_addr = ipconf->address;
   
   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
400
401
402
403
  __po_hi_c_driver_rasta_common_init();
  
  rtems_bsdnet_initialize_network();
/*
  #ifdef __PO_HI_DEBUG_INFO
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
#endif
jdelange's avatar
jdelange committed
409
*/
410
411
   leon_eth_device_id = id;

jdelange's avatar
jdelange committed
412
413
414

   __po_hi_transport_set_sending_func (leon_eth_device_id, __po_hi_c_driver_eth_leon_sender);

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

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

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

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

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

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

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

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

459
      /*
460
461
462
463
464
465
466
       * 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
467
468
469
//       __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);
470
471
472
473
474
475
   }

   /*
    * 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
476
477
478
479
480
481
   __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);

482
483
484
485
486
487
   for (dev = 0 ; dev < __PO_HI_NB_DEVICES ; dev++ )
   {
      if (dev == id)
      {
         continue;
      }
yoogx's avatar
yoogx committed
488
489
490
491
492
493
494
495
496
      
      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);
   
      if (bus_current_node != bus_connect_node)
      {
         continue;
      }
497
498
499
500
501
502
503

      __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
504
      ip_port = (unsigned short)ipconf->port;
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
531
532

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

535
536
537
538
539
540
541
542
543
544
         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
545
546
547
          *
          *
          memcpy( (char*) &( sa.sin_addr ) , (char*)hostinfo->h_addr , hostinfo->h_length );
548
549
550
551
552
553
554
          */
         tmp = (char*) &(sa.sin_addr);
         for (i=0 ; i<hostinfo->h_length ; i++)
         {
            tmp[i] = hostinfo->h_addr[i];
         }

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

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

562
         ret = connect (nodes[dev].socket,
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
                        (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
582
583
584
            __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))
585
            {
jdelange's avatar
jdelange committed
586
               __DEBUGMSG ("[DRIVER ETH] Device %d cannot send his id (ret=%d)\n", id, ret);
587
            }
jdelange's avatar
jdelange committed
588
            __DEBUGMSG ("[DRIVER ETH] Connection established with device %d, socket=%d (ret=%d)\n", dev, nodes[dev].socket, ret);
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
            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
615
   __DEBUGMSG ("[DRIVER ETH] INITIALIZATION DONE\n");
616

jdelange's avatar
jdelange committed
617
618
619
620
}
#endif


621
#if defined (__PO_HI_NEED_DRIVER_ETH_LEON)
jdelange's avatar
jdelange committed
622

623
624
__po_hi_msg_t              __po_hi_c_driver_eth_leon_sender_msg;

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

   unsigned long* swap_pointer;
   unsigned long swap_value;

   __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
651

652
653
654
655
656
#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
657

658
659
660
661
662
   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
663
      return __PO_HI_ERROR_TRANSPORT_SEND;
664
   }
jdelange's avatar
jdelange committed
665

666
667
668
669
   /*
    * After sending the entity identifier, we send the message which
    * contains the request.
    */
jdelange's avatar
jdelange committed
670

671
   size_to_write = __PO_HI_MESSAGES_MAX_SIZE;
jdelange's avatar
jdelange committed
672

673
674
675
676
677
   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;
678
      return __PO_HI_ERROR_TRANSPORT_SEND;
679
   }
jdelange's avatar
jdelange committed
680

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

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

691
   if (signal (SIGPIPE, SIG_IGN) == SIG_ERR)
jdelange's avatar
jdelange committed
692
   {
693
694
695
696
      __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
697
   }
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713

   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;
714
            return __PO_HI_ERROR_TRANSPORT_SEND;
715
716
717
718
         }
         break;
      }
#endif
719
      default:
720
721
      {
         request->port = destination_port;
722
723
         __po_hi_msg_reallocate (&__po_hi_c_driver_eth_leon_sender_msg);
         __po_hi_marshall_request (request, &__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
         swap_pointer  = (unsigned long*) &__po_hi_c_driver_eth_leon_sender_msg.content[0];
730
731
         swap_value    = *swap_pointer;
         *swap_pointer = __po_hi_swap_byte (swap_value);
732
         len = write (nodes[associated_device].socket, &(__po_hi_c_driver_eth_leon_sender_msg.content), size_to_write);
733
734
735
736
737
738

         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;
739
            return __PO_HI_ERROR_TRANSPORT_SEND;
740
         }
741
742

         request->port = __PO_HI_GQUEUE_INVALID_PORT;
743
744
745
746
747
         break;
      }
   }

   return __PO_HI_SUCCESS;
jdelange's avatar
jdelange committed
748
749
750
751
}
#endif

#endif