void OSTakeMutex(OSMutex *mutex)
{
OSMakeAtomic();
if(!mutex->val)
{
procEnq(_running, _tasks, &mutex->blocked);
_tasks[_running].status |= _OS_BLOCKED;
OSExitAtomic();
OSSwap();
}
else
mutex->val=0;
OSExitAtomic();
}
void OSCreateMutex(OSMutex *mutex)
{
OSMakeAtomic();
mutex->val=1;
initQ(mutex->procList, OSMAX_TASKS, &mutex->blocked);
OSExitAtomic();
}
void OSGiveMutex(OSMutex *mutex)
{
OSMakeAtomic();
unsigned char wakeProc=procDeq(&mutex->blocked);
if(wakeProc!=255)
{
_tasks[wakeProc].status &= ~_OS_BLOCKED;
procEnq(wakeProc, _tasks, &_ready);
OSExitAtomic();
OSPrioSwap();
}
else
mutex->val=1;
OSExitAtomic();
}
void OSCreateMutex(OSMutex *mutex)
{
unsigned char sreg;
OSMakeAtomic(&sreg);
mutex->val=1;
initQ(mutex->procList, _maxTasks, &mutex->blocked);
OSExitAtomic(sreg);
}
void OSSignal(OSCond *cond)
{
unsigned char sreg;
OSMakeAtomic(&sreg);
if(cond->blockedProcess != 255)
{
_tasks[cond->blockedProcess].status &= ~_OS_BLOCKED;
procEnq(cond->blockedProcess, _tasks, &_ready);
cond->blockedProcess=255;
cond->pendingWake=0;
OSExitAtomic(sreg);
OSPrioSwap();
}
else
cond->pendingWake=1;
OSExitAtomic(sreg);
}
void enq(int pid, tQueue *q)
{
OSMakeAtomic();
if(q->ctr >= q->len)
return;
q->qptr[q->tail]=pid;
q->ctr++;
q->tail = (q->tail+1) % q->len;
OSExitAtomic();
}
void OSTakeSema(TOSSema *sema)
{
unsigned char sreg;
OSMakeAtomic(&sreg);
if(sema->semaval>0)
sema->semaval--;
else
{
// Block current process
_tasks[_running].status |= _OS_BLOCKED;
// Enqueue this task
prioEnq(_running, _tasks, &sema->taskQ);
OSExitAtomic(sreg);
// Call scheduler.
OSSwap();
}
OSExitAtomic(sreg);
}
unsigned char procDeq(tQueue *q)
{
unsigned char ret=255;
OSMakeAtomic();
if(q->ctr>0)
{
ret=q->qptr[q->head];
q->head=(q->head+1)%q->len;
q->ctr--;
}
OSExitAtomic();
return ret;
}
void OSGiveSema(TOSSema *sema)
{
unsigned char sreg;
OSMakeAtomic(&sreg);
unsigned char tsk=procDeq(&sema->taskQ);
if(tsk != 255)
{
// Removed blocked flag
_tasks[tsk].status &= ~(_OS_BLOCKED);
procEnq(tsk, _tasks, &_ready);
// Call scheduler
OSExitAtomic(sreg);
OSPrioSwap();
}
else
if(sema->isBinary)
sema->semaval=1;
else
sema->semaval++;
OSExitAtomic(sreg);
}
void initQ(unsigned char *qbuf, unsigned char len, tQueue *q)
{
unsigned char i;
OSMakeAtomic();
q->head=0;
q->tail=0;
q->qptr=qbuf;
q->len=len;
q->ctr=0;
for(i=0; i<len; i++)
q->qptr[i]=255;
OSExitAtomic();
}
void OSCreateSema(TOSSema *sema, unsigned int initval, unsigned char isBinary)
{
unsigned char sreg;
OSMakeAtomic(&sreg);
sema->isBinary=isBinary;
if(!isBinary)
sema->semaval=initval;
else
if(initval)
sema->semaval=1;
else
sema->semaval=0;
initQ(sema->tasklist, _maxTasks, &sema->taskQ);
OSExitAtomic(sreg);
}
// Priority queue routines
void prioEnq(int pid, tTCB *tasklist, tQueue *q)
{
unsigned char i;
unsigned int iter=q->head;
unsigned char flag=0;
OSMakeAtomic();
if(q->ctr >= q->len)
return;
while(iter != q->tail && !flag)
{
flag=(tasklist[q->qptr[iter]].prio > tasklist[pid].prio);
if(!flag)
iter=(iter+1) % q->len;
}
// If we have found our spot, shift the rest down and insert. Otherwise insert at the end
if(flag)
{
if(q->tail > q->head)
for(i=q->tail-1; i>=iter && i != 255; i--)
q->qptr[(i+1)%q->len]=q->qptr[i];
else
{
for(i=(q->tail > 0 ? q->tail-1 : q->len-1); i!=iter; i=(i>0 ? i-1 : q->len-1))
q->qptr[(i+1)%q->len]=q->qptr[i];
// Last case
q->qptr[(i+1)%q->len]=q->qptr[i];
}
}
else
iter=q->tail;
q->tail=(q->tail+1)%q->len;
q->qptr[iter]=pid;
q->ctr++;
OSExitAtomic();
}
// Can only be used by one process.
void OSWait(OSCond *cond, OSMutex *mutex)
{
OSMakeAtomic();
if(!cond->pendingWake)
{
cond->blockedProcess=_running;
_tasks[_running].status |= _OS_BLOCKED;
OSGiveMutex(mutex);
OSMakeAtomic();
// If val is 1 it means no swap took place.
if(mutex->val)
OSSwap();
// Retake the mutex when we resume
OSTakeMutex(mutex);
}
else
cond->pendingWake=0;
OSExitAtomic();
}