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);
}
}
}
int __po_hi_c_driver_drvmgr_grspw_sender
(const __po_hi_task_id task_id,
const __po_hi_port_t port)
{
int len = -1;
int i;
int ts;
__po_hi_c_spacewire_conf_t* sender_conf;
__po_hi_c_spacewire_conf_t* receiver_conf;
__po_hi_local_port_t local_port;
__po_hi_request_t* request;
__po_hi_port_t destination_port;
__po_hi_device_id dev_id;
struct route_entry route; /* Routing table */
dev_id = __po_hi_get_device_from_port (port);
if (dev_id == invalid_device_id) {
__PO_HI_DEBUG_DEBUG ("[GRSPW] Invalid device id for sending\n");
return __PO_HI_UNAVAILABLE;
}
local_port = __po_hi_get_local_port_from_global_port (port);
request = __po_hi_gqueue_get_most_recent_value (task_id, local_port);
if (request->port == -1) {
__PO_HI_DEBUG_DEBUG
("[GRSPW SPACEWIRE] Send output task %d, port %d : no value to send\n",
task_id, port);
return __PO_HI_SUCCESS;
}
destination_port = __po_hi_gqueue_get_destination (task_id, local_port, 0);
__po_hi_msg_reallocate (&__po_hi_c_driver_drvmgr_grspw_sender_msg);
request->port = destination_port;
sender_conf = (__po_hi_c_spacewire_conf_t*)
__po_hi_get_device_configuration (dev_id);
receiver_conf = (__po_hi_c_spacewire_conf_t*)
__po_hi_get_device_configuration
(__po_hi_get_device_from_port (destination_port));
__po_hi_marshall_request
(request, &__po_hi_c_driver_drvmgr_grspw_sender_msg);
len = -1;
memset(&route, 0, sizeof(route));
route.dstadr[0]= 1;
#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_sender_msg.content[ts]);
}
__PO_HI_DEBUG_DEBUG ("|\n");
#endif
len = grspw_sending
(0, // XXX hardcoded
&route,
__po_hi_c_driver_drvmgr_grspw_sender_msg.content,
__PO_HI_MESSAGES_MAX_SIZE);
if (len < 0) {
__PO_HI_DEBUG_CRITICAL (" [GRSPW SPACEWIRE] failed !\n");
} else
if((0 <= len)&(len < __PO_HI_MESSAGES_MAX_SIZE)) {
__PO_HI_DEBUG_CRITICAL (" [GRSPW SPACEWIRE] Unable write !\n");
} else {
__PO_HI_DEBUG_DEBUG (" [GRSPW SPACEWIRE] Send OK !\n");
}
request->port = __PO_HI_GQUEUE_INVALID_PORT;
return __PO_HI_SUCCESS;
}
int __po_hi_gqueue_get_value (__po_hi_task_id id,
__po_hi_local_port_t port,
__po_hi_request_t* request)
{
__po_hi_request_t* ptr;
__PO_HI_DEBUG_DEBUG("before get_value for task-id %d , port = %d, offset = %d, woffset = %d, history_offset = %d, history_woffset = %d, port size = %d , fifo size = %d, gqueues_id adress = %d, \n\n", id, port, __po_hi_gqueues_offsets[id][port], __po_hi_gqueues_woffsets[id][port],__po_hi_gqueues_global_history_offset[id],__po_hi_gqueues_global_history_woffset[id], __po_hi_gqueues_sizes[id][port], __po_hi_gqueues_total_fifo_size[id], __po_hi_gqueues[id]);
ptr = &__po_hi_gqueues_most_recent_values[id][port];
/* Locking only the mutex of the semaphore */
int result = __po_hi_sem_mutex_wait_gqueue(__po_hi_gqueues_semaphores,id);
__DEBUGMSG("GQUEUE_SEM_MUTEX_WAIT %d %d\n", id, result);
assert(result == __PO_HI_SUCCESS);
/*
* If the port is an OUTPUT, with no value queued, the function returns
* nothing.
*/
if (__po_hi_gqueue_get_port_size(id,port) == -2)
{
__PO_HI_DEBUG_CRITICAL ("[GQUEUE] OUTPUT PORT, REQUEST NOT SET UP, task-id=%d, port=%d\n", id, port);
__DEBUGMSG("THE PORT IS AN OUTPUT, REQUEST NOT SET UP");
/* Releasing only the mutex of the semaphore*/
int rel = __po_hi_sem_mutex_release_gqueue(__po_hi_gqueues_semaphores,id);
__DEBUGMSG("GQUEUE_SEM_MUTEX_RELEASE %d %d\n", id, rel);
assert(rel == __PO_HI_SUCCESS);
return __PO_HI_INVALID;
}
/*
* If the port is an event port, with no value queued, then we block
* the thread.
*/
/* Empty port case 1 : NO FIFO INDATA */
if (__po_hi_gqueue_get_port_size(id,port) != __PO_HI_GQUEUE_FIFO_INDATA)
{
while (__po_hi_gqueues_port_is_empty[id][port] == 1)
{
/* Telling the semaphore to wait with putting its condvar on wait mode */
int res_sem = __po_hi_sem_wait_gqueue(__po_hi_gqueues_semaphores,id);
__DEBUGMSG("GQUEUE_SEM_WAIT %d %d\n", id, result);
assert(res_sem == __PO_HI_SUCCESS);
}
}
/* Empty port case 2 : FIFO INDATA */
if ((__po_hi_gqueue_get_port_size(id,port) == __PO_HI_GQUEUE_FIFO_INDATA) && (__po_hi_gqueue_used_size(id,port) == 0))
{
memcpy (request, ptr, sizeof (__po_hi_request_t));
//update_runtime (id, port, ptr);
}
else
{
/* The program ensures to read the information at the right place in the buffer.
* The right first offset has to be applied so that the right port is chosen.
* The right offset (read_offset) has to be applied not to erase fresh information.
*/
ptr = (__po_hi_gqueues[id]) + __po_hi_gqueues_first[id][port] + __po_hi_gqueues_offsets[id][port];
__PO_HI_DEBUG_DEBUG("Get_value if port not empty first + offsets = %d, gqueue_id adress = %d, first = %d, ptr (adress + first +offset) = %d, \n\n", __po_hi_gqueues_first[id][port] + __po_hi_gqueues_offsets[id][port],__po_hi_gqueues[id], __po_hi_gqueues_first[id][port], ptr);
memcpy (request, ptr, sizeof (__po_hi_request_t));
}
#if defined (MONITORING)
record_event(ANY, GET_VALUE, id, invalid_port_t, invalid_port_t, port, invalid_local_port_t , request);
#endif
__PO_HI_DEBUG_INFO ("[GQUEUE] Task %d get a value on port %d\n", id, port);
/* Releasing only the mutex of the semaphore*/
int res = __po_hi_sem_mutex_release_gqueue(__po_hi_gqueues_semaphores,id);
__DEBUGMSG("GQUEUE_SEM_MUTEX_RELEASE %d %d\n", id, res);
assert(res == __PO_HI_SUCCESS);
__PO_HI_DEBUG_DEBUG("After get_value for task-id %d , port = %d, offset = %d, woffset = %d, history_offset = %d, history_woffset = %d, port size = %d, fifo size = %d, gqueues adress = %d \n\n", id, port, __po_hi_gqueues_offsets[id][port], __po_hi_gqueues_woffsets[id][port],__po_hi_gqueues_global_history_offset[id],__po_hi_gqueues_global_history_woffset[id], __po_hi_gqueues_sizes[id][port], __po_hi_gqueues_total_fifo_size[id], __po_hi_gqueues[id]);
return __PO_HI_SUCCESS;
}
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_gqueue_init (__po_hi_task_id id,
__po_hi_uint8_t nb_ports,
__po_hi_port_t queue[],
__po_hi_int8_t sizes[],
__po_hi_uint8_t first[],
__po_hi_uint8_t offsets[],
__po_hi_uint8_t woffsets[],
__po_hi_uint8_t n_dest[],
__po_hi_port_t* destinations[],
__po_hi_uint8_t used_size[],
__po_hi_local_port_t history[],
__po_hi_request_t recent[],
__po_hi_uint8_t empties[],
__po_hi_uint16_t total_fifo_size)
{
__po_hi_uint8_t tmp;
__po_hi_uint16_t off;
__po_hi_request_t* request;
int err;
#if defined (RTEMS_PURE)
rtems_status_code ret;
#elif defined (XENO_NATIVE)
int ret;
#endif
__po_hi_gqueues_global_history_woffset[id] = 0;
__po_hi_gqueues_global_history_offset[id] = 0;
__po_hi_gqueues_n_empty[id] = nb_ports;
__po_hi_gqueues[id] = queue;
__po_hi_gqueues_most_recent_values[id] = recent;
__po_hi_gqueues_global_history[id] = history;
__po_hi_gqueues_woffsets[id] = woffsets;
__po_hi_gqueues_port_is_empty[id] = empties;
__po_hi_gqueues_nb_ports[id] = nb_ports;
__po_hi_gqueues_sizes[id] = sizes;
__po_hi_gqueues_first[id] = first;
__po_hi_gqueues_used_size[id] = used_size;
__po_hi_gqueues_offsets[id] = offsets;
__po_hi_gqueues_n_destinations[id] = n_dest;
__po_hi_gqueues_destinations[id] = destinations;
__po_hi_gqueues_total_fifo_size[id] = total_fifo_size;
__po_hi_gqueues_queue_is_empty[id] = 1;
#if defined (RTEMS_POSIX) || defined (POSIX) || defined (XENO_POSIX)
err = pthread_mutexattr_init (&__po_hi_gqueues_mutexes_attr[id]);
__DEBUGMSG("MUTEX_INIT %d %d\n", id, err);
err = pthread_condattr_init (&__po_hi_gqueues_conds_attr[id]);
__DEBUGMSG("MUTEX_INIT %d %d\n", id, err);
#if defined (POSIX) || defined (XENO_POSIX)
// XXX disabled for OS X
#ifndef __MACH__ // OS X bugs on this attribute
err = pthread_mutexattr_setpshared(&__po_hi_gqueues_mutexes_attr[id],PTHREAD_PROCESS_SHARED);
#endif
__DEBUGMSG("MUTEX_INIT %d\n", err);
#endif
err = pthread_mutex_init (&__po_hi_gqueues_mutexes[id], &__po_hi_gqueues_mutexes_attr[id]);
__DEBUGMSG("MUTEX_INIT %d %d\n", id, err);
err = pthread_cond_init (&__po_hi_gqueues_conds[id], &__po_hi_gqueues_conds_attr[id]);
__DEBUGMSG("COND_INIT %d %d\n", id, err);
#endif
#ifdef RTEMS_PURE
__PO_HI_DEBUG_INFO ("[GQUEUE] Create semaphore for queue of task %d\n", id);
ret = rtems_semaphore_create (rtems_build_name ('G', 'S', 'E' , 'A' + (char) id), 1, RTEMS_BINARY_SEMAPHORE, __PO_HI_DEFAULT_PRIORITY, &(__po_hi_gqueues_semaphores[id]));
if (ret != RTEMS_SUCCESSFUL)
{
__PO_HI_DEBUG_CRITICAL ("[GQUEUE] Cannot create semaphore, error code=%d\n", ret);
}
__PO_HI_DEBUG_INFO ("[GQUEUE] Create barrier for queue of task %d\n", id);
ret = rtems_barrier_create (rtems_build_name ('G', 'S', 'I' , 'A' + (char) id),RTEMS_BARRIER_AUTOMATIC_RELEASE , 10, &(__po_hi_gqueues_barriers[id]));
if (ret != RTEMS_SUCCESSFUL)
{
__PO_HI_DEBUG_CRITICAL ("[GQUEUE] Cannot create barrier, error code=%d\n", ret);
}
#endif
#ifdef XENO_NATIVE
ret = rt_mutex_create (&__po_hi_gqueues_mutexes[id], NULL);
if (ret != 0)
{
__PO_HI_DEBUG_CRITICAL ("[GQUEUE] Cannot create mutex code=%d\n", ret);
}
ret = rt_cond_create (&__po_hi_gqueues_conds[id], NULL);
if (ret != 0)
{
__PO_HI_DEBUG_CRITICAL ("[GQUEUE] Cannot create cond code=%d\n", ret);
}
#endif
#ifdef _WIN32
__po_hi_gqueues_events[id] = CreateEvent (NULL, FALSE, FALSE, NULL);
if (__po_hi_gqueues_events[id] == NULL)
{
__PO_HI_DEBUG_CRITICAL ("CreateEvent failed (%d)\n", GetLastError());
return;
}
InitializeCriticalSection (&__po_hi_gqueues_cs[id]);
#endif
off = 0;
for (tmp=0;tmp<nb_ports;tmp++)
{
__po_hi_gqueues_used_size[id][tmp] = 0;
if ( (sizes[tmp] != __PO_HI_GQUEUE_FIFO_INDATA)
&& (sizes[tmp] != __PO_HI_GQUEUE_FIFO_OUT))
{
__po_hi_gqueues_first[id][tmp]=off;
off += __po_hi_gqueues_sizes[id][tmp];
__po_hi_gqueues_offsets[id][tmp] = 0;
__po_hi_gqueues_woffsets[id][tmp] = 0;
__po_hi_gqueues_port_is_empty[id][tmp] = 1;
}
/* Set invalid all recent values */
request = (__po_hi_request_t*)&__po_hi_gqueues_most_recent_values[id][tmp];
request->port = __PO_HI_GQUEUE_INVALID_PORT;
}
#ifdef __PO_HI_DEBUG
__DEBUGMSG("Initialize global queue for task-id %d ... ", id);
for (tmp=0;tmp<nb_ports;tmp++)
{
__DEBUGMSG("port %d (used_size=%d,first=%d) ",
tmp,
__po_hi_gqueues_used_size[id][tmp],
__po_hi_gqueues_first[id][tmp]);
}
__DEBUGMSG(" ... done\n");
#endif
}
__po_hi_port_id_t __po_hi_gqueue_store_in (__po_hi_task_id id,
__po_hi_local_port_t port,
__po_hi_request_t* request)
{
#ifdef __PO_HI_GQUEUE_ASSERTIONS
__po_hi_port_id_t init_woffset = __po_hi_gqueues_woffsets[id][port];
__po_hi_uint32_t init_history_woffset = __po_hi_gqueues_global_history_woffset[id];
__po_hi_port_id_t init_used_size = __po_hi_gqueues_used_size[id][port];
__po_hi_port_id_t is_empty = __po_hi_gqueues_port_is_empty[id][port];
__po_hi_port_id_t nb_empty = __po_hi_gqueues_n_empty[id];
#endif
__po_hi_request_t* ptr;
__po_hi_request_t* tmp;
ptr = &__po_hi_gqueues_most_recent_values[id][port];
#ifdef __PO_HI_DEBUG
if (ptr == NULL)
{
__DEBUGMSG ("__po_hi_gqueue_store_in : NULL POINTER\n");
}
#endif
/* Locking only a mutex */
__PO_HI_DEBUG_DEBUG ("\nWaiting on Store_in on task %d, port = %d, size of port = %d\n", id, port,__po_hi_gqueue_get_port_size(id, port));
int result = __po_hi_sem_mutex_wait_gqueue(__po_hi_gqueues_semaphores,id);
__DEBUGMSG("GQUEUE_SEM_MUTEX_WAIT on task %d result = %d\n", id, result);
assert(result == __PO_HI_SUCCESS);
if (__po_hi_gqueue_get_port_size(id,port) == __PO_HI_GQUEUE_FIFO_INDATA)
{
memcpy(ptr,request,sizeof(*request));
__PO_HI_DEBUG_INFO ("[GQUEUE] BEWARE, for a FIFO_INDATA port, the used_size is always at 0 (not augmented in a store_in) task-id=%d, port=%d\n", id, port);
}
else
{
__DEBUGMSG ("[GQUEUE] Received message for task %d, port %d\n", id, port);
if (__po_hi_gqueue_used_size(id,port) == __po_hi_gqueue_get_port_size(id,port))
{
/* Releasing only a mutex */
int res = __po_hi_sem_mutex_release_gqueue(__po_hi_gqueues_semaphores,id);
__DEBUGMSG("GQUEUE_SEM_MTUEX_RELEASE %d %d\n", id, res);
assert(res == __PO_HI_SUCCESS);
__PO_HI_DEBUG_CRITICAL ("[GQUEUE] QUEUE FULL, task-id=%d, port=%d\n", id, port);
__DEBUGMSG ("[GQUEUE] Semaphore released (id=%d)\n", id);
return __PO_HI_ERROR_QUEUE_FULL;
}
__PO_HI_DEBUG_DEBUG("\nBefore store_in for task-id %d , port %d, offset = %d, woffset = %d, history_offset = %d, history_woffset = %d, port size = %d, fifo size = %d, gqueue id adress = %d,\n\n", id, port, __po_hi_gqueues_offsets[id][port], __po_hi_gqueues_woffsets[id][port],__po_hi_gqueues_global_history_offset[id],__po_hi_gqueues_global_history_woffset[id], __po_hi_gqueues_sizes[id][port], __po_hi_gqueues_total_fifo_size[id], __po_hi_gqueues[id]);
/* The program ensures to write the information at the right place in the buffer.
*
* The right first offset has to be applied so that the right
* port is chosen. The right woffset (writing_offset) has to be
* applied not to erase fresh information.
*/
__po_hi_uint32_t size;
tmp = __po_hi_gqueues[id];
size = __po_hi_gqueues_woffsets[id][port] + __po_hi_gqueues_first[id][port];
tmp = tmp + size;
__PO_HI_DEBUG_DEBUG(" Store_in first + woffsets = %d, first = %d, gqueue_id adress = %d, tmp (adress + woffset + first)= %d,\n\n", __po_hi_gqueues_first[id][port] + __po_hi_gqueues_woffsets[id][port],__po_hi_gqueues_first[id][port],__po_hi_gqueues[id], tmp);
memcpy (tmp , request, sizeof (__po_hi_request_t));
__po_hi_gqueues_woffsets[id][port] = (__po_hi_gqueues_woffsets[id][port] + 1 ) % __po_hi_gqueues_sizes[id][port];
__PO_HI_DEBUG_DEBUG ("\nBefore used_size ++, Store_in for task = %d, __po_hi_gqueues_used_size[id][port] = %d\n", id, __po_hi_gqueues_used_size[id][port]);
__po_hi_gqueues_used_size[id][port]++;
__PO_HI_DEBUG_DEBUG ("\nAfter used_size ++ , Store_in for task = %d, __po_hi_gqueues_used_size[id][port] = %d\n",id, __po_hi_gqueues_used_size[id][port]);
__PO_HI_INSTRUMENTATION_VCD_WRITE("r%d p%d.%d\n", __po_hi_gqueue_used_size(id,port), id, port);
/* The port where information has been written is stored */
__po_hi_gqueues_global_history[id][__po_hi_gqueues_global_history_woffset[id]] = port;
__po_hi_gqueues_global_history_woffset[id] = (__po_hi_gqueues_global_history_woffset[id] + 1 ) % __po_hi_gqueues_total_fifo_size[id];
if (__po_hi_gqueues_port_is_empty[id][port] == 1)
{
__po_hi_gqueues_port_is_empty[id][port] = 0;
__po_hi_gqueues_n_empty[id]--;
}
__po_hi_gqueues_queue_is_empty[id] = 0;
}
__PO_HI_DEBUG_DEBUG("\nAfter store_in for task-id %d , port %d, offset = %d, woffset = %d, history_offset = %d, history_woffset = %d, port size = %d, fifo size = %d, gqueue_id adress= %d, \n\n", id, port, __po_hi_gqueues_offsets[id][port], __po_hi_gqueues_woffsets[id][port],__po_hi_gqueues_global_history_offset[id],__po_hi_gqueues_global_history_woffset[id], __po_hi_gqueues_sizes[id][port], __po_hi_gqueues_total_fifo_size[id], __po_hi_gqueues[id]);
/* Releasing a complete semaphore */
int rel = __po_hi_sem_release_gqueue(__po_hi_gqueues_semaphores,id);
__DEBUGMSG("GQUEUE_SEM_RELEASE %d %d\n", id, rel);
assert(rel == __PO_HI_SUCCESS);
__DEBUGMSG ("[GQUEUE] store_in completed\n");
#ifdef __PO_HI_GQUEUE_ASSERTIONS
/* The port length is superior to 1 */
if ((__po_hi_gqueue_get_port_size(id,port) != __PO_HI_GQUEUE_FIFO_INDATA)&&(init_used_size != __po_hi_gqueue_get_port_size(id,port))){
__DEBUGMSG("\nThe woffset should be incremented by one and stay inferior to the port size");
assert(__po_hi_gqueues_woffsets[id][port] == (init_woffset + 1)% __po_hi_gqueues_sizes[id][port]);
assert(__po_hi_gqueues_woffsets[id][port] < __po_hi_gqueues_sizes[id][port]);
__DEBUGMSG("\nThe effective port size used should be incremented by one");
assert (__po_hi_gqueues_used_size[id][port] == init_used_size +1);
__DEBUGMSG("\nThe port array is filled by the right port so that the reading is done at the right port");
assert (__po_hi_gqueues_global_history[id][init_history_woffset] == port);
__DEBUGMSG("The woffset_index should then be incremented by one and stay inferior to the fifo size");
assert(__po_hi_gqueues_global_history_woffset[id] == (init_history_woffset + 1)% __po_hi_gqueues_total_fifo_size[id]);
assert(__po_hi_gqueues_global_history_woffset[id] < __po_hi_gqueues_total_fifo_size[id]);
__DEBUGMSG("\nIf this port queue was empty, the number of empty port is reduced by 1");
/* The port was empty */
if (is_empty == 1){
assert(__po_hi_gqueues_n_empty[id] == nb_empty - 1);
}
__DEBUGMSG("\nThis port queue must be considered not empty ");
assert (__po_hi_gqueues_port_is_empty[id][port] == 0);
__DEBUGMSG("\nThe task queue must be considered not empty ");
assert (__po_hi_gqueues_queue_is_empty[id] == 0);
}
#endif
return __PO_HI_SUCCESS;
}
int __po_hi_gqueue_get_value (__po_hi_task_id id,
__po_hi_local_port_t port,
__po_hi_request_t* request)
{
__po_hi_request_t* ptr;
#ifdef RTEMS_PURE
rtems_status_code ret;
#endif
#ifdef _WIN32
DWORD ret;
#endif
ptr = &__po_hi_gqueues_most_recent_values[id][port];
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
pthread_mutex_lock (&__po_hi_gqueues_mutexes[id]);
#elif defined (XENO_NATIVE)
rt_mutex_acquire (&__po_hi_gqueues_mutexes[id], TM_INFINITE);
#elif defined (RTEMS_PURE)
ret = rtems_semaphore_obtain (__po_hi_gqueues_semaphores[id], RTEMS_WAIT, RTEMS_NO_TIMEOUT);
if (ret != RTEMS_SUCCESSFUL)
{
__DEBUGMSG ("[GQUEUE] Cannot obtain semaphore in __po_hi_gqueue_store_in()\n");
}
#elif defined (_WIN32)
EnterCriticalSection(&__po_hi_gqueues_cs[id]);
#endif
/*
* If the port is an event port, with no value queued, then we block
* the thread.
*/
if (__po_hi_gqueues_sizes[id][port] != __PO_HI_GQUEUE_FIFO_INDATA)
{
while (__po_hi_gqueues_port_is_empty[id][port] == 1)
{
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
pthread_cond_wait (&__po_hi_gqueues_conds[id],
&__po_hi_gqueues_mutexes[id]);
#elif defined (XENO_NATIVE)
rt_cond_wait (&__po_hi_gqueues_conds[id], &__po_hi_gqueues_mutexes[id], TM_INFINITE);
#elif defined (RTEMS_PURE)
rtems_task_wake_after( RTEMS_YIELD_PROCESSOR );
#elif defined (_WIN32)
LeaveCriticalSection(&__po_hi_gqueues_cs[id]);
ret = WaitForSingleObject (__po_hi_gqueues_events[id], INFINITE);
if (ret == WAIT_FAILED)
{
__PO_HI_DEBUG_CRITICAL ("[GQUEUE] Wait failed\n");
}
EnterCriticalSection(&__po_hi_gqueues_cs[id]);
#endif
}
}
#if defined (MONITORING)
update_sporadic_dispatch (id, port);
#endif
if (__po_hi_gqueues_used_size[id][port] == 0)
{
memcpy (request, ptr, sizeof (__po_hi_request_t));
//update_runtime (id, port, ptr);
}
else
{
ptr = ((__po_hi_request_t *) &__po_hi_gqueues[id][port]) + __po_hi_gqueues_first[id][port] + __po_hi_gqueues_offsets[id][port];
memcpy (request, ptr, sizeof (__po_hi_request_t));
}
__PO_HI_DEBUG_INFO ("[GQUEUE] Task %d get a value on port %d\n", id, port);
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
pthread_mutex_unlock (&__po_hi_gqueues_mutexes[id]);
#elif defined (XENO_NATIVE)
rt_mutex_release (&__po_hi_gqueues_mutexes[id]);
#elif defined (RTEMS_PURE)
ret = rtems_semaphore_release (__po_hi_gqueues_semaphores[id]);
if (ret != RTEMS_SUCCESSFUL)
{
__DEBUGMSG ("[GQUEUE] Cannot release semaphore in __po_hi_gqueue_store_in()\n");
}
#elif defined (_WIN32)
LeaveCriticalSection(&__po_hi_gqueues_cs[id]);
#endif
return 0;
}
void __po_hi_gqueue_wait_for_incoming_event (__po_hi_task_id id,
__po_hi_local_port_t* port)
{
#ifdef RTEMS_PURE
rtems_status_code ret;
#endif
#ifdef _WIN32
DWORD ret;
#endif
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
int error = pthread_mutex_lock (&__po_hi_gqueues_mutexes[id]);
__DEBUGMSG("*** Locking (%d) %d\n", id, error);
#elif defined (XENO_NATIVE)
rt_mutex_acquire (&__po_hi_gqueues_mutexes[id], TM_INFINITE);
#elif defined (RTEMS_PURE)
ret = rtems_semaphore_obtain (__po_hi_gqueues_semaphores[id], RTEMS_WAIT, RTEMS_NO_TIMEOUT);
if (ret != RTEMS_SUCCESSFUL)
{
__DEBUGMSG ("[GQUEUE] Cannot obtain semaphore in __po_hi_gqueue_store_in()\n");
}
#elif defined (_WIN32)
EnterCriticalSection(&__po_hi_gqueues_cs[id]);
#endif
while(__po_hi_gqueues_queue_is_empty[id] == 1)
{
__PO_HI_INSTRUMENTATION_VCD_WRITE("0t%d\n", id);
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
__DEBUGMSG("*** Waiting (%d)\n", id);
int error = pthread_cond_wait (&__po_hi_gqueues_conds[id],
&__po_hi_gqueues_mutexes[id]);
__DEBUGMSG("*** Done Waiting (%d) %d\n", id, error);
#elif defined (XENO_NATIVE)
rt_cond_wait (&__po_hi_gqueues_conds[id], &__po_hi_gqueues_mutexes[id], TM_INFINITE);
#elif defined (RTEMS_PURE)
ret = rtems_semaphore_release (__po_hi_gqueues_semaphores[id]);
if (ret != RTEMS_SUCCESSFUL)
{
__DEBUGMSG ("[GQUEUE] Cannot obtain semaphore in __po_hi_gqueue_store_in()\n");
}
rtems_task_wake_after (1);
ret = rtems_semaphore_obtain (__po_hi_gqueues_semaphores[id], RTEMS_WAIT, RTEMS_NO_TIMEOUT);
if (ret != RTEMS_SUCCESSFUL)
{
__DEBUGMSG ("[GQUEUE] Cannot obtain semaphore in __po_hi_gqueue_store_in()\n");
}
else
{
__PO_HI_DEBUG_CRITICAL ("[GQUEUE] semaphore %d obtained\n", id);
}
#elif defined (_WIN32)
LeaveCriticalSection(&__po_hi_gqueues_cs[id]);
ret = WaitForSingleObject (__po_hi_gqueues_events[id], INFINITE);
if (ret == WAIT_FAILED)
{
__PO_HI_DEBUG_CRITICAL ("[GQUEUE] Wait failed\n");
}
EnterCriticalSection(&__po_hi_gqueues_cs[id]);
#endif
__PO_HI_INSTRUMENTATION_VCD_WRITE("1t%d\n", id);
}
__DEBUGMSG ("[GQUEUE] Gogo kiki\n");
*port = __po_hi_gqueues_global_history[id][__po_hi_gqueues_global_history_offset[id]];
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
pthread_mutex_unlock (&__po_hi_gqueues_mutexes[id]);
#elif defined (XENO_NATIVE)
rt_mutex_release (&__po_hi_gqueues_mutexes[id]);
#elif defined (_WIN32)
LeaveCriticalSection(&__po_hi_gqueues_cs[id]);
#elif defined (RTEMS_PURE)
ret = rtems_semaphore_release (__po_hi_gqueues_semaphores[id]);
if (ret != RTEMS_SUCCESSFUL)
{
__DEBUGMSG ("[GQUEUE] Cannot release semaphore in __po_hi_gqueue_store_in()\n");
}
__PO_HI_DEBUG_CRITICAL ("[GQUEUE] semaphore %d released\n", id);
#endif
}
__po_hi_uint8_t __po_hi_gqueue_store_in (__po_hi_task_id id,
__po_hi_local_port_t port,
__po_hi_request_t* request)
{
__po_hi_request_t* ptr;
__po_hi_request_t* tmp;
__po_hi_uint32_t size;
#ifdef RTEMS_PURE
rtems_status_code ret;
#endif
ptr = &__po_hi_gqueues_most_recent_values[id][port];
#ifdef __PO_HI_DEBUG
if (ptr == NULL)
{
__DEBUGMSG ("__po_hi_gqueue_store_in : NULL POINTER\n");
}
#endif
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
pthread_mutex_lock (&__po_hi_gqueues_mutexes[id]);
#elif defined (XENO_NATIVE)
rt_mutex_acquire (&__po_hi_gqueues_mutexes[id], TM_INFINITE);
#elif defined (RTEMS_PURE)
__DEBUGMSG ("[GQUEUE] Try to obtain semaphore for queue of task %d\n", id);
ret = rtems_semaphore_obtain (__po_hi_gqueues_semaphores[id], RTEMS_WAIT, RTEMS_NO_TIMEOUT);
if (ret != RTEMS_SUCCESSFUL)
{
__DEBUGMSG ("[GQUEUE] Cannot obtain semaphore in __po_hi_gqueue_store_in()\n");
}
__DEBUGMSG ("[GQUEUE] Semaphore got (id=%d)\n", id);
#elif defined (_WIN32)
EnterCriticalSection(&__po_hi_gqueues_cs[id]);
#endif
if (__po_hi_gqueues_sizes[id][port] == __PO_HI_GQUEUE_FIFO_INDATA)
{
memcpy(ptr,request,sizeof(*request));
}
else
{
__DEBUGMSG ("[GQUEUE] Received message for task %d, port %d\n", id, port);
if (__po_hi_gqueues_used_size[id][port] == __po_hi_gqueues_sizes[id][port])
{
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
pthread_mutex_unlock (&__po_hi_gqueues_mutexes[id]);
#elif defined (XENO_NATIVE)
rt_mutex_release (&__po_hi_gqueues_mutexes[id]);
#elif defined (RTEMS_PURE)
ret = rtems_semaphore_release (__po_hi_gqueues_semaphores[id]);
if (ret != RTEMS_SUCCESSFUL)
{
__PO_HI_DEBUG_CRITICAL ("[GQUEUE] Cannot release semaphore in __po_hi_gqueue_store_in()\n");
}
#elif defined (_WIN32)
LeaveCriticalSection(&__po_hi_gqueues_cs[id]);
#endif
__PO_HI_DEBUG_CRITICAL ("[GQUEUE] QUEUE FULL, task-id=%d, port=%d\n", id, port);
__DEBUGMSG ("[GQUEUE] Semaphore released (id=%d)\n", id);
return __PO_HI_ERROR_QUEUE_FULL;
}
tmp = (__po_hi_request_t*) &__po_hi_gqueues[id][port];
size = __po_hi_gqueues_woffsets[id][port] + __po_hi_gqueues_first[id][port];
tmp = tmp + size;
memcpy (tmp , request, sizeof (__po_hi_request_t));
__po_hi_gqueues_woffsets[id][port] = (__po_hi_gqueues_woffsets[id][port] + 1 ) % __po_hi_gqueues_sizes[id][port];
__po_hi_gqueues_used_size[id][port]++;
__PO_HI_INSTRUMENTATION_VCD_WRITE("r%d p%d.%d\n", __po_hi_gqueues_used_size[id][port], id, port);
__po_hi_gqueues_global_history[id][__po_hi_gqueues_global_history_woffset[id]] = port;
__po_hi_gqueues_global_history_woffset[id] = (__po_hi_gqueues_global_history_woffset[id] + 1 ) % __po_hi_gqueues_total_fifo_size[id];
if (__po_hi_gqueues_port_is_empty[id][port] == 1)
{
__po_hi_gqueues_port_is_empty[id][port] = 0;
__po_hi_gqueues_n_empty[id]--;
}
__po_hi_gqueues_queue_is_empty[id] = 0;
}
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
pthread_mutex_unlock (&__po_hi_gqueues_mutexes[id]);
int err = pthread_cond_signal (&__po_hi_gqueues_conds[id]);
__DEBUGMSG("*** Releasing (%d) %d\n", id, err);
#elif defined (XENO_NATIVE)
rt_mutex_release (&__po_hi_gqueues_mutexes[id]);
rt_cond_broadcast (&__po_hi_gqueues_conds[id]);
#elif defined (_WIN32)
LeaveCriticalSection(&__po_hi_gqueues_cs[id]);
if (! SetEvent(__po_hi_gqueues_events[id]) )
{
__DEBUGMSG("SetEvent failed (%d)\n", GetLastError());
}
#elif defined (RTEMS_PURE)
ret = rtems_semaphore_release (__po_hi_gqueues_semaphores[id]);
if (ret != RTEMS_SUCCESSFUL)
{
__PO_HI_DEBUG_CRITICAL ("[GQUEUE] Cannot release semaphore in __po_hi_gqueue_store_in()\n");
}
__DEBUGMSG ("[GQUEUE] Semaphore released (id=%d)\n", id);
#endif
__DEBUGMSG ("[GQUEUE] store_in completed\n");
return __PO_HI_SUCCESS;
}
int __po_hi_delay_until (const __po_hi_time_t* time)
{
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
pthread_mutex_t mutex;
pthread_cond_t cond;
struct timespec timer;
int ret;
timer.tv_sec = time->sec;
timer.tv_nsec = time->nsec;
if (pthread_mutex_init (&mutex, NULL) != 0)
{
__PO_HI_DEBUG_INFO ("[TIME] __po_hi_delay_until: cannot initialize mutex\n");
return (__PO_HI_ERROR_PTHREAD_MUTEX);
}
if (pthread_cond_init (&cond, NULL) != 0)
{
__PO_HI_DEBUG_INFO ("[TIME] __po_hi_delay_until: cannot initialize cond\n");
pthread_mutex_destroy (&mutex);
return (__PO_HI_ERROR_PTHREAD_COND);
}
pthread_mutex_lock (&mutex);
ret = pthread_cond_timedwait (&cond, &mutex, &timer);
if ( (ret != 0) && (ret != ETIMEDOUT))
{
__PO_HI_DEBUG_INFO ("[TIME] __po_hi_delay_until: delay until error\n");
ret = __PO_HI_ERROR_PTHREAD_COND;
}
else
{
ret = __PO_HI_SUCCESS;
}
pthread_mutex_unlock (&mutex);
if (pthread_cond_destroy (&cond) != 0)
{
ret = __PO_HI_ERROR_PTHREAD_COND;
}
if (pthread_mutex_destroy (&mutex) != 0)
{
ret = __PO_HI_ERROR_PTHREAD_MUTEX;
}
return (ret);
#elif defined (RTEMS_PURE)
return (__PO_HI_UNAVAILABLE);
#elif defined (XENO_NATIVE)
int ret;
ret = rt_task_sleep_until (rt_timer_ns2tsc ( (time->sec * 1000000000) + time->nsec));
if (ret)
{
__DEBUGMSG ("[TASK] Error in rt_task_sleep_until, ret=%d\n", ret);
return (__PO_HI_ERROR_PTHREAD_COND);
}
return (__PO_HI_SUCCESS);
#elif defined (_WIN32)
HANDLE hTimer = NULL;
LARGE_INTEGER ularge;
hTimer = CreateWaitableTimer(NULL, TRUE, NULL);
ularge = __po_hi_unix_seconds_to_windows_tick (time->sec, time->nsec);
if (!SetWaitableTimer(hTimer, &ularge, 0, NULL, NULL, 0))
{
__PO_HI_DEBUG_CRITICAL("[DELAY UNTIL] SetWaitableTimer failed (%d)\n", GetLastError());
return 2;
}
if (WaitForSingleObject(hTimer, INFINITE) != WAIT_OBJECT_0)
{
__PO_HI_DEBUG_CRITICAL("[DELAY UNTIL] WaitForSingleObject failed (%d)\n", GetLastError());
}
if (CloseHandle(hTimer) != TRUE)
{
__PO_HI_DEBUG_CRITICAL("[DELAY UNTIL] CloseHandle failed (%d)\n", GetLastError());
}
return __PO_HI_SUCCESS;
#else
return (__PO_HI_UNAVAILABLE);
#endif
}
void __po_hi_driver_sockets_init (__po_hi_device_id dev_id)
{
static int __po_hi_c_sockets_array_init_done = 0;
int ret;
#ifdef _WIN32
char FAR reuse;
#else
int reuse;
#endif
struct sockaddr_in sa;
unsigned short ip_port;
__po_hi_c_ip_conf_t* ipconf;
__po_hi_device_id dev;
__po_hi_device_id sent_id;
struct hostent* hostinfo;
__po_hi_time_t mytime;
__po_hi_time_t tmptime;
char *tmp;
__po_hi_time_t current_time;
int i;
__po_hi_c_sockets_listen_socket = -1;
__po_hi_c_sockets_device_id = dev_id;
if (__po_hi_c_sockets_array_init_done == 0) /* XXX */
{
for (dev = 0 ; dev < __PO_HI_NB_DEVICES ; dev++)
{
__po_hi_c_sockets_read_sockets[dev] = -1;
__po_hi_c_sockets_write_sockets[dev] = -1;
}
__po_hi_c_sockets_array_init_done = 1;
}
__po_hi_mutex_init (&__po_hi_c_sockets_send_mutex,__PO_HI_MUTEX_REGULAR, 0);
__po_hi_transport_set_sending_func (dev_id, __po_hi_driver_sockets_send);
ipconf = (__po_hi_c_ip_conf_t*)__po_hi_get_device_configuration (dev_id);
ip_port = ipconf->port;
__DEBUGMSG ("My configuration, addr=%s, port=%d\n", ipconf->address, ip_port );
/*
* 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.
*/
if (ip_port != 0)
{
__po_hi_c_sockets_listen_socket = socket (AF_INET, SOCK_STREAM, 0);
if (__po_hi_c_sockets_listen_socket == -1 )
{
#ifdef __PO_HI_DEBUG
__DEBUGMSG ("Cannot create socket for device %d\n", dev_id);
#endif
return;
}
__DEBUGMSG ("Socket created for addr=%s, port=%d, socket value=%d\n",
ipconf->address, ip_port, __po_hi_c_sockets_listen_socket);
reuse = 1;
if (setsockopt (__po_hi_c_sockets_listen_socket, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof (reuse)))
{
__PO_HI_DEBUG_CRITICAL ("[DRIVER SOCKETS] Error while making the receiving socket reusable\n");
}
sa.sin_addr.s_addr = htonl (INADDR_ANY); /* We listen on all adresses */
sa.sin_family = AF_INET;
sa.sin_port = htons (ip_port); /* Port provided by the generated code */
if( bind (__po_hi_c_sockets_listen_socket, (struct sockaddr *) &sa , sizeof (struct sockaddr_in) ) < 0 )
{
__PO_HI_DEBUG_CRITICAL ("Unable to bind socket and port on socket %d\n", __po_hi_c_sockets_listen_socket);
}
if( listen (__po_hi_c_sockets_listen_socket, __PO_HI_NB_DEVICES) < 0 )
{
__PO_HI_DEBUG_CRITICAL ("Cannot listen on socket %d\n", __po_hi_c_sockets_listen_socket);
}
/*
* 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
(-1, 0,__PO_HI_MAX_PRIORITY, 0, 0, (void* (*)(void))__po_hi_sockets_poller, &dev_id);
/* For now, we force affinity to 0 */
}
/*
* 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.
*/
for (dev = 0 ; dev < __PO_HI_NB_DEVICES ; dev++)
{
if (dev == dev_id)
{
continue;
}
if (__po_hi_transport_share_bus (dev, dev_id) == 0)
{
__DEBUGMSG ("[DRIVER SOCKETS] Device %d and device %d does not share the same bus, skip connecting them\n", dev, dev_id);
continue;
}
__DEBUGMSG ("[DRIVER SOCKETS] Will initialize connection with device %d\n", dev);
ipconf = (__po_hi_c_ip_conf_t*) __po_hi_get_device_configuration (dev);
ip_port = (unsigned short)ipconf->port;
__DEBUGMSG ("[DRIVER SOCKETS] Configuration for device %d, port=%d\n", dev, ip_port);
if (ip_port == 0)
{
__PO_HI_DEBUG_CRITICAL ("[DRIVER SOCKETS] Invalid remote port\n");
continue;
}
while (1)
{
__po_hi_c_sockets_write_sockets[dev] = socket (AF_INET, SOCK_STREAM, 0);
if (__po_hi_c_sockets_write_sockets[dev] == -1 )
{
__PO_HI_DEBUG_CRITICAL ("[DRIVER SOCKETS] Socket for dev %d is not created\n", dev);
return;
}
int NoDelayFlag = 1;
if(setsockopt(__po_hi_c_sockets_write_sockets[dev],IPPROTO_TCP,TCP_NODELAY,&NoDelayFlag,sizeof(NoDelayFlag))){
__PO_HI_DEBUG_CRITICAL ("[DRIVER SOCKETS] Unable to set TCP_NODELAY for dev %d\n", dev);
}
__DEBUGMSG ("[DRIVER SOCKETS] Socket for dev %d created, value=%d\n", dev, __po_hi_c_sockets_write_sockets[dev]);
hostinfo = gethostbyname ((char*)ipconf->address);
if (hostinfo == NULL )
{
__PO_HI_DEBUG_CRITICAL ("[DRIVER SOCKETS] Error while getting host informations for device %d\n", dev);
}
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];
}
/*
* We try to connect on the remote host. We try every
* second to connect on.
*/
__PO_HI_SET_SOCKET_TIMEOUT(__po_hi_c_sockets_write_sockets[dev], 500000);
ret = connect (__po_hi_c_sockets_write_sockets[dev],
(struct sockaddr*) &sa ,
sizeof (struct sockaddr_in));
#ifdef __PO_HI_USE_PROTOCOL_MYPROTOCOL_I
if (ret == 0)
{
__DEBUGMSG ("[DRIVER SOCKETS] Connection established with device %d, socket=%d\n", dev, __po_hi_c_sockets_write_sockets[dev]);
break;
}
else
{
__PO_HI_DEBUG_CRITICAL ("connect() failed, return=%d, %s\n", ret, strerror(errno));
}
#else
if (ret == 0)
{
__DEBUGMSG ("[DRIVER SOCKETS] Send my id (%d) to device %d through socket %d\n", dev_id, dev , __po_hi_c_sockets_write_sockets[dev]);
sent_id = __po_hi_swap_byte (dev_id);
#ifdef _WIN32
ret = send (__po_hi_c_sockets_write_sockets[dev], (char*) &sent_id, sizeof (__po_hi_device_id), 0);
#else
ret = write (__po_hi_c_sockets_write_sockets[dev], &sent_id, sizeof (__po_hi_device_id));
#endif
if (ret != sizeof (__po_hi_device_id))
{
__PO_HI_DEBUG_CRITICAL ("[DRIVER SOCKETS] Device %d cannot send his id, expected size=%lu, return value=%d\n", dev_id, sizeof (__po_hi_device_id), ret);
}
else
{
__DEBUGMSG ("[DRIVER SOCKETS] Connection established with device %d, socket=%d\n", dev, __po_hi_c_sockets_write_sockets[dev]);
break;
}
}
else
{
__DEBUGMSG ("connect() failed, return=%d\n", ret);
}
#endif
if (close (__po_hi_c_sockets_write_sockets[dev]))
{
__DEBUGMSG ("[DRIVER SOCKETS] Cannot close socket %d\n", __po_hi_c_sockets_write_sockets[dev]);
}
/*
* We wait 500ms each time we try to connect on the
* remote host
*/
__po_hi_get_time (¤t_time);
__po_hi_milliseconds (&tmptime, 500);
__po_hi_add_times (&mytime, ¤t_time, &tmptime);
__PO_HI_DEBUG_CRITICAL ("[DRIVER SOCKETS] Cannot connect on device %d, wait 500ms\n", dev);
__po_hi_delay_until (&mytime);
}
}
__DEBUGMSG ("[DRIVER SOCKETS] INITIALIZATION DONE\n");
}
