From 42b4730d889a2007bccbd642dea388db849d7ad5 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Wed, 29 Aug 2018 18:48:55 +0530 Subject: [PATCH] HAL_ChibiOS: add changes to hwdef to support STM32F1 based controller --- libraries/AP_HAL_ChibiOS/hwdef/common/board.c | 20 +++ .../AP_HAL_ChibiOS/hwdef/common/chconf.h | 6 + .../hwdef/common/chibios_board.mk | 6 +- .../AP_HAL_ChibiOS/hwdef/common/malloc.c | 9 +- .../AP_HAL_ChibiOS/hwdef/common/mcuconf.h | 4 + libraries/AP_HAL_ChibiOS/hwdef/common/posix.c | 2 +- libraries/AP_HAL_ChibiOS/hwdef/common/posix.h | 2 +- .../AP_HAL_ChibiOS/hwdef/common/stm32_util.c | 3 + .../hwdef/scripts/chibios_hwdef.py | 156 +++++++++++++++++- .../hwdef/scripts/dma_resolver.py | 5 +- 10 files changed, 197 insertions(+), 16 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/board.c b/libraries/AP_HAL_ChibiOS/hwdef/common/board.c index 8fab6d6235..ea64060699 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/board.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/board.c @@ -32,6 +32,22 @@ /* Driver local variables and types. */ /*===========================================================================*/ +/** + * @brief STM32 GPIO static initialization data. + */ +#ifdef STM32F100_MCUCONF + +const PALConfig pal_default_config = +{ + {VAL_GPIOA_ODR, VAL_GPIOA_CRL, VAL_GPIOA_CRH}, + {VAL_GPIOB_ODR, VAL_GPIOB_CRL, VAL_GPIOB_CRH}, + {VAL_GPIOC_ODR, VAL_GPIOC_CRL, VAL_GPIOC_CRH}, + {VAL_GPIOD_ODR, VAL_GPIOD_CRL, VAL_GPIOD_CRH}, + {VAL_GPIOE_ODR, VAL_GPIOE_CRL, VAL_GPIOE_CRH}, +}; + +#else //Other than STM32F1 series + /** * @brief Type of STM32 GPIO port setup. */ @@ -192,13 +208,17 @@ static void stm32_gpio_init(void) { #endif } +#endif //!STM32F100_MCUCONF + /** * @brief Early initialization code. * @details This initialization must be performed just after stack setup * and before any other initialization. */ void __early_init(void) { +#ifndef STM32F100_MCUCONF stm32_gpio_init(); +#endif stm32_clock_init(); } diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h index 6e34dd71b0..c7cfdfd5ca 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h @@ -68,7 +68,9 @@ * setting also defines the system tick time unit. We set this to 1000000 * in ArduPilot so we get maximum resolution for timing of delays */ +#ifndef CH_CFG_ST_FREQUENCY #define CH_CFG_ST_FREQUENCY 1000000 +#endif /** * @brief Time intervals data size. @@ -90,7 +92,9 @@ * The value one is not valid, timeouts are rounded up to * this value. */ +#ifndef CH_CFG_ST_TIMEDELTA #define CH_CFG_ST_TIMEDELTA 2 +#endif /* default to a large interrupt stack for now. We may trim this later @@ -548,7 +552,9 @@ * @note This debug option is not currently compatible with the * tickless mode. */ +#ifndef CH_DBG_THREADS_PROFILING #define CH_DBG_THREADS_PROFILING FALSE +#endif /** @} */ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk b/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk index 503f3ee2bb..334dd3aac6 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk @@ -5,7 +5,7 @@ # Compiler options here. ifeq ($(USE_OPT),) - USE_OPT = -Os -g -fomit-frame-pointer -falign-functions=16 -DCHPRINTF_USE_FLOAT=1 + USE_OPT = -Os -g -fomit-frame-pointer -falign-functions=16 endif # C specific options here (added to USE_OPT). @@ -171,8 +171,6 @@ INCDIR = $(CHIBIOS)/os/license \ # Compiler settings # -MCU = cortex-m4 - #TRGT = arm-elf- TRGT = arm-none-eabi- CC = $(TRGT)gcc @@ -211,7 +209,7 @@ CPPWARN = -Wall -Wextra -Wundef # # List all user C define here, like -D_DEBUG=1 -UDEFS = $(FATFS_FLAGS) -DHAL_BOARD_NAME=\"$(HAL_BOARD_NAME)\" +UDEFS = $(ENV_UDEFS) $(FATFS_FLAGS) -DHAL_BOARD_NAME=\"$(HAL_BOARD_NAME)\" ifeq ($(ENABLE_ASSERTS),yes) UDEFS += -DHAL_CHIBIOS_ENABLE_ASSERTS diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c b/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c index 8355a29feb..ce5c1fc33f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c @@ -69,6 +69,7 @@ void malloc_init(void) chHeapObjectInit(&dtcm_heap, (void *)DTCM_BASE_ADDRESS, DTCM_RAM_SIZE_KB*1024); #endif +#if DMA_RESERVE_SIZE != 0 /* create a DMA reserve heap, to ensure we keep some memory for DMA safe memory allocations @@ -78,6 +79,7 @@ void malloc_init(void) dma_reserve = chHeapAllocAligned(NULL, DMA_RESERVE_SIZE, MIN_ALIGNMENT); } chHeapObjectInit(&dma_reserve_heap, dma_reserve, DMA_RESERVE_SIZE); +#endif //#if DMA_RESERVE_SIZE != 0 } void *malloc_ccm(size_t size) @@ -133,13 +135,14 @@ void *malloc(size_t size) } } +#if DMA_RESERVE_SIZE != 0 // fall back to DMA reserve p = chHeapAllocAligned(&dma_reserve_heap, size, MIN_ALIGNMENT); if (p) { memset(p, 0, size); return p; } - +#endif return NULL; } @@ -155,9 +158,11 @@ void *malloc_dma(size_t size) // if we don't have DTCM memory then assume that main heap is DMA-safe p = chHeapAllocAligned(NULL, size, MIN_ALIGNMENT); #endif +#if DMA_RESERVE_SIZE != 0 if (p == NULL) { p = chHeapAllocAligned(&dma_reserve_heap, size, MIN_ALIGNMENT); } +#endif if (p) { memset(p, 0, size); } @@ -200,9 +205,11 @@ size_t mem_available(void) totalp += dtcm_available; #endif +#if DMA_RESERVE_SIZE != 0 size_t reserve_available = 0; chHeapStatus(&dma_reserve_heap, &reserve_available, NULL); totalp += reserve_available; +#endif return totalp; } diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h index 5ecf6b2425..f56be5c988 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h @@ -36,6 +36,9 @@ #include "hwdef.h" #pragma once +#ifdef STM32F100_MCUCONF +#include "stm32f1_mcuconf.h" +#else /* * STM32F4xx drivers configuration. * The following settings override the default settings present in @@ -384,3 +387,4 @@ */ #define STM32_WDG_USE_IWDG FALSE +#endif //!STM32F100_MCUCONF diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/posix.c b/libraries/AP_HAL_ChibiOS/hwdef/common/posix.c index 2fbf2d4670..3a9e34f3fb 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/posix.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/posix.c @@ -2536,7 +2536,7 @@ static void _fprintf_putc(struct _printf_t *p, char ch) /// @return size of printed result int -fprintf(FILE *fp, const char *fmt, ...) +__wrap_fprintf(FILE *fp, const char *fmt, ...) { va_list va; char* buf; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/posix.h b/libraries/AP_HAL_ChibiOS/hwdef/common/posix.h index 6a6e20377c..a45baf1a78 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/posix.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/posix.h @@ -388,7 +388,7 @@ int free_file_descriptor ( int fileno ); int new_file_descriptor ( void ); int posix_fopen_modes_to_open ( const char *mode ); -int fprintf(FILE *fp, const char *format, ...); +int __wrap_fprintf(FILE *fp, const char *format, ...); #ifdef __cplusplus } diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c index c73fb12040..084fc0379a 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c @@ -219,6 +219,7 @@ uint32_t get_fattime() return fattime; } +#if defined(HAL_USE_RTC) && HAL_USE_RTC // get RTC backup register 0 static uint32_t get_rtc_backup0(void) { @@ -252,6 +253,8 @@ void set_fast_reboot(enum rtc_boot_magic v) set_rtc_backup0(v); } +#endif //HAL_USE_RTC + /* enable peripheral power if needed This is done late to prevent problems with CTS causing SiK radios to stay in the bootloader. A diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index e32e1b9e1f..93a97c48ee 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -17,7 +17,13 @@ parser.add_argument( args = parser.parse_args() # output variables for each pin -vtypes = ['MODER', 'OTYPER', 'OSPEEDR', 'PUPDR', 'ODR', 'AFRL', 'AFRH'] +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'] + +vtypes = [] # number of pins in each port pincount = { @@ -96,10 +102,14 @@ def get_mcu_lib(mcu): def setup_mcu_type_defaults(): '''setup defaults for given mcu type''' - global pincount, ports, portmap + global pincount, ports, portmap, vtypes lib = get_mcu_lib(mcu_type) if hasattr(lib, 'pincount'): pincount = lib.pincount + if mcu_series == "STM32F100": + vtypes = f1_vtypes + else: + vtypes = f4f7_vtypes ports = pincount.keys() # setup default as input pins for port in ports: @@ -110,14 +120,21 @@ def setup_mcu_type_defaults(): def get_alt_function(mcu, pin, function): '''return alternative function number for a pin''' lib = get_mcu_lib(mcu) - alt_map = lib.AltFunction_map + + if hasattr(lib, "AltFunction_map"): + alt_map = lib.AltFunction_map + else: + # just check if Alt Func is available or not + for l in af_labels: + if function.startswith(l): + return 0 + return None if function and function.endswith("_RTS") and ( function.startswith('USART') or function.startswith('UART')): # we do software RTS return None - af_labels = ['USART', 'UART', 'SPI', 'I2C', 'SDIO', 'SDMMC', 'OTG', 'JT', 'TIM', 'CAN'] for l in af_labels: if function.startswith(l): s = pin + ":" + function @@ -152,6 +169,7 @@ class generic_pin(object): '''class to hold pin definition''' def __init__(self, port, pin, label, type, extra): + global mcu_series self.portpin = "P%s%u" % (port, pin) self.port = port self.pin = pin @@ -159,6 +177,20 @@ class generic_pin(object): self.type = type self.extra = extra self.af = None + self.sig_dir = 'INPUT' + if mcu_series == "STM32F100" and self.label is not None: + self.f1_pin_setup() + + def f1_pin_setup(self): + for l in af_labels: + if self.label.startswith(l): + if self.label.endswith(tuple(f1_input_sigs)): + self.sig_dir = 'INPUT' + self.extra.append('FLOATING') + elif self.label.endswith(tuple(f1_output_sigs)): + self.sig_dir = 'OUTPUT' + else: + error("Unknown signal type %s:%s for %s!" % (self.portpin, self.label, mcu_type)) def has_extra(self, v): '''return true if we have the given extra token''' @@ -281,6 +313,67 @@ class generic_pin(object): return None return self.get_AFIO() + #F1 series GPIO Cfg methods + + def get_ODR(self): + '''return one of LOW, HIGH''' + values = ['LOW', 'HIGH'] + v = 'HIGH' + if self.type == 'OUTPUT': + v = 'LOW' + for e in self.extra: + if e in values: + v = e + #for some controllers input pull up down is selected by ODR + if self.type == "INPUT": + v = 'LOW' + if 'PULLUP' in self.extra: + v = "HIGH" + return "PIN_ODR_%s(%uU)" % (v, self.pin) + + def get_CR(self): + '''return CR FLAGS''' + #Check Speed + if self.sig_dir != "INPUT": + speed_values = ['SPEED_LOW', 'SPEED_MEDIUM', 'SPEED_HIGH'] + v = 'SPEED_MEDIUM' + for e in self.extra: + if e in speed_values: + v = e + speed_str = "PIN_%s(%uU) |" % (v, self.pin) + else: + speed_str = "" + #Check Alternate function + if self.type.startswith('I2C'): + v = "AF_OD" + elif self.sig_dir == 'OUTPUT': + if self.af is not None: + v = "AF_PP" + else: + v = "OUTPUT_PP" + elif self.type.startswith('ADC'): + v = "ANALOG" + elif self.is_CS(): + v = "OUTPUT_PP" + elif self.is_RTS(): + v = "OUTPUT_PP" + else: + v = "PUD" + if 'FLOATING' in self.extra: + v = "NOPULL" + mode_str = "PIN_MODE_%s(%uU)" % (v, self.pin) + return "%s %s" % (speed_str, mode_str) + + def get_CRH(self): + if self.pin < 8: + return None + return self.get_CR() + + def get_CRL(self): + if self.pin >= 8: + return None + return self.get_CR() + def __str__(self): str = '' if self.af is not None: @@ -384,6 +477,17 @@ def write_mcu_config(f): f.write('#define STM32_USB_USE_OTG2 TRUE\n') if have_type_prefix('CAN'): enable_can(f) + + if get_config('PROCESS_STACK', required=False): + env_vars['PROCESS_STACK'] = get_config('PROCESS_STACK') + else: + env_vars['PROCESS_STACK'] = "0x2000" + + if get_config('MAIN_STACK', required=False): + env_vars['MAIN_STACK'] = get_config('MAIN_STACK') + else: + env_vars['MAIN_STACK'] = "0x400" + # write any custom STM32 defines for d in alllines: if d.startswith('STM32_'): @@ -428,6 +532,16 @@ def write_mcu_config(f): lib = get_mcu_lib(mcu_type) build_info = lib.build + + if mcu_series == "STM32F100": + cortex = "cortex-m3" + env_vars['CPU_FLAGS'] = ["-mcpu=%s" % cortex] + build_info['MCU'] = cortex + else: + cortex = "cortex-m4" + env_vars['CPU_FLAGS'] = ['-u_printf_float', "-mcpu=%s" % cortex, "-mfpu=fpv4-sp-d16", "-mfloat-abi=hard"] + build_info['MCU'] = cortex + build_info['ENV_UDEFS'] = "-DCHPRINTF_USE_FLOAT=1" # setup build variables for v in build_info.keys(): build_flags.append('%s=%s' % (v, build_info[v])) @@ -508,7 +622,7 @@ def copy_common_linkerscript(outdir, hwdef): def write_USB_config(f): '''write USB config defines''' if not have_type_prefix('OTG'): - return; + return f.write('// USB configuration\n') f.write('#define HAL_USB_VENDOR_ID %s\n' % get_config('USB_VENDOR', default=0x0483)) # default to ST f.write('#define HAL_USB_PRODUCT_ID %s\n' % get_config('USB_PRODUCT', default=0x5740)) @@ -1061,12 +1175,36 @@ def write_hwdef_header(outfilename): if len(romfs) > 0: f.write('#define HAL_HAVE_AP_ROMFS_EMBEDDED_H 1\n') - f.write(''' + if mcu_series == 'STM32F100': + f.write(''' /* * I/O ports initial setup, this configuration is established soon after reset * in the initialization code. * Please refer to the STM32 Reference Manual for details. */ +#define PIN_MODE_OUTPUT_PP(n) (0 << (((n) & 7) * 4)) +#define PIN_MODE_OUTPUT_OD(n) (4 << (((n) & 7) * 4)) +#define PIN_MODE_AF_PP(n) (8 << (((n) & 7) * 4)) +#define PIN_MODE_AF_OD(n) (12 << (((n) & 7) * 4)) +#define PIN_MODE_ANALOG(n) (0 << (((n) & 7) * 4)) +#define PIN_MODE_NOPULL(n) (4 << (((n) & 7) * 4)) +#define PIN_MODE_PUD(n) (8 << (((n) & 7) * 4)) +#define PIN_SPEED_MEDIUM(n) (1 << (((n) & 7) * 4)) +#define PIN_SPEED_LOW(n) (2 << (((n) & 7) * 4)) +#define PIN_SPEED_HIGH(n) (3 << (((n) & 7) * 4)) +#define PIN_ODR_HIGH(n) (1 << (((n) & 15))) +#define PIN_ODR_LOW(n) (0 << (((n) & 15))) +#define PIN_PULLUP(n) (1 << (((n) & 15))) +#define PIN_PULLDOWN(n) (0 << (((n) & 15))) +#define PIN_UNDEFINED(n) PIN_INPUT_PUD(n) +''') + else: + f.write(''' +/* +* I/O ports initial setup, this configuration is established soon after reset +* in the initialization code. +* Please refer to the STM32 Reference Manual for details. +*/ #define PIN_MODE_INPUT(n) (0U << ((n) * 2U)) #define PIN_MODE_OUTPUT(n) (1U << ((n) * 2U)) #define PIN_MODE_ALTERNATE(n) (2U << ((n) * 2U)) @@ -1168,9 +1306,10 @@ def write_env_py(filename): if os.path.exists(defaults_filename) and not args.bootloader: print("Adding defaults.parm") env_vars['DEFAULT_PARAMETERS'] = os.path.abspath(defaults_filename) - + # CHIBIOS_BUILD_FLAGS is passed to the ChibiOS makefile env_vars['CHIBIOS_BUILD_FLAGS'] = ' '.join(build_flags) + print env_vars['CPU_FLAGS'] pickle.dump(env_vars, open(filename, "wb")) def romfs_add(romfs_filename, filename): @@ -1197,8 +1336,9 @@ def process_line(line): config[a[0]] = a[1:] if a[0] == 'MCU': - global mcu_type + global mcu_type, mcu_series mcu_type = a[2] + mcu_series = a[1] setup_mcu_type_defaults() if a[0].startswith('P') and a[0][1] in ports: # it is a port/pin definition diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py index f83e7f9e7f..cc365a657c 100755 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py @@ -104,7 +104,10 @@ def write_dma_header(f, peripheral_list, mcu_type, dma_exclude=[], try: lib = importlib.import_module(mcu_type) - dma_map = lib.DMA_Map + if hasattr(lib, "DMA_Map"): + dma_map = lib.DMA_Map + else: + return except ImportError: print("Unable to find module for MCU %s" % mcu_type) sys.exit(1)