STATUS semQFlush(
SEM_ID semId
)
{
STATUS status;
int level;
INT_LOCK(level);
if (OBJ_VERIFY (semId, semClassId) != OK)
{
INT_UNLOCK (level);
status = ERROR;
}
else
{
/* Check next object */
if (Q_FIRST(&semId->qHead) == NULL)
{
INT_UNLOCK(level);
status = OK;
}
else
{
/* Enter kernel and flush pending queue */
kernelState = TRUE;
INT_UNLOCK(level);
vmxPendQFlush(&semId->qHead);
vmxExit();
status = OK;
}
}
return status;
}
STATUS wdDestroy(
WDOG_ID wdId,
BOOL dealloc
)
{
STATUS status;
int level;
/* Not callable from interrupts */
if (INT_RESTRICT() != OK)
{
status = ERROR;
}
else
{
/* Lock interrupts */
INT_LOCK(level);
/* Verify object */
if (OBJ_VERIFY(wdId, wdClassId) != OK )
{
INT_UNLOCK(level);
status = ERROR;
}
else
{
/* Terminate object */
objCoreTerminate(&wdId->objCore);
/* Enter kernel */
kernelState = TRUE;
/* Unlock interrupts */
INT_UNLOCK(level);
/* Cancel watchdog timer */
vmxWdCancel(wdId);
wdId->status = WDOG_DEAD;
taskSafe();
/* Exit kernel */
vmxExit();
/* Deallocate if requested */
if (dealloc == TRUE)
{
objFree(wdClassId, wdId);
}
taskUnsafe();
status = OK;
}
}
return status;
}
STATUS initServo(void) {
INT_LOCK();
// Servo1 pin config
DDRB = DDRB | 0x20; //making PORTB 5 pin output
PORTB = PORTB | 0x20; //setting PORTB 5 pin to logic 1
// Servo2 pin config
DDRB = DDRB | 0x40; //making PORTB 6 pin output
PORTB = PORTB | 0x40; //setting PORTB 6 pin to logic 1
// Servo3 pin config
DDRB = DDRB | 0x80; //making PORTB 7 pin output
PORTB = PORTB | 0x80; //setting PORTB 7 pin to logic 1
// Initialize timers
timer1Init();
/* Set initial motor positions */
OCR1AL = (SERVO1_MIN + SERVO1_MAX)/2;
OCR1AH = 0x00;
OCR1BL = (SERVO2_MIN + SERVO2_MAX)/2;
OCR1BH = 0x00;
OCR1CL = (SERVO3_MIN + SERVO3_MAX)/2;
OCR1CH = 0x00;
INT_UNLOCK();
return STATUS_OK;
}
/** Initialize ZigBee harware */
STATUS initZigbee() {
INT_LOCK(); /* Lock (disable) global interrupts */
/* Initailize UART1 for serial communiaction */
// Parameters:
// desired baud rate:9600
// actual baud rate:9600 (error 0.0%)
// char size: 8 bit
// parity: Disabled
UBRR0H = UBRRH_VALUE;
UBRR0L = UBRRL_VALUE;
#if USE_2X
UCSR0A |= _BV(U2X0);
#else
UCSR0A &= ~(_BV(U2X0));
#endif
UCSR0C = _BV(UCSZ01) | _BV(UCSZ00); /* 8-bit data */
UCSR0B = _BV(RXEN0) | _BV(TXEN0); /* Enable RX and TX */
INT_UNLOCK(); /* Enables the global interrupts */
return STATUS_OK;
}
LOCAL Q_MSG_NODE* qMsgEach(
Q_MSG_HEAD *pQHead,
FUNCPTR func,
ARG arg
)
{
Q_MSG_NODE *pNode;
int level;
/* Lock interrupts */
INT_LOCK(level);
/* Get first node */
pNode = pQHead->first;
/* While node not null and function returns true */
while ((pNode != NULL) && ((*func)(pNode, arg)))
{
pNode = pNode->next;
}
/* Unlock interrupts */
INT_UNLOCK(level);
return pNode;
}
STATUS netJobAdd(FUNCPTR func, ARG param0, ARG param1, ARG param2,
ARG param3, ARG param4)
{
int level, ringPut;
NET_JOB_NODE node;
/* Copy node */
node.func = func;
node.args[0] = param0;
node.args[1] = param1;
node.args[2] = param2;
node.args[3] = param3;
node.args[4] = param4;
/* Lock interrputs */
INT_LOCK(level);
/* Put job on ring buffer */
ringPut = rngBufPut(netWorkRing, (char *) &node, sizeof(node));
/* Unlock interrupts */
INT_UNLOCK(level);
if ( ringPut != sizeof(node) )
return ERROR;
/* Wake up network task */
semGive(netTaskSemId);
return OK;
}
STATUS wdCancel(
WDOG_ID wdId
)
{
STATUS status;
int level;
/* Lock interrupts */
INT_LOCK(level);
/* Verify object */
if (OBJ_VERIFY(wdId, wdClassId) != OK)
{
INT_UNLOCK(level);
status = ERROR;
}
else
{
/* If in kernel mode */
if (kernelState == TRUE)
{
/* Unlock interrupts */
INT_UNLOCK(level);
/* Add to kernel queue */
workQAdd1((FUNCPTR) vmxWdCancel, (ARG) wdId);
status = OK;
}
else
{
/* Enter kernel */
kernelState = TRUE;
/* Unlock interrupts */
INT_UNLOCK(level);
vmxWdCancel(wdId);
/* Exit kernel */
vmxExit();
status = OK;
}
}
return status;
}
STATUS semDestroy(
SEM_ID semId,
BOOL deallocate
)
{
STATUS status;
int level;
if (INT_RESTRICT() != OK)
{
errnoSet(S_intLib_NOT_ISR_CALLABLE);
status = ERROR;
}
else
{
INT_LOCK(level);
if (OBJ_VERIFY(semId, semClassId) != OK)
{
INT_UNLOCK(level);
status = ERROR;
}
else
{
objCoreTerminate(&semId->objCore);
/* Delete it */
kernelState = TRUE;
INT_UNLOCK(level);
vmxSemDelete(semId);
taskSafe();
vmxExit();
if (deallocate == TRUE)
{
objFree(semClassId, semId);
}
taskUnsafe();
status = OK;
}
}
return status;
}
/*
* These are interrupt on/off entry points. Disable interrupts
* during interrupt state transition.
*/
void brcms_intrson(struct brcms_info *wl)
{
unsigned long flags;
INT_LOCK(wl, flags);
brcms_c_intrson(wl->wlc);
INT_UNLOCK(wl, flags);
}
/** Set bot velocity using PWM (Pulse Width Modulation */
STATUS motorVelocitySet(BYTE leftMotor, BYTE rightMotor) {
INT_LOCK();
OCR5AL = leftMotor;
OCR5BL = rightMotor;
INT_UNLOCK();
return STATUS_OK;
}
/*
* These are interrupt on/off entry points. Disable interrupts
* during interrupt state transition.
*/
void wl_intrson(struct wl_info *wl)
{
unsigned long flags;
INT_LOCK(wl, flags);
wlc_intrson(wl->wlc);
INT_UNLOCK(wl, flags);
}
void wl_intrsrestore(struct wl_info *wl, u32 macintmask)
{
unsigned long flags;
INT_LOCK(wl, flags);
wlc_intrsrestore(wl->wlc, macintmask);
INT_UNLOCK(wl, flags);
}
LOCAL STATUS semBGive(
SEM_ID semId
)
{
STATUS status;
int level;
/* Lock interrupts */
INT_LOCK(level);
/* Verify class */
if (OBJ_VERIFY(semId, semClassId) != OK)
{
INT_UNLOCK(level);
status = ERROR;
}
else
{
/* Get next listening task from queue */
SEM_OWNER_SET(semId, Q_FIRST(&semId->qHead));
/* Check if no more tasks are waiting for this semaphore */
if (SEM_OWNER_GET(semId) == NULL)
{
INT_UNLOCK(level);
status = OK;
}
else
{
/* Enter kernel mode */
kernelState = TRUE;
INT_UNLOCK(level);
/* Unblock next task waiting */
vmxPendQGet(&semId->qHead);
/* Exit kernel mode */
vmxExit();
status = OK;
}
}
return status;
}
u32 wl_intrsoff(struct wl_info *wl)
{
unsigned long flags;
u32 status;
INT_LOCK(wl, flags);
status = wlc_intrsoff(wl->wlc);
INT_UNLOCK(wl, flags);
return status;
}
/** Get current bot velocity */
STATUS motorVelocityGet(BYTE *leftMotor, BYTE *rightMotor) {
ASSERT(leftMotor != NULL);
ASSERT(rightMotor != NULL);
INT_LOCK();
*leftMotor = OCR5AL;
*rightMotor = OCR5BL;
INT_UNLOCK();
return STATUS_OK;
}
/** Set bot motion direction */
STATUS motorDirectionSet(MotorDirection direction) {
BYTE PortARestore = 0;
INT_LOCK();
direction &= 0x0F; /* removing upper nibbel for the protection */
PortARestore = PORTA; /* reading the PORTA original status */
PortARestore &= 0xF0; /* making lower direction nibbel to 0 */
PortARestore |= direction; /* adding lower nibbel for forward command and
restoring the PORTA status */
PORTA = PortARestore; /* executing the command */
INT_UNLOCK();
return STATUS_OK;
}
STATUS getAdcValue(AdcChannel channel, UINT *value) {
UINT timeout = 0x1000;
*value = 0;
ASSERT(channel < ADC_LAST);
INT_LOCK();
/* Set MUX value */
if(channel > 7) {
ADCSRB = 0x08;
}
channel = channel & 0x07;
ADMUX = 0x20 | channel;
/* Set start conversion bit */
ADCSRA = ADCSRA | 0x40;
/* Wait for ADC conversion to complete */
while(((ADCSRA & 0x10)==0) && (timeout > 0)) {
timeout --;
}
if(timeout == 0) {
/* Failure to read ADC value */
INT_UNLOCK();
return !STATUS_OK;
}
*value = ADCH;
ADCSRA = ADCSRA | 0x10; /* Clear ADIF (ADC Interrupt Flag) by writing 1 to it */
ADCSRB = 0x00;
INT_UNLOCK();
return STATUS_OK;
}
/** Initialize left position encoder interrupt */
STATUS motorLeftPositionEncoderInit(void (*callbackLIntr)(void)) {
/* Check if argument is invalid or callback is already registered */
if(callbackLIntr == NULL || lposIsr != NULL)
return !STATUS_OK;
INT_LOCK();
EICRB = EICRB | 0x02; /* INT4 is set to falling edge */
EIMSK = EIMSK | 0x10; /* Enable INT4 */
INT_UNLOCK();
lposIsr = callbackLIntr;
return STATUS_OK;
}
LOCAL int qMsgInfo(
Q_MSG_HEAD *pQHead,
int nodeArray[],
int max
)
{
Q_MSG_NODE *pNode;
int *pElement;
int level;
int count;
/* Just count requested */
if (nodeArray == NULL)
{
count = pQHead->count;
}
else
{
/* Store element start */
pElement = nodeArray;
/* Lock interrupts */
INT_LOCK(level);
/* Get first node */
pNode = pQHead->first;
/* While node not null and max not reached */
while ((pNode != NULL) && (--max >= 0))
{
*(pElement++) = (int) pNode;
pNode = pNode->next;
}
/* Unlock interrupts */
INT_UNLOCK(level);
count = pElement - nodeArray;
}
return count;
}
static void wl_dpc(unsigned long data)
{
struct wl_info *wl;
wl = (struct wl_info *) data;
WL_LOCK(wl);
/* call the common second level interrupt handler */
if (wl->pub->up) {
if (wl->resched) {
unsigned long flags;
INT_LOCK(wl, flags);
wlc_intrsupd(wl->wlc);
INT_UNLOCK(wl, flags);
}
wl->resched = wlc_dpc(wl->wlc, true);
}
/* wlc_dpc() may bring the driver down */
if (!wl->pub->up)
goto done;
/* re-schedule dpc */
if (wl->resched)
tasklet_schedule(&wl->tasklet);
else {
/* re-enable interrupts */
wl_intrson(wl);
}
done:
WL_UNLOCK(wl);
}
STATUS taskDestroy(
int taskId,
BOOL freeStack,
unsigned timeout,
BOOL forceDestroy
)
{
STATUS status;
int i, level;
TCB_ID tcbId;
if (INT_RESTRICT() != OK)
{
errnoSet (S_intLib_NOT_ISR_CALLABLE);
return ERROR;
}
/* Get task context */
tcbId = taskTcb(taskId);
if (tcbId == NULL)
return ERROR;
/* If task self destruct and excption lib installed */
if (tcbId == taskIdCurrent) {
/* Wait for safe to destroy */
while (tcbId->safeCount > 0)
taskUnsafe();
/* Kill it */
status = excJobAdd(
(VOIDFUNCPTR) taskDestroy,
(ARG) tcbId,
(ARG) freeStack,
(ARG) WAIT_NONE,
(ARG) FALSE,
(ARG) 0,
(ARG) 0
);
/* Block here and suspend */
while(status == OK)
taskSuspend(0);
} /* End if task self destruct and exception lib installed */
taskDestroyLoop:
/* Lock interrupts */
INT_LOCK(level);
/* Check id */
if (TASK_ID_VERIFY(tcbId) != OK)
{
/* errno set by taskIdVerify() */
/* Unlock interrupts */
INT_UNLOCK(level);
return ERROR;
}
/* Mask all signals */
if (tcbId->pSignalInfo != NULL)
tcbId->pSignalInfo->sigt_blocked = 0xffffffff;
/* Block here for safe and running locked tasks */
while ( (tcbId->safeCount > 0) ||
( (tcbId->status == TASK_READY) && (tcbId->lockCount > 0) )
)
{
/* Enter kernel mode */
kernelState = TRUE;
/* Unlock interrupts */
INT_UNLOCK(level);
/* Check if force deletion, or suicide */
if (forceDestroy || (tcbId == taskIdCurrent))
{
/* Remove protections */
tcbId->safeCount = 0;
tcbId->lockCount = 0;
/* Check if flush of safety queue is needed */
if (Q_FIRST(&tcbId->safetyQ) != NULL)
vmxPendQFlush(&tcbId->safetyQ);
/* Exit trough kernel */
vmxExit();
}
else
{
/* Not forced deletion or suicide */
/* Put task on safe queue */
if (vmxPendQPut(&tcbId->safetyQ, timeout) != OK)
{
/* Exit trough kernel */
vmxExit();
errnoSet (S_taskLib_INVALID_TIMEOUT);
return ERROR;
}
/* Exit trough kernel */
status = vmxExit();
/* Check for restart */
if (status == SIG_RESTART)
{
timeout = (sigTimeoutRecalc)(timeout);
goto taskDestroyLoop;
}
/* Check if unsuccessful */
if (status == ERROR)
{
/* timer should have set errno to S_objLib_TIMEOUT */
return ERROR;
}
} /* End else forced or suicide */
/* Lock interrupts */
INT_LOCK(level);
/* Now verify class id again */
if (TASK_ID_VERIFY(tcbId) != OK)
{
/* errno set by taskIdVerify() */
/* Unlock interrupts */
INT_UNLOCK(level);
return ERROR;
}
} /* End while blocked by safety */
/* Now only one cadidate is selected for deletion */
/* Make myself safe */
taskSafe();
/* Protet deletion cadidate */
tcbId->safeCount++;
/* Check if not suicide */
if (tcbId != taskIdCurrent)
{
/* Enter kernel mode */
kernelState = TRUE;
/* Unlock interrupts */
INT_UNLOCK(level);
/* Suspend victim */
vmxSuspend(tcbId);
/* Exit trough kernel */
vmxExit();
}
else
{
/* Unlock interrupts */
INT_UNLOCK(level);
}
/* Run deletion hooks */
for (i = 0; i < MAX_TASK_DELETE_HOOKS; i++)
if (taskDeleteHooks[i] != NULL)
(*taskDeleteHooks[i])(tcbId);
/* Lock task */
taskLock();
/* If dealloc and options dealloc stack */
if ( freeStack && (tcbId->options & TASK_OPTIONS_DEALLOC_STACK) ) {
#if (_STACK_DIR == _STACK_GROWS_DOWN)
objFree(taskClassId, tcbId->pStackEnd);
#else /* _STACK_GROWS_UP */
objFree(taskClassId, tbcId - TASK_EXTRA_BYTES);
#endif /* _STACK_DIR */
}
/* Lock interrupts */
INT_LOCK(level);
/* Invalidate id */
objCoreTerminate(&tcbId->objCore);
/* Enter kernel mode */
kernelState = TRUE;
/* Unlock interrupts */
INT_UNLOCK(level);
/* Delete task */
status = vmxDelete(tcbId);
/* Check if safe quque needs to be flushed */
if (Q_FIRST(&tcbId->safetyQ) != NULL)
vmxPendQFlush(&tcbId->safetyQ);
/* Exit trough kernel */
vmxExit();
/* Unprotect */
taskUnlock();
taskUnsafe();
return OK;
}
STATUS msgQInfoGet(
MSG_Q_ID msgQId,
MSG_Q_INFO *pInfo
)
{
STATUS status;
int level;
Q_HEAD *pendQ;
/* Lock interrupts */
INT_LOCK(level);
/* Verify object class */
if (OBJ_VERIFY(msgQId, msgQClassId) != OK)
{
INT_UNLOCK(level);
status = ERROR;
}
else
{
/* If no messages */
if (msgQId->msgQ.count == 0)
{
pendQ = &msgQId->msgQ.pendQ;
}
else
{
pendQ = &msgQId->freeQ.pendQ;
}
/* If task id list requested */
if (pInfo->taskIdList != NULL)
{
Q_INFO(pendQ, pInfo->taskIdList, pInfo->taskIdListMax);
}
/* If message list or number requested */
if ((pInfo->msgPtrList != NULL) || (pInfo->msgLengthList != NULL))
{
pInfo->numMsg = 0;
if (pInfo->msgListMax > 0)
{
Q_EACH(&msgQId->msgQ, msgQInfoEach, pInfo);
}
}
/* Setup info structure */
pInfo->numMsg = msgQId->msgQ.count;
pInfo->numTask = Q_INFO(pendQ, NULL, 0);
pInfo->options = msgQId->options;
pInfo->maxMsg = msgQId->maxMsg;
pInfo->maxMsgLength = msgQId->maxMsgLength;
pInfo->sendTimeouts = msgQId->sendTimeouts;
pInfo->reciveTimeouts = msgQId->reciveTimeouts;
/* Unlock interrupts */
INT_UNLOCK(level);
status = OK;
}
return status;
}
STATUS msgQShow(
MSG_Q_ID msgQId,
int mode
)
{
STATUS status;
MSG_Q_INFO info;
TCB_ID tcbId;
char tmpString[15];
int level;
int taskIdList[20], taskDList[20];
char *msgPtrList[20];
int msgLengthList[20];
int i, j, len;
char *pMsg;
/* Clear info */
memset(&info, 0, sizeof(info));
/* If mode ge. 1 */
if (mode >= 1)
{
/* Get info for each message, task pending */
info.taskIdList = taskIdList;
info.taskIdListMax = NELEMENTS(taskIdList);
info.msgPtrList = msgPtrList;
info.msgLengthList = msgLengthList;
info.msgListMax = NELEMENTS(msgPtrList);
}
/* Lock interrupts */
INT_LOCK(level);
/* Get message queue info structure */
status = msgQInfoGet(msgQId, &info);
if (status != OK)
{
INT_UNLOCK(level);
printf("Invalid message queue id: %#x\n", (int) msgQId);
}
else
{
/* If show tasks pending */
if ((info.numTask > 0) && (mode >= 1))
{
/* For all in id list */
for (i = 0;
i < min(info.numTask, NELEMENTS(taskIdList));
i++)
{
/* Get tcb */
tcbId = (TCB_ID) taskIdList[i];
if (tcbId->status & TASK_DELAY)
{
taskDList[i] = Q_KEY(&tickQHead, &tcbId->tickNode, 1);
}
else
{
taskDList[i] = 0;
}
}
}
/* Unlock interrupts */
INT_UNLOCK(level);
/* Get options string */
if ((info.options & MSG_Q_TYPE_MASK) == MSG_Q_FIFO)
{
strcpy(tmpString, "MSG_Q_FIFO");
}
else
{
strcpy(tmpString, "MSG_Q_PRIORITY");
}
/* Print summary */
printf("\n");
printf("Message Queue Id : 0x%-10x\n", (int) msgQId);
if ((info.options & MSG_Q_TYPE_MASK) == MSG_Q_FIFO)
{
printf("Task Queuing : %-10s\n", "FIFO");
}
else
{
printf("Task Queuing : %-10s\n", "PRIORITY");
}
printf("Message Byte Len : %-10d\n", info.maxMsgLength);
printf("Messages Max : %-10d\n", info.maxMsg);
printf("Messages Queued : %-10d\n", info.numMsg);
if (info.numMsg == info.maxMsg)
{
printf("Senders Blocked : %-10d\n", info.numTask);
}
else
{
printf("Receivers Blocked : %-10d\n", info.numTask);
}
printf("Send timeouts : %-10d\n", info.sendTimeouts);
printf("Receive timeouts : %-10d\n", info.reciveTimeouts);
printf("Options : 0x%x\t%s\n", info.options, tmpString);
/* If detailed info requested */
if (mode >= 1)
{
/* If tasks pending */
if (info.numTask > 0)
{
/* Get sender/receiver string */
if (info.numMsg == info.maxMsg)
{
strcpy(tmpString, "Senders");
}
else
{
strcpy(tmpString, "Receivers");
}
printf("\n%s Blocked:\n", tmpString);
printf(" NAME TID PRI TIMEOUT\n");
printf("---------- -------- --- -------\n");
/* For all tasks */
for (i = 0; i < min(info.numTask, NELEMENTS(taskIdList)); i++)
{
printf(
"%-11.11s%8x %3d %7u\n",
taskName(taskIdList[i]),
taskIdList[i],
((TCB_ID) taskIdList[i])->priority,
taskDList[i]
);
}
}
/* If messages pending */
if (info.numMsg > 0)
{
printf(
"\nMessages queued:\n"
" # address length value\n"
);
/* For all messages */
for (i = 0; i < min(info.numMsg, NELEMENTS(msgPtrList)); i++)
{
/* Get message and length */
pMsg = msgPtrList[i];
len = msgLengthList[i];
printf("%3d %#10x %4d ", i + 1, (int) pMsg, len);
/* For length */
for (j = 0; j < min(len, 20); j++)
{
if ((j % 4) == 0)
{
printf(" 0x");
}
printf("%02x", pMsg[j] & 0xff);
}
if (len > 20)
{
printf(" ...");
}
printf("\n");
}
}
}
printf("\n");
}
return status;
}
STATUS wdStart(
WDOG_ID wdId,
int delay,
FUNCPTR func,
ARG arg
)
{
STATUS status;
int level;
/* Lock interrupts */
INT_LOCK(level);
/* Verify object */
if (OBJ_VERIFY(wdId, wdClassId) != OK)
{
INT_UNLOCK(level);
status = ERROR;
}
else
{
/* If in kernel mode */
if (kernelState == TRUE)
{
wdId->dfrStartCount++;
wdId->wdFunc = func;
wdId->wdArg = arg;
/* Unlock interrupts */
INT_UNLOCK(level);
/* Add to kernel queue */
workQAdd2((FUNCPTR) vmxWdStart, (ARG) wdId, (ARG) delay);
status = OK;
}
else
{
wdId->dfrStartCount++;
wdId->wdFunc = func;
wdId->wdArg = arg;
/* Enter kernel */
kernelState = TRUE;
/* Unlock interrupts */
INT_UNLOCK(level);
/* Start watchdog timer */
if (vmxWdStart(wdId, delay) != OK)
{
vmxExit();
status = ERROR;
}
else
{
/* Exit kernel */
vmxExit();
status = OK;
}
}
}
return status;
}
Q_MSG_NODE* qMsgGet(
MSG_Q_ID msgQId,
Q_MSG_HEAD *pQHead,
unsigned timeout
)
{
STATUS status;
int level;
Q_MSG_NODE *pNode;
/* Lock interrupts */
INT_LOCK(level);
while ((pNode = pQHead->first) == NULL)
{
if (timeout == WAIT_NONE)
{
errnoSet(S_objLib_UNAVAILABLE);
INT_UNLOCK(level);
pNode = NULL;
break;
}
/* Enter kernel mode */
kernelState = TRUE;
INT_UNLOCK(level);
/* Put task on hold */
vmxPendQPut(&pQHead->pendQ, timeout);
/* Exit trough kernel */
status = vmxExit();
if (status == SIG_RESTART)
{
pNode = (Q_MSG_NODE *) NONE;
break;
}
if (status != OK)
{
pNode = NULL;
break;
}
/* Verify object */
if (OBJ_VERIFY(msgQId, msgQClassId) != OK)
{
errnoSet(S_objLib_DELETED);
pNode = NULL;
break;
}
/* Lock interrupts */
INT_LOCK(level);
}
if ((pNode != NULL) && (pNode != (Q_MSG_NODE *) NONE))
{
pQHead->first = pNode->next;
pQHead->count--;
INT_UNLOCK(level);
}
return pNode;
}
STATUS qMsgPut(
MSG_Q_ID msgQId,
Q_MSG_HEAD *pQHead,
Q_MSG_NODE *pNode,
int key
)
{
STATUS status;
int level;
if (key == Q_MSG_PRI_TAIL)
{
/* Add to tail */
pNode->next = NULL;
/* Lock interrupts */
INT_LOCK(level);
/* Insert */
if (pQHead->first == NULL)
{
pQHead->last = pNode;
pQHead->first = pNode;
}
else
{
pQHead->last->next = pNode;
pQHead->last = pNode;
}
}
else
{
/* Lock interrupts */
INT_LOCK(level);
/* Insert at head */
if ((pNode->next = pQHead->first) == NULL)
{
pQHead->last = pNode;
}
pQHead->first = pNode;
}
/* Increase counter */
pQHead->count++;
if (kernelState == TRUE)
{
INT_UNLOCK(level);
workQAdd2((FUNCPTR) qMsgPendQGet, msgQId, pQHead);
}
else
{
/* Check if anybody is waiting for message */
if (Q_FIRST(&pQHead->pendQ) == NULL)
{
INT_UNLOCK(level);
status = OK;
}
else
{
/* Unlock pedning task waiting for message */
kernelState = TRUE;
INT_UNLOCK(level);
vmxPendQGet(&pQHead->pendQ);
vmxExit();
status = OK;
}
}
return status;
}
LOCAL STATUS semBTake(
SEM_ID semId,
unsigned timeout
)
{
STATUS status;
int level;
if (INT_RESTRICT() != OK)
{
errnoSet (S_intLib_NOT_ISR_CALLABLE);
status = ERROR;
}
else
{
/* Loop here if status is SIG_RESTART */
do
{
/* Lock interrupts */
INT_LOCK(level);
/* Verify class */
if (OBJ_VERIFY(semId, semClassId) != OK)
{
INT_UNLOCK(level);
status = ERROR;
break;
}
/* Check if it is already given back */
if (SEM_OWNER_GET(semId) == NULL)
{
/* Then take it */
SEM_OWNER_SET(semId, taskIdCurrent);
/* Unlock interrupts */
INT_UNLOCK(level);
status = OK;
break;
}
if (timeout == WAIT_NONE)
{
INT_UNLOCK(level);
errnoSet(S_objLib_UNAVAILABLE);
status = ERROR;
break;
}
/* Enter kernel mode */
kernelState = TRUE;
INT_UNLOCK(level);
/* Put on pending queue */
vmxPendQPut(&semId->qHead, timeout);
/* Exit through kernel */
status = vmxExit();
} while(status == SIG_RESTART);
}
return status;
}
