/*
* ======== SemaphoreMP_pend ========
*/
Bool SemaphoreMP_pend(SemaphoreMP_Object *obj)
{
UInt tskKey;
SemaphoreMP_PendElem *elem;
IArg gateMPKey;
/* Check for correct calling context */
Assert_isTrue((BIOS_getThreadType() == BIOS_ThreadType_Task),
SemaphoreMP_A_badContext);
elem = ThreadLocal_getspecific(SemaphoreMP_pendElemKey);
if (elem == NULL) {
/*
* Choose region zero (instead of the region that contains the
* SemaphoreMP) since region zero is always accessible by all cores
*/
elem = Memory_alloc(SharedRegion_getHeap(0),
sizeof(SemaphoreMP_PendElem), 0, NULL);
ThreadLocal_setspecific(SemaphoreMP_pendElemKey, elem);
}
/* Enter the gate */
gateMPKey = GateMP_enter((GateMP_Handle)obj->gate);
if (obj->cacheEnabled) {
Cache_inv(obj->attrs, sizeof(SemaphoreMP_Attrs), Cache_Type_ALL, TRUE);
}
/* check semaphore count */
if (obj->attrs->count == 0) {
/* lock task scheduler */
tskKey = Task_disable();
/* get task handle and block tsk */
elem->task = (Bits32)Task_self();
elem->procId = MultiProc_self();
Task_block((Task_Handle)elem->task);
if (obj->cacheEnabled) {
Cache_wbInv(elem, sizeof(SemaphoreMP_PendElem), Cache_Type_ALL, TRUE);
}
/* add it to pendQ */
ListMP_putTail((ListMP_Handle)obj->pendQ, (ListMP_Elem *)elem);
/* Leave the gate */
GateMP_leave((GateMP_Handle)obj->gate, gateMPKey);
Task_restore(tskKey);/* the calling task will switch out here */
return (TRUE);
}
else {
obj->attrs->count--;
if (obj->cacheEnabled) {
Cache_wbInv(obj->attrs, sizeof(SemaphoreMP_Attrs), Cache_Type_ALL,
TRUE);
}
/* Leave the gate */
GateMP_leave((GateMP_Handle)obj->gate, gateMPKey);
return (TRUE);
}
}
/*
* ======== GateMutexPri_Gate ========
* Returns FIRST_ENTER when it gets the gate, returns NESTED_ENTER
* on nested calls.
*/
IArg GateMutexPri_enter(GateMutexPri_Object *obj)
{
Task_Handle tsk;
UInt tskKey;
Int tskPri;
Task_PendElem elem;
Queue_Handle pendQ;
/* make sure we're not calling from Hwi or Swi context */
Assert_isTrue(((BIOS_getThreadType() == BIOS_ThreadType_Task) ||
(BIOS_getThreadType() == BIOS_ThreadType_Main)),
GateMutexPri_A_badContext);
pendQ = GateMutexPri_Instance_State_pendQ(obj);
tsk = Task_self();
/*
* Prior to tasks starting, Task_self() will return NULL.
* Simply return NESTED_ENTER here as, by definition, there is
* is only one thread running at this time.
*/
if (tsk == NULL) {
return (NESTED_ENTER);
}
tskPri = Task_getPri(tsk);
/*
* Gate may only be called from task context, so Task_disable is sufficient
* protection.
*/
tskKey = Task_disable();
/* If the gate is free, take it. */
if (obj->mutexCnt == 1) {
obj->mutexCnt = 0;
obj->owner = tsk;
obj->ownerOrigPri = tskPri;
Task_restore(tskKey);
return (FIRST_ENTER);
}
/* At this point, the gate is already held by someone. */
/* If this is a nested call to gate... */
if (obj->owner == tsk) {
Task_restore(tskKey);
return (NESTED_ENTER);
}
/*
* Donate priority if necessary. The owner is guaranteed to have the
* highest priority of anyone waiting on the gate, so just compare this
* task's priority against the owner's.
*/
if (tskPri > Task_getPri(obj->owner)) {
Task_setPri(obj->owner, tskPri);
}
/* Remove tsk from ready list. */
Task_block(tsk);
elem.task = tsk;
elem.clock = NULL;
/* leave a pointer for Task_delete() */
tsk->pendElem = &elem;
/* Insert tsk in wait queue in order by priority (high pri at head) */
GateMutexPri_insertPri(pendQ, (Queue_Elem *)&elem, tskPri);
/* Task_restore will call the scheduler and this task will block. */
Task_restore(tskKey);
tsk->pendElem = NULL;
/*
* At this point, tsk has the gate. Initialization of the gate is handled
* by the previous owner's call to leave.
*/
return (FIRST_ENTER);
}
