use alloc::vec::Vec;
use core::{fmt, net::IpAddr};
pub const IP_PROTO_TSMP: u8 = 99;
pub const TSMP_TYPE_REJECTED_CONN: u8 = b'!';
pub const TSMP_TYPE_PING: u8 = b'p';
pub const TSMP_TYPE_PONG: u8 = b'o';
pub const TSMP_TYPE_DISCO_ADVERTISEMENT: u8 = b'a';
const MIN_TSMP_SIZE: usize = 7;
pub const DISCO_KEY_LEN: usize = 32;
pub const DISCO_ADVERTISEMENT_LEN: usize = 1 + DISCO_KEY_LEN;
const IP4_HEADER_LEN: usize = 20;
const IP6_HEADER_LEN: usize = 40;
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct DiscoKeyAdvertisement {
pub src: IpAddr,
pub dst: IpAddr,
pub key: [u8; DISCO_KEY_LEN],
}
impl DiscoKeyAdvertisement {
pub fn key_is_zero(&self) -> bool {
self.key == [0u8; DISCO_KEY_LEN]
}
pub fn marshal(&self) -> Result<Vec<u8>, MarshalError> {
let mut body = [0u8; DISCO_ADVERTISEMENT_LEN];
body[0] = TSMP_TYPE_DISCO_ADVERTISEMENT;
body[1..].copy_from_slice(&self.key);
match (self.src, self.dst) {
(IpAddr::V4(src), IpAddr::V4(dst)) => Ok(generate4(src.octets(), dst.octets(), &body)),
(IpAddr::V6(src), IpAddr::V6(dst)) => Ok(generate6(src.octets(), dst.octets(), &body)),
_ => Err(MarshalError::MixedAddressFamilies),
}
}
pub fn parse(ip_packet: &[u8]) -> Option<Self> {
let (src, dst, body) = tsmp_body(ip_packet)?;
if body.len() < DISCO_ADVERTISEMENT_LEN || body[0] != TSMP_TYPE_DISCO_ADVERTISEMENT {
return None;
}
let key: [u8; DISCO_KEY_LEN] = body[1..DISCO_ADVERTISEMENT_LEN].try_into().ok()?;
Some(Self { src, dst, key })
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum MarshalError {
MixedAddressFamilies,
}
impl fmt::Display for MarshalError {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::MixedAddressFamilies => f.write_str("wrong address family for src/dst IP"),
}
}
}
impl core::error::Error for MarshalError {}
fn generate4(src: [u8; 4], dst: [u8; 4], payload: &[u8]) -> Vec<u8> {
let mut buf = alloc::vec![0u8; IP4_HEADER_LEN + payload.len()];
buf[IP4_HEADER_LEN..].copy_from_slice(payload);
buf[0] = 0x40 | (IP4_HEADER_LEN >> 2) as u8;
buf[1] = 0x00;
let total_len = buf.len() as u16;
buf[2..4].copy_from_slice(&total_len.to_be_bytes());
buf[4..6].copy_from_slice(&0u16.to_be_bytes());
buf[6..8].copy_from_slice(&0u16.to_be_bytes());
buf[8] = 64;
buf[9] = IP_PROTO_TSMP;
buf[10..12].copy_from_slice(&0u16.to_be_bytes());
buf[12..16].copy_from_slice(&src);
buf[16..20].copy_from_slice(&dst);
let checksum = ip4_checksum(&buf[..IP4_HEADER_LEN]);
buf[10..12].copy_from_slice(&checksum.to_be_bytes());
buf
}
fn generate6(src: [u8; 16], dst: [u8; 16], payload: &[u8]) -> Vec<u8> {
let mut buf = alloc::vec![0u8; IP6_HEADER_LEN + payload.len()];
buf[IP6_HEADER_LEN..].copy_from_slice(payload);
buf[0] = 0x60;
buf[4..6].copy_from_slice(&(payload.len() as u16).to_be_bytes());
buf[6] = IP_PROTO_TSMP;
buf[7] = 64;
buf[8..24].copy_from_slice(&src);
buf[24..40].copy_from_slice(&dst);
buf
}
fn ip4_checksum(b: &[u8]) -> u16 {
let mut ac: u32 = 0;
let mut chunks = b.chunks_exact(2);
for pair in &mut chunks {
ac += u32::from(u16::from_be_bytes([pair[0], pair[1]]));
}
if let [last] = chunks.remainder() {
ac += u32::from(*last) << 8;
}
while (ac >> 16) > 0 {
ac = (ac >> 16) + (ac & 0xffff);
}
!(ac as u16)
}
pub fn tsmp_body(b: &[u8]) -> Option<(IpAddr, IpAddr, &[u8])> {
match b.first()? >> 4 {
4 => tsmp_body4(b),
6 => tsmp_body6(b),
_ => None,
}
}
fn tsmp_body4(b: &[u8]) -> Option<(IpAddr, IpAddr, &[u8])> {
if b.len() < IP4_HEADER_LEN {
return None;
}
if b[9] != IP_PROTO_TSMP {
return None;
}
let length = usize::from(u16::from_be_bytes([b[2], b[3]]));
if b.len() < length {
return None;
}
let subofs = usize::from(b[0] & 0x0f) * 4;
if subofs > length {
return None;
}
let frag_flags = u16::from_be_bytes([b[6], b[7]]);
if frag_flags & 0x2000 != 0 || frag_flags & 0x1fff != 0 {
return None;
}
if b.len() - subofs < MIN_TSMP_SIZE {
return None;
}
let src = IpAddr::from([b[12], b[13], b[14], b[15]]);
let dst = IpAddr::from([b[16], b[17], b[18], b[19]]);
Some((src, dst, &b[subofs..length]))
}
fn tsmp_body6(b: &[u8]) -> Option<(IpAddr, IpAddr, &[u8])> {
if b.len() < IP6_HEADER_LEN {
return None;
}
if b[6] != IP_PROTO_TSMP {
return None;
}
let length = usize::from(u16::from_be_bytes([b[4], b[5]])) + IP6_HEADER_LEN;
if b.len() < length {
return None;
}
if b.len() - IP6_HEADER_LEN < MIN_TSMP_SIZE {
return None;
}
let src: [u8; 16] = b[8..24].try_into().ok()?;
let dst: [u8; 16] = b[24..40].try_into().ok()?;
Some((
IpAddr::from(src),
IpAddr::from(dst),
&b[IP6_HEADER_LEN..length],
))
}
#[cfg(test)]
mod tests {
use alloc::vec::Vec;
use super::*;
const KEY: [u8; DISCO_KEY_LEN] = [
0x9c, 0x5f, 0x3a, 0x01, 0x7d, 0xe2, 0x44, 0xb8, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff, 0x00, 0x0f, 0x1e, 0x2d, 0x3c, 0x4b, 0x5a,
0x69, 0x78,
];
fn ref_ip4_checksum(b: &[u8]) -> u16 {
let mut ac: u32 = 0;
for pair in b.chunks(2) {
ac += match pair {
[hi, lo] => u32::from(u16::from_be_bytes([*hi, *lo])),
[hi] => u32::from(*hi) << 8,
_ => 0,
};
}
while (ac >> 16) > 0 {
ac = (ac >> 16) + (ac & 0xffff);
}
!(ac as u16)
}
fn ref_generate4(proto: u8, src: [u8; 4], dst: [u8; 4], payload: &[u8]) -> Vec<u8> {
let mut buf = alloc::vec![0u8; IP4_HEADER_LEN + payload.len()];
buf[IP4_HEADER_LEN..].copy_from_slice(payload);
buf[0] = 0x40 | (IP4_HEADER_LEN >> 2) as u8;
buf[1] = 0x00;
let total_len = buf.len() as u16;
buf[2..4].copy_from_slice(&total_len.to_be_bytes());
buf[4..6].copy_from_slice(&0u16.to_be_bytes());
buf[6..8].copy_from_slice(&0u16.to_be_bytes());
buf[8] = 64;
buf[9] = proto;
buf[10..12].copy_from_slice(&0u16.to_be_bytes());
buf[12..16].copy_from_slice(&src);
buf[16..20].copy_from_slice(&dst);
let sum = ref_ip4_checksum(&buf[0..IP4_HEADER_LEN]);
buf[10..12].copy_from_slice(&sum.to_be_bytes());
buf
}
fn ref_generate6(next_header: u8, src: [u8; 16], dst: [u8; 16], payload: &[u8]) -> Vec<u8> {
let mut buf = alloc::vec![0u8; IP6_HEADER_LEN + payload.len()];
buf[IP6_HEADER_LEN..].copy_from_slice(payload);
buf[0] = 0x60;
buf[4..6].copy_from_slice(&(payload.len() as u16).to_be_bytes());
buf[6] = next_header;
buf[7] = 64;
buf[8..24].copy_from_slice(&src);
buf[24..40].copy_from_slice(&dst);
buf
}
fn advertisement_body(key: &[u8; DISCO_KEY_LEN]) -> Vec<u8> {
let mut body = alloc::vec![TSMP_TYPE_DISCO_ADVERTISEMENT];
body.extend_from_slice(key);
assert_eq!(
body.len(),
DISCO_ADVERTISEMENT_LEN,
"Go asserts this exact length in Marshal"
);
body
}
fn unhex(s: &str) -> Vec<u8> {
assert!(
s.len().is_multiple_of(2),
"hex string must have even length"
);
(0..s.len())
.step_by(2)
.map(|i| u8::from_str_radix(&s[i..i + 2], 16).expect("valid hex"))
.collect()
}
#[test]
fn marshals_the_bytes_go_marshals() {
let go_test_key = [b'a'; DISCO_KEY_LEN];
let src4 = [100u8, 64, 0, 2];
let dst4 = [100u8, 64, 0, 1];
let mut want4 = unhex("45000035000000004063b1e3");
want4.extend_from_slice(&src4);
want4.extend_from_slice(&dst4);
want4.push(b'a');
want4.extend_from_slice(&go_test_key);
let advert = DiscoKeyAdvertisement {
src: IpAddr::from(src4),
dst: IpAddr::from(dst4),
key: go_test_key,
};
let got = advert
.marshal()
.expect("a same-family advertisement marshals");
assert_eq!(
got, want4,
"IPv4 advertisement must be byte-identical to Go's"
);
assert_eq!(
got.len(),
IP4_HEADER_LEN + DISCO_ADVERTISEMENT_LEN,
"53 bytes"
);
assert_eq!(
ref_ip4_checksum(&got[..IP4_HEADER_LEN]),
0,
"a correct IPv4 header checksums to zero over the whole header"
);
let src6 = [0x20, 0x01, 0x0d, 0xb8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1];
let dst6 = [0x20, 0x01, 0x0d, 0xb8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2];
let mut want6 = unhex("6000000000216340");
want6.extend_from_slice(&src6);
want6.extend_from_slice(&dst6);
want6.push(b'a');
want6.extend_from_slice(&go_test_key);
let advert = DiscoKeyAdvertisement {
src: IpAddr::from(src6),
dst: IpAddr::from(dst6),
key: go_test_key,
};
let got = advert
.marshal()
.expect("a same-family advertisement marshals");
assert_eq!(
got, want6,
"IPv6 advertisement must be byte-identical to Go's"
);
assert_eq!(
got.len(),
IP6_HEADER_LEN + DISCO_ADVERTISEMENT_LEN,
"73 bytes"
);
}
#[test]
fn marshal_round_trips_through_parse() {
for (src, dst) in [
(IpAddr::from([100, 64, 0, 2]), IpAddr::from([100, 64, 0, 1])),
(
IpAddr::from([
0xfd, 0x7a, 0x11, 0x5c, 0xa1, 0xe0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2,
]),
IpAddr::from([
0xfd, 0x7a, 0x11, 0x5c, 0xa1, 0xe0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
]),
),
] {
let advert = DiscoKeyAdvertisement { src, dst, key: KEY };
let bytes = advert
.marshal()
.expect("same-family advertisement marshals");
assert_eq!(
DiscoKeyAdvertisement::parse(&bytes),
Some(advert),
"a marshalled advertisement must parse back identically ({src} -> {dst})"
);
}
}
#[test]
fn mixed_address_families_do_not_marshal() {
let v4 = IpAddr::from([100, 64, 0, 1]);
let v6 = IpAddr::from([
0xfd, 0x7a, 0x11, 0x5c, 0xa1, 0xe0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
]);
for (src, dst) in [(v4, v6), (v6, v4)] {
assert_eq!(
DiscoKeyAdvertisement { src, dst, key: KEY }.marshal(),
Err(MarshalError::MixedAddressFamilies),
"a mixed-family advertisement must not marshal ({src} -> {dst})"
);
}
}
#[test]
fn decodes_a_real_ipv4_advertisement() {
let pkt = ref_generate4(
IP_PROTO_TSMP,
[100, 64, 0, 2],
[100, 64, 0, 1],
&advertisement_body(&KEY),
);
assert_eq!(pkt.len(), IP4_HEADER_LEN + DISCO_ADVERTISEMENT_LEN);
assert_eq!(pkt[0], 0x45, "IPv4, IHL 5");
assert_eq!(&pkt[2..4], &[0x00, 0x35], "total length 53");
assert_eq!(pkt[9], 99, "IP proto TSMP");
assert_eq!(pkt[20], b'a', "TSMP disco-advertisement type byte");
let advert = DiscoKeyAdvertisement::parse(&pkt).expect("advertisement must decode");
assert_eq!(advert.key, KEY, "the advertised disco key is learned");
assert_eq!(advert.src, IpAddr::from([100, 64, 0, 2]));
assert_eq!(advert.dst, IpAddr::from([100, 64, 0, 1]));
assert!(!advert.key_is_zero());
}
#[test]
fn decodes_a_real_ipv6_advertisement() {
let src = [
0xfd, 0x7a, 0x11, 0x5c, 0xa1, 0xe0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2,
];
let dst = [
0xfd, 0x7a, 0x11, 0x5c, 0xa1, 0xe0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
];
let pkt = ref_generate6(IP_PROTO_TSMP, src, dst, &advertisement_body(&KEY));
let advert = DiscoKeyAdvertisement::parse(&pkt).expect("advertisement must decode");
assert_eq!(advert.key, KEY);
assert_eq!(advert.src, IpAddr::from(src));
assert_eq!(advert.dst, IpAddr::from(dst));
}
#[test]
fn zero_key_parses_but_is_flagged() {
let pkt = ref_generate4(
IP_PROTO_TSMP,
[100, 64, 0, 2],
[100, 64, 0, 1],
&advertisement_body(&[0u8; DISCO_KEY_LEN]),
);
let advert = DiscoKeyAdvertisement::parse(&pkt).expect("a zero-key advertisement parses");
assert!(
advert.key_is_zero(),
"the zero key must be recognizable so it is never learned"
);
}
#[test]
fn non_advertisements_do_not_parse() {
let src = [100, 64, 0, 2];
let dst = [100, 64, 0, 1];
let tsmp = |body: &[u8]| ref_generate4(IP_PROTO_TSMP, src, dst, body);
let mut ping = alloc::vec![TSMP_TYPE_PING];
ping.extend_from_slice(&[1, 2, 3, 4, 5, 6, 7, 8]);
let mut pong = alloc::vec![TSMP_TYPE_PONG];
pong.extend_from_slice(&[1, 2, 3, 4, 5, 6, 7, 8, 0, 0, 0, 0]);
let rejected = alloc::vec![TSMP_TYPE_REJECTED_CONN, 6, b'A', 0x1f, 0x90, 0x00, 0x50];
for (name, body) in [
("ping", ping),
("pong", pong),
("rejected-conn", rejected),
("unknown type byte", {
let mut b = advertisement_body(&KEY);
b[0] = b'Z';
b
}),
] {
assert!(
DiscoKeyAdvertisement::parse(&tsmp(&body)).is_none(),
"a {name} TSMP body must not parse as a disco-key advertisement"
);
}
let mut short = advertisement_body(&KEY);
short.truncate(DISCO_ADVERTISEMENT_LEN - 1);
assert!(
DiscoKeyAdvertisement::parse(&tsmp(&short)).is_none(),
"a truncated advertisement must not be half-parsed"
);
assert!(
DiscoKeyAdvertisement::parse(&tsmp(&[TSMP_TYPE_DISCO_ADVERTISEMENT])).is_none(),
"a bodyless advertisement must not parse"
);
assert!(
DiscoKeyAdvertisement::parse(&ref_generate4(6, src, dst, &advertisement_body(&KEY)))
.is_none(),
"a TCP packet whose payload happens to look like an advertisement must not parse"
);
assert!(DiscoKeyAdvertisement::parse(&advertisement_body(&KEY)).is_none());
assert!(DiscoKeyAdvertisement::parse(&[]).is_none());
}
#[test]
fn fragmented_tsmp_does_not_parse() {
let base = ref_generate4(
IP_PROTO_TSMP,
[100, 64, 0, 2],
[100, 64, 0, 1],
&advertisement_body(&KEY),
);
assert!(
DiscoKeyAdvertisement::parse(&base).is_some(),
"control: the unfragmented packet parses"
);
let mut more_frags = base.clone();
more_frags[6..8].copy_from_slice(&0x2000u16.to_be_bytes());
assert!(
DiscoKeyAdvertisement::parse(&more_frags).is_none(),
"a first TSMP fragment with MF set must not parse"
);
let mut later = base.clone();
later[6..8].copy_from_slice(&0x000au16.to_be_bytes());
assert!(
DiscoKeyAdvertisement::parse(&later).is_none(),
"a later TSMP fragment must not parse"
);
}
#[test]
fn body_is_bounded_by_the_ip_length_field() {
let src = [100, 64, 0, 2];
let dst = [100, 64, 0, 1];
let mut short_len = ref_generate4(IP_PROTO_TSMP, src, dst, &advertisement_body(&KEY));
let declared = (short_len.len() - 1) as u16;
short_len[2..4].copy_from_slice(&declared.to_be_bytes());
assert!(
DiscoKeyAdvertisement::parse(&short_len).is_none(),
"bytes past the IP total-length field must not be read as message content"
);
let mut long_len = ref_generate4(IP_PROTO_TSMP, src, dst, &advertisement_body(&KEY));
let declared = (long_len.len() + 1) as u16;
long_len[2..4].copy_from_slice(&declared.to_be_bytes());
assert!(
DiscoKeyAdvertisement::parse(&long_len).is_none(),
"a packet cut off before its declared IP length must not parse"
);
let mut extended = advertisement_body(&KEY);
extended.extend_from_slice(&[0xde, 0xad, 0xbe, 0xef]);
let pkt = ref_generate4(IP_PROTO_TSMP, src, dst, &extended);
assert_eq!(
DiscoKeyAdvertisement::parse(&pkt).map(|a| a.key),
Some(KEY),
"a longer body that still carries the type byte and key parses"
);
}
}