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");
}
int __po_hi_gqueue_get_count( __po_hi_task_id id, __po_hi_local_port_t port)
{
if (__po_hi_gqueue_get_port_size(id,port) == __PO_HI_GQUEUE_FIFO_INDATA)
{
__PO_HI_DEBUG_INFO ("[GQUEUE] BEWARE a FIFO_INDATA port will always have a get_count of 1, even if empty, task-id=%d, port=%d\n", id, port);
return 1; /* data port are always of size 1 */
}
else
{
return (__po_hi_gqueue_used_size(id,port));
}
}
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);
}
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_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;
}
__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;
}
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
}
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;
}
