@ SCB_BASE	0xE01FC000      @ System Control Block
@ #define PLLCON_OFFSET		0x080
@ #define PLLCFG_OFFSET		0x084
@ #define PLLSTAT_OFFSET	0x088
@ #define PLLFEED_OFFSET	0x08C
@ #define SCS_OFFSET		0x1A0
@ #define MAMCR_OFFSET		0x000
@ #define MAMTIM_OFFSET		0x004
@ #define MEMMAP_OFFSET		0x040

#ifdef CONFIG_PLL_SETUP
	ldr r0, =SCB_BASE
	ldr r1, [r0, #PLLSTAT_OFFSET]
	ldr r2, [r1]
	tst r2, #(1<<25)
	bne label_1
	
	/* Enable PLL, disconnected  */
	
	mov r3, #1
	str r3, [r0, #PLLCON_OFFSET]
	mov	r1, #0xAA
	mov	r2, #0x55
	str	r1, [r0, #PLLFEED_OFFSET]
	str	r2, [r0, #PLLFEED_OFFSET]
	
 	/*  Disable PLL, disconnected  */
	
label_1:
    mov r3, #0
    str r3, [r0, #PLLCON_OFFSET]
	mov	r1, #0xAA
	mov	r2, #0x55
	str	r1, [r0, #PLLFEED_OFFSET]
	str	r2, [r0, #PLLFEED_OFFSET]
	
	/*  Enable main OSC   */
	
	ldr r3, [r0, #SCS_OFFSET]
	ldr r2, [r3]
	orr r2, r2, #0x20
	strb r2, [r3]

	/* Use Fast I0 on P0 & P1 */

	ldr r1, [r0, #SCS_OFFSET]
	ldr r2, [r1]
	orr r2, r2, #0x01
	strb r2, [r1]

	/* Wait until main OSC is stable */
	
label_2:
	ldr r1, [r0, #SCS_OFFSET]
	ldr r2, [r1]
	tst r2, #0x40
	beq label_2

	/* Set PLL Clock Source,  CONFIG_PLL_CLKSRC = IRC=0 || MAIN=1 || RTC=2 */
	
	mov r1, #CONFIG_PLL_CLKSRC
	str r1, [r0, #LPC23XX_CLKSRCSEL_OFFSET]
	
	/* Set PLL Multiplier and Divider */

	mov r3, #CONFIG_PLLCFG_VALUE
	str r3, [r0, #PLLCFG_OFFSET]
	mov	r1, #0xAA
	mov	r2, #0x55
	str	r1, [r0, #PLLFEED_OFFSET]
	str	r2, [r0, #PLLFEED_OFFSET]

	/* Enable PLL, disconnected */
	
	mov r3, #1
	str r3, [r0, #PLLCON_OFFSET]
	mov	r1, #0xAA
	mov	r2, #0x55
	str	r1, [r0, #PLLFEED_OFFSET]
	str	r2, [r0, #PLLFEED_OFFSET]
	
	/* Set clock divider */
	
	mov r1, #LPC23XX_CCLK_DIV
	str r1, [r0, #LPC23XX_CCLKCFG_OFFSET]

	/* Wait until PLL Locked */
1:
	ldr r3, [r0, #PLLSTAT_OFFSET]
	ands r3, r3, #PLLSTAT_PLOCK
	beq	1b

	/* Enable and connect PLL */

	@mov	r3, #(PLLCON_PLLE | PLLCON_PLLC)
	mov r3, #3
	str	r3, [r0, #PLLCON_OFFSET]
	mov	r1, #0xAA
	mov	r2, #0x55
	str	r1, [r0, #PLLFEED_OFFSET]
	str	r2, [r0, #PLLFEED_OFFSET]
	
	/* Check for connect bit */
2:
	ldr r3, [r0, #PLLSTAT_OFFSET]
	ands r3, r3, #PLLSTAT_PLLC
	beq	2b

	/* PCLK is the same as CCLK */
	
	@ldr r1, =0x55555555 /* fixup error to big constant */
	mov r1, #0x55 
	str r1, [r0, #LPC23XX_PCLKSEL0_OFFSET]
	@str r2, [r0, #LPC23XX_PCLKSEL1_OFFSET]
	
	/* Configure the Memory Accelerator Module (MAM) */
	
	mov r3, #0
	mov r2, #3
	strb r3, [r0, #MAMCR_OFFSET]
	strb r2, [r0, #MAMTIM_OFFSET]
	mov r1, #2
	strb r1, [r0, #MAMCR_OFFSET]

#endif