po_hi_driver_sockets.c 17.7 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
 *
8
 * Copyright (C) 2010, 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
43

#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>

/*
44
 * This file (po_hi_sockets.c) provides function to handle
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
 * 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
63
 * send data to other nodes. A special socket if nodes[__po_hi_mynode] : this
64
65
66
67
68
 * 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
 */

julien.delange's avatar
julien.delange committed
69
70
__po_hi_inetnode_t nodes[__PO_HI_NB_DEVICES];
__po_hi_inetnode_t rnodes[__PO_HI_NB_DEVICES];
julien.delange's avatar
julien.delange committed
71

72
__po_hi_device_id socket_device_id;
73

74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
int __po_hi_driver_sockets_send (__po_hi_task_id task_id,
                                 __po_hi_port_t port)
{
   int                     len;
   int                     size_to_write;
   int                     optval = 0;
   socklen_t               optlen = 0;
   __po_hi_device_id       associated_device;
   __po_hi_local_port_t    local_port;
   __po_hi_request_t*      request;
   __po_hi_port_t          destination_port;
   __po_hi_msg_t           msg;

   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);

   associated_device = __po_hi_get_device_from_port (destination_port);

95
96
97
98
99
100
   if (request->port == -1)
   {

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

104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
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
161
162
163
164
165
   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;		
   }

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

   size_to_write = __PO_HI_MESSAGES_MAX_SIZE;

   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;		
   }

   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;		
   }

   /* 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__);
      close (nodes[associated_device].socket);
      nodes[associated_device].socket = -1;
      return __PO_HI_ERROR_TRANSPORT_SEND;
   }
   request->port = destination_port;
   __po_hi_msg_reallocate (&msg);
   __po_hi_marshall_request (request, &msg);

#ifdef __PO_HI_DEBUG
   __po_hi_messages_debug (&msg);
#endif

   len = write (nodes[associated_device].socket, &(msg.content), 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;		
   }

   return __PO_HI_SUCCESS;
}


166
void* __po_hi_sockets_poller (const __po_hi_device_id dev_id)
167
{
168
   (void) dev_id;
169
   __DEBUGMSG ("Poller launched, device-id=%d\n", socket_device_id);
julien.delange's avatar
julien.delange committed
170
171
172
173
174
175
176
   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;
julien.delange's avatar
julien.delange committed
177
   struct sockaddr_in sa;
178
   __po_hi_device_id  dev;
julien.delange's avatar
julien.delange committed
179
180
   __po_hi_node_t     dev_init;
   __po_hi_request_t  received_request;
julien.delange's avatar
julien.delange committed
181
   __po_hi_msg_t      msg;
182
   int                established = 0; 
julien.delange's avatar
julien.delange committed
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199

   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++)
   {
      rnodes[dev].socket = -1;
   }

   /*
    * Create a socket for each node that will communicate with us.
    */
   for (dev = 0; dev < __PO_HI_NB_DEVICES ; dev++)
   {
julien.delange's avatar
julien.delange committed
200
      if (dev != socket_device_id)
julien.delange's avatar
julien.delange committed
201
      {
202
         __DEBUGMSG ("[DRIVER SOCKETS] Poller waits for connection with device %d\n", dev);
julien.delange's avatar
julien.delange committed
203

204
         /*
205
         __PO_HI_SET_SOCKET_TIMEOUT(nodes[socket_device_id].socket,5);
206
         */
207
208
209
210

         established = 0;

         while (established == 0)
julien.delange's avatar
julien.delange committed
211
         {
212
213
214
215
216
217
218
219
220
221
222
223
224
            sock = accept (nodes[socket_device_id].socket, (struct sockaddr*) &sa, &socklen);

            __PO_HI_SET_SOCKET_TIMEOUT(sock,10);

            if (read (sock, &dev_init, sizeof (__po_hi_device_id)) != sizeof (__po_hi_device_id))
            {
               established = 0;
               __DEBUGMSG ("[DRIVER SOCKETS] Cannot read device-id for device %d, socket=%d\n", dev, sock);
            }
            else
            {
               established = 1;
            }
julien.delange's avatar
julien.delange committed
225
226
227
228
229
230
231
232
233
         }
         __DEBUGMSG ("[DRIVER SOCKETS] read device-id %d from socket=%d\n", dev_init, sock);
         rnodes[dev].socket = sock;
         if (sock > max_socket )
         {
            max_socket = sock;
         }	  
      }
   }
234
   __DEBUGMSG ("[DRIVER SOCKETS] Poller initialization finished, waiting for other tasks\n");
