/*****************************************************************************
  Exception handlers and startup code for Cogent CSB637.

  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.
 *****************************************************************************/

#include "targets/AT91RM9200.h"

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

#define INTERNAL_SRAM_START_ADDRESS 0x00200000

#define SDRAM_START_ADDRESS 0x20000000
#define SDRAM_LENGTH 0x08000000

#define FLASH_START_ADDRESS 0x10000000
#define FLASH_LENGTH 0x01000000

#define MMU_TRANSLATION_TABLE_START_ADDRESS 0x20000000

/*****************************************************************************
  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
#ifdef NO_VECTORED_IRQ_INTERRUPTS
  ldr pc, [pc, #irq_handler_address - . - 8]    /* irq */
#else
  ldr pc, [pc, #-0xF20]                         /* irq */
#endif
#ifdef NO_VECTORED_FIQ_INTERRUPTS
  ldr pc, [pc, #fiq_handler_address - . - 8]    /* fiq */
#else
  ldr pc, [pc, #-0xF20]                         /* fiq */
#endif

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

/******************************************************************************
  Function:    mmu_flat_initialise_level_1_table
  Description: Create a flat mapped translation table (VA == PA).
  Parameters:  r0 - Starting address of translation table.
 ******************************************************************************/
mmu_flat_initialise_level_1_table:
  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_section_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_section_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

/******************************************************************************
  Function:    mmu_flat_initialise_level_2_small_page_table
  Description: Create coarse page tables for address range.
  Parameters:  r0 - Starting address of translation table.
               r1 - Starting address.
               r2 - Length in bytes.
               r3 - Starting address of coarse page tables.
 ******************************************************************************/
mmu_flat_initialise_level_2_small_page_table:
  add r0, r0, r1, lsr #18 /* r0 = address in translation table */
  ldr r4, =0x000FFFF
  bic r1, r1, r4          /* r1 = small page descriptor (ap = 0, c = 0, b = 0) */
  orr r1, r1, #0x00000002 /* r1 = small page descriptor (small page) */
  mov r2, r2, lsr #20     /* r2 = number of translation table entries */
1:
  orr r4, r3, #0x00000001 /* r4 = coarse page table descriptor (domain = 0) */
  str r4, [r0], #4        /* store coarse page table descriptor in translation table */
  mov r4, #256            /* r4 = Number of coarse page table entries */
2:
  str r1, [r3], #4        /* store small page descriptor in coarse page table */
  add r1, r1, #0x00001000 /* increment small page table descriptor */
  subs r4, r4, #1         
  bne 2b
  subs r2, r2, #1
  bne 1b
  bx lr

/******************************************************************************
  Function:    mmu_flat_set_small_page_cacheable_region:
  Description: Create coarse page tables for address range.
  Parameters:  r0 - Starting address of translation table.
               r1 - Starting address.
               r2 - Length in bytes.
 ******************************************************************************/
mmu_flat_set_small_page_cacheable_region:
  add r0, r0, r1, lsr #18 /* r0 = address in translation table */
  ldr r0, [r0]
  ldr r3, =0x000003FF
  bic r0, r0, r3          /* r0 = first address of small page descriptor table */
  and r1, r1, #0x000FF000 /* r1 = L2 table index */
  add r0, r0, r1, lsr #10 /* r0 = starting address of small page descriptor */
  mov r2, r2, lsr #12     /* r2 = number of small page descriptor entries */
1:
  ldr r1, [r0]
  orr r1, r1, #0x00000008 /* set cacheable bit */
  str r1, [r0], #4
  subs r2, r2, #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 interrupts
   *****************************************************************************/
  ldr r0, =AIC_BASE
  ldr r1, =0xFFFFFFFF
  str r1, [r0, #AIC_IDCR_OFFSET]

#ifdef __FLASH_BUILD
 /*****************************************************************************
    Configure FLASH
   *****************************************************************************/
  ldr r0, =SMC_BASE
  ldr r1, =0x1100318A /* 10 wait states, 16 bit */
  str r1, [r0, #SMC_CSR0_OFFSET]

  /*****************************************************************************
    Initialise clocks
   *****************************************************************************/
  ldr r0, =PMC_BASE
  /* Switch to slow clock if not already enabled */
  ldr r1, [r0, #PMC_MCKR_OFFSET]
  cmp r1, #0
  beq enable_slow_clock_end
  bic r1, r1, #PMC_MCKR_PRES_MASK    /* Clear prescaler */
  str r1, [r0, #PMC_MCKR_OFFSET]
  bic r1, r1, #PMC_MCKR_CSS_MASK     /* Select slow clock */
  str r1, [r0, #PMC_MCKR_OFFSET]
1:
  ldr r1, [r0, #PMC_SR_OFFSET]
  tst r1, #PMC_SR_MCKRDY_MASK
  beq 1b
enable_slow_clock_end:

  /* Turn on the main oscillator */
  mov r1, #CKGR_MOR_MOSCEN
  str r1, [r0, #CKGR_MOR_OFFSET]
1:
  ldr r1, [r0, #CKGR_MCFR_OFFSET]
  tst r1, #CKGR_MCFR_MAINRDY
  beq 1b
    
  /* Configure PLLA */ 
  ldr r1, =0x20633E02 /* PLLA Clock = 184.32Mhz (3.6864Mhz main oscillator / 2 * 100) */
  str r1, [r0, #CKGR_PLLAR_OFFSET]
1:
  ldr r1, [r0, #PMC_SR_OFFSET]
  tst r1, #PMC_SR_LOCKA_MASK
  beq 1b

  /* Configure master and processor clock */
  ldr r1, =0x00000302 /* Clock source = PLLA, PCK = 184.32MHz MCK = 46.08Mhz */
  str r1, [r0, #PMC_MCKR_OFFSET]
1:
  ldr r1, [r0, #PMC_SR_OFFSET]
  tst r1, #PMC_SR_MCKRDY_MASK
  beq 1b

  /*****************************************************************************
    Configure PIO
   *****************************************************************************/
  ldr r0, =PIOC_BASE
  /* Set PC16:31 to alternate function D16:31 */
  ldr r1, =0xFFFF0000
  str r1, [r0, #PIOC_ASR_OFFSET] /* Assign PC16:31 to the peripheral A function */
  str r1, [r0, #PIOC_PDR_OFFSET] /* Disable PIO use of PC16:31 */

  /*****************************************************************************
    Configure SDRAM
   *****************************************************************************/
  ldr r0, =EBI_BASE
  ldr r2, =SDRAM_START_ADDRESS
  ldr r3, =0
  /* Only configure SDRAM if it has already been configured */
  ldr r1, [r0, #EBI_CSA_OFFSET]
  tst r1, #EBI_CSA_CS1A_MASK
  bne configure_sdram_end 
  /* Assign CS1 to SDRAM controller */
  ldr r1, =EBI_CSA_CS1A_MASK
  str r1, [r0, #EBI_CSA_OFFSET] 
  /* Pullup D0:15 */
  ldr r0, =0
  str r1, [r0, #EBI_CFGR_OFFSET]

  ldr r0, =SDRAMC_BASE
  ldr r1, =0x2188C155
  str r1, [r0, #SDRAMC_CR_OFFSET]

  /* Issue precharge all command */
  ldr r1, =2 /* Precharge all command */
  str r1, [r0, #SDRAMC_MR_OFFSET]
  str r3, [r2]

  /* Issue 8 refresh commands */
  ldr r1, =4 /* Refresh command */
  str r1, [r0, #SDRAMC_MR_OFFSET]
  str r3, [r2]
  str r3, [r2]
  str r3, [r2]
  str r3, [r2]
  str r3, [r2]
  str r3, [r2]
  str r3, [r2]
  str r3, [r2]

  /* Issue load mode register command */
  ldr r1, =3 /* Load mode register command */
  str r1, [r0, #SDRAMC_MR_OFFSET]
  str r3, [r2, #0x80]

  /* Set refresh rate */
  ldr r1, =0x200
  str r1, [r0, #SDRAMC_TR_OFFSET]
  str r3, [r2]

  /* Issue normal mode command */
  ldr r1, =0 /* Normal mode */
  str r1, [r0, #SDRAMC_MR_OFFSET]
  str r3, [r2]
configure_sdram_end:

  /* Copy exception vectors into Internal SRAM */
  ldr r8, =INTERNAL_SRAM_START_ADDRESS
  ldr r9, =_vectors
  ldmia r9!, {r0-r7}
  stmia r8!, {r0-r7}
  ldmia r9!, {r0-r6}
  stmia r8!, {r0-r6}

  /* Remap Internal SRAM to 0x00000000 */
  ldr r0, =MC_BASE
  ldr r1, =1
  str r1, [r0, #MC_RCR_OFFSET]
#endif

#ifndef NO_CACHE_ENABLE
  /* Set the tranlation table base address */
  ldr r0, =MMU_TRANSLATION_TABLE_START_ADDRESS
  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, =MMU_TRANSLATION_TABLE_START_ADDRESS
  bl mmu_flat_initialise_level_1_table

  /* Create small page table for first 1MB of SDRAM */
  ldr r0, =MMU_TRANSLATION_TABLE_START_ADDRESS
  ldr r1, =SDRAM_START_ADDRESS
  ldr r2, =0x00100000 /* 1MB = 1 coarse page table */
  ldr r3, =MMU_TRANSLATION_TABLE_START_ADDRESS + 0x4000
  bl mmu_flat_initialise_level_2_small_page_table

  /* Set cacheable region of first 1MB of SDRAM (first 32K non-cacheable) */
  ldr r0, =MMU_TRANSLATION_TABLE_START_ADDRESS
  ldr r1, =SDRAM_START_ADDRESS + 0x8000
  ldr r2, =(0x00100000-0x8000) 
  bl mmu_flat_set_small_page_cacheable_region

  /* Make the rest of SDRAM cacheable */
  ldr r0, =MMU_TRANSLATION_TABLE_START_ADDRESS
  ldr r1, =SDRAM_START_ADDRESS + 0x00100000
  ldr r2, =(SDRAM_LENGTH - 0x00100000) /* Make last 1MB of RAM non-cacheable */
  bl mmu_flat_set_section_cacheable_region
  
  /* Make FLASH cacheable */
  ldr r0, =MMU_TRANSLATION_TABLE_START_ADDRESS
  ldr r1, =FLASH_START_ADDRESS
  ldr r2, =FLASH_LENGTH
  bl mmu_flat_set_section_cacheable_region

  /* Make Internal SRAM cacheable */
  ldr r0, =MMU_TRANSLATION_TABLE_START_ADDRESS
  ldr r1, =INTERNAL_SRAM_START_ADDRESS
  ldr r2, =0x00100000
  bl mmu_flat_set_section_cacheable_region

  /* Make first 1MB of memory cacheable */
  ldr r0, =MMU_TRANSLATION_TABLE_START_ADDRESS
  ldr r1, =0x00000000
  ldr r2, =0x00100000
  bl mmu_flat_set_section_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 undef_handler
  
swi_handler:
  b swi_handler
  
pabort_handler:
  b pabort_handler
  
dabort_handler:
  b dabort_handler

#ifdef NO_VECTORED_IRQ_INTERRUPTS
irq_handler:
  b irq_handler
#endif

#ifdef NO_VECTORED_FIQ_INTERRUPTS  
fiq_handler:
  b fiq_handler
#endif

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

