/*
* processing of INP_KEY
*/
LOCAL BOOL execINP_KEY( InnerEvent *evt, ReceiveData *msg )
{
KeyTop keytop;
UW kbsel;
KeyState *ks;
if ( msg->head.cmd.err != DEV_OK ) {
/* resetting key state due to error */
kpAllResetKeyMap();
if ( isQueEmpty(&StatMach.useq) ) return FALSE;
kpExecKeyStateMachine((KeyState*)StatMach.useq.next, evt, msg);
return !isQueEmpty(&StatMach.useq);
}
keytop = toKeyTop(&msg->kb);
/* look for KeyState that matches keytop */
ks = (KeyState*)QueSearch(&StatMach.useq, &StatMach.useq,
keytop.w, offsetof(KeyState, keytop.w));
kbsel = ( ks == (KeyState*)&StatMach.useq )?
kpMgrInfo.kpState.stat.kbsel: ks->kbsel;
/* change keymap */
kpSetOrResetKeyMap(keytop, kbsel, msg->kb.stat.press);
if ( ks == (KeyState*)&StatMach.useq ) {
/* search empty KeyState */
ks = (KeyState*)QueRemoveNext(&StatMach.freq);
if ( ks == NULL ) return FALSE;
/* initialize KeyState */
ks->keytop = keytop;
ks->kbsel = kbsel;
ks->state = ( kpGetKeyKind(keytop) <= NormalKey )?
KS_RELEASE: SS_RELEASE;
memset(&ks->u, 0, sizeof(ks->u));
QueInsert((QUEUE*)ks, &StatMach.useq);
}
/* execute the state machine */
kpExecKeyStateMachine(ks, evt, msg);
return FALSE;
}
/*
* Device management cleanup function
*/
EXPORT void knl_devmgr_cleanup( void )
{
OpnCB *opncb;
/* Do nothing if it is not used even once */
if ( knl_resource_control_block.openq.next == NULL ) {
return;
}
LockDM();
/* Free suspend disable request */
knl_DisSusCnt -= knl_resource_control_block.dissus;
knl_resource_control_block.dissus = 0;
/* Close all open devices */
while ( !isQueEmpty(&(knl_resource_control_block.openq)) ) {
opncb = RESQ_OPNCB(knl_resource_control_block.openq.next);
/* Indicate that it is during close processing */
opncb->resid = 0;
UnlockDM();
/* Device close processing */
knl_close_device(opncb, 0);
LockDM();
}
UnlockDM();
return;
}
StatusType SleepTask ( TickType Timeout )
{
StatusType ercd = E_OK;
OS_CHECK_EXT(!in_indp(),E_OS_CALLEVEL);
OS_CHECK_EXT(isQueEmpty(&knl_ctxtsk->resque),E_OS_RESOURCE);
BEGIN_CRITICAL_SECTION;
if ( knl_ctxtsk->wupcnt > 0 ) {
knl_ctxtsk->wupcnt--;
} else {
ercd = E_OS_TMOUT;
if ( Timeout != 0 ) {
knl_ctxtsk->wspec = &knl_wspec_slp;
knl_ctxtsk->wid = 0;
knl_ctxtsk->wercd = &ercd;
knl_make_wait(Timeout);
QueInit(&knl_ctxtsk->tskque);
}
}
END_CRITICAL_SECTION;
Error_Exit:
return ercd;
}
/* |------------------+-------------------------------------------------------------| */
StatusType CancelAlarm ( AlarmType AlarmID )
{
StatusType ercd = E_OK;
ALMCB* almcb;
OS_CHECK_EXT((AlarmID < cfgOSEK_ALARM_NUM),E_OS_ID);
almcb = &knl_almcb_table[AlarmID];
OS_CHECK((!isQueEmpty(&almcb->almque)),E_OS_NOFUNC);
BEGIN_DISABLE_INTERRUPT;
QueRemove(&almcb->almque);
QueInit(&almcb->almque);
END_DISABLE_INTERRUPT;
Error_Exit:
#if(cfgOS_ERROR_HOOK == STD_ON)
if(E_OK != ercd)
{
BEGIN_CRITICAL_SECTION;
_errorhook_svcid = OSServiceId_CancelAlarm;
_errorhook_par1.almid = AlarmID;
CallErrorHook(ercd);
END_CRITICAL_SECTION;
}
#endif /* cfgOS_ERROR_HOOK */
return ercd;
}
/*
* Delete task from ready queue
* Take out TCB from the applicable priority task queue, and if the task
* queue becomes empty, clear the applicable bit from the bitmap area.
* In addition, update 'top_priority' if the deleted task had the highest
* priority. In such case, use the bitmap area to search the second
* highest priority task.
*/
EXPORT void knl_ready_queue_delete( RDYQUE *rq, TCB *tcb )
{
INT priority = tcb->priority;
INT i;
#if(cfgOSEK_FIFO_QUEUE_PER_PRIORITY == STD_OFF)
QueRemove(&tcb->tskque);
if ( !isQueEmpty(&rq->tskque[priority]) ) {
return;
}
#else
/* Only the first task can be deleted in the ready queue of priority. */
FifoQuePop(&rq->tskque[priority]);
if ( !isFifoQueEmpty(&rq->tskque[priority]) ) {
return;
}
#endif
knl_tstdlib_bitclr(rq->bitmap, priority);
if ( priority != rq->top_priority ) {
return;
}
i = knl_tstdlib_bitsearch1(rq->bitmap, priority, NUM_PRI - priority);
if ( i >= 0 ) {
rq->top_priority = priority + i;
} else {
rq->top_priority = NUM_PRI;
}
}
/*
* Refer message buffer state
*/
SYSCALL ER _td_ref_mbf( ID mbfid, TD_RMBF *pk_rmbf )
{
MBFCB *mbfcb;
TCB *tcb;
ER ercd = E_OK;
CHECK_MBFID(mbfid);
mbfcb = get_mbfcb(mbfid);
BEGIN_DISABLE_INTERRUPT;
if ( mbfcb->mbfid == 0 ) {
ercd = E_NOEXS;
} else {
pk_rmbf->exinf = mbfcb->exinf;
pk_rmbf->wtsk = wait_tskid(&mbfcb->recv_queue);
pk_rmbf->stsk = wait_tskid(&mbfcb->send_queue);
if ( !mbf_empty(mbfcb) ) {
pk_rmbf->msgsz = *(HEADER*)&mbfcb->buffer[mbfcb->head];
} else {
if ( !isQueEmpty(&mbfcb->send_queue) ) {
tcb = (TCB*)mbfcb->send_queue.next;
pk_rmbf->msgsz = tcb->winfo.smbf.msgsz;
} else {
pk_rmbf->msgsz = 0;
}
}
pk_rmbf->frbufsz = mbfcb->frbufsz;
pk_rmbf->maxmsz = mbfcb->maxmsz;
}
END_DISABLE_INTERRUPT;
return ercd;
}
/* |------------------+-------------------------------------------------------------| */
StatusType Schedule ( void )
{
StatusType ercd = E_OK;
OS_CHECK_EXT(!in_indp(),E_OS_CALLEVEL);
OS_CHECK_EXT(isQueEmpty(&knl_ctxtsk->resque),E_OS_RESOURCE);
BEGIN_CRITICAL_SECTION;
//if task has internal resource or task is non-premtable
if(knl_ready_queue.top_priority <= knl_ctxtsk->itskpri)
{ //release internal resource or for Non-Preemtable Task
knl_ctxtsk->priority = knl_ctxtsk->itskpri;
knl_reschedule();
}
END_CRITICAL_SECTION;
//re-get internal resource or for Non-Preemtable task
knl_ctxtsk->priority = knl_ctxtsk->runpri;
Error_Exit:
#if(cfgOS_ERROR_HOOK == STD_ON)
if(E_OK != ercd)
{
BEGIN_CRITICAL_SECTION;
_errorhook_svcid = OSServiceId_Schedule;
CallErrorHook(ercd);
END_CRITICAL_SECTION;
}
#endif /* cfgOS_ERROR_HOOK */
return ercd;
}
/* |------------------+-----------------------------------------------------------| */
StatusType GetResource (ResourceType ResID)
{
StatusType ercd = E_OK;
RESCB *rescb;
PRI ceilpri,oldpri;
OS_CHECK_EXT(!in_indp(),E_OS_CALLEVEL); //add as share resource with ISR was not supported
OS_CHECK_EXT((ResID < cfgOSEK_RESOURCE_NUM),E_OS_ID);
rescb = &knl_rescb_table[ResID];
OS_CHECK_EXT((isQueEmpty(&rescb->resque)),E_OS_ACCESS);
oldpri = knl_ctxtsk->priority;
ceilpri = knl_gres_table[ResID];
OS_CHECK_EXT((ceilpri <= knl_ctxtsk->itskpri),E_OS_ACCESS);
BEGIN_DISABLE_INTERRUPT;
if(ceilpri < oldpri)
{
knl_ctxtsk->priority = ceilpri;
}
rescb->tskpri = oldpri;
QueInsert(&rescb->resque,&knl_ctxtsk->resque);
END_DISABLE_INTERRUPT;
Error_Exit:
#if(cfgOS_ERROR_HOOK == STD_ON)
if(E_OK != ercd)
{
BEGIN_CRITICAL_SECTION;
_errorhook_svcid = OSServiceId_GetResource;
_errorhook_par1.resid = ResID;
CallErrorHook(ercd);
END_CRITICAL_SECTION;
}
#endif /* cfgOS_ERROR_HOOK */
return ercd;
}
/*
* Maximum free area size
*/
Inline INT MaxFreeSize( MPLCB *mplcb )
{
if ( isQueEmpty(&mplcb->freeque) ) {
return 0;
}
return FreeSize(mplcb->freeque.prev);
}
/*
* Get ID of the head task in the wait queue.
*/
EXPORT ID knl_wait_tskid( QUEUE *wait_queue )
{
if ( isQueEmpty(wait_queue) ) {
return 0;
}
return ((TCB*)wait_queue->next)->tskid;
}
/*
* Erase entry from synchronization queue
* Erase entry from the synchronization queue.
* If there is another in the synchronizaton queue, wake it up.
*/
LOCAL void leaveSyncWait( QUEUE *syncq, WaitQ *entry )
{
QueRemove(&entry->q);
if ( !isQueEmpty(syncq) ) {
SyncSignalDM(((WaitQ*)syncq->next)->tskid);
}
}
SYSCALL INT _tk_rcv_mbf_u( ID mbfid, void *msg, TMO_U tmout )
{
MBFCB *mbfcb;
TCB *tcb;
INT rcvsz;
ER ercd = E_OK;
CHECK_MBFID(mbfid);
CHECK_TMOUT(tmout);
CHECK_DISPATCH();
mbfcb = get_mbfcb(mbfid);
BEGIN_CRITICAL_SECTION;
if (mbfcb->mbfid == 0) {
ercd = E_NOEXS;
goto error_exit;
}
/* Check receive wait disable */
if ( is_diswai((GCB*)mbfcb, ctxtsk, TTW_RMBF) ) {
ercd = E_DISWAI;
goto error_exit;
}
if ( !mbf_empty(mbfcb) ) {
/* Read from message buffer */
rcvsz = mbf_to_msg(mbfcb, msg);
/* Accept message from sending task(s) */
mbf_wakeup(mbfcb);
} else if ( !isQueEmpty(&mbfcb->send_queue) ) {
/* Receive directly from send wait task */
tcb = (TCB*)mbfcb->send_queue.next;
rcvsz = tcb->winfo.smbf.msgsz;
memcpy(msg, tcb->winfo.smbf.msg, (UINT)rcvsz);
wait_release_ok(tcb);
mbf_wakeup(mbfcb);
} else {
ercd = E_TMOUT;
if ( tmout != TMO_POL ) {
/* Ready for receive wait */
ctxtsk->wspec = &wspec_rmbf;
ctxtsk->wid = mbfid;
ctxtsk->wercd = &ercd;
ctxtsk->winfo.rmbf.msg = msg;
ctxtsk->winfo.rmbf.p_msgsz = &rcvsz;
make_wait(tmout, mbfcb->mbfatr);
QueInsert(&ctxtsk->tskque, &mbfcb->recv_queue);
}
}
error_exit:
END_CRITICAL_SECTION;
return ( ercd < E_OK )? ercd: rcvsz;
}
/*
* Release all tasks connected to the wait queue, and define it
* as E_DLT error.
*/
EXPORT void knl_wait_delete( QUEUE *wait_queue )
{
TCB *tcb;
while ( !isQueEmpty(wait_queue) ) {
tcb = (TCB*)wait_queue->next;
knl_wait_release(tcb);
*tcb->wercd = E_DLT;
}
}
/*
* Enter Synchronization Queue
* entry is registered as the task that has entered synchronization queue
* return value
* FALSE entry is the first task that has entered the synchronization
* queue
* TRUE there were other tasks in the synchronization queue
*/
LOCAL BOOL enterSyncWait( QUEUE *syncq, WaitQ *entry )
{
BOOL wait;
wait = !isQueEmpty(syncq);
entry->tskid = tk_get_tid();
QueInsert(&entry->q, syncq);
return wait;
}
/* |------------------+-------------------------------------------------------------| */
StatusType ChainTask ( TaskType TaskID )
{
StatusType ercd = E_NOT_OK;
TCB * tcb;
TSTAT state;
OS_CHECK((TaskID<cfgOSEK_TASK_NUM),E_OS_ID);
OS_CHECK_EXT(!in_indp(),E_OS_CALLEVEL);
OS_CHECK_EXT(isQueEmpty(&knl_ctxtsk->resque),E_OS_RESOURCE);
DISABLE_INTERRUPT;
if(TaskID == knl_ctxtsk->tskid){
/* chain to itself */
knl_search_schedtsk();
knl_make_active(knl_ctxtsk);
}
else{
/* firstly terminate current running task knl_ctxtsk,
* and then activate TaskID */
tcb = &knl_tcb_table[TaskID];
state = (TSTAT)tcb->state;
if (TS_DORMANT != state) {
#if((cfgOS_CONFORMANCE_CLASS == ECC2) || (cfgOS_CONFORMANCE_CLASS == BCC2))
if(tcb->actcnt < knl_gtsk_table[TaskID].maxact)
{
tcb->actcnt += 1;
}
else
#endif
{
ercd = E_OS_LIMIT;
goto Error_Exit;
}
}
knl_ctxtsk->state = TS_DORMANT;
knl_search_schedtsk();
#if((cfgOS_CONFORMANCE_CLASS == ECC2) || (cfgOS_CONFORMANCE_CLASS == BCC2))
if(knl_ctxtsk->actcnt > 0)
{
knl_ctxtsk->actcnt -= 1;
knl_make_active(knl_ctxtsk);
}
#endif
if (TS_DORMANT == state) {
knl_make_active(tcb);
}
}
knl_force_dispatch();
/* No return */
Error_Exit:
return ercd;
}
/* |------------------+-------------------------------------------------------------| */
StatusType Schedule ( void )
{
StatusType ercd = E_NOT_OK;
OS_CHECK_EXT(!in_indp(),E_OS_CALLEVEL);
OS_CHECK_EXT(isQueEmpty(&knl_ctxtsk->resque),E_OS_RESOURCE);
//As Internal Resource was not supported,So in fact this API only has effect on
//Non-preemtable Task.
BEGIN_CRITICAL_SECTION;
knl_reschedule();
END_CRITICAL_SECTION;
Error_Exit:
return ercd;
}
/*
* Delete from FreeQue
*/
LOCAL void removeFreeQue( QUEUE *fq )
{
if ( !isQueEmpty(fq + 1) ) {
QUEUE *nq = (fq + 1)->next;
QueRemove(fq + 1);
QueInsert(nq + 1, nq);
QueRemove(nq);
QueInsert(nq, fq);
}
QueRemove(fq);
}
/*
* Event flag wait
*/
SYSCALL ER tk_wai_flg_impl( ID flgid, UINT waiptn, UINT wfmode, UINT *p_flgptn, TMO tmout )
{
FLGCB *flgcb;
ER ercd = E_OK;
CHECK_FLGID(flgid);
CHECK_PAR(waiptn != 0);
CHECK_PAR((wfmode & ~(TWF_ORW|TWF_CLR|TWF_BITCLR)) == 0);
CHECK_TMOUT(tmout);
CHECK_DISPATCH();
flgcb = get_flgcb(flgid);
BEGIN_CRITICAL_SECTION;
if ( flgcb->flgid == 0 ) {
ercd = E_NOEXS;
goto error_exit;
}
if ( (flgcb->flgatr & TA_WMUL) == 0 && !isQueEmpty(&flgcb->wait_queue) ) {
/* Disable multiple tasks wait */
ercd = E_OBJ;
goto error_exit;
}
/* Meet condition for release wait? */
if ( knl_eventflag_cond(flgcb, waiptn, wfmode) ) {
*p_flgptn = flgcb->flgptn;
/* Clear event flag */
if ( (wfmode & TWF_BITCLR) != 0 ) {
flgcb->flgptn &= ~waiptn;
}
if ( (wfmode & TWF_CLR) != 0 ) {
flgcb->flgptn = 0;
}
} else {
/* Ready for wait */
knl_ctxtsk->wspec = ( (flgcb->flgatr & TA_TPRI) != 0 )?
&knl_wspec_flg_tpri: &knl_wspec_flg_tfifo;
knl_ctxtsk->wercd = &ercd;
knl_ctxtsk->winfo.flg.waiptn = waiptn;
knl_ctxtsk->winfo.flg.wfmode = wfmode;
knl_ctxtsk->winfo.flg.p_flgptn = p_flgptn;
knl_gcb_make_wait((GCB*)flgcb, tmout);
}
error_exit:
END_CRITICAL_SECTION;
return ercd;
}
/*
* Send to mailbox
*/
SYSCALL ER _tk_snd_mbx( ID mbxid, T_MSG *pk_msg )
{
MBXCB *mbxcb;
TCB *tcb;
ER ercd = E_OK;
CHECK_MBXID(mbxid);
mbxcb = get_mbxcb(mbxid);
BEGIN_CRITICAL_SECTION;
if (mbxcb->mbxid == 0) {
ercd = E_NOEXS;
goto error_exit;
}
if ( (mbxcb->mbxatr & TA_MPRI) != 0 ) {
if ( ((T_MSG_PRI*)pk_msg)->msgpri <= 0 ) {
ercd = E_PAR;
goto error_exit;
}
}
if ( !isQueEmpty(&mbxcb->wait_queue) ) {
/* Directly send to receive wait task */
tcb = (TCB*)(mbxcb->wait_queue.next);
*tcb->winfo.mbx.ppk_msg = pk_msg;
wait_release_ok(tcb);
} else {
/* Connect message to queue */
if ( (mbxcb->mbxatr & TA_MPRI) != 0 ) {
/* Connect message to queue following priority */
queue_insert_mpri((T_MSG_PRI*)pk_msg, &mbxcb->mq_head);
} else {
/* Connect to end of queue */
nextmsg(pk_msg) = NULL;
if ( headmsg(mbxcb) == NULL ) {
headmsg(mbxcb) = pk_msg;
} else {
nextmsg(mbxcb->mq_tail) = pk_msg;
}
mbxcb->mq_tail = pk_msg;
}
}
error_exit:
END_CRITICAL_SECTION;
return ercd;
}
/*
* Device registration
*/
RAW_U32 _tk_def_dev( CONST UB *devnm, CONST T_DDEV *ddev, void *caller_gp )
{
DevCB *devcb;
INT len, evttyp;
ER ercd;
len = ChkSpaceBstrR(devnm, 0);
if ( len <= 0 || len > MaxRegDev ) {
RAW_ASSERT(0);
}
/* Search whether 'devnm' device is registered */
devcb = searchDevCB(devnm);
if ( devcb == 0 ) {
if ( ddev == 0 ) {
ercd = E_NOEXS;
goto err_ret2;
}
/* Get 'devcb' for new registration because it is not
registered */
devcb = newDevCB(devnm);
if ( devcb == NULL ) {
ercd = E_LIMIT;
goto err_ret2;
}
}
if ( ddev != NULL ) {
/* Set/update device registration information */
devcb->ddev = *ddev;
evttyp = TSEVT_DEVICE_REGIST;
} else {
if ( !isQueEmpty(&devcb->openq) ) {
/* In use (open) */
ercd = E_BUSY;
goto err_ret2;
}
/* Device unregistration */
delDevCB(devcb);
evttyp = TSEVT_DEVICE_DELETE;
}
return DID(devcb);
}
int deque()
{
int temp = -1;
if(isQueEmpty()==0)
{
temp = que[head];
head = (head + 1) % NUM_VERTEX;
}
{
return -1;
}
return temp;
}
/*
* Search the first task of wait queue include "tcb" with target.
* (Not insert "tcb" into wait queue.)
*
*/
EXPORT TCB* knl_gcb_top_of_wait_queue( GCB *gcb, TCB *tcb )
{
TCB *q;
if ( isQueEmpty(&gcb->wait_queue) ) {
return tcb;
}
q = (TCB*)gcb->wait_queue.next;
if ( (gcb->objatr & TA_TPRI) == 0 ) {
return q;
}
return ( tcb->priority < q->priority )? tcb: q;
}
void doBFS(int v)
{
enque(v);
while(isQueEmpty() == 0)
{
int k = deque();
if(visited[k] == 0)
{
printf("BFS visited %d \n",k);
visited[k] = 1;
addAdjacentNonVisitedVertexToQue(k);
}
}
}
/*
* Return fixed size memory block
*/
SYSCALL ER tk_rel_mpf_impl( ID mpfid, VP blf )
{
MPFCB *mpfcb;
TCB *tcb;
FREEL *free;
ER ercd = E_OK;
CHECK_MPFID(mpfid);
CHECK_DISPATCH();
mpfcb = get_mpfcb(mpfid);
knl_LockOBJ(&mpfcb->lock);
if ( mpfcb->mpfid == 0 ) {
ercd = E_NOEXS;
goto error_exit;
}
#if CHK_PAR
if ( blf < mpfcb->mempool || blf >= knl_mempool_end(mpfcb) || (((VB*)blf - (VB*)mpfcb->mempool) % mpfcb->blfsz) != 0 ) {
ercd = E_PAR;
goto error_exit;
}
#endif
DISABLE_INTERRUPT;
if ( !isQueEmpty(&mpfcb->wait_queue) ) {
/* Send memory block to waiting task,
and then release the task */
tcb = (TCB*)mpfcb->wait_queue.next;
*tcb->winfo.mpf.p_blf = blf;
knl_wait_release_ok(tcb);
ENABLE_INTERRUPT;
} else {
ENABLE_INTERRUPT;
/* Free memory block */
free = (FREEL*)blf;
free->next = mpfcb->freelist;
mpfcb->freelist = free;
mpfcb->frbcnt++;
}
error_exit:
knl_UnlockOBJ(&mpfcb->lock);
return ercd;
}
/* |------------------+-----------------------------------------------------------| */
StatusType GetResource (ResourceType ResID)
{
StatusType ercd = E_OK;
RESCB *rescb;
PRI newpri,oldpri;
OS_CHECK_EXT((ResID < cfgOSEK_RESOURCE_NUM),E_OS_ID);
rescb = &knl_rescb_table[ResID];
OS_CHECK_EXT((isQueEmpty(&rescb->resque)),E_OS_ACCESS);
if(in_indp()) /* Interrupt level */
{
/* not supported */
}
else
{
oldpri = knl_ctxtsk->priority;
newpri = knl_gres_table[ResID];
OS_CHECK_EXT((newpri < oldpri),E_OS_ACCESS);
BEGIN_DISABLE_INTERRUPT;
if(newpri < 0)
{
//TODO: share resourse with ISR
/* Task share resource with ISR */
/* should change IPL */
/* not supported */
}
else
{
knl_ctxtsk->priority = newpri;
rescb->tskpri = oldpri;
QueInsert(&rescb->resque,&knl_ctxtsk->resque);
}
END_DISABLE_INTERRUPT;
}
Error_Exit:
#if(cfgOS_ERROR_HOOK == STD_ON)
if(E_OK != ercd)
{
BEGIN_CRITICAL_SECTION;
_errorhook_svcid = OSServiceId_GetResource;
_errorhook_par1.resid = ResID;
CallErrorHook(ercd);
END_CRITICAL_SECTION;
}
#endif /* cfgOS_ERROR_HOOK */
return ercd;
}
/*
* Accept message and release wait task,
* as long as there are free message area.
*/
LOCAL void mbf_wakeup( MBFCB *mbfcb )
{
TCB *top;
INT msgsz;
while ( !isQueEmpty(&mbfcb->send_queue) ) {
top = (TCB*)mbfcb->send_queue.next;
msgsz = top->winfo.smbf.msgsz;
if ( !mbf_free(mbfcb, msgsz) ) {
break;
}
/* Store a message from waiting task and release it */
msg_to_mbf(mbfcb, top->winfo.smbf.msg, msgsz);
wait_release_ok(top);
}
}
/*
* Device management cleanup function
*/
EXPORT void devmgr_cleanup( ID resid, INT info )
{
ResCB *rescb;
OpnCB *opncb;
ER ercd;
ercd = tk_get_res(resid, DEVICE_SVC, (void**)&rescb);
if ( ercd < E_OK ) {
goto err_ret;
}
/* Do nothing if it is not used even once */
if ( rescb->openq.next == NULL ) {
return;
}
LockDM();
/* Free suspend disable request */
DisSusCnt -= rescb->dissus;
rescb->dissus = 0;
/* Close all open devices */
while ( !isQueEmpty(&rescb->openq) ) {
opncb = RESQ_OPNCB(rescb->openq.next);
/* Indicate that it is during close processing */
opncb->resid = -1;
/* Device close processing */
UnlockDM();
close_device(opncb, 0);
LockDM();
}
UnlockDM();
return;
err_ret:
DEBUG_PRINT(("devmgr_cleanup ercd = %d\n", ercd));
return;
}
/*
* Return to the free queue
* Call by executing "LockGDI()"
*/
LOCAL void gdi_relDRQ( DRQ *drq, GDI gdi )
{
UINT ptn = isQueEmpty(&gdi->freeq)? (FREEQ_RD|FREEQ_WR): 0;
if ( drq->req->cmd == TDC_READ ) {
if ( gdi->preq[0]-- == gdi->limit ) ptn |= FREEQ_RD;
} else {
if ( gdi->preq[1]-- == gdi->limit ) ptn |= FREEQ_WR;
}
if ( ptn != 0 ) {
tk_set_flg(gdi->flgid, ptn);
}
QueInsert((QUEUE*)drq, &gdi->freeq);
drq->req = NULL;
drq->wtid = 0;
drq->done = NotDone;
}
/* |------------------+--------------------------------------------------------------| */
StatusType TerminateTask ( void )
{
StatusType ercd = E_NOT_OK;
OS_CHECK_EXT(!in_indp(),E_OS_CALLEVEL);
OS_CHECK_EXT(isQueEmpty(&knl_ctxtsk->resque),E_OS_RESOURCE);
DISABLE_INTERRUPT;
#if(cfgOS_POST_TASK_HOOK == STD_ON)
PostTaskHook();
#endif
knl_ctxtsk->state = TS_DORMANT;
//release internal resource or for non-preemtable task
ReleaseInternalResource();
knl_ctxtsk->priority = knl_ctxtsk->itskpri;
knl_search_schedtsk();
#if((cfgOS_CONFORMANCE_CLASS == ECC2) || (cfgOS_CONFORMANCE_CLASS == BCC2))
if(knl_ctxtsk->actcnt > 0)
{
knl_ctxtsk->actcnt -= 1;
#if(cfgOSEK_FIFO_QUEUE_PER_PRIORITY == STD_OFF)
knl_make_active(knl_ctxtsk);
#else
knl_make_ready(knl_ctxtsk);
#endif
}
#endif
knl_force_dispatch();
/* No return */
Error_Exit:
#if(cfgOS_ERROR_HOOK == STD_ON)
if(E_OK != ercd)
{
BEGIN_CRITICAL_SECTION;
_errorhook_svcid = OSServiceId_TerminateTask;
CallErrorHook(ercd);
END_CRITICAL_SECTION;
}
#endif /* cfgOS_ERROR_HOOK */
return ercd;
}
EXPORT void knl_timer_handler( void )
{
TMEB *event;
BEGIN_DISABLE_INTERRUPT;
knl_current_time++;
/* Execute event that passed occurring time. */
while ( !isQueEmpty(&knl_timer_queue) ) {
event = (TMEB*)knl_timer_queue.next;
if ( event->time > knl_current_time ) {
break;
}
QueRemove(&event->queue);
if ( event->callback != (CBACK)NULL_FP ) {
(*event->callback)(event->arg);
}
}
END_DISABLE_INTERRUPT;
}
