int __po_hi_c_driver_eth_leon_sender (__po_hi_task_id task, __po_hi_port_t port) { 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) { #ifdef __PO_HI_DEBUG __DEBUGMSG (" [DRIVER SOCKETS] No data to write on port %d\n", port); #endif return __PO_HI_ERROR_TRANSPORT_SEND; } 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; } 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; return __PO_HI_ERROR_TRANSPORT_SEND; } break; } #endif default: { request->port = destination_port; __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); #ifdef __PO_HI_DEBUG __po_hi_messages_debug (&__po_hi_c_driver_eth_leon_sender_msg); #endif swap_pointer = (unsigned long*) &__po_hi_c_driver_eth_leon_sender_msg.content[0]; swap_value = *swap_pointer; *swap_pointer = __po_hi_swap_byte (swap_value); len = write (nodes[associated_device].socket, &(__po_hi_c_driver_eth_leon_sender_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; } request->port = __PO_HI_GQUEUE_INVALID_PORT; break; } } return __PO_HI_SUCCESS; }
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); } } } }
int __po_hi_driver_sockets_send (__po_hi_task_id task_id, __po_hi_port_t port) { int len; int size_to_write; #ifndef _WIN32 int optval = 0; socklen_t optlen = 0; #else char FAR optval = 0; int FAR optlen = 0; #endif __po_hi_device_id remote_device; __po_hi_device_id local_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; __po_hi_monitor_status_t device_status; 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); local_device = __po_hi_get_device_from_port (port); remote_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); __DEBUGMSG ("[DRIVER SOCKETS] Try to write from task=%d, port=%d, local_device=%d, remote device=%d, socket=%d\n", task_id, port, local_device, remote_device, __po_hi_c_sockets_write_sockets[remote_device]); if (request->port == -1) { __DEBUGMSG (" [DRIVER SOCKETS] No data to write on port %d\n", port); return __PO_HI_ERROR_TRANSPORT_SEND; } #if __PO_HI_MONITOR_ENABLED if (__po_hi_monitor_get_status_device (local_device, &device_status) != __PO_HI_SUCCESS) { __DEBUGMSG ("[DRIVER SOCKETS] Cannot get the status of device %d\n", local_device); return __PO_HI_ERROR_TRANSPORT_SEND; } if (device_status.status != po_hi_monitor_status_ok) { __DEBUGMSG ("[DRIVER SOCKETS] Device has a problem and is not able to process the request, aborting (device-id=%d, status= %d)\n", local_device, device_status); return __PO_HI_ERROR_TRANSPORT_SEND; } #endif if (__po_hi_c_sockets_write_sockets[remote_device] == -1 ) { #ifdef __PO_HI_DEBUG __DEBUGMSG (" [DRIVER SOCKETS] Invalid socket for port-id %d, device-id %d\n", destination_port, remote_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 (__po_hi_c_sockets_write_sockets[remote_device], SOL_SOCKET, SO_ERROR, &optval, &optlen) == -1) { __DEBUGMSG (" [error getsockopt() 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; } if (optval != 0) { __DEBUGMSG (" [error getsockopt() return code 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; } #ifndef _WIN32 /* * 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 (__po_hi_c_sockets_write_sockets[remote_device]); __po_hi_c_sockets_write_sockets[remote_device] = -1; return __PO_HI_ERROR_TRANSPORT_SEND; } #endif 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); #ifdef _WIN32 len = send (__po_hi_c_sockets_write_sockets[remote_device], &datawritten, size_to_write, 0); #else len = write (__po_hi_c_sockets_write_sockets[remote_device], &datawritten, size_to_write); #endif 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; } break; } #endif default: { request->port = destination_port; __po_hi_msg_reallocate (&__po_hi_c_sockets_send_msg); __po_hi_marshall_request (request, &__po_hi_c_sockets_send_msg); #ifdef __PO_HI_DEBUG __po_hi_messages_debug (&__po_hi_c_sockets_send_msg[remote_device]); #endif if (__po_hi_c_sockets_write_sockets[remote_device] != -1) { #ifdef _WIN32 len = send (__po_hi_c_sockets_write_sockets[remote_device], (char*) &(__po_hi_c_sockets_send_msg.content), size_to_write, 0); #else len = write (__po_hi_c_sockets_write_sockets[remote_device], &(__po_hi_c_sockets_send_msg.content), size_to_write); #endif if (len != size_to_write) { #if __PO_HI_MONITOR_ENABLED __po_hi_monitor_report_failure_device (remote_device, po_hi_monitor_failure_value); #endif __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; } } request->port = __PO_HI_GQUEUE_INVALID_PORT; break; } } return __PO_HI_SUCCESS; }
int __po_hi_transport_low_level_send (__po_hi_entity_t from, __po_hi_entity_t to, __po_hi_msg_t* msg) { __po_hi_node_t node; int len; int size_to_write; int optval = 0; socklen_t optlen = 0; node = entity_table[to]; if (nodes[node].socket == -1 ) { #ifdef __PO_HI_DEBUG __DEBUGMSG ("... failure ...\n"); #endif return __PO_HI_ERROR_TRANSPORT_SEND; } /* * After sending the entity identifier, we send the message which * contains the request. */ #ifdef __PO_HI_USE_GIOP size_to_write = msg->length; #else size_to_write = __PO_HI_MESSAGES_MAX_SIZE; #endif if (getsockopt (nodes[node].socket, SOL_SOCKET, SO_ERROR, &optval, &optlen) == -1) { __DEBUGMSG ("Error %s, %d\n", __FILE__, __LINE__); close (nodes[node].socket); nodes[node].socket = -1; return __PO_HI_ERROR_TRANSPORT_SEND; } if (optval != 0) { __DEBUGMSG ("Error %s, %d", __FILE__, __LINE__); close (nodes[node].socket); nodes[node].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 %s, %d", __FILE__, __LINE__); close (nodes[node].socket); nodes[node].socket = -1; return __PO_HI_ERROR_TRANSPORT_SEND; } #ifdef __PO_HI_DEBUG __po_hi_messages_debug (msg); #endif len = write (nodes[node].socket, &(msg->content), size_to_write); if (len != size_to_write) { __DEBUGMSG ("Error %s, %d", __FILE__, __LINE__); close (nodes[node].socket); nodes[node].socket = -1; return __PO_HI_ERROR_TRANSPORT_SEND; } __DEBUGMSG (" ... success ... \n"); return __PO_HI_SUCCESS; }
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; }