quadrature encoder: reduce PIO code size from 29 to 24 (#390)

Co-authored-by: Paulo Marques <[email protected]>

Author: pmarques-dev

pio/quadrature_encoder/quadrature_encoder.c

@@ -1,5 +1,5 @@
 /**
- * Copyright (c) 2021 pmarques-dev @ github
+ * Copyright (c) 2021-2023 pmarques-dev @ github
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -44,8 +44,10 @@ int main() {
     PIO pio = pio0;
     const uint sm = 0;
 
-    uint offset = pio_add_program(pio, &quadrature_encoder_program);
-    quadrature_encoder_program_init(pio, sm, offset, PIN_AB, 0);
+    // we don't really need to keep the offset, as this program must be loaded
+    // at offset 0
+    pio_add_program(pio, &quadrature_encoder_program);
+    quadrature_encoder_program_init(pio, sm, PIN_AB, 0);
 
     while (1) {
         // note: thanks to two's complement arithmetic delta will always

pio/quadrature_encoder/quadrature_encoder.pio

@@ -1,13 +1,12 @@
 ;
-; Copyright (c) 2021 pmarques-dev @ github
+; Copyright (c) 2021-2023 pmarques-dev @ github
 ;
 ; SPDX-License-Identifier: BSD-3-Clause
 ;
 
 .program quadrature_encoder
 
-; this code must be loaded into address 0, but at 29 instructions, it probably
-; wouldn't be able to share space with other programs anyway
+; the code must be loaded at address 0, because it uses computed jumps
 .origin 0
 
 
@@ -20,11 +19,11 @@
 ; keeps the current encoder count and is incremented / decremented according to
 ; the steps sampled
 
-; writing any non zero value to the TX FIFO makes the state machine push the
-; current count to RX FIFO between 6 to 18 clocks afterwards. The worst case
-; sampling loop takes 14 cycles, so this program is able to read step rates up
-; to sysclk / 14  (e.g., sysclk 125MHz, max step rate = 8.9 Msteps/sec)
-
+; the program keeps trying to write the current count to the RX FIFO without
+; blocking. To read the current count, the user code must drain the FIFO first
+; and wait for a fresh sample (takes ~4 SM cycles on average). The worst case
+; sampling loop takes 10 cycles, so this program is able to read step rates up
+; to sysclk / 10  (e.g., sysclk 125MHz, max step rate = 12.5 Msteps/sec)
 
 ; 00 state
 	JMP update	; read 00
@@ -60,41 +59,29 @@ decrement:
 	; this is where the main loop starts
 .wrap_target
 update:
-	; we start by checking the TX FIFO to see if the main code is asking for
-	; the current count after the PULL noblock, OSR will have either 0 if
-	; there was nothing or the value that was there
-	SET X, 0
-	PULL noblock
-
-	; since there are not many free registers, and PULL is done into OSR, we
-	; have to do some juggling to avoid losing the state information and
-	; still place the values where we need them
-	MOV X, OSR
-	MOV OSR, ISR
-
-	; the main code did not ask for the count, so just go to "sample_pins"
-	JMP !X, sample_pins
-
-	; if it did ask for the count, then we push it
-	MOV ISR, Y	; we trash ISR, but we already have a copy in OSR
-	PUSH
+	MOV ISR, Y
+	PUSH noblock
 
 sample_pins:
 	; we shift into ISR the last state of the 2 input pins (now in OSR) and
 	; the new state of the 2 pins, thus producing the 4 bit target for the
-	; computed jump into the correct action for this state
-	MOV ISR, NULL
-	IN OSR, 2
+	; computed jump into the correct action for this state. Both the PUSH
+	; above and the OUT below zero the other bits in ISR
+	OUT ISR, 2
 	IN PINS, 2
+
+	; save the state in the OSR, so that we can use ISR for other purposes
+	MOV OSR, ISR
+	; jump to the correct state machine action
 	MOV PC, ISR
 
 	; the PIO does not have a increment instruction, so to do that we do a
 	; negate, decrement, negate sequence
 increment:
-	MOV X, !Y
-	JMP X--, increment_cont
+	MOV Y, ~Y
+	JMP Y--, increment_cont
 increment_cont:
-	MOV Y, !X
+	MOV Y, ~Y
 .wrap	; the .wrap here avoids one jump instruction and saves a cycle too
 
 
@@ -106,16 +93,16 @@ increment_cont:
 
 // max_step_rate is used to lower the clock of the state machine to save power
 // if the application doesn't require a very high sampling rate. Passing zero
-// will set the clock to the maximum, which gives a max step rate of around
-// 8.9 Msteps/sec at 125MHz
+// will set the clock to the maximum
 
-static inline void quadrature_encoder_program_init(PIO pio, uint sm, uint offset, uint pin, int max_step_rate)
+static inline void quadrature_encoder_program_init(PIO pio, uint sm, uint pin, int max_step_rate)
 {
 	pio_sm_set_consecutive_pindirs(pio, sm, pin, 2, false);
 	gpio_pull_up(pin);
 	gpio_pull_up(pin + 1);
 
-	pio_sm_config c = quadrature_encoder_program_get_default_config(offset);
+	pio_sm_config c = quadrature_encoder_program_get_default_config(0);
+
 	sm_config_set_in_pins(&c, pin); // for WAIT, IN
 	sm_config_set_jmp_pin(&c, pin); // for JMP
 	// shift to left, autopull disabled
@@ -127,38 +114,28 @@ static inline void quadrature_encoder_program_init(PIO pio, uint sm, uint offset
 	if (max_step_rate == 0) {
 		sm_config_set_clkdiv(&c, 1.0);
 	} else {
-		// one state machine loop takes at most 14 cycles
-		float div = (float)clock_get_hz(clk_sys) / (14 * max_step_rate);
+		// one state machine loop takes at most 10 cycles
+		float div = (float)clock_get_hz(clk_sys) / (10 * max_step_rate);
 		sm_config_set_clkdiv(&c, div);
 	}
 
-	pio_sm_init(pio, sm, offset, &c);
+	pio_sm_init(pio, sm, 0, &c);
 	pio_sm_set_enabled(pio, sm, true);
 }
 
-
-// When requesting the current count we may have to wait a few cycles (average
-// ~11 sysclk cycles) for the state machine to reply. If we are reading multiple
-// encoders, we may request them all in one go and then fetch them all, thus
-// avoiding doing the wait multiple times. If we are reading just one encoder,
-// we can use the "get_count" function to request and wait
-
-static inline void quadrature_encoder_request_count(PIO pio, uint sm)
-{
-	pio->txf[sm] = 1;
-}
-
-static inline int32_t quadrature_encoder_fetch_count(PIO pio, uint sm)
-{
-	while (pio_sm_is_rx_fifo_empty(pio, sm))
-		tight_loop_contents();
-	return pio->rxf[sm];
-}
-
 static inline int32_t quadrature_encoder_get_count(PIO pio, uint sm)
 {
-	quadrature_encoder_request_count(pio, sm);
-	return quadrature_encoder_fetch_count(pio, sm);
+	uint ret;
+	int n;
+
+	// if the FIFO has N entries, we fetch them to drain the FIFO,
+	// plus one entry which will be guaranteed to not be stale
+	n = pio_sm_get_rx_fifo_level(pio, sm) + 1;
+	while (n > 0) {
+		ret = pio_sm_get_blocking(pio, sm);
+		n--;
+	}
+	return ret;
 }
 
 %}