.file "rom_reset.s"

/*
 * General notice:
 * This code is part of a boot-monitor package developed as a generic base
 * platform for embedded system designs.  As such, it is likely to be
 * distributed to various projects beyond the control of the original
 * author.  Please notify the author of any enhancements made or bugs found
 * so that all may benefit from the changes.  In addition, notification back
 * to the author will allow the new user to pick up changes that may have
 * been made by other users after this version of the code was distributed.
 *
 * Author:  Ed Sutter
 * email:   esutter@lucent.com
 * phone:   908-582-2351
 * 
 *
 * Modified for the TLL6219 - M. McDermott   3-24-2008
 *
 * rom_reset.s:
  */

#include "warmstart.h"
#include "mc9328mx21.h"
#include "config.h"

    /*
     * Have a separate stack for each processor mode.
     */

    /* define sizes for each mode's stack */
    .equ FiqStackSz, 4096
    .equ IrqStackSz, 4096
    .equ AbtStackSz, 4096
    .equ UndStackSz, 4096
    .equ SysStackSz, 4096

    /* declare the stacks */
    .extern MonStack
    .global FiqStack
    .global IrqStack
    .global AbtStack
    .global UndStack
    .global SysStack        
    
    /* allocate the stacks */
    .comm   FiqStack, FiqStackSz    /* for the FIQ mode */
    .comm   IrqStack, IrqStackSz    /* for the IRQ mode */
    .comm   AbtStack, AbtStackSz    /* for the Abort mode */
    .comm   UndStack, UndStackSz    /* for the Undef mode */
    .comm   SysStack, SysStackSz    /* for the System mode */
    /* User mode has the same stack as system mode. */

/*********************************************************************/
    
    .extern start

    .global reset
    .global coldstart
    .global lukewarmstart
    .global warmstart

    .text
        
    /*
     * Exception table at address 0
     */ 
reset:  
    b coldstart
    b undefined_instruction
    b software_interrupt
    b abort_prefetch
    b abort_data
    b not_assigned
    b interrupt_request
    b fast_interrupt_request

#include "etheraddr.S"
#include "moncomptr.S"
#include "alttfsdevtbl.S"

/*********************************************************************/

    /*
     * At the end of the reset sequence, MMU, Icache, Dcache,
     * and write buffer are all disabled.
     * Also IRQs and FIQs are disabled in the processor's CPSR
     * The operating mode is SVC (supervisory mode), and the
     * PC is vectored at 0x00000000. A branch in 0x00000000
     * brings us directly here.
	 *
	 */

coldstart:
	ldr	pc, =coldstart_1		// jump to actual ROM location
	nop

coldstart_1:
	ldr r0, =0x2001					/* allow access to all coprocessors */
	mcr	p15,0,r0,c15,c1,0
	nop
	nop
	nop

	ldr	r0, =0x00000078
	mcr	p15,0,r0,c1,c0,0		/* Disable MMU, caches, write buffer */
	nop
	nop
	nop

	ldr	r0, =0x00000000
	mcr	p15,0,r0,c8,c7,0   		/* flush TLB's */
	mcr	p15,0,r0,c7,c7,0   		/* flush Caches */
	mcr	p15,0,r0,c7,c10,4		/* Flush Write Buffer */
	nop
	nop
	nop

	mvn	r0, #0                   /* grant manager access to all domains */
	mcr	p15,0,r0,c3,c0,0  

/*	WM32 0x10000000 0x00040304
	WM32 0x10020000 0x3FFC0000
	WM32 0x10000004 0xFFFBFCFB
	WM32 0x10020004 0xFFFFFFFF
*/
	ldr r2, =0x00040304
	ldr r1, =0x10000000
    str r2, [r1]

	ldr r2, =0x3FFC0000
	ldr r1, =0x10020000
    str r2, [r1]

	ldr r2, =0xFFFBFCFB
	ldr r1, =0x10000004
    str r2, [r1]

	ldr r2, =0xFFFFFFFF
	ldr r1, =0x10020004
    str r2, [r1]

	bl	delay_200

