/**
*
* @param task
*
* @return TN_BOOL
*/
TN_BOOL try_lock_mutex (TN_TCB_S * task)
{
TN_MUTEX_S *mutex;
TN_BOOL need_switch_context;
need_switch_context = TN_FALSE;
mutex = get_mutex_by_task_wait_queue(task->pwait_queue);
if (mutex == TN_NULL)
return need_switch_context;
if (mutex->holder == TN_NULL)
{
if (enable_lock_mutex(task, TN_NULL))
{
queue_remove_entry(&(task->task_queue));
queue_remove_entry(&(task->block_queue));
if (task_wait_complete(task, TN_FALSE))
need_switch_context = TN_TRUE;
mutex->holder = task;
task->blk_task = TN_NULL;
queue_add_tail(&(task->mutex_queue),&(mutex->mutex_queue));
queue_add_tail(&tn_locked_mutexes_list,&(mutex->lock_mutex_queue));
}
}
return need_switch_context;
}
//----------------------------------------------------------------------------
void task_curr_to_wait_action(CDLL_QUEUE * wait_que,
int wait_reason,
unsigned long timeout)
{
task_to_non_runnable(tn_curr_run_task);
tn_curr_run_task->task_state = TSK_STATE_WAIT;
tn_curr_run_task->task_wait_reason = wait_reason;
tn_curr_run_task->tick_count = timeout;
//--- Add to the wait queue - FIFO
if(wait_que == NULL) //-- Thanks to Vavilov D.O.
{
queue_reset(&(tn_curr_run_task->task_queue));
}
else
{
queue_add_tail(wait_que, &(tn_curr_run_task->task_queue));
tn_curr_run_task->pwait_queue = wait_que;
}
//--- Add to the timers queue
if(timeout != TN_WAIT_INFINITE)
queue_add_tail(&tn_wait_timeout_list, &(tn_curr_run_task->timer_queue));
}
//----------------------------------------------------------------------------
int change_running_task_priority(TN_TCB * task, int new_priority)
{
int old_priority;
old_priority = task->priority;
//-- remove curr task from any (wait/ready) queue
queue_remove_entry(&(task->task_queue));
//-- If there are no ready tasks for the old priority
//-- clear ready bit for old priority
if(is_queue_empty(&(tn_ready_list[old_priority])))
tn_ready_to_run_bmp &= ~(1<<old_priority);
task->priority = new_priority;
//-- Add task to the end of ready queue for current priority
queue_add_tail(&(tn_ready_list[new_priority]), &(task->task_queue));
tn_ready_to_run_bmp |= 1 << new_priority;
find_next_task_to_run();
return TRUE;
}
static int osbdm_add_pathmove(
struct queue *queue,
tap_state_t *path,
int num_states)
{
assert(num_states <= 32);
struct sequence *next = queue_add_tail(queue, num_states);
if (!next) {
LOG_ERROR("BUG: can't allocate bit sequence");
return ERROR_FAIL;
}
uint32_t tms = 0;
for (int i = 0; i < num_states; i++) {
if (tap_state_transition(tap_get_state(), 1) == path[i]) {
tms |= (1 << i);
} else if (tap_state_transition(tap_get_state(), 0) == path[i]) {
tms &= ~(1 << i); /* This line not so needed */
} else {
LOG_ERROR("BUG: %s -> %s isn't a valid TAP state transition",
tap_state_name(tap_get_state()),
tap_state_name(path[i]));
return ERROR_FAIL;
}
tap_set_state(path[i]);
}
buf_set_u32(next->tms, 0, num_states, tms);
tap_set_end_state(tap_get_state());
return ERROR_OK;
}
static void test_queue_add_tail_two(void)
{
queue_node_t *root = &(q[0]), *elem1 = &(q[1]), *elem2 = &(q[2]);
elem1->data = 9084;
elem2->data = 57068;
queue_add_tail(root, elem1);
queue_add_tail(root, elem2);
TEST_ASSERT(root->next == elem1);
TEST_ASSERT_EQUAL_INT(9084, root->next->data);
TEST_ASSERT(root->next->next == elem2);
TEST_ASSERT_EQUAL_INT(57068, root->next->next->data);
TEST_ASSERT_NULL(root->next->next->next);
}
int coap_recv_queue_push_node(coap_endpoint_t * p_endpoint, coap_pkt_t * p_pkt)
{
assert(p_endpoint);
assert(p_pkt);
queue_init(&p_pkt->node);
queue_add_tail(&p_pkt->node, &p_endpoint->m_recv_queue);
p_endpoint->m_recv_queue_pkt_num++;
return 0;
}
static void test_queue_add_tail_one(void)
{
queue_node_t *root = &(q[0]), *elem = &(q[1]);
elem->data = 33893;
queue_add_tail(root, elem);
TEST_ASSERT(root->next == elem);
TEST_ASSERT_EQUAL_INT(33893, root->next->data);
TEST_ASSERT_NULL(root->next->next);
}
//----------------------------------------------------------------------------
void task_to_runnable(TN_TCB * task)
{
int priority;
priority = task->priority;
task->task_state = TSK_STATE_RUNNABLE;
task->pwait_queue = NULL;
//-- Add the task to the end of 'ready queue' for the current priority
queue_add_tail(&(tn_ready_list[priority]), &(task->task_queue));
tn_ready_to_run_bmp |= 1 << priority;
//-- less value - greater priority, so '<' operation is used here
if(priority < tn_next_task_to_run->priority)
tn_next_task_to_run = task;
}
//-----------------------------------------------------------------------------
int tn_task_create(TN_TCB * task, //-- task TCB
void (*task_func)(void *param), //-- task function
int priority, //-- task priority
unsigned int * task_stack_start, //-- task stack first addr in memory (bottom)
int task_stack_size, //-- task stack size (in sizeof(void*),not bytes)
void * param, //-- task function parameter
int option) //-- Creation option
{
TN_INTSAVE_DATA
int rc;
unsigned int * ptr_stack;
int i;
//-- Light weight checking of system tasks recreation
if((priority == 0 && ((option & TN_TASK_TIMER) == 0)) ||
(priority == TN_NUM_PRIORITY-1 && (option & TN_TASK_IDLE) == 0))
return TERR_WRONG_PARAM;
if((priority < 0 || priority > TN_NUM_PRIORITY-1) ||
task_stack_size < TN_MIN_STACK_SIZE ||
task_func == NULL || task == NULL || task_stack_start == NULL ||
task->id_task != 0) //-- recreation
return TERR_WRONG_PARAM;
rc = TERR_NO_ERR;
TN_CHECK_NON_INT_CONTEXT
if(tn_system_state == TN_ST_STATE_RUNNING)
tn_disable_interrupt();
//--- Init task TCB
task->task_func_addr = (void*)task_func;
task->task_func_param = param;
task->stk_start = (unsigned int*)task_stack_start; //-- Base address of task stack space
task->stk_size = task_stack_size; //-- Task stack size (in bytes)
task->base_priority = priority; //-- Task base priority
task->activate_count = 0; //-- Activation request count
task->id_task = TN_ID_TASK;
//-- Fill all task stack space by TN_FILL_STACK_VAL - only inside create_task
for(ptr_stack = task->stk_start,i = 0;i < task->stk_size; i++)
*ptr_stack-- = TN_FILL_STACK_VAL;
task_set_dormant_state(task);
//--- Init task stack
ptr_stack = tn_stack_init(task->task_func_addr,
task->stk_start,
task->task_func_param);
task->task_stk = ptr_stack; //-- Pointer to task top of stack,
//-- when not running
//-- Add task to created task queue
queue_add_tail(&tn_create_queue,&(task->create_queue));
tn_created_tasks_qty++;
if((option & TN_TASK_START_ON_CREATION) != 0)
task_to_runnable(task);
if(tn_system_state == TN_ST_STATE_RUNNING)
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;
}
/**
*
* @param mutex
*
* @return TN_RETVAL
*/
TN_RETVAL tnec_mutex_lock_polling (TN_MUTEX_S *mutex)
{
TN_UWORD tn_save_status_reg TN_UNUSED; /* for SR save */
TN_RETVAL rc;
if (mutex == TN_NULL)
return TERR_WRONG_PARAM;
if (mutex->id_mutex != TN_ID_MUTEX)
return TERR_NOEXS;
if (tn_is_non_task_context())
{
return TERR_WCONTEXT;
}
tn_disable_interrupt();
rc = TERR_NO_ERR;
for (;;)
{
if (tn_curr_run_task == mutex->holder)
{
rc = TERR_ILUSE;
break;
}
if (mutex->attr == TN_MUTEX_ATTR_CEILING &&
tn_curr_run_task->base_priority < mutex->ceil_priority
)
{
rc = TERR_ILUSE;
break;
}
if (mutex->holder == TN_NULL)
{
if (mutex->attr == TN_MUTEX_ATTR_CEILING)
{
if (enable_lock_mutex(tn_curr_run_task, TN_NULL))
{
mutex->holder = tn_curr_run_task;
tn_curr_run_task->blk_task = TN_NULL;
queue_add_tail(&(tn_curr_run_task->mutex_queue),&(mutex->mutex_queue));
queue_add_tail(&tn_locked_mutexes_list,&(mutex->lock_mutex_queue));
}
else
{
rc = TERR_TIMEOUT;
}
}
else if (mutex->attr == TN_MUTEX_ATTR_INHERIT)
{
mutex->holder = tn_curr_run_task;
queue_add_tail(&(tn_curr_run_task->mutex_queue),&(mutex->mutex_queue));
/* --- v.2.3 */
queue_add_tail(&tn_locked_mutexes_list,&(mutex->lock_mutex_queue));
/* --- */
}
}
else
{
rc = TERR_TIMEOUT;
}
break;
}
tn_enable_interrupt();
return rc;
}
/**
*
* @param mutex
* @param timeout
*
* @return TN_RETVAL
*/
TN_RETVAL tnec_mutex_lock (TN_MUTEX_S *mutex, TN_TIMEOUT timeout)
{
TN_UWORD tn_save_status_reg TN_UNUSED; /* for SR save */
TN_TCB_S *blk_task;
TN_RETVAL rc;
if (mutex == TN_NULL || timeout == 0)
return TERR_WRONG_PARAM;
if (mutex->id_mutex != TN_ID_MUTEX)
return TERR_NOEXS;
if (tn_is_non_task_context())
{
return TERR_WCONTEXT;
}
tn_disable_interrupt();
rc = TERR_NO_ERR;
for (;;)
{
if (tn_curr_run_task == mutex->holder)
{
rc = TERR_ILUSE;
break;
}
if (mutex->attr == TN_MUTEX_ATTR_CEILING &&
tn_curr_run_task->base_priority < mutex->ceil_priority)
{
rc = TERR_ILUSE;
break;
}
if (mutex->attr == TN_MUTEX_ATTR_CEILING)
{
if (mutex->holder == TN_NULL)
{
if (enable_lock_mutex(tn_curr_run_task, &blk_task))
{
mutex->holder = tn_curr_run_task;
tn_curr_run_task->blk_task = TN_NULL;
queue_add_tail(&(tn_curr_run_task->mutex_queue), &(mutex->mutex_queue));
queue_add_tail(&tn_locked_mutexes_list, &(mutex->lock_mutex_queue));
}
else
{
/*
Невозможно заблокировать мютекс
Протокол наследования:
Если текущий приоритет выполняемой задачи больше приоритета задачи, которая блокирует текущую
(удерживает мютекс), то блокирующая задача наследует приоритет текущей
*/
if (blk_task != TN_NULL)
{
if (tn_curr_run_task->priority < blk_task->priority)
{
set_current_priority(blk_task, tn_curr_run_task->priority);
}
}
queue_add_tail(&tn_blocked_tasks_list, &(tn_curr_run_task->block_queue));
tn_curr_run_task->blk_task = blk_task;
task_curr_to_wait_action(&(mutex->wait_queue), TSK_WAIT_REASON_MUTEX_C_BLK, timeout);
tn_enable_interrupt();
tn_switch_context();
return tn_curr_run_task->task_wait_rc;
}
}
else
{
if (tn_curr_run_task->priority < mutex->holder->priority)
{
set_current_priority(mutex->holder,tn_curr_run_task->priority);
}
task_curr_to_wait_action(&(mutex->wait_queue), TSK_WAIT_REASON_MUTEX_C, timeout);
tn_enable_interrupt();
tn_switch_context();
return tn_curr_run_task->task_wait_rc;
}
}
else if (mutex->attr == TN_MUTEX_ATTR_INHERIT)
{
if (mutex->holder == TN_NULL)
{
mutex->holder = tn_curr_run_task;
queue_add_tail(&(tn_curr_run_task->mutex_queue),&(mutex->mutex_queue));
/* --- v.2.3 */
queue_add_tail(&tn_locked_mutexes_list,&(mutex->lock_mutex_queue));
/* --- */
rc = TERR_NO_ERR;
}
else
{
if (tn_curr_run_task->priority < mutex->holder->priority)
{
set_current_priority(mutex->holder, tn_curr_run_task->priority);
}
task_curr_to_wait_action(&(mutex->wait_queue), TSK_WAIT_REASON_MUTEX_I,timeout);
tn_enable_interrupt();
tn_switch_context();
return tn_curr_run_task->task_wait_rc;
}
}
break;
}
tn_enable_interrupt();
return rc;
}
