void __po_hi_c_driver_drvmgr_grspw_poller (const __po_hi_device_id dev_id) { int len; int ts; while (true) { __PO_HI_DEBUG_DEBUG ("[GRSPW SPACEWIRE] Poller task activated \n"); /* Prepare the message for reading */ __po_hi_msg_reallocate (&__po_hi_c_driver_drvmgr_grspw_poller_msg); /* Call GRSPW driver wrapper */ len = grspw_receiving (1, // XXX Hardcoded value for receiving &__po_hi_c_driver_drvmgr_grspw_poller_msg.content[0]); __PO_HI_DEBUG_DEBUG ("[GRSPW SPACEWIRE] Poller received a message, len=%d\n", len); if (len <= 0) { __PO_HI_DEBUG_CRITICAL ("[GRSPW SPACEWIRE] Error while reading\n"); } else { #if __PO_HI_DEBUG_LEVEL >= __PO_HI_DEBUG_LEVEL_DEBUG __PO_HI_DEBUG_DEBUG ("Message content: |0x"); for (ts = 0 ; ts < __PO_HI_MESSAGES_MAX_SIZE ; ts++) { __PO_HI_DEBUG_DEBUG ("%x", __po_hi_c_driver_drvmgr_grspw_poller_msg.content[ts]); } __PO_HI_DEBUG_DEBUG ("|\n"); #endif /* Unmarshall request and do the upcall to the receiving thread */ __po_hi_c_driver_drvmgr_grspw_poller_msg.length = __PO_HI_MESSAGES_MAX_SIZE; __po_hi_unmarshall_request (&__po_hi_c_driver_drvmgr_grspw_request, &__po_hi_c_driver_drvmgr_grspw_poller_msg); __PO_HI_DEBUG_DEBUG ("[GRSPW SPACEWIRE] Destination port: %d\n", __po_hi_c_driver_drvmgr_grspw_request.port); __po_hi_main_deliver (&__po_hi_c_driver_drvmgr_grspw_request); } } }
void user_ports_polling () { __po_hi_port_t portno; __po_hi_node_t mynode; __po_hi_node_t tmpnode; __po_hi_request_t request; __po_hi_port_kind_t pkind; int ret; mynode = __po_hi_transport_get_mynode (); for (portno = 0 ; portno < __PO_HI_NB_PORTS ; portno++) { pkind = __po_hi_transport_get_port_kind (portno); tmpnode = __po_hi_transport_get_node_from_entity (__po_hi_get_entity_from_global_port (portno)); ret = -1; if (tmpnode == mynode) { if (pkind == __PO_HI_IN_DATA_INTER_PROCESS) { ret = XM_read_sampling_message (__po_hi_transport_xtratum_get_port (portno), &request, sizeof (__po_hi_request_t), 0); } if (pkind == __PO_HI_IN_EVENT_DATA_INTER_PROCESS) { ret = XM_receive_queuing_message (__po_hi_transport_xtratum_get_port (portno), &request, sizeof (__po_hi_request_t)); } __DEBUGMSG ("[XTRATUM POLLER] Poll port %d, corresponding xtratum port = %d, return=%d\n", portno, __po_hi_transport_xtratum_get_port (portno), ret); if (ret > 0) { __po_hi_main_deliver (&request); } } } }
int __po_hi_transport_send (__po_hi_entity_t from, __po_hi_entity_t to, __po_hi_msg_t* msg) { if (entity_table[from] == entity_table[to]) { #ifdef __PO_HI_DEBUG __DEBUGMSG (" ... deliver locally ... \n"); #endif __po_hi_main_deliver(msg); return __PO_HI_SUCCESS; } else { #ifdef __PO_HI_USE_GIOP return __po_hi_giop_send (from, to, msg); #else return __po_hi_transport_low_level_send (from, to, msg); #endif } }
void __po_hi_c_driver_spacewire_rasta_poller (const __po_hi_device_id dev_id) { int len; int j; int ts; __PO_HI_DEBUG_DEBUG ("[RASTA SPACEWIRE] Hello, i'm the spacewire poller !\n"); __po_hi_msg_reallocate (&__po_hi_c_driver_spacewire_rasta_poller_msg); len = read (po_hi_c_driver_rasta_spacewire_fd[dev_id], &__po_hi_c_driver_spacewire_rasta_poller_msg.content[0], __PO_HI_MESSAGES_MAX_SIZE); __PO_HI_DEBUG_DEBUG ("[RASTA SPACEWIRE] Poller received a message, len=%d\n", len); if (len < 0) { __PO_HI_DEBUG_DEBUG ("[RASTA SPACEWIRE] Error while reading\n"); } else { __PO_HI_DEBUG_DEBUG ("Message content: |0x"); for (ts = 0 ; ts < __PO_HI_MESSAGES_MAX_SIZE ; ts++) { __PO_HI_DEBUG_DEBUG ("%x", __po_hi_c_driver_spacewire_rasta_poller_msg.content[ts]); } __PO_HI_DEBUG_DEBUG ("|\n"); __po_hi_c_driver_spacewire_rasta_poller_msg.length = __PO_HI_MESSAGES_MAX_SIZE; __po_hi_unmarshall_request (&__po_hi_c_driver_spacewire_rasta_request, &__po_hi_c_driver_spacewire_rasta_poller_msg); __PO_HI_DEBUG_DEBUG ("[RASTA SPACEWIRE] Destination port: %d\n", __po_hi_c_driver_spacewire_rasta_request.port); __po_hi_main_deliver (&__po_hi_c_driver_spacewire_rasta_request); } }
int __po_hi_transport_send (__po_hi_task_id id, __po_hi_port_t port) { __po_hi_msg_t msg; __po_hi_request_t* request; __po_hi_uint8_t ndest; __po_hi_uint8_t i; __po_hi_local_port_t local_port; __po_hi_port_t destination_port; __po_hi_entity_t destination_entity; local_port = __po_hi_get_local_port_from_global_port (port); request = __po_hi_gqueue_get_most_recent_value (id, local_port); if (request->port == -1) { __PO_HI_DEBUG_DEBUG ("Send output task %d, port %d : no value to send\n", id, port); return __PO_HI_SUCCESS; } ndest = __po_hi_gqueue_get_destinations_number (id, local_port); __PO_HI_DEBUG_DEBUG ("Send value, emitter task %d, emitter port %d, emitter entity %d, destination ports :\n", id, port, __po_hi_port_global_to_entity[port]); #if __PO_HI_DEBUG_LEVEL >= __PO_HI_DEBUG_LEVEL_INFO __DEBUGMSG ("SENT Value: |"); { int s; int i; unsigned int* tmp; tmp = (unsigned int*) &request->vars; s = sizeof (request->vars); for (i = 0 ; i < s ; i+=4) { __DEBUGMSG("%x", *tmp); tmp++; fflush (stdout); } } __DEBUGMSG ("|\n"); #endif for (i=0 ; i < __po_hi_gqueue_get_destinations_number (id, local_port) ; i++) { destination_port = __po_hi_gqueue_get_destination (id, local_port, i); destination_entity = __po_hi_get_entity_from_global_port (destination_port); __PO_HI_DEBUG_DEBUG ("\t%d (entity=%d)", destination_port, destination_entity); __po_hi_msg_reallocate (&msg); request->port = destination_port; if (__po_hi_transport_get_node_from_entity (__po_hi_get_entity_from_global_port (port)) == __po_hi_transport_get_node_from_entity (__po_hi_get_entity_from_global_port (destination_port))) { __PO_HI_DEBUG_DEBUG (" [deliver locally]\n"); __po_hi_main_deliver (request); } #ifndef XM3_RTEMS_MODE else { __PO_HI_DEBUG_DEBUG (" [deliver remotely]\n"); __po_hi_transport_call_sending_func_by_port (id, port); } #else /* for XTratuM */ else { __po_hi_port_kind_t pkind = __po_hi_transport_get_port_kind (port); int ret; ret = -1; if (pkind == __PO_HI_OUT_DATA_INTER_PROCESS) { ret = XM_write_sampling_message (__po_hi_xtratum_port[port], request, sizeof (__po_hi_request_t)); } if (pkind == __PO_HI_OUT_EVENT_DATA_INTER_PROCESS) { ret = XM_send_queuing_message (__po_hi_xtratum_port[port], request, sizeof (__po_hi_request_t)); } if (ret < 0) { __PO_HI_DEBUG_CRITICAL ("[GQUEUE] Cannot deliver the data using inter-partitions ports, return=%d\n", ret); } else { __PO_HI_DEBUG_DEBUG ("[GQUEUE] Data delivered using inter-partitions ports, return=%d\n", ret); } } #endif }
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; }