From 191abf6b8fef75f61d44f134231311e7f235c23c Mon Sep 17 00:00:00 2001
From: Astro <astro@spaceboyz.net>
Date: Tue, 7 Jul 2020 23:51:33 +0200
Subject: [PATCH] mpidr: wrap with proper bitfield getters

Prevents callers from dealing with CORE_MASK.
---
 libcortex_a9/src/regs.rs     | 13 ++++++++++++-
 libsupport_zynq/src/abort.rs |  3 +--
 libsupport_zynq/src/boot.rs  |  4 +---
 3 files changed, 14 insertions(+), 6 deletions(-)

diff --git a/libcortex_a9/src/regs.rs b/libcortex_a9/src/regs.rs
index 2d5c261..f49d89f 100644
--- a/libcortex_a9/src/regs.rs
+++ b/libcortex_a9/src/regs.rs
@@ -91,8 +91,19 @@ pub struct HVBAR;
 def_reg_r!(HVBAR, u32, "mrc p15, 4, $0, c12, c0, 0");
 def_reg_w!(HVBAR, u32, "mcr p15, 4, $0, c12, c0, 0");
 
+/// Multiprocess Affinity Register
 pub struct MPIDR;
-def_reg_r!(MPIDR, u32, "mrc p15, 0, $0, c0, c0, 5");
+def_reg_r!(MPIDR, mpidr::Read, "mrc p15, 0, $0, c0, c0, 5");
+wrap_reg!(mpidr);
+register_bits!(mpidr,
+               /// CPU core index
+               cpu_id, u8, 0, 1);
+register_bits!(mpidr,
+               /// Processor index in "multi-socket" systems
+               cluster_id, u8, 8, 11);
+register_bit!(mpidr,
+              /// true if part of uniprocessor system
+              u, 30);
 
 pub struct DFAR;
 def_reg_r!(DFAR, u32, "mrc p15, 0, $0, c6, c0, 0");
diff --git a/libsupport_zynq/src/abort.rs b/libsupport_zynq/src/abort.rs
index 1a00648..254a53c 100644
--- a/libsupport_zynq/src/abort.rs
+++ b/libsupport_zynq/src/abort.rs
@@ -35,8 +35,7 @@ pub unsafe extern "C" fn PrefetchAbort() {
 pub unsafe extern "C" fn DataAbort() {
     stdio::drop_uart();
 
-    const CORE_MASK: u32 = 0x3;
-    println!("DataAbort on core {}", MPIDR.read() & CORE_MASK);
+    println!("DataAbort on core {}", MPIDR.read().cpu_id());
     println!("DFSR: {:03X}", DFSR.read());
 
     loop {}
diff --git a/libsupport_zynq/src/boot.rs b/libsupport_zynq/src/boot.rs
index 79eb9e6..649cc12 100644
--- a/libsupport_zynq/src/boot.rs
+++ b/libsupport_zynq/src/boot.rs
@@ -22,9 +22,7 @@ static mut CORE1_ENABLED: VolatileCell<bool> = VolatileCell::new(false);
 #[no_mangle]
 #[naked]
 pub unsafe extern "C" fn Reset() -> ! {
-    const CORE_MASK: u32 = 0x3;
-
-    match MPIDR.read() & CORE_MASK {
+    match MPIDR.read().cpu_id() {
         0 => {
             SP.write(&mut __stack0_start as *mut _ as u32);
             boot_core0();