From 38a008b8d1a3a20c401397b4e132fb578e5f0c10 Mon Sep 17 00:00:00 2001 From: John Tsiombikas Date: Tue, 17 Nov 2020 00:59:06 +0200 Subject: [PATCH] fighting with interrupt vectors --- Makefile | 2 +- rpikern.ld | 6 ++-- src/intr.c | 45 +++++++++++++++++++++++++++--- src/intr.h | 15 ++++++++++ src/intrasm.s | 86 ++++++++++++++++++++++++++++++++++++++++++++------------- src/main.c | 7 ++++- src/startup.s | 26 +++++++++++++++-- src/sysctl.h | 11 ++++++++ 8 files changed, 167 insertions(+), 31 deletions(-) diff --git a/Makefile b/Makefile index bb0b2bc..0b0cb54 100644 --- a/Makefile +++ b/Makefile @@ -32,7 +32,7 @@ QEMU_FLAGS = -vnc :0 -m 1024 -M raspi2 -serial stdio -d guest_errors $(bin): $(elf) $(OBJCOPY) -O binary $< $@ -$(elf): $(obj) +$(elf): $(obj) rpikern.ld $(LD) -o $@ $(obj) -Map link.map $(LDFLAGS) -include $(dep) diff --git a/rpikern.ld b/rpikern.ld index ddec699..9e344b1 100644 --- a/rpikern.ld +++ b/rpikern.ld @@ -1,8 +1,4 @@ SECTIONS { - .vect : { - * (.vect*); - } - . = 0x00008000; _stacktop = .; _kern_start = .; @@ -11,6 +7,7 @@ SECTIONS { KEEP(* (.startup*)); * (.text*); } + .rodata ALIGN(4): { * (.rodata*); } .data ALIGN(4): { * (.data*); } @@ -22,6 +19,7 @@ SECTIONS { } _bss_size = SIZEOF(.bss); + . = ALIGN(4); _kern_size = . - _kern_start; _mem_start = .; } diff --git a/src/intr.c b/src/intr.c index f9b0f1f..304aefd 100644 --- a/src/intr.c +++ b/src/intr.c @@ -1,6 +1,43 @@ +#include +#include "intr.h" #include "rpi.h" #include "rpi_ioreg.h" #include "asm.h" +#include "sysctl.h" +#include "debug.h" + +void startup(); +void intr_entry_nop(); +void intr_entry_irq(); + +void intr_init(void) +{ + /* setup interrupt vectors */ + setvect(INTR_RESET, (uint32_t)startup); + setvect(INTR_UNDEF, (uint32_t)intr_entry_nop); + setvect(INTR_SWI, (uint32_t)intr_entry_nop); + setvect(INTR_IABORT, (uint32_t)intr_entry_nop); + setvect(INTR_DABORT, (uint32_t)intr_entry_nop); + setvect(INTR_IRQ, (uint32_t)intr_entry_irq); + setvect(INTR_FIQ, (uint32_t)intr_entry_irq); + + printf("Exception vectors:\n"); + hexdump(0, 32); +} + +void setvect(int idx, uint32_t addr) +{ + uint32_t *ivec = 0; + uint32_t pc = (idx << 2) + 8; + + printf("setvect(%d, %lx)\n", idx, (unsigned long)addr); + + /* construct branch instruction */ + ivec[idx] = 0xea000000 | (addr - pc) >> 2; + + /* we also probably need to invalidate the instr. cache */ + sysctl_icache_inval(0, 32); +} void enable_arm_irq(int irq) { @@ -27,9 +64,9 @@ void disable_arm_irq(int irq) void enable_gpu_irq(int irq) { mem_barrier(); - if(irq == IRQ_GPU_TIMER1) { + /*if(irq == IRQ_GPU_TIMER1) { IRQ_FIQCTL_REG = IRQ_FIQCTL_SELGPU(IRQ_GPU_TIMER1) | IRQ_FIQCTL_ENABLE; - } else if(irq < 32) { + } else */if(irq < 32) { IRQ_ENABLE1_REG |= 1 << irq; } else { IRQ_ENABLE2_REG |= 1 << (irq - 32); @@ -40,9 +77,9 @@ void enable_gpu_irq(int irq) void disable_gpu_irq(int irq) { mem_barrier(); - if(irq == IRQ_GPU_TIMER1) { + /*if(irq == IRQ_GPU_TIMER1) { IRQ_FIQCTL_REG = 0; - } else if(irq < 32) { + } else */if(irq < 32) { IRQ_ENABLE1_REG &= ~(1 << irq); } else { IRQ_ENABLE2_REG &= ~(1 << (irq - 32)); diff --git a/src/intr.h b/src/intr.h index c3e7284..6f28787 100644 --- a/src/intr.h +++ b/src/intr.h @@ -1,6 +1,21 @@ #ifndef INTR_H_ #define INTR_H_ +#include + +enum { + INTR_RESET, + INTR_UNDEF, + INTR_SWI, + INTR_IABORT, + INTR_DABORT, + INTR_IRQ, + INTR_FIQ +}; + +void intr_init(void); +void setvect(int idx, uint32_t addr); + void enable_arm_irq(int irq); void disable_arm_irq(int irq); diff --git a/src/intrasm.s b/src/intrasm.s index 4ef2cd0..874520f 100644 --- a/src/intrasm.s +++ b/src/intrasm.s @@ -1,28 +1,76 @@ .code 32 - .section .vect - .extern startup -intr_vector: - b startup - b intr_entry_undef - b intr_entry_swi - b intr_entry_iabort - b intr_entry_dabort - b intr_entry_irq - - @ FIQ entry point used for timer interrupts - .extern num_ticks -intr_entry_fiq: - ldr r9, =num_ticks - ldr r8, [r9] - add r8, #1 - str r8, [r9] - subs pc, lr, #4 - +@ .section .vect,"a" +@ .extern startup +@intr_vector: +@ b startup +@ b intr_entry_undef +@ b intr_entry_swi +@ b intr_entry_iabort +@ b intr_entry_dabort +@ b intr_entry_irq +@ +@ @ FIQ entry point used for timer interrupts +@intr_entry_fiq: +@ ldr r9, =num_ticks +@ ldr r8, [r9] +@ add r8, #1 +@ str r8, [r9] +@ subs pc, lr, #4 +@ .align 8 +@ .ascii "xyzzy" +@ .text + + .global intr_entry_nop + .global intr_entry_undef + .global intr_entry_swi + .global intr_entry_iabort + .global intr_entry_dabort + .global intr_entry_irq + .global intr_entry_fiq + intr_entry_undef: intr_entry_swi: intr_entry_iabort: intr_entry_dabort: +intr_entry_nop: + subs pc, lr, #4 + intr_entry_irq: + ldr sp, =_stacktop @ HACK + + stmfd sp!, {r0, r1} + + mov r0, #'.' + .extern ser_putchar + bl ser_putchar + + ldr r1, =num_ticks + ldr r0, [r1] + add r0, #1 + str r0, [r1] + + @ setup next interrupt + ldr r1, =#0x3f003004 @ low counter in rpi2 TODO use rpi_iobase + ldr r0, [r1] + add r0, #4000 @ 1Mhz / 250hz = 4k counts + ldr r1, =#0x3f003010 @ compare 1 reg + str r0, [r1] + + @ clear interrupt + ldr r1, =#0x3f003000 + mov r0, #2 @ set bit 1 to clear interrupt from C1 + str r0, [r1] + + ldmfd sp!, {r0, r1} + subs pc, lr, #4 + +intr_entry_fiq: + ldr r9, =num_ticks + ldr r8, [r9] + add r8, #1 + str r8, [r9] subs pc, lr, #4 + +@ vi:set ft=armasm: diff --git a/src/main.c b/src/main.c index 18ed725..39a44d9 100644 --- a/src/main.c +++ b/src/main.c @@ -24,6 +24,10 @@ int main(void) static char cmdbuf[256]; static int cmdend; + unsigned int cpsr; + asm volatile("mrs %0, cpsr" : "=r"(cpsr)); + printf("CPSR: %x (mode: %x)\n", cpsr, cpsr & 0x1f); + rpi_init(); /*init_serial(115200); done in rpi_init now for early debugging */ con_init(); @@ -32,9 +36,10 @@ int main(void) printf("Main RAM base: %x, size: %u bytes\n", rpi_mem_base, rpi_mem_size); printf("Video RAM base: %x, size: %u bytes\n", rpi_vmem_base, rpi_vmem_size); - timer_init(); video_init(); + timer_init(); + intr_init(); enable_intr(); printf("Going interactive\n"); diff --git a/src/startup.s b/src/startup.s index 587ab95..84e7e94 100644 --- a/src/startup.s +++ b/src/startup.s @@ -5,13 +5,35 @@ .global startup startup: + cpsid if + @ stop all but one of the cores mrc p15, 0, r0, c0, c0, 5 ands r0, r0, #0xff bne exit - @ setup stack - ldr sp, =_stacktop + @ detect if we're running in hyp mode, and drop to svc + mrs r0, cpsr + and r1, r0, #0x1f + cmp r1, #0x1a + bne hypend + + bic r0, #0x1f + orr r0, #0x13 + @msr spsr_cxsf, r0 + add r0, pc, #4 + msr elr_hyp, r0 + @eret + mov pc, r0 +hypend: + + @ setup initial stacks, allow 4k stack for IRQs + @mov r0, #0x12 @ switch to IRQ mode + @msr cpsr, r0 + @ldr sp, =_stacktop + @mov r0, #0x13 @ switch to supervisor mode + @msr cpsr, r0 + ldr sp, =_stacktop - 4096 @ clear bss ldr r0, =_bss_start diff --git a/src/sysctl.h b/src/sysctl.h index 8d9634a..e5b75d5 100644 --- a/src/sysctl.h +++ b/src/sysctl.h @@ -14,5 +14,16 @@ : "memory"); \ } while(0) +#define sysctl_icache_inval(addr, len) \ + do { \ + register uint32_t a asm("r0") = addr; \ + asm volatile( \ + "\n0:\tmcr p15, 0, %0, c7, c5, 1" \ + "\n\tadd %0, #64" \ + "\n\tcmp %0, %1" \ + "\n\tblo 0b" \ + :: "r"(a), "r"(addr + len) \ + : "memory"); \ + } while(0) #endif /* SYSCTL_H_ */ -- 1.7.10.4