C++ LDST_L_BIT函数代码示例

static int
do_alignment_ldrstr(unsigned long addr, unsigned long instr, struct pt_regs *regs)
{
	unsigned int rd = RD_BITS(instr);

	ai_word += 1;

	if ((!LDST_P_BIT(instr) && LDST_W_BIT(instr)) || user_mode(regs))
		goto trans;

	if (LDST_L_BIT(instr)) {
		unsigned int val;
		get32_unaligned_check(val, addr);
		regs->uregs[rd] = val;
	} else
		put32_unaligned_check(regs->uregs[rd], addr);
	return TYPE_LDST;

 trans:
	if (LDST_L_BIT(instr)) {
		unsigned int val;
		get32t_unaligned_check(val, addr);
		regs->uregs[rd] = val;
	} else
		put32t_unaligned_check(regs->uregs[rd], addr);
	return TYPE_LDST;

 fault:
	return TYPE_FAULT;
}

static int
do_alignment_ldrhstrh(unsigned long addr, unsigned long instr, struct pt_regs *regs)
{
	unsigned int rd = RD_BITS(instr);

	if ((instr & 0x01f00ff0) == 0x01000090)
		goto swp;

	if ((instr & 0x90) != 0x90 || (instr & 0x60) == 0)
		goto bad;

	ai_half += 1;

	if (user_mode(regs))
		goto user;

	if (LDST_L_BIT(instr)) {
		unsigned long val;
		get16_unaligned_check(val, addr);

		/* signed half-word? */
		if (instr & 0x40)
			val = (signed long)((signed short) val);

		regs->uregs[rd] = val;
	} else
		put16_unaligned_check(regs->uregs[rd], addr);

	return TYPE_LDST;

 user:
 	if (LDST_L_BIT(instr)) {
 		unsigned long val;
 		get16t_unaligned_check(val, addr);

 		/* signed half-word? */
 		if (instr & 0x40)
 			val = (signed long)((signed short) val);

 		regs->uregs[rd] = val;
 	} else
 		put16t_unaligned_check(regs->uregs[rd], addr);

 	return TYPE_LDST;

 swp:
	printk(KERN_ERR "Alignment trap: not handling swp instruction\n");
 bad:
	return TYPE_ERROR;

 fault:
	return TYPE_FAULT;
}

static int
do_alignment_ldrhstrh(unsigned long addr, unsigned long instr, struct pt_regs *regs)
{
	unsigned int rd = RD_BITS(instr);

	ai_half += 1;

	if (user_mode(regs))
		goto user;

	if (LDST_L_BIT(instr)) {
		unsigned long val;
		get16_unaligned_check(val, addr);

		/* signed half-word? */
		if (instr & 0x40)
			val = (signed long)((signed short) val);

		regs->uregs[rd] = val;
	} else
		put16_unaligned_check(regs->uregs[rd], addr);

	return TYPE_LDST;

 user:
	if (LDST_L_BIT(instr)) {
		unsigned long val;
		unsigned int __ua_flags = uaccess_save_and_enable();

		get16t_unaligned_check(val, addr);
		uaccess_restore(__ua_flags);

		/* signed half-word? */
		if (instr & 0x40)
			val = (signed long)((signed short) val);

		regs->uregs[rd] = val;
	} else {
		unsigned int __ua_flags = uaccess_save_and_enable();
		put16t_unaligned_check(regs->uregs[rd], addr);
		uaccess_restore(__ua_flags);
	}

	return TYPE_LDST;

 fault:
	return TYPE_FAULT;
}

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

/*
 * 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;

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