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://taste.tuxfamily.org/wiki
jdelange's avatar
jdelange committed
7
 *
8
 * Copyright (C) 2011-2015 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
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;
147
   int                        established = 0;
148
149
150
151
152
153
154
155
156
157
158
159
160
   __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

#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;
210
         }
211
      }
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
   __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);
236
#endif
237
238
239
      }
#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

      sa.sin_addr.s_addr = htonl (INADDR_ANY);   /* We listen on all adresses */
385
      sa.sin_family = AF_INET;
386
387
      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
       * 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 ();

408
409
410
      __po_hi_create_generic_task
        (-1, 0,__PO_HI_MAX_PRIORITY, 0, 0, (void* (*)(void)) __po_hi_c_driver_eth_leon_poller, NULL);
      // XXX Why forcing core id to 0 ?
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
   }

   /*
    * 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
430
      ip_port = (unsigned short)ipconf->port;
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
458

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

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

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

483
484
485
486
487
         /*
          * We try to connect on the remote host. We try every
          * second to connect on.
          */

488
         ret = connect (nodes[dev].socket,
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
                        (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
508
509
510
            __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))
511
            {
jdelange's avatar
jdelange committed
512
               __DEBUGMSG ("[DRIVER ETH] Device %d cannot send his id (ret=%d)\n", id, ret);
513
            }
jdelange's avatar
jdelange committed
514
            __DEBUGMSG ("[DRIVER ETH] Connection established with device %d, socket=%d (ret=%d)\n", dev, nodes[dev].socket, ret);
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
540
            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
541
   __DEBUGMSG ("[DRIVER ETH] INITIALIZATION DONE\n");
542

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


547
#if defined (__PO_HI_NEED_DRIVER_ETH_LEON)
jdelange's avatar
jdelange committed
548

549
550
__po_hi_msg_t              __po_hi_c_driver_eth_leon_sender_msg;

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

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

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

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

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

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

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

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

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

   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;
640
            return __PO_HI_ERROR_TRANSPORT_SEND;
641
642
643
644
         }
         break;
      }
#endif
645
      default:
646
647
      {
         request->port = destination_port;
648
649
         __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);
650
651

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

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

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

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

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

#endif