stepper_controller/arduino/source/qfn.c

   1 /*****************************************************************************
   2 * Product: QF-nano implemenation
   3 * Last Updated for Version: 4.0.02
   4 * Date of the Last Update:  Jul 11, 2008
   5 *
   6 *                    Q u a n t u m     L e a P s
   7 *                    ---------------------------
   8 *                    innovating embedded systems
   9 *
  10 * Copyright (C) 2002-2008 Quantum Leaps, LLC. All rights reserved.
  11 *
  12 * This software may be distributed and modified under the terms of the GNU
  13 * General Public License version 2 (GPL) as published by the Free Software
  14 * Foundation and appearing in the file GPL.TXT included in the packaging of
  15 * this file. Please note that GPL Section 2[b] requires that all works based
  16 * on this software must also be made publicly available under the terms of
  17 * the GPL ("Copyleft").
  18 *
  19 * Alternatively, this software may be distributed and modified under the
  20 * terms of Quantum Leaps commercial licenses, which expressly supersede
  21 * the GPL and are specifically designed for licensees interested in
  22 * retaining the proprietary status of their code.
  23 *
  24 * Contact information:
  25 * Quantum Leaps Web site:  http://www.quantum-leaps.com
  26 * e-mail:                  [email protected]
  27 *****************************************************************************/
  28 #include "qpn_port.h"                                       /* QP-nano port */
  29
  30 Q_DEFINE_THIS_MODULE(qfn)
  31
  32 /**
  33 * \file
  34 * \ingroup qfn
  35 * QF-nano implementation.
  36 */
  37
  38 /* Global-scope objects ----------------------------------------------------*/
  39 uint8_t volatile QF_readySet_;                      /* ready-set of QF-nano */
  40
  41 /* local objects -----------------------------------------------------------*/
  42 static uint8_t const Q_ROM Q_ROM_VAR l_pow2Lkup[] = {
  43     0x00, 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80
  44 };
  45
  46 /*..........................................................................*/
  47 #if (Q_PARAM_SIZE != 0)
  48 void QActive_post(QActive *me, QSignal sig, QParam par) {
  49 #else
  50 void QActive_post(QActive *me, QSignal sig) {
  51 #endif
  52     QF_INT_LOCK();
  53     if (me->nUsed == (uint8_t)0) {
  54         ++me->nUsed;                             /* update number of events */
  55
  56         Q_SIG(me) = sig;                      /* deliver the event directly */
  57 #if (Q_PARAM_SIZE != 0)
  58         Q_PAR(me) = par;
  59 #endif
  60         QF_readySet_ |= Q_ROM_BYTE(l_pow2Lkup[me->prio]);    /* set the bit */
  61
  62 #ifdef QK_PREEMPTIVE
  63         QK_schedule_();                 /* check for synchronous preemption */
  64 #endif
  65     }
  66     else {
  67             /* the queue must be able to accept the event (cannot overflow) */
  68         Q_ASSERT(me->nUsed <= Q_ROM_BYTE(QF_active[me->prio].end));
  69         ++me->nUsed;                             /* update number of events */
  70                                 /* insert event into the ring buffer (FIFO) */
  71         ((QEvent *)Q_ROM_PTR(QF_active[me->prio].queue))[me->head].sig = sig;
  72 #if (Q_PARAM_SIZE != 0)
  73         ((QEvent *)Q_ROM_PTR(QF_active[me->prio].queue))[me->head].par = par;
  74 #endif
  75         if (me->head == (uint8_t)0) {
  76             me->head = Q_ROM_BYTE(QF_active[me->prio].end);/* wrap the head */
  77         }
  78         --me->head;
  79     }
  80     QF_INT_UNLOCK();
  81 }
  82 /*..........................................................................*/
  83 #if (Q_PARAM_SIZE != 0)
  84 void QActive_postISR(QActive *me, QSignal sig, QParam par)
  85 #else
  86 void QActive_postISR(QActive *me, QSignal sig)
  87 #endif
  88 {
  89 #ifdef QF_ISR_NEST
  90 #ifdef QF_ISR_KEY_TYPE
  91     QF_ISR_KEY_TYPE key;
  92     QF_ISR_LOCK(key);
  93 #else
  94     QF_INT_LOCK();
  95 #endif
  96 #endif
  97     if (me->nUsed == (uint8_t)0) {
  98         ++me->nUsed;                             /* update number of events */
  99
 100         Q_SIG(me) = sig;                      /* deliver the event directly */
 101 #if (Q_PARAM_SIZE != 0)
 102         Q_PAR(me) = par;
 103 #endif
 104         QF_readySet_ |= Q_ROM_BYTE(l_pow2Lkup[me->prio]);    /* set the bit */
 105     }
 106     else {
 107             /* the queue must be able to accept the event (cannot overflow) */
 108         Q_ASSERT(me->nUsed <= Q_ROM_BYTE(QF_active[me->prio].end));
 109         ++me->nUsed;                             /* update number of events */
 110                                 /* insert event into the ring buffer (FIFO) */
 111         ((QEvent *)Q_ROM_PTR(QF_active[me->prio].queue))[me->head].sig = sig;
 112 #if (Q_PARAM_SIZE != 0)
 113         ((QEvent *)Q_ROM_PTR(QF_active[me->prio].queue))[me->head].par = par;
 114 #endif
 115         if (me->head == (uint8_t)0) {
 116             me->head = Q_ROM_BYTE(QF_active[me->prio].end);/* wrap the head */
 117         }
 118         --me->head;
 119     }
 120
 121 #ifdef QF_ISR_NEST
 122 #ifdef QF_ISR_KEY_TYPE
 123     QF_ISR_UNLOCK(key);
 124 #else
 125     QF_INT_UNLOCK();
 126 #endif
 127 #endif
 128 }
 129 /*..........................................................................*/
 130 #if (Q_PARAM_SIZE != 0)
 131 void QActive_postLIFO(QActive *me, QSignal sig, QParam par) {
 132 #else
 133 void QActive_postLIFO(QActive *me, QSignal sig) {
 134 #endif
 135     QF_INT_LOCK();
 136     if (me->nUsed == (uint8_t)0) {
 137         ++me->nUsed;                             /* update number of events */
 138
 139         Q_SIG(me) = sig;                      /* deliver the event directly */
 140 #if (Q_PARAM_SIZE != 0)
 141         Q_PAR(me) = par;
 142 #endif
 143         QF_readySet_ |= Q_ROM_BYTE(l_pow2Lkup[me->prio]);    /* set the bit */
 144
 145 #ifdef QK_PREEMPTIVE
 146         QK_schedule_();                 /* check for synchronous preemption */
 147 #endif
 148     }
 149     else {
 150             /* the queue must be able to accept the event (cannot overflow) */
 151         Q_ASSERT(me->nUsed <= Q_ROM_BYTE(QF_active[me->prio].end));
 152         ++me->nUsed;                             /* update number of events */
 153                                 /* insert event into the ring buffer (LIFO) */
 154         ++me->tail;
 155         if (me->tail >= Q_ROM_BYTE(QF_active[me->prio].end))   /* wrap tail */
 156             me->tail = 0;
 157         ((QEvent *)Q_ROM_PTR(QF_active[me->prio].queue))[me->tail].sig = sig;
 158 #if (Q_PARAM_SIZE != 0)
 159         ((QEvent *)Q_ROM_PTR(QF_active[me->prio].queue))[me->tail].par = par;
 160 #endif
 161     }
 162     QF_INT_UNLOCK();
 163 }
 164 /*..........................................................................*/
 165 #if (Q_PARAM_SIZE != 0)
 166 void QActive_postISRLIFO(QActive *me, QSignal sig, QParam par)
 167 #else
 168 void QActive_postISRLIFO(QActive *me, QSignal sig)
 169 #endif
 170 {
 171 #ifdef QF_ISR_NEST
 172 #ifdef QF_ISR_KEY_TYPE
 173     QF_ISR_KEY_TYPE key;
 174     QF_ISR_LOCK(key);
 175 #else
 176     QF_INT_LOCK();
 177 #endif
 178 #endif
 179     if (me->nUsed == (uint8_t)0) {
 180         ++me->nUsed;                             /* update number of events */
 181
 182         Q_SIG(me) = sig;                      /* deliver the event directly */
 183 #if (Q_PARAM_SIZE != 0)
 184         Q_PAR(me) = par;
 185 #endif
 186         QF_readySet_ |= Q_ROM_BYTE(l_pow2Lkup[me->prio]);    /* set the bit */
 187     }
 188     else {
 189             /* the queue must be able to accept the event (cannot overflow) */
 190         Q_ASSERT(me->nUsed <= Q_ROM_BYTE(QF_active[me->prio].end));
 191         ++me->nUsed;                             /* update number of events */
 192                                 /* insert event into the ring buffer (LIFO) */
 193         ++me->tail;
 194         if (me->tail >= Q_ROM_BYTE(QF_active[me->prio].end))   /* wrap tail */
 195             me->tail = 0;
 196         ((QEvent *)Q_ROM_PTR(QF_active[me->prio].queue))[me->tail].sig = sig;
 197 #if (Q_PARAM_SIZE != 0)
 198         ((QEvent *)Q_ROM_PTR(QF_active[me->prio].queue))[me->tail].par = par;
 199 #endif
 200     }
 201
 202 #ifdef QF_ISR_NEST
 203 #ifdef QF_ISR_KEY_TYPE
 204     QF_ISR_UNLOCK(key);
 205 #else
 206     QF_INT_UNLOCK();
 207 #endif
 208 #endif
 209 }
 210 /*--------------------------------------------------------------------------*/
 211 #if (QF_TIMEEVT_CTR_SIZE != 0)
 212
 213 /*..........................................................................*/
 214 void QF_tick(void) {
 215     static uint8_t p;                /* declared static to save stack space */
 216     p = (uint8_t)QF_MAX_ACTIVE;
 217     do {
 218         static QActive *a;           /* declared static to save stack space */
 219         a = (QActive *)Q_ROM_PTR(QF_active[p].act);
 220         if (a->tickCtr != (QTimeEvtCtr)0) {
 221             if ((--a->tickCtr) == (QTimeEvtCtr)0) {
 222 #if (Q_PARAM_SIZE != 0)
 223                 QActive_postISR(a, (QSignal)Q_TIMEOUT_SIG, (QParam)0);
 224 #else
 225                 QActive_postISR(a, (QSignal)Q_TIMEOUT_SIG);
 226 #endif
 227             }
 228         }
 229     } while ((--p) != (uint8_t)0);
 230 }
 231
 232 #if (QF_TIMEEVT_CTR_SIZE > 1)
 233 /*..........................................................................*/
 234 void QActive_arm(QActive *me, QTimeEvtCtr tout) {
 235     QF_INT_LOCK();
 236     me->tickCtr = tout;
 237     QF_INT_UNLOCK();
 238 }
 239 /*..........................................................................*/
 240 void QActive_disarm(QActive *me) {
 241     QF_INT_LOCK();
 242     me->tickCtr = (QTimeEvtCtr)0;
 243     QF_INT_UNLOCK();
 244 }
 245 #endif                                     /* #if (QF_TIMEEVT_CTR_SIZE > 1) */
 246
 247 #endif                                    /* #if (QF_TIMEEVT_CTR_SIZE != 0) */
 248
 249 /*--------------------------------------------------------------------------*/
 250 #ifndef QK_PREEMPTIVE
 251
 252 void QF_run(void) {
 253     static uint8_t const Q_ROM Q_ROM_VAR log2Lkup[] = {
 254         0, 1, 2, 2, 3, 3, 3, 3, 4, 4, 4, 4, 4, 4, 4, 4
 255     };
 256     static uint8_t const Q_ROM Q_ROM_VAR invPow2Lkup[] = {
 257         0xFF, 0xFE, 0xFD, 0xFB, 0xF7, 0xEF, 0xDF, 0xBF, 0x7F
 258     };
 259     static QActive *a;               /* declared static to save stack space */
 260     static uint8_t p;                /* declared static to save stack space */
 261
 262                          /* set priorities all registered active objects... */
 263     for (p = (uint8_t)1; p <= (uint8_t)QF_MAX_ACTIVE; ++p) {
 264         a = (QActive *)Q_ROM_PTR(QF_active[p].act);
 265         Q_ASSERT(a != (QActive *)0);    /* QF_active[p] must be initialized */
 266         a->prio = p;               /* set the priority of the active object */
 267     }
 268          /* trigger initial transitions in all registered active objects... */
 269     for (p = (uint8_t)1; p <= (uint8_t)QF_MAX_ACTIVE; ++p) {
 270         a = (QActive *)Q_ROM_PTR(QF_active[p].act);
 271 #ifndef QF_FSM_ACTIVE
 272         QHsm_init((QHsm *)a);         /* take the initial transition in HSM */
 273 #else
 274         QFsm_init((QFsm *)a);         /* take the initial transition in FSM */
 275 #endif
 276     }
 277
 278     QF_onStartup();                              /* invoke startup callback */
 279
 280     for (;;) {                      /* the event loop of the vanilla kernel */
 281         QF_INT_LOCK();
 282         if (QF_readySet_ != (uint8_t)0) {
 283 #if (QF_MAX_ACTIVE > 4)
 284             if ((QF_readySet_ & 0xF0) != 0U) {        /* upper nibble used? */
 285                 p = (uint8_t)(Q_ROM_BYTE(log2Lkup[QF_readySet_ >> 4]) + 4);
 286             }
 287             else                    /* upper nibble of QF_readySet_ is zero */
 288 #endif
 289             {
 290                 p = Q_ROM_BYTE(log2Lkup[QF_readySet_]);
 291             }
 292             QF_INT_UNLOCK();
 293
 294             a = (QActive *)Q_ROM_PTR(QF_active[p].act);
 295
 296 #ifndef QF_FSM_ACTIVE
 297             QHsm_dispatch((QHsm *)a);                    /* dispatch to HSM */
 298 #else
 299             QFsm_dispatch((QFsm *)a);                    /* dispatch to FSM */
 300 #endif
 301
 302             QF_INT_LOCK();
 303             if ((--a->nUsed) == (uint8_t)0) {      /* queue becoming empty? */
 304                 QF_readySet_ &= Q_ROM_BYTE(invPow2Lkup[p]);/* clear the bit */
 305             }
 306             else {
 307                 Q_SIG(a) = ((QEvent *)Q_ROM_PTR(QF_active[a->prio].queue))
 308                                 [a->tail].sig;
 309 #if (Q_PARAM_SIZE != 0)
 310                 Q_PAR(a) = ((QEvent *)Q_ROM_PTR(QF_active[a->prio].queue))
 311                                 [a->tail].par;
 312 #endif
 313                 if (a->tail == (uint8_t)0) {                /* wrap around? */
 314                     a->tail = Q_ROM_BYTE(QF_active[a->prio].end);
 315                 }
 316                 --a->tail;
 317             }
 318             QF_INT_UNLOCK();
 319         }
 320         else {
 321             QF_onIdle();                                      /* see NOTE01 */
 322         }
 323     }
 324 }
 325
 326 #endif                                             /* #ifndef QK_PREEMPTIVE */
 327
 328 /*****************************************************************************
 329 * NOTE01:
 330 * QF_onIdle() must be called with interrupts locked because the determination
 331 * of the idle condition (no events in the queues) can be changed by any
 332 * interrupt. The QF_onIdle() MUST enable interrups internally, perhaps at the
 333 * same time as putting the CPU into a low-power mode.
 334 */