void __po_hi_c_driver_drvmgr_grspw_init (__po_hi_device_id id)
{
unsigned int node_addr;
__po_hi_c_spacewire_conf_t* drv_conf;
/* Initializes drvmgr subsystem */
__po_hi_c_driver_drvmgr_init ();
/* Set sending callback function */
__po_hi_transport_set_sending_func
(id, __po_hi_c_driver_drvmgr_grspw_sender);
/* Set up current node address */
drv_conf = (__po_hi_c_spacewire_conf_t*)
__po_hi_get_device_configuration (id);
node_addr = drv_conf->nodeaddr;
__PO_HI_DEBUG_INFO ("[GRSPW SPACEWIRE] Init, node address=%d\n", node_addr);
/* Setting up SpaceWire device */
__PO_HI_DEBUG_DEBUG ("[GRSPW SPACEWIRE] Initializing GRSPW driver \n");
grspw_api_init();
// XXX should also configure node address !
__PO_HI_DEBUG_DEBUG
("[GRSPW SPACEWIRE] Initialization complete\n");
}
void __po_hi_c_driver_rasta_common_init ()
{
if (__po_hi_c_driver_rasta_common_is_init == 1)
{
__PO_HI_DEBUG_DEBUG ("[RASTA COMMON] RASTA already initialized, pass init\n");
return;
}
__PO_HI_DEBUG_DEBUG ("[RASTA COMMON] Init\n");
init_pci();
__PO_HI_DEBUG_DEBUG ("[RASTA COMMON] Initializing RASTA ...");
/*
if (__po_hi_rasta_register() ){
__PO_HI_DEBUG_DEBUG (" ERROR !\n");
return;
}
*/
if (rasta_register() ){
__PO_HI_DEBUG_DEBUG(" ERROR !\n");
return;
}
__PO_HI_DEBUG_DEBUG (" OK !\n");
__po_hi_c_driver_rasta_common_is_init = 1;
}
void __po_hi_c_driver_spacewire_rasta_init (__po_hi_device_id id)
{
unsigned int node_addr;
__po_hi_c_spacewire_conf_t* drv_conf;
drv_conf = (__po_hi_c_spacewire_conf_t*) __po_hi_get_device_configuration (id);
node_addr = drv_conf->nodeaddr;
__PO_HI_DEBUG_INFO ("[RASTA SPACEWIRE] Init, node address=%d\n", node_addr);
__po_hi_c_driver_rasta_common_init ();
__po_hi_transport_set_sending_func (id, __po_hi_c_driver_spacewire_rasta_sender);
__PO_HI_DEBUG_DEBUG ("[RASTA SPACEWIRE] Open spacewire device %s ...", drv_conf->devname);
po_hi_c_driver_rasta_spacewire_fd[id] = open (drv_conf->devname, O_RDWR);
if (po_hi_c_driver_rasta_spacewire_fd[id] < 0)
{
__PO_HI_DEBUG_DEBUG (" ERROR !\n");
return;
}
__PO_HI_DEBUG_DEBUG (" OK !\n");
__PO_HI_DEBUG_DEBUG ("[RASTA SPACEWIRE] Configure spacewire device node address = %d ...", node_addr);
/*
__PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd, SPACEWIRE_IOCTRL_SET_COREFREQ, 0); // Core frequency in KHz (0 = full speed)
__PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd, SPACEWIRE_IOCTRL_SET_CLKDIV, 2); // Clock division factor
__PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd, SPACEWIRE_IOCTRL_SET_RMAPEN, 1); // No RMAP
__PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd, SPACEWIRE_IOCTRL_SET_NODEADDR, node_addr); // Not necessary
__PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // Blocking read
__PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd, SPACEWIRE_IOCTRL_SET_CHECK_RMAP, 0); // Do not check RMAP CRC
__PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd, SPACEWIRE_IOCTRL_SET_RM_PROT_ID, 0); // Do not remove protocol id
__PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 0); // Non blocking write
__PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // Blocking write if full
__PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd, SPACEWIRE_IOCTRL_SET_PROMISCUOUS, 1); // Receive from any source
*/
__PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd[id],SPACEWIRE_IOCTRL_SET_COREFREQ,30000);
__PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd[id],SPACEWIRE_IOCTRL_SET_NODEADDR, node_addr);
__PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd[id],SPACEWIRE_IOCTRL_SET_RXBLOCK,0);
__PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd[id],SPACEWIRE_IOCTRL_SET_TXBLOCK,0);
__PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd[id],SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL,1);
__PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd[id],SPACEWIRE_IOCTRL_SET_RM_PROT_ID,0);
__PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd[id], SPACEWIRE_IOCTRL_SET_PROMISCUOUS, 1); // Receive from any source
__PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd[id],SPACEWIRE_IOCTRL_START,2000);
}
void __po_hi_gqueue_store_out (__po_hi_task_id id,
__po_hi_local_port_t port,
__po_hi_request_t* request)
{
__po_hi_request_t* ptr;
request->port = __PO_HI_GQUEUE_OUT_PORT;
ptr = &__po_hi_gqueues_most_recent_values[id][port];
memcpy (ptr, request, sizeof (__po_hi_request_t));
__PO_HI_DEBUG_DEBUG ("__po_hi_gqueue_store_out() from task %d on port %d\n", id, port);
}
void __po_hi_gqueue_store_out (__po_hi_task_id id,
__po_hi_local_port_t port,
__po_hi_request_t* request)
{
__po_hi_request_t* ptr;
request->port = __PO_HI_GQUEUE_OUT_PORT;
ptr = &__po_hi_gqueues_most_recent_values[id][port];
memcpy (ptr, request, sizeof (__po_hi_request_t));
#if defined (TIMEBENCH)
po_hi_put_time_stamp(id, port,__po_hi_get_CPU_time( ));
#endif
__PO_HI_DEBUG_DEBUG ("__po_hi_gqueue_store_out() from task %d on port %d\n", id, port);
}
void __po_hi_gqueue_store_out (__po_hi_task_id id,
__po_hi_local_port_t port,
__po_hi_request_t* request)
{
__po_hi_request_t* ptr;
request->port = __PO_HI_GQUEUE_OUT_PORT;
ptr = &__po_hi_gqueues_most_recent_values[id][port];
memcpy (ptr, request, sizeof (__po_hi_request_t));
__PO_HI_DEBUG_DEBUG ("\n__Po_hi_gqueue_store_out() from task %d on port %d\n", id, port);
#if defined (MONITORING)
__DEBUGMSG("\nThe last value is the request to be stored");
record_event(ANY, STORE_OUT, id, invalid_port_t, invalid_port_t, port, invalid_local_port_t, request);
#endif
}
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 __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_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_DEBUG("[DELAY UNTIL] SetWaitableTimer failed (%d)\n", GetLastError());
return 2;
}
if (WaitForSingleObject(hTimer, INFINITE) != WAIT_OBJECT_0)
{
__PO_HI_DEBUG_DEBUG("[DELAY UNTIL] WaitForSingleObject failed (%d)\n", GetLastError());
}
if (CloseHandle(hTimer) != TRUE)
{
__PO_HI_DEBUG_DEBUG("[DELAY UNTIL] CloseHandle failed (%d)\n", GetLastError());
}
return __PO_HI_SUCCESS;
#else
return (__PO_HI_UNAVAILABLE);
#endif
}
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
}
int __po_hi_gqueue_next_value (__po_hi_task_id id, __po_hi_local_port_t port)
{
#ifdef __PO_HI_GQUEUE_ASSERTIONS
__po_hi_port_id_t init_offset = __po_hi_gqueues_offsets[id][port];
__po_hi_uint32_t init_history_offset = __po_hi_gqueues_global_history_offset[id];
__po_hi_port_id_t init_used_size = __po_hi_gqueues_used_size[id][port];
__po_hi_port_id_t nb_empty = __po_hi_gqueues_n_empty[id];
#endif
/* Incomplete semantics, Should discriminate and report whether
there is a next value or not */
if (__po_hi_gqueue_get_port_size(id,port) == __PO_HI_GQUEUE_FIFO_INDATA)
{
__PO_HI_DEBUG_INFO ("[GQUEUE] BEWARE, for a FIFO_INDATA port, the used_size is always at 0 (not reduced in a next_value) task-id=%d, port=%d\n", id, port);
return 1;
}
/* Locking a mutex */
__PO_HI_DEBUG_DEBUG ("\nWaiting on next_value 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 %d %d\n", id, result);
assert(result == __PO_HI_SUCCESS);
__PO_HI_DEBUG_DEBUG("\nBefore next_value for task-id %d , offset = %d, woffset = %d, history_offset = %d, history_woffset = %d, port_size = %d, fifo size = %d, gqueues adress = %d, \n\n", id, __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]);
__po_hi_gqueues_offsets[id][port] =
(__po_hi_gqueues_offsets[id][port] + 1)
% __po_hi_gqueues_sizes[id][port];
__PO_HI_DEBUG_DEBUG ("\nBefore -- on size, Next_value for task id = %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 -- on size , Next_value for task id = %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);
if (__po_hi_gqueue_used_size(id,port) == 0)
{
__po_hi_gqueues_n_empty[id]++;
__po_hi_gqueues_port_is_empty[id][port] = 1;
}
if (__po_hi_gqueues_n_empty[id] == __po_hi_gqueues_nb_ports[id])
{
__po_hi_gqueues_queue_is_empty[id] = 1;
}
__po_hi_gqueues_global_history_offset[id] =
(__po_hi_gqueues_global_history_offset[id] + 1)
% __po_hi_gqueues_total_fifo_size[id];
__PO_HI_DEBUG_DEBUG("\nAfter next_value for task-id %d , offset = %d, woffset = %d, history_offset = %d, history_woffset = %d , port size = %d, fifo size = %d, gqueue = %d \n\n", id, __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 mutex*/
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);
#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)){
__DEBUGMSG("\nThe woffset should be incremented by one");
assert(__po_hi_gqueues_offsets[id][port] == (init_offset + 1)% __po_hi_gqueues_sizes[id][port]);
assert(__po_hi_gqueues_offsets[id][port] < __po_hi_gqueues_sizes[id][port]);
__DEBUGMSG("\nThe effective port size used should be decremented by one");
assert (__po_hi_gqueues_used_size[id][port] == init_used_size -1);
__DEBUGMSG("The offset_index should then be incremented by one");
assert(__po_hi_gqueues_global_history_offset[id] == (init_history_offset + 1)% __po_hi_gqueues_total_fifo_size[id]);
assert(__po_hi_gqueues_global_history_offset[id] < __po_hi_gqueues_total_fifo_size[id]);
__DEBUGMSG("\nIf this port queue was empty, the number of empty port is reduced by 1");
/* If the port is now empty */
if (__po_hi_gqueue_used_size(id,port) == 0){
assert(__po_hi_gqueues_n_empty[id] == nb_empty + 1);
__DEBUGMSG("\nThis port queue must be considered empty ");
assert(__po_hi_gqueues_port_is_empty[id][port] == 1);
}
if (__po_hi_gqueues_n_empty[id] == __po_hi_gqueues_nb_ports[id])
{
assert(__po_hi_gqueues_queue_is_empty[id] == 1);
}
}
#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;
__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;
}
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_DEBUG ("[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
}
void __po_hi_gqueue_init (__po_hi_task_id id,
__po_hi_port_id_t nb_ports,
__po_hi_request_t queue[],
__po_hi_port_id_t sizes[],
__po_hi_port_id_t first[],
__po_hi_port_id_t offsets[],
__po_hi_port_id_t woffsets[],
__po_hi_port_id_t n_dest[],
__po_hi_port_t* destinations[],
__po_hi_port_id_t used_size[],
__po_hi_local_port_t history[],
__po_hi_request_t recent[],
__po_hi_port_id_t empties[],
__po_hi_uint32_t total_fifo_size)
{
__po_hi_port_id_t tmp;
__po_hi_uint32_t off; /* XXX May overflow for large value .. */
__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;
/* Using the semaphore API to initialize the semaphore_gqueue array */
int res = __po_hi_sem_init_gqueue(__po_hi_gqueues_semaphores,id);
__DEBUGMSG("GQUEUE_SEM_INIT %d %d\n", id, res);
assert(res == __PO_HI_SUCCESS);
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 */
__po_hi_request_t* 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_DEBUG_DEBUG("Initialize global queue for task %d , first = %d, history_offset = %d, history_woffset = %d, fifo size = %d, gqueue_id adress = %d\n\n", id, __po_hi_gqueues_first[id],__po_hi_gqueues_global_history_offset[id],__po_hi_gqueues_global_history_woffset[id], __po_hi_gqueues_total_fifo_size[id],__po_hi_gqueues[id] );
#if defined __PO_HI_GQUEUE_ASSERTIONS
__DEBUGMSG("\nInitialization parameter");
assert(__po_hi_gqueues_global_history_woffset[id] == 0);
assert(__po_hi_gqueues_global_history_offset[id] == 0);
assert(__po_hi_gqueues_n_empty[id] == nb_ports);
assert(__po_hi_gqueues[id] == queue);
assert(__po_hi_gqueues_most_recent_values[id] == recent);
assert(__po_hi_gqueues_global_history[id] == history);
assert(__po_hi_gqueues_woffsets[id] == woffsets);
assert(__po_hi_gqueues_port_is_empty[id] == empties);
assert(__po_hi_gqueues_nb_ports[id] == nb_ports);
assert(__po_hi_gqueues_sizes[id] == sizes);
assert(__po_hi_gqueues_first[id] == first);
assert(__po_hi_gqueues_used_size[id] == used_size);
assert(__po_hi_gqueues_offsets[id] == offsets);
assert(__po_hi_gqueues_n_destinations[id] == n_dest);
assert(__po_hi_gqueues_destinations[id] == destinations);
assert(__po_hi_gqueues_total_fifo_size[id] == total_fifo_size);
assert(__po_hi_gqueues_queue_is_empty[id] = 1);
for (__po_hi_port_id_t i = 0; i < nb_ports; i++){
assert(__po_hi_gqueues_used_size[id][i] == 0);
assert(__po_hi_gqueues_most_recent_values[id][i].port == __PO_HI_GQUEUE_INVALID_PORT);
if (i > 0){
/* Usually HAS TO be right */
//assert(__po_hi_gqueues_first[id][i] >= 0);
}
}
#endif
}
int __po_hi_c_driver_spacewire_rasta_sender (const __po_hi_task_id task_id, const __po_hi_port_t port)
{
int len = -1;
int i;
__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;
dev_id = __po_hi_get_device_from_port (port);
if (dev_id == invalid_device_id)
{
__PO_HI_DEBUG_DEBUG ("[RASTA SPW] 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 ("[RASTA 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_spacewire_rasta_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_spacewire_rasta_sender_msg);
len = -1;
if (sender_conf->use_router == TRUE)
{
__po_hi_c_driver_rasta_spacewire_sndbuf[0] = receiver_conf->nodeaddr;
memcpy (&__po_hi_c_driver_rasta_spacewire_sndbuf[1], __po_hi_c_driver_spacewire_rasta_sender_msg.content , __PO_HI_MESSAGES_MAX_SIZE);
len = write (po_hi_c_driver_rasta_spacewire_fd[dev_id], __po_hi_c_driver_rasta_spacewire_sndbuf, __PO_HI_MESSAGES_MAX_SIZE + 1);
}
else
{
len = write (po_hi_c_driver_rasta_spacewire_fd[dev_id], __po_hi_c_driver_spacewire_rasta_sender_msg.content, __PO_HI_MESSAGES_MAX_SIZE);
}
if (len < 0)
{
__PO_HI_DEBUG_DEBUG (" failed !\n");
}
else
{
__PO_HI_DEBUG_DEBUG (" OK !\n");
}
request->port = __PO_HI_GQUEUE_INVALID_PORT;
return 1;
}
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_DEBUG ("[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));
}
#if defined (TIMEBENCH)
po_hi_put_time_stamp(id, port,__po_hi_get_CPU_time( ));
#endif
__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;
}
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;
}
__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;
}