diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp index 5870fef12e..689cd1e7ff 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp @@ -48,7 +48,7 @@ #include #include -# if defined(STM32H7XX) +# if defined(STM32H7XX) || defined(STM32G4) #include "CANFDIface.h" #define FDCAN1_IT0_IRQHandler STM32_FDCAN1_IT0_HANDLER @@ -56,15 +56,33 @@ #define FDCAN2_IT0_IRQHandler STM32_FDCAN2_IT0_HANDLER #define FDCAN2_IT1_IRQHandler STM32_FDCAN2_IT1_HANDLER -#define FDCAN_FRAME_BUFFER_SIZE 4 // Buffer size for 8 bytes data field +#if defined(STM32G4) +// on G4 FIFO elements are spaced at 18 words +#define FDCAN_FRAME_BUFFER_SIZE 18 +#else +// on H7 they are spaced at 4 words +#define FDCAN_FRAME_BUFFER_SIZE 4 +#endif //Message RAM Allocations in Word lengths + +#if defined(STM32H7) +#define MAX_FILTER_LIST_SIZE 80U //80 element Standard Filter List elements or 40 element Extended Filter List +#define FDCAN_NUM_RXFIFO0_SIZE 104U //26 Frames +#define FDCAN_TX_FIFO_BUFFER_SIZE 128U //32 Frames +#define MESSAGE_RAM_END_ADDR 0x4000B5FC + +#elif defined(STM32G4) #define MAX_FILTER_LIST_SIZE 80U //80 element Standard Filter List elements or 40 element Extended Filter List #define FDCAN_NUM_RXFIFO0_SIZE 104U //26 Frames #define FDCAN_TX_FIFO_BUFFER_SIZE 128U //32 Frames #define MESSAGE_RAM_END_ADDR 0x4000B5FC +#else +#error "Unsupported MCU for FDCAN" +#endif + extern AP_HAL::HAL& hal; static_assert(STM32_FDCANCLK <= 80U*1000U*1000U, "FDCAN clock must be max 80MHz"); @@ -159,7 +177,6 @@ bool CANIface::computeTimings(const uint32_t target_bitrate, Timings& out_timing */ const uint32_t pclk = STM32_FDCANCLK; - static const int MaxBS1 = 16; static const int MaxBS2 = 8; @@ -326,6 +343,7 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline, (uint32_t(frame.data[6]) << 16) | (uint32_t(frame.data[5]) << 8) | (uint32_t(frame.data[4]) << 0); + //Set Add Request can_->TXBAR = (1 << index); @@ -358,6 +376,18 @@ int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_u bool CANIface::configureFilters(const CanFilterConfig* filter_configs, uint16_t num_configs) { +#if defined(STM32G4) + // not supported yet + can_->CCCR &= ~FDCAN_CCCR_INIT; // Leave init mode + uint32_t while_start_ms = AP_HAL::millis(); + while ((can_->CCCR & FDCAN_CCCR_INIT) == 1) { + if ((AP_HAL::millis() - while_start_ms) > REG_SET_TIMEOUT) { + return false; + } + } + initialised_ = true; + return true; +#else uint32_t num_extid = 0, num_stdid = 0; uint32_t total_available_list_size = MAX_FILTER_LIST_SIZE; uint32_t* filter_ptr; @@ -377,7 +407,11 @@ bool CANIface::configureFilters(const CanFilterConfig* filter_configs, CriticalSectionLocker lock; //Allocate Message RAM for Standard ID Filter List if (num_stdid == 0) { //No Frame with Standard ID is to be accepted +#if defined(STM32G4) + can_->RXGFC |= 0x2; //Reject All Standard ID Frames +#else can_->GFC |= 0x2; //Reject All Standard ID Frames +#endif } else if ((num_stdid < total_available_list_size) && (num_stdid <= 128)) { can_->SIDFC = (FDCANMessageRAMOffset_ << 2) | (num_stdid << 16); MessageRam_.StandardFilterSA = SRAMCAN_BASE + (FDCANMessageRAMOffset_ * 4U); @@ -468,6 +502,7 @@ bool CANIface::configureFilters(const CanFilterConfig* filter_configs, } initialised_ = true; return true; +#endif // defined(STM32G4) } uint16_t CANIface::getNumFilters() const @@ -490,16 +525,21 @@ bool CANIface::init(const uint32_t bitrate, const OperatingMode mode) #endif } - bitrate_ = bitrate; mode_ = mode; //Only do it once //Doing it second time will reset the previously initialised bus if (!clock_init_) { CriticalSectionLocker lock; +#if defined(STM32G4) + RCC->APB1ENR1 |= RCC_APB1ENR1_FDCANEN; + RCC->APB1RSTR1 |= RCC_APB1RSTR1_FDCANRST; + RCC->APB1RSTR1 &= ~RCC_APB1RSTR1_FDCANRST; +#else RCC->APB1HENR |= RCC_APB1HENR_FDCANEN; RCC->APB1HRSTR |= RCC_APB1HRSTR_FDCANRST; RCC->APB1HRSTR &= ~RCC_APB1HRSTR_FDCANRST; +#endif clock_init_ = true; } @@ -521,7 +561,6 @@ bool CANIface::init(const uint32_t bitrate, const OperatingMode mode) irq_init_ = true; } - // Setup FDCAN for configuration mode and disable all interrupts { CriticalSectionLocker lock; @@ -579,8 +618,14 @@ bool CANIface::init(const uint32_t bitrate, const OperatingMode mode) (timings.bs2 << FDCAN_NBTP_NTSEG2_Pos) | (timings.prescaler << FDCAN_NBTP_NBRP_Pos)); + can_->DBTP = ((timings.bs1 << FDCAN_DBTP_DTSEG1_Pos) | + (timings.bs2 << FDCAN_DBTP_DTSEG2_Pos) | + (timings.prescaler << FDCAN_DBTP_DBRP_Pos)); + //RX Config +#if defined(STM32H7) can_->RXESC = 0; //Set for 8Byte Frames +#endif //Setup Message RAM setupMessageRam(); @@ -595,9 +640,17 @@ bool CANIface::init(const uint32_t bitrate, const OperatingMode mode) FDCAN_IE_RF0FE | // Rx FIFO 0 FIFO Full FDCAN_IE_RF1NE | // RX FIFO 1 new message FDCAN_IE_RF1FE; // Rx FIFO 1 FIFO Full +#if defined(STM32G4) + can_->ILS = FDCAN_ILS_PERR | FDCAN_ILS_SMSG; +#else can_->ILS = FDCAN_ILS_TCL | FDCAN_ILS_BOE; //Set Line 1 for Transmit Complete Event Interrupt and Bus Off Interrupt +#endif // And Busoff error +#if defined(STM32G4) + can_->TXBTIE = 0x7; +#else can_->TXBTIE = 0xFFFFFFFF; +#endif can_->ILE = 0x3; // If mode is Filtered then we finish the initialisation in configureFilter method @@ -625,6 +678,16 @@ void CANIface::clear_rx() void CANIface::setupMessageRam() { +#if defined(STM32G4) + memset((void*)SRAMCAN_BASE, 0, 0x350); + MessageRam_.StandardFilterSA = SRAMCAN_BASE; + MessageRam_.ExtendedFilterSA = SRAMCAN_BASE + 0x70; + MessageRam_.RxFIFO0SA = SRAMCAN_BASE + 0xB0; + MessageRam_.RxFIFO1SA = SRAMCAN_BASE + 0x188; + MessageRam_.TxFIFOQSA = SRAMCAN_BASE + 0x278; + + can_->TXBC = 0; // fifo mode +#else uint32_t num_elements = 0; // Rx FIFO 0 start address and element count @@ -648,6 +711,7 @@ void CANIface::setupMessageRam() AP_HAL::panic("CANFDIface: Message RAM Overflow!"); return; } +#endif } void CANIface::handleTxCompleteInterrupt(const uint64_t timestamp_us) @@ -685,10 +749,12 @@ bool CANIface::readRxFIFO(uint8_t fifo_index) uint32_t index; uint64_t timestamp_us = AP_HAL::micros64(); if (fifo_index == 0) { +#if !defined(STM32G4) //Check if RAM allocated to RX FIFO if ((can_->RXF0C & FDCAN_RXF0C_F0S) == 0) { return false; } +#endif //Register Message Lost as a hardware error if ((can_->RXF0S & FDCAN_RXF0S_RF0L) != 0) { stats.rx_errors++; @@ -701,10 +767,12 @@ bool CANIface::readRxFIFO(uint8_t fifo_index) frame_ptr = (uint32_t *)(MessageRam_.RxFIFO0SA + (index * FDCAN_FRAME_BUFFER_SIZE * 4)); } } else if (fifo_index == 1) { +#if !defined(STM32G4) //Check if RAM allocated to RX FIFO if ((can_->RXF1C & FDCAN_RXF1C_F1S) == 0) { return false; } +#endif //Register Message Lost as a hardware error if ((can_->RXF1S & FDCAN_RXF1S_RF1L) != 0) { stats.rx_errors++; @@ -811,10 +879,12 @@ void CANIface::pollErrorFlags() bool CANIface::canAcceptNewTxFrame() const { +#if !defined(STM32G4) //Check if Tx FIFO is allocated if ((can_->TXBC & FDCAN_TXBC_TFQS) == 0) { return false; } +#endif if ((can_->TXFQS & FDCAN_TXFQS_TFQF) != 0) { return false; //we don't have free space } @@ -1002,6 +1072,6 @@ extern "C" } // extern "C" -#endif //defined(STM32H7XX) +#endif //defined(STM32H7XX) || defined(STM32G4) #endif //HAL_NUM_CAN_IFACES diff --git a/libraries/AP_HAL_ChibiOS/CANIface.h b/libraries/AP_HAL_ChibiOS/CANIface.h index a8bb1256f2..9224ada640 100644 --- a/libraries/AP_HAL_ChibiOS/CANIface.h +++ b/libraries/AP_HAL_ChibiOS/CANIface.h @@ -41,7 +41,7 @@ #pragma once #include "AP_HAL_ChibiOS.h" -# if defined(STM32H7XX) +# if defined(STM32H7XX) || defined(STM32G4) #include "CANFDIface.h" # else #if HAL_NUM_CAN_IFACES @@ -242,4 +242,4 @@ public: }; }; #endif //HAL_NUM_CAN_IFACES -#endif //# if defined(STM32H7XX) +#endif //# if defined(STM32H7XX) || defined(STM32G4) diff --git a/libraries/AP_HAL_ChibiOS/CanIface.cpp b/libraries/AP_HAL_ChibiOS/CanIface.cpp index 76a579bf5b..d4256eaa82 100644 --- a/libraries/AP_HAL_ChibiOS/CanIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CanIface.cpp @@ -48,7 +48,7 @@ #include #include -# if !defined(STM32H7XX) +# if !defined(STM32H7XX) && !defined(STM32G4) #include "CANIface.h" /* STM32F3's only CAN inteface does not have a number. */ diff --git a/libraries/AP_HAL_ChibiOS/GPIO.cpp b/libraries/AP_HAL_ChibiOS/GPIO.cpp index 5da4aa4427..639a55dcac 100644 --- a/libraries/AP_HAL_ChibiOS/GPIO.cpp +++ b/libraries/AP_HAL_ChibiOS/GPIO.cpp @@ -167,7 +167,7 @@ void GPIO::pinMode(uint8_t pin, uint8_t output) return; } g->mode = output?PAL_MODE_OUTPUT_PUSHPULL:PAL_MODE_INPUT; -#if defined(STM32F7) || defined(STM32H7) || defined(STM32F4) +#if defined(STM32F7) || defined(STM32H7) || defined(STM32F4) || defined(STM32G4) if (g->mode == PAL_MODE_OUTPUT_PUSHPULL) { // retain OPENDRAIN if already set iomode_t old_mode = palReadLineMode(g->pal_line); diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index 186386fed6..4e1afdbe5f 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -357,7 +357,7 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) (void *)this); osalDbgAssert(rxdma, "stream alloc failed"); chSysUnlock(); -#if defined(STM32F7) || defined(STM32H7) || defined(STM32F3) +#if defined(STM32F7) || defined(STM32H7) || defined(STM32F3) || defined(STM32G4) dmaStreamSetPeripheral(rxdma, &((SerialDriver*)sdef.serial)->usart->RDR); #else dmaStreamSetPeripheral(rxdma, &((SerialDriver*)sdef.serial)->usart->DR); @@ -453,7 +453,7 @@ void UARTDriver::dma_tx_allocate(Shared_DMA *ctx) (void *)this); osalDbgAssert(txdma, "stream alloc failed"); chSysUnlock(); -#if defined(STM32F7) || defined(STM32H7) || defined(STM32F3) +#if defined(STM32F7) || defined(STM32H7) || defined(STM32F3) || defined(STM32G4) dmaStreamSetPeripheral(txdma, &((SerialDriver*)sdef.serial)->usart->TDR); #else dmaStreamSetPeripheral(txdma, &((SerialDriver*)sdef.serial)->usart->DR); @@ -502,7 +502,7 @@ void UARTDriver::rx_irq_cb(void* self) #if defined(STM32F7) || defined(STM32H7) //disable dma, triggering DMA transfer complete interrupt uart_drv->rxdma->stream->CR &= ~STM32_DMA_CR_EN; -#elif defined(STM32F3) +#elif defined(STM32F3) || defined(STM32G4) //disable dma, triggering DMA transfer complete interrupt dmaStreamDisable(uart_drv->rxdma); uart_drv->rxdma->channel->CCR &= ~STM32_DMA_CR_EN; @@ -1113,7 +1113,7 @@ void UARTDriver::_rx_timer_tick(void) //Check if DMA is enabled //if not, it might be because the DMA interrupt was silenced //let's handle that here so that we can continue receiving -#if defined(STM32F3) +#if defined(STM32F3) || defined(STM32G4) bool enabled = (rxdma->channel->CCR & STM32_DMA_CR_EN); #else bool enabled = (rxdma->stream->CR & STM32_DMA_CR_EN); @@ -1533,7 +1533,7 @@ bool UARTDriver::set_options(uint16_t options) atx_line = sdef.tx_line; #endif -#if defined(STM32F7) || defined(STM32H7) || defined(STM32F3) +#if defined(STM32F7) || defined(STM32H7) || defined(STM32F3) || defined(STM32G4) // F7 has built-in support for inversion in all uarts ioline_t rx_line = (options & OPTION_SWAP)?atx_line:arx_line; ioline_t tx_line = (options & OPTION_SWAP)?arx_line:atx_line; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/board.c b/libraries/AP_HAL_ChibiOS/hwdef/common/board.c index 44824d232a..0f9a636dc0 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/board.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/board.c @@ -176,6 +176,9 @@ static void stm32_gpio_init(void) { #elif defined(STM32F3) rccResetAHB(STM32_GPIO_EN_MASK); rccEnableAHB(STM32_GPIO_EN_MASK, true); +#elif defined(STM32G4) + rccResetAHB2(STM32_GPIO_EN_MASK); + rccEnableAHB2(STM32_GPIO_EN_MASK, true); #else rccResetAHB1(STM32_GPIO_EN_MASK); rccEnableAHB1(STM32_GPIO_EN_MASK, true); diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h index 6c692ab288..d4d74e9fa2 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h @@ -68,8 +68,7 @@ extern "C" { #define osalDbgAssert(c, remark) do { if (!(c)) { fault_printf("%s:%d: %s", __FILE__, __LINE__, remark ); chDbgAssert(c, remark); } } while (0) #endif -#define PORT_INT_REQUIRED_STACK 256 -#endif +#endif // HAL_CHIBIOS_ENABLE_ASSERTS #if HAL_ENABLE_THREAD_STATISTICS #define CH_DBG_STATISTICS TRUE diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c index 32d659be7e..e9c1f6f255 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c @@ -126,6 +126,9 @@ static const uint32_t flash_memmap[STM32_FLASH_NPAGES] = { KB(32), KB(32), KB(32 #elif defined(STM32F303_MCUCONF) #define STM32_FLASH_NPAGES (BOARD_FLASH_SIZE/2) #define STM32_FLASH_FIXED_PAGE_SIZE 2 +#elif defined(STM32G4) +#define STM32_FLASH_NPAGES (BOARD_FLASH_SIZE/2) +#define STM32_FLASH_FIXED_PAGE_SIZE 2 #else #error "Unsupported processor for flash.c" #endif @@ -400,6 +403,13 @@ bool stm32_flash_erasepage(uint32_t page) // use 32 bit operations FLASH->CR = FLASH_CR_PSIZE_1 | snb | FLASH_CR_SER; FLASH->CR |= FLASH_CR_STRT; +#elif defined(STM32G4) + FLASH->CR = FLASH_CR_PER; + // rather oddly, PNB is a 7 bit field that the ref manual says can + // contain 8 bits we assume that for 512k single bank devices + // there is an 8th bit + FLASH->CR |= page<CR |= FLASH_CR_STRT; #else #error "Unsupported MCU" #endif @@ -679,6 +689,83 @@ failed: } #endif // STM32F1 or STM32F3 +#if defined(STM32G4) +static bool stm32_flash_write_g4(uint32_t addr, const void *buf, uint32_t count) +{ + uint32_t *b = (uint32_t *)buf; + + /* STM32G4 requires double-word access */ + if ((count & 7) || (addr & 7)) { + _flash_fail_line = __LINE__; + return false; + } + + if ((addr+count) >= STM32_FLASH_BASE+STM32_FLASH_SIZE) { + _flash_fail_line = __LINE__; + return false; + } + + // skip already programmed word pairs + while (count >= 8 && + getreg32(addr+0) == b[0] && + getreg32(addr+4) == b[1]) { + count -= 8; + addr += 8; + b += 2; + } + if (count == 0) { + return true; + } + + +#if STM32_FLASH_DISABLE_ISR + syssts_t sts = chSysGetStatusAndLockX(); +#endif + + stm32_flash_unlock(); + + stm32_flash_wait_idle(); + + // program in 16 bit steps + while (count >= 2) { + FLASH->CR = FLASH_CR_PG; + + putreg32(b[0], addr+0); + putreg32(b[1], addr+4); + + stm32_flash_wait_idle(); + + FLASH->CR = 0; + + if (getreg32(addr+0) != b[0] || + getreg32(addr+4) != b[1]) { + _flash_fail_line = __LINE__; + _flash_fail_addr = addr; + _flash_fail_count = count; + _flash_fail_buf = (uint8_t *)b; + goto failed; + } + + count -= 8; + b += 2; + addr += 8; + } + + stm32_flash_lock(); +#if STM32_FLASH_DISABLE_ISR + chSysRestoreStatusX(sts); +#endif + return true; + +failed: + stm32_flash_lock(); +#if STM32_FLASH_DISABLE_ISR + chSysRestoreStatusX(sts); +#endif + return false; +} +#endif // STM32G4 + bool stm32_flash_write(uint32_t addr, const void *buf, uint32_t count) { #if defined(STM32F1) || defined(STM32F3) @@ -687,6 +774,8 @@ bool stm32_flash_write(uint32_t addr, const void *buf, uint32_t count) return stm32_flash_write_f4f7(addr, buf, count); #elif defined(STM32H7) return stm32_flash_write_h7(addr, buf, count); +#elif defined(STM32G4) + return stm32_flash_write_g4(addr, buf, count); #else #error "Unsupported MCU" #endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h index 8900caa486..7b03805f9d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h @@ -45,6 +45,8 @@ #include "stm32f47_mcuconf.h" #elif defined(STM32H7) #include "stm32h7_mcuconf.h" +#elif defined(STM32G4) +#include "stm32g4_mcuconf.h" #else #error "Unsupported MCU" #endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c index a75bef595d..fd111c9f22 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c @@ -218,6 +218,8 @@ void get_rtc_backup(uint8_t idx, uint32_t *v, uint8_t n) #if defined(STM32F1) __IO uint32_t *dr = (__IO uint32_t *)&BKP->DR1; *v++ = (dr[n/2]&0xFFFF) | (dr[n/2+1]<<16); +#elif defined(STM32G4) + *v++ = ((__IO uint32_t *)&TAMP->BKP0R)[idx++]; #else *v++ = ((__IO uint32_t *)&RTC->BKP0R)[idx++]; #endif @@ -243,6 +245,8 @@ void set_rtc_backup(uint8_t idx, const uint32_t *v, uint8_t n) __IO uint32_t *dr = (__IO uint32_t *)&BKP->DR1; dr[n/2] = (*v) & 0xFFFF; dr[n/2+1] = (*v) >> 16; +#elif defined(STM32G4) + ((__IO uint32_t *)&TAMP->BKP0R)[idx++] = *v++; #else ((__IO uint32_t *)&RTC->BKP0R)[idx++] = *v++; #endif @@ -320,7 +324,7 @@ void peripheral_power_enable(void) #endif } -#if defined(STM32F7) || defined(STM32H7) || defined(STM32F4) || defined(STM32F3) +#if defined(STM32F7) || defined(STM32H7) || defined(STM32F4) || defined(STM32F3) || defined(STM32G4) /* read mode of a pin. This allows a pin config to be read, changed and then written back diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h index 004f68a405..7d6274650a 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h @@ -87,7 +87,7 @@ void malloc_init(void); read mode of a pin. This allows a pin config to be read, changed and then written back */ -#if defined(STM32F7) || defined(STM32H7) || defined(STM32F4) || defined(STM32F3) +#if defined(STM32F7) || defined(STM32H7) || defined(STM32F4) || defined(STM32F3) || defined(STM32G4) iomode_t palReadLineMode(ioline_t line); enum PalPushPull { diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32g4_mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32g4_mcuconf.h new file mode 100644 index 0000000000..d1d69d137b --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32g4_mcuconf.h @@ -0,0 +1,295 @@ +/* + 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. +*/ +/* + this header is modelled on the one for the Nucleo64 G474 board from ChibiOS + */ +#pragma once + +#ifndef STM32_LSECLK +#define STM32_LSECLK 32768U +#endif +#ifndef STM32_LSEDRV +#define STM32_LSEDRV (3U << 3U) +#endif + +/* + * STM32G4xx drivers configuration. + * The following settings override the default settings present in + * the various device driver implementation headers. + * Note that the settings for each driver only have effect if the whole + * driver is enabled in halconf.h. + * + * IRQ priorities: + * 15...0 Lowest...Highest. + * + * DMA priorities: + * 0...3 Lowest...Highest. + */ + +#ifndef MCUCONF_H +#define MCUCONF_H + +#define STM32G4xx_MCUCONF +#define STM32G473_MCUCONF +#define STM32G483_MCUCONF +#define STM32G474_MCUCONF +#define STM32G484_MCUCONF + +#if STM32_HSECLK == 0U +// no crystal +#define STM32_HSE_ENABLED FALSE +#define STM32_HSI16_ENABLED TRUE +#define STM32_PLLM_VALUE 2 +#define STM32_PLLSRC STM32_PLLSRC_HSI16 + +#elif STM32_HSECLK == 8000000U +#define STM32_HSE_ENABLED TRUE +#define STM32_HSI16_ENABLED FALSE +#define STM32_PLLM_VALUE 1 +#define STM32_PLLSRC STM32_PLLSRC_HSE + +#elif STM32_HSECLK == 16000000U +#define STM32_HSE_ENABLED TRUE +#define STM32_HSI16_ENABLED FALSE +#define STM32_PLLM_VALUE 2 +#define STM32_PLLSRC STM32_PLLSRC_HSE + +#elif STM32_HSECLK == 24000000U +#define STM32_HSE_ENABLED TRUE +#define STM32_HSI16_ENABLED FALSE +#define STM32_PLLM_VALUE 3 +#define STM32_PLLSRC STM32_PLLSRC_HSE + +#else +#error "Unsupported HSE clock" +#endif + +/* + * HAL driver system settings. + */ +#define STM32_NO_INIT FALSE +#define STM32_VOS STM32_VOS_RANGE1 +#define STM32_PWR_CR2 (PWR_CR2_PLS_LEV0) +#define STM32_PWR_CR3 (PWR_CR3_EIWF) +#define STM32_PWR_CR4 (0U) +#define STM32_HSI48_ENABLED TRUE +#define STM32_LSI_ENABLED FALSE +#define STM32_LSE_ENABLED FALSE +#define STM32_SW STM32_SW_PLLRCLK +#define STM32_PLLN_VALUE 42 +#define STM32_PLLPDIV_VALUE 0 +#define STM32_PLLP_VALUE 7 +#define STM32_PLLQ_VALUE 8 +#define STM32_PLLR_VALUE 2 +#define STM32_HPRE STM32_HPRE_DIV1 +#define STM32_PPRE1 STM32_PPRE1_DIV1 +#define STM32_PPRE2 STM32_PPRE2_DIV1 +#define STM32_MCOSEL STM32_MCOSEL_NOCLOCK +#define STM32_MCOPRE STM32_MCOPRE_DIV1 +#define STM32_LSCOSEL STM32_LSCOSEL_NOCLOCK + +/* + * Peripherals clock sources. + */ +#define STM32_USART1SEL STM32_USART1SEL_SYSCLK +#define STM32_USART2SEL STM32_USART2SEL_SYSCLK +#define STM32_USART3SEL STM32_USART3SEL_SYSCLK +#define STM32_UART4SEL STM32_UART4SEL_SYSCLK +#define STM32_UART5SEL STM32_UART5SEL_SYSCLK +#define STM32_LPUART1SEL STM32_LPUART1SEL_PCLK1 +#define STM32_I2C1SEL STM32_I2C1SEL_PCLK1 +#define STM32_I2C2SEL STM32_I2C2SEL_PCLK1 +#define STM32_I2C3SEL STM32_I2C3SEL_PCLK1 +#define STM32_I2C4SEL STM32_I2C4SEL_PCLK1 +#define STM32_LPTIM1SEL STM32_LPTIM1SEL_PCLK1 +#define STM32_SAI1SEL STM32_SAI1SEL_SYSCLK +#define STM32_I2S23SEL STM32_I2S23SEL_SYSCLK +#define STM32_FDCANSEL STM32_FDCANSEL_HSE // STM32_FDCANSEL_PLLQCLK +#define STM32_CLK48SEL STM32_CLK48SEL_HSI48 +#define STM32_ADC12SEL STM32_ADC12SEL_PLLPCLK +#define STM32_ADC345SEL STM32_ADC345SEL_PLLPCLK +#define STM32_QSPISEL STM32_QSPISEL_SYSCLK +#define STM32_RTCSEL STM32_RTCSEL_NOCLOCK + +/* + * IRQ system settings. + */ +#define STM32_IRQ_EXTI0_PRIORITY 6 +#define STM32_IRQ_EXTI1_PRIORITY 6 +#define STM32_IRQ_EXTI2_PRIORITY 6 +#define STM32_IRQ_EXTI3_PRIORITY 6 +#define STM32_IRQ_EXTI4_PRIORITY 6 +#define STM32_IRQ_EXTI5_9_PRIORITY 6 +#define STM32_IRQ_EXTI10_15_PRIORITY 6 +#define STM32_IRQ_EXTI164041_PRIORITY 6 +#define STM32_IRQ_EXTI17_PRIORITY 6 +#define STM32_IRQ_EXTI18_PRIORITY 6 +#define STM32_IRQ_EXTI19_PRIORITY 6 +#define STM32_IRQ_EXTI20_PRIORITY 6 +#define STM32_IRQ_EXTI212229_PRIORITY 6 +#define STM32_IRQ_EXTI30_32_PRIORITY 6 +#define STM32_IRQ_EXTI33_PRIORITY 6 + +#define STM32_IRQ_FDCAN1_PRIORITY 10 +#define STM32_IRQ_FDCAN2_PRIORITY 10 +#define STM32_IRQ_FDCAN3_PRIORITY 10 + +#define STM32_IRQ_TIM1_BRK_TIM15_PRIORITY 7 +#define STM32_IRQ_TIM1_UP_TIM16_PRIORITY 7 +#define STM32_IRQ_TIM1_TRGCO_TIM17_PRIORITY 7 +#define STM32_IRQ_TIM1_CC_PRIORITY 7 +#define STM32_IRQ_TIM2_PRIORITY 7 +#define STM32_IRQ_TIM3_PRIORITY 7 +#define STM32_IRQ_TIM4_PRIORITY 7 +#define STM32_IRQ_TIM5_PRIORITY 7 +#define STM32_IRQ_TIM6_PRIORITY 7 +#define STM32_IRQ_TIM7_PRIORITY 7 +#define STM32_IRQ_TIM8_UP_PRIORITY 7 +#define STM32_IRQ_TIM8_CC_PRIORITY 7 +#define STM32_IRQ_TIM20_UP_PRIORITY 7 +#define STM32_IRQ_TIM20_CC_PRIORITY 7 + +#define STM32_IRQ_USART1_PRIORITY 12 +#define STM32_IRQ_USART2_PRIORITY 12 +#define STM32_IRQ_USART3_PRIORITY 12 +#define STM32_IRQ_UART4_PRIORITY 12 +#define STM32_IRQ_UART5_PRIORITY 12 +#define STM32_IRQ_LPUART1_PRIORITY 12 + +/* + * ADC driver system settings. + */ +#define STM32_ADC_DUAL_MODE FALSE +#define STM32_ADC_COMPACT_SAMPLES FALSE +#define STM32_ADC_ADC1_DMA_PRIORITY 2 +#define STM32_ADC_ADC2_DMA_PRIORITY 2 +#define STM32_ADC_ADC3_DMA_PRIORITY 2 +#define STM32_ADC_ADC4_DMA_PRIORITY 2 +#define STM32_ADC_ADC12_IRQ_PRIORITY 5 +#define STM32_ADC_ADC3_IRQ_PRIORITY 5 +#define STM32_ADC_ADC4_IRQ_PRIORITY 5 +#define STM32_ADC_ADC1_DMA_IRQ_PRIORITY 5 +#define STM32_ADC_ADC2_DMA_IRQ_PRIORITY 5 +#define STM32_ADC_ADC3_DMA_IRQ_PRIORITY 5 +#define STM32_ADC_ADC4_DMA_IRQ_PRIORITY 5 +#define STM32_ADC_ADC12_CLOCK_MODE ADC_CCR_CKMODE_AHB_DIV4 +#define STM32_ADC_ADC345_CLOCK_MODE ADC_CCR_CKMODE_AHB_DIV4 +#define STM32_ADC_ADC12_PRESC ADC_CCR_PRESC_DIV2 +#define STM32_ADC_ADC345_PRESC ADC_CCR_PRESC_DIV2 + +/* + * CAN driver system settings. + */ +#define STM32_CAN_USE_FDCAN1 FALSE +#define STM32_CAN_USE_FDCAN2 FALSE +#define STM32_CAN_USE_FDCAN3 FALSE + +/* + * DAC driver system settings. + */ +#define STM32_DAC_DUAL_MODE FALSE +#define STM32_DAC_USE_DAC1_CH1 FALSE +#define STM32_DAC_USE_DAC1_CH2 FALSE +#define STM32_DAC_USE_DAC2_CH1 FALSE +#define STM32_DAC_USE_DAC3_CH1 FALSE +#define STM32_DAC_USE_DAC3_CH2 FALSE +#define STM32_DAC_USE_DAC4_CH1 FALSE +#define STM32_DAC_USE_DAC4_CH2 FALSE +#define STM32_DAC_DAC1_CH1_IRQ_PRIORITY 10 +#define STM32_DAC_DAC1_CH2_IRQ_PRIORITY 10 +#define STM32_DAC_DAC2_CH1_IRQ_PRIORITY 10 +#define STM32_DAC_DAC3_CH1_IRQ_PRIORITY 10 +#define STM32_DAC_DAC3_CH2_IRQ_PRIORITY 10 +#define STM32_DAC_DAC4_CH1_IRQ_PRIORITY 10 +#define STM32_DAC_DAC4_CH2_IRQ_PRIORITY 10 +#define STM32_DAC_DAC1_CH1_DMA_PRIORITY 2 +#define STM32_DAC_DAC1_CH2_DMA_PRIORITY 2 +#define STM32_DAC_DAC2_CH1_DMA_PRIORITY 2 +#define STM32_DAC_DAC3_CH1_DMA_PRIORITY 2 +#define STM32_DAC_DAC3_CH2_DMA_PRIORITY 2 +#define STM32_DAC_DAC4_CH1_DMA_PRIORITY 2 +#define STM32_DAC_DAC4_CH2_DMA_PRIORITY 2 + +/* + * I2C driver system settings. + */ +#define STM32_I2C_BUSY_TIMEOUT 50 +#define STM32_I2C_I2C1_IRQ_PRIORITY 5 +#define STM32_I2C_I2C2_IRQ_PRIORITY 5 +#define STM32_I2C_I2C3_IRQ_PRIORITY 5 +#define STM32_I2C_I2C4_IRQ_PRIORITY 5 +#define STM32_I2C_I2C1_DMA_PRIORITY 3 +#define STM32_I2C_I2C2_DMA_PRIORITY 3 +#define STM32_I2C_I2C3_DMA_PRIORITY 3 +#define STM32_I2C_I2C4_DMA_PRIORITY 3 +#define STM32_I2C_DMA_ERROR_HOOK(i2cp) osalSysHalt("DMA failure") + +/* + * SPI driver system settings. + */ +#define STM32_SPI_SPI1_DMA_PRIORITY 1 +#define STM32_SPI_SPI2_DMA_PRIORITY 1 +#define STM32_SPI_SPI3_DMA_PRIORITY 1 +#define STM32_SPI_SPI4_DMA_PRIORITY 1 +#define STM32_SPI_SPI1_IRQ_PRIORITY 10 +#define STM32_SPI_SPI2_IRQ_PRIORITY 10 +#define STM32_SPI_SPI3_IRQ_PRIORITY 10 +#define STM32_SPI_SPI4_IRQ_PRIORITY 10 +#define STM32_SPI_DMA_ERROR_HOOK(spip) osalSysHalt("DMA failure") + +/* + * ST driver system settings. + */ +#define STM32_ST_IRQ_PRIORITY 8 + +#ifndef STM32_ST_USE_TIMER +#define STM32_ST_USE_TIMER 2 +#endif + +/* + * TRNG driver system settings. + */ +#define STM32_TRNG_USE_RNG1 FALSE + +/* + * UART driver system settings. + */ +#define STM32_UART_USART1_DMA_PRIORITY 0 +#define STM32_UART_USART2_DMA_PRIORITY 0 +#define STM32_UART_USART3_DMA_PRIORITY 0 +#define STM32_UART_UART4_DMA_PRIORITY 0 +#define STM32_UART_UART5_DMA_PRIORITY 0 +#define STM32_UART_DMA_ERROR_HOOK(uartp) osalSysHalt("DMA failure") + +/* + * USB driver system settings. + */ +#define STM32_USB_USE_USB1 FALSE +#define STM32_USB_LOW_POWER_ON_SUSPEND FALSE +#define STM32_USB_USB1_HP_IRQ_PRIORITY 13 +#define STM32_USB_USB1_LP_IRQ_PRIORITY 14 + +/* + * WDG driver system settings. + */ +#define STM32_WDG_USE_IWDG FALSE + +/* + * WSPI driver system settings. + */ +#define STM32_WSPI_USE_QUADSPI1 FALSE + +#endif /* MCUCONF_H */ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c index a450643d20..8b56b7a080 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c @@ -37,6 +37,10 @@ #define WDG_RESET_STATUS (*(__IO uint32_t *)(RCC_BASE + 0x24)) #define WDG_RESET_CLEAR (1U<<24) #define WDG_RESET_IS_IWDG (1U<<29) +#elif defined(STM32G4) +#define WDG_RESET_STATUS (*(__IO uint32_t *)(RCC_BASE + 0x94)) +#define WDG_RESET_CLEAR (1U<<23) +#define WDG_RESET_IS_IWDG (1U<<29) #else #error "Unsupported IWDG MCU config" #endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32G474xx.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32G474xx.py new file mode 100644 index 0000000000..fd88f97889 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32G474xx.py @@ -0,0 +1,406 @@ +#!/usr/bin/env python +''' +tables for STM32G474xx MCUs +''' + +# additional build information for ChibiOS +build = { + "CHIBIOS_STARTUP_MK" : "os/common/startup/ARMCMx/compilers/GCC/mk/startup_stm32g4xx.mk", + "CHIBIOS_PLATFORM_MK" : "os/hal/ports/STM32/STM32G4xx/platform.mk" + } + +# MCU parameters +mcu = { + # location of MCU serial number + 'UDID_START' : 0x1FFF7590, + + # ram map, as list of (address, size-kb, flags) + # flags of 1 means DMA-capable (DMA and BDMA) + # flags of 2 means faster memory for CPU intensive work + # flags of 4 means memory can be used for SDMMC DMA + 'RAM_MAP' : [ + (0x20000000, 96, 1), # SRAM1/SRAM2 + (0x10000000, 32, 2), # SRAM1, SRAM2 + ], + + 'EXPECTED_CLOCK' : 168000000, +} + +# max pin package is 128 +pincount = { + 'A': 16, + 'B': 16, + 'C': 16, + 'D': 16, + 'E': 16, + 'F': 16, + 'G': 16, +} + +# no DMA map as we will dynamically allocate DMA channels using the DMAMUX +DMA_Map = None + +AltFunction_map = { + # format is PIN:FUNCTION : AFNUM + # extracted from g4.csv + "PA0:COMP1_OUT" : 8, + "PA0:EVENTOUT" : 15, + "PA0:TIM2_CH1" : 1, + "PA0:TIM2_ETR" : 14, + "PA0:TIM5_CH1" : 2, + "PA0:TIM8_BKIN" : 9, + "PA0:TIM8_ETR" : 10, + "PA0:USART2_CTS" : 7, + "PA0:USART2_NSS" : 7, + "PA1:EVENTOUT" : 15, + "PA1:RTC_REFIN" : 0, + "PA1:TIM15_CH1N" : 9, + "PA1:TIM2_CH2" : 1, + "PA1:TIM5_CH2" : 2, + "PA1:USART2_DE" : 7, + "PA1:USART2_RTS" : 7, + "PA2:COMP2_OUT" : 8, + "PA2:EVENTOUT" : 15, + "PA2:LPUART1_TX" : 12, + "PA2:QUADSPI1_BK1_NCS" : 10, + "PA2:TIM15_CH1" : 9, + "PA2:TIM2_CH3" : 1, + "PA2:TIM5_CH3" : 2, + "PA2:UCPD1_FRSTX1" : 14, + "PA2:UCPD1_FRSTX2" : 14, + "PA2:USART2_TX" : 7, + "PA3:EVENTOUT" : 15, + "PA3:LPUART1_RX" : 12, + "PA3:QUADSPI1_CLK" : 10, + "PA3:SAI1_CK1" : 3, + "PA3:SAI1_MCLK_A" : 13, + "PA3:TIM15_CH2" : 9, + "PA3:TIM2_CH4" : 1, + "PA3:TIM5_CH4" : 2, + "PA3:USART2_RX" : 7, + "PA4:EVENTOUT" : 15, + "PA4:I2S3_WS" : 6, + "PA4:SAI1_FS_B" : 13, + "PA4:SPI1_NSS" : 5, + "PA4:SPI3_NSS" : 6, + "PA4:TIM3_CH2" : 2, + "PA4:USART2_CK" : 7, + "PA5:EVENTOUT" : 15, + "PA5:SPI1_SCK" : 5, + "PA5:TIM2_CH1" : 1, + "PA5:TIM2_ETR" : 2, + "PA5:UCPD1_FRSTX1" : 14, + "PA5:UCPD1_FRSTX2" : 14, + "PA6:COMP1_OUT" : 8, + "PA6:EVENTOUT" : 15, + "PA6:LPUART1_CTS" : 12, + "PA6:QUADSPI1_BK1_IO3" : 10, + "PA6:SPI1_MISO" : 5, + "PA6:TIM16_CH1" : 1, + "PA6:TIM1_BKIN" : 6, + "PA6:TIM3_CH1" : 2, + "PA6:TIM8_BKIN" : 4, + "PA7:COMP2_OUT" : 8, + "PA7:EVENTOUT" : 15, + "PA7:QUADSPI1_BK1_IO2" : 10, + "PA7:SPI1_MOSI" : 5, + "PA7:TIM17_CH1" : 1, + "PA7:TIM1_CH1N" : 6, + "PA7:TIM3_CH2" : 2, + "PA7:TIM8_CH1N" : 4, + "PA7:UCPD1_FRSTX1" : 14, + "PA7:UCPD1_FRSTX2" : 14, + "PA8:COMP7_OUT" : 8, + "PA8:EVENTOUT" : 15, + "PA8:CAN3_RX" : 11, + "PA8:HRTIM1_CHA1" : 13, + "PA8:I2C2_SDA" : 4, + "PA8:I2C3_SCL" : 2, + "PA8:I2S2_MCK" : 5, + "PA8:RCC_MCO" : 0, + "PA8:SAI1_CK2" : 12, + "PA8:SAI1_SCK_A" : 14, + "PA8:TIM1_CH1" : 6, + "PA8:TIM4_ETR" : 10, + "PA8:USART1_CK" : 7, + "PA9:COMP5_OUT" : 8, + "PA9:EVENTOUT" : 15, + "PA9:HRTIM1_CHA2" : 13, + "PA9:I2C2_SCL" : 4, + "PA9:I2C3_SMBA" : 2, + "PA9:I2S3_MCK" : 5, + "PA9:SAI1_FS_A" : 14, + "PA9:TIM15_BKIN" : 9, + "PA9:TIM1_CH2" : 6, + "PA9:TIM2_CH3" : 10, + "PA9:USART1_TX" : 7, + "PA10:COMP6_OUT" : 8, + "PA10:CRS_SYNC" : 3, + "PA10:EVENTOUT" : 15, + "PA10:HRTIM1_CHB1" : 13, + "PA10:I2C2_SMBA" : 4, + "PA10:SAI1_D1" : 12, + "PA10:SAI1_SD_A" : 14, + "PA10:SPI2_MISO" : 5, + "PA10:TIM17_BKIN" : 1, + "PA10:TIM1_CH3" : 6, + "PA10:TIM2_CH4" : 10, + "PA10:TIM8_BKIN" : 11, + "PA10:USART1_RX" : 7, + "PA11:COMP1_OUT" : 8, + "PA11:EVENTOUT" : 15, + "PA11:CAN1_RX" : 9, + "PA11:HRTIM1_CHB2" : 13, + "PA11:I2S2_SD" : 5, + "PA11:SPI2_MOSI" : 5, + "PA11:TIM1_BKIN2" : 12, + "PA11:TIM1_CH1N" : 6, + "PA11:TIM1_CH4" : 11, + "PA11:TIM4_CH1" : 10, + "PA11:USART1_CTS" : 7, + "PA11:USART1_NSS" : 7, + "PA12:COMP2_OUT" : 8, + "PA12:EVENTOUT" : 15, + "PA12:CAN1_TX" : 9, + "PA12:HRTIM1_FLT1" : 13, + "PA12:I2S_CKIN" : 5, + "PA12:TIM16_CH1" : 1, + "PA12:TIM1_CH2N" : 6, + "PA12:TIM1_ETR" : 11, + "PA12:TIM4_CH2" : 10, + "PA12:USART1_DE" : 7, + "PA12:USART1_RTS" : 7, + "PA13:EVENTOUT" : 15, + "PA13:I2C1_SCL" : 4, + "PA13:I2C4_SCL" : 3, + "PA13:IR_OUT" : 5, + "PA13:SAI1_SD_B" : 13, + "PA13:JTMS-SWDIO" : 0, + "PA13:TIM16_CH1N" : 1, + "PA13:TIM4_CH3" : 10, + "PA13:USART3_CTS" : 7, + "PA13:USART3_NSS" : 7, + "PA14:EVENTOUT" : 15, + "PA14:I2C1_SDA" : 4, + "PA14:I2C4_SMBA" : 3, + "PA14:LPTIM1_OUT" : 1, + "PA14:SAI1_FS_B" : 13, + "PA14:JTCK-SWCLK" : 0, + "PA14:TIM1_BKIN" : 6, + "PA14:TIM8_CH2" : 5, + "PA14:USART2_TX" : 7, + "PA15:EVENTOUT" : 15, + "PA15:CAN3_TX" : 11, + "PA15:HRTIM1_FLT2" : 13, + "PA15:I2C1_SCL" : 4, + "PA15:I2S3_WS" : 6, + "PA15:SPI1_NSS" : 5, + "PA15:SPI3_NSS" : 6, + "PA15:SYS_JTDI" : 0, + "PA15:TIM1_BKIN" : 9, + "PA15:TIM2_CH1" : 1, + "PA15:TIM2_ETR" : 14, + "PA15:TIM8_CH1" : 2, + "PA15:USART2_RX" : 7, + "PB0:EVENTOUT" : 15, + "PB0:HRTIM1_FLT5" : 13, + "PB0:QUADSPI1_BK1_IO1" : 10, + "PB0:TIM1_CH2N" : 6, + "PB0:TIM3_CH3" : 2, + "PB0:TIM8_CH2N" : 4, + "PB0:UCPD1_FRSTX1" : 14, + "PB0:UCPD1_FRSTX2" : 14, + "PB1:COMP4_OUT" : 8, + "PB1:EVENTOUT" : 15, + "PB1:HRTIM1_SCOUT" : 13, + "PB1:LPUART1_DE" : 12, + "PB1:LPUART1_RTS" : 12, + "PB1:QUADSPI1_BK1_IO0" : 10, + "PB1:TIM1_CH3N" : 6, + "PB1:TIM3_CH4" : 2, + "PB1:TIM8_CH3N" : 4, + "PB2:EVENTOUT" : 15, + "PB2:HRTIM1_SCIN" : 13, + "PB2:I2C3_SMBA" : 4, + "PB2:LPTIM1_OUT" : 1, + "PB2:QUADSPI1_BK2_IO1" : 10, + "PB2:RTC_OUT2" : 0, + "PB2:TIM20_CH1" : 3, + "PB2:TIM5_CH1" : 2, + "PB3:CRS_SYNC" : 3, + "PB3:EVENTOUT" : 15, + "PB3:CAN3_RX" : 11, + "PB3:HRTIM1_EEV9" : 13, + "PB3:HRTIM1_SCOUT" : 12, + "PB3:I2S3_CK" : 6, + "PB3:SAI1_SCK_B" : 14, + "PB3:SPI1_SCK" : 5, + "PB3:SPI3_SCK" : 6, + "PB3:SYS_JTDO-SWO" : 0, + "PB3:TIM2_CH2" : 1, + "PB3:TIM3_ETR" : 10, + "PB3:TIM4_ETR" : 2, + "PB3:TIM8_CH1N" : 4, + "PB3:USART2_TX" : 7, + "PB4:EVENTOUT" : 15, + "PB4:CAN3_TX" : 11, + "PB4:HRTIM1_EEV7" : 13, + "PB4:SAI1_MCLK_B" : 14, + "PB4:SPI1_MISO" : 5, + "PB4:SPI3_MISO" : 6, + "PB4:SYS_JTRST" : 0, + "PB4:TIM16_CH1" : 1, + "PB4:TIM17_BKIN" : 10, + "PB4:TIM3_CH1" : 2, + "PB4:TIM8_CH2N" : 4, + "PB4:USART2_RX" : 7, + "PB5:EVENTOUT" : 15, + "PB5:CAN2_RX" : 9, + "PB5:HRTIM1_EEV6" : 13, + "PB5:I2C1_SMBA" : 4, + "PB5:I2C3_SDA" : 8, + "PB5:I2S3_SD" : 6, + "PB5:LPTIM1_IN1" : 11, + "PB5:SAI1_SD_B" : 12, + "PB5:SPI1_MOSI" : 5, + "PB5:SPI3_MOSI" : 6, + "PB5:TIM16_BKIN" : 1, + "PB5:TIM17_CH1" : 10, + "PB5:TIM3_CH2" : 2, + "PB5:TIM8_CH3N" : 3, + "PB5:USART2_CK" : 7, + "PB6:COMP4_OUT" : 8, + "PB6:EVENTOUT" : 15, + "PB6:CAN2_TX" : 9, + "PB6:HRTIM1_EEV4" : 13, + "PB6:HRTIM1_SCIN" : 12, + "PB6:LPTIM1_ETR" : 11, + "PB6:SAI1_FS_B" : 14, + "PB6:TIM16_CH1N" : 1, + "PB6:TIM4_CH1" : 2, + "PB6:TIM8_BKIN2" : 10, + "PB6:TIM8_CH1" : 5, + "PB6:TIM8_ETR" : 6, + "PB6:USART1_TX" : 7, + "PB7:COMP3_OUT" : 8, + "PB7:EVENTOUT" : 15, + "PB7:HRTIM1_EEV3" : 13, + "PB7:I2C1_SDA" : 4, + "PB7:I2C4_SDA" : 3, + "PB7:LPTIM1_IN2" : 11, + "PB7:TIM17_CH1N" : 1, + "PB7:TIM3_CH4" : 10, + "PB7:TIM4_CH2" : 2, + "PB7:TIM8_BKIN" : 5, + "PB7:USART1_RX" : 7, + "PB8:COMP1_OUT" : 8, + "PB8:EVENTOUT" : 15, + "PB8:CAN1_RX" : 9, + "PB8:HRTIM1_EEV8" : 13, + "PB8:I2C1_SCL" : 4, + "PB8:SAI1_CK1" : 3, + "PB8:SAI1_MCLK_A" : 14, + "PB8:TIM16_CH1" : 1, + "PB8:TIM1_BKIN" : 12, + "PB8:TIM4_CH3" : 2, + "PB8:TIM8_CH2" : 10, + "PB8:USART3_RX" : 7, + "PB9:COMP2_OUT" : 8, + "PB9:EVENTOUT" : 15, + "PB9:CAN1_TX" : 9, + "PB9:HRTIM1_EEV5" : 13, + "PB9:I2C1_SDA" : 4, + "PB9:IR_OUT" : 6, + "PB9:SAI1_D2" : 3, + "PB9:SAI1_FS_A" : 14, + "PB9:TIM17_CH1" : 1, + "PB9:TIM1_CH3N" : 12, + "PB9:TIM4_CH4" : 2, + "PB9:TIM8_CH3" : 10, + "PB9:USART3_TX" : 7, + "PB10:EVENTOUT" : 15, + "PB10:HRTIM1_FLT3" : 13, + "PB10:LPUART1_RX" : 8, + "PB10:QUADSPI1_CLK" : 10, + "PB10:SAI1_SCK_A" : 14, + "PB10:TIM1_BKIN" : 12, + "PB10:TIM2_CH3" : 1, + "PB10:USART3_TX" : 7, + "PB11:EVENTOUT" : 15, + "PB11:HRTIM1_FLT4" : 13, + "PB11:LPUART1_TX" : 8, + "PB11:QUADSPI1_BK1_NCS" : 10, + "PB11:TIM2_CH4" : 1, + "PB11:USART3_RX" : 7, + "PB12:EVENTOUT" : 15, + "PB12:CAN2_RX" : 9, + "PB12:HRTIM1_CHC1" : 13, + "PB12:I2C2_SMBA" : 4, + "PB12:I2S2_WS" : 5, + "PB12:LPUART1_DE" : 8, + "PB12:LPUART1_RTS" : 8, + "PB12:SPI2_NSS" : 5, + "PB12:TIM1_BKIN" : 6, + "PB12:TIM5_ETR" : 2, + "PB12:USART3_CK" : 7, + "PB13:EVENTOUT" : 15, + "PB13:CAN2_TX" : 9, + "PB13:HRTIM1_CHC2" : 13, + "PB13:I2S2_CK" : 5, + "PB13:LPUART1_CTS" : 8, + "PB13:SPI2_SCK" : 5, + "PB13:TIM1_CH1N" : 6, + "PB13:USART3_CTS" : 7, + "PB13:USART3_NSS" : 7, + "PB14:COMP4_OUT" : 8, + "PB14:EVENTOUT" : 15, + "PB14:HRTIM1_CHD1" : 13, + "PB14:SPI2_MISO" : 5, + "PB14:TIM15_CH1" : 1, + "PB14:TIM1_CH2N" : 6, + "PB14:USART3_DE" : 7, + "PB14:USART3_RTS" : 7, + "PB15:COMP3_OUT" : 3, + "PB15:EVENTOUT" : 15, + "PB15:HRTIM1_CHD2" : 13, + "PB15:I2S2_SD" : 5, + "PB15:RTC_REFIN" : 0, + "PB15:SPI2_MOSI" : 5, + "PB15:TIM15_CH1N" : 2, + "PB15:TIM15_CH2" : 1, + "PB15:TIM1_CH3N" : 4, + "PC13:EVENTOUT" : 15, + "PC13:TIM1_BKIN" : 2, + "PC13:TIM1_CH1N" : 4, + "PC13:TIM8_CH4N" : 6, + "PC14:EVENTOUT" : 15, + "PC15:EVENTOUT" : 15, + "PF0:EVENTOUT" : 15, + "PF0:I2C2_SDA" : 4, + "PF0:I2S2_WS" : 5, + "PF0:SPI2_NSS" : 5, + "PF0:TIM1_CH3N" : 6, + "PF1:EVENTOUT" : 15, + "PF1:I2S2_CK" : 5, + "PF1:SPI2_SCK" : 5, + "PG10:EVENTOUT" : 15, + "PG10:RCC_MCO" : 0, +} + +ADC1_map = { + # format is PIN : ADC1_CHAN + "PA0" : 1, + "PA1" : 2, + "PA2" : 3, + "PA3" : 4, + "PB14" : 5, + "PC0" : 6, + "PC1" : 7, + "PC2" : 8, + "PC3" : 9, + "PF0" : 10, + "PB12" : 11, + "PB1" : 12, + "PB11" : 14, + "PB0" : 15, +} diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py index 38a2285d44..7da4f99cfe 100755 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py @@ -127,7 +127,7 @@ def get_sharing_priority(periph_list, priority_list): highest = prio return highest -def generate_DMAMUX_map_mask(peripheral_list, channel_mask, noshare_list, dma_exclude): +def generate_DMAMUX_map_mask(peripheral_list, channel_mask, noshare_list, dma_exclude, stream_ofs): ''' generate a dma map suitable for a board with a DMAMUX @@ -153,7 +153,7 @@ def generate_DMAMUX_map_mask(peripheral_list, channel_mask, noshare_list, dma_ex available &= ~mask dma = (i // 8) + 1 stream = i % 8 - dma_map[p].append((dma,stream,0)) + dma_map[p].append((dma,stream)) idsets[p].add(i) break @@ -203,13 +203,22 @@ def generate_DMAMUX_map_mask(peripheral_list, channel_mask, noshare_list, dma_ex dma_map[p].append((dma,stream)) idsets[p].add(found) idx = (idx+1) % 16 + + if stream_ofs != 0: + # add in stream_ofs to cope with STM32G4 + for p in dma_map.keys(): + for (dma,stream) in dma_map[p]: + map2 = [] + map2.append((dma,stream+stream_ofs)) + dma_map[p] = map2 + if debug: print('dma_map: ', dma_map) print('idsets: ', idsets) print('available: 0x%04x' % available) return dma_map -def generate_DMAMUX_map(peripheral_list, noshare_list, dma_exclude): +def generate_DMAMUX_map(peripheral_list, noshare_list, dma_exclude, stream_ofs): ''' generate a dma map suitable for a board with a DMAMUX1 and DMAMUX2 ''' @@ -221,12 +230,12 @@ def generate_DMAMUX_map(peripheral_list, noshare_list, dma_exclude): dmamux2_peripherals.append(p) else: dmamux1_peripherals.append(p) - map1 = generate_DMAMUX_map_mask(dmamux1_peripherals, 0xFFFF, noshare_list, dma_exclude) + map1 = generate_DMAMUX_map_mask(dmamux1_peripherals, 0xFFFF, noshare_list, dma_exclude, stream_ofs) # there are 8 BDMA channels, but an issue has been found where if I2C4 and SPI6 # use neighboring channels then we sometimes lose a BDMA completion interrupt. To # avoid this we set the BDMA available mask to 0x33, which forces the channels not to be # adjacent. This issue was found on a CUAV-X7, with H743 RevV. - map2 = generate_DMAMUX_map_mask(dmamux2_peripherals, 0x55, noshare_list, dma_exclude) + map2 = generate_DMAMUX_map_mask(dmamux2_peripherals, 0x55, noshare_list, dma_exclude, stream_ofs) # translate entries from map2 to "DMA controller 3", which is used for BDMA for p in map2.keys(): streams = [] @@ -312,7 +321,12 @@ def write_dma_header(f, peripheral_list, mcu_type, dma_exclude=[], # ensure we don't assign dma for TIMx_CH as we share that with TIMx_UP timer_ch_periph = [periph for periph in peripheral_list if "_CH" in periph] dma_exclude += timer_ch_periph - dma_map = generate_DMAMUX_map(peripheral_list, noshare_list, dma_exclude) + if mcu_type.startswith("STM32G4"): + stream_ofs = 1 + else: + stream_ofs = 0 + + dma_map = generate_DMAMUX_map(peripheral_list, noshare_list, dma_exclude, stream_ofs) print("Writing DMA map") unassigned = []