Setting up paging in raspberry pi 3 model B in 32 bit mode

Question

I am trying to setup paging in rpi 3b in 32 bit mode.

First of all I am aiming for identity mapping all kernel code, so I started with short descriptor format using 1 MB sections.

Identity mapping works if I map all the RAM, but I think that is overkill as my kernel end is located at 0x15000 so it should be covered in first 1MB area. I am not sure what is that I am missing here. I have checked ARM ARM/TRM programmers guide but could not find anything fruitful to continue debugging.

Heres boot.s which contains assembly code for set up

    // To keep this in the first portion of the binary.
.section ".text.boot"

// Make _start global.
.globl _start
.global _get_stack_pointer
.global _exception_table
.global _enable_interrupts

.equ ARM_MODE_USR, 0x10;                                ;@ CPU in USR mode .. Normal User mode
.equ ARM_MODE_FIQ, 0x11;                                ;@ CPU in FIQ mode .. FIQ Processing
.equ ARM_MODE_IRQ, 0x12;                                ;@ CPU in IRQ mode .. IRQ Processing
.equ ARM_MODE_SVC, 0x13;                                ;@ CPU in SVC mode .. Service mode
.equ ARM_MODE_HYP, 0x1A;                                ;@ CPU in HYP mode .. Hypervisor mode  (ARM7/ARM8 only)
.equ ARM_MODE_UND, 0x1B;                                ;@ CPU in UND mode .. Undefined Instructions mode
.equ ARM_MODE_SYS, 0x1F;    

.equ ARM_MODE_MASK, 0x1F;                               ;@ Mask to clear all but CPU mode bits from cpsr register
.equ ARM_I_BIT,     0x80;                               ;@ IRQs disabled when set to 1
.equ ARM_F_BIT,     0x40;

.equ    CPSR_MODE_USER,         0x10
.equ    CPSR_MODE_FIQ,          0x11
.equ    CPSR_MODE_IRQ,          0x12
.equ    CPSR_MODE_SVR,          0x13
.equ    CPSR_MODE_ABORT,        0x17
.equ    CPSR_MODE_UNDEFINED,    0x1B
.equ    CPSR_MODE_SYSTEM,       0x1F

// See ARM section A2.5 (Program status registers)
.equ    CPSR_IRQ_INHIBIT,       0x80
.equ    CPSR_FIQ_INHIBIT,       0x40
.equ    CPSR_THUMB,             0x20

.equ    SCTLR_ENABLE_DATA_CACHE,        0x4
.equ    SCTLR_ENABLE_BRANCH_PREDICTION, 0x800
.equ    SCTLR_ENABLE_INSTRUCTION_CACHE, 0x1000


.balign 4

_start:
    ldr pc, _reset_h
    ldr pc, _undefined_instruction_vector_h
    ldr pc, _software_interrupt_vector_h
    ldr pc, _prefetch_abort_vector_h
    ldr pc, _data_abort_vector_h
    ldr pc, _unused_handler_h
    ldr pc, _interrupt_vector_h
    ldr pc, _fast_interrupt_vector_h

    _reset_h:                           .word   _reset_
    _undefined_instruction_vector_h:    .word   undefined_instruction_vector
    _software_interrupt_vector_h:       .word   software_interrupt_vector
    _prefetch_abort_vector_h:           .word   prefetch_abort_vector
    _data_abort_vector_h:               .word   data_abort_vector_asm
    _unused_handler_h:                  .word   _reset_
    _interrupt_vector_h:                .word   irq_handler_asm_wrapper
    _fast_interrupt_vector_h:           .word   fast_interrupt_vector


_reset_:
    mrc p15, 0, r6,c0,c0,5
    and     r6, r6, #3
    mov r7, #0
    cmp     r6, r7
    beq 2f
    // cpu id > 0, stop
1:  wfe
    b       1b
2:  // cpu id == 0


    mov r12, pc                                         ;@ Hold boot address in high register R12
    mrs r0, CPSR                                        ;@ Fetch the cpsr register
    orr r0, r0, #(ARM_I_BIT | ARM_F_BIT)                ;@ Disable Irq/Fiq
    and r11, r0, #ARM_MODE_MASK                         ;@ Clear all but CPU mode bits in register r11

    cmp r11, #ARM_MODE_HYP                              ;@ Check we are in HYP_MODE                                         
    bne .NotInHypMode                                   ;@ Branch if not equal meaning was not in HYP_MODE  
    bic r0, r0, #ARM_MODE_MASK                          ;@ Clear the CPU mode bits in register r0                           
    orr r0, r0, #ARM_MODE_SVC                           ;@ ARM_MODE_SVC bits onto register  
    msr spsr_cxsf,r0                                    ;@ Hold value in spsr_cxsf
    add lr,pc,#4                                        ;@ Calculate address of .NotInHypMode label

    msr ELR_hyp, lr
    eret

