po_hi_driver_sockets.c 19.4 KB
Newer Older
1
2
3
4
5
6
7
/*
 * 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.
 *
 * For more informations, please visit http://ocarina.enst.fr
 *
jdelange's avatar
jdelange committed
8
 * Copyright (C) 2010-2011, European Space Agency
9
10
11
 * Copyright (C) 2007-2008, GET-Telecom Paris.
 */

12
#include <deployment.h>
julien.delange's avatar
julien.delange committed
13
#include <marshallers.h>
14

julien.delange's avatar
julien.delange committed
15
16
#if (defined (__PO_HI_NEED_DRIVER_SOCKETS) || \
     defined (__PO_HI_NEED_DRIVER_RTEMS_NE2000_SOCKETS))
17

18
19
20
21
22
23
24
25
26
#include <po_hi_config.h>
#include <po_hi_task.h>
#include <po_hi_transport.h>
#include <po_hi_debug.h>
#include <po_hi_types.h>
#include <po_hi_messages.h>
#include <po_hi_returns.h>
#include <po_hi_main.h>
#include <po_hi_task.h>
27
#include <po_hi_gqueue.h>
28
#include <drivers/po_hi_driver_sockets.h>
29
30
31
32
33
34
35
36
37
38
39
40
41
42

#include <activity.h>

#include <signal.h>
#include <string.h>
#include <unistd.h>
#include <netdb.h>
#include <sys/types.h>
#include <sys/select.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/time.h>

43
44
#include <stdio.h>

45
/*
46
 * This file (po_hi_sockets.c) provides function to handle
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
 * communication between nodes in PolyORB-HI-C.  We don't use a
 * protocol to send data. For each data sent, we send before the
 * entity number provided by the generated file deployment.h, then, we
 * send the message.  Each entity has a fixed size
 * (sizeof(__po_hi_entity_t)), and each message has a max fixed size
 * (see the __PO_HI_MESSAGES_MAX_SIZE macro).
 */

/* The following declarations avoid conflicts
 * with current generated code.
 */

#ifndef __PO_HI_NB_NODES
#define __PO_HI_NB_NODES 1
#endif

/*
 * We have two arrays of sockets. The first array (nodes) is used to
65
 * send data to other nodes. A special socket if nodes[__po_hi_mynode] : this
66
67
68
69
70
 * socket is used to listen others processes.  The second array
 * (rnodes), is used to store all socket that are created by the
 * listen socket. This array is used only by the receiver_task
 */

jdelange's avatar
jdelange committed
71
72
73
74
75
76
int                  __po_hi_c_sockets_listen_socket;
int                  __po_hi_c_sockets_read_sockets[__PO_HI_NB_DEVICES];
int                  __po_hi_c_sockets_write_sockets[__PO_HI_NB_DEVICES];
__po_hi_request_t    __po_hi_c_sockets_poller_received_request;
__po_hi_msg_t        __po_hi_c_sockets_poller_msg;
__po_hi_msg_t        __po_hi_c_sockets_send_msg;
77

78
79
80
int __po_hi_driver_sockets_send (__po_hi_task_id task_id,
                                 __po_hi_port_t port)
{
81
82
83
84
   int                        len;
   int                        size_to_write;
   int                        optval = 0;
   socklen_t                  optlen = 0;
jdelange's avatar
jdelange committed
85
   __po_hi_device_id          remote_device;
86
87
88
89
90
91
92
93
94
   __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_id, local_port);
   destination_port        = __po_hi_gqueue_get_destination (task_id, local_port, 0);
jdelange's avatar
jdelange committed
95
   remote_device       = __po_hi_get_device_from_port (destination_port);
96
97
   protocol_id             = __po_hi_transport_get_protocol (port, destination_port);
   protocol_conf           = __po_hi_transport_get_protocol_configuration (protocol_id);
98

jdelange's avatar
jdelange committed
99

100
   __DEBUGMSG ("[DRIVER SOCKETS] Try to write from task=%d, port=%d, remote device=%d, socket=%d\n", task_id, port, remote_device, __po_hi_c_sockets_write_sockets[remote_device]);
