Problem disabling cache on ARM926ej-s

I've been using mmu.c to setup my MMU table
and enable the caches - for months now,
so I think that part is OK.

Now I need to disable the icache, dcache and mmu,
do some stuff and re-enable them.

But when I disable the cache, the prog starts to
behave like it has a will of its own.

If I don't disable the cache, then it works normally.


Please take a look...


// creates a mmu level 1 table with flat address space.

#define not_cacheable (1 << 1) // C=B=0 (bit 1 must be set = section 
descriptor)
#define     cacheable (7 << 1) // C=B=1 (bit 1 must be set = section 
descriptor)

#define SDRAMsize (32*M)

#define SDRAMstart  (AT91C_EBI_SDRAM >> 20)
#define SDRAMend   ((AT91C_EBI_SDRAM + SDRAMsize) >> 20)

static void mmu (void) {
    uint f;
    uint *pSDRAM = (uint*) AT91C_EBI_SDRAM;

    // create table
        // setup all linear addresses, AP(3), domain(0), bit 4(1)
        // as non-cacheable and as invalid entry
        for (f = 0; f <= 4095; f++)
            pSDRAM[f] = (f << 20) | (3 << 10) | (1 << 4);

        // 1st MB is not user accesible
        pSDRAM[0] &= (DWORD) ~(1 << 11);

        // 1st 6MB are not cacheable
        for (f = 0; f <= 5; f++)
            pSDRAM[f] |= not_cacheable;

        // peripherals are not cacheable
        pSDRAM[4095] |= not_cacheable;

        // EBI 6 not cacheable
        pSDRAM[AT91C_EBI_CS6 >> 20] |= not_cacheable;

        // whole SDRAM is cacheable
        for (f = SDRAMstart; f < SDRAMend; f++)
            pSDRAM[f] |= cacheable;

    // TTBR
        asm("LDR R0, =0x20000000");
        asm("MCR P15, 0, R0, C2, C0, 0");

    //assert DACR.D0 (client mode)
        asm("MRC P15, 0, R0, C3, C0, 0");
        asm("BIC R0, R0, #3");
        asm("ORR R0, R0, #1");
        asm("MCR P15, 0, R0, C3, C0, 0");

    // Enable Icache(12), Dcache(2), Alignment(1) and MMU(0)
        asm("MRC P15, 0, R0, C1, C0, 0");
        asm("ORR R0, R0, #4096");
        asm("ORR R0, R0, #7");
        asm("MCR P15, 0, R0, C1, C0, 0");
}




This is in sram0, called from user mode with SVC instruction.

disablecache:   mov r0,#0                       @ drain write buffer
                mcr p15, 0, r0, c7, c10, 4

                mcr p15, 0, r0, c7, c6, 0       @ invalidate dcache

                mrc p15, 0, r0, c1, c0, 0       @ Disable Icache(12), 
Dcache(2) and MMU(0)
                bic r0, r0, #4096
                bic r0, r0, #5
                mcr p15, 0, r0, c1, c0, 0

                mov pc,lr                       @ return to SWI handler



When I'm done, I re-enable the cache (but the prog never reaches here)

enablecache:    mrc p15, 0, r0, c1, c0, 0       @ Enable  Icache(12), 
Dcache(2), Alignment(1) and MMU(0)
                orr r0, r0, #4096
                orr r0, r0, #7
                mcr p15, 0, r0, c1, c0, 0

                mov pc,lr