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;
}
int do_alignment(unsigned long addr, int error_code, struct pt_regs *regs)
{
union offset_union offset;
unsigned long instr, instrptr;
int (*handler)(unsigned long addr, unsigned long instr, struct pt_regs *regs);
unsigned int type;
instrptr = instruction_pointer(regs);
instr = *(unsigned long *)instrptr;
if (user_mode(regs))
goto user;
ai_sys += 1;
fixup:
regs->ARM_pc += 4;
switch (CODING_BITS(instr)) {
case 0x00000000: /* ldrh or strh */
if (LDSTH_I_BIT(instr))
offset.un = (instr & 0xf00) >> 4 | (instr & 15);
else
offset.un = regs->uregs[RM_BITS(instr)];
handler = do_alignment_ldrhstrh;
break;
case 0x04000000: /* ldr or str immediate */
offset.un = OFFSET_BITS(instr);
handler = do_alignment_ldrstr;
break;
case 0x06000000: /* ldr or str register */
offset.un = regs->uregs[RM_BITS(instr)];
if (IS_SHIFT(instr)) {
unsigned int shiftval = SHIFT_BITS(instr);
switch(SHIFT_TYPE(instr)) {
case SHIFT_LSL:
offset.un <<= shiftval;
break;
case SHIFT_LSR:
offset.un >>= shiftval;
break;
case SHIFT_ASR:
offset.sn >>= shiftval;
break;
case SHIFT_RORRRX:
if (shiftval == 0) {
offset.un >>= 1;
if (regs->ARM_cpsr & PSR_C_BIT)
offset.un |= 1 << 31;
} else
offset.un = offset.un >> shiftval |
offset.un << (32 - shiftval);
break;
}
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;
}
static int
do_alignment(unsigned long addr, unsigned int error_code, struct pt_regs *regs)
{
union offset_union offset;
unsigned long instr, instrptr;
int (*handler)(unsigned long addr, unsigned long instr, struct pt_regs *regs);
unsigned int type;
if (user_mode(regs))
ai_user += 1;
else
ai_sys += 1;
instrptr = instruction_pointer(regs);
if (instrptr >= SVC_SPACE)
instr = *(unsigned long *)instrptr;
else
{
__asm__ __volatile__(
"ldw.u %0, [%1]\n"
:"=&r"(instr)
:"r"(instrptr));
}
regs->UCreg_pc += 4;
switch (CODING_BITS(instr)) {
case 0x40000120: /* ldrh or strh */
if (LDSTH_I_BIT(instr))
offset.un = (instr & 0x3e00) >> 4 | (instr & 31);
else
offset.un = regs->uregs[RM_BITS(instr)];
handler = do_alignment_ldrhstrh;
break;
case 0x60000000: /* ldr or str immediate */
case 0x60000100: /* ldr or str immediate */
case 0x60000020: /* ldr or str immediate */
case 0x60000120: /* ldr or str immediate */
offset.un = OFFSET_BITS(instr);
handler = do_alignment_ldrstr;
break;
case 0x40000000: /* ldr or str register */
offset.un = regs->uregs[RM_BITS(instr)];
{
unsigned int shiftval = SHIFT_BITS(instr);
switch(SHIFT_TYPE(instr)) {
case SHIFT_LSL:
offset.un <<= shiftval;
break;
case SHIFT_LSR:
offset.un >>= shiftval;
break;
case SHIFT_ASR:
offset.sn >>= shiftval;
break;
case SHIFT_RORRRX:
if (shiftval == 0) {
offset.un >>= 1;
if (regs->UCreg_csr & PSR_C_BIT)
offset.un |= 1 << 31;
} else
offset.un = offset.un >> shiftval |
offset.un << (32 - shiftval);
break;
}