101
102
103
104
105
106
   if (request->port == -1)
   {

#ifdef __PO_HI_DEBUG
      __DEBUGMSG (" [DRIVER SOCKETS] No data to write on port %d\n", port);
#endif
107
      return __PO_HI_ERROR_TRANSPORT_SEND;
108
109
   }

jdelange's avatar
jdelange committed
110
   if (__po_hi_c_sockets_write_sockets[remote_device] == -1 )
111
112
   {
#ifdef __PO_HI_DEBUG
113
      __DEBUGMSG (" [DRIVER SOCKETS] Invalid socket for port-id %d, device-id %d\n", destination_port, remote_device);
114
115
116
117
118
119
120
121
122
123
124
#endif
      return __PO_HI_ERROR_TRANSPORT_SEND;		
   }

   /*
    * After sending the entity identifier, we send the message which
    * contains the request.
    */

   size_to_write = __PO_HI_MESSAGES_MAX_SIZE;

jdelange's avatar
jdelange committed
125
   if (getsockopt (__po_hi_c_sockets_write_sockets[remote_device], SOL_SOCKET, SO_ERROR, &optval, &optlen) == -1)
126
127
   {
      __DEBUGMSG (" [error getsockopt() in file %s, line%d ]\n", __FILE__, __LINE__);
jdelange's avatar
jdelange committed
128
129
      close (__po_hi_c_sockets_write_sockets[remote_device]);
      __po_hi_c_sockets_write_sockets[remote_device] = -1;
130
131
132
133
134
135
      return __PO_HI_ERROR_TRANSPORT_SEND;		
   }

   if (optval != 0)
   {
      __DEBUGMSG (" [error getsockopt() return code in file %s, line%d ]\n", __FILE__, __LINE__);
jdelange's avatar
jdelange committed
136
137
      close (__po_hi_c_sockets_write_sockets[remote_device]);
      __po_hi_c_sockets_write_sockets[remote_device] = -1;
138
139
140
141
142
143
144
145
      return __PO_HI_ERROR_TRANSPORT_SEND;		
   }

   /* Ignore SIGPIPE to be able to recover from errors instead of crashing the node */

   if (signal (SIGPIPE, SIG_IGN) == SIG_ERR)
   {
      __DEBUGMSG (" [error signal() return code in file %s, line%d ]\n", __FILE__, __LINE__);
jdelange's avatar
jdelange committed
146
147
      close (__po_hi_c_sockets_write_sockets[remote_device]);
      __po_hi_c_sockets_write_sockets[remote_device] = -1;
148
149
      return __PO_HI_ERROR_TRANSPORT_SEND;
   }
150
151
152
153
154
155

   switch (protocol_id)
   {
#ifdef __PO_HI_USE_PROTOCOL_MYPROTOCOL_I
      case virtual_bus_myprotocol_i:
      {
jdelange's avatar
jdelange committed
156
157
158
         size_to_write = sizeof (int);
         int datawritten;
         protocol_conf->marshaller(request, &datawritten, &size_to_write);
jdelange's avatar
jdelange committed
159
         len = write (__po_hi_c_sockets_write_sockets[remote_device], &datawritten, size_to_write);
jdelange's avatar
jdelange committed
160
161
162
163

         if (len != size_to_write)
         {
            __DEBUGMSG (" [error write() length in file %s, line%d ]\n", __FILE__, __LINE__);
jdelange's avatar
jdelange committed
164
165
            close (__po_hi_c_sockets_write_sockets[remote_device]);
            __po_hi_c_write_sockets[remote_device] = -1;
jdelange's avatar
jdelange committed
166
167
            return __PO_HI_ERROR_TRANSPORT_SEND;		
         }
168
169
170
171
172
173
         break;
      }
#endif
      default: 
      {
         request->port = destination_port;
jdelange's avatar
jdelange committed
174
175
         __po_hi_msg_reallocate (&__po_hi_c_sockets_send_msg);
         __po_hi_marshall_request (request, &__po_hi_c_sockets_send_msg);
176
177

#ifdef __PO_HI_DEBUG
jdelange's avatar
jdelange committed
178
         __po_hi_messages_debug (&__po_hi_c_sockets_send_msg[remote_device]);
179
#endif
jdelange's avatar
jdelange committed
180
         if (__po_hi_c_sockets_write_sockets[remote_device] != -1)
181
         {
jdelange's avatar
jdelange committed
182
183
184
185
186
187
188
189
190
            len = write (__po_hi_c_sockets_write_sockets[remote_device], &(__po_hi_c_sockets_send_msg.content), size_to_write);

            if (len != size_to_write)
            {
               __DEBUGMSG (" [error write() length in file %s, line%d ]\n", __FILE__, __LINE__);
               close (__po_hi_c_sockets_write_sockets[remote_device]);
               __po_hi_c_sockets_write_sockets[remote_device] = -1;
               return __PO_HI_ERROR_TRANSPORT_SEND;		
            }
191
         }
192
193

         request->port = __PO_HI_GQUEUE_INVALID_PORT;
194
195
         break;
      }
196
197
198
199
200
201
   }

   return __PO_HI_SUCCESS;
}


