unaligned.c

linux-2.6.15.6

#ifndef __LP64__
	if (!flop)
		return -1;
#endif
	__asm__ __volatile__ (
"	mtsp %3, %%sr1\n"
"	depd,z	%2, 60, 3, %%r19\n"
"	depd	%%r0, 63, 3, %2\n"
"	mtsar	%%r19\n"
"	depdi,z	-2, %%sar, 64, %%r19\n"
"1:	ldd	0(%%sr1,%2),%%r20\n"
"2:	ldd	8(%%sr1,%2),%%r21\n"
"	shrpd	%%r0, %1, %%sar, %%r22\n"
"	shrpd	%1, %%r0, %%sar, %%r1\n"
"	and	%%r20, %%r19, %%r20\n"
"	andcm	%%r21, %%r19, %%r21\n"
"	or	%%r22, %%r20, %%r20\n"
"	or	%%r1, %%r21, %%r21\n"
"3:	std	%%r20,0(%%sr1,%2)\n"
"4:	std	%%r21,8(%%sr1,%2)\n"
"	copy	%%r0, %0\n"
"5:	\n"
"	.section .fixup,\"ax\"\n"
"6:	ldi	-2, %0\n"
	FIXUP_BRANCH(5b)
"	.previous\n"
"	.section __ex_table,\"aw\"\n"
#ifdef __LP64__
"	.dword	1b,6b\n"
"	.dword  2b,6b\n"
"	.dword	3b,6b\n"
"	.dword  4b,6b\n"
#else
"	.word	1b,6b\n"
"	.word	2b,6b\n"
"	.word	3b,6b\n"
"	.word	4b,6b\n"
#endif
"	.previous\n"
	: "=r" (ret)
	: "r" (val), "r" (regs->ior), "r" (regs->isr)
	: "r19", "r20", "r21", "r22", "r1" );
#else
    {
	unsigned long valh=(val>>32),vall=(val&0xffffffffl);
	__asm__ __volatile__ (
"	mtsp	%4, %%sr1\n"
"	zdep	%2, 29, 2, %%r19\n"
"	dep	%%r0, 31, 2, %2\n"
"	mtsar	%%r19\n"
"	zvdepi	-2, 32, %%r19\n"
"1:	ldw	0(%%sr1,%3),%%r20\n"
"2:	ldw	8(%%sr1,%3),%%r21\n"
"	vshd	%1, %2, %%r1\n"
"	vshd	%%r0, %1, %1\n"
"	vshd	%2, %%r0, %2\n"
"	and	%%r20, %%r19, %%r20\n"
"	andcm	%%r21, %%r19, %%r21\n"
"	or	%1, %%r20, %1\n"
"	or	%2, %%r21, %2\n"
"3:	stw	%1,0(%%sr1,%1)\n"
"4:	stw	%%r1,4(%%sr1,%3)\n"
"5:	stw	%2,8(%%sr1,%3)\n"
"	copy	%%r0, %0\n"
"6:	\n"
"	.section .fixup,\"ax\"\n"
"7:	ldi	-2, %0\n"
	FIXUP_BRANCH(6b)
"	.previous\n"
"	.section __ex_table,\"aw\"\n"
#ifdef __LP64__
"	.dword	1b,7b\n"
"	.dword  2b,7b\n"
"	.dword	3b,7b\n"
"	.dword  4b,7b\n"
"	.dword  5b,7b\n"
#else
"	.word	1b,7b\n"
"	.word	2b,7b\n"
"	.word	3b,7b\n"
"	.word	4b,7b\n"
"	.word  	5b,7b\n"
#endif
"	.previous\n"
	: "=r" (ret)
	: "r" (valh), "r" (vall), "r" (regs->ior), "r" (regs->isr)
	: "r19", "r20", "r21", "r1" );
    }
#endif

	return ret;
}

