unaligned.c

嵌入式系统设计与实例开发实验教材二源码 多线程应用程序设计 串行端口程序设计 AD接口实验 CAN总线通信实验 GPS通信实验 Linux内核移植与编译实验 IC卡读写实验 SD驱动使

/*
 * Architecture-specific unaligned trap handling.
 *
 * Copyright (C) 1999-2001 Hewlett-Packard Co
 * Copyright (C) 1999-2000 Stephane Eranian <[e-mail]>
 * Copyright (C) 2001 David Mosberger-Tang <[e-mail]>
 *
 * 2001/10/11	Fix unaligned access to rotating registers in s/w pipelined loops.
 * 2001/08/13	Correct size of extended floats (float_fsz) from 16 to 10 bytes.
 * 2001/01/17	Add support emulation of unaligned kernel accesses.
 */
#include <linux/kernel.h>
#include <linux/sched.h>
#include <linux/smp_lock.h>

#include <asm/uaccess.h>
#include <asm/rse.h>
#include <asm/processor.h>
#include <asm/unaligned.h>

extern void die_if_kernel(char *str, struct pt_regs *regs, long err) __attribute__ ((noreturn));

#undef DEBUG_UNALIGNED_TRAP

#ifdef DEBUG_UNALIGNED_TRAP
# define DPRINT(a...)	do { printk("%s.%u: ", __FUNCTION__, __LINE__); printk (a); } while (0)
# define DDUMP(str,vp,len)	dump(str, vp, len)

static void
dump (const char *str, void *vp, size_t len)
{
	unsigned char *cp = vp;
	int i;

	printk("%s", str);
	for (i = 0; i < len; ++i)
		printk (" %02x", *cp++);
	printk("\n");
}
#else
# define DPRINT(a...)
# define DDUMP(str,vp,len)
#endif

#define IA64_FIRST_STACKED_GR	32
#define IA64_FIRST_ROTATING_FR	32
#define SIGN_EXT9		0xffffffffffffff00ul

/*
 * For M-unit:
 *
 *  opcode |   m  |   x6    |
 * --------|------|---------|
 * [40-37] | [36] | [35:30] |
 * --------|------|---------|
 *     4   |   1  |    6    | = 11 bits
 * --------------------------
 * However bits [31:30] are not directly useful to distinguish between
 * load/store so we can use [35:32] instead, which gives the following
 * mask ([40:32]) using 9 bits. The 'e' comes from the fact that we defer
 * checking the m-bit until later in the load/store emulation.
 */
#define IA64_OPCODE_MASK	0x1ef
#define IA64_OPCODE_SHIFT	32

/*
 * Table C-28 Integer Load/Store
 *
 * We ignore [35:32]= 0x6, 0x7, 0xE, 0xF
 *
 * ld8.fill, st8.fill  MUST be aligned because the RNATs are based on
 * the address (bits [8:3]), so we must failed.
 */
#define LD_OP            0x080
#define LDS_OP           0x081
#define LDA_OP           0x082
#define LDSA_OP          0x083
#define LDBIAS_OP        0x084
#define LDACQ_OP         0x085
/* 0x086, 0x087 are not relevant */
#define LDCCLR_OP        0x088
#define LDCNC_OP         0x089
#define LDCCLRACQ_OP     0x08a
#define ST_OP            0x08c
#define STREL_OP         0x08d
/* 0x08e,0x8f are not relevant */

/*
 * Table C-29 Integer Load +Reg
 *
 * we use the ld->m (bit [36:36]) field to determine whether or not we have
 * a load/store of this form.
 */

/*
 * Table C-30 Integer Load/Store +Imm
 *
 * We ignore [35:32]= 0x6, 0x7, 0xE, 0xF
 *
 * ld8.fill, st8.fill  must be aligned because the Nat register are based on
 * the address, so we must fail and the program must be fixed.
 */
#define LD_IMM_OP            0x0a0
#define LDS_IMM_OP           0x0a1
#define LDA_IMM_OP           0x0a2
#define LDSA_IMM_OP          0x0a3
#define LDBIAS_IMM_OP        0x0a4
#define LDACQ_IMM_OP         0x0a5
/* 0x0a6, 0xa7 are not relevant */
#define LDCCLR_IMM_OP        0x0a8
#define LDCNC_IMM_OP         0x0a9
#define LDCCLRACQ_IMM_OP     0x0aa
#define ST_IMM_OP            0x0ac
#define STREL_IMM_OP         0x0ad
/* 0x0ae,0xaf are not relevant */

