swi270.asm

dm270 source code

;/*
;    DM270 ARM Evaluation Software
;
;    (c)Texas Instruments 2003
;*/

	.global	$go_swi
	.global	_go_swi
	.global	_EnableIRQ
	.global	_DisableIRQ
	.global	_EnableFIQ
	.global	_DisableFIQ
	.global	_SWI_Entry

;/*******************************************************************/
;/*		Branch to SWI routine                                       */
;/*******************************************************************/
	.state16
$go_swi:
	LDR		r0, [sp, #4]				;get pushed PC
	ADR		r1, _get_swi
	BX		r1

	.state32
_go_swi:
	LDR		r0, [sp]					;get pushed PC

_get_swi:
	NOP
	SUB 	r0, r0, #2					;
	MRS		r1, spsr
	TST 	r1, #0x20
	BNE		_from_thumb

_from_arm:
	SUB		r0, r0, #2
	LDR		r0, [r0]
	B 		_swi_id

_from_thumb:
	LDRH	r0, [r0]					;get SWI instruction

_swi_id:
	MOV		r1, #0xff					;
	AND		r0, r0, r1					;extract comment field
	MOV		r0, r0, LSL #2				;
	LDR		r1, swi_entry_base
	ADD		r0, r0, r1					;load base address of SWI table			

	MOV 	PC,	r0

swi_entry_base	.long 	_SWI_Entry

;/*******************************************************************/
;/*   ARM IRQ enable                                                */
;/*******************************************************************/

	.state32
_EnableIRQ:
	;*------------------------------------------------------
	;* clear IRQ disable bit
	;*------------------------------------------------------
	MRS		r0, spsr
	BIC		r0, r0, #0x80
	MSR		spsr, r0	

	B		_go_ret
	
;/*******************************************************************/
;/*   ARM IRQ disable                                               */
;/*******************************************************************/

	.state32
_DisableIRQ:
	;*------------------------------------------------------
	;* set IRQ disable bit
	;*------------------------------------------------------
	MRS		r0, spsr
	ORR		r0, r0, #0x80	;set IRQ bit
	MSR		spsr, r0		;disable IRQ 

	B 		_go_ret

;/*******************************************************************/
;/*   ARM FIQ enable                                                */
;/*******************************************************************/

	.state32
_EnableFIQ:
	;*------------------------------------------------------
	;* clear FIQ disable bit
	;*------------------------------------------------------
	MRS		r0, spsr
	BIC		r0, r0, #0x40
	MSR		spsr, r0	

	B		_go_ret
	
;/*******************************************************************/
;/*   ARM FIQ disable                                               */
;/*******************************************************************/

	.state32
_DisableFIQ:
	;*------------------------------------------------------
	;* set FIQ disable bit
	;*------------------------------------------------------
	MRS		r0, spsr
	ORR		r0, r0, #0x40	;set FRQ bit
	MSR		spsr, r0		;disable IRQ 

	B 		_go_ret


	.state32
_go_ret:
	TST		lr, #0x1
	BNE		_to_thumb
	
_to_arm:
	MOV		pc, lr
	
_to_thumb:
	BX		lr