efDataDiscreteRateMining preliminaryDataProcessing.c 韩正义 commit at 2021-04-01

efDataDiscreteRateMining/monitoringDataProcessing/preliminaryDataProcessing.c

@@ -827,3 +827,163 @@ do_alignment(unsigned long addr, unsigned int fsr, struct pt_regs *regs)
 				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;
+			}
+		}
+		handler = do_alignment_ldrstr;
+		break;
+
+	case 0x08000000:	/* ldm or stm, or thumb-2 32bit instruction */
+		if (thumb2_32b) {
+			offset.un = 0;
+			handler = do_alignment_t32_to_handler(&instr, regs, &offset);
+		} else {
+			offset.un = 0;
+			handler = do_alignment_ldmstm;
+		}
+		break;
+
+	default:
+		goto bad;
+	}
+
+	if (!handler)
+		goto bad;
+	type = handler(addr, instr, regs);
+
+	if (type == TYPE_ERROR || type == TYPE_FAULT) {
+		regs->ARM_pc -= isize;
+		goto bad_or_fault;
+	}
+
+	if (type == TYPE_LDST)
+		do_alignment_finish_ldst(addr, instr, regs, offset);
+
+	return 0;
+
+ bad_or_fault:
+	if (type == TYPE_ERROR)
+		goto bad;
+	/*
+	 * We got a fault - fix it up, or die.
+	 */
+	do_bad_area(addr, fsr, regs);
+	return 0;
+
+ swp:
+	printk(KERN_ERR "Alignment trap: not handling swp instruction\n");
+
+ bad:
+	/*
+	 * Oops, we didn't handle the instruction.
+	 */
+	printk(KERN_ERR "Alignment trap: not handling instruction "
+		"%0*lx at [<%08lx>]\n",
+		isize << 1,
+		isize == 2 ? tinstr : instr, instrptr);
+	ai_skipped += 1;
+	return 1;
+
+ user:
+	ai_user += 1;
+
+	if (ai_usermode & UM_WARN)
+		printk("Alignment trap: %s (%d) PC=0x%08lx Instr=0x%0*lx "
+		       "Address=0x%08lx FSR 0x%03x\n", current->comm,
+			task_pid_nr(current), instrptr,
+			isize << 1,
+			isize == 2 ? tinstr : instr,
+		        addr, fsr);
+
+	if (ai_usermode & UM_FIXUP)
+		goto fixup;
+
+	if (ai_usermode & UM_SIGNAL) {
+		siginfo_t si;
+
+		si.si_signo = SIGBUS;
+		si.si_errno = 0;
+		si.si_code = BUS_ADRALN;
+		si.si_addr = (void __user *)addr;
+
+		force_sig_info(si.si_signo, &si, current);
+	} else {
+		/*
+		 * We're about to disable the alignment trap and return to
+		 * user space.  But if an interrupt occurs before actually
+		 * reaching user space, then the IRQ vector entry code will
+		 * notice that we were still in kernel space and therefore
+		 * the alignment trap won't be re-enabled in that case as it
+		 * is presumed to be always on from kernel space.
+		 * Let's prevent that race by disabling interrupts here (they
+		 * are disabled on the way back to user space anyway in
+		 * entry-common.S) and disable the alignment trap only if
+		 * there is no work pending for this thread.
+		 */
+		raw_local_irq_disable();
+		if (!(current_thread_info()->flags & _TIF_WORK_MASK))
+			set_cr(cr_no_alignment);
+	}
+
+	return 0;
+}
+
+/*
+ * This needs to be done after sysctl_init, otherwise sys/ will be
+ * overwritten.  Actually, this shouldn't be in sys/ at all since
+ * it isn't a sysctl, and it doesn't contain sysctl information.
+ * We now locate it in /proc/cpu/alignment instead.
+ */
+static int __init alignment_init(void)
+{
+#ifdef CONFIG_PROC_FS
+	struct proc_dir_entry *res;
+
+	res = proc_create("cpu/alignment", S_IWUSR | S_IRUGO, NULL,
+			  &alignment_proc_fops);
+	if (!res)
+		return -ENOMEM;
+#endif
+
+	if (cpu_is_v6_unaligned()) {
+		cr_alignment &= ~CR_A;
+		cr_no_alignment &= ~CR_A;
+		set_cr(cr_alignment);
+		ai_usermode = safe_usermode(ai_usermode, false);
+	}
+
+	hook_fault_code(FAULT_CODE_ALIGNMENT, do_alignment, SIGBUS, BUS_ADRALN,
+			"alignment exception");
+
+	/*
+	 * ARMv6K and ARMv7 use fault status 3 (0b00011) as Access Flag section
+	 * fault, not as alignment error.
+	 *
+	 * TODO: handle ARMv6K properly. Runtime check for 'K' extension is
+	 * needed.
+	 */
+	if (cpu_architecture() <= CPU_ARCH_ARMv6) {
+		hook_fault_code(3, do_alignment, SIGBUS, BUS_ADRALN,
+				"alignment exception");
+	}
+
+	return 0;
+}
+
+fs_initcall(alignment_init);