;
; Copyright (c) 2023 Raspberry Pi (Trading) Ltd.
;
; SPDX-License-Identifier: BSD-3-Clause
;

.program quadrature_encoder

; the code must be loaded at address 0, because it uses computed jumps
.origin 0


; the code works by running a loop that continuously shifts the 2 phase pins into
; ISR and looks at the lower 4 bits to do a computed jump to an instruction that
; does the proper "do nothing" | "increment" | "decrement" action for that pin
; state change (or no change)

; ISR holds the last state of the 2 pins during most of the code. The Y register
; keeps the current encoder count and is incremented / decremented according to
; the steps sampled

; 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
    JMP decrement ; read 01
    JMP increment ; read 10
    JMP update    ; read 11

; 01 state
    JMP increment ; read 00
    JMP update    ; read 01
    JMP update    ; read 10
    JMP decrement ; read 11

; 10 state
    JMP decrement ; read 00
    JMP update    ; read 01
    JMP update    ; read 10
    JMP increment ; read 11

; to reduce code size, the last 2 states are implemented in place and become the
; target for the other jumps

; 11 state
    JMP update    ; read 00
    JMP increment ; read 01
decrement:
    ; note: the target of this instruction must be the next address, so that
    ; the effect of the instruction does not depend on the value of Y. The
    ; same is true for the "JMP X--" below. Basically "JMP Y--, <next addr>"
    ; is just a pure "decrement Y" instruction, with no other side effects
    JMP Y--, update ; read 10

    ; this is where the main loop starts
.wrap_target
update:
    MOV ISR, Y      ; read 11
    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. Both the PUSH
    ; above and the OUT below zero out 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 Y, ~Y
    JMP Y--, increment_cont
increment_cont:
    MOV Y, ~Y
.wrap    ; the .wrap here avoids one jump instruction and saves a cycle too



% c-sdk {

#include "hardware/clocks.h"
#include "hardware/gpio.h"

// 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

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(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
    sm_config_set_in_shift(&c, false, false, 32);
    // don't join FIFO's
    sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE);

    // passing "0" as the sample frequency,
    if (max_step_rate == 0) {
        sm_config_set_clkdiv(&c, 1.0);
    } else {
        // 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, 0, &c);
    pio_sm_set_enabled(pio, sm, true);
}

static inline int32_t quadrature_encoder_get_count(PIO pio, uint 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;
}

%}