-#define ao_quadrature_update(q) do { \
- ao_quadrature_state[q] = ((ao_quadrature_state[q] & 3) << 2); \
- ao_quadrature_state[q] |= ao_gpio_get(port(q), bita(q), 0); \
- ao_quadrature_state[q] |= ao_gpio_get(port(q), bitb(q), 0) << 1; \
- } while (0)
-
+static inline uint16_t
+ao_quadrature_read(struct stm_gpio *gpio, uint8_t pin_a, uint8_t pin_b) {
+ uint16_t v = stm_gpio_get_all(gpio);
+
+ return ~((((v >> pin_a) & 1) | (((v >> pin_b) & 1) << 1))) & 3;
+}
+
+#define _ao_quadrature_get(q) ao_quadrature_read(port(q), bita(q), bitb(q))
+
+static void
+_ao_quadrature_queue(uint8_t q, int8_t step)
+{
+ ao_quadrature_count[q] += step;
+#if AO_EVENT
+ ao_event_put_isr(AO_EVENT_QUADRATURE, q, step);
+#endif
+ ao_wakeup(&ao_quadrature_count[q]);
+}
+
+
+static void
+_ao_quadrature_set(uint8_t q, uint8_t new) {
+ uint8_t old = ao_quadrature_state[q];
+
+ if (old != new && new == 0) {
+ if (old & 2)
+ _ao_quadrature_queue(q, 1);
+ else if (old & 1)
+ _ao_quadrature_queue(q, -1);
+ }
+ ao_quadrature_state[q] = new;
+}