From 84d6d391ce2b94b2953e99e7f8401bd5f3e1a98d Mon Sep 17 00:00:00 2001
From: mwojcik <mw@m-labs.hk>
Date: Wed, 9 Mar 2022 17:23:18 +0800
Subject: [PATCH] libboard_zynq: eth phy for Marvell 88E1518 * add support for
 LED control registers * support for registers on different pages

---
 libboard_zynq/src/eth/phy/control.rs |  4 ++
 libboard_zynq/src/eth/phy/leds.rs    | 64 ++++++++++++++++++++++++++++
 libboard_zynq/src/eth/phy/mod.rs     |  5 +++
 libboard_zynq/src/eth/phy/pssr.rs    |  4 ++
 libboard_zynq/src/eth/phy/status.rs  |  4 ++
 5 files changed, 81 insertions(+)
 create mode 100644 libboard_zynq/src/eth/phy/leds.rs

diff --git a/libboard_zynq/src/eth/phy/control.rs b/libboard_zynq/src/eth/phy/control.rs
index 92825f6..b4b790d 100644
--- a/libboard_zynq/src/eth/phy/control.rs
+++ b/libboard_zynq/src/eth/phy/control.rs
@@ -82,6 +82,10 @@ impl PhyRegister for Control {
     fn addr() -> u8 {
         0
     }
+    
+    fn page() -> u8 {
+        0
+    }
 }
 
 impl From<u16> for Control {
diff --git a/libboard_zynq/src/eth/phy/leds.rs b/libboard_zynq/src/eth/phy/leds.rs
new file mode 100644
index 0000000..596cb63
--- /dev/null
+++ b/libboard_zynq/src/eth/phy/leds.rs
@@ -0,0 +1,64 @@
+use bit_field::BitField;
+use super::{PhyRegister, Link, LinkDuplex, LinkSpeed};
+
+#[derive(Copy, Clone, Debug, PartialEq)]
+pub enum LedControl {
+    OnLinkOffNoLink = 0b0000,
+    OnLinkBlinkActivityOffNoLink = 0b0001,
+    OnFullDuplexBlinkCollisionOffHalfDuplex = 0b0010,
+    OnActivityOffNoActivity = 0b0011,
+    BlinkActivityOffNoActivity = 0b0100,
+    OnTransmitOffNoTransmit = 0b0101,
+    On101000LinkOffElse = 0b0110,
+    On10LinkOffElse = 0b0111,
+    ForceOff = 0b1000,
+    ForceOn = 0b1001,
+    ForceHiZ = 0b1010,
+    ForceBlink = 0b1011,
+    //LED[0] only
+    Mode1 = 0b1100,
+    Mode2 = 0b1101,
+    Mode3 = 0b1110,
+    Mode4 = 0b1111
+}
+
+#[derive(Clone, Copy, Debug)]
+/// LED Control Register
+pub struct Leds(pub u16);
+
+impl Leds {
+    pub fn led(led_no: u8) -> LedControl {
+        match self.0.get_bits((led_no*4)..=(led_no*4+3)) {
+            0b0000 => LedControl::OnLinkOffNoLink,
+            0b0001 => LedControl::OnLinkBlinkActivityOffNoLink,
+            0b0010 => LedControl::OnFullDuplexBlinkCollisionOffHalfDuplex,
+            0b0011 => LedControl::OnActivityOffNoActivity,
+            0b0100 => LedControl::BlinkActivityOffNoActivity,
+            0b0101 => LedControl::OnTransmitOffNoTransmit,
+            0b0110 => LedControl::On101000LinkOffElse,
+            0b0111 => LedControl::On10LinkOffElse,
+            0b1000 => LedControl::ForceOff,
+            0b1001 => LedControl::ForceOn,
+            0b1010 => LedControl::ForceHiZ,
+            0b1011 => LedControl::ForceBlink,
+            0b1100 => LedControl::Mode1,
+            0b1101 => LedControl::Mode2,
+            0b1110 => LedControl::Mode3,
+            0b1111 => LedControl::Mode4,
+        }
+    }
+
+    pub fn set_led(led_no: u8, setting: LedControl) -> Self {
+        self.0.set_bits((led_no*4)..=(led_no*4+3), setting as u8)
+    }
+}
+
+impl PhyRegister for Leds {
+    fn addr() -> u8 {
+        0x10
+    }
+        
+    fn page() -> u8 {
+        3
+    }
+}
\ No newline at end of file
diff --git a/libboard_zynq/src/eth/phy/mod.rs b/libboard_zynq/src/eth/phy/mod.rs
index 68020a3..16c3d68 100644
--- a/libboard_zynq/src/eth/phy/mod.rs
+++ b/libboard_zynq/src/eth/phy/mod.rs
@@ -33,6 +33,7 @@ pub trait PhyAccess {
 
 pub trait PhyRegister {
     fn addr() -> u8;
+    fn page() -> u8;
 }
 
 
@@ -45,6 +46,8 @@ const OUI_MARVELL: u32 = 0x005043;
 const OUI_REALTEK: u32 = 0x000732;
 const OUI_LANTIQ : u32 = 0x355969;
 
+const PAGE_REGISTER: u8 = 0x16;
+
 impl Phy {
     /// Probe all addresses on MDIO for a known PHY
     pub fn find<PA: PhyAccess>(pa: &mut PA) -> Option<Phy> {
@@ -84,6 +87,7 @@ impl Phy {
         PA: PhyAccess,
         PR: PhyRegister + From<u16>,
     {
+        pa.write_phy(self.addr, PAGE_REGISTER, PR::page());
         pa.read_phy(self.addr, PR::addr()).into()
     }
 
@@ -93,6 +97,7 @@ impl Phy {
         PR: PhyRegister + From<u16> + Into<u16>,
         F: FnMut(PR) -> PR,
     {
+        pa.write_phy(self.addr, PAGE_REGISTER, PR::page());
         let reg = pa.read_phy(self.addr, PR::addr()).into();
         let reg = f(reg);
         pa.write_phy(self.addr, PR::addr(), reg.into())
diff --git a/libboard_zynq/src/eth/phy/pssr.rs b/libboard_zynq/src/eth/phy/pssr.rs
index 8af525e..6061253 100644
--- a/libboard_zynq/src/eth/phy/pssr.rs
+++ b/libboard_zynq/src/eth/phy/pssr.rs
@@ -43,6 +43,10 @@ impl PhyRegister for PSSR {
     fn addr() -> u8 {
         0x11
     }
+        
+    fn page() -> u8 {
+        0
+    }
 }
 
 impl From<u16> for PSSR {
diff --git a/libboard_zynq/src/eth/phy/status.rs b/libboard_zynq/src/eth/phy/status.rs
index 1ff8016..d39b080 100644
--- a/libboard_zynq/src/eth/phy/status.rs
+++ b/libboard_zynq/src/eth/phy/status.rs
@@ -100,6 +100,10 @@ impl PhyRegister for Status {
     fn addr() -> u8 {
         1
     }
+        
+    fn page() -> u8 {
+        0
+    }
 }
 
 impl From<u16> for Status {