//---------------------------------------------------------------------------- int tn_queue_delete(TN_DQUE * dque) { TN_INTSAVE_DATA CDLL_QUEUE * que; TN_TCB * task; #if TN_CHECK_PARAM if(dque == NULL) return TERR_WRONG_PARAM; if(dque->id_dque != TN_ID_DATAQUEUE) return TERR_NOEXS; #endif TN_CHECK_NON_INT_CONTEXT while(!is_queue_empty(&(dque->wait_send_list))) { tn_disable_interrupt(); //--- delete from sem wait queue que = queue_remove_head(&(dque->wait_send_list)); task = get_task_by_tsk_queue(que); if(task_wait_complete(task)) { task->task_wait_rc = TERR_DLT; tn_enable_interrupt(); tn_switch_context(); } } while(!is_queue_empty(&(dque->wait_receive_list))) { tn_disable_interrupt(); //--- delete from sem wait queue que = queue_remove_head(&(dque->wait_receive_list)); task = get_task_by_tsk_queue(que); if(task_wait_complete(task)) { task->task_wait_rc = TERR_DLT; tn_enable_interrupt(); tn_switch_context(); } } if(tn_chk_irq_disabled() == 0) // int enable tn_disable_interrupt(); dque->id_dque = 0; // Data queue not exists now tn_enable_interrupt(); return TERR_NO_ERR; }
//----------------------------------------------------------------------------- void task_to_non_runnable(TN_TCB * task) { int priority; CDLL_QUEUE * que; priority = task->priority; que = &(tn_ready_list[priority]); //-- remove the curr task from any queue (now - from ready queue) queue_remove_entry(&(task->task_queue)); if(is_queue_empty(que)) //-- No ready tasks for the curr priority { //-- remove 'ready to run' from the curr priority tn_ready_to_run_bmp &= ~(1<<priority); //-- Find highest priority ready to run - //-- at least, MSB bit must be set for the idle task find_next_task_to_run(); //-- v.2.6 } else //-- There are 'ready to run' task(s) for the curr priority { if(tn_next_task_to_run == task) tn_next_task_to_run = get_task_by_tsk_queue(que->next); } }
/** * * @param evf * * @return TN_BOOL */ TN_BOOL scan_event_waitqueue (TN_EVENT_S *evf) { CDLL_QUEUE_S *que; CDLL_QUEUE_S *tmp_que; TN_TCB_S *task; TN_BOOL fCond; TN_BOOL wcflag; TN_BOOL fBreak; /* Scan event wait queue (checking for empty - before function call) */ wcflag = TN_FALSE; fBreak = TN_FALSE; que = evf->wait_queue.next; for (;;) { task = get_task_by_tsk_queue(que); if (task->ewait_mode & TN_EVENT_WCOND_OR) { fCond = (TN_BOOL)((evf->pattern & task->ewait_pattern) != 0); } else { fCond = (TN_BOOL)((evf->pattern & task->ewait_pattern) == task->ewait_pattern); } if (fCond) { if (que->next == &(evf->wait_queue)) { queue_remove_entry(que); fBreak = TN_TRUE; } else { tmp_que = que->next; queue_remove_entry(que); que = tmp_que; } task->ewait_pattern = evf->pattern; if (task_wait_complete(task, TN_FALSE)) wcflag = TN_TRUE; if (fBreak) break; } else { if (que->next == &(evf->wait_queue)) break; else que = que->next; } } return wcflag; }
//---------------------------------------------------------------------------- void find_next_task_to_run(void) { int tmp; #ifndef USE_ASM_FFS int i; unsigned int mask; #endif #ifdef USE_ASM_FFS tmp = ffs_asm(tn_ready_to_run_bmp); tmp--; #else mask = 1; tmp = 0; for(i=0; i < TN_BITS_IN_INT; i++) //-- for each bit in bmp { if(tn_ready_to_run_bmp & mask) { tmp = i; break; } mask = mask<<1; } #endif tn_next_task_to_run = get_task_by_tsk_queue(tn_ready_list[tmp].next); }
/** * * @param dque * @param data_ptr * @param timeout * * @return TN_RETVAL */ TN_RETVAL tnnc_queue_send (TN_DQUE_S *dque, void *data_ptr, TN_TIMEOUT timeout) { TN_UWORD tn_save_status_reg TN_UNUSED; /* for SR save */ TN_RETVAL rc; CDLL_QUEUE_S *que; TN_TCB_S *task; /* Not check parameter error if (dque == TN_NULL || timeout == 0) return TERR_WRONG_PARAM; */ if (dque->id_dque != TN_ID_DATAQUEUE) return TERR_NOEXS; if (tn_is_non_task_context()) { return TERR_WCONTEXT; } tn_disable_interrupt(); if (!is_queue_empty(&(dque->wait_receive_list))) { que = queue_remove_head(&(dque->wait_receive_list)); task = get_task_by_tsk_queue(que); task->data_elem = data_ptr; if (task_wait_complete(task, TN_FALSE)) { tn_enable_interrupt(); tn_switch_context(); return TERR_NO_ERR; } rc = TERR_NO_ERR; } else { rc = dque_fifo_write(dque,data_ptr); if (rc != TERR_NO_ERR) { tn_curr_run_task->data_elem = data_ptr; task_curr_to_wait_action(&(dque->wait_send_list), TSK_WAIT_REASON_DQUE_WSEND, timeout); tn_enable_interrupt(); tn_switch_context(); return tn_curr_run_task->task_wait_rc; } } tn_enable_interrupt(); return rc; }
//---------------------------------------------------------------------------- int tn_mutex_delete(TN_MUTEX * mutex) { TN_INTSAVE_DATA CDLL_QUEUE * que; TN_TCB * task; #if TN_CHECK_PARAM if(mutex == NULL) return TERR_WRONG_PARAM; if(mutex->id_mutex != TN_ID_MUTEX) return TERR_NOEXS; #endif TN_CHECK_NON_INT_CONTEXT if(tn_curr_run_task != mutex->holder) { tn_enable_interrupt(); return TERR_ILUSE; } //-- Remove all tasks(if any) from mutex's wait queue while(!is_queue_empty(&(mutex->wait_queue))) { tn_disable_interrupt(); que = queue_remove_head(&(mutex->wait_queue)); task = get_task_by_tsk_queue(que); //-- If the task in system's blocked list, remove it if(task_wait_complete(task)) { task->task_wait_rc = TERR_DLT; tn_enable_interrupt(); tn_switch_context(); } } if(tn_chk_irq_disabled() == 0) tn_disable_interrupt(); if(mutex->holder != NULL) //-- If the mutex is locked { do_unlock_mutex(mutex); queue_reset(&(mutex->mutex_queue)); } mutex->id_mutex = 0; // Mutex not exists now tn_enable_interrupt(); return TERR_NO_ERR; }
//---------------------------------------------------------------------------- int tn_queue_send(TN_DQUE * dque, void * data_ptr, unsigned long timeout) { TN_INTSAVE_DATA int rc; CDLL_QUEUE * que; TN_TCB * task; #if TN_CHECK_PARAM if(dque == NULL || timeout == 0) return TERR_WRONG_PARAM; if(dque->id_dque != TN_ID_DATAQUEUE) return TERR_NOEXS; #endif TN_CHECK_NON_INT_CONTEXT tn_disable_interrupt(); //-- there are task(s) in the data queue's wait_receive list if(!is_queue_empty(&(dque->wait_receive_list))) { que = queue_remove_head(&(dque->wait_receive_list)); task = get_task_by_tsk_queue(que); task->data_elem = data_ptr; if(task_wait_complete(task)) { tn_enable_interrupt(); tn_switch_context(); return TERR_NO_ERR; } rc = TERR_NO_ERR; } else //-- the data queue's wait_receive list is empty { rc = dque_fifo_write(dque,data_ptr); if(rc != TERR_NO_ERR) //-- No free entries in the data queue { tn_curr_run_task->data_elem = data_ptr; //-- Store data_ptr task_curr_to_wait_action(&(dque->wait_send_list), TSK_WAIT_REASON_DQUE_WSEND, timeout); tn_enable_interrupt(); tn_switch_context(); return tn_curr_run_task->task_wait_rc; } } tn_enable_interrupt(); return rc; }
/** * * @param sem * * @return TN_RETVAL */ TN_RETVAL tnnc_sem_signal (TN_SEM_S *sem) { TN_UWORD tn_save_status_reg TN_UNUSED; /* for SR save */ TN_RETVAL rc; CDLL_QUEUE_S *que; TN_TCB_S *task; /* Not check parameter error if (sem == TN_NULL || sem->max_count == 0) return TERR_WRONG_PARAM; */ if (sem->id_sem != TN_ID_SEMAPHORE) return TERR_NOEXS; if (tn_is_non_task_context()) { return TERR_WCONTEXT; } tn_disable_interrupt(); if (!(is_queue_empty(&(sem->wait_queue)))) { que = queue_remove_head(&(sem->wait_queue)); task = get_task_by_tsk_queue(que); if (task_wait_complete(task, TN_FALSE)) { tn_enable_interrupt(); tn_switch_context(); return TERR_NO_ERR; } rc = TERR_NO_ERR; } else { if (sem->count < sem->max_count) { sem->count++; rc = TERR_NO_ERR; } else { rc = TERR_OVERFLOW; } } tn_enable_interrupt(); return rc; }
/** * * @param dque * @param data_ptr * * @return TN_RETVAL */ TN_RETVAL tnnc_queue_isend_polling (TN_DQUE_S *dque, void *data_ptr) { TN_UWORD tn_save_status_reg TN_UNUSED; TN_RETVAL rc; CDLL_QUEUE_S *que; TN_TCB_S *task; /* Not check parameter error if (dque == TN_NULL) return TERR_WRONG_PARAM; */ if (dque->id_dque != TN_ID_DATAQUEUE) return TERR_NOEXS; if (tn_is_non_sys_int_context()) { return TERR_WCONTEXT; } tn_idisable_interrupt(); if (!is_queue_empty(&(dque->wait_receive_list))) { que = queue_remove_head(&(dque->wait_receive_list)); task = get_task_by_tsk_queue(que); task->data_elem = data_ptr; if (task_wait_complete(task, TN_FALSE)) { tn_context_switch_request = TN_TRUE; tn_ienable_interrupt(); return TERR_NO_ERR; } rc = TERR_NO_ERR; } else { rc = dque_fifo_write(dque,data_ptr); if (rc != TERR_NO_ERR) { rc = TERR_TIMEOUT; } } tn_ienable_interrupt(); return rc; }
//---------------------------------------------------------------------------- // Release Semaphore Resource //---------------------------------------------------------------------------- int tn_sem_signal(TN_SEM * sem) { TN_INTSAVE_DATA int rc; //-- return code CDLL_QUEUE * que; TN_TCB * task; #if TN_CHECK_PARAM if(sem == NULL) return TERR_WRONG_PARAM; if(sem->max_count == 0) return TERR_WRONG_PARAM; if(sem->id_sem != TN_ID_SEMAPHORE) return TERR_NOEXS; #endif TN_CHECK_NON_INT_CONTEXT tn_disable_interrupt(); if(!(is_queue_empty(&(sem->wait_queue)))) { //--- delete from the sem wait queue que = queue_remove_head(&(sem->wait_queue)); task = get_task_by_tsk_queue(que); if(task_wait_complete(task)) { tn_enable_interrupt(); tn_switch_context(); return TERR_NO_ERR; } rc = TERR_NO_ERR; } else { if(sem->count < sem->max_count) { sem->count++; rc = TERR_NO_ERR; } else rc = TERR_OVERFLOW; } tn_enable_interrupt(); return rc; }
//---------------------------------------------------------------------------- int tn_queue_isend_polling(TN_DQUE * dque, void * data_ptr) { TN_INTSAVE_DATA_INT int rc; CDLL_QUEUE * que; TN_TCB * task; #if TN_CHECK_PARAM if(dque == NULL) return TERR_WRONG_PARAM; if(dque->id_dque != TN_ID_DATAQUEUE) return TERR_NOEXS; #endif TN_CHECK_INT_CONTEXT tn_idisable_interrupt(); //-- there are task(s) in the data queue's wait_receive list if(!is_queue_empty(&(dque->wait_receive_list))) { que = queue_remove_head(&(dque->wait_receive_list)); task = get_task_by_tsk_queue(que); task->data_elem = data_ptr; if(task_wait_complete(task)) { tn_ienable_interrupt(); return TERR_NO_ERR; } rc = TERR_NO_ERR; } else //-- the data queue's wait_receive list is empty { rc = dque_fifo_write(dque,data_ptr); if(rc != TERR_NO_ERR) //-- No free entries in data queue rc = TERR_TIMEOUT; //-- Just convert errorcode } tn_ienable_interrupt(); return rc; }
//---------------------------------------------------------------------------- int tn_sem_delete(TN_SEM * sem) { TN_INTSAVE_DATA CDLL_QUEUE * que; TN_TCB * task; #if TN_CHECK_PARAM if(sem == NULL) return TERR_WRONG_PARAM; if(sem->id_sem != TN_ID_SEMAPHORE) return TERR_NOEXS; #endif TN_CHECK_NON_INT_CONTEXT while(!is_queue_empty(&(sem->wait_queue))) { tn_disable_interrupt(); //--- delete from the sem wait queue que = queue_remove_head(&(sem->wait_queue)); task = get_task_by_tsk_queue(que); if(task_wait_complete(task)) { task->task_wait_rc = TERR_DLT; tn_enable_interrupt(); tn_switch_context(); } } if(tn_chk_irq_disabled() == 0) // int enable tn_disable_interrupt(); sem->id_sem = 0; // Semaphore not exists now tn_enable_interrupt(); return TERR_NO_ERR; }
//---------------------------------------------------------------------------- int tn_fmem_release(TN_FMP * fmp,void * p_data) { TN_INTSAVE_DATA CDLL_QUEUE * que; TN_TCB * task; #if TN_CHECK_PARAM if(fmp == NULL || p_data == NULL) return TERR_WRONG_PARAM; if(fmp->id_fmp != TN_ID_FSMEMORYPOOL) return TERR_NOEXS; #endif TN_CHECK_NON_INT_CONTEXT tn_disable_interrupt(); if(!is_queue_empty(&(fmp->wait_queue))) { que = queue_remove_head(&(fmp->wait_queue)); task = get_task_by_tsk_queue(que); task->data_elem = p_data; if(task_wait_complete(task)) { tn_enable_interrupt(); tn_switch_context(); return TERR_NO_ERR; } } else fm_put(fmp,p_data); tn_enable_interrupt(); return TERR_NO_ERR; }
//---------------------------------------------------------------------------- int tn_fmem_delete(TN_FMP * fmp) { TN_INTSAVE_DATA CDLL_QUEUE * que; TN_TCB * task; #if TN_CHECK_PARAM if(fmp == NULL) return TERR_WRONG_PARAM; if(fmp->id_fmp != TN_ID_FSMEMORYPOOL) return TERR_NOEXS; #endif TN_CHECK_NON_INT_CONTEXT while(!is_queue_empty(&(fmp->wait_queue))) { tn_disable_interrupt(); //--- delete from sem wait queue que = queue_remove_head(&(fmp->wait_queue)); task = get_task_by_tsk_queue(que); if(task_wait_complete(task)) { task->task_wait_rc = TERR_DLT; tn_enable_interrupt(); tn_switch_context(); } } if(tn_chk_irq_disabled() == 0) tn_disable_interrupt(); fmp->id_fmp = 0; //-- Fixed-size memory pool not exists now tn_enable_interrupt(); return TERR_NO_ERR; }
//---------------------------------------------------------------------------- int tn_event_delete(TN_EVENT * evf) { TN_INTSAVE_DATA CDLL_QUEUE * que; TN_TCB * task; #if TN_CHECK_PARAM if(evf == NULL) return TERR_WRONG_PARAM; if(evf->id_event != TN_ID_EVENT) return TERR_NOEXS; #endif TN_CHECK_NON_INT_CONTEXT tn_disable_interrupt(); // v.2.7 - thanks to Eugene Scopal while(!is_queue_empty(&(evf->wait_queue))) { //--- delete from sem wait queue que = queue_remove_head(&(evf->wait_queue)); task = get_task_by_tsk_queue(que); if(task_wait_complete(task)) { task->task_wait_rc = TERR_DLT; tn_enable_interrupt(); tn_switch_context(); tn_disable_interrupt(); // v.2.7 } } evf->id_event = 0; // Event not exists now tn_enable_interrupt(); return TERR_NO_ERR; }
//---------------------------------------------------------------------------- int tn_event_delete(TN_EVENT * evf) { TN_INTSAVE_DATA CDLL_QUEUE * que; TN_TCB * task; if(evf == NULL) return TERR_WRONG_PARAM; if(evf->id_event != TN_ID_EVENT) return TERR_NOEXS; TN_CHECK_NON_INT_CONTEXT while(!is_queue_empty(&(evf->wait_queue))) { if(tn_chk_irq_disabled() == 0) // int enable tn_disable_interrupt(); //--- delete from sem wait queue que = queue_remove_head(&(evf->wait_queue)); task = get_task_by_tsk_queue(que); if(task_wait_complete(task,FALSE)) { task->task_wait_rc = TERR_DLT; tn_enable_interrupt(); tn_switch_context(); } } if(tn_chk_irq_disabled() == 0) // int enable tn_disable_interrupt(); evf->id_event = 0; // Event not exists now tn_enable_interrupt(); return TERR_NO_ERR; }
/** * * @param dque * * @return TN_RETVAL */ TN_RETVAL tnnc_queue_delete (TN_DQUE_S *dque) { TN_UWORD tn_save_status_reg TN_UNUSED; /* for SR save */ CDLL_QUEUE_S *que; TN_TCB_S *task; /* Not check parameter error if (dque == TN_NULL) return TERR_WRONG_PARAM; */ if (dque->id_dque != TN_ID_DATAQUEUE) return TERR_NOEXS; if (tn_is_non_task_context()) { return TERR_WCONTEXT; } while (!is_queue_empty(&(dque->wait_send_list))) { /* if (tn_chk_irq_disabled() == 0) { tn_disable_interrupt(); } */ tn_disable_interrupt(); que = queue_remove_head(&(dque->wait_send_list)); task = get_task_by_tsk_queue(que); if (task_wait_complete(task, TN_FALSE)) { task->task_wait_rc = TERR_DLT; tn_enable_interrupt(); tn_switch_context(); } } while (!is_queue_empty(&(dque->wait_receive_list))) { /* if (tn_chk_irq_disabled() == 0) { tn_disable_interrupt(); } */ tn_disable_interrupt(); que = queue_remove_head(&(dque->wait_receive_list)); task = get_task_by_tsk_queue(que); if (task_wait_complete(task, TN_FALSE)) { task->task_wait_rc = TERR_DLT; tn_enable_interrupt(); tn_switch_context(); } } /* if (tn_chk_irq_disabled() == 0) { tn_disable_interrupt(); } */ tn_disable_interrupt(); dque->id_dque = TN_ID_UNKNOWN; tn_enable_interrupt(); return TERR_NO_ERR; }
//---------------------------------------------------------------------------- int tn_queue_receive(TN_DQUE * dque,void ** data_ptr,unsigned long timeout) { TN_INTSAVE_DATA int rc; //-- return code CDLL_QUEUE * que; TN_TCB * task; #if TN_CHECK_PARAM if(dque == NULL || timeout == 0 || data_ptr == NULL) return TERR_WRONG_PARAM; if(dque->id_dque != TN_ID_DATAQUEUE) return TERR_NOEXS; #endif TN_CHECK_NON_INT_CONTEXT tn_disable_interrupt(); rc = dque_fifo_read(dque,data_ptr); if(rc == TERR_NO_ERR) //-- There was entry(s) in data queue { if(!is_queue_empty(&(dque->wait_send_list))) { que = queue_remove_head(&(dque->wait_send_list)); task = get_task_by_tsk_queue(que); dque_fifo_write(dque,task->data_elem); //-- Put to data FIFO if(task_wait_complete(task)) { tn_enable_interrupt(); tn_switch_context(); return TERR_NO_ERR; } } } else //-- data FIFO is empty { if(!is_queue_empty(&(dque->wait_send_list))) { que = queue_remove_head(&(dque->wait_send_list)); task = get_task_by_tsk_queue(que); *data_ptr = task->data_elem; //-- Return to caller if(task_wait_complete(task)) { tn_enable_interrupt(); tn_switch_context(); return TERR_NO_ERR; } rc = TERR_NO_ERR; } else //-- wait_send_list is empty { task_curr_to_wait_action(&(dque->wait_receive_list), TSK_WAIT_REASON_DQUE_WRECEIVE,timeout); tn_enable_interrupt(); tn_switch_context(); //-- When returns to this point, in the data_elem have to be valid value *data_ptr = tn_curr_run_task->data_elem; //-- Return to caller return tn_curr_run_task->task_wait_rc; } } tn_enable_interrupt(); return rc; }
/** * * @param mutex * * @return TN_BOOL */ TN_BOOL do_unlock_mutex (TN_MUTEX_S *mutex) { CDLL_QUEUE_S *curr_que; TN_MUTEX_S *tmp_mutex; TN_TCB_S *task; TN_TCB_S *hi_pri_task = TN_NULL; TN_TCB_S *blk_task; TN_TCB_S *tmp_task; TN_UWORD priority; TN_UWORD pr; TN_BOOL need_switch_context = TN_FALSE; queue_remove_entry(&(mutex->mutex_queue)); queue_remove_entry(&(mutex->lock_mutex_queue)); if (is_queue_empty(&(tn_curr_run_task->mutex_queue))) { need_switch_context = set_current_priority(tn_curr_run_task, tn_curr_run_task->base_priority); } else { if (mutex->attr == TN_MUTEX_ATTR_INHERIT || mutex->attr == TN_MUTEX_ATTR_CEILING ) { pr = tn_curr_run_task->base_priority; curr_que = tn_curr_run_task->mutex_queue.next; for (;;) { tmp_mutex = get_mutex_by_mutex_queue(curr_que); pr = find_max_blocked_priority(tmp_mutex,pr); if (curr_que->next == &(tn_curr_run_task->mutex_queue)) break; else { curr_que = curr_que->next; } } if (pr != tn_curr_run_task->priority) { need_switch_context = set_current_priority(tn_curr_run_task,pr); } } } if (is_queue_empty(&(mutex->wait_queue))) { mutex->holder = TN_NULL; hi_pri_task = tn_curr_run_task; } else { if (mutex->attr == TN_MUTEX_ATTR_CEILING) { priority = TN_NUM_PRIORITY - 1; curr_que = mutex->wait_queue.next; for (;;) { task = get_task_by_tsk_queue(curr_que); if (task->priority < priority) { hi_pri_task = task; priority = task->priority; } if (curr_que->next == &(mutex->wait_queue)) break; else curr_que = curr_que->next; } if (enable_lock_mutex(hi_pri_task,&blk_task)) { queue_remove_entry(&(hi_pri_task->task_queue)); remove_task_from_blocked_list(hi_pri_task); if (task_wait_complete(hi_pri_task, TN_FALSE)) need_switch_context = TN_TRUE; mutex->holder = hi_pri_task; hi_pri_task->blk_task = TN_NULL; queue_add_tail(&(hi_pri_task->mutex_queue),&(mutex->mutex_queue)); queue_add_tail(&tn_locked_mutexes_list,&(mutex->lock_mutex_queue)); } else { if (!queue_contains_entry(&tn_blocked_tasks_list, &(hi_pri_task->block_queue))) { queue_add_tail(&tn_blocked_tasks_list,&(hi_pri_task->block_queue)); hi_pri_task->task_wait_reason = TSK_WAIT_REASON_MUTEX_C_BLK; hi_pri_task->blk_task = blk_task; } } } else if (mutex->attr == TN_MUTEX_ATTR_INHERIT) { curr_que = queue_remove_head(&(mutex->wait_queue)); task = get_task_by_tsk_queue(curr_que); if (task_wait_complete(task, TN_FALSE)) need_switch_context = TN_TRUE; mutex->holder = task; queue_add_tail(&(task->mutex_queue),&(mutex->mutex_queue)); /*--- v.2.3 */ queue_add_tail(&tn_locked_mutexes_list,&(mutex->lock_mutex_queue)); /*--- */ } } if (mutex->holder == TN_NULL && mutex->attr == TN_MUTEX_ATTR_CEILING) { if (!is_queue_empty(&tn_blocked_tasks_list)) { curr_que = tn_blocked_tasks_list.next; for (;;) { tmp_task = get_task_by_block_queue(curr_que); if (tmp_task != hi_pri_task) { if (try_lock_mutex(tmp_task)) { need_switch_context = TN_TRUE; } } if (curr_que->next == &tn_blocked_tasks_list) break; else curr_que = curr_que->next; } } } return need_switch_context; }
//---------------------------------------------------------------------------- int tn_queue_ireceive(TN_DQUE * dque,void ** data_ptr) { TN_INTSAVE_DATA_INT int rc; CDLL_QUEUE * que; TN_TCB * task; #if TN_CHECK_PARAM if(dque == NULL || data_ptr == NULL) return TERR_WRONG_PARAM; if(dque->id_dque != TN_ID_DATAQUEUE) return TERR_NOEXS; #endif TN_CHECK_INT_CONTEXT tn_idisable_interrupt(); rc = dque_fifo_read(dque,data_ptr); if(rc == TERR_NO_ERR) //-- There was entry(s) in data queue { if(!is_queue_empty(&(dque->wait_send_list))) { que = queue_remove_head(&(dque->wait_send_list)); task = get_task_by_tsk_queue(que); dque_fifo_write(dque,task->data_elem); //-- Put to data FIFO if(task_wait_complete(task)) { tn_ienable_interrupt(); return TERR_NO_ERR; } } } else //-- data FIFO is empty { if(!is_queue_empty(&(dque->wait_send_list))) { que = queue_remove_head(&(dque->wait_send_list)); task = get_task_by_tsk_queue(que); *data_ptr = task->data_elem; //-- Return to caller if(task_wait_complete(task)) { tn_ienable_interrupt(); return TERR_NO_ERR; } rc = TERR_NO_ERR; } else { rc = TERR_TIMEOUT; } } tn_ienable_interrupt(); return rc; }