More tidying up, proper SWI handler

7 files changed, 220 insertions, 120 deletions

diff --git a/Makefile b/Makefile
index 1d38433..b10fe7a 100644
--- a/Makefile
+++ b/Makefile
@@ -1,16 +1,16 @@
-all: kernel.img

-

-kernel.img: kernel.elf

-	arm-none-eabi-objcopy kernel.elf -O binary kernel.img

-

-kernel.elf : main.o slow_status_flash.o linkscript.ld

-	arm-none-eabi-ld --no-undefined main.o slow_status_flash.o -o kernel.elf -T linkscript.ld

-

-main.o: src/main.s

-	arm-none-eabi-as -Isrc src/main.s -o main.o

-

-slow_status_flash.o: src/slow_status_flash.s

-	arm-none-eabi-as -Isrc src/slow_status_flash.s -o slow_status_flash.o

-

-clean: 

-	rm -f main.o kernel.elf kernel.img

+SRCS=main.s slow_status_flash.s  flash_button.s swi_handler.s
+OBJS=main.o slow_status_flash.o  flash_button.o swi_handler.o
+
+all: kernel.img
+
+kernel.img: kernel.elf
+	arm-none-eabi-objcopy kernel.elf -O binary kernel.img
+
+kernel.elf: ${OBJS} linkscript.ld
+	arm-none-eabi-ld --no-undefined ${OBJS} -o kernel.elf -T linkscript.ld
+
+%.o: src/%.s
+	arm-none-eabi-as -Isrc $< -o $@
+
+clean:
+	rm -f ${OBJS} kernel.elf kernel.img
diff --git a/linkscript.ld b/linkscript.ld
index 897ef57..388f95c 100644
--- a/linkscript.ld
+++ b/linkscript.ld
@@ -1,8 +1,20 @@
-SECTIONS {

-	.init 0x8000 : { *(.init) }

-	. = ALIGN(0x1000);

-	. = . + 0x800;

-	stack_svc = .;

-	. = . + 0x800;

-	stack_usr = .;

-}

+SECTIONS {
+	.init 0x8000 : { *(.init) }
+	. = ALIGN(0x1000);
+	. = . + 0x800;
+	stack_svc_cpu0 = .;
+	. = . + 0x800;
+	stack_svc_cpu1 = .;
+	. = . + 0x800;
+	stack_svc_cpu2 = .;
+	. = . + 0x800;
+	stack_svc_cpu3 = .;
+	. = . + 0x800;
+	stack_usr_cpu0 = .;
+	. = . + 0x800;
+	stack_usr_cpu1 = .;
+	. = . + 0x800;
+	stack_usr_cpu2 = .;
+	. = . + 0x800;
+	stack_usr_cpu3 = .;
+}
diff --git a/src/flash_button.s b/src/flash_button.s
new file mode 100644
index 0000000..9b702e4
--- /dev/null
+++ b/src/flash_button.s
@@ -0,0 +1,68 @@
+/*
+ * flash_button.s
+ *
+ * Copyright © 2018 Thomas White <taw@bitwiz.org.uk>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+.include "swi_numbers.h"
+
+.global flash_button
+
+flash_button:
+	MOV	R0, #0			@ Get GPLEV0
+	SWI	OS_GetGPIO
+	TST	R0, #1<<17		@ Check GPIO 17
+	BLNE	flash_leds
+	B	flash_button
+
+flash_leds:
+	STMDB	R13!, {LR}
+
+	MOV	R2, #5			@ Number of times to flash
+
+1:
+	MOV	R0, #0			@ Flash first set of GPIOs
+
+	MOV	R1, #1<<16
+	SWI	OS_ClearGPIO
+	MOV	R1, #1<<18
+	SWI	OS_SetGPIO
+
+	BL	pause
+
+	MOV	R1, #1<<18
+	SWI	OS_ClearGPIO
+	MOV	R1, #1<<16
+	SWI	OS_SetGPIO
+
+	BL	pause
+
+	SUBS	R2, R2, #1
+	BNE	1b
+
+	MOV	R1, #1<<18
+	ORR	R1, #1<<16
+	SWI	OS_ClearGPIO
+
+	LDMIA	R13!, {PC}
+
+pause:
+	MOV	R1, #0x3f000
+1:
+	SUBS	R1, R1, #1
+	BNE	1b
+	MOV	PC, LR
diff --git a/src/main.s b/src/main.s
index b981b2a..40f3ff0 100644
--- a/src/main.s
+++ b/src/main.s
@@ -18,8 +18,6 @@
  *
  */
 
