/*
*********************************************************************************************************
*
*                                     MICIRUM BOARD SUPPORT PACKAGE
*
*                          (c) Copyright 2003-2006; Micrium, Inc.; Weston, FL
*
*               All rights reserved.  Protected by international copyright laws.
*
*               Micrium BSP is provided in source form for FREE evaluation, for educational
*               use or peaceful research.  If you plan on using Micrium BSP in a commercial
*               product you need to contact Micrium to properly license its use in your
*               product.  We provide ALL the source code for your convenience and to help
*               you experience Micrium BSP.  The fact that the source code is provided does
*               NOT mean that you can use it without paying a licensing fee.
*
*               Knowledge of the source code may NOT be used to develop a similar product.
*
*               Please help us continue to provide the Embedded community with the finest
*               software available.  Your honesty is greatly appreciated.
*********************************************************************************************************
*/

/*
*********************************************************************************************************
*
*                                BOARD SUPPORT PACKAGE (BSP) FUNCTIONS
*
*                                         EXCEPTION MANAGEMENT
*
* Filename      : bsp_exception.c
* Version       : V1.88
* Programmer(s) : Jean-Denis Hatier
*********************************************************************************************************
*/

/*
*********************************************************************************************************
*                                             INCLUDE FILES
*********************************************************************************************************
*/

#include  <includes.h>
#include  <reg_AT91RM9200.h>


/*
*********************************************************************************************************
*                                                DEFINES
*********************************************************************************************************
*/

                                                                /* ARM exception IDs                                    */
#define  BSP_CPU_ARM_EXECPT_RESET                                                                    0x00
#define  BSP_CPU_ARM_EXECPT_UNDEF_INSTR                                                              0x01
#define  BSP_CPU_ARM_EXECPT_SWI                                                                      0x02
#define  BSP_CPU_ARM_EXECPT_PREFETCH_ABORT                                                           0x03
#define  BSP_CPU_ARM_EXECPT_DATA_ABORT                                                               0x04
#define  BSP_CPU_ARM_EXECPT_ADDR_ABORT                                                               0x05
#define  BSP_CPU_ARM_EXECPT_IRQ                                                                      0x06
#define  BSP_CPU_ARM_EXECPT_FIQ                                                                      0x07
#define  BSP_CPU_ARM_EXECPT_MAX                                                                      0x08

                                                                /* ARM exception vectors addresses                      */
#define  BSP_CPU_ARM_EXCEPT_RESET_VECT_ADDR              (BSP_CPU_ARM_EXECPT_RESET          * 0x04 + 0x00)
#define  BSP_CPU_ARM_EXCEPT_UNDEF_INSTR_VECT_ADDR        (BSP_CPU_ARM_EXECPT_UNDEF_INSTR    * 0x04 + 0x00)
#define  BSP_CPU_ARM_EXCEPT_SWI_VECT_ADDR                (BSP_CPU_ARM_EXECPT_SWI            * 0x04 + 0x00)
#define  BSP_CPU_ARM_EXCEPT_PREFETCH_ABORT_VECT_ADDR     (BSP_CPU_ARM_EXECPT_PREFETCH_ABORT * 0x04 + 0x00)
#define  BSP_CPU_ARM_EXCEPT_DATA_ABORT_VECT_ADDR         (BSP_CPU_ARM_EXECPT_DATA_ABORT     * 0x04 + 0x00)
#define  BSP_CPU_ARM_EXCEPT_ADDR_ABORT_VECT_ADDR         (BSP_CPU_ARM_EXECPT_ADDR_ABORT     * 0x04 + 0x00)
#define  BSP_CPU_ARM_EXCEPT_IRQ_VECT_ADDR                (BSP_CPU_ARM_EXECPT_IRQ            * 0x04 + 0x00)
#define  BSP_CPU_ARM_EXCEPT_FIQ_VECT_ADDR                (BSP_CPU_ARM_EXECPT_FIQ            * 0x04 + 0x00)

                                                                /* ARM exception handlers addresses                     */
