__PO_HI_MAIN_TYPE __PO_HI_MAIN_NAME ()
{
__po_hi_initialize();
__po_hi_create_periodic_task(node_a_pinger_k,__po_hi_milliseconds(5000),2,pinger_job);
__po_hi_wait_initialization();
__po_hi_wait_for_tasks();
return (__PO_HI_MAIN_RETURN);
}
void* pinger_job ()
{
simple_type data_source_request_var;
__po_hi_request_t data_source_request;
__po_hi_gqueue_init(node_a_pinger_k,__po_hi_pinger_nb_ports,__po_hi_pinger_queue,__po_hi_pinger_fifo_size,__po_hi_pinger_first,__po_hi_pinger_offsets,__po_hi_pinger_woffsets,__po_hi_pinger_n_dest,__po_hi_pinger_destinations,__po_hi_pinger_used_size,__po_hi_pinger_history,__po_hi_pinger_recent,__po_hi_pinger_empties,__po_hi_pinger_total_fifo_size);
__po_hi_wait_initialization();
while (1)
{
/* Call implementation*/
do_ping_spg(&(data_source_request_var));
/* Set the OUT port values*/
data_source_request.vars.pinger_global_data_source.pinger_global_data_source = data_source_request_var;
data_source_request.port = data_source_request_var;
__po_hi_gqueue_store_out(node_a_pinger_k,pinger_local_data_source,&(data_source_request));
/* Send the OUT ports*/
__po_hi_gqueue_send_output(node_a_pinger_k,pinger_global_data_source);
__po_hi_wait_for_next_period(node_a_pinger_k);
}
}
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++)
{
rnodes[dev].socket = -1;
}
/*
* Create a socket for each node that will communicate with us.
*/
for (dev = 0; dev < __PO_HI_NB_DEVICES ; dev++)
{
if (dev != leon_eth_device_id)
{
__PO_HI_SET_SOCKET_TIMEOUT(nodes[leon_eth_device_id].socket,500000);
established = 0;
while (established == 0)
{
__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);
sock = accept (nodes[leon_eth_device_id].socket, (struct sockaddr*) &sa, &socklen);
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);
#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;
}
}
}
__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");
#endif
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;
}
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;
}
#else
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;
__DEBUGMSG ("[DRIVER ETH] Message received len=%d\n",(int)len);
#ifdef __PO_HI_DEBUG
__po_hi_messages_debug (&msg);
#endif
if (len == 0)
{
__DEBUGMSG ("[DRIVER ETH] Zero size from device %d\n",dev);
rnodes[dev].socket = -1;
continue;
}
swap_pointer = (unsigned long*) &__po_hi_c_driver_eth_leon_poller_msg.content[0];
swap_value = *swap_pointer;
*swap_pointer = __po_hi_swap_byte (swap_value);
__po_hi_unmarshall_request (& __po_hi_c_driver_eth_leon_poller_received_request, &__po_hi_c_driver_eth_leon_poller_msg);
#endif
__po_hi_main_deliver (& __po_hi_c_driver_eth_leon_poller_received_request);
}
}
}
}
void* __po_hi_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;
#ifdef __PO_HI_USE_GIOP
__po_hi_msg_t decoded_msg;
__po_hi_uint32_t has_more;
#endif
__po_hi_node_t node;
__po_hi_node_t node_init;
struct sockaddr_in sa;
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++)
{
rnodes[node].socket = -1;
}
/*
* Create a socket for each node that will communicate with us.
*/
for (node = 0; node < __PO_HI_NB_NODES ; node++)
{
if (node != mynode )
{
sock = accept (nodes[mynode].socket, (struct sockaddr*) &sa, &socklen);
if (read (sock, &node_init, sizeof (__po_hi_node_t)) != sizeof (__po_hi_node_t))
{
#ifdef __PO_HI_DEBUG
__DEBUGMSG ("Cannot read node-id for socket %d\n", sock);
#endif
continue;
}
rnodes[node].socket = sock;
if (sock > max_socket )
{
max_socket = sock;
}
}
}
#ifdef __PO_HI_DEBUG
__DEBUGMSG ("Receiver initialization finished\n");
#endif
__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)
{
FD_ZERO( &selector );
for (node = 0; node < __PO_HI_NB_NODES ; node++)
{
if ( (node != mynode ) && ( rnodes[node].socket != -1 ) )
{
FD_SET( rnodes[node].socket , &selector );
}
}
if (select (max_socket + 1, &selector, NULL, NULL, NULL) == -1 )
{
#ifdef __PO_HI_DEBUG
__DEBUGMSG ("Error on select for node %d\n", mynode);
#endif
}
#ifdef __PO_HI_DEBUG
__DEBUGMSG ("Receive message\n");
#endif
for (node = 0; node < __PO_HI_NB_NODES ; node++)
{
if ( (rnodes[node].socket != -1 ) && FD_ISSET(rnodes[node].socket, &selector))
{
#ifdef __PO_HI_DEBUG
__DEBUGMSG ("Receive message from node %d\n", node);
#endif
#ifdef __PO_HI_USE_GIOP
/* Decoding GIOP request is implemented as a two-step automata
*
* First step is to decode the header,
* Second step is to decode the payload
*/
__DEBUGMSG ("Using GIOP as protocol stack\n");
__DEBUGMSG (" -> Step 1 decode header\n");
len = read (rnodes[node].socket, &(msg.content), sizeof (__po_hi_giop_msg_hdr_t));
msg.length = len;
has_more = 0;
if (__po_hi_giop_decode_msg (&msg, &decoded_msg, &has_more) == __PO_HI_SUCCESS )
{
#ifdef __PO_HI_DEBUG
__DEBUGMSG ("Message was decoded, has_more=%d\n", has_more);
#endif
__DEBUGMSG (" -> Step 2 decode message\n");
len = recv (rnodes[node].socket, &(msg.content), has_more, MSG_WAITALL);
/* Here, we wait for the _full_ message to come */
msg.length = len;
if (__po_hi_giop_decode_msg (&msg, &decoded_msg, &has_more) == __PO_HI_SUCCESS )
{
/* Put the data in the message queue */
__po_hi_main_deliver (&decoded_msg);
}
else
{
break;
}
}
else
{
break;
}
#else
__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_main_deliver (&msg);
#endif
__po_hi_msg_reallocate(&msg); /* re-initialize the message */
}
}
}
return NULL;
}
void* __po_hi_sockets_poller (__po_hi_device_id* dev_id_addr)
{
#ifdef _WIN32
int socklen;
#else
socklen_t socklen;
#endif
/* See ACCEPT (2) for details on initial value of socklen */
int 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;
int ret;
__po_hi_device_id dev_id;
__po_hi_uint32_t n_connected;
socklen = sizeof (struct sockaddr);
max_socket = 0; /* Used to compute the max socket number, useful for listen() call */
dev_id = __po_hi_c_sockets_device_id;
__DEBUGMSG ("Poller launched, device-id=%d\n", dev_id);
n_connected = 0;
for (dev = 0; dev < __PO_HI_NB_DEVICES ; dev++)
{
if (__po_hi_transport_share_bus (dev, dev_id) == 1)
{
n_connected++;
}
}
__DEBUGMSG ("Number of devices that share the bus=%d\n", n_connected);
/*
* Create a socket for each node that will communicate with us.
*/
for (dev = 0; dev < n_connected - 1; dev++)
{
established = 0;
while (established == 0)
{
__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]);
sock = accept (__po_hi_c_sockets_listen_socket, (struct sockaddr*) &sa, &socklen);
if (sock == -1)
{
continue;
}
__DEBUGMSG ("[DRIVER SOCKETS] accept() passed, waiting for device id %d\n", dev);
#ifndef __PO_HI_USE_PROTOCOL_MYPROTOCOL_I
dev_init = 0;
#ifdef _WIN32
ret = recv (sock, (char*) &dev_init, sizeof (__po_hi_device_id), 0);
#else
ret = read (sock, &dev_init, sizeof (__po_hi_device_id));
#endif
if (ret != sizeof (__po_hi_device_id))
{
established = 0;
__DEBUGMSG ("[DRIVER SOCKETS] Cannot read device-id for device %d, socket=%d, ret=%d, read size=%d, expected size=%d\n", dev, sock, ret, ret, sizeof (__po_hi_device_id));
}
else
{
dev_init = __po_hi_swap_byte (dev_init);
__DEBUGMSG ("[DRIVER SOCKETS] read device-id %d from socket=%d\n", dev_init, sock);
established = 1;
}
#else
established = 1;
dev_init = dev;
#endif
}
__po_hi_c_sockets_read_sockets[dev_init] = sock;
if (sock > max_socket )
{
max_socket = sock;
}
}
__DEBUGMSG ("[DRIVER SOCKETS] Poller initialization finished, waiting for other tasks\n");
__po_hi_wait_initialization ();
__DEBUGMSG ("[DRIVER SOCKETS] 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 != dev_id ) && ( __po_hi_c_sockets_read_sockets[dev] != -1 ) )
{
__DEBUGMSG ("[DRIVER SOCKETS] Add socket %d to the selector\n", __po_hi_c_sockets_read_sockets[dev]);
FD_SET( __po_hi_c_sockets_read_sockets[dev], &selector );
}
}
if (select (max_socket + 1, &selector, NULL, NULL, NULL) == -1 )
{
#ifdef __PO_HI_DEBUG
__DEBUGMSG ("[DRIVER SOCKETS] Error on select for node %d\n", __po_hi_mynode);
#endif
}
#ifdef __PO_HI_DEBUG
__DEBUGMSG ("[DRIVER SOCKETS] Receive message\n");
#endif
for (dev = 0; dev < __PO_HI_NB_DEVICES ; dev++)
{
__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))
{
__DEBUGMSG ("[DRIVER SOCKETS] 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 (__po_hi_c_sockets_read_sockets[dev], &datareceived, sizeof (int), MSG_WAITALL);
__DEBUGMSG ("[DRIVER SOCKETS] Message received len=%d\n",(int)len);
if (len == 0)
{
__DEBUGMSG ("[DRIVER SOCKETS] Zero size from device %d\n",dev);
__po_hi_c_sockets_read_sockets[dev] = -1;
continue;
}
protocol_conf->unmarshaller (&__po_hi_c_sockets_poller_received_request, &datareceived, len);
__po_hi_c_sockets_poller_received_request.port = 1;
}
#else
memset (__po_hi_c_sockets_poller_msg.content, '\0', __PO_HI_MESSAGES_MAX_SIZE);
#ifdef _WIN32
len = recv (__po_hi_c_sockets_read_sockets[dev], __po_hi_c_sockets_poller_msg.content, __PO_HI_MESSAGES_MAX_SIZE, 0);
#else
len = recv (__po_hi_c_sockets_read_sockets[dev], __po_hi_c_sockets_poller_msg.content, __PO_HI_MESSAGES_MAX_SIZE, MSG_WAITALL);
#endif
__po_hi_c_sockets_poller_msg.length = len;
__DEBUGMSG ("[DRIVER SOCKETS] Message received len=%d\n",(int)len);
#ifdef __PO_HI_DEBUG
__po_hi_messages_debug (&__po_hi_c_sockets_poller_msg);
#endif
if (len <= 0)
{
__DEBUGMSG ("[DRIVER SOCKETS] Invalid size (%d) from device %d\n",len, dev);
__po_hi_c_sockets_read_sockets[dev] = -1;
continue;
}
__po_hi_unmarshall_request (&__po_hi_c_sockets_poller_received_request, &__po_hi_c_sockets_poller_msg);
#endif
__DEBUGMSG ("[DRIVER SOCKETS] Delivering message to %d\n",__po_hi_c_sockets_poller_received_request.port );
__po_hi_main_deliver (&__po_hi_c_sockets_poller_received_request);
}
}
}
return NULL;
}
