diff --git a/libraries/AP_HAL_ChibiOS/Device.cpp b/libraries/AP_HAL_ChibiOS/Device.cpp index 995ef906fe..4d35af5183 100644 --- a/libraries/AP_HAL_ChibiOS/Device.cpp +++ b/libraries/AP_HAL_ChibiOS/Device.cpp @@ -25,6 +25,10 @@ #include "Util.h" #include "hwdef/common/stm32_util.h" +#ifndef HAL_DEVICE_THREAD_STACK +#define HAL_DEVICE_THREAD_STACK 1024 +#endif + using namespace ChibiOS; static const AP_HAL::HAL &hal = AP_HAL::get_HAL(); @@ -114,7 +118,7 @@ AP_HAL::Device::PeriodicHandle DeviceBus::register_periodic_callback(uint32_t pe break; } - thread_ctx = thread_create_alloc(THD_WORKING_AREA_SIZE(1024), + thread_ctx = thread_create_alloc(THD_WORKING_AREA_SIZE(HAL_DEVICE_THREAD_STACK), name, thread_priority, /* Initial priority. */ DeviceBus::bus_thread, /* Thread function. */ diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp index e0f6e005fa..83ab8c24c6 100644 --- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp +++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp @@ -55,7 +55,7 @@ static Empty::UARTDriver uartGDriver; static Empty::UARTDriver uartHDriver; #endif -#if HAL_USE_I2C == TRUE +#if HAL_USE_I2C == TRUE && defined(HAL_I2C_DEVICE_LIST) static ChibiOS::I2CDeviceManager i2cDeviceManager; #else static Empty::I2CDeviceManager i2cDeviceManager; @@ -176,9 +176,7 @@ static void main_loop() ChibiOS::I2CBus::clear_all(); #endif -#if STM32_DMA_ADVANCED ChibiOS::Shared_DMA::init(); -#endif peripheral_power_enable(); hal.uartA->begin(115200); @@ -217,6 +215,7 @@ static void main_loop() stm32_watchdog_init(); } +#ifndef HAL_NO_LOGGING if (hal.util->was_watchdog_reset()) { AP::internalerror().error(AP_InternalError::error_t::watchdog_reset); const AP_HAL::Util::PersistentData &pd = last_persistent_data; @@ -234,6 +233,7 @@ static void main_loop() pd.fault_thd_prio, pd.fault_icsr); } +#endif #endif schedulerInstance.watchdog_pat(); diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp index e3a7a71639..b289edb3e7 100644 --- a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp +++ b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp @@ -18,7 +18,7 @@ #include #include "Util.h" -#if HAL_USE_I2C == TRUE +#if HAL_USE_I2C == TRUE && defined(HAL_I2C_DEVICE_LIST) #include "Scheduler.h" #include "hwdef/common/stm32_util.h" @@ -85,6 +85,7 @@ void I2CBus::clear_all() */ void I2CBus::clear_bus(uint8_t busidx) { +#if HAL_I2C_CLEAR_ON_TIMEOUT const struct I2CInfo &info = I2CD[busidx]; const iomode_t mode_saved = palReadLineMode(info.scl_line); palSetLineMode(info.scl_line, PAL_MODE_OUTPUT_PUSHPULL); @@ -93,8 +94,10 @@ void I2CBus::clear_bus(uint8_t busidx) hal.scheduler->delay_microseconds(10); } palSetLineMode(info.scl_line, mode_saved); +#endif } +#if HAL_I2C_CLEAR_ON_TIMEOUT /* read SDA on a bus, to check if it may be stuck */ @@ -107,6 +110,7 @@ uint8_t I2CBus::read_sda(uint8_t busidx) palSetLineMode(info.sda_line, mode_saved); return ret; } +#endif // setup I2C buses I2CDeviceManager::I2CDeviceManager(void) @@ -194,7 +198,7 @@ bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len) { if (!bus.semaphore.check_owner()) { - hal.console->printf("I2C: not owner of 0x%x\n", (unsigned)get_bus_id()); + hal.console->printf("I2C: not owner of 0x%x for addr 0x%02x\n", (unsigned)get_bus_id(), _address); return false; } diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp index 42530cc248..38ab3880e5 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp +++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp @@ -251,7 +251,7 @@ void Scheduler::reboot(bool hold_in_bootloader) } #endif -#ifndef NO_LOGGING +#ifndef HAL_NO_LOGGING //stop logging if (AP_Logger::get_singleton()) { AP::logger().StopLogging(); @@ -330,6 +330,7 @@ void Scheduler::_timer_thread(void *arg) } } +#ifndef HAL_NO_MONITOR_THREAD void Scheduler::_monitor_thread(void *arg) { Scheduler *sched = (Scheduler *)arg; @@ -369,6 +370,7 @@ void Scheduler::_monitor_thread(void *arg) } } } +#endif // HAL_NO_MONITOR_THREAD void Scheduler::_rcin_thread(void *arg) { diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index 4bcf0eb77c..d4c31d4c83 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -50,6 +50,18 @@ UARTDriver *UARTDriver::uart_drivers[UART_MAX_DRIVERS]; // caller threads #define EVT_DATA EVENT_MASK(0) +#ifndef HAL_UART_MIN_TX_SIZE +#define HAL_UART_MIN_TX_SIZE 1024 +#endif + +#ifndef HAL_UART_MIN_RX_SIZE +#define HAL_UART_MIN_RX_SIZE 512 +#endif + +#ifndef HAL_UART_STACK_SIZE +#define HAL_UART_STACK_SIZE 2048 +#endif + UARTDriver::UARTDriver(uint8_t _serial_num) : serial_num(_serial_num), sdef(_serial_tab[_serial_num]), @@ -101,7 +113,7 @@ void UARTDriver::thread_init(void) return; } #if CH_CFG_USE_HEAP == TRUE - uart_thread_ctx = thread_create_alloc(THD_WORKING_AREA_SIZE(2048), + uart_thread_ctx = thread_create_alloc(THD_WORKING_AREA_SIZE(HAL_UART_STACK_SIZE), "apm_uart", APM_UART_PRIORITY, uart_thread, @@ -128,8 +140,8 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) if (sdef.serial == nullptr) { return; } - uint16_t min_tx_buffer = 1024; - uint16_t min_rx_buffer = 512; + uint16_t min_tx_buffer = HAL_UART_MIN_TX_SIZE; + uint16_t min_rx_buffer = HAL_UART_MIN_RX_SIZE; if (sdef.is_usb) { // give more buffer space for log download on USB @@ -174,6 +186,7 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) _readbuf.clear(); } +#ifndef HAL_UART_NODMA if (rx_bounce_buf == nullptr) { rx_bounce_buf = (uint8_t *)hal.util->malloc_type(RX_BOUNCE_BUFSIZE, AP_HAL::Util::MEM_DMA_SAFE); } @@ -182,7 +195,8 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) chVTObjectInit(&tx_timeout); tx_bounce_buf_ready = true; } - +#endif + /* allocate the write buffer */ @@ -227,7 +241,8 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) } else { #if HAL_USE_SERIAL == TRUE if (_baudrate != 0) { - bool was_initialised = _device_initialised; +#ifndef HAL_UART_NODMA + bool was_initialised = _device_initialised; //setup Rx DMA if(!_device_initialised) { if(sdef.dma_rx) { @@ -259,6 +274,7 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) } _device_initialised = true; } +#endif // HAL_UART_NODMA sercfg.speed = _baudrate; // start with options from set_options() @@ -266,6 +282,7 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) sercfg.cr2 = _cr2_options; sercfg.cr3 = _cr3_options; +#ifndef HAL_UART_NODMA if (!sdef.dma_tx && !sdef.dma_rx) { } else { if (sdef.dma_rx) { @@ -276,12 +293,14 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) sercfg.cr3 |= USART_CR3_DMAT; } } - sercfg.cr2 |= USART_CR2_STOP1_BITS; sercfg.irq_cb = rx_irq_cb; +#endif // HAL_UART_NODMA + sercfg.cr2 |= USART_CR2_STOP1_BITS; sercfg.ctx = (void*)this; sdStart((SerialDriver*)sdef.serial, &sercfg); +#ifndef HAL_UART_NODMA if(sdef.dma_rx) { //Configure serial driver to skip handling RX packets //because we will handle them via DMA @@ -298,6 +317,7 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) dmaStreamEnable(rxdma); } } +#endif // HAL_UART_NODMA } #endif // HAL_USE_SERIAL } @@ -318,6 +338,7 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) } } +#ifndef HAL_UART_NODMA void UARTDriver::dma_tx_allocate(Shared_DMA *ctx) { #if HAL_USE_SERIAL == TRUE @@ -435,6 +456,7 @@ void UARTDriver::rxbuff_full_irq(void* self, uint32_t flags) } #endif // HAL_USE_SERIAL } +#endif // HAL_UART_NODMA void UARTDriver::begin(uint32_t b) { @@ -665,6 +687,7 @@ bool UARTDriver::wait_timeout(uint16_t n, uint32_t timeout_ms) return available() >= n; } +#ifndef HAL_UART_NODMA /* check for DMA completed for TX */ @@ -754,6 +777,7 @@ void UARTDriver::write_pending_bytes_DMA(uint32_t n) uint32_t timeout_us = ((1000000UL * (tx_len+2) * 10) / _baudrate) + 500; chVTSet(&tx_timeout, chTimeUS2I(timeout_us), handle_tx_timeout, this); } +#endif // HAL_UART_NODMA /* write any pending bytes to the device, non-DMA method @@ -805,9 +829,11 @@ void UARTDriver::write_pending_bytes(void) { uint32_t n; +#ifndef HAL_UART_NODMA if (sdef.dma_tx) { check_dma_tx_completion(); } +#endif // write any pending bytes n = _writebuf.available(); @@ -815,10 +841,13 @@ void UARTDriver::write_pending_bytes(void) return; } - if (!sdef.dma_tx) { - write_pending_bytes_NODMA(n); - } else { +#ifndef HAL_UART_NODMA + if (sdef.dma_tx) { write_pending_bytes_DMA(n); + } else +#endif + { + write_pending_bytes_NODMA(n); } // handle AUTO flow control mode @@ -826,6 +855,7 @@ void UARTDriver::write_pending_bytes(void) if (_first_write_started_us == 0) { _first_write_started_us = AP_HAL::micros(); } +#ifndef HAL_UART_NODMA if (sdef.dma_tx) { // when we are using DMA we have a reliable indication that a write // has completed from the DMA completion interrupt @@ -833,7 +863,9 @@ void UARTDriver::write_pending_bytes(void) _flow_control = FLOW_CONTROL_ENABLE; return; } - } else { + } else +#endif + { // without DMA we need to look at the number of bytes written into the queue versus the // remaining queue space uint32_t space = qSpaceI(&((SerialDriver*)sdef.serial)->oqueue); @@ -876,6 +908,7 @@ void UARTDriver::_timer_tick(void) { if (!_initialised) return; +#ifndef HAL_UART_NODMA if (sdef.dma_rx && rxdma) { _lock_rx_in_timer_tick = true; //Check if DMA is enabled @@ -900,6 +933,7 @@ void UARTDriver::_timer_tick(void) } _lock_rx_in_timer_tick = false; } +#endif // don't try IO on a disconnected USB port if (sdef.is_usb) { @@ -916,7 +950,10 @@ void UARTDriver::_timer_tick(void) } _in_timer = true; - if (!sdef.dma_rx) { +#ifndef HAL_UART_NODMA + if (!sdef.dma_rx) +#endif + { // try to fill the read buffer ByteBuffer::IoVec vec[2]; @@ -1055,12 +1092,16 @@ void UARTDriver::update_rts_line(void) */ bool UARTDriver::set_unbuffered_writes(bool on) { +#ifdef HAL_UART_NODMA + return false; +#else if (on && !sdef.dma_tx) { // we can't implement low latemcy writes safely without TX DMA return false; } unbuffered_writes = on; return true; +#endif } /* @@ -1096,11 +1137,13 @@ void UARTDriver::configure_parity(uint8_t v) } sdStart((SerialDriver*)sdef.serial, &sercfg); +#ifndef HAL_UART_NODMA if(sdef.dma_rx) { //Configure serial driver to skip handling RX packets //because we will handle them via DMA ((SerialDriver*)sdef.serial)->usart->CR1 &= ~USART_CR1_RXNEIE; } +#endif #endif // HAL_USE_SERIAL } @@ -1127,11 +1170,13 @@ void UARTDriver::set_stop_bits(int n) } sdStart((SerialDriver*)sdef.serial, &sercfg); +#ifndef HAL_UART_NODMA if(sdef.dma_rx) { //Configure serial driver to skip handling RX packets //because we will handle them via DMA ((SerialDriver*)sdef.serial)->usart->CR1 &= ~USART_CR1_RXNEIE; } +#endif #endif // HAL_USE_SERIAL } diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h index 4f23e39514..fe75b78636 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.h +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h @@ -63,12 +63,14 @@ public: struct SerialDef { BaseSequentialStream* serial; bool is_usb; +#ifndef HAL_UART_NODMA bool dma_rx; uint8_t dma_rx_stream_id; uint32_t dma_rx_channel_id; bool dma_tx; uint8_t dma_tx_stream_id; - uint32_t dma_tx_channel_id; + uint32_t dma_tx_channel_id; +#endif ioline_t rts_line; int8_t rxinv_gpio; uint8_t rxinv_polarity; @@ -113,7 +115,6 @@ public: } private: - bool tx_bounce_buf_ready; const SerialDef &sdef; // thread used for all UARTs @@ -145,20 +146,27 @@ private: // we use in-task ring buffers to reduce the system call cost // of ::read() and ::write() in the main loop +#ifndef HAL_UART_NODMA + bool tx_bounce_buf_ready; uint8_t *rx_bounce_buf; uint8_t *tx_bounce_buf; +#endif ByteBuffer _readbuf{0}; ByteBuffer _writebuf{0}; Semaphore _write_mutex; +#ifndef HAL_UART_NODMA const stm32_dma_stream_t* rxdma; const stm32_dma_stream_t* txdma; +#endif virtual_timer_t tx_timeout; bool _in_timer; bool _blocking_writes; bool _initialised; bool _device_initialised; bool _lock_rx_in_timer_tick = false; +#ifndef HAL_UART_NODMA Shared_DMA *dma_handle; +#endif static const SerialDef _serial_tab[]; // timestamp for receiving data on the UART, avoiding a lock @@ -186,17 +194,23 @@ private: // set to true for unbuffered writes (low latency writes) bool unbuffered_writes; +#ifndef HAL_UART_NODMA static void rx_irq_cb(void* sd); +#endif static void rxbuff_full_irq(void* self, uint32_t flags); static void tx_complete(void* self, uint32_t flags); static void handle_tx_timeout(void *arg); +#ifndef HAL_UART_NODMA void dma_tx_allocate(Shared_DMA *ctx); void dma_tx_deallocate(Shared_DMA *ctx); +#endif void update_rts_line(void); void check_dma_tx_completion(void); +#ifndef HAL_UART_NODMA void write_pending_bytes_DMA(uint32_t n); +#endif void write_pending_bytes_NODMA(uint32_t n); void write_pending_bytes(void); diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 6f4a6f1727..8c7d2e8369 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -224,7 +224,7 @@ uint64_t Util::get_hw_rtc() const return stm32_get_utc_usec(); } -#ifndef HAL_NO_FLASH_SUPPORT +#if !defined(HAL_NO_FLASH_SUPPORT) && !defined(HAL_NO_ROMFS_SUPPORT) bool Util::flash_bootloader() { @@ -272,7 +272,7 @@ bool Util::flash_bootloader() free(fw); return false; } -#endif //#ifndef HAL_NO_FLASH_SUPPORT +#endif // !HAL_NO_FLASH_SUPPORT && !HAL_NO_ROMFS_SUPPORT /* display system identifer - board type and serial number diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index c664d27055..8fa14ea4c5 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -99,7 +99,7 @@ private: get system clock in UTC microseconds */ uint64_t get_hw_rtc() const override; -#ifndef HAL_NO_FLASH_SUPPORT +#if !defined(HAL_NO_FLASH_SUPPORT) && !defined(HAL_NO_ROMFS_SUPPORT) bool flash_bootloader() override; #endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/board.c b/libraries/AP_HAL_ChibiOS/hwdef/common/board.c index 8c6ff79fbb..49f089b42b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/board.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/board.c @@ -36,7 +36,7 @@ /** * @brief STM32 GPIO static initialization data. */ -#ifdef STM32F100_MCUCONF +#if defined(STM32F100_MCUCONF) || defined(STM32F103_MCUCONF) const PALConfig pal_default_config = { @@ -222,7 +222,7 @@ static void stm32_gpio_init(void) { * and before any other initialization. */ void __early_init(void) { -#ifndef STM32F100_MCUCONF +#if !defined(STM32F100_MCUCONF) && !defined(STM32F103_MCUCONF) stm32_gpio_init(); #endif stm32_clock_init(); diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.c b/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.c index a33262c9a1..596eb995b5 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.c @@ -28,6 +28,8 @@ // on F76x we only consider first half of DTCM memory as DMA safe, 2nd half is used as fast memory for EKF // on F74x we only have 64k of DTCM #define IS_DMA_SAFE(addr) ((((uint32_t)(addr)) & ((0xFFFFFFFF & ~(64*1024U-1)) | 1U)) == 0x20000000) +#elif defined(STM32F1) +#define IS_DMA_SAFE(addr) true #else // this checks an address is in main memory and 16 bit aligned #define IS_DMA_SAFE(addr) ((((uint32_t)(addr)) & 0xF0000001) == 0x20000000) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c index 9b01cdeaa9..8ae0ceb1ce 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c @@ -111,6 +111,9 @@ static const uint32_t flash_memmap[STM32_FLASH_NPAGES] = { KB(32), KB(32), KB(32 #elif defined(STM32H7) #define STM32_FLASH_NPAGES (BOARD_FLASH_SIZE / 128) #define STM32_FLASH_FIXED_PAGE_SIZE 128 +#elif defined(STM32F1) +#define STM32_FLASH_NPAGES BOARD_FLASH_SIZE +#define STM32_FLASH_FIXED_PAGE_SIZE 1 #else #error "Unsupported processor for flash.c" #endif @@ -123,8 +126,13 @@ static bool flash_pageaddr_initialised; static bool flash_keep_unlocked; +#ifndef FLASH_KEY1 #define FLASH_KEY1 0x45670123 +#endif +#ifndef FLASH_KEY2 #define FLASH_KEY2 0xCDEF89AB +#endif + /* Some compiler options will convert short loads and stores into byte loads * and stores. We don't want this to happen for IO reads and writes! */ @@ -354,15 +362,19 @@ bool stm32_flash_erasepage(uint32_t page) FLASH->CR2 |= FLASH_CR_START; while (FLASH->SR2 & FLASH_SR_QW) ; } -#else - stm32_flash_wait_idle(); - +#elif defined(STM32F1) + FLASH->CR = FLASH_CR_PER; + FLASH->AR = stm32_flash_getpageaddr(page); + FLASH->CR |= FLASH_CR_STRT; +#elif defined(STM32F4) || defined(STM32F7) // the snb mask is not contiguous, calculate the mask for the page uint8_t snb = (((page % 12) << 3) | ((page / 12) << 7)); // use 32 bit operations FLASH->CR = FLASH_CR_PSIZE_1 | snb | FLASH_CR_SER; FLASH->CR |= FLASH_CR_STRT; +#else +#error "Unsupported MCU" #endif stm32_flash_wait_idle(); @@ -412,7 +424,7 @@ static bool stm32h7_flash_write32(uint32_t addr, const void *buf) return true; } -bool stm32_flash_write(uint32_t addr, const void *buf, uint32_t count) +static bool stm32_flash_write_h7(uint32_t addr, const void *buf, uint32_t count) { uint8_t *b = (uint8_t *)buf; @@ -438,9 +450,10 @@ bool stm32_flash_write(uint32_t addr, const void *buf, uint32_t count) return true; } -#else // not STM32H7 +#endif // STM32H7 -bool stm32_flash_write(uint32_t addr, const void *buf, uint32_t count) +#if defined(STM32F4) || defined(STM32F7) +static bool stm32_flash_write_f4f7(uint32_t addr, const void *buf, uint32_t count) { uint8_t *b = (uint8_t *)buf; @@ -531,7 +544,88 @@ failed: #endif return false; } -#endif // not STM32H7 + +#endif // STM32F4 || STM32F7 + +uint32_t _flash_fail_line; +uint32_t _flash_fail_addr; +uint32_t _flash_fail_count; +uint8_t *_flash_fail_buf; + +#if defined(STM32F1) +static bool stm32_flash_write_f1(uint32_t addr, const void *buf, uint32_t count) +{ + uint8_t *b = (uint8_t *)buf; + + /* STM32 requires half-word access */ + if (count & 1) { + _flash_fail_line = __LINE__; + return false; + } + + if ((addr+count) >= STM32_FLASH_BASE+STM32_FLASH_SIZE) { + _flash_fail_line = __LINE__; + return false; + } + +#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; + + putreg16(*(uint16_t *)b, addr); + + stm32_flash_wait_idle(); + + FLASH->CR = 0; + + if (getreg16(addr) != *(uint16_t *)b) { + _flash_fail_line = __LINE__; + _flash_fail_addr = addr; + _flash_fail_count = count; + _flash_fail_buf = b; + goto failed; + } + + count -= 2; + b += 2; + addr += 2; + } + + 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 // STM32F1 + +bool stm32_flash_write(uint32_t addr, const void *buf, uint32_t count) +{ +#if defined(STM32F1) + return stm32_flash_write_f1(addr, buf, count); +#elif defined(STM32F4) || defined(STM32F7) + return stm32_flash_write_f4f7(addr, buf, count); +#elif defined(STM32H7) + return stm32_flash_write_h7(addr, buf, count); +#else +#error "Unsupported MCU" +#endif +} void stm32_flash_keep_unlocked(bool set) { diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h index a4e99ba4b4..1ffd5d8982 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h @@ -37,7 +37,7 @@ // include generated config #include "hwdef.h" -#ifdef STM32F100_MCUCONF +#if defined(STM32F1) #include "stm32f1_mcuconf.h" #elif defined(STM32F4) || defined(STM32F7) #include "stm32f47_mcuconf.h" diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c index bce91867a3..b1effaddc2 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c @@ -215,13 +215,19 @@ uint32_t get_fattime() void get_rtc_backup(uint8_t idx, uint32_t *v, uint8_t n) { while (n--) { +#if defined(STM32F1) + __IO uint32_t *dr = (__IO uint32_t *)&BKP->DR1; + *v++ = (dr[n/2]&0xFFFF) | (dr[n/2+1]<<16); +#else *v++ = ((__IO uint32_t *)&RTC->BKP0R)[idx++]; +#endif } } // set n RTC backup registers starting at given idx void set_rtc_backup(uint8_t idx, const uint32_t *v, uint8_t n) { +#if !defined(STM32F1) if ((RCC->BDCR & RCC_BDCR_RTCEN) == 0) { RCC->BDCR |= STM32_RTCSEL; RCC->BDCR |= RCC_BDCR_RTCEN; @@ -230,9 +236,16 @@ void set_rtc_backup(uint8_t idx, const uint32_t *v, uint8_t n) PWR->CR |= PWR_CR_DBP; #else PWR->CR1 |= PWR_CR1_DBP; +#endif #endif while (n--) { +#if defined(STM32F1) + __IO uint32_t *dr = (__IO uint32_t *)&BKP->DR1; + dr[n/2] = (*v) & 0xFFFF; + dr[n/2+1] = (*v) >> 16; +#else ((__IO uint32_t *)&RTC->BKP0R)[idx++] = *v++; +#endif } } diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h index 623d66424a..16cfb47967 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h @@ -57,7 +57,8 @@ uint32_t get_fattime(void); enum rtc_boot_magic { RTC_BOOT_OFF = 0, RTC_BOOT_HOLD = 0xb0070001, - RTC_BOOT_FAST = 0xb0070002 + RTC_BOOT_FAST = 0xb0070002, + RTC_BOOT_CANBL = 0xb0080000 // ORd with 8 bit local node ID }; // see if RTC registers is setup for a fast reboot diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32f1_mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32f1_mcuconf.h index 87609be93a..543141b472 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32f1_mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32f1_mcuconf.h @@ -34,11 +34,39 @@ #define STM32_LSI_ENABLED FALSE #define STM32_HSE_ENABLED TRUE #define STM32_LSE_ENABLED FALSE + +#if STM32_HSECLK == 8000000U +#define STM32_SW STM32_SW_PLL +#define STM32_PLLSRC STM32_PLLSRC_HSE +#define STM32_PLLXTPRE STM32_PLLXTPRE_DIV1 +#define STM32_PLLMUL_VALUE 9 +#define STM32_PPRE1 STM32_PPRE1_DIV2 +#define STM32_PPRE2 STM32_PPRE2_DIV2 +#define STM32_ADCPRE STM32_ADCPRE_DIV4 +#else +#error "Unsupported STM32F1xx clock frequency" +#endif + +#ifndef STM32_SW #define STM32_SW STM32_SW_HSE +#endif + +#ifndef STM32_HPRE #define STM32_HPRE STM32_HPRE_DIV1 +#endif + +#ifndef STM32_PPRE1 #define STM32_PPRE1 STM32_PPRE1_DIV1 +#endif + +#ifndef STM32_PPRE2 #define STM32_PPRE2 STM32_PPRE2_DIV1 +#endif + +#ifndef STM32_ADCPRE #define STM32_ADCPRE STM32_ADCPRE_DIV2 +#endif + #define STM32_MCOSEL STM32_MCOSEL_NOCLOCK #define STM32_RTCSEL STM32_RTCSEL_HSEDIV #define STM32_PVD_ENABLE FALSE @@ -147,4 +175,4 @@ /* * WDG driver system settings. */ -#define STM32_WDG_USE_IWDG FALSE \ No newline at end of file +#define STM32_WDG_USE_IWDG FALSE diff --git a/libraries/AP_HAL_ChibiOS/hwdef/iomcu/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/iomcu/hwdef.dat index 53a273b7bd..6f9f5204b1 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/iomcu/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/iomcu/hwdef.dat @@ -129,7 +129,7 @@ define HAL_USE_RTC FALSE define HAL_NO_FLASH_SUPPORT TRUE define HAL_NO_UARTDRIVER TRUE define DISABLE_SERIAL_ESC_COMM TRUE -define NO_LOGGING TRUE +define HAL_NO_LOGGING TRUE define DMA_RESERVE_SIZE 0 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32F103xB.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32F103xB.py new file mode 100644 index 0000000000..c3e8220f38 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32F103xB.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python +''' +these tables are generated from the STM32 datasheets for the +STM32F103x8 +''' + +# additional build information for ChibiOS +build = { + "CHIBIOS_STARTUP_MK" : "os/common/startup/ARMCMx/compilers/GCC/mk/startup_stm32f1xx.mk", + "CHIBIOS_PLATFORM_MK" : "os/hal/ports/STM32/STM32F1xx/platform.mk", + "CHPRINTF_USE_FLOAT" : 'no', + "USE_FPU" : 'no' + } + +pincount = { + 'A': 16, + 'B': 16, + 'C': 16, + 'D': 16, + 'E': 16 +} + +# MCU parameters +mcu = { + # location of MCU serial number + 'UDID_START' : 0x1FFFF7E8, + + 'RAM_MAP' : [ + (0x20000000, 20, 1), # main memory, DMA safe + ] +} + +ADC1_map = { + # format is PIN : ADC1_CHAN + "PA0" : 0, + "PA1" : 1, + "PA2" : 2, + "PA3" : 3, + "PA4" : 4, + "PA5" : 5, + "PA6" : 6, + "PA7" : 7, + "PB0" : 8, + "PB1" : 9, + "PC0" : 10, + "PC1" : 11, + "PC2" : 12, + "PC3" : 13, + "PC4" : 14, + "PC5" : 15, +} + + +DMA_Map = { + # format is (DMA_TABLE, StreamNum, Channel) + "ADC1" : [(1,1,0)], + "TIM1_CH1" : [(1,2,0)], + "TIM1_CH3" : [(1,6,0)], + "TIM1_CH4" : [(1,4,0)], + "TIM1_UP" : [(1,5,0)], + "TIM2_CH1" : [(1,5,0)], + "TIM2_CH2" : [(1,7,0)], + "TIM2_CH3" : [(1,1,0)], + "TIM2_CH4" : [(1,7,0)], + "TIM2_UP" : [(1,2,0)], + "TIM3_CH1" : [(1,6,0)], + "TIM3_CH3" : [(1,2,0)], + "TIM3_CH4" : [(1,3,0)], + "TIM3_UP" : [(1,3,0)], + "TIM4_CH1" : [(1,1,0)], + "TIM4_CH2" : [(1,4,0)], + "TIM4_CH3" : [(1,5,0)], + "TIM4_UP" : [(1,7,0)], + "TIM5_CH1" : [(2,5,0)], + "TIM5_CH2" : [(2,4,0)], + "TIM5_CH3" : [(2,2,0)], + "TIM5_CH4" : [(2,1,0)], + "TIM5_UP" : [(2,2,0)], + "TIM8_CH1" : [(2,3,0)], + "TIM8_CH2" : [(2,5,0)], + "TIM8_CH3" : [(2,1,0)], + "TIM8_CH4" : [(2,2,0)], + "TIM8_UP" : [(2,1,0)], + "TIM6_UP" : [(2,3,0)], + "TIM7_UP" : [(2,4,0)], + "I2C1_RX" : [(1,7,0)], + "I2C1_TX" : [(1,6,0)], + "I2C2_RX" : [(1,5,0)], + "I2C2_TX" : [(1,4,0)], + "SPI1_RX" : [(1,2,0)], + "SPI1_TX" : [(1,3,0)], + "SPI2_RX" : [(1,4,0)], + "SPI2_TX" : [(1,5,0)], + "SPI3_RX" : [(2,1,0)], + "SPI3_TX" : [(2,2,0)], + "UART4_RX" : [(2,3,0)], + "UART4_TX" : [(2,5,0)], + "USART1_RX" : [(1,5,0)], + "USART1_TX" : [(1,4,0)], + "USART2_RX" : [(1,6,0)], + "USART2_TX" : [(1,7,0)], + "USART3_RX" : [(1,3,0)], + "USART3_TX" : [(1,2,0)], +} + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32F105xC.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32F105xC.py new file mode 100644 index 0000000000..6fdb0d3109 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32F105xC.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python +''' +setup for STM32F105xx +''' + +# additional build information for ChibiOS +build = { + "CHIBIOS_STARTUP_MK" : "os/common/startup/ARMCMx/compilers/GCC/mk/startup_stm32f1xx.mk", + "CHIBIOS_PLATFORM_MK" : "os/hal/ports/STM32/STM32F1xx/platform.mk", + "CHPRINTF_USE_FLOAT" : 'no', + "USE_FPU" : 'no' + } + +pincount = { + 'A': 16, + 'B': 16, + 'C': 16, + 'D': 16, + 'E': 16 +} + +# MCU parameters +mcu = { + # location of MCU serial number + 'UDID_START' : 0x1FFFF7E8, + + 'RAM_MAP' : [ + (0x20000000, 64, 1), # main memory, DMA safe + ] +} + +ADC1_map = { + # format is PIN : ADC1_CHAN + "PA0" : 0, + "PA1" : 1, + "PA2" : 2, + "PA3" : 3, + "PA4" : 4, + "PA5" : 5, + "PA6" : 6, + "PA7" : 7, + "PB0" : 8, + "PB1" : 9, + "PC0" : 10, + "PC1" : 11, + "PC2" : 12, + "PC3" : 13, + "PC4" : 14, + "PC5" : 15, +} + + +DMA_Map = { + # format is (DMA_TABLE, StreamNum, Channel) + "ADC1" : [(1,1,0)], + "TIM1_CH1" : [(1,2,0)], + "TIM1_CH3" : [(1,6,0)], + "TIM1_CH4" : [(1,4,0)], + "TIM1_UP" : [(1,5,0)], + "TIM2_CH1" : [(1,5,0)], + "TIM2_CH2" : [(1,7,0)], + "TIM2_CH3" : [(1,1,0)], + "TIM2_CH4" : [(1,7,0)], + "TIM2_UP" : [(1,2,0)], + "TIM3_CH1" : [(1,6,0)], + "TIM3_CH3" : [(1,2,0)], + "TIM3_CH4" : [(1,3,0)], + "TIM3_UP" : [(1,3,0)], + "TIM4_CH1" : [(1,1,0)], + "TIM4_CH2" : [(1,4,0)], + "TIM4_CH3" : [(1,5,0)], + "TIM4_UP" : [(1,7,0)], + "TIM5_CH1" : [(2,5,0)], + "TIM5_CH2" : [(2,4,0)], + "TIM5_CH3" : [(2,2,0)], + "TIM5_CH4" : [(2,1,0)], + "TIM5_UP" : [(2,2,0)], + "TIM8_CH1" : [(2,3,0)], + "TIM8_CH2" : [(2,5,0)], + "TIM8_CH3" : [(2,1,0)], + "TIM8_CH4" : [(2,2,0)], + "TIM8_UP" : [(2,1,0)], + "TIM6_UP" : [(2,3,0)], + "TIM7_UP" : [(2,4,0)], + "I2C1_RX" : [(1,7,0)], + "I2C1_TX" : [(1,6,0)], + "I2C2_RX" : [(1,5,0)], + "I2C2_TX" : [(1,4,0)], + "SPI1_RX" : [(1,2,0)], + "SPI1_TX" : [(1,3,0)], + "SPI2_RX" : [(1,4,0)], + "SPI2_TX" : [(1,5,0)], + "SPI3_RX" : [(2,1,0)], + "SPI3_TX" : [(2,2,0)], + "UART4_RX" : [(2,3,0)], + "UART4_TX" : [(2,5,0)], + "USART1_RX" : [(1,5,0)], + "USART1_TX" : [(1,4,0)], + "USART2_RX" : [(1,6,0)], + "USART2_TX" : [(1,7,0)], + "USART3_RX" : [(1,3,0)], + "USART3_TX" : [(1,2,0)], +} + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 7ab5820748..f212a4833f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -111,7 +111,7 @@ def setup_mcu_type_defaults(): lib = get_mcu_lib(mcu_type) if hasattr(lib, 'pincount'): pincount = lib.pincount - if mcu_series == "STM32F100": + if mcu_series.startswith("STM32F1"): vtypes = f1_vtypes else: vtypes = f4f7_vtypes @@ -190,7 +190,7 @@ class generic_pin(object): self.sig_dir = 'OUTPUT' else: self.sig_dir = 'INPUT' - if mcu_series == "STM32F100" and self.label is not None: + if mcu_series.startswith("STM32F1") and self.label is not None: self.f1_pin_setup() # check that labels and pin types are consistent @@ -211,6 +211,8 @@ class generic_pin(object): self.extra.append('FLOATING') elif self.label.endswith(tuple(f1_output_sigs)): self.sig_dir = 'OUTPUT' + elif l == 'I2C': + self.sig_dir = 'OUTPUT' else: error("Unknown signal type %s:%s for %s!" % (self.portpin, self.label, mcu_type)) @@ -335,7 +337,7 @@ class generic_pin(object): def get_ODR(self): '''return one of LOW, HIGH''' - if mcu_series == "STM32F100": + if mcu_series.startswith("STM32F1"): return self.get_ODR_F1() values = ['LOW', 'HIGH'] v = 'HIGH' @@ -400,7 +402,7 @@ class generic_pin(object): def get_CR(self): '''return CR FLAGS''' - if mcu_series == "STM32F100": + if mcu_series.startswith("STM32F1"): return self.get_CR_F1() if self.sig_dir != "INPUT": speed_values = ['SPEED_LOW', 'SPEED_MEDIUM', 'SPEED_HIGH'] @@ -547,7 +549,7 @@ def write_mcu_config(f): f.write('#define HAL_USE_SERIAL_USB TRUE\n') if 'OTG2' in bytype: f.write('#define STM32_USB_USE_OTG2 TRUE\n') - if have_type_prefix('CAN'): + if have_type_prefix('CAN') and not mcu_series.startswith("STM32F1"): enable_can(f) if get_config('PROCESS_STACK', required=False): @@ -565,6 +567,11 @@ def write_mcu_config(f): else: env_vars['IOMCU_FW'] = 0 + if get_config('PERIPH_FW', required=False): + env_vars['PERIPH_FW'] = get_config('PERIPH_FW') + else: + env_vars['PERIPH_FW'] = 0 + # write any custom STM32 defines for d in alllines: if d.startswith('STM32_'): @@ -600,7 +607,7 @@ def write_mcu_config(f): lib = get_mcu_lib(mcu_type) build_info = lib.build - if mcu_series == "STM32F100": + if mcu_series.startswith("STM32F1"): cortex = "cortex-m3" env_vars['CPU_FLAGS'] = ["-mcpu=%s" % cortex] build_info['MCU'] = cortex @@ -945,8 +952,11 @@ def write_UART_config(f): f.write( "#define HAL_%s_CONFIG { (BaseSequentialStream*) &SD%u, false, " % (dev, n)) - f.write("STM32_%s_RX_DMA_CONFIG, STM32_%s_TX_DMA_CONFIG, %s, " % - (dev, dev, rts_line)) + if mcu_series.startswith("STM32F1"): + f.write("%s, " % rts_line) + else: + f.write("STM32_%s_RX_DMA_CONFIG, STM32_%s_TX_DMA_CONFIG, %s, " % + (dev, dev, rts_line)) # add inversion pins, if any f.write("%d, " % get_gpio_bylabel(dev + "_RXINV")) @@ -1006,10 +1016,15 @@ def write_I2C_config(f): '''write I2C config defines''' if not have_type_prefix('I2C'): print("No I2C peripherals") - f.write('#define HAL_USE_I2C FALSE\n') + f.write(''' +#ifndef HAL_USE_I2C +#define HAL_USE_I2C FALSE +#endif +''') return if not 'I2C_ORDER' in config: - error("Missing I2C_ORDER config") + print("Missing I2C_ORDER config") + return i2c_list = config['I2C_ORDER'] f.write('// I2C configuration\n') if len(i2c_list) == 0: @@ -1398,7 +1413,7 @@ def write_hwdef_header(outfilename): if len(romfs) > 0: f.write('#define HAL_HAVE_AP_ROMFS_EMBEDDED_H 1\n') - if mcu_series == 'STM32F100': + if mcu_series.startswith('STM32F1'): f.write(''' /* * I/O ports initial setup, this configuration is established soon after reset diff --git a/libraries/AP_HAL_ChibiOS/shared_dma.cpp b/libraries/AP_HAL_ChibiOS/shared_dma.cpp index 45ad6a4f4c..a32e6161b6 100644 --- a/libraries/AP_HAL_ChibiOS/shared_dma.cpp +++ b/libraries/AP_HAL_ChibiOS/shared_dma.cpp @@ -20,7 +20,7 @@ code to handle sharing of DMA channels between peripherals */ -#if CH_CFG_USE_SEMAPHORES == TRUE && STM32_DMA_ADVANCED +#if CH_CFG_USE_SEMAPHORES == TRUE using namespace ChibiOS; @@ -211,4 +211,4 @@ void Shared_DMA::lock_all(void) } } -#endif // CH_CFG_USE_SEMAPHORES && STM32_DMA_ADVANCED +#endif // CH_CFG_USE_SEMAPHORES diff --git a/libraries/AP_HAL_ChibiOS/shared_dma.h b/libraries/AP_HAL_ChibiOS/shared_dma.h index 63e4874a3d..a0af229752 100644 --- a/libraries/AP_HAL_ChibiOS/shared_dma.h +++ b/libraries/AP_HAL_ChibiOS/shared_dma.h @@ -18,8 +18,6 @@ #include "AP_HAL_ChibiOS.h" -#if STM32_DMA_ADVANCED - #define SHARED_DMA_MAX_STREAM_ID (8*2) // DMA stream ID for stream_id2 when only one is needed @@ -106,6 +104,6 @@ private: Shared_DMA *obj; } locks[SHARED_DMA_MAX_STREAM_ID+1]; }; -#endif //#if STM32_DMA_ADVANCED + diff --git a/libraries/AP_HAL_ChibiOS/stdio.cpp b/libraries/AP_HAL_ChibiOS/stdio.cpp index 8cd5404442..5352edc290 100644 --- a/libraries/AP_HAL_ChibiOS/stdio.cpp +++ b/libraries/AP_HAL_ChibiOS/stdio.cpp @@ -121,6 +121,7 @@ __wrap_sscanf (const char *buf, const char *fmt, ...) return (count); } +#if defined(HAL_OS_FATFS_IO) && HAL_OS_FATFS_IO static char * _getbase(char *p, int *basep) { @@ -214,7 +215,6 @@ static int16_t atob(uint32_t *vp, char *p, int base) } -#if defined(HAL_OS_FATFS_IO) && HAL_OS_FATFS_IO /* * vsscanf(buf,fmt,ap) */