;***************************************************************************
; * Cache and MMU Configuration

mrc p15, 0, r0, c1, c0, 0 ; read CP15 register 1 into r0
bic r0, r0, #0x01 ; clear MMU enable
bic r0, r0, #(0x01 << 0x0c) ; disable I Cache
bic r0, r0, #(0x01 << 0x02) ; disable D Cache
mcr p15, 0, r0, c1, c0, 0 ; write value back

mov r0, #0x00
mcr p15, 0, r0, c7, c7, 0 ; invalidate caches
mcr p15, 0, r0, c8, c7, 0 ; invalidate TLBs


ldr r0, =ttb_first_level ; set start of Translation Table base (16k Boundary)
mcr p15, 0x00, r0, c2, c0, 0x00; write to CP15 register 2
;
; Create translation table for flat mapping
; Top 12 bits of VA is pointer into table
; Create 4096 entries from 000xxxxx to fffxxxxx
;
mov r1, #0x00 ; loop counter
ldr r2, =0xDF2 ; set access permissions (AP) for full access SVC/USR (11:10)
; set for domain 15 (8:5)
; must be 1 (4)
; set non cachable non bufferable (CB) (3:2)
; set for 1Mb section (1:0)
init_ttb0
orr r3, r2, r1, lsl #0x14
ldr r4, =sections
init_ttb1
ldr r5, [r4], #0x04
tst r5, #0x01
bne init_ttb2
cmp r1, r5, lsr #0x14
addlo r4, r4, #0x08
blo init_ttb1
ldr r5, [r4], #0x04
cmp r1, r5, lsr #0x14
addhs r4, r4, #0x04
bhs init_ttb1
ldr r5, [r4]
orr r3, r3, r5
init_ttb2
str r3, [r0, r1, lsl #0x02]
add r1, r1, #0x01
cmp r1, #0x1000
blo init_ttb0
;
; Init domains
;
mov r0, #(0x1 << 0x1e) ; must define behaviour for domain 15 (31:30), set client
mcr p15, 0x00, r0, c3, c0, 0x00; write to CP15 register 3
;
; Set global core configurations
;
mrc p15, 0x00, r0, c1, c0, 0x00; read CP15 register 1 into r0

orr r0, r0, #(0x01 << 0x0c) ; enable I Cache
orr r0, r0, #(0x01 << 0x02) ; enable D Cache
orr r0, r0, #0x01 ; enable MMU
;
; Additional configuration options
;
; orr r0, r0, #(0x01 << 0x0e) ; enable Round Robin cache replacement
; orr r0, r0, #(0x01 << 0x0d) ; enable Hi Vectors
orr r0, r0, #(0x01 << 0x01) ; enable data address alignment checking

mcr p15, 0x00, r0, c1, c0, 0x00; write cp15 register 1
nop
nop
nop
nop
nop
nop
nop
nop
bx lr

; ***************************************************************************
; *

sections
DCD 0x00000000 ; SRAM
DCD 0x00100000
DCD 0xC ; wb, buffered

DCD 0x70000000 ; DDRAM
DCD 0x72000000
DCD 0xC ; wb, buffered

DCD 0x00000001 ; END

; ***************************************************************************
; *

ttb_first_level EQU 0x70000000

#include "cp15.h" //Atmel at91lib

#define SRAM_ADDRESS AT91C_IRAM

#define TLB_NCNB 0xDF2 // Noncachable, Nonbufferable 11 0 1111 1 00 10
#define TLB_WT 0xDFA // Write-through 11 0 1111 1 10 10
#define TLB_WB 0xDFE // Write-back 11 0 1111 1 11 10

static unsigned char *BufMMU = (unsigned char *) (SRAM_ADDRESS + 0x4000);

void InitMMU(unsigned int *pTranslationTable)
{
int i;
int addSRAM;

// Program the TTB
TRACE_DEBUG("TTB = 0x%X\n\r", (unsigned int)pTranslationTable);

CP15_WriteTTB((unsigned int)pTranslationTable);

// Program the domain access register
CP15_WriteDomainAccessControl(0xFFFFFFFF); // domain 15: access are not checked

// Reset table entries
for (i = 0; i < 4096; i++) {
pTranslationTable[i] = 0;
}
// Program level 1 page table entry

// Vector adress
pTranslationTable[0x0] = (0x00000000)|TLB_NCNB;
// SRAM adress (with D cache)
addSRAM = (SRAM_ADDRESS >> 20);
TRACE_DEBUG("addSRAM = 0x%X\n\r", addSRAM);
TRACE_DEBUG("SRAM_ADDRESS = 0x%X\n\r", SRAM_ADDRESS);
pTranslationTable[addSRAM] = (SRAM_ADDRESS)|TLB_WB;
//DDRAM buf1 with write-through
pTranslationTable[0x701] = (AT91C_DDR2 + 0x00100000)|TLB_WT;
//DDRAM buf2 with write-through
pTranslationTable[0x702] = (AT91C_DDR2 + 0x00200000)|TLB_WT;
// Peripherals adress
pTranslationTable[0xFFF] = (0xFFF00000)|TLB_NCNB;

CP15_EnableMMU();
CP15_EnableDcache();
CP15_EnableIcache();

}

int main()
{
//somewhere in init
InitMMU((unsigned int*)BufMMU);
}