summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorDaniel Silverstone <dsilvers@digital-scurf.org>2018-04-02 19:29:49 +0100
committerDaniel Silverstone <dsilvers@digital-scurf.org>2018-04-02 19:29:49 +0100
commit62718172ed94f92e11d8450267890b89091fd898 (patch)
treee63632344892eacd39a59b7b4415eb4536d2359f
parent762c0d0a05ced71803e60d89c1f34880c761d03b (diff)
downloadcanopied-62718172ed94f92e11d8450267890b89091fd898.tar.bz2
Now supporting decoding incoming ISOTP
-rw-r--r--Cargo.toml3
-rw-r--r--src/isotp.rs168
-rw-r--r--src/main.rs27
3 files changed, 187 insertions, 11 deletions
diff --git a/Cargo.toml b/Cargo.toml
index 3652d8d..20247f8 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -3,7 +3,8 @@ authors = ["Daniel Silverstone <dsilvers@digital-scurf.org>"]
name = "canopied"
version = "0.1.0"
[dependencies]
+futures = "0.1.20"
mio = "0.6.14"
socketcan = "1.7.0"
time = "0.1.39"
-tokio = "0.1.4" \ No newline at end of file
+tokio = "0.1.4"
diff --git a/src/isotp.rs b/src/isotp.rs
new file mode 100644
index 0000000..f2c1a55
--- /dev/null
+++ b/src/isotp.rs
@@ -0,0 +1,168 @@
+use socketcan::CANFrame;
+
+use canstream::*;
+
+use std::collections::{HashMap, VecDeque};
+
+use std::cmp::min;
+
+const DEFAULT_BLOCKS: u8 = 16;
+
+struct IncomingPacket {
+ id: u16,
+ remaining_bytes: u16,
+ content: Vec<u8>,
+ block_size: u8,
+ blocks_until: u8,
+ next_counter: u8,
+}
+
+impl IncomingPacket {
+ fn new(frame: &CANFrame) -> IncomingPacket {
+ let data: &[u8] = frame.data();
+ let mut datavec = Vec::new();
+
+ for v in 2..8 {
+ datavec.push(data[v]);
+ }
+
+ IncomingPacket {
+ id: frame.id() as u16,
+ remaining_bytes: ((((data[0] as u16) & 0x0f) << 8) | (data[1] as u16)) - 6,
+ content: datavec,
+ block_size: DEFAULT_BLOCKS,
+ blocks_until: 0,
+ next_counter: 1,
+ }
+ }
+
+ fn process_more(&mut self, frame: &CANFrame) -> Result<bool, ()> {
+ let data: &[u8] = frame.data();
+ if (data[0] & 0x0f) != self.next_counter {
+ return Err(());
+ }
+ let provided = min(7, self.remaining_bytes);
+ for v in 1..(provided + 1) {
+ self.content.push(data[provided as usize]);
+ }
+ self.remaining_bytes -= provided;
+ if self.block_size != 0 {
+ self.blocks_until -= 1;
+ }
+ self.next_counter = (self.next_counter + 1) & 0xF;
+
+ Ok(self.block_size != 0 && self.blocks_until == 0)
+ }
+}
+
+pub struct ISOTP {
+ incoming: HashMap<u16, IncomingPacket>,
+ outgoing: VecDeque<CANFrame>,
+}
+
+impl ISOTP {
+ pub fn new() -> ISOTP {
+ ISOTP {
+ incoming: HashMap::new(),
+ outgoing: VecDeque::new(),
+ }
+ }
+
+ pub fn get_outgoing(&mut self) -> Option<CANFrame> {
+ self.outgoing.pop_front()
+ }
+
+ pub fn handle_frame(&mut self, frame: &CANFrame) -> Option<(u16, Vec<u8>)> {
+ let id = frame.id() as u16;
+ let data = frame.data();
+ match data[0] >> 4 {
+ 0 => {
+ println!("SingleFrame");
+ // Received a SingleFrame frame, return a Vec of the
+ // requisite number of bytes...
+ let len = (data[0] & 0xf) as usize;
+ if len < 1 || len > 7 {
+ return None;
+ }
+ let sliced = &data[1..len + 1];
+ return Some((frame.id() as u16, Vec::from(sliced)));
+ }
+ 1 => {
+ println!("FirstFrame");
+ // Received a FirstFrame frame, we need to cancel any
+ // ongoing receive for this ID
+ if self.incoming.contains_key(&id) {
+ println!("Removed partial");
+ self.incoming.remove(&id);
+ }
+ // Now we need to construct an incoming packet
+ let mut incoming = IncomingPacket::new(frame);
+ // And we need to queue a flow control for it.
+ self.queue_flow(&mut incoming);
+ // We insert this into the hash
+ self.incoming.insert(id, incoming);
+ // Finally we return None since we've not completed reception
+ return None;
+ }
+ 2 => {
+ println!("ConsecutiveFrame");
+ // Received a ConsecutiveFrame frame, we need to find the
+ // incoming which we want, and manipulate it with the incoming
+ // frame and hopefully we either succeed in completing packet
+ // or we have to send a flow. Failing both of those, we have
+ // to abort the incoming packet.
+ let mut ret = None;
+ let mut do_flow = false;
+ let mut do_delete = false;
+ if let Some(packet) = self.incoming.get_mut(&id) {
+ println!("Found packet");
+ if let Ok(flow) = packet.process_more(frame) {
+ println!("Processed a frame, no error");
+ if packet.remaining_bytes == 0 {
+ // We have received the packet fully
+ ret = Some((id, packet.content.clone()));
+ do_delete = true;
+ } else if flow {
+ // We probably need to send a flow packet
+ do_flow = true;
+ } else {
+ // We're just processing, return None for now
+ return None;
+ }
+ } else {
+ println!("Error processing frame");
+ do_delete = true;
+ }
+ } else {
+ println!("Didn't find a packet");
+ }
+ if do_flow {
+ let mut packet = self.incoming.remove(&id).unwrap();
+ self.queue_flow(&mut packet);
+ self.incoming.insert(id, packet);
+ }
+ if do_delete {
+ self.incoming.remove(&id);
+ }
+ return ret;
+ }
+ 3 => {
+ println!("Oh gods, a flow control frame!");
+ }
+ _ => {}
+ }
+ None
+ }
+
+ fn queue_flow(&mut self, packet: &mut IncomingPacket) {
+ // We queue a flow control packet here
+ let frame = CANFrame::new(
+ (packet.id as u32) - 8,
+ &[0x30, packet.block_size, 0x00],
+ false,
+ false,
+ ).unwrap();
+ self.outgoing.push_back(frame);
+ packet.blocks_until = packet.block_size;
+ }
+}
diff --git a/src/main.rs b/src/main.rs
index 6092e8d..9c79ffc 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -1,14 +1,15 @@
+extern crate futures;
extern crate mio;
extern crate socketcan;
-extern crate tokio;
-
extern crate time;
+extern crate tokio;
mod canstream;
+mod isotp;
use tokio::prelude::*;
-fn process_frame(frame: socketcan::CANFrame) {
+fn process_frame(frame: &socketcan::CANFrame) {
let ts = time::now_utc().to_timespec();
print!(
"({}.{:06}) {} {:03x}#",
@@ -26,18 +27,24 @@ fn process_frame(frame: socketcan::CANFrame) {
fn main() {
let (sink, stream) = canstream::CANStream::from_name("vcan0").unwrap().split();
let mut waiter = sink.wait();
+ let mut mytp = isotp::ISOTP::new();
tokio::run(
stream
.map_err(|e| println!("error = {:?}", e))
.for_each(move |frame| {
- process_frame(frame);
- if frame.id() == 0x7E4 {
- let reply = socketcan::CANFrame::new(0x123, &[0; 8], false, false).unwrap();
- waiter.send(reply).map_err(|_| ())?;
- waiter.flush().map_err(|_| ())
- } else {
- Ok(())
+ process_frame(&frame);
+ match mytp.handle_frame(&frame) {
+ None => println!("Nothing TPish"),
+ Some((id, data)) => {
+ println!("Received from {:03x} data {:?}", id, data);
+ }
+ }
+ while let Some(packet) = mytp.get_outgoing() {
+ println!("What's more, sending {:?}", packet);
+ waiter.send(packet).map_err(|_| ())?;
+ waiter.flush().map_err(|_| ())?;
}
+ Ok(())
}),
);
}