diff options
Diffstat (limited to 'platforms')
25 files changed, 438 insertions, 68 deletions
diff --git a/platforms/arm_atsam/eeprom_samd.c b/platforms/arm_atsam/eeprom_samd.c index 1c1e031e5d..9c42041f2d 100644 --- a/platforms/arm_atsam/eeprom_samd.c +++ b/platforms/arm_atsam/eeprom_samd.c @@ -15,15 +15,12 @@ */ #include "eeprom.h" #include "debug.h" +#include "util.h" #include "samd51j18a.h" #include "core_cm4.h" #include "component/nvmctrl.h" #include "eeprom_samd.h" -#ifndef MAX -# define MAX(X, Y) ((X) > (Y) ? (X) : (Y)) -#endif - #ifndef BUSY_RETRIES # define BUSY_RETRIES 10000 #endif diff --git a/platforms/avr/_wait.h b/platforms/avr/_wait.h index c1a598a428..39cbf618d2 100644 --- a/platforms/avr/_wait.h +++ b/platforms/avr/_wait.h @@ -15,7 +15,11 @@ */ #pragma once +// Need to disable GCC's "maybe-uninitialized" warning for this file, as it causes issues when running `KEEP_INTERMEDIATES=yes`. +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wmaybe-uninitialized" #include <util/delay.h> +#pragma GCC diagnostic pop // http://ww1.microchip.com/downloads/en/devicedoc/atmel-0856-avr-instruction-set-manual.pdf // page 22: Table 4-2. Arithmetic and Logic Instructions diff --git a/platforms/avr/drivers/i2c_master.c b/platforms/avr/drivers/i2c_master.c index 524494c99d..58939f3e00 100644 --- a/platforms/avr/drivers/i2c_master.c +++ b/platforms/avr/drivers/i2c_master.c @@ -23,6 +23,7 @@ #include "i2c_master.h" #include "timer.h" #include "wait.h" +#include "util.h" #ifndef F_SCL # define F_SCL 400000UL // SCL frequency @@ -37,8 +38,6 @@ #define TWBR_val (((F_CPU / F_SCL) - 16) / 2) -#define MAX(X, Y) ((X) > (Y) ? (X) : (Y)) - void i2c_init(void) { TWSR = 0; /* no prescaler */ TWBR = (uint8_t)TWBR_val; diff --git a/platforms/avr/drivers/ws2812.c b/platforms/avr/drivers/ws2812_bitbang.c index 5c0cb3b718..aad10d86b0 100644 --- a/platforms/avr/drivers/ws2812.c +++ b/platforms/avr/drivers/ws2812_bitbang.c @@ -38,10 +38,10 @@ static inline void ws2812_sendarray_mask(uint8_t *data, uint16_t datlen, uint8_t masklo, uint8_t maskhi); void ws2812_setleds(LED_TYPE *ledarray, uint16_t number_of_leds) { - DDRx_ADDRESS(RGB_DI_PIN) |= pinmask(RGB_DI_PIN); + DDRx_ADDRESS(WS2812_DI_PIN) |= pinmask(WS2812_DI_PIN); - uint8_t masklo = ~(pinmask(RGB_DI_PIN)) & PORTx_ADDRESS(RGB_DI_PIN); - uint8_t maskhi = pinmask(RGB_DI_PIN) | PORTx_ADDRESS(RGB_DI_PIN); + uint8_t masklo = ~(pinmask(WS2812_DI_PIN)) & PORTx_ADDRESS(WS2812_DI_PIN); + uint8_t maskhi = pinmask(WS2812_DI_PIN) | PORTx_ADDRESS(WS2812_DI_PIN); ws2812_sendarray_mask((uint8_t *)ledarray, number_of_leds * sizeof(LED_TYPE), masklo, maskhi); @@ -165,7 +165,7 @@ static inline void ws2812_sendarray_mask(uint8_t *data, uint16_t datlen, uint8_t " dec %0 \n\t" // '1' [+2] '0' [+2] " brne loop%=\n\t" // '1' [+3] '0' [+4] : "=&d"(ctr) - : "r"(curbyte), "I"(_SFR_IO_ADDR(PORTx_ADDRESS(RGB_DI_PIN))), "r"(maskhi), "r"(masklo)); + : "r"(curbyte), "I"(_SFR_IO_ADDR(PORTx_ADDRESS(WS2812_DI_PIN))), "r"(maskhi), "r"(masklo)); } SREG = sreg_prev; diff --git a/platforms/avr/drivers/ws2812_i2c.c b/platforms/avr/drivers/ws2812_i2c.c index 709f382254..f4a2fbe0b3 100644 --- a/platforms/avr/drivers/ws2812_i2c.c +++ b/platforms/avr/drivers/ws2812_i2c.c @@ -5,12 +5,12 @@ # error "RGBW not supported" #endif -#ifndef WS2812_ADDRESS -# define WS2812_ADDRESS 0xb0 +#ifndef WS2812_I2C_ADDRESS +# define WS2812_I2C_ADDRESS 0xB0 #endif -#ifndef WS2812_TIMEOUT -# define WS2812_TIMEOUT 100 +#ifndef WS2812_I2C_TIMEOUT +# define WS2812_I2C_TIMEOUT 100 #endif void ws2812_init(void) { @@ -25,5 +25,5 @@ void ws2812_setleds(LED_TYPE *ledarray, uint16_t leds) { s_init = true; } - i2c_transmit(WS2812_ADDRESS, (uint8_t *)ledarray, sizeof(LED_TYPE) * leds, WS2812_TIMEOUT); + i2c_transmit(WS2812_I2C_ADDRESS, (uint8_t *)ledarray, sizeof(LED_TYPE) * leds, WS2812_I2C_TIMEOUT); } diff --git a/platforms/chibios/boards/BONSAI_C4/configs/config.h b/platforms/chibios/boards/BONSAI_C4/configs/config.h index e412f73d3d..c5dbb25c45 100644 --- a/platforms/chibios/boards/BONSAI_C4/configs/config.h +++ b/platforms/chibios/boards/BONSAI_C4/configs/config.h @@ -67,8 +67,8 @@ // WS2812-style LED control on pin A10 #ifdef WS2812_DRIVER_PWM -# ifndef RGB_DI_PIN -# define RGB_DI_PIN PAL_LINE(GPIOA, 10) +# ifndef WS2812_DI_PIN +# define WS2812_DI_PIN PAL_LINE(GPIOA, 10) # endif # ifndef WS2812_PWM_DRIVER # define WS2812_PWM_DRIVER PWMD1 diff --git a/platforms/chibios/boards/STM32_F103_STM32DUINO/ld/STM32F103x6_stm32duino_bootloader.ld b/platforms/chibios/boards/STM32_F103_STM32DUINO/ld/STM32F103x6_stm32duino.ld index 18aaff2a23..18aaff2a23 100644 --- a/platforms/chibios/boards/STM32_F103_STM32DUINO/ld/STM32F103x6_stm32duino_bootloader.ld +++ b/platforms/chibios/boards/STM32_F103_STM32DUINO/ld/STM32F103x6_stm32duino.ld diff --git a/platforms/chibios/boards/STM32_F103_STM32DUINO/ld/STM32F103x8_stm32duino_bootloader.ld b/platforms/chibios/boards/STM32_F103_STM32DUINO/ld/STM32F103x8_stm32duino.ld index 465af12cab..465af12cab 100644 --- a/platforms/chibios/boards/STM32_F103_STM32DUINO/ld/STM32F103x8_stm32duino_bootloader.ld +++ b/platforms/chibios/boards/STM32_F103_STM32DUINO/ld/STM32F103x8_stm32duino.ld diff --git a/platforms/chibios/boards/STM32_F103_STM32DUINO/ld/STM32F103xB_stm32duino_bootloader.ld b/platforms/chibios/boards/STM32_F103_STM32DUINO/ld/STM32F103xB_stm32duino.ld index 3a47a33156..3a47a33156 100644 --- a/platforms/chibios/boards/STM32_F103_STM32DUINO/ld/STM32F103xB_stm32duino_bootloader.ld +++ b/platforms/chibios/boards/STM32_F103_STM32DUINO/ld/STM32F103xB_stm32duino.ld diff --git a/platforms/chibios/boards/common/ld/STM32F103x8_uf2boot.ld b/platforms/chibios/boards/common/ld/STM32F103x8_uf2boot.ld new file mode 100644 index 0000000000..adf43c362a --- /dev/null +++ b/platforms/chibios/boards/common/ld/STM32F103x8_uf2boot.ld @@ -0,0 +1,88 @@ +/* + ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ + +/* + * ST32F103x8 memory setup. + */ +MEMORY +{ + flash0 (rx) : org = 0x08000000 + 16K, len = 64k - 16K + flash1 (rx) : org = 0x00000000, len = 0 + flash2 (rx) : org = 0x00000000, len = 0 + flash3 (rx) : org = 0x00000000, len = 0 + flash4 (rx) : org = 0x00000000, len = 0 + flash5 (rx) : org = 0x00000000, len = 0 + flash6 (rx) : org = 0x00000000, len = 0 + flash7 (rx) : org = 0x00000000, len = 0 + ram0 (wx) : org = 0x20000000, len = 20k + ram1 (wx) : org = 0x00000000, len = 0 + ram2 (wx) : org = 0x00000000, len = 0 + ram3 (wx) : org = 0x00000000, len = 0 + ram4 (wx) : org = 0x00000000, len = 0 + ram5 (wx) : org = 0x00000000, len = 0 + ram6 (wx) : org = 0x00000000, len = 0 + ram7 (wx) : org = 0x00000000, len = 0 +} + +/* For each data/text section two region are defined, a virtual region + and a load region (_LMA suffix).*/ + +/* Flash region to be used for exception vectors.*/ +REGION_ALIAS("VECTORS_FLASH", flash0); +REGION_ALIAS("VECTORS_FLASH_LMA", flash0); + +/* Flash region to be used for constructors and destructors.*/ +REGION_ALIAS("XTORS_FLASH", flash0); +REGION_ALIAS("XTORS_FLASH_LMA", flash0); + +/* Flash region to be used for code text.*/ +REGION_ALIAS("TEXT_FLASH", flash0); +REGION_ALIAS("TEXT_FLASH_LMA", flash0); + +/* Flash region to be used for read only data.*/ +REGION_ALIAS("RODATA_FLASH", flash0); +REGION_ALIAS("RODATA_FLASH_LMA", flash0); + +/* Flash region to be used for various.*/ +REGION_ALIAS("VARIOUS_FLASH", flash0); +REGION_ALIAS("VARIOUS_FLASH_LMA", flash0); + +/* Flash region to be used for RAM(n) initialization data.*/ +REGION_ALIAS("RAM_INIT_FLASH_LMA", flash0); + +/* RAM region to be used for Main stack. This stack accommodates the processing + of all exceptions and interrupts.*/ +REGION_ALIAS("MAIN_STACK_RAM", ram0); + +/* RAM region to be used for the process stack. This is the stack used by + the main() function.*/ +REGION_ALIAS("PROCESS_STACK_RAM", ram0); + +/* RAM region to be used for data segment.*/ +REGION_ALIAS("DATA_RAM", ram0); +REGION_ALIAS("DATA_RAM_LMA", flash0); + +/* RAM region to be used for BSS segment.*/ +REGION_ALIAS("BSS_RAM", ram0); + +/* RAM region to be used for the default heap.*/ +REGION_ALIAS("HEAP_RAM", ram0); + +/* Generic rules inclusion.*/ +INCLUDE rules.ld + +/* Bootloader reset support */ +_board_magic_reg = ORIGIN(ram0) + 16k; /* this is based off the code within backup.c */ diff --git a/platforms/chibios/bootloader.mk b/platforms/chibios/bootloader.mk index 4812412344..fc898e7699 100644 --- a/platforms/chibios/bootloader.mk +++ b/platforms/chibios/bootloader.mk @@ -93,7 +93,6 @@ ifeq ($(strip $(BOOTLOADER)), kiibohd) endif ifeq ($(strip $(BOOTLOADER)), stm32duino) OPT_DEFS += -DBOOTLOADER_STM32DUINO - MCU_LDSCRIPT = STM32F103x8_stm32duino_bootloader BOARD = STM32_F103_STM32DUINO BOOTLOADER_TYPE = stm32duino diff --git a/platforms/chibios/chibios_config.h b/platforms/chibios/chibios_config.h index 4c8333f07b..52632a051e 100644 --- a/platforms/chibios/chibios_config.h +++ b/platforms/chibios/chibios_config.h @@ -33,26 +33,40 @@ # define RP2040_PWM_CHANNEL_A 1U # define RP2040_PWM_CHANNEL_B 2U -# define BACKLIGHT_PAL_MODE (PAL_MODE_ALTERNATE_PWM | PAL_RP_PAD_DRIVE12 | PAL_RP_GPIO_OE) +# ifndef BACKLIGHT_PAL_MODE +# define BACKLIGHT_PAL_MODE (PAL_MODE_ALTERNATE_PWM | PAL_RP_PAD_DRIVE12 | PAL_RP_GPIO_OE) +# endif # define BACKLIGHT_PWM_COUNTER_FREQUENCY 1000000 # define BACKLIGHT_PWM_PERIOD BACKLIGHT_PWM_COUNTER_FREQUENCY / 2048 -# define AUDIO_PWM_PAL_MODE (PAL_MODE_ALTERNATE_PWM | PAL_RP_PAD_DRIVE12 | PAL_RP_GPIO_OE) +# ifndef AUDIO_PWM_PAL_MODE +# define AUDIO_PWM_PAL_MODE (PAL_MODE_ALTERNATE_PWM | PAL_RP_PAD_DRIVE12 | PAL_RP_GPIO_OE) +# endif # define AUDIO_PWM_COUNTER_FREQUENCY 500000 # define usb_lld_endpoint_fields -# define I2C1_SCL_PAL_MODE (PAL_MODE_ALTERNATE_I2C | PAL_RP_PAD_SLEWFAST | PAL_RP_PAD_PUE | PAL_RP_PAD_DRIVE4) -# define I2C1_SDA_PAL_MODE I2C1_SCL_PAL_MODE +# ifndef I2C1_SCL_PAL_MODE +# define I2C1_SCL_PAL_MODE (PAL_MODE_ALTERNATE_I2C | PAL_RP_PAD_SLEWFAST | PAL_RP_PAD_PUE | PAL_RP_PAD_DRIVE4) +# endif +# ifndef I2C1_SDA_PAL_MODE +# define I2C1_SDA_PAL_MODE (PAL_MODE_ALTERNATE_I2C | PAL_RP_PAD_SLEWFAST | PAL_RP_PAD_PUE | PAL_RP_PAD_DRIVE4) +# endif # define USE_I2CV1_CONTRIB # if !defined(I2C1_CLOCK_SPEED) # define I2C1_CLOCK_SPEED 400000 # endif -# define SPI_SCK_PAL_MODE (PAL_MODE_ALTERNATE_SPI | PAL_RP_PAD_SLEWFAST | PAL_RP_PAD_DRIVE4) -# define SPI_MOSI_PAL_MODE SPI_SCK_PAL_MODE -# define SPI_MISO_PAL_MODE SPI_SCK_PAL_MODE +# ifndef SPI_SCK_PAL_MODE +# define SPI_SCK_PAL_MODE (PAL_MODE_ALTERNATE_SPI | PAL_RP_PAD_SLEWFAST | PAL_RP_PAD_DRIVE4) +# endif +# ifndef SPI_MOSI_PAL_MODE +# define SPI_MOSI_PAL_MODE (PAL_MODE_ALTERNATE_SPI | PAL_RP_PAD_SLEWFAST | PAL_RP_PAD_DRIVE4) +# endif +# ifndef SPI_MISO_PAL_MODE +# define SPI_MISO_PAL_MODE (PAL_MODE_ALTERNATE_SPI | PAL_RP_PAD_SLEWFAST | PAL_RP_PAD_DRIVE4) +# endif #endif // STM32 compatibility diff --git a/platforms/chibios/converters/elite_c_to_liatris/pre_converter.mk b/platforms/chibios/converters/elite_c_to_liatris/pre_converter.mk new file mode 100644 index 0000000000..b38823fa5f --- /dev/null +++ b/platforms/chibios/converters/elite_c_to_liatris/pre_converter.mk @@ -0,0 +1,2 @@ +CONVERTER:=platforms/chibios/converters/elite_c_to_rp2040_ce +ACTIVE_CONVERTER:=rp2040_ce diff --git a/platforms/chibios/converters/promicro_to_liatris/pre_converter.mk b/platforms/chibios/converters/promicro_to_liatris/pre_converter.mk new file mode 100644 index 0000000000..7b3130a5e9 --- /dev/null +++ b/platforms/chibios/converters/promicro_to_liatris/pre_converter.mk @@ -0,0 +1,2 @@ +CONVERTER:=platforms/chibios/converters/promicro_to_rp2040_ce +ACTIVE_CONVERTER:=rp2040_ce diff --git a/platforms/chibios/drivers/audio_dac_additive.c b/platforms/chibios/drivers/audio_dac_additive.c index 68ce13788e..d29147ca3b 100644 --- a/platforms/chibios/drivers/audio_dac_additive.c +++ b/platforms/chibios/drivers/audio_dac_additive.c @@ -19,6 +19,10 @@ #include <ch.h> #include <hal.h> +// Need to disable GCC's "tautological-compare" warning for this file, as it causes issues when running `KEEP_INTERMEDIATES=yes`. Corresponding pop at the end of the file. +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wtautological-compare" + /* Audio Driver: DAC @@ -335,3 +339,5 @@ void audio_driver_start(void) { active_tones_snapshot_length = 0; state = OUTPUT_SHOULD_START; } + +#pragma GCC diagnostic pop diff --git a/platforms/chibios/drivers/audio_dac_basic.c b/platforms/chibios/drivers/audio_dac_basic.c index 5f0cbf8f84..4ea23a2158 100644 --- a/platforms/chibios/drivers/audio_dac_basic.c +++ b/platforms/chibios/drivers/audio_dac_basic.c @@ -19,6 +19,10 @@ #include "ch.h" #include "hal.h" +// Need to disable GCC's "tautological-compare" warning for this file, as it causes issues when running `KEEP_INTERMEDIATES=yes`. Corresponding pop at the end of the file. +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wtautological-compare" + /* Audio Driver: DAC @@ -247,3 +251,5 @@ void audio_driver_start(void) { } gptStartContinuous(&AUDIO_STATE_TIMER, 2U); } + +#pragma GCC diagnostic pop diff --git a/platforms/chibios/drivers/ps2/ps2_io.c b/platforms/chibios/drivers/ps2/ps2_io.c index 906d85d848..9eb56d63da 100644 --- a/platforms/chibios/drivers/ps2/ps2_io.c +++ b/platforms/chibios/drivers/ps2/ps2_io.c @@ -4,6 +4,7 @@ // chibiOS headers #include "ch.h" #include "hal.h" +#include "gpio.h" /* Check port settings for clock and data line */ #if !(defined(PS2_CLOCK_PIN)) diff --git a/platforms/chibios/drivers/vendor/RP/RP2040/ps2_vendor.c b/platforms/chibios/drivers/vendor/RP/RP2040/ps2_vendor.c new file mode 100644 index 0000000000..937fa5de6f --- /dev/null +++ b/platforms/chibios/drivers/vendor/RP/RP2040/ps2_vendor.c @@ -0,0 +1,271 @@ +// Copyright 2022 Marek Kraus (@gamelaster) +// SPDX-License-Identifier: GPL-2.0-or-later + +#include "quantum.h" + +#include "hardware/pio.h" +#include "hardware/clocks.h" +#include "ps2.h" +#include "print.h" + +#if !defined(MCU_RP) +# error PIO Driver is only available for Raspberry Pi 2040 MCUs! +#endif + +#if defined(PS2_ENABLE) +# if defined(PS2_MOUSE_ENABLE) +# if !defined(PS2_MOUSE_USE_REMOTE_MODE) +# define BUFFERED_MODE_ENABLE +# endif +# else // PS2 Keyboard +# define BUFFERED_MODE_ENABLE +# endif +#endif + +#if PS2_DATA_PIN + 1 != PS2_CLOCK_PIN +# error PS/2 Clock pin must be followed by data pin! +#endif + +static inline void pio_serve_interrupt(void); + +#if defined(PS2_PIO_USE_PIO1) +static const PIO pio = pio1; + +OSAL_IRQ_HANDLER(RP_PIO1_IRQ_0_HANDLER) { + OSAL_IRQ_PROLOGUE(); + pio_serve_interrupt(); + OSAL_IRQ_EPILOGUE(); +} +#else +static const PIO pio = pio0; + +OSAL_IRQ_HANDLER(RP_PIO0_IRQ_0_HANDLER) { + OSAL_IRQ_PROLOGUE(); + pio_serve_interrupt(); + OSAL_IRQ_EPILOGUE(); +} +#endif + +#define PS2_WRAP_TARGET 0 +#define PS2_WRAP 20 + +// clang-format off +static const uint16_t ps2_program_instructions[] = { + // .wrap_target + 0x00c7, // 0: jmp pin, 7 + 0xe02a, // 1: set x, 10 + 0x2021, // 2: wait 0 pin, 1 + 0x4001, // 3: in pins, 1 + 0x20a1, // 4: wait 1 pin, 1 + 0x0042, // 5: jmp x--, 2 + 0x0000, // 6: jmp 0 + 0x00e9, // 7: jmp !osre, 9 + 0x0000, // 8: jmp 0 + 0xff81, // 9: set pindirs, 1 [31] + 0xe280, // 10: set pindirs, 0 [2] + 0xe082, // 11: set pindirs, 2 + 0x2021, // 12: wait 0 pin, 1 + 0xe029, // 13: set x, 9 + 0x6081, // 14: out pindirs, 1 + 0x20a1, // 15: wait 1 pin, 1 + 0x2021, // 16: wait 0 pin, 1 + 0x004e, // 17: jmp x--, 14 + 0xe083, // 18: set pindirs, 3 + 0x2021, // 19: wait 0 pin, 1 + 0x20a1, // 20: wait 1 pin, 1 + // .wrap +}; +// clang-format on + +static const struct pio_program ps2_program = { + .instructions = ps2_program_instructions, + .length = 21, + .origin = -1, +}; + +static int state_machine = -1; +static thread_reference_t tx_thread = NULL; + +#define BUFFER_SIZE 32 +static input_buffers_queue_t pio_rx_queue; +static __attribute__((aligned(4))) uint8_t pio_rx_buffer[BQ_BUFFER_SIZE(BUFFER_SIZE, sizeof(uint32_t))]; + +uint8_t ps2_error = PS2_ERR_NONE; + +void pio_serve_interrupt(void) { + uint32_t irqs = pio->ints0; + + if (irqs & (PIO_IRQ0_INTF_SM0_RXNEMPTY_BITS << state_machine)) { + osalSysLockFromISR(); + uint32_t* frame_buffer = (uint32_t*)ibqGetEmptyBufferI(&pio_rx_queue); + if (frame_buffer == NULL) { + osalSysUnlockFromISR(); + return; + } + *frame_buffer = pio_sm_get(pio, state_machine); + ibqPostFullBufferI(&pio_rx_queue, sizeof(uint32_t)); + osalSysUnlockFromISR(); + } + + if (irqs & (PIO_IRQ0_INTF_SM0_TXNFULL_BITS << state_machine)) { + pio_set_irq0_source_enabled(pio, pis_sm0_tx_fifo_not_full + state_machine, false); + osalSysLockFromISR(); + osalThreadResumeI(&tx_thread, MSG_OK); + osalSysUnlockFromISR(); + } +} + +void ps2_host_init(void) { + ibqObjectInit(&pio_rx_queue, false, pio_rx_buffer, sizeof(uint32_t), BUFFER_SIZE, NULL, NULL); + uint pio_idx = pio_get_index(pio); + + hal_lld_peripheral_unreset(pio_idx == 0 ? RESETS_ALLREG_PIO0 : RESETS_ALLREG_PIO1); + + state_machine = pio_claim_unused_sm(pio, true); + if (state_machine < 0) { + dprintln("ERROR: Failed to acquire state machine for PS/2!"); + ps2_error = PS2_ERR_NODATA; + return; + } + + uint offset = pio_add_program(pio, &ps2_program); + + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + PS2_WRAP_TARGET, offset + PS2_WRAP); + + // Set pindirs to input (output enable is inverted below) + pio_sm_set_consecutive_pindirs(pio, state_machine, PS2_DATA_PIN, 2, true); + sm_config_set_clkdiv(&c, (float)clock_get_hz(clk_sys) / (200.0f * KHZ)); + sm_config_set_set_pins(&c, PS2_DATA_PIN, 2); + sm_config_set_out_pins(&c, PS2_DATA_PIN, 1); + sm_config_set_out_shift(&c, true, true, 10); + sm_config_set_in_shift(&c, true, true, 11); + sm_config_set_jmp_pin(&c, PS2_CLOCK_PIN); + sm_config_set_in_pins(&c, PS2_DATA_PIN); + + // clang-format off + iomode_t pin_mode = PAL_RP_PAD_IE | + PAL_RP_GPIO_OE | + PAL_RP_PAD_SLEWFAST | + PAL_RP_PAD_DRIVE12 | + // Invert output enable so that pindirs=1 means input + // and indirs=0 means output. This way, out pindirs + // works correctly with the open-drain PS/2 interface. + // Setting pindirs=1 effectively pulls the line high, + // due to the pull-up resistor, while pindirs=0 pulls + // the line low. + PAL_RP_IOCTRL_OEOVER_DRVINVPERI | + (pio_idx == 0 ? PAL_MODE_ALTERNATE_PIO0 : PAL_MODE_ALTERNATE_PIO1); + // clang-format on + + palSetLineMode(PS2_DATA_PIN, pin_mode); + palSetLineMode(PS2_CLOCK_PIN, pin_mode); + + pio_set_irq0_source_enabled(pio, pis_sm0_rx_fifo_not_empty + state_machine, true); + pio_sm_init(pio, state_machine, offset, &c); + +#if defined(PS2_PIO_USE_PIO1) + nvicEnableVector(RP_PIO1_IRQ_0_NUMBER, CORTEX_MAX_KERNEL_PRIORITY); +#else + nvicEnableVector(RP_PIO0_IRQ_0_NUMBER, CORTEX_MAX_KERNEL_PRIORITY); +#endif + + pio_sm_set_enabled(pio, state_machine, true); +} + +static int bit_parity(int x) { + return !__builtin_parity(x); +} + +uint8_t ps2_host_send(uint8_t data) { + uint32_t frame = 0b1000000000; + frame = frame | data; + + if (bit_parity(data)) { + frame = frame | (1 << 8); + } + + pio_sm_put(pio, state_machine, frame); + + msg_t msg = MSG_OK; + osalSysLock(); + while (pio_sm_is_tx_fifo_full(pio, state_machine)) { + pio_set_irq0_source_enabled(pio, pis_sm0_tx_fifo_not_full + state_machine, true); + msg = osalThreadSuspendTimeoutS(&tx_thread, TIME_MS2I(100)); + if (msg < MSG_OK) { + pio_set_irq0_source_enabled(pio, pis_sm0_tx_fifo_not_full + state_machine, false); + ps2_error = PS2_ERR_NODATA; + osalSysUnlock(); + return 0; + } + } + osalSysUnlock(); + + return ps2_host_recv_response(); +} + +static uint8_t ps2_get_data_from_frame(uint32_t frame) { + uint8_t data = (frame >> 22) & 0xFF; + uint32_t start_bit = (frame & 0b00000000001000000000000000000000) ? 1 : 0; + uint32_t parity_bit = (frame & 0b01000000000000000000000000000000) ? 1 : 0; + uint32_t stop_bit = (frame & 0b10000000001000000000000000000000) ? 1 : 0; + + if (start_bit != 0) { + ps2_error = PS2_ERR_STARTBIT1; + return 0; + } + + if (parity_bit != bit_parity(data)) { + ps2_error = PS2_ERR_PARITY; + return 0; + } + + if (stop_bit != 1) { + ps2_error = PS2_ERR_STARTBIT2; + return 0; + } + + return data; +} + +uint8_t ps2_host_recv_response(void) { + uint32_t frame = 0; + msg_t msg = MSG_OK; + + msg = ibqReadTimeout(&pio_rx_queue, (uint8_t*)&frame, sizeof(uint32_t), TIME_MS2I(100)); + if (msg < MSG_OK) { + ps2_error = PS2_ERR_NODATA; + return 0; + } + + return ps2_get_data_from_frame(frame); +} + +#ifdef BUFFERED_MODE_ENABLE + +bool pbuf_has_data(void) { + osalSysLock(); + bool has_data = !ibqIsEmptyI(&pio_rx_queue); + osalSysUnlock(); + return has_data; +} + +uint8_t ps2_host_recv(void) { + uint32_t frame = 0; + msg_t msg = MSG_OK; + + uint8_t has_data = pbuf_has_data(); + if (has_data) { + msg = ibqReadTimeout(&pio_rx_queue, (uint8_t*)&frame, sizeof(uint32_t), TIME_MS2I(100)); + if (msg < MSG_OK) { + ps2_error = PS2_ERR_NODATA; + return 0; + } + } else { + ps2_error = PS2_ERR_NODATA; + } + + return frame != 0 ? ps2_get_data_from_frame(frame) : 0; +} + +#endif diff --git a/platforms/chibios/drivers/vendor/RP/RP2040/ws2812_vendor.c b/platforms/chibios/drivers/vendor/RP/RP2040/ws2812_vendor.c index a46b099195..99a6cfaba9 100644 --- a/platforms/chibios/drivers/vendor/RP/RP2040/ws2812_vendor.c +++ b/platforms/chibios/drivers/vendor/RP/RP2040/ws2812_vendor.c @@ -185,7 +185,7 @@ bool ws2812_init(void) { (pio_idx == 0 ? PAL_MODE_ALTERNATE_PIO0 : PAL_MODE_ALTERNATE_PIO1); // clang-format on - palSetLineMode(RGB_DI_PIN, rgb_pin_mode); + palSetLineMode(WS2812_DI_PIN, rgb_pin_mode); STATE_MACHINE = pio_claim_unused_sm(pio, true); if (STATE_MACHINE < 0) { @@ -195,11 +195,11 @@ bool ws2812_init(void) { uint offset = pio_add_program(pio, &ws2812_program); - pio_sm_set_consecutive_pindirs(pio, STATE_MACHINE, RGB_DI_PIN, 1, true); + pio_sm_set_consecutive_pindirs(pio, STATE_MACHINE, WS2812_DI_PIN, 1, true); pio_sm_config config = pio_get_default_sm_config(); sm_config_set_wrap(&config, offset + WS2812_WRAP_TARGET, offset + WS2812_WRAP); - sm_config_set_sideset_pins(&config, RGB_DI_PIN); + sm_config_set_sideset_pins(&config, WS2812_DI_PIN); sm_config_set_fifo_join(&config, PIO_FIFO_JOIN_TX); #if defined(WS2812_EXTERNAL_PULLUP) diff --git a/platforms/chibios/drivers/ws2812.c b/platforms/chibios/drivers/ws2812_bitbang.c index 55ac333b1e..d05deb1a50 100644 --- a/platforms/chibios/drivers/ws2812.c +++ b/platforms/chibios/drivers/ws2812_bitbang.c @@ -53,22 +53,22 @@ void sendByte(uint8_t byte) { // using something like wait_ns(is_one ? T1L : T0L) here throws off timings if (is_one) { // 1 - writePinHigh(RGB_DI_PIN); + writePinHigh(WS2812_DI_PIN); wait_ns(WS2812_T1H); - writePinLow(RGB_DI_PIN); + writePinLow(WS2812_DI_PIN); wait_ns(WS2812_T1L); } else { // 0 - writePinHigh(RGB_DI_PIN); + writePinHigh(WS2812_DI_PIN); wait_ns(WS2812_T0H); - writePinLow(RGB_DI_PIN); + writePinLow(WS2812_DI_PIN); wait_ns(WS2812_T0L); } } } void ws2812_init(void) { - palSetLineMode(RGB_DI_PIN, WS2812_OUTPUT_MODE); + palSetLineMode(WS2812_DI_PIN, WS2812_OUTPUT_MODE); } // Setleds for standard RGB diff --git a/platforms/chibios/drivers/ws2812_pwm.c b/platforms/chibios/drivers/ws2812_pwm.c index c4a591c10b..04c8279a97 100644 --- a/platforms/chibios/drivers/ws2812_pwm.c +++ b/platforms/chibios/drivers/ws2812_pwm.c @@ -308,7 +308,7 @@ void ws2812_init(void) { for (i = 0; i < WS2812_RESET_BIT_N; i++) ws2812_frame_buffer[i + WS2812_COLOR_BIT_N] = 0; // All reset bits are zero - palSetLineMode(RGB_DI_PIN, WS2812_OUTPUT_MODE); + palSetLineMode(WS2812_DI_PIN, WS2812_OUTPUT_MODE); // PWM Configuration //#pragma GCC diagnostic ignored "-Woverride-init" // Turn off override-init warning for this struct. We use the overriding ability to set a "default" channel config diff --git a/platforms/chibios/drivers/ws2812_spi.c b/platforms/chibios/drivers/ws2812_spi.c index 03ffbd7f82..c28f5007f1 100644 --- a/platforms/chibios/drivers/ws2812_spi.c +++ b/platforms/chibios/drivers/ws2812_spi.c @@ -136,7 +136,7 @@ static void set_led_color_rgb(LED_TYPE color, int pos) { } void ws2812_init(void) { - palSetLineMode(RGB_DI_PIN, WS2812_MOSI_OUTPUT_MODE); + palSetLineMode(WS2812_DI_PIN, WS2812_MOSI_OUTPUT_MODE); #ifdef WS2812_SPI_SCK_PIN palSetLineMode(WS2812_SPI_SCK_PIN, WS2812_SCK_OUTPUT_MODE); @@ -150,8 +150,8 @@ void ws2812_init(void) { WS2812_SPI_BUFFER_MODE, # endif NULL, // end_cb - PAL_PORT(RGB_DI_PIN), - PAL_PAD(RGB_DI_PIN), + PAL_PORT(WS2812_DI_PIN), + PAL_PAD(WS2812_DI_PIN), # if defined(WB32F3G71xx) || defined(WB32FQ95xx) 0, 0, @@ -170,8 +170,8 @@ void ws2812_init(void) { # endif NULL, // data_cb NULL, // error_cb - PAL_PORT(RGB_DI_PIN), - PAL_PAD(RGB_DI_PIN), + PAL_PORT(WS2812_DI_PIN), + PAL_PAD(WS2812_DI_PIN), |