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; }