StatusType WakeUpTask ( TaskType TaskID )
{
StatusType ercd = E_OK;
TCB *tcb;
TSTAT state;
OS_CHECK((TaskID<cfgOSEK_TASK_NUM),E_OS_ID);
OS_CHECK((TaskID != knl_ctxtsk->tskid),E_OS_ID);
tcb = &knl_tcb_table[TaskID];
BEGIN_CRITICAL_SECTION;
state = (TSTAT)tcb->state;
if ( !knl_task_alive(state) ) {
ercd = E_OS_ID;
} else if ( (state & TS_WAIT) != 0 && tcb->wspec == &knl_wspec_slp ) {
knl_wait_release_ok(tcb);
} else if ( tcb->wupcnt == UINT_MAX ) {
ercd = E_OS_QOVR;
} else {
++tcb->wupcnt;
}
END_CRITICAL_SECTION;
Error_Exit:
return ercd;
}
/*
* Processing if the priority of wait task changes
*/
LOCAL void sem_chg_pri( TCB *tcb, INT oldpri )
{
SEMCB *semcb;
QUEUE *queue;
TCB *top;
semcb = get_semcb(tcb->wid);
if ( oldpri >= 0 ) {
/* Reorder wait line */
knl_gcb_change_priority((GCB*)semcb, tcb);
}
if ( (semcb->sematr & TA_CNT) != 0 ) {
return;
}
/* From the head task in a wait queue, allocate semaphore counts
and release wait state as much as possible */
queue = semcb->wait_queue.next;
while ( queue != &semcb->wait_queue ) {
top = (TCB*)queue;
queue = queue->next;
/* Meet condition for releasing wait? */
if ( semcb->semcnt < top->winfo.sem.cnt ) {
break;
}
/* Release wait */
knl_wait_release_ok(top);
semcb->semcnt -= top->winfo.sem.cnt;
}
}
/*
* Free mutex when task is terminated
* Free all mutexes which the task holds.
* Do not need to handle mutex list and priority of terminated task.
*
*/
EXPORT void knl_signal_all_mutex( TCB *tcb )
{
MTXCB *mtxcb, *next_mtxcb;
TCB *next_tcb;
next_mtxcb = tcb->mtxlist;
while ( (mtxcb = next_mtxcb) != NULL ) {
next_mtxcb = mtxcb->mtxlist;
if ( mtx_waited(mtxcb) ) {
next_tcb = (TCB*)mtxcb->wait_queue.next;
/* Wake wait task */
knl_wait_release_ok(next_tcb);
/* Change mutex get task */
mtxcb->mtxtsk = next_tcb;
mtxcb->mtxlist = next_tcb->mtxlist;
next_tcb->mtxlist = mtxcb;
if ( (mtxcb->mtxatr & TA_CEILING) == TA_CEILING ) {
if ( next_tcb->priority > mtxcb->ceilpri ) {
/* Raise the priority for the task
that got lock to the highest
priority limit */
knl_change_task_priority(next_tcb,
mtxcb->ceilpri);
}
}
} else {
/* No wait task */
mtxcb->mtxtsk = NULL;
}
}
}
/*
* Event flag set
*/
SYSCALL ER tk_set_flg_impl( ID flgid, UINT setptn )
{
FLGCB *flgcb;
TCB *tcb;
QUEUE *queue;
UINT wfmode, waiptn;
ER ercd = E_OK;
CHECK_FLGID(flgid);
flgcb = get_flgcb(flgid);
BEGIN_CRITICAL_SECTION;
if ( flgcb->flgid == 0 ) {
ercd = E_NOEXS;
goto error_exit;
}
/* Set event flag */
flgcb->flgptn |= setptn;
/* Search task which should be released */
queue = flgcb->wait_queue.next;
while ( queue != &flgcb->wait_queue ) {
tcb = (TCB*)queue;
queue = queue->next;
/* Meet condition for release wait? */
waiptn = tcb->winfo.flg.waiptn;
wfmode = tcb->winfo.flg.wfmode;
if ( knl_eventflag_cond(flgcb, waiptn, wfmode) ) {
/* Release wait */
*tcb->winfo.flg.p_flgptn = flgcb->flgptn;
knl_wait_release_ok(tcb);
/* Clear event flag */
if ( (wfmode & TWF_BITCLR) != 0 ) {
if ( (flgcb->flgptn &= ~waiptn) == 0 ) {
break;
}
}
if ( (wfmode & TWF_CLR) != 0 ) {
flgcb->flgptn = 0;
break;
}
}
}
error_exit:
END_CRITICAL_SECTION;
return ercd;
}
/*
* Signal semaphore
*/
SYSCALL ER tk_sig_sem_impl( ID semid, INT cnt )
{
SEMCB *semcb;
TCB *tcb;
QUEUE *queue;
ER ercd = E_OK;
CHECK_SEMID(semid);
CHECK_PAR(cnt > 0);
semcb = get_semcb(semid);
BEGIN_CRITICAL_SECTION;
if ( semcb->semid == 0 ) {
ercd = E_NOEXS;
goto error_exit;
}
if ( cnt > (semcb->maxsem - semcb->semcnt) ) {
ercd = E_QOVR;
goto error_exit;
}
/* Return semaphore counts */
semcb->semcnt += cnt;
/* Search task that frees wait */
queue = semcb->wait_queue.next;
while ( queue != &semcb->wait_queue ) {
tcb = (TCB*)queue;
queue = queue->next;
/* Meet condition for Releasing wait? */
if ( semcb->semcnt < tcb->winfo.sem.cnt ) {
if ( (semcb->sematr & TA_CNT) == 0 ) {
break;
}
continue;
}
/* Release wait */
knl_wait_release_ok(tcb);
semcb->semcnt -= tcb->winfo.sem.cnt;
if ( semcb->semcnt <= 0 ) {
break;
}
}
error_exit:
END_CRITICAL_SECTION;
return ercd;
}
/*
* Unlock mutex
*/
SYSCALL ER tk_unl_mtx_impl( ID mtxid )
{
MTXCB *mtxcb;
TCB *tcb;
ER ercd = E_OK;
CHECK_MTXID(mtxid);
CHECK_INTSK();
mtxcb = get_mtxcb(mtxid);
BEGIN_CRITICAL_SECTION;
if ( mtxcb->mtxid == 0 ) {
ercd = E_NOEXS;
goto error_exit;
}
if ( mtxcb->mtxtsk != knl_ctxtsk ) {
ercd = E_ILUSE; /* This is not locked by its own task */
goto error_exit;
}
/* Delete the mutex from the list,
and adjust its own task priority if necessary. */
knl_release_mutex(knl_ctxtsk, mtxcb);
if ( mtx_waited(mtxcb) ) {
tcb = (TCB*)mtxcb->wait_queue.next;
/* Release wait */
knl_wait_release_ok(tcb);
/* Change mutex get task */
mtxcb->mtxtsk = tcb;
mtxcb->mtxlist = tcb->mtxlist;
tcb->mtxlist = mtxcb;
if ( (mtxcb->mtxatr & TA_CEILING) == TA_CEILING ) {
if ( tcb->priority > mtxcb->ceilpri ) {
/* Raise the priority of the task that
got lock to the highest priority limit */
knl_change_task_priority(tcb, mtxcb->ceilpri);
}
}
} else {
/* No wait task */
mtxcb->mtxtsk = NULL;
}
error_exit:
END_CRITICAL_SECTION;
return ercd;
}
/*
* 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;
}
/*
* Reply rendezvous
*/
SYSCALL ER tk_rpl_rdv_impl( RNO rdvno, VP msg, INT rmsgsz )
{
TCB *caltcb;
ER ercd = E_OK;
CHECK_RDVNO(rdvno);
CHECK_PAR(rmsgsz >= 0);
CHECK_INTSK();
caltcb = get_tcb(knl_get_tskid_rdvno(rdvno));
BEGIN_CRITICAL_SECTION;
if ( (caltcb->state & TS_WAIT) == 0
|| caltcb->wspec != &knl_wspec_rdv
|| rdvno != caltcb->winfo.rdv.rdvno ) {
ercd = E_OBJ;
goto error_exit;
}
#if CHK_PAR
if ( rmsgsz > caltcb->winfo.rdv.maxrmsz ) {
ercd = E_PAR;
goto error_exit;
}
#endif
/* Send message */
if ( rmsgsz > 0 ) {
memcpy(caltcb->winfo.rdv.msg, msg, (UINT)rmsgsz);
}
*caltcb->winfo.rdv.p_rmsgsz = rmsgsz;
knl_wait_release_ok(caltcb);
error_exit:
END_CRITICAL_SECTION;
return ercd;
}
/*
* Forward Rendezvous to Other Port
*/
SYSCALL ER tk_fwd_por_impl( ID porid, UINT calptn, RNO rdvno, VP msg, INT cmsgsz )
{
PORCB *porcb;
TCB *caltcb, *tcb;
QUEUE *queue;
RNO new_rdvno;
ER ercd = E_OK;
CHECK_PORID(porid);
CHECK_PAR(calptn != 0);
CHECK_RDVNO(rdvno);
CHECK_PAR(cmsgsz >= 0);
CHECK_INTSK();
porcb = get_porcb(porid);
caltcb = get_tcb(knl_get_tskid_rdvno(rdvno));
BEGIN_CRITICAL_SECTION;
if ( porcb->porid == 0 ) {
ercd = E_NOEXS;
goto error_exit;
}
#if CHK_PAR
if ( cmsgsz > porcb->maxcmsz ) {
ercd = E_PAR;
goto error_exit;
}
#endif
if ( (caltcb->state & TS_WAIT) == 0
|| caltcb->wspec != &knl_wspec_rdv
|| rdvno != caltcb->winfo.rdv.rdvno ) {
ercd = E_OBJ;
goto error_exit;
}
if ( porcb->maxrmsz > caltcb->winfo.rdv.maxrmsz ) {
ercd = E_OBJ;
goto error_exit;
}
#if CHK_PAR
if ( cmsgsz > caltcb->winfo.rdv.maxrmsz ) {
ercd = E_PAR;
goto error_exit;
}
#endif
/* Search accept wait task */
queue = porcb->accept_queue.next;
while ( queue != &porcb->accept_queue ) {
tcb = (TCB*)queue;
queue = queue->next;
if ( (calptn & tcb->winfo.acp.acpptn) == 0 ) {
continue;
}
/* Send message */
new_rdvno = knl_gen_rdvno(caltcb);
if ( cmsgsz > 0 ) {
memcpy(tcb->winfo.acp.msg, msg, (UINT)cmsgsz);
}
*tcb->winfo.acp.p_rdvno = new_rdvno;
*tcb->winfo.acp.p_cmsgsz = cmsgsz;
knl_wait_release_ok(tcb);
/* Change rendezvous end wait of the other task */
caltcb->winfo.rdv.rdvno = new_rdvno;
caltcb->winfo.rdv.msg = caltcb->winfo.cal.msg;
caltcb->winfo.rdv.maxrmsz = porcb->maxrmsz;
caltcb->winfo.rdv.p_rmsgsz = caltcb->winfo.cal.p_rmsgsz;
goto error_exit;
}
/* Change the other task to rendezvous call wait */
caltcb->wspec = ( (porcb->poratr & TA_TPRI) != 0 )?
&knl_wspec_cal_tpri: &knl_wspec_cal_tfifo;
caltcb->wid = porid;
caltcb->winfo.cal.calptn = calptn;
caltcb->winfo.cal.msg = caltcb->winfo.rdv.msg;
caltcb->winfo.cal.cmsgsz = cmsgsz;
caltcb->winfo.cal.p_rmsgsz = caltcb->winfo.rdv.p_rmsgsz;
knl_timer_insert(&caltcb->wtmeb, TMO_FEVR,
(CBACK)knl_wait_release_tmout, caltcb);
if ( (porcb->poratr & TA_TPRI) != 0 ) {
knl_queue_insert_tpri(caltcb, &porcb->call_queue);
} else {
QueInsert(&caltcb->tskque, &porcb->call_queue);
}
if ( cmsgsz > 0 ) {
memcpy(caltcb->winfo.cal.msg, msg, (UINT)cmsgsz);
}
error_exit:
END_CRITICAL_SECTION;
return ercd;
}
/*
* Call rendezvous
*/
SYSCALL INT tk_cal_por_impl( ID porid, UINT calptn, VP msg, INT cmsgsz, TMO tmout )
{
PORCB *porcb;
TCB *tcb;
QUEUE *queue;
RNO rdvno;
INT rmsgsz;
ER ercd = E_OK;
CHECK_PORID(porid);
CHECK_PAR(calptn != 0);
CHECK_PAR(cmsgsz >= 0);
CHECK_TMOUT(tmout);
CHECK_DISPATCH();
porcb = get_porcb(porid);
BEGIN_CRITICAL_SECTION;
if ( porcb->porid == 0 ) {
ercd = E_NOEXS;
goto error_exit;
}
#if CHK_PAR
if ( cmsgsz > porcb->maxcmsz ) {
ercd = E_PAR;
goto error_exit;
}
#endif
/* Search accept wait task */
queue = porcb->accept_queue.next;
while ( queue != &porcb->accept_queue ) {
tcb = (TCB*)queue;
queue = queue->next;
if ( (calptn & tcb->winfo.acp.acpptn) == 0 ) {
continue;
}
/* Send message */
rdvno = knl_gen_rdvno(knl_ctxtsk);
if ( cmsgsz > 0 ) {
memcpy(tcb->winfo.acp.msg, msg, (UINT)cmsgsz);
}
*tcb->winfo.acp.p_rdvno = rdvno;
*tcb->winfo.acp.p_cmsgsz = cmsgsz;
knl_wait_release_ok(tcb);
/* Ready for rendezvous end wait */
ercd = E_TMOUT;
knl_ctxtsk->wspec = &knl_wspec_rdv;
knl_ctxtsk->wid = 0;
knl_ctxtsk->wercd = &ercd;
knl_ctxtsk->winfo.rdv.rdvno = rdvno;
knl_ctxtsk->winfo.rdv.msg = msg;
knl_ctxtsk->winfo.rdv.maxrmsz = porcb->maxrmsz;
knl_ctxtsk->winfo.rdv.p_rmsgsz = &rmsgsz;
knl_make_wait(TMO_FEVR, porcb->poratr);
QueInit(&knl_ctxtsk->tskque);
goto error_exit;
}
/* Ready for rendezvous call wait */
knl_ctxtsk->wspec = ( (porcb->poratr & TA_TPRI) != 0 )?
&knl_wspec_cal_tpri: &knl_wspec_cal_tfifo;
knl_ctxtsk->wercd = &ercd;
knl_ctxtsk->winfo.cal.calptn = calptn;
knl_ctxtsk->winfo.cal.msg = msg;
knl_ctxtsk->winfo.cal.cmsgsz = cmsgsz;
knl_ctxtsk->winfo.cal.p_rmsgsz = &rmsgsz;
knl_gcb_make_wait((GCB*)porcb, tmout);
error_exit:
END_CRITICAL_SECTION;
return ( ercd < E_OK )? ercd: rmsgsz;
}