#define  BSP_CPU_ARM_EXCEPT_RESET_HANDLER_ADDR           (BSP_CPU_ARM_EXECPT_RESET          * 0x04 + 0x20)
#define  BSP_CPU_ARM_EXCEPT_UNDEF_INSTR_HANDLER_ADDR     (BSP_CPU_ARM_EXECPT_UNDEF_INSTR    * 0x04 + 0x20)
#define  BSP_CPU_ARM_EXCEPT_SWI_HANDLER_ADDR             (BSP_CPU_ARM_EXECPT_SWI            * 0x04 + 0x20)
#define  BSP_CPU_ARM_EXCEPT_PREFETCH_ABORT_HANDLER_ADDR  (BSP_CPU_ARM_EXECPT_PREFETCH_ABORT * 0x04 + 0x20)
#define  BSP_CPU_ARM_EXCEPT_DATA_ABORT_HANDLER_ADDR      (BSP_CPU_ARM_EXECPT_DATA_ABORT     * 0x04 + 0x20)
#define  BSP_CPU_ARM_EXCEPT_ADDR_ABORT_HANDLER_ADDR      (BSP_CPU_ARM_EXECPT_ADDR_ABORT     * 0x04 + 0x20)
#define  BSP_CPU_ARM_EXCEPT_IRQ_HANDLER_ADDR             (BSP_CPU_ARM_EXECPT_IRQ            * 0x04 + 0x20)
#define  BSP_CPU_ARM_EXCEPT_FIQ_HANDLER_ADDR             (BSP_CPU_ARM_EXECPT_FIQ            * 0x04 + 0x20)

                                                                /* ARM "Jump To Self" assembled instruction             */
#define  BSP_CPU_ARM_INSTR_JUMP_TO_SELF                   0xEAFFFFFE
                                                                /* ARM "Jump To Exception Handler" assembled instruction*/
#define  BSP_CPU_ARM_INSTR_JUMP_TO_HANDLER                0xE59FF018


/*
*********************************************************************************************************
*                                    INITIALIZE INTERRUPT CONTROLLER
*
* Description : This function should be called by your application code before you make use of any of the
*               functions found in this module.
*
* Arguments   : None.
*********************************************************************************************************
*/

void  BSP_InitExceptVect (void)
{
    (*(CPU_INT32U *)BSP_CPU_ARM_EXCEPT_RESET_VECT_ADDR)              = BSP_CPU_ARM_INSTR_JUMP_TO_HANDLER;
    (*(CPU_INT32U *)BSP_CPU_ARM_EXCEPT_RESET_HANDLER_ADDR)           = (CPU_INT32U)OS_CPU_ARM_EXCEPT_RESET_HANDLER;

    (*(CPU_INT32U *)BSP_CPU_ARM_EXCEPT_UNDEF_INSTR_VECT_ADDR)        = BSP_CPU_ARM_INSTR_JUMP_TO_HANDLER;
    (*(CPU_INT32U *)BSP_CPU_ARM_EXCEPT_UNDEF_INSTR_HANDLER_ADDR)     = (CPU_INT32U)OS_CPU_ARM_EXCEPT_UNDEF_INSTR_HANDLER;

    (*(CPU_INT32U *)BSP_CPU_ARM_EXCEPT_SWI_VECT_ADDR)                = BSP_CPU_ARM_INSTR_JUMP_TO_HANDLER;
    (*(CPU_INT32U *)BSP_CPU_ARM_EXCEPT_SWI_HANDLER_ADDR)             = (CPU_INT32U)OS_CPU_ARM_EXCEPT_SWI_HANDLER;

    (*(CPU_INT32U *)BSP_CPU_ARM_EXCEPT_PREFETCH_ABORT_VECT_ADDR)     = BSP_CPU_ARM_INSTR_JUMP_TO_HANDLER;
    (*(CPU_INT32U *)BSP_CPU_ARM_EXCEPT_PREFETCH_ABORT_HANDLER_ADDR)  = (CPU_INT32U)OS_CPU_ARM_EXCEPT_PREFETCH_ABORT_HANDLER;

    (*(CPU_INT32U *)BSP_CPU_ARM_EXCEPT_DATA_ABORT_VECT_ADDR)         = BSP_CPU_ARM_INSTR_JUMP_TO_HANDLER;
    (*(CPU_INT32U *)BSP_CPU_ARM_EXCEPT_DATA_ABORT_HANDLER_ADDR)      = (CPU_INT32U)OS_CPU_ARM_EXCEPT_DATA_ABORT_HANDLER;

    (*(CPU_INT32U *)BSP_CPU_ARM_EXCEPT_ADDR_ABORT_VECT_ADDR)         = BSP_CPU_ARM_INSTR_JUMP_TO_HANDLER;
    (*(CPU_INT32U *)BSP_CPU_ARM_EXCEPT_ADDR_ABORT_HANDLER_ADDR)      = (CPU_INT32U)OS_CPU_ARM_EXCEPT_ADDR_ABORT_HANDLER;

    (*(CPU_INT32U *)BSP_CPU_ARM_EXCEPT_IRQ_VECT_ADDR)                = BSP_CPU_ARM_INSTR_JUMP_TO_HANDLER;
    (*(CPU_INT32U *)BSP_CPU_ARM_EXCEPT_IRQ_HANDLER_ADDR)             = (CPU_INT32U)OS_CPU_ARM_EXCEPT_IRQ_HANDLER;

    (*(CPU_INT32U *)BSP_CPU_ARM_EXCEPT_FIQ_VECT_ADDR)                = BSP_CPU_ARM_INSTR_JUMP_TO_HANDLER;
    (*(CPU_INT32U *)BSP_CPU_ARM_EXCEPT_FIQ_HANDLER_ADDR)             = (CPU_INT32U)OS_CPU_ARM_EXCEPT_FIQ_HANDLER;
}