-@ vim:ft=armv5
-
 .include "swi_numbers.h"
 
 .section .init
@@ -56,7 +54,7 @@ reset:
 	ERET
 
 	/* Set up supervisor mode stack */
-	LDR	R13, =stack_svc
+	LDR	R13, =stack_svc_cpu0
 
 	/* Relocate vector table */
 	MOV	R10, #0x8000
@@ -66,95 +64,50 @@ reset:
 	LDMIA	R10!, {R0-R7}
 	STMIA	R11!, {R0-R7}
 
-	/* Set another CPU doing something */
-	LDR R0, =flash_status_led
-	MOV R1, #0x40000000
-	STR R0,[R1, #0xbc]
-	SEV
-
+	/* Configure GPIOs */
 	LDR	R8, =0x3f200000
 	LDR	R1, [R8, #0x04]		@ GPFSEL1
 	ORR	R1, R1, #1<<18		@ GPIO pin 16 is output
 	BIC	R1, R1, #3<<21		@ GPIO pin 17 is input
 	ORR	R1, R1, #1<<24		@ GPIO pin 18 is output
+	ORR	R1, R1, #1<<29		@ GPIO pin 29 is output
 	STR	R1, [R8, #0x04]		@ GPFSEL1
 
-	@ Flash green LED
-	MRC	P15, 0, R0, C0, C0, 5
-	AND	R0, R0, #0x0f
-	CMP	R0, #0
-	BNE	skip_flash
-
-	MOV	R1, #1<<18
-	STR	R1, [R8, #0x1c]		@ GPSET0
-	BL	pause
-	MOV	R1, #1<<18
-	STR	R1, [R8, #0x28]		@ GPCLR0
-	BL	pause
-
-skip_flash:
+	/* Set another CPU doing something */
+	LDR	R0, =init_cpu3
+	MOV	R1, #0x40000000
+	STR	R0,[R1, #0xbc]
+	SEV
 
-	@ Switch to USR mode and set up stack
+	/* Switch to USR mode and set up stack */
 	MRS	R0, CPSR
 	BIC	R0, R0, #0x1f
 	ORR	R0, R0, #0x10
 	MSR	CPSR_c, R0
-	LDR	R13, =stack_usr
-
-repeat:
-	MOV	R0, #0
-	SWI	0
-	TST	R0, #1<<17
-	MOVNE	R0, #1
-	SWINE	0
-	B	repeat
-
-
-swi_handler:
-	STMDB	R13!, {R8, LR}
-	LDR	R8, =0x3f200000
-	CMP	R0, #0
-	BEQ	get_gpio
-	CMP	R0, #1
-	BLEQ	flash
-	LDMIA	R13!, {R8, PC}^
-
-get_gpio:
-	LDR	R0, [R8, #0x34]		@ GPLEV0
-	LDMIA	R13!, {R8, PC}^
-
-
-flash:
-	STMDB	R13!, {R1, R2, R4, LR}
-	MOV	R4, #5
+	LDR	R13, =stack_usr_cpu0
 
-1:
-	MOV	R1, #1<<16
-	STR	R1, [R8, #0x28]		@ GPCLR0
-	MOV	R1, #1<<18
-	STR	R1, [R8, #0x1c]		@ GPSET0
+	/* Start user mode task */
+	B	flash_button
 
-	BL	pause
 
-	MOV	R1, #1<<18
-	STR	R1, [R8, #0x28]		@ GPCLR0
-	MOV	R1, #1<<16
-	STR	R1, [R8, #0x1c]		@ GPSET0
-
-	BL	pause
-
-	MOV	R1, #1<<18
-	ORR	R1, #1<<16
-	STR	R1, [R8, #0x28]		@ GPCLR0
+init_cpu3:
+	/* Out of HYP mode */
+	MRS	R0, CPSR
+	BIC	R0, R0, #0x1f
+	ORR	R0, R0, #0x13
+	MSR	SPSR_cxsf, R0
+	ADD	R0, PC, #4
+	MSR	ELR_hyp,r0
+	ERET
 
-	SUBS	R4, R4, #1
-	BNE	1b
+	/* Set up supervisor mode stack */
+	LDR	R13, =stack_svc_cpu3
 
-	LDMIA	R13!, {R1, R2, R4, PC}
+	/* Switch to USR mode and set up stack */
+	MRS	R0, CPSR
+	BIC	R0, R0, #0x1f
+	ORR	R0, R0, #0x10
+	MSR	CPSR_c, R0
+	LDR	R13, =stack_usr_cpu3
 
-pause:
-	MOV	R2, #0x3f000
-1:
-	SUBS	R2, R2, #1
-	BNE	1b
-	MOV	PC, LR
+	B	flash_status_led
diff --git a/src/slow_status_flash.s b/src/slow_status_flash.s
index afb7be4..988a825 100644
--- a/src/slow_status_flash.s
+++ b/src/slow_status_flash.s
@@ -18,31 +18,26 @@
  *
  */
 
-@ vim:ft=armv5
+.include "swi_numbers.h"
 
-.section .text
 .global flash_status_led
 
 flash_status_led:
-	LDR	R8, =0x3f200000
-	LDR	R1, [R8, #0x04]		@ GPFSEL1
-	ORR	R1, R1, #1<<18		@ GPIO pin 16 is output
-	BIC	R1, R1, #3<<21		@ GPIO pin 17 is input
-	ORR	R1, R1, #1<<24		@ GPIO pin 18 is output
-	STR	R1, [R8, #0x04]		@ GPFSEL1
 
 forever:
-	MRC	P15, 0, R0, C0, C0, 5
-	AND	R0, R0, #0x0f
-flashred:
+	SWI	OS_GetCPUID
+	MOV	R7, R0
+	MOV	R6, R7
+	MOV	R0, #0
+1:
 	MOV	R1, #1<<29
-	STR	R1, [R8, #0x1c]		@ GPSET0
+	SWI	OS_SetGPIO
 	BL	longpause
 	MOV	R1, #1<<29
-	STR	R1, [R8, #0x28]		@ GPCLR0
+	SWI	OS_ClearGPIO
 	BL	longpause
-	SUBS	R0, R0, #1
-	BNE	flashred
+	SUBS	R6, R6, #1
+	BNE	1b
 
 	BL	longpause
 	BL	longpause
@@ -51,7 +46,7 @@ flashred:
 
 longpause:
 	MOV	R2, #0x3f0000
-1:
+2:
 	SUBS	R2, R2, #1
-	BNE	1b
+	BNE	2b
 	MOV	PC, LR
diff --git a/src/swi_handler.s b/src/swi_handler.s
new file mode 100644
index 0000000..0d2082d
--- /dev/null
+++ b/src/swi_handler.s
@@ -0,0 +1,70 @@
+/*
+ * swi_handler.s
+ *
+ * Copyright © 2018 Thomas White <taw@bitwiz.org.uk>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+.include "swi_numbers.h"
+
+.global swi_handler
+
+swi_handler:
+	STMDB	R13!, { R12, LR}
+
+	LDR	R12, [LR, #-4]
+	BIC	R12, R12, #0xff000000
+
+	CMP	R12, #OS_GetGPIO
+	BEQ	get_gpio
+
+	CMP	R12, #OS_SetGPIO
+	BEQ	set_gpio
+
+	CMP	R12, #OS_ClearGPIO
+	BEQ	clear_gpio
+
+	CMP	R12, #OS_GetCPUID
+	BEQ	get_cpu_id
+
+	LDMIA	R13!, {R12, PC}^
+
+get_gpio:
+	STMDB	R13!, {R8}
+	LDR	R8, =0x3f200000
+	LDR	R0, [R8, #0x34]		@ GPLEV0
+	LDMIA	R13!, {R8}
+	LDMIA	R13!, {R12, PC}^
+
+set_gpio:
+	STMDB	R13!, {R8}
+	LDR	R8, =0x3f200000
+	STR	R1, [R8, #0x1c]		@ GPSET0
+	LDMIA	R13!, {R8}
+	LDMIA	R13!, {R12, PC}^
+
+clear_gpio:
+	STMDB	R13!, {R8}
+	LDR	R8, =0x3f200000
+	STR	R1, [R8, #0x28]		@ GPCLR0
+	LDMIA	R13!, {R8}
+	LDMIA	R13!, {R12, PC}^
+
+get_cpu_id:
+	MRC	P15, 0, R0, C0, C0, 5
+	AND	R0, R0, #0x0f
+	LDMIA	R13!, {R12, PC}^
+
diff --git a/src/swi_numbers.h b/src/swi_numbers.h
index 245fccc..384bd24 100644
--- a/src/swi_numbers.h
+++ b/src/swi_numbers.h
@@ -18,5 +18,7 @@
  *
  */
 
-#define OS_GetGPIO	(1)
-#define OS_SetGPIO	(2)
+OS_GetGPIO	= 1
+OS_SetGPIO	= 2
+OS_ClearGPIO	= 3
+OS_GetCPUID	= 4