csd_asm.S

#include "csd_zynq_peripherals.h"
#define SWITCH_ADDR 0x41210000

.extern csd_main

.section .csd_boot,"ax"

.align 8
// Our interrupt vector table
csd_entry:
	b csd_reset
	b .
	b .
	b .
	b .
	b .
	b csd_irq
	b .

.global main
csd_reset:
main:

	// Set VBAR (Vector Base Address Register) to my own interrupt vectors
    ldr     r0, =csd_entry
    mcr     p15, 0, r0, c12, c0, 0
    dsb
    isb

	// Read Cache Type Register (CTR)
	mrc p15, 0, r1, c0, c0, 1

	// Read Cache Level ID Register (CLIDR)
	mrc p15, 1, r2, c0, c0, 1

	ldr r7, =SWITCH_ADDR // load switch value
	ldr r8, [r7]

	cmp r8, #0

	beq disable_cache
	bne enable_cache


	disable_cache:
		@------------------------
		@ Disable Caches (L2)
		@------------------------
		ldr r0, =L2_reg1_ctrl
	    mov r1, #0x0
	    str r0, [r0]
		@------------------------
		@ Disable Caches (IL1, DL1)
		@------------------------
		mrc		p15, 0, r0, c1, c0, 0	@ read control register (CP15 register1)
		bic		r0, r0, #4096		    @ disable I bit (Instruction Cache)
		bic		r0, r0, #4		        @ disable C bit (Data and Unified Caches)
		mcr		p15, 0, r0, c1, c0, 0	@ write control register (CP15 register2)

		// read SCTLR (System Control Register) to r0
		mrc	p15, 0, r0, c1, c0, 0

		bl  csd_main

		b main

	enable_cache:
		@------------------------
		@ Enable Caches (L2)
		@------------------------
		ldr r0, =L2_reg1_ctrl
	    mov r1, #0x1
	    str r0, [r0]

		@------------------------
		@ Enable Caches (IL1, DL1)
		@------------------------
		mrc		p15, 0, r0, c1, c0, 0	@ read control register (CP15 register1)
		orr		r0, r0, #(1<<12)	    @ Enable I bit (Instruction Cache)
		orr		r0, r0, #(1<<2)         @ Enable C bit (Data and Unified Caches)
		mcr		p15, 0, r0, c1, c0, 0	@ write control register (CP15 register2)

		b main


forever:
	nop
	b forever

// Normal Interrupt Service Routine
csd_irq:
	b .

.end