From b3b65f9b7499b4595dbf9a689c6033a95e8559ff Mon Sep 17 00:00:00 2001
From: Astro <astro@spaceboyz.net>
Date: Wed, 19 Jun 2019 00:21:17 +0200
Subject: [PATCH] eth: find Phy

---
 src/eth/phy/id.rs  | 28 ++++++++++++++++++++++++++++
 src/eth/phy/mod.rs | 41 +++++++++++++++++++++++++++++++++++++++--
 src/main.rs        | 21 +++++++++++++++------
 3 files changed, 82 insertions(+), 8 deletions(-)
 create mode 100644 src/eth/phy/id.rs

diff --git a/src/eth/phy/id.rs b/src/eth/phy/id.rs
new file mode 100644
index 00000000..03e830b4
--- /dev/null
+++ b/src/eth/phy/id.rs
@@ -0,0 +1,28 @@
+use bit_field::BitField;
+
+use super::PhyAccess;
+
+
+#[derive(Clone, Debug)]
+pub struct PhyIdentifier {
+    pub oui: u32,
+    pub model: u8,
+    pub rev: u8,
+}
+
+pub fn identify_phy<PA: PhyAccess>(pa: &mut PA, addr: u8) -> Option<PhyIdentifier> {
+    let id1 = pa.read_phy(addr, 2);
+    let id2 = pa.read_phy(addr, 3);
+    if id1 != 0xFFFF || id2 != 0xFFFF {
+        let mut oui = 0;
+        oui.set_bits(6..=21, id1.get_bits(0..=15).into());
+        oui.set_bits(0..=5, id2.get_bits(10..=15).into());
+        Some(PhyIdentifier {
+            oui,
+            model: id2.get_bits(4..=9) as u8,
+            rev: id2.get_bits(0..=3) as u8,
+        })
+    } else {
+        None
+    }
+}
diff --git a/src/eth/phy/mod.rs b/src/eth/phy/mod.rs
index fe2576c5..1b270e59 100644
--- a/src/eth/phy/mod.rs
+++ b/src/eth/phy/mod.rs
@@ -1,8 +1,45 @@
+pub mod id;
+use id::{identify_phy, PhyIdentifier};
+
 pub trait PhyAccess {
     fn read_phy(&mut self, addr: u8, reg: u8) -> u16;
     fn write_phy(&mut self, addr: u8, reg: u8, data: u16);
 }
 
-pub trait PhyDevice {
-    fn init() -> Self;
+pub enum Phy {
+    Marvel88E1116R,
+    Rtl8211E,
+}
+
+const OUI_MARVEL: u32 = 0x005043;
+const OUI_REALTEK: u32 = 0x000732;
+
+impl Phy {
+    /// Probe all addresses on MDIO for a known PHY
+    pub fn find<PA: PhyAccess>(pa: &mut PA) -> Option<(u8, Phy)> {
+        for addr in 1..32 {
+            match identify_phy(pa, addr) {
+                Some(PhyIdentifier {
+                    oui: OUI_MARVEL,
+                    model: 36,
+                    ..
+                }) => return Some((addr, Phy::Marvel88E1116R)),
+                Some(PhyIdentifier {
+                    oui: OUI_REALTEK,
+                    model: 0b010001,
+                    rev: 0b0101,
+                }) => return Some((addr, Phy::Rtl8211E)),
+                _ => {}
+            }
+        }
+
+        None
+    }
+
+    pub fn name(&self) -> &'static str {
+        match self {
+            Phy::Marvel88E1116R => &"Marvel 88E1116R",
+            Phy::Rtl8211E => &"RTL8211E",
+        }
+    }
 }
diff --git a/src/main.rs b/src/main.rs
index c15c4063..2519ce3c 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -79,12 +79,21 @@ fn main() {
 
     let mut eth = eth::Eth::default([0x0, 0x17, 0xde, 0xea, 0xbe, 0xef]);
     writeln!(uart, "Eth on\r");
-    use eth::phy::PhyAccess;
-    for addr in 1..=31 {
-        let detect = eth.read_phy(addr, 1);
-        let id1 = eth.read_phy(addr, 2);
-        let id2 = eth.read_phy(addr, 3);
-        writeln!(uart, "phy {}: {:04X} {:04X} {:04X}\r", addr, detect, id1, id2);
+    match eth::phy::Phy::find(&mut eth) {
+        Some((addr, phy)) => {
+            writeln!(uart, "Found {} PHY at addr {}\r", phy.name(), addr);
+        }
+        None => {
+            use eth::phy::PhyAccess;
+            for addr in 1..32 {
+                match eth::phy::id::identify_phy(&mut eth, addr) {
+                    Some(identifier) => {
+                        writeln!(uart, "phy {}: {:?}\r", addr, identifier);
+                    }
+                    None => {}
+                }
+            }
+        }
     }
     while !uart.tx_fifo_empty() {}