From b20bf874d044c67bf72e6a054718e75531dc434a Mon Sep 17 00:00:00 2001 From: Stephane D'Alu Date: Wed, 29 Jun 2016 20:14:40 +0200 Subject: created more _lld_ fonctions. --- os/hal/ports/NRF51/NRF51822/hal_qei_lld.c | 216 +++++++++++++++++++----------- os/hal/ports/NRF51/NRF51822/hal_qei_lld.h | 16 ++- 2 files changed, 150 insertions(+), 82 deletions(-) diff --git a/os/hal/ports/NRF51/NRF51822/hal_qei_lld.c b/os/hal/ports/NRF51/NRF51822/hal_qei_lld.c index ffbed95..66694dc 100644 --- a/os/hal/ports/NRF51/NRF51822/hal_qei_lld.c +++ b/os/hal/ports/NRF51/NRF51822/hal_qei_lld.c @@ -31,6 +31,130 @@ /* Driver local definitions. */ /*===========================================================================*/ +static inline +bool qei_adjust_count(qeicnt_t *count, qeidelta_t *delta, + qeicnt_t min, qeicnt_t max, qeioverflow_t mode) { + // See: https://www.securecoding.cert.org/confluence/display/c/INT32-C.+Ensure+that+operations+on+signed+integers+do+not+result+in+overflow + + // Get values + const qeicnt_t _count = *count; + const qeidelta_t _delta = *delta; + + // Overflow operation + if ((_delta > 0) && (_count > (max - _delta))) { + switch(mode) { + case QEI_OVERFLOW_WRAP: + *delta = 0; + *count = (min + (_count - (max - _delta))) - 1; + break; + case QEI_OVERFLOW_DISCARD: + *delta = _delta; + *count = _count; + break; + case QEI_OVERFLOW_MINMAX: + *delta = _count - (max - _delta); + *count = max; + break; + } + return true; + + // Underflow operation + } else if ((_delta < 0) && (_count < (min - _delta))) { + switch(mode) { + case QEI_OVERFLOW_WRAP: + *delta = 0; + *count = (max + (_count - (min - _delta))) + 1; + break; + case QEI_OVERFLOW_DISCARD: + *delta = _delta; + *count = _count; + break; + case QEI_OVERFLOW_MINMAX: + *delta = _count - (min - _delta); + *count = min; + break; + } + return true; + + // Normal operation + } else { + *delta = 0; + *count = _count + _delta; + return false; + } +} + +qeidelta_t qei_lld_adjust_count(QEIDriver *qeip, qeidelta_t delta) { + // Get boundaries + qeicnt_t min = QEI_COUNT_MIN; + qeicnt_t max = QEI_COUNT_MAX; + if (qeip->config->min != qeip->config->max) { + min = qeip->config->min; + max = qeip->config->max; + } + + // Snapshot counter for later comparison + qeicnt_t count = qeip->count; + + // Adjust counter value + bool overflowed = qei_adjust_count(&qeip->count, &delta, + min, max, qeip->config->overflow); + + // Notify for value change + if ((qeip->count != count) && qeip->config->notify_cb) + qeip->config->notify_cb(qeip); + + // Notify for overflow (passing the remaining delta) + if (overflowed && qeip->config->overflow_cb) + qeip->config->overflow_cb(qeip, delta); + + // Remaining delta + return delta; +} + + +/** + * @brief Adjust the counter by delta. + * + * @param[in] qeip pointer to the @p QEIDriver object + * @param[in] delta the adjustement value + * @return The remaining delta (can occur during overflow) + * + * @api + */ +qeidelta_t qeiAdjust(QEIDriver *qeip, qeidelta_t delta) { + osalDbgCheck(qeip != NULL); + osalDbgAssert((qeip->state == QEI_ACTIVE), "invalid state"); + + osalSysLock(); + delta = qei_lld_adjust_count(qeip, delta); + osalSysUnlock(); + + return delta; +} + +/** + * @brief Set counter value + * + * @param[in] qeip pointer to the @p QEIDriver object + * @param[in] value the counter value + * + * @api + */ +void qeiSetCount(QEIDriver *qeip, qeicnt_t value) { + osalDbgCheck(qeip != NULL); + osalDbgAssert((qeip->state == QEI_READY) || (qeip->state == QEI_ACTIVE), + "invalid state"); + + osalSysLock(); + qei_lld_set_count(qeip, value); + osalSysUnlock(); +} + + + + + /** * @brief Common IRQ handler. * @@ -42,88 +166,28 @@ static void serve_interrupt(QEIDriver *qeip) { /* Accumulator overflowed */ if (qdec->EVENTS_ACCOF) { - qdec->EVENTS_ACCOF = 0; + qdec->EVENTS_ACCOF = 0; - qeip->overflowed++; - if (qeip->config->overflowed_cb) - qeip->config->overflowed_cb(qeip); + qeip->overflowed++; + if (qeip->config->overflowed_cb) + qeip->config->overflowed_cb(qeip); } /* Report ready */ if (qdec->EVENTS_REPORTRDY) { - qdec->EVENTS_REPORTRDY = 0; - - // Read and clear counters (due to shortcut) - int16_t acc = ( int16_t)qdec->ACCREAD; - uint16_t accdbl = (uint16_t)qdec->ACCDBLREAD; - - // Inverse direction if requested - if (qeip->config->dirinv) - acc = -acc; // acc is [-1024..+1023], its okay on int16_t - - // Get boundaries - qeicnt_t min = QEI_COUNT_MIN; - qeicnt_t max = QEI_COUNT_MAX; - if (qeip->config->min != qeip->config->max) { - min = qeip->config->min; - max = qeip->config->max; - } - - // Compute new value - qeidelta_t delta = 0; - qeicnt_t count = qeip->count + acc; - bool overflowed = false; - - // See: https://www.securecoding.cert.org/confluence/display/c/INT32-C.+Ensure+that+operations+on+signed+integers+do+not+result+in+overflow - - // Overflow - if ((acc > 0) && (qeip->count > (max - acc))) { - overflowed = true; - switch(qeip->config->overflow) { - case QEI_OVERFLOW_WRAP: - delta = 0; - count = (min + (qeip->count - (max - acc))) - 1; - break; - case QEI_OVERFLOW_DISCARD: - delta = acc; - count = qeip->count; - break; - case QEI_OVERFLOW_MINMAX: - delta = qeip->count - (max - acc); - count = max; - break; - } - - // Underflow - } else if ((acc < 0) && (qeip->count < (min - acc))) { - overflowed = true; - switch(qeip->config->overflow) { - case QEI_OVERFLOW_WRAP: - delta = 0; - count = (max + (qeip->count - (min - acc))) + 1; - break; - case QEI_OVERFLOW_DISCARD: - delta = acc; - count = qeip->count; - break; - case QEI_OVERFLOW_MINMAX: - delta = qeip->count - (min - acc); - count = min; - break; - } - } - - // Set value and notify - if (qeip->count != count) { - qeip->count = count; - if (qeip->config->notify_cb) - qeip->config->notify_cb(qeip); - } - - // Notify for overflow (passing the remaining delta) - if (overflowed && qeip->config->overflow_cb) - qeip->config->overflow_cb(qeip, delta); + qdec->EVENTS_REPORTRDY = 0; + + // Read (and clear counters due to shortcut) + int16_t acc = ( int16_t)qdec->ACCREAD; + uint16_t accdbl = (uint16_t)qdec->ACCDBLREAD; + + // Inverse direction if requested + if (qeip->config->dirinv) + acc = -acc; // acc is [-1024..+1023], its okay on int16_t + + // Adjust counter + qei_lld_adjust_count(qeip, acc); } } diff --git a/os/hal/ports/NRF51/NRF51822/hal_qei_lld.h b/os/hal/ports/NRF51/NRF51822/hal_qei_lld.h index e1485e3..813d169 100644 --- a/os/hal/ports/NRF51/NRF51822/hal_qei_lld.h +++ b/os/hal/ports/NRF51/NRF51822/hal_qei_lld.h @@ -31,7 +31,6 @@ #if (HAL_USE_QEI == TRUE) || defined(__DOXYGEN__) - /*===========================================================================*/ /* Driver constants. */ /*===========================================================================*/ @@ -71,7 +70,7 @@ #endif /** - * @brief QEID interrupt priority level setting. + * @brief QEID interrupt priority level setting for QDEC0. */ #if !defined(NRF51_QEI_QDEC0_IRQ_PRIORITY) || defined(__DOXYGEN__) #define NRF51_QEI_QDEC0_IRQ_PRIORITY 2 @@ -296,12 +295,12 @@ struct QEIDriver { /** * @brief Counter */ - qeicnt_t count; + qeicnt_t count; /** * @brief Number of time the MCU discarded updates due to * accumulator overflow */ - uint32_t overflowed; + uint32_t overflowed; /** * @brief Pointer to the QDECx registers block. */ @@ -332,8 +331,10 @@ struct QEIDriver { * @notapi */ #define qei_lld_set_count(qeip, value) \ - do { \ - (qeip)->count = value; \ + if ((qeip)->count != ((qeicnt_t)value)) { \ + (qeip)->count = value; \ + if ((qeip)->config->notify_cb) \ + (qeip)->config->notify_cb(qeip); \ } while(0) @@ -357,6 +358,9 @@ extern "C" { } #endif +void qeiSetCount(QEIDriver *qeip, qeicnt_t value); +qeidelta_t qeiAdjust(QEIDriver *qeip, qeidelta_t delta); + #endif /* HAL_USE_QEI */ #endif /* HAL_QEI_LLD_H */ -- cgit v1.2.3