From 84de029e74859d98c82047b04b62d87a35d12a27 Mon Sep 17 00:00:00 2001 From: jaseg Date: Sat, 10 Jun 2017 19:14:18 +0200 Subject: fw working commit --- fw/Makefile | 60 +++++++++ fw/base.c | 20 +++ fw/main.c | 98 +++++++++++++++ fw/mapparse.py | 129 +++++++++++++++++++ fw/mapvis.py | 25 ++++ fw/semihosting.c | 10 ++ fw/stm32_flash.ld | 124 +++++++++++++++++++ fw/system_stm32f0xx.c | 336 ++++++++++++++++++++++++++++++++++++++++++++++++++ 8 files changed, 802 insertions(+) create mode 100644 fw/Makefile create mode 100644 fw/base.c create mode 100644 fw/main.c create mode 100644 fw/mapparse.py create mode 100644 fw/mapvis.py create mode 100644 fw/semihosting.c create mode 100644 fw/stm32_flash.ld create mode 100644 fw/system_stm32f0xx.c (limited to 'fw') diff --git a/fw/Makefile b/fw/Makefile new file mode 100644 index 0000000..5496030 --- /dev/null +++ b/fw/Makefile @@ -0,0 +1,60 @@ +# put your *.o targets here, make should handle the rest! +CMSIS_PATH ?= STM32Cube/Drivers/CMSIS +CMSIS_DEV_PATH ?= $(CMSIS_PATH)/Device/ST/STM32F0xx +HAL_PATH ?= STM32Cube/Drivers/STM32F0xx_HAL_Driver + +CC := arm-none-eabi-gcc +LD := arm-none-eabi-ld +OBJCOPY := arm-none-eabi-objcopy +OBJDUMP := arm-none-eabi-objdump +SIZE := arm-none-eabi-size + +CFLAGS = -Wall -std=gnu11 -Os -fdump-rtl-expand +CFLAGS += -mlittle-endian -mcpu=cortex-m0 -march=armv6-m -mthumb +CFLAGS += -ffunction-sections -fdata-sections +LDFLAGS = -nostartfiles +#LDFLAGS += -specs=rdimon.specs -DSEMIHOSTING +LDFLAGS += -Wl,-Map=main.map -nostdlib +LDFLAGS += -Wl,--gc-sections +LIBS = -lgcc +#LIBS += -lrdimon + +# Technically we're using an STM32F030F4, but apart from the TSSOP20 package that one is largely identical to the +# STM32F030*6 and there is no separate device header provided for it, so we're faking a *6 device here. This is +# even documented in stm32f0xx.h. Thanks ST! +CFLAGS += -DSTM32F030x6 + +LDFLAGS += -Tstm32_flash.ld +CFLAGS += -I$(CMSIS_DEV_PATH)/Include -I$(CMSIS_PATH)/Include -I$(HAL_PATH)/Inc -Iconfig +LDFLAGS += -L$(CMSIS_PATH)/Lib/GCC -larm_cortexM0l_math + +################################################### + +.PHONY: program clean + +all: main.elf main.pdf + +%.o: %.c + $(CC) -c $(CFLAGS) -o $@ $^ + +%.o: %.s + $(CC) -c $(CFLAGS) -o $@ $^ + +%.dot: %.elf + r2 -a arm -qc 'aa;agC' $< 2>/dev/null >$@ + +main.elf: main.o startup_stm32f030x6.o system_stm32f0xx.o $(HAL_PATH)/Src/stm32f0xx_ll_utils.o base.o semihosting.o + $(CC) $(CFLAGS) $(LDFLAGS) -o $@ $^ $(LIBS) + $(OBJCOPY) -O ihex $@ $(@:.elf=.hex) + $(OBJCOPY) -O binary $@ $(@:.elf=.bin) + $(OBJDUMP) -St $@ >$(@:.elf=.lst) + $(SIZE) $@ + +program: main.elf openocd.cfg + openocd -f openocd.cfg -c "program $< verify reset exit" + +clean: + rm -f **.o + rm -f main.elf main.hex main.bin main.map main.lst + rm -f **.expand + diff --git a/fw/base.c b/fw/base.c new file mode 100644 index 0000000..8c9de6f --- /dev/null +++ b/fw/base.c @@ -0,0 +1,20 @@ + +#include + +int __errno = 0; +void *_impure_ptr = NULL; + +void __sinit(void) { +} + +void memset(void *s, int c, size_t n) { + char *end = (char *)s + n; + for (char *p = (char *)s; p < end; p++) + *p = (char)c; +} + +size_t strlen(const char *s) { + const char *start = s; + while (*s++); + return s - start - 1; +} diff --git a/fw/main.c b/fw/main.c new file mode 100644 index 0000000..d29ef9e --- /dev/null +++ b/fw/main.c @@ -0,0 +1,98 @@ + +#include +#include +#include +#include +#include +#include +/* + * Part number: STM32F030F4C6 + */ + +void tick(void) { + for(int i=0; i<50; i++) + __asm__("nop"); +} + +int main(void) { + RCC->CR |= RCC_CR_HSEON; + while (!(RCC->CR&RCC_CR_HSERDY)); + RCC->CFGR &= ~RCC_CFGR_PLLMUL_Msk & ~RCC_CFGR_SW_Msk; + RCC->CFGR |= (2<CFGR2 &= ~RCC_CFGR2_PREDIV_Msk; /* PREDIV=0 */ + RCC->CR |= RCC_CR_PLLON; + while (!(RCC->CR&RCC_CR_PLLRDY)); + RCC->CFGR |= (2<AHBENR |= RCC_AHBENR_GPIOAEN; + RCC->APB2ENR |= RCC_APB2ENR_SPI1EN; + + GPIOA->MODER |= + (2<OSPEEDR |= + (3<AFR[0] |= + (0<CR1 = SPI_CR1_SSM | SPI_CR1_SSI | SPI_CR1_SPE | (7<CR2 = (7<DR = 1<DR = 0; + } + while (SPI1->SR & SPI_SR_BSY); + tick(); + } + pos2 += 1; + if (pos2 == 8) { + pos2 = 0; + pos1 += 1; + if (pos1 == 6) + pos1 = 0; + } + /* Strobe both LED drivers and aux regs */ + GPIOA->BSRR = GPIO_BSRR_BS_9 | GPIO_BSRR_BS_10; + tick(); + GPIOA->BSRR = GPIO_BSRR_BR_9 | GPIO_BSRR_BR_10; + LL_mDelay(1); + } +} + +void NMI_Handler(void) { +} + +void HardFault_Handler(void) { + for(;;); +} + +void SVC_Handler(void) { +} + + +void PendSV_Handler(void) { +} + +void SysTick_Handler(void) { +} + diff --git a/fw/mapparse.py b/fw/mapparse.py new file mode 100644 index 0000000..c1f460a --- /dev/null +++ b/fw/mapparse.py @@ -0,0 +1,129 @@ + +import re +from collections import defaultdict, namedtuple + +Section = namedtuple('Section', ['name', 'offset', 'objects']) +ObjectEntry = namedtuple('ObjectEntry', ['filename', 'object', 'offset', 'size']) +FileEntry = namedtuple('FileEntry', ['section', 'object', 'offset', 'length']) + +class Memory: + def __init__(self, name, origin, length, attrs=''): + self.name, self.origin, self.length, self.attrs = name, origin, length, attrs + self.sections = {} + self.files = defaultdict(lambda: []) + self.totals = defaultdict(lambda: 0) + + def add_toplevel(self, name, offx, length): + self.sections[name] = Section(offx, length, []) + + def add_obj(self, name, offx, length, fn, obj): + base_section, sep, subsec = name[1:].partition('.') + base_section = '.'+base_section + if base_section in self.sections: + sec = secname, secoffx, secobjs = self.sections[base_section] + secobjs.append(ObjectEntry(fn, obj, offx, length)) + else: + sec = None + self.files[fn].append(FileEntry(sec, obj, offx, length)) + self.totals[fn] += length + +class MapFile: + def __init__(self, s): + self._lines = s.splitlines() + self.memcfg = {} + self.defaultmem = Memory('default', 0, 0xffffffffffffffff) + self._parse() + + def __getitem__(self, offx_or_name): + ''' Lookup a memory area by name or address ''' + if offx_or_name in self.memcfg: + return self.memcfg[offx_or_name] + + elif isinstance(offx_or_name, int): + for mem in self.memcfg.values(): + if mem.origin <= offx_or_name < mem.origin+mem.length: + return mem + else: + return self.defaultmem + + raise ValueError('Invalid argument type for indexing') + + def _skip(self, regex): + matcher = re.compile(regex) + for l in self: + if matcher.match(l): + break + + def __iter__(self): + while self._lines: + yield self._lines.pop(0) + + def _parse(self): + self._skip('^Memory Configuration') + + # Parse memory segmentation info + self._skip('^Name') + for l in self: + if not l: + break + name, origin, length, *attrs = l.split() + if not name.startswith('*'): + self.memcfg[name] = Memory(name, int(origin, 16), int(length, 16), attrs[0] if attrs else '') + + # Parse section information + toplevel_m = re.compile('^(\.[a-zA-Z0-9_.]+)\s+(0x[0-9a-fA-F]+)\s+(0x[0-9a-fA-F]+)') + secondlevel_m = re.compile('^ (\.[a-zA-Z0-9_.]+)\s+(0x[0-9a-fA-F]+)\s+(0x[0-9a-fA-F]+)\s+(.*)$') + secondlevel_linebreak_m = re.compile('^ (\.[a-zA-Z0-9_.]+)\n') + filelike = re.compile('^(/?[^()]*\.[a-zA-Z0-9-_]+)(\(.*\))?') + linebreak_section = None + for l in self: + # Toplevel section + match = toplevel_m.match(l) + if match: + name, offx, length = match.groups() + offx, length = int(offx, 16), int(length, 16) + self[offx].add_toplevel(name, offx, length) + + match = secondlevel_linebreak_m.match(l) + if match: + linebreak_section, = match.groups() + continue + + if linebreak_section: + l = ' {} {}'.format(linebreak_section, l) + linebreak_section = None + + # Second-level section + match = secondlevel_m.match(l) + if match: + name, offx, length, misc = match.groups() + match = filelike.match(misc) + if match: + fn, obj = match.groups() + obj = obj.strip('()') if obj else None + offx, length = int(offx, 16), int(length, 16) + self[offx].add_obj(name, offx, length, fn, obj) + + +if __name__ == '__main__': + import argparse + parser = argparse.ArgumentParser(description='Parser GCC map file') + parser.add_argument('mapfile', type=argparse.FileType('r'), help='The GCC .map file to parse') + parser.add_argument('-m', '--memory', type=str, help='The memory segments to print, comma-separated') + args = parser.parse_args() + mf = MapFile(args.mapfile.read()) + args.mapfile.close() + + mems = args.memory.split(',') if args.memory else mf.memcfg.keys() + + for name in mems: + mem = mf.memcfg[name] + print('Symbols by file for memory', name) + for tot, fn in reversed(sorted( (tot, fn) for fn, tot in mem.totals.items() )): + print(' {:>8} {}'.format(tot, fn)) + for length, offx, sec, obj in reversed(sorted(( (length, offx, sec, obj) for sec, obj, offx, length in + mem.files[fn] ), key=lambda e: e[0] )): + name = sec.name if sec else None + print(' {:>8} {:>#08x} {}'.format(length, offx, obj)) + #print('{:>16} 0x{:016x} 0x{:016x} ({:>24}) {}'.format(name, origin, length, length, attrs)) + diff --git a/fw/mapvis.py b/fw/mapvis.py new file mode 100644 index 0000000..4a71222 --- /dev/null +++ b/fw/mapvis.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python3 + +from matplotlib import pyplot as plt + +f, ax = plt.subplots(1, figsize=(3, 8)) +bar_width = 1 + +ax.bar([-bar_width/2], top, bottom=bottom, width=bar_width, label='foo') +ax.set_xticks([0], [filename]) +ax.set_ylabel('Memory usage (B)') +ax.set_xlabel('') + +ax.set_xlim([-bar_width/2, bar_width/2]) +ax.set_ylim([0, mem_max]) + +if __name__ == '__main__': + import argparse + import mapparse + parser = argparse.ArgumentParser(description='Visualize program memory usage using GCC map file') + parser.add_argument('mapfile', type=argparse.FileType('r'), description='Input GCC .map file') + args = parser.parse_args() + + mapping = mapparse.MapFile(args.mapfile.read()) + mapping. + diff --git a/fw/semihosting.c b/fw/semihosting.c new file mode 100644 index 0000000..96ebc7a --- /dev/null +++ b/fw/semihosting.c @@ -0,0 +1,10 @@ + +#define SYS_WRITE0 0x04 + +void write0(const char *c) __attribute__((naked)); +void write0(const char *c) { + __asm__("mov r1, %0" : : "r" (c)); + __asm__("mov r0, %0" : : "I" (SYS_WRITE0)); + __asm__("bkpt 0xab"); + __asm__("bx lr"); +} diff --git a/fw/stm32_flash.ld b/fw/stm32_flash.ld new file mode 100644 index 0000000..87e095e --- /dev/null +++ b/fw/stm32_flash.ld @@ -0,0 +1,124 @@ + +ENTRY(Reset_Handler) + +MEMORY { + FLASH (rx): ORIGIN = 0x08000000, LENGTH = 16K + RAM (xrw): ORIGIN = 0x20000000, LENGTH = 4K +} + +/* highest address of the user mode stack */ +_estack = 0x20001000; + +SECTIONS { + /* for Cortex devices, the beginning of the startup code is stored in the .isr_vector section, which goes to FLASH */ + .isr_vector : { + . = ALIGN(4); + KEEP(*(.isr_vector)) /* Startup code */ + . = ALIGN(4); + } >FLASH + + /* the program code is stored in the .text section, which goes to Flash */ + .text : { + . = ALIGN(4); + + *(.text) /* normal code */ + *(.text.*) /* -ffunction-sections code */ + *(.rodata) /* read-only data (constants) */ + *(.rodata*) /* -fdata-sections read only data */ + *(.glue_7) /* TBD - needed ? */ + *(.glue_7t) /* TBD - needed ? */ + + /* Necessary KEEP sections (see http://sourceware.org/ml/newlib/2005/msg00255.html) */ + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; + /* This is used by the startup in order to initialize the .data section */ + _sidata = _etext; + } >FLASH + + /* This is the initialized data section + The program executes knowing that the data is in the RAM + but the loader puts the initial values in the FLASH (inidata). + It is one task of the startup to copy the initial values from FLASH to RAM. */ + .data : AT ( _sidata ) { + . = ALIGN(4); + /* This is used by the startup in order to initialize the .data secion */ + _sdata = . ; + _data = . ; + + *(.data) + *(.data.*) + *(.RAMtext) + + . = ALIGN(4); + /* This is used by the startup in order to initialize the .data secion */ + _edata = . ; + } >RAM + + /* This is the uninitialized data section */ + .bss : { + . = ALIGN(4); + /* This is used by the startup in order to initialize the .bss secion */ + _sbss = .; + _bss = .; + + *(.bss) + *(.bss.*) /* patched by elias - allows the use of -fdata-sections */ + *(COMMON) + + . = ALIGN(4); + /* This is used by the startup in order to initialize the .bss secion */ + _ebss = . ; + } >RAM + + PROVIDE ( end = _ebss); + PROVIDE (_end = _ebss); + + __exidx_start = .; + __exidx_end = .; + + /* after that it's only debugging information. */ + + /* remove the debugging information from the standard libraries */ +/* /DISCARD/ : { + libc.a ( * ) + libm.a ( * ) + libgcc.a ( * ) + }*/ + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + /* DWARF debug sections. + Symbols in the DWARF debugging sections are relative to the beginning + of the section so we begin them at 0. */ + /* DWARF 1 */ + .debug 0 : { *(.debug) } + .line 0 : { *(.line) } + /* GNU DWARF 1 extensions */ + .debug_srcinfo 0 : { *(.debug_srcinfo) } + .debug_sfnames 0 : { *(.debug_sfnames) } + /* DWARF 1.1 and DWARF 2 */ + .debug_aranges 0 : { *(.debug_aranges) } + .debug_pubnames 0 : { *(.debug_pubnames) } + /* DWARF 2 */ + .debug_info 0 : { *(.debug_info .gnu.linkonce.wi.*) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_line 0 : { *(.debug_line) } + .debug_frame 0 : { *(.debug_frame) } + .debug_str 0 : { *(.debug_str) } + .debug_loc 0 : { *(.debug_loc) } + .debug_macinfo 0 : { *(.debug_macinfo) } + /* SGI/MIPS DWARF 2 extensions */ + .debug_weaknames 0 : { *(.debug_weaknames) } + .debug_funcnames 0 : { *(.debug_funcnames) } + .debug_typenames 0 : { *(.debug_typenames) } + .debug_varnames 0 : { *(.debug_varnames) } +} diff --git a/fw/system_stm32f0xx.c b/fw/system_stm32f0xx.c new file mode 100644 index 0000000..a43c3d6 --- /dev/null +++ b/fw/system_stm32f0xx.c @@ -0,0 +1,336 @@ +/** + ****************************************************************************** + * @file system_stm32f0xx.c + * copied from: STM32Cube/Drivers/CMSIS/Device/ST/STM32F0xx/Source/Templates + * @author MCD Application Team + * @version V2.3.1 + * @date 04-November-2016 + * @brief CMSIS Cortex-M0 Device Peripheral Access Layer System Source File. + * + * 1. This file provides two functions and one global variable to be called from + * user application: + * - SystemInit(): This function is called at startup just after reset and + * before branch to main program. This call is made inside + * the "startup_stm32f0xx.s" file. + * + * - SystemCoreClock variable: Contains the core clock (HCLK), it can be used + * by the user application to setup the SysTick + * timer or configure other parameters. + * + * - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must + * be called whenever the core clock is changed + * during program execution. + * + * 2. After each device reset the HSI (8 MHz) is used as system clock source. + * Then SystemInit() function is called, in "startup_stm32f0xx.s" file, to + * configure the system clock before to branch to main program. + * + * 3. This file configures the system clock as follows: + *============================================================================= + * Supported STM32F0xx device + *----------------------------------------------------------------------------- + * System Clock source | HSI + *----------------------------------------------------------------------------- + * SYSCLK(Hz) | 8000000 + *----------------------------------------------------------------------------- + * HCLK(Hz) | 8000000 + *----------------------------------------------------------------------------- + * AHB Prescaler | 1 + *----------------------------------------------------------------------------- + * APB1 Prescaler | 1 + *----------------------------------------------------------------------------- + *============================================================================= + ****************************************************************************** + * @attention + * + *

