/*****************************************************************************
  Cogent CSB637 loader startup code.

  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"

  .text
  .code 32
  .align 0

  .global _start
  .extern C_Entry
  .global stack_start
  .global stack_end

/*****************************************************************************
  Function    : _start
  Description : Minimal startup code for C system.
 *****************************************************************************/
_start:                         
  /*****************************************************************************
    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 */

 /*****************************************************************************
    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, =0x20000000 /* 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:

  /* Setup stack */ 
  add sp, pc, #1024 
  
  /* Jump to main entry point */
  bl C_Entry

  /* Return from main, loop forever. */
loop:
  b loop

stack_start:
  .fill 1024
stack_end:
                  