/*
 * Table C-32 Floating-point Load/Store
 */
#define LDF_OP           0x0c0
#define LDFS_OP          0x0c1
#define LDFA_OP          0x0c2
#define LDFSA_OP         0x0c3
/* 0x0c6 is irrelevant */
#define LDFCCLR_OP       0x0c8
#define LDFCNC_OP        0x0c9
/* 0x0cb is irrelevant  */
#define STF_OP           0x0cc

/*
 * Table C-33 Floating-point Load +Reg
 *
 * we use the ld->m (bit [36:36]) field to determine whether or not we have
 * a load/store of this form.
 */

/*
 * Table C-34 Floating-point Load/Store +Imm
 */
#define LDF_IMM_OP       0x0e0
#define LDFS_IMM_OP      0x0e1
#define LDFA_IMM_OP      0x0e2
#define LDFSA_IMM_OP     0x0e3
/* 0x0e6 is irrelevant */
#define LDFCCLR_IMM_OP   0x0e8
#define LDFCNC_IMM_OP    0x0e9
#define STF_IMM_OP       0x0ec

typedef struct {
	unsigned long	 qp:6;	/* [0:5]   */
	unsigned long    r1:7;	/* [6:12]  */
	unsigned long   imm:7;	/* [13:19] */
	unsigned long    r3:7;	/* [20:26] */
	unsigned long     x:1;  /* [27:27] */
	unsigned long  hint:2;	/* [28:29] */
	unsigned long x6_sz:2;	/* [30:31] */
	unsigned long x6_op:4;	/* [32:35], x6 = x6_sz|x6_op */
	unsigned long     m:1;	/* [36:36] */
	unsigned long    op:4;	/* [37:40] */
	unsigned long   pad:23; /* [41:63] */
} load_store_t;


typedef enum {
	UPD_IMMEDIATE,	/* ldXZ r1=[r3],imm(9) */
	UPD_REG		/* ldXZ r1=[r3],r2     */
} update_t;

/*
 * We use tables to keep track of the offsets of registers in the saved state.
 * This way we save having big switch/case statements.
 *
 * We use bit 0 to indicate switch_stack or pt_regs.
 * The offset is simply shifted by 1 bit.
 * A 2-byte value should be enough to hold any kind of offset
 *
 * In case the calling convention changes (and thus pt_regs/switch_stack)
 * simply use RSW instead of RPT or vice-versa.
 */

#define RPO(x)	((size_t) &((struct pt_regs *)0)->x)
#define RSO(x)	((size_t) &((struct switch_stack *)0)->x)

#define RPT(x)		(RPO(x) << 1)
#define RSW(x)		(1| RSO(x)<<1)

#define GR_OFFS(x)	(gr_info[x]>>1)
#define GR_IN_SW(x)	(gr_info[x] & 0x1)

#define FR_OFFS(x)	(fr_info[x]>>1)
#define FR_IN_SW(x)	(fr_info[x] & 0x1)

static u16 gr_info[32]={
	0,			/* r0 is read-only : WE SHOULD NEVER GET THIS */

	RPT(r1), RPT(r2), RPT(r3),

	RSW(r4), RSW(r5), RSW(r6), RSW(r7),

	RPT(r8), RPT(r9), RPT(r10), RPT(r11),
	RPT(r12), RPT(r13), RPT(r14), RPT(r15),

	RPT(r16), RPT(r17), RPT(r18), RPT(r19),
	RPT(r20), RPT(r21), RPT(r22), RPT(r23),
	RPT(r24), RPT(r25), RPT(r26), RPT(r27),
	RPT(r28), RPT(r29), RPT(r30), RPT(r31)
};

static u16 fr_info[32]={
	0,			/* constant : WE SHOULD NEVER GET THIS */
	0,			/* constant : WE SHOULD NEVER GET THIS */

	RSW(f2), RSW(f3), RSW(f4), RSW(f5),

	RPT(f6), RPT(f7), RPT(f8), RPT(f9),

	RSW(f10), RSW(f11), RSW(f12), RSW(f13), RSW(f14),
	RSW(f15), RSW(f16), RSW(f17), RSW(f18), RSW(f19),
	RSW(f20), RSW(f21), RSW(f22), RSW(f23), RSW(f24),
	RSW(f25), RSW(f26), RSW(f27), RSW(f28), RSW(f29),
	RSW(f30), RSW(f31)
};

