code.octet-stream.net Git - m17rt/blob - m17core/src/kiss.rs

   1 // Note FEND and FESC both have the top two bits set. In the header byte this corresponds
   2 // to high port numbers which are never used by M17 so we needn't bother (un)escaping it.
   3
   4 const FEND: u8 = 0xC0;
   5 const FESC: u8 = 0xDB;
   6 const TFEND: u8 = 0xDC;
   7 const TFESC: u8 = 0xDD;
   8
   9 pub const PORT_PACKET_BASIC: u8 = 0;
  10 pub const PORT_PACKET_FULL: u8 = 1;
  11 pub const PORT_STREAM: u8 = 2;
  12
  13 /// Maximum theoretical frame size for any valid M17 KISS frame.
  14 ///
  15 /// In M17 Full Packet Mode a 30-byte LSF is merged with a packet which may be up to
  16 /// 825 bytes in length. Supposing an (impossible) worst case that every byte is FEND
  17 /// or FESC, 1710 bytes is the maximum expected payload. With a FEND at each end and
  18 /// the KISS frame header byte we get 1713.
  19 pub const MAX_FRAME_LEN: usize = 1713;
  20
  21 /// Holder for any valid M17 KISS frame.
  22 ///
  23 /// For efficiency, `data` and `len` are exposed directly and received KISS data may
  24 /// be streamed directly into a pre-allocated `KissFrame`.
  25 #[derive(Debug, PartialEq, Eq, Clone)]
  26 pub struct KissFrame {
  27     pub data: [u8; MAX_FRAME_LEN],
  28     pub len: usize,
  29 }
  30
  31 impl KissFrame {
  32     /// Construct empty frame
  33     pub fn new_empty() -> Self {
  34         Self {
  35             data: [0u8; MAX_FRAME_LEN],
  36             len: 0,
  37         }
  38     }
  39
  40     /// Request to transmit a data packet (basic mode).
  41     ///
  42     /// A raw payload up to 822 bytes can be provided. The TNC will mark it as Raw format
  43     /// and automatically calculate the checksum. If required it will also chunk the packet
  44     /// into individual frames and transmit them sequentially.
  45     pub fn new_basic_packet(payload: &[u8]) -> Result<Self, KissError> {
  46         // M17 packet payloads can be up to 825 bytes in length
  47         // Type prefix (RAW = 0x00) occupies the first byte
  48         // Last 2 bytes are checksum
  49         if payload.len() > 822 {
  50             return Err(KissError::PayloadTooBig);
  51         }
  52         let mut data = [0u8; MAX_FRAME_LEN];
  53         let mut i = 0;
  54         push(&mut data, &mut i, FEND);
  55         push(
  56             &mut data,
  57             &mut i,
  58             kiss_header(PORT_PACKET_BASIC, KissCommand::DataFrame.proto_value()),
  59         );
  60
  61         i += escape(payload, &mut data[i..]);
  62         push(&mut data, &mut i, FEND);
  63
  64         Ok(KissFrame { data, len: i })
  65     }
  66
  67     /// Request to transmit a data packet (full mode).
  68     ///
  69     /// Sender must provide a 30-byte LSF and a full packet payload (up to 825 bytes)
  70     /// that will be combined into the frame. The packet payload must include the type
  71     /// code prefix and the CRC, both of which would have been calculated by the TNC if
  72     /// it was basic mode.
  73     pub fn new_full_packet(lsf: &[u8], packet: &[u8]) -> Result<Self, KissError> {
  74         if lsf.len() != 30 {
  75             return Err(KissError::LsfWrongSize);
  76         }
  77         if packet.len() > 825 {
  78             return Err(KissError::PayloadTooBig);
  79         }
  80         let mut data = [0u8; MAX_FRAME_LEN];
  81         let mut i = 0;
  82         push(&mut data, &mut i, FEND);
  83         push(
  84             &mut data,
  85             &mut i,
  86             kiss_header(PORT_PACKET_FULL, KissCommand::DataFrame.proto_value()),
  87         );
  88         i += escape(lsf, &mut data[i..]);
  89         i += escape(packet, &mut data[i..]);
  90         push(&mut data, &mut i, FEND);
  91
  92         Ok(KissFrame { data, len: i })
  93     }
  94
  95     /// Request to begin a stream data transfer (e.g. voice).
  96     ///
  97     /// An LSF payload of exactly 30 bytes must be provided.
  98     ///
  99     /// This must be followed by at least one stream data payload, ending with one that
 100     /// has the end of stream (EOS) bit set.
 101     pub fn new_stream_setup(lsf: &[u8]) -> Result<Self, KissError> {
 102         if lsf.len() != 30 {
 103             return Err(KissError::LsfWrongSize);
 104         }
 105         let mut data = [0u8; MAX_FRAME_LEN];
 106         let mut i = 0;
 107         push(&mut data, &mut i, FEND);
 108         push(
 109             &mut data,
 110             &mut i,
 111             kiss_header(PORT_STREAM, KissCommand::DataFrame.proto_value()),
 112         );
 113         i += escape(lsf, &mut data[i..]);
 114         push(&mut data, &mut i, FEND);
 115
 116         Ok(KissFrame { data, len: i })
 117     }
 118
 119     /// Transmit a segment of data in a stream transfer (e.g. voice).
 120     ///
 121     /// A data payload of 26 bytes including metadata must be provided. This must follow
 122     /// exactly the prescribed format (H.5.2 in the spec). The TNC will be watching for
 123     /// the EOS flag to know that this transmission has ended.
 124     pub fn new_stream_data(stream_data: &[u8]) -> Result<Self, KissError> {
 125         if stream_data.len() != 26 {
 126             return Err(KissError::StreamDataWrongSize);
 127         }
 128         let mut data = [0u8; MAX_FRAME_LEN];
 129         let mut i = 0;
 130         push(&mut data, &mut i, FEND);
 131         push(
 132             &mut data,
 133             &mut i,
 134             kiss_header(PORT_STREAM, KissCommand::DataFrame.proto_value()),
 135         );
 136         i += escape(stream_data, &mut data[i..]);
 137         push(&mut data, &mut i, FEND);
 138
 139         Ok(KissFrame { data, len: i })
 140     }
 141
 142     /// Request to set the TxDelay
 143     pub fn new_set_tx_delay(port: u8, units: u8) -> Self {
 144         let mut data = [0u8; MAX_FRAME_LEN];
 145         let mut i = 0;
 146         push(&mut data, &mut i, FEND);
 147         push(
 148             &mut data,
 149             &mut i,
 150             kiss_header(port, KissCommand::TxDelay.proto_value()),
 151         );
 152         push(&mut data, &mut i, units);
 153         push(&mut data, &mut i, FEND);
 154
 155         KissFrame { data, len: i }
 156     }
 157
 158     /// Request to set the persistence parameter P
 159     pub fn new_set_p(port: u8, units: u8) -> Self {
 160         let mut data = [0u8; MAX_FRAME_LEN];
 161         let mut i = 0;
 162         push(&mut data, &mut i, FEND);
 163         push(
 164             &mut data,
 165             &mut i,
 166             kiss_header(port, KissCommand::P.proto_value()),
 167         );
 168         push(&mut data, &mut i, units);
 169         push(&mut data, &mut i, FEND);
 170
 171         KissFrame { data, len: i }
 172     }
 173
 174     /// Request to set full duplex or not
 175     pub fn set_full_duplex(port: u8, full_duplex: bool) -> Self {
 176         let mut data = [0u8; MAX_FRAME_LEN];
 177         let mut i = 0;
 178         push(&mut data, &mut i, FEND);
 179         push(
 180             &mut data,
 181             &mut i,
 182             kiss_header(port, KissCommand::FullDuplex.proto_value()),
 183         );
 184         push(&mut data, &mut i, if full_duplex { 1 } else { 0 });
 185         push(&mut data, &mut i, FEND);
 186
 187         KissFrame { data, len: i }
 188     }
 189
 190     /// Return this frame's KISS command type.
 191     pub fn command(&self) -> Result<KissCommand, KissError> {
 192         KissCommand::from_proto(self.header_byte()? & 0x0f)
 193     }
 194
 195     /// Return the KISS port to which this frame relates. Should be 0, 1 or 2.
 196     pub fn port(&self) -> Result<u8, KissError> {
 197         Ok(self.header_byte()? >> 4)
 198     }
 199
 200     /// Payload part of the frame between the header byte and the trailing FEND, unescaped.
 201     pub fn decode_payload(&self, out: &mut [u8]) -> Result<usize, KissError> {
 202         let start = self
 203             .data
 204             .iter()
 205             .enumerate()
 206             .skip_while(|(_, b)| **b == FEND)
 207             .skip(1)
 208             .next()
 209             .ok_or(KissError::MalformedKissFrame)?
 210             .0;
 211         let end = self.data[start..]
 212             .iter()
 213             .enumerate()
 214             .skip_while(|(_, b)| **b != FEND)
 215             .next()
 216             .ok_or(KissError::MalformedKissFrame)?
 217             .0
 218             + start;
 219         Ok(unescape(&self.data[start..end], out))
 220     }
 221
 222     /// Borrow the frame as a slice
 223     pub fn as_bytes(&self) -> &[u8] {
 224         &self.data[..self.len]
 225     }
 226
 227     /// Return the header byte of the KISS frame, skipping over 0 or more prepended FENDs.
 228     fn header_byte(&self) -> Result<u8, KissError> {
 229         Ok(self
 230             .data
 231             .iter()
 232             .skip_while(|b| **b == FEND)
 233             .next()
 234             .cloned()
 235             .ok_or(KissError::MalformedKissFrame)?)
 236     }
 237 }
 238
 239 fn kiss_header(port: u8, command: u8) -> u8 {
 240     (port << 4) | (command & 0x0f)
 241 }
 242
 243 fn push(data: &mut [u8], idx: &mut usize, value: u8) {
 244     data[*idx] = value;
 245     *idx += 1;
 246 }
 247
 248 #[derive(Debug, PartialEq, Eq, Clone)]
 249 pub enum KissCommand {
 250     DataFrame,
 251     TxDelay,
 252     P,
 253     FullDuplex,
 254 }
 255
 256 impl KissCommand {
 257     fn from_proto(value: u8) -> Result<Self, KissError> {
 258         Ok(match value {
 259             0 => KissCommand::DataFrame,
 260             1 => KissCommand::TxDelay,
 261             2 => KissCommand::P,
 262             5 => KissCommand::FullDuplex,
 263             _ => return Err(KissError::UnsupportedKissCommand),
 264         })
 265     }
 266
 267     fn proto_value(&self) -> u8 {
 268         match self {
 269             KissCommand::DataFrame => 0,
 270             KissCommand::TxDelay => 1,
 271             KissCommand::P => 2,
 272             KissCommand::FullDuplex => 5,
 273         }
 274     }
 275 }
 276
 277 /// Accepts raw KISS data and emits one frame at a time.
 278 ///
 279 /// A frame will be emitted if there is at least one byte between FEND markers. It is up to the consumer
 280 /// to determine whether it's actually a valid frame.
 281 pub struct KissBuffer {
 282     /// Provisional frame, whose buffer might contain more than one sequential frame at a time
 283     frame: KissFrame,
 284     /// Number of bytes that have been written into `frame.data`, which may be more than the the length
 285     /// of the first valid frame, `frame.len`.
 286     written: usize,
 287     /// Whether we have emitted the first frame in `frame`'s buffer and now need to flush it out.
 288     first_frame_returned: bool,
 289 }
 290
 291 impl KissBuffer {
 292     /// Create new buffer
 293     pub fn new() -> Self {
 294         Self {
 295             frame: KissFrame::new_empty(),
 296             written: 0,
 297             first_frame_returned: false,
 298         }
 299     }
 300
 301     /// Return the space remaining for more data
 302     pub fn buf_remaining(&mut self) -> &mut [u8] {
 303         self.flush_first_frame();
 304         if self.written == self.frame.data.len() {
 305             // full buffer with no data means oversized frame
 306             // sender is doing something weird or a FEND got dropped
 307             // either way: flush it all and try to sync up again
 308             self.written = 0;
 309         }
 310         &mut self.frame.data[self.written..]
 311     }
 312
 313     /// Indicate how much data was written into the buffer provided by `buf_remaining()`.
 314     pub fn did_write(&mut self, len: usize) {
 315         self.written += len;
 316     }
 317
 318     /// Try to construct and retrieve the next frame in the buffer
 319     pub fn next_frame(&mut self) -> Option<&KissFrame> {
 320         self.flush_first_frame();
 321
 322         // If we have any data without a leading FEND, scan past it
 323         let mut i = 0;
 324         while i < self.written && self.frame.data[i] != FEND {
 325             i += 1;
 326         }
 327         self.move_to_start(i);
 328
 329         // If we do have a leading FEND, scan up up to the last one in the series
 330         i = 0;
 331         while (i + 1) < self.written && self.frame.data[i + 1] == FEND {
 332             i += 1;
 333         }
 334         if i != 0 {
 335             self.move_to_start(i);
 336         }
 337
 338         // Now, if we have FEND-something-FEND, return it
 339         if self.written >= 2 && self.frame.data[0] == FEND && self.frame.data[1] != FEND {
 340             i = 2;
 341             while i < self.written && self.frame.data[i] != FEND {
 342                 i += 1;
 343             }
 344             if i < self.written && self.frame.data[i] == FEND {
 345                 self.frame.len = i + 1;
 346                 self.first_frame_returned = true;
 347                 return Some(&self.frame);
 348             }
 349         }
 350
 351         None
 352     }
 353
 354     /// Check if we just returned a frame; if so, clear it out and position the buffer for the next frame.
 355     fn flush_first_frame(&mut self) {
 356         if !self.first_frame_returned {
 357             return;
 358         }
 359         self.first_frame_returned = false;
 360         // If we have previously returned a valid frame, in the simplest case `frame.data` contains FEND-something-FEND
 361         // So to find the trailing FEND we can start at index 2
 362         // Loop forward until we find that FEND, which must exist, and leave its index in `i`
 363         let mut i = 2;
 364         while self.frame.data[i] != FEND {
 365             i += 1;
 366         }
 367         // However if we have consecutive trailing FENDs we want to ignore them
 368         // Having found the trailing FEND, increment past any additional FENDs until we reach the end or something else
 369         while (i + 1) < self.written && self.frame.data[i + 1] == FEND {
 370             i += 1;
 371         }
 372         // Now take that final FEND and make it the start of our frame
 373         self.move_to_start(i);
 374     }
 375
 376     /// Shift all data in the buffer back to the beginning starting from the given index.
 377     fn move_to_start(&mut self, idx: usize) {
 378         for i in idx..self.written {
 379             self.frame.data[i - idx] = self.frame.data[i];
 380         }
 381         self.written -= idx;
 382     }
 383 }
 384
 385 #[derive(Debug)]
 386 pub enum KissError {
 387     MalformedKissFrame,
 388     UnsupportedKissCommand,
 389     PayloadTooBig,
 390     LsfWrongSize,
 391     StreamDataWrongSize,
 392 }
 393
 394 fn escape(src: &[u8], dst: &mut [u8]) -> usize {
 395     let mut i = 0;
 396     let mut j = 0;
 397     while i < src.len() && j < dst.len() {
 398         if src[i] == FEND {
 399             dst[j] = FESC;
 400             j += 1;
 401             dst[j] = TFEND;
 402         } else if src[i] == FESC {
 403             dst[j] = FESC;
 404             j += 1;
 405             dst[j] = TFESC;
 406         } else {
 407             dst[j] = src[i];
 408         }
 409         i += 1;
 410         j += 1;
 411     }
 412     j
 413 }
 414
 415 fn unescape(src: &[u8], dst: &mut [u8]) -> usize {
 416     let mut i = 0;
 417     let mut j = 0;
 418     while i < src.len() && j < dst.len() {
 419         if src[i] == FESC {
 420             if i == src.len() - 1 {
 421                 break;
 422             }
 423             i += 1;
 424             if src[i] == TFEND {
 425                 dst[j] = FEND;
 426             } else if src[i] == TFESC {
 427                 dst[j] = FESC;
 428             }
 429         } else {
 430             dst[j] = src[i];
 431         }
 432         i += 1;
 433         j += 1;
 434     }
 435     j
 436 }
 437
 438 #[cfg(test)]
 439 mod tests {
 440     use super::*;
 441
 442     #[test]
 443     fn test_escape() {
 444         let mut buf = [0u8; 1024];
 445
 446         let src = [0, 1, 2, 3, 4, 5];
 447         let n = escape(&src, &mut buf);
 448         assert_eq!(n, 6);
 449         assert_eq!(&buf[0..6], src);
 450
 451         let src = [0, 1, TFESC, 3, TFEND, 5];
 452         let n = escape(&src, &mut buf);
 453         assert_eq!(n, 6);
 454         assert_eq!(&buf[0..6], src);
 455
 456         let src = [0, 1, FEND, 3, 4, 5];
 457         let n = escape(&src, &mut buf);
 458         assert_eq!(n, 7);
 459         assert_eq!(&buf[0..7], &[0, 1, FESC, TFEND, 3, 4, 5]);
 460
 461         let src = [0, 1, 2, 3, 4, FESC];
 462         let n = escape(&src, &mut buf);
 463         assert_eq!(n, 7);
 464         assert_eq!(&buf[0..7], &[0, 1, 2, 3, 4, FESC, TFESC]);
 465     }
 466
 467     #[test]
 468     fn test_unescape() {
 469         let mut buf = [0u8; 1024];
 470
 471         let src = [0, 1, 2, 3, 4, 5];
 472         let n = unescape(&src, &mut buf);
 473         assert_eq!(n, 6);
 474         assert_eq!(&buf[0..6], src);
 475
 476         let src = [0, 1, TFESC, 3, TFEND, 5];
 477         let n = unescape(&src, &mut buf);
 478         assert_eq!(n, 6);
 479         assert_eq!(&buf[0..6], src);
 480
 481         let src = [0, 1, FESC, TFEND, 3, 4, 5];
 482         let n = unescape(&src, &mut buf);
 483         assert_eq!(n, 6);
 484         assert_eq!(&buf[0..6], &[0, 1, FEND, 3, 4, 5]);
 485
 486         let src = [0, 1, 2, 3, 4, FESC, TFESC];
 487         let n = unescape(&src, &mut buf);
 488         assert_eq!(n, 6);
 489         assert_eq!(&buf[0..6], &[0, 1, 2, 3, 4, FESC]);
 490     }
 491
 492     #[test]
 493     fn basic_packet_roundtrip() {
 494         let f = KissFrame::new_basic_packet(&[0, 1, 2, 3]).unwrap();
 495         assert_eq!(f.as_bytes(), &[FEND, 0, 0, 1, 2, 3, FEND]);
 496         let mut buf = [0u8; 1024];
 497         let n = f.decode_payload(&mut buf).unwrap();
 498         assert_eq!(&buf[..n], &[0, 1, 2, 3]);
 499     }
 500
 501     #[test]
 502     fn test_buffer_basic() {
 503         let mut buffer = KissBuffer::new();
 504
 505         // initial write is not a complete frame
 506         let buf = buffer.buf_remaining();
 507         buf[0] = FEND;
 508         buffer.did_write(1);
 509         assert!(buffer.next_frame().is_none());
 510
 511         // complete the frame
 512         let buf = buffer.buf_remaining();
 513         buf[0] = 0x10;
 514         buf[1] = 0x01;
 515         buf[2] = FEND;
 516         buffer.did_write(3);
 517
 518         // everything should parse
 519         let next = buffer.next_frame().unwrap();
 520         assert_eq!(next.len, 4);
 521         assert_eq!(&next.data[0..4], &[FEND, 0x10, 0x01, FEND]);
 522         assert_eq!(next.port().unwrap(), 1);
 523         assert_eq!(next.command().unwrap(), KissCommand::DataFrame);
 524         let mut payload_buf = [0u8; 1024];
 525         let n = next.decode_payload(&mut payload_buf).unwrap();
 526         assert_eq!(n, 1);
 527         assert_eq!(&payload_buf[0..n], &[0x01]);
 528     }
 529
 530     #[test]
 531     fn test_buffer_double() {
 532         let mut buffer = KissBuffer::new();
 533         let buf = buffer.buf_remaining();
 534         buf[0..8].copy_from_slice(&[FEND, 0x10, 0x01, FEND, FEND, 0x20, 02, FEND]);
 535         buffer.did_write(8);
 536
 537         let next = buffer.next_frame().unwrap();
 538         assert_eq!(next.port().unwrap(), 1);
 539         let next = buffer.next_frame().unwrap();
 540         assert_eq!(next.port().unwrap(), 2);
 541         assert!(buffer.next_frame().is_none());
 542     }
 543
 544     #[test]
 545     fn test_buffer_double_shared_fend() {
 546         let mut buffer = KissBuffer::new();
 547         let buf = buffer.buf_remaining();
 548         buf[0..7].copy_from_slice(&[FEND, 0x10, 0x01, FEND, 0x20, 02, FEND]);
 549         buffer.did_write(7);
 550
 551         let next = buffer.next_frame().unwrap();
 552         assert_eq!(next.port().unwrap(), 1);
 553         let next = buffer.next_frame().unwrap();
 554         assert_eq!(next.port().unwrap(), 2);
 555         assert!(buffer.next_frame().is_none());
 556     }
 557
 558     #[test]
 559     fn test_buffer_extra_fend() {
 560         let mut buffer = KissBuffer::new();
 561         let buf = buffer.buf_remaining();
 562         buf[0..10].copy_from_slice(&[FEND, FEND, FEND, 0x10, 0x01, FEND, FEND, 0x20, 02, FEND]);
 563         buffer.did_write(10);
 564
 565         let next = buffer.next_frame().unwrap();
 566         assert_eq!(next.port().unwrap(), 1);
 567         let next = buffer.next_frame().unwrap();
 568         assert_eq!(next.port().unwrap(), 2);
 569         assert!(buffer.next_frame().is_none());
 570     }
 571
 572     #[test]
 573     fn test_buffer_oversize_frame() {
 574         let mut buffer = KissBuffer::new();
 575         let buf = buffer.buf_remaining();
 576         buf[0] = FEND;
 577         let len = buf.len();
 578         assert_eq!(len, MAX_FRAME_LEN);
 579         buffer.did_write(len);
 580         assert!(buffer.next_frame().is_none());
 581
 582         let buf = buffer.buf_remaining();
 583         let len = buf.len();
 584         assert_eq!(len, MAX_FRAME_LEN); // should have flushed
 585         for i in 0..len / 2 {
 586             buf[i] = 0x00;
 587         }
 588         buffer.did_write(len / 2);
 589         assert!(buffer.next_frame().is_none());
 590
 591         // confirm we resync if input goes back to normal
 592         let buf = buffer.buf_remaining();
 593         buf[0..4].copy_from_slice(&[FEND, 0x10, 0x01, FEND]);
 594         buffer.did_write(4);
 595         let next = buffer.next_frame().unwrap();
 596         assert_eq!(next.port().unwrap(), 1);
 597         assert!(buffer.next_frame().is_none());
 598     }
 599 }