mirror of
https://github.com/EFForg/rayhunter.git
synced 2026-05-14 14:08:36 -07:00
logging enabled without qcsuper
This commit is contained in:
Will Greenberg
@@ -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"
|
||||
|
||||
253
Cargo.lock
generated
253
Cargo.lock
generated
@@ -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",
|
||||
]
|
||||
|
||||
@@ -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"
|
||||
|
||||
|
||||
322
src/diag.rs
Normal file
322
src/diag.rs
Normal file
@@ -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<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, 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<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
|
||||
// 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<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 {
|
||||
// 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],
|
||||
}));
|
||||
}
|
||||
}
|
||||
66
src/hdlc.rs
Normal file
66
src/hdlc.rs
Normal file
@@ -0,0 +1,66 @@
|
||||
use crc::Crc;
|
||||
use bytes::{Buf, BufMut};
|
||||
|
||||
pub fn hdlc_encapsulate(mut data: Vec<u8>, crc: &Crc<u16>) -> Vec<u8> {
|
||||
data.put_u16_le(crc.checksum(&data));
|
||||
|
||||
let mut result: Vec<u8> = 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<u8>, crc: &Crc<u16>) -> Vec<u8> {
|
||||
// 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::<u16>::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);
|
||||
}
|
||||
}
|
||||
202
src/main.rs
202
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<T> = Result<T, DiagDeviceError>;
|
||||
|
||||
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<i32> {
|
||||
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<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,
|
||||
use_mdm,
|
||||
})
|
||||
}
|
||||
|
||||
pub fn try_clone(&self) -> DiagResult<Self> {
|
||||
Ok(DiagDevice {
|
||||
file: self.file.try_clone()?,
|
||||
use_mdm: self.use_mdm,
|
||||
})
|
||||
}
|
||||
|
||||
pub fn read_response(&mut self) -> DiagResult<Option<Vec<Vec<u8>>>> {
|
||||
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<u8> = 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<Mutex<Option<TcpStream>>> = 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();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user