From 9765a486de17db5fcb9d5ce33d7eac9130d2f268 Mon Sep 17 00:00:00 2001
From: Mark Poliakov <mark@alnyan.me>
Date: Thu, 7 Oct 2021 14:01:59 +0300
Subject: [PATCH] feat(orangepi3): irq-safe gpio

---
 .../src/arch/aarch64/mach_orangepi3/gpio.rs   | 65 +++++++++++--------
 1 file changed, 37 insertions(+), 28 deletions(-)

diff --git a/kernel/src/arch/aarch64/mach_orangepi3/gpio.rs b/kernel/src/arch/aarch64/mach_orangepi3/gpio.rs
index 880bd0b..f92e213 100644
--- a/kernel/src/arch/aarch64/mach_orangepi3/gpio.rs
+++ b/kernel/src/arch/aarch64/mach_orangepi3/gpio.rs
@@ -3,6 +3,7 @@ use crate::dev::{
     gpio::{GpioDevice, PinConfig, PinMode, PullMode},
     Device,
 };
+use crate::sync::IrqSafeNullLock;
 use error::Errno;
 use tock_registers::interfaces::{Readable, Writeable};
 use tock_registers::register_structs;
@@ -23,7 +24,26 @@ register_structs! {
 }
 
 pub(super) struct Gpio {
-    regs: MemoryIo<Regs>,
+    regs: IrqSafeNullLock<MemoryIo<Regs>>,
+}
+
+impl Regs {
+    #[inline]
+    fn set_pin_cfg_inner(&self, pin: u32, cfg: u32) {
+        let reg = pin >> 3;
+        let shift = (pin & 0x7) * 4;
+        let tmp = self.CFG[reg as usize].get() & !(0xF << shift);
+        self.CFG[reg as usize].set(tmp | ((cfg & 0x7) << shift));
+    }
+
+    #[inline]
+    fn set_pin_pul_inner(&self, pin: u32, pul: u32) {
+        let reg = pin >> 4;
+        let shift = (pin & 0xF) * 2;
+        let tmp = self.PUL[reg as usize].get() & !(0x3 << shift);
+        self.PUL[reg as usize].set(tmp | ((pul & 0x3) << shift));
+    }
+
 }
 
 impl Device for Gpio {
@@ -38,6 +58,7 @@ impl Device for Gpio {
 
 impl GpioDevice for Gpio {
     unsafe fn set_pin_config(&self, pin: u32, cfg: &PinConfig) -> Result<(), Errno> {
+        let regs = self.regs.lock();
         let pull = match cfg.pull {
             PullMode::None => 0,
             PullMode::Up => 1,
@@ -45,21 +66,21 @@ impl GpioDevice for Gpio {
         };
 
         match cfg.mode {
-            PinMode::Disable => self.set_pin_cfg_inner(pin, 7),
+            PinMode::Disable => regs.set_pin_cfg_inner(pin, 7),
             PinMode::Input => {
-                self.set_pin_cfg_inner(pin, 0);
-                self.set_pin_pul_inner(pin, pull);
+                regs.set_pin_cfg_inner(pin, 0);
+                regs.set_pin_pul_inner(pin, pull);
             }
             PinMode::Output => {
-                self.set_pin_cfg_inner(pin, 1); // TODO is it the same for all pins?
-                self.set_pin_pul_inner(pin, pull);
+                regs.set_pin_cfg_inner(pin, 1); // TODO is it the same for all pins?
+                regs.set_pin_pul_inner(pin, pull);
             }
             PinMode::InputInterrupt => {
                 todo!()
             }
             PinMode::Alt => {
                 assert!(cfg.func > 1 && cfg.func < 7);
-                self.set_pin_cfg_inner(pin, cfg.func);
+                regs.set_pin_cfg_inner(pin, cfg.func);
             }
         }
         Ok(())
@@ -70,42 +91,30 @@ impl GpioDevice for Gpio {
     }
 
     fn set_pin(&self, pin: u32) {
-        self.regs.DAT.set(self.regs.DAT.get() | (1 << pin));
+        let regs = self.regs.lock();
+        regs.DAT.set(regs.DAT.get() | (1 << pin));
     }
 
     fn clear_pin(&self, pin: u32) {
-        self.regs.DAT.set(self.regs.DAT.get() & !(1 << pin));
+        let regs = self.regs.lock();
+        regs.DAT.set(regs.DAT.get() & !(1 << pin));
     }
 
     fn toggle_pin(&self, pin: u32) {
-        self.regs.DAT.set(self.regs.DAT.get() ^ (1 << pin));
+        let regs = self.regs.lock();
+        regs.DAT.set(regs.DAT.get() ^ (1 << pin));
     }
 
     fn read_pin(&self, pin: u32) -> Result<bool, Errno> {
-        Ok(self.regs.DAT.get() & (1 << pin) != 0)
+        let regs = self.regs.lock();
+        Ok(regs.DAT.get() & (1 << pin) != 0)
     }
 }
 
 impl Gpio {
-    #[inline]
-    fn set_pin_cfg_inner(&self, pin: u32, cfg: u32) {
-        let reg = pin >> 3;
-        let shift = (pin & 0x7) * 4;
-        let tmp = self.regs.CFG[reg as usize].get() & !(0xF << shift);
-        self.regs.CFG[reg as usize].set(tmp | ((cfg & 0x7) << shift));
-    }
-
-    #[inline]
-    fn set_pin_pul_inner(&self, pin: u32, pul: u32) {
-        let reg = pin >> 4;
-        let shift = (pin & 0xF) * 2;
-        let tmp = self.regs.PUL[reg as usize].get() & !(0x3 << shift);
-        self.regs.PUL[reg as usize].set(tmp | ((pul & 0x3) << shift));
-    }
-
     pub const unsafe fn new(base: usize) -> Self {
         Self {
-            regs: MemoryIo::new(base),
+            regs: IrqSafeNullLock::new(MemoryIo::new(base)),
         }
     }
 }