po_hi_driver_leon_eth.c 20.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://http://taste.tuxfamily.org/wiki
jdelange's avatar
jdelange committed
7
 *
yoogx's avatar
yoogx committed
8
 * Copyright (C) 2011-2014 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>
jdelange's avatar
jdelange committed
25
#include <drivers/po_hi_driver_leon_eth.h>
26
#include <drivers/po_hi_driver_sockets.h>
jdelange's avatar
jdelange committed
27
28
29
30
31
32
33
34
35
36
37
38
#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>
39
40
41
42
43
44
45
46
47
48
49
50
51
52
#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;
jdelange's avatar
jdelange committed
53
54
55
56
57


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

58
59
60
61
   #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
62

63
#define RTEMS_BSP_NETWORK_DRIVER_ATTACH RTEMS_BSP_NETWORK_DRIVER_ATTACH_SMC91111
64
65
66

#define RTEMS_BSP_NETWORK_DRIVER_NAME "open_eth1"

67
68
#include <bsp.h>
#include <rtems/rtems_bsdnet.h>
jdelange's avatar
jdelange committed
69

70
#ifdef RTEMS48
71
extern void rtems_bsdnet_loopattach();
72
#else
73
extern void rtems_bsdnet_initialize_loop();
74
#endif
75

76
77
static struct rtems_bsdnet_ifconfig loopback_config = {
   "lo0",            /* name */
78
#ifdef RTEMS48
79
   rtems_bsdnet_loopattach,   /* attach function */
80
81
82
#else
   rtems_bsdnet_initialize_loop,
#endif
83
   NULL,          /* link to next interface */
jdelange's avatar
jdelange committed
84

85
86
87
88
89
90
91
92
93
94
   "127.0.0.1",         /* IP address */
   "255.0.0.0",         /* IP net mask */
};

/*
 * Default network interface
 */
static struct rtems_bsdnet_ifconfig netdriver_config = {
   RTEMS_BSP_NETWORK_DRIVER_NAME,      /* name */
   RTEMS_BSP_NETWORK_DRIVER_ATTACH, /* attach function */
95
96
   0, /* link to next interface */
   /*
97
98
99
#ifdef RTEMS48
   loopback_config,
#else
100
   0,    
101
#endif
102
   */
103
104
105
106
107
108
109
110
111
112
113
114
   "255.255.255.255",       /* IP address */
   "255.255.255.255",     /* IP net mask */
   NULL,                           /* Driver supplies hardware address */
   0           /* Use default driver parameters */
};

/*
 * Network configuration
 */
struct rtems_bsdnet_config rtems_bsdnet_config = {
   &netdriver_config,
   NULL,
115
#ifdef RTEMS48
116
   100,        /* Default network task priority */
117
118
119
#else
   150,
#endif
120
121
122
123
124
125
126
127
128
   128*1024,      /* Default mbuf capacity */
   256*1024,      /* Default mbuf cluster capacity */
   "rtems_host",     /* Host name */
   "localnet",    /* Domain name */
   "255.255.255.255",    /* Gateway */
   "10.1.1.1",    /* Log host */
   {"255.255.255.255" }, /* Name server(s) */
   {"255.155.255.255" }, /* NTP server(s) */
};
jdelange's avatar
jdelange committed
129

130
131
__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
132

133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
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;
   int                        established = 0; 
   __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
161
   {
162
      rnodes[dev].socket = -1;
jdelange's avatar
jdelange committed
163
164
   }

165
166
167
168
   /*
    * 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
169
   {
170
171
172
      if (dev != leon_eth_device_id)
      {

jdelange's avatar
jdelange committed
173
         __PO_HI_SET_SOCKET_TIMEOUT(nodes[leon_eth_device_id].socket,500000);
174
175
176
177
178

         established = 0;

         while (established == 0)
         {
jdelange's avatar
jdelange committed
179
180

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

jdelange's avatar
jdelange committed
183
184
185
186
187
188
189
            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);
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211

#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;
         }	  
      }
jdelange's avatar
jdelange committed
212
   }
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
   __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);
#endif 
      }
#ifdef __PO_HI_DEBUG
      __DEBUGMSG ("[DRIVER ETH] Receive message\n");
jdelange's avatar
jdelange committed
240
241
#endif

242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
      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;
               }
263
264
               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;
265
266
267
            }

#else
268
269
270
            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;
271
272
273
274
275
            __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
276
277


278
279
280
281
282
283
284
            if (len == 0)
            {

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

289
            __po_hi_unmarshall_request (& __po_hi_c_driver_eth_leon_poller_received_request, &__po_hi_c_driver_eth_leon_poller_msg);
290
291
292

#endif

293
            __po_hi_main_deliver (& __po_hi_c_driver_eth_leon_poller_received_request);
294
295
296
         }
      }
   }  
jdelange's avatar
jdelange committed
297
298
299
300
301
302
303
304
}
#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)
{
305
306
307
308
309
310
311
312
313
314
315
316
317
318
   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
319
   unsigned short ip_port;
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
   int node;

   ipconf = (__po_hi_c_ip_conf_t*)__po_hi_get_device_configuration (id);

   netdriver_config.ip_address = ipconf->address;

   if (ipconf->exist.netmask == 1)
   {
      netdriver_config.ip_netmask = ipconf->netmask;
   }

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

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


  rtems_bsdnet_initialize_network ();
343
  /*
jdelange's avatar
jdelange committed
344
#ifdef __PO_HI_DEBUG_INFO
345
346
347
348
   rtems_bsdnet_show_if_stats ();
   rtems_bsdnet_show_inet_routes ();
   rtems_bsdnet_show_ip_stats ();
#endif
jdelange's avatar
jdelange committed
349
*/
350

