//----------------------------------------------------------------------------
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;
}
int pthread_cond_broadcast(struct pthread_cond_t *cond)
{
unsigned old_state = disableIRQ();
int other_prio = -1;
while (1) {
queue_node_t *head = queue_remove_head(&(cond->queue));
if (head == NULL) {
break;
}
tcb_t *other_thread = (tcb_t *) sched_threads[head->data];
if (other_thread) {
other_prio = max_prio(other_prio, other_thread->priority);
sched_set_status(other_thread, STATUS_PENDING);
}
head->data = -1u;
}
restoreIRQ(old_state);
if (other_prio >= 0) {
sched_switch(sched_active_thread->priority, other_prio);
}
return 0;
}
void vtimer_callback(void *ptr)
{
DEBUG("vtimer_callback ptr=%p\n", ptr);
(void) ptr;
in_callback = true;
hwtimer_id = -1;
/* get the vtimer that fired */
vtimer_t *timer = (vtimer_t *)queue_remove_head(&shortterm_queue_root);
if (timer) {
#if ENABLE_DEBUG
vtimer_print(timer);
#endif
DEBUG("vtimer_callback(): Shooting %" PRIu32 ".\n", timer->absolute.microseconds);
/* shoot timer */
timer->action(timer);
}
else {
DEBUG("vtimer_callback(): spurious call.\n");
}
in_callback = false;
update_shortterm();
}
void vtimer_callback(void *ptr) {
vtimer_t *timer;
in_callback = true;
hwtimer_id = -1;
timer = (vtimer_t *)queue_remove_head(&shortterm_queue_root);
#ifdef ENABLE_DEBUG
vtimer_print(timer);
#endif
DEBUG("vtimer_callback(): Shooting %lu.\n", timer->absolute.microseconds);
/* shoot timer */
if (timer->action == (void*) msg_send_int) {
msg_t msg;
msg.type = MSG_TIMER;
msg.content.value = (unsigned int) timer->arg;
msg_send_int(&msg, timer->pid);
} else {
timer->action(timer->arg);
}
in_callback = false;
update_shortterm();
}
static void test_queue_remove_head_empty(void)
{
queue_node_t *root = &(q[0]), *res;
res = queue_remove_head(root);
TEST_ASSERT_NULL(res);
}
extern void
wakeup_queue (uint16 * q)
{
uint16 head;
while ((head = queue_remove_head (q)))
wakeup (head);
}
static void test_queue_remove_head_one(void)
{
queue_node_t *root = &(q[0]), *elem = &(q[1]), *res;
elem->data = 62801;
queue_add_head(root, elem);
res = queue_remove_head(root);
TEST_ASSERT(res == elem);
TEST_ASSERT_EQUAL_INT(62801, res->data);
res = queue_remove_head(root);
TEST_ASSERT_NULL(res);
}
/**
*
* @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;
}
static void *
dequeue(struct queue *q)
{
struct msg *m;
if (isempty_queue(q))
err_quit("dequeue: the queue is empty");
m = queue_first(q);
queue_remove_head(q);
return (m);
}
/**
*
* @param attr
*/
void tnec_task_exit (TN_UWORD attr)
{
TN_UWORD tn_save_status_reg TN_UNUSED; /* for SR save */
TN_TCB_S *task;
CDLL_QUEUE_S *que;
TN_MUTEX_S *mutex;
TN_UWORD *ptr_stack;
if (tn_is_non_task_context())
return;
tn_save_sr(); /* just for PIC24/dsPIC */
tn_disable_interrupt();
while (!is_queue_empty(&(tn_curr_run_task->mutex_queue)))
{
que = queue_remove_head(&(tn_curr_run_task->mutex_queue));
mutex = get_mutex_by_mutex_queue(que);
do_unlock_mutex(mutex);
}
task = tn_curr_run_task;
task_to_non_runnable(tn_curr_run_task);
task_set_dormant_state(task);
ptr_stack = tn_stack_init(task->task_func_addr,
task->stk_start,
task->stk_size,
task->task_func_param
);
task->task_stk = ptr_stack;
if (task->activate_count > 0)
{
task->activate_count--;
task_to_runnable(task);
}
else
{
/* Alex B. - delete task only if activate counter = 0 ! */
if (attr == TN_EXIT_AND_DELETE_TASK)
{
queue_remove_entry(&(task->create_queue));
tn_created_tasks_qty--;
task->id_task = TN_ID_UNKNOWN;
}
}
tn_switch_context_exit(tn_save_status_reg); /* Exit from task with restore saved SR*/
}
/**
*
* @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;
}
//----------------------------------------------------------------------------
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 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 sem_post(sem_t *sem)
{
int old_state = disableIRQ();
++sem->value;
queue_node_t *next = queue_remove_head(&sem->queue);
if (next) {
tcb_t *next_process = (tcb_t*) next->data;
DEBUG("%s: waking up %s\n", active_thread->name, next_process->name);
sched_set_status(next_process, STATUS_PENDING);
sched_switch(active_thread->priority, next_process->priority);
}
restoreIRQ(old_state);
return 1;
}
//----------------------------------------------------------------------------
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;
}
void vtimer_tick(void *ptr) {
DEBUG("vtimer_tick().");
seconds += SECONDS_PER_TICK;
longterm_tick_start = longterm_tick_timer.absolute.microseconds;
longterm_tick_timer.absolute.microseconds = longterm_tick_timer.absolute.microseconds + MICROSECONDS_PER_TICK;
set_shortterm(&longterm_tick_timer);
while (longterm_queue_root.next) {
vtimer_t *timer = (vtimer_t*) longterm_queue_root.next;
if (timer->absolute.seconds == seconds) {
timer = (vtimer_t*) queue_remove_head(&longterm_queue_root);
set_shortterm(timer);
} else {
break;
}
}
update_shortterm();
}
void hw_remove(struct net_adapter *adapter)
{
struct buffer_descriptor *dsc;
/* Free the pending data packets and control packets */
spin_lock(&adapter->hw.q_send.lock);
while (!queue_empty(adapter->hw.q_send.head)) {
dump_debug("Freeing q_send");
dsc = (struct buffer_descriptor *)
queue_get_head(adapter->hw.q_send.head);
if (!dsc) {
dump_debug("Fail...node is null");
continue;
}
queue_remove_head(adapter->hw.q_send.head);
kfree(dsc->buffer);
kfree(dsc);
}
spin_unlock(&adapter->hw.q_send.lock);
}
void mutex_unlock(struct mutex_t *mutex)
{
DEBUG("%s: unlocking mutex. val: %u pid: %u\n", active_thread->name, mutex->val, thread_pid);
int irqstate = disableIRQ();
if (mutex->val != 0) {
if (mutex->queue.next) {
queue_node_t *next = queue_remove_head(&(mutex->queue));
tcb_t *process = (tcb_t*) next->data;
DEBUG("%s: waking up waiter.\n", process->name);
sched_set_status(process, STATUS_PENDING);
sched_switch(active_thread->priority, process->priority);
}
else {
mutex->val = 0;
}
}
restoreIRQ(irqstate);
}
//----------------------------------------------------------------------------
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_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_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 pthread_cond_signal(struct pthread_cond_t *cond)
{
unsigned old_state = disableIRQ();
queue_node_t *head = queue_remove_head(&(cond->queue));
int other_prio = -1;
if (head != NULL) {
tcb_t *other_thread = (tcb_t *) sched_threads[head->data];
if (other_thread) {
other_prio = other_thread->priority;
sched_set_status(other_thread, STATUS_PENDING);
}
head->data = -1u;
}
restoreIRQ(old_state);
if (other_prio >= 0) {
sched_switch(sched_active_thread->priority, other_prio);
}
return 0;
}
void vtimer_callback_tick(vtimer_t *timer)
{
(void) timer;
DEBUG("vtimer_callback_tick().\n");
seconds += SECONDS_PER_TICK;
longterm_tick_start = longterm_tick_timer.absolute.microseconds;
longterm_tick_timer.absolute.microseconds += MICROSECONDS_PER_TICK;
set_shortterm(&longterm_tick_timer);
while (longterm_queue_root.first) {
vtimer_t *timer = (vtimer_t *) longterm_queue_root.first;
if (timer->absolute.seconds == seconds) {
queue_remove_head(&longterm_queue_root);
set_shortterm(timer);
}
else {
break;
}
}
}
//----------------------------------------------------------------------------
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;
}
//----------------------------------------------------------------------------
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;
}
void hw_transmit_thread(struct work_struct *work)
{
struct buffer_descriptor *dsc;
struct hw_private_packet hdr;
struct net_adapter *adapter;
int nRet = 0;
adapter = container_of(work, struct net_adapter, transmit_work);
struct wimax_cfg *g_cfg = adapter->pdata->g_cfg;
wake_lock_timeout(&g_cfg->wimax_rxtx_lock, 0.2 * HZ);
mutex_lock(&adapter->rx_lock);
if (!gpio_get_value(WIMAX_EN)) {
dump_debug("WiMAX Power OFF!! (TX)");
adapter->halted = TRUE;
return;
}
/* prevent WiMAX modem suspend during tx phase */
mutex_lock(&g_cfg->suspend_mutex);
hw_device_wakeup(adapter);
while (!queue_empty(adapter->hw.q_send.head)) {
if (adapter->halted) {
/* send stop message */
hdr.id0 = 'W';
hdr.id1 = 'P';
hdr.code = HwCodeHaltedIndication;
hdr.value = 0;
if (sd_send(adapter, (unsigned char *)&hdr,
sizeof(struct hw_private_packet)))
dump_debug("halted,"
" send HaltIndication to FW err");
break;
}
dsc = (struct buffer_descriptor *)
queue_get_head(adapter->hw.q_send.head);
if (!dsc->buffer) {
dump_debug("dsc->buffer is NULL");
break;
}
if (!dsc) {
dump_debug("Fail...node is null");
mutex_unlock(&g_cfg->suspend_mutex);
break;
}
sdio_claim_host(adapter->func);
nRet = sd_send_data(adapter, dsc);
sdio_release_host(adapter->func);
queue_remove_head(adapter->hw.q_send.head);
kfree(dsc->buffer);
kfree(dsc);
if (nRet != STATUS_SUCCESS) {
dump_debug("SendData Fail******");
++adapter->XmitErr;
if (nRet == -ENOMEDIUM || nRet == /*-ETIMEOUT*/-110) {
adapter->halted = TRUE;
break;
}
}
}
mutex_unlock(&g_cfg->suspend_mutex);
mutex_unlock(&adapter->rx_lock);
return ;
}
static int _msg_receive(msg_t *m, int block)
{
dINT();
DEBUG("_msg_receive: %s: _msg_receive.\n", sched_active_thread->name);
tcb_t *me = (tcb_t*) sched_threads[sched_active_pid];
int queue_index = -1;
if (me->msg_array) {
queue_index = cib_get(&(me->msg_queue));
}
/* no message, fail */
if ((!block) && (queue_index == -1)) {
eINT();
return -1;
}
if (queue_index >= 0) {
DEBUG("_msg_receive: %s: _msg_receive(): We've got a queued message.\n", sched_active_thread->name);
*m = me->msg_array[queue_index];
}
else {
me->wait_data = (void *) m;
}
queue_node_t *node = queue_remove_head(&(me->msg_waiters));
if (node == NULL) {
DEBUG("_msg_receive: %s: _msg_receive(): No thread in waiting list.\n", sched_active_thread->name);
if (queue_index < 0) {
DEBUG("_msg_receive(): %s: No msg in queue. Going blocked.\n", sched_active_thread->name);
sched_set_status(me, STATUS_RECEIVE_BLOCKED);
eINT();
thread_yield();
/* sender copied message */
}
else {
eINT();
}
return 1;
}
else {
DEBUG("_msg_receive: %s: _msg_receive(): Waking up waiting thread.\n", sched_active_thread->name);
tcb_t *sender = (tcb_t*) node->data;
if (queue_index >= 0) {
/* We've already got a message from the queue. As there is a
* waiter, take it's message into the just freed queue space.
*/
m = &(me->msg_array[cib_put(&(me->msg_queue))]);
}
/* copy msg */
msg_t *sender_msg = (msg_t*) sender->wait_data;
*m = *sender_msg;
/* remove sender from queue */
if (sender->status != STATUS_REPLY_BLOCKED) {
sender->wait_data = NULL;
sched_set_status(sender, STATUS_PENDING);
}
eINT();
return 1;
}
DEBUG("This should have never been reached!\n");
}