Merge pull request #2 from EFForg/server

implements server
2026-06-12 15:53:30 -07:00 · 2023-11-15 09:50:40 -08:00
parent 2b776815eb 5bd2c3f653
commit 4729525235
3 changed files with 236 additions and 50 deletions
@@ -3,23 +3,18 @@
 version = 3

 [[package]]
-name = "bitflags"
-version = "2.4.1"
+name = "bytes"
+version = "1.5.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "327762f6e5a765692301e5bb513e0d9fef63be86bbc14528052b1cd3e6f03e07"
-
-[[package]]
-name = "cfg-if"
-version = "1.0.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd"
+checksum = "a2bd12c1caf447e69cd4528f47f94d203fd2582878ecb9e9465484c4148a8223"

 [[package]]
 name = "diag"
 version = "0.1.0"
 dependencies = [
+ "bytes",
 "libc",
- "nix",
+ "thiserror",
 ]

 [[package]]
@@ -29,12 +24,56 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "89d92a4743f9a61002fae18374ed11e7973f530cb3a3255fb354818118b2203c"

 [[package]]
-name = "nix"
-version = "0.27.1"
+name = "proc-macro2"
+version = "1.0.69"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2eb04e9c688eff1c89d72b407f168cf79bb9e867a9d3323ed6c01519eb9cc053"
+checksum = "134c189feb4956b20f6f547d2cf727d4c0fe06722b20a0eec87ed445a97f92da"
 dependencies = [
- "bitflags",
- "cfg-if",
- "libc",
+ "unicode-ident",
 ]
+
+[[package]]
+name = "quote"
+version = "1.0.33"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "5267fca4496028628a95160fc423a33e8b2e6af8a5302579e322e4b520293cae"
+dependencies = [
+ "proc-macro2",
+]
+
+[[package]]
+name = "syn"
+version = "2.0.39"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "23e78b90f2fcf45d3e842032ce32e3f2d1545ba6636271dcbf24fa306d87be7a"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "unicode-ident",
+]
+
+[[package]]
+name = "thiserror"
+version = "1.0.50"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f9a7210f5c9a7156bb50aa36aed4c95afb51df0df00713949448cf9e97d382d2"
+dependencies = [
+ "thiserror-impl",
+]
+
+[[package]]
+name = "thiserror-impl"
+version = "1.0.50"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "266b2e40bc00e5a6c09c3584011e08b06f123c00362c92b975ba9843aaaa14b8"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
+[[package]]
+name = "unicode-ident"
+version = "1.0.12"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b"
@@ -6,6 +6,7 @@ edition = "2021"
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html

 [dependencies]
+bytes = "1.5.0"
 libc = "0.2.150"
-nix = { version = "0.27.1", features = ["ioctl"]}
+thiserror = "1.0.50"

@@ -1,45 +1,191 @@
 use std::fs::File;
-use std::mem;
-use std::os::unix::io::AsRawFd;
+use std::io::{Cursor, Read, Write};
+use std::net::{TcpListener, TcpStream};
+use std::sync::{Arc, Mutex};
+use bytes::{Buf, BufMut};
+use std::os::fd::AsRawFd;
+use std::thread;
+use thiserror::Error;

