/* NOTE: the SST scheduler is entered and exited with interrupts LOCKED */
void sst_schedule(void)
{
sst_uint8_t pin = sst_curr_prio; /* save the initial priority */
sst_uint8_t p; /* the new priority */
/* is the new priority higher than the initial? */
//printf("%d | %d ", sst_curr_prio, sst_ready_set);
while ((p = log2lkup[sst_ready_set]) > pin)
{
task_cb *tcb = &g_task_cb[p - 1];
sst_event e = tcb->queue[tcb->tail];/* get the event out of the queue */
if ((++tcb->tail) == tcb->end)
{
tcb->tail = (sst_uint8_t)0;
}
if ((--tcb->nUsed) == (sst_uint8_t)0) /* is the queue becoming empty?*/
{
sst_ready_set &= ~tcb->mask; /* remove from the ready set */
}
sst_curr_prio = p; /* this becomes the current task priority */
SST_INT_UNLOCK(); /* unlock the interrupts */
(*tcb->task)(e); /* call the SST task */
SST_INT_LOCK(); /* lock the interrupts for the next pass */
}
sst_curr_prio = pin; /* restore the initial priority */
}
/*..........................................................................*/
void sst_mutex_unlock(sst_uint8_t org_prio) {
SST_INT_LOCK();
if (org_prio < sst_curr_prio) {
sst_curr_prio = org_prio; /* restore the saved priority to unlock */
sst_schedule(); /* invoke scheduler after lowering the priority */
}
SST_INT_UNLOCK();
}
/*..........................................................................*/
void SST_mutexUnlock(uint8_t orgPrio) {
SST_INT_LOCK();
if (orgPrio < SST_currPrio_) {
SST_currPrio_ = orgPrio; /* restore the saved priority to unlock */
SST_schedule_(); /* invoke scheduler after lowering the priority */
}
SST_INT_UNLOCK();
}
/*..........................................................................*/
uint8_t SST_mutexLock(uint8_t prioCeiling) {
uint8_t p;
SST_INT_LOCK();
p = SST_currPrio_; /* the original SST priority to return */
if (prioCeiling > SST_currPrio_) {
SST_currPrio_ = prioCeiling; /* raise the SST priority */
}
SST_INT_UNLOCK();
return p;
}
/*..........................................................................*/
sst_uint8_t sst_mutex_lock(sst_uint8_t prio_ceiling)
{
sst_uint8_t p;
SST_INT_LOCK();
p = sst_curr_prio; /* the original SST priority to return */
if (prio_ceiling > sst_curr_prio) {
sst_curr_prio = prio_ceiling; /* raise the SST priority */
}
SST_INT_UNLOCK();
return p;
}
/*..........................................................................*/
void SST_run(void) {
SST_start(); /* start ISRs */
SST_INT_LOCK();
SST_currPrio_ = (uint8_t)0; /* set the priority for the SST idle loop */
SST_schedule_(); /* process all events produced so far */
SST_INT_UNLOCK();
for (;;) { /* the SST idle loop */
SST_onIdle(); /* invoke the on-idle callback */
}
}
/*..........................................................................*/
void sst_run(void)
{
SST_INT_LOCK();
sst_curr_prio = (sst_uint8_t)0; /* set the priority for the SST idle loop */
//sst_schedule(); /* process all events produced so far */
SST_INT_UNLOCK();
while(1) { /* the SST idle loop */
/* invoke the on-idle callback */
if (sst_onidle != 0)
{
sst_onidle();
}
}
}
/* NOTE: the SST scheduler is entered and exited with interrupts LOCKED */
void SST_schedule_(void) {
static uint8_t const log2Lkup[] = {
0, 1, 2, 2, 3, 3, 3, 3, 4, 4, 4, 4, 4, 4, 4, 4,
5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8,
8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8,
8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8,
8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8,
8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8,
8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8,
8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8,
8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8
};
uint8_t pin = SST_currPrio_; /* save the initial priority */
uint8_t p; /* the new priority */
/* is the new priority higher than the initial? */
while ((p = log2Lkup[SST_readySet_]) > pin) {
TaskCB *tcb = &l_taskCB[p - 1];
/* get the event out of the queue */
SSTEvent e = tcb->queue__[tcb->tail__];
if ((++tcb->tail__) == tcb->end__) {
tcb->tail__ = (uint8_t)0;
}
if ((--tcb->nUsed__) == (uint8_t)0) {/* is the queue becoming empty?*/
SST_readySet_ &= ~tcb->mask__; /* remove from the ready set */
}
SST_currPrio_ = p; /* this becomes the current task priority */
SST_INT_UNLOCK(); /* unlock the interrupts */
(*tcb->task__)(e); /* call the SST task */
SST_INT_LOCK(); /* lock the interrupts for the next pass */
}
SST_currPrio_ = pin; /* restore the initial priority */
}
/*..........................................................................*/
uint8_t SST_post(uint8_t prio, SSTSignal sig, SSTParam par) {
TaskCB *tcb = &l_taskCB[prio - 1];
SST_INT_LOCK();
if (tcb->nUsed__ < tcb->end__) {
tcb->queue__[tcb->head__].sig = sig;/* insert the event at the head */
tcb->queue__[tcb->head__].par = par;
if ((++tcb->head__) == tcb->end__) {
tcb->head__ = (uint8_t)0; /* wrap the head */
}
if ((++tcb->nUsed__) == (uint8_t)1) { /* the first event? */
SST_readySet_ |= tcb->mask__; /* insert task to the ready set */
SST_schedule_(); /* check for synchronous preemption */
}
SST_INT_UNLOCK();
return (uint8_t)1; /* event successfully posted */
}
else {
SST_INT_UNLOCK();
return (uint8_t)0; /* queue full, event posting failed */
}
}
/*..........................................................................*/
sst_bool_t sst_post(sst_uint8_t prio, sst_signal sig, sst_param par)
{
task_cb *tcb = &g_task_cb[prio - 1];
SST_INT_LOCK();
if (tcb->nUsed < tcb->end) {
tcb->queue[tcb->head].sig = sig; /* insert the event at the head */
tcb->queue[tcb->head].par = par;
if ((++tcb->head) == tcb->end) {
tcb->head = (sst_uint8_t)0; /* wrap the head */
}
if ((++tcb->nUsed) == (sst_uint8_t)1) { /* the first event? */
sst_ready_set |= tcb->mask; /* insert task to the ready set */
sst_schedule(); /* check for synchronous preemption */
}
SST_INT_UNLOCK();
return (sst_bool_t)1; /* event successfully posted */
}
else {
SST_INT_UNLOCK();
return (sst_bool_t)0; /* queue full, event posting failed */
}
}
