Subversion Repositories or1k

/*

 *  ARM CPU Dependent Source

 *

 *

 *  COPYRIGHT (c) 2000 Canon Research Centre France SA.

 *  Emmanuel Raguet, mailto:raguet@crf.canon.fr

 *

 *  Copyright (c) 2002 Advent Networks, Inc

 *      Jay Monkman <jmonkman@adventnetworks.com>

 *

 *  The license and distribution terms for this file may be

 *  found in the file LICENSE in this distribution or at

 *  http://www.OARcorp.com/rtems/license.html.

 *

 */

#include <rtems/system.h>

#include <rtems.h>

#include <rtems/bspIo.h>

#include <rtems/score/isr.h>

#include <rtems/score/wkspace.h>

#include <rtems/score/thread.h>

#include <rtems/score/cpu.h>

/*  _CPU_Initialize

 *

 *  This routine performs processor dependent initialization.

 *

 *  INPUT PARAMETERS:

 *    cpu_table       - CPU table to initialize

 *    thread_dispatch - address of ISR disptaching routine (unused)

 *

 */

unsigned32 g_data_abort_cnt = 0;

unsigned32 g_data_abort_insn_list[1024];

void _CPU_Initialize(

  rtems_cpu_table  *cpu_table,

  void      (*thread_dispatch)      /* ignored on this CPU */

)

{

    _CPU_Table = *cpu_table;

}

/*

 *

 *  _CPU_ISR_Get_level - returns the current interrupt level

 */

unsigned32 _CPU_ISR_Get_level( void )

{

    unsigned32 reg;

    asm volatile ("mrs  %0, cpsr \n"           \

                  "and  %0,  %0, #0xc0 \n"     \

                  : "=r" (reg)                 \

                  : "0" (reg) );

    return reg;

}

/*

 *  _CPU_ISR_install_vector

 *

 *  This kernel routine installs the RTEMS handler for the

 *  specified vector.

 *

 *  Input parameters:

 *    vector      - interrupt vector number

 *    new_handler - replacement ISR for this vector number

 *    old_handler - pointer to store former ISR for this vector number

 *

 *  FIXME: This vector scheme should be changed to allow FIQ to be

 *         handled better. I'd like to be able to put VectorTable

 *         elsewhere - JTM

 *

 *

 *  Output parameters:  NONE

 *

 */

void _CPU_ISR_install_vector(

  unsigned32  vector,

  proc_ptr    new_handler,

  proc_ptr   *old_handler

)

{

    /* pointer on the redirection table in RAM */

    long *VectorTable = (long *)(MAX_EXCEPTIONS * 4);

    if (old_handler != NULL) {

        old_handler = *(proc_ptr *)(VectorTable + vector);

    }

    *(VectorTable + vector) = (long)new_handler ;

}

void _CPU_Context_Initialize(

  Context_Control  *the_context,

  unsigned32       *stack_base,

  unsigned32        size,

  unsigned32        new_level,

  void             *entry_point,

  boolean           is_fp

)

{

    the_context->register_sp = (unsigned32)stack_base + size ;

    the_context->register_lr = (unsigned32)entry_point;

    the_context->register_cpsr = new_level | 0x13;

}

/*

 *  _CPU_Install_interrupt_stack - this function is empty since the

 *  BSP must set up the interrupt stacks.

 */

void _CPU_Install_interrupt_stack( void )

{

}

void _defaultExcHandler (CPU_Exception_frame *ctx)

{

    printk("\n\r");

    printk("----------------------------------------------------------\n\r");

#if 0

    printk("Exception 0x%x caught at PC 0x%x by thread %d\n",

           ctx->register_pc, ctx->register_lr - 4,

           _Thread_Executing->Object.id);

#endif

    printk("----------------------------------------------------------\n\r");

    printk("Processor execution context at time of the fault was  :\n\r");

    printk("----------------------------------------------------------\n\r");

#if 0

    printk(" r0  = %8x  r1  = %8x  r2  = %8x  r3  = %8x\n\r",

           ctx->register_r0, ctx->register_r1,

           ctx->register_r2, ctx->register_r3);

    printk(" r4  = %8x  r5  = %8x  r6  = %8x  r7  = %8x\n\r",

           ctx->register_r4, ctx->register_r5,

           ctx->register_r6, ctx->register_r7);

    printk(" r8  = %8x  r9  = %8x  r10 = %8x\n\r",

           ctx->register_r8, ctx->register_r9, ctx->register_r10);

    printk(" fp  = %8x  ip  = %8x  sp  = %8x  pc  = %8x\n\r",

           ctx->register_fp, ctx->register_ip,

           ctx->register_sp, ctx->register_lr - 4);

    printk("----------------------------------------------------------\n\r");

#endif    

    if (_ISR_Nest_level > 0) {

        /*

         * In this case we shall not delete the task interrupted as

         * it has nothing to do with the fault. We cannot return either

         * because the eip points to the faulty instruction so...

         */

        printk("Exception while executing ISR!!!. System locked\n\r");

        while(1);

    }

    else {

        printk("*********** FAULTY THREAD WILL BE DELETED **************\n\r");

        rtems_task_delete(_Thread_Executing->Object.id);

    }

}