init_pll:

// ------------------------------------------------------------------	
// Setup the MPLL
// ------------------------------------------------------------------	

//	ldr r2, =0x007b1c73     // 266 MHz
//	ldr r2, =0x03811c89     // 240 MHz
//	ldr r2, =0x0087185c     // 224 MHz
	ldr r2, =0x01381cc5     // 256 MHz
//	ldr r2, =0x05381cc5     // 128 MHz

	ldr r1, =0x10027004
	str r2, [r1]

	bl	delay_200

// ------------------------------------------------------------------	
// Set SPLL to default - 288 MHz  
// ------------------------------------------------------------------	

	ldr r2, =0x0272216d
	ldr r1, =0x1002700c
	str r2, [r1]

// ------------------------------------------------------------------	
//   Set HCLK to 32 MHz for 224MHz
// ------------------------------------------------------------------	

//	ldr r2, =0x17001a07	// 224 MHZ CPU clock divide by 7


// ------------------------------------------------------------------	
//   Set HCLK to 66/64/60 MHz for various CPU clock frequencies
// ------------------------------------------------------------------	

	ldr r2, =0x17000e07     // 240/256/266 MHZ CPU clock divide by 4
//	ldr r2, =0x17000607     // 128 MHZ CPU clock divide by 2 
	ldr r1, =0x10027000
	str r2, [r1]

// ------------------------------------------------------------------	
//   Set PERCLK1 to 16 MHz for various CPU clock frequencies
// ------------------------------------------------------------------	

	ldr r2, =0x0307070f	// 256 MHz CPU Clock divide by 16
//	ldr r2, =0x0307070e	// 240 MHz CPU Clock divide by 15
//	ldr r2, =0x0307070d	// 224 MHz CPU clock divide by 14
//	ldr r2, =0x03070707	// 128 MHz CPU Clock divide by 8
	ldr r1, =0x1002701c     // Write to PCDR1
	str r2, [r1]
	
	bl	delay_200
	
// ------------------------------------------------------------------	
// Restart the PLLs 
// ------------------------------------------------------------------	

	ldr r2, =0x17600e07	// 240/256/266 MHz CPU Clock
//	ldr r2, =0x17601a07	// 224 MHz CPU Clock
//	ldr r2, =0x17600607	// 128 MHz CPU Clock
	ldr r1, =0x10027000
	str r2, [r1]

	bl	delay_200

	
	 
// ------------------------------------------------------------------	
// Setup source for CLK0
// ------------------------------------------------------------------	

//	ldr r2, =0x0000000A     // PERCLK1
	ldr r2, =0x00000008     // HCLK
	ldr r1, =0x10027028
	str r2, [r1]

	bl	delay_200

// ------------------------------------------------------------------	
// Setup the DRAM controller
// ------------------------------------------------------------------	


init_sdram0:
	// Initialize SDRAM Controller
	// issue Precharge All
	ldr r2, =0x9212C300
	ldr r1, =0xDF000000
	str r2, [r1]

	// read the SDRAM to make cycle occur
	ldr r4, =0xC0000000
	ldr r3, [r4]

	bl	delay_200

init_sdram1:
	// issue AutoRefresh (x8)
        ldr r2, =0xA212C300

        ldr r1, =0xDF000000
        str r2, [r1]

	// read the SDRAM to make cycle occur
	ldr r4, =0xC0000000
	ldr r3, [r4]
	ldr r3, [r4]
	ldr r3, [r4]
	ldr r3, [r4]
	ldr r3, [r4]
	ldr r3, [r4]
	ldr r3, [r4]
	ldr r3, [r4]

	bl	delay_200

init_sdram2:
	// issue Mode Register Set
        ldr r2, =0xB212C308

        ldr r1, =0xDF000000
        str r2, [r1]

	// read the SDRAM to make cycle occur
	// address is loaded into SDRAM Mode Register
	ldr r4, =0xC0119800

	ldr r3, [r4]

	bl	delay_200

