/*****************************************************************************
  Exception handlers and startup code for Freescale MC9328MXL.

  Copyright (c) 2005 Rowley Associates Limited.

  This file may be distributed under the terms of the License Agreement
  provided with this software. 
 
  THIS FILE IS PROVIDED AS IS WITH NO WARRANTY OF ANY KIND, INCLUDING THE
  WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *****************************************************************************/

  .section .vectors, "ax"
  .code 32
  .align 0
  
/*****************************************************************************
  Exception Vectors
 *****************************************************************************/
_vectors:
  ldr pc, [pc, #reset_handler_address - . - 8]  /* reset */
  ldr pc, [pc, #undef_handler_address - . - 8]  /* undefined instruction */
  ldr pc, [pc, #swi_handler_address - . - 8]    /* swi handler */
  ldr pc, [pc, #pabort_handler_address - . - 8] /* abort prefetch */
  ldr pc, [pc, #dabort_handler_address - . - 8] /* abort data */
  nop
  ldr pc, [pc, #irq_handler_address - . - 8]    /* irq */
  ldr pc, [pc, #fiq_handler_address - . - 8]    /* fiq */

reset_handler_address:
  .word reset_handler
undef_handler_address:
  .word undef_handler
swi_handler_address:
  .word swi_handler
pabort_handler_address:
  .word pabort_handler
dabort_handler_address:
  .word dabort_handler
irq_handler_address:
  .word irq_handler
fiq_handler_address:
  .word fiq_handler

  .section .init, "ax"
  .code 32
  .align 0

#define PLLCLK_BASE 0x0021B000
#define PLLCLK_CSCR_OFFS 0x00
#define PLLCLK_MPCTL0_OFFS 0x04
#define PLLCLK_PCDR_OFFS 0x20

#define EIM_BASE 0x00220000
#define EIM_CS0U_OFFS 0x00
#define EIM_CS0L_OFFS 0x04
#define EIM_CS1U_OFFS 0x08
#define EIM_CS1L_OFFS 0x0C
#define EIM_CS4U_OFFS 0x20
#define EIM_CS4L_OFFS 0x24

#define SDRAMC_BASE 0x00221000
#define SDRAMC_SDCTL0_OFFS 0x00
#define SDRAMC_SDCTL1_OFFS 0x04

#define AITC_BASE 0x00223000
#define AITC_INTENABLEH_OFFS 0x10
#define AITC_INTENABLEL_OFFS 0x14

#define RAM_START_ADDRESS 0x08000000
#define RAM_LENGTH 0x04000000
#define RAM_END_ADDRESS (RAM_START_ADDRESS + RAM_LENGTH)

#define ROM_START_ADDRESS 0x0C000000
#define ROM_LENGTH 0x01000000
#define ROM_END_ADDRESS (ROM_START_ADDRESS + ROM_LENGTH)

/******************************************************************************
  Function:    mmu_flat_initialise
  Description: Create a flat mapped translation table (VA == PA).
  Parameters:  r0 - Starting address of translation table.
 ******************************************************************************/
mmu_flat_initialise:
  ldr r1, =0x00001000
  ldr r2, =0x00000002 /* Section descriptor, domain = 0, AP = 0, base address = 0 */
1:
  str r2, [r0], #4
  add r2, r2, #0x00100000 /* Increment base address by 1MB */
  subs r1, r1, #1
  bne 1b
  bx lr

/******************************************************************************
  Function:    mmu_flat_set_cacheable_region
  Description: Set region of memory as cacheable in flat mapped translation
               table.
  Parameters:  r0 - Starting address of translation table.
               r1 - Starting address of cacheable region.
               r2 - Length of cacheable region in bytes.
 ******************************************************************************/
mmu_flat_set_cacheable_region:
  add r0, r0, r1, lsr #18
  mov r1, r2, lsr #20
1:
  ldr r2, [r0]
  orr r2, r2, #0x00000008 /* set cacheable bit */
  str r2, [r0], #4
  subs r1, r1, #1
  bne 1b
  bx lr

/******************************************************************************
  Reset handler
 ******************************************************************************/
reset_handler:
  /* Disable MMU and caches */
  mov r0, #0
  mcr p15, 0, r0, c7, c7, 0          /* Invalidate ICache and DCache */
  mcr p15, 0, r0, c7, c10, 4         /* Drain write buffer */
  mcr p15, 0, r0, c8, c7, 0          /* Invalidate I & D TLBs */
  mrc p15, 0, r0, c1, c0, 0          /* Read MMU control register */
  bic r0, r0, #0x00001000            /* Disable ICache */
  bic r0, r0, #0x00000007            /* Disable DCache, MMU and alignment fault */
#ifdef __BIG_ENDIAN
  orr r0, r0, #0x00000080            /* Set B bit */
#else
  bic r0, r0, #0x00000080            /* Clear B bit */
#endif
  mcr p15, 0, r0, c1, c0, 0          /* Write MMU control register */

  /* Disable all interrupts */       
  ldr r1, =AITC_BASE
  ldr r0, =0x00000000
  str r0, [r1, #AITC_INTENABLEH_OFFS]
  str r0, [r1, #AITC_INTENABLEL_OFFS]

  ldr r1, =PLLCLK_BASE
  /* Select CLKO mux to output HCLK(BCLK) */
  ldr r0, =0x2F00AC03
  str r0, [r1, #PLLCLK_CSCR_OFFS]
  /* 48Mhz system clock */
  ldr r0, =0x2F008403
  str r0, [r1, #PLLCLK_CSCR_OFFS]
  /* PERCLK1 = PLLCLK / 1 */
  ldr r0, =0x000B00B0
  str r0, [r1, #PLLCLK_PCDR_OFFS]

  /* Double clock (PRESC = 1)*/
  ldr r0, =0x2F000403
  str r0, [r1, #PLLCLK_CSCR_OFFS]

  /* Setup FCLK 192 MHz */
  ldr r0, =0x003F1437
  str r0, [r1, #PLLCLK_MPCTL0_OFFS]

  ldr r0, [r1, #PLLCLK_CSCR_OFFS]
  orr r0, r0, #(1 << 21)
  str r0, [r1, #PLLCLK_CSCR_OFFS]

  /* CS0 - Boot FLASH, 32 wait states, 8 bit */
  ldr r1, =EIM_BASE
  ldr r0, =0x00002000
  str r0, [r1, #EIM_CS0U_OFFS]
  ldr r0, =0x11110301
  str r0, [r1, #EIM_CS0L_OFFS]
  /* CS1 - SDRAM */
  ldr r0, =0x00000A00
  str r0, [r1, #EIM_CS1U_OFFS]
  ldr r0, =0x11110601
  str r0, [r1, #EIM_CS1L_OFFS]
  /* CS4 */
  ldr r0, =0x00000F01
  str r0, [r1, #EIM_CS4U_OFFS]
  ldr r0, =0x11110B01
  str r0, [r1, #EIM_CS4L_OFFS]

  /* SDRAM precharge command */
  ldr r1, =SDRAMC_BASE
  ldr r0, =0x92120200
  str r0, [r1, #SDRAMC_SDCTL0_OFFS]
  ldr r0, =0x08200000
  ldr r2, [r0]
  /* SDRAM Auto-refresh command */
  ldr r0, =0xA2120200
  str r0, [r1, #SDRAMC_SDCTL0_OFFS]
  ldr r0, =0x08000000
  ldr r2, [r0]
  ldr r2, [r0]
  ldr r2, [r0]
  ldr r2, [r0]
  ldr r2, [r0]
  ldr r2, [r0]
  ldr r2, [r0]
  ldr r2, [r0]
  /* SDRAM Set mode register command */
  ldr r0, =0xB2120200
  str r0, [r1, #SDRAMC_SDCTL0_OFFS]
  ldr r0, =0x08111800
  ldr r2, [r0]
  /* SDRAM Set normal mode */
  ldr r0, =0x82124200
  str r0, [r1, #SDRAMC_SDCTL0_OFFS]
  /* Initialise SyncFLASH */
  ldr r0, =0x81020303
  str r0, [r1, #SDRAMC_SDCTL1_OFFS]

#ifndef NO_CACHE_ENABLE
  /* Set the tranlation table base address */
  ldr r0, =(RAM_END_ADDRESS - 0x4000)
  mcr p15, 0, r0, c2, c0, 0          /* Write to TTB register */
     
  /* Setup the domain access control so accesses are not checked */
  ldr r0, =0xFFFFFFFF
  mcr p15, 0, r0, c3, c0, 0          /* Write to domain access control register */

  /* Create translation table */
  ldr r0, =(RAM_END_ADDRESS - 0x4000)
  bl mmu_flat_initialise

  /* Set cacheable RAM region */
  ldr r0, =(RAM_END_ADDRESS - 0x4000)
  ldr r1, =RAM_START_ADDRESS
  ldr r2, =(RAM_LENGTH - 0x00100000) /* Make last 1MB of RAM non-cacheable */
  bl mmu_flat_set_cacheable_region

  /* Set cacheable ROM region */
  ldr r0, =(RAM_END_ADDRESS - 0x4000)
  ldr r1, =ROM_START_ADDRESS
  ldr r2, =ROM_LENGTH
  bl mmu_flat_set_cacheable_region

  /* Enable the MMU and caches */
  mrc p15, 0, r0, c1, c0, 0          /* Read MMU control register */
  orr r0, r0, #0x00001000            /* Enable ICache */
  orr r0, r0, #0x00000007            /* Enable DCache, MMU and alignment fault */
  orr r0, r0, #0xC0000000            /* Enable asynchronous clocking */
  mcr p15, 0, r0, c1, c0, 0          /* Write MMU control register */
#endif

  /* Jump to the default C runtime startup code. */
  b _start

/******************************************************************************
  Default exception handlers
  (These are declared weak symbols so they can be redefined in user code)
 ******************************************************************************/
undef_handler:
  b 0 /* Endless loop */
 
swi_handler:
  b 0 /* Endless loop */
 
pabort_handler:
  b 0 /* Endless loop */
 
dabort_handler:
  b 0 /* Endless loop */
 
irq_handler:
  b 0 /* Endless loop */
 
fiq_handler:
  b 0 /* Endless loop */

  .weak undef_handler, swi_handler, pabort_handler, dabort_handler, irq_handler, fiq_handler
