Ashin Walpola
3eeccfb268
Every field of the GeoNetworking Source Position Vector this firmware sent was a compile-time constant: the bench coordinates, speed 0, heading 0, TST 0, station type passengerCar and one fixed MAC. The CAM inside described a moving cyclist while the GN header around it described a car parked at the bench. SERIAL_MSG_CAM_TX_PV (0x05) puts a 24-byte prefix ahead of the CAM UPER: MAC, station type, PAI, TST, latitude, longitude, speed and heading, all values the phone already has when it builds the CAM and none of which this chip can know. geonet_wrap_shb now takes them as a gn_lpv_t, and tx_radio_task hands the same MAC to dot11p_build_frame, so the 802.11 source address and the GN_ADDR MID stay one address across a pseudonym change. Speed is clamped rather than masked, since an overflowing 15-bit value flips its sign bit and reads as travelling backwards. This reverses the Phase 03 decision that the firmware owns the pseudonym. A pseudonym only protects anyone if the MAC, the GN_ADDR and the CAM's stationID change together, and the phone owns the stationID. The heartbeat gains a capability byte (payload[7], bit0 = CAM_TX_PV), appended so an app reading the first 7 bytes is unaffected. The app sends 0x05 only once it sees that bit, so app and firmware can be updated in either order. CAM_TX (0x01) is still handled and falls back to the bench values, with the station type corrected to cyclist to match the CAM. Verified on air from the COM10 test board, decoded independently by the CiT One's gnHeader: 24 of 24 CAM_TX_PV frames matched the sent position vector field by field, and so did the CAM station ID. The legacy path delivered 23 of 24 frames with no field mismatches. Flashed on the COM3 OBU and its boot log is clean. Also corrects the SERIAL_LINK_MAX_PAYLOAD comment, which still named the 400-byte receive capture buffer as the ceiling on the RX path. That buffer is 800 bytes now, so the serial link is the ceiling, and larger payloads are dropped and counted there.
104 lines
5.1 KiB
C
104 lines
5.1 KiB
C
#include "geonet.h"
|
|
#include <string.h>
|
|
|
|
// GeoNetworking is big-endian throughout, unlike this project's serial framing.
|
|
static void put_be16(uint8_t **p, uint16_t v)
|
|
{
|
|
*(*p)++ = (uint8_t)(v >> 8);
|
|
*(*p)++ = (uint8_t)(v);
|
|
}
|
|
|
|
static void put_be32(uint8_t **p, uint32_t v)
|
|
{
|
|
*(*p)++ = (uint8_t)(v >> 24);
|
|
*(*p)++ = (uint8_t)(v >> 16);
|
|
*(*p)++ = (uint8_t)(v >> 8);
|
|
*(*p)++ = (uint8_t)(v);
|
|
}
|
|
|
|
int geonet_wrap_shb(const uint8_t *its_payload, int its_len,
|
|
const gn_lpv_t *lpv,
|
|
uint16_t btp_dest_port,
|
|
uint8_t *out, size_t out_len)
|
|
{
|
|
// GN Basic Header (4) + GN Common Header (8) + SHB extended header (28)
|
|
// + BTP-B header (4) + ITS payload
|
|
int total = 4 + 8 + 28 + 4 + its_len;
|
|
if ((size_t)total > out_len) {
|
|
return -1;
|
|
}
|
|
|
|
uint8_t *p = out;
|
|
|
|
// ---- GN Basic Header (4 bytes) ---- (EN 302 636-4-1 clause 9.6)
|
|
*p++ = (uint8_t)((1 << 4) | 1); // version=1, NextHeader=1 (Common Header, unsecured)
|
|
*p++ = 0x00; // reserved
|
|
*p++ = 0x83; // lifetime (~60s in the base/multiplier encoding) - tune if needed
|
|
*p++ = 1; // remaining hop limit = 1 (SHB single-hop; matches CAM in the Rust reference)
|
|
|
|
// ---- GN Common Header (8 bytes) ---- (clause 9.7)
|
|
*p++ = (uint8_t)((2 << 4) | 0); // NextHeader=2 (BTP-B), reserved nibble
|
|
// HeaderType=5 (TSB), HeaderSubtype=0 (SINGLE_HOP) per table 9 - this is
|
|
// the actual encoding for single-hop broadcast. An earlier version of
|
|
// this code used (2,0), which is GEOUNICAST - wrong header type entirely
|
|
// for a broadcast frame; real receivers would try to match the
|
|
// destination-address extended header GeoUnicast expects and mishandle
|
|
// or reject the packet.
|
|
*p++ = (uint8_t)((5 << 4) | 0);
|
|
*p++ = 0x02; // traffic class: SCF=0, ChannelOffload=0, TC-ID=2 (clause 9.7.5)
|
|
*p++ = 0x80; // flags: bit0 = "is mobile" station (clause 9.7.2)
|
|
// Payload length = what follows the WHOLE GeoNetworking header
|
|
// (Basic+Common+Extended), i.e. BTP-B header + ITS payload only - does
|
|
// NOT include the 24-byte extended header itself. An earlier version of
|
|
// this code wrongly added the 24 bytes in here too.
|
|
uint16_t payload_len = (uint16_t)(4 + its_len);
|
|
*p++ = (uint8_t)(payload_len >> 8);
|
|
*p++ = (uint8_t)(payload_len & 0xFF);
|
|
*p++ = 1; // max hop limit = 1, matches basic header RHL (SHB single-hop)
|
|
*p++ = 0x00; // reserved
|
|
|
|
// ---- SHB extended header: Source Position Vector (24) + Reserved (4) = 28 bytes ----
|
|
// (clause 9.5.2). GN_ADDR (8 bytes) is itself structured, not a raw
|
|
// pseudonym (clause 9.5.1): bit0 M-flag(0=auto-derived), bits1-5 ITS-S
|
|
// type (5-bit), bits6-15 reserved(=0), then octets2-7 = MID, which is
|
|
// defined to BE the link-layer (802.11) address - so this must match
|
|
// the source address dot11p_build_frame uses, not just "look similar."
|
|
uint8_t gn_addr[8];
|
|
gn_addr[0] = (uint8_t)((0 << 7) | ((lpv->station_type & 0x1F) << 2)); // M=0, ST=station_type, top 2 reserved bits=0
|
|
gn_addr[1] = 0x00; // remaining 8 reserved bits
|
|
memcpy(&gn_addr[2], lpv->mac, 6); // MID = link-layer address
|
|
memcpy(p, gn_addr, 8); p += 8;
|
|
// TST (4 bytes): when the position below was acquired, ms, TimestampIts mod 2^32.
|
|
put_be32(&p, lpv->tst_ms);
|
|
// Latitude/Longitude (4+4 bytes, signed, 1/10 microdegree) - fixed-width binary fields, not
|
|
// UPER bit-packed like the CAM payload's own position.
|
|
put_be32(&p, (uint32_t)lpv->lat_tenmicrodeg);
|
|
put_be32(&p, (uint32_t)lpv->lon_tenmicrodeg);
|
|
// PAI (1 bit) + Speed (15 bits, signed, 0.01 m/s). Clamped, not masked: a 15-bit value that
|
|
// overflows wraps its sign bit and reads as travelling backwards at speed.
|
|
int32_t speed = lpv->speed_cms;
|
|
if (speed > 16383) speed = 16383;
|
|
if (speed < -16384) speed = -16384;
|
|
put_be16(&p, (uint16_t)(((lpv->pai ? 1u : 0u) << 15) | ((uint16_t)speed & 0x7FFFu)));
|
|
// Heading (16 bits, 0.1 degree from north, clockwise, 0..3599).
|
|
put_be16(&p, (uint16_t)(lpv->heading_decideg % 3600u));
|
|
// Reserved (4 bytes) - clause 9.8.4: the SHB extended header is the 24-byte Source Position
|
|
// Vector FOLLOWED BY a 4-byte reserved field (media-dependent data), 28 bytes in total. These
|
|
// four bytes were missing, which is why a standards-compliant receiver read our CAM payload's
|
|
// first two bytes (0x02 0x02 = protocolVersion/messageID) as the BTP destination port and saw
|
|
// 514 instead of 2001 - confirmed against live air capture, 2026-08-13. Our own gn_unwrap.c
|
|
// had the identical off-by-four, so ESP32<->ESP32 worked and nothing else did.
|
|
*p++ = 0x00; *p++ = 0x00; *p++ = 0x00; *p++ = 0x00;
|
|
|
|
// ---- BTP-B header (4 bytes) ----
|
|
*p++ = (uint8_t)(btp_dest_port >> 8);
|
|
*p++ = (uint8_t)(btp_dest_port & 0xFF);
|
|
*p++ = 0x00; *p++ = 0x00; // destination port info, unused for BTP-B
|
|
|
|
// ---- ITS payload (CAM UPER bytes from the phone) ----
|
|
memcpy(p, its_payload, its_len);
|
|
p += its_len;
|
|
|
|
return (int)(p - out);
|
|
}
|