X-Git-Url: http://git.mutantstargoat.com/user/nuclear/?p=rpikern;a=blobdiff_plain;f=src%2Fintr.c;fp=src%2Fintr.c;h=304aefdb6cad809dcdb330d6c9661dca96a717b2;hp=f9b0f1f17d107963a035db42c74cc08da37462fc;hb=38a008b8d1a3a20c401397b4e132fb578e5f0c10;hpb=d80ebb8add60dd01b37d21c21a1a9d971f0f9dce

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 <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));