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