diff options
| -rw-r--r-- | Cargo.toml | 3 | ||||
| -rw-r--r-- | src/isotp.rs | 168 | ||||
| -rw-r--r-- | src/main.rs | 27 |
3 files changed, 187 insertions, 11 deletions
@@ -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(()) }), ); } |