julien.delange's avatar
julien.delange committed
235
   __po_hi_wait_initialization ();
236
   __DEBUGMSG ("[DRIVER SOCKETS] Other tasks are initialized, let's start the polling !\n");
julien.delange's avatar
julien.delange committed
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255

   /*
    * 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 != socket_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
256
         __DEBUGMSG ("[DRIVER SOCKETS] Error on select for node %d\n", __po_hi_mynode);
julien.delange's avatar
julien.delange committed
257
258
259
260
#endif 
      }
#ifdef __PO_HI_DEBUG
      __DEBUGMSG ("[DRIVER SOCKETS] Receive message\n");
261
#endif
julien.delange's avatar
julien.delange committed
262
263
264

      for (dev = 0; dev < __PO_HI_NB_DEVICES ; dev++)
      {
265
         __DEBUGMSG ("[DRIVER SOCKETS] Try to watch if it comes from device %d (socket=%d)\n", dev, rnodes[dev].socket);
julien.delange's avatar
julien.delange committed
266
267
268
269
270
         if ( (rnodes[dev].socket != -1 ) && FD_ISSET(rnodes[dev].socket, &selector))
         {
            __DEBUGMSG ("[DRIVER SOCKETS] Receive message from dev %d\n", dev);
            memset (msg.content, '\0', __PO_HI_MESSAGES_MAX_SIZE);
            len = recv (rnodes[dev].socket, msg.content, __PO_HI_MESSAGES_MAX_SIZE, MSG_WAITALL);
271
            msg.length = len;
julien.delange's avatar
julien.delange committed
272
273
            __DEBUGMSG ("[DRIVER SOCKETS] Message received len=%d\n",(int)len);

274
275
276
277
278
#ifdef __PO_HI_DEBUG
   __po_hi_messages_debug (&msg);
#endif


julien.delange's avatar
julien.delange committed
279
280
            if (len == 0)
            {
281
282

               __DEBUGMSG ("[DRIVER SOCKETS] Zero size from device %d\n",dev);
julien.delange's avatar
julien.delange committed
283
284
285
286
287
288
               rnodes[dev].socket = -1;
               continue;
            }

            __po_hi_unmarshall_request (&received_request, &msg);

289

julien.delange's avatar
julien.delange committed
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
            __po_hi_main_deliver (&received_request);
         }
      }
   }  
   return NULL;
}


/*
 * Old receiver code that is based on PolyORB-HI-C for AADLv1
 * Would be considered as deprecated.
 */