/*
*********************************************************************************************************
*                                          EXCEPTION HANDLER
*
* Arguments   : None.
*********************************************************************************************************
*/

typedef  void  (*BSP_PFNCT)(void);

void  OS_EXCEPT_HANDLER (CPU_DATA  except_id)
{
    BSP_PFNCT    pfnct;
    CPU_INT32U  *sp;


    if ((except_id == BSP_CPU_ARM_EXECPT_FIQ) ||
        (except_id == BSP_CPU_ARM_EXECPT_IRQ)) {
        pfnct = (BSP_PFNCT)*AT91C_AIC_IVR;                      /* Read the interrupt vector from the AIC.              */
        if (pfnct != (BSP_PFNCT)0) {                            /* Make sure we don't have a NULL pointer.              */
            (*pfnct)();                                         /* Execute the ISR for the interrupting device.         */
        }

    } else {
        sp = (CPU_INT32U *)OSTCBCur->OSTCBStkPtr;
        printf("\nCPU_ARM_EXCEPTION #%d trapped.\n", except_id);
        printf("R0  : 0x%08x\n", *(sp + 0x01));
        printf("R1  : 0x%08x\n", *(sp + 0x02));
        printf("R2  : 0x%08x\n", *(sp + 0x03));
        printf("R3  : 0x%08x\n", *(sp + 0x04));
        printf("R4  : 0x%08x\n", *(sp + 0x05));
        printf("R5  : 0x%08x\n", *(sp + 0x06));
        printf("R6  : 0x%08x\n", *(sp + 0x07));
        printf("R7  : 0x%08x\n", *(sp + 0x08));
        printf("R8  : 0x%08x\n", *(sp + 0x09));
        printf("R9  : 0x%08x\n", *(sp + 0x0a));
        printf("R10 : 0x%08x\n", *(sp + 0x0b));
        printf("R11 : 0x%08x\n", *(sp + 0x0c));
        printf("R12 : 0x%08x\n", *(sp + 0x0d));
        printf("SP  : 0x%08x\n", sp);
        printf("LR  : 0x%08x\n", *(sp + 0x0e));
        printf("PC  : 0x%08x\n", *(sp + 0x0f));
        printf("CPSR: 0x%08x\n", *(sp + 0x00));

                                                                /* Infinite loop on other exceptions.                   */
                                                                /* Should be replaced by other behavior (reboot, etc.)  */
        while (DEF_TRUE);
    }
}