cpuExcHandlerType _currentExcHandler = _defaultExcHandler;

extern void _Exception_Handler_Undef_Swi();

extern void _Exception_Handler_Abort();

extern void _exc_data_abort();

/* FIXME: put comments here */

void rtems_exception_init_mngt()

{

        ISR_Level level;

      _CPU_ISR_Disable(level);

      _CPU_ISR_install_vector(ARM_EXCEPTION_UNDEF,

                              _Exception_Handler_Undef_Swi,

                              NULL);

      _CPU_ISR_install_vector(ARM_EXCEPTION_SWI,

                              _Exception_Handler_Undef_Swi,

                              NULL);

      _CPU_ISR_install_vector(ARM_EXCEPTION_PREF_ABORT,

                              _Exception_Handler_Abort,

                              NULL);

      _CPU_ISR_install_vector(ARM_EXCEPTION_DATA_ABORT,

                              _exc_data_abort,

                              NULL);

      _CPU_ISR_install_vector(ARM_EXCEPTION_FIQ,

                              _Exception_Handler_Abort,

                              NULL);

      _CPU_ISR_install_vector(ARM_EXCEPTION_IRQ,

                              _Exception_Handler_Abort,

                              NULL);

      _CPU_ISR_Enable(level);

}

#define INSN_MASK         0xc5

#define INSN_STM1         0x80

#define INSN_STM2         0x84

#define INSN_STR          0x40

#define INSN_STRB         0x44

#define INSN_LDM1         0x81

#define INSN_LDM23        0x85

#define INSN_LDR          0x41

#define INSN_LDRB         0x45

#define GET_RD(x)         ((x & 0x0000f000) >> 12)

#define GET_RN(x)         ((x & 0x000f0000) >> 16)

#define GET_U(x)              ((x & 0x00800000) >> 23)

#define GET_I(x)              ((x & 0x02000000) >> 25)

#define GET_REG(r, ctx)      (((unsigned32 *)ctx)[r])

#define SET_REG(r, ctx, v)   (((unsigned32 *)ctx)[r] = v)

#define GET_OFFSET(insn)     (insn & 0xfff)

/* This function is supposed to figure out what caused the

 * data abort, do that, then return.

 *

 * All unhandled instructions cause the system to hang.

 */

void do_data_abort(unsigned32 insn, unsigned32 spsr,

                   CPU_Exception_frame *ctx)

{

    unsigned8  decode;

    unsigned8  insn_type;

    unsigned32 rn;

    unsigned32 rd;

    unsigned8  *src_addr;

    unsigned8  *dest_addr;

    unsigned32  tmp;

    g_data_abort_insn_list[g_data_abort_cnt & 0x3ff] = ctx->register_lr - 8;

    g_data_abort_cnt++;

    decode = ((insn >> 20) & 0xff);

    insn_type = decode & INSN_MASK;

    switch(insn_type) {

    case INSN_STM1:

        printk("\n\nINSN_STM1\n");

        break;

    case INSN_STM2:

        printk("\n\nINSN_STM2\n");

        break;

    case INSN_STR:

        printk("\n\nINSN_STR\n");

        break;

    case INSN_STRB:

        printk("\n\nINSN_STRB\n");

        break;

    case INSN_LDM1:

        printk("\n\nINSN_LDM1\n");

        break;

    case INSN_LDM23:

        printk("\n\nINSN_LDM23\n");

        break;

    case INSN_LDR:

        printk("\n\nINSN_LDR\n");

        rn = GET_RN(insn);

        rd = GET_RD(insn);

        /* immediate offset/index */

        if (GET_I(insn) == 0) {

            switch(decode & 0x12) {  /* P and W bits */

            case 0x00:  /* P=0, W=0 -> base is updated, post-indexed */

                printk("\tPost-indexed\n");

                break;

            case 0x02:  /* P=0, W=1 -> user mode access */

                printk("\tUser mode\n");

                break;

            case 0x10:  /* P=1, W=0 -> base not updated */

                src_addr = GET_REG(rn, ctx);

                if (GET_U(insn) == 0) {

                    src_addr -= GET_OFFSET(insn);

                } else {

                    src_addr += GET_OFFSET(insn);

                }

                tmp = (src_addr[0] |

                       (src_addr[1] << 8) |

                       (src_addr[2] << 16) |

                       (src_addr[3] << 24));

                SET_REG(rd, ctx, tmp);

                return;

                break;

            case 0x12:  /* P=1, W=1 -> base is updated, pre-indexed */

                printk("\tPre-indexed\n");

                break;

            }

        }

        break;

    case INSN_LDRB:

        printk("\n\nINSN_LDRB\n");

        break;

    default:

        printk("\n\nUnrecognized instruction\n");

        break;

    }

    printk("data_abort at address 0x%x, instruction: 0x%x,   spsr = 0x%x\n",

           ctx->register_lr - 8, insn, spsr);

    /* disable interrupts, wait forever */

    _CPU_ISR_Disable(tmp);

    while(1) {

        continue;

    }

    return;

}