ashin-walpola/MicrOBU
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
0e9525162da700c05c39887138694d8cebd204dd
MicrOBU/microbu-esp32c5/external/vanetza-idf/ports/esp_idf/src/stack.cpp
T
Ashin Walpola 0e9525162d Keep the colleague's microbu-esp32c5 tree in this repository 
obu-firmware builds against vanetza-idf from microbu-esp32c5/external, but
that tree was gitignored, so a clone of this repository could not build the
firmware it ships. It is now committed here as ordinary files in its own
folder, microbu-esp32c5/: the colleague's commit cf4b99f plus the V2X2MAP
bridge's signature verification (--trust) used on the bench. Nothing is
fetched from or pushed to the colleague's repository; this repository and
its remotes carry everything. The folder's own .gitignore keeps build output,
downloaded components and private key material out, as it did there; the
committed file set is identical to that repository's tracked files.

The ESP32-C5 is still flashed from obu-firmware/, which only takes
vanetza-idf from microbu-esp32c5/, so the two stay separate folders.
FLASHING.md says how to take a newer version of the colleague's tree (copy
it over the folder, rebuild, test, commit).
2026-09-23 17:46:40 +02:00

334 lines
18 KiB
C++
Raw Blame History

#include <vanetza_idf/stack.hpp>
#if VIDF_SECURITY
#include <vanetza_idf/security.hpp>
#endif
#include <vanetza/access/access_category.hpp>
#include <vanetza/btp/header.hpp>
#include <vanetza/btp/header_conversion.hpp>
#include <vanetza/dcc/interface.hpp>
#include <vanetza/dcc/mapping.hpp>
#include <vanetza/dcc/data_request.hpp>
#include <vanetza/geonet/dcc_information_sharing.hpp>
#include <vanetza/geonet/router.hpp>
#include <vanetza/geonet/transport_interface.hpp>
#include <vanetza/net/packet.hpp>
#include <vanetza/net/packet_variant.hpp>
#include <vanetza/common/byte_view.hpp>
#include <algorithm>
#include <cmath>
#include <stdexcept>
#include <utility>
namespace vanetza_idf {
namespace gn = vanetza::geonet;
using namespace vanetza;
StackConfig::StackConfig() {
// Explicit ITS-G5 subset. Defaults are upstream MIB, not R2 certification.
mib.itsGnIfType = gn::InterfaceType::ITS_G5;
mib.itsGnSnDecapResultHandling = gn::SecurityDecapHandling::Strict;
mib.itsGnSecurity = true;
}
class Stack::Impl : public dcc::RequestInterface, public gn::TransportInterface {
public:
StackConfig cfg;
ManualRuntime& runtime;
Access& access;
vanetza::security::SecurityEntity* security;
security::IdChangeService* id_change;
gn::Router router;
// SYS-DCC-003 / GAP-DCC-001: Release-2 DCC_NET. Declared after router: it holds a
// reference to router's location table, which must already be constructed.
gn::DccInformationSharing dcc_information_sharing;
std::optional<dcc::ChannelLoad> global_cbr;
Receive receive;
ReceiveGn receive_gn;
Report report;
bool position_valid = false;
bool change_pending = false;
std::optional<security::SubscriptionHandle> subscription;
Impl(StackConfig c, ManualRuntime& rt, Access& al, vanetza::security::SecurityEntity* sec,
security::IdChangeService* ids) :
cfg(std::move(c)), runtime(rt), access(al), security(sec), id_change(ids), router(rt, cfg.mib),
// TS 103 836-4-2 clause 5.3: "all ITS-S shall start with a random time offset" so that
// stations do not all recompute CBR_G in lockstep. No RNG dependency (host and ESP-IDF
// newlib without configured hardware entropy behave differently): the station's own
// startup time already differs from its neighbours', so its microsecond-level position
// within the 100 ms trigger interval (itsGNCBRGTriggerInterval) serves as that offset.
dcc_information_sharing(rt, router.get_location_table(), dcc::ChannelLoad(0.62),
UnitInterval(double(rt.now().time_since_epoch().count() % 100000) / 100000.0)) {
router.set_access_interface(this);
router.set_security_entity(sec);
router.set_dcc_field_generator(&dcc_information_sharing);
dcc_information_sharing.on_global_cbr_update = [this](const gn::CbrAggregator& agg) {
global_cbr = agg.get_global_cbr();
};
// MIB.itsGnLocalGnAddr is otherwise inert: Router::update_position only
// touches timestamp/latitude/longitude/speed/heading, never the address,
// and the router's own m_local_position_vector.gn_addr starts at
// vanetza::geonet::Address()'s default (all-zero mid, not manually
// configured) until this is called. Every GN packet's source position
// vector carries that address, so leaving this out silently transmits
// the wrong (default) station address regardless of what the caller
// configured -- undetected by BTP's official ATS, which never checks
// the source GN address, but not so for GeoNetworking's.
router.set_address(cfg.mib.itsGnLocalGnAddr);
router.set_transport_handler(gn::UpperProtocol::BTP_A, this);
router.set_transport_handler(gn::UpperProtocol::BTP_B, this);
router.set_transport_handler(gn::UpperProtocol::Unknown, this);
// TS 103 836-4-1 V2.2.1 clause 10.2.1.4: the GN core of an anonymously addressed
// station subscribes to the identifier-change service at startup (SN-IDCHANGE-SUBSCRIBE)
// and derives its MID from the identifier of the security entity (TS 102 940 clause 6.5).
if (cfg.mib.itsGnLocalAddrConfMethod == gn::AddrConfMethod::Anonymous && id_change) {
subscription = id_change->subscribe(
[this](security::IdChangeCommand command, const security::Identifier& id, const ByteBuffer&,
std::shared_ptr<security::IdChangeResponder> responder) { on_id_change(command, id, responder); });
const auto current = id_change->current_identifier();
if (current != security::Identifier {}) apply_identifier(current);
}
}
~Impl() override {
// clause 10.2.1.4: unsubscribe when the router shuts down (SN-IDCHANGE-UNSUBSCRIBE)
if (subscription && id_change) id_change->unsubscribe(*subscription);
}
// TS 102 940 V2.1.1 clause 6.5: the 48 least significant bits of the HashedId8 become the
// MAC-layer / GN MID identifier. A source address must be individual (IEEE Std 802 clause
// 8.2.2, I/G bit 0) and this one is not OUI-assigned (U/L bit 1).
void apply_identifier(const security::Identifier& id) {
MacAddress mid;
std::copy(id.begin() + 2, id.end(), mid.octets.begin());
mid.octets[0] = (mid.octets[0] & 0xfe) | 0x02;
cfg.mib.itsGnLocalGnAddr.mid(mid);
router.set_address(cfg.mib.itsGnLocalGnAddr);
}
// TS 102 723-8 V1.1.1 clauses 6.3.1.2 and 6.3.1.3: hook function of the GN core.
void on_id_change(security::IdChangeCommand command, const security::Identifier& id,
const std::shared_ptr<security::IdChangeResponder>& responder) {
switch (command) {
case security::IdChangeCommand::PREPARE:
change_pending = true;
router.flush_forwarding_buffers(); // caches shall be flushed
if (responder) responder->respond(true);
break;
case security::IdChangeCommand::COMMIT:
apply_identifier(id);
change_pending = false;
if (responder) responder->respond(true);
break;
case security::IdChangeCommand::ABORT:
change_pending = false;
break;
case security::IdChangeCommand::DEREG:
change_pending = false;
subscription.reset();
break;
}
}
void request(const dcc::DataRequest& req, std::unique_ptr<ChunkPacket> packet) override {
auto out = cfg.radio_parameters;
out.source = req.source;
out.destination = req.destination;
// DCC profile to IEEE 802.1D priority: upstream TS 102 687 mapping.
// DCC enforcement belongs to the injected Access adapter; no fake CBR.
out.priority = access::user_priority(dcc::map_profile_onto_ac(req.dcc_profile));
const auto view = create_byte_view(*packet, OsiLayer::Network, max_osi_layer());
out.data.assign(view.begin(), view.end());
auto result = validate(out, cfg.maximum_gnpdu);
if (result == Result::accepted) result = access.request(std::move(out));
if (report) report(result);
}
void indicate(const gn::DataIndication& ind, std::unique_ptr<UpPacket> packet) override {
if (ind.upper_protocol == gn::UpperProtocol::Unknown) {
// GN-DATA.indication for a packet with no registered upper protocol
// (Common Header next_header "Any"): the raw SDU, no BTP header to strip.
if (!receive_gn) return;
const auto view = create_byte_view(*packet, OsiLayer::Transport, max_osi_layer());
receive_gn(GnIndication {ind, ByteBuffer(view.begin(), view.end())});
return;
}
// IF-NF-002: validate length BEFORE parsing; upstream header parsing
// alone does not reject a short BTP PDU. Never read an incomplete port.
const auto view = create_byte_view(*packet, OsiLayer::Transport, max_osi_layer());
if (view.size() < 4 || !receive) return;
BtpIndication out {};
out.type = ind.upper_protocol == gn::UpperProtocol::BTP_A ? BtpType::a : BtpType::b;
out.destination_port = (std::uint16_t(view[0]) << 8) | view[1];
const std::uint16_t second = (std::uint16_t(view[2]) << 8) | view[3];
if (out.type == BtpType::a) out.source_port = second;
else out.destination_port_info = second;
out.gn = ind;
// SN-DECAP.confirm -> NF-SAP: TS 102 723-8 V2.0.0 clause 5 /
// V1.1.1 Table 27 and TS 103 836-5-1 Annex A.3.
if (ind.certificate_id)
out.certificate_id = ByteBuffer(ind.certificate_id->begin(), ind.certificate_id->end());
out.data.assign(view.begin() + 4, view.end());
receive(std::move(out));
}
};
Stack::Stack(StackConfig config, ManualRuntime& runtime, Access& access,
vanetza::security::SecurityEntity* security, security::IdChangeService* id_change) {
if (config.mib.itsGnMaxSduSize < 4 || config.mib.itsGnMaxSduSize > 65535 ||
config.maximum_gnpdu < config.mib.itsGnMaxSduSize ||
config.mib.itsGnIfType != gn::InterfaceType::ITS_G5 ||
config.mib.itsGnSnDecapResultHandling != gn::SecurityDecapHandling::Strict)
throw std::invalid_argument("Invalid ITS-G5 stack configuration");
#if VIDF_SECURITY
if (!id_change) {
if (auto* own = dynamic_cast<security::SecurityEntity*>(security)) id_change = &own->id_change();
}
#endif
impl_ = std::make_unique<Impl>(std::move(config), runtime, access, security, id_change);
}
Stack::~Stack() = default;
const StackConfig& Stack::config() const { return impl_->cfg; }
security::IdChangeService* Stack::id_change() { return impl_->subscription ? impl_->id_change : nullptr; }
vanetza::security::SecurityEntity* Stack::security_entity() { return impl_->security; }
bool Stack::identity_change_pending() const { return impl_->change_pending; }
const gn::Address& Stack::address() const { return impl_->cfg.mib.itsGnLocalGnAddr; }
Result Stack::set_address(const gn::Address& address) {
if (impl_->cfg.mib.itsGnLocalAddrConfMethod != gn::AddrConfMethod::Managed) return Result::unsupported;
if (impl_->change_pending) return Result::identity_change_pending;
impl_->cfg.mib.itsGnLocalGnAddr = address;
impl_->router.set_address(address);
return Result::accepted;
}
void Stack::on_receive(Receive receive) { impl_->receive = std::move(receive); }
void Stack::on_receive_gn(ReceiveGn receive) { impl_->receive_gn = std::move(receive); }
void Stack::on_access_result(Report report) { impl_->report = std::move(report); }
void Stack::report_local_channel_load(dcc::ChannelLoad load) { impl_->dcc_information_sharing.update_local_cbr(load); }
void Stack::report_tx_power(unsigned dbm) { impl_->dcc_information_sharing.set_tx_power(dbm); }
std::optional<dcc::ChannelLoad> Stack::global_channel_busy_ratio() const { return impl_->global_cbr; }
Result Stack::advance(Clock::time_point time) {
if (time < impl_->runtime.now()) return Result::time_regression;
impl_->runtime.trigger(time);
return Result::accepted;
}
Result Stack::update_position(const PositionFix& fix) {
if (!has_horizontal_position(fix) || fix.latitude.value() < -90 || fix.latitude.value() > 90 ||
fix.longitude.value() < -180 || fix.longitude.value() > 180 ||
fix.timestamp > impl_->runtime.now()) return Result::invalid_argument;
impl_->router.update_position(fix);
impl_->position_valid = true;
return Result::accepted;
}
Result Stack::indicate(AlDataIndication ind) {
if (ind.data.empty() || ind.data.size() > impl_->cfg.maximum_gnpdu) return Result::invalid_argument;
try {
auto packet = std::make_unique<UpPacket>(CohesivePacket(std::move(ind.data), OsiLayer::Network));
impl_->router.indicate(std::move(packet), ind.source, ind.destination);
return Result::accepted; // submitted for processing, NOT a receive verdict
} catch (const std::bad_alloc&) { return Result::resource_limit; }
catch (const std::exception&) { return Result::rejected; }
}
Result Stack::request(BtpRequest req) {
// IF-NF-001: mutually exclusive BTP header variants and conditional fields.
if ((req.type != BtpType::a && req.type != BtpType::b) ||
(req.type == BtpType::a && (!req.source_port || req.destination_port_info)) ||
(req.type == BtpType::b && req.source_port)) return Result::invalid_argument;
if (req.data.size() > impl_->cfg.mib.itsGnMaxSduSize - 4u) return Result::resource_limit;
if (req.communication_profile != gn::CommunicationProfile::ITS_G5 &&
req.communication_profile != gn::CommunicationProfile::Unspecified) return Result::unsupported;
if (req.security_profile && *req.security_profile != impl_->cfg.security_profile) return Result::unsupported;
if (impl_->cfg.mib.itsGnSecurity && !impl_->security) return Result::security_unavailable;
if (impl_->change_pending) return Result::identity_change_pending;
if (!impl_->position_valid) return Result::rejected;
if (req.maximum_hop_limit && (*req.maximum_hop_limit == 0 || *req.maximum_hop_limit > 255))
return Result::invalid_argument;
if (req.transport != gn::TransportType::SHB && req.transport != gn::TransportType::GBC)
return Result::unsupported; // Upstream GUC, GAC, TSB are unimplemented: GAP-GN-001.
if (req.transport == gn::TransportType::GBC && !boost::get<gn::Area>(&req.destination))
return Result::invalid_argument;
if (req.transport == gn::TransportType::SHB && !boost::get<std::nullptr_t>(&req.destination))
return Result::invalid_argument;
try {
auto packet = std::make_unique<DownPacket>();
// TS 103 836-5-1 clauses 7 and 8.2: both headers occupy four octets.
if (req.type == BtpType::a) {
btp::HeaderA header { host_cast(req.destination_port), host_cast(*req.source_port) };
(*packet)[OsiLayer::Transport] = header;
} else {
btp::HeaderB header { host_cast(req.destination_port), host_cast(req.destination_port_info.value_or(0)) };
(*packet)[OsiLayer::Transport] = header;
}
(*packet)[OsiLayer::Application] = std::move(req.data);
auto apply = [&](gn::DataRequest& gn_req) {
gn_req.upper_protocol = req.type == BtpType::a ? gn::UpperProtocol::BTP_A : gn::UpperProtocol::BTP_B;
gn_req.communication_profile = req.communication_profile;
gn_req.its_aid = req.its_aid;
gn_req.permissions = std::move(req.permissions);
gn_req.security_context = std::move(req.security_context);
gn_req.traffic_class = req.traffic_class;
if (req.maximum_lifetime) gn_req.maximum_lifetime = *req.maximum_lifetime;
if (req.maximum_hop_limit) gn_req.max_hop_limit = *req.maximum_hop_limit;
if (req.repetition) gn_req.repetition = *req.repetition;
};
gn::DataConfirm confirm;
if (req.transport == gn::TransportType::SHB) {
gn::ShbDataRequest gn_req(impl_->cfg.mib);
apply(gn_req);
confirm = impl_->router.request(gn_req, std::move(packet));
} else {
gn::GbcDataRequest gn_req(impl_->cfg.mib);
apply(gn_req);
gn_req.destination = boost::get<gn::Area>(req.destination);
confirm = impl_->router.request(gn_req, std::move(packet));
}
return confirm.accepted() ? Result::accepted : Result::rejected;
} catch (const std::bad_alloc&) { return Result::resource_limit; }
catch (const std::exception&) { return Result::rejected; }
}
Result Stack::request(GnRequest req) {
if (req.data.size() > impl_->cfg.mib.itsGnMaxSduSize) return Result::resource_limit;
if (req.communication_profile != gn::CommunicationProfile::ITS_G5 &&
req.communication_profile != gn::CommunicationProfile::Unspecified) return Result::unsupported;
if (impl_->cfg.mib.itsGnSecurity && !impl_->security) return Result::security_unavailable;
if (impl_->change_pending) return Result::identity_change_pending;
if (!impl_->position_valid) return Result::rejected;
if (req.maximum_hop_limit && (*req.maximum_hop_limit == 0 || *req.maximum_hop_limit > 255))
return Result::invalid_argument;
if (req.transport != gn::TransportType::SHB && req.transport != gn::TransportType::GBC)
return Result::unsupported; // Upstream GUC, GAC, TSB are unimplemented: GAP-GN-001.
if (req.transport == gn::TransportType::GBC && !boost::get<gn::Area>(&req.destination))
return Result::invalid_argument;
if (req.transport == gn::TransportType::SHB && !boost::get<std::nullptr_t>(&req.destination))
return Result::invalid_argument;
try {
auto packet = std::make_unique<DownPacket>();
(*packet)[OsiLayer::Application] = std::move(req.data);
auto apply = [&](gn::DataRequest& gn_req) {
gn_req.upper_protocol = gn::UpperProtocol::Unknown;
gn_req.communication_profile = req.communication_profile;
gn_req.its_aid = req.its_aid;
gn_req.permissions = std::move(req.permissions);
gn_req.security_context = std::move(req.security_context);
gn_req.traffic_class = req.traffic_class;
if (req.maximum_lifetime) gn_req.maximum_lifetime = *req.maximum_lifetime;
if (req.maximum_hop_limit) gn_req.max_hop_limit = *req.maximum_hop_limit;
if (req.repetition) gn_req.repetition = *req.repetition;
};
gn::DataConfirm confirm;
if (req.transport == gn::TransportType::SHB) {
gn::ShbDataRequest gn_req(impl_->cfg.mib);
apply(gn_req);
confirm = impl_->router.request(gn_req, std::move(packet));
} else {
gn::GbcDataRequest gn_req(impl_->cfg.mib);
apply(gn_req);
gn_req.destination = boost::get<gn::Area>(req.destination);
confirm = impl_->router.request(gn_req, std::move(packet));
}
return confirm.accepted() ? Result::accepted : Result::rejected;
} catch (const std::bad_alloc&) { return Result::resource_limit; }
catch (const std::exception&) { return Result::rejected; }
}
}
Reference in New Issue View Git Blame Copy Permalink