From 7d55716104596d24862bc7f23db44ed176624985 Mon Sep 17 00:00:00 2001 From: Will Greenberg Date: Sat, 2 Dec 2023 23:43:02 -0800 Subject: [PATCH] logging enabled without qcsuper --- .cargo/config.toml | 3 + Cargo.lock | 253 ++++++++++++++++++++++++++++++++++- Cargo.toml | 3 + src/diag.rs | 322 +++++++++++++++++++++++++++++++++++++++++++++ src/hdlc.rs | 66 ++++++++++ src/main.rs | 202 ++-------------------------- 6 files changed, 656 insertions(+), 193 deletions(-) create mode 100644 src/diag.rs create mode 100644 src/hdlc.rs diff --git a/.cargo/config.toml b/.cargo/config.toml index 63aa865..2280655 100644 --- a/.cargo/config.toml +++ b/.cargo/config.toml @@ -2,5 +2,8 @@ target = "armv7-unknown-linux-gnueabihf" rustflags = ["-C", "target-feature=+crt-static"] +[alias] +test_pc = "test --target=x86_64-unknown-linux-gnu" + [target.armv7-unknown-linux-gnueabihf] linker = "arm-linux-gnueabihf-gcc" diff --git a/Cargo.lock b/Cargo.lock index 9e2e35c..5d969ab 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -2,27 +2,214 @@ # It is not intended for manual editing. version = 3 +[[package]] +name = "bitvec" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1bc2832c24239b0141d5674bb9174f9d68a8b5b3f2753311927c172ca46f7e9c" +dependencies = [ + "funty", + "radium", + "tap", + "wyz", +] + +[[package]] +name = "byteorder" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" + +[[package]] +name = "byteorder_slice" +version = "3.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0b294e30387378958e8bf8f4242131b930ea615ff81e8cac2440cea0a6013190" +dependencies = [ + "byteorder", +] + [[package]] name = "bytes" version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a2bd12c1caf447e69cd4528f47f94d203fd2582878ecb9e9465484c4148a8223" +[[package]] +name = "crc" +version = "3.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "86ec7a15cbe22e59248fc7eadb1907dab5ba09372595da4d73dd805ed4417dfe" +dependencies = [ + "crc-catalog", +] + +[[package]] +name = "crc-catalog" +version = "2.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "19d374276b40fb8bbdee95aef7c7fa6b5316ec764510eb64b8dd0e2ed0d7e7f5" + +[[package]] +name = "darling" +version = "0.14.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7b750cb3417fd1b327431a470f388520309479ab0bf5e323505daf0290cd3850" +dependencies = [ + "darling_core", + "darling_macro", +] + +[[package]] +name = "darling_core" +version = "0.14.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "109c1ca6e6b7f82cc233a97004ea8ed7ca123a9af07a8230878fcfda9b158bf0" +dependencies = [ + "fnv", + "ident_case", + "proc-macro2", + "quote", + "strsim", + "syn 1.0.109", +] + +[[package]] +name = "darling_macro" +version = "0.14.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a4aab4dbc9f7611d8b55048a3a16d2d010c2c8334e46304b40ac1cc14bf3b48e" +dependencies = [ + "darling_core", + "quote", + "syn 1.0.109", +] + +[[package]] +name = "deku" +version = "0.16.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "819b87cc7a05b3abe3fc38e59b3980a5fd3162f25a247116441a9171d3e84481" +dependencies = [ + "bitvec", + "deku_derive", +] + +[[package]] +name = "deku_derive" +version = "0.16.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4e2ca12572239215a352a74ad7c776d7e8a914f8a23511c6cbedddd887e5009e" +dependencies = [ + "darling", + "proc-macro-crate", + "proc-macro2", + "quote", + "syn 1.0.109", +] + +[[package]] +name = "derive-into-owned" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2c9d94d81e3819a7b06a8638f448bc6339371ca9b6076a99d4a43eece3c4c923" +dependencies = [ + "proc-macro2", + "quote", + "syn 1.0.109", +] + [[package]] name = "diag" version = "0.1.0" dependencies = [ "bytes", + "crc", + "deku", "libc", + "pcap-file", "thiserror", ] +[[package]] +name = "equivalent" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5443807d6dff69373d433ab9ef5378ad8df50ca6298caf15de6e52e24aaf54d5" + +[[package]] +name = "fnv" +version = "1.0.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" + +[[package]] +name = "funty" +version = "2.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e6d5a32815ae3f33302d95fdcb2ce17862f8c65363dcfd29360480ba1001fc9c" + +[[package]] +name = "hashbrown" +version = "0.14.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "290f1a1d9242c78d09ce40a5e87e7554ee637af1351968159f4952f028f75604" + +[[package]] +name = "ident_case" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39" + +[[package]] +name = "indexmap" +version = "2.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d530e1a18b1cb4c484e6e34556a0d948706958449fca0cab753d649f2bce3d1f" +dependencies = [ + "equivalent", + "hashbrown", +] + [[package]] name = "libc" version = "0.2.150" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "89d92a4743f9a61002fae18374ed11e7973f530cb3a3255fb354818118b2203c" +[[package]] +name = "memchr" +version = "2.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f665ee40bc4a3c5590afb1e9677db74a508659dfd71e126420da8274909a0167" + +[[package]] +name = "once_cell" +version = "1.18.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dd8b5dd2ae5ed71462c540258bedcb51965123ad7e7ccf4b9a8cafaa4a63576d" + +[[package]] +name = "pcap-file" +version = "2.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1fc1f139757b058f9f37b76c48501799d12c9aa0aa4c0d4c980b062ee925d1b2" +dependencies = [ + "byteorder_slice", + "derive-into-owned", + "thiserror", +] + +[[package]] +name = "proc-macro-crate" +version = "1.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7f4c021e1093a56626774e81216a4ce732a735e5bad4868a03f3ed65ca0c3919" +dependencies = [ + "once_cell", + "toml_edit", +] + [[package]] name = "proc-macro2" version = "1.0.69" @@ -41,6 +228,29 @@ dependencies = [ "proc-macro2", ] +[[package]] +name = "radium" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dc33ff2d4973d518d823d61aa239014831e521c75da58e3df4840d3f47749d09" + +[[package]] +name = "strsim" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623" + +[[package]] +name = "syn" +version = "1.0.109" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + [[package]] name = "syn" version = "2.0.39" @@ -52,6 +262,12 @@ dependencies = [ "unicode-ident", ] +[[package]] +name = "tap" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "55937e1799185b12863d447f42597ed69d9928686b8d88a1df17376a097d8369" + [[package]] name = "thiserror" version = "1.0.50" @@ -69,7 +285,24 @@ checksum = "266b2e40bc00e5a6c09c3584011e08b06f123c00362c92b975ba9843aaaa14b8" dependencies = [ "proc-macro2", "quote", - "syn", + "syn 2.0.39", +] + +[[package]] +name = "toml_datetime" +version = "0.6.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3550f4e9685620ac18a50ed434eb3aec30db8ba93b0287467bca5826ea25baf1" + +[[package]] +name = "toml_edit" +version = "0.19.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1b5bb770da30e5cbfde35a2d7b9b8a2c4b8ef89548a7a6aeab5c9a576e3e7421" +dependencies = [ + "indexmap", + "toml_datetime", + "winnow", ] [[package]] @@ -77,3 +310,21 @@ name = "unicode-ident" version = "1.0.12" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b" + +[[package]] +name = "winnow" +version = "0.5.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "829846f3e3db426d4cee4510841b71a8e58aa2a76b1132579487ae430ccd9c7b" +dependencies = [ + "memchr", +] + +[[package]] +name = "wyz" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "05f360fc0b24296329c78fda852a1e9ae82de9cf7b27dae4b7f62f118f77b9ed" +dependencies = [ + "tap", +] diff --git a/Cargo.toml b/Cargo.toml index f5346b0..8c4ef28 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -7,6 +7,9 @@ edition = "2021" [dependencies] bytes = "1.5.0" +crc = "3.0.1" +deku = "0.16.0" libc = "0.2.150" +pcap-file = "2.0.0" thiserror = "1.0.50" diff --git a/src/diag.rs b/src/diag.rs new file mode 100644 index 0000000..7471388 --- /dev/null +++ b/src/diag.rs @@ -0,0 +1,322 @@ +use crate::hdlc::{hdlc_encapsulate, hdlc_decapsulate}; + +use std::fs::File; +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; +use crc::{Crc, Algorithm}; +use deku::prelude::*; + +const BUFFER_LEN: usize = 1024 * 1024 * 10; +const USER_SPACE_DATA_TYPE: i32 = 32; +const MEMORY_DEVICE_MODE: i32 = 2; +const DIAG_IOCTL_REMOTE_DEV: u32 = 32; +const DIAG_IOCTL_SWITCH_LOGGING: u32 = 7; + +// this is sorta based on the params qcsuper uses, plus what seems to be used in +// https://github.com/fgsect/scat/blob/f1538b397721df3ab8ba12acd26716abcf21f78b/util.py#L47 +pub const CRC_CCITT_ALG: Algorithm = Algorithm { + poly: 0x1021, + init: 0xffff, + refin: true, + refout: true, + width: 16, + xorout: 0xffff, + check: 0x2189, + residue: 0x0000, +}; + +pub type DiagResult = Result; + +#[derive(Error, Debug)] +pub 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), +} + +#[derive(Debug, Clone, PartialEq, DekuWrite)] +#[deku(type = "u32")] +pub enum Request { + #[deku(id = "115")] + LogConfig(LogConfigRequest), +} + +#[derive(Debug, Clone, PartialEq, DekuWrite)] +#[deku(type = "u32", endian = "little")] +pub enum LogConfigRequest { + #[deku(id = "1")] + RetrieveIdRanges, + + #[deku(id = "3")] + SetMask { + log_type: u32, + log_mask_bitsize: u32, + log_mask: Vec, + } +} + +// kinda unpleasant deku hackery here. deku expects an enum's variant to be +// right before its data, but in this case, a status value comes between the +// variants and the data. so we need to use deku's context (ctx) feature to pass +// those opcodes down to their respective parsers. +#[derive(Debug, Clone, DekuRead)] +pub struct Response { + opcode: u32, + subopcode: u32, + status: u32, + #[deku(ctx = "*opcode, *subopcode")] + payload: ResponsePayload, +} + +#[derive(Debug, Clone, DekuRead)] +#[deku(ctx = "opcode: u32, subopcode: u32", id = "opcode")] +pub enum ResponsePayload { + #[deku(id = "115")] + LogConfig(#[deku(ctx = "subopcode")] LogConfigResponse), +} + +#[derive(Debug, Clone, DekuRead)] +#[deku(ctx = "subopcode: u32", id = "subopcode")] +pub enum LogConfigResponse { + #[deku(id = "1")] + RetrieveIdRanges { + log_mask_sizes: [u32; 16], + }, + + #[deku(id = "3")] + SetMask, +} + +// Triggers the diag device's debug logging mode +fn enable_frame_readwrite(fd: i32, mode: i32) -> DiagResult<()> { + unsafe { + if libc::ioctl(fd, DIAG_IOCTL_SWITCH_LOGGING.into(), mode, 0, 0, 0) < 0 { + let ret = libc::ioctl( + fd, + DIAG_IOCTL_SWITCH_LOGGING.into(), + &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)) + } + } + } + Ok(()) +} + +// Unsure of what MDM actually stands for, but if `use_mdm` is > 0, then +// an additional mask is included in every diag request +fn determine_use_mdm(fd: i32) -> DiagResult { + let use_mdm: i32 = 0; + unsafe { + if libc::ioctl(fd, DIAG_IOCTL_REMOTE_DEV.into(), &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) +} + +pub struct DiagDevice { + file: File, + use_mdm: i32, + crc: Crc, +} + +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, + crc: Crc::::new(&CRC_CCITT_ALG), + use_mdm, + }) + } + + pub fn try_clone(&self) -> DiagResult { + Ok(DiagDevice { + file: self.file.try_clone()?, + crc: Crc::::new(&CRC_CCITT_ALG), + 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)?; + match Response::from_bytes((&hdlc_decapsulate(msg, &self.crc), 0)) { + // todo: handle leftover bytes + Ok(((_, leftover_bytes), res)) => { + if leftover_bytes > 0 { + println!("warning: {} leftover bytes when parsing response", leftover_bytes); + } + messages.push(res); + }, + Err(e) => { + println!("error parsing response: {:?}", e); + continue; + } + } + } + + Ok(Some(messages)) + } + + pub fn write_request(&mut self, req: Request) -> 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(hdlc_encapsulate(req.to_bytes().unwrap(), &self.crc)); + 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(()) + } + + pub fn config_logs(&mut self) -> DiagResult<()> { + // todo: replace panics w/ errors + + println!("retrieving diag logging capabilities..."); + self.write_request(Request::LogConfig(LogConfigRequest::RetrieveIdRanges))?; + + let res = self.read_response()? + .expect("got unexpected non-userspace message from device") + .pop().expect("no LogConfigRequest::RetrieveIdRanges response received"); + if res.status != 0 { + let msg = format!("LogConfigRequest::RetrieveIdRanges failed with status {}", res.status); + return Err(DiagDeviceError::DeviceReadFailed(msg)); + } + if let ResponsePayload::LogConfig(LogConfigResponse::RetrieveIdRanges { log_mask_sizes }) = res.payload { + // for each log type, send a logging mask of all 1's equal to its respective mask size + for (log_type, &log_mask_bitsize) in log_mask_sizes.iter().enumerate() { + if log_mask_bitsize == 0 { + continue; + } + self.write_request(build_log_mask_request(log_type as u32, log_mask_bitsize))?; + let set_mask_res = self.read_response()? + .expect("unexpected non-userspace message from device") + .pop().expect("expected response, got none"); + if set_mask_res.status != 0 { + eprintln!("LogConfigRequest::SetMask failed with status {}", set_mask_res.status); + } + if let ResponsePayload::LogConfig(LogConfigResponse::SetMask) = set_mask_res.payload { + println!("registered logging for type {}", log_type); + } else { + panic!("unexpected response payload: {:?}", set_mask_res.payload); + } + } + } else { + panic!("unexpected response payload: {:?}", res.payload); + } + + Ok(()) + } +} + +// register logging for each supported log type. it seems that "log_mask_sizes" is an array of +// numbers for each log type, where each number is how many bits are in that log mask +fn build_log_mask_request(log_type: u32, log_mask_bitsize: u32) -> Request { + // if log_mask_bitsize = 8n + k, then we need n+1 bytes to store the mask, with the last + // byte having k bits set + let mask_len = (log_mask_bitsize as usize + 7) / 8; + let mut log_mask = vec![0xff; mask_len]; + if log_mask_bitsize % 8 != 0 { + log_mask[mask_len - 1] = 0xff >> (8 - (log_mask_bitsize as usize % 8)); + } + + Request::LogConfig(LogConfigRequest::SetMask { + log_type: log_type as u32, + log_mask_bitsize, + log_mask, + }) +} + +#[cfg(test)] +mod test { + use super::*; + + #[test] + fn test_request_serialization() { + let req = Request::LogConfig(LogConfigRequest::RetrieveIdRanges); + assert_eq!(req.to_bytes().unwrap(), vec![115, 0, 0, 0, 1, 0, 0, 0]); + + let req = Request::LogConfig(LogConfigRequest::SetMask { + log_type: 0, + log_mask_bitsize: 0, + log_mask: vec![], + }); + assert_eq!(req.to_bytes().unwrap(), vec![ + 115, 0, 0, 0, + 3, 0, 0, 0, + 0, 0, 0, 0, + 0, 0, 0, 0, + ]); + } + + #[test] + fn test_build_log_mask_request() { + assert_eq!(build_log_mask_request(0, 1), Request::LogConfig(LogConfigRequest::SetMask { + log_type: 0, + log_mask_bitsize: 1, + log_mask: vec![0x01], + })); + assert_eq!(build_log_mask_request(0, 2), Request::LogConfig(LogConfigRequest::SetMask { + log_type: 0, + log_mask_bitsize: 2, + log_mask: vec![0x03], + })); + assert_eq!(build_log_mask_request(0, 8), Request::LogConfig(LogConfigRequest::SetMask { + log_type: 0, + log_mask_bitsize: 8, + log_mask: vec![0xff], + })); + assert_eq!(build_log_mask_request(0, 9), Request::LogConfig(LogConfigRequest::SetMask { + log_type: 0, + log_mask_bitsize: 9, + log_mask: vec![0xff, 0x01], + })); + } +} diff --git a/src/hdlc.rs b/src/hdlc.rs new file mode 100644 index 0000000..49f0092 --- /dev/null +++ b/src/hdlc.rs @@ -0,0 +1,66 @@ +use crc::Crc; +use bytes::{Buf, BufMut}; + +pub fn hdlc_encapsulate(mut data: Vec, crc: &Crc) -> Vec { + data.put_u16_le(crc.checksum(&data)); + + let mut result: Vec = data.iter() + .flat_map(|&b| match b { + // TODO: is this too expensive? + 0x7e => vec![0x7d, 0x5e], + 0x7d => vec![0x7d, 0x5d], + _ => vec![b], + }) + .collect(); + result.push(0x7e); + result +} + +pub fn hdlc_decapsulate(mut data: Vec, crc: &Crc) -> Vec { + // TODO: return errors instead of panicking + if data.len() < 3 { + panic!("data too short to be HDLC encapsulated"); + } + + assert_eq!(data.pop(), Some(0x7e)); // ensure data ends w/ trailing character + let mut unescaped = Vec::new(); + let mut escaping = false; + for i in 0..data.len() { + let b = data[i]; + if escaping { + match b { + 0x5e => unescaped.push(0x7e), + 0x5d => unescaped.push(0x7d), + _ => panic!("invalid HDLC escape sequence"), + } + escaping = false; + } else if b == 0x7d { + escaping = true + } else { + unescaped.push(b); + } + } + + // pop off the u16 checksum, check it against what we calculated + let checksum_hi = unescaped.pop().unwrap(); + let checksum_lo = unescaped.pop().unwrap(); + let checksum = [checksum_lo, checksum_hi].as_slice().get_u16_le(); + assert_eq!(checksum, crc.checksum(&unescaped)); // ensure checksums match + + unescaped +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_hdlc_encapsulate() { + let crc = Crc::::new(&crate::diag::CRC_CCITT_ALG); + let data = vec![0x01, 0x02, 0x03, 0x04]; + let expected = vec![1, 2, 3, 4, 145, 57, 126]; + let encapsulated = hdlc_encapsulate(data.clone(), &crc); + assert_eq!(&encapsulated, &expected); + assert_eq!(hdlc_decapsulate(encapsulated, &crc), data); + } +} diff --git a/src/main.rs b/src/main.rs index c5d4417..a8197f3 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,201 +1,19 @@ -use std::fs::File; -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; +mod hdlc; +mod diag; -type DiagResult = Result; - -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; - -#[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), -} - -struct DiagDevice { - file: File, - use_mdm: i32, -} - -// Triggers the diag device's debug logging mode -fn enable_frame_readwrite(fd: i32, mode: i32) -> DiagResult<()> { - unsafe { - if libc::ioctl(fd, DIAG_IOCTL_SWITCH_LOGGING, mode, 0, 0, 0) < 0 { - let ret = libc::ioctl( - fd, - DIAG_IOCTL_SWITCH_LOGGING, - &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)) - } - } - } - Ok(()) -} - -// Unsure of what MDM actually stands for, but if `use_mdm` is > 0, then -// an additional mask is included in every diag request -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(()) - } -} +use crate::hdlc::{hdlc_encapsulate, hdlc_decapsulate}; +use crate::diag::{DiagDevice}; fn main() -> std::io::Result<()> { - println!("Starting server"); - let listener = TcpListener::bind("0.0.0.0:43555")?; + let mut dev = DiagDevice::new().unwrap(); + dev.config_logs().unwrap(); - // Since we only care about one client at a time, store a copy of that - // client's TcpStream in a mutex. This lets us write to the client from a - // separate thread - let client_mutex: Arc>> = Arc::new(Mutex::new(None)); - - // initialize the diag device and create a cloned handle to its file. this - // lets us perform reads and writes in separate threads. i *think* this is - // sound - let mut dev_reader = DiagDevice::new().unwrap(); - let mut dev_writer = dev_reader.try_clone().unwrap(); - - // Spawn a thread to continuously read from the diag device, sending any - // messages to the client - let client_mutex_clone = client_mutex.clone(); - 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 a client (only one is accepted at a time), - // 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; + let msgs = dev.read_response().unwrap(); + if let Some(msgs) = msgs { + for msg in msgs { + println!("msg: {:?}", msg); } - println!("< Got {} bytes from client", bytes_read); - dev_writer.write_request(&buf[0..bytes_read]).unwrap(); } } }