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; }