diff --git a/Cargo.lock b/Cargo.lock index ad6b329..9e2e35c 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -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" diff --git a/Cargo.toml b/Cargo.toml index 5fd8df4..f5346b0 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -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" diff --git a/src/main.rs b/src/main.rs index 525bfd8..9772a33 100644 --- a/src/main.rs +++ b/src/main.rs @@ -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 = Result; - 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 { + 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 { + 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 { + Ok(DiagDevice { + file: self.file.try_clone()?, + use_mdm: self.use_mdm, + }) + } + + pub fn read_response(&mut self) -> DiagResult>>> { + 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 = 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>> = 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(); + } + } +}