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); } } }