init_sdram3:
	// Set Normal Mode and enable refresh
        ldr r2, =0x8212C300

        ldr r1, =0xDF000000
        str r2, [r1]

	// read the SDRAM to make cycle occur
	ldr r4, =0xC0112420
	ldr r3, [r4]

	bl	delay_200


/********************************************************************/

midstart:
    ldr r0, =INITIALIZE
    
    /* fall-through to 'lukewarmstart' */

/********************************************************************/
    
lukewarmstart:  
    /* Save the argument to r11 */
    mov r11, r0

    /*
     * *** DO NOT TOUCH R11 ***
     */ 

    /*
     * Set-up the stack-pointers for all operating modes
     */

    /* FIQ mode */
    mrs r0, cpsr                /* move CPSR to r0 */
    bic r0, r0, #0x1f           /* clear all mode bits */
    orr r0, r0, #0x11           /* set FIQ mode bits */
    msr CPSR_c, r0              /* move back to CPSR */
    ldr sp, =(FiqStack + FiqStackSz - 4)    /* initialize the stack ptr */
    /* IRQ mode */
    mrs r0, cpsr                /* move CPSR to r0 */
    bic r0, r0, #0x1f           /* clear all mode bits */
    orr r0, r0, #0x12           /* set IRQ mode bits */
    msr CPSR_c, r0              /* move back to CPSR */
    ldr sp, =(IrqStack + IrqStackSz - 4)    /* initialize the stack ptr */
    /* Abort mode */
    mrs r0, cpsr                /* move CPSR to r0 */
    bic r0, r0, #0x1f           /* clear all mode bits */
    orr r0, r0, #0x17           /* set Abort mode bits */
    msr CPSR_c, r0              /* move back to CPSR */
    ldr sp, =(AbtStack + AbtStackSz - 4)    /* initialize the stack ptr */
    /* Undef mode */
    mrs r0, cpsr                /* move CPSR to r0 */
    bic r0, r0, #0x1f           /* clear all mode bits */
    orr r0, r0, #0x1b           /* set Undef mode bits */
    msr CPSR_c, r0              /* move back to CPSR */
    ldr sp, =(UndStack + UndStackSz - 4)    /* initialize the stack ptr */
    /* System mode */
    mrs r0, cpsr                /* move CPSR to r0 */
    bic r0, r0, #0x1f           /* clear all mode bits */
    orr r0, r0, #0x1f           /* set System mode bits */
    msr CPSR_c, r0              /* move back to CPSR */
    ldr sp, =(SysStack + SysStackSz - 4)    /* initialize the stack ptr */
    /* 'warmstart' will take us back to SVC mode
       stack for SVC mode will also be setup in warmstart */

    mov r0, r11     /* get argument back from r11 */
    b   warmstart

    
/********************************************************************/
    
warmstart:
    /* Save the argument to r11 */
    mov r11, r0

    /*
     * *** DO NOT TOUCH R11 ***
     */ 


    /* Change (back) to SVC mode */
    mrs r0, cpsr                /* move CPSR to r0 */
    bic r0, r0, #0x1f           /* clear all mode bits */
    orr r0, r0, #0x13           /* set System mode bits */
    msr CPSR_c, r0              /* move back to CPSR */
    /* Reset the stack pointer for the SVC mode (our current mode) */
    ldr sp, =(MonStack + MONSTACKSIZE - 4)

    /*
     * Restore argument which was saved to r11 and jump to
     * the C function start().
     */ 
    
    mov r0, r11
jump_to_c:
    bl start

    /* the C code should never return */
    b reset

.align 4


//********************************************************************/
// simple delay loop
delay_200:
	ldr		r3, =200			/* loop count */
delay_loop:
	subs	r3,r3,#1
	bne		delay_loop
	nop

	mov		pc, lr