diff options
Diffstat (limited to 'os/hal/src/hal_qei.c')
| -rw-r--r-- | os/hal/src/hal_qei.c | 163 |
1 files changed, 162 insertions, 1 deletions
diff --git a/os/hal/src/hal_qei.c b/os/hal/src/hal_qei.c index a2b7303..9b084f7 100644 --- a/os/hal/src/hal_qei.c +++ b/os/hal/src/hal_qei.c @@ -42,6 +42,92 @@ /* Driver local functions. */
/*===========================================================================*/
+/**
+ * @brief Helper for correclty handling overflow/underflow
+ *
+ * @details Underflow/overflow will be handled according to mode:
+ * QEI_OVERFLOW_WRAP: counter value will wrap around.
+ * QEI_OVERFLOW_DISCARD: counter will not change
+ * QEI_OVERFLOW_MINMAX: counter will be updated upto min or max.
+ *
+ * @note This function is for use by low level driver.
+ *
+ * @param[in,out] count counter value
+ * @param[in,out] delta adjustment value
+ * @param[in] min minimum allowed value for counter
+ * @param[in] max maximum allowed value for counter
+ * @param[in] mode how to handle overflow
+ *
+ * @return true if counter underflow/overflow occured or
+ * was due to occur
+ *
+ */
+static inline
+bool qei_adjust_count(qeicnt_t *count, qeidelta_t *delta,
+ qeicnt_t min, qeicnt_t max, qeioverflow_t mode) {
+ /* For information on signed integer overflow see:
+ * https://www.securecoding.cert.org/confluence/x/RgE
+ */
+
+ /* 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;
+#if QEI_USE_OVERFLOW_DISCARD == TRUE
+ case QEI_OVERFLOW_DISCARD:
+ *delta = _delta;
+ *count = _count;
+ break;
+#endif
+#if QEI_USE_OVERFLOW_MINMAX == TRUE
+ case QEI_OVERFLOW_MINMAX:
+ *delta = _count - (max - _delta);
+ *count = max;
+ break;
+#endif
+ }
+ 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;
+#if QEI_USE_OVERFLOW_DISCARD == TRUE
+ case QEI_OVERFLOW_DISCARD:
+ *delta = _delta;
+ *count = _count;
+ break;
+#endif
+#if QEI_USE_OVERFLOW_MINMAX == TRUE
+ case QEI_OVERFLOW_MINMAX:
+ *delta = _count - (min - _delta);
+ *count = min;
+ break;
+#endif
+ }
+ return true;
+
+ /* Normal operation
+ */
+ } else {
+ *delta = 0;
+ *count = _count + _delta;
+ return false;
+ }
+}
+
/*===========================================================================*/
/* Driver exported functions. */
/*===========================================================================*/
@@ -168,6 +254,81 @@ qeicnt_t qeiGetCount(QEIDriver *qeip) { }
/**
+ * @brief Set counter value.
+ *
+ * @param[in] qeip pointer to the @p QEIDriver object.
+ * @param[in] value the new 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 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 = qeiAdjustI(qeip, delta);
+ osalSysUnlock();
+
+ 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 qeiAdjustI(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;
+ }
+
+ /* Get counter */
+ qeicnt_t count = qei_lld_get_count(qeip);
+
+ /* Adjust counter value */
+ bool overflowed = qei_adjust_count(&count, &delta,
+ min, max, qeip->config->overflow);
+
+ /* Notify for value change */
+ qei_lld_set_count(qeip, count);
+
+ /* Notify for overflow (passing the remaining delta) */
+ if (overflowed && qeip->config->overflow_cb)
+ qeip->config->overflow_cb(qeip, delta);
+
+ /* Remaining delta */
+ return delta;
+}
+
+
+/**
* @brief Returns the counter delta from last reading.
*
* @param[in] qeip pointer to the @p QEIDriver object
@@ -203,7 +364,7 @@ qeidelta_t qeiUpdateI(QEIDriver *qeip) { "invalid state");
cnt = qei_lld_get_count(qeip);
- delta = cnt - qeip->last;
+ delta = (qeicnt_t)(cnt - qeip->last);
qeip->last = cnt;
return delta;
|