© COPYRIGHT(c) 2016 STMicroelectronics

+ * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * 3. Neither the name of STMicroelectronics nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************** + */ + +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f0xx_system + * @{ + */ + +/** @addtogroup STM32F0xx_System_Private_Includes + * @{ + */ + +#include "stm32f0xx.h" + +/** + * @} + */ + +/** @addtogroup STM32F0xx_System_Private_TypesDefinitions + * @{ + */ + +/** + * @} + */ + +/** @addtogroup STM32F0xx_System_Private_Defines + * @{ + */ +#if !defined (HSE_VALUE) + #define HSE_VALUE ((uint32_t)8000000) /*!< Default value of the External oscillator in Hz. + This value can be provided and adapted by the user application. */ +#endif /* HSE_VALUE */ + +#if !defined (HSI_VALUE) + #define HSI_VALUE ((uint32_t)8000000) /*!< Default value of the Internal oscillator in Hz. + This value can be provided and adapted by the user application. */ +#endif /* HSI_VALUE */ + +#if !defined (HSI48_VALUE) +#define HSI48_VALUE ((uint32_t)48000000) /*!< Default value of the HSI48 Internal oscillator in Hz. + This value can be provided and adapted by the user application. */ +#endif /* HSI48_VALUE */ +/** + * @} + */ + +/** @addtogroup STM32F0xx_System_Private_Macros + * @{ + */ + +/** + * @} + */ + +/** @addtogroup STM32F0xx_System_Private_Variables + * @{ + */ + /* This variable is updated in three ways: + 1) by calling CMSIS function SystemCoreClockUpdate() + 2) by calling HAL API function HAL_RCC_GetHCLKFreq() + 3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency + Note: If you use this function to configure the system clock there is no need to + call the 2 first functions listed above, since SystemCoreClock variable is + updated automatically. + */ +uint32_t SystemCoreClock = 8000000; + +const uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; +const uint8_t APBPrescTable[8] = {0, 0, 0, 0, 1, 2, 3, 4}; + +/** + * @} + */ + +/** @addtogroup STM32F0xx_System_Private_FunctionPrototypes + * @{ + */ + +/** + * @} + */ + +/** @addtogroup STM32F0xx_System_Private_Functions + * @{ + */ + +/** + * @brief Setup the microcontroller system. + * Initialize the default HSI clock source, vector table location and the PLL configuration is reset. + * @param None + * @retval None + */ +void SystemInit(void) +{ + /* Reset the RCC clock configuration to the default reset state ------------*/ + /* Set HSION bit */ + RCC->CR |= (uint32_t)0x00000001U; + +#if defined (STM32F051x8) || defined (STM32F058x8) + /* Reset SW[1:0], HPRE[3:0], PPRE[2:0], ADCPRE and MCOSEL[2:0] bits */ + RCC->CFGR &= (uint32_t)0xF8FFB80CU; +#else + /* Reset SW[1:0], HPRE[3:0], PPRE[2:0], ADCPRE, MCOSEL[2:0], MCOPRE[2:0] and PLLNODIV bits */ + RCC->CFGR &= (uint32_t)0x08FFB80CU; +#endif /* STM32F051x8 or STM32F058x8 */ + + /* Reset HSEON, CSSON and PLLON bits */ + RCC->CR &= (uint32_t)0xFEF6FFFFU; + + /* Reset HSEBYP bit */ + RCC->CR &= (uint32_t)0xFFFBFFFFU; + + /* Reset PLLSRC, PLLXTPRE and PLLMUL[3:0] bits */ + RCC->CFGR &= (uint32_t)0xFFC0FFFFU; + + /* Reset PREDIV[3:0] bits */ + RCC->CFGR2 &= (uint32_t)0xFFFFFFF0U; + +#if defined (STM32F072xB) || defined (STM32F078xx) + /* Reset USART2SW[1:0], USART1SW[1:0], I2C1SW, CECSW, USBSW and ADCSW bits */ + RCC->CFGR3 &= (uint32_t)0xFFFCFE2CU; +#elif defined (STM32F071xB) + /* Reset USART2SW[1:0], USART1SW[1:0], I2C1SW, CECSW and ADCSW bits */ + RCC->CFGR3 &= (uint32_t)0xFFFFCEACU; +#elif defined (STM32F091xC) || defined (STM32F098xx) + /* Reset USART3SW[1:0], USART2SW[1:0], USART1SW[1:0], I2C1SW, CECSW and ADCSW bits */ + RCC->CFGR3 &= (uint32_t)0xFFF0FEACU; +#elif defined (STM32F030x6) || defined (STM32F030x8) || defined (STM32F031x6) || defined (STM32F038xx) || defined (STM32F030xC) + /* Reset USART1SW[1:0], I2C1SW and ADCSW bits */ + RCC->CFGR3 &= (uint32_t)0xFFFFFEECU; +#elif defined (STM32F051x8) || defined (STM32F058xx) + /* Reset USART1SW[1:0], I2C1SW, CECSW and ADCSW bits */ + RCC->CFGR3 &= (uint32_t)0xFFFFFEACU; +#elif defined (STM32F042x6) || defined (STM32F048xx) + /* Reset USART1SW[1:0], I2C1SW, CECSW, USBSW and ADCSW bits */ + RCC->CFGR3 &= (uint32_t)0xFFFFFE2CU; +#elif defined (STM32F070x6) || defined (STM32F070xB) + /* Reset USART1SW[1:0], I2C1SW, USBSW and ADCSW bits */ + RCC->CFGR3 &= (uint32_t)0xFFFFFE6CU; + /* Set default USB clock to PLLCLK, since there is no HSI48 */ + RCC->CFGR3 |= (uint32_t)0x00000080U; +#else + #warning "No target selected" +#endif + + /* Reset HSI14 bit */ + RCC->CR2 &= (uint32_t)0xFFFFFFFEU; + + /* Disable all interrupts */ + RCC->CIR = 0x00000000U; + +} + +/** + * @brief Update SystemCoreClock variable according to Clock Register Values. + * The SystemCoreClock variable contains the core clock (HCLK), it can + * be used by the user application to setup the SysTick timer or configure + * other parameters. + * + * @note Each time the core clock (HCLK) changes, this function must be called + * to update SystemCoreClock variable value. Otherwise, any configuration + * based on this variable will be incorrect. + * + * @note - The system frequency computed by this function is not the real + * frequency in the chip. It is calculated based on the predefined + * constant and the selected clock source: + * + * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*) + * + * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**) + * + * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) + * or HSI_VALUE(*) multiplied/divided by the PLL factors. + * + * (*) HSI_VALUE is a constant defined in stm32f0xx_hal.h file (default value + * 8 MHz) but the real value may vary depending on the variations + * in voltage and temperature. + * + * (**) HSE_VALUE is a constant defined in stm32f0xx_hal.h file (default value + * 8 MHz), user has to ensure that HSE_VALUE is same as the real + * frequency of the crystal used. Otherwise, this function may + * have wrong result. + * + * - The result of this function could be not correct when using fractional + * value for HSE crystal. + * + * @param None + * @retval None + */ +void SystemCoreClockUpdate (void) +{ + uint32_t tmp = 0, pllmull = 0, pllsource = 0, predivfactor = 0; + + /* Get SYSCLK source -------------------------------------------------------*/ + tmp = RCC->CFGR & RCC_CFGR_SWS; + + switch (tmp) + { + case RCC_CFGR_SWS_HSI: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + case RCC_CFGR_SWS_HSE: /* HSE used as system clock */ + SystemCoreClock = HSE_VALUE; + break; + case RCC_CFGR_SWS_PLL: /* PLL used as system clock */ + /* Get PLL clock source and multiplication factor ----------------------*/ + pllmull = RCC->CFGR & RCC_CFGR_PLLMUL; + pllsource = RCC->CFGR & RCC_CFGR_PLLSRC; + pllmull = ( pllmull >> 18) + 2; + predivfactor = (RCC->CFGR2 & RCC_CFGR2_PREDIV) + 1; + + if (pllsource == RCC_CFGR_PLLSRC_HSE_PREDIV) + { + /* HSE used as PLL clock source : SystemCoreClock = HSE/PREDIV * PLLMUL */ + SystemCoreClock = (HSE_VALUE/predivfactor) * pllmull; + } +#if defined(STM32F042x6) || defined(STM32F048xx) || defined(STM32F072xB) || defined(STM32F078xx) || defined(STM32F091xC) || defined(STM32F098xx) + else if (pllsource == RCC_CFGR_PLLSRC_HSI48_PREDIV) + { + /* HSI48 used as PLL clock source : SystemCoreClock = HSI48/PREDIV * PLLMUL */ + SystemCoreClock = (HSI48_VALUE/predivfactor) * pllmull; + } +#endif /* STM32F042x6 || STM32F048xx || STM32F072xB || STM32F078xx || STM32F091xC || STM32F098xx */ + else + { +#if defined(STM32F042x6) || defined(STM32F048xx) || defined(STM32F070x6) \ + || defined(STM32F078xx) || defined(STM32F071xB) || defined(STM32F072xB) \ + || defined(STM32F070xB) || defined(STM32F091xC) || defined(STM32F098xx) || defined(STM32F030xC) + /* HSI used as PLL clock source : SystemCoreClock = HSI/PREDIV * PLLMUL */ + SystemCoreClock = (HSI_VALUE/predivfactor) * pllmull; +#else + /* HSI used as PLL clock source : SystemCoreClock = HSI/2 * PLLMUL */ + SystemCoreClock = (HSI_VALUE >> 1) * pllmull; +#endif /* STM32F042x6 || STM32F048xx || STM32F070x6 || + STM32F071xB || STM32F072xB || STM32F078xx || STM32F070xB || + STM32F091xC || STM32F098xx || STM32F030xC */ + } + break; + default: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + } + /* Compute HCLK clock frequency ----------------*/ + /* Get HCLK prescaler */ + tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; + /* HCLK clock frequency */ + SystemCoreClock >>= tmp; +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ + -- cgit