Mirror/rayhunter
1
0
Fork 0
mirror of https://github.com/EFForg/rayhunter.git synced 2026-05-05 11:29:08 -07:00
Code Issues Packages Projects Releases Wiki Activity

Mostly working now, except for log parsing and random checksum failures

Browse Source
This commit is contained in:
Will Greenberg
2023-12-04 15:38:01 -08:00
parent 7d55716104
commit db18abae6f
4 changed files with 316 additions and 220 deletions
Download Patch File Download Diff File Expand all files Collapse all files

264
src/diag.rs
View File

@@ -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<u16> = Algorithm {
poly: 0x1021,
init: 0xffff,
refin: true,
refout: true,
width: 16,
xorout: 0xffff,
check: 0x2189,
residue: 0x0000,
};
pub type DiagResult<T> = Result<T, DiagDeviceError>;
#[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<u8>,
}
#[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<HdlcEncapsulatedResponse>,
}
#[derive(Debug, Clone, DekuRead)]
pub struct HdlcEncapsulatedResponse {
pub len: u32,
#[deku(count = "len")]
pub data: Vec<u8>,
}
// 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<i32> {
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<u16>,
}
impl DiagDevice {
pub fn new() -> DiagResult<Self> {
let file = File::options()
.read(true)
.write(true)
.open("/dev/diag")?;
let fd = file.as_raw_fd();
enable_frame_readwrite(fd, MEMORY_DEVICE_MODE)?;
let use_mdm = determine_use_mdm(fd)?;
Ok(DiagDevice {
file,
crc: Crc::<u16>::new(&CRC_CCITT_ALG),
use_mdm,
})
}
pub fn try_clone(&self) -> DiagResult<Self> {
Ok(DiagDevice {
file: self.file.try_clone()?,
crc: Crc::<u16>::new(&CRC_CCITT_ALG),
use_mdm: self.use_mdm,
})
}
pub fn read_response(&mut self) -> DiagResult<Option<Vec<Response>>> {
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<u8> = 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,
]);
}
}

215
src/diag_device.rs Normal file
View File

@@ -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<T> = Result<T, DiagDeviceError>;
#[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<u16> = 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<u16>,
}
impl DiagDevice {
pub fn new() -> DiagResult<Self> {
let file = File::options()
.read(true)
.write(true)
.open("/dev/diag")?;
let fd = file.as_raw_fd();
enable_frame_readwrite(fd, MEMORY_DEVICE_MODE)?;
let use_mdm = determine_use_mdm(fd)?;
Ok(DiagDevice {
file,
crc: Crc::<u16>::new(&CRC_CCITT_ALG),
use_mdm,
})
}
fn parse_response_container(&self, container: ResponseContainer) -> DiagResult<Vec<Response>> {
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<Vec<Response>> {
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<i32> {
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)
}

46
src/hdlc.rs
View File

@@ -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<u8>, crc: &Crc<u16>) -> Vec<u8> {
data.put_u16_le(crc.checksum(&data));
@@ -16,13 +36,17 @@ pub fn hdlc_encapsulate(mut data: Vec<u8>, crc: &Crc<u16>) -> Vec<u8> {
result
}
pub fn hdlc_decapsulate(mut data: Vec<u8>, crc: &Crc<u16>) -> Vec<u8> {
pub fn hdlc_decapsulate(mut data: Vec<u8>, crc: &Crc<u16>) -> Result<Vec<u8>, 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<u8>, crc: &Crc<u16>) -> Vec<u8> {
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<u8>, crc: &Crc<u16>) -> Vec<u8> {
}
// 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::<u16>::new(&crate::diag::CRC_CCITT_ALG);
let crc = Crc::<u16>::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));
}
}

11
src/main.rs
View File

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