rp2/clocks_extra: Implement custom clocks_init function.

Adapts pico-sdk clocks_init() into clocks_init_optional_usb() which takes
an argument to initialise USB clocks or not.

To avoid a code size increase the SDK clocks_init() function is linker
wrapped to become clocks_init_optional_usb(true).

This work was funded through GitHub Sponsors.

Signed-off-by: Angus Gratton <angus@redyak.com.au>

4 files changed, 138 insertions, 0 deletions

diff --git a/LICENSE b/LICENSE
index 469ae1927..341698fa4 100644
--- a/LICENSE
+++ b/LICENSE
@@ -73,6 +73,7 @@ used during the build process and is not part of the compiled source code.
             /ppp_set_auth.* (Apache-2.0)
         /rp2
             /mutex_extra.c (BSD-3-clause)
+            /clocks_extra.c (BSD-3-clause)
         /stm32
             /usbd*.c (MCD-ST Liberty SW License Agreement V2)
             /stm32_it.* (MIT + BSD-3-clause)
diff --git a/ports/rp2/CMakeLists.txt b/ports/rp2/CMakeLists.txt
index d3ecee586..8f5680092 100644
--- a/ports/rp2/CMakeLists.txt
+++ b/ports/rp2/CMakeLists.txt
@@ -117,6 +117,7 @@ set(MICROPY_SOURCE_DRIVERS
 )
 
 set(MICROPY_SOURCE_PORT
+    clocks_extra.c
     fatfs_port.c
     help.c
     machine_bitstream.c
@@ -453,6 +454,7 @@ target_compile_options(${MICROPY_TARGET} PRIVATE
 target_link_options(${MICROPY_TARGET} PRIVATE
     -Wl,--defsym=__micropy_c_heap_size__=${MICROPY_C_HEAP_SIZE}
     -Wl,--wrap=dcd_event_handler
+    -Wl,--wrap=clocks_init
 )
 
 # Apply optimisations to performance-critical source code.
diff --git a/ports/rp2/clocks_extra.c b/ports/rp2/clocks_extra.c
new file mode 100644
index 000000000..e2c97962c
--- /dev/null
+++ b/ports/rp2/clocks_extra.c
@@ -0,0 +1,102 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include "pico.h"
+#include "clocks_extra.h"
+#include "hardware/regs/clocks.h"
+#include "hardware/platform_defs.h"
+#include "hardware/clocks.h"
+#include "hardware/watchdog.h"
+#include "hardware/pll.h"
+#include "hardware/xosc.h"
+#include "hardware/irq.h"
+#include "hardware/gpio.h"
+
+#define RTC_CLOCK_FREQ_HZ       (USB_CLK_KHZ * KHZ / 1024)
+
+// Wrap the SDK's clocks_init() function to save code size
+void __wrap_clocks_init(void) {
+    clocks_init_optional_usb(true);
+}
+
+// Copy of clocks_init() from pico-sdk, with USB
+// PLL and clock init made optional (for light sleep wakeup).
+void clocks_init_optional_usb(bool init_usb) {
+    // Start tick in watchdog, the argument is in 'cycles per microsecond' i.e. MHz
+    watchdog_start_tick(XOSC_KHZ / KHZ);
+
+    // Modification: removed FPGA check here
+
+    // Disable resus that may be enabled from previous software
+    clocks_hw->resus.ctrl = 0;
+
+    // Enable the xosc
+    xosc_init();
+
+    // Before we touch PLLs, switch sys and ref cleanly away from their aux sources.
+    hw_clear_bits(&clocks_hw->clk[clk_sys].ctrl, CLOCKS_CLK_SYS_CTRL_SRC_BITS);
+    while (clocks_hw->clk[clk_sys].selected != 0x1) {
+        tight_loop_contents();
+    }
+    hw_clear_bits(&clocks_hw->clk[clk_ref].ctrl, CLOCKS_CLK_REF_CTRL_SRC_BITS);
+    while (clocks_hw->clk[clk_ref].selected != 0x1) {
+        tight_loop_contents();
+    }
+
+    /// \tag::pll_init[]
+    pll_init(pll_sys, PLL_COMMON_REFDIV, PLL_SYS_VCO_FREQ_KHZ * KHZ, PLL_SYS_POSTDIV1, PLL_SYS_POSTDIV2);
+    if (init_usb) {
+        pll_init(pll_usb, PLL_COMMON_REFDIV, PLL_USB_VCO_FREQ_KHZ * KHZ, PLL_USB_POSTDIV1, PLL_USB_POSTDIV2);
+    }
+    /// \end::pll_init[]
+
+    // Configure clocks
+    // CLK_REF = XOSC (usually) 12MHz / 1 = 12MHz
+    clock_configure(clk_ref,
+        CLOCKS_CLK_REF_CTRL_SRC_VALUE_XOSC_CLKSRC,
+        0,             // No aux mux
+        XOSC_KHZ * KHZ,
+        XOSC_KHZ * KHZ);
+
+    /// \tag::configure_clk_sys[]
+    // CLK SYS = PLL SYS (usually) 125MHz / 1 = 125MHz
+    clock_configure(clk_sys,
+        CLOCKS_CLK_SYS_CTRL_SRC_VALUE_CLKSRC_CLK_SYS_AUX,
+        CLOCKS_CLK_SYS_CTRL_AUXSRC_VALUE_CLKSRC_PLL_SYS,
+        SYS_CLK_KHZ * KHZ,
+        SYS_CLK_KHZ * KHZ);
+    /// \end::configure_clk_sys[]
+
+    if (init_usb) {
+        // CLK USB = PLL USB 48MHz / 1 = 48MHz
+        clock_configure(clk_usb,
+            0, // No GLMUX
+            CLOCKS_CLK_USB_CTRL_AUXSRC_VALUE_CLKSRC_PLL_USB,
+            USB_CLK_KHZ * KHZ,
+            USB_CLK_KHZ * KHZ);
+    }
+
+    // CLK ADC = PLL USB 48MHZ / 1 = 48MHz
+    clock_configure(clk_adc,
+        0,             // No GLMUX
+        CLOCKS_CLK_ADC_CTRL_AUXSRC_VALUE_CLKSRC_PLL_USB,
+        USB_CLK_KHZ * KHZ,
+        USB_CLK_KHZ * KHZ);
+
+    // CLK RTC = PLL USB 48MHz / 1024 = 46875Hz
+    clock_configure(clk_rtc,
+        0,             // No GLMUX
+        CLOCKS_CLK_RTC_CTRL_AUXSRC_VALUE_CLKSRC_PLL_USB,
+        USB_CLK_KHZ * KHZ,
+        RTC_CLOCK_FREQ_HZ);
+
+    // CLK PERI = clk_sys. Used as reference clock for Peripherals. No dividers so just select and enable
+    // Normally choose clk_sys or clk_usb
+    clock_configure(clk_peri,
+        0,
+        CLOCKS_CLK_PERI_CTRL_AUXSRC_VALUE_CLK_SYS,
+        SYS_CLK_KHZ * KHZ,
+        SYS_CLK_KHZ * KHZ);
+}
diff --git a/ports/rp2/clocks_extra.h b/ports/rp2/clocks_extra.h
new file mode 100644
index 000000000..40f77f53b
--- /dev/null
+++ b/ports/rp2/clocks_extra.h
@@ -0,0 +1,33 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2024 Angus Gratton
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifndef MICROPY_INCLUDED_RP2_CLOCKS_EXTRA_H
+#define MICROPY_INCLUDED_RP2_CLOCKS_EXTRA_H
+
+#include "hardware/clocks.h"
+
+void clocks_init_optional_usb(bool init_usb);
+
+#endif // MICROPY_INCLUDED_RP2_CLOCKS_EXTRA_H