fernvale: Get IRQs to at least do something

IRQs now do something.  They still don't work, though.

Makefile

@@ -8,14 +8,17 @@ LDFLAGS = --nostdlib -T fernvale.ld
 LIBS =
 
 SRC_C = \
-	main.c \
-	vectors.c \
-	serial.c \
 	bionic.c \
-	vsprintf.c \
-	utils.c
+	cmd-irq.c \
+	irq.c \
+	main.c \
+	serial.c \
+	utils.c \
+	vectors.c \
+	vsprintf.c
 
 SRC_S = \
+	irqasm.S \
 	start.S
 
 OBJ = $(addprefix $(BUILD)/, $(SRC_S:.S=.o) $(SRC_C:.c=.o))

cmd-irq.c

@@ -0,0 +1,52 @@
+#include <string.h>
+
+#include "bionic.h"
+#include "irq.h"
+#include "printf.h"
+
+static void print_help(void)
+{
+	printf("Usage:\n");
+	printf("irq sim [num]      Simulate IRQ [num]\n");
+	printf("irq enable [num]   Enable IRQ [num]\n");
+	printf("irq disable [num]  Disable IRQ [num]\n");
+}
+
+int cmd_irq(int argc, char **argv)
+{
+	int num;
+
+	if (argc != 2) {
+		print_help();
+		return -1;
+	}
+
+	num = _strtoul(argv[1], NULL, 0);
+	if (num >= __irq_max__) {
+		printf("Only %d IRQs present\n", __irq_max__);
+		return -1;
+	}
+
+	if (!_strcasecmp(argv[0], "sim")) {
+		printf("Simulating IRQ %d\n", num);
+		irq_stimulate(num);
+	}
+
+	else if (!_strcasecmp(argv[0], "enable")) {
+		printf("Enabling IRQ %d\n", num);
+		irq_enable(num);
+	}
+
+	else if (!_strcasecmp(argv[0], "disable")) {
+		printf("Disabling IRQ %d\n", num);
+		irq_disable(num);
+	}
+
+	else {
+		printf("Unknown command\n");
+		print_help();
+		return -1;
+	}
+
+	return 0;
+}

fernvale.ld