202

jdelange's avatar
jdelange committed
203
void* __po_hi_sockets_poller (__po_hi_device_id* dev_id_addr)
204
{
jdelange's avatar
jdelange committed
205
   socklen_t                  socklen = sizeof (struct sockaddr);
julien.delange's avatar
julien.delange committed
206
207
   /* See ACCEPT (2) for details on initial value of socklen */

jdelange's avatar
jdelange committed
208
209
210
211
212
213
214
215
   __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; 
jdelange's avatar
jdelange committed
216
   int                        ret;
jdelange's avatar
jdelange committed
217
   unsigned short             ip_port;
jdelange's avatar
jdelange committed
218
   __po_hi_protocol_conf_t*   protocol_conf;
jdelange's avatar
jdelange committed
219
220
221
222
223
224
225
226
227
228
229
   __po_hi_c_ip_conf_t*       ipconf;
   __po_hi_device_id          dev_id;
   __po_hi_device_id          sent_id;
   struct hostent*            hostinfo;

   __po_hi_time_t             mytime;
   __po_hi_time_t             tmptime;

   char                       *tmp;
   __po_hi_time_t             current_time;
   int                        i;
julien.delange's avatar
julien.delange committed
230
231
232

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

jdelange's avatar
jdelange committed
233
234
235
236
   dev_id = *dev_id_addr;

   __DEBUGMSG ("Poller launched, device-id=%d\n", dev_id);

julien.delange's avatar
julien.delange committed
237
238
239
   /*
    * Create a socket for each node that will communicate with us.
    */
jdelange's avatar
jdelange committed
240
   for (dev = 0; dev < __PO_HI_NB_NODES - 1 ; dev++)
julien.delange's avatar
julien.delange committed
241
   {
242
243
244
         established = 0;

         while (established == 0)
julien.delange's avatar
julien.delange committed
245
         {
246
            __DEBUGMSG ("[DRIVER SOCKETS] Poller waits for connection with device %d (reading device=%d, socket=%d)\n", dev, dev_id, __po_hi_c_sockets_read_sockets[dev]);
jdelange's avatar
jdelange committed
247
            sock = accept (__po_hi_c_sockets_listen_socket, (struct sockaddr*) &sa, &socklen);
248

jdelange's avatar
jdelange committed
249
250
251
252
253
254
            if (sock == -1)
            {
               continue;
            }

            __DEBUGMSG ("[DRIVER SOCKETS] accept() passed, waiting for device id %d\n", dev);
255

256
#ifndef __PO_HI_USE_PROTOCOL_MYPROTOCOL_I
jdelange's avatar
jdelange committed
257
            dev_init = 0;
jdelange's avatar
jdelange committed
258
            ret = read (sock, &dev_init, sizeof (__po_hi_device_id));
jdelange's avatar
jdelange committed
259

jdelange's avatar
jdelange committed
260
            if (ret != sizeof (__po_hi_device_id))
261
262
            {
               established = 0;
jdelange's avatar
jdelange committed
263
               __DEBUGMSG ("[DRIVER SOCKETS] Cannot read device-id for device %d, socket=%d, ret=%d\n", dev, sock, ret);
264
265
266
            }
            else
            {
jdelange's avatar
jdelange committed
267
268
269
270
271
272
273
               dev_init = __po_hi_swap_byte (dev_init);
               if (__po_hi_c_sockets_read_sockets[dev_init] != -1)
               {
                  established = 0;
                  __DEBUGMSG ("[DRIVER SOCKETS] Invalid id sent on socket=%d, received device id=%d\n", sock, dev_init);
               }

274
               __DEBUGMSG ("[DRIVER SOCKETS] read device-id %d from socket=%d\n", dev_init, sock);
275
276
               established = 1;
            }
277
278
279
#else
            established = 1;
#endif
julien.delange's avatar
julien.delange committed
280
         }
jdelange's avatar
jdelange committed
281
         __po_hi_c_sockets_read_sockets[dev_init] = sock;
julien.delange's avatar
julien.delange committed
282
283
284
285
286
         if (sock > max_socket )
         {
            max_socket = sock;
         }	  
   }
287
   __DEBUGMSG ("[DRIVER SOCKETS] Poller initialization finished, waiting for other tasks\n");
julien.delange's avatar
julien.delange committed
288
   __po_hi_wait_initialization ();
289
   __DEBUGMSG ("[DRIVER SOCKETS] Other tasks are initialized, let's start the polling !\n");
julien.delange's avatar
julien.delange committed
290
291
292
293
294
295
296
297
298
299

   /*
    * 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++)
      {
jdelange's avatar
jdelange committed
300
         if ( (dev != dev_id ) && ( __po_hi_c_sockets_read_sockets[dev] != -1 ) )
julien.delange's avatar
julien.delange committed
301
         {
jdelange's avatar
jdelange committed
302
            FD_SET( __po_hi_c_sockets_read_sockets[dev], &selector );
julien.delange's avatar
julien.delange committed
303
304
305
306
307
308
         }
      }

      if (select (max_socket + 1, &selector, NULL, NULL, NULL) == -1 )
      {
#ifdef __PO_HI_DEBUG
309
         __DEBUGMSG ("[DRIVER SOCKETS] Error on select for node %d\n", __po_hi_mynode);
julien.delange's avatar
julien.delange committed
310
311
312
313
#endif 
      }
#ifdef __PO_HI_DEBUG
      __DEBUGMSG ("[DRIVER SOCKETS] Receive message\n");
314
#endif
julien.delange's avatar
julien.delange committed
315
316
317

      for (dev = 0; dev < __PO_HI_NB_DEVICES ; dev++)
      {
jdelange's avatar
jdelange committed
318
319
         __DEBUGMSG ("[DRIVER SOCKETS] Try to watch if it comes from device %d (socket=%d)\n", dev, __po_hi_c_sockets_read_sockets[dev]);
         if ( (__po_hi_c_sockets_read_sockets[dev] != -1 ) && FD_ISSET(__po_hi_c_sockets_read_sockets[dev], &selector))
julien.delange's avatar
julien.delange committed
320
321
         {
            __DEBUGMSG ("[DRIVER SOCKETS] Receive message from dev %d\n", dev);
jdelange's avatar
jdelange committed
322
323
324
325
326
327
#ifdef __PO_HI_USE_PROTOCOL_MYPROTOCOL_I
            {

               protocol_conf = __po_hi_transport_get_protocol_configuration (virtual_bus_myprotocol_i);

               int datareceived;
jdelange's avatar
jdelange committed
328
               len = recv (__po_hi_c_sockets_read_sockets[dev], &datareceived, sizeof (int), MSG_WAITALL);
jdelange's avatar
jdelange committed
329
330
331
332
               __DEBUGMSG ("[DRIVER SOCKETS] Message received len=%d\n",(int)len);
               if (len == 0)
               {
                  __DEBUGMSG ("[DRIVER SOCKETS] Zero size from device %d\n",dev);
jdelange's avatar
jdelange committed
333
                  __po_hi_c_sockets_read_sockets[dev] = -1;
jdelange's avatar
jdelange committed
334
335
                  continue;
               }
jdelange's avatar
jdelange committed
336
               protocol_conf->unmarshaller (&__po_hi_c_sockets_poller_received_request, &datareceived, len);
337
               __po_hi_sockets_poller_received_request.port = 1;
jdelange's avatar
jdelange committed
338
339
340
            }

#else
jdelange's avatar
jdelange committed
341
342
343
            memset (__po_hi_c_sockets_poller_msg.content, '\0', __PO_HI_MESSAGES_MAX_SIZE);
            len = recv (__po_hi_c_sockets_read_sockets[dev], __po_hi_c_sockets_poller_msg.content, __PO_HI_MESSAGES_MAX_SIZE, MSG_WAITALL);
            __po_hi_c_sockets_poller_msg.length = len;
julien.delange's avatar
julien.delange committed
344
345
            __DEBUGMSG ("[DRIVER SOCKETS] Message received len=%d\n",(int)len);

346
#ifdef __PO_HI_DEBUG
jdelange's avatar
jdelange committed
347
   __po_hi_messages_debug (&__po_hi_c_sockets_poller_msg);
348
349
350
#endif


julien.delange's avatar
julien.delange committed
351
352
            if (len == 0)
            {
353
               __DEBUGMSG ("[DRIVER SOCKETS] Zero size from device %d\n",dev);
jdelange's avatar
jdelange committed
354
               __po_hi_c_sockets_read_sockets[dev] = -1;
julien.delange's avatar
julien.delange committed
355
356
357
               continue;
            }

jdelange's avatar
jdelange committed
358
            __po_hi_unmarshall_request (&__po_hi_c_sockets_poller_received_request, &__po_hi_c_sockets_poller_msg);
jdelange's avatar
jdelange committed
359
#endif
360

jdelange's avatar
jdelange committed
361
            __po_hi_main_deliver (&__po_hi_c_sockets_poller_received_request);
julien.delange's avatar
julien.delange committed
362
363
364
365
366
367
368
         }
      }
   }  
   return NULL;
}


369
void __po_hi_driver_sockets_init (__po_hi_device_id dev_id)
370
{
jdelange's avatar
jdelange committed
371
372
373
374
   int                     ret;
   int                     reuse;
   struct sockaddr_in      sa;
   unsigned short          ip_port;
375

jdelange's avatar
jdelange committed
376
   __po_hi_c_ip_conf_t*    ipconf;
377
378
379
380
381
382
383
384
385
386
387
388
389
   __po_hi_device_id       dev;

   __po_hi_device_id          sent_id;
   struct hostent*            hostinfo;

   __po_hi_time_t             mytime;
   __po_hi_time_t             tmptime;

   char                       *tmp;
   __po_hi_time_t             current_time;
   int                        i;


julien.delange's avatar
julien.delange committed
390

jdelange's avatar
jdelange committed
391
   __po_hi_c_sockets_listen_socket = -1;
julien.delange's avatar
julien.delange committed
392

393
   for (dev = 0 ; dev < __PO_HI_NB_DEVICES ; dev++)
394
   {
395
396
      __po_hi_c_sockets_read_sockets[dev]   = -1;
      __po_hi_c_sockets_write_sockets[dev]  = -1;
397
398
   }

399
   ipconf = (__po_hi_c_ip_conf_t*)__po_hi_get_device_configuration (dev_id);
400
401
   ip_port = (int)ipconf->port;

jdelange's avatar
jdelange committed
402
   __DEBUGMSG ("My configuration, addr=%s, port=%d\n", ipconf->address, ip_port );
403

404
   /*
julien.delange's avatar
julien.delange committed
405
406
407
    * 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.
408
    */
409
   if (ip_port != 0)
410
   {
jdelange's avatar
jdelange committed
411
      __po_hi_c_sockets_listen_socket = socket (AF_INET, SOCK_STREAM, 0);
julien.delange's avatar
julien.delange committed
412

jdelange's avatar
jdelange committed
413

jdelange's avatar
jdelange committed
414
      if (__po_hi_c_sockets_listen_socket == -1 )
415
      {
julien.delange's avatar
julien.delange committed
416
#ifdef __PO_HI_DEBUG
417
         __DEBUGMSG ("Cannot create socket for device %d\n", dev_id);
julien.delange's avatar
julien.delange committed
418
419
420
#endif
         return;
      }
421

422
      __DEBUGMSG ("Socket created for addr=%s, port=%d, socket value=%d\n", ipconf->address, ip_port, __po_hi_c_sockets_listen_socket);
jdelange's avatar
jdelange committed
423

julien.delange's avatar
julien.delange committed
424
      reuse = 1;
jdelange's avatar
jdelange committed
425

jdelange's avatar
jdelange committed
426
      if (setsockopt (__po_hi_c_sockets_listen_socket, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof (reuse)))
jdelange's avatar
jdelange committed
427
428
429
      {
         __DEBUGMSG ("[DRIVER SOCKETS] Error while making the receiving socket reusable\n");
      }
julien.delange's avatar
julien.delange committed
430
431
432

      sa.sin_addr.s_addr = htonl (INADDR_ANY);   /* We listen on all adresses */
      sa.sin_family = AF_INET;                   
433
      sa.sin_port = htons (ip_port);   /* Port provided by the generated code */
julien.delange's avatar
julien.delange committed
434

jdelange's avatar
jdelange committed
435
      if( bind (__po_hi_c_sockets_listen_socket, ( struct sockaddr * ) &sa , sizeof( struct sockaddr_in ) ) < 0 )
julien.delange's avatar
julien.delange committed
436
      {
437
         __DEBUGMSG ("Unable to bind socket and port on socket %d\n", __po_hi_c_sockets_listen_socket);
julien.delange's avatar
julien.delange committed
438
439
      }

jdelange's avatar
jdelange committed
440
      if( listen (__po_hi_c_sockets_listen_socket, __PO_HI_NB_DEVICES) < 0 )
julien.delange's avatar
julien.delange committed
441
      {
jdelange's avatar
jdelange committed
442
         __DEBUGMSG ("Cannot listen on socket %d\n", __po_hi_c_sockets_listen_socket);
443
      }
julien.delange's avatar
julien.delange committed
444
445
446
447
448
449
450
451
452

      /* 
       * 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 
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
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
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
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
         (-1, 0,__PO_HI_MAX_PRIORITY, 0, (void* (*)(void))__po_hi_sockets_poller, &dev_id);
   }


   /*
    * 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 == dev_id)
      {
         continue;
      }

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

      ip_port = 0;

      ipconf = (__po_hi_c_ip_conf_t*) __po_hi_get_device_configuration (dev);
      ip_port = (unsigned short)ipconf->port;

      __DEBUGMSG ("[DRIVER SOCKETS] Configuration for device %d, port=%d\n", dev, ip_port);

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

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

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

         __DEBUGMSG ("[DRIVER SOCKETS] Socket for dev %d created, value=%d\n", dev, __po_hi_c_sockets_write_sockets[dev]);

         hostinfo = NULL;

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

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

         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 :
          *
          *
          * memcpy( (char*) &( sa.sin_addr ) , (char*)hostinfo->h_addr , hostinfo->h_length );
          */
         tmp = (char*) &(sa.sin_addr);
         for (i=0 ; i<hostinfo->h_length ; i++)
         {
            tmp[i] = hostinfo->h_addr[i];
         }


         /*
          * We try to connect on the remote host. We try every
          * second to connect on.
          */
         __PO_HI_SET_SOCKET_TIMEOUT(__po_hi_c_sockets_write_sockets[dev], 500000);
         ret = connect (__po_hi_c_sockets_write_sockets[dev], 
                        (struct sockaddr*) &sa ,
                        sizeof (struct sockaddr_in));

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

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

#else
         if (ret == 0)
         {
            __DEBUGMSG ("[DRIVER SOCKETS] Send my id (%d) to device %d through socket %d\n", dev_id, dev , __po_hi_c_sockets_write_sockets[dev]);

            sent_id = __po_hi_swap_byte (dev_id);
            if (write (__po_hi_c_sockets_write_sockets[dev], &sent_id, sizeof (__po_hi_device_id)) != sizeof (__po_hi_device_id))
            {
               __DEBUGMSG ("[DRIVER SOCKETS] Device %d cannot send his id\n", dev_id);
            }
            __DEBUGMSG ("[DRIVER SOCKETS] Connection established with device %d, socket=%d\n", dev, __po_hi_c_sockets_write_sockets[dev]);

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

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

         /*
          * 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 SOCKETS] Cannot connect on device %d, wait 500ms\n", dev);
         __po_hi_delay_until (&mytime);
      }
581
   }
582

jdelange's avatar
jdelange committed
583
   __DEBUGMSG ("[DRIVER SOCKETS] INITIALIZATION DONE\n");
584
}
585

586
587
#endif