diff --git a/.gitignore b/.gitignore
index 5ce8d86..c6fe8ab 100644
--- a/.gitignore
+++ b/.gitignore
@@ -48,3 +48,5 @@ software/.vscode/.cortex-debug.registers.state.json
story-player/build/
story-editor/tools/build-audio-System_Qt5_15_8-Debug/
+
+software/.vscode/.cortex-debug.peripherals.state.json
diff --git a/software/.gdbinit b/software/.gdbinit
new file mode 100644
index 0000000..5b1312a
--- /dev/null
+++ b/software/.gdbinit
@@ -0,0 +1,29 @@
+define armex
+ printf "EXEC_RETURN (LR):\n",
+ info registers $lr
+ if (0x04 == (0x04 & (int)$lr))
+ printf "Uses MSP 0x%x return.\n", $msp
+ set $armex_base = (int)$msp
+ else
+ printf "Uses PSP 0x%x return.\n", $psp
+ set $armex_base = (int)$psp
+ end
+
+ printf "xPSR 0x%x\n", *($armex_base+28)
+ printf "ReturnAddress 0x%x\n", *($armex_base+24)
+ printf "LR (R14) 0x%x\n", *($armex_base+20)
+ printf "R12 0x%x\n", *($armex_base+16)
+ printf "R3 0x%x\n", *($armex_base+12)
+ printf "R2 0x%x\n", *($armex_base+8)
+ printf "R1 0x%x\n", *($armex_base+4)
+ printf "R0 0x%x\n", *($armex_base)
+ printf "Return instruction:\n"
+ x/i *($armex_base+24)
+ printf "LR instruction:\n"
+ x/i *($armex_base+20)
+end
+
+document armex
+ARMv7 Exception entry behavior.
+xPSR, ReturnAddress, LR (R14), R12, R3, R2, R1, and R0
+end
\ No newline at end of file
diff --git a/software/.vscode/.cortex-debug.peripherals.state.json b/software/.vscode/.cortex-debug.peripherals.state.json
deleted file mode 100644
index 7aed850..0000000
--- a/software/.vscode/.cortex-debug.peripherals.state.json
+++ /dev/null
@@ -1 +0,0 @@
-[{"node":"TIMER","expanded":true,"format":0,"pinned":false},{"node":"TIMER.INTR","expanded":true,"format":0},{"node":"TIMER.INTE","expanded":true,"format":0}]
\ No newline at end of file
diff --git a/software/CMakeLists.txt b/software/CMakeLists.txt
index aa26966..5c75f7c 100644
--- a/software/CMakeLists.txt
+++ b/software/CMakeLists.txt
@@ -39,7 +39,7 @@ set(OST_SRCS
system/qor_armv6m.s
system/audio_player.c
system/vm_task.c
- system/hmi_task.c
+ system/usb_task.c
system/fs_task.c
system/ff/ff.c
system/ff/ffsystem.c
diff --git a/software/platform/raspberry-pico-w/CMakeLists.txt b/software/platform/raspberry-pico-w/CMakeLists.txt
index 55deb4d..816ccb9 100644
--- a/software/platform/raspberry-pico-w/CMakeLists.txt
+++ b/software/platform/raspberry-pico-w/CMakeLists.txt
@@ -31,7 +31,7 @@ add_library(
)
pico_generate_pio_header(${PROJECT_NAME} ${CMAKE_CURRENT_LIST_DIR}/pico_i2s.pio)
-pico_generate_pio_header(${PROJECT_NAME} ${CMAKE_CURRENT_LIST_DIR}/i2s.pio)
+pico_generate_pio_header(${PROJECT_NAME} ${CMAKE_CURRENT_LIST_DIR}/pio_rotary_encoder.pio)
target_link_libraries(${PROJECT_NAME} INTERFACE pico_stdlib hardware_exception cmsis_core tinyusb_device tinyusb_board)
target_include_directories(${PROJECT_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR})
diff --git a/software/platform/raspberry-pico-w/i2s.pio b/software/platform/raspberry-pico-w/i2s.pio
deleted file mode 100644
index 05d914c..0000000
--- a/software/platform/raspberry-pico-w/i2s.pio
+++ /dev/null
@@ -1,253 +0,0 @@
-; i2s.pio
-;
-; Author: Daniel Collins
-; Date: 2022-02-25
-;
-; Copyright (c) 2022 Daniel Collins
-;
-; This file is part of rp2040_i2s_example.
-;
-; rp2040_i2s_example is free software: you can redistribute it and/or modify it under
-; the terms of the GNU General Public License, version 3 as published by the
-; Free Software Foundation.
-;
-; rp2040_i2s_example is distributed in the hope that it will be useful, but WITHOUT ANY
-; WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
-; A PARTICULAR PURPOSE. See the GNU General Public License for more details.
-;
-; You should have received a copy of the GNU General Public License along with
-; rp2040_i2s_example. If not, see .
-
-; An I2S bi-directional peripheral with master clock (SCK) output.
-
-.program i2s_sck
-; Transmit an I2S system / master clock.
-;
-; Usually this needs to be 256 * fs (the word clock rate).
-; (e.g. for 48kHz audio, this should be precisely 12.288000 MHz.)
-; Since all of these transmit at 1 I2S-clock per 2 sys-clock cycles,
-; you need to set the clock divider at 2x the desired rate
-; (for the 48kHz example, this should be 2 * 12,288,000 = 24.5760000 MHz).
-;
-; Use this as one state machine of the I2S PIO unit as a whole for perfect
-; synchronization, e.g.:
-; state machine 0 = clock (divide to 48000 * 256 * 2 = 24.576000 MHz for 48kHz stereo audio)
-; state machine 1 = out (divide to 48000 * 64 * 2 = 6.144000 MHz for 48kHz stereo audio)
-; state machine 2 = in (divide to 48000 * 256 * 2 = 24.576000 MHz for 48kHz stereo audio)
-;
-; One output pin is used for the clock output.
-
-.wrap_target
- set pins 1
- set pins 0
-.wrap
-
-.program i2s_out_master
-; I2S audio output block. Synchronous with clock and input.
-; Must run at BCK * 2.
-;
-; This block also outputs the word clock (also called frame or LR clock) and
-; the bit clock.
-;
-; Set register x to (bit depth - 2) (e.g. for 24 bit audio, set to 22).
-; Note that if this is needed to be synchronous with the SCK module,
-; it is not possible to run 24-bit frames with an SCK of 256x fs. You must either
-; run SCK at 384x fs (if your codec permits this) or use 32-bit frames, which
-; work fine with 24-bit codecs.
-
-.side_set 2
-
-public entry_point:
- ; /--- LRCLK
- ; |/-- BCLK
-frameL: ; ||
- set x, 30 side 0b00 ; start of Left frame
- pull noblock side 0b01 ; One clock after edge change with no data
-dataL:
- out pins, 1 side 0b00
- jmp x-- dataL side 0b01
-
-frameR:
- set x, 30 side 0b10
- pull noblock side 0b11 ; One clock after edge change with no data
-dataR:
- out pins, 1 side 0b10
- jmp x-- dataR side 0b11
-
-.program i2s_bidi_slave
-; I2S audio bidirectional (input/output) block, both in slave mode.
-; Requires external BCK and LRCK, usually from the codec directly.
-; This block provides both the output and input components together,
-; so should not be used in combination with either of the other output or
-; input units on the same I2S bus.
-;
-; Input pin order: DIN, BCK, LRCK
-; Set JMP pin to LRCK.
-;
-; Clock synchronously with the system clock, or *at least* 4x the usual
-; bit clock for a given fs (e.g. for 48kHz 24-bit, clock at at least
-; (48000 * 24 * 2 (stereo)) * 4 = 9.216 MHz. Ideally 8x or more.
-
-start_l:
- wait 1 pin 1 ; DIN should be sampled on rising transition of BCK
- in pins, 1 ; first "bit" of new frame is actually LSB of last frame, per I2S
- pull noblock ;
- push noblock ;
- wait 0 pin 1 ; ignore first BCK transition after edge
-public_entry_point:
- wait 0 pin 2 ; wait for L frame to come around before we start
- out pins 1 ; update DOUT on falling BCK edge
-loop_l:
- wait 1 pin 1 ; DIN should be sampled on rising transition of BCK
- in pins, 1 ; read DIN
- wait 0 pin 1 ; DOUT should be updated on falling transition of BCK
- out pins 1 ; update DOUT
- jmp pin start_r ; if LRCK has gone high, we're "done" with this word (1 bit left to read)
- jmp loop_l ;
-start_r:
- wait 1 pin 1 ; wait for the last bit of the previous frame
- in pins, 1 ; first "bit" of new frame is actually LSB of last frame, per I2S
- pull noblock ; pull next output word from FIFO
- push noblock ; push the completed word to the FIFO
- wait 0 pin 1 ; wait for next clock cycle
- out pins 1 ; update DOUT on falling edge
-loop_r:
- wait 1 pin 1 ;
- in pins 1 ;
- wait 0 pin 1 ;
- out pins 1 ; update DOUT
- jmp pin loop_r ; if LRCK is still high, we're still sampling this word
- ; implicit jmp to start_l: otherwise, start the loop over
-
-
-; I2S Audio Input - Slave or Synchronous with Output Master
-; Inputs must be sequential in order: DIN, BCK, LRCK
-; Must run at same speed of SCK block, or at least 4x BCK.
-;
-; NOTE: Set JMP pin to LRCK pin.
-; NOTE: The very first word read is potentially corrupt, since there is a
-; chance to start in the middle of an L frame and only read part of it.
-; Nevertheless, the frame order should be synchronized (first word is L,
-; second is R, etc.)
-
-.program i2s_in_slave
-
-start_sample_l:
- wait 1 pin 1 ; DIN should be sampled on rising transition of BCK
- in pins, 1 ; first "bit" of new frame is actually LSB of last frame, per I2S
- push noblock ; push however many bits we read into the FIFO
- wait 0 pin 1 ; ignore first BCK transition after edge
-public_entry_point:
- wait 0 pin 2 ; wait for L frame to come around before we start
-sample_l:
- wait 1 pin 1 ; DIN should be sampled on rising transition of BCK
- in pins, 1 ; read DIN
- wait 0 pin 1 ; don't sample more than once per rising edge, wait for next clock
- jmp pin start_sample_r ; if LRCK has gone high, we're "done" with this word (1 bit left to read)
- jmp sample_l
-start_sample_r:
- wait 1 pin 1 ; wait for the last bit of the previous frame
- in pins, 1 ; first "bit" of new frame is actually LSB of last frame, per I2S
- push noblock ; push the completed word to the FIFO
- wait 0 pin 1 ; wait for next clock cycle
-sample_r:
- wait 1 pin 1
- in pins, 1
- wait 0 pin 1
- jmp pin sample_r ; if LRCK is still high, we're still sampling this word
- ; implicit jmp to start_sample_l: otherwise, start the loop over
-
-% c-sdk {
-
-// These constants are the I2S clock to pio clock ratio
-const int i2s_sck_program_pio_mult = 2;
-const int i2s_out_master_program_pio_mult = 2;
-
-/*
- * System ClocK (SCK) is only required by some I2S peripherals.
- * This outputs it at 1 SCK per 2 PIO clocks, so scale the dividers correctly
- * first.
- * NOTE: Most peripherals require that this is *perfectly* aligned in ratio,
- * if not phase, to the bit and word clocks of any master peripherals.
- * It is up to you to ensure that the divider config is set up for a
- * precise (not approximate) ratio between the BCK, LRCK, and SCK outputs.
- */
-static void i2s_sck_program_init(PIO pio, uint8_t sm, uint8_t offset, uint8_t sck_pin) {
- pio_gpio_init(pio, sck_pin);
- pio_sm_config sm_config = i2s_sck_program_get_default_config(offset);
- sm_config_set_set_pins(&sm_config, sck_pin, 1);
-
- uint pin_mask = (1u << sck_pin);
- pio_sm_set_pins_with_mask(pio, sm, 0, pin_mask); // zero output
- pio_sm_set_pindirs_with_mask(pio, sm, pin_mask, pin_mask);
-
- pio_sm_init(pio, sm, offset, &sm_config);
-}
-
-static inline void i2s_out_master_program_init(PIO pio, uint8_t sm, uint8_t offset, uint8_t bit_depth, uint8_t dout_pin, uint8_t clock_pin_base) {
- pio_gpio_init(pio, dout_pin);
- pio_gpio_init(pio, clock_pin_base);
- pio_gpio_init(pio, clock_pin_base + 1);
-
- pio_sm_config sm_config = i2s_out_master_program_get_default_config(offset);
- sm_config_set_out_pins(&sm_config, dout_pin, 1);
- sm_config_set_sideset_pins(&sm_config, clock_pin_base);
- sm_config_set_out_shift(&sm_config, false, false, bit_depth);
- sm_config_set_fifo_join(&sm_config, PIO_FIFO_JOIN_TX);
- pio_sm_init(pio, sm, offset, &sm_config);
-
- uint32_t pin_mask = (1u << dout_pin) | (3u << clock_pin_base);
- pio_sm_set_pins_with_mask(pio, sm, 0, pin_mask); // zero output
- pio_sm_set_pindirs_with_mask(pio, sm, pin_mask, pin_mask);
-}
-
-static inline void i2s_bidi_slave_program_init(PIO pio, uint8_t sm, uint8_t offset, uint8_t dout_pin, uint8_t in_pin_base) {
- pio_gpio_init(pio, dout_pin);
- pio_gpio_init(pio, in_pin_base);
- pio_gpio_init(pio, in_pin_base + 1);
- pio_gpio_init(pio, in_pin_base + 2);
-
- pio_sm_config sm_config = i2s_bidi_slave_program_get_default_config(offset);
- sm_config_set_out_pins(&sm_config, dout_pin, 1);
- sm_config_set_in_pins(&sm_config, in_pin_base);
- sm_config_set_jmp_pin(&sm_config, in_pin_base + 2);
- sm_config_set_out_shift(&sm_config, false, false, 0);
- sm_config_set_in_shift(&sm_config, false, false, 0);
- pio_sm_init(pio, sm, offset, &sm_config);
-
- // Setup output pins
- uint32_t pin_mask = (1u << dout_pin);
- pio_sm_set_pins_with_mask(pio, sm, 0, pin_mask); // zero output
- pio_sm_set_pindirs_with_mask(pio, sm, pin_mask, pin_mask);
-
- // Setup input pins
- pin_mask = (7u << in_pin_base); // Three input pins
- pio_sm_set_pindirs_with_mask(pio, sm, 0, pin_mask);
-}
-
-/*
- * Designed to be used with output master module, requiring overlapping pins:
- * din_pin_base + 0 = input pin
- * din_pin_base + 1 = out_master clock_pin_base
- * din_pin_base + 2 = out_master clock_pin_base + 1
- *
- * Intended to be run at SCK rate (4x BCK), so clock same as SCK module if using
- * it, or 4x the BCK frequency (BCK is 64x fs, so 256x fs).
- */
-static inline void i2s_in_slave_program_init(PIO pio, uint8_t sm, uint8_t offset, uint8_t din_pin_base) {
- pio_gpio_init(pio, din_pin_base);
- gpio_set_pulls(din_pin_base, false, false);
- gpio_set_dir(din_pin_base, GPIO_IN);
-
- pio_sm_config sm_config = i2s_in_slave_program_get_default_config(offset);
- sm_config_set_in_pins(&sm_config, din_pin_base);
- sm_config_set_in_shift(&sm_config, false, false, 0);
- sm_config_set_fifo_join(&sm_config, PIO_FIFO_JOIN_RX);
- sm_config_set_jmp_pin(&sm_config, din_pin_base + 2);
- pio_sm_init(pio, sm, offset, &sm_config);
-
- uint32_t pin_mask = (7u << din_pin_base); // Three input pins
- pio_sm_set_pindirs_with_mask(pio, sm, 0, pin_mask);
-}
-
-%}
\ No newline at end of file
diff --git a/software/platform/raspberry-pico-w/pico_hal_wrapper.c b/software/platform/raspberry-pico-w/pico_hal_wrapper.c
index e7fe221..aba5c35 100644
--- a/software/platform/raspberry-pico-w/pico_hal_wrapper.c
+++ b/software/platform/raspberry-pico-w/pico_hal_wrapper.c
@@ -26,6 +26,7 @@
#include "pico_lcd_spi.h"
#include "audio_player.h"
#include "pico_i2s.h"
+#include "pio_rotary_encoder.pio.h"
// ===========================================================================================================
// CONSTANTS / DEFINES
@@ -56,13 +57,31 @@ const uint8_t SD_CARD_PRESENCE = 24;
#define UART_TX_PIN 0
#define UART_RX_PIN 1
+// PICO alarm (RTOS uses Alarm 0 and IRQ 0)
+#define ALARM_NUM 1
+#define ALARM_IRQ TIMER_IRQ_1
+
// ===========================================================================================================
// GLOBAL VARIABLES
// ===========================================================================================================
static __attribute__((aligned(8))) pio_i2s i2s;
static volatile uint32_t msTicks = 0;
static audio_ctx_t audio_ctx;
+
+// Buttons
static ost_button_callback_t ButtonCallback = NULL;
+static uint32_t DebounceTs = 0;
+static bool IsDebouncing = false;
+static uint32_t ButtonsState = 0;
+static uint32_t ButtonsStatePrev = 0;
+
+// Rotary encoder
+// pio 0 is used
+PIO pio = pio1;
+// state machine 0
+uint8_t sm = 0;
+
+static int new_value, delta, old_value = 0;
// ===========================================================================================================
// PROTOTYPES
@@ -74,47 +93,95 @@ void __isr __time_critical_func(audio_i2s_dma_irq_handler)();
// ===========================================================================================================
// OST HAL IMPLEMENTATION
// ===========================================================================================================
-
void ost_system_delay_ms(uint32_t delay)
{
busy_wait_ms(delay);
}
-static uint32_t DebounceTs = 0;
-static bool IsDebouncing = false;
-static uint32_t ButtonsState = 0;
-static uint32_t ButtonsStatePrev = 0;
-
-void gpio_callback(uint gpio, uint32_t events)
+void check_buttons()
{
- static bool one_time = true;
-
- if (events & GPIO_IRQ_EDGE_FALL)
+ if (!gpio_get(ROTARY_BUTTON))
{
- DebounceTs = time_us_32() / 1000;
- IsDebouncing = true;
+ ButtonsState |= OST_BUTTON_OK;
}
- else if (IsDebouncing)
+ else
{
- IsDebouncing = false;
- uint32_t current = time_us_32() / 1000;
- if (current - DebounceTs > 50)
- {
- if (gpio_get(ROTARY_BUTTON))
- {
- ButtonsState |= OST_BUTTON_OK;
- }
+ ButtonsState &= ~OST_BUTTON_OK;
+ }
- if ((ButtonsStatePrev != ButtonsState) && (ButtonCallback != NULL))
+ // note: thanks to two's complement arithmetic delta will always
+ // be correct even when new_value wraps around MAXINT / MININT
+ new_value = quadrature_encoder_get_count(pio, sm);
+ delta = new_value - old_value;
+ old_value = new_value;
+
+ if (delta > 0)
+ {
+ ButtonsState |= OST_BUTTON_LEFT;
+ }
+ else if (delta < 0)
+ {
+ ButtonsState |= OST_BUTTON_RIGHT;
+ }
+ else
+ {
+ ButtonsState &= ~OST_BUTTON_LEFT;
+ ButtonsState &= ~OST_BUTTON_RIGHT;
+ }
+
+ if (IsDebouncing)
+ {
+ // Même état pendant X millisecondes, on valide
+ if ((ButtonsStatePrev == ButtonsState) && (ButtonCallback != NULL))
+ {
+ if (ButtonsState != 0)
{
ButtonCallback(ButtonsState);
}
+ }
+ IsDebouncing = false;
+ ButtonsStatePrev = ButtonsState;
+ }
+ else
+ {
+ // Changement d'état détecté
+ if (ButtonsStatePrev != ButtonsState)
+ {
ButtonsStatePrev = ButtonsState;
+ IsDebouncing = true;
}
}
}
+static void alarm_in_us(uint32_t delay_us);
+
+static void alarm_irq(void)
+{
+ // Clear the alarm irq
+ hw_clear_bits(&timer_hw->intr, 1u << ALARM_NUM);
+ alarm_in_us(10000);
+
+ check_buttons();
+}
+
+static void alarm_in_us(uint32_t delay_us)
+{
+ // Enable the interrupt for our alarm (the timer outputs 4 alarm irqs)
+ hw_set_bits(&timer_hw->inte, 1u << ALARM_NUM);
+
+ // Enable interrupt in block and at processor
+
+ // Alarm is only 32 bits so if trying to delay more
+ // than that need to be careful and keep track of the upper
+ // bits
+ uint64_t target = timer_hw->timerawl + delay_us;
+
+ // Write the lower 32 bits of the target time to the alarm which
+ // will arm it
+ timer_hw->alarm[ALARM_NUM] = (uint32_t)target;
+}
+
void ost_system_initialize()
{
set_sys_clock_khz(125000, true);
@@ -160,17 +227,22 @@ void ost_system_initialize()
ST7789_Fill_Color(MAGENTA);
//------------------- Rotary encoder init
- gpio_init(ROTARY_A);
- gpio_set_dir(ROTARY_A, GPIO_IN);
-
- gpio_init(ROTARY_B);
- gpio_set_dir(ROTARY_B, GPIO_IN);
-
gpio_init(ROTARY_BUTTON);
gpio_set_dir(ROTARY_BUTTON, GPIO_IN);
gpio_disable_pulls(ROTARY_BUTTON);
- gpio_set_irq_enabled_with_callback(ROTARY_BUTTON, GPIO_IRQ_EDGE_FALL | GPIO_IRQ_EDGE_RISE, true, &gpio_callback);
+ // Rotary Encoder is managed by the PIO !
+ // 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, ROTARY_A, 0);
+
+ // Set irq handler for alarm irq
+ irq_set_exclusive_handler(ALARM_IRQ, alarm_irq);
+ // Enable the alarm irq
+ irq_set_enabled(ALARM_IRQ, true);
+
+ alarm_in_us(100000);
//------------------- Init SDCARD
gpio_init(SD_CARD_CS);
diff --git a/software/platform/raspberry-pico-w/pico_i2s.pio b/software/platform/raspberry-pico-w/pico_i2s.pio
index 582cf7f..f1e6751 100644
--- a/software/platform/raspberry-pico-w/pico_i2s.pio
+++ b/software/platform/raspberry-pico-w/pico_i2s.pio
@@ -1,25 +1,5 @@
; i2s.pio
;
-; Author: Daniel Collins
-; Date: 2022-02-25
-;
-; Copyright (c) 2022 Daniel Collins
-;
-; This file is part of rp2040_i2s_example.
-;
-; rp2040_i2s_example is free software: you can redistribute it and/or modify it under
-; the terms of the GNU General Public License, version 3 as published by the
-; Free Software Foundation.
-;
-; rp2040_i2s_example is distributed in the hope that it will be useful, but WITHOUT ANY
-; WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
-; A PARTICULAR PURPOSE. See the GNU General Public License for more details.
-;
-; You should have received a copy of the GNU General Public License along with
-; rp2040_i2s_example. If not, see .
-
-; An I2S bi-directional peripheral with master clock (SCK) output.
-
.program i2s_out_master
; I2S audio output block. Synchronous with clock and input.
diff --git a/software/platform/raspberry-pico-w/pio_rotary_encoder.pio b/software/platform/raspberry-pico-w/pio_rotary_encoder.pio
new file mode 100644
index 0000000..f478972
--- /dev/null
+++ b/software/platform/raspberry-pico-w/pio_rotary_encoder.pio
@@ -0,0 +1,141 @@
+;
+; 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--, "
+ ; 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;
+}
+
+%}
diff --git a/software/system/hmi_task.h b/software/system/hmi_task.h
deleted file mode 100644
index 70ce9e5..0000000
--- a/software/system/hmi_task.h
+++ /dev/null
@@ -1,10 +0,0 @@
-#ifndef HMI_TASK_H
-#define HMI_TASK_H
-
-#include
-
-void hmi_task_initialize();
-
-void hmi_task_ost_ready(uint32_t number_of_stories);
-
-#endif // HMI_TASK_H
diff --git a/software/system/main.c b/software/system/main.c
index fa531b5..9aa7494 100644
--- a/software/system/main.c
+++ b/software/system/main.c
@@ -14,7 +14,7 @@
#include "chip32_vm.h"
#include "system.h"
-#include "hmi_task.h"
+#include "usb_task.h"
#include "vm_task.h"
#include "fs_task.h"
@@ -32,7 +32,7 @@ void IdleTask(void *args)
while (1)
{
// Instrumentation, power saving, os functions won't work here
- // __asm volatile("wfi");
+ __asm volatile("wfi");
}
}
@@ -45,7 +45,7 @@ int main()
ost_system_initialize();
// 2. Test the printf output
- debug_printf("\r\n [OST] Starting OpenStoryTeller tests: V%d.%d\r\n", 1, 0);
+ debug_printf("\r\n[OST] Starting OpenStoryTeller tests: V%d.%d\r\n", 1, 0);
// 3. Filesystem / SDCard initialization
filesystem_mount();
@@ -54,7 +54,7 @@ int main()
qor_init(125000000UL);
// 5. Initialize the tasks
- hmi_task_initialize();
+ usb_task_initialize();
vm_task_initialize();
fs_task_initialize();
diff --git a/software/system/ost_hal.h b/software/system/ost_hal.h
index 13a63d8..23a8006 100644
--- a/software/system/ost_hal.h
+++ b/software/system/ost_hal.h
@@ -40,7 +40,9 @@ extern "C"
OST_BUTTON_HOME = 0x02,
OST_BUTTON_PAUSE = 0x04,
OST_BUTTON_VOLUME_UP = 0x08,
- OST_BUTTON_VOLUME_DOWN = 0x10
+ OST_BUTTON_VOLUME_DOWN = 0x10,
+ OST_BUTTON_LEFT = 0x20,
+ OST_BUTTON_RIGHT = 0x40
} ost_button_t;
// ----------------------------------------------------------------------------
diff --git a/software/system/qor.h b/software/system/qor.h
index 2f30665..3f984af 100644
--- a/software/system/qor.h
+++ b/software/system/qor.h
@@ -1,7 +1,7 @@
#ifndef QOR_H
#define QOR_H
-#define QOR_VERSION "0.1"
+#define QOR_VERSION "0.2"
#include
#include
diff --git a/software/system/qor_armv6m.s b/software/system/qor_armv6m.s
index 48df9b0..aeb8f94 100644
--- a/software/system/qor_armv6m.s
+++ b/software/system/qor_armv6m.s
@@ -40,7 +40,7 @@ qor_go:
mov lr, r5 @ Update LR
pop {r3} @ Task code
pop {r2} @ Pop and discard XPSR. */
- cpsie i @ Enable interrupts before launching the
+ cpsie i @ Enable interrupts
bx r3 @ /* Finally, jump to the user defined task code. */
diff --git a/software/system/hmi_task.c b/software/system/usb_task.c
similarity index 86%
rename from software/system/hmi_task.c
rename to software/system/usb_task.c
index d0fabc1..0343c4f 100644
--- a/software/system/hmi_task.c
+++ b/software/system/usb_task.c
@@ -43,14 +43,14 @@ typedef enum
// GLOBAL STORY VARIABLES
// ===========================================================================================================
-static qor_tcb_t HmiTcb;
-static uint32_t HmiStack[4096];
+static qor_tcb_t UsbTcb;
+static uint32_t UsbStack[4096];
-static qor_mbox_t HmiMailBox;
+static qor_mbox_t UsbMailBox;
-static ost_hmi_event_t HmiEvent;
+static ost_hmi_event_t UsbEvent;
-static ost_hmi_event_t *HmiQueue[10];
+static ost_hmi_event_t *UsbQueue[10];
static ost_system_state_t OstState = OST_SYS_WAIT_INDEX;
@@ -59,7 +59,7 @@ static ost_context_t OstContext;
// ===========================================================================================================
// HMI TASK (user interface, buttons manager, LCD)
// ===========================================================================================================
-void HmiTask(void *args)
+void UsbTask(void *args)
{
ost_hmi_event_t *e = NULL;
@@ -76,14 +76,14 @@ void HmiTask(void *args)
#include "msc_disk.h"
-void hmi_task_initialize()
+void usb_task_initialize()
{
OstState = OST_SYS_WAIT_INDEX;
- qor_mbox_init(&HmiMailBox, (void **)&HmiQueue, 10);
+ qor_mbox_init(&UsbMailBox, (void **)&UsbQueue, 10);
msc_disk_initialize();
- qor_create_thread(&HmiTcb, HmiTask, HmiStack, sizeof(HmiStack) / sizeof(HmiStack[0]), HMI_TASK_PRIORITY, "HmiTask"); // less priority is the HMI (user inputs and LCD)
+ qor_create_thread(&UsbTcb, UsbTask, UsbStack, sizeof(UsbStack) / sizeof(UsbStack[0]), HMI_TASK_PRIORITY, "UsbTask"); // less priority is the HMI (user inputs and LCD)
}
//--------------------------------------------------------------------+
diff --git a/software/system/usb_task.h b/software/system/usb_task.h
new file mode 100644
index 0000000..248f492
--- /dev/null
+++ b/software/system/usb_task.h
@@ -0,0 +1,8 @@
+#ifndef USB_TASK_H
+#define USB_TASK_H
+
+#include
+
+void usb_task_initialize();
+
+#endif // USB_TASK_H
diff --git a/software/system/vm_task.c b/software/system/vm_task.c
index 835176d..ea22822 100644
--- a/software/system/vm_task.c
+++ b/software/system/vm_task.c
@@ -27,15 +27,15 @@ typedef enum
} ost_vm_ev_type_t;
typedef struct
{
+ uint32_t button_mask;
ost_vm_ev_type_t ev;
const char *story_dir;
- uint32_t button_mask;
} ost_vm_event_t;
typedef enum
{
OST_VM_STATE_HOME,
- OST_VM_STATE_HOME_WAIT_FS,
+ OST_VM_STATE_HOME_WAIT_LOAD_STORY,
OST_VM_STATE_RUN_STORY,
OST_VM_STATE_WAIT_BUTTON,
OST_VM_STATE_ERROR //!< General error, cannot continue
@@ -160,11 +160,10 @@ void VmTask(void *args)
chip32_result_t run_result;
ost_vm_event_t *message = NULL;
uint32_t res = 0;
- bool isHome = true;
ost_vm_state_t VmState = OST_VM_STATE_HOME;
fs_task_scan_index(read_index_callback);
- isHome = true;
+ bool run_script = false;
while (1)
{
@@ -181,11 +180,72 @@ void VmTask(void *args)
// La lecture de l'index est terminée, on demande l'affichage des médias
fs_task_play_index();
break;
+ case VM_EV_BUTTON_EVENT:
+ // debug_printf("B: %x", message->button_mask);
+ if ((message->button_mask & OST_BUTTON_OK) == OST_BUTTON_OK)
+ {
+ VmState = OST_VM_STATE_HOME_WAIT_LOAD_STORY;
+ debug_printf("OK\r\n");
+ fs_task_load_story(m_rom_data);
+ }
+ break;
case VM_EV_ERROR:
default:
break;
}
+ case OST_VM_STATE_HOME_WAIT_LOAD_STORY:
+ if (message->ev == VM_EV_START_STORY_EVENT)
+ {
+ // Launch the execution of a story
+ chip32_initialize(&m_chip32_ctx);
+ VmState = OST_VM_STATE_RUN_STORY;
+ run_script = true;
+ }
+
+ // Pas de break, on enchaîne sur le déroulement du script
+ case OST_VM_STATE_RUN_STORY:
+ {
+
+ if (message->ev == VM_EV_END_OF_SOUND)
+ {
+ run_script = true;
+ }
+
+ if (message->ev == VM_EV_BUTTON_EVENT)
+ {
+ if ((message->button_mask & OST_BUTTON_OK) == OST_BUTTON_OK)
+ {
+ debug_printf("OK\r\n");
+ m_chip32_ctx.registers[R0] = 0x01;
+ run_script = true;
+ }
+ else if ((message->button_mask & OST_BUTTON_LEFT) == OST_BUTTON_LEFT)
+ {
+ debug_printf("<-\r\n");
+ m_chip32_ctx.registers[R0] = 0x02;
+ run_script = true;
+ }
+ else if ((message->button_mask & OST_BUTTON_RIGHT) == OST_BUTTON_RIGHT)
+ {
+ debug_printf("->\r\n");
+ m_chip32_ctx.registers[R0] = 0x04;
+ run_script = true;
+ }
+ }
+
+ if (run_script)
+ {
+ do
+ {
+ run_result = chip32_step(&m_chip32_ctx);
+
+ } while (run_result == VM_OK);
+ }
+ run_script = false;
+
+ break;
+ }
default:
break;
}
@@ -214,52 +274,3 @@ void vm_task_initialize()
ost_button_register_callback(button_callback);
}
-
-/*
- case OST_VM_STATE_RUN_STORY:
- do
- {
- run_result = chip32_step(&m_chip32_ctx);
-
- } while (run_result == VM_OK);
-
- // pour le moment, dans tous les cas on attend un événement
- // Que ce soit par erreur, ou l'attente un appui boutton...
- VmState = OST_VM_STATE_WAIT_BUTTON;
- break;
-
-case OST_VM_STATE_ERROR:
-qor_sleep(20);
-break;
-case OST_VM_STATE_WAIT_BUTTON:
-default:
-
-if (isHome)
-{
- if (message->ev == VM_EV_BUTTON_EVENT)
- {
- if ((message->button_mask & OST_BUTTON_OK) == OST_BUTTON_OK)
- {
- // Load story from disk
- debug_printf("Clicked OK\r\n");
- fs_task_load_story(m_rom_data);
- }
- }
- else if (message->ev == VM_EV_START_STORY_EVENT)
- {
- // Launch the execution of a story
- chip32_initialize(&m_chip32_ctx);
- isHome = false;
- VmState = OST_VM_STATE_RUN_STORY;
- }
-}
-else
-{
- // VM en cours d'exécution
- if (message->ev == VM_EV_END_OF_SOUND)
- {
- VmState = OST_VM_STATE_RUN_STORY;
- }
-}
-}
-*/