@@ -39,7 +39,7 @@ SECTIONS
 {
 	/* The OS entry point is here */
 
-	. = 0x00002000; /* bootloader will copy data to this address */
+	. = 0x00003460; /* bootloader will copy data to this address */
 	.text : {
 		_stext = ABSOLUTE(.);
 		KEEP(*(vectors))

include/irq.h

@@ -0,0 +1,84 @@
+#ifndef __IRQ_H__
+#define __IRQ_H__
+
+enum irq_number {
+	irq_unk_0,
+	irq_unk_1,
+	irq_unk_2,
+	irq_unk_3,
+	irq_unk_4,
+	irq_unk_5,
+	irq_unk_6,
+	irq_unk_7,
+	irq_unk_8,
+	irq_unk_9,
+	irq_unk_10,
+	irq_unk_11,
+	irq_unk_12,
+	irq_unk_13,
+	irq_unk_14,
+	irq_uart1,
+	irq_unk_16,
+	irq_uart2,
+	irq_unk_18,
+	irq_unk_19,
+	irq_unk_20,
+	irq_unk_21,
+	irq_unk_22,
+	irq_unk_23,
+	irq_unk_24,
+	irq_unk_25,
+	irq_unk_26,
+	irq_unk_27,
+	irq_unk_28,
+	irq_unk_29,
+	irq_unk_30,
+	irq_unk_31,
+	irq_unk_32,
+	irq_unk_33,
+	irq_unk_34,
+	irq_unk_35,
+	irq_unk_36,
+	irq_unk_37,
+	irq_unk_38,
+	irq_unk_39,
+	irq_unk_40,
+	irq_unk_41,
+	irq_unk_42,
+	irq_unk_43,
+	irq_unk_44,
+	irq_unk_45,
+	irq_unk_46,
+	irq_unk_47,
+	__irq_max__,
+};
+
+int irq_init(void);
+int fiq_init(void);
+int irq_enable(enum irq_number irq_num);
+int irq_disable(enum irq_number irq_num);
+void irq_acknowledge(enum irq_number irq_num);
+void irq_mask_acknowledge(enum irq_number irq_num);
+void irq_dispatch(void);
+void irq_stimulate(enum irq_number irq_num);
+void irq_stimulate_reset(enum irq_number irq_num);
+
+void irq_register_handler(enum irq_number irq_num,
+		void (*handler)(enum irq_number irq_num, void *opaque),
+		void *opaque);
+
+#define IRQ_BASE (0xa0060000)
+#define IRQ_MASK_OFF 0x00	/* IRQ mask (enabled/disabled) */
+#define IRQ_SENSE_OFF 0x60	/* IRQ sensitivity (edge vs level) */
+#define IRQ_STIM_OFF 0xc0	/* IRQ "stimulate" (for debug) */
+#define IRQ_STATUS_OFF 0x100	/* IRQ status (IRQ firing or not) */
+#define FIQ_STATUS_OFF 0x140	/* FIQ status (FIQ firing or not) */
+#define IRQ_ACK_OFF 0x160	/* IRQ acknowledge (write a 1 to acknowledge) */
+#define FIQ_SOURCE_OFF 0x180	/* IRQ number used as FIQ */
+
+#define IRQ_SET 0x20		/* Offset from OFF for "SET" behaviour */
+#define IRQ_CLR 0x40		/* Offset from OFF for "CLR" behaviour */
+
+#define IRQ_NUM_ADJ(x) (((x) > 32) ? 4 : 0) /* Offset for IRQs > 32 */
+
+#endif /* __IRQ_H__ */

include/memio.h

@@ -0,0 +1,35 @@
+#ifndef __MEMIO_H__
+#define __MEMIO_H__
+#include <stdint.h>
+
+inline void writeb(uint8_t value, uint32_t addr)
+{
+	*((volatile uint8_t *)addr) = value;
+}
+
+inline uint8_t readb(uint32_t addr)
+{
+	return *(volatile uint8_t *)addr;
+}
+
+inline void writew(uint16_t value, uint32_t addr)
+{
+	*((volatile uint16_t *)addr) = value;
+}
+
+inline uint16_t readw(uint32_t addr)
+{
+	return *(volatile uint16_t *)addr;
+}
+
+inline void writel(uint32_t value, uint32_t addr)
+{
+	*((volatile uint32_t *)addr) = value;
+}
+
+inline uint32_t readl(uint32_t addr)
+{
+	return *(volatile uint32_t *)addr;
+}
+
+#endif /* __MEMIO_H__ */

irq.c

@@ -0,0 +1,141 @@
+#include "serial.h"
+#include "printf.h"
+#include "irq.h"
+#include "memio.h"
+
+static struct {
+	void (*handler)(enum irq_number irq_num, void *opaque);
+	void *opaque;
+} handlers[__irq_max__];
+
+int irq_init(void)
+{
+	register int var;
+
+	/* Acknowledge all interrupts */
+	writel(0xffffffff, IRQ_BASE + IRQ_MASK_OFF + IRQ_NUM_ADJ(0));
+	writel(0xffffffff, IRQ_BASE + IRQ_MASK_OFF + IRQ_NUM_ADJ(32));
+
+	asm volatile ("mrs %0, cpsr":"=r" (var));
+	if (!(var & 0x80)) {
+		serial_puts("Interrupts already enabled\n");
+		return -1;
+	}
+
+	serial_puts("Interrupts were disabled.  Re-enabling...\n");
+	var &= ~0x80;
+	var |= 0x40;
+	var &= ~0x1f;
+	var |= 0x10;
+	asm volatile ("msr cpsr, %0":"=r" (var));
+
+	return 0;
+}
+
+int fiq_init(void)
+{
+	serial_puts("FIQs compiled out\n");
+	return -1;
+	/*
+	register int var;
+	asm volatile ("mrs %0, cpsr":"=r" (var));
+	if (!(var & 0x40)) {
+		serial_puts("FIQ already enabled\n");
+		return -1;
+	}
+
+	serial_puts("FIQ was disabled.  Re-enabling...\n");
+	var &= ~0x40;
+	asm volatile ("msr cpsr, %0":"=r" (var));
+
+	return 0;
+	*/
+}
+
+int irq_enable(enum irq_number irq_num)
+{
+	uint32_t reg = IRQ_BASE + IRQ_MASK_OFF + IRQ_CLR + IRQ_NUM_ADJ(irq_num);
+	if (irq_num >= __irq_max__)
+		return -1;
+
+	writel(1 << (irq_num & 31), reg);
+	return 0;
+}
+
+int irq_disable(enum irq_number irq_num)
+{
+	uint32_t reg = IRQ_BASE + IRQ_MASK_OFF + IRQ_SET + IRQ_NUM_ADJ(irq_num);
+
+	if (irq_num >= __irq_max__)
+		return -1;
+
+	writel(1 << (irq_num & 31), reg);
+	return 0;
+}
+
+void irq_stimulate(enum irq_number irq_num)
+{
+	uint32_t reg = IRQ_BASE + IRQ_STIM_OFF + IRQ_SET + IRQ_NUM_ADJ(irq_num);
+	writel(1 << (irq_num & 31), reg);
+}
+
+void irq_stimulate_reset(enum irq_number irq_num)
+{
+	uint32_t reg = IRQ_BASE + IRQ_STIM_OFF + IRQ_CLR + IRQ_NUM_ADJ(irq_num);
+	writel(1 << (irq_num & 31), reg);
+}
+
+void irq_acknowledge(enum irq_number irq_num)
+{
+	uint32_t reg = IRQ_BASE + IRQ_ACK_OFF + IRQ_NUM_ADJ(irq_num);
+
+	if (irq_num >= __irq_max__)
+		return;
+
+	writel(1 << (irq_num & 31), reg);
+	return;
+}
+
+void irq_mask_acknowledge(enum irq_number irq_num)
+{
+	irq_disable(irq_num);
+	irq_acknowledge(irq_num);
+}
+
+void irq_register_handler(enum irq_number irq_num,
+                void (*handler)(enum irq_number irq_num, void *opaque),
+		void *opaque)
+{
+	if (irq_num >= __irq_max__)
+		return;
+	handlers[irq_num].handler = handler;
+	handlers[irq_num].opaque  = opaque;
+}
+
+static void irq_dispatch_one(enum irq_number irq_num)
+{
+	if (handlers[irq_num].handler)
+		handlers[irq_num].handler(irq_num, handlers[irq_num].opaque);
+	else
+		printf("Unhandled IRQ: %d\n", irq_num);
+	irq_acknowledge(irq_num);
+}
+
+void irq_dispatch(void)
+{
+	uint32_t reg = IRQ_BASE + IRQ_STATUS_OFF;
+	uint32_t val;
+	int i;
+
+	val = readl(reg);
+
+	for (i = 0; i < 32; i++)
+		if (val & (1 << i))
+			irq_dispatch_one(i);
+
+	reg += IRQ_BASE + IRQ_STATUS_OFF + 4;
+	val = readl(reg);
+	for (i = 0; i < (__irq_max__ - 32); i++)
+		if (val & (1 << i))
+			irq_dispatch_one(32 + i);
+}

irqasm.S

@@ -0,0 +1,170 @@
+#define MODE_MASK       0x0000001f /* Bits 0-4: Mode bits */
+#  define USR26_MODE    0x00000000 /*   26-bit User mode */
+#  define FIQ26_MODE    0x00000001 /*   26-bit FIQ mode */
+#  define IRQ26_MODE    0x00000002 /*   26-bit IRQ mode */
+#  define SVC26_MODE    0x00000003 /*   26-bit Supervisor mode */
+#  define MODE32_BIT    0x00000010 /*   Bit 4: 32-bit mode */
+#  define USR_MODE      0x00000010 /*   32-bit User mode */
+#  define FIQ_MODE      0x00000011 /*   32-bit FIQ mode */
+#  define IRQ_MODE      0x00000012 /*   32-bit IRQ mode */
+#  define SVC_MODE      0x00000013 /*   32-bit Supervisor mode */
+#  define ABT_MODE      0x00000017 /*   32-bit Abort mode */
+#  define UND_MODE      0x0000001b /*   32-bit Undefined mode */
+#  define SYSTEM_MODE   0x0000001f /*   32-bit System mode */
+#define PSR_T_BIT       0x00000020 /* Bit 5: Thumb state */
+#define PSR_F_BIT       0x00000040 /* Bit 6: FIQ disable */
+#define PSR_I_BIT       0x00000080 /* Bit 7: IRQ disable */
+                                   /* Bits 8-23: Reserved */
+#define PSR_J_BIT       0x01000000 /* Bit 24: Jazelle state bit */
+                                   /* Bits 25-26: Reserved */
+#define PSR_Q_BIT       0x08000000 /* Bit 27: Sticky overflow */
+#define PSR_V_BIT       0x10000000 /* Bit 28: Overflow */
+#define PSR_C_BIT       0x20000000 /* Bit 29: Carry/Borrow/Extend */
+#define PSR_Z_BIT       0x40000000 /* Bit 30: Zero */
+#define PSR_N_BIT       0x80000000 /* Bit 31: Negative/Less than */
+
+/* CR1 bits (CP#15 CR1) */
+
+#define CR_M            0x00000001 /* MMU enable                          */
+#define CR_A            0x00000002 /* Alignment abort enable              */
+#define CR_C            0x00000004 /* Dcache enable                       */
+#define CR_W            0x00000008 /* Write buffer enable                 */
+#define CR_P            0x00000010 /* 32-bit exception handler            */
+#define CR_D            0x00000020 /* 32-bit data address range           */
+#define CR_L            0x00000040 /* Implementation defined              */
+#define CR_B            0x00000080 /* Big endian                          */
+#define CR_S            0x00000100 /* System MMU protection               */
+#define CR_R            0x00000200 /* ROM MMU protection                  */
+#define CR_F            0x00000400 /* Implementation defined              */
+#define CR_Z            0x00000800 /* Implementation defined              */
+#define CR_I            0x00001000 /* Icache enable                       */
+#define CR_V            0x00002000 /* Vectors relocated to 0xffff0000     */
+#define CR_RR           0x00004000 /* Round Robin cache replacement       */
+#define CR_L4           0x00008000 /* LDR pc can set T bit                */
+#define CR_DT           0x00010000
+#define CR_IT           0x00040000
+#define CR_ST           0x00080000
+#define CR_FI           0x00200000 /* Fast interrupt (lower latency mode) */
+#define CR_U            0x00400000 /* Unaligned access operation          */
+#define CR_XP           0x00800000 /* Extended page tables                */
+#define CR_VE           0x01000000 /* Vectored interrupts                 */
+
+#define REG_R0              (0)
+#define REG_R1              (1)
+#define REG_R2              (2)
+#define REG_R3              (3)
+#define REG_R4              (4)
+#define REG_R5              (5)
+#define REG_R6              (6)
+#define REG_R7              (7)
+#define REG_R8              (8)
+#define REG_R9              (9)
+#define REG_R10             (10)
+#define REG_R11             (11)
+#define REG_R12             (12)
+#define REG_R13             (13)
+#define REG_R14             (14)
+#define REG_R15             (15)
+#define REG_CPSR            (16)
+
+#define XCPTCONTEXT_REGS    (17)
+#define XCPTCONTEXT_SIZE    (4 * XCPTCONTEXT_REGS)
+
+#define REG_A1              REG_R0
+#define REG_A2              REG_R1
+#define REG_A3              REG_R2
+#define REG_A4              REG_R3
+#define REG_V1              REG_R4
+#define REG_V2              REG_R5
+#define REG_V3              REG_R6
+#define REG_V4              REG_R7
+#define REG_V5              REG_R8
+#define REG_V6              REG_R9
+#define REG_V7              REG_R10
+#define REG_SB              REG_R9
+#define REG_SL              REG_R10
+#define REG_FP              REG_R11
+#define REG_IP              REG_R12
+#define REG_SP              REG_R13
+#define REG_LR              REG_R14
+#define REG_PC              REG_R15
+
+.section data
+g_irqtmp:
+	.word   0               /* Saved lr */
+	.word   0               /* Saved spsr */
+g_undeftmp:
+	.word   0               /* Saved lr */
+	.word   0               /* Saved spsr */
+g_aborttmp:
+	.word   0               /* Saved lr */
+	.word   0               /* Saved spsr */
+
+.section text
+.global irq_handler
+
+irq_handler:
+        /* On entry, we are in IRQ mode.  We are free to use
+         * the IRQ mode r13 and r14.
+         */
+        ldr     r13, .Lirqtmp
+        sub     lr, lr, #4
+        str     lr, [r13]                       @ save lr_IRQ
+        mrs     lr, spsr
+        str     lr, [r13, #4]                   @ save spsr_IRQ
+
+        /* Then switch back to SVC mode */
+
+        bic     lr, lr, #MODE_MASK      /* Keep F and T bits */
+        orr     lr, lr, #(SVC_MODE | PSR_I_BIT)
+        msr     cpsr_c, lr              /* Switch to SVC mode */
+
+        /* Create a context structure.  First set aside a stack frame
+         * and store r0-r12 into the frame.
+         */
+
+        sub     sp, sp, #XCPTCONTEXT_SIZE
+        stmia   sp, {r0-r12}            /* Save the SVC mode regs */
+
+        /* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */
+
+        add     r1, sp, #XCPTCONTEXT_SIZE
+        mov     r2, r14
+
+        /* Get the values for r15(pc) and CPSR in r3 and r4 */
+
+        ldr     r0, .Lirqtmp            /* Points to temp storage */
+        ldmia   r0, {r3, r4}            /* Recover r1=lr_IRQ, r2=spsr_IRQ */
+
+        add     r0, sp, #(4*REG_SP)     /* Offset to pc, cpsr storage */
+        stmia   r0, {r1-r4}
+
+        /* Then call the IRQ handler with interrupts disabled. */
+
+        mov     fp, #0                  /* Init frame pointer */
+        mov     r0, sp                  /* Get r0=xcp */
+
+#if CONFIG_ARCH_INTERRUPTSTACK > 3
+        ldr     sp, .Lirqstackbase      /* SP = interrupt stack base */
+        str     r0, [sp]                /* Save the user stack pointer */
+        bl      irq_handler_c             /* Call the handler */
+        ldr     sp, [sp]                /* Restore the user stack pointer */
+#else
+        bl      irq_handler_c             /* Call the handler */
+
+        /* Restore the CPSR, SVC mode registers and return */
+.Lnoirqset:
+        ldr     r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */
+        msr     spsr, r0
+#endif
+        ldmia   sp, {r0-r15}^           /* Return */
+
+.Lirqtmp:
+        .word   g_irqtmp
+#if CONFIG_ARCH_INTERRUPTSTACK > 3
+.Lirqstackbase:
+        .word   g_intstackbase
+#endif
+        .size   irq_handler, . - irq_handler
+        .align  5
+

main.c

@@ -1,8 +1,10 @@
 #include <string.h>
+#include "bionic.h"
+#include "memio.h"
+#include "irq.h"
+#include "printf.h"
 #include "serial.h"
 #include "utils.h"
-#include "bionic.h"
-#include "printf.h"
 
 //#define AUTOMATED
 
@@ -79,36 +81,6 @@ static int serial_get_line(char *bfr, int len)
 }
 #endif
 
-static inline void writeb(uint8_t value, uint32_t addr)
-{
-	*((volatile uint8_t *)addr) = value;
-}
-
-static inline uint8_t readb(uint32_t addr)
-{
-	return *(volatile uint8_t *)addr;
-}
-
-static inline void writew(uint16_t value, uint32_t addr)
-{
-	*((volatile uint16_t *)addr) = value;
-}
-
-static inline uint16_t readw(uint32_t addr)
-{
-	return *(volatile uint16_t *)addr;
-}
-
-static inline void writel(uint32_t value, uint32_t addr)
-{
-	*((volatile uint32_t *)addr) = value;
-}
-
-static inline uint32_t readl(uint32_t addr)
-{
-	return *(volatile uint32_t *)addr;
-}
-
 /*
 static int wdt_kick(void)
 {
@@ -222,45 +194,8 @@ static int list_registers(void)
 	return 0;
 }
 
-static int enable_irq(void)
-{
-	register int var;
-	asm volatile ("mrs %0, cpsr":"=r" (var));
-	if (!(var & 0x80)) {
-		serial_puts("Interrupts already enabled\n");
-		return -1;
-	}
-
-//	serial_puts("Interrupts were disabled.  Re-enabling...\n");
-	var &= ~0x80;
-	var &= ~0x1f;
-	var |= 0x10;
-	asm volatile ("msr cpsr, %0":"=r" (var));
-
-	return 0;
-}
-
-static int enable_fiq(void)
-{
-	register int var;
-	asm volatile ("mrs %0, cpsr":"=r" (var));
-	if (!(var & 0x40)) {
-		serial_puts("FIQ already enabled\n");
-		return -1;
-	}
-
-//	serial_puts("FIQ was disabled.  Re-enabling...\n");
-	var &= ~0x40;
-	asm volatile ("msr cpsr, %0":"=r" (var));
-
-	return 0;
-}
-
 static int do_init(void)
 {
-	void *rv_start = (void *)0x88;
-	void *rv_end = (void *)0x88 + 256;
-
 	list_registers();
 	serial_init();
 
@@ -272,10 +207,8 @@ static int do_init(void)
 
 	serial_puts("\n\nFernly shell\n");
 
-	/* Copy exception vectors to address 0 */
-	_memcpy((void *)0, rv_start, rv_end - rv_start);
-	enable_irq();
-	enable_fiq();
+	irq_init();
+	fiq_init();
 
 	return 0;
 }
@@ -404,8 +337,11 @@ static int loop(void)
 #else /* AUTOMATED */
 
 static int cmd_help(int argc, char **argv);
+static int cmd_hex(int argc, char **argv);
+static int cmd_swi(int argc, char **argv);
 static int cmd_reboot(int argc, char **argv);
 static int cmd_args(int argc, char **argv);
+int cmd_irq(int argc, char **argv);
 
 static struct {
 	int (*func)(int argc, char **argv);
@@ -422,18 +358,35 @@ static struct {
 		.name = "reboot",
 		.help = "Reboot Fernvale",
 	},
+	{
+		.func = cmd_hex,
+		.name = "hex",
+		.help = "Print area of memory as hex",
+	},
+	{
+		.func = cmd_irq,
+		.name = "irq",
+		.help = "Manipulate IRQs",
+	},
 	{
 		.func = cmd_args,
 		.name = "args",
 		.help = "Print contents of arc and argv",
 	},
+	{
+		.func = cmd_swi,
+		.name = "swi",
+		.help = "Generate software interrupt",
+	},
 };
 
 int cmd_help(int argc, char **argv)
 {
 	int i;
 
+	printf("Fernly shell help.  Available commands:\n");
 	for (i = 0; i < sizeof(commands) / sizeof(*commands); i++) {
+		serial_putc('\t');
 		serial_puts(commands[i].name);
 		serial_putc('\t');
 		serial_puts(commands[i].help);
@@ -455,6 +408,33 @@ int cmd_args(int argc, char **argv)
 	return 0;
 }
 
+int cmd_hex(int argc, char **argv)
+{
+	uint32_t offset;
+	int count = 0x200;
+	if (argc < 1) {
+		printf("Usage: hex [offset] [[count]]\n");
+		return -1;
+	}
+
+	offset = _strtoul(argv[0], NULL, 0);
+
+	if (argc > 1)
+		count = _strtoul(argv[1], NULL, 0);
+
+	serial_print_hex((const void *)offset, count);
+	return 0;
+}
+
+int cmd_swi(int argc, char **argv)
+{
+	printf("Generating SWI...\n");
+	asm volatile ("swi #0\n");
+	printf("Returned from SWI\n");
+
+	return 0;
+}
+
 int cmd_reboot(int argc, char **argv)
 {
 	printf("Rebooting...\n");

memio.c

@@ -0,0 +1,31 @@
+#include <stdint.h>
+
+inline void writeb(uint8_t value, uint32_t addr)
+{
+	*((volatile uint8_t *)addr) = value;
+}
+
+inline uint8_t readb(uint32_t addr)
+{
+	return *(volatile uint8_t *)addr;
+}
+
+inline void writew(uint16_t value, uint32_t addr)
+{
+	*((volatile uint16_t *)addr) = value;
+}
+
+inline uint16_t readw(uint32_t addr)
+{
+	return *(volatile uint16_t *)addr;
+}
+
+inline void writel(uint32_t value, uint32_t addr)
+{
+	*((volatile uint32_t *)addr) = value;
+}
+
+inline uint32_t readl(uint32_t addr)
+{
+	return *(volatile uint32_t *)addr;
+}

start.S

@@ -21,19 +21,21 @@ clear_stack:
 	mov	r0, #66
 	bl	uart_putc
 
-# Copy 0x10000 bytes from =spi_offset to psram at 0x20000
+# Copy 0x10000 bytes from =spi_offset to psram at 0x3460
 copy_code_to_ram:
-	ldr	r0, =0x00002000 // Source offset
-	ldr	r1, =0x10003460 // spi_offset
-	mov	r2, r1
-	mov	r3, #0x10000
-	add	r2, r2, r3
+	ldr	r0, =0x00003460 // Target offset
+	ldr	r1, =0x10003460 // Source offset
+	mov	r2, #0x10000
+	bl	asm_memcpy
 
-copy_code_to_ram_loop:
-	cmp	r1, r2
-	ldrcc	r3, [r1], #4
-	strcc	r3, [r0], #4
-	bcc	copy_code_to_ram_loop
+# Copy reset vector table to address 0
+copy_reset_vectors:
+	mov	r0, #0
+	ldr	r1, =0x3500
+	mov	r2, #0x100
+	bl	asm_memcpy
+	
+# Begin executing out of psram
 	
 # Jump to main, which ought to be in psram now
 jump_to_main:
@@ -43,6 +45,17 @@ jump_to_main:
 	ldr	r0, =reset_handler
 	mov	pc, r0
 
+asm_memcpy:
+	mov	r3, r1
+	add	r3, r3, r2
+
+asm_memcpy_loop:
+	cmp	r1, r3
+	ldrcc	r2, [r1], #4
+	strcc	r2, [r0], #4
+	bcc	asm_memcpy_loop
+	bx	lr
+
 uart_putc:
 	ldr	r2, =0xa0080014 // uart offset
 uart_putc_ready_wait:
@@ -55,46 +68,31 @@ uart_putc_ready_wait:
 
 .global rv_start
 rv_start:
-	b	do_reset
-	b	do_undef
-	b	do_swi
-	b	do_prefetch_abort
-	b	do_data_abort
-	b	do_reserved
-	b	do_irq
-	b	do_fiq
+	ldr	pc, .Lreset_handler
+	ldr	pc, .Lundef_handler
+	ldr	pc, .Lswi_handler
+	ldr	pc, .Lprefetch_abort_handler
+	ldr	pc, .Ldata_abort_handler
+	ldr	pc, .Lreserved_handler
+	ldr	pc, .Lirq_handler
+	ldr	pc, .Lfiq_handler
 
-do_reset:
-	ldr	r0, =reset_handler
-	mov	pc, r0
-
-do_undef:
-	ldr	r0, =undef_handler
-	mov	pc, r0
-
-do_swi:
-	ldr	r0, =swi_handler
-	mov	pc, r0
-
-do_prefetch_abort:
-	ldr	r0, =prefetch_abort_handler
-	mov	pc, r0
-
-do_data_abort:
-	ldr	r0, =data_abort_handler
-	mov	pc, r0
-
-do_reserved:
-	ldr	r0, =reserved_handler
-	mov	pc, r0
-
-do_irq:
-	ldr	r0, =irq_handler
-	mov	pc, r0
-
-do_fiq:
-	ldr	r0, =fiq_handler
-	mov	pc, r0
+.Lreset_handler:
+	.long	reset_handler
+.Lundef_handler:
+	.long	undef_handler
+.Lswi_handler:
+	.long	swi_handler
+.Lprefetch_abort_handler:
+	.long	prefetch_abort_handler
+.Ldata_abort_handler:
+	.long	data_abort_handler
+.Lreserved_handler:
+	.long	reserved_handler
+.Lirq_handler:
+	.long	irq_handler_c
+.Lfiq_handler:
+	.long	fiq_handler
 
 .global rv_end
 rv_end:
@@ -103,7 +101,6 @@ rv_end:
 
 
 .global __udiv64
-
 __udiv64:
 	adds	r0,r0,r0
 	adc	r1,r1,r1
@@ -136,4 +133,3 @@ ram_bzero:
 	mov	pc, r0
 ram_bzero_addr:
         .long	0x700073bc
-

vectors.c

@@ -20,25 +20,32 @@ void swi_handler(void) {
 
 void prefetch_abort_handler(void) {
 	serial_puts("Prefetch abort exception\n");
+	while(1);
 	return;
 }
 
 void data_abort_handler(void) {
 	serial_puts("Data abort exception\n");
+	while(1);
 	return;
 }
 
 void reserved_handler(void) {
 	serial_puts("Handled some IRQ that shouldn't exist\n");
+	while(1);
 	return;
 }
 
-void irq_handler(void) {
+extern void irq_dispatch(void);
+void irq_handler_c(void) {
 	serial_puts("Handled IRQ\n");
+//	while(1);
+	irq_dispatch();
 	return;
 }
 
 void fiq_handler(void) {
 	serial_puts("Handled FIQ\n");
+//	while(1);
 	return;
 }