void __po_hi_gqueue_wait_for_incoming_event (__po_hi_task_id id, __po_hi_local_port_t* 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); while(po_hi_gqueues_queue_is_empty(id) == 1) { __PO_HI_INSTRUMENTATION_VCD_WRITE("0t%d\n", id); /* 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, res_sem); assert(res_sem == __PO_HI_SUCCESS); __PO_HI_INSTRUMENTATION_VCD_WRITE("1t%d\n", id); } *port = __po_hi_gqueues_global_history[id][__po_hi_gqueues_global_history_offset[id]]; #if defined (MONITORING) record_event(SPORADIC, WAIT_FOR, id, invalid_port_t, invalid_port_t, *port, invalid_local_port_t, NULL); #endif /** Releasing only the mutex of the semaphore*/ 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); #ifdef __PO_HI_GQUEUE_ASSERTIONS __DEBUGMSG("\nThe task queue must be considered not empty "); assert (*port == __po_hi_gqueues_global_history[id][__po_hi_gqueues_global_history_offset[id]]); #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; }
__po_hi_port_id_t __po_hi_gqueue_store_in (__po_hi_task_id id, __po_hi_local_port_t port, __po_hi_request_t* request) { #ifdef __PO_HI_GQUEUE_ASSERTIONS __po_hi_port_id_t init_woffset = __po_hi_gqueues_woffsets[id][port]; __po_hi_uint32_t init_history_woffset = __po_hi_gqueues_global_history_woffset[id]; __po_hi_port_id_t init_used_size = __po_hi_gqueues_used_size[id][port]; __po_hi_port_id_t is_empty = __po_hi_gqueues_port_is_empty[id][port]; __po_hi_port_id_t nb_empty = __po_hi_gqueues_n_empty[id]; #endif __po_hi_request_t* ptr; __po_hi_request_t* tmp; ptr = &__po_hi_gqueues_most_recent_values[id][port]; #ifdef __PO_HI_DEBUG if (ptr == NULL) { __DEBUGMSG ("__po_hi_gqueue_store_in : NULL POINTER\n"); } #endif /* Locking only a mutex */ __PO_HI_DEBUG_DEBUG ("\nWaiting on Store_in on task %d, port = %d, size of port = %d\n", id, port,__po_hi_gqueue_get_port_size(id, port)); int result = __po_hi_sem_mutex_wait_gqueue(__po_hi_gqueues_semaphores,id); __DEBUGMSG("GQUEUE_SEM_MUTEX_WAIT on task %d result = %d\n", id, result); assert(result == __PO_HI_SUCCESS); if (__po_hi_gqueue_get_port_size(id,port) == __PO_HI_GQUEUE_FIFO_INDATA) { memcpy(ptr,request,sizeof(*request)); __PO_HI_DEBUG_INFO ("[GQUEUE] BEWARE, for a FIFO_INDATA port, the used_size is always at 0 (not augmented in a store_in) task-id=%d, port=%d\n", id, port); } else { __DEBUGMSG ("[GQUEUE] Received message for task %d, port %d\n", id, port); if (__po_hi_gqueue_used_size(id,port) == __po_hi_gqueue_get_port_size(id,port)) { /* Releasing only a mutex */ int res = __po_hi_sem_mutex_release_gqueue(__po_hi_gqueues_semaphores,id); __DEBUGMSG("GQUEUE_SEM_MTUEX_RELEASE %d %d\n", id, res); assert(res == __PO_HI_SUCCESS); __PO_HI_DEBUG_CRITICAL ("[GQUEUE] QUEUE FULL, task-id=%d, port=%d\n", id, port); __DEBUGMSG ("[GQUEUE] Semaphore released (id=%d)\n", id); return __PO_HI_ERROR_QUEUE_FULL; } __PO_HI_DEBUG_DEBUG("\nBefore store_in for task-id %d , port %d, offset = %d, woffset = %d, history_offset = %d, history_woffset = %d, port size = %d, fifo size = %d, gqueue id adress = %d,\n\n", id, port, __po_hi_gqueues_offsets[id][port], __po_hi_gqueues_woffsets[id][port],__po_hi_gqueues_global_history_offset[id],__po_hi_gqueues_global_history_woffset[id], __po_hi_gqueues_sizes[id][port], __po_hi_gqueues_total_fifo_size[id], __po_hi_gqueues[id]); /* The program ensures to write the information at the right place in the buffer. * * The right first offset has to be applied so that the right * port is chosen. The right woffset (writing_offset) has to be * applied not to erase fresh information. */ __po_hi_uint32_t size; tmp = __po_hi_gqueues[id]; size = __po_hi_gqueues_woffsets[id][port] + __po_hi_gqueues_first[id][port]; tmp = tmp + size; __PO_HI_DEBUG_DEBUG(" Store_in first + woffsets = %d, first = %d, gqueue_id adress = %d, tmp (adress + woffset + first)= %d,\n\n", __po_hi_gqueues_first[id][port] + __po_hi_gqueues_woffsets[id][port],__po_hi_gqueues_first[id][port],__po_hi_gqueues[id], tmp); memcpy (tmp , request, sizeof (__po_hi_request_t)); __po_hi_gqueues_woffsets[id][port] = (__po_hi_gqueues_woffsets[id][port] + 1 ) % __po_hi_gqueues_sizes[id][port]; __PO_HI_DEBUG_DEBUG ("\nBefore used_size ++, Store_in for task = %d, __po_hi_gqueues_used_size[id][port] = %d\n", id, __po_hi_gqueues_used_size[id][port]); __po_hi_gqueues_used_size[id][port]++; __PO_HI_DEBUG_DEBUG ("\nAfter used_size ++ , Store_in for task = %d, __po_hi_gqueues_used_size[id][port] = %d\n",id, __po_hi_gqueues_used_size[id][port]); __PO_HI_INSTRUMENTATION_VCD_WRITE("r%d p%d.%d\n", __po_hi_gqueue_used_size(id,port), id, port); /* The port where information has been written is stored */ __po_hi_gqueues_global_history[id][__po_hi_gqueues_global_history_woffset[id]] = port; __po_hi_gqueues_global_history_woffset[id] = (__po_hi_gqueues_global_history_woffset[id] + 1 ) % __po_hi_gqueues_total_fifo_size[id]; if (__po_hi_gqueues_port_is_empty[id][port] == 1) { __po_hi_gqueues_port_is_empty[id][port] = 0; __po_hi_gqueues_n_empty[id]--; } __po_hi_gqueues_queue_is_empty[id] = 0; } __PO_HI_DEBUG_DEBUG("\nAfter store_in for task-id %d , port %d, offset = %d, woffset = %d, history_offset = %d, history_woffset = %d, port size = %d, fifo size = %d, gqueue_id adress= %d, \n\n", id, port, __po_hi_gqueues_offsets[id][port], __po_hi_gqueues_woffsets[id][port],__po_hi_gqueues_global_history_offset[id],__po_hi_gqueues_global_history_woffset[id], __po_hi_gqueues_sizes[id][port], __po_hi_gqueues_total_fifo_size[id], __po_hi_gqueues[id]); /* Releasing a complete semaphore */ int rel = __po_hi_sem_release_gqueue(__po_hi_gqueues_semaphores,id); __DEBUGMSG("GQUEUE_SEM_RELEASE %d %d\n", id, rel); assert(rel == __PO_HI_SUCCESS); __DEBUGMSG ("[GQUEUE] store_in completed\n"); #ifdef __PO_HI_GQUEUE_ASSERTIONS /* The port length is superior to 1 */ if ((__po_hi_gqueue_get_port_size(id,port) != __PO_HI_GQUEUE_FIFO_INDATA)&&(init_used_size != __po_hi_gqueue_get_port_size(id,port))){ __DEBUGMSG("\nThe woffset should be incremented by one and stay inferior to the port size"); assert(__po_hi_gqueues_woffsets[id][port] == (init_woffset + 1)% __po_hi_gqueues_sizes[id][port]); assert(__po_hi_gqueues_woffsets[id][port] < __po_hi_gqueues_sizes[id][port]); __DEBUGMSG("\nThe effective port size used should be incremented by one"); assert (__po_hi_gqueues_used_size[id][port] == init_used_size +1); __DEBUGMSG("\nThe port array is filled by the right port so that the reading is done at the right port"); assert (__po_hi_gqueues_global_history[id][init_history_woffset] == port); __DEBUGMSG("The woffset_index should then be incremented by one and stay inferior to the fifo size"); assert(__po_hi_gqueues_global_history_woffset[id] == (init_history_woffset + 1)% __po_hi_gqueues_total_fifo_size[id]); assert(__po_hi_gqueues_global_history_woffset[id] < __po_hi_gqueues_total_fifo_size[id]); __DEBUGMSG("\nIf this port queue was empty, the number of empty port is reduced by 1"); /* The port was empty */ if (is_empty == 1){ assert(__po_hi_gqueues_n_empty[id] == nb_empty - 1); } __DEBUGMSG("\nThis port queue must be considered not empty "); assert (__po_hi_gqueues_port_is_empty[id][port] == 0); __DEBUGMSG("\nThe task queue must be considered not empty "); assert (__po_hi_gqueues_queue_is_empty[id] == 0); } #endif return __PO_HI_SUCCESS; }
int __po_hi_gqueue_next_value (__po_hi_task_id id, __po_hi_local_port_t port) { #ifdef RTEMS_PURE rtems_status_code ret; #endif /* incomplete semantics, should discriminate and report whether there is a next value or not */ /* XXX change and use assert ? */ if (__po_hi_gqueues_sizes[id][port] == __PO_HI_GQUEUE_FIFO_INDATA) { return 1; } #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 (_WIN32) EnterCriticalSection(&__po_hi_gqueues_cs[id]); #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"); } #endif __po_hi_gqueues_offsets[id][port] = (__po_hi_gqueues_offsets[id][port] + 1) % __po_hi_gqueues_sizes[id][port]; __po_hi_gqueues_used_size[id][port]--; __PO_HI_INSTRUMENTATION_VCD_WRITE("r%d p%d.%d\n", __po_hi_gqueues_used_size[id][port], id, port); if (__po_hi_gqueues_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]; #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 __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_CRITICAL ("[GQUEUE] Wait failed\n"); } EnterCriticalSection(&__po_hi_gqueues_cs[id]); #endif __PO_HI_INSTRUMENTATION_VCD_WRITE("1t%d\n", id); } __DEBUGMSG ("[GQUEUE] Gogo kiki\n"); *port = __po_hi_gqueues_global_history[id][__po_hi_gqueues_global_history_offset[id]]; #if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX) pthread_mutex_unlock (&__po_hi_gqueues_mutexes[id]); #elif defined (XENO_NATIVE) rt_mutex_release (&__po_hi_gqueues_mutexes[id]); #elif defined (_WIN32) LeaveCriticalSection(&__po_hi_gqueues_cs[id]); #elif defined (RTEMS_PURE) ret = rtems_semaphore_release (__po_hi_gqueues_semaphores[id]); if (ret != RTEMS_SUCCESSFUL) { __DEBUGMSG ("[GQUEUE] Cannot release semaphore in __po_hi_gqueue_store_in()\n"); } __PO_HI_DEBUG_CRITICAL ("[GQUEUE] semaphore %d released\n", id); #endif }
__po_hi_uint8_t __po_hi_gqueue_store_in (__po_hi_task_id id, __po_hi_local_port_t port, __po_hi_request_t* request) { __po_hi_request_t* ptr; __po_hi_request_t* tmp; __po_hi_uint32_t size; #ifdef RTEMS_PURE rtems_status_code ret; #endif ptr = &__po_hi_gqueues_most_recent_values[id][port]; #ifdef __PO_HI_DEBUG if (ptr == NULL) { __DEBUGMSG ("__po_hi_gqueue_store_in : NULL POINTER\n"); } #endif #if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX) pthread_mutex_lock (&__po_hi_gqueues_mutexes[id]); #elif defined (XENO_NATIVE) rt_mutex_acquire (&__po_hi_gqueues_mutexes[id], TM_INFINITE); #elif defined (RTEMS_PURE) __DEBUGMSG ("[GQUEUE] Try to obtain semaphore for queue of task %d\n", id); ret = rtems_semaphore_obtain (__po_hi_gqueues_semaphores[id], RTEMS_WAIT, RTEMS_NO_TIMEOUT); if (ret != RTEMS_SUCCESSFUL) { __DEBUGMSG ("[GQUEUE] Cannot obtain semaphore in __po_hi_gqueue_store_in()\n"); } __DEBUGMSG ("[GQUEUE] Semaphore got (id=%d)\n", id); #elif defined (_WIN32) EnterCriticalSection(&__po_hi_gqueues_cs[id]); #endif if (__po_hi_gqueues_sizes[id][port] == __PO_HI_GQUEUE_FIFO_INDATA) { memcpy(ptr,request,sizeof(*request)); } else { __DEBUGMSG ("[GQUEUE] Received message for task %d, port %d\n", id, port); if (__po_hi_gqueues_used_size[id][port] == __po_hi_gqueues_sizes[id][port]) { #if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX) pthread_mutex_unlock (&__po_hi_gqueues_mutexes[id]); #elif defined (XENO_NATIVE) rt_mutex_release (&__po_hi_gqueues_mutexes[id]); #elif defined (RTEMS_PURE) ret = rtems_semaphore_release (__po_hi_gqueues_semaphores[id]); if (ret != RTEMS_SUCCESSFUL) { __PO_HI_DEBUG_CRITICAL ("[GQUEUE] Cannot release semaphore in __po_hi_gqueue_store_in()\n"); } #elif defined (_WIN32) LeaveCriticalSection(&__po_hi_gqueues_cs[id]); #endif __PO_HI_DEBUG_CRITICAL ("[GQUEUE] QUEUE FULL, task-id=%d, port=%d\n", id, port); __DEBUGMSG ("[GQUEUE] Semaphore released (id=%d)\n", id); return __PO_HI_ERROR_QUEUE_FULL; } tmp = (__po_hi_request_t*) &__po_hi_gqueues[id][port]; size = __po_hi_gqueues_woffsets[id][port] + __po_hi_gqueues_first[id][port]; tmp = tmp + size; memcpy (tmp , request, sizeof (__po_hi_request_t)); __po_hi_gqueues_woffsets[id][port] = (__po_hi_gqueues_woffsets[id][port] + 1 ) % __po_hi_gqueues_sizes[id][port]; __po_hi_gqueues_used_size[id][port]++; __PO_HI_INSTRUMENTATION_VCD_WRITE("r%d p%d.%d\n", __po_hi_gqueues_used_size[id][port], id, port); __po_hi_gqueues_global_history[id][__po_hi_gqueues_global_history_woffset[id]] = port; __po_hi_gqueues_global_history_woffset[id] = (__po_hi_gqueues_global_history_woffset[id] + 1 ) % __po_hi_gqueues_total_fifo_size[id]; if (__po_hi_gqueues_port_is_empty[id][port] == 1) { __po_hi_gqueues_port_is_empty[id][port] = 0; __po_hi_gqueues_n_empty[id]--; } __po_hi_gqueues_queue_is_empty[id] = 0; } #if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX) pthread_mutex_unlock (&__po_hi_gqueues_mutexes[id]); int err = pthread_cond_signal (&__po_hi_gqueues_conds[id]); __DEBUGMSG("*** Releasing (%d) %d\n", id, err); #elif defined (XENO_NATIVE) rt_mutex_release (&__po_hi_gqueues_mutexes[id]); rt_cond_broadcast (&__po_hi_gqueues_conds[id]); #elif defined (_WIN32) LeaveCriticalSection(&__po_hi_gqueues_cs[id]); if (! SetEvent(__po_hi_gqueues_events[id]) ) { __DEBUGMSG("SetEvent failed (%d)\n", GetLastError()); } #elif defined (RTEMS_PURE) ret = rtems_semaphore_release (__po_hi_gqueues_semaphores[id]); if (ret != RTEMS_SUCCESSFUL) { __PO_HI_DEBUG_CRITICAL ("[GQUEUE] Cannot release semaphore in __po_hi_gqueue_store_in()\n"); } __DEBUGMSG ("[GQUEUE] Semaphore released (id=%d)\n", id); #endif __DEBUGMSG ("[GQUEUE] store_in completed\n"); return __PO_HI_SUCCESS; }