/*
* Get open management block
*/
LOCAL OpnCB* newOpnCB( DevCB *devcb, INT unitno, UINT omode, ResCB *rescb )
{
OpnCB *opncb;
/* Get space in open management block */
opncb = (OpnCB*)QueRemoveNext(&knl_FreeOpnCB);
if ( opncb == NULL ) {
return NULL; /* No space */
}
/* Register as open device */
QueInsert(&opncb->q, &devcb->openq);
QueInsert(&opncb->resq, &rescb->openq);
opncb->devcb = devcb;
opncb->unitno = unitno;
opncb->omode = omode;
QueInit(&opncb->requestq);
opncb->waitone = 0;
opncb->nwaireq = 0;
opncb->abort_tskid = 0;
opncb->resid = 0; /* Indicate that open processing is not completed */
return opncb;
}
/*
* 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);
}
/*
* Initialization sequence of device input/output-related
*/
EXPORT ER initDevIO( void )
{
INT i;
ER ercd;
i = _tk_get_cfn(SCTAG_TMAXOPNDEV, &MaxOpnDev, 1);
if ( i < 1 ) {
ercd = E_SYS;
goto err_ret;
}
i = _tk_get_cfn(SCTAG_TMAXREQDEV, &MaxReqDev, 1);
if ( i < 1 ) {
ercd = E_SYS;
goto err_ret;
}
/* Generate open management information table */
OpnCBtbl = Imalloc((UINT)MaxOpnDev * sizeof(OpnCB));
if ( OpnCBtbl == NULL ) {
ercd = E_NOMEM;
goto err_ret;
}
QueInit(&FreeOpnCB);
for ( i = 0; i < MaxOpnDev; ++i ) {
OpnCBtbl[i].resid = 0;
QueInsert(&OpnCBtbl[i].q, &FreeOpnCB);
}
/* Generate request management information table */
ReqCBtbl = Imalloc((UINT)MaxReqDev * sizeof(ReqCB));
if ( ReqCBtbl == NULL ) {
ercd = E_NOMEM;
goto err_ret;
}
QueInit(&FreeReqCB);
for ( i = 0; i < MaxReqDev; ++i ) {
ReqCBtbl[i].opncb = NULL;
QueInsert(&ReqCBtbl[i].q, &FreeReqCB);
}
return E_OK;
err_ret:
DEBUG_PRINT(("initDevIO ercd = %d\n", ercd));
return ercd;
}
/*
* Free request management block
*/
LOCAL void delReqCB( ReqCB *reqcb )
{
QueRemove(&reqcb->q);
QueInsert(&reqcb->q, &FreeReqCB);
reqcb->opncb = NULL;
}
/*
* Delete message buffer
*/
SYSCALL ER _tk_del_mbf( ID mbfid )
{
MBFCB *mbfcb;
VB *msgbuf = NULL;
ER ercd = E_OK;
CHECK_MBFID(mbfid);
mbfcb = get_mbfcb(mbfid);
BEGIN_CRITICAL_SECTION;
if ( mbfcb->mbfid == 0 ) {
ercd = E_NOEXS;
} else {
msgbuf = mbfcb->buffer;
/* Release wait state of task (E_DLT) */
wait_delete(&mbfcb->recv_queue);
wait_delete(&mbfcb->send_queue);
/* Return to FreeQue */
QueInsert(&mbfcb->send_queue, &free_mbfcb);
mbfcb->mbfid = 0;
}
END_CRITICAL_SECTION;
if ( msgbuf != NULL ) {
Ifree(msgbuf);
}
return ercd;
}
/*
* Initialization of variable size memory pool control block
*/
EXPORT ER memorypool_initialize( void )
{
MPLCB *mplcb, *end;
W n;
/* Get system information */
n = _tk_get_cfn(SCTAG_TMAXMPLID, &max_mplid, 1);
if ( n < 1 || NUM_MPLID < 1 ) {
return E_SYS;
}
/* Create variable size memory pool control block */
mplcb_table = Imalloc((UINT)NUM_MPLID * sizeof(MPLCB));
if ( mplcb_table == NULL ) {
return E_NOMEM;
}
/* Register all control blocks onto FeeQue */
QueInit(&free_mplcb);
end = mplcb_table + NUM_MPLID;
for ( mplcb = mplcb_table; mplcb < end; mplcb++ ) {
mplcb->mplid = 0;
QueInsert(&mplcb->wait_queue, &free_mplcb);
}
return E_OK;
}
/*
* Initialization of message buffer control block
*/
EXPORT ER messagebuffer_initialize( void )
{
MBFCB *mbfcb, *end;
W n;
/* Get system information */
n = _tk_get_cfn(SCTAG_TMAXMBFID, &max_mbfid, 1);
if ( n < 1 || NUM_MBFID < 1 ) {
return E_SYS;
}
/* Create message buffer control block */
mbfcb_table = Imalloc((UINT)NUM_MBFID * sizeof(MBFCB));
if ( mbfcb_table == NULL ) {
return E_NOMEM;
}
/* Register all control blocks onto FeeQue */
QueInit(&free_mbfcb);
end = mbfcb_table + NUM_MBFID;
for ( mbfcb = mbfcb_table; mbfcb < end; mbfcb++ ) {
mbfcb->mbfid = 0;
QueInsert(&mbfcb->send_queue, &free_mbfcb);
}
return E_OK;
}
/*
* Delete rendezvous port
*/
SYSCALL ER tk_del_por_impl( ID porid )
{
PORCB *porcb;
ER ercd = E_OK;
CHECK_PORID(porid);
CHECK_INTSK();
porcb = get_porcb(porid);
BEGIN_CRITICAL_SECTION;
if ( porcb->porid == 0 ) {
ercd = E_NOEXS;
} else {
/* Release wait state of task (E_DLT) */
knl_wait_delete(&porcb->call_queue);
knl_wait_delete(&porcb->accept_queue);
/* Return to FreeQue */
QueInsert(&porcb->call_queue, &knl_free_porcb);
porcb->porid = 0;
}
END_CRITICAL_SECTION;
return ercd;
}
/*
* Object lock
* Do not call from critical section
*/
EXPORT void knl_LockOBJ( OBJLOCK *loc )
{
BOOL klocked;
retry:
BEGIN_CRITICAL_SECTION;
klocked = knl_ctxtsk->klocked;
if ( !klocked ) {
if ( loc->wtskq.next == NULL ) {
/* Lock */
QueInit(&loc->wtskq);
knl_ctxtsk->klocked = klocked = TRUE;
knl_ready_queue.klocktsk = knl_ctxtsk;
} else {
/* Ready for lock */
knl_ready_queue_delete(&knl_ready_queue, knl_ctxtsk);
knl_ctxtsk->klockwait = TRUE;
QueInsert(&knl_ctxtsk->tskque, &loc->wtskq);
knl_schedtsk = knl_ready_queue_top(&knl_ready_queue);
knl_dispatch_request();
}
}
END_CRITICAL_SECTION;
/* Since wait could be freed without getting lock,
need to re-try if lock is not got */
if ( !klocked ) {
goto retry;
}
}
/*
* Initialization of mutex control block
*/
EXPORT ER mutex_initialize(void)
{
MTXCB *mtxcb, *end;
W n;
/* Get system information */
n = _tk_get_cfn(SCTAG_TMAXMTXID, &max_mtxid, 1);
if ( n < 1 || NUM_MTXID < 1 ) {
return E_SYS;
}
/* Create mutex control block */
mtxcb_table = Imalloc((UINT)NUM_MTXID * sizeof(MTXCB));
if ( mtxcb_table == NULL ) {
return E_NOMEM;
}
/* Register all control blocks onto FeeQue */
QueInit(&free_mtxcb);
end = mtxcb_table + NUM_MTXID;
for( mtxcb = mtxcb_table; mtxcb < end; mtxcb++ ) {
mtxcb->mtxid = 0;
QueInsert(&mtxcb->wait_queue, &free_mtxcb);
}
return E_OK;
}
/*
* initialize state machine
*/
EXPORT ER kpInitializeStateMachine( void )
{
ER err;
int i;
/* key */
QueInit(&StatMach.useq);
QueInit(&StatMach.freq);
for ( i = 0; i < MaxKey; ++i ) {
err = initAlmStat(&StatMach.key[i].alm, KeyTmoutKind(i));
if ( err != E_OK ) return err;
QueInsert((QUEUE*)&StatMach.key[i], &StatMach.freq);
}
StatMach.spressMsg.cmd.read = 1;
StatMach.spressMsg.cmd.cmd = PIC_SPRESS;
StatMach.keyupMsg.cmd.read = 1;
StatMach.keyupMsg.cmd.cmd = PIC_KEYUP;
/* PD button */
for ( i = 0; i < NumOfPdBut; ++i ) {
StatMach.pdBut[i].state = BS_RELEASE;
err = initAlmStat(&StatMach.pdBut[i].alm, PdButTmoutKind(i));
if ( err != E_OK ) return err;
StatMach.pdBut[i].button = PdButKind(i);
}
return E_OK;
}
/*
* Free request management block
*/
EXPORT void knl_delReqCB( ReqCB *reqcb )
{
QueRemove(&reqcb->q);
QueInsert(&reqcb->q, &knl_FreeReqCB);
reqcb->opncb = NULL;
}
/*
* Delete mutex
*/
SYSCALL ER _tk_del_mtx( ID mtxid )
{
MTXCB *mtxcb;
ER ercd = E_OK;
CHECK_MTXID(mtxid);
mtxcb = get_mtxcb(mtxid);
BEGIN_CRITICAL_SECTION;
if ( mtxcb->mtxid == 0 ) {
ercd = E_NOEXS;
} else {
/* If there is a task that holds mutex to delete,
* delete the mutex from the list
* and adjust the task priority if necessary.
*/
if ( mtxcb->mtxtsk != NULL ) {
release_mutex(mtxcb->mtxtsk, mtxcb);
}
/* Free wait state of task (E_DLT) */
wait_delete(&mtxcb->wait_queue);
/* Return to FreeQue */
QueInsert(&mtxcb->wait_queue, &free_mtxcb);
mtxcb->mtxid = 0;
}
END_CRITICAL_SECTION;
return ercd;
}
/*
* Initialization of port control block
*/
EXPORT ER rendezvous_initialize( void )
{
PORCB *porcb, *end;
W n;
/* Get system information */
n = _tk_get_cfn(SCTAG_TMAXPORID, &max_porid, 1);
if ( n < 1 || NUM_PORID < 1 ) {
return E_SYS;
}
/* Create port control block */
porcb_table = Imalloc((UINT)NUM_PORID * sizeof(PORCB));
if ( porcb_table == NULL ) {
return E_NOMEM;
}
/* Register all control blocks onto FeeQue */
QueInit(&free_porcb);
end = porcb_table + NUM_PORID;
for ( porcb = porcb_table; porcb < end; porcb++ ) {
porcb->porid = 0;
QueInsert(&porcb->call_queue, &free_porcb);
}
return E_OK;
}
/*
* Delete variable size memory pool
*/
SYSCALL ER _tk_del_mpl( ID mplid )
{
MPLCB *mplcb;
void *mempool = NULL;
ATR memattr = 0;
ER ercd = E_OK;
CHECK_MPLID(mplid);
CHECK_DISPATCH();
mplcb = get_mplcb(mplid);
BEGIN_CRITICAL_SECTION;
if ( mplcb->mplid == 0 ) {
ercd = E_NOEXS;
} else {
mempool = mplcb->areaque.next;
memattr = mplcb->mplatr;
/* Free wait state of task (E_DLT) */
wait_delete(&mplcb->wait_queue);
/* Return to FreeQue */
QueInsert(&mplcb->wait_queue, &free_mplcb);
mplcb->mplid = 0;
}
END_CRITICAL_SECTION;
if ( ercd == E_OK ) {
IAfree(mempool, memattr);
}
return ercd;
}
/*
* Registration of free area on FreeQue
* FreeQue is composed of 2 types: Queue that links the
* different size of areas by size and queue that links the
* same size of areas.
*
* freeque
* |
* | +-----------------------+ +-----------------------+
* | | AreaQue | | AreaQue |
* | +-----------------------+ +-----------------------+
* *---> FreeQue Size order | | EmptyQue |
* | | FreeQue Same size --------->| FreeQue Same size ----->
* | | | | |
* | | | | |
* | +-----------------------+ +-----------------------+
* | | AreaQue | | AreaQue |
* v +-----------------------+ +-----------------------+
*/
EXPORT void knl_appendFreeArea( IMACB *imacb, QUEUE *aq )
{
QUEUE *fq;
W size = AreaSize(aq);
/* Registration position search */
/* Search the free area whose size is equal to 'blksz',
* or larger than 'blksz' but closest.
* If it does not exist, return '&imacb->freeque'.
*/
fq = knl_searchFreeArea(imacb, size);
/* Register */
clrAreaFlag(aq, AREA_USE);
if ( fq != &imacb->freeque && FreeSize(fq) == size ) {
/* FreeQue Same size */
(aq + 2)->next = (fq + 1)->next;
(fq + 1)->next = aq + 2;
(aq + 2)->prev = fq + 1;
if( (aq + 2)->next != NULL ) {
(aq + 2)->next->prev = aq + 2;
}
(aq + 1)->next = NULL;
} else {
/* FreeQue Size order */
QueInsert(aq + 1, fq);
(aq + 2)->next = NULL;
(aq + 2)->prev = (QUEUE*)size;
}
}
/* |------------------+-----------------------------------------------------------| */
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;
}
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;
}
/*
* Free open management block
*/
LOCAL void delOpnCB( OpnCB *opncb )
{
QueRemove(&opncb->q);
QueRemove(&opncb->resq);
QueInsert(&opncb->q, &FreeOpnCB);
opncb->resid = 0;
}
/*
* Initialization sequence of device input/output-related
*/
EXPORT ER knl_initDevIO( void )
{
INT i;
QueInit(&knl_FreeOpnCB);
for ( i = 0; i < CFN_MAX_OPNDEV; ++i ) {
knl_OpnCBtbl[i].resid = 0;
QueInsert(&knl_OpnCBtbl[i].q, &knl_FreeOpnCB);
}
QueInit(&knl_FreeReqCB);
for ( i = 0; i < CFN_MAX_REQDEV; ++i ) {
knl_ReqCBtbl[i].opncb = NULL;
QueInsert(&knl_ReqCBtbl[i].q, &knl_FreeReqCB);
}
return E_OK;
}
/*
* Free open management block
*/
EXPORT void knl_delOpnCB( OpnCB *opncb, BOOL free )
{
QueRemove(&opncb->q);
QueRemove(&opncb->resq);
if ( free ) {
QueInsert(&opncb->q, &knl_FreeOpnCB);
}
opncb->resid = 0;
}
/*
* Insert timer event to timer event queue
*/
LOCAL void knl_enqueue_tmeb( TMEB *event )
{
QUEUE *q;
for ( q = knl_timer_queue.next; q != &knl_timer_queue; q = q->next ) {
if ( event->time < ((TMEB*)q)->time) {
break;
}
}
QueInsert(&event->queue, q);
}
/*
* 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;
}
/*
* Registration of free area on FreeQue
* FreeQue is composed of 2 types: Queue that links the
* different size of areas by size and queue that links the
* same size of areas.
*
* freeque
* |
* | +-----------------------+ +-----------------------+
* | | AreaQue | | AreaQue |
* | +-----------------------+ +-----------------------+
* *---> FreeQue Size order | *----> FreeQue Same size ----->
* | | FreeQue Same size -----* | EmptyQue |
* | | | | |
* | | | | |
* | +-----------------------+ +-----------------------+
* | | AreaQue | | AreaQue |
* v +-----------------------+ +-----------------------+
*/
LOCAL void appendFreeArea( MPLCB *mplcb, QUEUE *aq )
{
QUEUE *fq;
INT size = AreaSize(aq);
/* Registration position search */
/* Search the free area whose size is equal to 'blksz',
* or larger than 'blksz' but closest.
* If it does not exist, return '&mplcb->freeque'.
*/
fq = searchFreeArea(mplcb, size);
/* Register */
clrAreaFlag(aq, AREA_USE);
if ( fq != &mplcb->freeque && FreeSize(fq) == size ) {
QueInsert(aq + 1, fq + 1);
} else {
QueInsert(aq + 1, fq);
}
QueInit(aq + 2);
}
/*
* Registration of free area on FreeQue
* FreeQue is composed of 2 types: Queue that links the different
* size of areas by size
* and queue that links the same size areas.
*
* imacb->freeque
* |
* | +-----------------------+ +-----------------------+
* | | AreaQue | | AreaQue |
* | +-----------------------+ +-----------------------+
* *---> FreeQue by size | *----> FreeQue same size ---->
* | | FreeQue same size ----* | EmptyQue |
* | | | | |
* | | | | |
* | +-----------------------+ +-----------------------+
* | | AreaQue | | AreaQue |
* v +-----------------------+ +-----------------------+
*/
LOCAL void appendFreeArea( QUEUE *aq, IMACB *imacb )
{
QUEUE *fq;
size_t size = AreaSize(aq);
/* Registration position search */
/* Search free area whose size is equal to 'blksz',
* or closest and larger than 'blksz'.
* If it can not be found, return '&imacb->freeque'.
*/
fq = searchFreeArea(size, imacb);
/* Registration */
clrAreaFlag(aq, AREA_USE);
if ( (fq != &imacb->freeque) && (FreeSize(fq) == size) ) {
QueInsert(aq + 1, fq + 1);
} else {
QueInsert(aq + 1, fq);
}
QueInit(aq + 2);
}
/*
* Change the active task state to wait state and connect to the timer wait
* queue and the object wait queue. Also set 'wid' in 'knl_ctxtsk'.
*/
EXPORT void knl_gcb_make_wait( GCB *gcb, TMO tmout )
{
*knl_ctxtsk->wercd = E_TMOUT;
if ( tmout != TMO_POL ) {
knl_ctxtsk->wid = gcb->objid;
knl_make_wait(tmout, gcb->objatr);
if ( (gcb->objatr & TA_TPRI) != 0 ) {
knl_queue_insert_tpri(knl_ctxtsk, &gcb->wait_queue);
} else {
QueInsert(&knl_ctxtsk->tskque, &gcb->wait_queue);
}
}
}
/*
* Task deletion
* Call from critical section
*/
EXPORT void knl_del_tsk( TCB *tcb )
{
#if USE_IMALLOC
if ( (tcb->tskatr & TA_USERBUF) == 0 ) {
/* User buffer is not used */
/* Free system stack */
VP stack = (VB*)tcb->isstack - tcb->sstksz;
knl_Ifree(stack);
}
#endif
/* Return control block to FreeQue */
QueInsert(&tcb->tskque, &knl_free_tcb);
tcb->state = TS_NONEXIST;
}
/*
* Delete from FreeQue
*/
EXPORT void knl_removeFreeQue( QUEUE *fq )
{
if ( fq->next == NULL ) { /* FreeQue Same size */
(fq + 1)->prev->next = (fq + 1)->next;
if ( (fq + 1)->next != NULL ) {
(fq + 1)->next->prev = (fq + 1)->prev;
}
} else { /* FreeQue Size order */
if ( (fq + 1)->next != NULL ) { /* having FreeQue Same size */
QueInsert((fq + 1)->next - 1, fq);
(fq + 1)->next->prev = (fq + 1)->prev;
}
QueRemove(fq);
}
}
/*
* TCB Initialization
*/
EXPORT ER task_initialize( void )
{
INT i;
TCB *tcb;
ID tskid;
/* Get system information */
i = _tk_get_cfn(SCTAG_TMAXTSKID, &max_tskid, 1);
if ( i < 1 || NUM_TSKID < 1 ) {
return E_SYS;
}
i = _tk_get_cfn(SCTAG_TSYSSTKSZ, &default_sstksz, 1);
if ( i < 1 || default_sstksz < MIN_SYS_STACK_SIZE ) {
return E_SYS;
}
i = _tk_get_cfn(SCTAG_TSVCLIMIT, &svc_call_limit, 1);
if ( i < 1 || svc_call_limit < 0 || svc_call_limit > 3 ) {
return E_SYS;
}
/* Allocate TCB area */
tcb_table = Imalloc((UINT)NUM_TSKID * sizeof(TCB));
if ( tcb_table == NULL ) {
return E_NOMEM;
}
/* Initialize task execution control information */
ctxtsk = schedtsk = NULL;
ready_queue_initialize(&ready_queue);
dispatch_disabled = DDS_ENABLE;
/* Register all TCBs onto FreeQue */
QueInit(&free_tcb);
for ( tcb = tcb_table, i = 0; i < NUM_TSKID; tcb++, i++ ) {
tskid = ID_TSK(i);
tcb->tskid = tskid;
tcb->state = TS_NONEXIST;
#ifdef NUM_PORID
tcb->wrdvno = tskid;
#endif
InitSVCLOCK(&tcb->svclock);
tcb->svclocked = NULL;
QueInsert(&tcb->tskque, &free_tcb);
}
return E_OK;
}
/*
* 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;
}