void* __po_hi_sockets_receiver_task (void)
{
   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;
   __po_hi_msg_t      msg;
julien.delange's avatar
julien.delange committed
312
313
314
315
316
   __po_hi_node_t     node;
   __po_hi_node_t     node_init;
   __po_hi_request_t  received_request;
   struct sockaddr_in sa;

317

julien.delange's avatar
julien.delange committed
318
319
320
321
322
323
324
325
   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 (node = 0 ; node < __PO_HI_NB_NODES ; node++)
   {
326
      rnodes[node].socket = -1;
julien.delange's avatar
julien.delange committed
327
328
329
330
331
332
333
   }

   /*
    * Create a socket for each node that will communicate with us.
    */
   for (node = 0; node < __PO_HI_NB_NODES ; node++)
   {
334
      if (node != __po_hi_mynode )
julien.delange's avatar
julien.delange committed
335
      {
336
         sock = accept (nodes[__po_hi_mynode].socket, (struct sockaddr*) &sa, &socklen);
337
338
339
340
         if (sock == -1)
         {
            __DEBUGMSG ("accept() failed, return=%d\n", sock);
         }
julien.delange's avatar
julien.delange committed
341
342
343
344
345
346

         if (read (sock, &node_init, sizeof (__po_hi_node_t)) != sizeof (__po_hi_node_t))
         {
            __DEBUGMSG ("Cannot read node-id for socket %d\n", sock);
            continue;
         }
347

julien.delange's avatar
julien.delange committed
348
349
350
351
352
353
354
         rnodes[node].socket = sock;
         if (sock > max_socket )
         {
            max_socket = sock;
         }	  
      }
   }
355
#ifdef __PO_HI_DEBUG
julien.delange's avatar
julien.delange committed
356
   __DEBUGMSG ("Receiver initialization finished\n");
357
#endif
julien.delange's avatar
julien.delange committed
358
359
360
361
362
363
364
   __po_hi_wait_initialization ();

   /*
    * Then, listen and receive data on the socket, identify the node
    * which send the data and put it in its message queue
    */
   while (1)
365
   {
julien.delange's avatar
julien.delange committed
366
367
368
      FD_ZERO( &selector );
      for (node = 0; node < __PO_HI_NB_NODES ; node++)
      {
369
         if ( (node != __po_hi_mynode ) && ( rnodes[node].socket != -1 ) )
julien.delange's avatar
julien.delange committed
370
371
372
373
374
375
376
         {
            FD_SET( rnodes[node].socket , &selector );
         }
      }

      if (select (max_socket + 1, &selector, NULL, NULL, NULL) == -1 )
      {
377
#ifdef __PO_HI_DEBUG
378
         __DEBUGMSG ("Error on select for node %d\n", __po_hi_mynode);
379
#endif 
julien.delange's avatar
julien.delange committed
380
      }
381
#ifdef __PO_HI_DEBUG
julien.delange's avatar
julien.delange committed
382
      __DEBUGMSG ("Receive message\n");
383
#endif
julien.delange's avatar
julien.delange committed
384
385
386

      for (node = 0; node < __PO_HI_NB_NODES ; node++)
      {
387
         if ( (rnodes[node].socket != -1 ) && FD_ISSET(rnodes[node].socket, &selector))
julien.delange's avatar
julien.delange committed
388
         {
389
#ifdef __PO_HI_DEBUG
julien.delange's avatar
julien.delange committed
390
            __DEBUGMSG ("Receive message from node %d\n", node);
391
#endif
julien.delange's avatar
julien.delange committed
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407

            __DEBUGMSG ("Using raw protocol stack\n");
            len = recv (rnodes[node].socket, &(msg.content), __PO_HI_MESSAGES_MAX_SIZE, MSG_WAITALL);
            msg.length = len;
            if (len != __PO_HI_MESSAGES_MAX_SIZE )
            {
               __DEBUGMSG ("ERROR, %u %d", (unsigned int) len, __PO_HI_MESSAGES_MAX_SIZE);
               close (rnodes[node].socket);
               rnodes[node].socket = -1;
               continue;
            }
            __DEBUGMSG ("Message delivered");

            __po_hi_unmarshall_request (&received_request, &msg);

            __po_hi_main_deliver (&received_request);
julien.delange's avatar
julien.delange committed
408

julien.delange's avatar
julien.delange committed
409
410
411
            __po_hi_msg_reallocate(&msg);        /* re-initialize the message */
         }
      }
412
   }  
julien.delange's avatar
julien.delange committed
413
   return NULL;
414
415
}

