/*
    TMHalAsm.S, Copyright (c) by Silvio Dragone & David Reist, 
                              rework by Claudio Jeker
    Swiss Federal Institute of Technology,
    Computer Engineering and Networks Laboratory.

    TOPSY -- A Teachable Operating System.
             Implementation of a tiny and simple
             micro kernel for teaching purposes.

    For further information, please visit http://www.tik.ee.ethz.ch/~topsy

    This software is provided under the terms of the GNU General Public Licence.

    A full copy of the GNU GPL is provided in the file COPYING found in the 
    development root of Topsy.
*/
/*
	
    File:                  $Source: /home/ruf/cvs_Topsy/cvs/Topsy/Threads/sa1100/TMHalAsm.S,v $
    Affiliation:           ETH Zuerich, TIK
    Version:               $Revision: 2893 $
    Last Date of Change:   $Date: 2006-11-28 16:36:02 +0100 (Die, 28 Nov 2006) $  by: $Author: rnatau $

*/

#include "asm.h"
#include "cpu.h"
#include "SyscallMsg.h"

.text
	
/* Offset values in the ProcContext structure (expressed in bytes) */
#define CONTEXTSIZE         17*4

.equ ARGUMENT0_OFFSET,        0x0
.equ ARGUMENT1_OFFSET,        0x4
.equ ARGUMENT2_OFFSET,        0x8
.equ ARGUMENT3_OFFSET,        0xC
.equ SAVEGPR0_OFFSET,        0x10
.equ SAVEGPR1_OFFSET,        0x14
.equ SAVEGPR2_OFFSET,        0x18
.equ SAVEGPR3_OFFSET,        0x1C
.equ SAVEGPR4_OFFSET,        0x20
.equ SAVEGPR5_OFFSET,        0x24
.equ STACKLIMIT_OFFSET,      0x28
.equ FRAMEPOINTER_OFFSET,    0x2C
.equ SCRATCHREGISTER_OFFSET, 0x30
.equ STACKPOINTER_OFFSET,    0x34
.equ LINKREGISTER_OFFSET,    0x38
.equ PROGRAMCOUNTER_OFFSET,  0x3C
.equ STATUSREGISTER_OFFSET,  0x40

/* Offset of the processor context in the Thread structure */
.equ THREADCONTEXT_OFFSET, 0x0
.equ THREADID_OFFSET,      0x4
.equ SCHEDRUNNING_OFFSET,  0x0

/* General constants */
#define BIT_UNSET 0
#define BIT_SET   1


