64 lines
1.6 KiB
Rust
64 lines
1.6 KiB
Rust
use num_traits::ToBytes;
|
|
|
|
pub struct Response {
|
|
buffer: Vec<u8>,
|
|
checksum: u8,
|
|
}
|
|
|
|
impl Response {
|
|
pub fn new(mut buffer: Vec<u8>, ack: bool) -> Self {
|
|
buffer.clear();
|
|
if ack {
|
|
buffer.push(b'+');
|
|
}
|
|
buffer.push(b'$');
|
|
Self {
|
|
buffer,
|
|
checksum: 0,
|
|
}
|
|
}
|
|
|
|
pub fn write_str(self, str: &str) -> Self {
|
|
let mut me = self;
|
|
for byte in str.as_bytes() {
|
|
me = me.write_byte(*byte);
|
|
}
|
|
me
|
|
}
|
|
|
|
pub fn write_byte(mut self, byte: u8) -> Self {
|
|
if byte == b'}' || byte == b'#' || byte == b'$' || byte == b'*' {
|
|
self.buffer.push(b'}');
|
|
self.checksum = self.checksum.wrapping_add(b'}');
|
|
let escaped = byte ^ 0x20;
|
|
self.buffer.push(escaped);
|
|
self.checksum = self.checksum.wrapping_add(escaped);
|
|
} else {
|
|
self.buffer.push(byte);
|
|
self.checksum = self.checksum.wrapping_add(byte);
|
|
}
|
|
self
|
|
}
|
|
|
|
pub fn write_hex<T: ToBytes>(mut self, value: T) -> Self {
|
|
for byte in value.to_le_bytes().as_ref() {
|
|
for digit in [(byte >> 4), (byte & 0xf)] {
|
|
let char = if digit > 9 {
|
|
b'a' + digit - 10
|
|
} else {
|
|
b'0' + digit
|
|
};
|
|
self.buffer.push(char);
|
|
self.checksum = self.checksum.wrapping_add(char);
|
|
}
|
|
}
|
|
self
|
|
}
|
|
|
|
pub fn finish(mut self) -> Vec<u8> {
|
|
let checksum = self.checksum;
|
|
self.buffer.push(b'#');
|
|
self.write_hex(checksum).buffer
|
|
}
|
|
}
|