Phase 03: real CAM UPER codec + ESP32-C5 TX/RX serial link
- Firmware: rewrite obu-firmware TX loop to be serial-driven (no on-chip timer), add promiscuous RX + GeoNetworking/BTP unwrap (gn_unwrap.c), add binary UART framing to the phone (serial_link.c/.h). Drop local cam_encode() - CAM is now built on the phone. - Kotlin: byte-exact UPER CAM encoder/decoder ported from cam.c (BitWriter/BitReader/CamUperCodec), matching SerialFrame codec, real UsbSerialTransport (usb-serial-for-android), CamTransmitLoop (1Hz base rate, event/geofence boost, ESP32-C5-only), wired into CamUseCaseRepository for RX and TripRecordingService for TX. - Add V2X message retention: persist all CAM (own+remote) to Room while recording, drop otherwise (DB v2 -> v3 migration). - Add jitpack repo + usb-serial-for-android dependency. Fixes: UsbSerialTransport now uses SerialInputOutputManager.start()/stop() (this lib version manages its own thread internally) instead of manual Runnable/Thread submission, which didn't compile.
This commit is contained in:
Ashin Walpola
@@ -0,0 +1,16 @@
|
||||
# wifi_patches.c is intentionally NOT in this list anymore - superseded by
|
||||
# tx_custom.c (see that file for why). Left on disk, unused, for history.
|
||||
#
|
||||
# cam.c is ALSO intentionally not in this list anymore (Phase 03): CAM is now built on the
|
||||
# phone and sent down over serial_link, so this firmware never encodes CAM itself. cam.c/.h are
|
||||
# left on disk as the byte-exact reference the Kotlin encoder was ported from - do not delete.
|
||||
#
|
||||
# serial_link.c/.h - binary phone<->ESP32 UART framing (Phase 03).
|
||||
# gn_unwrap.c/.h - strips 802.11/LLC-SNAP/GeoNetworking/BTP-B off received frames down to CAM
|
||||
# UPER bytes, for forwarding to the phone over serial_link.
|
||||
idf_component_register(
|
||||
SRCS "main.c" "denm.c" "geonet.c" "dot11p.c" "tx_custom.c" "serial_link.c" "gn_unwrap.c"
|
||||
INCLUDE_DIRS "."
|
||||
REQUIRES esp_event esp_netif nvs_flash driver esp_phy
|
||||
PRIV_REQUIRES esp_wifi
|
||||
)
|
||||
@@ -0,0 +1,131 @@
|
||||
#include "cam.h"
|
||||
#include <string.h>
|
||||
|
||||
// MSB-first bit packer - identical approach to denm.c (ASN.1 UPER is a
|
||||
// bitstream, not a byte stream).
|
||||
typedef struct {
|
||||
uint8_t *buf;
|
||||
size_t buf_len;
|
||||
size_t bit_pos;
|
||||
} bitwriter_t;
|
||||
|
||||
static void bw_init(bitwriter_t *bw, uint8_t *buf, size_t len)
|
||||
{
|
||||
bw->buf = buf;
|
||||
bw->buf_len = len;
|
||||
bw->bit_pos = 0;
|
||||
memset(buf, 0, len);
|
||||
}
|
||||
|
||||
static void bw_put_bits(bitwriter_t *bw, uint64_t value, int nbits)
|
||||
{
|
||||
for (int i = nbits - 1; i >= 0; i--) {
|
||||
size_t byte_idx = bw->bit_pos / 8;
|
||||
int bit_idx = 7 - (int)(bw->bit_pos % 8);
|
||||
if (byte_idx >= bw->buf_len) {
|
||||
return; // overflow guard - check return value of cam_encode
|
||||
}
|
||||
uint8_t bit = (value >> i) & 1;
|
||||
bw->buf[byte_idx] = (uint8_t)(bw->buf[byte_idx] | (bit << bit_idx));
|
||||
bw->bit_pos++;
|
||||
}
|
||||
}
|
||||
|
||||
static size_t bw_byte_len(const bitwriter_t *bw)
|
||||
{
|
||||
return (bw->bit_pos + 7) / 8;
|
||||
}
|
||||
|
||||
int cam_encode(const cam_fields_t *f, uint8_t *buf, size_t buf_len)
|
||||
{
|
||||
bitwriter_t bw;
|
||||
bw_init(&bw, buf, buf_len);
|
||||
|
||||
// ---- ItsPduHeader ---- (SEQUENCE, no OPTIONALs, no "..." -> no preamble)
|
||||
bw_put_bits(&bw, 2, 8); // protocolVersion INTEGER(0..255) = 2
|
||||
bw_put_bits(&bw, 2, 8); // messageID INTEGER(0..255) = cam(2)
|
||||
bw_put_bits(&bw, f->station_id, 32); // stationID StationID INTEGER(0..4294967295)
|
||||
|
||||
// ---- CoopAwareness ---- (SEQUENCE, no OPTIONALs, no "...")
|
||||
// generationDeltaTime GenerationDeltaTime INTEGER(0..65535) -> 16 bits
|
||||
bw_put_bits(&bw, f->generation_delta_time, 16);
|
||||
|
||||
// ---- CamParameters ---- (SEQUENCE, EXTENSIBLE "...", 2 OPTIONALs:
|
||||
// lowFrequencyContainer, specialVehicleContainer)
|
||||
bw_put_bits(&bw, 0, 1); // extension bit: no extension additions
|
||||
bw_put_bits(&bw, 1, 1); // lowFrequencyContainer present
|
||||
bw_put_bits(&bw, 0, 1); // specialVehicleContainer absent
|
||||
|
||||
// ---- BasicContainer ---- (SEQUENCE, EXTENSIBLE "...", no OPTIONALs)
|
||||
bw_put_bits(&bw, 0, 1); // extension bit: none
|
||||
bw_put_bits(&bw, f->station_type, 8); // stationType StationType INTEGER(0..255)
|
||||
|
||||
// ReferencePosition (SEQUENCE, no OPTIONALs/"..."), identical widths to
|
||||
// DENM eventPosition (see denm.c for the constraint derivations):
|
||||
// Latitude INTEGER(-900000000..900000001) -> 31 bits, offset from -900000000
|
||||
uint32_t lat_offset = (uint32_t)((int64_t)f->latitude_tenmicrodeg - (-900000000));
|
||||
bw_put_bits(&bw, lat_offset, 31);
|
||||
// Longitude INTEGER(-1800000000..1800000001) -> 32 bits, offset from -1800000000
|
||||
uint32_t lon_offset = (uint32_t)((int64_t)f->longitude_tenmicrodeg - (-1800000000));
|
||||
bw_put_bits(&bw, lon_offset, 32);
|
||||
// PosConfidenceEllipse: SemiAxisLength(0..4095)->12, HeadingValue(0..3601)->12
|
||||
bw_put_bits(&bw, 4095, 12); // semiMajorConfidence: unavailable
|
||||
bw_put_bits(&bw, 4095, 12); // semiMinorConfidence: unavailable
|
||||
bw_put_bits(&bw, 3601, 12); // semiMajorOrientation: unavailable
|
||||
// Altitude: AltitudeValue(-100000..800001)->20 (offset from -100000),
|
||||
// AltitudeConfidence ENUM 16 values -> 4 bits
|
||||
bw_put_bits(&bw, 900001, 20); // 800001 ("unavailable") - (-100000) = 900001
|
||||
bw_put_bits(&bw, 15, 4); // altitudeConfidence: unavailable(15)
|
||||
|
||||
// ---- HighFrequencyContainer ---- CHOICE { basicVehicleContainerHighFrequency,
|
||||
// rsuContainerHighFrequency, ... } - EXTENSIBLE, 2 root alternatives.
|
||||
bw_put_bits(&bw, 0, 1); // CHOICE extension bit: value is in root
|
||||
bw_put_bits(&bw, 0, 1); // index: 0 = basicVehicleContainerHighFrequency (1 bit for 2 alts)
|
||||
|
||||
// BasicVehicleContainerHighFrequency (SEQUENCE, NOT extensible, 7 OPTIONALs
|
||||
// accelerationControl..cenDsrcTollingZone - all absent).
|
||||
bw_put_bits(&bw, 0, 7); // 7 optional-presence bits, all absent
|
||||
|
||||
// Heading: HeadingValue(0..3601)->12, HeadingConfidence(1..127)->7 (offset from 1)
|
||||
bw_put_bits(&bw, f->heading_ddeg, 12);
|
||||
bw_put_bits(&bw, 127 - 1, 7); // headingConfidence: unavailable(127)
|
||||
// Speed: SpeedValue(0..16383)->14, SpeedConfidence(1..127)->7 (offset from 1)
|
||||
bw_put_bits(&bw, f->speed_cm_s, 14);
|
||||
bw_put_bits(&bw, 127 - 1, 7); // speedConfidence: unavailable(127)
|
||||
// DriveDirection ENUM {forward,backward,unavailable} -> 2 bits
|
||||
bw_put_bits(&bw, 2, 2); // unavailable
|
||||
// VehicleLength: VehicleLengthValue(1..1023)->10 (offset from 1),
|
||||
// VehicleLengthConfidenceIndication ENUM 5 values -> 3 bits
|
||||
bw_put_bits(&bw, (uint32_t)f->vehicle_length_dm - 1, 10);
|
||||
bw_put_bits(&bw, 4, 3); // vehicleLengthConfidenceIndication: unavailable(4)
|
||||
// VehicleWidth INTEGER(1..62) -> 6 bits (offset from 1)
|
||||
bw_put_bits(&bw, (uint32_t)f->vehicle_width_dm - 1, 6);
|
||||
// LongitudinalAcceleration: value(-160..161)->9 (offset from -160),
|
||||
// AccelerationConfidence(0..102)->7
|
||||
bw_put_bits(&bw, 161 - (uint32_t)(-160), 9); // longitudinalAccelerationValue: unavailable(161)
|
||||
bw_put_bits(&bw, 102, 7); // confidence: unavailable(102)
|
||||
// Curvature: CurvatureValue(-1023..1023)->11 (offset from -1023),
|
||||
// CurvatureConfidence ENUM 8 values -> 3 bits
|
||||
bw_put_bits(&bw, 1023 - (uint32_t)(-1023), 11); // curvatureValue: unavailable(1023)
|
||||
bw_put_bits(&bw, 7, 3); // curvatureConfidence: unavailable(7)
|
||||
// CurvatureCalculationMode ENUM {yawRateUsed,yawRateNotUsed,unavailable} -> 2 bits
|
||||
bw_put_bits(&bw, 2, 2); // unavailable
|
||||
// YawRate: YawRateValue(-32766..32767)->16 (offset from -32766),
|
||||
// YawRateConfidence ENUM 8 values -> 3 bits
|
||||
bw_put_bits(&bw, 32767 - (uint32_t)(-32766), 16); // yawRateValue: unavailable(32767)
|
||||
bw_put_bits(&bw, 7, 3); // yawRateConfidence: unavailable(7)
|
||||
|
||||
// ---- LowFrequencyContainer ---- CHOICE { basicVehicleContainerLowFrequency,
|
||||
// ... } - EXTENSIBLE, 1 root alternative (index needs 0 bits).
|
||||
bw_put_bits(&bw, 0, 1); // CHOICE extension bit: value is in root
|
||||
|
||||
// BasicVehicleContainerLowFrequency (SEQUENCE, no OPTIONALs/"...")
|
||||
// vehicleRole VehicleRole ENUM 16 values -> 4 bits
|
||||
bw_put_bits(&bw, 0, 4); // default(0)
|
||||
// exteriorLights ExteriorLights BIT STRING(SIZE(8)) -> 8 bits, all off
|
||||
bw_put_bits(&bw, 0, 8);
|
||||
// pathHistory PathHistory ::= SEQUENCE(SIZE(0..40)) OF PathPoint -> count 0..40 = 6 bits
|
||||
bw_put_bits(&bw, 0, 6); // empty path history
|
||||
|
||||
return (int)bw_byte_len(&bw);
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
#ifndef CAM_H
|
||||
#define CAM_H
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
|
||||
// Minimal CAM (Cooperative Awareness Message) per ETSI EN 302 637-2 v1.4.1
|
||||
// (CAM-PDU-Descriptions) + TS 102 894-2 v1.3.1 (CDD / ITS-Container), matching
|
||||
// the field set the working Rust reference (esp32-c_its-companion, feat/tx-cam,
|
||||
// src/applogic/cam_tx.rs) transmits:
|
||||
// - ItsPduHeader (protocolVersion 2, messageID 2 = cam)
|
||||
// - CoopAwareness { generationDeltaTime, camParameters }
|
||||
// - CamParameters {
|
||||
// basicContainer { stationType, referencePosition },
|
||||
// highFrequencyContainer = basicVehicleContainerHighFrequency { ... },
|
||||
// lowFrequencyContainer = basicVehicleContainerLowFrequency { ... }
|
||||
// }
|
||||
// All vehicle-dynamics fields we don't measure are encoded as their ASN.1
|
||||
// "unavailable" value. Speed is a real 0 (correct for a stationary station).
|
||||
|
||||
typedef struct {
|
||||
uint32_t station_id;
|
||||
uint8_t station_type; // StationType(0..255): 5 = passengerCar
|
||||
uint16_t generation_delta_time; // TimestampIts mod 65536 (ms); 0 until a real clock is wired
|
||||
int32_t latitude_tenmicrodeg; // Latitude, 1/10 microdegree
|
||||
int32_t longitude_tenmicrodeg; // Longitude, 1/10 microdegree
|
||||
uint16_t speed_cm_s; // SpeedValue, 0.01 m/s units (0 = stationary)
|
||||
uint16_t heading_ddeg; // HeadingValue, 0.1 deg units (0..3600), 3601 = unavailable
|
||||
uint16_t vehicle_length_dm; // VehicleLengthValue(1..1023), 10cm steps
|
||||
uint8_t vehicle_width_dm; // VehicleWidth(1..62), 10cm steps
|
||||
} cam_fields_t;
|
||||
|
||||
// Encodes the CAM as ASN.1 UPER. Returns bytes written, or -1 if buf too small.
|
||||
int cam_encode(const cam_fields_t *f, uint8_t *buf, size_t buf_len);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,153 @@
|
||||
#include "denm.h"
|
||||
#include <string.h>
|
||||
|
||||
// Minimal MSB-first bit packer - ASN.1 UPER is a bitstream, not a byte
|
||||
// stream, so we can't just memcpy structs.
|
||||
typedef struct {
|
||||
uint8_t *buf;
|
||||
size_t buf_len;
|
||||
size_t bit_pos;
|
||||
} bitwriter_t;
|
||||
|
||||
static void bw_init(bitwriter_t *bw, uint8_t *buf, size_t len)
|
||||
{
|
||||
bw->buf = buf;
|
||||
bw->buf_len = len;
|
||||
bw->bit_pos = 0;
|
||||
memset(buf, 0, len);
|
||||
}
|
||||
|
||||
static void bw_put_bits(bitwriter_t *bw, uint64_t value, int nbits)
|
||||
{
|
||||
for (int i = nbits - 1; i >= 0; i--) {
|
||||
size_t byte_idx = bw->bit_pos / 8;
|
||||
int bit_idx = 7 - (int)(bw->bit_pos % 8);
|
||||
if (byte_idx >= bw->buf_len) {
|
||||
return; // overflow guard - silently truncates, check return value of denm_encode
|
||||
}
|
||||
uint8_t bit = (value >> i) & 1;
|
||||
bw->buf[byte_idx] = (uint8_t)(bw->buf[byte_idx] | (bit << bit_idx));
|
||||
bw->bit_pos++;
|
||||
}
|
||||
}
|
||||
|
||||
static size_t bw_byte_len(const bitwriter_t *bw)
|
||||
{
|
||||
return (bw->bit_pos + 7) / 8;
|
||||
}
|
||||
|
||||
int denm_encode(const denm_fields_t *f, uint8_t *buf, size_t buf_len)
|
||||
{
|
||||
bitwriter_t bw;
|
||||
bw_init(&bw, buf, buf_len);
|
||||
|
||||
// ---- ItsPduHeader ---- (SEQUENCE, no OPTIONALs, no "..." -> no preamble at all)
|
||||
bw_put_bits(&bw, 2, 8); // protocolVersion INTEGER(0..255) = 2
|
||||
bw_put_bits(&bw, 1, 8); // messageID INTEGER(0..255) = denm(1)
|
||||
bw_put_bits(&bw, f->station_id, 32); // stationID = StationID INTEGER(0..4294967295) = 32 bits
|
||||
|
||||
// ---- DenmPayload (DecentralizedEnvironmentalNotificationMessage) ----
|
||||
// No "..." on this SEQUENCE -> no extension bit, just the 3-bit
|
||||
// optional-component preamble in declared order: situation, location,
|
||||
// alacarte. "No additional parameters" means location/alacarte stay
|
||||
// absent.
|
||||
bw_put_bits(&bw, 1, 1); // situation present
|
||||
bw_put_bits(&bw, 0, 1); // location absent
|
||||
bw_put_bits(&bw, 0, 1); // alacarte absent
|
||||
|
||||
// ---- ManagementContainer ----
|
||||
// This SEQUENCE ends in "..." in the real ASN.1 module -> extensible,
|
||||
// so it needs a leading 1-bit extension flag (0 = no extension
|
||||
// additions used) BEFORE the 5-bit optional/default preamble
|
||||
// (termination, relevanceDistance, relevanceTrafficDirection,
|
||||
// validityDuration, transmissionInterval, in that declared order). An
|
||||
// earlier version of this code omitted the extension bit entirely,
|
||||
// which would shift every single bit after it and corrupt the whole
|
||||
// rest of the message for any spec-compliant decoder.
|
||||
bw_put_bits(&bw, 0, 1); // ManagementContainer extension bit: none used
|
||||
bw_put_bits(&bw, f->terminate ? 1 : 0, 1); // termination present only when cancelling
|
||||
bw_put_bits(&bw, 0, 1); // relevanceDistance absent
|
||||
bw_put_bits(&bw, 0, 1); // relevanceTrafficDirection absent
|
||||
bw_put_bits(&bw, 0, 1); // validityDuration absent -> default 600s applies
|
||||
bw_put_bits(&bw, 0, 1); // transmissionInterval absent
|
||||
|
||||
// actionID = ActionID{ originatingStationID StationID(32), sequenceNumber
|
||||
// SequenceNumber(0..65535, 16 bits) } - no OPTIONALs/"..." -> no preamble.
|
||||
// Keep sequenceNumber constant across repeats of the SAME event - it's
|
||||
// the caller's job (see main.c) to only bump it on a genuinely new event
|
||||
// and reuse it for that event's eventual termination message.
|
||||
bw_put_bits(&bw, f->station_id, 32);
|
||||
bw_put_bits(&bw, f->sequence_number, 16);
|
||||
|
||||
// detectionTime / referenceTime: TimestampIts INTEGER(0..4398046511103)
|
||||
// = exactly 42 bits (2^42), ms since 2004-01-01T00:00:00Z. NOT WIRED UP
|
||||
// YET - there's no RTC/NTP sync in this skeleton, so this is 0 (decodes
|
||||
// as 2004-01-01). Wire in SNTP or a GNSS UTC fix before this is real.
|
||||
bw_put_bits(&bw, 0, 42);
|
||||
bw_put_bits(&bw, 0, 42);
|
||||
|
||||
// termination VALUE - only emitted when present (per the preamble bit
|
||||
// above - UPER never encodes a value for an absent optional component).
|
||||
// Termination ::= ENUMERATED{isCancellation(0), isNegation(1)}, no
|
||||
// "...", 2 values -> 1 bit.
|
||||
if (f->terminate) {
|
||||
bw_put_bits(&bw, 0, 1); // isCancellation
|
||||
}
|
||||
|
||||
// eventPosition (ReferencePosition ::= SEQUENCE{latitude, longitude,
|
||||
// positionConfidenceEllipse, altitude} - no OPTIONALs/"..." -> no
|
||||
// preamble, straight concatenation). Widths below are each field's
|
||||
// exact constrained-INTEGER range size from ITS-Container.asn, encoded
|
||||
// as an unsigned offset from the type's declared minimum - NOT assumed
|
||||
// to match neighboring fields (latitude and longitude are different
|
||||
// widths, which is easy to miss).
|
||||
// Latitude ::= INTEGER(-900000000..900000001) -> range 1800000002 -> 31 bits
|
||||
uint32_t lat_offset = (uint32_t)(f->latitude_tenmicrodeg - (-900000000));
|
||||
bw_put_bits(&bw, lat_offset, 31);
|
||||
// Longitude ::= INTEGER(-1800000000..1800000001) -> range 3600000002 -> 32 bits
|
||||
uint32_t lon_offset = (uint32_t)(f->longitude_tenmicrodeg - (-1800000000));
|
||||
bw_put_bits(&bw, lon_offset, 32);
|
||||
// PosConfidenceEllipse ::= SEQUENCE{semiMajorConfidence, semiMinorConfidence,
|
||||
// semiMajorOrientation} - no preamble.
|
||||
// SemiAxisLength ::= INTEGER(0..4095) -> 12 bits (not 16 - this was wrong before)
|
||||
bw_put_bits(&bw, 4095, 12); // semiMajorConfidence: unavailable
|
||||
bw_put_bits(&bw, 4095, 12); // semiMinorConfidence: unavailable
|
||||
// HeadingValue ::= INTEGER(0..3601) -> 12 bits (not 16 - this was wrong before)
|
||||
bw_put_bits(&bw, 3601, 12); // semiMajorOrientation: unavailable
|
||||
// Altitude ::= SEQUENCE{altitudeValue, altitudeConfidence} - no preamble.
|
||||
// AltitudeValue ::= INTEGER(-100000..800001) -> range 900002 -> 20 bits
|
||||
// (not 24 - this was wrong before), offset-encoded from -100000.
|
||||
bw_put_bits(&bw, 900001, 20); // 800001 ("unavailable") - (-100000) = 900001
|
||||
// AltitudeConfidence ::= ENUMERATED, 16 named values, no "..." -> 4 bits
|
||||
bw_put_bits(&bw, 15, 4); // unavailable
|
||||
|
||||
// stationType: StationType INTEGER(0..255) -> 8 bits fixed regardless of
|
||||
// how sparse the named values are.
|
||||
bw_put_bits(&bw, f->station_type, 8);
|
||||
|
||||
// ---- SituationContainer ----
|
||||
// This SEQUENCE also ends in "..." -> its own 1-bit extension flag,
|
||||
// THEN the 2-bit preamble (linkedCause, eventHistory), THEN the
|
||||
// mandatory field values. An earlier version of this code put the
|
||||
// linkedCause/eventHistory bits at the END instead of the start, and
|
||||
// had no extension bit at all - both are structural bugs that would
|
||||
// desync any spec-compliant decoder from this point on.
|
||||
bw_put_bits(&bw, 0, 1); // SituationContainer extension bit: none used
|
||||
bw_put_bits(&bw, 0, 1); // linkedCause absent
|
||||
bw_put_bits(&bw, 0, 1); // eventHistory absent
|
||||
|
||||
// informationQuality: InformationQuality INTEGER(0..7) -> 3 bits
|
||||
bw_put_bits(&bw, 1, 3); // low quality - no real sensor input, just the hazard-light GPIO
|
||||
|
||||
// eventType: CauseCode ::= SEQUENCE{causeCode, subCauseCode, ...} - this
|
||||
// inner SEQUENCE is ALSO extensible ("..."), so it gets its own leading
|
||||
// extension bit before its two mandatory fields.
|
||||
bw_put_bits(&bw, 0, 1); // CauseCode extension bit: none used
|
||||
bw_put_bits(&bw, f->cause_code, 8); // CauseCodeType INTEGER(0..255) -> 8 bits
|
||||
bw_put_bits(&bw, f->sub_cause_code, 8); // SubCauseCodeType INTEGER(0..255) -> 8 bits
|
||||
|
||||
// linkedCause / eventHistory: both absent, already signalled in the
|
||||
// preamble above - UPER writes no value bits for them.
|
||||
|
||||
return (int)bw_byte_len(&bw);
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
#ifndef DENM_H
|
||||
#define DENM_H
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
// Full CauseCodeType enumeration, straight from the authoritative source:
|
||||
// ETSI TS 102 894-2 (CDD) ITS-Container.asn, CauseCodeType definition.
|
||||
// (Values 1/2/3/14/26/27/91/94/95/97 were already cross-checked earlier
|
||||
// against a real captured DENM; the rest are now confirmed the same way,
|
||||
// from the actual ASN.1 module rather than guessed.)
|
||||
#define DENM_CAUSE_RESERVED 0
|
||||
#define DENM_CAUSE_TRAFFIC_CONDITION 1
|
||||
#define DENM_CAUSE_ACCIDENT 2
|
||||
#define DENM_CAUSE_ROADWORKS 3
|
||||
#define DENM_CAUSE_IMPASSABILITY 5
|
||||
#define DENM_CAUSE_ADVERSE_WEATHER_ADHESION 6
|
||||
#define DENM_CAUSE_AQUAPLANNING 7
|
||||
#define DENM_CAUSE_HAZARDOUS_LOCATION_SURFACE_CONDITION 9
|
||||
#define DENM_CAUSE_HAZARDOUS_LOCATION_OBSTACLE_ON_ROAD 10
|
||||
#define DENM_CAUSE_HAZARDOUS_LOCATION_ANIMAL_ON_ROAD 11
|
||||
#define DENM_CAUSE_HUMAN_PRESENCE_ON_ROAD 12
|
||||
#define DENM_CAUSE_WRONG_WAY_DRIVING 14
|
||||
#define DENM_CAUSE_RESCUE_AND_RECOVERY_WORK_IN_PROGRESS 15
|
||||
#define DENM_CAUSE_ADVERSE_WEATHER_EXTREME 17
|
||||
#define DENM_CAUSE_ADVERSE_WEATHER_VISIBILITY 18
|
||||
#define DENM_CAUSE_ADVERSE_WEATHER_PRECIPITATION 19
|
||||
#define DENM_CAUSE_SLOW_VEHICLE 26
|
||||
#define DENM_CAUSE_DANGEROUS_END_OF_QUEUE 27
|
||||
#define DENM_CAUSE_VEHICLE_BREAKDOWN 91
|
||||
#define DENM_CAUSE_POST_CRASH 92
|
||||
#define DENM_CAUSE_HUMAN_PROBLEM 93
|
||||
#define DENM_CAUSE_STATIONARY_VEHICLE 94
|
||||
#define DENM_CAUSE_EMERGENCY_VEHICLE_APPROACHING 95
|
||||
#define DENM_CAUSE_HAZARDOUS_LOCATION_DANGEROUS_CURVE 96
|
||||
#define DENM_CAUSE_COLLISION_RISK 97
|
||||
#define DENM_CAUSE_SIGNAL_VIOLATION 98
|
||||
#define DENM_CAUSE_DANGEROUS_SITUATION 99
|
||||
|
||||
typedef struct {
|
||||
uint32_t station_id;
|
||||
uint16_t sequence_number; // keep constant across repeats of the SAME event; only bump on a genuinely new event
|
||||
uint8_t cause_code; // e.g. 94 = stationaryVehicle
|
||||
uint8_t sub_cause_code; // 0 = unspecified
|
||||
uint8_t station_type; // StationType, e.g. 5 = passengerCar - match geonet_wrap_shb's station_type param
|
||||
int32_t latitude_tenmicrodeg; // 1/10 microdegree; 0 = placeholder/unavailable
|
||||
int32_t longitude_tenmicrodeg; // 1/10 microdegree; 0 = placeholder/unavailable
|
||||
bool terminate; // true = encode this as a Termination(isCancellation) message instead of a normal update
|
||||
} denm_fields_t;
|
||||
|
||||
// Encodes a minimal DENM (ItsPduHeader + ManagementContainer +
|
||||
// SituationContainer only - no location/alacarte containers) as ASN.1 UPER,
|
||||
// per the actual ETSI EN 302 637-3 / TS 102 894-2 ASN.1 modules (fetched
|
||||
// from forge.etsi.org, not reconstructed from memory). Returns bytes
|
||||
// written, or -1 if buf too small.
|
||||
//
|
||||
// Two things worth knowing if you're reading this against the modules
|
||||
// yourself: ManagementContainer, SituationContainer, and CauseCode are all
|
||||
// declared with a trailing "..." (extensible), which means each needs its
|
||||
// own leading extension bit in the UPER encoding - easy to miss, and this
|
||||
// code got it wrong in an earlier version. Field bit-widths below (e.g.
|
||||
// latitude=31 bits, longitude=32 bits, position-confidence fields=12 bits,
|
||||
// altitudeValue=20 bits) are derived directly from each type's declared
|
||||
// INTEGER constraint range, not assumed to match neighboring fields.
|
||||
int denm_encode(const denm_fields_t *f, uint8_t *buf, size_t buf_len);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,57 @@
|
||||
#include "dot11p.h"
|
||||
#include <string.h>
|
||||
|
||||
int dot11p_build_frame(const uint8_t *gn_payload, int gn_len,
|
||||
const uint8_t src_mac[6],
|
||||
uint8_t *out, size_t out_len, bool qos)
|
||||
{
|
||||
static const uint8_t broadcast[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
|
||||
static const uint8_t llc_snap[8] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00, 0x89, 0x47};
|
||||
|
||||
int hdr_len = qos ? 26 : 24; // QoS Data adds a 2-byte QoS Control field
|
||||
int total = hdr_len + 8 /* LLC/SNAP */ + gn_len;
|
||||
if ((size_t)total > out_len) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
uint8_t *p = out;
|
||||
|
||||
// Frame Control: version=0, type=Data(2), subtype=QoS Data(8) -> bytes
|
||||
// 0x88 0x00. This is what real ITS-G5 hardware actually transmits.
|
||||
//
|
||||
// Back on QoS Data again (previously downgraded to non-QoS, subtype 0,
|
||||
// as a working-but-nonstandard fallback - see git history / old comments
|
||||
// here for that whole detour). What changed: main.c no longer calls
|
||||
// esp_wifi_80211_tx() at all - it now goes through
|
||||
// esp_wifi_80211_tx_custom() (tx_custom.c, pulled from
|
||||
// opentrafficmap/its-g5-receiver-firmware_txenabled), which bypasses the
|
||||
// frame-type sanity check entirely by never calling the code path that
|
||||
// contains it. Frame subtype is no longer gated, so there's no reason
|
||||
// left to avoid matching real hardware here.
|
||||
// Frame Control byte 0: version=0, type=Data(2). Subtype: QoS Data(8)=0x88
|
||||
// for the tx_custom path, or plain Data(0)=0x08 for the standard
|
||||
// esp_wifi_80211_tx() path (which rejects QoS Data outright).
|
||||
*p++ = qos ? 0x88 : 0x08; *p++ = 0x00;
|
||||
// Duration
|
||||
*p++ = 0x00; *p++ = 0x00;
|
||||
// Addr1 = destination = broadcast
|
||||
memcpy(p, broadcast, 6); p += 6;
|
||||
// Addr2 = source (our pseudonym)
|
||||
memcpy(p, src_mac, 6); p += 6;
|
||||
// Addr3 = BSSID = broadcast (no BSS exists in OCB mode)
|
||||
memcpy(p, broadcast, 6); p += 6;
|
||||
// Sequence control - left at 0; en_sys_seq=true fills this in for us
|
||||
*p++ = 0x00; *p++ = 0x00;
|
||||
// QoS Control field - only present in QoS Data frames
|
||||
if (qos) {
|
||||
*p++ = 0x00; *p++ = 0x00; // best-effort access category
|
||||
}
|
||||
|
||||
// LLC/SNAP (Ethertype 0x8947 = GeoNetworking)
|
||||
memcpy(p, llc_snap, 8); p += 8;
|
||||
|
||||
// GeoNetworking + BTP + DENM payload
|
||||
memcpy(p, gn_payload, gn_len); p += gn_len;
|
||||
|
||||
return (int)(p - out);
|
||||
}
|
||||
@@ -0,0 +1,31 @@
|
||||
#ifndef DOT11P_H
|
||||
#define DOT11P_H
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
// Wraps a GeoNetworking-layer payload in an 802.11 OCB frame: QoS Data
|
||||
// (subtype 8, 26-byte header), matching real ITS-G5 hardware, broadcast, no
|
||||
// BSS (Addr1=Addr3=broadcast), LLC/SNAP with Ethertype 0x8947
|
||||
// (GeoNetworking's registered Ethertype). Output is ready to hand straight
|
||||
// to esp_wifi_80211_tx_custom() (tx_custom.c) - NOT esp_wifi_80211_tx(),
|
||||
// which rejects this frame type outright. `src_mac` is used as Addr2 - pass
|
||||
// the same 6 bytes you gave geonet_wrap_shb, since GN_ADDR's MID field is
|
||||
// defined to be this same link-layer address. Returns bytes written, or -1
|
||||
// if out buffer too small.
|
||||
//
|
||||
// History: this used to be downgraded to non-QoS Data (subtype 0) because
|
||||
// esp_wifi_80211_tx() rejects QoS Data ("unsupport QoS frame type" / esp_err
|
||||
// 258) and an attempted linker-override bypass (old main/wifi_patches.c)
|
||||
// didn't work. Restored to QoS Data now that main.c transmits via
|
||||
// esp_wifi_80211_tx_custom() instead, which bypasses that gate entirely
|
||||
// (see tx_custom.c) - so there's no longer a reason to deviate from the
|
||||
// real frame format.
|
||||
// qos=true -> QoS Data (subtype 8, 26-byte header) for esp_wifi_80211_tx_custom()
|
||||
// qos=false -> plain Data (subtype 0, 24-byte header) which the STANDARD
|
||||
// esp_wifi_80211_tx() accepts (used for the standard-TX isolation test)
|
||||
int dot11p_build_frame(const uint8_t *gn_payload, int gn_len,
|
||||
const uint8_t src_mac[6],
|
||||
uint8_t *out, size_t out_len, bool qos);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,85 @@
|
||||
#include "geonet.h"
|
||||
#include <string.h>
|
||||
|
||||
int geonet_wrap_shb(const uint8_t *its_payload, int its_len,
|
||||
const uint8_t mac[6], uint8_t station_type,
|
||||
int32_t latitude_tenmicrodeg, int32_t longitude_tenmicrodeg,
|
||||
uint16_t btp_dest_port,
|
||||
uint8_t *out, size_t out_len)
|
||||
{
|
||||
// GN Basic Header (4) + GN Common Header (8) + SHB source LPV (24)
|
||||
// + BTP-B header (4) + ITS payload
|
||||
int total = 4 + 8 + 24 + 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 Long Position Vector (24 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) | ((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], mac, 6); // MID = link-layer address
|
||||
memcpy(p, gn_addr, 8); p += 8;
|
||||
// Timestamp (4 bytes, ms since 2004-01-01 mod 2^32) - placeholder 0,
|
||||
// same caveat as detectionTime in denm.c.
|
||||
memset(p, 0, 4); p += 4;
|
||||
// Latitude/Longitude (4+4 bytes, signed, big-endian, 1/10 microdegree) -
|
||||
// fixed-width binary fields, not UPER bit-packed.
|
||||
uint32_t lat_u = (uint32_t)latitude_tenmicrodeg;
|
||||
*p++ = (uint8_t)(lat_u >> 24); *p++ = (uint8_t)(lat_u >> 16);
|
||||
*p++ = (uint8_t)(lat_u >> 8); *p++ = (uint8_t)(lat_u);
|
||||
uint32_t lon_u = (uint32_t)longitude_tenmicrodeg;
|
||||
*p++ = (uint8_t)(lon_u >> 24); *p++ = (uint8_t)(lon_u >> 16);
|
||||
*p++ = (uint8_t)(lon_u >> 8); *p++ = (uint8_t)(lon_u);
|
||||
// PAI(1 bit) + Speed(15 bits), packed into 2 bytes: 0 = PAI false,
|
||||
// speed 0 - which is actually correct semantics for a STATIONARY
|
||||
// vehicle beacon, not just a placeholder.
|
||||
*p++ = 0x00; *p++ = 0x00;
|
||||
// Heading (16 bits, 0.1 degree units): 0 = due north / unavailable
|
||||
*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 (DENM UPER bytes) ----
|
||||
memcpy(p, its_payload, its_len);
|
||||
p += its_len;
|
||||
|
||||
return (int)(p - out);
|
||||
}
|
||||
@@ -0,0 +1,44 @@
|
||||
#ifndef GEONET_H
|
||||
#define GEONET_H
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
|
||||
// Wraps an ITS application payload (e.g. from denm_encode) with a minimal
|
||||
// GeoNetworking Basic Header + Common Header + Single-Hop-Broadcast
|
||||
// extended header (HeaderType=TSB(5), HeaderSubtype=SINGLE_HOP(0), per
|
||||
// ETSI EN 302 636-4-1 table 9), then prepends a BTP-B header addressed to
|
||||
// the DENM service port (2002).
|
||||
//
|
||||
// `mac` is the 6-byte pseudonym/link-layer address - pass the SAME address
|
||||
// you hand to dot11p_build_frame's src address, since GN_ADDR's MID field
|
||||
// (the last 6 bytes of the 8-byte GN_ADDR) is defined to BE that
|
||||
// link-layer address (EN 302 636-4-1 clause 9.5.1). `station_type` is the
|
||||
// 5-bit ITS-S type from the same clause (5 = passengerCar) and gets packed
|
||||
// into GN_ADDR alongside the address.
|
||||
//
|
||||
// `latitude_tenmicrodeg`/`longitude_tenmicrodeg` go into the Source Long
|
||||
// Position Vector (clause 9.5.2) as plain 32-bit signed big-endian fields -
|
||||
// NOT UPER bit-packed like the DENM payload's position fields, this is a
|
||||
// fixed-width binary protocol. Pass the SAME values you gave denm_encode's
|
||||
// eventPosition, so the GN-layer position and the DENM's own claimed
|
||||
// position agree.
|
||||
//
|
||||
// Deliberate simplification: real DENM dissemination normally uses
|
||||
// GeoBroadcast (GBC, HeaderType=4) so RSUs/OBUs can forward it across an
|
||||
// area - that needs a sequence number + geo-area fields this skeleton
|
||||
// doesn't build yet. Single-hop broadcast is simpler and is the
|
||||
// best-tested decode path in the receiver firmware you already have
|
||||
// working (same extended header shape as CAM). Fine for a single-vehicle
|
||||
// beacon; revisit if you need real multi-hop forwarding later.
|
||||
//
|
||||
// `btp_dest_port` is the BTP-B destination port for the service being carried
|
||||
// (ETSI TS 103 248): 2001 = CAM, 2002 = DENM, 2003 = MAPEM, 2004 = SPATEM, ...
|
||||
//
|
||||
// Returns bytes written, or -1 if out buffer too small.
|
||||
int geonet_wrap_shb(const uint8_t *its_payload, int its_len,
|
||||
const uint8_t mac[6], uint8_t station_type,
|
||||
int32_t latitude_tenmicrodeg, int32_t longitude_tenmicrodeg,
|
||||
uint16_t btp_dest_port,
|
||||
uint8_t *out, size_t out_len);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,114 @@
|
||||
#include "gn_unwrap.h"
|
||||
#include <string.h>
|
||||
|
||||
// Mirrors dot11p.c / geonet.c's constants and layout, in reverse. Keep these two files in sync
|
||||
// if either the TX-side frame shape or these constants change.
|
||||
#define GN_ETHERTYPE (0x8947)
|
||||
#define LLC_SNAP_HEADER_LEN (8)
|
||||
#define IEEE80211_HEADER_LEN (24) // non-QoS Data
|
||||
#define IEEE80211_QOS_CTRL_LEN (2) // extra field QoS Data frames add
|
||||
#define IEEE80211_FC_TYPE_DATA (2)
|
||||
#define IEEE80211_FC_QOS_SUBTYPE_BIT (0x08)
|
||||
|
||||
#define GN_BASIC_HEADER_LEN (4)
|
||||
#define GN_COMMON_HEADER_LEN (8)
|
||||
#define GN_SHB_EXT_HEADER_LEN (24) // Source Long Position Vector, geonet.c's SHB shape
|
||||
#define BTP_B_HEADER_LEN (4)
|
||||
|
||||
#define GN_HEADER_TYPE_TSB (5) // Topologically-Scoped Broadcast
|
||||
#define GN_HEADER_SUBTYPE_SINGLE_HOP (0)
|
||||
|
||||
#define BTP_DEST_PORT_CAM (2001) // ETSI TS 103 248
|
||||
|
||||
static const uint8_t s_llc_snap_prefix[6] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00};
|
||||
|
||||
bool gn_unwrap_cam(const uint8_t *frame, int frame_len,
|
||||
const uint8_t **out_cam, int *out_cam_len)
|
||||
{
|
||||
if (!frame || frame_len < IEEE80211_HEADER_LEN) {
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t fc0 = frame[0];
|
||||
uint8_t fc1 = frame[1];
|
||||
uint8_t type = (fc0 >> 2) & 0x03;
|
||||
uint8_t subtype = (fc0 >> 4) & 0x0F;
|
||||
bool to_ds = fc1 & 0x01;
|
||||
bool from_ds = fc1 & 0x02;
|
||||
|
||||
// Only plain broadcast Data frames, no WDS - matches what dot11p_build_frame ever produces
|
||||
// (and what real ITS-G5 hardware sends).
|
||||
if (type != IEEE80211_FC_TYPE_DATA || (to_ds && from_ds)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
int offset = IEEE80211_HEADER_LEN;
|
||||
if (subtype & IEEE80211_FC_QOS_SUBTYPE_BIT) {
|
||||
offset += IEEE80211_QOS_CTRL_LEN;
|
||||
}
|
||||
|
||||
if (frame_len < offset + LLC_SNAP_HEADER_LEN) {
|
||||
return false;
|
||||
}
|
||||
if (memcmp(frame + offset, s_llc_snap_prefix, sizeof(s_llc_snap_prefix)) != 0) {
|
||||
return false;
|
||||
}
|
||||
uint16_t ethertype = ((uint16_t)frame[offset + 6] << 8) | frame[offset + 7];
|
||||
if (ethertype != GN_ETHERTYPE) {
|
||||
return false;
|
||||
}
|
||||
offset += LLC_SNAP_HEADER_LEN;
|
||||
|
||||
// ---- GN Basic Header (4 bytes) ---- nothing here we need to validate for our purposes;
|
||||
// just skip it. (version/NextHeader in byte0, lifetime in byte2, RHL in byte3.)
|
||||
if (frame_len < offset + GN_BASIC_HEADER_LEN) {
|
||||
return false;
|
||||
}
|
||||
offset += GN_BASIC_HEADER_LEN;
|
||||
|
||||
// ---- GN Common Header (8 bytes) ----
|
||||
if (frame_len < offset + GN_COMMON_HEADER_LEN) {
|
||||
return false;
|
||||
}
|
||||
uint8_t next_header = (frame[offset + 0] >> 4) & 0x0F;
|
||||
uint8_t header_type = (frame[offset + 1] >> 4) & 0x0F;
|
||||
uint8_t header_subtype = frame[offset + 1] & 0x0F;
|
||||
if (next_header != 2 /* BTP-B */) {
|
||||
return false;
|
||||
}
|
||||
if (header_type != GN_HEADER_TYPE_TSB || header_subtype != GN_HEADER_SUBTYPE_SINGLE_HOP) {
|
||||
// Not a single-hop-broadcast frame - e.g. GeoBroadcast (DENM-style dissemination) or
|
||||
// something this project doesn't transmit/expect. Not an error, just not for us yet -
|
||||
// see gn_unwrap.h's note on scope.
|
||||
return false;
|
||||
}
|
||||
offset += GN_COMMON_HEADER_LEN;
|
||||
|
||||
// ---- SHB extended header (24 bytes) ---- skip straight past it, we don't need the
|
||||
// sender's claimed position/speed/heading here (the CAM payload has its own, more precise
|
||||
// versions of those same fields).
|
||||
if (frame_len < offset + GN_SHB_EXT_HEADER_LEN) {
|
||||
return false;
|
||||
}
|
||||
offset += GN_SHB_EXT_HEADER_LEN;
|
||||
|
||||
// ---- BTP-B header (4 bytes) ----
|
||||
if (frame_len < offset + BTP_B_HEADER_LEN) {
|
||||
return false;
|
||||
}
|
||||
uint16_t dest_port = ((uint16_t)frame[offset + 0] << 8) | frame[offset + 1];
|
||||
if (dest_port != BTP_DEST_PORT_CAM) {
|
||||
return false; // e.g. DENM (2002) - not decoded by this project yet
|
||||
}
|
||||
offset += BTP_B_HEADER_LEN;
|
||||
|
||||
// ---- Whatever's left is the CAM UPER payload ----
|
||||
int cam_len = frame_len - offset;
|
||||
if (cam_len <= 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
*out_cam = frame + offset;
|
||||
*out_cam_len = cam_len;
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,39 @@
|
||||
#ifndef GN_UNWRAP_H
|
||||
#define GN_UNWRAP_H
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
// Inverse of geonet_wrap_shb() + dot11p_build_frame(): takes a raw 802.11 frame as delivered by
|
||||
// the WiFi driver's promiscuous RX callback and strips 802.11 header -> LLC/SNAP -> GeoNetworking
|
||||
// Basic/Common/extended header -> BTP-B header, leaving just the ITS payload (CAM UPER bytes)
|
||||
// and the sender's station id (GN_ADDR MID).
|
||||
//
|
||||
// Deliberately narrow, matching what this project actually transmits: only handles the
|
||||
// Single-Hop-Broadcast (TSB, HeaderType=5/Subtype=0) extended header shape, same as
|
||||
// geonet_wrap_shb() builds - the same "best-tested decode path" rationale documented there.
|
||||
// A real receiver would also need GeoBroadcast (HeaderType=4, used by DENM dissemination in
|
||||
// real deployments) and possibly Beacon/GeoUnicast - out of scope for now since nothing this
|
||||
// project talks to sends those. Extend header_type handling here if that changes.
|
||||
//
|
||||
// Only accepts BTP-B destination port 2001 (CAM, per ETSI TS 103 248) - other ports (e.g. 2002
|
||||
// DENM) are silently rejected since the phone-side decoder only understands CAM right now.
|
||||
//
|
||||
// Returns true and fills *out_cam / *out_cam_len (pointing INTO the input frame buffer, not a
|
||||
// copy - valid only as long as `frame` is) if this was a well-formed, CAM-carrying SHB frame
|
||||
// this project can decode. Returns false otherwise (wrong ethertype, wrong header type, wrong
|
||||
// BTP port, truncated, or FCS/promiscuous-capture garbage - all common and expected on an
|
||||
// open-air capture, not logged as errors by the caller).
|
||||
//
|
||||
// No station id is extracted here on purpose: CAM's own ItsPduHeader.stationID (the first real
|
||||
// field inside the UPER payload this function hands back, per cam.c) is already the meaningful
|
||||
// application-level identifier - the Kotlin-side decoder reads it from there. The GN_ADDR MID
|
||||
// this frame also carries is a separate, link-layer-only pseudonym; extracting and forwarding
|
||||
// it too would just be a second, easily-confused "station id" for no benefit here.
|
||||
//
|
||||
// RSSI is NOT extracted here either - it comes from the promiscuous callback's own packet
|
||||
// metadata (wifi_pkt_rx_ctrl_t.rssi in main.c), not from anything inside the frame bytes.
|
||||
bool gn_unwrap_cam(const uint8_t *frame, int frame_len,
|
||||
const uint8_t **out_cam, int *out_cam_len);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,316 @@
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "freertos/queue.h"
|
||||
#include "driver/gpio.h"
|
||||
#include "esp_wifi.h"
|
||||
#include "esp_event.h"
|
||||
#include "esp_netif.h"
|
||||
#include "nvs_flash.h"
|
||||
#include "esp_log.h"
|
||||
#include "hal/modem_syscon_ll.h" // modem_syscon_ll_enable_fe_40m_clock() - see initialize_wifi
|
||||
#include "denm.h"
|
||||
#include "geonet.h"
|
||||
#include "dot11p.h"
|
||||
#include "tx_custom.h" // not called below - kept available for the QoS-Data/tx_custom path if
|
||||
// esp_wifi_80211_tx's non-QoS frame ever proves insufficient again
|
||||
#include "serial_link.h"
|
||||
#include "gn_unwrap.h"
|
||||
|
||||
static const char *TAG = "obu-tx";
|
||||
|
||||
// Phase 03: CAM is no longer built on this chip. The phone fuses its own GNSS+IMU, UPER-encodes
|
||||
// CAM itself, and hands the finished bytes down over serial_link (SERIAL_MSG_CAM_TX) - this
|
||||
// firmware's job on transmit shrinks to "GeoNetworking/BTP-wrap + 802.11-wrap + key the PA the
|
||||
// instant a CAM arrives." There is no on-chip transmit timer anymore; the phone's send cadence
|
||||
// (1 Hz baseline, faster near intersections/events - all decided app-side) IS the air cadence.
|
||||
// See cam.c/.h - no longer built (removed from CMakeLists), kept on disk for field-layout
|
||||
// reference only, since the phone's Kotlin encoder is a byte-exact port of it.
|
||||
//
|
||||
// On receive, this firmware now also runs a promiscuous callback (gn_unwrap.c strips
|
||||
// 802.11/LLC-SNAP/GeoNetworking/BTP-B down to the raw CAM UPER payload) and forwards every CAM
|
||||
// it hears over the same serial link (SERIAL_MSG_CAM_RX), for the phone's detection engine.
|
||||
// Single half-duplex radio doing both jobs, same as real ITS-G5 hardware.
|
||||
|
||||
// Target frequency: 5900 MHz (ITS-G5 G5-CCH, channel 180). This is what the
|
||||
// working Rust reference transmits on, proving the C5 PA reaches it despite the
|
||||
// 5885 datasheet max. The reference sets band-mode 5G, then phy_11p_set +
|
||||
// phy_change_channel(5900) directly - it does NOT call esp_wifi_set_channel at
|
||||
// all, so we don't either (channel 180 isn't a normal Wi-Fi channel anyway).
|
||||
#define TX_FREQ_MHZ 5900
|
||||
|
||||
// ---- CAM beacon profile (used for the GeoNetworking layer only now - see below) ----
|
||||
#define STATION_TYPE 5 // passengerCar (TS 102 894-2 StationType) - matches gn_addr's ST field
|
||||
#define BTP_PORT_CAM 2001 // BTP-B destination port for CAM (ETSI TS 103 248)
|
||||
|
||||
// Bench location, hardcoded since there's no GNSS module wired in yet and the unit is genuinely
|
||||
// stationary here: 53°33'16.8"N 10°01'20.6"E, in 1/10-microdegree units. Used ONLY for the
|
||||
// GeoNetworking Source Long Position Vector now (geonet_wrap_shb's own claimed position) - the
|
||||
// CAM payload's own referencePosition comes from the phone's real GNSS and can legitimately
|
||||
// differ from this bench placeholder until the GN layer is also given a real position source.
|
||||
// TODO: feed this from the phone too (e.g. a lightweight position update piggybacked on
|
||||
// SERIAL_MSG_CAM_TX, or a new small message type) instead of a fixed bench location.
|
||||
#define BENCH_LATITUDE_TENMICRODEG 535546667
|
||||
#define BENCH_LONGITUDE_TENMICRODEG 100223889
|
||||
|
||||
// Single source of truth for the pseudonym/link-layer address: used both as
|
||||
// the 802.11 source MAC (Addr2) and as GN_ADDR's MID field, since the GN
|
||||
// spec defines those as being the same address. Locally-administered bit
|
||||
// set (0x02) per normal MAC convention. Fixed/non-rotating for now - real
|
||||
// stacks rotate this every 5-15 min for privacy. Owned entirely by this firmware (not the
|
||||
// phone) per the Phase 03 design decision - simplest given the phone never needs to know it.
|
||||
static const uint8_t pseudonym_mac[6] = {0x02, 0x00, 0x00, 0x00, 0x00, 0x01};
|
||||
|
||||
// Undocumented libphy.a calls that push the radio into 802.11p OCB mode on
|
||||
// the 5.9 GHz ITS-G5 band. See docs/04-transmit-setup.md for source + what
|
||||
// to do if the linker can't find these symbols in your ESP-IDF version.
|
||||
extern void phy_11p_set(int enable, int unused);
|
||||
extern void phy_change_channel(int freq_mhz, int bw_mode, int sec_chan_offset, int unused);
|
||||
|
||||
// ============================================================================
|
||||
// ---- TX path: phone -> serial_link -> queue -> radio task -> air ----------
|
||||
// ============================================================================
|
||||
|
||||
// One CAM-to-transmit item. Fixed-size (no malloc) since SERIAL_LINK_MAX_PAYLOAD bounds it -
|
||||
// simplest safe option for a queue this small and this hot.
|
||||
typedef struct {
|
||||
uint8_t data[SERIAL_LINK_MAX_PAYLOAD];
|
||||
int len;
|
||||
} cam_tx_item_t;
|
||||
|
||||
static QueueHandle_t s_tx_queue;
|
||||
|
||||
// Called directly from serial_link's UART RX task the instant a checksummed SERIAL_MSG_CAM_TX
|
||||
// frame arrives - MUST be fast (documented in serial_link.h), so this only copies into a queue
|
||||
// item and returns; the actual GeoNetworking-wrap + 802.11-wrap + radio TX happens in
|
||||
// tx_radio_task below, off the UART parsing path entirely. xQueueSend with 0 timeout: if the
|
||||
// radio task is somehow behind, drop this CAM rather than stall UART frame parsing - the next
|
||||
// one is only ~1s (or less, at elevated rate) away regardless.
|
||||
static void on_cam_tx_from_phone(const uint8_t *cam_uper, int cam_len)
|
||||
{
|
||||
if (cam_len <= 0 || cam_len > SERIAL_LINK_MAX_PAYLOAD) {
|
||||
ESP_LOGW(TAG, "on_cam_tx_from_phone: bad length %d", cam_len);
|
||||
return;
|
||||
}
|
||||
cam_tx_item_t item;
|
||||
item.len = cam_len;
|
||||
memcpy(item.data, cam_uper, (size_t)cam_len);
|
||||
if (xQueueSend(s_tx_queue, &item, 0) != pdTRUE) {
|
||||
ESP_LOGW(TAG, "tx queue full, dropping CAM from phone");
|
||||
}
|
||||
}
|
||||
|
||||
static void tx_radio_task(void *arg)
|
||||
{
|
||||
(void)arg;
|
||||
cam_tx_item_t item;
|
||||
|
||||
while (1) {
|
||||
if (xQueueReceive(s_tx_queue, &item, portMAX_DELAY) != pdTRUE) {
|
||||
continue;
|
||||
}
|
||||
|
||||
uint8_t gn_payload[160];
|
||||
int gn_len = geonet_wrap_shb(item.data, item.len, pseudonym_mac, STATION_TYPE,
|
||||
BENCH_LATITUDE_TENMICRODEG, BENCH_LONGITUDE_TENMICRODEG,
|
||||
BTP_PORT_CAM, gn_payload, sizeof(gn_payload));
|
||||
if (gn_len <= 0) {
|
||||
ESP_LOGW(TAG, "geonet_wrap_shb failed (cam_len=%d)", item.len);
|
||||
continue;
|
||||
}
|
||||
|
||||
uint8_t frame[300];
|
||||
int frame_len = dot11p_build_frame(gn_payload, gn_len, pseudonym_mac, frame,
|
||||
sizeof(frame), false);
|
||||
if (frame_len <= 0) {
|
||||
ESP_LOGW(TAG, "dot11p_build_frame failed (gn_len=%d)", gn_len);
|
||||
continue;
|
||||
}
|
||||
|
||||
// Standard, well-tested raw-TX API with a non-QoS Data frame - same path validated
|
||||
// during Phase 2 bring-up (see git history for the tx_custom.c A/B test that led here).
|
||||
esp_err_t err = esp_wifi_80211_tx(WIFI_IF_STA, frame, frame_len, true);
|
||||
if (err != ESP_OK) {
|
||||
ESP_LOGW(TAG, "esp_wifi_80211_tx failed: %d", err);
|
||||
} else {
|
||||
ESP_LOGI(TAG, "CAM sent (%d bytes) @ %d MHz", frame_len, TX_FREQ_MHZ);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// ---- RX path: air -> promiscuous cb -> queue -> forward task -> serial_link
|
||||
// ============================================================================
|
||||
|
||||
// Promiscuous RX callbacks run in the WiFi driver's own task context and must stay short - so,
|
||||
// same pattern as the TX side and as the reference sniffer firmware (cmd_sniffer.c's
|
||||
// queue_packet), this just copies the frame and queues it; gn_unwrap_cam() and the serial write
|
||||
// both happen in rx_forward_task instead.
|
||||
typedef struct {
|
||||
uint8_t data[400]; // generous vs. our own ~300-byte TX frames; longer frames are truncated
|
||||
int len;
|
||||
int8_t rssi;
|
||||
} rx_item_t;
|
||||
|
||||
static QueueHandle_t s_rx_queue;
|
||||
|
||||
static void wifi_promisc_rx_cb(void *recv_buf, wifi_promiscuous_pkt_type_t type)
|
||||
{
|
||||
if (type == WIFI_PKT_MISC) {
|
||||
return; // no payload of interest, mirrors cmd_sniffer.c's handling
|
||||
}
|
||||
wifi_promiscuous_pkt_t *packet = (wifi_promiscuous_pkt_t *)recv_buf;
|
||||
if (packet->rx_ctrl.rx_state) {
|
||||
return; // frame had an error (mirrors cmd_sniffer.c)
|
||||
}
|
||||
|
||||
#if CONFIG_SOC_WIFI_HE_SUPPORT
|
||||
int length = packet->rx_ctrl.dump_len;
|
||||
#else
|
||||
int length = packet->rx_ctrl.sig_len - 4 /* FCS */;
|
||||
#endif
|
||||
if (length <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
rx_item_t item;
|
||||
item.len = length > (int)sizeof(item.data) ? (int)sizeof(item.data) : length;
|
||||
memcpy(item.data, packet->payload, (size_t)item.len);
|
||||
item.rssi = packet->rx_ctrl.rssi;
|
||||
|
||||
// 0 timeout: never block the WiFi driver's own task waiting for queue space.
|
||||
xQueueSend(s_rx_queue, &item, 0);
|
||||
}
|
||||
|
||||
static void rx_forward_task(void *arg)
|
||||
{
|
||||
(void)arg;
|
||||
rx_item_t item;
|
||||
|
||||
while (1) {
|
||||
if (xQueueReceive(s_rx_queue, &item, portMAX_DELAY) != pdTRUE) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const uint8_t *cam = NULL;
|
||||
int cam_len = 0;
|
||||
// Most promiscuously-captured frames are NOT CAM (management/control frames, other
|
||||
// ITS-G5 traffic types, our own loopback if the driver echoes it) - gn_unwrap_cam
|
||||
// returning false here is the common case, not an error.
|
||||
if (gn_unwrap_cam(item.data, item.len, &cam, &cam_len)) {
|
||||
serial_link_send_cam_rx(item.rssi, cam, cam_len);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
|
||||
void app_main(void)
|
||||
{
|
||||
ESP_ERROR_CHECK(nvs_flash_init());
|
||||
ESP_ERROR_CHECK(esp_netif_init());
|
||||
ESP_ERROR_CHECK(esp_event_loop_create_default());
|
||||
|
||||
s_tx_queue = xQueueCreate(4, sizeof(cam_tx_item_t));
|
||||
s_rx_queue = xQueueCreate(8, sizeof(rx_item_t));
|
||||
if (!s_tx_queue || !s_rx_queue) {
|
||||
ESP_LOGE(TAG, "queue creation failed - halting");
|
||||
return;
|
||||
}
|
||||
|
||||
// Enable the modem FRONT-END 40 MHz clock BEFORE esp_wifi_init(). This is
|
||||
// the one step the proven-working receiver firmware
|
||||
// (its-g5-receiver-firmware_txenabled, main/main.c -> initialize_wifi())
|
||||
// performs that this OBU was missing. Without the FE clock enabled the
|
||||
// 5 GHz front-end / transmit chain is not fully clocked - which matches the
|
||||
// exact symptom here: the radio calibrates (boot RF ping) and receives
|
||||
// fine, but data frames are accepted by the API and never actually key the
|
||||
// PA. This is a low-level modem_syscon register write via the HAL LL layer,
|
||||
// copied verbatim from the reference firmware.
|
||||
modem_syscon_ll_enable_fe_40m_clock(&MODEM_SYSCON, 1);
|
||||
|
||||
wifi_init_config_t wifi_cfg = WIFI_INIT_CONFIG_DEFAULT();
|
||||
ESP_ERROR_CHECK(esp_wifi_init(&wifi_cfg));
|
||||
ESP_ERROR_CHECK(esp_wifi_set_storage(WIFI_STORAGE_RAM)); // match reference initialize_wifi()
|
||||
ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
|
||||
ESP_ERROR_CHECK(esp_wifi_start());
|
||||
|
||||
// ---- Regulatory / TX-authorization override -----------------------------
|
||||
// THE fix for "RX works but TX is silent". By default the driver uses
|
||||
// WIFI_COUNTRY_POLICY_AUTO, whose 5 GHz regulatory table does NOT authorize
|
||||
// transmit on the 5.9 GHz ITS band (and treats DFS channels as no-IR /
|
||||
// radar-gated). Receiving is never gated - which is exactly why the sniffer
|
||||
// hears traffic but our own frames never key the PA, and why the only RF
|
||||
// seen from this board is the uninhibited PHY-calibration burst at boot.
|
||||
//
|
||||
// Switching to WIFI_COUNTRY_POLICY_MANUAL with an explicit 5 GHz channel
|
||||
// mask (wifi_5g_channel_mask, which only takes effect under manual policy)
|
||||
// tells the driver these channels are permitted and lifts the transmit
|
||||
// gate. Manual policy = the operator asserts regulatory responsibility, which is
|
||||
// appropriate for licensed/university research on the ITS band.
|
||||
wifi_country_t ctry = {
|
||||
.cc = "US", // nominal under manual policy
|
||||
.schan = 1,
|
||||
.nchan = 11,
|
||||
.policy = WIFI_COUNTRY_POLICY_MANUAL,
|
||||
.wifi_5g_channel_mask = 0x1FFFFFFE, // all 5 GHz channels, bits 1..28 (incl. 140 and 177)
|
||||
};
|
||||
esp_err_t ctry_err = esp_wifi_set_country(&ctry);
|
||||
if (ctry_err != ESP_OK) {
|
||||
ESP_LOGW(TAG, "esp_wifi_set_country(MANUAL) failed: %d (continuing)", ctry_err);
|
||||
}
|
||||
// Ensure the PA runs at full configured power (not a reduced regulatory
|
||||
// default). Units are 0.25 dBm; 80 = 20 dBm.
|
||||
esp_wifi_set_max_tx_power(80);
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
// Force the dual-band C5 onto its 5 GHz PHY. This MUST be called after
|
||||
// esp_wifi_start() - calling it before returns ESP_ERR_WIFI_NOT_STARTED
|
||||
// (0x3002 / 12290). Locking the band to 5G explicitly keeps the driver
|
||||
// from ever falling back to 2.4 GHz ch1 (the old "stuck at primary=1"
|
||||
// symptom), which would key the wrong PHY and make us inaudible to a
|
||||
// 5.9 GHz sniffer/peer. Valid 5 GHz channels on the C5 are 36..177. Not
|
||||
// ESP_ERROR_CHECK'd: log and continue if a given IDF build differs.
|
||||
esp_err_t band_err = esp_wifi_set_band_mode(WIFI_BAND_MODE_5G_ONLY);
|
||||
if (band_err != ESP_OK) {
|
||||
ESP_LOGW(TAG, "esp_wifi_set_band_mode(5G_ONLY) failed: %d (continuing)", band_err);
|
||||
}
|
||||
|
||||
// Disable Wi-Fi power save. An unassociated STA with the default
|
||||
// WIFI_PS_MIN_MODEM power save sleeps its radio between beacons it will
|
||||
// never receive (we're not joined to any AP), and drops outbound raw
|
||||
// frames while asleep. Also matters for RX now: a sleeping radio misses
|
||||
// incoming CAMs just as easily as it drops outbound ones. Must be called
|
||||
// after esp_wifi_start().
|
||||
ESP_ERROR_CHECK(esp_wifi_set_ps(WIFI_PS_NONE));
|
||||
|
||||
// Register the promiscuous RX callback BEFORE enabling promiscuous mode, so there's no
|
||||
// window where promiscuous mode is on but nothing is registered to receive frames from it.
|
||||
ESP_ERROR_CHECK(esp_wifi_set_promiscuous_rx_cb(wifi_promisc_rx_cb));
|
||||
|
||||
// Enable promiscuous mode. Doubles as the fix for raw-TX being silently dropped
|
||||
// (ESP-IDF only actually emits raw frames when the MAC is promiscuous or associated to an
|
||||
// AP - plain unassociated STA is neither) AND as what makes RX possible at all outside a
|
||||
// joined BSS. One radio, one mode, both jobs - see file header comment.
|
||||
ESP_ERROR_CHECK(esp_wifi_set_promiscuous(true));
|
||||
|
||||
// Force 802.11p OCB mode on the ITS-G5 channel, exactly like the working
|
||||
// Rust reference (esp32-c_its-companion, src/radio.rs setup_wifi_sniffer):
|
||||
// enable 802.11p, then jump straight to the target frequency. With band-mode
|
||||
// already locked to 5 GHz above, NO esp_wifi_set_channel priming is needed -
|
||||
// channel 180 (5900 MHz) isn't a normal Wi-Fi channel anyway. phy_change_channel
|
||||
// takes the frequency in MHz.
|
||||
ESP_LOGI(TAG, "about to call phy_11p_set...");
|
||||
phy_11p_set(1, 0);
|
||||
ESP_LOGI(TAG, "phy_11p_set returned, about to call phy_change_channel(%d)...", TX_FREQ_MHZ);
|
||||
phy_change_channel(TX_FREQ_MHZ, 1, 0, 0);
|
||||
ESP_LOGI(TAG, "phy_change_channel returned");
|
||||
|
||||
xTaskCreate(tx_radio_task, "tx_radio", 4096, NULL, 6, NULL);
|
||||
xTaskCreate(rx_forward_task, "rx_forward", 4096, NULL, 5, NULL);
|
||||
serial_link_init(on_cam_tx_from_phone);
|
||||
|
||||
ESP_LOGW(TAG, "OCB @ %d MHz - TX/RX armed, driven by serial_link (no on-chip TX timer)",
|
||||
TX_FREQ_MHZ);
|
||||
}
|
||||
@@ -0,0 +1,199 @@
|
||||
#include "serial_link.h"
|
||||
#include <string.h>
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "driver/uart.h"
|
||||
#include "esp_log.h"
|
||||
|
||||
static const char *TAG = "serial_link";
|
||||
|
||||
#define SYNC0 0xAA
|
||||
#define SYNC1 0x55
|
||||
|
||||
static serial_link_cam_tx_cb_t s_on_cam_tx;
|
||||
|
||||
// ---- CRC-16/CCITT-FALSE (poly 0x1021, init 0xFFFF, no reflect, no xorout) ----
|
||||
// Bytewise (no table) - frames here are at most SERIAL_LINK_MAX_PAYLOAD + 3 bytes, so table
|
||||
// lookup isn't worth the flash/RAM tradeoff. MUST match the Kotlin-side implementation exactly
|
||||
// (see app SerialFrame.kt) or every frame will be silently rejected as corrupt.
|
||||
static uint16_t crc16_ccitt_false(const uint8_t *data, size_t len)
|
||||
{
|
||||
uint16_t crc = 0xFFFF;
|
||||
for (size_t i = 0; i < len; i++) {
|
||||
crc ^= (uint16_t)data[i] << 8;
|
||||
for (int b = 0; b < 8; b++) {
|
||||
crc = (crc & 0x8000) ? (uint16_t)((crc << 1) ^ 0x1021) : (uint16_t)(crc << 1);
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
static bool send_frame(uint8_t type, const uint8_t *payload, int len)
|
||||
{
|
||||
if (len < 0 || len > SERIAL_LINK_MAX_PAYLOAD) {
|
||||
ESP_LOGW(TAG, "send_frame: payload too large (%d)", len);
|
||||
return false;
|
||||
}
|
||||
|
||||
// type(1) + length(2) + payload(len) is what the CRC covers.
|
||||
uint8_t head[3];
|
||||
head[0] = type;
|
||||
head[1] = (uint8_t)(len & 0xFF);
|
||||
head[2] = (uint8_t)((len >> 8) & 0xFF);
|
||||
|
||||
uint16_t crc;
|
||||
{
|
||||
// Compute CRC over head+payload without a combined buffer copy: CRC is a running
|
||||
// state, so feed it in two calls worth of bytes by concatenating into a small stack
|
||||
// buffer (payload is capped at SERIAL_LINK_MAX_PAYLOAD, so head+payload comfortably
|
||||
// fits on the stack).
|
||||
uint8_t crc_buf[3 + SERIAL_LINK_MAX_PAYLOAD];
|
||||
memcpy(crc_buf, head, 3);
|
||||
if (len > 0) memcpy(crc_buf + 3, payload, (size_t)len);
|
||||
crc = crc16_ccitt_false(crc_buf, (size_t)(3 + len));
|
||||
}
|
||||
|
||||
uint8_t sync[2] = {SYNC0, SYNC1};
|
||||
uint8_t crc_bytes[2] = {(uint8_t)(crc & 0xFF), (uint8_t)((crc >> 8) & 0xFF)};
|
||||
|
||||
// Four separate writes rather than one assembled buffer - simplest given payload is
|
||||
// already wherever the caller has it (avoids a second copy of up to 160 bytes).
|
||||
int wrote = 0;
|
||||
wrote += uart_write_bytes(SERIAL_LINK_UART_NUM, sync, sizeof(sync));
|
||||
wrote += uart_write_bytes(SERIAL_LINK_UART_NUM, head, sizeof(head));
|
||||
if (len > 0) wrote += uart_write_bytes(SERIAL_LINK_UART_NUM, payload, (size_t)len);
|
||||
wrote += uart_write_bytes(SERIAL_LINK_UART_NUM, crc_bytes, sizeof(crc_bytes));
|
||||
|
||||
return wrote == (int)(sizeof(sync) + sizeof(head) + len + sizeof(crc_bytes));
|
||||
}
|
||||
|
||||
bool serial_link_send_cam_rx(int8_t rssi, const uint8_t *cam_uper, int cam_len)
|
||||
{
|
||||
if (cam_len < 0 || cam_len > SERIAL_LINK_MAX_PAYLOAD - 1) {
|
||||
ESP_LOGW(TAG, "send_cam_rx: cam_len too large (%d)", cam_len);
|
||||
return false;
|
||||
}
|
||||
uint8_t payload[SERIAL_LINK_MAX_PAYLOAD];
|
||||
payload[0] = (uint8_t)rssi;
|
||||
memcpy(payload + 1, cam_uper, (size_t)cam_len);
|
||||
return send_frame(SERIAL_MSG_CAM_RX, payload, 1 + cam_len);
|
||||
}
|
||||
|
||||
bool serial_link_send_status(uint8_t status)
|
||||
{
|
||||
return send_frame(SERIAL_MSG_STATUS, &status, 1);
|
||||
}
|
||||
|
||||
// ---- RX framing state machine ----
|
||||
// Runs in its own task, byte-at-a-time off the UART driver's RX ring buffer (via
|
||||
// uart_read_bytes with a short timeout, not raw ISR access - simplest correct option for a
|
||||
// link this slow/small; revisit if CAM traffic volume ever makes this a bottleneck).
|
||||
typedef enum {
|
||||
WAIT_SYNC0,
|
||||
WAIT_SYNC1,
|
||||
WAIT_TYPE,
|
||||
WAIT_LEN_LO,
|
||||
WAIT_LEN_HI,
|
||||
WAIT_PAYLOAD,
|
||||
WAIT_CRC_LO,
|
||||
WAIT_CRC_HI,
|
||||
} rx_state_t;
|
||||
|
||||
static void rx_task(void *arg)
|
||||
{
|
||||
(void)arg;
|
||||
rx_state_t state = WAIT_SYNC0;
|
||||
uint8_t type = 0;
|
||||
uint16_t len = 0;
|
||||
uint16_t payload_idx = 0;
|
||||
uint8_t payload[SERIAL_LINK_MAX_PAYLOAD];
|
||||
uint16_t crc_recv = 0;
|
||||
|
||||
uint8_t byte;
|
||||
while (1) {
|
||||
int n = uart_read_bytes(SERIAL_LINK_UART_NUM, &byte, 1, pdMS_TO_TICKS(50));
|
||||
if (n <= 0) continue;
|
||||
|
||||
switch (state) {
|
||||
case WAIT_SYNC0:
|
||||
state = (byte == SYNC0) ? WAIT_SYNC1 : WAIT_SYNC0;
|
||||
break;
|
||||
case WAIT_SYNC1:
|
||||
state = (byte == SYNC1) ? WAIT_TYPE : (byte == SYNC0 ? WAIT_SYNC1 : WAIT_SYNC0);
|
||||
break;
|
||||
case WAIT_TYPE:
|
||||
type = byte;
|
||||
state = WAIT_LEN_LO;
|
||||
break;
|
||||
case WAIT_LEN_LO:
|
||||
len = byte;
|
||||
state = WAIT_LEN_HI;
|
||||
break;
|
||||
case WAIT_LEN_HI:
|
||||
len |= (uint16_t)byte << 8;
|
||||
if (len > SERIAL_LINK_MAX_PAYLOAD) {
|
||||
ESP_LOGW(TAG, "rx: length %u exceeds max, resyncing", len);
|
||||
state = WAIT_SYNC0; // can't trust this frame boundary at all - drop to resync
|
||||
} else if (len == 0) {
|
||||
payload_idx = 0;
|
||||
state = WAIT_CRC_LO;
|
||||
} else {
|
||||
payload_idx = 0;
|
||||
state = WAIT_PAYLOAD;
|
||||
}
|
||||
break;
|
||||
case WAIT_PAYLOAD:
|
||||
payload[payload_idx++] = byte;
|
||||
if (payload_idx >= len) state = WAIT_CRC_LO;
|
||||
break;
|
||||
case WAIT_CRC_LO:
|
||||
crc_recv = byte;
|
||||
state = WAIT_CRC_HI;
|
||||
break;
|
||||
case WAIT_CRC_HI: {
|
||||
crc_recv |= (uint16_t)byte << 8;
|
||||
|
||||
uint8_t crc_buf[3 + SERIAL_LINK_MAX_PAYLOAD];
|
||||
crc_buf[0] = type;
|
||||
crc_buf[1] = (uint8_t)(len & 0xFF);
|
||||
crc_buf[2] = (uint8_t)((len >> 8) & 0xFF);
|
||||
if (len > 0) memcpy(crc_buf + 3, payload, len);
|
||||
uint16_t crc_calc = crc16_ccitt_false(crc_buf, (size_t)(3 + len));
|
||||
|
||||
if (crc_calc == crc_recv) {
|
||||
if (type == SERIAL_MSG_CAM_TX && s_on_cam_tx) {
|
||||
s_on_cam_tx(payload, len);
|
||||
} else if (type != SERIAL_MSG_CAM_TX) {
|
||||
ESP_LOGW(TAG, "rx: unexpected frame type 0x%02x from phone, ignoring", type);
|
||||
}
|
||||
} else {
|
||||
ESP_LOGW(TAG, "rx: CRC mismatch (got %04x want %04x), dropping frame", crc_recv, crc_calc);
|
||||
}
|
||||
state = WAIT_SYNC0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void serial_link_init(serial_link_cam_tx_cb_t on_cam_tx)
|
||||
{
|
||||
s_on_cam_tx = on_cam_tx;
|
||||
|
||||
uart_config_t cfg = {
|
||||
.baud_rate = SERIAL_LINK_BAUD,
|
||||
.data_bits = UART_DATA_8_BITS,
|
||||
.parity = UART_PARITY_DISABLE,
|
||||
.stop_bits = UART_STOP_BITS_1,
|
||||
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
|
||||
.source_clk = UART_SCLK_DEFAULT,
|
||||
};
|
||||
ESP_ERROR_CHECK(uart_driver_install(SERIAL_LINK_UART_NUM, 1024, 1024, 0, NULL, 0));
|
||||
ESP_ERROR_CHECK(uart_param_config(SERIAL_LINK_UART_NUM, &cfg));
|
||||
ESP_ERROR_CHECK(uart_set_pin(SERIAL_LINK_UART_NUM, SERIAL_LINK_TX_GPIO, SERIAL_LINK_RX_GPIO,
|
||||
UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
|
||||
|
||||
xTaskCreate(rx_task, "serial_link_rx", 4096, NULL, 6, NULL);
|
||||
ESP_LOGI(TAG, "serial_link up on UART%d, TX=GPIO%d RX=GPIO%d @ %d baud",
|
||||
SERIAL_LINK_UART_NUM, SERIAL_LINK_TX_GPIO, SERIAL_LINK_RX_GPIO, SERIAL_LINK_BAUD);
|
||||
}
|
||||
@@ -0,0 +1,63 @@
|
||||
#ifndef SERIAL_LINK_H
|
||||
#define SERIAL_LINK_H
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
// Binary framing for the phone <-> ESP32-C5 link (Phase 03). Deliberately NOT the same UART as
|
||||
// the ESP-IDF console/ESP_LOG output (UART0, see sdkconfig CONFIG_ESP_CONSOLE_UART_NUM=0) -
|
||||
// mixing binary frames with human-readable log text on one wire would corrupt both. This runs
|
||||
// on a dedicated UART (see SERIAL_LINK_UART_NUM / TX / RX pins below - CHANGE THESE to match
|
||||
// your board's actual wiring from the USB-C connector's UART bridge to ESP32-C5 GPIOs).
|
||||
//
|
||||
// Frame format (both directions, symmetric):
|
||||
// [0xAA][0x55][type:1][length:2 LE][payload: length bytes][crc16:2 LE]
|
||||
// CRC16 is CRC-16/CCITT-FALSE (poly 0x1021, init 0xFFFF, no reflect, no xorout), computed over
|
||||
// type + length + payload only (not the two sync bytes). Same algorithm must be used on the
|
||||
// Kotlin side (see app SerialFrame.kt) - frames that don't checksum are silently dropped.
|
||||
//
|
||||
// Direction / types:
|
||||
// SERIAL_MSG_CAM_TX (0x01), phone -> ESP32: payload is a raw CAM UPER byte string, already
|
||||
// built by the phone (position/speed/heading/yaw rate baked in). On receipt the ESP32
|
||||
// immediately GeoNetworking-wraps and transmits it - this IS the transmit clock now, there
|
||||
// is no independent on-chip timer. See main.c's rx-driven tx path.
|
||||
// SERIAL_MSG_CAM_RX (0x02), ESP32 -> phone: payload is [rssi:1 signed][CAM UPER bytes...] - a
|
||||
// CAM received over the air, already stripped of its 802.11/LLC-SNAP/GeoNetworking/BTP-B
|
||||
// framing by gn_unwrap.c. The phone never sees raw 802.11 frames. No station id is carried
|
||||
// separately - CAM's own ItsPduHeader.stationID (the first field inside the UPER bytes) is
|
||||
// already the meaningful identifier; see gn_unwrap.h for why a second one isn't added here.
|
||||
// SERIAL_MSG_STATUS (0x03), ESP32 -> phone: 1-byte heartbeat (0 = ok), sent periodically so
|
||||
// the phone can distinguish "link idle" from "link dead" independent of CAM traffic.
|
||||
#define SERIAL_MSG_CAM_TX 0x01
|
||||
#define SERIAL_MSG_CAM_RX 0x02
|
||||
#define SERIAL_MSG_STATUS 0x03
|
||||
|
||||
// CHANGE THESE to match your board's actual USB-C -> UART bridge wiring. UART0 is already
|
||||
// claimed by the console/ESP_LOG; picking UART1 here to stay clear of it. These are common
|
||||
// free GPIOs on ESP32-C5 devkits but are NOT guaranteed free on your specific board - check
|
||||
// your schematic before flashing.
|
||||
#define SERIAL_LINK_UART_NUM 1
|
||||
#define SERIAL_LINK_TX_GPIO 4
|
||||
#define SERIAL_LINK_RX_GPIO 5
|
||||
#define SERIAL_LINK_BAUD 115200
|
||||
|
||||
// Max CAM payload this link will carry. cam.c sizes its own encode buffer at 96 bytes; 160
|
||||
// gives headroom for the RX path's extra station_id+rssi prefix plus margin.
|
||||
#define SERIAL_LINK_MAX_PAYLOAD 160
|
||||
|
||||
// Initializes the dedicated UART and its background RX-framing task. Call once from app_main,
|
||||
// after nvs/event loop init. `on_cam_tx` is invoked (from the RX task's context - keep it fast,
|
||||
// it blocks the next frame's parsing) whenever a complete, checksummed SERIAL_MSG_CAM_TX frame
|
||||
// arrives from the phone.
|
||||
typedef void (*serial_link_cam_tx_cb_t)(const uint8_t *cam_uper, int cam_len);
|
||||
void serial_link_init(serial_link_cam_tx_cb_t on_cam_tx);
|
||||
|
||||
// Sends a SERIAL_MSG_CAM_RX frame to the phone: rssi + the CAM UPER bytes gn_unwrap.c extracted
|
||||
// from an over-the-air frame. Returns true if the frame was written to the UART (not an
|
||||
// end-to-end ack - the phone may still drop it, e.g. serial buffer overrun).
|
||||
bool serial_link_send_cam_rx(int8_t rssi, const uint8_t *cam_uper, int cam_len);
|
||||
|
||||
// Sends a 1-byte SERIAL_MSG_STATUS heartbeat frame.
|
||||
bool serial_link_send_status(uint8_t status);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,196 @@
|
||||
// Copied verbatim (no logic changes) from opentrafficmap/its-g5-receiver-firmware_txenabled,
|
||||
// main/tx_custom.c (https://codeberg.org/opentrafficmap/its-g5-receiver-firmware_txenabled),
|
||||
// same authors as the receiver firmware (V2X2MAP) already used on the RX side of this
|
||||
// project. Same chip (ESP32-C5), same class of problem (getting a raw 802.11 frame past
|
||||
// esp_wifi_80211_tx()'s built-in frame-type gate), and a proven-different approach from our
|
||||
// own abandoned main/wifi_patches.c attempt - see docs/04-transmit-setup.md for why that one
|
||||
// didn't work and why this one is expected to.
|
||||
//
|
||||
// WHAT THIS DOES DIFFERENTLY FROM esp_wifi_80211_tx(): it doesn't call the public API at all.
|
||||
// It reaches one layer deeper into the closed WiFi driver - ic_ebuf_alloc() (allocates an
|
||||
// internal driver buffer), ieee80211_post_hmac_tx() (submits that buffer straight to the MAC
|
||||
// for transmission) - and never goes through the code path that contains the QoS-frame-type
|
||||
// sanity check that was rejecting us. Notice line "esp_err_t result = 0;//ieee80211_raw_frame_
|
||||
// sanity_check(...)" below: the upstream authors don't override that check (like our old
|
||||
// wifi_patches.c tried to), they just never call the function that calls it.
|
||||
//
|
||||
// REAL RISK, carried over from upstream, not introduced by us: this skips ALL frame-type and
|
||||
// sanity validation, same caveat as our old override attempt. A malformed frame from a bug
|
||||
// elsewhere in our own code could behave worse (silent corruption, crash) than a clean
|
||||
// rejection.
|
||||
//
|
||||
// UNVERIFIED FOR OUR EXACT TOOLCHAIN - things worth checking before trusting this blindly:
|
||||
// 1. The symbols this depends on (ieee80211_post_hmac_tx, ic_ebuf_alloc, ic_get_default_sched,
|
||||
// g_osi_funcs_p, g_wifi_global_lock) are undocumented/internal. We confirmed via `nm`
|
||||
// earlier that ieee80211_raw_frame_sanity_check exists in OUR esp32c5/IDF libnet80211.a -
|
||||
// we have NOT yet independently confirmed these other four/five symbols exist in our
|
||||
// exact ESP-IDF version (as opposed to whatever version the upstream repo's pinned
|
||||
// esp-idf submodule uses). If the linker can't find one of these, that's the first thing
|
||||
// to check - see docs/04-transmit-setup.md for the nm command.
|
||||
// 2. x_eb_txdesc_t / x_middle_data_t / x_ebuf_t below are REVERSE-ENGINEERED struct layouts
|
||||
// of closed-source internal WiFi driver types, pinned only by a sizeof() static_assert -
|
||||
// that assert catches a total-size mismatch but NOT a field-order/semantic mismatch if a
|
||||
// different IDF version shuffled internal fields while keeping the same total size. If our
|
||||
// ESP-IDF version differs meaningfully from upstream's, this could compile and link fine
|
||||
// but write to the wrong offsets internally. Worth checking `idf.py --version` against
|
||||
// whatever esp-idf commit opentrafficmap's repo has pinned as a submodule, as a rough
|
||||
// compatibility signal (not a guarantee either way).
|
||||
#include "esp_private/wifi_os_adapter.h"
|
||||
#include "esp_wifi.h"
|
||||
|
||||
#include "tx_custom.h"
|
||||
|
||||
esp_err_t ieee80211_raw_frame_sanity_check(wifi_interface_t ifx, const void *buffer, int32_t len, bool en_sys_seq);
|
||||
esp_err_t ieee80211_post_hmac_tx(void *ebuf);
|
||||
void *ic_ebuf_alloc(const void *packet, uint32_t unknown, uint32_t len);
|
||||
void *ic_get_default_sched(void);
|
||||
|
||||
extern wifi_osi_funcs_t *g_osi_funcs_p;
|
||||
extern void *g_wifi_global_lock;
|
||||
|
||||
typedef struct x_eb_txdesc
|
||||
{
|
||||
uint32_t flags;
|
||||
uint32_t field_4;
|
||||
uint32_t field_8;
|
||||
uint8_t rate;
|
||||
uint8_t field_d;
|
||||
uint8_t field_e;
|
||||
uint8_t field_f;
|
||||
uint32_t field_10;
|
||||
uint32_t field_14;
|
||||
uint32_t timestamp;
|
||||
void* sched;
|
||||
uint32_t field_20;
|
||||
uint32_t field_24;
|
||||
uint32_t field_28;
|
||||
union {
|
||||
uint32_t field_2c_32;
|
||||
struct {
|
||||
uint8_t field_2c;
|
||||
uint8_t field_2d;
|
||||
uint8_t field_2e;
|
||||
uint8_t field_2f;
|
||||
};
|
||||
};
|
||||
union {
|
||||
uint32_t field_30_32;
|
||||
struct {
|
||||
uint8_t field_30;
|
||||
uint8_t field_31;
|
||||
uint8_t field_32;
|
||||
uint8_t field_33;
|
||||
};
|
||||
};
|
||||
uint32_t field_34;
|
||||
uint32_t field_38;
|
||||
uint32_t field_3c;
|
||||
uint32_t field_40;
|
||||
uint32_t field_44;
|
||||
} x_eb_txdesc_t;
|
||||
static_assert(sizeof(x_eb_txdesc_t) == 0x48);
|
||||
|
||||
typedef struct x_middle_data
|
||||
{
|
||||
uint32_t field_40;
|
||||
uint8_t* buf;
|
||||
uint32_t field_48;
|
||||
uint32_t field_4c;
|
||||
} x_middle_data_t;
|
||||
static_assert(sizeof(x_middle_data_t) == 0x10);
|
||||
|
||||
typedef struct x_ebuf
|
||||
{
|
||||
uint32_t field_0;
|
||||
x_middle_data_t* ds_head;
|
||||
x_middle_data_t* ds_tail;
|
||||
uint16_t field_c;
|
||||
uint16_t field_e;
|
||||
uint32_t extra_data_start;
|
||||
uint16_t header_length;
|
||||
uint32_t data_length;
|
||||
uint16_t field_1c;
|
||||
uint8_t alloc_type;
|
||||
uint8_t field_1f;
|
||||
uint32_t field_20;
|
||||
uint8_t field_24;
|
||||
uint8_t field_25;
|
||||
uint8_t field_26;
|
||||
uint8_t field_27;
|
||||
uint32_t field_28;
|
||||
uint8_t field_2c;
|
||||
uint32_t field_30;
|
||||
uint32_t next_free;
|
||||
x_eb_txdesc_t* txdesc;
|
||||
uint16_t field_3c;
|
||||
uint8_t field_3e;
|
||||
uint8_t field_3f;
|
||||
} x_ebuf_t;
|
||||
static_assert(sizeof(x_ebuf_t) == 0x40);
|
||||
|
||||
esp_err_t esp_wifi_80211_tx_custom(wifi_interface_t ifx, const void *buffer, int32_t len, bool en_sys_seq, wifi_tx_rate_config_t *tx_rate_config, wifi_band_t band, wifi_bandwidth_t bw)
|
||||
{
|
||||
esp_err_t result = 0;//ieee80211_raw_frame_sanity_check(ifx, buffer, len, en_sys_seq);
|
||||
|
||||
if (!result)
|
||||
{
|
||||
g_osi_funcs_p->_mutex_lock(g_wifi_global_lock);
|
||||
x_ebuf_t* eb = ic_ebuf_alloc(buffer, 1, len);
|
||||
|
||||
if (eb)
|
||||
{
|
||||
//eb->data_length = len - 0x1a;
|
||||
eb->data_length = 0;
|
||||
x_eb_txdesc_t *txdesc_1 = eb->txdesc;
|
||||
//eb->header_length = 0x1a;
|
||||
eb->header_length = len;
|
||||
txdesc_1->flags |= 0x4000;
|
||||
txdesc_1->sched = ic_get_default_sched();
|
||||
wifi_phy_rate_t rate = tx_rate_config->rate;
|
||||
x_eb_txdesc_t *txdesc = eb->txdesc;
|
||||
|
||||
if (rate)
|
||||
txdesc->rate = (char)rate;
|
||||
else if (band != WIFI_BAND_5G)
|
||||
txdesc->rate = 0;
|
||||
else
|
||||
txdesc->rate = (char)WIFI_PHY_RATE_6M;
|
||||
|
||||
wifi_phy_mode_t phymode = tx_rate_config->phymode;
|
||||
|
||||
if (phymode == WIFI_PHY_MODE_HE20)
|
||||
{
|
||||
txdesc->flags |= 0x80000000;
|
||||
txdesc->field_2f =
|
||||
(char)((((uint32_t)tx_rate_config->ersu + 6) & 0xf) << 3)
|
||||
| (txdesc->field_2f & 0x87);
|
||||
|
||||
if ((uint32_t)tx_rate_config->dcm)
|
||||
txdesc->field_31 |= 0x80;
|
||||
}
|
||||
else if (phymode == WIFI_PHY_MODE_VHT20)
|
||||
txdesc->flags |= 0x1000000;
|
||||
|
||||
// No idea if this is correct, but this is what the original code does...
|
||||
uint32_t bw_is_bw40 = bw == WIFI_BW40;
|
||||
txdesc->field_8 = (bw_is_bw40 << 0xf) | (txdesc->field_8 & 0xffff7fff);
|
||||
|
||||
if (en_sys_seq)
|
||||
txdesc->flags |= 1;
|
||||
|
||||
txdesc->field_10 =
|
||||
(txdesc->field_10 & 0xfff3ffff) | ((ifx & WIFI_IF_MAX) << 0x12);
|
||||
txdesc->field_14 = 0x100;
|
||||
|
||||
ieee80211_post_hmac_tx(eb);
|
||||
g_osi_funcs_p->_mutex_unlock(g_wifi_global_lock);
|
||||
}
|
||||
else
|
||||
{
|
||||
result = ESP_ERR_NO_MEM;
|
||||
g_osi_funcs_p->_mutex_unlock(g_wifi_global_lock);
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
@@ -0,0 +1,15 @@
|
||||
// Copied from opentrafficmap/its-g5-receiver-firmware_txenabled, main/tx_custom.h.
|
||||
// See tx_custom.c for what this does and why we pulled it in.
|
||||
#pragma once
|
||||
|
||||
#include "esp_wifi.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
esp_err_t esp_wifi_80211_tx_custom(wifi_interface_t ifx, const void *buffer, int32_t len, bool en_sys_seq, wifi_tx_rate_config_t *tx_rate_config, wifi_band_t band, wifi_bandwidth_t bw);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,49 @@
|
||||
#include <stdint.h>
|
||||
|
||||
// RETIRED - no longer built (removed from main/CMakeLists.txt SRCS), kept only
|
||||
// for history. Confirmed not to work: linked cleanly with -Wl,-zmuldefs but
|
||||
// the QoS-frame rejection persisted identically. Also turned out to be based
|
||||
// on the wrong function signature - the real ieee80211_raw_frame_sanity_check
|
||||
// takes (wifi_interface_t ifx, const void *buffer, int32_t len, bool
|
||||
// en_sys_seq), confirmed from opentrafficmap/its-g5-receiver-firmware_txenabled's
|
||||
// main/tx_custom.c, not the 3x int32_t guessed below. Superseded by
|
||||
// tx_custom.c, which bypasses esp_wifi_80211_tx() (and the function that
|
||||
// calls this check) entirely instead of trying to neutralize the check.
|
||||
// See docs/04-transmit-setup.md.
|
||||
|
||||
// Overrides a function inside the closed-source WiFi library that gates
|
||||
// which raw 802.11 frame types esp_wifi_80211_tx() will accept. By default
|
||||
// it only allows beacon/probe-request/probe-response/action and non-QoS
|
||||
// data frames - it explicitly rejects QoS Data (subtype 8), which is what
|
||||
// real ITS-G5/802.11p hardware actually transmits and expects.
|
||||
//
|
||||
// This is the same technique used by ESP32 WiFi-security tools (deauther/
|
||||
// injection projects) to unlock raw frame injection: define a function with
|
||||
// the exact same name as the library's gate, and link with -Wl,-zmuldefs
|
||||
// (see CMakeLists.txt) so the linker accepts having two definitions of the
|
||||
// same symbol instead of erroring with "multiple definition of
|
||||
// `ieee80211_raw_frame_sanity_check'" - and takes this one instead of the
|
||||
// library's.
|
||||
//
|
||||
// Confirmed present for THIS target/IDF version: `nm` on
|
||||
// components/esp_wifi/lib/esp32c5/libnet80211.a (IDF v5.5.4) shows
|
||||
// `ieee80211_raw_frame_sanity_check` as a normal (non-weak) global text
|
||||
// symbol in ieee80211_node.o. The exact argument count/meaning is
|
||||
// reverse-engineered from community ESP32 (Xtensa) deauther tools, not
|
||||
// confirmed byte-for-byte against esp32c5's actual implementation - if
|
||||
// frames still get rejected, or this crashes, the real signature may take
|
||||
// different arguments than assumed here.
|
||||
//
|
||||
// Real risk, not just an inconvenience: this disables ALL sanity checking
|
||||
// on raw frames going through esp_wifi_80211_tx(), not just the QoS-type
|
||||
// gate. Whatever else that check validates (frame length bounds, etc.) is
|
||||
// now unchecked. Malformed frames from a bug elsewhere in this codebase
|
||||
// could behave worse (silent corruption, crash) than they would have with
|
||||
// the check in place, where they'd have just been rejected cleanly.
|
||||
int ieee80211_raw_frame_sanity_check(int32_t arg1, int32_t arg2, int32_t arg3)
|
||||
{
|
||||
(void)arg1;
|
||||
(void)arg2;
|
||||
(void)arg3;
|
||||
return 0; // 0 = "frame is sane" - always pass
|
||||
}
|
||||
Reference in New Issue
Block a user