diff --git a/.github/workflows/pull-request.yml b/.github/workflows/pull-request.yml index 8b1470c2..534e2e8d 100644 --- a/.github/workflows/pull-request.yml +++ b/.github/workflows/pull-request.yml @@ -264,8 +264,8 @@ jobs: run: | python -m pip install --upgrade pip pip install --upgrade platformio + rm -rf ~/.platformio/platforms/raspberrypi* pio pkg install --global --platform https://github.com/maxgerhardt/platform-raspberrypi.git - pio pkg update --global --platform https://github.com/maxgerhardt/platform-raspberrypi.git pio pkg install --global --tool symlink://. cp -f /home/runner/work/arduino-pico/arduino-pico/tools/json/*.json /home/runner/.platformio/platforms/raspberrypi/boards/. - name: Build Multicore Example @@ -321,8 +321,8 @@ jobs: run: | python -m pip install --upgrade pip pip install --upgrade platformio + rm -rf ~/.platformio/platforms/raspberrypi* pio pkg install --global --platform https://github.com/maxgerhardt/platform-raspberrypi.git - pio pkg update --global --platform https://github.com/maxgerhardt/platform-raspberrypi.git pio pkg install --global --tool symlink://. cp -f /home/runner/work/arduino-pico/arduino-pico/tools/json/*.json /home/runner/.platformio/platforms/raspberrypi/boards/. - name: Build Every Variant diff --git a/libraries/LowPower/LowPower.cpp b/libraries/LowPower/LowPower.cpp new file mode 100644 index 00000000..57206418 --- /dev/null +++ b/libraries/LowPower/LowPower.cpp @@ -0,0 +1,81 @@ +/* + * Copyright (c) 2024 Maximilian Gerhardt + * + * SPDX-License-Identifier: BSD-3-Clause + */ +#include "LowPower.h" +#include +#include + +LowPowerClass LowPower; + +void LowPowerClass::setOscillatorType(dormant_source_t oscillator) { + this->oscillator = oscillator; +} + +static uint32_t pinStatusToGPIOEvent(PinStatus status) { + switch (status) { + case HIGH: return IO_BANK0_DORMANT_WAKE_INTE0_GPIO0_LEVEL_HIGH_BITS; + case LOW: return IO_BANK0_DORMANT_WAKE_INTE0_GPIO0_LEVEL_LOW_BITS; + case FALLING: return IO_BANK0_DORMANT_WAKE_INTE0_GPIO0_EDGE_LOW_BITS; + case RISING: return IO_BANK0_DORMANT_WAKE_INTE0_GPIO0_EDGE_HIGH_BITS; + // Change activates on both a rising or a falling edge + case CHANGE: + return IO_BANK0_DORMANT_WAKE_INTE0_GPIO0_EDGE_HIGH_BITS | + IO_BANK0_DORMANT_WAKE_INTE0_GPIO0_EDGE_LOW_BITS; + default: return 0; // unreachable + } +} + +void LowPowerClass::dormantUntilGPIO(pin_size_t wakeup_gpio, PinStatus wakeup_type){ + sleep_run_from_dormant_source(this->oscillator); + uint32_t event = pinStatusToGPIOEvent(wakeup_type); + + // turn of brown out detector (BOD), just pisses away power + vreg_and_chip_reset_hw->bod &= ~VREG_AND_CHIP_RESET_BOD_EN_BITS; + // set digital voltage to only 0.8V instead of 1.10V. burns less static power + vreg_and_chip_reset_hw->vreg &= ~VREG_AND_CHIP_RESET_VREG_VSEL_BITS; + // power down additional stuff + syscfg_hw->mempowerdown |= + SYSCFG_MEMPOWERDOWN_USB_BITS // USB Memory + ; + + sleep_goto_dormant_until_pin((uint) wakeup_gpio, event); + + syscfg_hw->mempowerdown &= + ~(SYSCFG_MEMPOWERDOWN_USB_BITS) // USB Memory + ; + // We only reach the next line after waking up + vreg_and_chip_reset_hw->bod |= VREG_AND_CHIP_RESET_BOD_EN_BITS; // turn BOD back on + // default 1.10V again + vreg_and_chip_reset_hw->vreg |= VREG_AND_CHIP_RESET_VREG_VSEL_RESET << VREG_AND_CHIP_RESET_VREG_VSEL_LSB; + + sleep_power_up(); + + // startup crystal oscillator (?) +#if (defined(PICO_RP2040) && (F_CPU != 125000000)) || (defined(PICO_RP2350) && (F_CPU != 150000000)) + set_sys_clock_khz(F_CPU / 1000, true); +#endif +} + +static void sleep_callback(void) { } + +// For RP2040 this example needs an external clock fed into the GP20 +// Note: Only GP20 and GP22 can be used for clock input, See the GPIO function table in the datasheet. +// You can use another Pico to generate this. See the clocks/hello_gpout example for more details. +// rp2040: clock_gpio_init(21, CLOCKS_CLK_GPOUT3_CTRL_AUXSRC_VALUE_CLK_RTC, 1); // 46875Hz can only export a clock on gpios 21,23,24,25 and only 21 is exposed by Pico +// RP2350 has an LPOSC it can use, so doesn't need this +// also need an initial value like +// struct timespec ts = { .tv_sec = 1723124088, .tv_nsec = 0 }; +// aon_timer_start(&ts); +void LowPowerClass::dormantFor(uint32_t milliseconds) { + sleep_run_from_dormant_source(this->oscillator); + struct timespec ts = {}; + aon_timer_get_time(&ts); + if (milliseconds >= 1000) { + ts.tv_sec += milliseconds / 1000; + milliseconds = milliseconds % 1000; + } + ts.tv_nsec += milliseconds * 1000ul * 1000ul; + sleep_goto_dormant_until(&ts, &sleep_callback); +} diff --git a/libraries/LowPower/LowPower.h b/libraries/LowPower/LowPower.h new file mode 100644 index 00000000..e726a05b --- /dev/null +++ b/libraries/LowPower/LowPower.h @@ -0,0 +1,36 @@ +/* + * Copyright (c) 2024 Maximilian Gerhardt + * + * SPDX-License-Identifier: BSD-3-Clause + */ +#pragma once +#include +#include "utility/sleep.h" + +class LowPowerClass { +public: + /** + * Set the oscillator type that is shutdown during sleep and re-enabled after. + * The default is "crystal oscillator" (DORMANT_SOURCE_XOSC). + */ + void setOscillatorType(dormant_source_t oscillator); + /** + * Put the chip in "DORMANT" mode and wake up from a GPIO pin. + * The "wakeup" type can e.g. be "FALLING", so that a falling edge on that GPIO + * wakes the chip up again. + * This cannot be "CHANGE" + */ + void dormantUntilGPIO(pin_size_t wakeup_gpio, PinStatus wakeup_type); + /** + * Put the chip in "DORMANT" for a specified amount of time. + * Note that this does not work on RP2040 chips, unless you connect a 32.768kHz + * oscillator to specific pins. (TODO exact documentation) + */ + void dormantFor(uint32_t milliseconds); + +private: + // sane default value, most boards run on the crystal oscillator + dormant_source_t oscillator = DORMANT_SOURCE_XOSC; +}; + +extern LowPowerClass LowPower; \ No newline at end of file diff --git a/libraries/LowPower/examples/DormantGPIOWakeup/dormant_gpio_wakeup.ino b/libraries/LowPower/examples/DormantGPIOWakeup/dormant_gpio_wakeup.ino new file mode 100644 index 00000000..73efab86 --- /dev/null +++ b/libraries/LowPower/examples/DormantGPIOWakeup/dormant_gpio_wakeup.ino @@ -0,0 +1,55 @@ +#include + +#define GPIO_EXIT_DORMANT_MODE 3 /* connect GP3 to GND once in DORMANT mode */ + +void setup() { + Serial1.setTX(12); + Serial1.setRX(13); + Serial1.begin(115200); + pinMode(LED_BUILTIN, OUTPUT); + digitalWrite(LED_BUILTIN, HIGH); + pinMode(GPIO_EXIT_DORMANT_MODE, INPUT_PULLUP); // pull pin that will get us out of sleep mode +} + +void loop() { + if (Serial1.available() > 0) { + (void) Serial1.read(); + digitalWrite(LED_BUILTIN, LOW); + Serial1.end(); // disable the UART + pinMode(12, INPUT_PULLUP); // Serial TX + gpio_set_input_enabled(12, false); + pinMode(13, INPUT_PULLUP); // Serial RX + gpio_set_input_enabled(13, false); + pinMode(24, INPUT); // can measure VBUS on the pico + gpio_set_input_enabled(24, false); + pinMode(23, INPUT_PULLDOWN); // connected to PS of switching regulator; default pulldown + gpio_set_input_enabled(23, false); + pinMode(29, INPUT); // connected to Q1 / GPIO29_ADC3, which is a 3:1 voltage divider for VSYS + gpio_set_input_enabled(29, false); + // free standing / floating GPIOs + for(auto p : {0, 1, 2, /*3,*/ 4, 5, 6, 7, 8, 9, 10, 11, /*12, 13,*/ 14, 15, 16, 17, 18, 19, 20, 21, 22, /*23, 24,*/ 25, 26, 27, 28, 29}) { + pinMode(p, INPUT); // best performance! + //pinMode(p, INPUT_PULLDOWN); + //pinMode(p, INPUT_PULLUP); + //pinMode(p, OUTPUT); digitalWrite(p, HIGH); // drive HIGH + //pinMode(p, OUTPUT); digitalWrite(p, LOW); // drive LOW + gpio_set_input_enabled(p, false); // disable input gate + gpio_set_input_hysteresis_enabled(p, false); // additionally disable schmitt input gate + // ^-- makes no difference + } + gpio_set_input_enabled(30, false); // SWCLK + gpio_set_input_enabled(31, false); // SWD(IO) + + // we will get out of sleep when an interrupt occurs. + // this will shutdown the crystal oscillator until an interrupt occurs. + LowPower.dormantUntilGPIO(GPIO_EXIT_DORMANT_MODE, FALLING); + // this will only be reached after wakeup. + // we don't actually know the time duration during which we were dormant. + // so, the absolute value of millis() will be messed up. + digitalWrite(LED_BUILTIN, HIGH); + Serial1.begin(115200); // start UART again + } + Serial1.println("hello, world"); + digitalWrite(LED_BUILTIN, digitalRead(LED_BUILTIN) ^ 1); + delay(500); +} \ No newline at end of file diff --git a/libraries/LowPower/library.properties b/libraries/LowPower/library.properties new file mode 100644 index 00000000..1e99db9b --- /dev/null +++ b/libraries/LowPower/library.properties @@ -0,0 +1,10 @@ +name=LowPower +version=1.0 +author=Maximilian Gerhardt +maintainer=Maximilian Gerhardt +sentence=Low Power support for RP2040 and RP2350 +paragraph= +category=Data Storage +url=http://github.com/earlephilhower/arduino-pico +architectures=rp2040 +dot_a_linkage=true diff --git a/libraries/LowPower/utility/rosc.c b/libraries/LowPower/utility/rosc.c new file mode 100644 index 00000000..164102bf --- /dev/null +++ b/libraries/LowPower/utility/rosc.c @@ -0,0 +1,69 @@ +/* + * Copyright (c) 2020 Raspberry Pi (Trading) Ltd. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include "pico.h" + +// For MHZ definitions etc +#include "hardware/clocks.h" +#include "rosc.h" + +// Given a ROSC delay stage code, return the next-numerically-higher code. +// Top result bit is set when called on maximum ROSC code. +uint32_t next_rosc_code(uint32_t code) { + return ((code | 0x08888888u) + 1u) & 0xf7777777u; +} + +uint rosc_find_freq(uint32_t low_mhz, uint32_t high_mhz) { + // TODO: This could be a lot better + rosc_set_div(1); + for (uint32_t code = 0; code <= 0x77777777u; code = next_rosc_code(code)) { + rosc_set_freq(code); + uint rosc_mhz = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_ROSC_CLKSRC) / 1000; + if ((rosc_mhz >= low_mhz) && (rosc_mhz <= high_mhz)) { + return rosc_mhz; + } + } + return 0; +} + +void rosc_set_div(uint32_t div) { + assert(div <= 31 && div >= 1); + rosc_write(&rosc_hw->div, ROSC_DIV_VALUE_PASS + div); +} + +void rosc_set_freq(uint32_t code) { + rosc_write(&rosc_hw->freqa, (ROSC_FREQA_PASSWD_VALUE_PASS << ROSC_FREQA_PASSWD_LSB) | (code & 0xffffu)); + rosc_write(&rosc_hw->freqb, (ROSC_FREQA_PASSWD_VALUE_PASS << ROSC_FREQA_PASSWD_LSB) | (code >> 16u)); +} + +void rosc_set_range(uint range) { + // Range should use enumvals from the headers and thus have the password correct + rosc_write(&rosc_hw->ctrl, (ROSC_CTRL_ENABLE_VALUE_ENABLE << ROSC_CTRL_ENABLE_LSB) | range); +} + +void rosc_disable(void) { + uint32_t tmp = rosc_hw->ctrl; + tmp &= (~ROSC_CTRL_ENABLE_BITS); + tmp |= (ROSC_CTRL_ENABLE_VALUE_DISABLE << ROSC_CTRL_ENABLE_LSB); + rosc_write(&rosc_hw->ctrl, tmp); + // Wait for stable to go away + while(rosc_hw->status & ROSC_STATUS_STABLE_BITS); +} + +void rosc_set_dormant(void) { + // WARNING: This stops the rosc until woken up by an irq + rosc_write(&rosc_hw->dormant, ROSC_DORMANT_VALUE_DORMANT); + // Wait for it to become stable once woken up + while(!(rosc_hw->status & ROSC_STATUS_STABLE_BITS)); +} + +void rosc_enable(void) { + //Re-enable the rosc + rosc_write(&rosc_hw->ctrl, ROSC_CTRL_ENABLE_BITS); + + //Wait for it to become stable once restarted + while (!(rosc_hw->status & ROSC_STATUS_STABLE_BITS)); +} \ No newline at end of file diff --git a/libraries/LowPower/utility/rosc.h b/libraries/LowPower/utility/rosc.h new file mode 100644 index 00000000..ac43a6b8 --- /dev/null +++ b/libraries/LowPower/utility/rosc.h @@ -0,0 +1,92 @@ +/* + * Copyright (c) 2020 Raspberry Pi (Trading) Ltd. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#ifndef _HARDWARE_ROSC_H_ +#define _HARDWARE_ROSC_H_ + +#include "pico.h" +#include "hardware/structs/rosc.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** \file rosc.h + * \defgroup hardware_rosc hardware_rosc + * + * Ring Oscillator (ROSC) API + * + * A Ring Oscillator is an on-chip oscillator that requires no external crystal. Instead, the output is generated from a series of + * inverters that are chained together to create a feedback loop. RP2040 boots from the ring oscillator initially, meaning the + * first stages of the bootrom, including booting from SPI flash, will be clocked by the ring oscillator. If your design has a + * crystal oscillator, you’ll likely want to switch to this as your reference clock as soon as possible, because the frequency is + * more accurate than the ring oscillator. + */ + +/*! \brief Set frequency of the Ring Oscillator + * \ingroup hardware_rosc + * + * \param code The drive strengths. See the RP2040 datasheet for information on this value. + */ +void rosc_set_freq(uint32_t code); + +/*! \brief Set range of the Ring Oscillator + * \ingroup hardware_rosc + * + * Frequency range. Frequencies will vary with Process, Voltage & Temperature (PVT). + * Clock output will not glitch when changing the range up one step at a time. + * + * \param range 0x01 Low, 0x02 Medium, 0x03 High, 0x04 Too High. + */ +void rosc_set_range(uint range); + +/*! \brief Disable the Ring Oscillator + * \ingroup hardware_rosc + * + */ +void rosc_disable(void); + +/*! \brief Put Ring Oscillator in to dormant mode. + * \ingroup hardware_rosc + * + * The ROSC supports a dormant mode,which stops oscillation until woken up up by an asynchronous interrupt. + * This can either come from the RTC, being clocked by an external clock, or a GPIO pin going high or low. + * If no IRQ is configured before going into dormant mode the ROSC will never restart. + * + * PLLs should be stopped before selecting dormant mode. + */ +void rosc_set_dormant(void); + +// FIXME: Add doxygen + +uint32_t next_rosc_code(uint32_t code); + +uint rosc_find_freq(uint32_t low_mhz, uint32_t high_mhz); + +void rosc_set_div(uint32_t div); + +inline static void rosc_clear_bad_write(void) { + hw_clear_bits(&rosc_hw->status, ROSC_STATUS_BADWRITE_BITS); +} + +inline static bool rosc_write_okay(void) { + return !(rosc_hw->status & ROSC_STATUS_BADWRITE_BITS); +} + +inline static void rosc_write(io_rw_32 *addr, uint32_t value) { + rosc_clear_bad_write(); + assert(rosc_write_okay()); + *addr = value; + assert(rosc_write_okay()); +}; + +void rosc_enable(void); + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/libraries/LowPower/utility/sleep.c b/libraries/LowPower/utility/sleep.c new file mode 100644 index 00000000..61897749 --- /dev/null +++ b/libraries/LowPower/utility/sleep.c @@ -0,0 +1,300 @@ +/* + * Copyright (c) 2020 Raspberry Pi (Trading) Ltd. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include + +#include "pico.h" + +#include "pico/stdlib.h" +#include "sleep.h" + +#include "hardware/pll.h" +#include "hardware/regs/clocks.h" +#include "hardware/clocks.h" +#include "hardware/watchdog.h" +#include "hardware/xosc.h" +#include "rosc.h" +#include "hardware/regs/io_bank0.h" +// For __wfi +#include "hardware/sync.h" +#include "pico/runtime_init.h" + +#ifdef __riscv +#include "hardware/riscv.h" +#else +// For scb_hw so we can enable deep sleep +#include "hardware/structs/scb.h" +#endif + +#if !PICO_RP2040 +#include "hardware/powman.h" +#endif + +// The difference between sleep and dormant is that ALL clocks are stopped in dormant mode, +// until the source (either xosc or rosc) is started again by an external event. +// In sleep mode some clocks can be left running controlled by the SLEEP_EN registers in the clocks +// block. For example you could keep clk_rtc running. Some destinations (proc0 and proc1 wakeup logic) +// can't be stopped in sleep mode otherwise there wouldn't be enough logic to wake up again. + +static dormant_source_t _dormant_source; + +bool dormant_source_valid(dormant_source_t dormant_source) +{ + switch (dormant_source) { + case DORMANT_SOURCE_XOSC: + return true; + case DORMANT_SOURCE_ROSC: + return true; +#if !PICO_RP2040 + case DORMANT_SOURCE_LPOSC: + return true; +#endif + default: + return false; + } +} + +// In order to go into dormant mode we need to be running from a stoppable clock source: +// either the xosc or rosc with no PLLs running. This means we disable the USB and ADC clocks +// and all PLLs +void sleep_run_from_dormant_source(dormant_source_t dormant_source) { + assert(dormant_source_valid(dormant_source)); + _dormant_source = dormant_source; + + uint src_hz; + uint clk_ref_src; + switch (dormant_source) { + case DORMANT_SOURCE_XOSC: + src_hz = XOSC_HZ; + clk_ref_src = CLOCKS_CLK_REF_CTRL_SRC_VALUE_XOSC_CLKSRC; + break; + case DORMANT_SOURCE_ROSC: + src_hz = 6500 * KHZ; // todo + clk_ref_src = CLOCKS_CLK_REF_CTRL_SRC_VALUE_ROSC_CLKSRC_PH; + break; +#if !PICO_RP2040 + case DORMANT_SOURCE_LPOSC: + src_hz = 32 * KHZ; + clk_ref_src = CLOCKS_CLK_REF_CTRL_SRC_VALUE_LPOSC_CLKSRC; + break; +#endif + default: + hard_assert(false); + } + + // CLK_REF = XOSC or ROSC + clock_configure(clk_ref, + clk_ref_src, + 0, // No aux mux + src_hz, + src_hz); + + // CLK SYS = CLK_REF + clock_configure(clk_sys, + CLOCKS_CLK_SYS_CTRL_SRC_VALUE_CLK_REF, + 0, // Using glitchless mux + src_hz, + src_hz); + + // CLK ADC = 0MHz + clock_stop(clk_adc); + clock_stop(clk_usb); +#if PICO_RP2350 + clock_stop(clk_hstx); +#endif + +#if PICO_RP2040 + // CLK RTC = ideally XOSC (12MHz) / 256 = 46875Hz but could be rosc + uint clk_rtc_src = (dormant_source == DORMANT_SOURCE_XOSC) ? + CLOCKS_CLK_RTC_CTRL_AUXSRC_VALUE_XOSC_CLKSRC : + CLOCKS_CLK_RTC_CTRL_AUXSRC_VALUE_ROSC_CLKSRC_PH; + + clock_configure(clk_rtc, + 0, // No GLMUX + clk_rtc_src, + src_hz, + 46875); +#endif + + // CLK PERI = clk_sys. Used as reference clock for Peripherals. No dividers so just select and enable + clock_configure(clk_peri, + 0, + CLOCKS_CLK_PERI_CTRL_AUXSRC_VALUE_CLK_SYS, + src_hz, + src_hz); + + pll_deinit(pll_sys); + pll_deinit(pll_usb); + + // Assuming both xosc and rosc are running at the moment + if (dormant_source == DORMANT_SOURCE_XOSC) { + // Can disable rosc + rosc_disable(); + } else { + // Can disable xosc + xosc_disable(); + } + + // Reconfigure uart with new clocks + //setup_default_uart(); +} + +static void processor_deep_sleep(void) { + // Enable deep sleep at the proc +#ifdef __riscv + uint32_t bits = RVCSR_MSLEEP_POWERDOWN_BITS; + if (!get_core_num()) { + bits |= RVCSR_MSLEEP_DEEPSLEEP_BITS; + } + riscv_set_csr(RVCSR_MSLEEP_OFFSET, bits); +#else + scb_hw->scr |= ARM_CPU_PREFIXED(SCR_SLEEPDEEP_BITS); +#endif +} + +void sleep_goto_sleep_until(struct timespec *ts, aon_timer_alarm_handler_t callback) +{ + + // We should have already called the sleep_run_from_dormant_source function + // This is only needed for dormancy although it saves power running from xosc while sleeping + //assert(dormant_source_valid(_dormant_source)); + +#if PICO_RP2040 + clocks_hw->sleep_en0 = CLOCKS_SLEEP_EN0_CLK_RTC_RTC_BITS; + clocks_hw->sleep_en1 = 0x0; +#else + clocks_hw->sleep_en0 = CLOCKS_SLEEP_EN0_CLK_REF_POWMAN_BITS; + clocks_hw->sleep_en1 = 0x0; +#endif + + aon_timer_enable_alarm(ts, callback, false); + + stdio_flush(); + + // Enable deep sleep at the proc + processor_deep_sleep(); + + // Go to sleep + __wfi(); +} + +bool sleep_goto_sleep_for(uint32_t delay_ms, hardware_alarm_callback_t callback) +{ + // We should have already called the sleep_run_from_dormant_source function + // This is only needed for dormancy although it saves power running from xosc while sleeping + //assert(dormant_source_valid(_dormant_source)); + + // Turn off all clocks except for the timer + clocks_hw->sleep_en0 = 0x0; +#if PICO_RP2040 + clocks_hw->sleep_en1 = CLOCKS_SLEEP_EN1_CLK_SYS_TIMER_BITS; +#elif PICO_RP2350 + clocks_hw->sleep_en1 = CLOCKS_SLEEP_EN1_CLK_REF_TICKS_BITS | CLOCKS_SLEEP_EN1_CLK_SYS_TIMER0_BITS; +#else +#error Unknown processor +#endif + + int alarm_num = hardware_alarm_claim_unused(true); + hardware_alarm_set_callback(alarm_num, callback); + absolute_time_t t = make_timeout_time_ms(delay_ms); + if (hardware_alarm_set_target(alarm_num, t)) { + hardware_alarm_set_callback(alarm_num, NULL); + hardware_alarm_unclaim(alarm_num); + return false; + } + + stdio_flush(); + + // Enable deep sleep at the proc + processor_deep_sleep(); + + // Go to sleep + __wfi(); + return true; +} + +static void _go_dormant(void) { + assert(dormant_source_valid(_dormant_source)); + + if (_dormant_source == DORMANT_SOURCE_XOSC) { + xosc_dormant(); + } else { + rosc_set_dormant(); + } +} + +void sleep_goto_dormant_until(struct timespec *ts, aon_timer_alarm_handler_t callback) { + // We should have already called the sleep_run_from_dormant_source function + +#if PICO_RP2040 + clocks_hw->sleep_en0 = CLOCKS_SLEEP_EN0_CLK_RTC_RTC_BITS; + clocks_hw->sleep_en1 = 0x0; +#else + assert(_dormant_source == DORMANT_SOURCE_LPOSC); + uint64_t restore_ms = powman_timer_get_ms(); + powman_timer_set_1khz_tick_source_lposc(); + powman_timer_set_ms(restore_ms); + + clocks_hw->sleep_en0 = CLOCKS_SLEEP_EN0_CLK_REF_POWMAN_BITS; + clocks_hw->sleep_en1 = 0x0; +#endif + + // Set the AON timer to wake up the proc from dormant mode + aon_timer_enable_alarm(ts, callback, true); + + stdio_flush(); + + // Enable deep sleep at the proc + processor_deep_sleep(); + + // Go dormant + _go_dormant(); +} + +void sleep_goto_dormant_until_pin(uint gpio_pin, uint32_t event) { + // Configure the appropriate IRQ at IO bank 0 + assert(gpio_pin < NUM_BANK0_GPIOS); + + // Assume GPIO pin as initialized, don't reinit. + // so, a user can do pinMode(x, INPUT_PULLUP), etc. + //gpio_init(gpio_pin); + gpio_set_input_enabled(gpio_pin, true); + //gpio_set_pulls(gpio_pin, true, false); + gpio_set_dormant_irq_enabled(gpio_pin, event, true); + + _go_dormant(); + // Execution stops here until woken up + + // Clear the irq so we can go back to dormant mode again if we want + gpio_acknowledge_irq(gpio_pin, event); + gpio_set_dormant_irq_enabled(gpio_pin, event, false); + // gpio_set_input_enabled(gpio_pin, false); +} + +// To be called after waking up from sleep/dormant mode to restore system clocks properly +void sleep_power_up(void) +{ + // Re-enable the ring oscillator, which will essentially kickstart the proc + rosc_enable(); + + // Reset the sleep enable register so peripherals and other hardware can be used + clocks_hw->sleep_en0 |= ~(0u); + clocks_hw->sleep_en1 |= ~(0u); + + // Restore all clocks + clocks_init(); + +#if PICO_RP2350 + // make powerman use xosc again + uint64_t restore_ms = powman_timer_get_ms(); + powman_timer_set_1khz_tick_source_xosc(); + powman_timer_set_ms(restore_ms); +#endif + + // UART needs to be reinitialised with the new clock frequencies for stable output + //setup_default_uart(); +} \ No newline at end of file diff --git a/libraries/LowPower/utility/sleep.h b/libraries/LowPower/utility/sleep.h new file mode 100644 index 00000000..867e378c --- /dev/null +++ b/libraries/LowPower/utility/sleep.h @@ -0,0 +1,147 @@ +/* + * Copyright (c) 2020 Raspberry Pi (Trading) Ltd. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#ifndef _PICO_SLEEP_H_ +#define _PICO_SLEEP_H_ + +#include "pico.h" +#include "rosc.h" + +#include "pico/aon_timer.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** \file sleep.h + * \defgroup hardware_sleep hardware_sleep + * + * Lower Power Sleep API + * + * The difference between sleep and dormant is that ALL clocks are stopped in dormant mode, + * until the source (either xosc or rosc) is started again by an external event. + * In sleep mode some clocks can be left running controlled by the SLEEP_EN registers in the clocks + * block. For example you could keep clk_rtc running. Some destinations (proc0 and proc1 wakeup logic) + * can't be stopped in sleep mode otherwise there wouldn't be enough logic to wake up again. + * + * \subsection sleep_example Example + * \addtogroup hardware_sleep + * \include hello_sleep.c + + */ +typedef enum { + DORMANT_SOURCE_NONE, + DORMANT_SOURCE_XOSC, + DORMANT_SOURCE_ROSC, + DORMANT_SOURCE_LPOSC, // rp2350 only +} dormant_source_t; + +/*! \brief Set all clock sources to the the dormant clock source to prepare for sleep. + * \ingroup hardware_sleep + * + * \param dormant_source The dormant clock source to use + */ +void sleep_run_from_dormant_source(dormant_source_t dormant_source); + +/*! \brief Set the dormant clock source to be the crystal oscillator + * \ingroup hardware_sleep + */ +static inline void sleep_run_from_xosc(void) { + sleep_run_from_dormant_source(DORMANT_SOURCE_XOSC); +} + +#if !PICO_RP2040 +static inline void sleep_run_from_lposc(void) { + sleep_run_from_dormant_source(DORMANT_SOURCE_LPOSC); +} +#endif + +/*! \brief Set the dormant clock source to be the ring oscillator + * \ingroup hardware_sleep + */ +static inline void sleep_run_from_rosc(void) { + sleep_run_from_dormant_source(DORMANT_SOURCE_ROSC); +} + +/*! \brief Send system to sleep until the specified time + * \ingroup hardware_sleep + * + * One of the sleep_run_* functions must be called prior to this call + * + * \param ts The time to wake up + * \param callback Function to call on wakeup. + */ +void sleep_goto_sleep_until(struct timespec *ts, aon_timer_alarm_handler_t callback); + +/*! \brief Send system to sleep for a specified duration in milliseconds. This provides an alternative to sleep_goto_sleep_until +to allow for shorter duration sleeps. + * \ingroup hardware_sleep + * + * One of the sleep_run_* functions must be called prior to this call + * + * \param delay_ms The duration to sleep for in milliseconds. + * \param callback Function to call on wakeup. + * \return Returns true if the device went to sleep + */ +bool sleep_goto_sleep_for(uint32_t delay_ms, hardware_alarm_callback_t callback); + +/*! \brief Send system to dormant until the specified time, note for RP2040 the RTC must be driven by an external clock + * \ingroup hardware_sleep + * + * One of the sleep_run_* functions must be called prior to this call + * + * \param ts The time to wake up + * \param callback Function to call on wakeup. + */ +void sleep_goto_dormant_until(struct timespec *ts, aon_timer_alarm_handler_t callback); + +/*! \brief Send system to sleep until the specified GPIO changes + * \ingroup hardware_sleep + * + * One of the sleep_run_* functions must be called prior to this call + * + * \param gpio_pin The pin to provide the wake up + * \param edge true for leading edge, false for trailing edge + * \param high true for active high, false for active low + */ + +void sleep_goto_dormant_until_pin(uint gpio_pin, uint32_t event); + +/*! \brief Send system to sleep until a leading high edge is detected on GPIO + * \ingroup hardware_sleep + * + * One of the sleep_run_* functions must be called prior to this call + * + * \param gpio_pin The pin to provide the wake up + */ +static inline void sleep_goto_dormant_until_edge_high(uint gpio_pin) { + sleep_goto_dormant_until_pin(gpio_pin, IO_BANK0_DORMANT_WAKE_INTE0_GPIO0_EDGE_HIGH_BITS); +} + +/*! \brief Send system to sleep until a high level is detected on GPIO + * \ingroup hardware_sleep + * + * One of the sleep_run_* functions must be called prior to this call + * + * \param gpio_pin The pin to provide the wake up + */ +static inline void sleep_goto_dormant_until_level_high(uint gpio_pin) { + sleep_goto_dormant_until_pin(gpio_pin, IO_BANK0_DORMANT_WAKE_INTE0_GPIO0_LEVEL_HIGH_BITS); +} + +/*! \brief Reconfigure clocks to wake up properly from sleep/dormant mode + * \ingroup hardware_sleep + * + * This must be called immediately after continuing execution when waking up from sleep/dormant mode + * + */ +void sleep_power_up(void); + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file