1 files changed, 100 insertions, 0 deletions
diff --git a/src/machine/machine_rp2040_pll.go b/src/machine/machine_rp2040_pll.go
new file mode 100644
index 000000000..275aa25f1
--- /dev/null
+++ b/src/machine/machine_rp2040_pll.go
@@ -0,0 +1,100 @@
+// +build rp2040
+
+package machine
+
+import (
+	"device/rp"
+	"runtime/volatile"
+	"unsafe"
+)
+
+type pll struct {
+	cs       volatile.Register32
+	pwr      volatile.Register32
+	fbDivInt volatile.Register32
+	prim     volatile.Register32
+}
+
+var (
+	pllSys = (*pll)(unsafe.Pointer(rp.PLL_SYS))
+	pllUSB = (*pll)(unsafe.Pointer(rp.PLL_USB))
+)
+
+// init initializes pll (Sys or USB) given the following parameters.
+//
+// Input clock divider, refdiv.
+//
+// Requested output frequency from the VCO (voltage controlled oscillator), vcoFreq.
+//
+// Post Divider 1, postDiv1 with range 1-7 and be >= postDiv2.
+//
+// Post Divider 2, postDiv2 with range 1-7.
+func (pll *pll) init(refdiv, vcoFreq, postDiv1, postDiv2 uint32) {
+	refFreq := xoscFreq / refdiv
+
+	// What are we multiplying the reference clock by to get the vco freq
+	// (The regs are called div, because you divide the vco output and compare it to the refclk)
+	fbdiv := vcoFreq / (refFreq * MHz)
+
+	// Check fbdiv range
+	if !(fbdiv >= 16 && fbdiv <= 320) {
+		panic("fbdiv should be in the range [16,320]")
+	}
+
+	// Check divider ranges
+	if !((postDiv1 >= 1 && postDiv1 <= 7) && (postDiv2 >= 1 && postDiv2 <= 7)) {
+		panic("postdiv1, postdiv1 should be in the range [1,7]")
+	}
+
+	// postDiv1 should be >= postDiv2
+	// from appnote page 11
+	// postdiv1 is designed to operate with a higher input frequency
+	// than postdiv2
+	if postDiv1 < postDiv2 {
+		panic("postdiv1 should be greater than or equal to postdiv2")
+	}
+
+	// Check that reference frequency is no greater than vco / 16
+	if refFreq > vcoFreq/16 {
+		panic("reference frequency should not be greater than vco frequency divided by 16")
+	}
+
+	// div1 feeds into div2 so if div1 is 5 and div2 is 2 then you get a divide by 10
+	pdiv := postDiv1<<rp.PLL_SYS_PRIM_POSTDIV1_Pos | postDiv2<<rp.PLL_SYS_PRIM_POSTDIV2_Pos
+
+	if pll.cs.HasBits(rp.PLL_SYS_CS_LOCK) &&
+		refdiv == pll.cs.Get()&rp.PLL_SYS_CS_REFDIV_Msk &&
+		fbdiv == pll.fbDivInt.Get()&rp.PLL_SYS_FBDIV_INT_FBDIV_INT_Msk &&
+		pdiv == pll.prim.Get()&(rp.PLL_SYS_PRIM_POSTDIV1_Msk&rp.PLL_SYS_PRIM_POSTDIV2_Msk) {
+		// do not disrupt PLL that is already correctly configured and operating
+		return
+	}
+
+	var pllRst uint32
+	if pll == pllSys {
+		pllRst = rp.RESETS_RESET_PLL_SYS
+	} else {
+		pllRst = rp.RESETS_RESET_PLL_USB
+	}
+	resetBlock(pllRst)
+	unresetBlockWait(pllRst)
+
+	// Load VCO-related dividers before starting VCO
+	pll.cs.Set(refdiv)
+	pll.fbDivInt.Set(fbdiv)
+
+	// Turn on PLL
+	pwr := uint32(rp.PLL_SYS_PWR_PD | rp.PLL_SYS_PWR_VCOPD)
+	pll.pwr.ClearBits(pwr)
+
+	// Wait for PLL to lock
+	for !(pll.cs.HasBits(rp.PLL_SYS_CS_LOCK)) {
+	}
+
+	// Set up post dividers
+	pll.prim.Set(pdiv)
+
+	// Turn on post divider
+	pll.pwr.ClearBits(rp.PLL_SYS_PWR_POSTDIVPD)
+
+}