From abc26d1993f5f33b63fb2288001dd16f38cac683 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Sun, 30 May 2021 01:17:40 +0530 Subject: [PATCH] AP_HAL_ChibiOS: add QSPI Device Driver in HAL --- .../AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h | 4 + .../AP_HAL_ChibiOS/AP_HAL_ChibiOS_Private.h | 1 + libraries/AP_HAL_ChibiOS/Device.cpp | 9 +- libraries/AP_HAL_ChibiOS/Device.h | 4 +- .../AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp | 7 + libraries/AP_HAL_ChibiOS/QSPIDevice.cpp | 168 ++++++++++++++++++ libraries/AP_HAL_ChibiOS/QSPIDevice.h | 132 ++++++++++++++ libraries/AP_HAL_ChibiOS/SPIDevice.cpp | 2 - libraries/AP_HAL_ChibiOS/Util.cpp | 2 +- .../hwdef/common/bouncebuffer.c | 10 +- .../hwdef/common/bouncebuffer.h | 4 +- .../AP_HAL_ChibiOS/hwdef/common/malloc.c | 18 +- .../AP_HAL_ChibiOS/hwdef/common/stm32_util.h | 2 +- .../hwdef/common/stm32h7_mcuconf.h | 13 ++ .../hwdef/scripts/chibios_hwdef.py | 71 +++++++- 15 files changed, 424 insertions(+), 23 deletions(-) create mode 100644 libraries/AP_HAL_ChibiOS/QSPIDevice.cpp create mode 100644 libraries/AP_HAL_ChibiOS/QSPIDevice.h diff --git a/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h index c24330f6a6..4579705520 100644 --- a/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h +++ b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h @@ -20,6 +20,10 @@ namespace ChibiOS { class SPIDevice; class SPIDeviceDriver; class SPIDeviceManager; + class QSPIBus; + class QSPIDesc; + class QSPIDevice; + class QSPIDeviceManager; class Storage; class UARTDriver; class Util; diff --git a/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Private.h b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Private.h index e704d3d974..4b172482e9 100644 --- a/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Private.h +++ b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Private.h @@ -16,3 +16,4 @@ #include "I2CDevice.h" #include "Flash.h" #include "DSP.h" +#include "QSPIDevice.h" diff --git a/libraries/AP_HAL_ChibiOS/Device.cpp b/libraries/AP_HAL_ChibiOS/Device.cpp index 444f081bbb..f5ad017de9 100644 --- a/libraries/AP_HAL_ChibiOS/Device.cpp +++ b/libraries/AP_HAL_ChibiOS/Device.cpp @@ -18,7 +18,7 @@ #include #include -#if HAL_USE_I2C == TRUE || HAL_USE_SPI == TRUE +#if HAL_USE_I2C == TRUE || HAL_USE_SPI == TRUE || HAL_USE_WSPI == TRUE #include "Scheduler.h" #include "Semaphores.h" @@ -40,6 +40,13 @@ DeviceBus::DeviceBus(uint8_t _thread_priority) : bouncebuffer_init(&bounce_buffer_rx, 10, false); } +DeviceBus::DeviceBus(uint8_t _thread_priority, bool axi_sram) : + thread_priority(_thread_priority) +{ + bouncebuffer_init(&bounce_buffer_tx, 10, axi_sram); + bouncebuffer_init(&bounce_buffer_rx, 10, axi_sram); +} + /* per-bus callback thread */ diff --git a/libraries/AP_HAL_ChibiOS/Device.h b/libraries/AP_HAL_ChibiOS/Device.h index c1b62f7a71..9bd218d412 100644 --- a/libraries/AP_HAL_ChibiOS/Device.h +++ b/libraries/AP_HAL_ChibiOS/Device.h @@ -19,7 +19,7 @@ #include "Semaphores.h" #include "AP_HAL_ChibiOS.h" -#if HAL_USE_I2C == TRUE || HAL_USE_SPI == TRUE +#if HAL_USE_I2C == TRUE || HAL_USE_SPI == TRUE || HAL_USE_WSPI == TRUE #include "Scheduler.h" #include "shared_dma.h" @@ -30,6 +30,8 @@ namespace ChibiOS { class DeviceBus { public: DeviceBus(uint8_t _thread_priority = APM_I2C_PRIORITY); + + DeviceBus(uint8_t _thread_priority, bool axi_sram); struct DeviceBus *next; Semaphore semaphore; diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp index fbc0a230d4..57ac2b410b 100644 --- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp +++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp @@ -110,6 +110,12 @@ static Empty::Flash flashDriver; static ChibiOS::CANIface* canDrivers[HAL_NUM_CAN_IFACES]; #endif +#if HAL_USE_WSPI == TRUE && defined(HAL_QSPI_DEVICE_LIST) +static ChibiOS::QSPIDeviceManager qspiDeviceManager; +#else +static Empty::QSPIDeviceManager qspiDeviceManager; +#endif + #if HAL_WITH_IO_MCU HAL_UART_IO_DRIVER; #include @@ -129,6 +135,7 @@ HAL_ChibiOS::HAL_ChibiOS() : &uartIDriver, &i2cDeviceManager, &spiDeviceManager, + &qspiDeviceManager, &analogIn, &storageDriver, &uartADriver, diff --git a/libraries/AP_HAL_ChibiOS/QSPIDevice.cpp b/libraries/AP_HAL_ChibiOS/QSPIDevice.cpp new file mode 100644 index 0000000000..d3ee593f5c --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/QSPIDevice.cpp @@ -0,0 +1,168 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * 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. + * + * This file 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 program. If not, see . + * + * Code by + * Andy Piper + * Siddharth Bharat Purohit, Cubepilot Pty. Ltd. + */ + +#include "QSPIDevice.h" + +#include +#include +#include +#include +#include "Util.h" +#include "Scheduler.h" +#include "Semaphores.h" +#include +#include "hwdef/common/stm32_util.h" + +#if HAL_USE_WSPI == TRUE && defined(HAL_QSPI_DEVICE_LIST) + +using namespace ChibiOS; +extern const AP_HAL::HAL& hal; + +static const struct QSPIDriverInfo { + WSPIDriver *driver; + uint8_t busid; // used for device IDs in parameters +} qspi_devices[] = { HAL_QSPI_BUS_LIST }; + +#define QSPIDEV_MODE1 0 +#define QSPIDEV_MODE3 STM32_DCR_CK_MODE + +// device list comes from hwdef.dat +QSPIDesc QSPIDeviceManager::device_table[] = { HAL_QSPI_DEVICE_LIST }; + +// Check clock sanity during runtime +#define XSTR(x) STR(x) +#define STR(x) #x + +#if (STM32_QSPICLK % HAL_QSPI1_CLK) +#error "QSPI Clock" STR(STM32_QSPICLK) " needs to be divisible by selected clock" STR(HAL_QSPI1_CLK) +#endif + +#if defined(STM32_WSPI_USE_QUADSPI2) && STM32_WSPI_USE_QUADSPI2 +#if (STM32_QSPICLK % HAL_QSPI2_CLK) +#error "QSPI Clock" STR(STM32_QSPICLK) " needs to be divisible by selected clock" STR(HAL_QSPI2_CLK) +#endif +#endif + +bool QSPIDevice::transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) +{ + if (!acquire_bus(true)) { + return false; + } + if (!bus.bouncebuffer_setup(send, send_len, recv, recv_len)) { + acquire_bus(false); + return false; + } + wspi_command_t mode; + bool ret = true; + mode.cmd = _cmd_hdr.cmd; + mode.cfg = _cmd_hdr.cfg; + mode.addr = _cmd_hdr.addr; + mode.alt = _cmd_hdr.alt; + mode.dummy = _cmd_hdr.dummy; + + if (send_len == 0 && recv_len == 0) { + // This is just a command + ret = wspiCommand(qspi_devices[device_desc.bus].driver, &mode); + } else if (send_len > 0 && recv == 0) { + // This is a send cmd + ret = wspiSend(qspi_devices[device_desc.bus].driver, &mode, send_len, send); + } else if (send_len == 0 && recv_len >= 1) { + // This is a receive cmd, + // we only consume first byte of send + ret = wspiReceive(qspi_devices[device_desc.bus].driver, &mode, recv_len, recv); + } else { + // Can't handle this transaction type + ret = false; + } + bus.bouncebuffer_finish(send, recv, recv_len); + acquire_bus(false); + return ret; +} + +void QSPIDevice::set_cmd_header(const CommandHeader& cmd_hdr) +{ + _cmd_hdr = cmd_hdr; +} + + +bool QSPIDevice::acquire_bus(bool acquire) +{ + if (!bus.semaphore.check_owner()) { + return false; + } + if (acquire) { + wspiAcquireBus(qspi_devices[device_desc.bus].driver); + if (qspi_devices[device_desc.bus].driver->config != &bus.wspicfg) { + // Initialise and Start WSPI driver + bus.wspicfg.end_cb = nullptr; + bus.wspicfg.error_cb = nullptr; + bus.wspicfg.dcr = STM32_DCR_FSIZE(device_desc.size_pow2) | + STM32_DCR_CSHT(device_desc.ncs_clk_delay - 1) | + device_desc.mode; + wspiStart(qspi_devices[device_desc.bus].driver, &bus.wspicfg); + } + } else { + wspiReleaseBus(qspi_devices[device_desc.bus].driver); + } + return true; +} + +/* + return a SPIDevice given a string device name + */ +AP_HAL::OwnPtr +QSPIDeviceManager::get_device(const char *name) +{ + /* Find the bus description in the table */ + uint8_t i; + for (i = 0; i(nullptr); + } + + QSPIDesc &desc = device_table[i]; + + // find the bus + QSPIBus *busp; + for (busp = buses; busp; busp = (QSPIBus *)busp->next) { + if (busp->bus == desc.bus) { + break; + } + } + if (busp == nullptr) { + // create a new one + busp = new QSPIBus(desc.bus); + if (busp == nullptr) { + return nullptr; + } + busp->next = buses; + busp->bus = desc.bus; + + buses = busp; + } + + return AP_HAL::OwnPtr(new QSPIDevice(*busp, desc)); +} + +#endif // #if HAL_USE_QSPI == TRUE && defined(HAL_QPI_DEVICE_LIST) diff --git a/libraries/AP_HAL_ChibiOS/QSPIDevice.h b/libraries/AP_HAL_ChibiOS/QSPIDevice.h new file mode 100644 index 0000000000..49283d8428 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/QSPIDevice.h @@ -0,0 +1,132 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * 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. + * + * This file 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 program. If not, see . + * + * Code by + * Andy Piper + * Siddharth Bharat Purohit, Cubepilot Pty. Ltd. + */ + +#pragma once + +#include +#include +#include +#include "AP_HAL_ChibiOS.h" + +#if HAL_USE_WSPI == TRUE && defined(HAL_QSPI_DEVICE_LIST) + +#include "Semaphores.h" +#include "Scheduler.h" +#include "Device.h" + +namespace ChibiOS +{ + +struct QSPIDesc { + QSPIDesc(const char *_name, uint8_t _bus, + uint32_t _mode, uint32_t _speed, + uint8_t _size_pow2, uint8_t _ncs_clk_shift) + : name(_name), bus(_bus), mode(_mode), speed(_speed), + size_pow2(_size_pow2), ncs_clk_delay(_ncs_clk_shift) + { + } + + const char *name; // name of the device + uint8_t bus; // qspi bus being used + uint8_t device; // device id + uint32_t mode; // clock mode + uint32_t speed; // clock speed + uint8_t size_pow2; // size as power of 2 + uint8_t ncs_clk_delay; // number of clk cycles to wait while transitioning NCS + +}; + +class QSPIBus : public DeviceBus +{ +public: + QSPIBus(uint8_t _bus) : + DeviceBus(APM_SPI_PRIORITY, true), + bus(_bus) {} + + uint8_t bus; + WSPIConfig wspicfg; + bool qspi_started; +}; + +class QSPIDevice : public AP_HAL::QSPIDevice +{ +public: + static QSPIDevice *from(AP_HAL::QSPIDevice *dev) + { + return static_cast(dev); + } + + QSPIDevice(QSPIBus &_bus, QSPIDesc &_device_desc) : + bus(_bus), + device_desc(_device_desc) + {} + + bool set_speed(Speed speed) override + { + return true; + } + + PeriodicHandle register_periodic_callback(uint32_t period_usec, PeriodicCb) override + { + return nullptr; + } + bool adjust_periodic_callback(PeriodicHandle h, uint32_t period_usec) override + { + return false; + } + + /* See AP_HAL::Device::transfer() */ + bool transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) override; + + void set_cmd_header(const CommandHeader& cmd_hdr) override; + + AP_HAL::Semaphore* get_semaphore() override + { + // if asking for invalid bus number use bus 0 semaphore + return &bus.semaphore; + } + + bool acquire_bus(bool acquire); + +private: + QSPIBus &bus; + QSPIDesc &device_desc; + CommandHeader _cmd_hdr; +}; + +class QSPIDeviceManager : public AP_HAL::QSPIDeviceManager +{ +public: + friend class QSPIDevice; + + static QSPIDeviceManager *from(AP_HAL::QSPIDeviceManager *qspi_mgr) + { + return static_cast(qspi_mgr); + } + + AP_HAL::OwnPtr get_device(const char *name) override; +private: + static QSPIDesc device_table[]; + QSPIBus *buses; +}; + +} + +#endif // #if HAL_USE_WSPI == TRUE && defined(HAL_QSPI_DEVICE_LIST) diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp index 6965a4fba3..94844c2b04 100644 --- a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp +++ b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp @@ -69,8 +69,6 @@ static const struct SPIDriverInfo { uint8_t dma_channel_tx; } spi_devices[] = { HAL_SPI_BUS_LIST }; -#define MHZ (1000U*1000U) -#define KHZ (1000U) // device list comes from hwdef.dat ChibiOS::SPIDesc SPIDeviceManager::device_table[] = { HAL_SPI_DEVICE_LIST }; diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 64e014da46..0588559a11 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -366,7 +366,7 @@ bool Util::was_watchdog_reset() const return stm32_was_watchdog_reset(); } -#if CH_DBG_ENABLE_STACK_CHECK == TRUE +#if CH_DBG_ENABLE_STACK_CHECK == TRUE && !defined(HAL_BOOTLOADER_BUILD) /* display stack usage as text buffer for @SYS/threads.txt */ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.c b/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.c index 840d1bd612..c93fc5d891 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.c @@ -41,13 +41,13 @@ /* initialise a bouncebuffer */ -void bouncebuffer_init(struct bouncebuffer_t **bouncebuffer, uint32_t prealloc_bytes, bool sdcard) +void bouncebuffer_init(struct bouncebuffer_t **bouncebuffer, uint32_t prealloc_bytes, bool axi_sram) { (*bouncebuffer) = calloc(1, sizeof(struct bouncebuffer_t)); osalDbgAssert(((*bouncebuffer) != NULL), "bouncebuffer init"); - (*bouncebuffer)->is_sdcard = sdcard; + (*bouncebuffer)->on_axi_sram = axi_sram; if (prealloc_bytes) { - (*bouncebuffer)->dma_buf = sdcard?malloc_sdcard_dma(prealloc_bytes):malloc_dma(prealloc_bytes); + (*bouncebuffer)->dma_buf = axi_sram?malloc_axi_sram(prealloc_bytes):malloc_dma(prealloc_bytes); if ((*bouncebuffer)->dma_buf) { (*bouncebuffer)->size = prealloc_bytes; } @@ -71,7 +71,7 @@ bool bouncebuffer_setup_read(struct bouncebuffer_t *bouncebuffer, uint8_t **buf, if (bouncebuffer->size > 0) { free(bouncebuffer->dma_buf); } - bouncebuffer->dma_buf = bouncebuffer->is_sdcard?malloc_sdcard_dma(size):malloc_dma(size); + bouncebuffer->dma_buf = bouncebuffer->on_axi_sram?malloc_axi_sram(size):malloc_dma(size); if (!bouncebuffer->dma_buf) { bouncebuffer->size = 0; return false; @@ -116,7 +116,7 @@ bool bouncebuffer_setup_write(struct bouncebuffer_t *bouncebuffer, const uint8_t if (bouncebuffer->size > 0) { free(bouncebuffer->dma_buf); } - bouncebuffer->dma_buf = bouncebuffer->is_sdcard?malloc_sdcard_dma(size):malloc_dma(size); + bouncebuffer->dma_buf = bouncebuffer->on_axi_sram?malloc_axi_sram(size):malloc_dma(size); if (!bouncebuffer->dma_buf) { bouncebuffer->size = 0; return false; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.h b/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.h index 89732db576..f015c663d6 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.h @@ -16,13 +16,13 @@ struct bouncebuffer_t { uint8_t *orig_buf; uint32_t size; bool busy; - bool is_sdcard; + bool on_axi_sram; }; /* initialise a bouncebuffer */ -void bouncebuffer_init(struct bouncebuffer_t **bouncebuffer, uint32_t prealloc_bytes, bool sdcard); +void bouncebuffer_init(struct bouncebuffer_t **bouncebuffer, uint32_t prealloc_bytes, bool axi_sram); /* setup for reading from a device into memory, allocating a bouncebuffer if needed diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c b/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c index 7b9799780c..eb42c726a3 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c @@ -38,7 +38,7 @@ #define MEM_REGION_FLAG_DMA_OK 1 #define MEM_REGION_FLAG_FAST 2 -#define MEM_REGION_FLAG_SDCARD 4 +#define MEM_REGION_FLAG_AXI_BUS 4 #ifdef HAL_CHIBIOS_ENABLE_MALLOC_GUARD static mutex_t mem_mutex; @@ -119,7 +119,7 @@ static void *malloc_flags(size_t size, uint32_t flags) if (size == 0) { return NULL; } - const uint8_t dma_flags = (MEM_REGION_FLAG_DMA_OK | MEM_REGION_FLAG_SDCARD); + const uint8_t dma_flags = (MEM_REGION_FLAG_DMA_OK | MEM_REGION_FLAG_AXI_BUS); const uint8_t alignment = (flags&dma_flags?DMA_ALIGNMENT:MIN_ALIGNMENT); void *p = NULL; uint8_t i; @@ -145,8 +145,8 @@ static void *malloc_flags(size_t size, uint32_t flags) !(memory_regions[i].flags & MEM_REGION_FLAG_DMA_OK)) { continue; } - if ((flags & MEM_REGION_FLAG_SDCARD) && - !(memory_regions[i].flags & MEM_REGION_FLAG_SDCARD)) { + if ((flags & MEM_REGION_FLAG_AXI_BUS) && + !(memory_regions[i].flags & MEM_REGION_FLAG_AXI_BUS)) { continue; } if ((flags & MEM_REGION_FLAG_FAST) && @@ -219,7 +219,7 @@ static void *malloc_flags_guard(size_t size, uint32_t flags) { chMtxLock(&mem_mutex); - if (flags & (MEM_REGION_FLAG_DMA_OK | MEM_REGION_FLAG_SDCARD)) { + if (flags & (MEM_REGION_FLAG_DMA_OK | MEM_REGION_FLAG_AXI_BUS)) { size = (size + (DMA_ALIGNMENT-1U)) & ~(DMA_ALIGNMENT-1U); } else { size = (size + (MIN_ALIGNMENT-1U)) & ~(MIN_ALIGNMENT-1U); @@ -353,13 +353,13 @@ void *malloc_dma(size_t size) } /* - allocate DMA-safe memory for microSD transfers. This is only - different on H7 where SDMMC IDMA can't use SRAM4 + allocate from memory connected to AXI Bus if available + else just allocate dma safe memory */ -void *malloc_sdcard_dma(size_t size) +void *malloc_axi_sram(size_t size) { #if defined(STM32H7) - return malloc_flags(size, MEM_REGION_FLAG_SDCARD); + return malloc_flags(size, MEM_REGION_FLAG_AXI_BUS); #else return malloc_flags(size, MEM_REGION_FLAG_DMA_OK); #endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h index 3a33c673a2..57cc52749d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h @@ -31,7 +31,7 @@ void show_stack_usage(void); // allocation functions in malloc.c size_t mem_available(void); void *malloc_dma(size_t size); -void *malloc_sdcard_dma(size_t size); +void *malloc_axi_sram(size_t size); void *malloc_fastmem(size_t size); thread_t *thread_create_alloc(size_t size, const char *name, tprio_t prio, tfunc_t pf, void *arg); diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h index 5d38d64f13..49112668c2 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h @@ -73,7 +73,11 @@ #define STM32_VOS STM32_VOS_SCALE1 #define STM32_PWR_CR1 (PWR_CR1_SVOS_1 | PWR_CR1_SVOS_0) #define STM32_PWR_CR2 (PWR_CR2_BREN) +#ifdef SMPS_PWR +#define STM32_PWR_CR3 (PWR_CR3_SMPSEN | PWR_CR3_USB33DEN) +#else #define STM32_PWR_CR3 (PWR_CR3_LDOEN | PWR_CR3_USB33DEN) +#endif #define STM32_PWR_CPUCR 0 /* @@ -553,3 +557,12 @@ #define STM32_SDC_MAX_CLOCK 12500000 #endif +#ifndef STM32_WSPI_USE_QUADSPI1 +#define STM32_WSPI_USE_QUADSPI1 FALSE +#endif + +#if STM32_WSPI_USE_QUADSPI1 +#define STM32_WSPI_QUADSPI1_MDMA_CHANNEL STM32_MDMA_CHANNEL_ID_ANY +#define STM32_WSPI_QUADSPI1_MDMA_PRIORITY 1 +#define STM32_WSPI_QUADSPI1_PRESCALER_VALUE ((STM32_QSPICLK / HAL_QSPI1_CLK) - 1) +#endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 373ba80957..a6b48ce0fa 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -30,7 +30,7 @@ f4f7_vtypes = ['MODER', 'OTYPER', 'OSPEEDR', 'PUPDR', 'ODR', 'AFRL', 'AFRH'] f1_vtypes = ['CRL', 'CRH', 'ODR'] f1_input_sigs = ['RX', 'MISO', 'CTS'] f1_output_sigs = ['TX', 'MOSI', 'SCK', 'RTS', 'CH1', 'CH2', 'CH3', 'CH4'] -af_labels = ['USART', 'UART', 'SPI', 'I2C', 'SDIO', 'SDMMC', 'OTG', 'JT', 'TIM', 'CAN'] +af_labels = ['USART', 'UART', 'SPI', 'I2C', 'SDIO', 'SDMMC', 'OTG', 'JT', 'TIM', 'CAN', 'QUADSPI'] default_gpio = ['INPUT', 'FLOATING'] @@ -80,12 +80,18 @@ altlabel = {} # list of SPI devices spidev = [] +# list of QSPI devices +qspidev = [] + # dictionary of ROMFS files romfs = {} # SPI bus list spi_list = [] +# list of QSPI devices +qspi_list = [] + # all config lines in order alllines = [] @@ -990,6 +996,7 @@ def write_SPI_table(f): f.write("#define HAL_WITH_SPI_%s 1\n" % dev[0].upper().replace("-","_")) f.write("\n") + def write_SPI_config(f): '''write SPI config defines''' global spi_list @@ -1011,6 +1018,60 @@ def write_SPI_config(f): write_SPI_table(f) +def write_QSPI_table(f): + '''write SPI device table''' + f.write('\n// QSPI device table\n') + devlist = [] + for dev in qspidev: + if len(dev) != 6: + print("Badly formed QSPIDEV line %s" % dev) + name = '"' + dev[0] + '"' + bus = dev[1] + mode = dev[2] + speed = dev[3] + if not bus.startswith('QUADSPI') or bus not in qspi_list: + error("Bad QUADSPI bus in QSPIDEV line %s" % dev) + if mode not in ['MODE1', 'MODE3']: + error("Bad MODE in QSPIDEV line %s" % dev) + if not speed.endswith('*MHZ') and not speed.endswith('*KHZ'): + error("Bad speed value %s in SPIDEV line %s" % (speed, dev)) + + devidx = len(devlist) + f.write( + '#define HAL_QSPI_DEVICE%-2u QSPIDesc(%-17s, %2u, QSPIDEV_%s, %7s)\n' + % (devidx, name, qspi_list.index(bus), mode, speed)) + devlist.append('HAL_QSPI_DEVICE%u' % devidx) + f.write('#define HAL_QSPI_DEVICE_LIST %s\n\n' % ','.join(devlist)) + for dev in qspidev: + f.write("#define HAL_HAS_WSPI_%s 1\n" % dev[0].upper().replace("-", "_")) + f.write("#define HAL_QSPI%d_CLK (%s)" % (int(bus[7:]), speed)) + f.write("\n") + + +def write_QSPI_config(f): + '''write SPI config defines''' + global qspi_list + if len(qspidev) == 0: + # nothing to do + return + for t in list(bytype.keys()) + list(alttype.keys()): + if t.startswith('QUADSPI'): + qspi_list.append(t) + qspi_list = sorted(qspi_list) + if len(qspi_list) == 0: + return + f.write('#define HAL_USE_WSPI TRUE\n') + devlist = [] + for dev in qspi_list: + n = int(dev[7:]) + devlist.append('HAL_QSPI%u_CONFIG' % n) + f.write( + '#define HAL_QSPI%u_CONFIG { &WSPID%u, %u}\n' + % (n, n, n)) + f.write('#define HAL_QSPI_BUS_LIST %s\n\n' % ','.join(devlist)) + write_QSPI_table(f) + + def parse_spi_device(dev): '''parse a SPI:xxx device item''' a = dev.split(':') @@ -1725,6 +1786,8 @@ def write_peripheral_enable(f): f.write('#define STM32_USB_USE_%s TRUE\n' % type) if type.startswith('I2C'): f.write('#define STM32_I2C_USE_%s TRUE\n' % type) + if type.startswith('QUADSPI'): + f.write('#define STM32_WSPI_USE_%s TRUE\n' % type) def get_dma_exclude(periph_list): @@ -1786,12 +1849,16 @@ def write_hwdef_header(outfilename): #define FALSE 0 #endif +#define MHZ (1000U*1000U) +#define KHZ (1000U) + ''') dma_noshare.extend(get_config('DMA_NOSHARE', default='', aslist=True)) write_mcu_config(f) write_SPI_config(f) + write_QSPI_config(f) write_ADC_config(f) write_GPIO_config(f) write_IMU_config(f) @@ -2090,6 +2157,8 @@ def process_line(line): setup_mcu_type_defaults() elif a[0] == 'SPIDEV': spidev.append(a[1:]) + elif a[0] == 'QSPIDEV': + qspidev.append(a[1:]) elif a[0] == 'IMU': imu_list.append(a[1:]) elif a[0] == 'COMPASS':