/* Invalidate ALAT entry for integer register REGNO.  */
static void
invala_gr (int regno)
{
#	define F(reg)	case reg: __asm__ __volatile__ ("invala.e r%0" :: "i"(reg)); break

	switch (regno) {
		F(  0); F(  1); F(  2); F(  3); F(  4); F(  5); F(  6); F(  7);
		F(  8); F(  9); F( 10); F( 11); F( 12); F( 13); F( 14); F( 15);
		F( 16); F( 17); F( 18); F( 19); F( 20); F( 21); F( 22); F( 23);
		F( 24); F( 25); F( 26); F( 27); F( 28); F( 29); F( 30); F( 31);
		F( 32); F( 33); F( 34); F( 35); F( 36); F( 37); F( 38); F( 39);
		F( 40); F( 41); F( 42); F( 43); F( 44); F( 45); F( 46); F( 47);
		F( 48); F( 49); F( 50); F( 51); F( 52); F( 53); F( 54); F( 55);
		F( 56); F( 57); F( 58); F( 59); F( 60); F( 61); F( 62); F( 63);
		F( 64); F( 65); F( 66); F( 67); F( 68); F( 69); F( 70); F( 71);
		F( 72); F( 73); F( 74); F( 75); F( 76); F( 77); F( 78); F( 79);
		F( 80); F( 81); F( 82); F( 83); F( 84); F( 85); F( 86); F( 87);
		F( 88); F( 89); F( 90); F( 91); F( 92); F( 93); F( 94); F( 95);
		F( 96); F( 97); F( 98); F( 99); F(100); F(101); F(102); F(103);
		F(104); F(105); F(106); F(107); F(108); F(109); F(110); F(111);
		F(112); F(113); F(114); F(115); F(116); F(117); F(118); F(119);
		F(120); F(121); F(122); F(123); F(124); F(125); F(126); F(127);
	}
#	undef F
}

/* Invalidate ALAT entry for floating-point register REGNO.  */
static void
invala_fr (int regno)
{
#	define F(reg)	case reg: __asm__ __volatile__ ("invala.e f%0" :: "i"(reg)); break

	switch (regno) {
		F(  0); F(  1); F(  2); F(  3); F(  4); F(  5); F(  6); F(  7);
		F(  8); F(  9); F( 10); F( 11); F( 12); F( 13); F( 14); F( 15);
		F( 16); F( 17); F( 18); F( 19); F( 20); F( 21); F( 22); F( 23);
		F( 24); F( 25); F( 26); F( 27); F( 28); F( 29); F( 30); F( 31);
		F( 32); F( 33); F( 34); F( 35); F( 36); F( 37); F( 38); F( 39);
		F( 40); F( 41); F( 42); F( 43); F( 44); F( 45); F( 46); F( 47);
		F( 48); F( 49); F( 50); F( 51); F( 52); F( 53); F( 54); F( 55);
		F( 56); F( 57); F( 58); F( 59); F( 60); F( 61); F( 62); F( 63);
		F( 64); F( 65); F( 66); F( 67); F( 68); F( 69); F( 70); F( 71);
		F( 72); F( 73); F( 74); F( 75); F( 76); F( 77); F( 78); F( 79);
		F( 80); F( 81); F( 82); F( 83); F( 84); F( 85); F( 86); F( 87);
		F( 88); F( 89); F( 90); F( 91); F( 92); F( 93); F( 94); F( 95);
		F( 96); F( 97); F( 98); F( 99); F(100); F(101); F(102); F(103);
		F(104); F(105); F(106); F(107); F(108); F(109); F(110); F(111);
		F(112); F(113); F(114); F(115); F(116); F(117); F(118); F(119);
		F(120); F(121); F(122); F(123); F(124); F(125); F(126); F(127);
	}
#	undef F
}