void handle_unaligned(struct pt_regs *regs)
{
	static unsigned long unaligned_count = 0;
	static unsigned long last_time = 0;
	unsigned long newbase = R1(regs->iir)?regs->gr[R1(regs->iir)]:0;
	int modify = 0;
	int ret = ERR_NOTHANDLED;
	struct siginfo si;
	register int flop=0;	/* true if this is a flop */

	/* log a message with pacing */
	if (user_mode(regs)) {
		if (current->thread.flags & PARISC_UAC_SIGBUS) {
			goto force_sigbus;
		}

		if (unaligned_count > 5 && jiffies - last_time > 5*HZ) {
			unaligned_count = 0;
			last_time = jiffies;
		}

		if (!(current->thread.flags & PARISC_UAC_NOPRINT) 
		    && ++unaligned_count < 5) {
			char buf[256];
			sprintf(buf, "%s(%d): unaligned access to 0x" RFMT " at ip=0x" RFMT "\n",
				current->comm, current->pid, regs->ior, regs->iaoq[0]);
			printk(KERN_WARNING "%s", buf);
#ifdef DEBUG_UNALIGNED
			show_regs(regs);
#endif		
		}

		if (!unaligned_enabled)
			goto force_sigbus;
	}

	/* handle modification - OK, it's ugly, see the instruction manual */
	switch (MAJOR_OP(regs->iir))
	{
	case 0x03:
	case 0x09:
	case 0x0b:
		if (regs->iir&0x20)
		{
			modify = 1;
			if (regs->iir&0x1000)		/* short loads */
				if (regs->iir&0x200)
					newbase += IM5_3(regs->iir);
				else
					newbase += IM5_2(regs->iir);
			else if (regs->iir&0x2000)	/* scaled indexed */
			{
				int shift=0;
				switch (regs->iir & OPCODE1_MASK)
				{
				case OPCODE_LDH_I:
					shift= 1; break;
				case OPCODE_LDW_I:
					shift= 2; break;
				case OPCODE_LDD_I:
				case OPCODE_LDDA_I:
					shift= 3; break;
				}
				newbase += (R2(regs->iir)?regs->gr[R2(regs->iir)]:0)<<shift;
			} else				/* simple indexed */
				newbase += (R2(regs->iir)?regs->gr[R2(regs->iir)]:0);
		}
		break;
	case 0x13:
	case 0x1b:
		modify = 1;
		newbase += IM14(regs->iir);
		break;
	case 0x14:
	case 0x1c:
		if (regs->iir&8)
		{
			modify = 1;
			newbase += IM14(regs->iir&~0xe);
		}
		break;
	case 0x16:
	case 0x1e:
		modify = 1;
		newbase += IM14(regs->iir&6);
		break;
	case 0x17:
	case 0x1f:
		if (regs->iir&4)
		{
			modify = 1;
			newbase += IM14(regs->iir&~4);
		}
		break;
	}

	/* TODO: make this cleaner... */
	switch (regs->iir & OPCODE1_MASK)
	{
	case OPCODE_LDH_I:
	case OPCODE_LDH_S:
		ret = emulate_ldh(regs, R3(regs->iir));
		break;

	case OPCODE_LDW_I:
	case OPCODE_LDWA_I:
	case OPCODE_LDW_S:
	case OPCODE_LDWA_S:
		ret = emulate_ldw(regs, R3(regs->iir),0);
		break;

	case OPCODE_STH:
		ret = emulate_sth(regs, R2(regs->iir));
		break;

	case OPCODE_STW:
	case OPCODE_STWA:
		ret = emulate_stw(regs, R2(regs->iir),0);
		break;

#ifdef CONFIG_PA20
	case OPCODE_LDD_I:
	case OPCODE_LDDA_I:
	case OPCODE_LDD_S:
	case OPCODE_LDDA_S:
		ret = emulate_ldd(regs, R3(regs->iir),0);
		break;

	case OPCODE_STD:
	case OPCODE_STDA:
		ret = emulate_std(regs, R2(regs->iir),0);
		break;
#endif

	case OPCODE_FLDWX:
	case OPCODE_FLDWS:
	case OPCODE_FLDWXR:
	case OPCODE_FLDWSR:
		flop=1;
		ret = emulate_ldw(regs,FR3(regs->iir),1);
		break;

	case OPCODE_FLDDX:
	case OPCODE_FLDDS:
		flop=1;
		ret = emulate_ldd(regs,R3(regs->iir),1);
		break;

	case OPCODE_FSTWX:
	case OPCODE_FSTWS:
	case OPCODE_FSTWXR:
	case OPCODE_FSTWSR:
		flop=1;
		ret = emulate_stw(regs,FR3(regs->iir),1);
		break;

	case OPCODE_FSTDX:
	case OPCODE_FSTDS:
		flop=1;
		ret = emulate_std(regs,R3(regs->iir),1);
		break;

	case OPCODE_LDCD_I:
	case OPCODE_LDCW_I:
	case OPCODE_LDCD_S:
	case OPCODE_LDCW_S:
		ret = ERR_NOTHANDLED;	/* "undefined", but lets kill them. */
		break;
	}
#ifdef CONFIG_PA20
	switch (regs->iir & OPCODE2_MASK)
	{
	case OPCODE_FLDD_L:
		flop=1;
		ret = emulate_ldd(regs,R2(regs->iir),1);
		break;
	case OPCODE_FSTD_L:
		flop=1;
		ret = emulate_std(regs, R2(regs->iir),1);
		break;

#ifdef CONFIG_PA20
	case OPCODE_LDD_L:
		ret = emulate_ldd(regs, R2(regs->iir),0);
		break;
	case OPCODE_STD_L:
		ret = emulate_std(regs, R2(regs->iir),0);
		break;
#endif
	}
#endif
	switch (regs->iir & OPCODE3_MASK)
	{
	case OPCODE_FLDW_L:
		flop=1;
		ret = emulate_ldw(regs, R2(regs->iir),0);
		break;
	case OPCODE_LDW_M:
		ret = emulate_ldw(regs, R2(regs->iir),1);
		break;

	case OPCODE_FSTW_L:
		flop=1;
		ret = emulate_stw(regs, R2(regs->iir),1);
		break;
	case OPCODE_STW_M:
		ret = emulate_stw(regs, R2(regs->iir),0);
		break;
	}
	switch (regs->iir & OPCODE4_MASK)
	{
	case OPCODE_LDH_L:
		ret = emulate_ldh(regs, R2(regs->iir));
		break;
	case OPCODE_LDW_L:
	case OPCODE_LDWM:
		ret = emulate_ldw(regs, R2(regs->iir),0);
		break;
	case OPCODE_STH_L:
		ret = emulate_sth(regs, R2(regs->iir));
		break;
	case OPCODE_STW_L:
	case OPCODE_STWM:
		ret = emulate_stw(regs, R2(regs->iir),0);
		break;
	}

	if (modify && R1(regs->iir))
		regs->gr[R1(regs->iir)] = newbase;


	if (ret == ERR_NOTHANDLED)
		printk(KERN_CRIT "Not-handled unaligned insn 0x%08lx\n", regs->iir);

	DPRINTF("ret = %d\n", ret);

	if (ret)
	{
		printk(KERN_CRIT "Unaligned handler failed, ret = %d\n", ret);
		die_if_kernel("Unaligned data reference", regs, 28);

		if (ret == ERR_PAGEFAULT)
		{
			si.si_signo = SIGSEGV;
			si.si_errno = 0;
			si.si_code = SEGV_MAPERR;
			si.si_addr = (void __user *)regs->ior;
			force_sig_info(SIGSEGV, &si, current);
		}
		else
		{
force_sigbus:
			/* couldn't handle it ... */
			si.si_signo = SIGBUS;
			si.si_errno = 0;
			si.si_code = BUS_ADRALN;
			si.si_addr = (void __user *)regs->ior;
			force_sig_info(SIGBUS, &si, current);
		}
		
		return;
	}

	/* else we handled it, let life go on. */
	regs->gr[0]|=PSW_N;
}

/*
 * NB: check_unaligned() is only used for PCXS processors right
 * now, so we only check for PA1.1 encodings at this point.
 */

int
check_unaligned(struct pt_regs *regs)
{
	unsigned long align_mask;

	/* Get alignment mask */

	align_mask = 0UL;
	switch (regs->iir & OPCODE1_MASK) {

	case OPCODE_LDH_I:
	case OPCODE_LDH_S:
	case OPCODE_STH:
		align_mask = 1UL;
		break;

	case OPCODE_LDW_I:
	case OPCODE_LDWA_I:
	case OPCODE_LDW_S:
	case OPCODE_LDWA_S:
	case OPCODE_STW:
	case OPCODE_STWA:
		align_mask = 3UL;
		break;

	default:
		switch (regs->iir & OPCODE4_MASK) {
		case OPCODE_LDH_L:
		case OPCODE_STH_L:
			align_mask = 1UL;
			break;
		case OPCODE_LDW_L:
		case OPCODE_LDWM:
		case OPCODE_STW_L:
		case OPCODE_STWM:
			align_mask = 3UL;
			break;
		}
		break;
	}

	return (int)(regs->ior & align_mask);
}