ENTRY(SoftwareIntrruptExceptionHandler)
	ldr sp, =EXCSTACK_SVC
	str lr, [sp, #-8]								// store the return address on stack
	str	ip, [sp, #-12]							// store working register

	mrs	ip, cpsr										// get the CPSR
	orr	ip, ip, #0xC0								// disable all interrupts, no nesting!
	msr	cpsr_c, ip									// write it back to disable all interrupts
	
	mrs ip, spsr										// get CPSR of interrupted thread
	str ip, [sp, #-4]								// store the status
	
	ldr ip, [sp, #-12]							// get original back
	sub sp, sp, #8									// set stack pointer
	bl	saveContext
	
	mov a0, #EXC_SOFT_INTR					// a1-a3 are already set

	bl exceptionDispatcher					// go to the high level dispatcher

	b excRestoreContext							// done...

ENTRY(FIQExceptionHandler)
	ldr sp, =EXCSTACK_FIQ
	sub	lr, lr, #4									// XXX adjust return address before storing it
	str lr, [sp, #-8]								// store the return address on stack
	str	ip, [sp, #-12]							// store working register

	mrs	ip, cpsr										// get the CPSR
	orr	ip, ip, #0xC0								// disable all interrupts, no nesting!
	msr	cpsr_c, ip									// write it back to disable all interrupts
	
	mrs ip, spsr										// get CPSR of interrupted thread
	str ip, [sp, #-4]								// store the status
	
	ldr ip, [sp, #-12]							// get original back
	sub sp, sp, #8									// set stack pointer
	bl	saveContext
	
	mov a0, #EXC_FIQ_INTR
	ldr	a1, =SA_FIQ_SOURCE 					// Get interrupt controller IRQ address
	ldr	a1, [a1]										// Get IRQ pending register
	mov a2, #0											// not needed
	mov a3, #0											// not needed

	bl exceptionDispatcher					// go to the high level dispatcher

	b excRestoreContext							// done...
	
ENTRY(IRQExceptionHandler)
	ldr sp, =EXCSTACK_IRQ
	sub	lr, lr, #4									// XXX adjust return address before storing it
	str lr, [sp, #-8]								// store the return address on stack
	str	ip, [sp, #-12]							// store working register

	mrs	ip, cpsr										// get the CPSR
	orr	ip, ip, #0xC0								// disable all interrupts, no nesting!
	msr	cpsr_c, ip									// write it back to disable all interrupts
	
	mrs ip, spsr										// get CPSR of interrupted thread
	str ip, [sp, #-4]								// store the status
	
	ldr ip, [sp, #-12]							// get original back
	sub sp, sp, #8									// set stack pointer
	bl	saveContext
	
	mov a0, #EXC_IRQ_INTR
	ldr	a1, =SA_IRQ_SOURCE 					// Get interrupt controller IRQ address
	ldr	a1, [a1]										// Get IRQ pending register
	mov a2, #0											// not needed
	mov a3, #0											// not needed

	bl exceptionDispatcher					// go to the high level dispatcher

	b excRestoreContext							// done...

ENTRY(UndefinedInstructionExceptionHandler)
	ldr sp, =EXCSTACK_UND
	sub	lr, lr, #4									// XXX adjust return address before storing it
	str lr, [sp, #-8]								// store the return address on stack
	str	ip, [sp, #-12]							// store working register

	mrs	ip, cpsr										// get the CPSR
	orr	ip, ip, #0xC0								// disable all interrupts, no nesting!
	msr	cpsr_c, ip									// write it back to disable all interrupts
	
	mrs ip, spsr										// get CPSR of interrupted thread
	str ip, [sp, #-4]								// store the status
	
	ldr ip, [sp, #-12]							// get original back
	sub sp, sp, #8									// set stack pointer
	bl	saveContext
	
	mov a0, #EXC_UNDEF_INSTR				// store the casue of the exception
	ldr a1, [sp, #PROGRAMCOUNTER_OFFSET]		// set fault address
	mov a2, #0											// not needed
	mov a3, #0											// not needed

	bl exceptionDispatcher					// go to the high level dispatcher

	b excRestoreContext							// done...

ENTRY(DataAbortExceptionHandler)
	ldr sp, =EXCSTACK_ABT
	sub	lr, lr, #8									// XXX adjust return address before storing it
	str lr, [sp, #-8]								// store the return address on stack
	str	ip, [sp, #-12]							// store working register

	mrs	ip, cpsr										// get the CPSR
	orr	ip, ip, #0xC0								// disable all interrupts, no nesting!
	msr	cpsr_c, ip									// write it back to disable all interrupts
	
	mrs ip, spsr										// get CPSR of interrupted thread
	str ip, [sp, #-4]								// store the status
	
	ldr ip, [sp, #-12]							// get original back
	sub sp, sp, #8									// set stack pointer
	bl	saveContext
	
	mov a0, #EXC_ABORT_DATA					// store the casue of the exception
	mrc p15, 0, a1, c5, c0, 0				// get fault status
	mrc p15, 0, a2, c6, c0, 0				// get fault address
	mov a3, #0											// not needed

	bl exceptionDispatcher					// go to the high level dispatcher

	b excRestoreContext							// done...

ENTRY(PrefetchAbortExceptionHandler)
	ldr sp, =EXCSTACK_ABT
	sub	lr, lr, #4									// XXX adjust return address before storing it
	str lr, [sp, #-8]								// store the return address on stack
	str	ip, [sp, #-12]							// store working register

	mrs	ip, cpsr										// get the CPSR
	orr	ip, ip, #0xC0								// disable all interrupts, no nesting!
	msr	cpsr_c, ip									// write it back to disable all interrupts
	
	mrs ip, spsr										// get CPSR of interrupted thread
	str ip, [sp, #-4]								// store the status
	
	ldr ip, [sp, #-12]							// get original back
	sub sp, sp, #8									// set stack pointer
	bl	saveContext
	
	mov a0, #EXC_ABORT_PREFETCH			// store the casue of the exception
	mov a1, #0											// set fault status to 0
	ldr a2, [sp, #PROGRAMCOUNTER_OFFSET]		// set fault address
	mov a3, #0											// not needed

	bl exceptionDispatcher					// go to the high level dispatcher

	b excRestoreContext							// done...


	
/* Save context of current thread. All registers are saved directly 
 * onto the stack. 
 * Note: pc, status and the casue is stored befor we call saveContext! 
 */ 
ENTRY( saveContext )

	stmdb	sp!, {a0 - lr}^						// Store registers a0-lr of register bank
																	// user/system mode onto the stack
	/*
		 Now the stack looks like this:
				status 	// stored before calling saveContext
				pc			// stored before calling saveContext
				lr
				sp
				ip
				fp
				v6
				...
				v0
				a3
				...
				a0
	*/
	
	stmdb sp!, {v0, ip}							// store v0 and ip so the
																	// exceptionContextFlag can be updated
	ldr ip, =exceptionContextFlag		// update exceptionContextFlag
	ldr v0, [ip]										// for an entry (store) add 1
	add v0, v0, #1									// for an exite (restore) add -1
	str v0, [ip]

	ldmia	sp!, {v0, ip}							// Reload v0 and working register
END( saveContext )
	
/* Restore context of new thread. All registers are restored from the 
 * ProcContext structure that is on the stack.
 */ 
ENTRY( excRestoreContext )
	ldr a0, =exceptionContextFlag		// update exceptionContextFlag
	ldr a1, [a0]
	sub a1, a1, #1
	str a1, [a0]
	
	/*mov a0, sp
	mov a1, #0
	mov a2, #CONTEXTSIZE
	bl dumpMemory*/
	
	ldmia	sp, {a0 - lr}^						// Restore registers a0-lr of register bank
																	// user/system mode onto the stack
	add sp, sp, #PROGRAMCOUNTER_OFFSET		// does not work with ! flags XXX
	
	nop											// Cannot use banked register immediatly after!
	nop
	nop

	ldr	lr, [sp, #4]								// Load CPSR of Context into lr
	msr	spsr_fsxc, lr								// Interrupted process's CPSR 

	ldr	lr, [sp]										// Load PC of PCB into LR_irq/fiq
	add sp, sp, #8

	movs	pc, lr										// return to address in LR_irq, 
																	// with SPSR_irq -> CPSR transfer


/* Restore context of new thread. All registers but ip are restored from the 
 * ProcContext structure in the thread structure reference given by the 
 * first argument to restoreContext(), thus register a0. This function is
 * different from excRestoreContext() because the caller of this function
 * is in the same mode (system) as the restored thread, so there is no SPSR
 * or banked registers which have to be restored :-)
 */ 
ENTRY( restoreContext )
	mov	ip, a0													// since IP has no valid value in it ...
																			// we use it to point on PCB

	/* This is multiple instruction which does the same 
		 as all instructions below, but ...
	 */
	ldmib	ip, {a1 - fp}									// Restore A1 - FP of a new PCB
	
	/*
	ldr	a1, [ip, #ARGUMENT1_OFFSET]			// Restore A1 at PCB location
	ldr	a2, [ip, #ARGUMENT2_OFFSET]			// Restore A2 at PCB location
	ldr	a3, [ip, #ARGUMENT3_OFFSET]			// Restore A3 at PCB location
	ldr	v0, [ip, #SAVEGPR0_OFFSET]			// Restore V0 at PCB location
	ldr	v1, [ip, #SAVEGPR1_OFFSET]			// Restore V1 at PCB location
	ldr	v2, [ip, #SAVEGPR2_OFFSET]			// Restore V2 at PCB location
	ldr	v3, [ip, #SAVEGPR3_OFFSET]			// Restore V3 at PCB location
	ldr	v4, [ip, #SAVEGPR4_OFFSET]			// Restore V4 at PCB location
	ldr	v5, [ip, #SAVEGPR5_OFFSET]	 		// Restore V5 at PCB location
	ldr	sl, [ip, #STACKLIMIT_OFFSET]		// Restore SL at PCB location
	ldr	fp, [ip, #FRAMEPOINTER_OFFSET]	// Restore FP at PCB location
	*/
	
	ldr lr, =exceptionContextFlag					// update exceptionContextFlag
	ldr sp, [lr]
	sub sp, sp, #1
	str sp, [lr]

	ldr	sp, [ip, #STACKPOINTER_OFFSET]		// Restore SP
	ldr	lr, [ip, #LINKREGISTER_OFFSET]		// Restore LR

	ldr	a0, [ip, #STATUSREGISTER_OFFSET]
	msr	cpsr_fsxc, a0											// Restore CPSR

	ldr	a0, [ip, #ARGUMENT0_OFFSET]				// Restore A0
	ldr	pc, [ip, #PROGRAMCOUNTER_OFFSET]	// Restore PC => branch to this thread!



/* This code performs transparently to the user a tmThreadExit() when
 * the main thread routine returns 
 */
ENTRY(automaticThreadExit)
	mov	a1, pc										// a1 = pc + 8
	add	a1, a1, #8								// a1 -> EXIT-MESSAGE
	mov	a3, #dSYSCALL_SEND_OP 		// a3 = SYSCALL_SEND_OP
	swi														// syscall
LABEL(endAutomaticThreadExit)


/* Not used */
ENTRY( disableInterrupts )
	mrs ip, cpsr									// get CPSR
	orr ip, ip, #0xC0							// disable interrupts FIQ & IRQ
	msr	cpsr_c, ip								// write it back to disable all interrupts
END( disbableInterrupts )

ENTRY( enableInterrupts )
	mrs ip, cpsr									// get CPSR
	and ip, ip, #0x3F							// enable interrupts FIQ & IRQ
	msr	cpsr_c, ip								// write it back to disable all interrupts
END( enableInterrupts )
