arm64: Enable MOPS in userspace

Detect presence of FEAT_MOPS, and enable instruction set and set HWCAP2 flag
if present.

Add handler for MOE exceptions.

Reviewed by:	andrew
Sponsored by:	Arm Ltd
Differential Revision:	https://reviews.freebsd.org/D54558

sys/arm64/arm64/identcpu.c

@@ -1094,6 +1094,11 @@ static const struct mrs_field_value id_aa64isar2_mops[] = {
 	MRS_FIELD_VALUE_END,
 };
 
+static const struct mrs_field_hwcap id_aa64isar2_mops_caps[] = {
+	MRS_HWCAP(2, HWCAP2_MOPS, ID_AA64ISAR2_MOPS_IMPL),
+	MRS_HWCAP_END
+};
+
 static const struct mrs_field_value id_aa64isar2_apa3[] = {
 	MRS_FIELD_VALUE(ID_AA64ISAR2_APA3_NONE, ""),
 	MRS_FIELD_VALUE(ID_AA64ISAR2_APA3_PAC, "APA3 PAC"),
@@ -1149,7 +1154,8 @@ static const struct mrs_field id_aa64isar2_fields[] = {
 	MRS_FIELD(ID_AA64ISAR2, PAC_frac, false, MRS_LOWER, 0,
 	    id_aa64isar2_pac_frac),
 	MRS_FIELD(ID_AA64ISAR2, BC, false, MRS_LOWER, 0, id_aa64isar2_bc),
-	MRS_FIELD(ID_AA64ISAR2, MOPS, false, MRS_LOWER, 0, id_aa64isar2_mops),
+	MRS_FIELD_HWCAP(ID_AA64ISAR2, MOPS, false, MRS_LOWER, MRS_USERSPACE,
+	    id_aa64isar2_mops, id_aa64isar2_mops_caps),
 	MRS_FIELD_HWCAP(ID_AA64ISAR2, APA3, false, MRS_LOWER, MRS_USERSPACE,
 	    id_aa64isar2_apa3, id_aa64isar2_apa3_caps),
 	MRS_FIELD_HWCAP(ID_AA64ISAR2, GPA3, false, MRS_LOWER, MRS_USERSPACE,

sys/arm64/arm64/machdep.c

@@ -219,6 +219,41 @@ CPU_FEAT(feat_pan, "Privileged access never",
     pan_check, NULL, pan_enable, pan_disabled,
     CPU_FEAT_AFTER_DEV | CPU_FEAT_PER_CPU);
 
+static cpu_feat_en
+mops_check(const struct cpu_feat *feat __unused, u_int midr __unused)
+{
+	uint64_t id_aa64isar2;
+
+	if (!get_kernel_reg(ID_AA64ISAR2_EL1, &id_aa64isar2))
+		return (FEAT_ALWAYS_DISABLE);
+	if (ID_AA64ISAR2_MOPS_VAL(id_aa64isar2) == ID_AA64ISAR2_MOPS_NONE)
+		return (FEAT_ALWAYS_DISABLE);
+
+	return (FEAT_DEFAULT_ENABLE);
+}
+
+static bool
+mops_enable(const struct cpu_feat *feat __unused,
+    cpu_feat_errata errata_status __unused, u_int *errata_list __unused,
+    u_int errata_count __unused)
+{
+	WRITE_SPECIALREG(sctlr_el1, READ_SPECIALREG(sctlr_el1) | SCTLR_MSCEn);
+	isb();
+
+	return (true);
+}
+
+static void
+mops_disabled(const struct cpu_feat *feat __unused)
+{
+	WRITE_SPECIALREG(sctlr_el1, READ_SPECIALREG(sctlr_el1) & ~SCTLR_MSCEn);
+	isb();
+}
+
+CPU_FEAT(feat_mops, "MOPS",
+    mops_check, NULL, mops_enable, mops_disabled,
+    CPU_FEAT_AFTER_DEV | CPU_FEAT_PER_CPU);
+
 bool
 has_hyp(void)
 {

sys/arm64/arm64/trap.c

@@ -597,6 +597,66 @@ do_el1h_sync(struct thread *td, struct trapframe *frame)
 	}
 }
 
+static void
+handle_moe(struct thread *td, struct trapframe *frame, uint64_t esr)
+{
+	uint64_t src;
+	uint64_t dest;
+	uint64_t size;
+	int src_reg;
+	int dest_reg;
+	int size_reg;
+	int format_option;
+
+	format_option = esr & ISS_MOE_FORMAT_OPTION_MASK;
+	dest_reg = (esr & ISS_MOE_DESTREG_MASK) >> ISS_MOE_DESTREG_SHIFT;
+	size_reg = (esr & ISS_MOE_SIZEREG_MASK) >> ISS_MOE_SIZEREG_SHIFT;
+	dest = frame->tf_x[dest_reg];
+	size = frame->tf_x[size_reg];
+
+	/*
+	 * Put the registers back in the original format suitable for a
+	 * prologue instruction, using the generic return routine from the
+	 * Arm ARM (DDI 0487I.a) rules CNTMJ and MWFQH.
+	 */
+	if (esr & ISS_MOE_MEMINST) {
+		/* SET* instruction */
+		if (format_option == ISS_MOE_FORMAT_OPTION_A ||
+		    format_option == ISS_MOE_FORMAT_OPTION_A2) {
+			/* Format is from Option A; forward set */
+			frame->tf_x[dest_reg] = dest + size;
+			frame->tf_x[size_reg] = -size;
+		}
+	} else {
+		/* CPY* instruction */
+		src_reg = (esr & ISS_MOE_SRCREG_MASK) >> ISS_MOE_SRCREG_SHIFT;
+		src = frame->tf_x[src_reg];
+
+		if (format_option == ISS_MOE_FORMAT_OPTION_B ||
+		    format_option == ISS_MOE_FORMAT_OPTION_B2) {
+			/* Format is from Option B */
+			if (frame->tf_spsr & PSR_N) {
+				/* Backward copy */
+				frame->tf_x[dest_reg] = dest - size;
+				frame->tf_x[src_reg] = src + size;
+			}
+		} else {
+			/* Format is from Option A */
+			if (frame->tf_x[size_reg] & (1UL << 63)) {
+				/* Forward copy */
+				frame->tf_x[dest_reg] = dest + size;
+				frame->tf_x[src_reg] = src + size;
+				frame->tf_x[size_reg] = -size;
+			}
+		}
+	}
+
+	if (esr & ISS_MOE_FROM_EPILOGUE)
+		frame->tf_elr -= 8;
+	else
+		frame->tf_elr -= 4;
+}
+
 void
 do_el0_sync(struct thread *td, struct trapframe *frame)
 {
@@ -738,6 +798,10 @@ do_el0_sync(struct thread *td, struct trapframe *frame)
 		    exception);
 		userret(td, frame);
 		break;
+	case EXCP_MOE:
+		handle_moe(td, frame, esr);
+		userret(td, frame);
+		break;
 	default:
 		call_trapsignal(td, SIGBUS, BUS_OBJERR, (void *)frame->tf_elr,
 		    exception);