From d10afb02323c2c86142f9aa8b6e8cd803ebc7bc1 Mon Sep 17 00:00:00 2001 From: Anthony Rabine Date: Tue, 8 Aug 2023 17:37:40 +0200 Subject: [PATCH] changed hmi task into usb_task, qor fix, buttons and rotary encoder driver --- .gitignore | 2 + software/.gdbinit | 29 ++ .../.cortex-debug.peripherals.state.json | 1 - software/CMakeLists.txt | 2 +- .../platform/raspberry-pico-w/CMakeLists.txt | 2 +- software/platform/raspberry-pico-w/i2s.pio | 253 ------------------ .../raspberry-pico-w/pico_hal_wrapper.c | 130 +++++++-- .../platform/raspberry-pico-w/pico_i2s.pio | 20 -- .../raspberry-pico-w/pio_rotary_encoder.pio | 141 ++++++++++ software/system/hmi_task.h | 10 - software/system/main.c | 8 +- software/system/ost_hal.h | 4 +- software/system/qor.h | 2 +- software/system/qor_armv6m.s | 2 +- software/system/{hmi_task.c => usb_task.c} | 18 +- software/system/usb_task.h | 8 + software/system/vm_task.c | 117 ++++---- 17 files changed, 365 insertions(+), 384 deletions(-) create mode 100644 software/.gdbinit delete mode 100644 software/.vscode/.cortex-debug.peripherals.state.json delete mode 100644 software/platform/raspberry-pico-w/i2s.pio create mode 100644 software/platform/raspberry-pico-w/pio_rotary_encoder.pio delete mode 100644 software/system/hmi_task.h rename software/system/{hmi_task.c => usb_task.c} (86%) create mode 100644 software/system/usb_task.h 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; - } -} -} -*/