Changes to resulting previous merge of arch/c5471 and arch/dm320 into arch/arm and

also to adding lpc214x to arch/arm.


git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@194 42af7a65-404d-4744-a932-0658087f49c3
This commit is contained in:
patacongo 2007-04-29 21:50:53 +00:00
parent af000ee096
commit a19bd351b5
12 changed files with 139 additions and 611 deletions

View File

@ -146,8 +146,15 @@ arch/arm
NuttX operates on the ARM9EJS of this dual core processor. This port NuttX operates on the ARM9EJS of this dual core processor. This port
complete, verified, and included in the NuttX release 0.2.1. complete, verified, and included in the NuttX release 0.2.1.
arch/arm/include/lpc214x and arch/arm/src/lpc214x
These directories provide support for NXP LPC214x family of
processors.
STATUS: This port is in progress and should be available in the
nuttx-0.2.5 release.
arch/m68322 arch/m68322
A work in progress. A work in progress.
STATUS: Stalled for the moment.
arch/pjrc-8051 arch/pjrc-8051
8051 Microcontroller. This port is not quite ready for prime time. 8051 Microcontroller. This port is not quite ready for prime time.

View File

@ -99,10 +99,10 @@ ifeq ($(CONFIG_RRLOAD_BINARY),y)
fi fi
endif endif
.depend: Makefile $(SRCS) .depend: Makefile chip/Make.defs $(SRCS)
@if [ -e board/Makefile ]; then \ @if [ -e board/Makefile ]; then \
$(MAKE) -C board TOPDIR=$(TOPDIR) depend ; \ $(MAKE) -C board TOPDIR=$(TOPDIR) depend ; \
if fi
$(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@ @touch $@

View File

@ -37,12 +37,12 @@ HEAD_ASRC = up_nommuhead.S
CMN_ASRCS = up_saveusercontext.S up_fullcontextrestore.S CMN_ASRCS = up_saveusercontext.S up_fullcontextrestore.S
CMN_CSRCS = up_allocateheap.c up_assert.c up_blocktask.c up_copystate.c \ CMN_CSRCS = up_allocateheap.c up_assert.c up_blocktask.c up_copystate.c \
up_createstack.c up_dataabort.c up_delay.c up_exit.c up_idle.c \ up_createstack.c up_dataabort.c up_delay.c up_doirq.c \
up_initialize.c up_initialstate.c up_interruptcontext.c \ up_exit.c up_idle.c up_initialize.c up_initialstate.c \
up_prefetchabort.c up_releasepending.c up_releasestack.c \ up_interruptcontext.c up_prefetchabort.c up_releasepending.c \
up_reprioritizertr.c up_schedulesigaction.c up_sigdeliver.c \ up_releasestack.c up_reprioritizertr.c up_schedulesigaction.c \
up_syscall.c up_unblocktask.c up_undefinedinsn.c up_usestack.c up_sigdeliver.c up_syscall.c up_unblocktask.c \
up_undefinedinsn.c up_usestack.c
CHIP_ASRCS = c5471_lowputc.S c5471_vectors.S CHIP_ASRCS = c5471_lowputc.S c5471_vectors.S
CHIP_CSRCS = c5471_doirq.c c5471_irq.c c5471_serial.c c5471_timerisr.c \ CHIP_CSRCS = c5471_irq.c c5471_serial.c c5471_timerisr.c c5471_watchdog.c
c5471_watchdog.c

View File

@ -1,104 +0,0 @@
/************************************************************
* c5471/c5471_doirq.c
*
* Copyright (C) 2007 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name Gregory Nutt nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
************************************************************/
/************************************************************
* Included Files
************************************************************/
#include <nuttx/config.h>
#include <sys/types.h>
#include <nuttx/irq.h>
#include <nuttx/arch.h>
#include <assert.h>
#include "up_arch.h"
#include "os_internal.h"
#include "up_internal.h"
/************************************************************
* Definitions
************************************************************/
/************************************************************
* Public Data
************************************************************/
/************************************************************
* Private Data
************************************************************/
/************************************************************
* Private Functions
************************************************************/
/************************************************************
* Public Funtions
************************************************************/
void c5471_doirq(int irq, uint32* regs)
{
up_ledon(LED_INIRQ);
#ifdef CONFIG_SUPPRESS_INTERRUPTS
PANIC(OSERR_ERREXCEPTION);
#else
if ((unsigned)irq < NR_IRQS)
{
/* Current regs non-zero indicates that we are processing
* an interrupt; current_regs is also used to manage
* interrupt level context switches.
*/
current_regs = regs;
/* Mask and acknowledge the interrupt */
up_maskack_irq(irq);
/* Deliver the IRQ */
irq_dispatch(irq, regs);
/* Indicate that we are no long in an interrupt handler */
current_regs = NULL;
/* Unmask the last interrupt (global interrupts are still
* disabled.
*/
up_enable_irq(irq);
}
up_ledoff(LED_INIRQ);
#endif
}

View File

@ -156,7 +156,7 @@ up_vectorirq:
mov fp, #0 /* Init frame pointer */ mov fp, #0 /* Init frame pointer */
mov r1, sp /* Get r1=xcp */ mov r1, sp /* Get r1=xcp */
bl c5471_doirq /* Call the handler */ bl up_doirq /* Call the handler */
/* Restore the CPSR, SVC modr registers and return */ /* Restore the CPSR, SVC modr registers and return */
.Lnoirqset: .Lnoirqset:

View File

@ -316,8 +316,4 @@
* Public Function Prototypes * Public Function Prototypes
************************************************************/ ************************************************************/
#ifndef __ASSEMBLY__
extern void c5471_doirq(int irq, uint32* regs);
#endif
#endif /* __C5471_CHIP_H */ #endif /* __C5471_CHIP_H */

View File

@ -101,7 +101,8 @@ extern void up_boot(void);
extern void up_copystate(uint32 *dest, uint32 *src); extern void up_copystate(uint32 *dest, uint32 *src);
extern void up_dataabort(uint32 *regs); extern void up_dataabort(uint32 *regs);
extern void up_delay(int milliseconds); extern void up_delay(int milliseconds);
extern void up_doirq(uint32 *regs); extern void up_decodeirq(uint32 *regs);
extern void up_doirq(int irq, uint32 *regs);
extern void up_fullcontextrestore(uint32 *regs) __attribute__ ((noreturn)); extern void up_fullcontextrestore(uint32 *regs) __attribute__ ((noreturn));
extern void up_irqinitialize(void); extern void up_irqinitialize(void);
extern void up_prefetchabort(uint32 *regs); extern void up_prefetchabort(uint32 *regs);

View File

@ -128,7 +128,7 @@ up_vectorirq:
mov fp, #0 /* Init frame pointer */ mov fp, #0 /* Init frame pointer */
mov r0, sp /* Get r0=xcp */ mov r0, sp /* Get r0=xcp */
bl up_doirq /* Call the handler */ bl up_decodeirq /* Call the handler */
/* Restore the CPSR, SVC modr registers and return */ /* Restore the CPSR, SVC modr registers and return */
.Lnoirqset: .Lnoirqset:
@ -138,6 +138,7 @@ up_vectorirq:
.Lirqtmp: .Lirqtmp:
.word g_irqtmp .word g_irqtmp
.size up_vectorirq, . - up_vectorirq
.align 5 .align 5
@ -183,6 +184,7 @@ up_vectorswi:
ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */ ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */
msr spsr, r0 msr spsr, r0
ldmia sp, {r0-r15}^ /* Return */ ldmia sp, {r0-r15}^ /* Return */
.size up_vectorswi, . - up_vectorswi
.align 5 .align 5
@ -251,6 +253,7 @@ up_vectordata:
.Ldaborttmp: .Ldaborttmp:
.word g_aborttmp .word g_aborttmp
.size up_vectordata, . - up_vectordata
.align 5 .align 5
@ -317,6 +320,7 @@ up_vectorprefetch:
.Lpaborttmp: .Lpaborttmp:
.word g_aborttmp .word g_aborttmp
.size up_vectorprefetch, . - up_vectorprefetch
.align 5 .align 5
@ -383,6 +387,7 @@ up_vectorundefinsn:
.Lundeftmp: .Lundeftmp:
.word g_undeftmp .word g_undeftmp
.size up_vectorundefinsn, . - up_vectorundefinsn
.align 5 .align 5
@ -397,6 +402,9 @@ up_vectorundefinsn:
.type up_vectorfiq, %function .type up_vectorfiq, %function
up_vectorfiq: up_vectorfiq:
subs pc, lr, #4 subs pc, lr, #4
.size up_vectofiq, . - up_vectorfiq
.align 5
/******************************************************************** /********************************************************************
* Name: up_vectoraddrexcption * Name: up_vectoraddrexcption
@ -410,40 +418,5 @@ up_vectorfiq:
.type up_vectoraddrexcptn, %function .type up_vectoraddrexcptn, %function
up_vectoraddrexcptn: up_vectoraddrexcptn:
b up_vectoraddrexcptn b up_vectoraddrexcptn
.size up_vectoaddrexcptn, . - up_vectoraddrexcptn
/**************************************************************************
* Vector initialization block.
**************************************************************************/
/* These will be relocated to VECTOR_BASE. */
.globl _vector_start
_vector_start:
ldr pc, .Lresethandler /* 0x00: Reset */
ldr pc, .Lundefinedhandler /* 0x04: Undefined instruction */
ldr pc, .Lswihandler /* 0x08: Software interrupt */
ldr pc, .Lprefetchaborthandler /* 0x0c: Prefetch abort */
ldr pc, .Ldataaborthandler /* 0x10: Data abort */
ldr pc, .Laddrexcptnhandler /* 0x14: Address exception */
ldr pc, .Lirqhandler /* 0x18: IRQ */
ldr pc, .Lfiqhandler /* 0x1c: FIQ */
.Lresethandler:
.long __start
.Lundefinedhandler:
.long up_vectorundefinsn
.Lswihandler:
.long up_vectorswi
.Lprefetchaborthandler:
.long up_vectorprefetch
.Ldataaborthandler:
.long up_vectordata
.Laddrexcptnhandler:
.long up_vectoraddrexcptn
.Lirqhandler:
.long up_vectorirq
.Lfiqhandler:
.long up_vectorfiq
.globl _vector_end
_vector_end:
.end .end

View File

@ -0,0 +1,103 @@
/********************************************************************
* common/up_vectortab.S
*
* Copyright (C) 2007 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name Gregory Nutt nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
********************************************************************/
/********************************************************************
* Included Files
********************************************************************/
#include <nuttx/config.h>
/********************************************************************
* Definitions
********************************************************************/
/********************************************************************
* Global Data
********************************************************************/
/********************************************************************
* Assembly Macros
********************************************************************/
/********************************************************************
* Name: _vector_start
*
* Description:
* Vector initialization block
********************************************************************/
.globl _vector_start
/* These will be relocated to VECTOR_BASE. */
_vector_start:
ldr pc, .Lresethandler /* 0x00: Reset */
ldr pc, .Lundefinedhandler /* 0x04: Undefined instruction */
ldr pc, .Lswihandler /* 0x08: Software interrupt */
ldr pc, .Lprefetchaborthandler /* 0x0c: Prefetch abort */
ldr pc, .Ldataaborthandler /* 0x10: Data abort */
ldr pc, .Laddrexcptnhandler /* 0x14: Address exception */
ldr pc, .Lirqhandler /* 0x18: IRQ */
ldr pc, .Lfiqhandler /* 0x1c: FIQ */
.globl __start
.globl up_vectorundefinsn
.globl up_vectorswi
.globl up_vectorprefetch
.globl up_vectordata
.globl up_vectoraddrexcptn
.globl up_vectorirq
.globl up_vectorfiq
.Lresethandler:
.long __start
.Lundefinedhandler:
.long up_vectorundefinsn
.Lswihandler:
.long up_vectorswi
.Lprefetchaborthandler:
.long up_vectorprefetch
.Ldataaborthandler:
.long up_vectordata
.Laddrexcptnhandler:
.long up_vectoraddrexcptn
.Lirqhandler:
.long up_vectorirq
.Lfiqhandler:
.long up_vectorfiq
.globl _vector_end
_vector_end:
.end

View File

@ -35,7 +35,8 @@
HEAD_ASRC = up_head.S HEAD_ASRC = up_head.S
CMN_ASRCS = up_cache.S up_fullcontextrestore.S up_saveusercontext.S CMN_ASRCS = up_cache.S up_fullcontextrestore.S up_saveusercontext.S \
up_vectors.S up_vectortab.S
CMN_CSRCS = up_assert.c up_blocktask.c up_copystate.c up_createstack.c \ CMN_CSRCS = up_assert.c up_blocktask.c up_copystate.c up_createstack.c \
up_dataabort.c up_delay.c up_exit.c up_idle.c \ up_dataabort.c up_delay.c up_exit.c up_idle.c \
up_initialize.c up_initialstate.c up_interruptcontext.c \ up_initialize.c up_initialstate.c up_interruptcontext.c \
@ -44,7 +45,7 @@ CMN_CSRCS = up_assert.c up_blocktask.c up_copystate.c up_createstack.c \
up_sigdeliver.c up_syscall.c up_unblocktask.c \ up_sigdeliver.c up_syscall.c up_unblocktask.c \
up_undefinedinsn.c up_usestack.c up_undefinedinsn.c up_usestack.c
CHIP_ASRCS = dm320_lowputc.S dm320_restart.S dm320_vectors.S CHIP_ASRCS = dm320_lowputc.S dm320_restart.S
CHIP_CSRCS = dm320_allocateheap.c dm320_boot.c dm320_doirq.c dm320_irq.c \ CHIP_CSRCS = dm320_allocateheap.c dm320_boot.c dm320_decodeirq.c \
dm320_serial.c dm320_timerisr.c dm320_irq.c dm320_serial.c dm320_timerisr.c

View File

@ -1,5 +1,5 @@
/******************************************************************************** /********************************************************************************
* dm320/dm320_doirq.c * dm320/dm320_decodeirq.c
* *
* Copyright (C) 2007 Gregory Nutt. All rights reserved. * Copyright (C) 2007 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr> * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
@ -67,7 +67,7 @@
* Public Funtions * Public Funtions
********************************************************************************/ ********************************************************************************/
void up_doirq(uint32* regs) void up_decodeirq(uint32* regs)
{ {
#ifdef CONFIG_SUPPRESS_INTERRUPTS #ifdef CONFIG_SUPPRESS_INTERRUPTS
lib_lowprintf("Unexpected IRQ\n"); lib_lowprintf("Unexpected IRQ\n");

View File

@ -1,449 +0,0 @@
/********************************************************************
* dm320/dm320_vectors.S
*
* Copyright (C) 2007 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name Gregory Nutt nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
********************************************************************/
/********************************************************************
* Included Files
********************************************************************/
#include <nuttx/config.h>
#include <nuttx/irq.h>
#include "up_arch.h"
/********************************************************************
* Definitions
********************************************************************/
/********************************************************************
* Global Data
********************************************************************/
.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 */
/********************************************************************
* Assembly Macros
********************************************************************/
/********************************************************************
* Private Functions
********************************************************************/
.text
/********************************************************************
* Public Functions
********************************************************************/
.text
/********************************************************************
* Name: up_vectorirq
*
* Description:
* Interrupt excetpion. Entered in IRQ mode with spsr = SVC
* CPSR, lr = SVC PC
********************************************************************/
.globl up_vectorirq
.type up_vectorirq, %function
up_vectorirq:
/* 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 */
bl up_doirq /* Call the handler */
/* Restore the CPSR, SVC modr registers and return */
.Lnoirqset:
ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */
msr spsr, r0
ldmia sp, {r0-r15}^ /* Return */
.Lirqtmp:
.word g_irqtmp
.align 5
/********************************************************************
* Function: up_vectorswi
*
* Description:
* SWI interrupt. We enter the SWI in SVC mode
********************************************************************/
.globl up_vectorswi
.type up_vectorswi, %function
up_vectorswi:
/* 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), r14(lr), r15(pc)
* and CPSR in r1-r4 */
add r1, sp, #XCPTCONTEXT_SIZE
mov r2, r14 /* R14 is altered on return from SWI */
mov r3, r14 /* Save r14 as the PC as well */
mrs r4, spsr /* Get the saved CPSR */
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
/* Then call the SWI handler with interrupt disabled.
* void up_syscall(struct xcptcontext *xcp)
*/
mov fp, #0 /* Init frame pointer */
mov r0, sp /* Get r0=xcp */
bl up_syscall /* Call the handler */
/* Restore the CPSR, SVC modr registers and return */
ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */
msr spsr, r0
ldmia sp, {r0-r15}^ /* Return */
.align 5
/********************************************************************
* Name: up_vectordata
*
* Description:
* Data abort Exception dispatcher. Give control to data
* abort handler. This function is entered in ABORT mode
* with spsr = SVC CPSR, lr = SVC PC
*
********************************************************************/
.globl up_vectordata
.type up_vectordata, %function
up_vectordata:
/* On entry we are free to use the ABORT mode registers
* r13 and r14
*/
ldr r13, .Ldaborttmp /* Points to temp storage */
sub lr, lr, #8 /* Fixup return */
str lr, [r13] /* Save in temp storage */
mrs lr, spsr /* Get SPSR */
str lr, [r13, #4] /* Save in temp storage */
/* 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, .Ldaborttmp /* 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 data abort handler with interrupt disabled.
* void up_dataabort(struct xcptcontext *xcp)
*/
mov fp, #0 /* Init frame pointer */
mov r0, sp /* Get r0=xcp */
bl up_dataabort /* Call the handler */
/* Restore the CPSR, SVC modr registers and return */
ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */
msr spsr_cxsf, r0
ldmia sp, {r0-r15}^ /* Return */
.Ldaborttmp:
.word g_aborttmp
.align 5
/********************************************************************
* Name: up_vectorprefetch
*
* Description:
* Prefetch abort exception. Entered in ABT mode with
* spsr = SVC CPSR, lr = SVC PC
********************************************************************/
.globl up_vectorprefetch
.type up_vectorprefetch, %function
up_vectorprefetch:
/* On entry we are free to use the ABORT mode registers
* r13 and r14
*/
ldr r13, .Lpaborttmp /* Points to temp storage */
sub lr, lr, #4 /* Fixup return */
str lr, [r13] /* Save in temp storage */
mrs lr, spsr /* Get SPSR */
str lr, [r13, #4] /* Save in temp storage */
/* 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, .Lpaborttmp /* 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 prefetch abort handler with interrupt disabled.
* void up_prefetchabort(struct xcptcontext *xcp)
*/
mov fp, #0 /* Init frame pointer */
mov r0, sp /* Get r0=xcp */
bl up_prefetchabort /* Call the handler */
/* Restore the CPSR, SVC modr registers and return */
ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */
msr spsr_cxsf, r0
ldmia sp, {r0-r15}^ /* Return */
.Lpaborttmp:
.word g_aborttmp
.align 5
/********************************************************************
* Name: up_vectorundefinsn
*
* Description:
* Undefined instruction entry exception. Entered in
* UND mode, spsr = SVC CPSR, lr = SVC PC
*
********************************************************************/
.globl up_vectorundefinsn
.type up_vectorundefinsn, %function
up_vectorundefinsn:
/* On entry we are free to use the UND mode registers
* r13 and r14
*/
ldr r13, .Lundeftmp /* Points to temp storage */
str lr, [r13] /* Save in temp storage */
mrs lr, spsr /* Get SPSR */
str lr, [r13, #4] /* Save in temp storage */
/* 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, .Lundeftmp /* 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 undef insn handler with interrupt disabled.
* void up_undefinedinsn(struct xcptcontext *xcp)
*/
mov fp, #0 /* Init frame pointer */
mov r0, sp /* Get r0=xcp */
bl up_undefinedinsn /* Call the handler */
/* Restore the CPSR, SVC modr registers and return */
ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */
msr spsr_cxsf, r0
ldmia sp, {r0-r15}^ /* Return */
.Lundeftmp:
.word g_undeftmp
.align 5
/********************************************************************
* Name: up_vectorfiq
*
* Description:
* Shouldn't happen
********************************************************************/
.globl up_vectorfiq
.type up_vectorfiq, %function
up_vectorfiq:
subs pc, lr, #4
/********************************************************************
* Name: up_vectoraddrexcption
*
* Description:
* Shouldn't happen
*
********************************************************************/
.globl up_vectoraddrexcptn
.type up_vectoraddrexcptn, %function
up_vectoraddrexcptn:
b up_vectoraddrexcptn
/**************************************************************************
* Vector initialization block.
**************************************************************************/
/* These will be relocated to VECTOR_BASE. */
.globl _vector_start
_vector_start:
ldr pc, .Lresethandler /* 0x00: Reset */
ldr pc, .Lundefinedhandler /* 0x04: Undefined instruction */
ldr pc, .Lswihandler /* 0x08: Software interrupt */
ldr pc, .Lprefetchaborthandler /* 0x0c: Prefetch abort */
ldr pc, .Ldataaborthandler /* 0x10: Data abort */
ldr pc, .Laddrexcptnhandler /* 0x14: Address exception */
ldr pc, .Lirqhandler /* 0x18: IRQ */
ldr pc, .Lfiqhandler /* 0x1c: FIQ */
.Lresethandler:
.long __start
.Lundefinedhandler:
.long up_vectorundefinsn
.Lswihandler:
.long up_vectorswi
.Lprefetchaborthandler:
.long up_vectorprefetch
.Ldataaborthandler:
.long up_vectordata
.Laddrexcptnhandler:
.long up_vectoraddrexcptn
.Lirqhandler:
.long up_vectorirq
.Lfiqhandler:
.long up_vectorfiq
.globl _vector_end
_vector_end:
.end