From 70bd2a13487da5e713f49c2a3712fd36c1d89ba4 Mon Sep 17 00:00:00 2001
From: Daniel Silverstone <dsilvers@digital-scurf.org>
Date: Mon, 9 Apr 2018 16:32:42 +0100
Subject: Bunch of fixups, support decoding raw frames

---
 src/definitions.rs | 118 ++++++++++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 112 insertions(+), 6 deletions(-)

(limited to 'src/definitions.rs')

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)
+    }
 }
-- 
cgit v1.2.1