From 0f534a0d1167368f3840de2af6285d06cf8ad1fd Mon Sep 17 00:00:00 2001 From: Will Greenberg Date: Sat, 2 Dec 2023 10:56:57 -0800 Subject: [PATCH 01/17] slightly clearer server comment --- src/main.rs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main.rs b/src/main.rs index 0cca3da..c5d4417 100644 --- a/src/main.rs +++ b/src/main.rs @@ -170,7 +170,8 @@ fn main() -> std::io::Result<()> { } }); - // Accept connections from clients, writing any data received to the diag device + // 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()?; From 7d55716104596d24862bc7f23db44ed176624985 Mon Sep 17 00:00:00 2001 From: Will Greenberg Date: Sat, 2 Dec 2023 23:43:02 -0800 Subject: [PATCH 02/17] 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(); } } } From db18abae6fdc0c9e7ed3aca93e04d67da50549ea Mon Sep 17 00:00:00 2001 From: Will Greenberg Date: Mon, 4 Dec 2023 15:38:01 -0800 Subject: [PATCH 03/17] Mostly working now, except for log parsing and random checksum failures --- src/diag.rs | 264 +++++++++++---------------------------------- src/diag_device.rs | 215 ++++++++++++++++++++++++++++++++++++ src/hdlc.rs | 46 ++++++-- src/main.rs | 11 +- 4 files changed, 316 insertions(+), 220 deletions(-) create mode 100644 src/diag_device.rs diff --git a/src/diag.rs b/src/diag.rs index 7471388..b97904f 100644 --- a/src/diag.rs +++ b/src/diag.rs @@ -1,45 +1,15 @@ -use crate::hdlc::{hdlc_encapsulate, hdlc_decapsulate}; +//! Diag protocol serialization/deserialization -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, DekuWrite)] +pub struct RequestContainer { + pub data_type: DataType, + #[deku(skip)] + pub use_mdm: bool, + #[deku(skip, cond = "!*use_mdm")] + pub mdm_field: i32, + pub hdlc_encapsulated_request: Vec, } #[derive(Debug, Clone, PartialEq, DekuWrite)] @@ -63,6 +33,30 @@ pub enum LogConfigRequest { } } +#[derive(Debug, Clone, PartialEq, DekuRead, DekuWrite)] +#[deku(type = "u32", endian = "little")] +pub enum DataType { + #[deku(id = "32")] + UserSpace, + #[deku(id_pat = "_")] + Other(u32), +} + +#[derive(Debug, Clone, DekuRead)] +pub struct ResponseContainer { + pub data_type: DataType, + pub num_responses: u32, + #[deku(count = "num_responses")] + pub responses: Vec, +} + +#[derive(Debug, Clone, DekuRead)] +pub struct HdlcEncapsulatedResponse { + pub len: u32, + #[deku(count = "len")] + pub data: 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 @@ -71,9 +65,9 @@ pub enum LogConfigRequest { pub struct Response { opcode: u32, subopcode: u32, - status: u32, + pub status: u32, #[deku(ctx = "*opcode, *subopcode")] - payload: ResponsePayload, + pub payload: ResponsePayload, } #[derive(Debug, Clone, DekuRead)] @@ -95,170 +89,9 @@ pub enum LogConfigResponse { 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 { +pub 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; @@ -319,4 +152,29 @@ mod test { log_mask: vec![0xff, 0x01], })); } + + #[test] + fn test_request_container() { + let req = RequestContainer { + data_type: DataType::UserSpace, + use_mdm: false, + mdm_field: -1, + hdlc_encapsulated_request: vec![1, 2, 3, 4], + }; + assert_eq!(req.to_bytes().unwrap(), vec![ + 32, 0, 0, 0, + 1, 2, 3, 4, + ]); + let req = RequestContainer { + data_type: DataType::UserSpace, + use_mdm: true, + mdm_field: -1, + hdlc_encapsulated_request: vec![1, 2, 3, 4], + }; + assert_eq!(req.to_bytes().unwrap(), vec![ + 32, 0, 0, 0, + 255, 255, 255, 255, + 1, 2, 3, 4, + ]); + } } diff --git a/src/diag_device.rs b/src/diag_device.rs new file mode 100644 index 0000000..de3dc35 --- /dev/null +++ b/src/diag_device.rs @@ -0,0 +1,215 @@ +use crate::hdlc::{hdlc_encapsulate, hdlc_decapsulate, HdlcError}; +use crate::diag::{Response, ResponsePayload, Request, LogConfigRequest, LogConfigResponse, build_log_mask_request, RequestContainer, DataType, ResponseContainer}; + +use std::fs::File; +use std::io::{Cursor, Read, Write}; +use bytes::{Buf, BufMut}; +use std::os::fd::AsRawFd; +use thiserror::Error; +use crc::{Crc, Algorithm}; +use deku::prelude::*; + +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), + #[error("Nonzero status code {0} for diag request: {1:?}")] + RequestFailed(u32, Request), + #[error("Didn't receive response for request: {0:?}")] + NoResponse(Request), + #[error("HDLC error {0}")] + HdlcError(#[from] HdlcError), + #[error("Deku error {0}")] + DekuError(#[from] DekuError), +} + +// 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, +}; + +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; + +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, + }) + } + + fn parse_response_container(&self, container: ResponseContainer) -> DiagResult> { + let mut result = Vec::new(); + for msg in container.responses { + let data = hdlc_decapsulate(msg.data, &self.crc)?; + match Response::from_bytes((&data, 0)) { + Ok(((_, leftover_bytes), res)) => { + if leftover_bytes > 0 { + println!("warning: {} leftover bytes when Response", leftover_bytes); + } + result.push(res); + }, + Err(e) => { + println!("{:?}", data); + println!("error parsing response: {:?}", e); + }, + } + } + Ok(result) + } + + pub fn read_response(&mut self) -> DiagResult> { + let mut buf = vec![0; BUFFER_LEN]; + + loop { + let _ = self.file.read(&mut buf)?; + let ((_, leftover_bytes), res_container) = ResponseContainer::from_bytes((&buf, 0))?; + if leftover_bytes > 0 { + println!("warning: {} leftover bytes when parsing ResponseContainer", leftover_bytes); + } + if res_container.data_type == DataType::UserSpace { + return self.parse_response_container(res_container); + } else { + println!("skipping non-userspace message...") + } + } + } + + pub fn write_request(&mut self, req: &Request) -> DiagResult<()> { + let buf = RequestContainer { + data_type: DataType::UserSpace, + use_mdm: self.use_mdm > 0, + mdm_field: -1, + hdlc_encapsulated_request: hdlc_encapsulate(req.to_bytes().unwrap(), &self.crc), + }.to_bytes().unwrap(); + 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)); + } + println!("{}. wrote {} bytes to device", buf.len(), ret); + } + Ok(()) + } + + fn retrieve_id_ranges(&mut self) -> DiagResult<[u32; 16]> { + let req = Request::LogConfig(LogConfigRequest::RetrieveIdRanges); + self.write_request(&req)?; + + for res in self.read_response()? { + match res.payload { + ResponsePayload::LogConfig(LogConfigResponse::RetrieveIdRanges { log_mask_sizes }) => { + if res.status != 0 { + return Err(DiagDeviceError::RequestFailed(res.status, req)); + } + return Ok(log_mask_sizes); + }, + _ => println!("skipping non-LogConfigResponse response..."), + } + } + + return Err(DiagDeviceError::NoResponse(req)); + } + + fn set_log_mask(&mut self, log_type: u32, log_mask_bitsize: u32) -> DiagResult<()> { + // send a logging mask of all 1's equal to its respective mask size + let req = build_log_mask_request(log_type, log_mask_bitsize); + self.write_request(&req)?; + + for res in self.read_response()? { + if let ResponsePayload::LogConfig(LogConfigResponse::SetMask) = res.payload { + if res.status != 0 { + return Err(DiagDeviceError::RequestFailed(res.status, req)); + } + return Ok(()); + } + } + + return Err(DiagDeviceError::NoResponse(req)); + } + + pub fn config_logs(&mut self) -> DiagResult<()> { + println!("retrieving diag logging capabilities..."); + let log_mask_sizes = self.retrieve_id_ranges()?; + println!("log mask sizes: {:?}", log_mask_sizes); + + for (log_type, &log_mask_bitsize) in log_mask_sizes.iter().enumerate() { + if log_mask_bitsize > 0 { + println!("setting logging for log_type {}", log_type); + self.set_log_mask(log_type as u32, log_mask_bitsize)?; + println!("enabled logging for log type {}", log_type); + } + } + + Ok(()) + } +} + +// 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) +} diff --git a/src/hdlc.rs b/src/hdlc.rs index 49f0092..dcabf00 100644 --- a/src/hdlc.rs +++ b/src/hdlc.rs @@ -1,5 +1,25 @@ +//! HDLC stands for "High-level Data Link Control", which the diag protocol uses +//! to encapsulate its messages. QCSuper's docs describe this in more detail +//! here: +//! https://github.com/P1sec/QCSuper/blob/master/docs/The%20Diag%20protocol.md#the-diag-protocol-over-usb + use crc::Crc; use bytes::{Buf, BufMut}; +use thiserror::Error; + +#[derive(Debug, Error, PartialEq)] +pub enum HdlcError { + #[error("Invalid checksum (expected {0}, got {1})")] + InvalidChecksum(u16, u16), + #[error("Invalid HDLC escape sequence: [0x7d, {0}]")] + InvalidEscapeSequence(u8), + #[error("No trailing character found (expected 0x7e, got {0}))")] + NoTrailingCharacter(u8), + #[error("Missing checksum")] + MissingChecksum, + #[error("Data too short to be HDLC encapsulated")] + TooShort, +} pub fn hdlc_encapsulate(mut data: Vec, crc: &Crc) -> Vec { data.put_u16_le(crc.checksum(&data)); @@ -16,13 +36,17 @@ pub fn hdlc_encapsulate(mut data: Vec, crc: &Crc) -> Vec { result } -pub fn hdlc_decapsulate(mut data: Vec, crc: &Crc) -> Vec { +pub fn hdlc_decapsulate(mut data: Vec, crc: &Crc) -> Result, HdlcError> { // TODO: return errors instead of panicking if data.len() < 3 { - panic!("data too short to be HDLC encapsulated"); + return Err(HdlcError::TooShort); + } + + let last_char = data.pop().unwrap(); // safe since len() >= 3 + if last_char != 0x7e { + return Err(HdlcError::NoTrailingCharacter(last_char)); } - 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() { @@ -31,7 +55,7 @@ pub fn hdlc_decapsulate(mut data: Vec, crc: &Crc) -> Vec { match b { 0x5e => unescaped.push(0x7e), 0x5d => unescaped.push(0x7d), - _ => panic!("invalid HDLC escape sequence"), + _ => return Err(HdlcError::InvalidEscapeSequence(b)), } escaping = false; } else if b == 0x7d { @@ -42,12 +66,14 @@ pub fn hdlc_decapsulate(mut data: Vec, crc: &Crc) -> Vec { } // 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_hi = unescaped.pop().ok_or(HdlcError::MissingChecksum)?; + let checksum_lo = unescaped.pop().ok_or(HdlcError::MissingChecksum)?; let checksum = [checksum_lo, checksum_hi].as_slice().get_u16_le(); - assert_eq!(checksum, crc.checksum(&unescaped)); // ensure checksums match + if checksum != crc.checksum(&unescaped) { + return Err(HdlcError::InvalidChecksum(checksum, crc.checksum(&unescaped))); + } - unescaped + Ok(unescaped) } #[cfg(test)] @@ -56,11 +82,11 @@ mod tests { #[test] fn test_hdlc_encapsulate() { - let crc = Crc::::new(&crate::diag::CRC_CCITT_ALG); + let crc = Crc::::new(&crate::diag_device::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); + assert_eq!(hdlc_decapsulate(encapsulated, &crc), Ok(data)); } } diff --git a/src/main.rs b/src/main.rs index a8197f3..684d839 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,19 +1,16 @@ mod hdlc; mod diag; +mod diag_device; -use crate::hdlc::{hdlc_encapsulate, hdlc_decapsulate}; -use crate::diag::{DiagDevice}; +use crate::diag_device::DiagDevice; fn main() -> std::io::Result<()> { let mut dev = DiagDevice::new().unwrap(); dev.config_logs().unwrap(); loop { - let msgs = dev.read_response().unwrap(); - if let Some(msgs) = msgs { - for msg in msgs { - println!("msg: {:?}", msg); - } + for msg in dev.read_response().unwrap() { + println!("msg: {:?}", msg); } } } From e6fab5c27d82811222848050a22f8c279b14f19e Mon Sep 17 00:00:00 2001 From: Will Greenberg Date: Mon, 4 Dec 2023 16:47:42 -0800 Subject: [PATCH 04/17] log parsing --- Cargo.lock | 230 +++++++++++++++++++++++++++++++++++++++++++++ Cargo.toml | 1 + src/diag.rs | 78 ++++++++++++--- src/diag_device.rs | 73 ++++++++------ 4 files changed, 338 insertions(+), 44 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 5d969ab..f3caa6b 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -2,6 +2,27 @@ # It is not intended for manual editing. version = 3 +[[package]] +name = "android-tzdata" +version = "0.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e999941b234f3131b00bc13c22d06e8c5ff726d1b6318ac7eb276997bbb4fef0" + +[[package]] +name = "android_system_properties" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "819e7219dbd41043ac279b19830f2efc897156490d7fd6ea916720117ee66311" +dependencies = [ + "libc", +] + +[[package]] +name = "autocfg" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d468802bab17cbc0cc575e9b053f41e72aa36bfa6b7f55e3529ffa43161b97fa" + [[package]] name = "bitvec" version = "1.0.1" @@ -14,6 +35,12 @@ dependencies = [ "wyz", ] +[[package]] +name = "bumpalo" +version = "3.14.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7f30e7476521f6f8af1a1c4c0b8cc94f0bee37d91763d0ca2665f299b6cd8aec" + [[package]] name = "byteorder" version = "1.5.0" @@ -35,6 +62,41 @@ version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a2bd12c1caf447e69cd4528f47f94d203fd2582878ecb9e9465484c4148a8223" +[[package]] +name = "cc" +version = "1.0.83" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f1174fb0b6ec23863f8b971027804a42614e347eafb0a95bf0b12cdae21fc4d0" +dependencies = [ + "libc", +] + +[[package]] +name = "cfg-if" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" + +[[package]] +name = "chrono" +version = "0.4.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7f2c685bad3eb3d45a01354cedb7d5faa66194d1d58ba6e267a8de788f79db38" +dependencies = [ + "android-tzdata", + "iana-time-zone", + "js-sys", + "num-traits", + "wasm-bindgen", + "windows-targets", +] + +[[package]] +name = "core-foundation-sys" +version = "0.8.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06ea2b9bc92be3c2baa9334a323ebca2d6f074ff852cd1d7b11064035cd3868f" + [[package]] name = "crc" version = "3.0.1" @@ -124,6 +186,7 @@ name = "diag" version = "0.1.0" dependencies = [ "bytes", + "chrono", "crc", "deku", "libc", @@ -155,6 +218,29 @@ version = "0.14.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "290f1a1d9242c78d09ce40a5e87e7554ee637af1351968159f4952f028f75604" +[[package]] +name = "iana-time-zone" +version = "0.1.58" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8326b86b6cff230b97d0d312a6c40a60726df3332e721f72a1b035f451663b20" +dependencies = [ + "android_system_properties", + "core-foundation-sys", + "iana-time-zone-haiku", + "js-sys", + "wasm-bindgen", + "windows-core", +] + +[[package]] +name = "iana-time-zone-haiku" +version = "0.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f31827a206f56af32e590ba56d5d2d085f558508192593743f16b2306495269f" +dependencies = [ + "cc", +] + [[package]] name = "ident_case" version = "1.0.1" @@ -171,18 +257,42 @@ dependencies = [ "hashbrown", ] +[[package]] +name = "js-sys" +version = "0.3.66" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cee9c64da59eae3b50095c18d3e74f8b73c0b86d2792824ff01bbce68ba229ca" +dependencies = [ + "wasm-bindgen", +] + [[package]] name = "libc" version = "0.2.150" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "89d92a4743f9a61002fae18374ed11e7973f530cb3a3255fb354818118b2203c" +[[package]] +name = "log" +version = "0.4.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b5e6163cb8c49088c2c36f57875e58ccd8c87c7427f7fbd50ea6710b2f3f2e8f" + [[package]] name = "memchr" version = "2.6.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f665ee40bc4a3c5590afb1e9677db74a508659dfd71e126420da8274909a0167" +[[package]] +name = "num-traits" +version = "0.2.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "39e3200413f237f41ab11ad6d161bc7239c84dcb631773ccd7de3dfe4b5c267c" +dependencies = [ + "autocfg", +] + [[package]] name = "once_cell" version = "1.18.0" @@ -311,6 +421,126 @@ version = "1.0.12" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b" +[[package]] +name = "wasm-bindgen" +version = "0.2.89" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ed0d4f68a3015cc185aff4db9506a015f4b96f95303897bfa23f846db54064e" +dependencies = [ + "cfg-if", + "wasm-bindgen-macro", +] + +[[package]] +name = "wasm-bindgen-backend" +version = "0.2.89" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1b56f625e64f3a1084ded111c4d5f477df9f8c92df113852fa5a374dbda78826" +dependencies = [ + "bumpalo", + "log", + "once_cell", + "proc-macro2", + "quote", + "syn 2.0.39", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-macro" +version = "0.2.89" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0162dbf37223cd2afce98f3d0785506dcb8d266223983e4b5b525859e6e182b2" +dependencies = [ + "quote", + "wasm-bindgen-macro-support", +] + +[[package]] +name = "wasm-bindgen-macro-support" +version = "0.2.89" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f0eb82fcb7930ae6219a7ecfd55b217f5f0893484b7a13022ebb2b2bf20b5283" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.39", + "wasm-bindgen-backend", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-shared" +version = "0.2.89" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7ab9b36309365056cd639da3134bf87fa8f3d86008abf99e612384a6eecd459f" + +[[package]] +name = "windows-core" +version = "0.51.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f1f8cf84f35d2db49a46868f947758c7a1138116f7fac3bc844f43ade1292e64" +dependencies = [ + "windows-targets", +] + +[[package]] +name = "windows-targets" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a2fa6e2155d7247be68c096456083145c183cbbbc2764150dda45a87197940c" +dependencies = [ + "windows_aarch64_gnullvm", + "windows_aarch64_msvc", + "windows_i686_gnu", + "windows_i686_msvc", + "windows_x86_64_gnu", + "windows_x86_64_gnullvm", + "windows_x86_64_msvc", +] + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2b38e32f0abccf9987a4e3079dfb67dcd799fb61361e53e2882c3cbaf0d905d8" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dc35310971f3b2dbbf3f0690a219f40e2d9afcf64f9ab7cc1be722937c26b4bc" + +[[package]] +name = "windows_i686_gnu" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a75915e7def60c94dcef72200b9a8e58e5091744960da64ec734a6c6e9b3743e" + +[[package]] +name = "windows_i686_msvc" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8f55c233f70c4b27f66c523580f78f1004e8b5a8b659e05a4eb49d4166cca406" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "53d40abd2583d23e4718fddf1ebec84dbff8381c07cae67ff7768bbf19c6718e" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0b7b52767868a23d5bab768e390dc5f5c55825b6d30b86c844ff2dc7414044cc" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ed94fce61571a4006852b7389a063ab983c02eb1bb37b47f8272ce92d06d9538" + [[package]] name = "winnow" version = "0.5.19" diff --git a/Cargo.toml b/Cargo.toml index 8c4ef28..823290a 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -7,6 +7,7 @@ edition = "2021" [dependencies] bytes = "1.5.0" +chrono = "0.4.31" crc = "3.0.1" deku = "0.16.0" libc = "0.2.150" diff --git a/src/diag.rs b/src/diag.rs index b97904f..20387dd 100644 --- a/src/diag.rs +++ b/src/diag.rs @@ -1,5 +1,6 @@ //! Diag protocol serialization/deserialization +use chrono::{DateTime, Local, FixedOffset}; use deku::prelude::*; #[derive(Debug, Clone, DekuWrite)] @@ -43,31 +44,66 @@ pub enum DataType { } #[derive(Debug, Clone, DekuRead)] -pub struct ResponseContainer { +pub struct MessagesContainer { pub data_type: DataType, pub num_responses: u32, #[deku(count = "num_responses")] - pub responses: Vec, + pub messages: Vec, } #[derive(Debug, Clone, DekuRead)] -pub struct HdlcEncapsulatedResponse { +pub struct HdlcEncapsulatedMessage { pub len: u32, #[deku(count = "len")] pub data: 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, - pub status: u32, - #[deku(ctx = "*opcode, *subopcode")] - pub payload: ResponsePayload, +#[deku(type = "u8")] +pub enum Message { + #[deku(id = "16")] + Log { + pending_msgs: u8, + outer_length: u16, + inner_length: u16, + log_type: u16, + timestamp: Timestamp, + #[deku(count = "inner_length - 12")] + payload: 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. + #[deku(id_pat = "_")] + Response { + opcode: u32, + subopcode: u32, + status: u32, + #[deku(ctx = "*opcode, *subopcode")] + payload: ResponsePayload, + }, +} + +#[derive(Debug, Clone, DekuRead)] +#[deku(endian = "little")] +pub struct Timestamp { + pub ts: u64, +} + +impl Timestamp { + pub fn to_datetime(&self) -> DateTime { + // Upper 48 bits: epoch at 1980-01-06 00:00:00, incremented by 1 for 1/800s + // Lower 16 bits: time since last 1/800s tick in 1/32 chip units + let ts_upper = self.ts >> 16; + let ts_lower = self.ts & 0xffff; + let epoch = chrono::DateTime::parse_from_rfc3339("1980-01-06T00:00:00-00:00").unwrap(); + let mut delta_seconds = ts_upper as f64 * 1.25; + delta_seconds += ts_lower as f64 / 40960.0; + let ts_delta = chrono::Duration::milliseconds(delta_seconds as i64); + epoch + ts_delta + } } #[derive(Debug, Clone, DekuRead)] @@ -177,4 +213,20 @@ mod test { 1, 2, 3, 4, ]); } + + #[test] + fn test_message_parsing() { + let msg_bytes = vec![16, 0, 26, 0, 26, 0, 167, 24, 38, 161, 72, 107, 146, 30, 2, 1, 1, 1, 0, 0, 0, 0, 0, 140, 10, 0, 0, 220, 5, 0]; + match Message::from_bytes((msg_bytes.as_slice(), 0)) { + Ok((_, Message::Log { pending_msgs, outer_length, inner_length, log_type, timestamp, payload })) => { + assert_eq!(pending_msgs, 0); + assert_eq!(outer_length, 26); + assert_eq!(inner_length, 26); + assert_eq!(log_type, 6311); + assert_eq!(timestamp.to_datetime().date_naive(), chrono::NaiveDate::from_ymd_opt(2023, 12, 4).unwrap()); + assert_eq!(payload, vec![1, 1, 0, 0, 0, 0, 0, 140, 10, 0, 0, 220, 5, 0]); + }, + _ => panic!("failed to parse message"), + } + } } diff --git a/src/diag_device.rs b/src/diag_device.rs index de3dc35..aba508d 100644 --- a/src/diag_device.rs +++ b/src/diag_device.rs @@ -1,5 +1,5 @@ use crate::hdlc::{hdlc_encapsulate, hdlc_decapsulate, HdlcError}; -use crate::diag::{Response, ResponsePayload, Request, LogConfigRequest, LogConfigResponse, build_log_mask_request, RequestContainer, DataType, ResponseContainer}; +use crate::diag::{Message, ResponsePayload, Request, LogConfigRequest, LogConfigResponse, build_log_mask_request, RequestContainer, DataType, MessagesContainer}; use std::fs::File; use std::io::{Cursor, Read, Write}; @@ -72,32 +72,35 @@ impl DiagDevice { }) } - fn parse_response_container(&self, container: ResponseContainer) -> DiagResult> { + fn parse_response_container(&self, container: MessagesContainer) -> DiagResult> { let mut result = Vec::new(); - for msg in container.responses { - let data = hdlc_decapsulate(msg.data, &self.crc)?; - match Response::from_bytes((&data, 0)) { - Ok(((_, leftover_bytes), res)) => { - if leftover_bytes > 0 { - println!("warning: {} leftover bytes when Response", leftover_bytes); - } - result.push(res); - }, - Err(e) => { - println!("{:?}", data); - println!("error parsing response: {:?}", e); + for msg in container.messages { + match hdlc_decapsulate(msg.data, &self.crc) { + Ok(data) => match Message::from_bytes((&data, 0)) { + Ok(((_, leftover_bytes), res)) => { + if leftover_bytes > 0 { + println!("warning: {} leftover bytes when Response", leftover_bytes); + } + result.push(res); + }, + Err(e) => { + println!("error parsing response: {:?}", e); + }, }, + Err(err) => { + println!("error decapsulating response: {:?}", err); + } } } Ok(result) } - pub fn read_response(&mut self) -> DiagResult> { + pub fn read_response(&mut self) -> DiagResult> { let mut buf = vec![0; BUFFER_LEN]; loop { let _ = self.file.read(&mut buf)?; - let ((_, leftover_bytes), res_container) = ResponseContainer::from_bytes((&buf, 0))?; + let ((_, leftover_bytes), res_container) = MessagesContainer::from_bytes((&buf, 0))?; if leftover_bytes > 0 { println!("warning: {} leftover bytes when parsing ResponseContainer", leftover_bytes); } @@ -124,7 +127,7 @@ impl DiagDevice { let msg = format!("write failed with error code {}", ret); return Err(DiagDeviceError::DeviceReadFailed(msg)); } - println!("{}. wrote {} bytes to device", buf.len(), ret); + println!("wrote {} bytes to device", ret); } Ok(()) } @@ -133,15 +136,18 @@ impl DiagDevice { let req = Request::LogConfig(LogConfigRequest::RetrieveIdRanges); self.write_request(&req)?; - for res in self.read_response()? { - match res.payload { - ResponsePayload::LogConfig(LogConfigResponse::RetrieveIdRanges { log_mask_sizes }) => { - if res.status != 0 { - return Err(DiagDeviceError::RequestFailed(res.status, req)); - } - return Ok(log_mask_sizes); + for msg in self.read_response()? { + match msg { + Message::Log { .. } => println!("skipping log response..."), + Message::Response { payload, status, .. } => match payload { + ResponsePayload::LogConfig(LogConfigResponse::RetrieveIdRanges { log_mask_sizes }) => { + if status != 0 { + return Err(DiagDeviceError::RequestFailed(status, req)); + } + return Ok(log_mask_sizes); + }, + _ => println!("skipping non-LogConfigResponse response..."), }, - _ => println!("skipping non-LogConfigResponse response..."), } } @@ -153,12 +159,17 @@ impl DiagDevice { let req = build_log_mask_request(log_type, log_mask_bitsize); self.write_request(&req)?; - for res in self.read_response()? { - if let ResponsePayload::LogConfig(LogConfigResponse::SetMask) = res.payload { - if res.status != 0 { - return Err(DiagDeviceError::RequestFailed(res.status, req)); - } - return Ok(()); + for msg in self.read_response()? { + match msg { + Message::Log { .. } => println!("skipping log response..."), + Message::Response { payload, status, .. } => { + if let ResponsePayload::LogConfig(LogConfigResponse::SetMask) = payload { + if status != 0 { + return Err(DiagDeviceError::RequestFailed(status, req)); + } + return Ok(()); + } + }, } } From ccd9dfa2a735db27457e4ff4b2a4b2cc834c8765 Mon Sep 17 00:00:00 2001 From: Will Greenberg Date: Mon, 4 Dec 2023 16:54:46 -0800 Subject: [PATCH 05/17] appease clippy --- src/diag.rs | 4 ++-- src/diag_device.rs | 8 +++----- src/hdlc.rs | 3 +-- 3 files changed, 6 insertions(+), 9 deletions(-) diff --git a/src/diag.rs b/src/diag.rs index 20387dd..6b6c213 100644 --- a/src/diag.rs +++ b/src/diag.rs @@ -1,6 +1,6 @@ //! Diag protocol serialization/deserialization -use chrono::{DateTime, Local, FixedOffset}; +use chrono::{DateTime, FixedOffset}; use deku::prelude::*; #[derive(Debug, Clone, DekuWrite)] @@ -137,7 +137,7 @@ pub fn build_log_mask_request(log_type: u32, log_mask_bitsize: u32) -> Request { } Request::LogConfig(LogConfigRequest::SetMask { - log_type: log_type as u32, + log_type, log_mask_bitsize, log_mask, }) diff --git a/src/diag_device.rs b/src/diag_device.rs index aba508d..a35e40e 100644 --- a/src/diag_device.rs +++ b/src/diag_device.rs @@ -2,8 +2,7 @@ use crate::hdlc::{hdlc_encapsulate, hdlc_decapsulate, HdlcError}; use crate::diag::{Message, ResponsePayload, Request, LogConfigRequest, LogConfigResponse, build_log_mask_request, RequestContainer, DataType, MessagesContainer}; use std::fs::File; -use std::io::{Cursor, Read, Write}; -use bytes::{Buf, BufMut}; +use std::io::Read; use std::os::fd::AsRawFd; use thiserror::Error; use crc::{Crc, Algorithm}; @@ -43,7 +42,6 @@ pub const CRC_CCITT_ALG: Algorithm = Algorithm { }; 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; @@ -151,7 +149,7 @@ impl DiagDevice { } } - return Err(DiagDeviceError::NoResponse(req)); + Err(DiagDeviceError::NoResponse(req)) } fn set_log_mask(&mut self, log_type: u32, log_mask_bitsize: u32) -> DiagResult<()> { @@ -173,7 +171,7 @@ impl DiagDevice { } } - return Err(DiagDeviceError::NoResponse(req)); + Err(DiagDeviceError::NoResponse(req)) } pub fn config_logs(&mut self) -> DiagResult<()> { diff --git a/src/hdlc.rs b/src/hdlc.rs index dcabf00..51861dd 100644 --- a/src/hdlc.rs +++ b/src/hdlc.rs @@ -49,8 +49,7 @@ pub fn hdlc_decapsulate(mut data: Vec, crc: &Crc) -> Result, Hd let mut unescaped = Vec::new(); let mut escaping = false; - for i in 0..data.len() { - let b = data[i]; + for b in data { if escaping { match b { 0x5e => unescaped.push(0x7e), From be058507f132308124cae7d504721518a86a5647 Mon Sep 17 00:00:00 2001 From: Will Greenberg Date: Tue, 5 Dec 2023 18:11:17 -0800 Subject: [PATCH 06/17] wip --- src/diag.rs | 44 ++++++++---------- src/diag_device.rs | 26 +++++++++-- src/log_codes.rs | 108 +++++++++++++++++++++++++++++++++++++++++++++ src/main.rs | 2 + 4 files changed, 152 insertions(+), 28 deletions(-) create mode 100644 src/log_codes.rs diff --git a/src/diag.rs b/src/diag.rs index 6b6c213..ea61b24 100644 --- a/src/diag.rs +++ b/src/diag.rs @@ -125,15 +125,23 @@ pub enum LogConfigResponse { SetMask, } -// 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 -pub 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)); +pub fn build_log_mask_request(log_type: u32, log_mask_bitsize: u32, accepted_log_codes: &[u32]) -> Request { + let mut current_byte: u8 = 0; + let mut num_bits_written: u8 = 0; + let mut log_mask: Vec = vec![]; + for i in 0..log_mask_bitsize { + let log_code: u32 = (log_type << 12) | i; + if accepted_log_codes.contains(&log_code) { + println!("enabling {:x}", log_code); + current_byte |= 1 << num_bits_written; + } + num_bits_written += 1; + + if num_bits_written == 8 || i == log_mask_bitsize - 1 { + current_byte = 0; + num_bits_written = 0; + log_mask.push(current_byte); + } } Request::LogConfig(LogConfigRequest::SetMask { @@ -167,26 +175,12 @@ mod test { #[test] fn test_build_log_mask_request() { - assert_eq!(build_log_mask_request(0, 1), Request::LogConfig(LogConfigRequest::SetMask { + let accepted = vec![0x412f]; + assert_eq!(build_log_mask_request(0, 1, &accepted), 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], - })); } #[test] diff --git a/src/diag_device.rs b/src/diag_device.rs index a35e40e..134caa1 100644 --- a/src/diag_device.rs +++ b/src/diag_device.rs @@ -1,5 +1,6 @@ use crate::hdlc::{hdlc_encapsulate, hdlc_decapsulate, HdlcError}; use crate::diag::{Message, ResponsePayload, Request, LogConfigRequest, LogConfigResponse, build_log_mask_request, RequestContainer, DataType, MessagesContainer}; +use crate::log_codes; use std::fs::File; use std::io::Read; @@ -41,6 +42,27 @@ pub const CRC_CCITT_ALG: Algorithm = Algorithm { residue: 0x0000, }; +pub const LOG_CODES_FOR_RAW_PACKET_LOGGING: [u32; 11] = [ + // Layer 2: + log_codes::LOG_GPRS_MAC_SIGNALLING_MESSAGE_C, // 0x5226 + + // Layer 3: + log_codes::LOG_GSM_RR_SIGNALING_MESSAGE_C, // 0x512f + log_codes::WCDMA_SIGNALLING_MESSAGE, // 0x412f + log_codes::LOG_LTE_RRC_OTA_MSG_LOG_C, // 0xb0c0 + log_codes::LOG_NR_RRC_OTA_MSG_LOG_C, // 0xb821 + + // NAS: + log_codes::LOG_UMTS_NAS_OTA_MESSAGE_LOG_PACKET_C, // 0x713a + log_codes::LOG_LTE_NAS_ESM_OTA_IN_MSG_LOG_C, // 0xb0e2 + log_codes::LOG_LTE_NAS_ESM_OTA_OUT_MSG_LOG_C, // 0xb0e3 + log_codes::LOG_LTE_NAS_EMM_OTA_IN_MSG_LOG_C, // 0xb0ec + log_codes::LOG_LTE_NAS_EMM_OTA_OUT_MSG_LOG_C, // 0xb0ed + + // User IP traffic: + log_codes::LOG_DATA_PROTOCOL_LOGGING_C // 0x11eb +]; + const BUFFER_LEN: usize = 1024 * 1024 * 10; const MEMORY_DEVICE_MODE: i32 = 2; const DIAG_IOCTL_REMOTE_DEV: u32 = 32; @@ -125,7 +147,6 @@ impl DiagDevice { let msg = format!("write failed with error code {}", ret); return Err(DiagDeviceError::DeviceReadFailed(msg)); } - println!("wrote {} bytes to device", ret); } Ok(()) } @@ -153,8 +174,7 @@ impl DiagDevice { } fn set_log_mask(&mut self, log_type: u32, log_mask_bitsize: u32) -> DiagResult<()> { - // send a logging mask of all 1's equal to its respective mask size - let req = build_log_mask_request(log_type, log_mask_bitsize); + let req = build_log_mask_request(log_type, log_mask_bitsize, &LOG_CODES_FOR_RAW_PACKET_LOGGING); self.write_request(&req)?; for msg in self.read_response()? { diff --git a/src/log_codes.rs b/src/log_codes.rs new file mode 100644 index 0000000..7e90bed --- /dev/null +++ b/src/log_codes.rs @@ -0,0 +1,108 @@ +//! Enumerates some releVant diag log codes. Copied from QCSuper + + +// These are 2G-related log types. + +pub const LOG_GSM_RR_SIGNALING_MESSAGE_C: u32 = 0x512f; + +pub const DCCH: u32 = 0x00; +pub const BCCH: u32 = 0x01; +pub const L2_RACH: u32 = 0x02; +pub const CCCH: u32 = 0x03; +pub const SACCH: u32 = 0x04; +pub const SDCCH: u32 = 0x05; +pub const FACCH_F: u32 = 0x06; +pub const FACCH_H: u32 = 0x07; +pub const L2_RACH_WITH_NO_DELAY: u32 = 0x08; + +// These are GPRS-related log types. + +pub const LOG_GPRS_MAC_SIGNALLING_MESSAGE_C: u32 = 0x5226; + +pub const PACCH_RRBP_CHANNEL: u32 = 0x03; +pub const UL_PACCH_CHANNEL: u32 = 0x04; +pub const DL_PACCH_CHANNEL: u32 = 0x83; + +pub const PACKET_CHANNEL_REQUEST: u32 = 0x20; + +// These are 5G-related log types. + +pub const LOG_NR_RRC_OTA_MSG_LOG_C: u32 = 0xb821; + +// These are 4G-related log types. + +pub const LOG_LTE_RRC_OTA_MSG_LOG_C: u32 = 0xb0c0; +pub const LOG_LTE_NAS_ESM_OTA_IN_MSG_LOG_C: u32 = 0xb0e2; +pub const LOG_LTE_NAS_ESM_OTA_OUT_MSG_LOG_C: u32 = 0xb0e3; +pub const LOG_LTE_NAS_EMM_OTA_IN_MSG_LOG_C: u32 = 0xb0ec; +pub const LOG_LTE_NAS_EMM_OTA_OUT_MSG_LOG_C: u32 = 0xb0ed; + +pub const LTE_BCCH_BCH_V0: u32 = 1; +pub const LTE_BCCH_DL_SCH_V0: u32 = 2; +pub const LTE_MCCH_V0: u32 = 3; +pub const LTE_PCCH_V0: u32 = 4; +pub const LTE_DL_CCCH_V0: u32 = 5; +pub const LTE_DL_DCCH_V0: u32 = 6; +pub const LTE_UL_CCCH_V0: u32 = 7; +pub const LTE_UL_DCCH_V0: u32 = 8; + +pub const LTE_BCCH_BCH_V14: u32 = 1; +pub const LTE_BCCH_DL_SCH_V14: u32 = 2; +pub const LTE_MCCH_V14: u32 = 4; +pub const LTE_PCCH_V14: u32 = 5; +pub const LTE_DL_CCCH_V14: u32 = 6; +pub const LTE_DL_DCCH_V14: u32 = 7; +pub const LTE_UL_CCCH_V14: u32 = 8; +pub const LTE_UL_DCCH_V14: u32 = 9; + +pub const LTE_BCCH_BCH_V9: u32 = 8; +pub const LTE_BCCH_DL_SCH_V9: u32 = 9; +pub const LTE_MCCH_V9: u32 = 10; +pub const LTE_PCCH_V9: u32 = 11; +pub const LTE_DL_CCCH_V9: u32 = 12; +pub const LTE_DL_DCCH_V9: u32 = 13; +pub const LTE_UL_CCCH_V9: u32 = 14; +pub const LTE_UL_DCCH_V9: u32 = 15; + +pub const LTE_BCCH_BCH_V19: u32 = 1; +pub const LTE_BCCH_DL_SCH_V19: u32 = 3; +pub const LTE_MCCH_V19: u32 = 6; +pub const LTE_PCCH_V19: u32 = 7; +pub const LTE_DL_CCCH_V19: u32 = 8; +pub const LTE_DL_DCCH_V19: u32 = 9; +pub const LTE_UL_CCCH_V19: u32 = 10; +pub const LTE_UL_DCCH_V19: u32 = 11; + +pub const LTE_BCCH_BCH_NB: u32 = 45; +pub const LTE_BCCH_DL_SCH_NB: u32 = 46; +pub const LTE_PCCH_NB: u32 = 47; +pub const LTE_DL_CCCH_NB: u32 = 48; +pub const LTE_DL_DCCH_NB: u32 = 49; +pub const LTE_UL_CCCH_NB: u32 = 50; +pub const LTE_UL_DCCH_NB: u32 = 52; + +// These are 3G-related log types. + +pub const RRCLOG_SIG_UL_CCCH: u32 = 0; +pub const RRCLOG_SIG_UL_DCCH: u32 = 1; +pub const RRCLOG_SIG_DL_CCCH: u32 = 2; +pub const RRCLOG_SIG_DL_DCCH: u32 = 3; +pub const RRCLOG_SIG_DL_BCCH_BCH: u32 = 4; +pub const RRCLOG_SIG_DL_BCCH_FACH: u32 = 5; +pub const RRCLOG_SIG_DL_PCCH: u32 = 6; +pub const RRCLOG_SIG_DL_MCCH: u32 = 7; +pub const RRCLOG_SIG_DL_MSCH: u32 = 8; +pub const RRCLOG_EXTENSION_SIB: u32 = 9; +pub const RRCLOG_SIB_CONTAINER: u32 = 10; + + +// 3G layer 3 packets: + +pub const WCDMA_SIGNALLING_MESSAGE: u32 = 0x412f; + + +// Upper layers + +pub const LOG_DATA_PROTOCOL_LOGGING_C: u32 = 0x11eb; + +pub const LOG_UMTS_NAS_OTA_MESSAGE_LOG_PACKET_C: u32 = 0x713a; diff --git a/src/main.rs b/src/main.rs index 684d839..8575630 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,6 +1,7 @@ mod hdlc; mod diag; mod diag_device; +mod log_codes; use crate::diag_device::DiagDevice; @@ -9,6 +10,7 @@ fn main() -> std::io::Result<()> { dev.config_logs().unwrap(); loop { + println!("waiting for message..."); for msg in dev.read_response().unwrap() { println!("msg: {:?}", msg); } From 9dda8eb9524cd03089750ef0f4e9e3b62302f121 Mon Sep 17 00:00:00 2001 From: Will Greenberg Date: Tue, 5 Dec 2023 18:26:17 -0800 Subject: [PATCH 07/17] wip --- src/diag_device.rs | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/diag_device.rs b/src/diag_device.rs index 134caa1..d14ba6b 100644 --- a/src/diag_device.rs +++ b/src/diag_device.rs @@ -119,6 +119,7 @@ impl DiagDevice { let mut buf = vec![0; BUFFER_LEN]; loop { + println!("waiting to read"); let _ = self.file.read(&mut buf)?; let ((_, leftover_bytes), res_container) = MessagesContainer::from_bytes((&buf, 0))?; if leftover_bytes > 0 { @@ -175,6 +176,7 @@ impl DiagDevice { fn set_log_mask(&mut self, log_type: u32, log_mask_bitsize: u32) -> DiagResult<()> { let req = build_log_mask_request(log_type, log_mask_bitsize, &LOG_CODES_FOR_RAW_PACKET_LOGGING); + println!("setting log mask {} {:?}", log_type, &req); self.write_request(&req)?; for msg in self.read_response()? { From bdb12aba9ea6ba4b48d07ab348cfe1831b48b70b Mon Sep 17 00:00:00 2001 From: Will Greenberg Date: Tue, 5 Dec 2023 18:41:35 -0800 Subject: [PATCH 08/17] fixed log config stage, now getting actual logs --- src/diag.rs | 23 ++++++++++++++++------- src/diag_device.rs | 4 ---- src/main.rs | 1 - 3 files changed, 16 insertions(+), 12 deletions(-) diff --git a/src/diag.rs b/src/diag.rs index ea61b24..9430683 100644 --- a/src/diag.rs +++ b/src/diag.rs @@ -132,15 +132,14 @@ pub fn build_log_mask_request(log_type: u32, log_mask_bitsize: u32, accepted_log for i in 0..log_mask_bitsize { let log_code: u32 = (log_type << 12) | i; if accepted_log_codes.contains(&log_code) { - println!("enabling {:x}", log_code); current_byte |= 1 << num_bits_written; } num_bits_written += 1; if num_bits_written == 8 || i == log_mask_bitsize - 1 { + log_mask.push(current_byte); current_byte = 0; num_bits_written = 0; - log_mask.push(current_byte); } } @@ -153,6 +152,7 @@ pub fn build_log_mask_request(log_type: u32, log_mask_bitsize: u32, accepted_log #[cfg(test)] mod test { + use crate::diag_device::LOG_CODES_FOR_RAW_PACKET_LOGGING; use super::*; #[test] @@ -175,11 +175,20 @@ mod test { #[test] fn test_build_log_mask_request() { - let accepted = vec![0x412f]; - assert_eq!(build_log_mask_request(0, 1, &accepted), Request::LogConfig(LogConfigRequest::SetMask { - log_type: 0, - log_mask_bitsize: 1, - log_mask: vec![0x01], + let log_type = 11; + let bitsize = 513; + let req = build_log_mask_request(log_type, bitsize, &LOG_CODES_FOR_RAW_PACKET_LOGGING); + assert_eq!(req, Request::LogConfig(LogConfigRequest::SetMask { + log_type: log_type, + log_mask_bitsize: bitsize, + log_mask: vec![ + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, + 0x1, 0x0, 0x0, 0x0, 0xc, 0x30, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, + 0x0, 0x0, 0x0, 0x0, 0x0, + ], })); } diff --git a/src/diag_device.rs b/src/diag_device.rs index d14ba6b..eb712f5 100644 --- a/src/diag_device.rs +++ b/src/diag_device.rs @@ -119,7 +119,6 @@ impl DiagDevice { let mut buf = vec![0; BUFFER_LEN]; loop { - println!("waiting to read"); let _ = self.file.read(&mut buf)?; let ((_, leftover_bytes), res_container) = MessagesContainer::from_bytes((&buf, 0))?; if leftover_bytes > 0 { @@ -176,7 +175,6 @@ impl DiagDevice { fn set_log_mask(&mut self, log_type: u32, log_mask_bitsize: u32) -> DiagResult<()> { let req = build_log_mask_request(log_type, log_mask_bitsize, &LOG_CODES_FOR_RAW_PACKET_LOGGING); - println!("setting log mask {} {:?}", log_type, &req); self.write_request(&req)?; for msg in self.read_response()? { @@ -199,11 +197,9 @@ impl DiagDevice { pub fn config_logs(&mut self) -> DiagResult<()> { println!("retrieving diag logging capabilities..."); let log_mask_sizes = self.retrieve_id_ranges()?; - println!("log mask sizes: {:?}", log_mask_sizes); for (log_type, &log_mask_bitsize) in log_mask_sizes.iter().enumerate() { if log_mask_bitsize > 0 { - println!("setting logging for log_type {}", log_type); self.set_log_mask(log_type as u32, log_mask_bitsize)?; println!("enabled logging for log type {}", log_type); } diff --git a/src/main.rs b/src/main.rs index 8575630..8a63cae 100644 --- a/src/main.rs +++ b/src/main.rs @@ -10,7 +10,6 @@ fn main() -> std::io::Result<()> { dev.config_logs().unwrap(); loop { - println!("waiting for message..."); for msg in dev.read_response().unwrap() { println!("msg: {:?}", msg); } From 196eeba520741e09c84a61f3f773716f856ce08b Mon Sep 17 00:00:00 2001 From: Will Greenberg Date: Wed, 6 Dec 2023 15:19:52 -0800 Subject: [PATCH 09/17] Parse log bodies --- src/diag.rs | 174 ++++++++++++++++++++++++++++++++++++++------- src/diag_device.rs | 3 +- src/main.rs | 9 +-- 3 files changed, 154 insertions(+), 32 deletions(-) diff --git a/src/diag.rs b/src/diag.rs index 9430683..3379fd5 100644 --- a/src/diag.rs +++ b/src/diag.rs @@ -1,7 +1,7 @@ //! Diag protocol serialization/deserialization use chrono::{DateTime, FixedOffset}; -use deku::prelude::*; +use deku::{prelude::*, bitvec::{BitSlice, Msb0}}; #[derive(Debug, Clone, DekuWrite)] pub struct RequestContainer { @@ -58,7 +58,7 @@ pub struct HdlcEncapsulatedMessage { pub data: Vec, } -#[derive(Debug, Clone, DekuRead)] +#[derive(Debug, Clone, PartialEq, DekuRead)] #[deku(type = "u8")] pub enum Message { #[deku(id = "16")] @@ -68,8 +68,9 @@ pub enum Message { inner_length: u16, log_type: u16, timestamp: Timestamp, - #[deku(count = "inner_length - 12")] - payload: Vec, + //#[deku(count = "inner_length - 12")] + #[deku(ctx = "*log_type, *inner_length - 12")] + body: LogBody, }, // kinda unpleasant deku hackery here. deku expects an enum's variant to be @@ -86,7 +87,111 @@ pub enum Message { }, } -#[derive(Debug, Clone, DekuRead)] +#[derive(Debug, Clone, PartialEq, DekuRead)] +#[deku(ctx = "log_type: u16, hdr_len: u16", id = "log_type")] +pub enum LogBody { + #[deku(id = "0x412f")] + WcdmaSignallingMessage { + channel_type: u8, + radio_bearer: u8, + length: u16, + #[deku(count = "length")] + msg: Vec, + }, + #[deku(id = "0x512f")] + GsmRrSignallingMessage { + channel_type: u8, + message_type: u8, + length: u8, + #[deku(count = "length")] + msg: Vec, + }, + #[deku(id = "0x5226")] + GprsMacSignallingMessage { + channel_type: u8, + message_type: u8, + length: u8, + #[deku(count = "length")] + msg: Vec, + }, + #[deku(id = "0xb0c0")] + LteRrcOtaMessage { + ext_header_version: u8, + rrc_rel: u8, + rrc_version: u8, + #[deku(skip, cond = "*ext_header_version < 25")] // handle post-NR releases + nc_rrc_rel: Option, + bearer_id: u8, + phy_cell_id: u16, + + // extended header. some of these fields need manual parsing based on + // header version + #[deku(reader = "read_lte_rrc_ota_message_log_freq(deku::rest, *ext_header_version)")] + freq: u32, + sfn: u16, + channel_type: u8, + #[deku(reader = "read_lte_rrc_ota_message_log_msg(deku::rest)")] + msg: Vec, + }, + #[deku(id_pat = "0xb0e2 | 0xb0e3 | 0xb0ec | 0xb0ed")] + Nas4GMessage { + ext_header_version: u8, + rrc_rel: u8, + rrc_version_minor: u8, + rrc_version_major: u8, + #[deku(count = "hdr_len - 4")] // is this right?? + msg: Vec, + }, + #[deku(id = "0x11eb")] + IpTraffic { + #[deku(count = "hdr_len - 8")] // is this right??? + msg: Vec, + }, + #[deku(id = "0x713a")] + UmtsNasOtaMessage { + is_uplink: u8, + length: u32, + #[deku(count = "length")] + msg: Vec, + }, + #[deku(id = "0xb821")] + NrRrcOtaMessage { + #[deku(count = "hdr_len")] + msg: Vec, + } +} + +fn read_lte_rrc_ota_message_log_freq(rest: &BitSlice, ext_header_version: u8) -> Result<(&BitSlice, u32), DekuError> { + if ext_header_version < 8 { + let (rest, freq) = u16::read(rest, ())?; + Ok((rest, freq as u32)) + } else { + let (rest, freq) = u32::read(rest, ())?; + Ok((rest, freq)) + } +} + +fn read_lte_rrc_ota_message_log_msg(rest: &BitSlice) -> Result<(&BitSlice, Vec), DekuError> { + let (mut rest, length) = u16::read(rest, ())?; + if length != rest.len() as u16 / 8 { + let (new_rest, _) = u16::read(rest, ())?; + rest = new_rest; + } + let (new_rest, length) = u16::read(rest, ())?; + rest = new_rest; + if length != rest.len() as u16 / 8 { + return Err(DekuError::Incomplete(NeedSize::new(length as usize * 8))); + } + let mut result = Vec::new(); + for _ in 0..length { + let (new_rest, byte) = u8::read(rest, ())?; + rest = new_rest; + result.push(byte); + } + Ok((rest, result)) +} + +#[derive(Debug, Clone, PartialEq, DekuRead)] #[deku(endian = "little")] pub struct Timestamp { pub ts: u64, @@ -106,14 +211,14 @@ impl Timestamp { } } -#[derive(Debug, Clone, DekuRead)] +#[derive(Debug, Clone, PartialEq, DekuRead)] #[deku(ctx = "opcode: u32, subopcode: u32", id = "opcode")] pub enum ResponsePayload { #[deku(id = "115")] LogConfig(#[deku(ctx = "subopcode")] LogConfigResponse), } -#[derive(Debug, Clone, DekuRead)] +#[derive(Debug, Clone, PartialEq, DekuRead)] #[deku(ctx = "subopcode: u32", id = "subopcode")] pub enum LogConfigResponse { #[deku(id = "1")] @@ -152,9 +257,10 @@ pub fn build_log_mask_request(log_type: u32, log_mask_bitsize: u32, accepted_log #[cfg(test)] mod test { - use crate::diag_device::LOG_CODES_FOR_RAW_PACKET_LOGGING; use super::*; + // Just about all of these test cases from manually parsing diag packets w/ QCSuper + #[test] fn test_request_serialization() { let req = Request::LogConfig(LogConfigRequest::RetrieveIdRanges); @@ -177,17 +283,17 @@ mod test { fn test_build_log_mask_request() { let log_type = 11; let bitsize = 513; - let req = build_log_mask_request(log_type, bitsize, &LOG_CODES_FOR_RAW_PACKET_LOGGING); + let req = build_log_mask_request(log_type, bitsize, &crate::diag_device::LOG_CODES_FOR_RAW_PACKET_LOGGING); assert_eq!(req, Request::LogConfig(LogConfigRequest::SetMask { log_type: log_type, log_mask_bitsize: bitsize, log_mask: vec![ - 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, - 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, - 0x1, 0x0, 0x0, 0x0, 0xc, 0x30, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, - 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, - 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, - 0x0, 0x0, 0x0, 0x0, 0x0, + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x1, 0x0, + 0x0, 0x0, 0xc, 0x30, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, + 0x0, ], })); } @@ -218,18 +324,32 @@ mod test { } #[test] - fn test_message_parsing() { - let msg_bytes = vec![16, 0, 26, 0, 26, 0, 167, 24, 38, 161, 72, 107, 146, 30, 2, 1, 1, 1, 0, 0, 0, 0, 0, 140, 10, 0, 0, 220, 5, 0]; - match Message::from_bytes((msg_bytes.as_slice(), 0)) { - Ok((_, Message::Log { pending_msgs, outer_length, inner_length, log_type, timestamp, payload })) => { - assert_eq!(pending_msgs, 0); - assert_eq!(outer_length, 26); - assert_eq!(inner_length, 26); - assert_eq!(log_type, 6311); - assert_eq!(timestamp.to_datetime().date_naive(), chrono::NaiveDate::from_ymd_opt(2023, 12, 4).unwrap()); - assert_eq!(payload, vec![1, 1, 0, 0, 0, 0, 0, 140, 10, 0, 0, 220, 5, 0]); + fn test_logs() { + env_logger::init(); + let data = vec![ + 16, 0, 38, 0, 38, 0, 192, 176, 26, 165, 245, 135, 118, 35, 2, 1, 20, + 14, 48, 0, 160, 0, 2, 8, 0, 0, 217, 15, 5, 0, 0, 0, 0, 7, 0, 64, 1, + 238, 173, 213, 77, 208 + ]; + let msg = Message::from_bytes((&data, 0)).unwrap().1; + assert_eq!(msg, Message::Log { + pending_msgs: 0, + outer_length: 38, + inner_length: 38, + log_type: 0xb0c0, + timestamp: Timestamp { ts: 72659535985485082 }, + body: LogBody::LteRrcOtaMessage { + ext_header_version: 20, + rrc_rel: 14, + rrc_version: 48, + nc_rrc_rel: None, + bearer_id: 0, + phy_cell_id: 160, + freq: 2050, + sfn: 4057, + channel_type: 5, + msg: vec![0x40, 0x1, 0xee, 0xad, 0xd5, 0x4d, 0xd0], }, - _ => panic!("failed to parse message"), - } + }); } } diff --git a/src/diag_device.rs b/src/diag_device.rs index eb712f5..413adf5 100644 --- a/src/diag_device.rs +++ b/src/diag_device.rs @@ -99,12 +99,13 @@ impl DiagDevice { Ok(data) => match Message::from_bytes((&data, 0)) { Ok(((_, leftover_bytes), res)) => { if leftover_bytes > 0 { - println!("warning: {} leftover bytes when Response", leftover_bytes); + println!("warning: {} leftover bytes when parsing Message", leftover_bytes); } result.push(res); }, Err(e) => { println!("error parsing response: {:?}", e); + println!("{:?}", data); }, }, Err(err) => { diff --git a/src/main.rs b/src/main.rs index 8a63cae..68fbe6d 100644 --- a/src/main.rs +++ b/src/main.rs @@ -5,12 +5,13 @@ mod log_codes; use crate::diag_device::DiagDevice; -fn main() -> std::io::Result<()> { - let mut dev = DiagDevice::new().unwrap(); - dev.config_logs().unwrap(); +fn main() -> diag_device::DiagResult<()> { + env_logger::init(); + let mut dev = DiagDevice::new()?; + dev.config_logs()?; loop { - for msg in dev.read_response().unwrap() { + for msg in dev.read_response()? { println!("msg: {:?}", msg); } } From 9617ec3023649a2a76000c3bb6bd509cb5d56f65 Mon Sep 17 00:00:00 2001 From: Will Greenberg Date: Thu, 7 Dec 2023 16:27:57 -0800 Subject: [PATCH 10/17] add logging crates --- Cargo.lock | 245 ++++++++++++++++++++++++++++++++++++++++++++++++++-- Cargo.toml | 4 +- src/main.rs | 2 + 3 files changed, 241 insertions(+), 10 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index f3caa6b..0077895 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -2,6 +2,15 @@ # It is not intended for manual editing. version = 3 +[[package]] +name = "aho-corasick" +version = "1.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b2969dcb958b36655471fc61f7e416fa76033bdd4bfed0678d8fee1e2d07a1f0" +dependencies = [ + "memchr", +] + [[package]] name = "android-tzdata" version = "0.1.1" @@ -23,6 +32,12 @@ version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d468802bab17cbc0cc575e9b053f41e72aa36bfa6b7f55e3529ffa43161b97fa" +[[package]] +name = "bitflags" +version = "2.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "327762f6e5a765692301e5bb513e0d9fef63be86bbc14528052b1cd3e6f03e07" + [[package]] name = "bitvec" version = "1.0.1" @@ -88,7 +103,7 @@ dependencies = [ "js-sys", "num-traits", "wasm-bindgen", - "windows-targets", + "windows-targets 0.48.5", ] [[package]] @@ -155,6 +170,7 @@ checksum = "819b87cc7a05b3abe3fc38e59b3980a5fd3162f25a247116441a9171d3e84481" dependencies = [ "bitvec", "deku_derive", + "log", ] [[package]] @@ -189,17 +205,42 @@ dependencies = [ "chrono", "crc", "deku", + "env_logger", "libc", + "log", "pcap-file", "thiserror", ] +[[package]] +name = "env_logger" +version = "0.10.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "95b3f3e67048839cb0d0781f445682a35113da7121f7c949db0e2be96a4fbece" +dependencies = [ + "humantime", + "is-terminal", + "log", + "regex", + "termcolor", +] + [[package]] name = "equivalent" version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5443807d6dff69373d433ab9ef5378ad8df50ca6298caf15de6e52e24aaf54d5" +[[package]] +name = "errno" +version = "0.3.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a258e46cdc063eb8519c00b9fc845fc47bcfca4130e2f08e88665ceda8474245" +dependencies = [ + "libc", + "windows-sys 0.52.0", +] + [[package]] name = "fnv" version = "1.0.7" @@ -218,6 +259,18 @@ version = "0.14.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "290f1a1d9242c78d09ce40a5e87e7554ee637af1351968159f4952f028f75604" +[[package]] +name = "hermit-abi" +version = "0.3.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d77f7ec81a6d05a3abb01ab6eb7590f6083d08449fe5a1c8b1e620283546ccb7" + +[[package]] +name = "humantime" +version = "2.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a3a5bfb195931eeb336b2a7b4d761daec841b97f947d34394601737a7bba5e4" + [[package]] name = "iana-time-zone" version = "0.1.58" @@ -257,6 +310,17 @@ dependencies = [ "hashbrown", ] +[[package]] +name = "is-terminal" +version = "0.4.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cb0889898416213fab133e1d33a0e5858a48177452750691bde3666d0fdbaf8b" +dependencies = [ + "hermit-abi", + "rustix", + "windows-sys 0.48.0", +] + [[package]] name = "js-sys" version = "0.3.66" @@ -272,6 +336,12 @@ version = "0.2.150" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "89d92a4743f9a61002fae18374ed11e7973f530cb3a3255fb354818118b2203c" +[[package]] +name = "linux-raw-sys" +version = "0.4.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c4cd1a83af159aa67994778be9070f0ae1bd732942279cabb14f86f986a21456" + [[package]] name = "log" version = "0.4.20" @@ -344,6 +414,48 @@ version = "0.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "dc33ff2d4973d518d823d61aa239014831e521c75da58e3df4840d3f47749d09" +[[package]] +name = "regex" +version = "1.10.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "380b951a9c5e80ddfd6136919eef32310721aa4aacd4889a8d39124b026ab343" +dependencies = [ + "aho-corasick", + "memchr", + "regex-automata", + "regex-syntax", +] + +[[package]] +name = "regex-automata" +version = "0.4.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5f804c7828047e88b2d32e2d7fe5a105da8ee3264f01902f796c8e067dc2483f" +dependencies = [ + "aho-corasick", + "memchr", + "regex-syntax", +] + +[[package]] +name = "regex-syntax" +version = "0.8.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c08c74e62047bb2de4ff487b251e4a92e24f48745648451635cec7d591162d9f" + +[[package]] +name = "rustix" +version = "0.38.26" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9470c4bf8246c8daf25f9598dca807fb6510347b1e1cfa55749113850c79d88a" +dependencies = [ + "bitflags", + "errno", + "libc", + "linux-raw-sys", + "windows-sys 0.52.0", +] + [[package]] name = "strsim" version = "0.10.0" @@ -378,6 +490,15 @@ version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "55937e1799185b12863d447f42597ed69d9928686b8d88a1df17376a097d8369" +[[package]] +name = "termcolor" +version = "1.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ff1bc3d3f05aff0403e8ac0d92ced918ec05b666a43f83297ccef5bea8a3d449" +dependencies = [ + "winapi-util", +] + [[package]] name = "thiserror" version = "1.0.50" @@ -475,13 +596,62 @@ version = "0.2.89" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7ab9b36309365056cd639da3134bf87fa8f3d86008abf99e612384a6eecd459f" +[[package]] +name = "winapi" +version = "0.3.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5c839a674fcd7a98952e593242ea400abe93992746761e38641405d28b00f419" +dependencies = [ + "winapi-i686-pc-windows-gnu", + "winapi-x86_64-pc-windows-gnu", +] + +[[package]] +name = "winapi-i686-pc-windows-gnu" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" + +[[package]] +name = "winapi-util" +version = "0.1.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f29e6f9198ba0d26b4c9f07dbe6f9ed633e1f3d5b8b414090084349e46a52596" +dependencies = [ + "winapi", +] + +[[package]] +name = "winapi-x86_64-pc-windows-gnu" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" + [[package]] name = "windows-core" version = "0.51.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f1f8cf84f35d2db49a46868f947758c7a1138116f7fac3bc844f43ade1292e64" dependencies = [ - "windows-targets", + "windows-targets 0.48.5", +] + +[[package]] +name = "windows-sys" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "677d2418bec65e3338edb076e806bc1ec15693c5d0104683f2efe857f61056a9" +dependencies = [ + "windows-targets 0.48.5", +] + +[[package]] +name = "windows-sys" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "282be5f36a8ce781fad8c8ae18fa3f9beff57ec1b52cb3de0789201425d9a33d" +dependencies = [ + "windows-targets 0.52.0", ] [[package]] @@ -490,13 +660,28 @@ version = "0.48.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9a2fa6e2155d7247be68c096456083145c183cbbbc2764150dda45a87197940c" dependencies = [ - "windows_aarch64_gnullvm", - "windows_aarch64_msvc", - "windows_i686_gnu", - "windows_i686_msvc", - "windows_x86_64_gnu", - "windows_x86_64_gnullvm", - "windows_x86_64_msvc", + "windows_aarch64_gnullvm 0.48.5", + "windows_aarch64_msvc 0.48.5", + "windows_i686_gnu 0.48.5", + "windows_i686_msvc 0.48.5", + "windows_x86_64_gnu 0.48.5", + "windows_x86_64_gnullvm 0.48.5", + "windows_x86_64_msvc 0.48.5", +] + +[[package]] +name = "windows-targets" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8a18201040b24831fbb9e4eb208f8892e1f50a37feb53cc7ff887feb8f50e7cd" +dependencies = [ + "windows_aarch64_gnullvm 0.52.0", + "windows_aarch64_msvc 0.52.0", + "windows_i686_gnu 0.52.0", + "windows_i686_msvc 0.52.0", + "windows_x86_64_gnu 0.52.0", + "windows_x86_64_gnullvm 0.52.0", + "windows_x86_64_msvc 0.52.0", ] [[package]] @@ -505,42 +690,84 @@ version = "0.48.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2b38e32f0abccf9987a4e3079dfb67dcd799fb61361e53e2882c3cbaf0d905d8" +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cb7764e35d4db8a7921e09562a0304bf2f93e0a51bfccee0bd0bb0b666b015ea" + [[package]] name = "windows_aarch64_msvc" version = "0.48.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "dc35310971f3b2dbbf3f0690a219f40e2d9afcf64f9ab7cc1be722937c26b4bc" +[[package]] +name = "windows_aarch64_msvc" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bbaa0368d4f1d2aaefc55b6fcfee13f41544ddf36801e793edbbfd7d7df075ef" + [[package]] name = "windows_i686_gnu" version = "0.48.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a75915e7def60c94dcef72200b9a8e58e5091744960da64ec734a6c6e9b3743e" +[[package]] +name = "windows_i686_gnu" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a28637cb1fa3560a16915793afb20081aba2c92ee8af57b4d5f28e4b3e7df313" + [[package]] name = "windows_i686_msvc" version = "0.48.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8f55c233f70c4b27f66c523580f78f1004e8b5a8b659e05a4eb49d4166cca406" +[[package]] +name = "windows_i686_msvc" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ffe5e8e31046ce6230cc7215707b816e339ff4d4d67c65dffa206fd0f7aa7b9a" + [[package]] name = "windows_x86_64_gnu" version = "0.48.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "53d40abd2583d23e4718fddf1ebec84dbff8381c07cae67ff7768bbf19c6718e" +[[package]] +name = "windows_x86_64_gnu" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3d6fa32db2bc4a2f5abeacf2b69f7992cd09dca97498da74a151a3132c26befd" + [[package]] name = "windows_x86_64_gnullvm" version = "0.48.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0b7b52767868a23d5bab768e390dc5f5c55825b6d30b86c844ff2dc7414044cc" +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1a657e1e9d3f514745a572a6846d3c7aa7dbe1658c056ed9c3344c4109a6949e" + [[package]] name = "windows_x86_64_msvc" version = "0.48.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ed94fce61571a4006852b7389a063ab983c02eb1bb37b47f8272ce92d06d9538" +[[package]] +name = "windows_x86_64_msvc" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dff9641d1cd4be8d1a070daf9e3773c5f67e78b4d9d42263020c057706765c04" + [[package]] name = "winnow" version = "0.5.19" diff --git a/Cargo.toml b/Cargo.toml index 823290a..38eabfd 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -9,8 +9,10 @@ edition = "2021" bytes = "1.5.0" chrono = "0.4.31" crc = "3.0.1" -deku = "0.16.0" +deku = { version = "0.16.0", features = ["logging"] } +env_logger = "0.10.1" libc = "0.2.150" +log = "0.4.20" pcap-file = "2.0.0" thiserror = "1.0.50" diff --git a/src/main.rs b/src/main.rs index 68fbe6d..b7da4fc 100644 --- a/src/main.rs +++ b/src/main.rs @@ -6,7 +6,9 @@ mod log_codes; use crate::diag_device::DiagDevice; fn main() -> diag_device::DiagResult<()> { + // this should eventually be removed for prod env_logger::init(); + let mut dev = DiagDevice::new()?; dev.config_logs()?; From 470a263593dfc2b96a9e8ea6ca416d5936e8915e Mon Sep 17 00:00:00 2001 From: Will Greenberg Date: Thu, 7 Dec 2023 16:29:17 -0800 Subject: [PATCH 11/17] better log parsing --- src/diag.rs | 158 ++++++++++++++++++++++++++++++++++++---------------- 1 file changed, 110 insertions(+), 48 deletions(-) diff --git a/src/diag.rs b/src/diag.rs index 3379fd5..15bf77b 100644 --- a/src/diag.rs +++ b/src/diag.rs @@ -115,23 +115,10 @@ pub enum LogBody { msg: Vec, }, #[deku(id = "0xb0c0")] - LteRrcOtaMessage { + LteRrcOtaMessage{ ext_header_version: u8, - rrc_rel: u8, - rrc_version: u8, - #[deku(skip, cond = "*ext_header_version < 25")] // handle post-NR releases - nc_rrc_rel: Option, - bearer_id: u8, - phy_cell_id: u16, - - // extended header. some of these fields need manual parsing based on - // header version - #[deku(reader = "read_lte_rrc_ota_message_log_freq(deku::rest, *ext_header_version)")] - freq: u32, - sfn: u16, - channel_type: u8, - #[deku(reader = "read_lte_rrc_ota_message_log_msg(deku::rest)")] - msg: Vec, + #[deku(ctx = "*ext_header_version")] + packet: LteRrcOtaPacket, }, #[deku(id_pat = "0xb0e2 | 0xb0e3 | 0xb0ec | 0xb0ed")] Nas4GMessage { @@ -161,34 +148,106 @@ pub enum LogBody { } } -fn read_lte_rrc_ota_message_log_freq(rest: &BitSlice, ext_header_version: u8) -> Result<(&BitSlice, u32), DekuError> { - if ext_header_version < 8 { - let (rest, freq) = u16::read(rest, ())?; - Ok((rest, freq as u32)) - } else { - let (rest, freq) = u32::read(rest, ())?; - Ok((rest, freq)) - } +/* + item_struct = namedtuple('QcDiagLteRrcOtaPacket', 'rrc_rel_maj rrc_rel_min rbid pci earfcn sfn_subfn pdu_num len') + item_struct_v5 = namedtuple('QcDiagLteRrcOtaPacketV5', 'rrc_rel_maj rrc_rel_min rbid pci earfcn sfn_subfn pdu_num sib_mask len') + item_struct_v25 = namedtuple('QcDiagLteRrcOtaPacketV25', 'rrc_rel_maj rrc_rel_min nr_rrc_rel_maj nr_rrc_rel_min rbid pci earfcn sfn_subfn pdu_num sib_mask len') + if pkt_version >= 25: + # Version 25, 26, 27 + item = item_struct_v25._make(struct.unpack('= 8: + # Version 8, 9, 12, 13, 15, 16, 19, 20, 22, 24 + item = item_struct_v5._make(struct.unpack('= 5: + # Version 6, 7 + item = item_struct_v5._make(struct.unpack(', + }, + #[deku(id_pat = "5..=7")] + V5 { + rrc_rel_maj: u8, + rrc_rel_min: u8, + bearer_id: u8, + phy_cell_id: u16, + earfcn: u16, + sfn_subfn: u16, + pdu_num: u8, + sib_mask: u32, + len: u16, + #[deku(count = "len")] + packet: Vec, + }, + #[deku(id_pat = "8..=24")] + V8 { + rrc_rel_maj: u8, + rrc_rel_min: u8, + bearer_id: u8, + phy_cell_id: u16, + earfcn: u32, + sfn_subfn: u16, + pdu_num: u8, + sib_mask: u32, + len: u16, + #[deku(count = "len")] + packet: Vec, + }, + #[deku(id_pat = "25..")] + V25 { + rrc_rel_maj: u8, + rrc_rel_min: u8, + nr_rrc_rel_maj: u8, + nr_rrc_rel_min: u8, + bearer_id: u8, + phy_cell_id: u16, + earfcn: u32, + sfn_subfn: u16, + pdu_num: u8, + sib_mask: u32, + len: u16, + #[deku(count = "len")] + packet: Vec, + }, } -fn read_lte_rrc_ota_message_log_msg(rest: &BitSlice) -> Result<(&BitSlice, Vec), DekuError> { - let (mut rest, length) = u16::read(rest, ())?; - if length != rest.len() as u16 / 8 { - let (new_rest, _) = u16::read(rest, ())?; - rest = new_rest; +impl LteRrcOtaPacket { + fn get_sfn_subfn(&self) -> u16 { + match self { + LteRrcOtaPacket::V0 { sfn_subfn, .. } => *sfn_subfn, + LteRrcOtaPacket::V5 { sfn_subfn, .. } => *sfn_subfn, + LteRrcOtaPacket::V8 { sfn_subfn, .. } => *sfn_subfn, + LteRrcOtaPacket::V25 { sfn_subfn, .. } => *sfn_subfn, + } } - let (new_rest, length) = u16::read(rest, ())?; - rest = new_rest; - if length != rest.len() as u16 / 8 { - return Err(DekuError::Incomplete(NeedSize::new(length as usize * 8))); + pub fn get_sfn(&self) -> u16 { + self.get_sfn_subfn() >> 4 } - let mut result = Vec::new(); - for _ in 0..length { - let (new_rest, byte) = u8::read(rest, ())?; - rest = new_rest; - result.push(byte); + + pub fn get_subfn(&self) -> u16 { + self.get_sfn_subfn() & 0xf } - Ok((rest, result)) } #[derive(Debug, Clone, PartialEq, DekuRead)] @@ -340,15 +399,18 @@ mod test { timestamp: Timestamp { ts: 72659535985485082 }, body: LogBody::LteRrcOtaMessage { ext_header_version: 20, - rrc_rel: 14, - rrc_version: 48, - nc_rrc_rel: None, - bearer_id: 0, - phy_cell_id: 160, - freq: 2050, - sfn: 4057, - channel_type: 5, - msg: vec![0x40, 0x1, 0xee, 0xad, 0xd5, 0x4d, 0xd0], + packet: LteRrcOtaPacket::V8 { + rrc_rel_maj: 14, + rrc_rel_min: 48, + bearer_id: 0, + phy_cell_id: 160, + earfcn: 2050, + sfn_subfn: 4057, + pdu_num: 5, + sib_mask: 0, + len: 7, + packet: vec![0x40, 0x1, 0xee, 0xad, 0xd5, 0x4d, 0xd0], + }, }, }); } From 76555fe68c2da013d761b22da6df35bf27cc8154 Mon Sep 17 00:00:00 2001 From: Will Greenberg Date: Thu, 7 Dec 2023 17:20:22 -0800 Subject: [PATCH 12/17] make hdlc operations work on slices --- src/diag_device.rs | 5 +++-- src/hdlc.rs | 21 +++++++++++---------- 2 files changed, 14 insertions(+), 12 deletions(-) diff --git a/src/diag_device.rs b/src/diag_device.rs index 413adf5..42e9f83 100644 --- a/src/diag_device.rs +++ b/src/diag_device.rs @@ -95,7 +95,7 @@ impl DiagDevice { fn parse_response_container(&self, container: MessagesContainer) -> DiagResult> { let mut result = Vec::new(); for msg in container.messages { - match hdlc_decapsulate(msg.data, &self.crc) { + match hdlc_decapsulate(&msg.data, &self.crc) { Ok(data) => match Message::from_bytes((&data, 0)) { Ok(((_, leftover_bytes), res)) => { if leftover_bytes > 0 { @@ -110,6 +110,7 @@ impl DiagDevice { }, Err(err) => { println!("error decapsulating response: {:?}", err); + println!("{:?}", &msg.data); } } } @@ -138,7 +139,7 @@ impl DiagDevice { data_type: DataType::UserSpace, use_mdm: self.use_mdm > 0, mdm_field: -1, - hdlc_encapsulated_request: hdlc_encapsulate(req.to_bytes().unwrap(), &self.crc), + hdlc_encapsulated_request: hdlc_encapsulate(&req.to_bytes().unwrap(), &self.crc), }.to_bytes().unwrap(); unsafe { let fd = self.file.as_raw_fd(); diff --git a/src/hdlc.rs b/src/hdlc.rs index 51861dd..7c9ab3c 100644 --- a/src/hdlc.rs +++ b/src/hdlc.rs @@ -21,35 +21,36 @@ pub enum HdlcError { TooShort, } -pub fn hdlc_encapsulate(mut data: Vec, crc: &Crc) -> Vec { - data.put_u16_le(crc.checksum(&data)); +pub fn hdlc_encapsulate(data: &[u8], crc: &Crc) -> Vec { + let mut result: Vec = vec![]; + result.put_u16_le(crc.checksum(&data)); - let mut result: Vec = data.iter() + let escaped = data.iter() .flat_map(|&b| match b { // TODO: is this too expensive? 0x7e => vec![0x7d, 0x5e], 0x7d => vec![0x7d, 0x5d], _ => vec![b], - }) - .collect(); + }); + result.extend(escaped); result.push(0x7e); result } -pub fn hdlc_decapsulate(mut data: Vec, crc: &Crc) -> Result, HdlcError> { +pub fn hdlc_decapsulate(data: &[u8], crc: &Crc) -> Result, HdlcError> { // TODO: return errors instead of panicking if data.len() < 3 { return Err(HdlcError::TooShort); } - let last_char = data.pop().unwrap(); // safe since len() >= 3 + let last_char = data[data.len() - 1]; // safe since len() >= 3 if last_char != 0x7e { return Err(HdlcError::NoTrailingCharacter(last_char)); } let mut unescaped = Vec::new(); let mut escaping = false; - for b in data { + for &b in data { if escaping { match b { 0x5e => unescaped.push(0x7e), @@ -84,8 +85,8 @@ mod tests { let crc = Crc::::new(&crate::diag_device::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); + let encapsulated = hdlc_encapsulate(&data, &crc); assert_eq!(&encapsulated, &expected); - assert_eq!(hdlc_decapsulate(encapsulated, &crc), Ok(data)); + assert_eq!(hdlc_decapsulate(&encapsulated, &crc), Ok(data)); } } From 10bafc3faae714feff756efbed957673ff08beeb Mon Sep 17 00:00:00 2001 From: Will Greenberg Date: Thu, 7 Dec 2023 18:49:39 -0800 Subject: [PATCH 13/17] fix hdlc ops --- src/hdlc.rs | 32 +++++++++++++++++++------------- 1 file changed, 19 insertions(+), 13 deletions(-) diff --git a/src/hdlc.rs b/src/hdlc.rs index 7c9ab3c..1d8b9d5 100644 --- a/src/hdlc.rs +++ b/src/hdlc.rs @@ -23,16 +23,23 @@ pub enum HdlcError { pub fn hdlc_encapsulate(data: &[u8], crc: &Crc) -> Vec { let mut result: Vec = vec![]; - result.put_u16_le(crc.checksum(&data)); - let escaped = data.iter() - .flat_map(|&b| match b { - // TODO: is this too expensive? - 0x7e => vec![0x7d, 0x5e], - 0x7d => vec![0x7d, 0x5d], - _ => vec![b], - }); - result.extend(escaped); + for &b in data { + match b { + 0x7e => result.extend([0x7d, 0x5e]), + 0x7d => result.extend([0x7d, 0x5d]), + _ => result.push(b), + } + } + + for b in crc.checksum(&data).to_le_bytes() { + match b { + 0x7e => result.extend([0x7d, 0x5e]), + 0x7d => result.extend([0x7d, 0x5d]), + _ => result.push(b), + } + } + result.push(0x7e); result } @@ -43,14 +50,13 @@ pub fn hdlc_decapsulate(data: &[u8], crc: &Crc) -> Result, HdlcErro return Err(HdlcError::TooShort); } - let last_char = data[data.len() - 1]; // safe since len() >= 3 - if last_char != 0x7e { - return Err(HdlcError::NoTrailingCharacter(last_char)); + if data[data.len() - 1] != 0x7e { + return Err(HdlcError::NoTrailingCharacter(data[data.len() - 1])); } let mut unescaped = Vec::new(); let mut escaping = false; - for &b in data { + for &b in &data[..data.len() - 1] { if escaping { match b { 0x5e => unescaped.push(0x7e), From 978901b233a56f5b3073e7aedd15d2497d5ed7a2 Mon Sep 17 00:00:00 2001 From: Will Greenberg Date: Fri, 8 Dec 2023 16:38:23 -0800 Subject: [PATCH 14/17] fix packet reading and leftover bytes checks --- src/diag.rs | 24 +----------------------- src/diag_device.rs | 25 ++++++++++++++++--------- src/hdlc.rs | 3 +-- 3 files changed, 18 insertions(+), 34 deletions(-) diff --git a/src/diag.rs b/src/diag.rs index 15bf77b..b35b8a0 100644 --- a/src/diag.rs +++ b/src/diag.rs @@ -1,7 +1,7 @@ //! Diag protocol serialization/deserialization use chrono::{DateTime, FixedOffset}; -use deku::{prelude::*, bitvec::{BitSlice, Msb0}}; +use deku::prelude::*; #[derive(Debug, Clone, DekuWrite)] pub struct RequestContainer { @@ -148,28 +148,6 @@ pub enum LogBody { } } -/* - item_struct = namedtuple('QcDiagLteRrcOtaPacket', 'rrc_rel_maj rrc_rel_min rbid pci earfcn sfn_subfn pdu_num len') - item_struct_v5 = namedtuple('QcDiagLteRrcOtaPacketV5', 'rrc_rel_maj rrc_rel_min rbid pci earfcn sfn_subfn pdu_num sib_mask len') - item_struct_v25 = namedtuple('QcDiagLteRrcOtaPacketV25', 'rrc_rel_maj rrc_rel_min nr_rrc_rel_maj nr_rrc_rel_min rbid pci earfcn sfn_subfn pdu_num sib_mask len') - if pkt_version >= 25: - # Version 25, 26, 27 - item = item_struct_v25._make(struct.unpack('= 8: - # Version 8, 9, 12, 13, 15, 16, 19, 20, 22, 24 - item = item_struct_v5._make(struct.unpack('= 5: - # Version 6, 7 - item = item_struct_v5._make(struct.unpack(' match Message::from_bytes((&data, 0)) { - Ok(((_, leftover_bytes), res)) => { - if leftover_bytes > 0 { - println!("warning: {} leftover bytes when parsing Message", leftover_bytes); + Ok(((leftover_bytes, _), res)) => { + if leftover_bytes.len() > 0 { + println!("warning: {} leftover bytes when parsing Message", leftover_bytes.len()); } result.push(res); }, @@ -118,13 +118,20 @@ impl DiagDevice { } pub fn read_response(&mut self) -> DiagResult> { - let mut buf = vec![0; BUFFER_LEN]; + let mut packet_buf = vec![0; BUFFER_LEN]; loop { - let _ = self.file.read(&mut buf)?; - let ((_, leftover_bytes), res_container) = MessagesContainer::from_bytes((&buf, 0))?; - if leftover_bytes > 0 { - println!("warning: {} leftover bytes when parsing ResponseContainer", leftover_bytes); + let mut packet = vec![]; + while !packet.ends_with(&[0x7e]) { + let bytes_read = self.file.read(&mut packet_buf)?; + packet.extend(&packet_buf[0..bytes_read]); + // clear out the buffer so we don't accidentally read stale data + packet_buf.clear(); + packet_buf.resize(BUFFER_LEN, 0); + } + let ((leftover_bytes, _), res_container) = MessagesContainer::from_bytes((&packet, 0))?; + if leftover_bytes.len() > 0 { + println!("warning: {} leftover bytes when parsing ResponseContainer", leftover_bytes.len()); } if res_container.data_type == DataType::UserSpace { return self.parse_response_container(res_container); diff --git a/src/hdlc.rs b/src/hdlc.rs index 1d8b9d5..78dc663 100644 --- a/src/hdlc.rs +++ b/src/hdlc.rs @@ -4,7 +4,7 @@ //! https://github.com/P1sec/QCSuper/blob/master/docs/The%20Diag%20protocol.md#the-diag-protocol-over-usb use crc::Crc; -use bytes::{Buf, BufMut}; +use bytes::Buf; use thiserror::Error; #[derive(Debug, Error, PartialEq)] @@ -45,7 +45,6 @@ pub fn hdlc_encapsulate(data: &[u8], crc: &Crc) -> Vec { } pub fn hdlc_decapsulate(data: &[u8], crc: &Crc) -> Result, HdlcError> { - // TODO: return errors instead of panicking if data.len() < 3 { return Err(HdlcError::TooShort); } From e3e50b95e57a70d54c88aace35d90fa26845fa03 Mon Sep 17 00:00:00 2001 From: Will Greenberg Date: Fri, 8 Dec 2023 17:51:13 -0800 Subject: [PATCH 15/17] simplify packet reading a bit --- src/diag_device.rs | 39 +++++++++++++++++++++------------------ src/main.rs | 6 +++++- 2 files changed, 26 insertions(+), 19 deletions(-) diff --git a/src/diag_device.rs b/src/diag_device.rs index b57ed46..591f947 100644 --- a/src/diag_device.rs +++ b/src/diag_device.rs @@ -68,24 +68,22 @@ const MEMORY_DEVICE_MODE: i32 = 2; const DIAG_IOCTL_REMOTE_DEV: u32 = 32; const DIAG_IOCTL_SWITCH_LOGGING: u32 = 7; -pub struct DiagDevice { - file: File, +pub struct DiagDevice<'a> { + file: &'a File, + accumulator: Vec, use_mdm: i32, crc: Crc, } -impl DiagDevice { - pub fn new() -> DiagResult { - let file = File::options() - .read(true) - .write(true) - .open("/dev/diag")?; +impl<'a> DiagDevice<'a> { + pub fn new(file: &'a File) -> DiagResult { let fd = file.as_raw_fd(); enable_frame_readwrite(fd, MEMORY_DEVICE_MODE)?; let use_mdm = determine_use_mdm(fd)?; Ok(DiagDevice { + accumulator: vec![], file, crc: Crc::::new(&CRC_CCITT_ALG), use_mdm, @@ -117,18 +115,23 @@ impl DiagDevice { Ok(result) } - pub fn read_response(&mut self) -> DiagResult> { - let mut packet_buf = vec![0; BUFFER_LEN]; + fn next_packet(&mut self) -> DiagResult> { + let mut read_buf = vec![0; BUFFER_LEN]; + while !self.accumulator.contains(&0x7e) { + let bytes_read = self.file.read(&mut read_buf).unwrap(); + self.accumulator.extend(&read_buf[0..bytes_read]); + // clear out the buffer so we don't accidentally read stale data + read_buf.clear(); + read_buf.resize(BUFFER_LEN, 0); + } + let idx = self.accumulator.iter().position(|&x| x == 0x7e).unwrap(); + Ok(self.accumulator.drain(0..idx + 1).collect::>()) + } + + pub fn read_response(&mut self) -> DiagResult> { loop { - let mut packet = vec![]; - while !packet.ends_with(&[0x7e]) { - let bytes_read = self.file.read(&mut packet_buf)?; - packet.extend(&packet_buf[0..bytes_read]); - // clear out the buffer so we don't accidentally read stale data - packet_buf.clear(); - packet_buf.resize(BUFFER_LEN, 0); - } + let packet = self.next_packet()?; let ((leftover_bytes, _), res_container) = MessagesContainer::from_bytes((&packet, 0))?; if leftover_bytes.len() > 0 { println!("warning: {} leftover bytes when parsing ResponseContainer", leftover_bytes.len()); diff --git a/src/main.rs b/src/main.rs index b7da4fc..1d56fa4 100644 --- a/src/main.rs +++ b/src/main.rs @@ -9,7 +9,11 @@ fn main() -> diag_device::DiagResult<()> { // this should eventually be removed for prod env_logger::init(); - let mut dev = DiagDevice::new()?; + let file = std::fs::File::options() + .read(true) + .write(true) + .open("/dev/diag")?; + let mut dev = DiagDevice::new(&file)?; dev.config_logs()?; loop { From 2e4d54928d2443d7ece3e9750dd126a43772dc27 Mon Sep 17 00:00:00 2001 From: Will Greenberg Date: Tue, 12 Dec 2023 13:59:03 -0800 Subject: [PATCH 16/17] maybe fix read loop? --- src/diag.rs | 12 +++++++++--- src/diag_device.rs | 23 +++++------------------ 2 files changed, 14 insertions(+), 21 deletions(-) diff --git a/src/diag.rs b/src/diag.rs index b35b8a0..4eb19c3 100644 --- a/src/diag.rs +++ b/src/diag.rs @@ -46,8 +46,8 @@ pub enum DataType { #[derive(Debug, Clone, DekuRead)] pub struct MessagesContainer { pub data_type: DataType, - pub num_responses: u32, - #[deku(count = "num_responses")] + pub num_messages: u32, + #[deku(count = "num_messages")] pub messages: Vec, } @@ -362,7 +362,6 @@ mod test { #[test] fn test_logs() { - env_logger::init(); let data = vec![ 16, 0, 38, 0, 38, 0, 192, 176, 26, 165, 245, 135, 118, 35, 2, 1, 20, 14, 48, 0, 160, 0, 2, 8, 0, 0, 217, 15, 5, 0, 0, 0, 0, 7, 0, 64, 1, @@ -392,4 +391,11 @@ mod test { }, }); } + + #[test] + fn test_fuck() { + env_logger::init(); + let data = vec![32, 0, 0, 0, 1, 0, 0, 0, 122, 1, 0, 0, 16, 0, 38, 0, 38, 0, 192, 176, 153, 128, 249, 211, 218, 62, 2, 1, 20, 14, 48, 0, 160, 0, 14, 6, 1, 0, 217, 39, 5, 0, 0, 0, 0, 7, 0, 64, 2, 14, 213, 72, 153, 192, 168, 216, 126]; + dbg!(MessagesContainer::from_bytes((&data, 0)).unwrap()); + } } diff --git a/src/diag_device.rs b/src/diag_device.rs index 591f947..e6ad5b2 100644 --- a/src/diag_device.rs +++ b/src/diag_device.rs @@ -63,7 +63,7 @@ pub const LOG_CODES_FOR_RAW_PACKET_LOGGING: [u32; 11] = [ log_codes::LOG_DATA_PROTOCOL_LOGGING_C // 0x11eb ]; -const BUFFER_LEN: usize = 1024 * 10; +const BUFFER_LEN: usize = 1024 * 1024 * 10; const MEMORY_DEVICE_MODE: i32 = 2; const DIAG_IOCTL_REMOTE_DEV: u32 = 32; const DIAG_IOCTL_SWITCH_LOGGING: u32 = 7; @@ -115,26 +115,13 @@ impl<'a> DiagDevice<'a> { Ok(result) } - fn next_packet(&mut self) -> DiagResult> { - let mut read_buf = vec![0; BUFFER_LEN]; - while !self.accumulator.contains(&0x7e) { - let bytes_read = self.file.read(&mut read_buf).unwrap(); - self.accumulator.extend(&read_buf[0..bytes_read]); - // clear out the buffer so we don't accidentally read stale data - read_buf.clear(); - read_buf.resize(BUFFER_LEN, 0); - } - - let idx = self.accumulator.iter().position(|&x| x == 0x7e).unwrap(); - Ok(self.accumulator.drain(0..idx + 1).collect::>()) - } - pub fn read_response(&mut self) -> DiagResult> { + let mut read_buf = vec![0; BUFFER_LEN]; loop { - let packet = self.next_packet()?; - let ((leftover_bytes, _), res_container) = MessagesContainer::from_bytes((&packet, 0))?; + let bytes_read = self.file.read(&mut read_buf).unwrap(); + let ((leftover_bytes, _), res_container) = MessagesContainer::from_bytes((&read_buf[0..bytes_read], 0))?; if leftover_bytes.len() > 0 { - println!("warning: {} leftover bytes when parsing ResponseContainer", leftover_bytes.len()); + println!("warning: {} leftover bytes when parsing MessagesContainer", leftover_bytes.len()); } if res_container.data_type == DataType::UserSpace { return self.parse_response_container(res_container); From dffa76792e646785aed2c03b712652c3f121adbe Mon Sep 17 00:00:00 2001 From: Will Greenberg Date: Tue, 12 Dec 2023 15:03:12 -0800 Subject: [PATCH 17/17] yet more read loop fixes --- src/diag_device.rs | 41 +++++++++++++++++++++-------------------- 1 file changed, 21 insertions(+), 20 deletions(-) diff --git a/src/diag_device.rs b/src/diag_device.rs index e6ad5b2..9989fd3 100644 --- a/src/diag_device.rs +++ b/src/diag_device.rs @@ -70,7 +70,7 @@ const DIAG_IOCTL_SWITCH_LOGGING: u32 = 7; pub struct DiagDevice<'a> { file: &'a File, - accumulator: Vec, + read_buf: Vec, use_mdm: i32, crc: Crc, } @@ -83,7 +83,7 @@ impl<'a> DiagDevice<'a> { let use_mdm = determine_use_mdm(fd)?; Ok(DiagDevice { - accumulator: vec![], + read_buf: vec![0; BUFFER_LEN], file, crc: Crc::::new(&CRC_CCITT_ALG), use_mdm, @@ -93,22 +93,24 @@ impl<'a> DiagDevice<'a> { fn parse_response_container(&self, container: MessagesContainer) -> DiagResult> { let mut result = Vec::new(); for msg in container.messages { - match hdlc_decapsulate(&msg.data, &self.crc) { - Ok(data) => match Message::from_bytes((&data, 0)) { - Ok(((leftover_bytes, _), res)) => { - if leftover_bytes.len() > 0 { - println!("warning: {} leftover bytes when parsing Message", leftover_bytes.len()); - } - result.push(res); + for sub_msg in msg.data.split_inclusive(|&b| b == 0x7e) { + match hdlc_decapsulate(&sub_msg, &self.crc) { + Ok(data) => match Message::from_bytes((&data, 0)) { + Ok(((leftover_bytes, _), res)) => { + if leftover_bytes.len() > 0 { + println!("warning: {} leftover bytes when parsing Message", leftover_bytes.len()); + } + result.push(res); + }, + Err(e) => { + println!("error parsing response: {:?}", e); + println!("{:?}", data); + }, }, - Err(e) => { - println!("error parsing response: {:?}", e); - println!("{:?}", data); - }, - }, - Err(err) => { - println!("error decapsulating response: {:?}", err); - println!("{:?}", &msg.data); + Err(err) => { + println!("error decapsulating response: {:?}", err); + println!("{:?}", &sub_msg); + } } } } @@ -116,10 +118,9 @@ impl<'a> DiagDevice<'a> { } pub fn read_response(&mut self) -> DiagResult> { - let mut read_buf = vec![0; BUFFER_LEN]; loop { - let bytes_read = self.file.read(&mut read_buf).unwrap(); - let ((leftover_bytes, _), res_container) = MessagesContainer::from_bytes((&read_buf[0..bytes_read], 0))?; + let bytes_read = self.file.read(&mut self.read_buf).unwrap(); + let ((leftover_bytes, _), res_container) = MessagesContainer::from_bytes((&self.read_buf[0..bytes_read], 0))?; if leftover_bytes.len() > 0 { println!("warning: {} leftover bytes when parsing MessagesContainer", leftover_bytes.len()); }