-fn main() -> std::io::Result<()> {
-    const DIAG_IOCTL_SWITCH_LOGGING: u32 = 7;
-    const MEMORY_DEVICE_MODE: i32 = 2;
-    const DIAG_IOCTL_REMOTE_DEV: u32 = 32;
+type DiagResult<T> = Result<T, DiagDeviceError>;

-    let mut mode_param: [i32; 3] = [MEMORY_DEVICE_MODE, -1, 0]; // diag_logging_mode_param_t
-    let use_mdm: i32 = 0;
+const BUFFER_LEN: usize = 1024 * 1024 * 10;
+const USER_SPACE_DATA_TYPE: i32 = 32;
+const DIAG_IOCTL_REMOTE_DEV: u32 = 32;
+const MEMORY_DEVICE_MODE: i32 = 2;
+const DIAG_IOCTL_SWITCH_LOGGING: u32 = 7;

-    println!("Initializing DIAG");
+#[derive(Error, Debug)]
+enum DiagDeviceError {
+    #[error("IO error {0}")]
+    IO(#[from] std::io::Error),
+    #[error("Failed to initialize /dev/diag: {0}")]
+    InitializationFailed(String),
+    #[error("Failed to read diag device: {0}")]
+    DeviceReadFailed(String),
+}

-    let diag_file = File::options()
-         .read(true)
-         .write(true)
-         .open("/dev/diag")?;
-    let diag_fd = diag_file.as_raw_fd();
+struct DiagDevice {
+    pub file: File,
+    use_mdm: i32,
+}

+fn enable_frame_readwrite(fd: i32, mode: i32) -> DiagResult<()> {
    unsafe {
-        if libc::ioctl(diag_fd, DIAG_IOCTL_SWITCH_LOGGING, MEMORY_DEVICE_MODE, 0, 0, 0) < 0
-            && libc::ioctl(
-                diag_fd,
+        if libc::ioctl(fd, DIAG_IOCTL_SWITCH_LOGGING, mode, 0, 0, 0) < 0 {
+            let ret = libc::ioctl(
+                fd,
                DIAG_IOCTL_SWITCH_LOGGING,
-                &mut mode_param as *mut _,
-                mem::size_of::<[i32; 3]>(), 0, 0, 0, 0
-            ) < 0
-        {
-            println!("ioctl failed 1");
-            //std::process::exit(1);
+                &mut [mode, -1, 0] as *mut _, // diag_logging_mode_param_t
+                std::mem::size_of::<[i32; 3]>(), 0, 0, 0, 0
+            );
+            if ret < 0 {
+                let msg = format!("DIAG_IOCTL_SWITCH_LOGGING ioctl failed with error code {}", ret);
+                return Err(DiagDeviceError::InitializationFailed(msg))
+            }
        }
    }
-
-    unsafe {
-        if libc::ioctl(diag_fd, DIAG_IOCTL_REMOTE_DEV, &use_mdm as *const i32) < 0 {
-            println!("ioctl failed 2");
-            std::process::exit(1);
-        }
-    }
-
-    println!("successfully opened diag device");
-
    Ok(())
 }
+
+fn determine_use_mdm(fd: i32) -> DiagResult<i32> {
+    let use_mdm: i32 = 0;
+    unsafe {
+        if libc::ioctl(fd, DIAG_IOCTL_REMOTE_DEV, &use_mdm as *const i32) < 0 {
+            let msg = format!("DIAG_IOCTL_REMOTE_DEV ioctl failed with error code {}", 0);
+            return Err(DiagDeviceError::InitializationFailed(msg))
+        }
+    }
+    Ok(use_mdm)
+}
+
+impl DiagDevice {
+    pub fn new() -> DiagResult<Self> {
+        let file = File::options()
+            .read(true)
+            .write(true)
+            .open("/dev/diag")?;
+        let fd = file.as_raw_fd();
+
+        enable_frame_readwrite(fd, MEMORY_DEVICE_MODE)?;
+        let use_mdm = determine_use_mdm(fd)?;
+
+        Ok(DiagDevice {
+            file,
+            use_mdm,
+        })
+    }
+
+    pub fn try_clone(&self) -> DiagResult<Self> {
+        Ok(DiagDevice {
+            file: self.file.try_clone()?,
+            use_mdm: self.use_mdm,
+        })
+    }
+
+    pub fn read_response(&mut self) -> DiagResult<Option<Vec<Vec<u8>>>> {
+        let mut buf = vec![0; BUFFER_LEN];
+        let bytes_read = self.file.read(&mut buf)?;
+        if bytes_read < 4 {
+            let msg = format!("read {} bytes from diag device, expected > 4", bytes_read);
+            return Err(DiagDeviceError::DeviceReadFailed(msg));
+        }
+        let mut reader = Cursor::new(buf);
+
+        if reader.get_i32_le() != USER_SPACE_DATA_TYPE {
+            return Ok(None);
+        }
+
+        let num_messages = reader.get_u32_le();
+        let mut messages = Vec::new();
+
+        for _ in 0..num_messages {
+            let msg_len = reader.get_u32_le() as usize;
+            let mut msg = vec![0; msg_len];
+            reader.read_exact(&mut msg)?;
+            messages.push(msg);
+        }
+
+        Ok(Some(messages))
+    }
+
+    pub fn write_request(&mut self, req: &[u8]) -> DiagResult<()> {
+        let mut buf: Vec<u8> = vec![];
+        buf.put_i32_le(USER_SPACE_DATA_TYPE);
+        if self.use_mdm > 0 {
+            buf.put_i32_le(-1);
+        }
+        buf.extend_from_slice(req);
+        unsafe {
+            let fd = self.file.as_raw_fd();
+            let buf_ptr = buf.as_ptr() as *const libc::c_void;
+            let ret = libc::write(fd, buf_ptr, buf.len());
+            if ret < 0 {
+                let msg = format!("write failed with error code {}", ret);
+                return Err(DiagDeviceError::DeviceReadFailed(msg));
+            }
+        }
+        Ok(())
+    }
+}
+
+fn main() -> std::io::Result<()> {
+    println!("Starting server");
+    let listener = TcpListener::bind("0.0.0.0:43555")?;
+
+    let client_mutex: Arc<Mutex<Option<TcpStream>>> = Arc::new(Mutex::new(None));
+
+    let mut dev_reader = DiagDevice::new().unwrap();
+    let mut dev_writer = dev_reader.try_clone().unwrap();
+
+    let client_mutex_clone = client_mutex.clone();
+    // Spawn a thread to continuously read from the diag device, sending
+    // messages to the client (if any)
+    thread::spawn(move || {
+        loop {
+            match dev_reader.read_response() {
+                Ok(Some(msgs)) => {
+                    if let Some(client_writer) = client_mutex_clone.lock().unwrap().as_mut() {
+                        println!("> Writing {} diag messages to client", msgs.len());
+                        for msg in msgs {
+                            client_writer.write_all(&msg).unwrap();
+                        }
+                    }
+                },
+                Ok(None) => {},
+                Err(err) => {
+                    println!("Unable to read from /dev/diag: {}", err);
+                    return;
+                },
+            }
+        }
+    });
+
+    // Accept connections from clients, writing any data received to the diag device
+    loop {
+        println!("Waiting for client");
+        let (mut client_reader, _) = listener.accept()?;
+
+        println!("Client connected");
+        let client_writer = client_reader.try_clone()?;
+        {
+            let mut client_writer_mutex = client_mutex.lock().unwrap();
+            *client_writer_mutex = Some(client_writer);
+        }
+
+        let mut buf = vec![0; BUFFER_LEN];
+        loop {
+            let bytes_read = client_reader.read(&mut buf).unwrap();
+            if bytes_read == 0 {
+                println!("Client disconnected");
+                {
+                    let mut client_writer_mutex = client_mutex.lock().unwrap();
+                    *client_writer_mutex = None;
+                }
+                break;
+            }
+            println!("< Got {} bytes from client", bytes_read);
+            dev_writer.write_request(&buf[0..bytes_read]).unwrap();
+        }
+    }
+}