summaryrefslogtreecommitdiff
path: root/src/isotp.rs
blob: f2c1a55b693ad3734ec29bdc1665e23c20f594bc (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
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;
    }
}