/* Return from trap. */
USI
frv_rett (SIM_CPU *current_cpu, PCADDR pc, BI debug_field)
{
USI new_pc;
/* if (normal running mode and debug_field==0
PC=PCSR
PSR.ET=1
PSR.S=PSR.PS
else if (debug running mode and debug_field==1)
PC=(BPCSR)
PSR.ET=BPSR.BET
PSR.S=BPSR.BS
change to normal running mode
*/
int psr_s = GET_H_PSR_S ();
int psr_et = GET_H_PSR_ET ();
/* Check for exceptions in the priority order listed in the FRV Architecture
Volume 2. */
if (! psr_s)
{
/* Halt if PSR.ET is not set. See chapter 6 of the LSI. */
if (! psr_et)
{
SIM_DESC sd = CPU_STATE (current_cpu);
sim_engine_halt (sd, current_cpu, NULL, pc, sim_stopped, SIM_SIGTRAP);
}
/* privileged_instruction interrupt will have already been queued by
frv_detect_insn_access_interrupts. */
new_pc = pc + 4;
}
else if (psr_et)
{
/* Halt if PSR.S is set. See chapter 6 of the LSI. */
if (psr_s)
{
SIM_DESC sd = CPU_STATE (current_cpu);
sim_engine_halt (sd, current_cpu, NULL, pc, sim_stopped, SIM_SIGTRAP);
}
frv_queue_program_interrupt (current_cpu, FRV_ILLEGAL_INSTRUCTION);
new_pc = pc + 4;
}
else if (! CPU_DEBUG_STATE (current_cpu) && debug_field == 0)
{
USI psr = GET_PSR ();
/* Return from normal running state. */
new_pc = GET_H_SPR (H_SPR_PCSR);
SET_PSR_ET (psr, 1);
SET_PSR_S (psr, GET_PSR_PS (psr));
sim_queue_fn_si_write (current_cpu, frvbf_h_spr_set, H_SPR_PSR, psr);
}
else if (CPU_DEBUG_STATE (current_cpu) && debug_field == 1)
{
USI psr = GET_PSR ();
/* Return from debug state. */
new_pc = GET_H_SPR (H_SPR_BPCSR);
SET_PSR_ET (psr, GET_H_BPSR_BET ());
SET_PSR_S (psr, GET_H_BPSR_BS ());
sim_queue_fn_si_write (current_cpu, frvbf_h_spr_set, H_SPR_PSR, psr);
CPU_DEBUG_STATE (current_cpu) = 0;
}
else
new_pc = pc + 4;
return new_pc;
}
void __attribute__((naked)) isr_fault()
{
unsigned int sp, lr, psr, usp;
sp = GET_SP ();
psr = GET_PSR();
lr = GET_LR ();
usp = GET_USP();
printk("\nKernel SP 0x%08x\n"
"Stacked PSR 0x%08x\n"
"Stacked PC 0x%08x\n"
"Stacked LR 0x%08x\n"
"Current LR 0x%08x\n"
"Current PSR 0x%08x(vector number:%d)\n", sp,
*(unsigned int *)(sp + 28),
*(unsigned int *)(sp + 24),
*(unsigned int *)(sp + 20),
lr, psr, psr & 0x1ff);
printk("\nUser SP 0x%08x\n"
"Stacked PSR 0x%08x\n"
"Stacked PC 0x%08x\n"
"Stacked LR 0x%08x\n",
usp,
*(unsigned int *)(usp + 28),
*(unsigned int *)(usp + 24),
*(unsigned int *)(usp + 20));
printk("\ncurrent->sp 0x%08x\n"
"current->base 0x%08x\n"
"current->heap 0x%08x\n"
"current->kernel 0x%08x\n"
"current->kernel->sp 0x%08x\n"
"current->state 0x%08x\n"
"current->irqflag 0x%08x\n"
"current->addr 0x%08x\n"
"current 0x%08x\n"
, current->mm.sp, current->mm.base, current->mm.heap,
current->mm.kernel.base, current->mm.kernel.sp, current->state,
current->irqflag, current->addr, current);
printk("\ncurrent context\n");
unsigned int i;
for (i = 0; i < NR_CONTEXT*2; i++)
printk("[0x%08x] 0x%08x\n", usp + i*4, ((unsigned int *)usp)[i]);
printk("\nSCB_ICSR 0x%08x\n"
"SCB_CFSR 0x%08x\n"
"SCB_HFSR 0x%08x\n"
"SCB_MMFAR 0x%08x\n"
"SCB_BFAR 0x%08x\n",
SCB_ICSR, SCB_CFSR, SCB_HFSR, SCB_MMFAR, SCB_BFAR);
/* led for debugging */
#ifdef LED_DEBUG
SET_PORT_CLOCK(ENABLE, PORTD);
SET_PORT_PIN(PORTD, 2, PIN_OUTPUT_50MHZ);
unsigned int j;
while (1) {
PUT_PORT(PORTD, GET_PORT(PORTD) ^ 4);
for (i = 100; i; i--) {
for (j = 10; j; j--) {
__asm__ __volatile__(
"nop \n\t"
"nop \n\t"
"nop \n\t"
"nop \n\t"
"nop \n\t"
"nop \n\t"
"nop \n\t"
"nop \n\t"
"nop \n\t"
"nop \n\t"
::: "memory");
}
}
}
#endif
}
