static void
do_alignment_finish_ldst(unsigned long addr, unsigned long instr, struct pt_regs *regs, union offset_union offset)
{
if (!LDST_U_BIT(instr))
offset.un = -offset.un;
if (!LDST_P_BIT(instr))
addr += offset.un;
if (!LDST_P_BIT(instr) || LDST_W_BIT(instr))
regs->uregs[RN_BITS(instr)] = addr;
}
static int
do_alignment_exception(struct pt_regs *regs)
{
unsigned int instr, rd, rn, correction, nr_regs, regbits;
unsigned long eaddr;
union { unsigned long un; signed long sn; } offset;
if (user_mode(regs)) {
set_cr(cr_no_alignment);
ai_user += 1;
return 0;
}
ai_sys += 1;
instr = *(unsigned long *)instruction_pointer(regs);
correction = 4; /* sometimes 8 on ARMv3 */
regs->ARM_pc += correction + 4;
rd = RD_BITS(instr);
rn = RN_BITS(instr);
eaddr = regs->uregs[rn];
switch(CODING_BITS(instr)) {
case 0x00000000:
if ((instr & 0x0ff00ff0) == 0x01000090) {
ai_skipped += 1;
printk(KERN_ERR "Unaligned trap: not handling swp instruction\n");
return 1;
}
if (((instr & 0x0e000090) == 0x00000090) && (instr & 0x60) != 0) {
ai_half += 1;
if (LDSTH_I_BIT(instr))
offset.un = (instr & 0xf00) >> 4 | (instr & 15);
else
offset.un = regs->uregs[RM_BITS(instr)];
if (LDST_P_BIT(instr)) {
if (LDST_U_BIT(instr))
eaddr += offset.un;
else
eaddr -= offset.un;
}
if (LDST_L_BIT(instr))
regs->uregs[rd] = get_unaligned((unsigned short *)eaddr);
else
put_unaligned(regs->uregs[rd], (unsigned short *)eaddr);
/* signed half-word? */
if (instr & 0x40)
regs->uregs[rd] = (long)((short) regs->uregs[rd]);
if (!LDST_P_BIT(instr)) {
if (LDST_U_BIT(instr))
eaddr += offset.un;
else
eaddr -= offset.un;
regs->uregs[rn] = eaddr;
} else if (LDST_W_BIT(instr))
regs->uregs[rn] = eaddr;
break;
}
/*
* 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/ARM920T 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 message off if we've got a ARM92[02], otherwise
* [ls]dm alignment faults are noisy!
*/
#if !(defined CONFIG_CPU_ARM922T) && !(defined CONFIG_CPU_ARM920T)
/*
* 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
if (user_mode(regs)) {
for (regbits = REGMASK_BITS(instr), rd = 0; regbits;
regbits >>= 1, rd += 1)
if (regbits & 1) {
if (LDST_L_BIT(instr)) {
unsigned int val;
get32t_unaligned_check(val, eaddr);
regs->uregs[rd] = val;
} else
put32t_unaligned_check(regs->uregs[rd], eaddr);
eaddr += 4;
}
} else {
for (regbits = REGMASK_BITS(instr), rd = 0; regbits;
/*
* 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;
}
