/* NOTE: the QK scheduler is entered and exited with interrupts DISABLED */
void QK_sched_(uint8_t p) {
uint8_t pin = QK_currPrio_; /* save the initial priority */
QActive *a;
#ifdef QK_TLS /* thread-local storage used? */
uint8_t pprev = pin;
#endif
do {
QEvt const *e;
a = QF_active_[p]; /* obtain the pointer to the AO */
QK_currPrio_ = p; /* this becomes the current task priority */
#ifdef QK_TLS /* thread-local storage used? */
if (p != pprev) { /* are we changing threads? */
QK_TLS(a); /* switch new thread-local storage */
pprev = p;
}
#endif
QS_BEGIN_NOCRIT_(QS_QK_SCHEDULE, QS_priv_.aoObjFilter, a)
QS_TIME_(); /* timestamp */
QS_U8_(p); /* the priority of the AO */
QS_U8_(pin); /* the preempted priority */
QS_END_NOCRIT_()
QF_INT_ENABLE(); /* unconditionally enable interrupts */
e = QActive_get_(a); /* get the next event for this AO */
QMSM_DISPATCH(&a->super, e); /* dispatch to the AO */
QF_gc(e); /* garbage collect the event, if necessary */
QF_INT_DISABLE(); /* disable interrupts */
#if (QF_MAX_ACTIVE <= 8) /* new highest-prio AO ready to run */
QPSet8_findMax(&QK_readySet_, p);
#else
QPSet64_findMax(&QK_readySet_, p);
#endif
if (p <= pin) { /* below the current preemption threshold? */
p = (uint8_t)0;
}
#ifndef QK_NO_MUTEX
else if (p <= QK_ceilingPrio_) { /* below the mutex ceiling? */
p = (uint8_t)0;
}
else {
/* empty */
}
#endif
} while (p != (uint8_t)0);
QK_currPrio_ = pin; /* restore the initial priority */
#ifdef QK_TLS /* thread-local storage used? */
if (pin != (uint8_t)0) { /* no extended context for the idle loop */
a = QF_active_[pin]; /* the pointer to the preempted AO */
QK_TLS(a); /* restore the original TLS */
}
#endif
}
void QF_publish_(QEvt const * const e, void const * const sender)
#endif
{
QF_CRIT_STAT_
/* make sure that the published signal is within the configured range */
Q_REQUIRE(e->sig < (QSignal)QF_maxSignal_);
QF_CRIT_ENTRY_();
QS_BEGIN_NOCRIT_(QS_QF_PUBLISH, (void *)0, (void *)0)
QS_TIME_(); /* the timestamp */
QS_OBJ_(sender); /* the sender object */
QS_SIG_(e->sig); /* the signal of the event */
QS_2U8_(e->poolId_, e->refCtr_);/* pool Id & ref Count of the event */
QS_END_NOCRIT_()
if (e->poolId_ != (uint8_t)0) { /* is it a dynamic event? */
QF_EVT_REF_CTR_INC_(e); /* increment reference counter, NOTE01 */
}
QF_CRIT_EXIT_();
#if (QF_MAX_ACTIVE <= 8)
{
uint8_t tmp = QF_subscrList_[e->sig].bits[0];
while (tmp != (uint8_t)0) {
uint8_t p = QF_LOG2(tmp);
tmp &= Q_ROM_BYTE(QF_invPwr2Lkup[p]); /* clear subscriber bit */
Q_ASSERT(QF_active_[p] != (QActive *)0); /* must be registered */
/* QACTIVE_POST() asserts internally if the queue overflows */
QACTIVE_POST(QF_active_[p], e, sender);
}
}
#else
{
uint_t i = (uint_t)Q_DIM(QF_subscrList_[0].bits);
do { /* go through all bytes in the subscription list */
uint8_t tmp;
--i;
tmp = QF_PTR_AT_(QF_subscrList_, e->sig).bits[i];
while (tmp != (uint8_t)0) {
uint8_t p = QF_LOG2(tmp);
tmp &= Q_ROM_BYTE(QF_invPwr2Lkup[p]);/*clear subscriber bit */
p = (uint8_t)(p + (uint8_t)(i << 3));/* adjust the priority */
Q_ASSERT(QF_active_[p] != (QActive *)0);/*must be registered*/
/* QACTIVE_POST() asserts internally if the queue overflows */
QACTIVE_POST(QF_active_[p], e, sender);
}
} while (i != (uint_t)0);
}
#endif
QF_gc(e); /* run the garbage collector, see NOTE01 */
}
/*..........................................................................*/
static void *thread_routine(void *arg) { /* the expected POSIX signature */
((QActive *)arg)->running = (uint8_t)1; /* allow the thread loop to run */
while (((QActive *)arg)->running) { /* QActive_stop() stopps the loop */
QEvent const *e = QActive_get_((QActive *)arg);/*wait for the event */
QF_ACTIVE_DISPATCH_(&((QActive *)arg)->super, e);/* dispatch to SM */
QF_gc(e); /* check if the event is garbage, and collect it if so */
}
QF_remove_((QActive *)arg);/* remove this object from any subscriptions */
return (void *)0; /* return success */
}
/*..........................................................................*/
static DWORD WINAPI thread_function(LPVOID arg) { /* for CreateThread() */
((QActive *)arg)->running = (uint8_t)1; /* allow the thread loop to run */
while (((QActive *)arg)->running) { /* QActive_stop() stops the loop */
QEvent const *e = QActive_get_((QActive *)arg); /* wait for event */
QF_ACTIVE_DISPATCH_((QHsm *)arg, e); /* dispatch to the AO's SM */
QF_gc(e); /* check if the event is garbage, and collect it if so */
}
QF_remove_((QActive *)arg);/* remove this object from any subscriptions */
return 0; /* return success */
}
/*..........................................................................*/
static DWORD WINAPI thread_function(LPVOID arg) { /* for CreateThread() */
do { /* QActive_stop() stops the loop */
QEvt const *e = QActive_get_((QActive *)arg); /* wait for event */
QMSM_DISPATCH((QMsm *)arg, e); /* dispatch to the AO's SM */
QF_gc(e); /* check if the event is garbage, and collect it if so */
} while (((QActive *)arg)->thread != (HANDLE)0);
QF_remove_((QActive *)arg);/* remove this object from any subscriptions */
CloseHandle(((QActive *)arg)->osObject); /* cleanup the OS event */
return 0; /* return success */
}
/*..........................................................................*/
static void task_function(void *pdata) { /* uC/OS-II task signature */
((QActive *)pdata)->running = (uint8_t)1; /* enable the thread-loop */
while (((QActive *)pdata)->running) {
QEvent const *e = QActive_get_((QActive *)pdata);
QF_ACTIVE_DISPATCH_(&((QActive *)pdata)->super, e);
QF_gc(e); /* check if the event is garbage, and collect it if so */
}
QF_remove_((QActive *)pdata); /* remove this object from the framework */
OSTaskDel(OS_PRIO_SELF); /* make uC/OS-II forget about this task */
}
/* thread for active objects -----------------------------------------------*/
static void ao_thread(void const *argument); /* prototype */
static void ao_thread(void const *argument) { /* RTX signature */
QActive *act = (QActive *)argument;
act->osObject = (uint32_t)1; /* enable thread-loop, see NOTE2 */
while (act->osObject != (uint32_t)0) {
QEvt const *e = QActive_get_(act);
QMSM_DISPATCH(&act->super, e);
QF_gc(e); /* check if the event is garbage, and collect it if so */
}
QF_remove_(act); /* remove this object from QF */
osThreadTerminate(act->thread); /* CMSIS-RTX: remove the thread */
}
/*..........................................................................*/
static void *thread_routine(void *arg) { /* the expected POSIX signature */
QActive *act = (QActive *)arg;
/* loop until m_thread is cleared in QActive_stop() */
do {
QEvt const *e = QActive_get_(act); /* wait for the event */
QMSM_DISPATCH(&act->super, e); /* dispatch to the SM */
QF_gc(e); /* check if the event is garbage, and collect it if so */
} while (act->thread != (uint8_t)0);
QF_remove_(act); /* remove this object from any subscriptions */
pthread_cond_destroy(&act->osObject); /* cleanup the condition variable */
return (void *)0; /* return success */
}
/*..........................................................................*/
void QF_run(void) {
uint8_t p;
QActive *a;
QEvent const *e;
QF_INT_LOCK_KEY_
QF_onStartup(); /* startup callback */
for (;;) { /* the background loop */
QF_INT_LOCK_();
#if (QF_MAX_ACTIVE <= 8)
if (QPSet8_notEmpty(&QF_readySet_)) {
QPSet8_findMax(&QF_readySet_, p);
#else
if (QPSet64_notEmpty(&QF_readySet_)) {
QPSet64_findMax(&QF_readySet_, p);
#endif
a = QF_active_[p];
QF_INT_UNLOCK_();
e = QActive_get_(a); /* get the next event for this AO */
QF_ACTIVE_DISPATCH_(&a->super, e); /* dispatch to the AO */
QF_gc(e); /* determine if event is garbage and collect it if so */
}
else {
#ifndef QF_INT_KEY_TYPE
QF_onIdle(); /* see NOTE01 */
#else
QF_onIdle(intLockKey_); /* see NOTE01 */
#endif /* QF_INT_KEY_TYPE */
}
}
}
/*..........................................................................*/
void QActive_start(QActive *me, uint8_t prio,
QEvent const *qSto[], uint32_t qLen,
void *stkSto, uint32_t stkSize,
QEvent const *ie)
{
Q_REQUIRE(((uint8_t)0 < prio) && (prio <= (uint8_t)QF_MAX_ACTIVE)
&& (stkSto == (void *)0)); /* does not need per-actor stack */
(void)stkSize; /* avoid the "unused parameter" compiler warning */
QEQueue_init(&me->eQueue, qSto, (QEQueueCtr)qLen);/* initialize QEQueue */
me->prio = prio; /* set the QF priority of this active object */
QF_add_(me); /* make QF aware of this active object */
QF_ACTIVE_INIT_(&me->super, ie); /* execute initial transition */
QS_FLUSH(); /* flush the trace buffer to the host */
}
static DWORD WINAPI ao_thread(LPVOID arg) { /* for CreateThread() */
QActive *act = (QActive *)arg;
/* Active Object's event loop. QActive_stop() stops the loop */
do {
QEvt const *e = QActive_get_(act); /* wait for event */
QMSM_DISPATCH(&act->super, e); /* dispatch to the AO's SM */
QF_gc(e); /* check if the event is garbage, and collect it if so */
} while (act->thread != (HANDLE)0);
QActive_unsubscribeAll(act); /* make sure that no events are subscribed */
QF_remove_(act); /* remove this object from any subscriptions */
CloseHandle(act->osObject); /* cleanup the OS event */
free((void *)act->eQueue.ring); /* free the fudged queue storage */
return (DWORD)0; /* return success */
}
/*..........................................................................*/
static void thread_function(void *pVoid) { /* embOS signature */
QActive *act = (QActive *)pVoid;
#ifdef __TARGET_FPU_VFP
/* does the task use the FPU? see NOTE1 */
if ((act->osObject & QF_TASK_USES_FPU) != (uint32_t)0) {
OS_ExtendTaskContext_VFP();
}
#endif /* __TARGET_FPU_VFP */
act->osObject = (uint32_t)1; /* enable the event-loop, see NOTE2 */
while (act->osObject != (uint32_t)0) { /* event-loop */
QEvt const *e = QActive_get_(act);
QMSM_DISPATCH(&act->super, e);
QF_gc(e); /* check if the event is garbage, and collect it if so */
}
QF_remove_(act); /* remove this object from QF */
OS_DeleteMB(&act->eQueue);
OS_TerminateTask(&act->thread);
}
/**
* \description
* The "extended" QK scheduler performs all the steps of the regular scheduler
* QK_sched_() and additionally switches the Thread-Local Storage (TLS) and
* handles the extended context-switch.
*
* \arguments
* \arg[in] \c p priority of the next AO to schedule
*
* \note The "extended" QK scheduler needs to be called only to handle
* "asynchronous" preemption, under the assumption that neither the ISRs
* nor the QK idle loop use TLS or the co-processors requiring
* extended context switch (see [PSiCC2] Section 10.4.3).
*
* \note QK_schedExt_() must be always called with interrupts DISABLED.
*
* \note The extended scheduler might enable interrupts internally,
* but always returns with interrupts DISABLED.
*/
void QK_schedExt_(uint_fast8_t p) {
uint_fast8_t pin = QK_currPrio_; /* save the initial priority */
/* thread-local storage used? */
#ifdef QK_TLS
uint_fast8_t pprev = pin;
#endif
QActive *a;
/* extended context-switch used? */
#ifdef QK_EXT_SAVE
/* aren't we preempting the idle loop? (idle loop has prio==0) */
if (pin != (uint_fast8_t)0) {
a = QF_active_[pin]; /* the pointer to the preempted AO */
QK_EXT_SAVE(a); /* save the extended context */
}
#endif
/* loop until have ready-to-run AOs of higher priority than the initial */
do {
QEvt const *e;
a = QF_active_[p]; /* obtain the pointer to the AO */
QK_currPrio_ = p; /* this becomes the current task priority */
/* thread-local storage used? */
#ifdef QK_TLS
/* are we changing threads? */
if (p != pprev) {
QK_TLS(a); /* switch new thread-local storage */
pprev = p;
}
#endif
QS_BEGIN_NOCRIT_(QS_QK_SCHEDULE, QS_priv_.aoObjFilter, a)
QS_TIME_(); /* timestamp */
QS_U8_((uint8_t)p); /* the priority of the AO */
QS_U8_((uint8_t)pin); /* the preempted priority */
QS_END_NOCRIT_()
QF_INT_ENABLE(); /* unconditionally enable interrupts */
/* perform the run-to-completion (RTS) step...
* 1. retrieve the event from the AO's event queue, which by this
* time must be non-empty and QActive_get_() asserts it.
* 2. dispatch the event to the AO's state machine.
* 3. determine if event is garbage and collect it if so
*/
e = QActive_get_(a);
QMSM_DISPATCH(&a->super, e);
QF_gc(e);
QF_INT_DISABLE(); /* disable interrupts */
/* find new highest-priority AO ready to run... */
#if (QF_MAX_ACTIVE <= 8)
QPSet8_findMax(&QK_readySet_, p);
#else
QPSet64_findMax(&QK_readySet_, p);
#endif
/* is the new priority below the current preemption threshold? */
if (p <= pin) {
p = (uint_fast8_t)0;
}
#ifndef QK_NO_MUTEX
/* is the new priority below the mutex ceiling? */
else if (p <= QK_ceilingPrio_) {
p = (uint_fast8_t)0;
}
else {
/* empty */
}
#endif
} while (p != (uint_fast8_t)0);
QK_currPrio_ = pin; /* restore the initial priority */
#if defined(QK_TLS) || defined(QK_EXT_RESTORE)
/* aren't we preempting the idle loop? (idle loop has prio==0) */
if (pin != (uint_fast8_t)0) {
a = QF_active_[pin]; /* the pointer to the preempted AO */
/* thread-local storage used? */
#ifdef QK_TLS
QK_TLS(a); /* restore the original TLS */
#endif
/* extended context-switch used? */
#ifdef QK_EXT_RESTORE
QK_EXT_RESTORE(a); /* restore the extended context */
#endif
}
#endif
}
void QK_schedule_(void) {
#else
void QK_schedule_(QF_INT_KEY_TYPE intLockKey_) {
#endif
uint8_t p;
/* the QK scheduler must be called at task level only */
Q_REQUIRE(QK_intNest_ == (uint8_t)0);
#if (QF_MAX_ACTIVE <= 8)
if (QPSet8_notEmpty(&QK_readySet_)) {
/* determine the priority of the highest-priority task ready to run */
QPSet8_findMax(&QK_readySet_, p);
#else
if (QPSet64_notEmpty(&QK_readySet_)) {
/* determine the priority of the highest-priority task ready to run */
QPSet64_findMax(&QK_readySet_, p);
#endif
#ifdef QK_NO_MUTEX
if (p > QK_currPrio_) { /* do we have a preemption? */
#else /* QK priority-ceiling mutexes allowed */
if ((p > QK_currPrio_) && (p > QK_ceilingPrio_)) {
#endif
uint8_t pin = QK_currPrio_; /* save the initial priority */
QActive *a;
#ifdef QK_TLS /* thread-local storage used? */
uint8_t pprev = pin;
#endif
do {
QEvent const *e;
a = QF_active_[p]; /* obtain the pointer to the AO */
QK_currPrio_ = p; /* this becomes the current task priority */
#ifdef QK_TLS /* thread-local storage used? */
if (p != pprev) { /* are we changing threads? */
QK_TLS(a); /* switch new thread-local storage */
pprev = p;
}
#endif
QS_BEGIN_NOLOCK_(QS_QK_SCHEDULE, QS_aoObj_, a)
QS_TIME_(); /* timestamp */
QS_U8_(p); /* the priority of the AO */
QS_U8_(pin); /* the preempted priority */
QS_END_NOLOCK_()
QK_INT_UNLOCK_(); /* unlock the interrupts */
e = QActive_get_(a); /* get the next event for this AO */
QF_ACTIVE_DISPATCH_(&a->super, e); /* dispatch to the AO */
QF_gc(e); /* garbage collect the event, if necessary */
QK_INT_LOCK_();
/* determine the highest-priority AO ready to run */
#if (QF_MAX_ACTIVE <= 8)
if (QPSet8_notEmpty(&QK_readySet_)) {
QPSet8_findMax(&QK_readySet_, p);
#else
if (QPSet64_notEmpty(&QK_readySet_)) {
QPSet64_findMax(&QK_readySet_, p);
#endif
}
else {
p = (uint8_t)0;
}
#ifdef QK_NO_MUTEX
} while (p > pin); /* is the new priority higher than initial? */
#else /* QK priority-ceiling mutexes allowed */
} while ((p > pin) && (p > QK_ceilingPrio_));
#endif
QK_currPrio_ = pin; /* restore the initial priority */
#ifdef QK_TLS /* thread-local storage used? */
if (pin != (uint8_t)0) {/*no extended context for the idle loop */
a = QF_active_[pin]; /* the pointer to the preempted AO */
QK_TLS(a); /* restore the original TLS */
}
#endif
}
}
}
void CPLR_Task( void* pvParameters )
{
(void) pvParameters;
QEvt const *evt; /* This pointer to an event always lives but should be
garbage collected after finishing to process it so the
memory in the pool to which it points can be reused. If
this thread needs to wait on another event while still
processing this (main) one, a different local pointer
should be used and garbage collected after. */
DC3Error_t status = ERR_NONE; /* Keep track of failures of various func
calls and commands. If this is ever set
to something other than ERR_NONE, it will
be printed out at the end of the for loop*/
for (;;) { /* Beginning of the thread forever loop */
/* Check if there's data in the queue and process it if there. */
evt = QEQueue_get(&CPLR_evtQueue);
if ( evt != (QEvt *)0 ) { /* Check whether an event is present in queue */
switch( evt->sig ) { /* Identify the event by its signal enum */
case CPLR_ETH_SYS_TEST_SIG:
DBG_printf(
"Received CPLR_ETH_SYS_TEST_SIG (%d) signal with event EthEvt of len: %d\n",
evt->sig,
((EthEvt const *)evt)->msg_len
);
#if 0
/* Going to use this signal to test some stuff in this thread */
/* Do a read from the EEPROM on the I2C Bus */
status = I2C_readDevMemEVT(
_DC3_EEPROM, // DC3I2CDevice_t iDev,
0x00, // uint16_t offset,
17, // uint16_t bytesToRead,
_DC3_ACCESS_FRT, // AccessType_t accType,
NULL // QActive* callingAO
);
if ( ERR_NONE != status ) {
ERR_printf("Error calling I2C_readDevMemEVT()\n");
goto CPLR_Task_ERR_HANDLE; /* Stop and jump to error handling */
}
/* If we got here, there were no errors during the call to
* I2C_readDevMemEVT() so we can expect an event from that driver
*/
uint32_t timeout = 1000;
QEvt const *evtI2CDone = 0;
/* We only expect an I2C read done signal and no others */
do {
evtI2CDone = QEQueue_get(&CPLR_evtQueue);
if (evtI2CDone != (QEvt *)0 ) {
break;
} else {
vTaskDelay(1);
}
} while ( --timeout != 0 );
if ( 0 == timeout ) {
ERR_printf("Timed out waiting for an event\n");
} else {
switch( evtI2CDone->sig ) {
case I2C1_DEV_READ_DONE_SIG:
DBG_printf("Got I2C1_DEV_READ_DONE_SIG\n");
break;
default:
WRN_printf("Unknown signal %d\n", evtI2CDone->sig);
break;
}
QF_gc(evt);
}
#endif
uint8_t buffer[20];
uint16_t bytesRead = 0;
DBG_printf("Issuing I2C_readDevMemFRT()\n");
status = I2C_readDevMemFRT(
_DC3_EEPROM, // const DC3I2CDevice_t iDev,
0x00, // const uint16_t offset,
17, // const uint16_t nBytesToRead
sizeof(buffer), // const uint16_t nBufferSize,
buffer, // uint8_t const *pBuffer,
&bytesRead // uint16_t *pBytesRead,
);
char tmp[120];
uint16_t tmpLen = 0;
status = CON_hexToStr(
(const uint8_t *)buffer, // data to convert
bytesRead, // length of data to convert
tmp, // where to write output
sizeof(tmp), // max size of output buffer
&tmpLen, // size of the resulting output
0, // no columns
' ', // separator
true // bPrintX
);
DBG_printf("I2C_readDevMemFRT() returned having read %d bytes: %s\n", bytesRead, tmp);
break;
default:
WRN_printf("Received an unknown signal: %d. Ignoring...\n", evt->sig);
break;
}
/* If any data from the event needs to be used after garbage
* collection, it should be locally stored if. */
CPLR_Task_ERR_HANDLE: /* Handle any error that may have occurred. */
/* Print error if exists */
ERR_COND_OUTPUT(
status,
_DC3_ACCESS_FRT,
"Error 0x%08x occurred in FreeRTOS thread!!!\n",
status
);
QF_gc(evt); /* !!! Don't forget to garbage collect the event after
processing the event. After this, any data to which
this pointer points to may not be valid and should not
be referenced. */
}
// vTaskSuspend(NULL);
vTaskDelay(1); /* Always task-delay so this thread can go to sleep while
other threads/AOs can process their data */
} /* End of the thread forever loop */
}
