/*
* Free open management block
*/
LOCAL void delOpnCB( OpnCB *opncb )
{
QueRemove(&opncb->q);
QueRemove(&opncb->resq);
QueInsert(&opncb->q, &FreeOpnCB);
opncb->resid = 0;
}
/*
* 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;
}
/*
* 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);
}
/*
* Free request management block
*/
EXPORT void knl_delReqCB( ReqCB *reqcb )
{
QueRemove(&reqcb->q);
QueInsert(&reqcb->q, &knl_FreeReqCB);
reqcb->opncb = NULL;
}
/*
* Free request management block
*/
LOCAL void delReqCB( ReqCB *reqcb )
{
QueRemove(&reqcb->q);
QueInsert(&reqcb->q, &FreeReqCB);
reqcb->opncb = NULL;
}
/* |------------------+------------------------------------------------------------| */
StatusType ReleaseResource ( 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((knl_ctxtsk->resque.prev == &rescb->resque),E_OS_NOFUNC);
oldpri = rescb->tskpri;
ceilpri = knl_gres_table[ResID];
OS_CHECK_EXT((ceilpri <= knl_ctxtsk->itskpri),E_OS_ACCESS);
BEGIN_CRITICAL_SECTION;
knl_ctxtsk->priority = oldpri;
QueRemove(&rescb->resque);
QueInit(&rescb->resque);
if(oldpri > knl_ready_queue.top_priority)
{
knl_preempt();
}
END_CRITICAL_SECTION;
Error_Exit:
#if(cfgOS_ERROR_HOOK == STD_ON)
if(E_OK != ercd)
{
BEGIN_CRITICAL_SECTION;
_errorhook_svcid = OSServiceId_ReleaseResource;
_errorhook_par1.resid = ResID;
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;
}
}
/* |------------------+-------------------------------------------------------------| */
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;
}
/*
* 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);
}
}
EXPORT void knl_wait_release_tmout( TCB *tcb )
{
QueRemove(&tcb->tskque);
knl_make_non_wait(tcb);
if ( tcb->wspec->rel_wai_hook != NULL ) {
(*tcb->wspec->rel_wai_hook)(tcb);
}
}
/* |------------------+------------------------------------------------------------| */
StatusType ReleaseResource ( 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];
if(in_indp()) /* Interrupt level */
{
/* not supported */
}
else
{
OS_CHECK_EXT((knl_ctxtsk->resque.prev == &rescb->resque),E_OS_NOFUNC);
oldpri = knl_gres_table[ResID];
newpri = rescb->tskpri;
OS_CHECK_EXT((newpri > oldpri),E_OS_ACCESS);
BEGIN_CRITICAL_SECTION;
if(oldpri < 0)
{
/* Task share resource with ISR */
/* should change IPL */
/* not supported */
}
else
{
knl_ctxtsk->priority = newpri;
QueRemove(&rescb->resque);
QueInit(&rescb->resque);
if(newpri > knl_ready_queue.top_priority)
{
knl_preempt();
}
}
END_CRITICAL_SECTION;
}
Error_Exit:
#if(cfgOS_ERROR_HOOK == STD_ON)
if(E_OK != ercd)
{
BEGIN_CRITICAL_SECTION;
_errorhook_svcid = OSServiceId_ReleaseResource;
_errorhook_par1.resid = ResID;
CallErrorHook(ercd);
END_CRITICAL_SECTION;
}
#endif /* cfgOS_ERROR_HOOK */
return ercd;
}
Public dslFrameBuffer * DslFrameNativeDequeBuffer(dslFrame *pFrame)
{
dslFrameBuffer *pBuf;
pBuf = (dslFrameBuffer *) QueFirst((QueHeader *)&pFrame->head);
if (NULL != pBuf) {
pFrame->totalLength -= DslFrameNativeBufferGetLength(pBuf);
pFrame->bufCnt--;
QueRemove((QueHeader *)&pFrame->head);
}
return pBuf;
}
/*
* 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);
}
}
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;
}
/*
* An insertion into the queue that waits for accepting a request
* Call by executing "LockGDI()"
*/
LOCAL ER gdi_sndreq( DRQ *drq, GDI gdi )
{
T_DEVREQ *req = drq->req;
UINT setptn;
ER err;
if ( req->abort ) { err = E_ABORT; goto err_ret1; }
/* Insert into the queue that waits for accepting */
setptn = 0;
if ( req->cmd == TDC_READ ) {
if ( req->start < 0 ) {
if ( gdi->acpa[0]++ == 0 ) setptn |= DRP_AREAD;
} else {
if ( gdi->acpd[0]++ == 0 ) setptn |= DRP_DREAD;
}
} else {
if ( req->start < 0 ) {
if ( gdi->acpa[1]++ == 0 ) setptn |= DRP_AWRITE;
} else {
if ( gdi->acpd[1]++ == 0 ) setptn |= DRP_DWRITE;
}
}
QueInsert((QUEUE*)drq, &gdi->acpq);
if ( setptn != 0 ) {
/* Notify the insertion into the queue that waits for accepting */
err = tk_set_flg(gdi->flgid, setptn);
if ( err < E_OK ) goto err_ret2;
}
return E_OK;
err_ret2:
cntacpq(req, gdi, --);
QueRemove((QUEUE*)drq);
err_ret1:
DEBUG_PRINT(("gdi_sndreq err = %d\n", err));
return err;
}
EXPORT void knl_timer_handler( void )
{
TMEB *event;
knl_clear_hw_timer_interrupt(); /* Clear timer interrupt */
BEGIN_CRITICAL_SECTION;
knl_current_time = ll_add(knl_current_time, uitoll(CFN_TIMER_PERIOD));
#if USE_DBGSPT && defined(USE_FUNC_TD_INF_TSK)
if ( knl_ctxtsk != NULL ) {
/* Task at execution */
if ( knl_ctxtsk->sysmode > 0 ) {
knl_ctxtsk->stime += CFN_TIMER_PERIOD;
} else {
knl_ctxtsk->utime += CFN_TIMER_PERIOD;
}
}
#endif
/* Execute event that passed occurring time. */
while ( !isQueEmpty(&knl_timer_queue) ) {
event = (TMEB*)knl_timer_queue.next;
if ( ll_cmp(event->time, knl_current_time) > 0 ) {
break;
}
QueRemove(&event->queue);
if ( event->callback != NULL ) {
(*event->callback)(event->arg);
}
}
END_CRITICAL_SECTION;
knl_end_of_hw_timer_interrupt(); /* Clear timer interrupt */
}
EXPORT void knl_wait_release( TCB *tcb )
{
knl_timer_delete(&tcb->wtmeb);
QueRemove(&tcb->tskque);
knl_make_non_wait(tcb);
}
/*
* When the task priority changes, adjust the task position at the wait queue.
* It is called only if the object attribute TA_TPRI is specified.
*
*/
EXPORT void knl_gcb_change_priority( GCB *gcb, TCB *tcb )
{
QueRemove(&tcb->tskque);
knl_queue_insert_tpri(tcb, &gcb->wait_queue);
}
/*
* Accept requests
* Fetch one request from the request accept queue.
* When there is no request in queue, the device driver shall enter into a wait status until request is made.
* Specify "acpptn" by ORing the pattern of accepted request type (TDC_READ/TDC_WRITE)
* or the value obtained in "DEVREQ_ACPPTN()" in the pattern of user command.
* In addition, the request regarding specific data and attribute data can be individually accepted.
* (Refer to the following descriptions of the accept-wait-extended function).
* As for the acceptance of request, normal request (including the case of individual specification of "TDC_READ", "TDC_WRITE", "specific data", and "attribute data")
* is prioritized over the user command.
* However, normal request and user command may be simultaneously received.
* Specify a time-out time in milli second for "tmout".
* "TMO_POL" and "TMO_FEVR" also can be specified.
* Return the received request pattern or error to the return value.
* Return the pattern of the accepted request in the format specified for "acpptn".
* Briefly, when the accept-wait-extended function is used,
* the request for the specific data and the request for attribute are individually indicated.
* In this case,"DRP_ADSEL" is also set.
*
* In the case of Normal request
* Return the pattern, which indicates the type of the accepted request, to the return value.
* Return the accepted request to "*devreq"
* In the case of User command
* Return the pattern, which indicates the accepted user command, to the return value.
* When several kinds of user command are accumulated,
* the pattern, in which all the user commands specified at "acpptn" are collectively accepted and ORed, shall be returned
*
* Return NULL to "*devreq"
* In the case where normal request and user command are simultaneously accepted.
* Return the pattern, which ORed both the accepted normal request and user command
* , to the return value.
* Return the accepted normal request to "*devreq".
* In the case of time-out or error
* Return the error code to the return value. "E_TMOUT" is returned in case of time out.
* "*devreq" is indeterminate.
*
* "exinf" of the accepted request(T_DEVREQ) shall never be changed.
* Checking of "buf" space (ChkSpace) is already executed in driver I/F
* Reply to user command (GDI_Reply) is unnecessary.
*
* Generally, the next request is newly received after one request is accepted and processed
* and the result is returned.
* However, several requests may be received and simultaneously processed.
* In the case of processing several requests simultaneously, the several request processing tasks may execute each "GDI_Accept()" to be processed in parallel.
* Or one processing task may execute several "GDI_Accept()" to be processed simultaneously.
*
* It does not matter that the order of accepting request and the order of returning result are not necessarily the same.
*
*Accept-wait-extended function (Specify the accept-wait for specific data and attribute data individually)
* DRP_DREAD Read the specific data
* DRP_DWRITE Write the specific data
* DRP_AREAD Read the attribute data
* DRP_AWRITE Write the attribute data
* These shall be specified by ORing these above.
*
* "DRP_DREAD |DRP_AREAD" is equivalent to "DRP_READ".
* "DRP_DWRITE|DRP_AWRITE" is equivalent to "DRP_WRITE".
*
* These specific data and attribute data individual specification cannot be simultaneously combined
* and used with "DRP_READ" and "DRP_WRITE" .
*/
EXPORT INT GDI_Accept( T_DEVREQ **devreq, INT _acpptn, TMO tmout, GDI gdi )
{
DRQ *drq;
T_DEVREQ *req;
QUEUE *q;
INT reqptn, rptn;
UINT acpptn, aptn;
SYSTIM stime, ctime;
TMO tmo;
ER err;
if ( (_acpptn & DRP_ADSEL) == 0 ) {
/* Normal specification */
if ( (_acpptn & ~(DRP_NORMREQ|DRP_USERCMD)) != 0 )
{ err = E_PAR; goto err_ret; }
acpptn = ((_acpptn & DRP_NORMREQ) << 8) | _acpptn;
} else {
/* Extended specification*/
if ( (_acpptn & ~(DRP_REQMASK|DRP_USERCMD)) != 0 )
{ err = E_PAR; goto err_ret; }
acpptn = _acpptn & ~DRP_ADSEL;
}
aptn = 0;
tmo = TMO_FEVR;
stime.lo = 0;
for ( ;; ) {
/* Fetch the request from queue */
LockGDI(gdi);
drq = NULL;
req = NULL; rptn = 0; /* "warning" measures */
for ( q = gdi->acpq.next; q != &gdi->acpq; q = q->next ) {
req = ((DRQ*)q)->req;
rptn = DEVREQ_ACPPTN(req->cmd);
if ( req->start < 0 ) rptn <<= 8;
if ( (rptn & acpptn) == 0 ) continue;
drq = (DRQ*)q;
cntacpq(req, gdi, --);
QueRemove(q);
QueInit(q);
break;
}
UnlockGDI(gdi);
reqptn = aptn & DRP_USERCMD;
if ( drq != NULL ) {
if ( req->abort ) {
/* Abort processing */
req->error = E_ABORT;
GDI_Reply(req, gdi);
continue;
}
/* Normal request was accepted */
*devreq = req;
reqptn |= rptn;
if ( (aptn &= ~rptn & DRP_REQMASK) != 0 ) {
/* Reset the unnecessarily cleared flag */
tk_set_flg(gdi->flgid, aptn);
}
break;
}
if ( reqptn != 0 ) {
/* Only the user command is accepted */
*devreq = NULL;
break;
}
/* Remaining waiting time */
if ( tmout != TMO_FEVR ) {
err = tk_get_otm(&ctime);
if ( err < E_OK ) goto err_ret;
if ( tmo < 0 ) {
stime = ctime;
tmo = tmout;
} else {
tmo = tmout - (ctime.lo - stime.lo);
if ( tmo < 0 ) tmo = 0;
}
}
/* Wait for the request to come */
err = tk_wai_flg(gdi->flgid, acpptn, TWF_ORW | TWF_BITCLR,
&aptn, tmo);
if ( err < E_OK ) goto err_ret;
aptn &= acpptn;
}
if ( (reqptn & DRP_REQMASK) != 0 ) {
if ( (_acpptn & DRP_ADSEL) == 0 ) {
/* Normal specification */
reqptn |= (reqptn & (DRP_NORMREQ << 8)) >> 8;
reqptn &= ~(DRP_NORMREQ << 8);
} else {
/*
* release KeyState
*/
EXPORT void kpReleaseKey( KeyState *ks )
{
QueRemove((QUEUE*)ks);
QueInsert((QUEUE*)ks, &StatMach.freq);
}