From 43942973cd9b61dc94bb52be53a99f8ca711eb7e Mon Sep 17 00:00:00 2001 From: Megan Wachs Date: Tue, 31 Jan 2017 19:02:18 -0800 Subject: Move plic_driver code to a common location. --- bsp/drivers/plic/plic_driver.c | 127 +++++++++++++++++++++++++++++++++++++++ bsp/drivers/plic/plic_driver.h | 46 ++++++++++++++ bsp/env/common.mk | 1 + software/demo_gpio/Makefile | 6 +- software/demo_gpio/demo_gpio.c | 2 +- software/demo_gpio/plic_driver.c | 127 --------------------------------------- software/demo_gpio/plic_driver.h | 46 -------------- 7 files changed, 179 insertions(+), 176 deletions(-) create mode 100644 bsp/drivers/plic/plic_driver.c create mode 100644 bsp/drivers/plic/plic_driver.h delete mode 100644 software/demo_gpio/plic_driver.c delete mode 100644 software/demo_gpio/plic_driver.h diff --git a/bsp/drivers/plic/plic_driver.c b/bsp/drivers/plic/plic_driver.c new file mode 100644 index 0000000..b27d7a5 --- /dev/null +++ b/bsp/drivers/plic/plic_driver.c @@ -0,0 +1,127 @@ +// See LICENSE for license details. + +#include "sifive/devices/plic.h" +#include "plic/plic_driver.h" +#include "platform.h" +#include "encoding.h" +#include + + +// Note that there are no assertions or bounds checking on these +// parameter values. + +void volatile_memzero(uint8_t * base, unsigned int size) +{ + volatile uint8_t * ptr; + for (ptr = base; ptr < (base + size); ptr++){ + *ptr = 0; + } +} + +void PLIC_init ( + plic_instance_t * this_plic, + uintptr_t base_addr, + uint32_t num_sources, + uint32_t num_priorities + ) +{ + + this_plic->base_addr = base_addr; + this_plic->num_sources = num_sources; + this_plic->num_priorities = num_priorities; + + // Disable all interrupts (don't assume that these registers are reset). + unsigned long hart_id = read_csr(mhartid); + volatile_memzero((uint8_t*) (this_plic->base_addr + + PLIC_ENABLE_OFFSET + + (hart_id << PLIC_ENABLE_SHIFT_PER_TARGET)), + (num_sources + 8) / 8); + + // Set all priorities to 0 (equal priority -- don't assume that these are reset). + volatile_memzero ((uint8_t *)(this_plic->base_addr + + PLIC_PRIORITY_OFFSET), + (num_sources + 1) << PLIC_PRIORITY_SHIFT_PER_SOURCE); + + // Set the threshold to 0. + volatile plic_threshold* threshold = (plic_threshold*) + (this_plic->base_addr + + PLIC_THRESHOLD_OFFSET + + (hart_id << PLIC_THRESHOLD_SHIFT_PER_TARGET)); + + *threshold = 0; + +} + +void PLIC_set_threshold (plic_instance_t * this_plic, + plic_threshold threshold){ + + unsigned long hart_id = read_csr(mhartid); + volatile plic_threshold* threshold_ptr = (plic_threshold*) (this_plic->base_addr + + PLIC_THRESHOLD_OFFSET + + (hart_id << PLIC_THRESHOLD_SHIFT_PER_TARGET)); + + *threshold_ptr = threshold; + +} + + +void PLIC_enable_interrupt (plic_instance_t * this_plic, plic_source source){ + + unsigned long hart_id = read_csr(mhartid); + volatile uint8_t * current_ptr = (volatile uint8_t *)(this_plic->base_addr + + PLIC_ENABLE_OFFSET + + (hart_id << PLIC_ENABLE_SHIFT_PER_TARGET) + + (source >> 3)); + uint8_t current = *current_ptr; + current = current | ( 1 << (source & 0x7)); + *current_ptr = current; + +} + +void PLIC_disable_interrupt (plic_instance_t * this_plic, plic_source source){ + + unsigned long hart_id = read_csr(mhartid); + volatile uint8_t * current_ptr = (volatile uint8_t *) (this_plic->base_addr + + PLIC_ENABLE_OFFSET + + (hart_id << PLIC_ENABLE_SHIFT_PER_TARGET) + + (source >> 3)); + uint8_t current = *current_ptr; + current = current & ~(( 1 << (source & 0x7))); + *current_ptr = current; + +} + +void PLIC_set_priority (plic_instance_t * this_plic, plic_source source, plic_priority priority){ + + if (this_plic->num_priorities > 0) { + volatile plic_priority * priority_ptr = (volatile plic_priority *) + (this_plic->base_addr + + PLIC_PRIORITY_OFFSET + + (source << PLIC_PRIORITY_SHIFT_PER_SOURCE)); + *priority_ptr = priority; + } +} + +plic_source PLIC_claim_interrupt(plic_instance_t * this_plic){ + + unsigned long hart_id = read_csr(mhartid); + + volatile plic_source * claim_addr = (volatile plic_source * ) + (this_plic->base_addr + + PLIC_CLAIM_OFFSET + + (hart_id << PLIC_CLAIM_SHIFT_PER_TARGET)); + + return *claim_addr; + +} + +void PLIC_complete_interrupt(plic_instance_t * this_plic, plic_source source){ + + unsigned long hart_id = read_csr(mhartid); + volatile plic_source * claim_addr = (volatile plic_source *) (this_plic->base_addr + + PLIC_CLAIM_OFFSET + + (hart_id << PLIC_CLAIM_SHIFT_PER_TARGET)); + *claim_addr = source; + +} + diff --git a/bsp/drivers/plic/plic_driver.h b/bsp/drivers/plic/plic_driver.h new file mode 100644 index 0000000..66410be --- /dev/null +++ b/bsp/drivers/plic/plic_driver.h @@ -0,0 +1,46 @@ +// See LICENSE file for licence details + +#ifndef PLIC_DRIVER_H +#define PLIC_DRIVER_H + +#include "platform.h" + +typedef struct __plic_instance_t +{ + uintptr_t base_addr; + + uint32_t num_sources; + uint32_t num_priorities; + +} plic_instance_t; + +typedef uint32_t plic_source; +typedef uint32_t plic_priority; +typedef uint32_t plic_threshold; + +void PLIC_init ( + plic_instance_t * this_plic, + uintptr_t base_addr, + uint32_t num_sources, + uint32_t num_priorities + ); + +void PLIC_set_threshold (plic_instance_t * this_plic, + plic_threshold threshold); + +void PLIC_enable_interrupt (plic_instance_t * this_plic, + plic_source source); + +void PLIC_disable_interrupt (plic_instance_t * this_plic, + plic_source source); + +void PLIC_set_priority (plic_instance_t * this_plic, + plic_source source, + plic_priority priority); + +plic_source PLIC_claim_interrupt(plic_instance_t * this_plic); + +void PLIC_complete_interrupt(plic_instance_t * this_plic, + plic_source source); + +#endif diff --git a/bsp/env/common.mk b/bsp/env/common.mk index 420c196..c828306 100644 --- a/bsp/env/common.mk +++ b/bsp/env/common.mk @@ -19,6 +19,7 @@ C_SRCS += $(PLATFORM_DIR)/init.c LINKER_SCRIPT := $(PLATFORM_DIR)/link.lds INCLUDES += -I$(BSP_BASE)/include +INCLUDES += -I$(BSP_BASE)/drivers/ INCLUDES += -I$(ENV_DIR) INCLUDES += -I$(PLATFORM_DIR) diff --git a/software/demo_gpio/Makefile b/software/demo_gpio/Makefile index 04a11b7..b181c5f 100644 --- a/software/demo_gpio/Makefile +++ b/software/demo_gpio/Makefile @@ -1,7 +1,9 @@ TARGET = demo_gpio -C_SRCS += demo_gpio.c -C_SRCS += plic_driver.c CFLAGS += -O2 -fno-builtin-printf -DUSE_PLIC -DUSE_M_TIME BSP_BASE = ../../bsp + +C_SRCS += demo_gpio.c +C_SRCS += $(BSP_BASE)/drivers/plic/plic_driver.c + include $(BSP_BASE)/env/common.mk diff --git a/software/demo_gpio/demo_gpio.c b/software/demo_gpio/demo_gpio.c index 00bc8cd..bfa388e 100644 --- a/software/demo_gpio/demo_gpio.c +++ b/software/demo_gpio/demo_gpio.c @@ -4,7 +4,7 @@ #include #include "platform.h" #include -#include "plic_driver.h" +#include "plic/plic_driver.h" #include "encoding.h" #include #include "stdatomic.h" diff --git a/software/demo_gpio/plic_driver.c b/software/demo_gpio/plic_driver.c deleted file mode 100644 index 01b7e6e..0000000 --- a/software/demo_gpio/plic_driver.c +++ /dev/null @@ -1,127 +0,0 @@ -// See LICENSE for license details. - -#include "sifive/devices/plic.h" -#include "plic_driver.h" -#include "platform.h" -#include "encoding.h" -#include - - -// Note that there are no assertions or bounds checking on these -// parameter values. - -void volatile_memzero(uint8_t * base, unsigned int size) -{ - volatile uint8_t * ptr; - for (ptr = base; ptr < (base + size); ptr++){ - *ptr = 0; - } -} - -void PLIC_init ( - plic_instance_t * this_plic, - uintptr_t base_addr, - uint32_t num_sources, - uint32_t num_priorities - ) -{ - - this_plic->base_addr = base_addr; - this_plic->num_sources = num_sources; - this_plic->num_priorities = num_priorities; - - // Disable all interrupts (don't assume that these registers are reset). - unsigned long hart_id = read_csr(mhartid); - volatile_memzero((uint8_t*) (this_plic->base_addr + - PLIC_ENABLE_OFFSET + - (hart_id << PLIC_ENABLE_SHIFT_PER_TARGET)), - (num_sources + 8) / 8); - - // Set all priorities to 0 (equal priority -- don't assume that these are reset). - volatile_memzero ((uint8_t *)(this_plic->base_addr + - PLIC_PRIORITY_OFFSET), - (num_sources + 1) << PLIC_PRIORITY_SHIFT_PER_SOURCE); - - // Set the threshold to 0. - volatile plic_threshold* threshold = (plic_threshold*) - (this_plic->base_addr + - PLIC_THRESHOLD_OFFSET + - (hart_id << PLIC_THRESHOLD_SHIFT_PER_TARGET)); - - *threshold = 0; - -} - -void PLIC_set_threshold (plic_instance_t * this_plic, - plic_threshold threshold){ - - unsigned long hart_id = read_csr(mhartid); - volatile plic_threshold* threshold_ptr = (plic_threshold*) (this_plic->base_addr + - PLIC_THRESHOLD_OFFSET + - (hart_id << PLIC_THRESHOLD_SHIFT_PER_TARGET)); - - *threshold_ptr = threshold; - -} - - -void PLIC_enable_interrupt (plic_instance_t * this_plic, plic_source source){ - - unsigned long hart_id = read_csr(mhartid); - volatile uint8_t * current_ptr = (volatile uint8_t *)(this_plic->base_addr + - PLIC_ENABLE_OFFSET + - (hart_id << PLIC_ENABLE_SHIFT_PER_TARGET) + - (source >> 3)); - uint8_t current = *current_ptr; - current = current | ( 1 << (source & 0x7)); - *current_ptr = current; - -} - -void PLIC_disable_interrupt (plic_instance_t * this_plic, plic_source source){ - - unsigned long hart_id = read_csr(mhartid); - volatile uint8_t * current_ptr = (volatile uint8_t *) (this_plic->base_addr + - PLIC_ENABLE_OFFSET + - (hart_id << PLIC_ENABLE_SHIFT_PER_TARGET) + - (source >> 3)); - uint8_t current = *current_ptr; - current = current & ~(( 1 << (source & 0x7))); - *current_ptr = current; - -} - -void PLIC_set_priority (plic_instance_t * this_plic, plic_source source, plic_priority priority){ - - if (this_plic->num_priorities > 0) { - volatile plic_priority * priority_ptr = (volatile plic_priority *) - (this_plic->base_addr + - PLIC_PRIORITY_OFFSET + - (source << PLIC_PRIORITY_SHIFT_PER_SOURCE)); - *priority_ptr = priority; - } -} - -plic_source PLIC_claim_interrupt(plic_instance_t * this_plic){ - - unsigned long hart_id = read_csr(mhartid); - - volatile plic_source * claim_addr = (volatile plic_source * ) - (this_plic->base_addr + - PLIC_CLAIM_OFFSET + - (hart_id << PLIC_CLAIM_SHIFT_PER_TARGET)); - - return *claim_addr; - -} - -void PLIC_complete_interrupt(plic_instance_t * this_plic, plic_source source){ - - unsigned long hart_id = read_csr(mhartid); - volatile plic_source * claim_addr = (volatile plic_source *) (this_plic->base_addr + - PLIC_CLAIM_OFFSET + - (hart_id << PLIC_CLAIM_SHIFT_PER_TARGET)); - *claim_addr = source; - -} - diff --git a/software/demo_gpio/plic_driver.h b/software/demo_gpio/plic_driver.h deleted file mode 100644 index 66410be..0000000 --- a/software/demo_gpio/plic_driver.h +++ /dev/null @@ -1,46 +0,0 @@ -// See LICENSE file for licence details - -#ifndef PLIC_DRIVER_H -#define PLIC_DRIVER_H - -#include "platform.h" - -typedef struct __plic_instance_t -{ - uintptr_t base_addr; - - uint32_t num_sources; - uint32_t num_priorities; - -} plic_instance_t; - -typedef uint32_t plic_source; -typedef uint32_t plic_priority; -typedef uint32_t plic_threshold; - -void PLIC_init ( - plic_instance_t * this_plic, - uintptr_t base_addr, - uint32_t num_sources, - uint32_t num_priorities - ); - -void PLIC_set_threshold (plic_instance_t * this_plic, - plic_threshold threshold); - -void PLIC_enable_interrupt (plic_instance_t * this_plic, - plic_source source); - -void PLIC_disable_interrupt (plic_instance_t * this_plic, - plic_source source); - -void PLIC_set_priority (plic_instance_t * this_plic, - plic_source source, - plic_priority priority); - -plic_source PLIC_claim_interrupt(plic_instance_t * this_plic); - -void PLIC_complete_interrupt(plic_instance_t * this_plic, - plic_source source); - -#endif -- cgit v1.2.1-18-gbd029 From 0adc37422f48e9468d980a1d98dcc65bd7ab3043 Mon Sep 17 00:00:00 2001 From: Megan Wachs Date: Tue, 31 Jan 2017 20:25:38 -0800 Subject: add a 'driver' common PRCI functions. --- bsp/drivers/fe300prci/fe300prci_driver.c | 187 +++++++++++++++++++++++++++++++ bsp/drivers/fe300prci/fe300prci_driver.h | 54 +++++++++ bsp/env/hifive1.h | 2 + 3 files changed, 243 insertions(+) create mode 100644 bsp/drivers/fe300prci/fe300prci_driver.c create mode 100644 bsp/drivers/fe300prci/fe300prci_driver.h diff --git a/bsp/drivers/fe300prci/fe300prci_driver.c b/bsp/drivers/fe300prci/fe300prci_driver.c new file mode 100644 index 0000000..3338b81 --- /dev/null +++ b/bsp/drivers/fe300prci/fe300prci_driver.c @@ -0,0 +1,187 @@ +// See LICENSE file for license details + +#include "drivers/fe300prci/fe300prci_driver.h" +#include "platform.h" +#include + +#define rdmcycle(x) { \ + uint32_t lo, hi, hi2; \ + __asm__ __volatile__ ("1:\n\t" \ + "csrr %0, mcycleh\n\t" \ + "csrr %1, mcycle\n\t" \ + "csrr %2, mcycleh\n\t" \ + "bne %0, %2, 1b\n\t" \ + : "=r" (hi), "=r" (lo), "=r" (hi2)) ; \ + *(x) = lo | ((uint64_t) hi << 32); \ + } + +static uint32_t PRCI_measure_mcycle_freq(uint32_t mtime_ticks, uint32_t mtime_freq) +{ + + uint32_t start_mtime = CLINT_REG(CLINT_MTIME); + uint32_t end_mtime = start_mtime + mtime_ticks + 1; + + // Make sure we won't get rollover. + while (end_mtime < start_mtime){ + start_mtime = CLINT_REG(CLINT_MTIME); + end_mtime = start_mtime + mtime_ticks + 1; + } + + // Don't start measuring until mtime edge. + uint32_t tmp = start_mtime; + do { + start_mtime = CLINT_REG(CLINT_MTIME); + } while (start_mtime == tmp); + + uint64_t start_mcycle; + rdmcycle(&start_mcycle); + + while (CLINT_REG(CLINT_MTIME) < end_mtime) ; + + uint64_t end_mcycle; + rdmcycle(&end_mcycle); + uint32_t difference = (uint32_t) (end_mcycle - start_mcycle); + + uint64_t freq = ((uint64_t) difference * mtime_freq) / mtime_ticks; + return (uint32_t) freq & 0xFFFFFFFF; + +} + + +static void PRCI_use_hfrosc(int div, int trim) +{ + // Make sure the HFROSC is running at its default setting + // It is OK to change this even if we are running off of it. + + PRCI_REG(PRCI_HFROSCCFG) = (ROSC_DIV(div) | ROSC_TRIM(trim) | ROSC_EN(1)); + + while ((PRCI_REG(PRCI_HFROSCCFG) & ROSC_RDY(1)) == 0); + + PRCI_REG(PRCI_PLLCFG) &= ~PLL_SEL(1); +} + +static void PRCI_use_pll(int refsel, int bypass, + int r, int f, int q, int finaldiv, + int hfroscdiv, int hfrosctrim) +{ + // Ensure that we aren't running off the PLL before we mess with it. + if (PRCI_REG(PRCI_PLLCFG) & PLL_SEL(1)) { + // Make sure the HFROSC is running at its default setting + PRCI_use_hfrosc(4, 16); + } + + // Set PLL Source to be HFXOSC if available. + uint32_t config_value = 0; + + config_value |= PLL_REFSEL(refsel); + + if (bypass) { + // Bypass + config_value |= PLL_BYPASS(1); + + PRCI_REG(PRCI_PLLCFG) = config_value; + + // If we don't have an HFXTAL, this doesn't really matter. + // Set our Final output divide to divide-by-1: + PRCI_REG(PRCI_PLLDIV) = (PLL_FINAL_DIV_BY_1(1) | PLL_FINAL_DIV(0)); + } else { + + // To overclock, use the hfrosc + if (hfrosctrim >= 0 && hfroscdiv >= 0) { + PRCI_use_hfrosc(hfroscdiv, hfrosctrim); + } + + // Set DIV Settings for PLL + + // (Legal values of f_REF are 6-48MHz) + + // Set DIVR to divide-by-2 to get 8MHz frequency + // (legal values of f_R are 6-12 MHz) + + config_value |= PLL_BYPASS(1); + config_value |= PLL_R(r); + + // Set DIVF to get 512Mhz frequncy + // There is an implied multiply-by-2, 16Mhz. + // So need to write 32-1 + // (legal values of f_F are 384-768 MHz) + config_value |= PLL_F(f); + + // Set DIVQ to divide-by-2 to get 256 MHz frequency + // (legal values of f_Q are 50-400Mhz) + config_value |= PLL_Q(q); + + // Set our Final output divide to divide-by-1: + if (finaldiv == 1){ + PRCI_REG(PRCI_PLLDIV) = (PLL_FINAL_DIV_BY_1(1) | PLL_FINAL_DIV(0)); + } else { + PRCI_REG(PRCI_PLLDIV) = (PLL_FINAL_DIV(finaldiv)); + } + + PRCI_REG(PRCI_PLLCFG) = config_value; + + // Un-Bypass the PLL. + PRCI_REG(PRCI_PLLCFG) &= ~PLL_BYPASS(1); + + // Wait for PLL Lock + // Note that the Lock signal can be glitchy. + // Need to wait 100 us + // RTC is running at 32kHz. + // So wait 4 ticks of RTC. + uint32_t now = CLINT_REG(CLINT_MTIME); + while (CLINT_REG(CLINT_MTIME) - now < 4) ; + + // Now it is safe to check for PLL Lock + while ((PRCI_REG(PRCI_PLLCFG) & PLL_LOCK(1)) == 0); + + } + + // Switch over to PLL Clock source + PRCI_REG(PRCI_PLLCFG) |= PLL_SEL(1); +} + +static void PRCI_use_default_clocks() +{ + // Turn off the LFROSC + AON_REG(AON_LFROSC) &= ~ROSC_EN(1); + + // Use HFROSC + PRCI_use_hfrosc(4, 16); +} + +// This is a generic function, which +// doesn't span the entire range of HFROSC settings. +// It only adjusts the trim, which can span a hundred MHz or so. +// This function needs improvement to ensure that the PLL settings +// are legal every step of the way. +// It returns the actual measured CPU frequency. + +uint32_t PRCI_set_hfrosctrim_for_f_cpu(uint32_t f_cpu) +{ + + int hfrosctrim = 16; + PRCI_use_default_clocks(); + PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, 16); + // Ignore the first run (for icache reasons) + uint32_t cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); + cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); + + if (cpu_freq > F_CPU) { + while((cpu_freq > F_CPU) && (hfrosctrim >= 0) ){ + PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, hfrosctrim ); + cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); + hfrosctrim--; + } + return cpu_freq; + } + + while(cpu_freq < F_CPU) { + if (hfrosctrim >= 0x20) { + break; + } + PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, hfrosctrim ); + cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); + hfrosctrim++; + } + return cpu_freq; +} diff --git a/bsp/drivers/fe300prci/fe300prci_driver.h b/bsp/drivers/fe300prci/fe300prci_driver.h new file mode 100644 index 0000000..052edcb --- /dev/null +++ b/bsp/drivers/fe300prci/fe300prci_driver.h @@ -0,0 +1,54 @@ +// See LICENSE file for license details + +#ifndef _FE300PRCI_DRIVER_H_ +#define _FE300PRCI_DRIVER_H_ + +#include + + +/* Measure and return the approximate frequency of the + * CPU, as given by measuring the mcycle counter against + * the mtime ticks. + */ +static uint32_t PRCI_measure_mcycle_freq(uint32_t mtime_ticks, uint32_t mtime_freq); + +/* Safely switch over to the HFROSC using the given div + * and trim settings. + */ +static void PRCI_use_hfrosc(int div, int trim); + +/* Safely switch over to the PLL using the given + * settings. + * + * Note that not all combinations of the inputs are actually + * legal, and this function does not check for their + * legality ("safely" means that this function won't turn off + * or glitch the clock the CPU is actually running off, but + * doesn't protect against you making it too fast or slow.) + */ + +static void PRCI_use_pll(int refsel, int bypass, + int r, int f, int q, int finaldiv, + int hfroscdiv, int hfrosctrim); + +/* Use the default clocks configured at reset. + * This is ~16Mhz HFROSC and turns off the LFROSC + * (on the current FE310 Dev Platforms, an external LFROSC is + * used as it is more power efficient). + */ +static void PRCI_use_default_clocks(); + +/* This routine will adjust the HFROSC trim + * as the PLL clock source, measure the resulting + * frequency, and attempt to get close to the requested + * frequency. It returns the actual measured frequency. + * + * Note that the requested frequency must be within the + * range supported by the PLL so not all values are + * achievable with this function, and not all + * are guaranteed to actually work. + */ +uint32_t PRCI_set_hfrosctrim_for_f_cpu(uint32_t f_cpu); + +#endif + diff --git a/bsp/env/hifive1.h b/bsp/env/hifive1.h index 11b98f8..4c65f18 100644 --- a/bsp/env/hifive1.h +++ b/bsp/env/hifive1.h @@ -74,6 +74,8 @@ #define HAS_HFXOSC 1 #define HAS_LFROSC_BYPASS 1 +#define RTC_FREQ 32768 + void write_hex(int fd, uint32_t hex); #endif /* _SIFIVE_HIFIVE1_H */ -- cgit v1.2.1-18-gbd029 From 08a72bb956a25fd94d965fb4474f3ddcca1ed1e6 Mon Sep 17 00:00:00 2001 From: Megan Wachs Date: Wed, 1 Feb 2017 16:39:38 -0800 Subject: Additional PRCI driver support. Make the code usable by C++ programs as well. --- bsp/drivers/fe300prci/fe300prci_driver.c | 39 ++++++++++++++++++++++++-------- bsp/drivers/fe300prci/fe300prci_driver.h | 19 ++++++++++++---- bsp/drivers/plic/plic_driver.h | 6 ++++- bsp/env/common.mk | 2 +- 4 files changed, 50 insertions(+), 16 deletions(-) diff --git a/bsp/drivers/fe300prci/fe300prci_driver.c b/bsp/drivers/fe300prci/fe300prci_driver.c index 3338b81..9a16d5d 100644 --- a/bsp/drivers/fe300prci/fe300prci_driver.c +++ b/bsp/drivers/fe300prci/fe300prci_driver.c @@ -1,6 +1,6 @@ // See LICENSE file for license details -#include "drivers/fe300prci/fe300prci_driver.h" +#include "fe300prci/fe300prci_driver.h" #include "platform.h" #include @@ -15,7 +15,7 @@ *(x) = lo | ((uint64_t) hi << 32); \ } -static uint32_t PRCI_measure_mcycle_freq(uint32_t mtime_ticks, uint32_t mtime_freq) +uint32_t PRCI_measure_mcycle_freq(uint32_t mtime_ticks, uint32_t mtime_freq) { uint32_t start_mtime = CLINT_REG(CLINT_MTIME); @@ -48,7 +48,7 @@ static uint32_t PRCI_measure_mcycle_freq(uint32_t mtime_ticks, uint32_t mtime_fr } -static void PRCI_use_hfrosc(int div, int trim) +void PRCI_use_hfrosc(int div, int trim) { // Make sure the HFROSC is running at its default setting // It is OK to change this even if we are running off of it. @@ -60,7 +60,7 @@ static void PRCI_use_hfrosc(int div, int trim) PRCI_REG(PRCI_PLLCFG) &= ~PLL_SEL(1); } -static void PRCI_use_pll(int refsel, int bypass, +void PRCI_use_pll(int refsel, int bypass, int r, int f, int q, int finaldiv, int hfroscdiv, int hfrosctrim) { @@ -70,7 +70,7 @@ static void PRCI_use_pll(int refsel, int bypass, PRCI_use_hfrosc(4, 16); } - // Set PLL Source to be HFXOSC if available. + // Set PLL Source to be HFXOSC if desired. uint32_t config_value = 0; config_value |= PLL_REFSEL(refsel); @@ -115,7 +115,7 @@ static void PRCI_use_pll(int refsel, int bypass, if (finaldiv == 1){ PRCI_REG(PRCI_PLLDIV) = (PLL_FINAL_DIV_BY_1(1) | PLL_FINAL_DIV(0)); } else { - PRCI_REG(PRCI_PLLDIV) = (PLL_FINAL_DIV(finaldiv)); + PRCI_REG(PRCI_PLLDIV) = (PLL_FINAL_DIV(finaldiv-1)); } PRCI_REG(PRCI_PLLCFG) = config_value; @@ -138,9 +138,16 @@ static void PRCI_use_pll(int refsel, int bypass, // Switch over to PLL Clock source PRCI_REG(PRCI_PLLCFG) |= PLL_SEL(1); + + // If we're running off HFXOSC, turn off the HFROSC to + // save power. + if (refsel) { + PRCI_REG(PRCI_HFROSCCFG) &= ~ROSC_EN(1); + } + } -static void PRCI_use_default_clocks() +void PRCI_use_default_clocks() { // Turn off the LFROSC AON_REG(AON_LFROSC) &= ~ROSC_EN(1); @@ -149,11 +156,25 @@ static void PRCI_use_default_clocks() PRCI_use_hfrosc(4, 16); } +void PRCI_use_hfxosc(uint32_t finaldiv) +{ + + PRCI_use_pll(1, // Use HFXTAL + 1, // Bypass = 1 + 0, // PLL settings don't matter + 0, // PLL settings don't matter + 0, // PLL settings don't matter + finaldiv, + -1, + -1); +} + // This is a generic function, which // doesn't span the entire range of HFROSC settings. // It only adjusts the trim, which can span a hundred MHz or so. -// This function needs improvement to ensure that the PLL settings -// are legal every step of the way. +// This function does not check the legality of the PLL settings +// at all, ant it is quite possible to configure invalid PLL settings +// this way. // It returns the actual measured CPU frequency. uint32_t PRCI_set_hfrosctrim_for_f_cpu(uint32_t f_cpu) diff --git a/bsp/drivers/fe300prci/fe300prci_driver.h b/bsp/drivers/fe300prci/fe300prci_driver.h index 052edcb..75dcf00 100644 --- a/bsp/drivers/fe300prci/fe300prci_driver.h +++ b/bsp/drivers/fe300prci/fe300prci_driver.h @@ -3,19 +3,26 @@ #ifndef _FE300PRCI_DRIVER_H_ #define _FE300PRCI_DRIVER_H_ -#include +__BEGIN_DECLS +#include /* Measure and return the approximate frequency of the * CPU, as given by measuring the mcycle counter against * the mtime ticks. */ -static uint32_t PRCI_measure_mcycle_freq(uint32_t mtime_ticks, uint32_t mtime_freq); +uint32_t PRCI_measure_mcycle_freq(uint32_t mtime_ticks, uint32_t mtime_freq); /* Safely switch over to the HFROSC using the given div * and trim settings. */ -static void PRCI_use_hfrosc(int div, int trim); +void PRCI_use_hfrosc(int div, int trim); + +/* Safely switch over to the 16MHz HFXOSC, + * applying the finaldiv clock divider (1 is the lowest + * legal value). + */ +void PRCI_use_hfxosc(uint32_t finaldiv); /* Safely switch over to the PLL using the given * settings. @@ -27,7 +34,7 @@ static void PRCI_use_hfrosc(int div, int trim); * doesn't protect against you making it too fast or slow.) */ -static void PRCI_use_pll(int refsel, int bypass, +void PRCI_use_pll(int refsel, int bypass, int r, int f, int q, int finaldiv, int hfroscdiv, int hfrosctrim); @@ -36,7 +43,7 @@ static void PRCI_use_pll(int refsel, int bypass, * (on the current FE310 Dev Platforms, an external LFROSC is * used as it is more power efficient). */ -static void PRCI_use_default_clocks(); +void PRCI_use_default_clocks(); /* This routine will adjust the HFROSC trim * as the PLL clock source, measure the resulting @@ -50,5 +57,7 @@ static void PRCI_use_default_clocks(); */ uint32_t PRCI_set_hfrosctrim_for_f_cpu(uint32_t f_cpu); +__END_DECLS + #endif diff --git a/bsp/drivers/plic/plic_driver.h b/bsp/drivers/plic/plic_driver.h index 66410be..b14b442 100644 --- a/bsp/drivers/plic/plic_driver.h +++ b/bsp/drivers/plic/plic_driver.h @@ -3,6 +3,8 @@ #ifndef PLIC_DRIVER_H #define PLIC_DRIVER_H +__BEGIN_DECLS + #include "platform.h" typedef struct __plic_instance_t @@ -42,5 +44,7 @@ plic_source PLIC_claim_interrupt(plic_instance_t * this_plic); void PLIC_complete_interrupt(plic_instance_t * this_plic, plic_source source); - + +__END_DECLS + #endif diff --git a/bsp/env/common.mk b/bsp/env/common.mk index c828306..74f5582 100644 --- a/bsp/env/common.mk +++ b/bsp/env/common.mk @@ -48,7 +48,7 @@ $(ASM_OBJS): %.o: %.S $(HEADERS) $(CC) $(CFLAGS) $(INCLUDES) -c -o $@ $< $(C_OBJS): %.o: %.c $(HEADERS) - $(CC) $(CFLAGS) $(INCLUDES) -c -o $@ $< + $(CC) $(CFLAGS) $(INCLUDES) -include sys/cdefs.h -c -o $@ $< .PHONY: clean clean: -- cgit v1.2.1-18-gbd029 From b003c52b0f5460c6e0e15cb29171164530490e08 Mon Sep 17 00:00:00 2001 From: Megan Wachs Date: Wed, 1 Feb 2017 18:03:37 -0800 Subject: PRCI: add control for the target CPU frequency (to allow a minimum, maximum, or closest match) --- bsp/drivers/fe300prci/fe300prci_driver.c | 63 ++++++++++++++++++++++++++------ bsp/drivers/fe300prci/fe300prci_driver.h | 10 ++++- bsp/drivers/plic/plic_driver.h | 1 + 3 files changed, 61 insertions(+), 13 deletions(-) diff --git a/bsp/drivers/fe300prci/fe300prci_driver.c b/bsp/drivers/fe300prci/fe300prci_driver.c index 9a16d5d..0e555a6 100644 --- a/bsp/drivers/fe300prci/fe300prci_driver.c +++ b/bsp/drivers/fe300prci/fe300prci_driver.c @@ -177,7 +177,7 @@ void PRCI_use_hfxosc(uint32_t finaldiv) // this way. // It returns the actual measured CPU frequency. -uint32_t PRCI_set_hfrosctrim_for_f_cpu(uint32_t f_cpu) +uint32_t PRCI_set_hfrosctrim_for_f_cpu(uint32_t f_cpu, PRCI_freq_target target ) { int hfrosctrim = 16; @@ -186,23 +186,62 @@ uint32_t PRCI_set_hfrosctrim_for_f_cpu(uint32_t f_cpu) // Ignore the first run (for icache reasons) uint32_t cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); + int prev_trim = hfrosctrim; + int prev_freq = cpu_freq; if (cpu_freq > F_CPU) { - while((cpu_freq > F_CPU) && (hfrosctrim >= 0) ){ - PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, hfrosctrim ); - cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); - hfrosctrim--; - } + while((cpu_freq > F_CPU) && (hfrosctrim > 0) ){ + prev_freq = cpu_freq; + prev_trim = hfrosctrim; + hfrosctrim--; + PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, hfrosctrim ); + cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); + } + + // Check for over/undershoot + switch(target) { + case(PRCI_FREQ_CLOSEST): + if ((prev_freq - F_CPU) < (F_CPU - cpu_freq) && target == PRCI_FREQ_CLOSEST) { + PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, prev_trim); + cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); + } + break; + case(PRCI_FREQ_OVERSHOOT): + PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, prev_trim); + cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); + break; + //default: + // Do Nothing + } + return cpu_freq; } - - while(cpu_freq < F_CPU) { - if (hfrosctrim >= 0x20) { - break; - } + + while(cpu_freq < F_CPU && hfrosctrim < 0x20) { + prev_freq = cpu_freq; + prev_trim = hfrosctrim; + hfrosctrim ++; PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, hfrosctrim ); cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); - hfrosctrim++; } + + + // Check for over/undershoot + switch(target) { + case(PRCI_FREQ_CLOSEST): + if ((F_CPU - prev_freq) < (cpu_freq - F_CPU)) { + PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, prev_trim); + cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); + } + break; + case(PRCI_FREQ_UNDERSHOOT): + PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, prev_trim); + cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); + break; + //default: + // Do Nothing + } + return cpu_freq; + } diff --git a/bsp/drivers/fe300prci/fe300prci_driver.h b/bsp/drivers/fe300prci/fe300prci_driver.h index 75dcf00..243e6de 100644 --- a/bsp/drivers/fe300prci/fe300prci_driver.h +++ b/bsp/drivers/fe300prci/fe300prci_driver.h @@ -7,6 +7,14 @@ __BEGIN_DECLS #include +typedef enum prci_freq_target { + + PRCI_FREQ_OVERSHOOT, + PRCI_FREQ_CLOSEST, + PRCI_FREQ_UNDERSHOOT + +} PRCI_freq_target; + /* Measure and return the approximate frequency of the * CPU, as given by measuring the mcycle counter against * the mtime ticks. @@ -55,7 +63,7 @@ void PRCI_use_default_clocks(); * achievable with this function, and not all * are guaranteed to actually work. */ -uint32_t PRCI_set_hfrosctrim_for_f_cpu(uint32_t f_cpu); +uint32_t PRCI_set_hfrosctrim_for_f_cpu(uint32_t f_cpu, PRCI_freq_target target); __END_DECLS diff --git a/bsp/drivers/plic/plic_driver.h b/bsp/drivers/plic/plic_driver.h index b14b442..e7d609b 100644 --- a/bsp/drivers/plic/plic_driver.h +++ b/bsp/drivers/plic/plic_driver.h @@ -3,6 +3,7 @@ #ifndef PLIC_DRIVER_H #define PLIC_DRIVER_H + __BEGIN_DECLS #include "platform.h" -- cgit v1.2.1-18-gbd029 From 4c4c4cd044978d9f72a872b1b8f674cd6f061c1b Mon Sep 17 00:00:00 2001 From: Megan Wachs Date: Wed, 1 Feb 2017 18:36:22 -0800 Subject: PRCI: This doesn't exist on the Arty and potentially other devices, so exclude it if there is not PRCI_BASE_ADDR on the system. --- bsp/drivers/fe300prci/fe300prci_driver.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/bsp/drivers/fe300prci/fe300prci_driver.c b/bsp/drivers/fe300prci/fe300prci_driver.c index 0e555a6..8491554 100644 --- a/bsp/drivers/fe300prci/fe300prci_driver.c +++ b/bsp/drivers/fe300prci/fe300prci_driver.c @@ -1,7 +1,9 @@ // See LICENSE file for license details -#include "fe300prci/fe300prci_driver.h" #include "platform.h" + +#ifdef PRCI_BASE_ADDR +#include "fe300prci/fe300prci_driver.h" #include #define rdmcycle(x) { \ @@ -245,3 +247,5 @@ uint32_t PRCI_set_hfrosctrim_for_f_cpu(uint32_t f_cpu, PRCI_freq_target target ) return cpu_freq; } + +#endif -- cgit v1.2.1-18-gbd029 From 2579554bb17038dffa9af57649c1e83e614efbc6 Mon Sep 17 00:00:00 2001 From: Megan Wachs Date: Thu, 2 Feb 2017 09:56:52 -0800 Subject: prci: Simplify the targeted f_cpu function by measuring HFROSC frequency first, then using it as the PLL source. This is also safer as it is less risk of using PLL out of its range. --- bsp/drivers/fe300prci/fe300prci_driver.c | 85 ++++++++++++++++---------------- bsp/drivers/fe300prci/fe300prci_driver.h | 16 ++++-- 2 files changed, 54 insertions(+), 47 deletions(-) diff --git a/bsp/drivers/fe300prci/fe300prci_driver.c b/bsp/drivers/fe300prci/fe300prci_driver.c index 8491554..5fb2c27 100644 --- a/bsp/drivers/fe300prci/fe300prci_driver.c +++ b/bsp/drivers/fe300prci/fe300prci_driver.c @@ -175,75 +175,74 @@ void PRCI_use_hfxosc(uint32_t finaldiv) // doesn't span the entire range of HFROSC settings. // It only adjusts the trim, which can span a hundred MHz or so. // This function does not check the legality of the PLL settings -// at all, ant it is quite possible to configure invalid PLL settings +// at all, and it is quite possible to configure invalid PLL settings // this way. // It returns the actual measured CPU frequency. uint32_t PRCI_set_hfrosctrim_for_f_cpu(uint32_t f_cpu, PRCI_freq_target target ) { - int hfrosctrim = 16; - PRCI_use_default_clocks(); - PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, 16); + int hfrosctrim = 0; + int hfroscdiv = 4; + int prev_trim = 0; + + // In this function we use PLL settings which + // will give us a 32x multiplier from the output + // of the HFROSC source to the output of the + // PLL. We first measure our HFROSC to get the + // right trim, then finally use it as the PLL source. + // We should really check here that the f_cpu + // requested is something in the limit of the PLL. For + // now that is up to the user. + + uint32_t desired_hfrosc_freq = f_cpu / 32; + + PRCI_use_hfrosc(hfroscdiv, hfrosctrim); + // Ignore the first run (for icache reasons) - uint32_t cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); + uint32_t cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); + cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); - int prev_trim = hfrosctrim; - int prev_freq = cpu_freq; + uint32_t prev_freq = cpu_freq; - if (cpu_freq > F_CPU) { - while((cpu_freq > F_CPU) && (hfrosctrim > 0) ){ - prev_freq = cpu_freq; - prev_trim = hfrosctrim; - hfrosctrim--; - PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, hfrosctrim ); - cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); - } - - // Check for over/undershoot - switch(target) { - case(PRCI_FREQ_CLOSEST): - if ((prev_freq - F_CPU) < (F_CPU - cpu_freq) && target == PRCI_FREQ_CLOSEST) { - PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, prev_trim); - cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); - } - break; - case(PRCI_FREQ_OVERSHOOT): - PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, prev_trim); - cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); - break; - //default: - // Do Nothing - } - - return cpu_freq; - } - - while(cpu_freq < F_CPU && hfrosctrim < 0x20) { - prev_freq = cpu_freq; + while ((cpu_freq < desired_hfrosc_freq) && hfrosctrim < 0x1F){ prev_trim = hfrosctrim; + prev_freq = cpu_freq; hfrosctrim ++; - PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, hfrosctrim ); + PRCI_use_hfrosc(hfroscdiv, hfrosctrim); cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); - } + } + // We couldn't go low enough + if (prev_freq > desired_hfrosc_freq){ + PRCI_use_pll(0, 0, 1, 31, 1, 1, hfroscdiv, prev_trim); + cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); + return cpu_freq; + } + // We couldn't go high enough + if (cpu_freq < desired_hfrosc_freq){ + PRCI_use_pll(0, 0, 1, 31, 1, 1, hfroscdiv, prev_trim); + cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); + return cpu_freq; + } + // Check for over/undershoot switch(target) { case(PRCI_FREQ_CLOSEST): - if ((F_CPU - prev_freq) < (cpu_freq - F_CPU)) { - PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, prev_trim); + if ((desired_hfrosc_freq - prev_freq) < (cpu_freq - desired_hfrosc_freq)) { + PRCI_use_pll(0, 0, 1, 31, 1, 1, hfroscdiv, prev_trim); cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); } break; case(PRCI_FREQ_UNDERSHOOT): - PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, prev_trim); + PRCI_use_pll(0, 0, 1, 31, 1, 1, hfroscdiv, prev_trim); cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); break; //default: // Do Nothing } - + return cpu_freq; } diff --git a/bsp/drivers/fe300prci/fe300prci_driver.h b/bsp/drivers/fe300prci/fe300prci_driver.h index 243e6de..7100f46 100644 --- a/bsp/drivers/fe300prci/fe300prci_driver.h +++ b/bsp/drivers/fe300prci/fe300prci_driver.h @@ -54,15 +54,23 @@ void PRCI_use_pll(int refsel, int bypass, void PRCI_use_default_clocks(); /* This routine will adjust the HFROSC trim - * as the PLL clock source, measure the resulting - * frequency, and attempt to get close to the requested - * frequency. It returns the actual measured frequency. + * while using HFROSC as the clock source, + * measure the resulting frequency, then + * use it as the PLL clock source, + * in an attempt to get over, under, or close to the + * requested frequency. It returns the actual measured + * frequency. * * Note that the requested frequency must be within the * range supported by the PLL so not all values are * achievable with this function, and not all - * are guaranteed to actually work. + * are guaranteed to actually work. The PLL + * is rated higher than the hardware. + * + * There is no check on the desired f_cpu frequency, it + * is up to the user to specify something reasonable. */ + uint32_t PRCI_set_hfrosctrim_for_f_cpu(uint32_t f_cpu, PRCI_freq_target target); __END_DECLS -- cgit v1.2.1-18-gbd029 From 557c3ed0dc025b73a919f3e2a4068808092f48c1 Mon Sep 17 00:00:00 2001 From: Megan Wachs Date: Thu, 2 Feb 2017 11:33:50 -0800 Subject: prci: Clean up the hfrosc-driven-pll FPU testing. --- bsp/drivers/fe300prci/fe300prci_driver.c | 26 ++++++++++++++------------ 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/bsp/drivers/fe300prci/fe300prci_driver.c b/bsp/drivers/fe300prci/fe300prci_driver.c index 5fb2c27..2d9c52f 100644 --- a/bsp/drivers/fe300prci/fe300prci_driver.c +++ b/bsp/drivers/fe300prci/fe300prci_driver.c @@ -182,9 +182,9 @@ void PRCI_use_hfxosc(uint32_t finaldiv) uint32_t PRCI_set_hfrosctrim_for_f_cpu(uint32_t f_cpu, PRCI_freq_target target ) { - int hfrosctrim = 0; - int hfroscdiv = 4; - int prev_trim = 0; + uint32_t hfrosctrim = 0; + uint32_t hfroscdiv = 4; + uint32_t prev_trim = 0; // In this function we use PLL settings which // will give us a 32x multiplier from the output @@ -195,22 +195,23 @@ uint32_t PRCI_set_hfrosctrim_for_f_cpu(uint32_t f_cpu, PRCI_freq_target target ) // requested is something in the limit of the PLL. For // now that is up to the user. - uint32_t desired_hfrosc_freq = f_cpu / 32; + // This will undershoot for frequencies not divisible by 16. + uint32_t desired_hfrosc_freq = (f_cpu/ 16); PRCI_use_hfrosc(hfroscdiv, hfrosctrim); // Ignore the first run (for icache reasons) - uint32_t cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); + uint32_t cpu_freq = PRCI_measure_mcycle_freq(3000, RTC_FREQ); - cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); + cpu_freq = PRCI_measure_mcycle_freq(3000, RTC_FREQ); uint32_t prev_freq = cpu_freq; - while ((cpu_freq < desired_hfrosc_freq) && hfrosctrim < 0x1F){ + while ((cpu_freq < desired_hfrosc_freq) && (hfrosctrim < 0x1F)){ prev_trim = hfrosctrim; prev_freq = cpu_freq; hfrosctrim ++; PRCI_use_hfrosc(hfroscdiv, hfrosctrim); - cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); + cpu_freq = PRCI_measure_mcycle_freq(3000, RTC_FREQ); } // We couldn't go low enough @@ -232,17 +233,18 @@ uint32_t PRCI_set_hfrosctrim_for_f_cpu(uint32_t f_cpu, PRCI_freq_target target ) case(PRCI_FREQ_CLOSEST): if ((desired_hfrosc_freq - prev_freq) < (cpu_freq - desired_hfrosc_freq)) { PRCI_use_pll(0, 0, 1, 31, 1, 1, hfroscdiv, prev_trim); - cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); + } else { + PRCI_use_pll(0, 0, 1, 31, 1, 1, hfroscdiv, hfrosctrim); } break; case(PRCI_FREQ_UNDERSHOOT): PRCI_use_pll(0, 0, 1, 31, 1, 1, hfroscdiv, prev_trim); - cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); break; - //default: - // Do Nothing + default: + PRCI_use_pll(0, 0, 1, 31, 1, 1, hfroscdiv, hfrosctrim); } + cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); return cpu_freq; } -- cgit v1.2.1-18-gbd029