From 22738b649272c012b41a8338e1c99da9ba12a20c Mon Sep 17 00:00:00 2001 From: Matthew Selby Date: Tue, 13 May 2025 09:52:45 +0100 Subject: [PATCH 01/11] Add SYS_INCLUDE_DIRS to top level Makefile. Add directories in $(SYS_INCLUDE_DIRS) to the search path via -isystem. This allows a workaround for https://github.com/raspberrypi/pico-sdk/issues/2451 by using system headers, which are more tolerant of macro redefinitions. --- Makefile | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Makefile b/Makefile index 456dc49dbc..7be4e6a6d2 100644 --- a/Makefile +++ b/Makefile @@ -305,6 +305,7 @@ EXTRA_WARNING_FLAGS := -Wold-style-definition CFLAGS += $(ARCH_FLAGS) \ $(addprefix -D,$(OPTIONS)) \ $(addprefix -I,$(INCLUDE_DIRS)) \ + $(addprefix -isystem,$(SYS_INCLUDE_DIRS)) \ $(DEBUG_FLAGS) \ -std=gnu17 \ -Wall -Wextra -Werror -Wunsafe-loop-optimizations -Wdouble-promotion \ @@ -331,6 +332,7 @@ ASFLAGS = $(ARCH_FLAGS) \ $(DEBUG_FLAGS) \ -x assembler-with-cpp \ $(addprefix -I,$(INCLUDE_DIRS)) \ + $(addprefix -isystem,$(SYS_INCLUDE_DIRS)) \ -MMD -MP ifeq ($(LD_FLAGS),) @@ -359,6 +361,7 @@ endif CPPCHECK = cppcheck $(CSOURCES) --enable=all --platform=unix64 \ --std=c99 --inline-suppr --quiet --force \ $(addprefix -I,$(INCLUDE_DIRS)) \ + $(addprefix -isystem,$(SYS_INCLUDE_DIRS)) \ -I/usr/include -I/usr/include/linux TARGET_NAME := $(TARGET) From c46c3825a08982f48d9a5d0c5e1f8f349f3ed008 Mon Sep 17 00:00:00 2001 From: Matthew Selby Date: Tue, 13 May 2025 10:03:04 +0100 Subject: [PATCH 02/11] PICO: Linker script and flash config updates. Linker script based on rp2350/memmap_default.ld from pico-sdk. Fix to flash config (erase on sector boundaries). --- src/platform/PICO/config_flash.c | 13 +- src/platform/PICO/link/pico_rp2350.ld | 500 +++++++++++++++----------- 2 files changed, 301 insertions(+), 212 deletions(-) diff --git a/src/platform/PICO/config_flash.c b/src/platform/PICO/config_flash.c index 78ab2563d2..99ac0bc040 100644 --- a/src/platform/PICO/config_flash.c +++ b/src/platform/PICO/config_flash.c @@ -47,19 +47,22 @@ void configClearFlags(void) configStreamerResult_e configWriteWord(uintptr_t address, config_streamer_buffer_type_t *buffer) { + // TODO: synchronise second core... see e.g. pico-examples flash_program, uses flash_safe_execute. + + // pico-sdk flash_range functions use the offset from start of FLASH + uint32_t flash_offs = address - XIP_BASE; + uint32_t interrupts = save_and_disable_interrupts(); - if (address == __config_start) { + if ((flash_offs % FLASH_SECTOR_SIZE) == 0) { // Erase the flash sector before writing - flash_range_erase(address, FLASH_PAGE_SIZE); + flash_range_erase(flash_offs, FLASH_SECTOR_SIZE); } STATIC_ASSERT(CONFIG_STREAMER_BUFFER_SIZE == sizeof(config_streamer_buffer_type_t) * CONFIG_STREAMER_BUFFER_SIZE, "CONFIG_STREAMER_BUFFER_SIZE does not match written size"); // Write data to flash - // TODO: synchronise second core... - - flash_range_program(address, buffer, CONFIG_STREAMER_BUFFER_SIZE); + flash_range_program(flash_offs, buffer, CONFIG_STREAMER_BUFFER_SIZE); restore_interrupts(interrupts); return CONFIG_RESULT_SUCCESS; diff --git a/src/platform/PICO/link/pico_rp2350.ld b/src/platform/PICO/link/pico_rp2350.ld index a645ded915..27babd6e94 100644 --- a/src/platform/PICO/link/pico_rp2350.ld +++ b/src/platform/PICO/link/pico_rp2350.ld @@ -1,243 +1,329 @@ -/* Specify the memory areas */ +/* Based on GCC ARM embedded samples. + Defines the following symbols for use by code: + __exidx_start + __exidx_end + __etext + __data_start__ + __preinit_array_start + __preinit_array_end + __init_array_start + __init_array_end + __fini_array_start + __fini_array_end + __data_end__ + __bss_start__ + __bss_end__ + __end__ + end + __HeapLimit + __StackLimit + __StackTop + __stack (== StackTop) +*/ + MEMORY { - FLASH_X (rx) : ORIGIN = 0x10000000, LENGTH = 16K - FLASH_CONFIG (r) : ORIGIN = 0x10004000, LENGTH = 16K - FLASH (rx) : ORIGIN = 0x10008000, LENGTH = 992K - RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 512K - SCRATCH_X(rwx) : ORIGIN = 0x20080000, LENGTH = 4k - SCRATCH_Y(rwx) : ORIGIN = 0x20081000, LENGTH = 4k + /* TODO: Assuming for now that target has >= 4MB boot flash */ + FLASH (rx) : ORIGIN = 0x10000000, LENGTH = 4032K + FLASH_CONFIG (r) : ORIGIN = 0x103F0000, LENGTH = 64K + RAM(rwx) : ORIGIN = 0x20000000, LENGTH = 512k + SCRATCH_X(rwx) : ORIGIN = 0x20080000, LENGTH = 4k + SCRATCH_Y(rwx) : ORIGIN = 0x20081000, LENGTH = 4k } -REGION_ALIAS("STACKRAM", RAM) -REGION_ALIAS("FASTRAM", RAM) -REGION_ALIAS("MOVABLE_FLASH", FLASH) +ENTRY(_entry_point) -/* Entry Point */ -ENTRY(Reset_Handler) - -/* Highest address of the user mode stack */ -_estack = ORIGIN(STACKRAM) + LENGTH(STACKRAM) - 8; /* Reserve 2 x 4bytes for info across reset */ - -/* Base address where the config is stored. */ -__config_start = ORIGIN(FLASH_CONFIG); -__config_end = ORIGIN(FLASH_CONFIG) + LENGTH(FLASH_CONFIG); - -/* Generate a link error if heap and stack don't fit into RAM */ -_Min_Heap_Size = 0; /* required amount of heap */ -_Min_Stack_Size = 0x800; /* required amount of stack */ - -/* Define output sections */ SECTIONS { - /* The startup code goes first into FLASH_X */ - .isr_vector : - { - . = ALIGN(512); - PROVIDE (isr_vector_table_base = .); - KEEP(*(.isr_vector)) /* Startup code */ - . = ALIGN(4); - } >FLASH_X + .flash_begin : { + __flash_binary_start = .; + } > FLASH - /* The program code and other data goes into FLASH */ - .text : - { - . = ALIGN(4); - *(.text) /* .text sections (code) */ - *(.text*) /* .text* sections (code) */ - *(.rodata) /* .rodata sections (constants, strings, etc.) */ - *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ - *(.glue_7) /* glue arm to thumb code */ - *(.glue_7t) /* glue thumb to arm code */ - *(.eh_frame) + /* The bootrom will enter the image at the point indicated in your + IMAGE_DEF, which is usually the reset handler of your vector table. - KEEP (*(.init)) - KEEP (*(.fini)) + The debugger will use the ELF entry point, which is the _entry_point + symbol, and in our case is *different from the bootrom's entry point.* + This is used to go back through the bootrom on debugger launches only, + to perform the same initial flash setup that would be performed on a + cold boot. + */ - . = ALIGN(4); - _etext = .; /* define a global symbols at end of code */ - } >FLASH + .text : { + __logical_binary_start = .; + KEEP (*(.vectors)) + KEEP (*(.binary_info_header)) + __binary_info_header_end = .; + KEEP (*(.embedded_block)) + __embedded_block_end = .; + KEEP (*(.reset)) + /* TODO revisit this now memset/memcpy/float in ROM */ + /* bit of a hack right now to exclude all floating point and time critical (e.g. memset, memcpy) code from + * FLASH ... we will include any thing excluded here in .data below by default */ + *(.init) + *libgcc.a:cmse_nonsecure_call.o + *(EXCLUDE_FILE(*libgcc.a: *libc.a:*lib_a-mem*.o *libm.a:) .text*) + *(.fini) + /* Pull all c'tors into .text */ + *crtbegin.o(.ctors) + *crtbegin?.o(.ctors) + *(EXCLUDE_FILE(*crtend?.o *crtend.o) .ctors) + *(SORT(.ctors.*)) + *(.ctors) + /* Followed by destructors */ + *crtbegin.o(.dtors) + *crtbegin?.o(.dtors) + *(EXCLUDE_FILE(*crtend?.o *crtend.o) .dtors) + *(SORT(.dtors.*)) + *(.dtors) - .ARM.extab : - { - *(.ARM.extab* .gnu.linkonce.armextab.*) - } >FLASH + . = ALIGN(4); + /* preinit data */ + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP(*(SORT(.preinit_array.*))) + KEEP(*(.preinit_array)) + PROVIDE_HIDDEN (__preinit_array_end = .); + + . = ALIGN(4); + /* init data */ + PROVIDE_HIDDEN (__init_array_start = .); + KEEP(*(SORT(.init_array.*))) + KEEP(*(.init_array)) + PROVIDE_HIDDEN (__init_array_end = .); + + . = ALIGN(4); + /* finit data */ + PROVIDE_HIDDEN (__fini_array_start = .); + *(SORT(.fini_array.*)) + *(.fini_array) + PROVIDE_HIDDEN (__fini_array_end = .); + + *(.eh_frame*) + . = ALIGN(4); + } > FLASH + + /* Note the boot2 section is optional, and should be discarded if there is + no reference to it *inside* the binary, as it is not called by the + bootrom. (The bootrom performs a simple best-effort XIP setup and + leaves it to the binary to do anything more sophisticated.) However + there is still a size limit of 256 bytes, to ensure the boot2 can be + stored in boot RAM. + + Really this is a "XIP setup function" -- the name boot2 is historic and + refers to its dual-purpose on RP2040, where it also handled vectoring + from the bootrom into the user image. + */ + + .boot2 : { + __boot2_start__ = .; + *(.boot2) + __boot2_end__ = .; + } > FLASH + + ASSERT(__boot2_end__ - __boot2_start__ <= 256, + "ERROR: Pico second stage bootloader must be no more than 256 bytes in size") + + .rodata : { + *(EXCLUDE_FILE(*libgcc.a: *libc.a:*lib_a-mem*.o *libm.a:) .rodata*) + *(.srodata*) + . = ALIGN(4); + *(SORT_BY_ALIGNMENT(SORT_BY_NAME(.flashdata*))) + . = ALIGN(4); + } > FLASH + + .ARM.extab : + { + *(.ARM.extab* .gnu.linkonce.armextab.*) + } > FLASH - .ARM : - { __exidx_start = .; - *(.ARM.exidx*) __exidx_end = .; - } >FLASH + .ARM.exidx : + { + *(.ARM.exidx* .gnu.linkonce.armexidx.*) + } > FLASH + __exidx_end = .; - .pg_registry : - { - PROVIDE_HIDDEN (__pg_registry_start = .); - KEEP (*(.pg_registry)) - KEEP (*(SORT(.pg_registry.*))) - PROVIDE_HIDDEN (__pg_registry_end = .); - } >FLASH - - .pg_resetdata : - { - PROVIDE_HIDDEN (__pg_resetdata_start = .); - KEEP (*(.pg_resetdata)) - PROVIDE_HIDDEN (__pg_resetdata_end = .); - } >FLASH - - /* used by the startup to initialize data */ - _sidata = LOADADDR(.data); - - /* Initialized data sections goes into RAM, load LMA copy after code */ - .data : - { + /* Machine inspectable binary information */ . = ALIGN(4); - _sdata = .; /* create a global symbol at data start */ - *(.data) /* .data sections */ - *(.data*) /* .data* sections */ - + __binary_info_start = .; + .binary_info : + { + KEEP(*(.binary_info.keep.*)) + *(.binary_info.*) + } > FLASH + __binary_info_end = .; . = ALIGN(4); - *(.after_data.*) - . = ALIGN(4); - /* preinit data */ - PROVIDE_HIDDEN (__mutex_array_start = .); - KEEP(*(SORT(.mutex_array.*))) - KEEP(*(.mutex_array)) - PROVIDE_HIDDEN (__mutex_array_end = .); - . = ALIGN(4); - _edata = .; /* define a global symbol at data end */ - } >RAM AT >FLASH + .ram_vector_table (NOLOAD): { + *(.ram_vector_table) + } > RAM - /* Uninitialized data section */ - . = ALIGN(4); - .bss (NOLOAD) : - { - /* This is used by the startup in order to initialize the .bss secion */ - _sbss = .; /* define a global symbol at bss start */ - __bss_start__ = _sbss; - *(.bss) - *(SORT_BY_ALIGNMENT(.bss*)) - *(COMMON) + .uninitialized_data (NOLOAD): { + . = ALIGN(4); + *(.uninitialized_data*) + } > RAM - . = ALIGN(4); - _ebss = .; /* define a global symbol at bss end */ - __bss_end__ = _ebss; - } >RAM + .data : { + __data_start__ = .; + *(vtable) - /* Uninitialized data section */ - . = ALIGN(4); - .sram2 (NOLOAD) : - { - /* This is used by the startup in order to initialize the .sram2 secion */ - _ssram2 = .; /* define a global symbol at sram2 start */ - __sram2_start__ = _ssram2; - *(.sram2) - *(SORT_BY_ALIGNMENT(.sram2*)) + *(.time_critical*) - . = ALIGN(4); - _esram2 = .; /* define a global symbol at sram2 end */ - __sram2_end__ = _esram2; - } >RAM + /* remaining .text and .rodata; i.e. stuff we exclude above because we want it in RAM */ + *(.text*) + . = ALIGN(4); + *(.rodata*) + . = ALIGN(4); - /* used during startup to initialized fastram_data */ - _sfastram_idata = LOADADDR(.fastram_data); + *(.data*) + *(.sdata*) - /* Initialized FAST_DATA section for unsuspecting developers */ - .fastram_data : - { - . = ALIGN(4); - _sfastram_data = .; /* create a global symbol at data start */ - *(.fastram_data) /* .data sections */ - *(.fastram_data*) /* .data* sections */ + . = ALIGN(4); + *(.after_data.*) + . = ALIGN(4); + /* preinit data */ + PROVIDE_HIDDEN (__mutex_array_start = .); + KEEP(*(SORT(.mutex_array.*))) + KEEP(*(.mutex_array)) + PROVIDE_HIDDEN (__mutex_array_end = .); - . = ALIGN(4); - _efastram_data = .; /* define a global symbol at data end */ - } >FASTRAM AT >FLASH + *(.jcr) + . = ALIGN(4); + } > RAM AT> FLASH - . = ALIGN(4); - .fastram_bss (NOLOAD) : - { - _sfastram_bss = .; - __fastram_bss_start__ = _sfastram_bss; - *(.fastram_bss) - *(SORT_BY_ALIGNMENT(.fastram_bss*)) + .tdata : { + . = ALIGN(4); + *(.tdata .tdata.* .gnu.linkonce.td.*) + /* All data end */ + __tdata_end = .; + } > RAM AT> FLASH + PROVIDE(__data_end__ = .); - . = ALIGN(4); - _efastram_bss = .; - __fastram_bss_end__ = _efastram_bss; - } >FASTRAM + /* __etext is (for backwards compatibility) the name of the .data init source pointer (...) */ + __etext = LOADADDR(.data); - /* used during startup to initialized dmaram_data */ - _sdmaram_idata = LOADADDR(.dmaram_data); + .tbss (NOLOAD) : { + . = ALIGN(4); + __bss_start__ = .; + __tls_base = .; + *(.tbss .tbss.* .gnu.linkonce.tb.*) + *(.tcommon) - . = ALIGN(32); - .dmaram_data : - { - PROVIDE(dmaram_start = .); - _sdmaram = .; - _dmaram_start__ = _sdmaram; - _sdmaram_data = .; /* create a global symbol at data start */ - *(.dmaram_data) /* .data sections */ - *(.dmaram_data*) /* .data* sections */ - . = ALIGN(32); - _edmaram_data = .; /* define a global symbol at data end */ - } >RAM AT >FLASH + __tls_end = .; + } > RAM - . = ALIGN(32); - .dmaram_bss (NOLOAD) : - { - _sdmaram_bss = .; - __dmaram_bss_start__ = _sdmaram_bss; - *(.dmaram_bss) - *(SORT_BY_ALIGNMENT(.dmaram_bss*)) - . = ALIGN(32); - _edmaram_bss = .; - __dmaram_bss_end__ = _edmaram_bss; - } >RAM + .bss (NOLOAD) : { + . = ALIGN(4); + __tbss_end = .; - . = ALIGN(32); - .DMA_RAM (NOLOAD) : - { - KEEP(*(.DMA_RAM)) - PROVIDE(dmaram_end = .); - _edmaram = .; - _dmaram_end__ = _edmaram; - } >RAM + *(SORT_BY_ALIGNMENT(SORT_BY_NAME(.bss*))) + *(COMMON) + PROVIDE(__global_pointer$ = . + 2K); + *(.sbss*) + . = ALIGN(4); + __bss_end__ = .; + } > RAM - .DMA_RW_AXI (NOLOAD) : - { - . = ALIGN(32); - PROVIDE(dmarwaxi_start = .); - _sdmarwaxi = .; - _dmarwaxi_start__ = _sdmarwaxi; - KEEP(*(.DMA_RW_AXI)) - PROVIDE(dmarwaxi_end = .); - _edmarwaxi = .; - _dmarwaxi_end__ = _edmarwaxi; - } >RAM + .heap (NOLOAD): + { + __end__ = .; + end = __end__; + KEEP(*(.heap*)) + } > RAM + /* historically on GCC sbrk was growing past __HeapLimit to __StackLimit, however + to be more compatible, we now set __HeapLimit explicitly to where the end of the heap is */ + __HeapLimit = ORIGIN(RAM) + LENGTH(RAM); - .persistent_data (NOLOAD) : - { - __persistent_data_start__ = .; - *(.persistent_data) - . = ALIGN(4); - __persistent_data_end__ = .; - } >RAM + /* Start and end symbols must be word-aligned */ + .scratch_x : { + __scratch_x_start__ = .; + *(.scratch_x.*) + . = ALIGN(4); + __scratch_x_end__ = .; + } > SCRATCH_X AT > FLASH + __scratch_x_source__ = LOADADDR(.scratch_x); + + .scratch_y : { + __scratch_y_start__ = .; + *(.scratch_y.*) + . = ALIGN(4); + __scratch_y_end__ = .; + } > SCRATCH_Y AT > FLASH + __scratch_y_source__ = LOADADDR(.scratch_y); + + /* .stack*_dummy section doesn't contains any symbols. It is only + * used for linker to calculate size of stack sections, and assign + * values to stack symbols later + * + * stack1 section may be empty/missing if platform_launch_core1 is not used */ + + /* by default we put core 0 stack at the end of scratch Y, so that if core 1 + * stack is not used then all of SCRATCH_X is free. + */ + .stack1_dummy (NOLOAD): + { + *(.stack1*) + } > SCRATCH_X + .stack_dummy (NOLOAD): + { + KEEP(*(.stack*)) + } > SCRATCH_Y + + /* keep embedded end block as final entry into FLASH + * - helps protect against partial/corrupt load into flash + */ + .flash_end : { + KEEP(*(.embedded_end_block*)) + PROVIDE(__flash_binary_end = .); + } > FLASH =0xaa - /* User_heap_stack section, used to check that there is enough RAM left */ - _heap_stack_end = ORIGIN(STACKRAM)+LENGTH(STACKRAM) - 8; /* 8 bytes to allow for alignment */ - _heap_stack_begin = _heap_stack_end - _Min_Stack_Size - _Min_Heap_Size; - . = _heap_stack_begin; - ._user_heap_stack : - { - . = ALIGN(4); - PROVIDE ( end = . ); - PROVIDE ( _end = . ); - . = . + _Min_Heap_Size; - . = . + _Min_Stack_Size; - . = ALIGN(4); - } >STACKRAM = 0xa5 + /* Base address where the config is stored. */ + __config_start = ORIGIN(FLASH_CONFIG); + __config_end = ORIGIN(FLASH_CONFIG) + LENGTH(FLASH_CONFIG); + + .pg_registry : + { + PROVIDE_HIDDEN (__pg_registry_start = .); + KEEP (*(.pg_registry)) + KEEP (*(SORT(.pg_registry.*))) + PROVIDE_HIDDEN (__pg_registry_end = .); + } >FLASH + + .pg_resetdata : + { + PROVIDE_HIDDEN (__pg_resetdata_start = .); + KEEP (*(.pg_resetdata)) + PROVIDE_HIDDEN (__pg_resetdata_end = .); + } >FLASH + + + /* stack limit is poorly named, but historically is maximum heap ptr */ + __StackLimit = ORIGIN(RAM) + LENGTH(RAM); + __StackOneTop = ORIGIN(SCRATCH_X) + LENGTH(SCRATCH_X); + __StackTop = ORIGIN(SCRATCH_Y) + LENGTH(SCRATCH_Y); + __StackOneBottom = __StackOneTop - SIZEOF(.stack1_dummy); + __StackBottom = __StackTop - SIZEOF(.stack_dummy); + PROVIDE(__stack = __StackTop); + + /* picolibc and LLVM */ + PROVIDE (__heap_start = __end__); + PROVIDE (__heap_end = __HeapLimit); + PROVIDE( __tls_align = MAX(ALIGNOF(.tdata), ALIGNOF(.tbss)) ); + PROVIDE( __tls_size_align = (__tls_size + __tls_align - 1) & ~(__tls_align - 1)); + PROVIDE( __arm32_tls_tcb_offset = MAX(8, __tls_align) ); + + /* llvm-libc */ + PROVIDE (_end = __end__); + PROVIDE (__llvm_libc_heap_limit = __HeapLimit); + + /* Check if data + heap + stack exceeds RAM limit */ + ASSERT(__StackLimit >= __HeapLimit, "region RAM overflowed") + + ASSERT( __binary_info_header_end - __logical_binary_start <= 1024, "Binary info must be in first 1024 bytes of the binary") + ASSERT( __embedded_block_end - __logical_binary_start <= 4096, "Embedded block must be in first 4096 bytes of the binary") + + /* todo assert on extra code */ /* Remove information from the standard libraries */ /DISCARD/ : @@ -247,5 +333,5 @@ SECTIONS libgcc.a ( * ) } - .ARM.attributes 0 : { *(.ARM.attributes) } } + From f07cb9cc27e3b7f8e5bbf522d504c0dbfd84a8e0 Mon Sep 17 00:00:00 2001 From: Matthew Selby Date: Tue, 13 May 2025 15:39:49 +0100 Subject: [PATCH 03/11] PICO: Build changes (makefiles, headers). PICO.mk updates, tidy-ups, allow for PICO_trace platform_mcu.h #include "pico.h" rather than messing around with addressmap.h (can do since including as system headers now). PICO/target/RP2350[AB]/target.h undef USE_MSP_DISPLAYPORT because won't compile debug when USE_MSP_DISPLAYPORT defined without USE_OSD. --- src/platform/PICO/mk/PICO.mk | 230 ++++++++++++++++++-- src/platform/PICO/mk/PICO_trace.mk | 4 + src/platform/PICO/pico_trace.c | 25 +++ src/platform/PICO/pico_trace.h | 5 + src/platform/PICO/platform_mcu.h | 27 ++- src/platform/PICO/target/RP2350A/target.h | 247 ++++++++++++++++++++++ src/platform/PICO/target/RP2350B/target.h | 34 ++- 7 files changed, 529 insertions(+), 43 deletions(-) create mode 100644 src/platform/PICO/mk/PICO_trace.mk create mode 100644 src/platform/PICO/pico_trace.c create mode 100644 src/platform/PICO/pico_trace.h create mode 100644 src/platform/PICO/target/RP2350A/target.h diff --git a/src/platform/PICO/mk/PICO.mk b/src/platform/PICO/mk/PICO.mk index 54cc1f12fb..cb7cfd9379 100644 --- a/src/platform/PICO/mk/PICO.mk +++ b/src/platform/PICO/mk/PICO.mk @@ -1,6 +1,24 @@ # # Raspberry PICO Make file include # +# The top level Makefile adds $(MCU_COMMON_SRC) and $(DEVICE_STDPERIPH_SRC) to SRC collection. +# + +PICO_TRACE = 1 + +PICO_LIB_OPTIMISATION := -O2 -fuse-linker-plugin -ffast-math -fmerge-all-constants + +# This file PICO.mk is $(TARGET_PLATFORM_DIR)/mk/$(TARGET_MCU_FAMILY).mk +PICO_MK_DIR = $(TARGET_PLATFORM_DIR)/mk + +ifneq ($(PICO_TRACE),) +include $(PICO_MK_DIR)/PICO_trace.mk +endif + +RP2350_TARGETS = RP2350A RP2350B +ifneq ($(filter $(TARGET_MCU), $(RP2350_TARGETS)),) +RP2350_TARGET = $(TARGET_MCU) +endif ifeq ($(DEBUG_HARDFAULTS),PICO) CFLAGS += -DDEBUG_HARDFAULTS @@ -13,16 +31,18 @@ CMSIS_DIR := $(SDK_DIR)/rp2_common/cmsis/stub/CMSIS #STDPERIPH STDPERIPH_DIR := $(SDK_DIR) -STDPERIPH_SRC := \ + +PICO_LIB_SRC = \ + rp2_common/pico_crt0/crt0.S \ rp2_common/hardware_sync_spin_lock/sync_spin_lock.c \ rp2_common/hardware_gpio/gpio.c \ - rp2_common/pico_stdio/stdio.c \ rp2_common/hardware_uart/uart.c \ rp2_common/hardware_irq/irq.c \ rp2_common/hardware_irq/irq_handler_chain.S \ rp2_common/hardware_timer/timer.c \ rp2_common/hardware_clocks/clocks.c \ rp2_common/hardware_pll/pll.c \ + rp2_common/hardware_dma/dma.c \ rp2_common/hardware_spi/spi.c \ rp2_common/hardware_i2c/i2c.c \ rp2_common/hardware_adc/adc.c \ @@ -36,7 +56,33 @@ STDPERIPH_SRC := \ common/pico_sync/lock_core.c \ common/hardware_claim/claim.c \ common/pico_sync/critical_section.c \ - rp2_common/hardware_sync/sync.c + rp2_common/hardware_sync/sync.c \ + rp2_common/pico_bootrom/bootrom.c \ + rp2_common/pico_runtime_init/runtime_init.c \ + rp2_common/pico_runtime_init/runtime_init_clocks.c \ + rp2_common/pico_runtime_init/runtime_init_stack_guard.c \ + rp2_common/pico_runtime/runtime.c \ + rp2_common/hardware_ticks/ticks.c \ + rp2_common/hardware_xosc/xosc.c \ + common/pico_sync/sem.c \ + common/pico_time/timeout_helper.c \ + common/pico_util/datetime.c \ + common/pico_util/pheap.c \ + common/pico_util/queue.c \ + rp2350/pico_platform/platform.c \ + rp2_common/pico_atomic/atomic.c \ + rp2_common/pico_bootrom/bootrom.c \ + rp2_common/pico_bootrom/bootrom_lock.c \ + rp2_common/pico_divider/divider_compiler.c \ + rp2_common/pico_double/double_math.c \ + rp2_common/pico_flash/flash.c \ + rp2_common/pico_float/float_math.c \ + rp2_common/hardware_divider/divider.c \ + rp2_common/hardware_vreg/vreg.c \ + rp2_common/pico_standard_binary_info/standard_binary_info.c \ + rp2_common/pico_clib_interface/newlib_interface.c \ + rp2_common/pico_malloc/malloc.c \ + rp2_common/pico_stdlib/stdlib.c TINY_USB_SRC_DIR = tinyUSB/src TINYUSB_SRC := \ @@ -48,26 +94,39 @@ TINYUSB_SRC := \ $(TINY_USB_SRC_DIR)/portable/raspberrypi/rp2040/dcd_rp2040.c \ $(TINY_USB_SRC_DIR)/portable/raspberrypi/rp2040/rp2040_usb.c +# TODO which of these do we need? +TINYUSB_SRC += \ + $(TINY_USB_SRC_DIR)/class/vendor/vendor_device.c \ + $(TINY_USB_SRC_DIR)/class/net/ecm_rndis_device.c \ + $(TINY_USB_SRC_DIR)/class/net/ncm_device.c \ + $(TINY_USB_SRC_DIR)/class/dfu/dfu_rt_device.c \ + $(TINY_USB_SRC_DIR)/class/dfu/dfu_device.c \ + $(TINY_USB_SRC_DIR)/class/msc/msc_device.c \ + $(TINY_USB_SRC_DIR)/class/midi/midi_device.c \ + $(TINY_USB_SRC_DIR)/class/video/video_device.c \ + $(TINY_USB_SRC_DIR)/class/hid/hid_device.c \ + $(TINY_USB_SRC_DIR)/class/usbtmc/usbtmc_device.c \ + $(TINY_USB_SRC_DIR)/class/audio/audio_device.c + + VPATH := $(VPATH):$(STDPERIPH_DIR) -DEVICE_STDPERIPH_SRC := \ - $(STDPERIPH_SRC) \ - $(TINYUSB_SRC) - -ifeq ($(TARGET_MCU),RP2350B) +ifdef RP2350_TARGET TARGET_MCU_LIB_LOWER = rp2350 TARGET_MCU_LIB_UPPER = RP2350 endif #CMSIS VPATH := $(VPATH):$(CMSIS_DIR)/Core/Include:$(CMSIS_DIR)/Device/$(TARGET_MCU_LIB_UPPER)/Include -CMSIS_SRC := \ +CMSIS_SRC := INCLUDE_DIRS += \ $(TARGET_PLATFORM_DIR) \ $(TARGET_PLATFORM_DIR)/include \ $(TARGET_PLATFORM_DIR)/usb \ - $(TARGET_PLATFORM_DIR)/startup \ + $(TARGET_PLATFORM_DIR)/startup + +SYS_INCLUDE_DIRS = \ $(SDK_DIR)/common/pico_bit_ops_headers/include \ $(SDK_DIR)/common/pico_base_headers/include \ $(SDK_DIR)/common/boot_picoboot_headers/include \ @@ -166,25 +225,139 @@ INCLUDE_DIRS += \ $(SDK_DIR)/$(TARGET_MCU_LIB_LOWER)/hardware_structs/include \ $(LIB_MAIN_DIR)/tinyUSB/src +SYS_INCLUDE_DIRS += \ + $(SDK_DIR)/rp2350/boot_stage2/include + #Flags ARCH_FLAGS = -mthumb -mcpu=cortex-m33 -march=armv8-m.main+fp+dsp -mcmse -DEVICE_FLAGS = +# Automatically treating constants as single-precision breaks pico-sdk (-Werror=double-promotion) +# We should go through BF code and explicitly declare constants as single rather than double as required, +# rather than relying on this flag. +# ARCH_FLAGS += -fsingle-precision-constant + +PICO_STDIO_USB_FLAGS = \ + -DLIB_PICO_PRINTF=1 \ + -DLIB_PICO_PRINTF_PICO=1 \ + -DLIB_PICO_STDIO=1 \ + -DLIB_PICO_STDIO_USB=1 \ + -DCFG_TUSB_DEBUG=0 \ + -DCFG_TUSB_MCU=OPT_MCU_RP2040 \ + -DCFG_TUSB_OS=OPT_OS_PICO \ + -DLIB_PICO_FIX_RP2040_USB_DEVICE_ENUMERATION=1 \ + -DPICO_RP2040_USB_DEVICE_UFRAME_FIX=1 \ + -DPICO_STDIO_USB_CONNECT_WAIT_TIMEOUT_MS=3000 \ + -DLIB_PICO_UNIQUEID=1 + +PICO_STDIO_LD_FLAGS = \ + -Wl,--wrap=sprintf \ + -Wl,--wrap=snprintf \ + -Wl,--wrap=vsnprintf \ + -Wl,--wrap=printf \ + -Wl,--wrap=vprintf \ + -Wl,--wrap=puts \ + -Wl,--wrap=putchar \ + -Wl,--wrap=getchar + +EXTRA_LD_FLAGS += $(PICO_STDIO_LD_FLAGS) $(PICO_TRACE_LD_FLAGS) + +ifdef RP2350_TARGET + +# Q. do we need LIB_BOOT_STAGE_2_HEADERS? +# TODO review LIB_PICO options +DEVICE_FLAGS += \ + -D$(RP2350_TARGET) \ + -DPICO_RP2350_A2_SUPPORTED=1 \ + -DLIB_BOOT_STAGE2_HEADERS=1 \ + -DLIB_PICO_ATOMIC=1 \ + -DLIB_PICO_BIT_OPS=1 \ + -DLIB_PICO_BIT_OPS_PICO=1 \ + -DLIB_PICO_CLIB_INTERFACE=1 \ + -DLIB_PICO_CRT0=1 \ + -DLIB_PICO_CXX_OPTIONS=1 \ + -DLIB_PICO_DIVIDER=1 \ + -DLIB_PICO_DIVIDER_COMPILER=1 \ + -DLIB_PICO_DOUBLE=1 \ + -DLIB_PICO_DOUBLE_PICO=1 \ + -DLIB_PICO_FLOAT=1 \ + -DLIB_PICO_FLOAT_PICO=1 \ + -DLIB_PICO_FLOAT_PICO_VFP=1 \ + -DLIB_PICO_INT64_OPS=1 \ + -DLIB_PICO_INT64_OPS_COMPILER=1 \ + -DLIB_PICO_MALLOC=1 \ + -DLIB_PICO_MEM_OPS=1 \ + -DLIB_PICO_MEM_OPS_COMPILER=1 \ + -DLIB_PICO_NEWLIB_INTERFACE=1 \ + -DLIB_PICO_PLATFORM=1 \ + -DLIB_PICO_PLATFORM_COMPILER=1 \ + -DLIB_PICO_PLATFORM_PANIC=1 \ + -DLIB_PICO_PLATFORM_SECTIONS=1 \ + -DLIB_PICO_PRINTF=1 \ + -DLIB_PICO_PRINTF_PICO=1 \ + -DLIB_PICO_RUNTIME=1 \ + -DLIB_PICO_RUNTIME_INIT=1 \ + -DLIB_PICO_STANDARD_BINARY_INFO=1 \ + -DLIB_PICO_STANDARD_LINK=1 \ + -DLIB_PICO_STDIO=1 \ + -DLIB_PICO_STDIO_UART=1 \ + -DLIB_PICO_STDLIB=1 \ + -DLIB_PICO_SYNC=1 \ + -DLIB_PICO_SYNC_CRITICAL_SECTION=1 \ + -DLIB_PICO_SYNC_MUTEX=1 \ + -DLIB_PICO_SYNC_SEM=1 \ + -DLIB_PICO_TIME=1 \ + -DLIB_PICO_TIME_ADAPTER=1 \ + -DLIB_PICO_UTIL=1 \ + -DPICO_32BIT=1 \ + -DPICO_BUILD=1 \ + -DPICO_COPY_TO_RAM=0 \ + -DPICO_CXX_ENABLE_EXCEPTIONS=0 \ + -DPICO_NO_FLASH=0 \ + -DPICO_NO_HARDWARE=0 \ + -DPICO_ON_DEVICE=1 \ + -DPICO_RP2350=1 \ + -DPICO_USE_BLOCKED_RAM=0 -ifeq ($(TARGET_MCU),RP2350B) -DEVICE_FLAGS += -DRP2350B -DLIB_BOOT_STAGE2_HEADERS=1 -DLIB_PICO_ATOMIC=1 -DLIB_PICO_BIT_OPS=1 -DLIB_PICO_BIT_OPS_PICO=1 -DLIB_PICO_CLIB_INTERFACE=1 -DLIB_PICO_CRT0=1 -DLIB_PICO_CXX_OPTIONS=1 -DLIB_PICO_DIVIDER=1 -DLIB_PICO_DIVIDER_COMPILER=1 -DLIB_PICO_DOUBLE=1 -DLIB_PICO_DOUBLE_PICO=1 -DLIB_PICO_FLOAT=1 -DLIB_PICO_FLOAT_PICO=1 -DLIB_PICO_FLOAT_PICO_VFP=1 -DLIB_PICO_INT64_OPS=1 -DLIB_PICO_INT64_OPS_COMPILER=1 -DLIB_PICO_MALLOC=1 -DLIB_PICO_MEM_OPS=1 -DLIB_PICO_MEM_OPS_COMPILER=1 -DLIB_PICO_NEWLIB_INTERFACE=1 -DLIB_PICO_PLATFORM=1 -DLIB_PICO_PLATFORM_COMPILER=1 -DLIB_PICO_PLATFORM_PANIC=1 -DLIB_PICO_PLATFORM_SECTIONS=1 -DLIB_PICO_PRINTF=1 -DLIB_PICO_PRINTF_PICO=1 -DLIB_PICO_RUNTIME=1 -DLIB_PICO_RUNTIME_INIT=1 -DLIB_PICO_STANDARD_BINARY_INFO=1 -DLIB_PICO_STANDARD_LINK=1 -DLIB_PICO_STDIO=1 -DLIB_PICO_STDIO_UART=1 -DLIB_PICO_STDLIB=1 -DLIB_PICO_SYNC=1 -DLIB_PICO_SYNC_CRITICAL_SECTION=1 -DLIB_PICO_SYNC_MUTEX=1 -DLIB_PICO_SYNC_SEM=1 -DLIB_PICO_TIME=1 -DLIB_PICO_TIME_ADAPTER=1 -DLIB_PICO_UTIL=1 -DPICO_32BIT=1 -DPICO_BUILD=1 -DPICO_COPY_TO_RAM=0 -DPICO_CXX_ENABLE_EXCEPTIONS=0 -DPICO_NO_FLASH=0 -DPICO_NO_HARDWARE=0 -DPICO_ON_DEVICE=1 -DPICO_RP2350=1 -DPICO_USE_BLOCKED_RAM=0 LD_SCRIPT = $(LINKER_DIR)/pico_rp2350.ld STARTUP_SRC = PICO/startup/bs2_default_padded_checksummed.S -MCU_FLASH_SIZE = 4096 + # Override the OPTIMISE_SPEED compiler setting to save flash space on these 512KB targets. # Performance is only slightly affected but around 50 kB of flash are saved. OPTIMISE_SPEED = -O2 -STDPERIPH_SRC += \ - common/RP2350/pico_platform/platform.c +# TODO tidy up - +# we might lose some/all of pico_stdio_*, pico_printf if not using PICO_TRACE +PICO_LIB_SRC += \ + rp2_common/pico_stdio/stdio.c \ + rp2_common/pico_printf/printf.c -MCU_SRC += \ - system_RP2350.c +ifneq ($(PICO_TRACE),) +PICO_LIB_SRC += \ + rp2_common/pico_stdio_uart/stdio_uart.c +endif + +PICO_STDIO_USB_SRC = \ + rp2_common/pico_stdio_usb/stdio_usb.c \ + rp2_common/pico_stdio_usb/reset_interface.c \ + rp2_common/pico_fix/rp2040_usb_device_enumeration/rp2040_usb_device_enumeration.c \ + rp2_common/pico_bit_ops/bit_ops_aeabi.S \ + rp2_common/pico_stdio_usb/stdio_usb_descriptors.c + +# TODO check +# rp2_common/pico_stdio_usb/stdio_usb_descriptors.c \ +# vs PICO/usb/usb_descriptors.c + +PICO_LIB_SRC += $(PICO_STDIO_USB_SRC) + +# Work around https://github.com/raspberrypi/pico-sdk/issues/2451 +# by using system headers, which are more tolerant of macro redefinitions. + +SYS_INCLUDE_DIRS += \ + $(SDK_DIR)/rp2_common/pico_fix/rp2040_usb_device_enumeration/include + +# TODO use system_RP2350.c instead of writing into PICO/system.c +# MCU_COMMON_SRC += \ +# system_RP2350.c else $(error Unknown MCU for Raspberry PICO target) @@ -192,6 +365,8 @@ endif DEVICE_FLAGS += -DHSE_VALUE=$(HSE_VALUE) -DPICO +DEVICE_FLAGS += $(PICO_STDIO_USB_FLAGS) + MCU_COMMON_SRC = \ drivers/accgyro/accgyro_mpu.c \ drivers/dshot_bitbang_decode.c \ @@ -213,7 +388,20 @@ MCU_COMMON_SRC = \ PICO/exti_pico.c \ PICO/config_flash.c \ PICO/serial_usb_vcp_pico.c \ - PICO/usb/usb_cdc.c \ - PICO/usb/usb_descriptors.c + PICO/usb/usb_cdc.c -DEVICE_FLAGS += +DEVICE_STDPERIPH_SRC := \ + $(PICO_LIB_SRC) \ + $(STDPERIPH_SRC) \ + $(TINYUSB_SRC) \ + $(PICO_TRACE_SRC) + +# Add a target-specific definition for PICO_LIB_TARGETS in order +# to remove -flto=auto for pico-sdk file compilation +# (to avoid problem of interaction with wrapping -Wl,wrap=...). +# Place this at the end because we require PICO_LIB_TARGETS to be expanded before setting the target + +PICO_LIB_OBJS = $(addsuffix .o, $(basename $(PICO_LIB_SRC))) +PICO_LIB_OBJS += $(addsuffix .o, $(basename $(PICO_TRACE_SRC))) +PICO_LIB_TARGETS := $(foreach pobj, $(PICO_LIB_OBJS), %/$(pobj)) +$(PICO_LIB_TARGETS): CC_DEFAULT_OPTIMISATION := $(PICO_LIB_OPTIMISATION) diff --git a/src/platform/PICO/mk/PICO_trace.mk b/src/platform/PICO/mk/PICO_trace.mk new file mode 100644 index 0000000000..41d0cb64a0 --- /dev/null +++ b/src/platform/PICO/mk/PICO_trace.mk @@ -0,0 +1,4 @@ +PICO_WRAPPED_FUNCTIONS = main +PICO_WRAPPED_FUNCTIONS += init initEEPROM isEEPROMVersionValid +PICO_TRACE_LD_FLAGS += $(foreach fn, $(PICO_WRAPPED_FUNCTIONS), -Wl,--wrap=$(fn)) +PICO_TRACE_SRC += PICO/pico_trace.c diff --git a/src/platform/PICO/pico_trace.c b/src/platform/PICO/pico_trace.c new file mode 100644 index 0000000000..49c5d6aa5f --- /dev/null +++ b/src/platform/PICO/pico_trace.c @@ -0,0 +1,25 @@ +#include "pico_trace.h" + +// Wrap main to insert the initialisation code. +extern int main(int argc, char * argv[]); +extern int REAL_FUNC(main)(int argc, char * argv[]); +int WRAPPER_FUNC(main)(int argc, char * argv[]) +{ + stdio_init_all(); + return REAL_FUNC(main)(argc, argv); +} + +#define TRACEvoidvoid(x) \ + extern void x(void);\ + extern void REAL_FUNC(x)(void); \ + void WRAPPER_FUNC(x)(void)\ + {\ + tprintf("*** enter " #x " ***");\ + REAL_FUNC(x)();\ + tprintf("*** exit " #x " ***");\ + } + +// remember to add to PICO_WRAPPED_FUNCTIONS in PICO_trace.mk +TRACEvoidvoid(init) +TRACEvoidvoid(initEEPROM) +TRACEvoidvoid(isEEPROMVersionValid) diff --git a/src/platform/PICO/pico_trace.h b/src/platform/PICO/pico_trace.h new file mode 100644 index 0000000000..78ac3799b8 --- /dev/null +++ b/src/platform/PICO/pico_trace.h @@ -0,0 +1,5 @@ +#include "pico/stdio.h" +#include "pico/platform/compiler.h" + +#define tprintf(fmt,...) do {stdio_printf(fmt, ## __VA_ARGS__); stdio_printf("\n"); } while (0) + diff --git a/src/platform/PICO/platform_mcu.h b/src/platform/PICO/platform_mcu.h index 4d386f58fd..578e151f70 100644 --- a/src/platform/PICO/platform_mcu.h +++ b/src/platform/PICO/platform_mcu.h @@ -21,7 +21,13 @@ #pragma once -#define _ADDRESSMAP_H +#include "RP2350.h" + +#include "pico.h" +#include "pico/stdlib.h" +#include "hardware/spi.h" +#include "hardware/dma.h" +#include "hardware/flash.h" #define NVIC_PriorityGroup_2 0x500 @@ -31,17 +37,6 @@ #define SPI_IO_AF_SDI_CFG 0 #define SPI_IO_CS_CFG 0 -// Register address offsets for atomic RMW aliases -#define REG_ALIAS_RW_BITS (_u(0x0) << _u(12)) -#define REG_ALIAS_XOR_BITS (_u(0x1) << _u(12)) -#define REG_ALIAS_SET_BITS (_u(0x2) << _u(12)) -#define REG_ALIAS_CLR_BITS (_u(0x3) << _u(12)) - -#include "RP2350.h" -#include "pico/stdlib.h" -#include "hardware/spi.h" -#include "hardware/dma.h" - #if defined(RP2350A) || defined(RP2350B) typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState; @@ -55,7 +50,7 @@ typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState; #define DMA_TypeDef void* #define DMA_InitTypeDef void* //#define DMA_Channel_TypeDef -#define SPI_TypeDef SPI0_Type + #define ADC_TypeDef void* #define USART_TypeDef uart_inst_t #define TIM_OCInitTypeDef void* @@ -70,9 +65,12 @@ typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState; //#define EXTI_InitTypeDef //#define IRQn_Type void* +// We have to use SPI0_Type (or void) because config will pass in SPI0, SPI1, +// which are defined in pico-sdk as SPI0_Type*. +// SPI_INST converts to the correct type for use in pico-sdk functions. +#define SPI_TypeDef SPI0_Type #define SPI_INST(spi) ((spi_inst_t *)(spi)) - #endif #define DMA_DATA_ZERO_INIT @@ -106,7 +104,6 @@ extern uint32_t systemUniqueId[3]; #define UART_TX_BUFFER_ATTRIBUTE #define UART_RX_BUFFER_ATTRIBUTE -#define MAX_SPI_PIN_SEL 4 #define SERIAL_TRAIT_PIN_CONFIG 1 #define xDMA_GetCurrDataCounter(dma_resource) (((dma_channel_hw_t *)(dma_resource))->transfer_count) diff --git a/src/platform/PICO/target/RP2350A/target.h b/src/platform/PICO/target/RP2350A/target.h new file mode 100644 index 0000000000..3c7df6607c --- /dev/null +++ b/src/platform/PICO/target/RP2350A/target.h @@ -0,0 +1,247 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight 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 this software. + * + * If not, see . + */ + +#pragma once + +#ifdef PICO_STDIO_TEST +void _bprintf(const char * format, ...); +//#define bprintf(fmt,...) _bprintf(fmt, ## __VA_ARGS__) +#define bprintf(fmt,...) do {stdio_printf(fmt, ## __VA_ARGS__); stdio_printf("\n"); } while (0) +//int __wrap_printf(const char * fmt, ...); +//#define bprintf __wrap_printf +#else +#define bprintf(fmt,...) +#endif + + +#ifndef RP2350A +#define RP2350A +#endif + +#ifndef TARGET_BOARD_IDENTIFIER +#define TARGET_BOARD_IDENTIFIER "235B" +// TODO 235A? +#endif + +#ifndef USBD_PRODUCT_STRING +#define USBD_PRODUCT_STRING "Betaflight RP2350B" +// TODO +#endif + +#define USE_IO +#define USE_UART0 +#define USE_UART1 + +#define USE_SPI +#define USE_SPI_DEVICE_0 +#define USE_SPI_DEVICE_1 + +// one of these ... +// #define USE_SPI_DMA_ENABLE_EARLY +#define USE_SPI_DMA_ENABLE_LATE + +#undef USE_SOFTSERIAL1 +#undef USE_SOFTSERIAL2 + +#define USE_VCP + +#undef USE_TRANSPONDER +#undef USE_FLASH +#undef USE_FLASH_CHIP +#undef USE_SDCARD + +#undef USE_TIMER +#undef USE_I2C +#undef USE_RCC +#undef USE_CLI +#undef USE_RX_PWM +#undef USE_RX_PPM +#undef USE_RX_SPI +#undef USE_RX_CC2500 +#undef USE_SERIAL_4WAY_BLHELI_INTERFACE +#undef USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#undef USE_SERIAL_4WAY_SK_BOOTLOADER +#undef USE_MULTI_GYRO +#undef USE_BARO + +#undef USE_RANGEFINDER_HCSR04 +#undef USE_CRSF +#undef USE_TELEMETRY_CRSF +#undef USE_RX_EXPRESSLRS +#undef USE_MAX7456 +#undef USE_MAG +#undef USE_MAG_HMC5883 +#undef USE_MAG_SPI_HMC5883 +#undef USE_VTX_RTC6705 +#undef USE_VTX_RTC6705_SOFTSPI +#undef USE_RX_SX1280 +#undef USE_SRXL +#undef USE_TELEMETRY +#undef USE_OSD +#undef USE_SPEKTRUM +#undef USE_SPEKTRUM_BIND +#undef USE_MSP +#undef USE_MSP_UART +#undef USE_MSP_DISPLAYPORT +#undef USE_GPS +#undef USE_GPS_UBLOX +#undef USE_GPS_MTK +#undef USE_GPS_NMEA +#undef USE_GPS_SERIAL +#undef USE_GPS_SONAR +#undef USE_GPS_UBLOX7 +#undef USE_GPS_UBLOX8 +#undef USE_GPS_RESCUE + +#undef USE_VTX +#undef USE_VTX_TRAMP +#undef USE_VTX_SMARTAUDIO +#undef USE_SPEKTRUM_VTX_CONTROL +#undef USE_VTX_COMMON +#undef USE_FLASH +#undef USE_FLASH_CHIP +#undef USE_FLASH_M25P16 +#undef USE_FLASH_W25N01G +#undef USE_FLASH_W25N02K +#undef USE_FLASH_W25M +#undef USE_FLASH_W25M512 +#undef USE_FLASH_W25M02G +#undef USE_FLASH_W25Q128FV +#undef USE_FLASH_PY25Q128HA +#undef USE_FLASH_W25Q64FV + +#undef USE_RPM_LIMIT + +#undef USE_SERVOS +#undef USE_LED_STRIP +#undef USE_OSD +#undef USE_OSD_SD +#undef USE_OSD_HD +#undef USE_OSD_QUICK_MENU +#undef USE_GPS +#undef USE_POSITION_HOLD + +//#define FLASH_PAGE_SIZE 0x1 +#define CONFIG_IN_FLASH + +// Pico flash writes are all aligned and in batches of FLASH_PAGE_SIZE (256) +#define FLASH_CONFIG_STREAMER_BUFFER_SIZE FLASH_PAGE_SIZE +#define FLASH_CONFIG_BUFFER_TYPE uint8_t + +/* to be moved to a config file once target if working + defaults as per Laurel board for now */ + +#define LED0_PIN P25 +#undef LED1_PIN +#undef LED2_PIN + +#define SPI0_SCK_PIN P2 +#define SPI0_SDI_PIN P4 +#define SPI0_SDO_PIN P3 + +#define SPI1_SCK_PIN P26 +#define SPI1_SDI_PIN P24 +#define SPI1_SDO_PIN P27 + +#define SDCARD_CS_PIN P25 +#define FLASH_CS_PIN P25 +#define MAX7456_SPI_CS_PIN P17 + +#define GYRO_1_CS_PIN P1 +#define GYRO_2_CS_PIN NONE + +#define MAX7456_SPI_INSTANCE SPI1 +#define SDCARD_SPI_INSTANCE SPI1 +#define GYRO_1_SPI_INSTANCE SPI0 + +#define USE_GYRO +#define USE_GYRO_SPI_ICM42688P +#define USE_ACC +#define USE_ACC_SPI_ICM42688P +//#define USE_FLASH +//#define USE_FLASH_W25Q128FV +//#define USE_MAX7456 + +/* + +SPI0_CS P1 +SPI0_SCLK P2 +SPI0_COPI P3 +SPI0_CIPO P4 +BUZZER P5 +LED0 P6 +LED1 P7 +UART1_TX P8 +UART1_RX P9 +I2C1_SDA P10 +I2C1_SCL P11 +UART0_TX P12 +UART0_RX P13 + +OSD_CS P17 + +UART2_TX P20 +UART2_RX P21 + +GYRO_INT P22 + +GYRO_CLK P23 + +SPI1_CIPO P24 +SPI1_CS P25 +SPI1_SCLK P26 +SPI1_COPI P27 + +MOTOR1 P28 +MOTOR2 P29 +MOTOR3 P30 +MOTOR4 P31 + +SPARE1 P32 +SPARE2 P33 + +UART3_TX P34 +UART3_RX P35 + +DVTX_SBUS_RX P36 +TELEM_RX P37 +LED_STRIP P38 +RGB_LED P39 + +VBAT_SENSE P40 +CURR_SENSE P41 +ADC_SPARE P42 + +I2C0_SDA P44 +I2C0_SCL P45 + +SPARE3 P47 + +*/ + +#define SPIDEV_COUNT 2 +#define MAX_SPI_PIN_SEL 4 + +#define UART_RX_BUFFER_SIZE 1024 +#define UART_TX_BUFFER_SIZE 1024 + +#define UARTHARDWARE_MAX_PINS 8 +#define UART_TRAIT_AF_PORT 1 diff --git a/src/platform/PICO/target/RP2350B/target.h b/src/platform/PICO/target/RP2350B/target.h index d28d57bd54..4b35bf6834 100644 --- a/src/platform/PICO/target/RP2350B/target.h +++ b/src/platform/PICO/target/RP2350B/target.h @@ -21,6 +21,17 @@ #pragma once +#ifdef PICO_STDIO_TEST +void _bprintf(const char * format, ...); +//#define bprintf(fmt,...) _bprintf(fmt, ## __VA_ARGS__) +#define bprintf(fmt,...) do {stdio_printf(fmt, ## __VA_ARGS__); stdio_printf("\n"); } while (0) +//int __wrap_printf(const char * fmt, ...); +//#define bprintf __wrap_printf +#else +#define bprintf(fmt,...) +#endif + + #ifndef RP2350B #define RP2350B #endif @@ -41,6 +52,10 @@ #define USE_SPI_DEVICE_0 #define USE_SPI_DEVICE_1 +// one of these ... +// #define USE_SPI_DMA_ENABLE_EARLY +#define USE_SPI_DMA_ENABLE_LATE + #undef USE_SOFTSERIAL1 #undef USE_SOFTSERIAL2 @@ -83,6 +98,7 @@ #undef USE_SPEKTRUM_BIND #undef USE_MSP #undef USE_MSP_UART +#undef USE_MSP_DISPLAYPORT #undef USE_GPS #undef USE_GPS_UBLOX #undef USE_GPS_MTK @@ -121,20 +137,22 @@ #undef USE_GPS #undef USE_POSITION_HOLD - //#define FLASH_PAGE_SIZE 0x1 #define CONFIG_IN_FLASH -#define FLASH_CONFIG_STREAMER_BUFFER_SIZE 256 +// Pico flash writes are all aligned and in batches of FLASH_PAGE_SIZE (256) +#define FLASH_CONFIG_STREAMER_BUFFER_SIZE FLASH_PAGE_SIZE #define FLASH_CONFIG_BUFFER_TYPE uint8_t -/* to be moved to a config file once target if working */ +/* to be moved to a config file once target if working + defaults as per Laurel board for now */ + #define LED0_PIN P6 #define LED1_PIN P7 -#define SPI0_SCK_PIN P5 -#define SPI0_SDI_PIN P6 -#define SPI0_SDO_PIN P7 +#define SPI0_SCK_PIN P2 +#define SPI0_SDI_PIN P4 +#define SPI0_SDO_PIN P3 #define SPI1_SCK_PIN P26 #define SPI1_SDI_PIN P24 @@ -217,8 +235,10 @@ SPARE3 P47 */ #define SPIDEV_COUNT 2 +#define MAX_SPI_PIN_SEL 6 + #define UART_RX_BUFFER_SIZE 1024 #define UART_TX_BUFFER_SIZE 1024 -#define UARTHARDWARE_MAX_PINS 4 +#define UARTHARDWARE_MAX_PINS 12 #define UART_TRAIT_AF_PORT 1 From da49a5e3da7759e48de5e92eb640db71638a28b3 Mon Sep 17 00:00:00 2001 From: Matthew Selby Date: Tue, 13 May 2025 15:43:32 +0100 Subject: [PATCH 04/11] Add MCU_TYPE_RP2350A (for Pico2 prototyping). --- src/main/build/build_config.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/build/build_config.h b/src/main/build/build_config.h index 4665627e47..22a59c65f3 100644 --- a/src/main/build/build_config.h +++ b/src/main/build/build_config.h @@ -63,6 +63,7 @@ typedef enum { MCU_TYPE_APM32F405, MCU_TYPE_APM32F407, MCU_TYPE_AT32F435M, + MCU_TYPE_RP2350A, MCU_TYPE_RP2350B, MCU_TYPE_COUNT, MCU_TYPE_UNKNOWN = 255, From 57bc7431b4be0ccfa5b7849b1a7b62198d8f4351 Mon Sep 17 00:00:00 2001 From: Matthew Selby Date: Tue, 13 May 2025 16:10:43 +0100 Subject: [PATCH 05/11] PICO updates to system.c, targets. system.c don't want .vectors section or various handler functions that are provided by pico-sdk crt0. Add target/RP2350A files (for Pico2 prototyping). --- src/platform/PICO/system.c | 62 ++++++++++++++++------ src/platform/PICO/target/RP2350A/.exclude | 0 src/platform/PICO/target/RP2350A/target.mk | 30 +++++++++++ src/platform/PICO/target/RP2350B/target.mk | 33 ++++++++++-- 4 files changed, 105 insertions(+), 20 deletions(-) create mode 100644 src/platform/PICO/target/RP2350A/.exclude create mode 100644 src/platform/PICO/target/RP2350A/target.mk diff --git a/src/platform/PICO/system.c b/src/platform/PICO/system.c index 0912512f68..b52afc76ea 100644 --- a/src/platform/PICO/system.c +++ b/src/platform/PICO/system.c @@ -42,12 +42,8 @@ void Default_Handler(void); // cycles per microsecond static uint32_t usTicks = 0; -void (* const vector_table[])() __attribute__((section(".vectors"))) = { - (void (*)())0x20000000, // Initial Stack Pointer - Reset_Handler, // Interrupt Handler for reset - Default_Handler, // Default handler for other interrupts -}; - +// SystemInit and SystemCoreClock variables/functions, +// as per pico-sdk rp2_common/cmsis/stub/CMSIS/Device/RP2350/Source/system_RP2350.c uint32_t SystemCoreClock; /* System Clock Frequency (Core Clock)*/ void SystemCoreClockUpdate (void) @@ -60,6 +56,26 @@ void __attribute__((constructor)) SystemInit (void) SystemCoreClockUpdate(); } + +///////////////////////////////////////////////// + +// TODO: check: don't define functions here provided by pico-sdk crt0 +// crt0.S defines the vector table in the .vectors section, with +// initial stack pointer at __StackTop (defined in linker script), +// and with reset handler pointing to code inside crt0.S +#if 0 + +void (* const vector_table[])() __attribute__((section(".vectors"))) = { + (void (*)())0x20000000, // Initial Stack Pointer + Reset_Handler, // Interrupt Handler for reset + Default_Handler, // Default handler for other interrupts +}; + +void Default_Handler(void) +{ + while (1); // Infinite loop on default handler +} + void Reset_Handler(void) { // Initialize data segments @@ -85,11 +101,17 @@ void Reset_Handler(void) main(0, 0); } -void Default_Handler(void) + +void __unhandled_user_irq(void) { - while (1); // Infinite loop on default handler + // TODO } +#endif + +///////////////////////////////////////////////////// + + void systemReset(void) { //TODO: implement @@ -131,17 +153,26 @@ uint32_t microsISR(void) return micros(); } +#define PICO_NON_BUSY_SLEEP void delayMicroseconds(uint32_t us) { +#ifdef PICO_NON_BUSY_SLEEP + sleep_us(us); +#else uint32_t now = micros(); while (micros() - now < us); +#endif } void delay(uint32_t ms) { +#ifdef PICO_NON_BUSY_SLEEP + sleep_ms(ms); +#else while (ms--) { delayMicroseconds(1000); } +#endif } uint32_t getCycleCounter(void) @@ -151,7 +182,10 @@ uint32_t getCycleCounter(void) uint32_t clockMicrosToCycles(uint32_t micros) { - return micros / usTicks; + if (!usTicks) { + usTicks = clock_get_hz(clk_sys) / 1000000; + } + return micros * usTicks; } static void indicate(uint8_t count, uint16_t duration) @@ -198,10 +232,6 @@ void failureMode(failureMode_e mode) #endif } -void __unhandled_user_irq(void) -{ - // TODO -} static void unusedPinInit(IO_t io) { @@ -218,10 +248,12 @@ void unusedPinsInit(void) const mcuTypeInfo_t *getMcuTypeInfo(void) { static const mcuTypeInfo_t info = { -#if defined(RP2350B) +#if defined(RP2350A) + .id = MCU_TYPE_RP2350A, .name = "RP2350A" +#elif defined(RP2350B) .id = MCU_TYPE_RP2350B, .name = "RP2350B" #else -#error MCU Type info not defined for STM (or clone) +#error MCU Type info not defined for PICO / variant #endif }; return &info; diff --git a/src/platform/PICO/target/RP2350A/.exclude b/src/platform/PICO/target/RP2350A/.exclude new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/platform/PICO/target/RP2350A/target.mk b/src/platform/PICO/target/RP2350A/target.mk new file mode 100644 index 0000000000..12afe2db30 --- /dev/null +++ b/src/platform/PICO/target/RP2350A/target.mk @@ -0,0 +1,30 @@ +TARGET_MCU := RP2350A +TARGET_MCU_FAMILY := PICO + +MCU_FLASH_SIZE = 4096 + +DEVICE_FLAGS += -DPICO_RP2350A=1 + +# For pico-sdk, define flash-related attributes +# 4194304 = 4 * 1024 * 1024 +DEVICE_FLAGS += \ + -DPICO_FLASH_SPI_CLKDIV=2 \ + -DPICO_FLASH_SIZE_BYTES=4194304 \ + -DPICO_BOOT_STAGE2_CHOOSE_W25Q080=1 + +# Define some default pins, so that we can debug/trace using pico_stdio (uart, usb) +# or run some pico-examples programs. +# These ones are suitable for a Pico2 board. +DEVICE_FLAGS += \ + -DPICO_DEFAULT_UART=0 \ + -DPICO_DEFAULT_UART_TX_PIN=0 \ + -DPICO_DEFAULT_UART_RX_PIN=1 \ + -DPICO_DEFAULT_LED_PIN=25 \ + -DPICO_DEFAULT_I2C=0 \ + -DPICO_DEFAULT_I2C_SDA_PIN=4 \ + -DPICO_DEFAULT_I2C_SCL_PIN=5 \ + -DPICO_DEFAULT_SPI=0 \ + -DPICO_DEFAULT_SPI_SCK_PIN=18 \ + -DPICO_DEFAULT_SPI_TX_PIN=19 \ + -DPICO_DEFAULT_SPI_RX_PIN=16 \ + -DPICO_DEFAULT_SPI_CSN_PIN=17 diff --git a/src/platform/PICO/target/RP2350B/target.mk b/src/platform/PICO/target/RP2350B/target.mk index 143d7738f9..6fc126af95 100644 --- a/src/platform/PICO/target/RP2350B/target.mk +++ b/src/platform/PICO/target/RP2350B/target.mk @@ -1,8 +1,31 @@ TARGET_MCU := RP2350B TARGET_MCU_FAMILY := PICO -DEVICE_FLAGS += \ - -DPICO_RP2350B=1 \ - -DPICO_RP2350_A2_SUPPORTED=1 \ - -DPICO_FLASH_SPI_CLKDIV=2 \ - -DPICO_BOOT_STAGE2_CHOOSE_W25Q080=1 +MCU_FLASH_SIZE = 8192 + +# In pico-sdk, PICO_RP2350A=0 means RP2350B family. +DEVICE_FLAGS += -DPICO_RP2350A=0 + +# For pico-sdk, define flash-related attributes +# 8388608 = 8 * 1024 * 1024 +DEVICE_FLAGS += \ + -DPICO_FLASH_SPI_CLKDIV=2 \ + -DPICO_FLASH_SIZE_BYTES=8388608 \ + -DPICO_BOOT_STAGE2_CHOOSE_W25Q080=1 + +# Define some default pins, so that we can debug/trace using pico_stdio (uart, usb) +# or run some pico-examples programs. +# These ones are suitable for a Laurel board, with UART1 for stdio. +DEVICE_FLAGS += \ + -DPICO_DEFAULT_UART=1 \ + -DPICO_DEFAULT_UART_TX_PIN=8 \ + -DPICO_DEFAULT_UART_RX_PIN=9 \ + -DPICO_DEFAULT_LED_PIN=6 \ + -DPICO_DEFAULT_I2C=0 \ + -DPICO_DEFAULT_I2C_SDA_PIN=44 \ + -DPICO_DEFAULT_I2C_SCL_PIN=45 \ + -DPICO_DEFAULT_SPI=0 \ + -DPICO_DEFAULT_SPI_SCK_PIN=2 \ + -DPICO_DEFAULT_SPI_TX_PIN=3 \ + -DPICO_DEFAULT_SPI_RX_PIN=4 \ + -DPICO_DEFAULT_SPI_CSN_PIN=1 From 1635dc806ee2ecb3b8e16b60c41ee5ace0ad4793 Mon Sep 17 00:00:00 2001 From: Matthew Selby Date: Tue, 13 May 2025 16:14:10 +0100 Subject: [PATCH 06/11] PICO pin updates. Add gpio_init call to IOConfigGPIO (now we can see LEDs flashing). Add ranges of available GPIO pins for UART0, UART1 on RP2350A and RP2350B. --- src/platform/PICO/io_def_generated.h | 5 +-- src/platform/PICO/io_pico.c | 4 ++- src/platform/PICO/serial_uart_pico.c | 46 ++++++++++++++++++++++++++++ 3 files changed, 52 insertions(+), 3 deletions(-) diff --git a/src/platform/PICO/io_def_generated.h b/src/platform/PICO/io_def_generated.h index 832196d9e4..02a5745c36 100644 --- a/src/platform/PICO/io_def_generated.h +++ b/src/platform/PICO/io_def_generated.h @@ -38,6 +38,7 @@ #undef DEFIO_TAG_MAKE #define DEFIO_TAG_MAKE(pin) ((ioTag_t)(((1) << DEFIO_PORT_BITSHIFT) | (pin))) +// GPIOID for potential different banks ("ports") of GPIO pins - for RP2350 there's only one #undef DEFIO_TAG_GPIOID #define DEFIO_TAG_GPIOID(tag) (((tag) >> DEFIO_PORT_BITSHIFT) - 1) @@ -225,9 +226,9 @@ // DEFIO_IO_USED_COUNT is number of io pins supported on target #if defined(RP2350A) -#define DEFIO_IO_USED_COUNT 30 +#define DEFIO_IO_USED_COUNT DEFIO_USED_COUNT #elif defined(RP2350B) -#define DEFIO_IO_USED_COUNT 48 +#define DEFIO_IO_USED_COUNT DEFIO_USED_COUNT #endif // DEFIO_PORT_USED_LIST - comma separated list of bitmask for all used ports. diff --git a/src/platform/PICO/io_pico.c b/src/platform/PICO/io_pico.c index bf7f71c267..8e7b8f3942 100644 --- a/src/platform/PICO/io_pico.c +++ b/src/platform/PICO/io_pico.c @@ -91,7 +91,9 @@ void IOConfigGPIO(IO_t io, ioConfig_t cfg) return; } - gpio_set_dir(IO_Pin(io), (cfg & 0x01)); + uint16_t ioPin = IO_Pin(io); + gpio_init(ioPin); + gpio_set_dir(ioPin, (cfg & 0x01)); } IO_t IOGetByTag(ioTag_t tag) diff --git a/src/platform/PICO/serial_uart_pico.c b/src/platform/PICO/serial_uart_pico.c index 8c7105e5e6..ed984f83c6 100644 --- a/src/platform/PICO/serial_uart_pico.c +++ b/src/platform/PICO/serial_uart_pico.c @@ -46,11 +46,35 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { .reg = uart0, .rxPins = { { DEFIO_TAG_E(P1) }, + { DEFIO_TAG_E(P3) }, + { DEFIO_TAG_E(P13) }, + { DEFIO_TAG_E(P15) }, { DEFIO_TAG_E(P17) }, + { DEFIO_TAG_E(P19) }, + { DEFIO_TAG_E(P29) }, +#ifdef RP2350B + { DEFIO_TAG_E(P31) }, + { DEFIO_TAG_E(P33) }, + { DEFIO_TAG_E(P35) }, + { DEFIO_TAG_E(P45) }, + { DEFIO_TAG_E(P47) }, +#endif }, .txPins = { { DEFIO_TAG_E(P0) }, + { DEFIO_TAG_E(P2) }, + { DEFIO_TAG_E(P12) }, + { DEFIO_TAG_E(P14) }, { DEFIO_TAG_E(P16) }, + { DEFIO_TAG_E(P18) }, + { DEFIO_TAG_E(P28) }, +#ifdef RP2350B + { DEFIO_TAG_E(P30) }, + { DEFIO_TAG_E(P32) }, + { DEFIO_TAG_E(P34) }, + { DEFIO_TAG_E(P44) }, + { DEFIO_TAG_E(P46) }, +#endif }, .irqn = UART0_IRQ, .txBuffer = uart0TxBuffer, @@ -66,13 +90,35 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { .reg = uart1, .rxPins = { { DEFIO_TAG_E(P5) }, + { DEFIO_TAG_E(P7) }, { DEFIO_TAG_E(P9) }, + { DEFIO_TAG_E(P11) }, + { DEFIO_TAG_E(P21) }, + { DEFIO_TAG_E(P23) }, { DEFIO_TAG_E(P25) }, + { DEFIO_TAG_E(P27) }, +#ifdef RP2350B + { DEFIO_TAG_E(P37) }, + { DEFIO_TAG_E(P39) }, + { DEFIO_TAG_E(P41) }, + { DEFIO_TAG_E(P43) }, +#endif }, .txPins = { { DEFIO_TAG_E(P4) }, + { DEFIO_TAG_E(P6) }, + { DEFIO_TAG_E(P8) }, + { DEFIO_TAG_E(P10) }, { DEFIO_TAG_E(P20) }, + { DEFIO_TAG_E(P22) }, { DEFIO_TAG_E(P24) }, + { DEFIO_TAG_E(P26) }, +#ifdef RP2350B + { DEFIO_TAG_E(P36) }, + { DEFIO_TAG_E(P38) }, + { DEFIO_TAG_E(P40) }, + { DEFIO_TAG_E(P42) }, +#endif }, .irqn = UART1_IRQ, .txBuffer = uart1TxBuffer, From 12ca97eacbc16f1433a1f65fee739102f82b735d Mon Sep 17 00:00:00 2001 From: Matthew Selby Date: Tue, 13 May 2025 18:20:49 +0100 Subject: [PATCH 07/11] PICO define away stdio_pico_stub.c. --- src/platform/PICO/stdio_pico_stub.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/platform/PICO/stdio_pico_stub.c b/src/platform/PICO/stdio_pico_stub.c index 41312f0e22..11fc1a5a6c 100644 --- a/src/platform/PICO/stdio_pico_stub.c +++ b/src/platform/PICO/stdio_pico_stub.c @@ -1,3 +1,5 @@ +#if 0 +TODO remove this in favour of rp2_common/pico_clib_interface/newlib_interface.c /* * This file is part of Betaflight. * @@ -82,3 +84,5 @@ int _write(int handle, char *buffer, int length) UNUSED(length); return -1; } + +#endif From 17ac3d98959846a1f43bfaba2c2781038cbc6c45 Mon Sep 17 00:00:00 2001 From: Matthew Selby Date: Tue, 13 May 2025 22:21:35 +0100 Subject: [PATCH 08/11] PICO: Allow compilation with DEBUG=GDB (add dshot methods). --- src/platform/PICO/dshot_pico.c | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/platform/PICO/dshot_pico.c b/src/platform/PICO/dshot_pico.c index 695f1323c6..61d84bab51 100644 --- a/src/platform/PICO/dshot_pico.c +++ b/src/platform/PICO/dshot_pico.c @@ -403,10 +403,26 @@ bool dshotPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig) return true; } +// TODO do we just undef USE_DSHOT_BITBANG ? bool isDshotBitbangActive(const motorDevConfig_t *motorDevConfig) { /* DSHOT BIT BANG not required for PICO */ UNUSED(motorDevConfig); return false; } + +bool dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig) +{ + // TODO: not required + UNUSED(device); + UNUSED(motorConfig); + return false; +} + +dshotBitbangStatus_e dshotBitbangGetStatus(void) +{ + // TODO: not required + return 0; +} + #endif // USE_DSHOT From de04ac1f119959a2d92fab214c90c623abec18c3 Mon Sep 17 00:00:00 2001 From: Matthew Selby Date: Tue, 13 May 2025 18:22:16 +0100 Subject: [PATCH 09/11] PICO bus_spi_pico changes - work in progress... Testing w/o DMA Fix SPI_SPEED_20MHZ Add GPIO pin ranges Initial implementation of spiInternalInitStream and spiSequenceStart (mode, speed set up, based on STM32/bus_spi_ll.c) spiCalculateDivider records numbers as calculated in pico-sdk spi_set_baudrate, for use later in spiCalculateClock. --- src/platform/PICO/bus_spi_pico.c | 235 +++++++++++++++++++++++++++++-- 1 file changed, 220 insertions(+), 15 deletions(-) diff --git a/src/platform/PICO/bus_spi_pico.c b/src/platform/PICO/bus_spi_pico.c index 9d6c075733..ce70cb2731 100644 --- a/src/platform/PICO/bus_spi_pico.c +++ b/src/platform/PICO/bus_spi_pico.c @@ -19,6 +19,13 @@ * If not, see . */ +/* + * Clock divider code based on pico-sdk/src/rp2_common/hardware_spi.c + * Copyright (c) 2020 Raspberry Pi (Trading) Ltd. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + #include #include #include @@ -26,6 +33,7 @@ #include "platform.h" #ifdef USE_SPI +#define TESTING_NO_DMA 1 #include "common/maths.h" #include "drivers/bus.h" @@ -43,29 +51,43 @@ #include "pg/bus_spi.h" -#define SPI_SPEED_20MHZ 2000000 +#define SPI_SPEED_20MHZ 20000000 +#define SPI_DATAWIDTH 8 +#define SPI_DMA_THRESHOLD 8 const spiHardware_t spiHardware[] = { -#ifdef RP2350B { .device = SPIDEV_0, .reg = SPI0, .sckPins = { + { DEFIO_TAG_E(P2) }, { DEFIO_TAG_E(P6) }, { DEFIO_TAG_E(P18) }, { DEFIO_TAG_E(P22) }, +#ifdef RP2350B + { DEFIO_TAG_E(P34) }, + { DEFIO_TAG_E(P38) }, +#endif }, .misoPins = { { DEFIO_TAG_E(P0) }, { DEFIO_TAG_E(P4) }, { DEFIO_TAG_E(P16) }, { DEFIO_TAG_E(P20) }, +#ifdef RP2350B + { DEFIO_TAG_E(P32) }, + { DEFIO_TAG_E(P36) }, +#endif }, .mosiPins = { { DEFIO_TAG_E(P3) }, { DEFIO_TAG_E(P7) }, { DEFIO_TAG_E(P19) }, { DEFIO_TAG_E(P23) }, +#ifdef RP2350B + { DEFIO_TAG_E(P35) }, + { DEFIO_TAG_E(P39) }, +#endif }, }, { @@ -74,17 +96,34 @@ const spiHardware_t spiHardware[] = { .sckPins = { { DEFIO_TAG_E(P10) }, { DEFIO_TAG_E(P14) }, + { DEFIO_TAG_E(P26) }, +#ifdef RP2350B + { DEFIO_TAG_E(P30) }, + { DEFIO_TAG_E(P42) }, + { DEFIO_TAG_E(P46) }, +#endif }, .misoPins = { + { DEFIO_TAG_E(P8) }, { DEFIO_TAG_E(P12) }, + { DEFIO_TAG_E(P24) }, { DEFIO_TAG_E(P28) }, +#ifdef RP2350B + { DEFIO_TAG_E(P40) }, + { DEFIO_TAG_E(P44) }, +#endif }, .mosiPins = { { DEFIO_TAG_E(P11) }, { DEFIO_TAG_E(P15) }, + { DEFIO_TAG_E(P27) }, +#ifdef RP2350B + { DEFIO_TAG_E(P31) }, + { DEFIO_TAG_E(P43) }, + { DEFIO_TAG_E(P47) }, +#endif }, }, -#endif }; void spiPinConfigure(const struct spiPinConfig_s *pConfig) @@ -118,7 +157,8 @@ void spiPinConfigure(const struct spiPinConfig_s *pConfig) } } -static spi_inst_t *getSpiInstanceByDevice(SPI0_Type *spi) +/* + static spi_inst_t *getSpiInstanceByDevice(SPI_TypeDef *spi) { if (spi == SPI0) { return spi0; @@ -127,28 +167,97 @@ static spi_inst_t *getSpiInstanceByDevice(SPI0_Type *spi) } return NULL; } +*/ + +static void spiSetClockFromSpeed(spi_inst_t *spi, uint16_t speed) +{ + uint32_t freq = spiCalculateClock(speed); + spi_set_baudrate(spi, freq); +} + +/* + +enum spi_cpha_t { SPI_CPHA_0 = 0, SPI_CPHA_1 = 1 } +Enumeration of SPI CPHA (clock phase) values. + +enum spi_cpol_t { SPI_CPOL_0 = 0, SPI_CPOL_1 = 1 } +Enumeration of SPI CPOL (clock polarity) values. + +enum spi_order_t { SPI_LSB_FIRST = 0, SPI_MSB_FIRST = 1 } +Enumeration of SPI bit-order values. + + +static void spi_set_format (spi_inst_t * spi, uint data_bits, spi_cpol_t cpol, spi_cpha_t cpha, __unused spi_order_t order) [inline], [static] + +Configure SPI. + +Configure how the SPI serialises and deserialises data on the wire + +Parameters + +spi +SPI instance specifier, either spi0 or spi1 + +data_bits +Number of data bits per transfer. Valid values 4..16. + +cpol +SSPCLKOUT polarity, applicable to Motorola SPI frame format only. + +cpha +SSPCLKOUT phase, applicable to Motorola SPI frame format only + +order +Must be SPI_MSB_FIRST, no other values supported on the PL022 + + +*/ + void spiInitDevice(SPIDevice device) { + // maybe here set getSpiInstanceByDevice(spi->dev) SPI device with + // settings like + // STM does + //SetRXFIFOThreshold ...QF (1/4 full presumably) + // Init -> full duplex, master, 8biut, baudrate, MSBfirst, no CRC, + // Clock = PolarityHigh, Phase_2Edge + + const spiDevice_t *spi = &spiDevice[device]; if (!spi->dev) { return; } - spi_init(getSpiInstanceByDevice(spi->dev), SPI_SPEED_20MHZ); + spi_init(SPI_INST(spi->dev), SPI_SPEED_20MHZ); gpio_set_function(IO_PINBYTAG(spi->miso), GPIO_FUNC_SPI); gpio_set_function(IO_PINBYTAG(spi->mosi), GPIO_FUNC_SPI); gpio_set_function(IO_PINBYTAG(spi->sck), GPIO_FUNC_SPI); } +void spiInitBusDMA(void) +{ + //TODO: implement + // if required to set up mappings of peripherals to DMA instances? + // can just start off with dma_claim_unused_channel in spiInternalInitStream? +} + void spiInternalResetStream(dmaChannelDescriptor_t *descriptor) { //TODO: implement UNUSED(descriptor); } +bool spiInternalReadWriteBufPolled(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len) +{ + // TODO optimise with 16-bit transfers as per stm bus_spi_ll code + int bytesProcessed = spi_write_read_blocking(SPI_INST(instance), txData, rxData, len); + return bytesProcessed == len; +} + +// Initialise DMA before first segment transfer void spiInternalInitStream(const extDevice_t *dev, bool preInit) { UNUSED(preInit); @@ -159,7 +268,7 @@ void spiInternalInitStream(const extDevice_t *dev, bool preInit) dev->bus->dmaTx->channel = dma_tx; dev->bus->dmaRx->channel = dma_rx; dev->bus->dmaTx->irqHandlerCallback = NULL; - dev->bus->dmaRx->irqHandlerCallback = spiInternalResetStream; + dev->bus->dmaRx->irqHandlerCallback = spiInternalResetStream; // TODO: implement - correct callback const spiDevice_t *spi = &spiDevice[spiDeviceByInstance(dev->bus->busType_u.spi.instance)]; dma_channel_config config = dma_channel_get_default_config(dma_tx); @@ -175,31 +284,127 @@ void spiInternalInitStream(const extDevice_t *dev, bool preInit) dma_channel_configure(dma_rx, &config, dev->rxBuf, &spi_get_hw(SPI_INST(spi->dev))->dr, 0, false); } +// Start DMA transfer for the current segment void spiInternalStartDMA(const extDevice_t *dev) { - dma_channel_set_trans_count(dev->bus->dmaTx->channel, dev->bus->curSegment->len + 1, false); - dma_channel_set_trans_count(dev->bus->dmaRx->channel, dev->bus->curSegment->len + 1, false); + // TODO check correct, was len + 1 now len + dma_channel_set_trans_count(dev->bus->dmaTx->channel, dev->bus->curSegment->len, false); + dma_channel_set_trans_count(dev->bus->dmaRx->channel, dev->bus->curSegment->len, false); dma_channel_start(dev->bus->dmaTx->channel); dma_channel_start(dev->bus->dmaRx->channel); } - + // DMA transfer setup and start void spiSequenceStart(const extDevice_t *dev) { //TODO: implementation for PICO - UNUSED(dev); + // base on STM32/bus_spi_ll.c + + busDevice_t *bus = dev->bus; + SPI_TypeDef *instance = bus->busType_u.spi.instance; + spiDevice_t *spi = &spiDevice[spiDeviceByInstance(instance)]; + bool dmaSafe = dev->useDMA; +#if TESTING_NO_DMA + dmaSafe = false; +#endif + uint32_t xferLen = 0; + uint32_t segmentCount = 0; + + // + bus->initSegment = true; + + + // Switch bus speed + if (dev->busType_u.spi.speed != bus->busType_u.spi.speed) { + spiSetClockFromSpeed(SPI_INST(instance), dev->busType_u.spi.speed); + bus->busType_u.spi.speed = dev->busType_u.spi.speed; + } + + // Switch SPI clock polarity/phase if necessary + if (dev->busType_u.spi.leadingEdge != bus->busType_u.spi.leadingEdge) { + uint8_t sckPin = IO_PINBYTAG(spi->sck); + gpio_set_slew_rate(sckPin, GPIO_SLEW_RATE_FAST); + // Betaflight busDevice / SPI supports modes 0 (leadingEdge True), 3 (leadingEdge False) + // 0: (CPOL 0, CPHA 0) + // 3: (CPOL 1, CPHA 1) + if (dev->busType_u.spi.leadingEdge) { + spi_set_format(SPI_INST(instance), SPI_DATAWIDTH, SPI_CPOL_0, SPI_CPHA_0, SPI_MSB_FIRST); + } + else { + spi_set_format(SPI_INST(instance), SPI_DATAWIDTH, SPI_CPOL_1, SPI_CPHA_1, SPI_MSB_FIRST); + } + + bus->busType_u.spi.leadingEdge = dev->busType_u.spi.leadingEdge; + } + + // NB for RP2350 targets, heap and stack will be in SRAM (single-cycle), + // so there are no cache issues with DMA. + for (busSegment_t *checkSegment = (busSegment_t *)bus->curSegment; checkSegment->len; checkSegment++) { + segmentCount++; + xferLen += checkSegment->len; + } + + // Use DMA if possible + // If there are more than one segments, or a single segment with negateCS negated in the list terminator then force DMA irrespective of length + if (bus->useDMA && dmaSafe && ((segmentCount > 1) || + (xferLen >= SPI_DMA_THRESHOLD) || + !bus->curSegment[segmentCount].negateCS)) { + spiProcessSegmentsDMA(dev); + } else { + spiProcessSegmentsPolled(dev); + } } uint16_t spiCalculateDivider(uint32_t freq) { /* - Divider is probably not needed for the PICO as the baud rate on the - SPI bus can be set directly. + SPI clock is set in Betaflight code by calling spiSetClkDivisor, which records a uint16_t value into a .speed field. + In order to maintain this code (for simplicity), record the prescale and postdiv numbers as calculated in + pico-sdk/src/rp2_common/hardware_spi.c: spi_set_baudrate() - In anycase max SPI clock is half the system clock frequency. - Therefore the minimum divider is 2. + prescale and postdiv are in range 1..255 and are packed into the return value. */ - return MAX(2, (((clock_get_hz(clk_sys) + (freq / 2)) / freq) + 1) & ~1); + + uint32_t spiClock = clock_get_hz(clk_peri); + uint32_t prescale, postdiv; + // Find smallest prescale value which puts output frequency in range of + // post-divide. Prescale is an even number from 2 to 254 inclusive. + for (prescale = 2; prescale <= 254; prescale += 2) { + if (spiClock < prescale * 256 * (uint64_t) freq) + break; + } + if (prescale > 254) { + prescale = 254; + } + + // Find largest post-divide which makes output <= freq. Post-divide is + // an integer in the range 1 to 256 inclusive. + for (postdiv = 256; postdiv > 1; --postdiv) { + if (spiClock / (prescale * (postdiv - 1)) > freq) + break; + } + + // Store prescale, (postdiv - 1), both in range 0 to 255. + return (uint16_t)((prescale << 8) + (postdiv - 1)); } + +uint32_t spiCalculateClock(uint16_t speed) +{ + /* + speed contains packed values of prescale and postdiv. + Retrieve a frequency which will recreate the same prescale and postdiv on a call to spi_set_baudrate(). + */ + uint32_t spiClock = clock_get_hz(clk_peri); + uint32_t prescale = speed >> 8; + uint32_t postdivMinusOne = speed & 0xFF; + + // Set freq to reverse the calculation, so that we would end up with the same prescale and postdiv, + // hence the same frequency as if we had requested directly from spiCalculateDivider(). + uint32_t freq = 1 + (spiClock/prescale)/(postdivMinusOne + 1); + + return freq; +} + + #endif From 85a4fe8dccb4a3442c4fc2ec71b09a46210f6561 Mon Sep 17 00:00:00 2001 From: Matthew Selby Date: Thu, 15 May 2025 00:39:09 +0100 Subject: [PATCH 10/11] PICO: add MOTOR pins. --- src/platform/PICO/target/RP2350A/target.h | 6 ++++++ src/platform/PICO/target/RP2350B/target.h | 5 +++++ 2 files changed, 11 insertions(+) diff --git a/src/platform/PICO/target/RP2350A/target.h b/src/platform/PICO/target/RP2350A/target.h index 3c7df6607c..1ce5f13b82 100644 --- a/src/platform/PICO/target/RP2350A/target.h +++ b/src/platform/PICO/target/RP2350A/target.h @@ -153,6 +153,7 @@ void _bprintf(const char * format, ...); #undef LED1_PIN #undef LED2_PIN +// These pin selections are currently fairly arbitrary for RP2350A #define SPI0_SCK_PIN P2 #define SPI0_SDI_PIN P4 #define SPI0_SDO_PIN P3 @@ -168,6 +169,11 @@ void _bprintf(const char * format, ...); #define GYRO_1_CS_PIN P1 #define GYRO_2_CS_PIN NONE +#define MOTOR1_PIN P18 +#define MOTOR2_PIN P19 +#define MOTOR3_PIN P20 +#define MOTOR4_PIN P21 + #define MAX7456_SPI_INSTANCE SPI1 #define SDCARD_SPI_INSTANCE SPI1 #define GYRO_1_SPI_INSTANCE SPI0 diff --git a/src/platform/PICO/target/RP2350B/target.h b/src/platform/PICO/target/RP2350B/target.h index 4b35bf6834..1bc17dfee1 100644 --- a/src/platform/PICO/target/RP2350B/target.h +++ b/src/platform/PICO/target/RP2350B/target.h @@ -165,6 +165,11 @@ void _bprintf(const char * format, ...); #define GYRO_1_CS_PIN P1 #define GYRO_2_CS_PIN NONE +#define MOTOR1_PIN P28 +#define MOTOR2_PIN P29 +#define MOTOR3_PIN P30 +#define MOTOR4_PIN P31 + #define MAX7456_SPI_INSTANCE SPI1 #define SDCARD_SPI_INSTANCE SPI1 #define GYRO_1_SPI_INSTANCE SPI0 From a305b8833bfacf1879932b819575b9035a72d4ec Mon Sep 17 00:00:00 2001 From: Matthew Selby Date: Thu, 15 May 2025 11:33:08 +0100 Subject: [PATCH 11/11] PICO: Comments on IOConfigGPIO --- src/platform/PICO/io_pico.c | 15 ++++++++++++++- src/platform/PICO/platform_mcu.h | 15 +++++++++------ 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/src/platform/PICO/io_pico.c b/src/platform/PICO/io_pico.c index 8e7b8f3942..75ba6cf200 100644 --- a/src/platform/PICO/io_pico.c +++ b/src/platform/PICO/io_pico.c @@ -87,13 +87,26 @@ void IOToggle(IO_t io) void IOConfigGPIO(IO_t io, ioConfig_t cfg) { + /* +TODO: update to support the following +IOCFG_AF_PP +IOCFG_IN_FLOATING +IOCFG_IPD +IOCFG_IPU +IOCFG_OUT_OD +IOCFG_OUT_PP +IO_RESET_CFG + +SPI_IO_CS_CFG (as defined) +SPI_IO_CS_HIGH_CFG (as defined) + */ if (!io) { return; } uint16_t ioPin = IO_Pin(io); gpio_init(ioPin); - gpio_set_dir(ioPin, (cfg & 0x01)); + gpio_set_dir(ioPin, (cfg & 0x01)); // 0 = in, 1 = out } IO_t IOGetByTag(ioTag_t tag) diff --git a/src/platform/PICO/platform_mcu.h b/src/platform/PICO/platform_mcu.h index 578e151f70..aa9bdd37c1 100644 --- a/src/platform/PICO/platform_mcu.h +++ b/src/platform/PICO/platform_mcu.h @@ -31,12 +31,6 @@ #define NVIC_PriorityGroup_2 0x500 -#define SPI_IO_AF_CFG 0 -#define SPI_IO_AF_SCK_CFG_HIGH 0 -#define SPI_IO_AF_SCK_CFG_LOW 0 -#define SPI_IO_AF_SDI_CFG 0 -#define SPI_IO_CS_CFG 0 - #if defined(RP2350A) || defined(RP2350B) typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState; @@ -84,6 +78,7 @@ typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState; #define IO_CONFIG(mode, speed, pupd) ((mode) | ((speed) << 2) | ((pupd) << 5)) +// TODO update these and IOConfigGPIO #define IOCFG_OUT_PP IO_CONFIG(GPIO_OUT, 0, 0) #define IOCFG_OUT_OD IO_CONFIG(GPIO_OUT, 0, 0) #define IOCFG_AF_PP 0 @@ -92,6 +87,14 @@ typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState; #define IOCFG_IPU IO_CONFIG(GPIO_IN, 0, 0) #define IOCFG_IN_FLOATING IO_CONFIG(GPIO_IN, 0, 0) +// TODO update these and IOConfigGPIO +#define SPI_IO_AF_CFG 0 +#define SPI_IO_AF_SCK_CFG_HIGH 0 +#define SPI_IO_AF_SCK_CFG_LOW 0 +#define SPI_IO_AF_SDI_CFG 0 +#define SPI_IO_CS_CFG 0 + + #define SERIAL_UART_FIRST_INDEX 0 extern uint32_t systemUniqueId[3];