U32 static adc_read_data(U8 adc_channel)
{
U32 adc_value_ave = 0;
T3CLRI = 0x55;
ADCCP = adc_channel;
ADCCON |= (1UL<<7);
tsk_lock ();
while (!ADCSTA);
adc_value_ave += (ADCDAT >> 16);
adc_value_ave = 0;
tsk_unlock ();
for (U16 i=0;i<128;i++)
{
tsk_lock ();
while (!ADCSTA);
adc_value_ave += (ADCDAT >> 16);
tsk_unlock ();
}
ADCCON &= ~(1UL<<7);
adc_value_ave >>= 7;
return adc_value_ave;
}
void os_evt_set (U16 event_flags, OS_TID task_id) {
/* Set one or more event flags of a selectable task. */
P_TCB p_tcb;
p_tcb = os_active_TCB[task_id-1];
if (p_tcb == NULL) {
return;
}
tsk_lock();
p_tcb->events |= event_flags;
event_flags = p_tcb->waits;
/* If the task is not waiting for an event, it should not be put */
/* to ready state. */
if (p_tcb->state == WAIT_AND) {
/* Check for AND-connected events */
if ((p_tcb->events & event_flags) == event_flags) {
goto wkup;
}
}
if (p_tcb->state == WAIT_OR) {
/* Check for OR-connected events */
if (p_tcb->events & event_flags) {
p_tcb->waits &= p_tcb->events;
wkup: p_tcb->events &= ~event_flags;
os_rmv_dly (p_tcb);
p_tcb->state = READY;
p_tcb->ret_val = OS_R_EVT;
os_dispatch (p_tcb);
}
}
tsk_unlock();
}
int SwitchPortCPU::outputPacket(PriPacket& pkg) {
if( pkg.getSourcePort() == CPU_Port_Sn ) {
return -1;
}
uint16 RxLen = 0;
uint8* RxData = pkg.getStdStream(&RxLen);
if( RxLen < 30 || RxData == 0 ) {
std::cout << "SwitchPortCPU send error: len " << (int)RxLen << std::endl;
return -1;
}
#ifdef EZ_DEBUG
trace->sendOnePkg();
#endif
tsk_lock();
OS_FRAME* frame = alloc_mem (RxLen | 0x80000000);
tsk_unlock();
if (frame != NULL) {
memcpy( &frame->data[0], RxData, RxLen);
put_in_queue(frame);
os_evt_set(0x0001, t_tcpTask);
return 1;
}
else {
std::cout << "SwitchPortCPU::outputPacket() alloc_mem(" << (int)RxLen << ") error" << std::endl;
}
return -1;
}
OS_TID os_tsk_create0 (FUNCP task, U32 prio_stksz, void *stk, void *argv) {
/* Start a new task declared with "task". */
P_TCB task_context;
tsk_lock();
/* Priority 0 is reserved for idle task! */
if ((prio_stksz & 0xFF) == 0) {
prio_stksz += 1;
}
task_context = _alloc_box (mp_tcb);
if (task_context == NULL) {
tsk_unlock();
return (0);
}
/* If "size != 0" use a private user provided stack. */
task_context->stack = stk;
task_context->priv_stack = prio_stksz >> 8;
/* Pass parameter 'argv' to 'os_init_context' */
task_context->p_msg = argv;
/* For 'size == 0' system allocates the user stack from the memory pool. */
os_init_context (task_context, prio_stksz & 0xFF, task, __TRUE);
/* Find a free entry in 'os_active_TCB' table. */
os_set_TID (task_context);
rt_notify (task_context->ptask, (__TRUE << 8) | task_context->task_id);
os_dispatch (task_context);
tsk_unlock();
return ((OS_TID)task_context->task_id);
}
void os_evt_clr (U16 clear_flags, OS_TID task_id) {
/* Clear one or more event flags (identified by "clear_flags") of a */
/* selectable task (identified by "task"). */
P_TCB p_tcb = os_active_TCB[task_id-1];
if (p_tcb == NULL) {
return;
}
tsk_lock ();
p_tcb->events &= ~clear_flags;
tsk_unlock ();
}
__naked void os_sys_init1 (void) {
/* Initialize system and start up a first task. */
U32 i;
rt_init ();
tsk_lock ();
/* Initialize dynamic memory and task TCB pointers to NULL. */
for (i = 0; i < os_maxtaskrun; i++) {
os_active_TCB[i] = NULL;
}
_init_box (&mp_tcb, mp_tcb_size, sizeof(struct OS_TCB));
_init_box (&mp_stk, mp_stk_size, BOX_ALIGN_8 | (U16)(os_stackinfo));
_init_box ((U32 *)m_tmr, mp_tmr_size, sizeof(struct OS_TMR));
/* Set up TCB of idle demon */
os_idle_TCB.task_id = 255;
os_idle_TCB.priv_stack = 0;
os_init_context (&os_idle_TCB, 0, os_idle_demon, __FALSE);
/* Set up ready list: initially empty */
os_rdy.cb_type = HCB;
os_rdy.p_lnk = NULL;
/* Set up delay list: initially empty */
os_dly.cb_type = HCB;
os_dly.p_dlnk = NULL;
os_dly.p_blnk = NULL;
os_dly.delta_time = 0;
/* Fix SP and system variables to assume idle task is running */
/* Transform main program into idle task by assuming idle TCB */
os_set_env (&os_idle_TCB);
os_runtask = &os_idle_TCB;
os_runtask->state = RUNNING;
/* Initialize ps queue */
os_psq->first = 0;
os_psq->last = 0;
os_psq->size = os_fifo_size;
/* Initialize system clock timer */
os_tmr_init ();
os_init_robin ();
/* Start up first user task before entering the endless loop */
os_sys_run ((FUNCP)os_tsk_create0);
/* Call body of idle task: contains an endless loop */
os_idle_demon();
/* This point never reached if idle task contains endless loop */
for (;;);
}
void os_tsk_pass (void) {
/* Allow tasks of same priority level to run cooperatively.*/
P_TCB p_new;
tsk_lock();
p_new = os_get_same_rdy_prio();
if (p_new != NULL) {
os_put_prio ((P_XCB)&os_rdy, os_runtask);
os_runtask->state = READY;
os_switch_tasks (p_new);
}
tsk_unlock();
}
OS_RESULT os_mbx_send (OS_ID mailbox, void *p_msg, U16 timeout) {
/* Send message to a mailbox */
P_MCB p_MCB = mailbox;
P_TCB p_TCB;
tsk_lock();
m:if (p_MCB->p_lnk != NULL && p_MCB->count == 0) {
/* A task is waiting for message */
p_TCB = os_get_first ((P_XCB)p_MCB);
p_TCB->p_msg = p_msg;
p_TCB->ret_val = OS_R_MBX;
os_rmv_dly (p_TCB);
os_dispatch (p_TCB);
}
else {
/* Store message in mailbox queue */
if (p_MCB->count == p_MCB->size) {
/* No free message entry, wait for one. If message queue is full, */
/* then no task is waiting for message. The 'p_MCB->p_lnk' list */
/* pointer can now be reused for send message waits task list. */
if (timeout == 0) {
goto wto;
}
if (p_MCB->p_lnk != NULL) {
os_put_prio ((P_XCB)p_MCB, os_runtask);
}
else {
p_MCB->p_lnk = os_runtask;
os_runtask->p_lnk = NULL;
os_runtask->p_rlnk = (P_TCB)p_MCB;
/* Signal the 'isr_mbx_receive ()' that the task is waiting */
/* to send a message */
p_MCB->isr_st = 1;
}
if (os_block(timeout, WAIT_MBX) == OS_R_TMO) {
wto: tsk_unlock();
return (OS_R_TMO);
}
/* A message has been fetched, check the mailbox again. */
goto m;
}
/* Yes, there is a free entry in a mailbox. */
p_MCB->msg[p_MCB->first] = p_msg;
_incw (&p_MCB->count);
if (++p_MCB->first == p_MCB->size) {
p_MCB->first = 0;
}
}
tsk_unlock();
return (OS_R_OK);
}
OS_RESULT os_mut_release (OS_ID mutex) {
/* Release a mutex object */
P_MUCB p_MCB = mutex;
P_TCB p_TCB;
tsk_lock();
if (p_MCB->level == 0 || p_MCB->owner != os_runtask) {
/* Unbalanced mutex release or task is not the owner */
tsk_unlock ();
return (OS_R_NOK);
}
if (--p_MCB->level != 0) {
goto ok;
}
/* Restore owner task's priority. */
os_runtask->prio = p_MCB->prio;
if (p_MCB->p_lnk != NULL) {
/* A task is waiting for this mutex. */
p_TCB = os_get_first ((P_XCB)p_MCB);
p_TCB->ret_val = OS_R_MUT;
os_rmv_dly (p_TCB);
/* A waiting task becomes the owner of this mutex. */
p_MCB->level = 1;
p_MCB->owner = p_TCB;
p_MCB->prio = p_TCB->prio;
/* Priority inversion, check which task continues. */
if (os_runtask->prio >= os_rdy_prio()) {
os_dispatch (p_TCB);
}
else {
/* Ready task has higher priority than running task. */
os_put_prio (&os_rdy, os_runtask);
os_put_prio (&os_rdy, p_TCB);
os_runtask->state = READY;
p_TCB->state = READY;
os_dispatch (NULL);
}
}
else {
/* Check if own priority raised by priority inversion. */
if (os_rdy_prio() > os_runtask->prio) {
os_put_prio (&os_rdy, os_runtask);
os_runtask->state = READY;
os_dispatch (NULL);
}
}
ok:tsk_unlock();
return (OS_R_OK);
}
void os_mbx_init (OS_ID mailbox, U16 mbx_size) {
/* Initialize a mailbox */
P_MCB p_MCB = mailbox;
tsk_lock();
p_MCB->cb_type = MCB;
p_MCB->isr_st = 0;
p_MCB->p_lnk = NULL;
p_MCB->first = 0;
p_MCB->last = 0;
p_MCB->count = 0;
p_MCB->size = (mbx_size + sizeof(void *) - sizeof(struct OS_MCB)) /
(U32)sizeof (void *);
tsk_unlock();
}
void os_mut_init (OS_ID mutex) {
/* Initialize a mutex object */
P_MUCB p_MCB = mutex;
if (os_runtask) {
tsk_lock();
}
p_MCB->cb_type = MUCB;
p_MCB->prio = 0;
p_MCB->level = 0;
p_MCB->p_lnk = NULL;
p_MCB->owner = NULL;
if (os_runtask) {
tsk_unlock();
}
}
OS_RESULT os_mbx_wait (OS_ID mailbox, void **message, U16 timeout) {
/* Receive a message; possibly wait for it */
P_MCB p_MCB = mailbox;
P_TCB p_TCB;
tsk_lock();
/* If a message is available in the fifo buffer */
/* remove it from the fifo buffer and return. */
if (p_MCB->count) {
*message = p_MCB->msg[p_MCB->last];
_decw (&p_MCB->count);
if (++p_MCB->last == p_MCB->size) {
p_MCB->last = 0;
}
if (p_MCB->p_lnk != NULL) {
/* A task is waiting to send message */
p_TCB = os_get_first ((P_XCB)p_MCB);
os_rmv_dly (p_TCB);
os_dispatch (p_TCB);
}
tsk_unlock();
return (OS_R_OK);
}
/* No message available: wait for one */
if (timeout == 0) {
goto rto;
}
if (p_MCB->p_lnk != NULL) {
os_put_prio ((P_XCB)p_MCB, os_runtask);
}
else {
p_MCB->p_lnk = os_runtask;
os_runtask->p_lnk = NULL;
os_runtask->p_rlnk = (P_TCB)p_MCB;
}
if (os_block(timeout, WAIT_MBX) == OS_R_TMO) {
rto:tsk_unlock();
*message = NULL;
return (OS_R_TMO);
}
tsk_unlock();
*message = os_runtask->p_msg;
return (OS_R_MBX);
}
OS_RESULT os_mut_wait (OS_ID mutex, U16 timeout) {
/* Wait for a mutex, continue when mutex is free. */
P_MUCB p_MCB = mutex;
tsk_lock();
if (p_MCB->level == 0) {
p_MCB->owner = os_runtask;
p_MCB->prio = os_runtask->prio;
goto inc;
}
if (p_MCB->owner == os_runtask) {
/* OK, running task is the owner of this mutex. */
inc:p_MCB->level++;
tsk_unlock();
return (OS_R_OK);
}
/* Mutex owned by another task, wait until released. */
if (timeout == 0) {
goto rto;
}
/* Raise the owner task priority if lower than current priority. */
/* This priority inversion is called priority inheritance. */
if (p_MCB->prio < os_runtask->prio) {
p_MCB->owner->prio = os_runtask->prio;
os_resort_prio (p_MCB->owner);
}
if (p_MCB->p_lnk != NULL) {
os_put_prio ((P_XCB)p_MCB, os_runtask);
}
else {
p_MCB->p_lnk = os_runtask;
os_runtask->p_lnk = NULL;
os_runtask->p_rlnk = (P_TCB)p_MCB;
}
if (os_block(timeout, WAIT_MUT) == OS_R_TMO) {
rto:tsk_unlock();
return (OS_R_TMO);
}
/* A mutex has been released. */
tsk_unlock();
return (OS_R_MUT);
}
OS_RESULT os_tsk_prio (OS_TID task_id, U8 new_prio) {
/* Change execution priority of a task to "new_prio". */
P_TCB p_task;
tsk_lock();
if (task_id == 0) {
/* Change execution priority of calling task. */
os_runtask->prio = new_prio;
os_runtask->prio_base = new_prio;
run:if (os_rdy_prio() > new_prio) {
os_put_prio (&os_rdy, os_runtask);
os_runtask->state = READY;
os_dispatch (NULL);
}
tsk_unlock();
return (OS_R_OK);
}
/* Find the task in the "os_active_TCB" array. */
if (task_id > os_maxtaskrun || os_active_TCB[task_id-1] == NULL) {
/* Task with "task_id" not found or not started. */
tsk_unlock ();
return (OS_R_NOK);
}
p_task = os_active_TCB[task_id-1];
p_task->prio = new_prio;
p_task->prio_base = new_prio;
if (p_task == os_runtask) {
goto run;
}
os_resort_prio (p_task);
if (p_task->state == READY) {
/* Task enqueued in a ready list. */
p_task = os_get_first (&os_rdy);
os_dispatch (p_task);
}
tsk_unlock ();
return (OS_R_OK);
}
OS_RESULT os_evt_wait (U16 wait_flags, U16 timeout, BOOL and_wait) {
/* Wait for one or more event flags with optional time-out. */
/* "wait_flags" identifies the flags to wait for. */
/* "timeout" is the time-out limit in system ticks (0xffff if no time-out) */
/* "and_wait" specifies the AND-ing of "wait_flags" as condition to be met */
/* to complete the wait. (OR-ing if set to 0). */
U32 block_state;
tsk_lock();
if (and_wait) {
/* Check for AND-connected events */
if ((os_runtask->events & wait_flags) == wait_flags) {
os_runtask->events &= ~wait_flags;
tsk_unlock();
return (OS_R_EVT);
}
block_state = WAIT_AND;
}
else {
/* Check for OR-connected events */
if (os_runtask->events & wait_flags) {
os_runtask->waits = os_runtask->events & wait_flags;
os_runtask->events &= ~wait_flags;
tsk_unlock();
return (OS_R_EVT);
}
block_state = WAIT_OR;
}
/* Task has to wait */
os_runtask->waits = wait_flags;
if (os_block(timeout, (U8)block_state) == OS_R_TMO) {
tsk_unlock();
return (OS_R_TMO);
}
tsk_unlock();
return (OS_R_EVT);
}
OS_RESULT os_tsk_delete (OS_TID task_id) {
/* Terminate the task identified with "task_id". */
P_TCB task_context;
P_TCB p_TCB;
P_MUCB p_MCB, p_MCB0;
tsk_lock();
if (task_id == 0 || task_id == os_runtask->task_id) {
/* Terminate itself. */
os_runtask->state = INACTIVE;
os_stk_check (0);
p_MCB = os_runtask->p_mlnk;
while (p_MCB) {
/* Release mutexes owned by this task */
if (p_MCB->p_lnk) {
/* A task is waiting for mutex. */
p_TCB = os_get_first ((P_XCB)p_MCB);
p_TCB->ret_val = OS_R_MUT;
os_rmv_dly (p_TCB);
p_TCB->state = READY;
os_put_prio (&os_rdy, p_TCB);
/* A waiting task becomes the owner of this mutex. */
p_MCB0 = p_MCB;
p_MCB->level = 1;
p_MCB->owner = p_TCB;
p_MCB->p_mlnk = p_TCB->p_mlnk;
p_TCB->p_mlnk = p_MCB;
p_MCB = p_MCB0->p_mlnk;
}
else {
p_MCB = p_MCB->p_mlnk;
}
}
os_active_TCB[os_runtask->task_id-1] = NULL;
_free_box (mp_stk, os_runtask->stack);
os_runtask->stack = NULL;
rt_notify (os_runtask->ptask, (__FALSE << 8) | os_runtask->task_id);
_free_box (mp_tcb, os_runtask);
os_runtask = NULL;
os_dispatch (NULL);
/* The program should never come to this point. */
}
else {
/* Find the task in the "os_active_TCB" array. */
if (task_id > os_maxtaskrun || os_active_TCB[task_id-1] == NULL) {
/* Task with "task_id" not found or not started. */
tsk_unlock ();
return (OS_R_NOK);
}
task_context = os_active_TCB[task_id-1];
os_rmv_list (task_context);
os_rmv_dly (task_context);
p_MCB = task_context->p_mlnk;
while (p_MCB) {
/* Release mutexes owned by this task */
if (p_MCB->p_lnk) {
/* A task is waiting for mutex. */
p_TCB = os_get_first ((P_XCB)p_MCB);
p_TCB->ret_val = OS_R_MUT;
os_rmv_dly (p_TCB);
p_TCB->state = READY;
os_put_prio (&os_rdy, p_TCB);
/* A waiting task becomes the owner of this mutex. */
p_MCB0 = p_MCB;
p_MCB->level = 1;
p_MCB->owner = p_TCB;
p_MCB->p_mlnk = p_TCB->p_mlnk;
p_TCB->p_mlnk = p_MCB;
p_MCB = p_MCB0->p_mlnk;
}
else {
p_MCB = p_MCB->p_mlnk;
}
}
os_active_TCB[task_id-1] = NULL;
_free_box (mp_stk, task_context->stack);
task_context->stack = NULL;
rt_notify (task_context->ptask, (__FALSE << 8) | task_context->task_id);
_free_box (mp_tcb, task_context);
tsk_unlock ();
if (os_rdy_prio() > os_runtask->prio) {
/* Ready task has higher priority than running task. */
os_runtask->state = READY;
os_put_prio (&os_rdy, os_runtask);
os_dispatch (NULL);
}
}
return (OS_R_OK);
}