351
352
   leon_eth_device_id = id;

jdelange's avatar
jdelange committed
353
354
355

   __po_hi_transport_set_sending_func (leon_eth_device_id, __po_hi_c_driver_eth_leon_sender);

356
357
358
359
360
   for (node = 0 ; node < __PO_HI_NB_DEVICES ; node++)
   {
      nodes[node].socket = -1;
   }

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

jdelange's avatar
jdelange committed
363
   __DEBUGMSG ("My configuration, addr=%s, port=%d\n", ipconf->address, ip_port);
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381

   /*
    * 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
382
      __DEBUGMSG ("[DRIVER ETH] Receiving socket for device %d created, value=%d\n", id, nodes[id].socket);
383
384
385
386
387

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

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

jdelange's avatar
jdelange committed
393
      if( listen( nodes[id].socket , __PO_HI_NB_ENTITIES ) == -1)
394
395
396
397
      {
         __DEBUGMSG ("Cannot listen on socket %d\n", nodes[id].socket);
      }

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

400
401
402
403
404
405
406
407
408
      /* 
       * 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 ();

      __po_hi_create_generic_task 
jdelange's avatar
jdelange committed
409
         (-1, 0,__PO_HI_MAX_PRIORITY, 0, (void* (*)(void)) __po_hi_c_driver_eth_leon_poller, NULL);
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
   }

   /*
    * 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.
    */
   for (dev = 0 ; dev < __PO_HI_NB_DEVICES ; dev++ )
   {
      if (dev == id)
      {
         continue;
      }

      __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
429
      ip_port = (unsigned short)ipconf->port;
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457

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

460
461
462
463
464
465
466
467
468
469
         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
470
471
472
          *
          *
          memcpy( (char*) &( sa.sin_addr ) , (char*)hostinfo->h_addr , hostinfo->h_length );
473
474
475
476
477
478
479
          */
         tmp = (char*) &(sa.sin_addr);
         for (i=0 ; i<hostinfo->h_length ; i++)
         {
            tmp[i] = hostinfo->h_addr[i];
         }

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

482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
         /*
          * We try to connect on the remote host. We try every
          * second to connect on.
          */

         ret = connect (nodes[dev].socket, 
                        (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
507
508
509
            __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))
510
            {
jdelange's avatar
jdelange committed
511
               __DEBUGMSG ("[DRIVER ETH] Device %d cannot send his id (ret=%d)\n", id, ret);
512
            }
jdelange's avatar
jdelange committed
513
            __DEBUGMSG ("[DRIVER ETH] Connection established with device %d, socket=%d (ret=%d)\n", dev, nodes[dev].socket, ret);
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
            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
540
   __DEBUGMSG ("[DRIVER ETH] INITIALIZATION DONE\n");
541

jdelange's avatar
jdelange committed
542
543
544
545
546
547
}
#endif


#if defined (__PO_HI_NEED_DRIVER_ETH_LEON) 

548
549
__po_hi_msg_t              __po_hi_c_driver_eth_leon_sender_msg;

jdelange's avatar
jdelange committed
550
551
int  __po_hi_c_driver_eth_leon_sender (__po_hi_task_id task, __po_hi_port_t port)
{
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
   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
576

577
578
579
580
581
#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
582

583
584
585
586
587
588
589
   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
      return __PO_HI_ERROR_TRANSPORT_SEND;		
   }
jdelange's avatar
jdelange committed
590

591
592
593
594
   /*
    * After sending the entity identifier, we send the message which
    * contains the request.
    */
jdelange's avatar
jdelange committed
595

596
   size_to_write = __PO_HI_MESSAGES_MAX_SIZE;
jdelange's avatar
jdelange committed
597

598
599
600
601
602
603
604
   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;
      return __PO_HI_ERROR_TRANSPORT_SEND;		
   }
jdelange's avatar
jdelange committed
605

606
607
608
609
610
611
612
   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;
      return __PO_HI_ERROR_TRANSPORT_SEND;		
   }
jdelange's avatar
jdelange committed
613

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

616
   if (signal (SIGPIPE, SIG_IGN) == SIG_ERR)
jdelange's avatar
jdelange committed
617
   {
618
619
620
621
      __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
622
   }
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646

   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;
            return __PO_HI_ERROR_TRANSPORT_SEND;		
         }
         break;
      }
#endif
      default: 
      {
         request->port = destination_port;
647
648
         __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);
649
650

#ifdef __PO_HI_DEBUG
651
         __po_hi_messages_debug (&__po_hi_c_driver_eth_leon_sender_msg);
jdelange's avatar
jdelange committed
652
653
#endif

654
         swap_pointer  = (unsigned long*) &__po_hi_c_driver_eth_leon_sender_msg.content[0];
655
656
         swap_value    = *swap_pointer;
         *swap_pointer = __po_hi_swap_byte (swap_value);
657
         len = write (nodes[associated_device].socket, &(__po_hi_c_driver_eth_leon_sender_msg.content), size_to_write);
658
659
660
661
662
663
664
665

         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;
            return __PO_HI_ERROR_TRANSPORT_SEND;		
         }
666
667

         request->port = __PO_HI_GQUEUE_INVALID_PORT;
668
669
670
671
672
         break;
      }
   }

   return __PO_HI_SUCCESS;
jdelange's avatar
jdelange committed
673
674
675
676
}
#endif

#endif