static void fixup_load(struct disasm_state *state, struct pt_regs *regs,
struct callee_regs *cregs)
{
int val;
/* register write back */
if ((state->aa == 1) || (state->aa == 2)) {
set_reg(state->wb_reg, state->src1 + state->src2, regs, cregs);
if (state->aa == 2)
state->src2 = 0;
}
if (state->zz == 0) {
get32_unaligned_check(val, state->src1 + state->src2);
} else {
get16_unaligned_check(val, state->src1 + state->src2);
if (state->x)
val = (val << 16) >> 16;
}
if (state->pref == 0)
set_reg(state->dest, val, regs, cregs);
return;
fault: state->fault = 1;
}
static int
do_alignment_ldrstr(unsigned long addr, unsigned long instr, struct pt_regs *regs)
{
unsigned int rd = RD_BITS(instr);
ai_word += 1;
if ((!LDST_P_BIT(instr) && LDST_W_BIT(instr)) || user_mode(regs))
goto trans;
if (LDST_L_BIT(instr)) {
unsigned int val;
get32_unaligned_check(val, addr);
regs->uregs[rd] = val;
} else
put32_unaligned_check(regs->uregs[rd], addr);
return TYPE_LDST;
trans:
if (LDST_L_BIT(instr)) {
unsigned int val;
get32t_unaligned_check(val, addr);
regs->uregs[rd] = val;
} else
put32t_unaligned_check(regs->uregs[rd], addr);
return TYPE_LDST;
fault:
return TYPE_FAULT;
}
static int
do_alignment_ldrhstrh(unsigned long addr, unsigned long instr, struct pt_regs *regs)
{
unsigned int rd = RD_BITS(instr);
if ((instr & 0x4b003fe0) == 0x40000120) /* old value 0x40002120, can't judge swap instr correctly */
goto swp;
ai_half += 1;
if (LDST_L_BIT(instr)) {
unsigned long val;
get16_unaligned_check(val, addr);
/* signed half-word? */
if (instr & 0x80)
val = (signed long)((signed short) val);
regs->uregs[rd] = val;
} else
put16_unaligned_check(regs->uregs[rd], addr);
return TYPE_LDST;
swp:
/* only handle swap word, for swap byte should not active this alignment exception */
get32_unaligned_check(regs->uregs[RD_BITS(instr)], addr);
put32_unaligned_check(regs->uregs[RM_BITS(instr)], addr);
return TYPE_SWAP;
fault:
return TYPE_FAULT;
}
static int
do_alignment_ldrdstrd(unsigned long addr, unsigned long instr,
struct pt_regs *regs)
{
unsigned int rd = RD_BITS(instr);
if (((rd & 1) == 1) || (rd == 14))
goto bad;
ai_dword += 1;
if (user_mode(regs))
goto user;
if ((instr & 0xf0) == 0xd0) {
unsigned long val;
get32_unaligned_check(val, addr);
regs->uregs[rd] = val;
get32_unaligned_check(val, addr + 4);
regs->uregs[rd + 1] = val;
} else {
put32_unaligned_check(regs->uregs[rd], addr);
put32_unaligned_check(regs->uregs[rd + 1], addr + 4);
}
return TYPE_LDST;
user:
if ((instr & 0xf0) == 0xd0) {
unsigned long val;
get32t_unaligned_check(val, addr);
regs->uregs[rd] = val;
get32t_unaligned_check(val, addr + 4);
regs->uregs[rd + 1] = val;
} else {
put32t_unaligned_check(regs->uregs[rd], addr);
put32t_unaligned_check(regs->uregs[rd + 1], addr + 4);
}
return TYPE_LDST;
bad:
return TYPE_ERROR;
fault:
return TYPE_FAULT;
}
/*
* LDM/STM alignment handler.
*
* There are 4 variants of this instruction:
*
* B = rn pointer before instruction, A = rn pointer after instruction
* ------ increasing address ----->
* | | r0 | r1 | ... | rx | |
* PU = 01 B A
* PU = 11 B A
* PU = 00 A B
* PU = 10 A B
*/
static int
do_alignment_ldmstm(unsigned long addr, unsigned long instr, struct pt_regs *regs)
{
unsigned int rd, rn, correction, nr_regs, regbits;
unsigned long eaddr, newaddr;
if (LDM_S_BIT(instr))
goto bad;
correction = 4; /* processor implementation defined */
regs->ARM_pc += correction;
ai_multi += 1;
/* count the number of registers in the mask to be transferred */
nr_regs = hweight16(REGMASK_BITS(instr)) * 4;
rn = RN_BITS(instr);
newaddr = eaddr = regs->uregs[rn];
if (!LDST_U_BIT(instr))
nr_regs = -nr_regs;
newaddr += nr_regs;
if (!LDST_U_BIT(instr))
eaddr = newaddr;
if (LDST_P_EQ_U(instr)) /* U = P */
eaddr += 4;
/*
* For alignment faults on the ARM922T the MMU makes
* the FSR (and hence addr) equal to the updated base address
* of the multiple access rather than the restored value.
* Switch this messsage off if we've got a ARM922, otherwise
* [ls]dm alignment faults are noisy!
*/
#if !(defined CONFIG_CPU_ARM922T)
/*
* This is a "hint" - we already have eaddr worked out by the
* processor for us.
*/
if (addr != eaddr) {
printk(KERN_ERR "LDMSTM: PC = %08lx, instr = %08lx, "
"addr = %08lx, eaddr = %08lx\n",
instruction_pointer(regs), instr, addr, eaddr);
show_regs(regs);
}
#endif
for (regbits = REGMASK_BITS(instr), rd = 0; regbits; regbits >>= 1, rd += 1)
if (regbits & 1) {
if (LDST_L_BIT(instr))
get32_unaligned_check(regs->uregs[rd], eaddr);
else
put32_unaligned_check(regs->uregs[rd], eaddr);
eaddr += 4;
}
if (LDST_W_BIT(instr))
regs->uregs[rn] = newaddr;
if (!LDST_L_BIT(instr) || !(REGMASK_BITS(instr) & (1 << 15)))
regs->ARM_pc -= correction;
return TYPE_DONE;
fault:
regs->ARM_pc -= correction;
return TYPE_FAULT;
bad:
printk(KERN_ERR "Alignment trap: not handling ldm with s-bit set\n");
return TYPE_ERROR;
}
/*
* LDM/STM alignment handler.
*
* There are 4 variants of this instruction:
*
* B = rn pointer before instruction, A = rn pointer after instruction
* ------ increasing address ----->
* | | r0 | r1 | ... | rx | |
* PU = 01 B A
* PU = 11 B A
* PU = 00 A B
* PU = 10 A B
*/
static int
do_alignment_ldmstm(unsigned long addr, unsigned long instr, struct pt_regs *regs)
{
unsigned int rd, rn, pc_correction, reg_correction, nr_regs, regbits;
unsigned long eaddr, newaddr;
if (LDM_S_BIT(instr))
goto bad;
pc_correction = 4; /* processor implementation defined */
ai_multi += 1;
/* count the number of registers in the mask to be transferred */
nr_regs = hweight16(REGMASK_BITS(instr)) * 4;
rn = RN_BITS(instr);
newaddr = eaddr = regs->uregs[rn];
if (!LDST_U_BIT(instr))
nr_regs = -nr_regs;
newaddr += nr_regs;
if (!LDST_U_BIT(instr))
eaddr = newaddr;
if (LDST_P_EQ_U(instr)) /* U = P */
eaddr += 4;
/*
* This is a "hint" - we already have eaddr worked out by the
* processor for us.
*/
if (addr != eaddr) {
printk(KERN_ERR "LDMSTM: PC = %08lx, instr = %08lx, "
"addr = %08lx, eaddr = %08lx\n",
instruction_pointer(regs), instr, addr, eaddr);
show_regs(regs);
}
if (LDM_H_BIT(instr))
reg_correction = 0x10;
else
reg_correction = 0x00;
for (regbits = REGMASK_BITS(instr), rd = 0; regbits; regbits >>= 1, rd += 1)
if (regbits & 1) {
if (LDST_L_BIT(instr))
get32_unaligned_check(regs->uregs[rd + reg_correction], eaddr);
else
put32_unaligned_check(regs->uregs[rd + reg_correction], eaddr);
eaddr += 4;
}
if (LDST_W_BIT(instr))
regs->uregs[rn] = newaddr;
return TYPE_DONE;
fault:
regs->UCreg_pc -= pc_correction;
return TYPE_FAULT;
bad:
printk(KERN_ERR "Alignment trap: not handling ldm with s-bit set\n");
return TYPE_ERROR;
}
