diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/definitions.rs | 118 | ||||
| -rw-r--r-- | src/main.rs | 35 | ||||
| -rw-r--r-- | src/types.rs | 89 |
3 files changed, 233 insertions, 9 deletions
diff --git a/src/definitions.rs b/src/definitions.rs index 6059c93..aa78809 100644 --- a/src/definitions.rs +++ b/src/definitions.rs @@ -1,5 +1,6 @@ use serde_yaml; -use serde_derive; + +use types::{NamedValue, Value}; #[derive(Debug, Deserialize)] pub struct RawECURequest { @@ -30,8 +31,8 @@ impl RawECURequest { #[derive(Debug, Deserialize)] pub struct RawECUResponseEntry { name: String, - offset: u16, - length: u16, + offset: usize, + length: usize, check: Option<Vec<u8>>, kind: Option<String>, base: Option<u8>, @@ -40,7 +41,7 @@ pub struct RawECUResponseEntry { #[derive(Debug, Deserialize)] pub struct RawECUResponse { - length: u16, + length: usize, values: Vec<RawECUResponseEntry>, } @@ -53,8 +54,36 @@ pub struct RawECUEntry { } impl RawECUEntry { - pub fn get() -> Vec<RawECUEntry> { - serde_yaml::from_slice(include_bytes!("../definitions/raw.yaml")).unwrap() + pub fn get_all() -> &'static [RawECUEntry] { + lazy_static! { + static ref ECUS: Vec<RawECUEntry> = + serde_yaml::from_slice(include_bytes!("../definitions/raw.yaml")).unwrap(); + } + &ECUS + } + + pub fn ecu_for(id: u16, payload: &[u8]) -> Option<&'static RawECUEntry> { + 'ecu: for ecu in RawECUEntry::get_all() { + if (ecu.request().sendto() == id || ecu.request().recvfrom() == id + || ecu.request().flowctrl() == id) + && ecu.response.length == payload.len() + { + for entry in &ecu.response.values { + if let Some(ref vals) = entry.check { + let has = &payload[entry.offset..(entry.offset + vals.len())]; + trace!("Check: Does {:?} == {:?}", vals, has); + for i in 0..vals.len() { + if has[i] != vals[i] { + trace!("Oops, not the same"); + continue 'ecu; + } + } + } + } + return Some(ecu); + } + } + None } pub fn name(&self) -> &str { @@ -68,4 +97,81 @@ impl RawECUEntry { pub fn request(&self) -> &RawECURequest { &self.request } + + pub fn decode(&self, payload: &[u8]) -> Option<Vec<NamedValue>> { + let mut ret = Vec::new(); + + if payload.len() != self.response.length { + warn!( + "Unable to decode, length incorrect. {} != {}", + payload.len(), + self.response.length + ); + return None; + } + + for entry in &self.response.values { + if let Some(ref kind) = entry.kind { + let value: Value = match kind.as_ref() { + "unsigned" => { + trace!("Unsigned"); + match entry.length { + 1 => payload[entry.offset].into(), + 2 => (((payload[entry.offset] as u16) << 8) + + (payload[entry.offset + 1] as u16)) + .into(), + 3 => (((payload[entry.offset] as u32) << 16) + + ((payload[entry.offset + 1] as u32) << 8) + + (payload[entry.offset + 2] as u32)) + .into(), + 4 => (((payload[entry.offset] as u32) << 24) + + ((payload[entry.offset + 1] as u32) << 16) + + ((payload[entry.offset + 2] as u32) << 8) + + (payload[entry.offset + 3] as u32)) + .into(), + _ => panic!("Unhandled length: {}", entry.length), + } + } + "signed" => { + trace!("Signed"); + match entry.length { + 1 => (payload[entry.offset] as i8).into(), + 2 => ((((payload[entry.offset] as u16) << 8) + + (payload[entry.offset + 1] as u16)) + as i16) + .into(), + 3 => ((((payload[entry.offset] as u32) << 16) + + ((payload[entry.offset + 1] as u32) << 8) + + (payload[entry.offset + 2] as u32)) + as i32) + .into(), + 4 => ((((payload[entry.offset] as u32) << 24) + + ((payload[entry.offset + 1] as u32) << 16) + + ((payload[entry.offset + 2] as u32) << 8) + + (payload[entry.offset + 3] as u32)) + as i32) + .into(), + _ => panic!("Unhandled length: {}", entry.length), + } + } + "bit" => { + trace!("Bit"); + ((payload[entry.offset] & (1 << entry.base.unwrap())) != 0).into() + } + _ => { + panic!("Unhandled"); + } + }; + let name = { + let mut n = self.name.clone(); + n.push('.'); + n.push_str(&entry.name); + n + }; + ret.push(NamedValue::new(&name, Some(value))); + } + } + + Some(ret) + } } diff --git a/src/main.rs b/src/main.rs index a0a4b7c..2b3b7db 100644 --- a/src/main.rs +++ b/src/main.rs @@ -11,6 +11,10 @@ extern crate serde; extern crate serde_derive; extern crate serde_yaml; +#[macro_use] +extern crate lazy_static; + +mod types; mod canstream; mod isotp; mod definitions; @@ -21,10 +25,27 @@ use futures::sync::mpsc; use tokio::timer::Delay; use std::time::{Duration, Instant}; +fn packet_print(id: u16, bytes: &[u8]) -> String { + let mut ret: String = String::new(); + + ret.push_str(&format!( + "{:03X}/{:03X} (len={:2})", + id - 8, + id, + bytes.len() + )); + + for b in bytes { + ret.push_str(&format!(" {:02X}", b)); + } + + ret +} + fn main() { env_logger::init(); - let raw_defs = definitions::RawECUEntry::get(); + let raw_defs = definitions::RawECUEntry::get_all(); info!("Read {} raw ecu entries", raw_defs.len()); @@ -52,7 +73,8 @@ fn main() { ); let mut counter = 1; - for def in &raw_defs { + for def in raw_defs { + break; let req = def.request(); let dest = req.sendto(); let packet = req.packet().to_vec(); @@ -80,7 +102,14 @@ fn main() { stream .map_err(|e| error!("error = {:?}", e)) .for_each(move |frame| { - info!("Received ISO-TP frame: {:?}", frame); + info!( + "Received ISO-TP packet: {}", + packet_print(frame.0, &frame.1) + ); + let ecu = definitions::RawECUEntry::ecu_for(frame.0, &frame.1); + if let Some(ecu) = ecu { + info!("Decoded: {:?}", ecu.decode(&frame.1)); + } Ok(()) }), diff --git a/src/types.rs b/src/types.rs new file mode 100644 index 0000000..6b0aacd --- /dev/null +++ b/src/types.rs @@ -0,0 +1,89 @@ +#[derive(Copy, Clone, Debug)] +pub enum Value { + Boolean(bool), + Unsigned(u32), + Signed(i32), + Floating(f64), +} + +impl Value { + pub fn from_bool(v: bool) -> Value { + Value::Boolean(v) + } + pub fn from_unsigned(v: u32) -> Value { + Value::Unsigned(v) + } + pub fn from_signed(v: i32) -> Value { + Value::Signed(v) + } + pub fn from_floating(v: f64) -> Value { + Value::Floating(v) + } +} + +impl From<bool> for Value { + fn from(v: bool) -> Self { + Value::from_bool(v) + } +} + +impl From<u8> for Value { + fn from(v: u8) -> Self { + Value::from_unsigned(v as u32) + } +} + +impl From<u16> for Value { + fn from(v: u16) -> Self { + Value::from_unsigned(v as u32) + } +} + +impl From<u32> for Value { + fn from(v: u32) -> Self { + Value::from_unsigned(v) + } +} + +impl From<i8> for Value { + fn from(v: i8) -> Self { + Value::from_signed(v as i32) + } +} + +impl From<i16> for Value { + fn from(v: i16) -> Self { + Value::from_signed(v as i32) + } +} + +impl From<i32> for Value { + fn from(v: i32) -> Self { + Value::from_signed(v) + } +} + +impl From<f64> for Value { + fn from(v: f64) -> Self { + Value::from_floating(v) + } +} + +#[derive(Clone, Debug)] +pub struct NamedValue { + name: String, + value: Option<Value>, +} + +impl NamedValue { + pub fn new(name: &str, value: Option<Value>) -> NamedValue { + NamedValue { + name: name.into(), + value: value, + } + } + + pub fn get_value(&self) -> Option<Value> { + self.value + } +} |