stepper_controller/arduino/source/qkn.c

   1 /*****************************************************************************
   2 * Product: QK-nano implemenation
   3 * Last Updated for Version: 4.0.03
   4 * Date of the Last Update:  Dec 26, 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(qkn)
  31
  32 /**
  33 * \file
  34 * \ingroup qkn
  35 * QK-nano implementation.
  36 */
  37
  38 #ifndef QK_PREEMPTIVE
  39     #error "The required header file qkn.h is not included in qpn_port.h"
  40 #endif
  41
  42 /* Global-scope objects ----------------------------------------------------*/
  43                                       /* start with the QK scheduler locked */
  44 uint8_t volatile QK_currPrio_ = (uint8_t)(QF_MAX_ACTIVE + 1);
  45
  46 #ifdef QF_ISR_NEST
  47 uint8_t volatile QK_intNest_;              /* start with nesting level of 0 */
  48 #endif
  49
  50 #ifdef QK_MUTEX
  51 uint8_t volatile QK_ceilingPrio_;            /* ceiling priority of a mutex */
  52 #endif
  53
  54 /* local objects -----------------------------------------------------------*/
  55 static QActive *l_act;                                         /* ptr to AO */
  56
  57 /*..........................................................................*/
  58 void QF_run(void) {
  59     static uint8_t p;                /* declared static to save stack space */
  60
  61                          /* set priorities all registered active objects... */
  62     for (p = (uint8_t)1; p <= (uint8_t)QF_MAX_ACTIVE; ++p) {
  63         l_act = (QActive *)Q_ROM_PTR(QF_active[p].act);
  64         Q_ASSERT(l_act != (QActive *)0);/* QF_active[p] must be initialized */
  65         l_act->prio = p;           /* set the priority of the active object */
  66     }
  67          /* trigger initial transitions in all registered active objects... */
  68     for (p = (uint8_t)1; p <= (uint8_t)QF_MAX_ACTIVE; ++p) {
  69         l_act = (QActive *)Q_ROM_PTR(QF_active[p].act);
  70 #ifndef QF_FSM_ACTIVE
  71         QHsm_init((QHsm *)l_act);                          /* initial tran. */
  72 #else
  73         QFsm_init((QFsm *)l_act);                          /* initial tran. */
  74 #endif
  75     }
  76
  77     QF_INT_LOCK();
  78     QK_currPrio_ = (uint8_t)0;     /* set the priority for the QK idle loop */
  79     QK_SCHEDULE_();                   /* process all events produced so far */
  80     QF_INT_UNLOCK();
  81
  82     QF_onStartup();                             /* invoke startup callback  */
  83
  84     for (;;) {                                    /* enter the QK idle loop */
  85         QK_onIdle();                         /* invoke the on-idle callback */
  86     }
  87 }
  88 /*..........................................................................*/
  89 /* NOTE: the QK scheduler is entered and exited with interrupts LOCKED */
  90 void QK_schedule_(void) Q_REENTRANT {
  91     static uint8_t const Q_ROM Q_ROM_VAR log2Lkup[] = {
  92         0, 1, 2, 2, 3, 3, 3, 3, 4, 4, 4, 4, 4, 4, 4, 4
  93     };
  94     static uint8_t const Q_ROM Q_ROM_VAR invPow2Lkup[] = {
  95         0xFF, 0xFE, 0xFD, 0xFB, 0xF7, 0xEF, 0xDF, 0xBF, 0x7F
  96     };
  97
  98     uint8_t pin;                            /* the initial QK-nano priority */
  99     uint8_t p;           /* new highest-priority active object ready to run */
 100
 101           /* determine the priority of the highest-priority AO ready to run */
 102 #if (QF_MAX_ACTIVE > 4)
 103     if ((QF_readySet_ & 0xF0) != 0) {                 /* upper nibble used? */
 104         p = (uint8_t)(Q_ROM_BYTE(log2Lkup[QF_readySet_ >> 4]) + 4);
 105     }
 106     else                            /* upper nibble of QF_readySet_ is zero */
 107 #endif
 108     {
 109         p = Q_ROM_BYTE(log2Lkup[QF_readySet_]);
 110     }
 111
 112 #ifdef QK_MUTEX                     /* QK priority-ceiling mutexes allowed? */
 113     if ((p > QK_currPrio_) && (p > QK_ceilingPrio_)) {
 114 #else
 115     if (p > QK_currPrio_) {                     /* do we have a preemption? */
 116 #endif                                                          /* QK_MUTEX */
 117
 118         pin = QK_currPrio_;                    /* save the initial priority */
 119         do {
 120             QK_currPrio_ = p;  /* new priority becomes the current priority */
 121             QF_INT_UNLOCK();          /* unlock interrupts to launch a task */
 122
 123                                       /* dispatch to HSM (execute RTC step) */
 124 #ifndef QF_FSM_ACTIVE
 125             QHsm_dispatch((QHsm *)Q_ROM_PTR(QF_active[p].act));
 126 #else
 127             QFsm_dispatch((QFsm *)Q_ROM_PTR(QF_active[p].act));
 128 #endif
 129
 130             QF_INT_LOCK();
 131                /* set cb and a again, in case they change over the RTC step */
 132             l_act = (QActive *)Q_ROM_PTR(QF_active[p].act);
 133
 134             if ((--l_act->nUsed) == (uint8_t)0) {/*is queue becoming empty? */
 135                                                      /* clear the ready bit */
 136                 QF_readySet_ &= Q_ROM_BYTE(invPow2Lkup[p]);
 137             }
 138             else {
 139                 Q_SIG(l_act) = ((QEvent *)Q_ROM_PTR(QF_active[p].queue))
 140                                    [l_act->tail].sig;
 141 #if (Q_PARAM_SIZE != 0)
 142                 Q_PAR(l_act) = ((QEvent *)Q_ROM_PTR(QF_active[p].queue))
 143                                    [l_act->tail].par;
 144 #endif
 145                 if (l_act->tail == (uint8_t)0) {            /* wrap around? */
 146                     l_act->tail = Q_ROM_BYTE(QF_active[p].end);/* wrap tail */
 147                 }
 148                 --l_act->tail;
 149             }
 150                           /* determine the highest-priority AO ready to run */
 151             if (QF_readySet_ != (uint8_t)0) {
 152 #if (QF_MAX_ACTIVE > 4)
 153                 if ((QF_readySet_ & 0xF0) != 0) {     /* upper nibble used? */
 154                     p = (uint8_t)(Q_ROM_BYTE(log2Lkup[QF_readySet_ >> 4])+ 4);
 155                 }
 156                 else                /* upper nibble of QF_readySet_ is zero */
 157 #endif
 158                 {
 159                     p = Q_ROM_BYTE(log2Lkup[QF_readySet_]);
 160                 }
 161             }
 162             else {
 163                 p = (uint8_t)0;                    /* break out of the loop */
 164             }
 165
 166 #ifdef QK_MUTEX                     /* QK priority-ceiling mutexes allowed? */
 167         } while ((p > pin) && (p > QK_ceilingPrio_));
 168 #else
 169         } while (p > pin);
 170 #endif                                                          /* QK_MUTEX */
 171         QK_currPrio_ = pin;                 /* restore the initial priority */
 172     }
 173 }
 174
 175 #ifdef QK_MUTEX
 176 /*..........................................................................*/
 177 QMutex QK_mutexLock(uint8_t prioCeiling) {
 178     uint8_t mutex;
 179     QF_INT_LOCK();
 180     mutex = QK_ceilingPrio_;  /* the original QK priority ceiling to return */
 181     if (QK_ceilingPrio_ < prioCeiling) {
 182         QK_ceilingPrio_ = prioCeiling;     /* raise the QK priority ceiling */
 183     }
 184     QF_INT_UNLOCK();
 185     return mutex;
 186 }
 187 /*..........................................................................*/
 188 void QK_mutexUnlock(QMutex mutex) {
 189     QF_INT_LOCK();
 190     if (QK_ceilingPrio_ > mutex) {
 191         QK_ceilingPrio_ = mutex;      /* restore the saved priority ceiling */
 192         QK_SCHEDULE_();
 193     }
 194     QF_INT_UNLOCK();
 195 }
 196 #endif                                                   /* #ifdef QK_MUTEX */