.NotInHypMode:
    mov r0,#0x8000
    mov r1,#0x0000
    ldmia r0!,{r2,r3,r4,r5,r6,r7,r8,r9}
    stmia r1!,{r2,r3,r4,r5,r6,r7,r8,r9}
    ldmia r0!,{r2,r3,r4,r5,r6,r7,r8,r9}
    stmia r1!,{r2,r3,r4,r5,r6,r7,r8,r9}

    ;@ (PSR_IRQ_MODE|PSR_FIQ_DIS|PSR_IRQ_DIS)
    mov r0,#0xD2
    msr cpsr_c,r0
    mov sp,#0x3000

    ;@ (PSR_FIQ_MODE|PSR_FIQ_DIS|PSR_IRQ_DIS)
    ;@mov r0,#0xD1
    ;@msr cpsr_c,r0
    ;@mov sp,#0x000

    ;@ (PSR_SVC_MODE|PSR_FIQ_DIS|PSR_IRQ_DIS)
    mov r0,#0xD3
    msr cpsr_c,r0
    mov sp,#0x8000000

    // enable unaligned address access
    mrc p15, 0, r0, c1, c0, 0
    orr r0, #1 << 22
    mcr p15, 0, r0, c1, c0, 0

    // Enable VFP ------------------------------------------------------------

    // r1 = Access Control Register
    MRC p15, #0, r1, c1, c0, #2
    // enable full access for p10,11
    ORR r1, r1, #(0xf << 20)
    // ccess Control Register = r1
    MCR p15, #0, r1, c1, c0, #2
    MOV r1, #0
    // flush prefetch buffer because of FMXR below
    MCR p15, #0, r1, c7, c5, #4
    // and CP 10 & 11 were only just enabled
    // Enable VFP itself
    MOV r0,#0x40000000
    // FPEXC = r0
    FMXR FPEXC, r0


    // Call clear-bss.c
    bl _clear_bss


    // halt
_inf_loop:
    b       _inf_loop

.globl PUT32
PUT32:
    str r1,[r0]
    bx lr

.globl GET32
GET32:
    ldr r0,[r0]
    bx lr

.globl dummy
dummy:
    mov     pc, lr

.globl enable_irq
enable_irq:
    mrs r0,cpsr
    bic r0,r0,#0x80
    msr cpsr_c,r0
    bx lr

_get_stack_pointer:
    // Return the stack pointer value
    str     sp, [sp]
    ldr     r0, [sp]

    // Return from the function
    mov     pc, lr


_enable_interrupts:
    mrs     r0, cpsr
    bic     r0, r0, #0x80
    msr     cpsr_c, r0
    mov     pc, lr

irq_handler_asm_wrapper:
    sub     lr, lr, #4
    srsdb   sp!, #0x13
    cpsid   if, #0x13
    push    {r0-r3, r12, lr}
    and     r1, sp, #4
    sub     sp, sp, r1
    push    {r1}
    bl      irq_handler
    pop     {r1}
    add     sp, sp, r1
    pop     {r0-r3, r12, lr}
    rfeia   sp!

.globl start_mmu
start_mmu:
    mov r2,#0
    mcr p15,0,r2,c8,c7,0 ;@ invalidate tlb
    mcr p15,0,r2,c7,c10,4 ;@ DSB ??

    ;@ Domain 0 Manager access. No permissions checked for access
    MRC p15, 0, r2, c3, c0, 0 ;@ Read DACR into Rt
    ORR r2, #3
    MCR p15, 0, r2, c3, c0, 0 ;@ Write Rt to DACR

    MCR p15,0,r0,c2,c0,0 ;@ Write Rt to TTBR0
    MCR p15, 0, r0, c2, c0, 1 ;@ Write Rt to TTBR1

    MRC p15, 0, r2, c1, c0, 0 ;@ Read SCTLR into Rt
    orr r2,r2,r1
    MCR p15, 0, r2, c1, c0, 0 ;@ Write Rt to SCTLR.

    bx lr