static void
set_rse_reg (struct pt_regs *regs, unsigned long r1, unsigned long val, int nat)
{
	struct switch_stack *sw = (struct switch_stack *) regs - 1;
	unsigned long *bsp, *bspstore, *addr, *rnat_addr, *ubs_end;
	unsigned long *kbs = (void *) current + IA64_RBS_OFFSET;
	unsigned long rnats, nat_mask;
	unsigned long on_kbs;
	long sof = (regs->cr_ifs) & 0x7f;
	long sor = 8 * ((regs->cr_ifs >> 14) & 0xf);
	long rrb_gr = (regs->cr_ifs >> 18) & 0x7f;
	long ridx;

	if ((r1 - 32) > sor)
		ridx = -sof + (r1 - 32);
	else if ((r1 - 32) < (sor - rrb_gr))
		ridx = -sof + (r1 - 32) + rrb_gr;
	else
		ridx = -sof + (r1 - 32) - (sor - rrb_gr);

	DPRINT("r%lu, sw.bspstore=%lx pt.bspstore=%lx sof=%ld sol=%ld ridx=%ld\n",
	       r1, sw->ar_bspstore, regs->ar_bspstore, sof, (regs->cr_ifs >> 7) & 0x7f, ridx);

	if ((r1 - 32) >= sof) {
		/* this should never happen, as the "rsvd register fault" has higher priority */
		DPRINT("ignoring write to r%lu; only %lu registers are allocated!\n", r1, sof);
		return;
	}

	on_kbs = ia64_rse_num_regs(kbs, (unsigned long *) sw->ar_bspstore);
	addr = ia64_rse_skip_regs((unsigned long *) sw->ar_bspstore, ridx);
	if (addr >= kbs) {
		/* the register is on the kernel backing store: easy... */
		rnat_addr = ia64_rse_rnat_addr(addr);
		if ((unsigned long) rnat_addr >= sw->ar_bspstore)
			rnat_addr = &sw->ar_rnat;
		nat_mask = 1UL << ia64_rse_slot_num(addr);

		*addr = val;
		if (nat)
			*rnat_addr |=  nat_mask;
		else
			*rnat_addr &= ~nat_mask;
		return;
	}

	/*
	 * Avoid using user_mode() here: with "epc", we cannot use the privilege level to
	 * infer whether the interrupt task was running on the kernel backing store.
	 */
	if (regs->r12 >= TASK_SIZE) {
		DPRINT("ignoring kernel write to r%lu; register isn't on the RBS!", r1);
		return;
	}

	bspstore = (unsigned long *)regs->ar_bspstore;
	ubs_end = ia64_rse_skip_regs(bspstore, on_kbs);
	bsp     = ia64_rse_skip_regs(ubs_end, -sof);
	addr    = ia64_rse_skip_regs(bsp, ridx + sof);

	DPRINT("ubs_end=%p bsp=%p addr=%p\n", (void *) ubs_end, (void *) bsp, (void *) addr);

	ia64_poke(current, sw, (unsigned long) ubs_end, (unsigned long) addr, val);

	rnat_addr = ia64_rse_rnat_addr(addr);

	ia64_peek(current, sw, (unsigned long) ubs_end, (unsigned long) rnat_addr, &rnats);
	DPRINT("rnat @%p = 0x%lx nat=%d old nat=%ld\n",
	       (void *) rnat_addr, rnats, nat, (rnats >> ia64_rse_slot_num(addr)) & 1);

	nat_mask = 1UL << ia64_rse_slot_num(addr);
	if (nat)
		rnats |=  nat_mask;
	else
		rnats &= ~nat_mask;
	ia64_poke(current, sw, (unsigned long) ubs_end, (unsigned long) rnat_addr, rnats);

	DPRINT("rnat changed to @%p = 0x%lx\n", (void *) rnat_addr, rnats);
}


