fighting with interrupt vectors

diff --git a/Makefile b/Makefile
index bb0b2bc..0b0cb54 100644 (file)
--- 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 (file)
--- 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 (file)
--- a/src/intr.c
+++ b/src/intr.c
@@ -1,6 +1,43 @@
+#include <stdio.h>
+#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 (file)
--- a/src/intr.h
+++ b/src/intr.h
@@ -1,6 +1,21 @@
 #ifndef INTR_H_
 #define INTR_H_
 
+#include <stdint.h>
+
+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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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_ */