julien.delange's avatar
julien.delange committed
416
void __po_hi_driver_sockets_init (__po_hi_device_id id)
417
{
julien.delange's avatar
julien.delange committed
418
419
420
421
   int                i;
   int                ret;
   int                reuse;
   char               *tmp;
422
   __po_hi_uint16_t   dev;
julien.delange's avatar
julien.delange committed
423
   __po_hi_time_t     mytime;
424
425
   __po_hi_time_t     tmptime;
   __po_hi_time_t     current_time;
426
   struct sockaddr_in sa;
julien.delange's avatar
julien.delange committed
427
   struct hostent*    hostinfo;
428
429


430
431
432
433
   __po_hi_c_ip_conf_t* ipconf;
   char ip_addr[16];
   int ip_port;
   int node;
julien.delange's avatar
julien.delange committed
434

435
   socket_device_id = id;
julien.delange's avatar
julien.delange committed
436

437
   for (node = 0 ; node < __PO_HI_NB_DEVICES ; node++)
438
   {
julien.delange's avatar
julien.delange committed
439
      nodes[node].socket = -1;
440
441
   }

442
443
444
445
446
   ipconf = (__po_hi_c_ip_conf_t*)__po_hi_get_device_configuration (id);
   ip_port = (int)ipconf->port;

   __DEBUGMSG ("My configuration, addr=%s, port=%lld\n", ipconf->address, ipconf->port );

447
   /*
julien.delange's avatar
julien.delange committed
448
449
450
    * 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.
451
    */
452
   if (ip_port != 0)
453
   {
454
      nodes[id].socket = socket (AF_INET, SOCK_STREAM, 0);
julien.delange's avatar
julien.delange committed
455

456
      if (nodes[id].socket == -1 )
457
      {
julien.delange's avatar
julien.delange committed
458
#ifdef __PO_HI_DEBUG
459
         __DEBUGMSG ("Cannot create socket for device %d\n", id);
julien.delange's avatar
julien.delange committed
460
461
462
#endif
         return;
      }
463

julien.delange's avatar
julien.delange committed
464
      reuse = 1;
465
      setsockopt (nodes[id].socket, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof (reuse));
julien.delange's avatar
julien.delange committed
466
467
468

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

471
      if( bind( nodes[id].socket , ( struct sockaddr * ) &sa , sizeof( struct sockaddr_in ) ) < 0 )
julien.delange's avatar
julien.delange committed
472
473
      {
#ifdef __PO_HI_DEBUG
474
         __DEBUGMSG ("Unable to bind socket and port on socket %d\n", nodes[id].socket);
julien.delange's avatar
julien.delange committed
475
476
477
#endif
      }

478
      if( listen( nodes[id].socket , __PO_HI_NB_ENTITIES ) < 0 )
julien.delange's avatar
julien.delange committed
479
480
      {
#ifdef __PO_HI_DEBUG
481
         __DEBUGMSG ("Cannot listen on socket %d\n", nodes[id].socket);
julien.delange's avatar
julien.delange committed
482
#endif
483
      }
julien.delange's avatar
julien.delange committed
484
485
486
487
488
489
490
491
492
493

      /* 
       * 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 
494
         (-1, 0,__PO_HI_MAX_PRIORITY, 0, __po_hi_sockets_poller);
495
496
497
   }

   /*
julien.delange's avatar
julien.delange committed
498
499
    * 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.
500
    */
501
   for (dev = 0 ; dev < __PO_HI_NB_DEVICES ; dev++ )
502
   {
503
      if (dev == id)
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
         continue;
      }

      __DEBUGMSG ("[DRIVER SOCKETS] 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);
      ip_port = (int)ipconf->port;

      __DEBUGMSG ("[DRIVER SOCKETS] Configuration for device %d, addr=%s, port=%d\n", dev, ipconf->address, ip_port);

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

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

         if (nodes[dev].socket == -1 )
529
         {
530
531
532
            __DEBUGMSG ("[DRIVER SOCKETS] Socket for dev %d is not created\n", dev);
            return;
         }
533

534
         __DEBUGMSG ("[DRIVER SOCKETS] Socket for dev %d created, value=%d\n", dev, nodes[dev].socket);
julien.delange's avatar
julien.delange committed
535

536
         hostinfo = gethostbyname ((char*)ipconf->address);
julien.delange's avatar
julien.delange committed
537

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

543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
         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];
         }
561

562
563
564
565
566
         /*
          * We try to connect on the remote host. We try every
          * second to connect on.
         __PO_HI_SET_SOCKET_TIMEOUT(nodes[dev].socket,5);
          */
567

568
569
570
         ret = connect (nodes[dev].socket, 
                        (struct sockaddr*) &sa ,
                        sizeof (struct sockaddr_in));
julien.delange's avatar
julien.delange committed
571

572
573
         if (ret == 0)
         {
julien.delange's avatar
julien.delange committed
574

575
576
            __DEBUGMSG ("[DRIVER SOCKETS] Send my id (%d)\n", id);
            if (write (nodes[dev].socket, &id, sizeof (__po_hi_device_id)) != sizeof (__po_hi_device_id))
julien.delange's avatar
julien.delange committed
577
            {
578
               __DEBUGMSG ("[DRIVER SOCKETS] Device %d cannot send his id\n", id);
julien.delange's avatar
julien.delange committed
579
            }
580
581
582
583
584
585
586
            __DEBUGMSG ("[DRIVER SOCKETS] Connection established with device %d, socket=%d\n", dev, nodes[dev].socket);
            break;
         }
         else
         {
            __DEBUGMSG ("connect() failed, return=%d\n", ret);
         }
julien.delange's avatar
julien.delange committed
587

588
589
590
         if (close (nodes[dev].socket))
         {
            __DEBUGMSG ("[DRIVER SOCKETS] Cannot close socket %d\n", nodes[dev].socket);
591
         }
julien.delange's avatar
julien.delange committed
592

593
594
595
596
         /*
          * We wait 500ms each time we try to connect on the
          * remote host
          */
597

598
599
600
         __po_hi_get_time (&current_time);
         __po_hi_milliseconds (&tmptime, 500);
         __po_hi_add_times (&mytime, &current_time, &tmptime);
601
         __DEBUGMSG ("[DRIVER SOCKETS] Cannot connect on device %d, wait 500ms\n", dev);
602
         __po_hi_delay_until (&mytime);
603
604
      }
   }
605

606
}
607

608
609
#endif