.section .text

.equ rst_base,		0x40020000	// Subsystems reset reg base 7.5.3
.equ rst_clr,		0x40023000 	// Adding 0x3000 according 2.1.3
.equ io_bank,		0x40028000
.equ io_bank_gp25,	0x400280cc
.equ sio_base, 		0xd0000000
.equ big_num,		0x00f00000	// For the delay.
.equ pads_base,		0x40038000  // Pad control registers base.
.equ pads_set,		0x4003a000
.equ pads_clr,		0x4003b000
.equ resets_wrd,	0b100001001000000 // Bits 14 (PLL_SYS), 9 (PADS_BANK0) and 6 (IO_BANK0)
// Clocks.
.equ xosc_base,			0x40048000
.equ xosc_set,			0x4004a000
.equ xosc_en_mask,		0xfab000
.equ clk_base,			0x40010000
.equ clk_set, 			0x40012000
.equ clk_clr, 			0x40013000
.equ pll_base,			0x40050000
.equ pll_clr,			0x40053000

.thumb_func // This makes sure that the address of start has its LSB set to signal thumb mode.
.global hello_start
hello_start:
// Bring PLL, PADS and IO out of reset
	ldr r0, =rst_clr
	ldr r2, =resets_wrd
	str r2, [r0, #0]
check_rst:
	ldr r0, =rst_base
	ldr r1, [r0, #0x8]		// Offset to RESET_DONE from rst_base
	and r1, r1, r2			// Check bit 14, 9 and 6 to ensure the subsystems we care about are out of reset.
	cmp r1, r2
	bne check_rst
clocks_setup:
// Configure and enable the crystal oscilator
	// Set the freq range.
	ldr r0, =xosc_base
	mov r1, #0xaa			// 1-15MHz range
	lsl r1, r1, #4
	str	r1, [r0]
	// Set the startup delay.
	mov r1, #0xc4			// This is the defualt value and it's around 4ms at 12MHz
	str r1, [r0, #0x0c]
	// Enable the clock.
	ldr r0, =xosc_set
	ldr r1, =xosc_en_mask
	str r1, [r0]
	// Wait for it to be ready.
	ldr r0, =xosc_base
xosc_rdy:
	ldr r1, [r0, #0x04]
	lsr r1, #31				// The stable flag is bit 31
	beq xosc_rdy			// If the flag is zero then keep checking.
	// Set the crystal osc as the reference clock.
	ldr r0, =clk_base
	mov r1, #0x02
	str r1, [r0, #0x30]		// Set XOSC_CLKSRC on the CLK_REF_CTRL reg.
	// Wait for it to be selected.
	mov r2, #0b100
xosc_ref_selected:
	ldr r1, [r0, #0x38]
	and r1, r1, r2
	cmp r1, r2
	bne xosc_ref_selected
// Configure the PLL and wait for it to lock.
// Requested: 150.0 MHz
// FREF: 12MHz (This comes from the XOSC on the board)
// REFDIV: 1
// FBDIV: 125 (VCO = 1500.0 MHz)
// PD1: 5
// PD2: 2
// (FREF / REFDIV) × FBDIV / (POSTDIV1 × POSTDIV2)
	// Typically you'd reset the PLL subsystem here, but already did so maybe it'll work.
	ldr r0, =pll_base
	// Set the refdiv
	mov r1, #1				// This is the reset value, just setting it for completeness
	str r1, [r0, #0]
	// Set the fbdiv
	mov r1, #125
	str r1, [r0, #0x08]
	// Turn on PLL
	ldr r0, =pll_clr
	mov r1, #0b100001		// Clearing bits 5 (VCOPD) and 0 (PD)
	str r1, [r0, #0x04]
	// Wait for it to lock (stabilize)
pll_locked:
	ldr r0, =pll_base
	ldr r1, [r0, #0]
	lsr r1, #31				// Bit 31 of the CS reg is LOCK
	beq pll_locked			// Wait until it's not 0
	// Set Post dividers.
	mov r1, #5
	lsl r1, r1, #4			// PD1 goes on bits 18:16
	mov r2, #2
	orr	r1, r1, r2
	lsl r1, r1, #12			// PD2 goes on bits 14:12
	str r1, [r0, #0x0c]
	// Turn on Post dividers.
	ldr r0, =pll_clr
	mov r1, #0b1000
	str r1, [r0, #0x04]
// Change the sys clock source to the PLL
	// Glitchlessly change the sys source to clk ref
	ldr r0, =clk_clr
	mov r1, #1
	str r1, [r0, #0x3c]
	// Wait for it to be selected
	ldr r0, =clk_base
	mov r2, #1
sys_clk_ref_selected:
	ldr r1, [r0, #0x44]
	and r1, r1, r2
	cmp r1, r2
	bne sys_clk_ref_selected
	// Change the aux clock to pll, will glitch but it's fine because we're on the ref clock as source now.
	ldr r0, =clk_clr
	mov r1, #0b111
	lsl r1, r1, #5 // Clear all bits on AUXSRC, which will make it CLKSRC_PLL_SYS
	str r1, [r0, #0x3c]
	// Glitchlessly change the sys source to AUX
	ldr r0, =clk_set
	mov r1, #1
	str r1, [r0, #0x3c]
	// Wait for it to be selected
	ldr r0, =clk_base
	mov r2, #0b10
sys_clk_aux_selected:
	ldr r1, [r0, #0x44]
	and r1, r1, r2
	cmp r1, r2
	bne sys_clk_aux_selected
configure_peripheral:		
	// See: https://github.com/raspberrypi/pico-sdk/blob/ee68c78d0afae2b69c03ae1a72bf5cc267a2d94c/src/rp2_common/hardware_gpio/gpio.c#L38
	// Set pad input and output enabled.
	ldr r0, =pads_set
	mov r2, #0b1000000		// Set bit 6 to enable input.
	str r2, [r0, #0x68]
	ldr r0, =pads_clr
	mov r2, #0b10000000		// Clear bit 7 to enable output.
	str r2, [r0, #0x68]
	// Set function 5 (SIO) to GPIO25
	ldr r0, =io_bank_gp25
	mov r1, #5				// FSEL 5 (SIO)
	str r1, [r0]
	// Remove isolation control on the pad control now that it's connected to its peripheral (SIO).
	ldr r0, =pads_clr
	mov r2, #1				// Clear bit 8 to remove isolation control.
	lsl r2, r2, #8
	str r2, [r0, #0x68]
	// Enable the output
	ldr r0, =sio_base
	mov r1, #1
	lsl r1, r1, #25			// Only enable gpio25
	str r1, [r0, #0x38]		// GPIO_OE_SET offset
blink:
	ldr r0, =sio_base
	mov r1, #1
	lsl r1, r1, #25
led_loop:
	str r1, [r0, #0x18] // GPIO_OUT_SET
	ldr r3, =big_num
	bl delay

	str r1, [r0, #0x20] // GPIO_OUT_CLR
	ldr r3, =big_num
	bl delay

	b led_loop

delay:
	sub r3, #1
	bne delay
	bx lr

// An interrupt handler that just spins.
.thumb_func
.global isr_default
isr_default:
	b isr_default