/**
*
* Makes the connection between the Id of the exception source and the
* associated handler that is to run when the exception is recognized. The
* argument provided in this call as the DataPtr is used as the argument
* for the handler when it is called.
*
* @param Exception Id contains the ID of the exception source and should
* be in the range of 0 to XEXC_ID_LAST. See xexception_l.h for
further information.
* @param Handler to the handler for that exception.
* @param DataPtr is a reference to data that will be passed to the handler
* when it gets called.*
* @return None.
*
* @note
*
* None.
*
****************************************************************************/
void XExc_RegisterHandler(Xuint8 ExceptionId, XExceptionHandler Handler,
void *DataPtr)
{
XExc_VectorTable[ExceptionId].Handler = Handler;
XExc_VectorTable[ExceptionId].DataPtr = DataPtr;
XExc_VectorTable[ExceptionId].ReadOnlySDA = (void *)mfgpr(XREG_GPR2);
XExc_VectorTable[ExceptionId].ReadWriteSDA = (void *)mfgpr(XREG_GPR13);
}
void init_idle_task (void)
{
idle_task_pid = proc_create (PRIO_LOWEST); // Idle task (PID 0).
ptable[idle_task_pid].state = PROC_RUN; // Idle task assumed to be running as soon as the kernel starts
#ifndef PPC_CPU_440
ptable[idle_task_pid].pcontext.regs[CTX_INDEX_MSR] = mfmsr () | XIL_EXCEPTION_NON_CRITICAL;
#else
// We set MSR[DS] = 1 here, because that is the TLB scheme we use to
// separate instruction and data space on the 440.
ptable[idle_task_pid].pcontext.regs[CTX_INDEX_MSR] = mfmsr () | XIL_EXCEPTION_NON_CRITICAL | XREG_MSR_TLB_DATA_TS;
#endif
ptable[idle_task_pid].pcontext.regs[CTX_INDEX_PC] = (unsigned int)idle_task;
ptable[idle_task_pid].pcontext.regs[CTX_INDEX_GPR(1)] = mfgpr (1);
ptable[idle_task_pid].pcontext.regs[CTX_INDEX_GPR(2)] = mfgpr (2);
ptable[idle_task_pid].pcontext.regs[CTX_INDEX_GPR(13)]= mfgpr (13);
SET_CURRENT_PROCESS (idle_task_pid);
}
/* Exception handler (fatal).
* Attempt to print out a backtrace.
*/
void FreeRTOS_ExHandler(void *data)
{
unsigned *fp, lr;
static int exception_count = 0;
int offset = (int)data;
xil_printf("\n\rEXCEPTION, HALTED!\n\r");
fp = (unsigned*)mfgpr(11); /* get current frame pointer */
if (! ptr_valid(fp)) {
goto spin;
}
/* Fetch Data Fault Address from CP15 */
lr = mfcp(XREG_CP15_DATA_FAULT_ADDRESS);
xil_printf("Data Fault Address: 0x%08x\n\r", lr);
/* The exception frame is built by DataAbortHandler (for example) in
* FreeRTOS/Source/portable/GCC/Zynq/port_asm_vectors.s:
* stmdb sp!,{r0-r3,r12,lr}
* and the initial handler function (i.e. DataAbortInterrupt() ) in
* standalone_bsp/src/arm/vectors.c, which is the standard compiler EABI :
* push {fp, lr}
*
* The relative position of the frame build in port_asm_vectors.s is assumed,
* as there is no longer any direct reference to it. If this file (or vectors.c)
* are modified this location will need to be updated.
*
* r0+r1+r2+r3+r12+lr = 5 registers to get to the initial link register where
* the exception occurred.
*/
xil_printf("FP: 0x%08x LR: 0x%08x\n\r", (unsigned)fp, *(fp + 5) - offset);
xil_printf("R0: 0x%08x R1: 0x%08x\n\r", *(fp + 0), *(fp + 1));
xil_printf("R2: 0x%08x R3: 0x%08x\n\r", *(fp + 2), *(fp + 3));
xil_printf("R12: 0x%08x\n\r", *(fp + 4));
spin:
exception_count++;
if (exception_count > 1) {
/* Nested exceptions */
while (1) {;}
}
while (1) {;}
}
static sval
decode_spr_ins(struct cpu_thread* thr, uval addr, uval32 ins)
{
struct thread_control_area *tca = get_tca();
uval id = thr->vregs->active_vsave;
struct vexc_save_regs *vr = &thr->vregs->vexc_save[id];
sval ret = -1;
uval opcode = extract_bits(ins, 0, 6);
uval type = extract_bits(ins, 21, 10);
uval spr_0_4 = extract_bits(ins, 16, 5);
uval spr_5_9 = extract_bits(ins, 11, 5);
uval gpr = extract_bits(ins, 6, 5);
uval spr = (spr_0_4 << 5) | spr_5_9;
/* mfmsr */
if (opcode == 31 && type == 83) {
//hprintf("mfmsr r%ld at 0x%lx\n",gpr, addr);
mtgpr(thr, gpr, thr->vregs->v_msr);
tca->srr0 += sizeof(uval32);
return 0;
}
/* mtmsrd */
if (opcode == 31 && (type == 178 || type == 146)) {
uval64 val = mfgpr(thr, gpr);
//hprintf("mtmsrd r%ld <- 0x%llx at 0x%lx\n", gpr, val, addr);
uval64 chg_mask = ~0ULL;
uval l = extract_bits(ins, 15, 1);
if (type == 146) {
// mtmsr , 32-bits
chg_mask = 0xffffffff;
}
if (l == 1) {
chg_mask = (MSR_EE | MSR_RI);
}
/* These are the only bits we can change here */
val = (val & chg_mask) | (thr->vregs->v_msr & ~chg_mask);
set_v_msr(thr, val);
val = thr->vregs->v_msr;
val |= V_LPAR_MSR_ON;
val &= ~V_LPAR_MSR_OFF;
tca->srr1 = val;
tca->srr0 += sizeof(uval32);
return 0;
}
/* mfspr */
#define SET_GPR(label, src) \
case label: mtgpr(thr, gpr, src); break;
if (opcode == 31 && type == 339) {
ret = 0;
switch (spr) {
SET_GPR(SPRN_SRR0, vr->v_srr0);
SET_GPR(SPRN_SRR1, vr->v_srr1);
SET_GPR(SPRN_PVR, mfpvr());
SET_GPR(SPRN_PIR, mfpir());
case SPRN_DSISR:
case SPRN_DAR:
mtgpr(thr, gpr, 0);
break;
case SPRN_HID0:
case SPRN_HID1:
case SPRN_HID4:
case SPRN_HID5:
mtgpr(thr, gpr, 0xdeadbeeffeedfaceULL);
break;
default:
ret = -1;
break;
}
if (ret != -1) {
tca->srr0 += sizeof(uval32);
return ret;
}
}
#define SET_VREG(label, field) \
case label: thr->vregs->field = mfgpr(thr, gpr); break;
/* mtspr */
if (opcode == 31 && type == 467) {
ret = 0;
switch (spr) {
SET_VREG(SPRN_SPRG0, v_sprg0);
SET_VREG(SPRN_SPRG1, v_sprg1);
SET_VREG(SPRN_SPRG2, v_sprg2);
SET_VREG(SPRN_SPRG3, v_sprg3);
case SPRN_DEC:
partition_set_dec(thr, mfgpr(thr, gpr));
thr->vregs->v_dec = mfgpr(thr, gpr);
break;
case SPRN_SRR0:
vr->v_srr0 = mfgpr(thr, gpr);
break;
case SPRN_SRR1:
vr->v_srr1 = mfgpr(thr, gpr);
break;
case SPRN_DSISR:
case SPRN_DAR:
break;
default:
ret = -1;
break;
}
if (ret != -1) {
tca->srr0 += sizeof(uval32);
return ret;
}
}
/* rfid */
if (opcode == 19 && type == 18) {
uval val = vr->v_srr1;
set_v_msr(thr, val);
val |= V_LPAR_MSR_ON;
val &= ~V_LPAR_MSR_OFF;
tca->srr1 = val;
tca->srr0 = vr->v_srr0;
hprintf("rfid: %lx -> %lx\n",addr, vr->v_srr0);
return 0;
}
if (ret == -1) {
hprintf("Decode instruction: %ld %ld %ld %ld\n",
opcode, type, spr, gpr);
}
return ret;
}
void vPortExceptionHandler( void *pvExceptionID )
{
extern void *pxCurrentTCB;
/* Fill an xPortRegisterDump structure with the MicroBlaze context as it
was immediately before the exception occurrence. */
/* First fill in the name and handle of the task that was in the Running
state when the exception occurred. */
xRegisterDump.xCurrentTaskHandle = pxCurrentTCB;
xRegisterDump.pcCurrentTaskName = pcTaskGetName( NULL );
configASSERT( pulStackPointerOnFunctionEntry );
/* Obtain the values of registers that were stacked prior to this function
being called, and may have changed since they were stacked. */
xRegisterDump.ulR3 = pulStackPointerOnFunctionEntry[ portexR3_STACK_OFFSET ];
xRegisterDump.ulR4 = pulStackPointerOnFunctionEntry[ portexR4_STACK_OFFSET ];
xRegisterDump.ulR5 = pulStackPointerOnFunctionEntry[ portexR5_STACK_OFFSET ];
xRegisterDump.ulR6 = pulStackPointerOnFunctionEntry[ portexR6_STACK_OFFSET ];
xRegisterDump.ulR7 = pulStackPointerOnFunctionEntry[ portexR7_STACK_OFFSET ];
xRegisterDump.ulR8 = pulStackPointerOnFunctionEntry[ portexR8_STACK_OFFSET ];
xRegisterDump.ulR9 = pulStackPointerOnFunctionEntry[ portexR9_STACK_OFFSET ];
xRegisterDump.ulR10 = pulStackPointerOnFunctionEntry[ portexR10_STACK_OFFSET ];
xRegisterDump.ulR11 = pulStackPointerOnFunctionEntry[ portexR11_STACK_OFFSET ];
xRegisterDump.ulR12 = pulStackPointerOnFunctionEntry[ portexR12_STACK_OFFSET ];
xRegisterDump.ulR15_return_address_from_subroutine = pulStackPointerOnFunctionEntry[ portexR15_STACK_OFFSET ];
xRegisterDump.ulR18 = pulStackPointerOnFunctionEntry[ portexR18_STACK_OFFSET ];
xRegisterDump.ulR19 = pulStackPointerOnFunctionEntry[ portexR19_STACK_OFFSET ];
xRegisterDump.ulMSR = pulStackPointerOnFunctionEntry[ portexMSR_STACK_OFFSET ];
/* Obtain the value of all other registers. */
xRegisterDump.ulR2_small_data_area = mfgpr( R2 );
xRegisterDump.ulR13_read_write_small_data_area = mfgpr( R13 );
xRegisterDump.ulR14_return_address_from_interrupt = mfgpr( R14 );
xRegisterDump.ulR16_return_address_from_trap = mfgpr( R16 );
xRegisterDump.ulR17_return_address_from_exceptions = mfgpr( R17 );
xRegisterDump.ulR20 = mfgpr( R20 );
xRegisterDump.ulR21 = mfgpr( R21 );
xRegisterDump.ulR22 = mfgpr( R22 );
xRegisterDump.ulR23 = mfgpr( R23 );
xRegisterDump.ulR24 = mfgpr( R24 );
xRegisterDump.ulR25 = mfgpr( R25 );
xRegisterDump.ulR26 = mfgpr( R26 );
xRegisterDump.ulR27 = mfgpr( R27 );
xRegisterDump.ulR28 = mfgpr( R28 );
xRegisterDump.ulR29 = mfgpr( R29 );
xRegisterDump.ulR30 = mfgpr( R30 );
xRegisterDump.ulR31 = mfgpr( R31 );
xRegisterDump.ulR1_SP = ( ( uint32_t ) pulStackPointerOnFunctionEntry ) + portexASM_HANDLER_STACK_FRAME_SIZE;
xRegisterDump.ulEAR = mfear();
xRegisterDump.ulESR = mfesr();
xRegisterDump.ulEDR = mfedr();
/* Move the saved program counter back to the instruction that was executed
when the exception occurred. This is only valid for certain types of
exception. */
xRegisterDump.ulPC = xRegisterDump.ulR17_return_address_from_exceptions - portexINSTRUCTION_SIZE;
#if( XPAR_MICROBLAZE_USE_FPU != 0 )
{
xRegisterDump.ulFSR = mffsr();
}
#else
{
xRegisterDump.ulFSR = 0UL;
}
#endif
/* Also fill in a string that describes what type of exception this is.
The string uses the same ID names as defined in the MicroBlaze standard
library exception header files. */
switch( ( uint32_t ) pvExceptionID )
{
case XEXC_ID_FSL :
xRegisterDump.pcExceptionCause = ( int8_t * const ) "XEXC_ID_FSL";
break;
case XEXC_ID_UNALIGNED_ACCESS :
xRegisterDump.pcExceptionCause = ( int8_t * const ) "XEXC_ID_UNALIGNED_ACCESS";
break;
case XEXC_ID_ILLEGAL_OPCODE :
xRegisterDump.pcExceptionCause = ( int8_t * const ) "XEXC_ID_ILLEGAL_OPCODE";
break;
case XEXC_ID_M_AXI_I_EXCEPTION :
xRegisterDump.pcExceptionCause = ( int8_t * const ) "XEXC_ID_M_AXI_I_EXCEPTION or XEXC_ID_IPLB_EXCEPTION";
break;
case XEXC_ID_M_AXI_D_EXCEPTION :
xRegisterDump.pcExceptionCause = ( int8_t * const ) "XEXC_ID_M_AXI_D_EXCEPTION or XEXC_ID_DPLB_EXCEPTION";
break;
case XEXC_ID_DIV_BY_ZERO :
xRegisterDump.pcExceptionCause = ( int8_t * const ) "XEXC_ID_DIV_BY_ZERO";
break;
case XEXC_ID_STACK_VIOLATION :
xRegisterDump.pcExceptionCause = ( int8_t * const ) "XEXC_ID_STACK_VIOLATION or XEXC_ID_MMU";
break;
#if( XPAR_MICROBLAZE_USE_FPU != 0 )
case XEXC_ID_FPU :
xRegisterDump.pcExceptionCause = ( int8_t * const ) "XEXC_ID_FPU see ulFSR value";
break;
#endif /* XPAR_MICROBLAZE_USE_FPU */
}
/* vApplicationExceptionRegisterDump() is a callback function that the
application can optionally define to receive the populated xPortRegisterDump
structure. If the application chooses not to define a version of
vApplicationExceptionRegisterDump() then the weekly defined default
implementation within this file will be called instead. */
vApplicationExceptionRegisterDump( &xRegisterDump );
/* Must not attempt to leave this function! */
for( ;; )
{
portNOP();
}
}
/**
*
* This function is the primary interrupt handler for the driver. It must be
* connected to the interrupt source such that is called when an interrupt of
* the interrupt controller is active. It will resolve which interrupts are
* active and enabled and call the appropriate interrupt handler. It uses
* the AckBeforeService flag in the configuration data to determine when to
* acknowledge the interrupt. Highest priority interrupts are serviced first.
* This function assumes that an interrupt vector table has been previously
* initialized.It does not verify that entries in the table are valid before
* calling an interrupt handler. In Cascade mode this function calls
* XIntc_CascadeHandler to handle interrupts of Master and Slave controllers.
* This functions also handles interrupts nesting by saving and restoring link
* register of Microblaze and Interrupt Level register of interrupt controller
* properly.
* @param DeviceId is the zero-based device ID defined in xparameters.h
* of the interrupting interrupt controller. It is used as a direct
* index into the configuration data, which contains the vector
* table for the interrupt controller. Note that even though the
* argument is a void pointer, the value is not a pointer but the
* actual device ID. The void pointer type is necessary to meet
* the XInterruptHandler typedef for interrupt handlers.
*
* @return None.
*
* @note For nested interrupts, this function saves microblaze r14
* register on entry and restores on exit. This is required since
* compiler does not support nesting. This function enables
* Microblaze interrupts after blocking further interrupts
* from the current interrupt number and interrupts below current
* interrupt proirity by writing to Interrupt Level Register of
* INTC on entry. On exit, it disables microblaze interrupts and
* restores ILR register default value(0xFFFFFFFF)back. It is
* recommended to increase STACK_SIZE in linker script for nested
* interrupts.
*
******************************************************************************/
void XIntc_DeviceInterruptHandler(void *DeviceId)
{
u32 IntrStatus;
u32 IntrMask = 1;
int IntrNumber;
XIntc_Config *CfgPtr;
u32 Imr;
/* Get the configuration data using the device ID */
CfgPtr = &XIntc_ConfigTable[(u32)DeviceId];
#if XPAR_INTC_0_INTC_TYPE != XIN_INTC_NOCASCADE
if (CfgPtr->IntcType != XIN_INTC_NOCASCADE) {
XIntc_CascadeHandler(DeviceId);
}
else
#endif
{ /* This extra brace is required for compilation in Cascade Mode */
#if XPAR_XINTC_HAS_ILR == TRUE
#ifdef __MICROBLAZE__
volatile u32 R14_register;
/* Save r14 register */
R14_register = mfgpr(r14);
#endif
volatile u32 ILR_reg;
/* Save ILR register */
ILR_reg = Xil_In32(CfgPtr->BaseAddress + XIN_ILR_OFFSET);
#endif
/* Get the interrupts that are waiting to be serviced */
IntrStatus = XIntc_GetIntrStatus(CfgPtr->BaseAddress);
/* Mask the Fast Interrupts */
if (CfgPtr->FastIntr == TRUE) {
Imr = XIntc_In32(CfgPtr->BaseAddress + XIN_IMR_OFFSET);
IntrStatus &= ~Imr;
}
/* Service each interrupt that is active and enabled by
* checking each bit in the register from LSB to MSB which
* corresponds to an interrupt input signal
*/
for (IntrNumber = 0; IntrNumber < CfgPtr->NumberofIntrs;
IntrNumber++) {
if (IntrStatus & 1) {
XIntc_VectorTableEntry *TablePtr;
#if XPAR_XINTC_HAS_ILR == TRUE
/* Write to ILR the current interrupt
* number
*/
Xil_Out32(CfgPtr->BaseAddress +
XIN_ILR_OFFSET, IntrNumber);
/* Read back ILR to ensure the value
* has been updated and it is safe to
* enable interrupts
*/
Xil_In32(CfgPtr->BaseAddress +
XIN_ILR_OFFSET);
/* Enable interrupts */
Xil_ExceptionEnable();
#endif
/* If the interrupt has been setup to
* acknowledge it before servicing the
* interrupt, then ack it */
if (CfgPtr->AckBeforeService & IntrMask) {
XIntc_AckIntr(CfgPtr->BaseAddress,
IntrMask);
}
/* The interrupt is active and enabled, call
* the interrupt handler that was setup with
* the specified parameter
*/
TablePtr = &(CfgPtr->HandlerTable[IntrNumber]);
TablePtr->Handler(TablePtr->CallBackRef);
/* If the interrupt has been setup to
* acknowledge it after it has been serviced
* then ack it
*/
if ((CfgPtr->AckBeforeService &
IntrMask) == 0) {
XIntc_AckIntr(CfgPtr->BaseAddress,
IntrMask);
}
#if XPAR_XINTC_HAS_ILR == TRUE
/* Disable interrupts */
Xil_ExceptionDisable();
/* Restore ILR */
Xil_Out32(CfgPtr->BaseAddress + XIN_ILR_OFFSET,
ILR_reg);
#endif
/*
* Read the ISR again to handle architectures
* with posted write bus access issues.
*/
XIntc_GetIntrStatus(CfgPtr->BaseAddress);
/*
* If only the highest priority interrupt is to
* be serviced, exit loop and return after
* servicing
* the interrupt
*/
if (CfgPtr->Options == XIN_SVC_SGL_ISR_OPTION) {
#if XPAR_XINTC_HAS_ILR == TRUE
#ifdef __MICROBLAZE__
/* Restore r14 */
mtgpr(r14, R14_register);
#endif
#endif
return;
}
}
/* Move to the next interrupt to check */
IntrMask <<= 1;
IntrStatus >>= 1;
/* If there are no other bits set indicating that all
* interrupts have been serviced, then exit the loop
*/
if (IntrStatus == 0) {
break;
}
}
#if XPAR_XINTC_HAS_ILR == TRUE
#ifdef __MICROBLAZE__
/* Restore r14 */
mtgpr(r14, R14_register);
#endif
#endif
}
}
/** \internal
* \brief The exception handler for the system call exception.
*
* This function is registered at initialization time to be the system
* call exception handler. It calls one of several functions based on the
* system call number and returns the results of the system call back
* to the caller by way of the param1 variable in the system call
* data structure.
*/
void* _system_call_handler(Huint c,void *p2,void *p3,void *p4,void *p5,void *p6)
{
void *ret;
if (c > 16)
{
/* Pseudo assembler instructions */
#define stringify(s) tostring(s)
#define tostring(s) #s
#define mfgpr(rn) ({ unsigned int _rval; \
__asm__ __volatile__ ( \
"or\t%0,r0," stringify(rn) "\n" : "=d"(_rval) \
); \
_rval; \
})
unsigned int temp_reg;
temp_reg = mfgpr(r15);
printf("[TID = %u] Illegal Syscall ID 0x%08x, r15 = 0x%08x\n",_current_thread(),c,temp_reg);
while(1);
}
#ifdef HTHREADS_SMP
while( !_get_syscall_lock() );
#endif
hprofile_hist_capture( SYSCALL, SCTYPE, c );
switch( c )
{
case HTHREAD_SYSCALL_CREATE:
ret = (void*)_syscall_create( (hthread_t*)p2,
(hthread_attr_t*)p3,
(hthread_start_t)p4,
p5 );
break;
case HTHREAD_SYSCALL_JOIN:
ret = (void*)_syscall_join( (hthread_t)p2, (void**)p3 );
break;
case HTHREAD_SYSCALL_DETACH:
ret = (void*)_syscall_detach( (hthread_t)p2 );
break;
case HTHREAD_SYSCALL_YIELD:
ret = (void*)_syscall_yield();
break;
case HTHREAD_SYSCALL_EXIT:
_syscall_exit( p2 );
ret = NULL;
break;
case HTHREAD_SYSCALL_CURRENTID:
ret = (void*)_syscall_current_id();
break;
case HTHREAD_SYSCALL_SETPRI:
ret = (void*)_syscall_set_schedparam( (hthread_t)p2, (Huint)p3 );
break;
case HTHREAD_SYSCALL_GETPRI:
ret = (void*)_syscall_get_schedparam( (hthread_t)p2, (Hint*)p3 );
break;
case HTHREAD_SYSCALL_MUTEX_LOCK:
ret = (void*)_syscall_mutex_lock((hthread_mutex_t*)p2);
break;
case HTHREAD_SYSCALL_MUTEX_UNLOCK:
ret = (void*)_syscall_mutex_unlock((hthread_mutex_t*)p2);
break;
case HTHREAD_SYSCALL_MUTEX_TRYLOCK:
ret = (void*)_syscall_mutex_trylock((hthread_mutex_t*)p2);
break;
case HTHREAD_SYSCALL_COND_WAIT:
ret = (void*)_syscall_cond_wait((hthread_cond_t*)p2,
(hthread_mutex_t*)p3);
break;
case HTHREAD_SYSCALL_COND_SIGNAL:
ret = (void*)_syscall_cond_signal((hthread_cond_t*)p2);
break;
case HTHREAD_SYSCALL_COND_BROADCAST:
ret = (void*)_syscall_cond_broadcast((hthread_cond_t*)p2);
break;
case HTHREAD_SYSCALL_INTRASSOC:
ret = (void*)_syscall_intr_assoc( (Huint)p2 );
break;
case HTHREAD_SYSCALL_SCHED:
ret = (void*)_syscall_sched();
break;
default:
printf("***** Bad Syscall ID (0x%08x) ******\n",c);
ret = (void*)EINVAL;
break;
}
#ifdef HTHREADS_SMP
while( !_release_syscall_lock() );
#endif
return ret;
}
