From f45cc97134adc8e45371d3ed61e675dee83bd2cc Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Fri, 25 Sep 2026 14:53:19 -0500 Subject: [PATCH] AOL Define host-to-Panda AOL intent, policy and wire contracts with neutral defaults before vehicle adapters register capabilities. Co-authored-by: dirwin31 <83434411+dirwin31@users.noreply.github.com> --- openpilot/selfdrive/pandad/aol_protocol.h | 173 ++++++++++++ openpilot/selfdrive/pandad/aol_wire.h | 125 +++++++++ openpilot/selfdrive/pandad/panda.cc | 11 + openpilot/selfdrive/pandad/panda.h | 12 +- .../pandad/tests/test_aol_protocol.cc | 257 ++++++++++++++++++ .../selfdrive/pandad/tests/test_aol_wire.cc | 115 ++++++++ openpilot/starpilot/aol/README.md | 43 +++ openpilot/starpilot/aol/__init__.py | 1 + openpilot/starpilot/aol/intent.py | 221 +++++++++++++++ openpilot/starpilot/aol/policy.py | 23 ++ openpilot/starpilot/aol/tests/test_wire.py | 71 +++++ openpilot/starpilot/aol/wire.py | 200 ++++++++++++++ openpilot/starpilot/car/__init__.py | 0 openpilot/starpilot/car/ford/aol_policy.h | 17 ++ openpilot/starpilot/car/mazda/aol_policy.h | 9 + openpilot/starpilot/car/tesla/aol_policy.h | 15 + openpilot/starpilot/car/toyota/aol_policy.h | 9 + 17 files changed, 1299 insertions(+), 3 deletions(-) create mode 100644 openpilot/selfdrive/pandad/aol_protocol.h create mode 100644 openpilot/selfdrive/pandad/aol_wire.h create mode 100644 openpilot/selfdrive/pandad/tests/test_aol_protocol.cc create mode 100644 openpilot/selfdrive/pandad/tests/test_aol_wire.cc create mode 100644 openpilot/starpilot/aol/README.md create mode 100644 openpilot/starpilot/aol/__init__.py create mode 100644 openpilot/starpilot/aol/intent.py create mode 100644 openpilot/starpilot/aol/policy.py create mode 100644 openpilot/starpilot/aol/tests/test_wire.py create mode 100644 openpilot/starpilot/aol/wire.py create mode 100644 openpilot/starpilot/car/__init__.py create mode 100644 openpilot/starpilot/car/ford/aol_policy.h create mode 100644 openpilot/starpilot/car/mazda/aol_policy.h create mode 100644 openpilot/starpilot/car/tesla/aol_policy.h create mode 100644 openpilot/starpilot/car/toyota/aol_policy.h diff --git a/openpilot/selfdrive/pandad/aol_protocol.h b/openpilot/selfdrive/pandad/aol_protocol.h new file mode 100644 index 0000000000..bd9e8ea55a --- /dev/null +++ b/openpilot/selfdrive/pandad/aol_protocol.h @@ -0,0 +1,173 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#ifdef __APPLE__ +#include +#endif + +#include "panda/board/health.h" + +constexpr uint64_t AOL_AXIS_MESSAGE_MAX_AGE_NS = 200000000ULL; + +struct AolSafetyProfile { + uint8_t mode; + bool (*accepts_param)(uint16_t); + bool normal_runtime; +}; + +struct AolProfileRegistry { + const AolSafetyProfile *profiles; + size_t count; + + const AolSafetyProfile *find(uint8_t mode) const { + if (profiles == nullptr) return nullptr; + for (size_t i = 0; i < count; ++i) { + if (profiles[i].mode == mode) return &profiles[i]; + } + return nullptr; + } + + const AolSafetyProfile *find(uint8_t mode, uint16_t param) const { + if (profiles == nullptr) return nullptr; + for (size_t i = 0; i < count; ++i) { + if (profiles[i].mode == mode && profiles[i].accepts_param != nullptr && profiles[i].accepts_param(param)) return &profiles[i]; + } + return nullptr; + } +}; + +// Python messaging.new_message and the AOL host use time.monotonic_ns(). +// Linux CLOCK_BOOTTIME includes suspend time; the regular pandad clocks stay +// unchanged, while this protocol uses the matching monotonic epoch only. +inline uint64_t aol_monotonic_ns() { +#ifdef __APPLE__ + mach_timebase_info_data_t scale{}; + if (mach_timebase_info(&scale) != KERN_SUCCESS || scale.denom == 0U) return 0U; + const __uint128_t ns = (static_cast<__uint128_t>(mach_absolute_time()) * scale.numer) / scale.denom; + return ns <= std::numeric_limits::max() ? static_cast(ns) : 0U; +#else + timespec ts{}; + if (clock_gettime(CLOCK_MONOTONIC, &ts) != 0 || ts.tv_sec < 0 || ts.tv_nsec < 0 || ts.tv_nsec >= 1000000000L) return 0U; + const __uint128_t ns = static_cast<__uint128_t>(ts.tv_sec) * 1000000000U + ts.tv_nsec; + return ns <= std::numeric_limits::max() ? static_cast(ns) : 0U; +#endif +} + +inline std::optional parse_aol_status(const unsigned char *data, int count) { + if (data == nullptr || count != static_cast(sizeof(aol_safety_health_t))) return std::nullopt; + aol_safety_health_t status{}; + std::memcpy(&status, data, sizeof(status)); + if (status.magic != AOL_SAFETY_PROTOCOL_MAGIC || status.version != AOL_SAFETY_PROTOCOL_VERSION) return std::nullopt; + return status; +} + +inline bool aol_capable(const std::optional &status, uint8_t expected_mode, + const AolProfileRegistry ®istry) { + const AolSafetyProfile *profile = registry.find(expected_mode, status ? status->safety_param : 0U); + return status && profile && profile->accepts_param && (status->capability_flags & 0x1U) != 0U && + status->safety_mode == expected_mode && profile->accepts_param(status->safety_param); +} + +inline bool aol_runtime_enabled(bool replay, const std::optional &status, + const AolProfileRegistry ®istry) { + if (replay) return true; + if (!status) return false; + const AolSafetyProfile *profile = registry.find(status->safety_mode, status->safety_param); + return profile && profile->normal_runtime && aol_capable(status, profile->mode, registry); +} + +struct AolAxisInput { + bool alive_valid = false; + bool qualified = false; + std::string session; + uint64_t observed_ns = 0; + uint64_t valid_until_ns = 0; + uint64_t message_ns = 0; + bool desired_lateral = false; + bool desired_longitudinal = false; + bool retain_lateral_arm = false; + bool optional_set_release = false; +}; + +struct AolWritePlan { + bool capable = false; + uint8_t request_mask = 0; + bool retain_lateral_arm = false; +}; + +struct AolOutcome { + bool compatible = false; + std::optional status; + std::string session; +}; + +// One pandad owner negotiates a new host session with a native zero-request +// acknowledgement before allowing either independent axis to be requested. +class AolAxisNegotiator { +public: + explicit AolAxisNegotiator(const AolProfileRegistry ®istry) : registry_(registry) {} + + AolWritePlan prepare(const std::optional &before, const AolAxisInput &axis, + uint64_t now_ns, uint8_t expected_mode) { + const uint16_t param = before ? before->safety_param : 0U; + const bool optional_capable = before && ((before->capability_flags & 0x2U) != 0U); + if (expected_mode != expected_mode_ || param != expected_param_ || optional_capable != optional_capable_) { + reset_pending_ = true; + } + expected_mode_ = expected_mode; + expected_param_ = param; + optional_capable_ = optional_capable; + if (!aol_capable(before, expected_mode, registry_)) { + reset_pending_ = true; + return {}; + } + if (axis.alive_valid && axis.qualified && !axis.session.empty() && + axis.observed_ns <= now_ns && now_ns <= axis.valid_until_ns && + axis.message_ns <= now_ns && now_ns - axis.message_ns <= AOL_AXIS_MESSAGE_MAX_AGE_NS) { + if (axis.session != session_) { + session_ = axis.session; + reset_pending_ = true; + } + if (!reset_pending_) { + return {true, static_cast((axis.desired_lateral ? 0x1U : 0U) | + (axis.desired_longitudinal ? 0x2U : 0U) | + ((axis.optional_set_release && (before->capability_flags & 0x2U) != 0U && + (before->safety_param == 0x815U || before->safety_param == 0x895U)) ? 0x4U : 0U)), + axis.retain_lateral_arm}; + } + } + return {true, 0U}; + } + + AolOutcome complete(const AolWritePlan &plan, bool write_ok, + const std::optional &after) { + if (!plan.capable || !write_ok || !aol_capable(after, expected_mode_, registry_)) { + reset_pending_ = true; + return {}; + } + if (after->safety_param != expected_param_ || + (((plan.request_mask & 0x4U) != 0U) && (after->capability_flags & 0x2U) == 0U)) { + reset_pending_ = true; + return {}; + } + if (reset_pending_ && plan.request_mask == 0U && after->request_mask == 0U) { + reset_pending_ = false; + } + return {true, after, reset_pending_ ? "" : session_}; + } + +private: + AolProfileRegistry registry_; + std::string session_; + bool reset_pending_ = true; + uint8_t expected_mode_ = 0; + uint16_t expected_param_ = 0; + bool optional_capable_ = false; +}; diff --git a/openpilot/selfdrive/pandad/aol_wire.h b/openpilot/selfdrive/pandad/aol_wire.h new file mode 100644 index 0000000000..fb611a589a --- /dev/null +++ b/openpilot/selfdrive/pandad/aol_wire.h @@ -0,0 +1,125 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include + +#include "openpilot/cereal/gen/cpp/custom.capnp.h" + +// Inner bounded, flat Cap'n Proto payload for Event.aolSafetyWire @124 :Data. +// The outer Event validity remains a separate native status check. +struct AolPandaSlot { + uint16_t slot_index = 0; + std::string hardware_serial; + uint16_t safety_model = 0; + uint16_t safety_param = 0; + uint16_t alternative_experience = 0; + bool controls_allowed = false; + bool safety_rx_checks_invalid = false; + bool heartbeat_lost = false; + uint32_t faults = 0; +}; + +struct AolPandaStateSnapshot { + // Source Event BOOTTIME stamp: provenance only, not the wire's lease clock. + uint64_t source_mono_time = 0; + std::vector slots; +}; + +struct AolSafetyWireFields { + uint16_t protocol_version = 0; + bool compatible = false; + uint64_t observed_mono_time = 0; + uint64_t valid_until_mono_time = 0; + uint16_t safety_model = 0; + uint16_t safety_param = 0; + bool lateral_allowed = false; + bool longitudinal_allowed = false; + bool requested_lateral = false; + bool requested_longitudinal = false; + std::string panda_serial; + std::string axis_session_id; + AolPandaStateSnapshot inventory; +}; + +inline std::vector encode_aol_safety_wire(const AolSafetyWireFields &fields) { + if (fields.panda_serial.size() > 96 || fields.axis_session_id.size() > 96 || + fields.valid_until_mono_time < fields.observed_mono_time) return {}; + if (fields.inventory.source_mono_time == 0U || fields.inventory.slots.empty() || fields.inventory.slots.size() > 2U) return {}; + for (const auto &slot : fields.inventory.slots) { + if (slot.hardware_serial.empty() || slot.hardware_serial.size() > 96U) return {}; + } + capnp::MallocMessageBuilder builder; + auto out = builder.initRoot(); + out.setKind(1U); + out.setVersion(2U); + out.setProtocolVersion(fields.protocol_version); + out.setCompatible(fields.compatible); + out.setObservedMonoTime(fields.observed_mono_time); + out.setValidUntilMonoTime(fields.valid_until_mono_time); + out.setSafetyModel(fields.safety_model); + out.setSafetyParam(fields.safety_param); + out.setLateralAllowed(fields.lateral_allowed); + out.setLongitudinalAllowed(fields.longitudinal_allowed); + out.setRequestedLateral(fields.requested_lateral); + out.setRequestedLongitudinal(fields.requested_longitudinal); + out.setPandaSerial(fields.panda_serial); + out.setAxisSessionId(fields.axis_session_id); + out.setSourcePandaStatesMonoTime(fields.inventory.source_mono_time); + auto inventory = out.initPandaInventory(fields.inventory.slots.size()); + for (size_t i = 0; i < fields.inventory.slots.size(); ++i) { + const auto &slot = fields.inventory.slots[i]; + auto entry = inventory[i]; + entry.setSlotIndex(slot.slot_index); + entry.setHardwareSerial(slot.hardware_serial); + entry.setSafetyModel(slot.safety_model); + entry.setSafetyParam(slot.safety_param); + entry.setAlternativeExperience(slot.alternative_experience); + entry.setControlsAllowed(slot.controls_allowed); + entry.setSafetyRxChecksInvalid(slot.safety_rx_checks_invalid); + entry.setHeartbeatLost(slot.heartbeat_lost); + entry.setFaults(slot.faults); + } + auto flat = capnp::messageToFlatArray(builder); + auto bytes = flat.asBytes(); + if (bytes.size() > 512) return {}; + return {bytes.begin(), bytes.end()}; +} + +// Card's armed bit keeps a previously authorized session alive while its axis +// request is zero. It never grants an actuator request or native permission. +inline bool aol_armed_intent(kj::ArrayPtr bytes, uint64_t message_ns, + uint64_t car_state_ns, uint64_t now_ns, bool optional_release = false) { + constexpr uint64_t MAX_AGE_NS = 30000000ULL; + if (bytes.size() < 16U || bytes.size() > 512U || bytes.size() % 8U != 0U || + message_ns == 0U || car_state_ns == 0U || message_ns > now_ns || + car_state_ns > now_ns || now_ns - message_ns > MAX_AGE_NS || + (message_ns > car_state_ns ? message_ns - car_state_ns : car_state_ns - message_ns) > MAX_AGE_NS) return false; + uint32_t segments = 0U, words = 0U; + std::memcpy(&segments, bytes.begin(), sizeof(segments)); + std::memcpy(&words, bytes.begin() + 4U, sizeof(words)); + if (segments != 0U || words > 63U || bytes.size() != 8U + 8U * words) return false; + try { + auto aligned = kj::heapArray(bytes.size() / sizeof(capnp::word)); + std::memcpy(aligned.begin(), bytes.begin(), bytes.size()); + capnp::ReaderOptions options; + options.traversalLimitInWords = 63U; + options.nestingLimit = 4; + capnp::FlatArrayMessageReader reader(aligned.asPtr(), options); + auto intent = reader.getRoot(); + auto producer = intent.getProducerSessionId(); + return intent.getKind() == 2U && intent.getVersion() == 1U && + producer.size() > 0U && producer.size() <= 96U && + intent.getSettingsQualified() && (optional_release ? intent.getOptionalSetRelease() : intent.getLateralArmed()) && + intent.getCarStateLogMonoTime() == message_ns && + intent.getObservedMonoTime() <= now_ns && + now_ns - intent.getObservedMonoTime() <= MAX_AGE_NS && + now_ns <= intent.getValidUntilMonoTime(); + } catch (const kj::Exception &) { + return false; + } +} diff --git a/openpilot/selfdrive/pandad/panda.cc b/openpilot/selfdrive/pandad/panda.cc index d067a73107..af79ba435b 100644 --- a/openpilot/selfdrive/pandad/panda.cc +++ b/openpilot/selfdrive/pandad/panda.cc @@ -1,4 +1,5 @@ #include "selfdrive/pandad/panda.h" +#include "selfdrive/pandad/aol_protocol.h" #include @@ -90,6 +91,12 @@ std::optional Panda::get_state() { return err >= 0 ? std::make_optional(health) : std::nullopt; } +std::optional Panda::get_aol_safety_state() { + aol_safety_health_t status {0}; + int count = handle->control_read(0xd5, 0, 0, (unsigned char*)&status, sizeof(status)); + return parse_aol_status(reinterpret_cast(&status), count); +} + std::optional Panda::get_can_state(uint16_t can_number) { can_health_t can_health {0}; int err = handle->control_read(0xc2, can_number, 0, (unsigned char*)&can_health, sizeof(can_health)); @@ -138,6 +145,10 @@ void Panda::send_heartbeat(bool engaged) { handle->control_write(0xf3, engaged, 0); } +bool Panda::set_aol_axis_request(uint8_t axis_mask) { + return handle->control_write(0xf5, axis_mask & 0x3U, 0) >= 0; +} + void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) { handle->control_write(0xde, bus, (speed * 10)); } diff --git a/openpilot/selfdrive/pandad/panda.h b/openpilot/selfdrive/pandad/panda.h index b0fb637b6b..8a987dfb04 100644 --- a/openpilot/selfdrive/pandad/panda.h +++ b/openpilot/selfdrive/pandad/panda.h @@ -8,6 +8,7 @@ #include #include #include +#include #include "openpilot/cereal/gen/cpp/car.capnp.h" #include "openpilot/cereal/gen/cpp/log.capnp.h" @@ -45,10 +46,11 @@ struct can_frame { class Panda { private: - std::unique_ptr handle; + std::unique_ptr handle; public: - Panda(std::string serial); + Panda(std::string serial, uint32_t bus_offset=0); + const uint32_t bus_offset; cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN; @@ -69,6 +71,7 @@ public: uint16_t get_fan_speed(); void set_ir_pwr(uint16_t ir_pwr); std::optional get_state(); + std::optional get_aol_safety_state(); std::optional get_can_state(uint16_t can_number); void set_loopback(bool loopback); std::optional> get_firmware_version(); @@ -77,6 +80,7 @@ public: void set_power_saving(bool power_saving); void enable_deepsleep(); void send_heartbeat(bool engaged); + bool set_aol_axis_request(uint8_t axis_mask); void set_can_speed_kbps(uint16_t bus, uint16_t speed); void set_can_fd_auto(uint16_t bus, bool enabled); void set_data_speed_kbps(uint16_t bus, uint16_t speed); @@ -90,7 +94,9 @@ protected: uint8_t receive_buffer[RECV_SIZE + sizeof(can_header) + 64]; uint32_t receive_buffer_size = 0; - Panda() {} + Panda(uint32_t bus_offset=0) : bus_offset(bus_offset) {} + Panda(std::unique_ptr comms, cereal::PandaState::PandaType type, uint32_t offset=0) + : handle(std::move(comms)), bus_offset(offset), hw_type(type) {} void pack_can_buffer(const capnp::List::Reader &can_data_list, std::function write_func); bool unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector &out_vec); diff --git a/openpilot/selfdrive/pandad/tests/test_aol_protocol.cc b/openpilot/selfdrive/pandad/tests/test_aol_protocol.cc new file mode 100644 index 0000000000..11f2859d32 --- /dev/null +++ b/openpilot/selfdrive/pandad/tests/test_aol_protocol.cc @@ -0,0 +1,257 @@ +#include "selfdrive/pandad/aol_protocol.h" + +#include +#include +#include +#include + +namespace { +constexpr uint8_t TEST_MODE = 7U; +constexpr uint8_t TEST_ALT_MODE = 8U; +constexpr uint16_t TEST_PARAM = 0x1234U; +constexpr uint64_t NOW = 1000000000ULL; + +bool test_param(uint16_t param) { return param == TEST_PARAM; } +bool test_alt_param(uint16_t param) { return param == 0x2345U; } +constexpr AolSafetyProfile TEST_PROFILES[] = { + {TEST_MODE, test_param, false}, {TEST_ALT_MODE, test_alt_param, false}, +}; +const AolProfileRegistry TEST_REGISTRY{TEST_PROFILES, std::size(TEST_PROFILES)}; + +aol_safety_health_t status(uint8_t request = 0U, uint8_t permission = 0U) { + return {AOL_SAFETY_PROTOCOL_MAGIC, AOL_SAFETY_PROTOCOL_VERSION, request, permission, + TEST_MODE, TEST_PARAM, 0x1U}; +} + +std::vector bytes(const aol_safety_health_t &s) { + std::vector raw(sizeof(s)); + std::memcpy(raw.data(), &s, sizeof(s)); + return raw; +} + +struct FakeTransport { + std::deque> replies; + bool write_ok = true; + bool wrote = false; + uint8_t last_write = 0xffU; + + std::optional read() { + if (replies.empty()) return std::nullopt; + auto raw = replies.front(); + replies.pop_front(); + return parse_aol_status(raw.data(), static_cast(raw.size())); + } + + bool write(uint8_t request) { + wrote = true; + last_write = request; + return write_ok; + } +}; + +struct Step { + AolWritePlan plan; + AolOutcome outcome; +}; + +Step run(AolAxisNegotiator &negotiator, FakeTransport &transport, const AolAxisInput &axis, + uint64_t now_ns = NOW, uint8_t expected_mode = TEST_MODE) { + auto before = transport.read(); + auto plan = negotiator.prepare(before, axis, now_ns, expected_mode); + bool write_ok = plan.capable && transport.write(plan.request_mask); + auto after = write_ok ? transport.read() : std::nullopt; + return {plan, negotiator.complete(plan, write_ok, after)}; +} + +AolAxisInput axis(const char *session = "drive-1", bool lat = true, bool lon = false) { + return {true, true, session, NOW - 1000000ULL, NOW + 100000000ULL, + NOW - 1000000ULL, lat, lon}; +} + +void queue(FakeTransport &transport, const aol_safety_health_t &before, + const aol_safety_health_t &after) { + transport.replies.push_back(bytes(before)); + transport.replies.push_back(bytes(after)); +} +} // namespace + +int main() { + static_assert(sizeof(aol_safety_health_t) == 11U); + static_assert(offsetof(aol_safety_health_t, safety_param) == 8U); + static_assert(offsetof(aol_safety_health_t, capability_flags) == 10U); + const uint64_t sampled_mono = aol_monotonic_ns(); + assert(sampled_mono > 0U && aol_monotonic_ns() >= sampled_mono); + + auto valid = status(); + auto valid_bytes = bytes(valid); + assert(!parse_aol_status(nullptr, 0)); + assert(!parse_aol_status(valid_bytes.data(), static_cast(valid_bytes.size()) - 1)); + valid.magic = 0; + auto wrong_magic = bytes(valid); + assert(!parse_aol_status(wrong_magic.data(), static_cast(wrong_magic.size()))); + valid = status(); + valid.version = AOL_SAFETY_PROTOCOL_VERSION + 1U; + auto wrong_version = bytes(valid); + assert(!parse_aol_status(wrong_version.data(), static_cast(wrong_version.size()))); + + auto unsupported = status(); + const AolProfileRegistry empty_registry{nullptr, 0U}; + assert(!aol_capable(unsupported, TEST_MODE, empty_registry)); + assert(!aol_runtime_enabled(false, unsupported, empty_registry)); + assert(aol_runtime_enabled(true, unsupported, empty_registry)); + const AolProfileRegistry null_storage{nullptr, 1U}; + assert(!aol_capable(unsupported, TEST_MODE, null_storage)); + const AolSafetyProfile null_predicate{TEST_MODE, nullptr, true}; + const AolProfileRegistry null_predicate_registry{&null_predicate, 1U}; + assert(!aol_capable(unsupported, TEST_MODE, null_predicate_registry)); + assert(!aol_runtime_enabled(false, unsupported, null_predicate_registry)); + unsupported.capability_flags = 0U; // RELEASE/old firmware + assert(!aol_capable(unsupported, TEST_MODE, TEST_REGISTRY)); + unsupported = status(); + unsupported.safety_mode = 1U; + assert(!aol_capable(unsupported, TEST_MODE, TEST_REGISTRY)); + unsupported = status(); + unsupported.safety_param ^= 0x1U; + assert(!aol_capable(unsupported, TEST_MODE, TEST_REGISTRY)); + + AolAxisNegotiator negotiator(TEST_REGISTRY); + AolAxisNegotiator temporary_registry_negotiator(AolProfileRegistry{TEST_PROFILES, std::size(TEST_PROFILES)}); + FakeTransport temporary_registry_transport; + queue(temporary_registry_transport, status(), status()); + const auto temporary_registry_result = run(temporary_registry_negotiator, temporary_registry_transport, axis()); + assert(temporary_registry_result.plan.capable && temporary_registry_result.outcome.compatible); + FakeTransport t; + t.replies.push_back({}); // absent/short first reply cannot authorize a write + auto result = run(negotiator, t, axis()); + assert(!result.plan.capable && !result.outcome.compatible && !t.wrote); + + queue(t, status(0x3U, 0x3U), status()); // new process flushes old request first + result = run(negotiator, t, axis()); + assert(result.plan.capable && t.last_write == 0U && result.outcome.compatible); + assert(result.outcome.session == "drive-1"); + queue(t, status(), status(0x1U, 0x1U)); + result = run(negotiator, t, axis()); + assert(t.last_write == 0x1U && result.outcome.status->request_mask == 0x1U); + + queue(t, status(0x1U, 0x1U), status(0x1U, 0x1U)); + result = run(negotiator, t, axis("drive-1", true, true)); + assert(t.last_write == 0x3U && result.outcome.compatible); + assert(result.outcome.status->request_mask == 0x1U); // old long ack; lat remains usable + queue(t, status(0x3U, 0x3U), status(0x3U, 0x3U)); + result = run(negotiator, t, axis("drive-1", false, true)); + assert(t.last_write == 0x2U && result.outcome.status->request_mask == 0x3U); + + queue(t, status(0x3U, 0x3U), status()); + result = run(negotiator, t, axis("drive-2")); + assert(t.last_write == 0U && result.outcome.session == "drive-2"); + queue(t, status(), status(0x1U, 0x1U)); + result = run(negotiator, t, axis("drive-2")); + assert(t.last_write == 0x1U); + + auto stale = axis("drive-2"); + stale.valid_until_ns = NOW - 1U; + queue(t, status(0x1U, 0x1U), status()); + result = run(negotiator, t, stale); + assert(t.last_write == 0U && result.outcome.status->request_mask == 0U); + + t.replies.push_back(bytes(status())); + t.write_ok = false; + result = run(negotiator, t, axis("drive-3")); + assert(t.last_write == 0U && !result.outcome.compatible && result.outcome.session.empty()); + t.write_ok = true; + queue(t, status(), status(0x1U, 0x1U)); // zero write without zero ack cannot finish reset + result = run(negotiator, t, axis("drive-3")); + assert(t.last_write == 0U && result.outcome.session.empty()); + queue(t, status(), status()); + result = run(negotiator, t, axis("drive-3")); + assert(t.last_write == 0U && result.outcome.session == "drive-3"); + + t.replies.push_back(bytes(status())); + t.replies.push_back({}); // failed second read invalidates this tick and resets + result = run(negotiator, t, axis("drive-3")); + assert(t.last_write == 0x1U && !result.outcome.compatible); + queue(t, status(), status()); + result = run(negotiator, t, axis("drive-3")); + assert(t.last_write == 0U && result.outcome.session == "drive-3"); + + auto wrong_identity = status(); + wrong_identity.safety_mode = 1U; + queue(t, status(), wrong_identity); // safety identity changed after the write + result = run(negotiator, t, axis("drive-3")); + assert(t.last_write == 0x1U && !result.outcome.compatible); + queue(t, status(), status()); + result = run(negotiator, t, axis("drive-3")); + assert(t.last_write == 0U && result.outcome.session == "drive-3"); + + AolAxisNegotiator mode_switch_negotiator(TEST_REGISTRY); + FakeTransport switch_transport; + queue(switch_transport, status(), status()); + result = run(mode_switch_negotiator, switch_transport, axis("same-session")); + assert(switch_transport.last_write == 0U && result.outcome.compatible); + queue(switch_transport, status(), status(0x1U, 0x1U)); + result = run(mode_switch_negotiator, switch_transport, axis("same-session")); + assert(switch_transport.last_write == 0x1U); + auto alternate = status(); + alternate.safety_mode = TEST_ALT_MODE; + alternate.safety_param = 0x2345U; + queue(switch_transport, alternate, alternate); + result = run(mode_switch_negotiator, switch_transport, axis("same-session"), NOW, TEST_ALT_MODE); + assert(switch_transport.last_write == 0U && result.outcome.compatible); + + // A fresh Python axis frame uses CLOCK_MONOTONIC. Treating BOOTTIME as the + // same clock after a nine-second suspend leaves the native request at zero. + AolAxisNegotiator mono_negotiator(TEST_REGISTRY); + FakeTransport mono_transport; + queue(mono_transport, status(), status()); + result = run(mono_negotiator, mono_transport, axis("clock-session"), NOW); + assert(result.plan.request_mask == 0U && result.outcome.compatible); + queue(mono_transport, status(), status(0x1U, 0x1U)); + result = run(mono_negotiator, mono_transport, axis("clock-session"), NOW + 1000000ULL); + assert(result.plan.request_mask == 0x1U); + queue(mono_transport, status(0x1U, 0x1U), status()); + result = run(mono_negotiator, mono_transport, axis("clock-session"), NOW + 201000000ULL); + assert(result.plan.request_mask == 0U); // genuine stale frame still revoked + + // Suspended axes keep only the already-negotiated physical arm. No actuator + // request is added, and a fresh session or stale frame cannot retain it. + AolAxisNegotiator paused_negotiator(TEST_REGISTRY); + FakeTransport paused_transport; + auto paused = axis("paused-session", false, false); + paused.retain_lateral_arm = true; + queue(paused_transport, status(), status()); + result = run(paused_negotiator, paused_transport, paused); + assert(result.plan.request_mask == 0U && !result.plan.retain_lateral_arm); + for (int i = 0; i < 5; ++i) { + queue(paused_transport, status(), status()); + result = run(paused_negotiator, paused_transport, paused); + assert(result.plan.request_mask == 0U && result.plan.retain_lateral_arm); + } + queue(paused_transport, status(), status(0x1U, 0x1U)); + result = run(paused_negotiator, paused_transport, axis("paused-session")); + assert(result.plan.request_mask == 0x1U); + queue(paused_transport, status(0x1U, 0x1U), status()); + result = run(paused_negotiator, paused_transport, paused, NOW + 201000000ULL); + assert(result.plan.request_mask == 0U && !result.plan.retain_lateral_arm); + paused.alive_valid = false; + queue(paused_transport, status(), status()); + result = run(paused_negotiator, paused_transport, paused); + assert(result.plan.request_mask == 0U && !result.plan.retain_lateral_arm); + paused.alive_valid = true; + paused.session = "replacement-session"; + queue(paused_transport, status(), status()); + result = run(paused_negotiator, paused_transport, paused); + assert(result.plan.request_mask == 0U && !result.plan.retain_lateral_arm); + paused.retain_lateral_arm = false; + queue(paused_transport, status(), status()); + result = run(paused_negotiator, paused_transport, paused); + assert(result.plan.request_mask == 0U && !result.plan.retain_lateral_arm); + + AolAxisNegotiator boot_negotiator(TEST_REGISTRY); + FakeTransport boot_transport; + queue(boot_transport, status(), status()); + result = run(boot_negotiator, boot_transport, axis("clock-session"), NOW + 9000000000ULL); + assert(result.plan.request_mask == 0U && result.outcome.compatible); + queue(boot_transport, status(), status()); + result = run(boot_negotiator, boot_transport, axis("clock-session"), NOW + 9001000000ULL); + assert(result.plan.request_mask == 0U); +} diff --git a/openpilot/selfdrive/pandad/tests/test_aol_wire.cc b/openpilot/selfdrive/pandad/tests/test_aol_wire.cc new file mode 100644 index 0000000000..76ddcc2ba8 --- /dev/null +++ b/openpilot/selfdrive/pandad/tests/test_aol_wire.cc @@ -0,0 +1,115 @@ +#include "selfdrive/pandad/aol_wire.h" +#include "openpilot/cereal/gen/cpp/log.capnp.h" + +#include +#include +#include + +static constexpr uint64_t now = 1000000000ULL; + +int main() { + const AolSafetyWireFields fields = {1, true, 100, 200, 5, 34, true, false, true, false, "panda", "axis", + {now + 9000000000ULL, {{0, "panda", 5, 34, 0, true, false, false, 0}}}}; + auto bytes = encode_aol_safety_wire(fields); + assert(!bytes.empty() && bytes.size() <= 512); + capnp::MallocMessageBuilder event_builder; + auto event = event_builder.initRoot(); + event.setAolSafetyWire(kj::arrayPtr(bytes.data(), bytes.size())); + assert(event.isAolSafetyWire()); + auto aligned = kj::heapArray(bytes.size() / sizeof(capnp::word)); + std::memcpy(aligned.begin(), bytes.data(), bytes.size()); + capnp::FlatArrayMessageReader current(aligned.asPtr()); + auto wire = current.getRoot(); + assert(wire.getVersion() == 2U && wire.getPandaSerial() == "panda"); + assert(wire.getSourcePandaStatesMonoTime() == now + 9000000000ULL); + assert(wire.getPandaInventory().size() == 1U); + assert(wire.getPandaInventory()[0].getHardwareSerial() == "panda"); + assert(wire.getPandaInventory()[0].getSafetyParam() == 34U); + // Historic exact v1 bytes remain readable after adding the custom fields. + const uint64_t legacy_words[] = { + 0x0000000900000000ULL, 0x0002000400000000ULL, 0x0005000100010b01ULL, + 0x0000000000000064ULL, 0x00000000000000c8ULL, 0x0000000000000022ULL, + 0x0000003200000005ULL, 0x0000002a00000005ULL, 0x00000061646e6170ULL, + 0x0000000073697861ULL + }; + capnp::FlatArrayMessageReader legacy(kj::arrayPtr(reinterpret_cast(legacy_words), + sizeof(legacy_words) / sizeof(capnp::word))); + auto old_wire = legacy.getRoot(); + assert(old_wire.getVersion() == 1U && old_wire.getPandaSerial() == "panda"); + assert(old_wire.getSourcePandaStatesMonoTime() == 0U && old_wire.getPandaInventory().size() == 0U); + for (uint8_t value : bytes) std::printf("%02x", value); + std::printf("\n"); + auto bad = fields; + bad.panda_serial = std::string(97, 'x'); + assert(encode_aol_safety_wire(bad).empty()); + bad = fields; + bad.valid_until_mono_time = 99; + assert(encode_aol_safety_wire(bad).empty()); + bad = fields; + bad.inventory = {}; + assert(encode_aol_safety_wire(bad).empty()); + bad = fields; + bad.inventory.slots[0].hardware_serial = std::string(97, 'x'); + assert(encode_aol_safety_wire(bad).empty()); + bad = fields; + bad.inventory.slots.push_back({1, "auxiliary", 19, 0, 0, false, false, false, 0}); + auto pair_bytes = encode_aol_safety_wire(bad); + assert(!pair_bytes.empty() && pair_bytes.size() <= 512U); + bad.inventory.slots.push_back({2, "third", 19, 0, 0, false, false, false, 0}); + assert(encode_aol_safety_wire(bad).empty()); + capnp::MallocMessageBuilder intent_builder; + auto intent = intent_builder.initRoot(); + intent.setKind(2); + intent.setVersion(1); + intent.setProducerSessionId("card-session"); + intent.setCarStateLogMonoTime(now - 10000000ULL); + intent.setObservedMonoTime(now - 10000000ULL); + intent.setValidUntilMonoTime(now + 100000000ULL); + intent.setSettingsQualified(true); + intent.setLateralArmed(true); + auto armed = [&](uint64_t message_ns = now - 10000000ULL, uint64_t source_ns = now - 10000000ULL) { + auto flat = capnp::messageToFlatArray(intent_builder); + return aol_armed_intent(flat.asBytes(), message_ns, source_ns, now); + }; + assert(armed()); + assert(armed(now - 10000000ULL, now - 20000000ULL)); // newest Card may precede the next axis publication + assert(!armed(0)); + assert(!armed(now + 1)); + assert(!armed(now - 31000000ULL)); + assert(!armed(now - 10000000ULL, now + 1)); + assert(!armed(now - 10000000ULL, now - 41000000ULL)); + intent.setLateralArmed(false); + assert(!armed()); + intent.setLateralArmed(true); + intent.setSettingsQualified(false); + assert(!armed()); + intent.setSettingsQualified(true); + intent.setObservedMonoTime(now + 1); + assert(!armed()); + intent.setObservedMonoTime(now - 31000000ULL); + assert(!armed()); + intent.setObservedMonoTime(now - 10000000ULL); + intent.setValidUntilMonoTime(now - 1); + assert(!armed()); + intent.setValidUntilMonoTime(now + 100000000ULL); + intent.setKind(1); + assert(!armed()); + intent.setKind(2); + intent.setVersion(2); + assert(!armed()); + intent.setVersion(1); + intent.setProducerSessionId(""); + assert(!armed()); + intent.setProducerSessionId(std::string(97, 'x')); + assert(!armed()); + intent.setProducerSessionId("card-session"); + auto flat = capnp::messageToFlatArray(intent_builder); + auto raw = flat.asBytes(); + std::vector malformed(raw.begin(), raw.end()); + malformed[0] = 1; // multiple segments are forbidden + assert(!aol_armed_intent(kj::arrayPtr(malformed.data(), malformed.size()), now - 10000000ULL, now - 10000000ULL, now)); + malformed[0] = 0; + malformed[8] = 0xff; // malformed root pointer + assert(!aol_armed_intent(kj::arrayPtr(malformed.data(), malformed.size()), now - 10000000ULL, now - 10000000ULL, now)); + return 0; +} diff --git a/openpilot/starpilot/aol/README.md b/openpilot/starpilot/aol/README.md new file mode 100644 index 0000000000..d199548f7d --- /dev/null +++ b/openpilot/starpilot/aol/README.md @@ -0,0 +1,43 @@ +# Independent lateral and longitudinal control + +AOL connects Card button intent, SelfdriveD axis decisions, Controls output, +driver monitoring and acknowledged Panda permissions. Either control axis can +remain active independently while its permission is valid. + +The shared owner handles settings, gestures, transport, freshness and axis +arbitration. Vehicle adapters in `starpilot/car//aol.py` own supported +CarParams, safety profiles and button assignments. `policy.py` defines the +contract; `vehicle.py` dispatches to registered adapters. Unknown or malformed +configurations receive no capability. Settings availability, intent admission +and ordinary runtime support are separate: a saved preference cannot grant +steering or longitudinal permission. Current adapters do not cover the entire +vehicle catalog. + +Card publishes desired intent; SelfdriveD resolves the two desired axes; Controls +requires matching current native permission before actuating either axis. +Session changes, expired messages, invalid required CAN and the existing +actuator limits still apply. Lateral-only operation uses engaged driver +monitoring independently of the Always On DM preference. + +Intent and axis messages have a 30 ms freshness bound; native acknowledgments +have a 200 ms bound. The host and pandad use the same monotonic clock. A new +session first negotiates an acknowledged zero-axis request. These checks are +independent of each vehicle's Panda safety policy. + +`aolAxisState` uses custom slot 16. Intent and safety use reserved Data events 125 +and 124 with bounded, versioned flat Cap'n Proto payloads. Mapd slots 17–19 remain +separate. Decode stored messages with their writing schema; never reinterpret +an incompatible cache or reuse an occupied event identifier. Upstream vehicle +schemas remain unchanged. + +`AolBrakePauseSpeedMps` stores meters per second. If absent, `PauseAOLOnBrake` +retains its historical effective meters-per-second value, regardless of the old +UI label. An invalid threshold disables the additional AOL request without +rewriting the preference. Settings convert display units when writing the new +key. Invalid CAN or transfer of button ownership cancels an incomplete hold; +a deliberate axis pause remains latched until changed by the driver. + +Common transport and arbitration tests live in `aol/tests`. Vehicle host tests +live in `selfdrive/car/tests`, and native safety tests in +`opendbc_repo/opendbc/safety/tests`. Keep new vehicle admission and its positive +and negative safety tests with the corresponding vehicle port. diff --git a/openpilot/starpilot/aol/__init__.py b/openpilot/starpilot/aol/__init__.py new file mode 100644 index 0000000000..39544bc48f --- /dev/null +++ b/openpilot/starpilot/aol/__init__.py @@ -0,0 +1 @@ +"""Independent-axis intent and permission contracts.""" diff --git a/openpilot/starpilot/aol/intent.py b/openpilot/starpilot/aol/intent.py new file mode 100644 index 0000000000..456b60b661 --- /dev/null +++ b/openpilot/starpilot/aol/intent.py @@ -0,0 +1,221 @@ +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path + +from opendbc.car.structs import car +from openpilot.cereal import log +from openpilot.selfdrive.car.cruise import CRUISE_LONG_PRESS + +ButtonType = car.CarState.ButtonEvent.Type +GearShifter = car.CarState.GearShifter + +AOL_TOGGLE = 9 +PAUSE_LATERAL = 3 +PAUSE_LONGITUDINAL = 4 + +@dataclass(frozen=True) +class AolSettings: + enabled: bool + pause_brake_mps: float + lkas_action: int + main_action: int + distance_actions: tuple[int, int, int] + cancel_actions: tuple[int, int, int] + + +def _button_action(params, key: str) -> int: + try: + value = int(params.get(key, return_default=True) or '0') + except (TypeError, ValueError): + return 0 + return value if value in (0, PAUSE_LATERAL, PAUSE_LONGITUDINAL, AOL_TOGGLE) else 0 + + +def read_settings(params) -> AolSettings: + # Legacy thresholds are effectively SI despite the mph label; preserve them until replaced. + # Read raw bytes so malformed overrides cannot silently restore AOL. + pause_mps = 0.0 + for key in ('AolBrakePauseSpeedMps', 'PauseAOLOnBrake'): + try: + pause_mps = float(Path(params.get_param_path(key)).read_bytes()) + except FileNotFoundError: + continue + except (OSError, ValueError): + pause_mps = float('nan') + break + valid_pause = 0.0 <= pause_mps <= 100.0 + return AolSettings( + enabled=params.get_bool('AlwaysOnLateral') and valid_pause, pause_brake_mps=pause_mps if valid_pause else 0.0, + lkas_action=_button_action(params, 'LKASButtonControl'), + main_action=_button_action(params, 'MainCruiseButtonControl'), + distance_actions=(_button_action(params, 'DistanceButtonControl'), + _button_action(params, 'LongDistanceButtonControl'), + _button_action(params, 'VeryLongDistanceButtonControl')), + cancel_actions=(_button_action(params, 'CancelButtonControl'), + _button_action(params, 'LongCancelButtonControl'), + _button_action(params, 'VeryLongCancelButtonControl')), + ) + + +def independent_axis_requested(settings: AolSettings) -> bool: + pause = (PAUSE_LATERAL, PAUSE_LONGITUDINAL) + return settings.enabled or any(action in pause for action in + (settings.lkas_action, settings.main_action, *settings.distance_actions)) + + +def disarming_fault(events, CS) -> bool: + """Temporary steering/known non-driving gears pause output, not driver intent.""" + if CS.steerFaultPermanent: + return True + for event in events: + if event.immediateDisable: + return True + if event.softDisable: + if event.name == log.OnroadEvent.EventName.steerTempUnavailable: + continue + if event.name == log.OnroadEvent.EventName.wrongGear and CS.gearShifter != GearShifter.unknown: + continue + return True + return False + + +class AolCardIntent: + def __init__(self, settings: AolSettings, *, explicit_latch: bool = False): + self.settings = settings + self.explicit_latch = explicit_latch + self.allowed_latch = False + self.pause_lateral = False + self.pause_longitudinal = False + self._gesture_neutral_seen = False + self._distance_neutral_seen = False + self._main_held = False + self._lkas_held = False + self._fault_inhibit = False + self._last_latch_edge_ns = 0 + self._native_rejection_ns = 0 + self._timers = {int(ButtonType.gapAdjustCruise): 0} + self._held = {int(ButtonType.gapAdjustCruise): False} + + def _perform(self, action: int) -> None: + if action == AOL_TOGGLE and self.settings.enabled and not self.explicit_latch: + self.allowed_latch = not self.allowed_latch + if self.allowed_latch: + self.pause_lateral = False + elif action == PAUSE_LATERAL: + self.pause_lateral = not self.pause_lateral + elif action == PAUSE_LONGITUDINAL: + self.pause_longitudinal = not self.pause_longitudinal + + def update(self, CS, *, consumed_buttons: frozenset[int] = frozenset(), fault_active: bool | None = None, + now_ns: int = 0, native_rejection_ns: int = 0, standard_enabled: bool = False) -> None: + if self.explicit_latch and self._native_rejection_ns < native_rejection_ns <= now_ns: + self._native_rejection_ns = native_rejection_ns + if self._last_latch_edge_ns < native_rejection_ns <= now_ns: + # Native authorization was revoked. Keep neutral/held-button history so + # the next fresh physical press arms once, without replaying a held one. + self.allowed_latch = False + if self.explicit_latch and (fault_active is not None or CS.steerFaultPermanent): + self._fault_inhibit = bool(fault_active or CS.steerFaultPermanent) + if self._fault_inhibit: + self.allowed_latch = False + self._gesture_neutral_seen = False + if not CS.canValid or CS.canTimeout: + self.allowed_latch = False + self._gesture_neutral_seen = False + self._distance_neutral_seen = False + self._main_held = False + self._lkas_held = False + for button in self._held: + self._held[button] = False + self._timers[button] = 0 + return + + if not self.settings.enabled: + self.allowed_latch = False + if self.explicit_latch: + self._gesture_neutral_seen = False + if not independent_axis_requested(self.settings): + self.pause_lateral = False + self.pause_longitudinal = False + + lkas_managed = self.settings.lkas_action == AOL_TOGGLE + main_managed = self.settings.main_action == AOL_TOGGLE + if self.explicit_latch and CS.accFaulted: + self.allowed_latch = False + self._gesture_neutral_seen = False + elif self.settings.enabled and not self.explicit_latch and not lkas_managed and not main_managed: + self.allowed_latch = bool(CS.cruiseState.available) + elif not self.explicit_latch and not CS.cruiseState.available: + self.allowed_latch = False + + for button in consumed_buttons: + if button in self._held: + self._held[button] = False + self._timers[button] = 0 + + distance_events = [event for event in CS.buttonEvents if event.type in (ButtonType.gapAdjustCruise, ButtonType.lkas)] + if not self._distance_neutral_seen: + self._distance_neutral_seen = not any(event.pressed for event in distance_events) + + if self.explicit_latch: + previously_held = self._main_held or self._lkas_held + cancel_pressed = False + for event in CS.buttonEvents: + button = int(event.type.raw) + if button == int(ButtonType.mainCruise): + self._main_held = bool(event.pressed) + elif button == int(ButtonType.lkas): + self._lkas_held = bool(event.pressed) + elif button == int(ButtonType.cancel) and event.pressed: + cancel_pressed = True + gesture_held = self._main_held or self._lkas_held + if not previously_held and gesture_held: + self._last_latch_edge_ns = now_ns + if cancel_pressed: + self.allowed_latch = False + elif not self._gesture_neutral_seen: + self._gesture_neutral_seen = not gesture_held + elif not previously_held and gesture_held and self.settings.enabled and not CS.accFaulted and not self._fault_inhibit: + # The physical gesture owns lateral intent even while ordinary cruise + # is engaged. Dropping only the AOL latch leaves standard steering on. + lateral_engaged = not self.pause_lateral and (self.allowed_latch or standard_enabled) + self.allowed_latch = not lateral_engaged + self.pause_lateral = lateral_engaged + + for event in CS.buttonEvents: + button = int(event.type.raw) + if button in consumed_buttons: + continue + if self.explicit_latch and button in (int(ButtonType.cancel), int(ButtonType.lkas), int(ButtonType.mainCruise)): + continue + if button == int(ButtonType.gapAdjustCruise) and not self._distance_neutral_seen: + continue + if button == int(ButtonType.lkas) and event.pressed: + self._perform(self.settings.lkas_action) + elif button == int(ButtonType.mainCruise) and event.pressed: + self._perform(self.settings.main_action) + elif button in self._held: + if event.pressed: + self._held[button] = True + self._timers[button] = 0 + else: + ticks = self._timers[button] + actions = self.settings.distance_actions if button == int(ButtonType.gapAdjustCruise) else self.settings.cancel_actions + if 0 < ticks < CRUISE_LONG_PRESS: + self._perform(actions[0]) + self._held[button] = False + self._timers[button] = 0 + for button, held in self._held.items(): + if held: + self._timers[button] += 1 + actions = self.settings.distance_actions if button == int(ButtonType.gapAdjustCruise) else self.settings.cancel_actions + if self._timers[button] == CRUISE_LONG_PRESS: + self._perform(actions[1]) + elif self._timers[button] == CRUISE_LONG_PRESS * 5: + self._perform(actions[2]) + + def output(self, CS) -> tuple[bool, bool, bool]: + driving = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown) + return (self.allowed_latch and self.settings.enabled and CS.canValid and driving, + self.pause_lateral, self.pause_longitudinal) diff --git a/openpilot/starpilot/aol/policy.py b/openpilot/starpilot/aol/policy.py new file mode 100644 index 0000000000..9428c66207 --- /dev/null +++ b/openpilot/starpilot/aol/policy.py @@ -0,0 +1,23 @@ +"""Vehicle-provided AOL capabilities; the default grants no authority.""" + +from dataclasses import dataclass + + +@dataclass(frozen=True) +class AolVehiclePolicy: + intent_supported: bool = False + settings_supported: bool = False + runtime_supported: bool = False + normal_runtime_supported: bool = False + ordinary_axis_ack_required: bool = False + cruise_main_required: bool = True + explicit_latch: bool = False + distance_personality: bool = False + fixed_cruise_buttons: bool = False + lkas_button_supported: bool = True + physical_stalk_owner: bool = False + distance_pause_only: bool = False + paddle_pause: bool = False + safety_param_addition: int = 0 + alternative_experience_addition: int = 0 + full_axis_runtime_required: bool = False diff --git a/openpilot/starpilot/aol/tests/test_wire.py b/openpilot/starpilot/aol/tests/test_wire.py new file mode 100644 index 0000000000..dc0bcee21d --- /dev/null +++ b/openpilot/starpilot/aol/tests/test_wire.py @@ -0,0 +1,71 @@ +"""Reserved Data transport rejects malformed or historically colliding payloads.""" + +import unittest +import struct +from unittest.mock import Mock, patch + +from openpilot.cereal import custom, messaging +from openpilot.starpilot.aol.wire import (IntentState, SafetyState, MAX_WIRE_BYTES, decode_intent, + decode_safety, encode_intent, encode_safety) + + +class AolWireTests(unittest.TestCase): + def test_native_cpp_flat_fixture_decodes(self): + # Exact output of test_aol_wire.cc (the native encoder test asserts it too). + raw = bytes.fromhex('00000000090000000000000004000200010b0100010005006400000000000000' + + 'c80000000000000022000000000000000500000032000000050000002a000000' + + '70616e64610000006178697300000000') + self.assertEqual(decode_safety(raw), SafetyState(1, True, 100, 200, 5, 34, + True, False, True, False, 'panda', 'axis')) + + def test_roundtrip_immutable_and_bounded(self): + intent = IntentState('card', 3, 100, 100, 120, True, False, True, True) + safety = SafetyState(1, True, 100, 200, 5, 34, True, False, True, False, 'panda', 'axis') + self.assertEqual(decode_intent(encode_intent(intent)), intent) + self.assertEqual(decode_safety(encode_safety(safety)), safety) + decoded = decode_intent(encode_intent(intent)) + self.assertIsNotNone(decoded) + assert decoded is not None + with self.assertRaises(AttributeError): + decoded.__setattr__('allowedLatch', False) + with self.assertRaises(ValueError): + encode_intent(IntentState('x' * 97, 3, 100, 100, 120, True, False, True, True)) + + def test_corrupt_wrong_kind_version_and_pointer_fail_closed(self): + valid = encode_intent(IntentState('card', 3, 100, 100, 120, True, False, True, True)) + # Even a valid packed payload cannot trigger decompression on the flat path. + packed_payload = bytes.fromhex('100950040257010b010105016401c8012211053211052a1f70616e64610f61786973') + for bad in (b'', valid[:3], valid + b'X' * MAX_WIRE_BYTES, b'\xff' * 12, + packed_payload, + struct.pack(' str: + if not isinstance(value, str) or len(value.encode('utf-8')) > MAX_ID_BYTES: + raise ValueError('AOL wire identifier is invalid') + return value + + +def _payload(raw_value, schema, kind: int, versions=(WIRE_VERSION,)): + if not isinstance(raw_value, (bytes, bytearray, memoryview)): + return None + byte_count = raw_value.nbytes if isinstance(raw_value, memoryview) else len(raw_value) + if not 16 <= byte_count <= MAX_WIRE_BYTES or byte_count % 8: + return None + try: + raw = bytes(raw_value) + if len(raw) != byte_count: + return None + segment_count_minus_one, segment_words = struct.unpack_from(' MAX_WIRE_WORDS or len(raw) != 8 + 8 * segment_words: + return None + with schema.from_bytes(raw, traversal_limit_in_words=MAX_WIRE_WORDS, nesting_limit=4) as decoded: + if int(decoded.kind) != kind or int(decoded.version) not in versions: + return None + return decoded.to_dict() + except (ValueError, OverflowError, RuntimeError, UnicodeDecodeError, capnp.KjException): + return None + + +def _memoized(decode): + # Consumers poll at 100 Hz while wires change at 10-100 Hz; results are frozen, so reuse the last exact-bytes decode. + last = [None, None] + + def wrapper(raw): + key = bytes(raw) if isinstance(raw, (bytes, bytearray, memoryview)) else None + if key is None: + return decode(raw) + if last[0] != key: + last[0], last[1] = key, decode(key) + return last[1] + wrapper.__wrapped__ = decode + return wrapper + + +@_memoized +def decode_safety(raw: bytes) -> SafetyState | None: + value = _payload(raw, custom.AolAxisState.SafetyWire, SAFETY_KIND, (WIRE_VERSION, SAFETY_INVENTORY_VERSION)) + if value is None: + return None + try: + source_ns = int(value.get('sourcePandaStatesMonoTime', 0)) + inventory = tuple(PandaSlot(int(slot['slotIndex']), _bounded_id(str(slot.get('hardwareSerial', ''))), + int(slot['safetyModel']), int(slot['safetyParam']), int(slot['alternativeExperience']), + bool(slot['controlsAllowed']), bool(slot['safetyRxChecksInvalid']), + bool(slot['heartbeatLost']), int(slot['faults'])) + for slot in value.get('pandaInventory', ())) + if (int(value['version']) == WIRE_VERSION and (source_ns or inventory) or + int(value['version']) == SAFETY_INVENTORY_VERSION and + (source_ns <= 0 or not 1 <= len(inventory) <= MAX_PANDA_SLOTS)): + return None + result = SafetyState(int(value['protocolVersion']), bool(value['compatible']), int(value['observedMonoTime']), + int(value['validUntilMonoTime']), int(value['safetyModel']), int(value['safetyParam']), + bool(value['lateralAllowed']), bool(value['longitudinalAllowed']), bool(value['requestedLateral']), + bool(value['requestedLongitudinal']), _bounded_id(str(value.get('pandaSerial', ''))), + _bounded_id(str(value.get('axisSessionId', ''))), source_ns, inventory) + if result.validUntilMonoTime < result.observedMonoTime: + return None + return result + except (KeyError, ValueError, OverflowError, RuntimeError, capnp.KjException): + return None + + +@_memoized +def decode_intent(raw: bytes) -> IntentState | None: + value = _payload(raw, custom.AolAxisState.IntentWire, INTENT_KIND) + if value is None: + return None + try: + result = IntentState(_bounded_id(str(value.get('producerSessionId', ''))), int(value['sequence']), + int(value['carStateLogMonoTime']), int(value['observedMonoTime']), int(value['validUntilMonoTime']), + bool(value['allowedLatch']), bool(value['pauseLateral']), bool(value['pauseLongitudinal']), + bool(value['settingsQualified']), bool(value.get('lateralArmed', False)), bool(value.get('optionalSetRelease', False))) + if result.validUntilMonoTime < result.observedMonoTime: + return None + return result + except (KeyError, ValueError, OverflowError, RuntimeError, capnp.KjException): + return None + + +def encode_intent(value: IntentState) -> bytes: + _bounded_id(value.producerSessionId) + message = custom.AolAxisState.IntentWire.new_message( + kind=INTENT_KIND, version=WIRE_VERSION, producerSessionId=value.producerSessionId, + sequence=value.sequence, carStateLogMonoTime=value.carStateLogMonoTime, + observedMonoTime=value.observedMonoTime, validUntilMonoTime=value.validUntilMonoTime, + allowedLatch=value.allowedLatch, pauseLateral=value.pauseLateral, + pauseLongitudinal=value.pauseLongitudinal, settingsQualified=value.settingsQualified, + lateralArmed=value.lateralArmed, optionalSetRelease=value.optionalSetRelease) + data = message.to_bytes() + if len(data) > MAX_WIRE_BYTES: + raise ValueError('AOL intent payload exceeds wire bound') + return data + + +def encode_safety(value: SafetyState) -> bytes: + _bounded_id(value.pandaSerial) + _bounded_id(value.axisSessionId) + inventory = value.pandaInventory + inventory_version = bool(inventory or value.sourcePandaStatesMonoTime) + if inventory_version and (value.sourcePandaStatesMonoTime <= 0 or not 1 <= len(inventory) <= MAX_PANDA_SLOTS): + raise ValueError('AOL Panda inventory is invalid') + for slot in inventory: + _bounded_id(slot.hardwareSerial) + message = custom.AolAxisState.SafetyWire.new_message( + kind=SAFETY_KIND, version=SAFETY_INVENTORY_VERSION if inventory_version else WIRE_VERSION, + protocolVersion=value.protocolVersion, + compatible=value.compatible, observedMonoTime=value.observedMonoTime, + validUntilMonoTime=value.validUntilMonoTime, safetyModel=value.safetyModel, + safetyParam=value.safetyParam, lateralAllowed=value.lateralAllowed, + longitudinalAllowed=value.longitudinalAllowed, requestedLateral=value.requestedLateral, + requestedLongitudinal=value.requestedLongitudinal, pandaSerial=value.pandaSerial, + axisSessionId=value.axisSessionId, sourcePandaStatesMonoTime=value.sourcePandaStatesMonoTime, + pandaInventory=[{'slotIndex': slot.slotIndex, 'hardwareSerial': slot.hardwareSerial, 'safetyModel': slot.safetyModel, + 'safetyParam': slot.safetyParam, 'alternativeExperience': slot.alternativeExperience, + 'controlsAllowed': slot.controlsAllowed, 'safetyRxChecksInvalid': slot.safetyRxChecksInvalid, + 'heartbeatLost': slot.heartbeatLost, 'faults': slot.faults} for slot in inventory]) + data = message.to_bytes() + if len(data) > MAX_WIRE_BYTES: + raise ValueError('AOL safety payload exceeds wire bound') + return data diff --git a/openpilot/starpilot/car/__init__.py b/openpilot/starpilot/car/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/starpilot/car/ford/aol_policy.h b/openpilot/starpilot/car/ford/aol_policy.h new file mode 100644 index 0000000000..1957acdddc --- /dev/null +++ b/openpilot/starpilot/car/ford/aol_policy.h @@ -0,0 +1,17 @@ +#pragma once + +#include "openpilot/selfdrive/pandad/aol_protocol.h" + +static inline bool ford_aol_param(uint16_t param) { + bool result = false; + switch (param) { + case 8U: case 9U: case 10U: case 11U: case 12U: case 13U: + case 18U: case 19U: case 32U: case 33U: case 66U: case 67U: + result = true; + break; + default: + break; + } + return result; +} +inline constexpr AolSafetyProfile FORD_AOL_PROFILE{6U, ford_aol_param, true}; diff --git a/openpilot/starpilot/car/mazda/aol_policy.h b/openpilot/starpilot/car/mazda/aol_policy.h new file mode 100644 index 0000000000..a15d7642f0 --- /dev/null +++ b/openpilot/starpilot/car/mazda/aol_policy.h @@ -0,0 +1,9 @@ +#pragma once + +#include "selfdrive/pandad/aol_protocol.h" + +inline bool mazda_aol_param(uint16_t param) { + return param == 0U; +} + +inline constexpr AolSafetyProfile MAZDA_AOL_PROFILE{13U, mazda_aol_param, true}; diff --git a/openpilot/starpilot/car/tesla/aol_policy.h b/openpilot/starpilot/car/tesla/aol_policy.h new file mode 100644 index 0000000000..aa074f6102 --- /dev/null +++ b/openpilot/starpilot/car/tesla/aol_policy.h @@ -0,0 +1,15 @@ +#pragma once + +#include "selfdrive/pandad/aol_protocol.h" + +inline bool tesla_preap_aol_param(uint16_t param) { + return param == 0U; +} + +inline constexpr AolSafetyProfile TESLA_PREAP_AOL_PROFILE{39U, tesla_preap_aol_param, true}; + +inline bool tesla_screen_aol_param(uint16_t param) { + return param == 512U || param == 513U || param == 1536U || param == 1537U; +} + +inline constexpr AolSafetyProfile TESLA_SCREEN_AOL_PROFILE{10U, tesla_screen_aol_param, true}; diff --git a/openpilot/starpilot/car/toyota/aol_policy.h b/openpilot/starpilot/car/toyota/aol_policy.h new file mode 100644 index 0000000000..c8bbd9cacd --- /dev/null +++ b/openpilot/starpilot/car/toyota/aol_policy.h @@ -0,0 +1,9 @@ +#pragma once + +#include "selfdrive/pandad/aol_protocol.h" + +inline bool toyota_aol_param(uint16_t param) { + return (param == 73U) || (param == 585U) || (param == 4169U) || (param == 4681U); +} + +inline constexpr AolSafetyProfile TOYOTA_AOL_PROFILE{2U, toyota_aol_param, true};