Sign ITS messages on the ESP32-C5 with vanetza-idf, over USB or BLE
obu-firmware is now a port of the colleague's standalone VRU station (microbu-esp32c5/firmware, kept beside this repository and gitignored): the vanetza-idf C-ITS stack with the TS 103 097 security entity, credentials in NVS, the station-link v1 protocol over the native USB port (frame type 0x10 in the existing 0xAA55 framing) and over a BLE GATT peripheral, and its ITS-G5 radio adapter. The phone still builds CAM and VAM; the board adds GeoNetworking/BTP and signs with the provisioned authorization ticket. The private key never leaves the board. Builds with ESP-IDF 6.0.2 only, which vanetza-idf pins for the radio's private driver ABI. The previous C firmware stays on disk unbuilt; a full-flash backup of the bench board is kept in firmware-backups/ (gitignored). Changed against the colleague's firmware, marked MicrOBU: in the sources: - Reception unchanged for the app. vanetza-idf drops what it cannot verify (unsigned traffic, every RSU), so each captured frame also goes through the previous gn_unwrap.c and reaches the phone as link opcode V2X_RX (0x85), whose body is the old SERIAL_MSG_V2X_RX payload. - Unsigned transmission still possible, with the previous geonet.c header; the phone chooses per message. - Console on UART0 (CH343 port); the native USB port carries only link frames. - BLE advertising pauses while the USB link is in use: BLE and ITS-G5 share one RF front end. - NVS 80 KB (app at 0x20000). At 24 KB, with Wi-Fi settings the previous firmware left behind, the BLE bond could not be stored and the phone had to pair on every connection. - Bench fixes: the radio queue is drained before the first PoTi (no RX and ~177 queue drops before); the station loop waited pdMS_TO_TICKS(5) = 0 ticks at 100 Hz and starved the idle task; the 2.4 KB RX capture buffer is off the Wi-Fi task stack; BLE notifications longer than the MTU are dropped instead of cut short, MTU 517; serial writes are skipped with no USB host. - Manual country policy and TX-power read-back from the previous radio setup; logs for BLE encryption changes and the number of stored bonds. Verified on the bench board (COM3) with the phone over USB and BLE: CAM and VAM, signed and unsigned, go out; reception of the sim car and the RSU's CAM/SPATEM/MAPEM continues; the board survives app restarts and reconnects. See docs/06-signed-its-vam-ble.md.
This commit is contained in:
Ashin Walpola
@@ -0,0 +1,653 @@
|
||||
#include "station.hpp"
|
||||
#include <vanetza_idf/nf_sap.hpp>
|
||||
#include <vanetza_idf/sf_sap.hpp>
|
||||
#include <vanetza_idf/nvs_credential_store.hpp>
|
||||
#include <vanetza/geonet/serialization_buffer.hpp>
|
||||
#include <vanetza/geonet/areas.hpp>
|
||||
#include <vanetza/units/angle.hpp>
|
||||
#include <vanetza/units/length.hpp>
|
||||
#include <vanetza/units/velocity.hpp>
|
||||
#include <esp_log.h>
|
||||
#include <esp_timer.h>
|
||||
#include "c5_radio.hpp"
|
||||
extern "C" {
|
||||
#include "geonet.h"
|
||||
#include "gn_unwrap.h"
|
||||
}
|
||||
#include <algorithm>
|
||||
#include <iterator>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
|
||||
namespace microbu {
|
||||
using namespace vanetza_idf;
|
||||
using vanetza::ByteBuffer;
|
||||
namespace gn = vanetza::geonet;
|
||||
namespace {
|
||||
const char* TAG = "station";
|
||||
|
||||
link::Code code(Result result) { return static_cast<link::Code>(result); }
|
||||
|
||||
// Numbering of the link's "Packet transport type" field matches the TRANSP_CORE.request/.indication
|
||||
// service primitive parameter of ETSI TS 103 836-4-1 annex J.2/J.4 (0 GUC, 1 SHB, 2 TSB, 3 GBC,
|
||||
// 4 GAC); it does not reuse the HT wire encoding of clause 9.7.4 table 9, which numbers these
|
||||
// differently (GEOUNICAST=2, GEOANYCAST=3, GEOBROADCAST=4, TSB=5, no separate SHB value there).
|
||||
std::uint8_t transport_number(gn::TransportType t) {
|
||||
switch (t) {
|
||||
case gn::TransportType::GUC: return 0;
|
||||
case gn::TransportType::SHB: return 1;
|
||||
case gn::TransportType::TSB: return 2;
|
||||
case gn::TransportType::GBC: return 3;
|
||||
case gn::TransportType::GAC: return 4;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
std::optional<gn::TransportType> transport_from(std::uint8_t n) {
|
||||
switch (n) {
|
||||
case 0: return gn::TransportType::GUC;
|
||||
case 1: return gn::TransportType::SHB;
|
||||
case 2: return gn::TransportType::TSB;
|
||||
case 3: return gn::TransportType::GBC;
|
||||
case 4: return gn::TransportType::GAC;
|
||||
}
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
// Wire DestinationArea -> vanetza gn::Area (GBC destination).
|
||||
gn::Area area_from(const link::DestinationArea& a) {
|
||||
gn::Area area;
|
||||
switch (a.shape) {
|
||||
case 0: { gn::Circle c; c.r = double(a.distance_a) * vanetza::units::si::meter; area.shape = c; break; }
|
||||
case 1: { gn::Rectangle r; r.a = double(a.distance_a) * vanetza::units::si::meter; r.b = double(a.distance_b) * vanetza::units::si::meter; area.shape = r; break; }
|
||||
default: { gn::Ellipse e; e.a = double(a.distance_a) * vanetza::units::si::meter; e.b = double(a.distance_b) * vanetza::units::si::meter; area.shape = e; break; }
|
||||
}
|
||||
area.position = gn::GeodeticPosition(a.latitude / 1.0e7 * vanetza::units::degree, a.longitude / 1.0e7 * vanetza::units::degree);
|
||||
area.angle = vanetza::units::Angle(double(a.angle) * vanetza::units::degree);
|
||||
return area;
|
||||
}
|
||||
|
||||
// gn::Area -> wire DestinationArea (the inverse of area_from).
|
||||
struct AreaToLink : boost::static_visitor<link::DestinationArea> {
|
||||
const gn::Area& area;
|
||||
explicit AreaToLink(const gn::Area& a) : area(a) {}
|
||||
link::DestinationArea common() const {
|
||||
link::DestinationArea out;
|
||||
out.latitude = static_cast<std::int32_t>(area.position.latitude.value() * 1.0e7);
|
||||
out.longitude = static_cast<std::int32_t>(area.position.longitude.value() * 1.0e7);
|
||||
out.angle = static_cast<std::uint16_t>(area.angle.value());
|
||||
return out;
|
||||
}
|
||||
link::DestinationArea operator()(const gn::Circle& c) const { auto o = common(); o.shape = 0; o.distance_a = c.r.value(); return o; }
|
||||
link::DestinationArea operator()(const gn::Rectangle& r) const { auto o = common(); o.shape = 1; o.distance_a = r.a.value(); o.distance_b = r.b.value(); return o; }
|
||||
link::DestinationArea operator()(const gn::Ellipse& e) const { auto o = common(); o.shape = 2; o.distance_a = e.a.value(); o.distance_b = e.b.value(); return o; }
|
||||
};
|
||||
|
||||
// 0 unsecured (no security envelope), 1 + VerificationReport ordinal otherwise
|
||||
struct ReportToLink : boost::static_visitor<std::uint8_t> {
|
||||
ReportToLink() = default;
|
||||
std::uint8_t operator()(boost::blank) const { return 0; }
|
||||
std::uint8_t operator()(vanetza::security::VerificationReport r) const { return 1 + static_cast<std::uint8_t>(r); }
|
||||
};
|
||||
}
|
||||
|
||||
// Credentials, backend, trust configuration, ticket pool and the entity built on them.
|
||||
struct Station::Security {
|
||||
BackendMbedTls backend;
|
||||
security::TrustConfiguration trust;
|
||||
security::CertificatePool pool {backend};
|
||||
std::unique_ptr<security::SecurityEntity> entity;
|
||||
bool loaded = false;
|
||||
security::ApplyReport report;
|
||||
};
|
||||
|
||||
std::int64_t Station::Clock::now_us() const {
|
||||
return base_its_us + (esp_timer_get_time() - base_esp_us);
|
||||
}
|
||||
|
||||
Station::Station() = default;
|
||||
Station::~Station() { teardown(); }
|
||||
|
||||
void Station::teardown() {
|
||||
rebuilding_ = true; // the DEREG of TS 102 723-8 Figure 15 is ours, not the phone's
|
||||
stack_.reset(); // router timers before the runtime
|
||||
// AccessStack::Dcc's Limeric holds a reference to *runtime_ and calls into it from its
|
||||
// destructor (Runtime::cancel): it must be destroyed before runtime_ is.
|
||||
access_stack_.reset();
|
||||
security_.reset();
|
||||
runtime_.reset();
|
||||
dcc_cca_last_.reset();
|
||||
dcc_last_sample_its_us_ = 0;
|
||||
pending_responders_.clear();
|
||||
rebuilding_ = false;
|
||||
}
|
||||
|
||||
// TS 102 894-2 station config -> the GeoNetworking MIB (TS 103 836-4-1).
|
||||
void Station::apply_mib(StackConfig& config, const link::StationConfigure& c) const {
|
||||
config.mib.itsGnLocalGnAddr.mid(vanetza::MacAddress {c.mid[0], c.mid[1], c.mid[2], c.mid[3], c.mid[4], c.mid[5]});
|
||||
config.mib.itsGnLocalGnAddr.station_type(static_cast<gn::StationType>(c.station_type & 0x1F));
|
||||
config.mib.itsGnLocalGnAddr.is_manually_configured(c.address_configuration == 0);
|
||||
config.mib.itsGnLocalAddrConfMethod = c.address_configuration == 1 ? gn::AddrConfMethod::Anonymous : gn::AddrConfMethod::Auto;
|
||||
config.mib.itsGnSecurity = c.security == 1;
|
||||
config.mib.vanetzaDisableBeaconing = c.beaconing == 0;
|
||||
config.mib.itsGnDefaultTrafficClass = gn::TrafficClass(c.default_traffic_class);
|
||||
gn::Lifetime lifetime; lifetime.raw(c.default_lifetime);
|
||||
config.mib.itsGnDefaultPacketLifetime = lifetime;
|
||||
config.mib.itsGnMaxPacketLifetime = lifetime < config.mib.itsGnMaxPacketLifetime ? config.mib.itsGnMaxPacketLifetime : lifetime;
|
||||
config.radio_parameters.channel_number = c.channel_number;
|
||||
config.radio_parameters.transmit_power_dbm = c.transmit_power_dbm;
|
||||
}
|
||||
|
||||
// Carry the phone's SF-SAP pseudonym-change subscriptions (TS 102 723-8 clause 6.3)
|
||||
// over to the identity manager of a freshly rebuilt security entity.
|
||||
void Station::resubscribe_id_change() {
|
||||
std::map<std::uint64_t, std::uint64_t> renewed;
|
||||
for (const auto& [handle, old_service] : link_subscriptions_) {
|
||||
(void)old_service;
|
||||
renewed[handle] = security_->entity->id_change().subscribe(
|
||||
[this, handle](security::IdChangeCommand command, const security::Identifier& id, const ByteBuffer& data,
|
||||
std::shared_ptr<security::IdChangeResponder> responder) {
|
||||
link::IdChangeEvent event;
|
||||
event.subscription = handle;
|
||||
event.command = static_cast<std::uint8_t>(command);
|
||||
std::copy(id.begin(), id.end(), event.id);
|
||||
event.subscriber_data = data;
|
||||
if (responder) pending_responders_[handle] = responder;
|
||||
if (id_event_ && !rebuilding_) id_event_(event);
|
||||
});
|
||||
}
|
||||
link_subscriptions_ = renewed;
|
||||
}
|
||||
|
||||
// (Re)build stack and security entity from config_, the stored credentials and the current clock.
|
||||
link::Code Station::build() {
|
||||
teardown();
|
||||
const auto& c = *config_;
|
||||
runtime_ = std::make_unique<vanetza::ManualRuntime>(vanetza::Clock::time_point(std::chrono::microseconds(clock_.synchronised ? clock_.now_us() : 0)));
|
||||
StackConfig config;
|
||||
apply_mib(config, c);
|
||||
radio_parameters_ = config.radio_parameters;
|
||||
vanetza::security::SecurityEntity* entity = nullptr;
|
||||
if (config.mib.itsGnSecurity) {
|
||||
security_ = std::make_unique<Security>();
|
||||
security::Credentials credentials;
|
||||
security::NvsCredentialStore store;
|
||||
const auto load = store.load(credentials);
|
||||
if (load == Result::accepted) {
|
||||
security_->report = security::apply(credentials, security_->trust, security_->pool);
|
||||
security_->loaded = security_->report.result == Result::accepted && !security_->pool.empty();
|
||||
ESP_LOGI(TAG, "credentials from NVS: %u roots, %u authorities, %u tickets (result %d)",
|
||||
unsigned(security_->report.roots), unsigned(security_->report.authorities), unsigned(security_->report.tickets),
|
||||
int(security_->report.result));
|
||||
} else if (load == Result::rejected) {
|
||||
ESP_LOGW(TAG, "no credentials in NVS: secured requests will be refused until provisioned");
|
||||
} else {
|
||||
ESP_LOGE(TAG, "stored credentials do not decode (result %d)", int(load));
|
||||
}
|
||||
security_->entity = std::make_unique<security::SecurityEntity>(*runtime_, *this, security_->backend, security_->pool, security_->trust);
|
||||
entity = security_->entity.get();
|
||||
resubscribe_id_change();
|
||||
}
|
||||
stack_ = std::make_unique<Stack>(config, *runtime_, *this, entity);
|
||||
stack_->on_receive([this](BtpIndication indication) { deliver(std::move(indication)); });
|
||||
#if CONFIG_MICROBU_TEST_CHANNEL
|
||||
stack_->on_receive_gn([this](GnIndication indication) { record(3, std::move(indication.data)); });
|
||||
#endif
|
||||
stack_->on_access_result([](Result) { /* counted in Station::request, the adapter itself */ });
|
||||
// DCC_ACC gate in front of whatever radio_ currently is (rebuilt here rather than in
|
||||
// Station::configure() because it must reference the fresh runtime_, not a torn-down one --
|
||||
// see the destruction-order comment in Station::teardown()).
|
||||
if (radio_) {
|
||||
access_stack_ = std::make_unique<AccessStack>(*radio_);
|
||||
access_stack_->enable_dcc(*runtime_);
|
||||
stack_->report_tx_power(static_cast<unsigned>(std::lround(radio_parameters_.transmit_power_dbm)));
|
||||
}
|
||||
dcc_cca_last_.reset();
|
||||
dcc_last_sample_its_us_ = clock_.synchronised ? clock_.now_us() : 0;
|
||||
if (have_fix_) apply_position();
|
||||
return link::Code::accepted;
|
||||
}
|
||||
|
||||
link::Code Station::configure(const link::StationConfigure& c, link::Bytes& detail) {
|
||||
const bool radio_changed = !config_ || config_->radio != c.radio || config_->channel_number != c.channel_number ||
|
||||
config_->transmit_power_dbm != c.transmit_power_dbm;
|
||||
if (radio_changed) {
|
||||
radio_.reset();
|
||||
if (c.radio != 0) {
|
||||
C5RadioConfig rc;
|
||||
rc.channel_number = c.channel_number;
|
||||
rc.transmit_power_dbm = c.transmit_power_dbm;
|
||||
rc.laboratory_transmission = c.radio == 2;
|
||||
radio_ = std::make_unique<C5Radio>(rc);
|
||||
const auto error = radio_->start();
|
||||
if (error != ESP_OK) {
|
||||
ESP_LOGE(TAG, "radio start failed: %s", esp_err_to_name(error));
|
||||
radio_.reset();
|
||||
return link::Code::rejected;
|
||||
}
|
||||
ESP_LOGI(TAG, "radio on channel %u, %s", unsigned(c.channel_number), c.radio == 2 ? "transmit and receive" : "receive only");
|
||||
}
|
||||
}
|
||||
config_ = c;
|
||||
link_subscriptions_.clear(); // a configuration starts a phone session: earlier subscriptions are void
|
||||
counters_ = link::Status {}; // and the session's counters start at zero
|
||||
const auto result = build();
|
||||
if (result != link::Code::accepted) return result;
|
||||
link::Writer w;
|
||||
ByteBuffer address;
|
||||
gn::serialize_into_buffer(stack_->address(), address);
|
||||
w.bytes(address);
|
||||
security::Identifier identifier {};
|
||||
if (security_ && security_->entity) identifier = security_->entity->id_change().current_identifier();
|
||||
w.bytes(identifier.data(), 8);
|
||||
w.u8(security_ && security_->loaded ? 1 : 0);
|
||||
w.u8(security_ ? static_cast<std::uint8_t>(std::min<std::size_t>(security_->pool.size(), 255)) : 0);
|
||||
detail = w.out;
|
||||
counters_.configured = 1;
|
||||
return link::Code::accepted;
|
||||
}
|
||||
|
||||
void Station::apply_position() {
|
||||
if (!stack_ || !have_fix_) return;
|
||||
// never ahead of the station clock (Stack::update_position rejects that)
|
||||
const auto now = runtime_->now();
|
||||
if (fix_.timestamp > now) fix_.timestamp = now;
|
||||
const auto result = stack_->update_position(fix_);
|
||||
if (result != Result::accepted) ESP_LOGW(TAG, "position rejected: %d", int(result));
|
||||
}
|
||||
|
||||
link::Code Station::poti(const link::PotiUpdate& p) {
|
||||
if (p.latitude < -900000000 || p.latitude > 900000000 || p.longitude < -1800000000 || p.longitude > 1800000000)
|
||||
return link::Code::invalid_argument;
|
||||
// ITS clock: the phone's PoTi time is the reference; the local esp_timer runs between updates.
|
||||
const std::int64_t its_us = static_cast<std::int64_t>(p.timestamp_ms) * 1000;
|
||||
const std::int64_t esp_us = esp_timer_get_time();
|
||||
link::Code result = link::Code::accepted;
|
||||
if (!clock_.synchronised) {
|
||||
clock_ = Clock {true, its_us, esp_us};
|
||||
if (config_) build(); // the runtime started at 0: restart it at real time
|
||||
} else {
|
||||
const std::int64_t drift = its_us - clock_.now_us();
|
||||
if (drift >= 0) {
|
||||
clock_.base_its_us = its_us; clock_.base_esp_us = esp_us; // forward: step immediately
|
||||
} else if (drift > -1000000) {
|
||||
// small backward drift: hold the local clock, it catches up with the next updates
|
||||
} else {
|
||||
// the reference moved back by more than a second: restart the station at that time
|
||||
ESP_LOGW(TAG, "ITS time moved back by %lld ms, restarting the station", static_cast<long long>(-drift / 1000));
|
||||
clock_ = Clock {true, its_us, esp_us};
|
||||
if (config_) build();
|
||||
result = link::Code::time_regression;
|
||||
}
|
||||
}
|
||||
fix_ = vanetza::PositionFix {};
|
||||
fix_.timestamp = vanetza::Clock::time_point(std::chrono::microseconds(std::min(its_us, clock_.now_us())));
|
||||
fix_.latitude = p.latitude / 1.0e7 * vanetza::units::degree;
|
||||
fix_.longitude = p.longitude / 1.0e7 * vanetza::units::degree;
|
||||
fix_.confidence.semi_major = p.semi_major_cm / 100.0 * vanetza::units::si::meter;
|
||||
fix_.confidence.semi_minor = p.semi_minor_cm / 100.0 * vanetza::units::si::meter;
|
||||
fix_.confidence.orientation = p.orientation_deci_degree / 10.0 * vanetza::units::true_north_degrees;
|
||||
fix_.speed = (p.has_speed() ? p.speed_cm_s / 100.0 : 0.0) * vanetza::units::si::meters_per_second;
|
||||
fix_.course = (p.has_heading() ? p.heading_deci_degree / 10.0 : 0.0) * vanetza::units::true_north_degrees;
|
||||
if (p.has_altitude()) fix_.altitude = vanetza::ConfidentQuantity<vanetza::units::Length>(p.altitude_cm / 100.0 * vanetza::units::si::meter);
|
||||
have_fix_ = true;
|
||||
last_poti_ = p;
|
||||
++counters_.poti_updates;
|
||||
tick();
|
||||
apply_position();
|
||||
return result;
|
||||
}
|
||||
|
||||
link::Code Station::btp_request(const link::BtpDataRequest& q) {
|
||||
if (!stack_) return link::Code::not_configured;
|
||||
if (!clock_.synchronised || !have_fix_) { ++counters_.requests_refused; return link::Code::rejected; }
|
||||
NF_SAP::BTP_DATA_request request;
|
||||
request.fl_sdu = q.fl_sdu;
|
||||
request.length = q.fl_sdu.size();
|
||||
request.btp_type = q.btp_type == 0 ? BtpType::a : BtpType::b;
|
||||
request.destination_port = q.destination_port;
|
||||
if (request.btp_type == BtpType::a) request.source_port = q.destination_port_info;
|
||||
else request.destination_port_info = q.destination_port_info;
|
||||
const auto transport = transport_from(q.gn_packet_transport_type);
|
||||
if (!transport) { ++counters_.requests_refused; return link::Code::invalid_argument; }
|
||||
request.gn_packet_transport_type = *transport;
|
||||
|
||||
switch (q.gn_communication_profile) {
|
||||
case 0: request.gn_communication_profile = gn::CommunicationProfile::Unspecified; break;
|
||||
case 1: request.gn_communication_profile = gn::CommunicationProfile::ITS_G5; break;
|
||||
case 2: request.gn_communication_profile = gn::CommunicationProfile::LTE_V2X; break;
|
||||
default: ++counters_.requests_refused; return link::Code::invalid_argument;
|
||||
}
|
||||
// MicrOBU: the colleague's firmware refuses unsecured requests outright. Here the phone's
|
||||
// "Sign outgoing messages" setting decides, per request; 0 means the station's configuration.
|
||||
if (q.gn_security_profile > 2) {
|
||||
++counters_.requests_refused;
|
||||
return link::Code::invalid_argument;
|
||||
}
|
||||
const bool secured = q.gn_security_profile == 2 || (q.gn_security_profile == 0 && config_->security == 1);
|
||||
if (!secured) return unsecured_request(q);
|
||||
request.gn_security_profile = NF_SAP::SecurityProfile::SECURED;
|
||||
request.gn_traffic_class = q.gn_traffic_class == 0xFF ? stack_->config().mib.itsGnDefaultTrafficClass : gn::TrafficClass(q.gn_traffic_class);
|
||||
if (q.gn_maximum_packet_lifetime != 0xFF) { gn::Lifetime l; l.raw(q.gn_maximum_packet_lifetime); request.gn_maximum_packet_lifetime = l; }
|
||||
if (q.gn_maximum_hop_limit) request.gn_maximum_hop_limit = q.gn_maximum_hop_limit;
|
||||
if (q.gn_repetition_interval_ms) {
|
||||
gn::DataRequest::Repetition repetition;
|
||||
repetition.interval = (q.gn_repetition_interval_ms / 1000.0) * vanetza::units::si::seconds;
|
||||
repetition.maximum = (q.gn_repetition_maximum_ms / 1000.0) * vanetza::units::si::seconds;
|
||||
request.gn_repetition = repetition;
|
||||
}
|
||||
if (*transport == gn::TransportType::GBC) {
|
||||
if (!q.area) { ++counters_.requests_refused; return link::Code::invalid_argument; }
|
||||
request.gn_destination_address = area_from(*q.area);
|
||||
}
|
||||
request.its_aid = q.its_aid;
|
||||
request.permissions = q.permissions;
|
||||
request.context_information = q.context;
|
||||
tick(); // the packet carries the current time and position
|
||||
ESP_LOGI(TAG, "heap before sign: free=%lu min=%lu dma=%lu stack_hwm=%u",
|
||||
(unsigned long)esp_get_free_heap_size(),
|
||||
(unsigned long)esp_get_minimum_free_heap_size(),
|
||||
(unsigned long)heap_caps_get_free_size(MALLOC_CAP_DMA),
|
||||
(unsigned)uxTaskGetStackHighWaterMark(nullptr));
|
||||
Result result = Result::rejected;
|
||||
try {
|
||||
result = NF_SAP::BTP_DATA_request_submit(*stack_, std::move(request));
|
||||
if (result == Result::accepted) ++counters_.requests_accepted; else ++counters_.requests_refused;
|
||||
} catch (const std::exception& e) {
|
||||
ESP_LOGE(TAG, "BTP_DATA_request_submit exception: %s", e.what());
|
||||
++counters_.requests_refused;
|
||||
return link::Code::rejected;
|
||||
}
|
||||
return code(result);
|
||||
}
|
||||
|
||||
void Station::forward_raw(const ByteBuffer& frame, int rssi) {
|
||||
if (!raw_its_) return;
|
||||
// Most captured frames are not ITS traffic this handles; false is the common case, not an error.
|
||||
gn_rx_t rx;
|
||||
if (!gn_unwrap_its(frame.data(), static_cast<int>(frame.size()), &rx)) return;
|
||||
constexpr std::size_t prefix = 14;
|
||||
if (rx.truncated || rx.payload_len <= 0 ||
|
||||
static_cast<std::size_t>(rx.payload_len) + prefix + link::header_size > link::maximum_message) {
|
||||
++raw_oversize_;
|
||||
static std::int64_t last_report = 0; // one line per 10 s at most, the counter has the rest
|
||||
const auto now = esp_timer_get_time();
|
||||
if (now - last_report > 10000000) {
|
||||
last_report = now;
|
||||
ESP_LOGW(TAG, "V2X_RX: port %u message of %d bytes does not fit a link message (%lu dropped so far)",
|
||||
unsigned(rx.btp_dest_port), rx.payload_len, (unsigned long)raw_oversize_);
|
||||
}
|
||||
return;
|
||||
}
|
||||
link::Writer w;
|
||||
w.u16(rx.btp_dest_port);
|
||||
w.u8(static_cast<std::uint8_t>(static_cast<std::int8_t>(std::clamp(rssi, -128, 127))));
|
||||
w.u8((rx.has_geo_area ? 0x01 : 0) | (rx.signed_unverified ? 0x02 : 0));
|
||||
w.i32(rx.geo_area_lat_tenmicrodeg);
|
||||
w.i32(rx.geo_area_lon_tenmicrodeg);
|
||||
w.u16(rx.geo_area_distance_a_m);
|
||||
w.bytes(rx.payload, static_cast<std::size_t>(rx.payload_len));
|
||||
++raw_forwarded_;
|
||||
raw_its_(w.out);
|
||||
}
|
||||
|
||||
link::Code Station::unsecured_request(const link::BtpDataRequest& q) {
|
||||
// geonet.c builds exactly one packet shape: BTP-B inside a GN single-hop broadcast. That is
|
||||
// what CAM and VAM are; anything else still needs the stack, which here only signs.
|
||||
if (q.btp_type != 1 || q.gn_packet_transport_type != 1) {
|
||||
++counters_.requests_refused;
|
||||
return link::Code::unsupported;
|
||||
}
|
||||
gn_lpv_t lpv {};
|
||||
std::copy(std::begin(config_->mid), std::end(config_->mid), lpv.mac);
|
||||
lpv.station_type = config_->station_type;
|
||||
lpv.pai = last_poti_.pai();
|
||||
lpv.tst_ms = static_cast<std::uint32_t>(last_poti_.timestamp_ms); // TimestampIts mod 2^32
|
||||
lpv.lat_tenmicrodeg = last_poti_.latitude;
|
||||
lpv.lon_tenmicrodeg = last_poti_.longitude;
|
||||
lpv.speed_cms = static_cast<std::int16_t>(last_poti_.has_speed() ? std::min<unsigned>(last_poti_.speed_cm_s, 16383) : 0);
|
||||
lpv.heading_decideg = last_poti_.has_heading() ? last_poti_.heading_deci_degree : 0;
|
||||
ByteBuffer pdu(q.fl_sdu.size() + 64);
|
||||
const int length = geonet_wrap_shb(q.fl_sdu.data(), static_cast<int>(q.fl_sdu.size()), &lpv,
|
||||
q.destination_port, pdu.data(), pdu.size());
|
||||
if (length <= 0) {
|
||||
++counters_.requests_refused;
|
||||
return link::Code::invalid_argument;
|
||||
}
|
||||
pdu.resize(static_cast<std::size_t>(length));
|
||||
AlDataRequest frame = radio_parameters_;
|
||||
frame.source = vanetza::MacAddress {lpv.mac[0], lpv.mac[1], lpv.mac[2], lpv.mac[3], lpv.mac[4], lpv.mac[5]};
|
||||
frame.destination = vanetza::cBroadcastMacAddress;
|
||||
frame.data = std::move(pdu);
|
||||
const auto result = request(std::move(frame));
|
||||
if (result == Result::accepted) ++counters_.requests_accepted; else ++counters_.requests_refused;
|
||||
return code(result);
|
||||
}
|
||||
|
||||
void Station::deliver(BtpIndication received) {
|
||||
auto indication = NF_SAP::BTP_DATA_indication_from(std::move(received));
|
||||
++counters_.indications;
|
||||
#if CONFIG_MICROBU_TEST_CHANNEL
|
||||
// The test channel sees every BTP indication as well (kind 2, the HIL SUT layout).
|
||||
if (mirror_ != Mirror::off) {
|
||||
link::Writer w;
|
||||
w.u8(indication.btp_type == BtpType::b ? 1 : 0);
|
||||
const auto port = indication.destination_port;
|
||||
const auto info = indication.source_port.value_or(indication.destination_port_info.value_or(0));
|
||||
w.out.push_back(port >> 8); w.out.push_back(port & 0xFF); // big-endian like hil_sut.cpp
|
||||
w.out.push_back(info >> 8); w.out.push_back(info & 0xFF);
|
||||
w.bytes(indication.received_fl_sdu);
|
||||
record(2, w.out);
|
||||
}
|
||||
#endif
|
||||
if (!indication_) return;
|
||||
link::BtpDataIndication out;
|
||||
out.btp_type = indication.btp_type == BtpType::b ? 1 : 0;
|
||||
out.destination_port = indication.destination_port;
|
||||
out.destination_port_info = indication.source_port.value_or(indication.destination_port_info.value_or(0));
|
||||
out.gn_packet_transport_type = transport_number(indication.gn.transport_type);
|
||||
out.gn_traffic_class = indication.gn.traffic_class.raw();
|
||||
out.gn_remaining_packet_lifetime = indication.gn.remaining_packet_lifetime ? indication.gn.remaining_packet_lifetime->raw() : 0xFF;
|
||||
out.gn_remaining_hop_limit = indication.gn.remaining_hop_limit ? static_cast<std::uint8_t>(std::min(*indication.gn.remaining_hop_limit, 254u)) : 0xFF;
|
||||
ByteBuffer address;
|
||||
gn::serialize_into_buffer(indication.gn.source_position.gn_addr, address);
|
||||
std::copy_n(address.begin(), std::min<std::size_t>(address.size(), 8), out.source_gn_address);
|
||||
out.source_timestamp = indication.gn.source_position.timestamp.raw();
|
||||
out.source_latitude = indication.gn.source_position.latitude.value();
|
||||
out.source_longitude = indication.gn.source_position.longitude.value();
|
||||
out.security_report = boost::apply_visitor(ReportToLink {}, indication.gn.security_report);
|
||||
out.its_aid = indication.gn.its_aid.value_or(0);
|
||||
if (indication.gn.permissions) out.permissions = *indication.gn.permissions;
|
||||
if (indication.gn.certificate_id) { out.certificate_present = true; std::copy(indication.gn.certificate_id->begin(), indication.gn.certificate_id->end(), out.certificate_id); }
|
||||
if (const auto* area = boost::get<gn::Area>(&indication.gn.destination)) out.area = boost::apply_visitor(AreaToLink {*area}, area->shape);
|
||||
out.received_fl_sdu = std::move(indication.received_fl_sdu);
|
||||
indication_(out);
|
||||
}
|
||||
|
||||
link::Code Station::provision(const link::Bytes& bundle, link::ApplyReport& report) {
|
||||
try {
|
||||
security::Credentials credentials;
|
||||
if (!security::decode(bundle, credentials) || credentials.roots.empty() || credentials.tickets.empty()) {
|
||||
return link::Code::invalid_argument;
|
||||
}
|
||||
security::NvsCredentialStore store;
|
||||
const auto saved = store.save(credentials);
|
||||
if (saved != Result::accepted) return code(saved);
|
||||
if (config_ && config_->security) {
|
||||
const auto result = build();
|
||||
if (result != link::Code::accepted) return result;
|
||||
if (security_) {
|
||||
report.roots = security_->report.roots;
|
||||
report.authorities = security_->report.authorities;
|
||||
report.tickets = security_->report.tickets;
|
||||
ESP_LOGI(TAG, "credentials provisioned: %u roots, %u authorities, %u tickets",
|
||||
unsigned(report.roots), unsigned(report.authorities), unsigned(report.tickets));
|
||||
}
|
||||
return link::Code::accepted;
|
||||
}
|
||||
report.roots = credentials.roots.size();
|
||||
report.authorities = credentials.authorities.size();
|
||||
report.tickets = credentials.tickets.size();
|
||||
return link::Code::accepted;
|
||||
} catch (const std::bad_alloc&) {
|
||||
ESP_LOGE(TAG, "out of memory provisioning credentials");
|
||||
return link::Code::resource_limit;
|
||||
} catch (const std::exception& e) {
|
||||
ESP_LOGE(TAG, "failed provisioning credentials: %s", e.what());
|
||||
return link::Code::invalid_argument;
|
||||
}
|
||||
}
|
||||
|
||||
link::Code Station::erase_credentials() {
|
||||
security::NvsCredentialStore store;
|
||||
const auto erased = store.erase();
|
||||
if (erased != Result::accepted && erased != Result::rejected) return code(erased);
|
||||
if (config_ && config_->security) return build();
|
||||
return link::Code::accepted;
|
||||
}
|
||||
|
||||
link::Code Station::subscribe(const link::Bytes& subscriber_data, std::uint64_t& subscription) {
|
||||
if (!security_ || !security_->entity) return link::Code::security_unavailable;
|
||||
static std::uint64_t next_handle = 1;
|
||||
const auto handle = next_handle++;
|
||||
const auto service = security_->entity->id_change().subscribe(
|
||||
[this, handle](security::IdChangeCommand command, const security::Identifier& id, const ByteBuffer& data,
|
||||
std::shared_ptr<security::IdChangeResponder> responder) {
|
||||
link::IdChangeEvent event;
|
||||
event.subscription = handle;
|
||||
event.command = static_cast<std::uint8_t>(command);
|
||||
std::copy(id.begin(), id.end(), event.id);
|
||||
event.subscriber_data = data;
|
||||
if (responder) pending_responders_[handle] = responder;
|
||||
if (id_event_ && !rebuilding_) id_event_(event);
|
||||
}, subscriber_data);
|
||||
link_subscriptions_[handle] = service;
|
||||
subscription = handle;
|
||||
return link::Code::accepted;
|
||||
}
|
||||
|
||||
link::Code Station::unsubscribe(std::uint64_t subscription) {
|
||||
const auto it = link_subscriptions_.find(subscription);
|
||||
if (it == link_subscriptions_.end()) return link::Code::invalid_argument;
|
||||
Result result = Result::accepted;
|
||||
if (security_ && security_->entity) result = security_->entity->id_change().unsubscribe(it->second);
|
||||
link_subscriptions_.erase(it);
|
||||
pending_responders_.erase(subscription);
|
||||
return code(result);
|
||||
}
|
||||
|
||||
link::Code Station::event_response(std::uint64_t subscription, bool return_code) {
|
||||
const auto it = pending_responders_.find(subscription);
|
||||
if (it == pending_responders_.end()) return link::Code::invalid_argument;
|
||||
it->second->respond(return_code);
|
||||
pending_responders_.erase(it);
|
||||
return link::Code::accepted;
|
||||
}
|
||||
|
||||
link::Code Station::trigger() {
|
||||
if (!security_ || !security_->entity) return link::Code::security_unavailable;
|
||||
return code(security_->entity->id_change().trigger());
|
||||
}
|
||||
|
||||
link::Code Station::lock(std::uint8_t seconds, std::uint64_t& handle) {
|
||||
if (!security_ || !security_->entity) return link::Code::security_unavailable;
|
||||
handle = security_->entity->id_change().lock(seconds);
|
||||
return link::Code::accepted;
|
||||
}
|
||||
|
||||
link::Code Station::unlock(std::uint64_t handle) {
|
||||
if (!security_ || !security_->entity) return link::Code::security_unavailable;
|
||||
return code(security_->entity->id_change().unlock(handle));
|
||||
}
|
||||
|
||||
void Station::tick() {
|
||||
// MicrOBU: the stack needs the ITS clock, which only the phone's first PoTi sets. The radio
|
||||
// does not: its queue (16 frames) must be drained and raw frames forwarded from the moment the
|
||||
// station is configured. The colleague's version returned early here, so a phone that had
|
||||
// configured the station but not yet sent a PoTi (no trip recording, no pinger) received
|
||||
// nothing and every frame on air overflowed the queue into radio_dropped.
|
||||
const bool running = stack_ && clock_.synchronised;
|
||||
if (running) {
|
||||
const auto now = vanetza::Clock::time_point(std::chrono::microseconds(clock_.now_us()));
|
||||
if (now > runtime_->now()) stack_->advance(now);
|
||||
}
|
||||
if (radio_) {
|
||||
radio_->poll([this, running](AlDataIndication indication) {
|
||||
++counters_.radio_received;
|
||||
if (received_) received_();
|
||||
if (running && stack_) stack_->indicate(std::move(indication));
|
||||
}, [this](const ByteBuffer& frame, int rssi, std::uint32_t) { forward_raw(frame, rssi); });
|
||||
counters_.radio_dropped = radio_->dropped_frames();
|
||||
}
|
||||
if (running) sample_dcc_channel_load();
|
||||
}
|
||||
|
||||
// Sample the hardware LCBR counters at the T_Cbr cadence (TS 102 687 clause 5.4 /
|
||||
// TS 103 836-4-2 clause 5.2, both 100 ms) and feed both DCC entities. The first sample after
|
||||
// (re)build only establishes the baseline counter snapshot -- a CBR delta needs two samples.
|
||||
void Station::sample_dcc_channel_load() {
|
||||
if (!radio_ || !access_stack_) return;
|
||||
const auto its_us = clock_.now_us();
|
||||
if (its_us - dcc_last_sample_its_us_ < 100000) return;
|
||||
const auto counters = radio_->read_cca_counters();
|
||||
if (dcc_cca_last_) {
|
||||
const vanetza::dcc::ChannelLoad local_cbr(C5Radio::calculate_cbr(counters, *dcc_cca_last_));
|
||||
stack_->report_local_channel_load(local_cbr);
|
||||
const auto global_cbr = stack_->global_channel_busy_ratio();
|
||||
// Consume Release-2 CBR_G for DCC_ACC when available, else LCBR.
|
||||
access_stack_->report_channel_load(global_cbr ? *global_cbr : local_cbr);
|
||||
}
|
||||
dcc_cca_last_ = counters;
|
||||
dcc_last_sample_its_us_ = its_us;
|
||||
}
|
||||
|
||||
link::Status Station::status() {
|
||||
link::Status s = counters_;
|
||||
s.uptime_ms = static_cast<std::uint32_t>(esp_timer_get_time() / 1000);
|
||||
s.configured = stack_ ? 1 : 0;
|
||||
if (stack_) {
|
||||
ByteBuffer address;
|
||||
gn::serialize_into_buffer(stack_->address(), address);
|
||||
std::copy_n(address.begin(), std::min<std::size_t>(address.size(), 8), s.gn_address);
|
||||
s.change_pending = stack_->identity_change_pending() ? 1 : 0;
|
||||
}
|
||||
if (security_ && security_->entity) {
|
||||
const auto id = security_->entity->id_change().current_identifier();
|
||||
std::copy(id.begin(), id.end(), s.identifier);
|
||||
s.tickets = static_cast<std::uint8_t>(std::min<std::size_t>(security_->pool.size(), 255));
|
||||
const auto& st = security_->entity->statistics();
|
||||
s.signed_messages = st.signed_messages; s.refused_no_ticket = st.refused_no_ticket;
|
||||
s.refused_change_pending = st.refused_change_pending; s.refused_permission = st.refused_permission;
|
||||
s.sign_failed = st.failed; s.verified = st.verified;
|
||||
s.rejected = st.rejected_profile + st.rejected_signer + st.rejected_certificate + st.rejected_signature + st.rejected_time + st.replayed;
|
||||
}
|
||||
s.its_time_ms = clock_.synchronised ? static_cast<std::uint64_t>(clock_.now_us() / 1000) : 0;
|
||||
return s;
|
||||
}
|
||||
|
||||
// ---- access adapter: radio, mirrored or diverted to the software lower tester ----
|
||||
Result Station::request(AlDataRequest request) {
|
||||
#if CONFIG_MICROBU_TEST_CHANNEL
|
||||
if (mirror_ != Mirror::off) record(1, request.data);
|
||||
if (mirror_ == Mirror::divert) { ++counters_.radio_submitted; return overflow_ ? Result::resource_limit : Result::accepted; }
|
||||
#endif
|
||||
if (radio_) {
|
||||
// Adaptive DCC_ACC gates here when access_stack_ has it enabled (build()); it falls
|
||||
// through to radio_->request() unchanged otherwise.
|
||||
const auto result = access_stack_ ? access_stack_->request(std::move(request)) : radio_->request(std::move(request));
|
||||
if (result == Result::accepted) {
|
||||
++counters_.radio_submitted;
|
||||
if (disseminated_) disseminated_();
|
||||
}
|
||||
else {
|
||||
++counters_.radio_failed;
|
||||
static std::int64_t last_report = 0; // one line per second, the counter has the rest
|
||||
const auto now = esp_timer_get_time();
|
||||
if (now - last_report > 1000000) { last_report = now; ESP_LOGW(TAG, "radio refused a frame: result %d", int(result)); }
|
||||
}
|
||||
return result;
|
||||
}
|
||||
++counters_.radio_failed;
|
||||
return Result::unsupported; // no radio configured and nothing diverting: the request has nowhere to go
|
||||
}
|
||||
|
||||
} // namespace microbu
|
||||
Reference in New Issue
Block a user