summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorDaniel Silverstone <dsilvers@digital-scurf.org>2018-04-03 19:31:15 +0100
committerDaniel Silverstone <dsilvers@digital-scurf.org>2018-04-03 19:31:15 +0100
commit39e6030a357789637eed2e69a8b791d63911e18b (patch)
tree2bb22752804f9277cdca79aa829a14eccc23e4f8
parentef9fc91c613fc038a5f160e595176fdf7b51efe0 (diff)
downloadcanopied-39e6030a357789637eed2e69a8b791d63911e18b.tar.bz2
We can transmit packets now
-rw-r--r--src/isotp.rs117
-rw-r--r--src/main.rs3
2 files changed, 113 insertions, 7 deletions
diff --git a/src/isotp.rs b/src/isotp.rs
index 4d4794a..0085e0d 100644
--- a/src/isotp.rs
+++ b/src/isotp.rs
@@ -55,21 +55,113 @@ impl IncomingPacket {
}
}
+struct OutgoingPacket {
+ queue: VecDeque<CANFrame>,
+ block_size: Option<usize>,
+}
+
+impl OutgoingPacket {
+ fn new(destination: u16, payload: &[u8]) -> OutgoingPacket {
+ let mut packets = VecDeque::new();
+
+ if payload.len() < 8 {
+ // Single Frame payload, set it up...
+ let mut sf_payload = [0; 8];
+ sf_payload[0] = payload.len() as u8;
+ for v in 0..payload.len() {
+ sf_payload[v + 1] = payload[v];
+ }
+ packets
+ .push_back(CANFrame::new(destination as u32, &sf_payload, false, false).unwrap());
+ } else {
+ // Multi frame payload, set it up...
+ let mut ff_payload = [0; 8];
+ ff_payload[0] = (0x10 | (payload.len() >> 8)) as u8;
+ ff_payload[1] = (payload.len() & 0xFF) as u8;
+ for v in 0..6 {
+ ff_payload[v + 2] = payload[v];
+ }
+ packets
+ .push_back(CANFrame::new(destination as u32, &ff_payload, false, false).unwrap());
+ let mut sent = 6;
+ let mut counter = 1;
+ while sent < payload.len() {
+ let mut cf_payload = [0; 8];
+ cf_payload[0] = (0x20 | counter) as u8;
+ counter = (counter + 1) & 0xF;
+ let this_packet = min(payload.len() - sent, 7);
+ for v in 0..this_packet {
+ cf_payload[v + 1] = payload[sent + v];
+ }
+ sent += this_packet;
+ packets.push_back(
+ CANFrame::new(destination as u32, &cf_payload, false, false).unwrap(),
+ );
+ }
+ }
+
+ OutgoingPacket {
+ queue: packets,
+ block_size: None,
+ }
+ }
+
+ fn queue_some(&mut self, txqueue: &mut VecDeque<CANFrame>) {
+ trace!("Queueing some frames {:?}", self.block_size);
+ let mut to_send = self.block_size.unwrap_or(1);
+ if to_send == 0 {
+ to_send = self.queue.len();
+ }
+ trace!("Chosing to send {} frames this time", to_send);
+ for _ in 0..to_send {
+ if let Some(packet) = self.queue.pop_front() {
+ txqueue.push_back(packet);
+ }
+ }
+ trace!("There are {} packets left", self.queue.len())
+ }
+
+ fn is_finished(&self) -> bool {
+ self.queue.len() == 0
+ }
+
+ fn set_block_size(&mut self, block_size: u8) {
+ self.block_size = Some(block_size as usize);
+ }
+}
+
pub struct ISOTP {
incoming: HashMap<u16, IncomingPacket>,
- outgoing: VecDeque<CANFrame>,
+ outgoing: HashMap<u16, OutgoingPacket>,
+ txqueue: VecDeque<CANFrame>,
}
impl ISOTP {
pub fn new() -> ISOTP {
ISOTP {
incoming: HashMap::new(),
- outgoing: VecDeque::new(),
+ outgoing: HashMap::new(),
+ txqueue: VecDeque::new(),
}
}
- pub fn get_outgoing(&mut self) -> Option<CANFrame> {
- self.outgoing.pop_front()
+ pub fn get_txqueue(&mut self) -> Option<CANFrame> {
+ self.txqueue.pop_front()
+ }
+
+ pub fn send_packet(&mut self, destination: u16, payload: &[u8]) {
+ let answerer = destination + 8;
+ let mut packet = OutgoingPacket::new(destination, payload);
+ trace!("Preparing to send packet...");
+ if self.queue_some_outgoing(&mut packet) {
+ trace!(
+ "There's more to queue, waiting for flow control to {:x}",
+ answerer
+ );
+ self.outgoing.insert(answerer, packet);
+ } else {
+ trace!("Nothing more to send!");
+ }
}
pub fn handle_frame(&mut self, frame: &CANFrame) -> Option<(u16, Vec<u8>)> {
@@ -147,7 +239,15 @@ impl ISOTP {
return ret;
}
3 => {
- trace!("Oh gods, a flow control frame!");
+ // A FlowControl frame means we need to check if we have an
+ // outgoing packet for this, and if we do, we take it out
+ // of our map, poke it gently, and put it back if necessary.
+ if let Some(mut outpacket) = self.outgoing.remove(&id) {
+ outpacket.set_block_size(data[1]);
+ if self.queue_some_outgoing(&mut outpacket) {
+ self.outgoing.insert(id, outpacket);
+ }
+ }
}
_ => {}
}
@@ -162,7 +262,12 @@ impl ISOTP {
false,
false,
).unwrap();
- self.outgoing.push_back(frame);
+ self.txqueue.push_back(frame);
packet.blocks_until = packet.block_size;
}
+
+ fn queue_some_outgoing(&mut self, packet: &mut OutgoingPacket) -> bool {
+ packet.queue_some(&mut self.txqueue);
+ !packet.is_finished()
+ }
}
diff --git a/src/main.rs b/src/main.rs
index 0548678..36307e6 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -26,9 +26,10 @@ fn main() {
None => {}
Some((id, data)) => {
info!("Received from {:03x} data {:?}", id, data);
+ mytp.send_packet(0x123, &data);
}
}
- while let Some(packet) = mytp.get_outgoing() {
+ while let Some(packet) = mytp.get_txqueue() {
info!(" Sending CAN frame: {:X}", packet);
waiter.send(packet).map_err(|_| ())?;
waiter.flush().map_err(|_| ())?;