static void
get_rse_reg (struct pt_regs *regs, unsigned long r1, unsigned long *val, int *nat)
{
	struct switch_stack *sw = (struct switch_stack *) regs - 1;
	unsigned long *bsp, *addr, *rnat_addr, *ubs_end, *bspstore;
	unsigned long *kbs = (void *) current + IA64_RBS_OFFSET;
	unsigned long rnats, nat_mask;
	unsigned long on_kbs;
	long sof = (regs->cr_ifs) & 0x7f;
	long sor = 8 * ((regs->cr_ifs >> 14) & 0xf);
	long rrb_gr = (regs->cr_ifs >> 18) & 0x7f;
	long ridx;

	if ((r1 - 32) > sor)
		ridx = -sof + (r1 - 32);
	else if ((r1 - 32) < (sor - rrb_gr))
		ridx = -sof + (r1 - 32) + rrb_gr;
	else
		ridx = -sof + (r1 - 32) - (sor - rrb_gr);

	DPRINT("r%lu, sw.bspstore=%lx pt.bspstore=%lx sof=%ld sol=%ld ridx=%ld\n",
	       r1, sw->ar_bspstore, regs->ar_bspstore, sof, (regs->cr_ifs >> 7) & 0x7f, ridx);

	if ((r1 - 32) >= sof) {
		/* this should never happen, as the "rsvd register fault" has higher priority */
		DPRINT("ignoring read from r%lu; only %lu registers are allocated!\n", r1, sof);
		return;
	}

	on_kbs = ia64_rse_num_regs(kbs, (unsigned long *) sw->ar_bspstore);
	addr = ia64_rse_skip_regs((unsigned long *) sw->ar_bspstore, ridx);
	if (addr >= kbs) {
		/* the register is on the kernel backing store: easy... */
		*val = *addr;
		if (nat) {
			rnat_addr = ia64_rse_rnat_addr(addr);
			if ((unsigned long) rnat_addr >= sw->ar_bspstore)
				rnat_addr = &sw->ar_rnat;
			nat_mask = 1UL << ia64_rse_slot_num(addr);
			*nat = (*rnat_addr & nat_mask) != 0;
		}
		return;
	}

	/*
	 * Avoid using user_mode() here: with "epc", we cannot use the privilege level to
	 * infer whether the interrupt task was running on the kernel backing store.
	 */
	if (regs->r12 >= TASK_SIZE) {
		DPRINT("ignoring kernel read of r%lu; register isn't on the RBS!", r1);
		return;
	}

	bspstore = (unsigned long *)regs->ar_bspstore;
	ubs_end = ia64_rse_skip_regs(bspstore, on_kbs);
	bsp     = ia64_rse_skip_regs(ubs_end, -sof);
	addr    = ia64_rse_skip_regs(bsp, ridx + sof);

	DPRINT("ubs_end=%p bsp=%p addr=%p\n", (void *) ubs_end, (void *) bsp, (void *) addr);

	ia64_peek(current, sw, (unsigned long) ubs_end, (unsigned long) addr, val);

	if (nat) {
		rnat_addr = ia64_rse_rnat_addr(addr);
		nat_mask = 1UL << ia64_rse_slot_num(addr);

		DPRINT("rnat @%p = 0x%lx\n", (void *) rnat_addr, rnats);

		ia64_peek(current, sw, (unsigned long) ubs_end, (unsigned long) rnat_addr, &rnats);
		*nat = (rnats & nat_mask) != 0;
	}
}


static void
setreg (unsigned long regnum, unsigned long val, int nat, struct pt_regs *regs)
{
	struct switch_stack *sw = (struct switch_stack *) regs - 1;
	unsigned long addr;
	unsigned long bitmask;
	unsigned long *unat;

	/*
	 * First takes care of stacked registers
	 */
	if (regnum >= IA64_FIRST_STACKED_GR) {
		set_rse_reg(regs, regnum, val, nat);
		return;
	}

	/*
	 * Using r0 as a target raises a General Exception fault which has higher priority
	 * than the Unaligned Reference fault.
	 */

	/*
	 * Now look at registers in [0-31] range and init correct UNAT
	 */
	if (GR_IN_SW(regnum)) {
		addr = (unsigned long)sw;
		unat = &sw->ar_unat;
	} else {
		addr = (unsigned long)regs;
		unat = &sw->caller_unat;
	}
	DPRINT("tmp_base=%lx switch_stack=%s offset=%d\n",
	       addr, unat==&sw->ar_unat ? "yes":"no", GR_OFFS(regnum));
	/*
	 * add offset from base of struct
	 * and do it !
	 */
	addr += GR_OFFS(regnum);

	*(unsigned long *)addr = val;

	/*
	 * We need to clear the corresponding UNAT bit to fully emulate the load
	 * UNAT bit_pos = GR[r3]{8:3} form EAS-2.4
	 */
	bitmask   = 1UL << (addr >> 3 & 0x3f);
	DPRINT("*0x%lx=0x%lx NaT=%d prev_unat @%p=%lx\n", addr, val, nat, (void *) unat, *unat);
	if (nat) {
		*unat |= bitmask;
	} else {
		*unat &= ~bitmask;
	}
	DPRINT("*0x%lx=0x%lx NaT=%d new unat: %p=%lx\n", addr, val, nat, (void *) unat,*unat);
}

#define IA64_FPH_OFFS(r) (r - IA64_FIRST_ROTATING_FR)

static void
setfpreg (unsigned long regnum, struct ia64_fpreg *fpval, struct pt_regs *regs)
{
	struct switch_stack *sw = (struct switch_stack *)regs - 1;
	unsigned long addr;

	/*
	 * From EAS-2.5: FPDisableFault has higher priority than Unaligned
	 * Fault. Thus, when we get here, we know the partition is enabled.