data_abort_vector_asm:
    mov r6,lr
    ldr r8,[r6,#-8]
    mrc p15,0,r4,c5,c0,0 ;@ data/combined
    mrc p15,0,r5,c5,c0,1 ;@ instruction
    mov sp,#0x00004000
    bl hexstrings
    mov r0,r4
    bl hexstrings
    mov r0,r5
    bl hexstrings
    mov r0,r6
    bl hexstrings
    mov r0,r8
    bl hexstrings
    mov r0,r7
    bl hexstrings
123:
    b 123b

.globl stop_mmu
stop_mmu:
    mrc p15,0,r2,c1,c0,0
    bic r2,#0x1000
    bic r2,#0x0004
    bic r2,#0x0001
    mcr p15,0,r2,c1,c0,0
    bx lr

.globl invalidate_tlbs
invalidate_tlbs:
    mov r2,#0
    mcr p15,0,r2,c8,c7,0  ;@ invalidate tlb
    mcr p15,0,r2,c7,c10,4 ;@ DSB ??
    bx lr

.globl read_cpu_id
read_cpu_id:
    // read cpu id, stop slave cores
    // mrc p15, 0, r0, c0, c0, 0
    mrc p15, 0, r0,c0,c0,5
    bx lr

-- identity mapping code----

    #include <kernel/virtmem.h>
#include <kernel/uart0.h>

void initialize_virtual_memory(void)
{
    // Works when this is not commented
    // unsigned int ra;
    // for (ra = 0;; ra += 0x00100000)
    // {
    //     mmu_section(ra, ra, 0x0000);
    //     if (ra == 0xFFF00000)
    //         break;
    // }

    // Mapping first 1MB should be sufficient , this doesn't work
    mmu_section(0x00000000, 0x00000000, 0x0000);
    mmu_section(0x00100000, 0x00000000, 0x0000);

    //peripherals
    mmu_section(0x3f000000, 0x3f000000, 0x0000); //NOT CACHED!
    mmu_section(0x3f200000, 0x3f200000, 0x0000); //NOT CACHED!

    printf("Enabling MMU \n ");
    start_mmu(MMUTABLEBASE, 0x00800001);
}

unsigned int mmu_section(unsigned int vadd, unsigned int padd, unsigned int flags)
{
    unsigned int table1EntryOffset;
    unsigned int table1EntryAddress;
    unsigned int tableEntry;

    table1EntryOffset = (vadd >> 20) << 2; // get only most significant 12 bits
    //and multiply it by 4 as each entry is 4 Bytes 32bits

    // MMU table base should be at 16KB granularity, Least signficant 12 bits will be always 0. hence do OR with that
    table1EntryAddress = MMUTABLEBASE | table1EntryOffset;

    // 31: 20  12 bits are physical 12 ms bits from physical address
    tableEntry = (padd & 0xFFF00000);

    // entry[1:0] = 0b10 for section entry
    tableEntry = tableEntry | 2;

    // Access permissions should be 11 for full access entry [11:10] = 0b11
    tableEntry = tableEntry | 0xC00;

    //hexstrings(rb); hexstring(rc);
    // printf("\n entryAddr: 0x%x, entry value:0x%x \n", table1EntryAddress, tableEntry);
    PUT32(table1EntryAddress, tableEntry);
    return (0);
}

---Linker script ----

    ENTRY(_start)

SECTIONS
{
    /* Starts at LOADER_ADDR. */
    . = 0x8000;
    __start = .;
    __text_start = .;
    .text :
    {
        KEEP(*(.text.boot))
        *(.text)
    }
    . = ALIGN(4096); /* align to page size */
    __text_end = .;

    __rodata_start = .;
    .rodata :
    {
        *(.rodata)
    }
    . = ALIGN(4096); /* align to page size */
    __rodata_end = .;

    __data_start = .;
    .data :
    {
        *(.data)
    }
    . = ALIGN(4096); /* align to page size */
    __data_end = .;

    __bss_start = .;
    .bss :
    {
        bss = .;
        *(.bss)
    }
    . = ALIGN(4096); /* align to page size */
    __bss_end = .;
    __end = .;
}

I am hoping for some direction to progress my debugging here. Linked issue in RPI forum which nobody answered till now. RPI forum post