Update to work on real hardware

kernel/aarch64/aarch64.S

@@ -125,13 +125,25 @@ die:
 .globl AArch64_startTask 
 AArch64_startTask:
 	bl Task_unlockScheduler
-	blr x1
+	mov x0, x20
+	blr x21
 	
 hang:
 	/* todo: implement task exit here */
 	WFE
 	b hang
 
+.globl AArch64_getPState
+AArch64_getPState:
+	mrs x23, NZCV
+	mrs x3, DAIF
+	orr x23, x23, x3
+	mrs x3, CurrentEL
+	lsr x3, x3, 0x02
+	/* set third bit: todo: find out its real meaning (registers width?) */
+	orr x3, x3, 0x04 
+	orr x0, x23, x3
+	ret
 
 .globl AArch64_switchContext
 AArch64_switchContext:
@@ -157,6 +169,7 @@ AArch64_switchContext:
 	mrs x3, DAIF
 	orr x23, x23, x3
 	mrs x3, CurrentEL
+	lsr x3, x3, 0x02
 	orr x23, x23, x3
 
 	stp x30, x30, [sp, 16 * 15]
@@ -227,9 +240,11 @@ el1h_sync:
 
 el1h_irq:
 	SAVE_REGISTERS
+	mov x0, sp
+	bl Task_setStackPointer
 	bl IRQ_onInterrupt
-	cbz x0, load_regs
-	mov sp, x0 /* task context must be switched */
+	bl Task_getStackPointer 
+	mov sp, x0
 load_regs:
 	LOAD_REGISTERS
 	eret

kernel/aarch64/aarch64.h

@@ -16,6 +16,7 @@
 #ifndef AARCH64_H_F3AA8888_CEB3_4C16_9835_C3F23B040BB5
 #define AARCH64_H_F3AA8888_CEB3_4C16_9835_C3F23B040BB5
 
+#include <leos/types.h>
 #include "aarch64_reg.h"
 #include "aarch64_irq.h"
 
@@ -46,6 +47,9 @@ AArch64_memzero(void *addr, unsigned long size);
 extern void
 AArch64_startTask(void *arg, void *callback);
 
+extern uint32_t
+AArch64_getPState(void);
+
 extern void
 AArch64_switchContext(void *currentTask, void *nextTask);