Merge Tesla safety firmware with current qualification fixes

2026-10-28 00:53:45 +08:00 · 2026-10-22 22:10:23 -05:00
parent 7610f33129 5acadf1d91
commit 107e99a5fc
47 changed files with 263 additions and 148 deletions
@@ -1,11 +1,11 @@
 #pragma once
 #include "opendbc/safety/declarations.h"
 #include "tesla_preap_pedal.h"
+#include "tesla_preap_radar.h"

 static bool tesla_preap_admitted = false;
 static bool tesla_preap_pedal_long_armed = false;
 static bool tesla_preap_pedal_pull_seen = false;
-static uint32_t tesla_preap_pedal_pull_ts = 0U;
 static bool tesla_preap_doors_open = true;
 static int tesla_preap_gear = 0;
 static int tesla_preap_lever_previous = 2;
@@ -54,21 +54,6 @@ static void tesla_preap_reset(void) {
  tesla_preap_echo_pending = false;
 }

-static uint8_t tesla_preap_crc(const CANPacket_t *msg) {
-  unsigned int crc = 255U;
-  for (int i = 0; i < 7; i++) {
-    crc ^= msg->data[i];
-    for (int bit = 0; bit < 8; bit++) {
-      crc = ((crc & 128U) != 0U) ? ((crc << 1U) ^ 29U) : (crc << 1U);
-      crc &= 255U;
-    }
-  }
-  return (uint8_t)(crc ^ 255U);
-}
-
-
-#include "tesla_preap_radar.h"
-
 static uint32_t tesla_preap_checksum(const CANPacket_t *msg) { return msg->data[7]; }
 static uint32_t tesla_preap_compute_checksum(const CANPacket_t *msg) { return tesla_preap_crc(msg); }

@@ -145,6 +130,8 @@ static void tesla_preap_optional_rx(const CANPacket_t *msg) {
      // real zero/disabled command before any resumed active demand.
      tesla_preap_pedal_tx_seen = false;
      tesla_preap_pedal_tx_enabled = false;
+    } else {
+      // Healthy feedback without driver demand preserves the accepted TX state.
    }
  }

@@ -238,6 +225,7 @@ static void tesla_preap_rx_hook(const CANPacket_t *msg) {
          if (established_counter && (lever == 2) && (tesla_preap_lever_previous == 0) && tesla_preap_sources_ready(!tesla_preap_axis) &&
              (tesla_preap_gear == 4) && !tesla_preap_doors_open && !steering_disengage && (!brake_pressed || tesla_preap_axis)) {
            if (tesla_preap_pedal_configured) {
+              static uint32_t tesla_preap_pedal_pull_ts = 0U;
              const uint32_t now = microsecond_timer_get();
              tesla_preap_pedal_long_armed = tesla_preap_pedal_pull_seen && !brake_pressed &&
                tesla_preap_pedal_feedback_ready(now) &&
@@ -271,10 +259,9 @@ static bool tesla_preap_tx_hook(const CANPacket_t *msg) {
    const bool allowed = tesla_preap_admitted && tesla_preap_pedal_long_armed && tesla_preap_sources_ready(true) &&
      (tesla_preap_gear == 4) && !tesla_preap_doors_open && !brake_pressed && !steering_disengage &&
      !tesla_preap_eps_fault && get_longitudinal_allowed();
-    return tesla_preap_pedal_tx(msg->data, msg->bus, microsecond_timer_get(),
+    accepted = tesla_preap_pedal_tx(msg->data, msg->bus, microsecond_timer_get(),
      (float)vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, allowed);
-  }
-  if (msg->bus == 1U) {
+  } else if (msg->bus == 1U) {
    accepted = tesla_preap_radar_tx(msg);
  } else if (msg->addr == 0x214U) {
    const unsigned int sum = 0x14U + 2U + msg->data[0] + msg->data[1];
@@ -320,7 +307,7 @@ static bool tesla_preap_fwd_hook(int bus, int addr) {

 static safety_config tesla_preap_init(uint16_t param) {
  const bool pedal = ((param == 1U) || (param == 3U) || (param == 7U)) && tesla_preap_pedal_profile_init(param);
-  if ((param != 1U) && (param != 3U) && (param != 7U)) { tesla_preap_pedal_staged_ready = false; }
+  if ((param != 1U) && (param != 3U) && (param != 7U)) { tesla_preap_pedal_startup.staged_ready = false; }
  tesla_preap_admitted = (((param == 0U) || (param == 2U) || (param == 6U)) || pedal) &&
    ((alternative_experience == 0) || (alternative_experience == 32));
  tesla_preap_axis = alternative_experience == 32;
@@ -6,28 +6,30 @@ static bool tesla_preap_pedal_finite(float value) {
  union { float value; uint32_t bits; } word = {.value = value};
  return (word.bits & 0x7F800000U) != 0x7F800000U;
 }
-static float tesla_preap_pedal_abs(float value) { return value < 0.0F ? -value : value; }
+static float tesla_preap_pedal_abs(float value) { return (value < 0.0F) ? -value : value; }

 // Fork-local startup USB envelope. No CAN packet configures these values.
 #define TESLA_PREAP_PEDAL_CONFIG_WORDS 24U
-static uint16_t tesla_preap_pedal_staged[24];
-static uint16_t tesla_preap_pedal_active_words[24];
-static unsigned int tesla_preap_pedal_next_word = 0U;
-static bool tesla_preap_pedal_staged_ready = false;
+// The startup capsule is shared by native mode admission and its USB owner.
+typedef struct {
+  uint16_t staged[24];
+  uint16_t active_words[24];
+  unsigned int next_word;
+  bool staged_ready;
+} TeslaPreapPedalStartupState;
+static TeslaPreapPedalStartupState tesla_preap_pedal_startup = {0};
 static bool tesla_preap_pedal_configured = false;
 static float tesla_preap_pedal_factor = 0.0F;
 static float tesla_preap_pedal_zero = 0.0F;
 static unsigned int tesla_preap_pedal_bus = 0U;
 static bool tesla_preap_pedal_feedback_seen = false;
 static bool tesla_preap_pedal_feedback_healthy = false;
-static uint8_t tesla_preap_pedal_feedback_counter = 0U;
 static uint32_t tesla_preap_pedal_feedback_ts = 0U;
 static float tesla_preap_pedal_feedback_di = 0.0F;
 static unsigned int tesla_preap_pedal_feedback_raw_one = 0U;
 static unsigned int tesla_preap_pedal_feedback_raw_two = 0U;
 static bool tesla_preap_pedal_tx_seen = false;
 static bool tesla_preap_pedal_tx_enabled = false;
-static uint8_t tesla_preap_pedal_tx_counter = 0U;
 static unsigned int tesla_preap_pedal_last_raw_one = 0U;
 static unsigned int tesla_preap_pedal_last_raw_two = 0U;

@@ -45,51 +47,21 @@ static bool tesla_preap_pedal_config_valid(const uint16_t *words) {
  bool valid = (words[0] == 1U) && ((words[1] == 0U) || (words[1] == 2U)) &&
    ((words[2] == 1U) || (words[2] == 3U) || (words[2] == 7U)) && (words[3] == 0xD051U) &&
    tesla_preap_pedal_finite(factor) && (tesla_preap_pedal_abs(factor) > 0.000001F) && tesla_preap_pedal_finite(zero) && hash_present;
-  if (!valid) { return false; }
-  const float demands[2] = {-5.0F, 90.0F};
-  for (unsigned int d = 0U; d < 2U; d++) {
-    const float physical = zero + (demands[d] / factor) + 22.85856576F;
-    const float raw_one = physical / 0.050796813F, raw_two = physical / 0.101593626F;
-    const unsigned int one = raw_one < 0.0F ? 0U : raw_one > 65534.0F ? 65534U : (unsigned int)raw_one;
-    const unsigned int two = raw_two < 0.0F ? 0U : raw_two > 65534.0F ? 65534U : (unsigned int)raw_two;
-    const int difference = (int)one - (2 * (int)two);
-    valid &= difference >= 0 && difference <= 1;
+  if (valid) {
+    const float demands[2] = {-5.0F, 90.0F};
+    for (unsigned int d = 0U; d < 2U; d++) {
+      const float physical = zero + (demands[d] / factor) + 22.85856576F;
+      const float raw_one = physical / 0.050796813F;
+      const float raw_two = physical / 0.101593626F;
+      const unsigned int one = (raw_one < 0.0F) ? 0U : ((raw_one > 65534.0F) ? 65534U : (unsigned int)raw_one);
+      const unsigned int two = (raw_two < 0.0F) ? 0U : ((raw_two > 65534.0F) ? 65534U : (unsigned int)raw_two);
+      const int difference = (int)one - (2 * (int)two);
+      valid &= (difference >= 0) && (difference <= 1);
+    }
  }
  return valid;
 }

-static inline bool tesla_preap_pedal_config_word(uint16_t index, uint16_t value, bool may_configure) {
-  if (!may_configure || tesla_preap_pedal_configured) { return false; }
-  if (index == 0U) {
-    tesla_preap_pedal_next_word = 0U;
-    tesla_preap_pedal_staged_ready = false;
-  }
-  if (index == 0xFFFFU) {
-    tesla_preap_pedal_staged_ready = (value == 0xC051U) &&
-      (tesla_preap_pedal_next_word == TESLA_PREAP_PEDAL_CONFIG_WORDS) &&
-      tesla_preap_pedal_config_valid(tesla_preap_pedal_staged);
-    return tesla_preap_pedal_staged_ready;
-  }
-  if ((index != tesla_preap_pedal_next_word) || (index >= TESLA_PREAP_PEDAL_CONFIG_WORDS)) {
-    tesla_preap_pedal_next_word = 0U;
-    tesla_preap_pedal_staged_ready = false;
-    return false;
-  }
-  tesla_preap_pedal_staged[index] = value;
-  tesla_preap_pedal_next_word++;
-  return true;
-}
-
-static inline unsigned int tesla_preap_pedal_config_read(uint8_t *out) {
-  if (!tesla_preap_pedal_configured && !tesla_preap_pedal_staged_ready) { return 0U; }
-  const uint16_t *words = tesla_preap_pedal_configured ? tesla_preap_pedal_active_words : tesla_preap_pedal_staged;
-  for (unsigned int i = 0U; i < TESLA_PREAP_PEDAL_CONFIG_WORDS; i++) {
-    out[2U * i] = words[i] & 0xFFU;
-    out[(2U * i) + 1U] = words[i] >> 8U;
-  }
-  return TESLA_PREAP_PEDAL_CONFIG_WORDS * 2U;
-}
-
 static void tesla_preap_pedal_mode_reset(uint16_t mode) {
  tesla_preap_pedal_configured = false;
  tesla_preap_pedal_feedback_seen = false;
@@ -99,22 +71,22 @@ static void tesla_preap_pedal_mode_reset(uint16_t mode) {
  tesla_preap_pedal_last_raw_one = 0U;
  tesla_preap_pedal_last_raw_two = 0U;
  if (mode != 39U) {
-    tesla_preap_pedal_staged_ready = false;
-    tesla_preap_pedal_next_word = 0U;
+    tesla_preap_pedal_startup.staged_ready = false;
+    tesla_preap_pedal_startup.next_word = 0U;
  }
 }

 static bool tesla_preap_pedal_profile_init(uint16_t param) {
-  const bool good = tesla_preap_pedal_staged_ready && (tesla_preap_pedal_staged[2] == param) &&
-    tesla_preap_pedal_config_valid(tesla_preap_pedal_staged);
-  tesla_preap_pedal_staged_ready = false;  // a startup capsule is consumed exactly once
+  const bool good = tesla_preap_pedal_startup.staged_ready && (tesla_preap_pedal_startup.staged[2] == param) &&
+    tesla_preap_pedal_config_valid(tesla_preap_pedal_startup.staged);
+  tesla_preap_pedal_startup.staged_ready = false;  // a startup capsule is consumed exactly once
  if (good) {
    for (unsigned int i = 0U; i < TESLA_PREAP_PEDAL_CONFIG_WORDS; i++) {
-      tesla_preap_pedal_active_words[i] = tesla_preap_pedal_staged[i];
+      tesla_preap_pedal_startup.active_words[i] = tesla_preap_pedal_startup.staged[i];
    }
-    tesla_preap_pedal_factor = tesla_preap_pedal_word_float(&tesla_preap_pedal_active_words[4]);
-    tesla_preap_pedal_zero = tesla_preap_pedal_word_float(&tesla_preap_pedal_active_words[6]);
-    tesla_preap_pedal_bus = tesla_preap_pedal_active_words[1];
+    tesla_preap_pedal_factor = tesla_preap_pedal_word_float(&tesla_preap_pedal_startup.active_words[4]);
+    tesla_preap_pedal_zero = tesla_preap_pedal_word_float(&tesla_preap_pedal_startup.active_words[6]);
+    tesla_preap_pedal_bus = tesla_preap_pedal_startup.active_words[1];
  }
  tesla_preap_pedal_configured = good;
  return good;
@@ -125,25 +97,28 @@ static float tesla_preap_pedal_di(unsigned int raw, float scale) {
 }

 static bool tesla_preap_pedal_feedback(const uint8_t *data, unsigned int bus, uint32_t now) {
-  if (!tesla_preap_pedal_configured || (bus != tesla_preap_pedal_bus)) { return false; }
-  const unsigned int one = ((unsigned int)data[0] << 8U) | data[1];
-  const unsigned int two = ((unsigned int)data[2] << 8U) | data[3];
-  const uint8_t counter = data[4] & 15U;
-  const bool monotonic = !tesla_preap_pedal_feedback_seen || ((uint32_t)(now - tesla_preap_pedal_feedback_ts) > 0U);
-  const bool channels = (one < 65535U) && (two < 65535U) &&
-    (tesla_preap_pedal_abs(((float)one * 0.050796813F) - ((float)two * 0.101593626F)) <= (2.0F * 0.101593626F));
-  const bool valid = ((data[4] >> 4U) == 0U) && channels && monotonic;
-  const bool next = tesla_preap_pedal_feedback_seen && valid &&
-    (counter == ((tesla_preap_pedal_feedback_counter + 1U) & 15U));
-  tesla_preap_pedal_feedback_healthy = next;
-  tesla_preap_pedal_feedback_seen = valid;
-  tesla_preap_pedal_feedback_counter = counter;
-  tesla_preap_pedal_feedback_ts = now;
-  const float a = tesla_preap_pedal_di(one, 0.050796813F);
-  const float b = tesla_preap_pedal_di(two, 0.101593626F);
-  tesla_preap_pedal_feedback_raw_one = one;
-  tesla_preap_pedal_feedback_raw_two = two;
-  tesla_preap_pedal_feedback_di = (a > b) ? a : b;
+  bool next = false;
+  if (tesla_preap_pedal_configured && (bus == tesla_preap_pedal_bus)) {
+    static uint8_t tesla_preap_pedal_feedback_counter = 0U;
+    const unsigned int one = ((unsigned int)data[0] << 8U) | data[1];
+    const unsigned int two = ((unsigned int)data[2] << 8U) | data[3];
+    const uint8_t counter = data[4] & 15U;
+    const bool monotonic = !tesla_preap_pedal_feedback_seen || ((uint32_t)(now - tesla_preap_pedal_feedback_ts) > 0U);
+    const bool channels = (one < 65535U) && (two < 65535U) &&
+      (tesla_preap_pedal_abs(((float)one * 0.050796813F) - ((float)two * 0.101593626F)) <= (2.0F * 0.101593626F));
+    const bool valid = ((data[4] >> 4U) == 0U) && channels && monotonic;
+    next = tesla_preap_pedal_feedback_seen && valid &&
+      (counter == ((tesla_preap_pedal_feedback_counter + 1U) & 15U));
+    tesla_preap_pedal_feedback_healthy = next;
+    tesla_preap_pedal_feedback_seen = valid;
+    tesla_preap_pedal_feedback_counter = counter;
+    tesla_preap_pedal_feedback_ts = now;
+    const float a = tesla_preap_pedal_di(one, 0.050796813F);
+    const float b = tesla_preap_pedal_di(two, 0.101593626F);
+    tesla_preap_pedal_feedback_raw_one = one;
+    tesla_preap_pedal_feedback_raw_two = two;
+    tesla_preap_pedal_feedback_di = (a > b) ? a : b;
+  }
  return next;
 }

@@ -171,53 +146,57 @@ static bool tesla_preap_pedal_channel_bounds(unsigned int raw, unsigned int prev
  const float second = (tesla_preap_pedal_zero + (tesla_preap_pedal_max_di(speed) / tesla_preap_pedal_factor) + 22.85856576F) / scale;
  // Match the working Dom codec's0..65534 clamp, including a valid floor
  // saturation. Config admission separately rejects incompatible channel tops.
-  const float bounded_first = first < 0.0F ? 0.0F : first > 65534.0F ? 65534.0F : first;
-  const float bounded_second = second < 0.0F ? 0.0F : second > 65534.0F ? 65534.0F : second;
-  const float low = bounded_first < bounded_second ? bounded_first : bounded_second;
-  const float high = bounded_first > bounded_second ? bounded_first : bounded_second;
+  const float bounded_first = (first < 0.0F) ? 0.0F : ((first > 65534.0F) ? 65534.0F : first);
+  const float bounded_second = (second < 0.0F) ? 0.0F : ((second > 65534.0F) ? 65534.0F : second);
+  const float low = (bounded_first < bounded_second) ? bounded_first : bounded_second;
+  const float high = (bounded_first > bounded_second) ? bounded_first : bounded_second;
  // One ADC count covers Float32-vs-host quotient rounding without a DI-based
  // tolerance growing with calibration factor.
  const unsigned int floor_low = (unsigned int)low;
-  const unsigned int lower = floor_low > 0U ? floor_low - 1U : 0U;
-  const unsigned int upper = (unsigned int)high < 65534U ? (unsigned int)high + 1U : 65534U;
+  const unsigned int lower = (floor_low > 0U) ? (floor_low - 1U) : 0U;
+  const unsigned int upper = ((unsigned int)high < 65534U) ? ((unsigned int)high + 1U) : 65534U;
  const float unit = tesla_preap_pedal_abs(tesla_preap_pedal_factor) * scale;
-  const float up = 5.0F / unit, down = 2.5F / unit;
-  const unsigned int rate_up = up >= 65534.0F ? 65534U : (unsigned int)up + 1U;
-  const unsigned int rate_down = down >= 65534.0F ? 65534U : (unsigned int)down + 1U;
-  const int delta = tesla_preap_pedal_factor > 0.0F ? (int)raw - (int)previous : (int)previous - (int)raw;
-  return raw >= lower && raw <= upper && delta <= (int)rate_up && delta >= -(int)rate_down;
+  const float up = 5.0F / unit;
+  const float down = 2.5F / unit;
+  const unsigned int rate_up = (up >= 65534.0F) ? 65534U : ((unsigned int)up + 1U);
+  const unsigned int rate_down = (down >= 65534.0F) ? 65534U : ((unsigned int)down + 1U);
+  const int delta = (tesla_preap_pedal_factor > 0.0F) ? ((int)raw - (int)previous) : ((int)previous - (int)raw);
+  return (raw >= lower) && (raw <= upper) && (delta <= (int)rate_up) && (delta >= -(int)rate_down);
 }

 static bool tesla_preap_pedal_tx(const uint8_t *data, unsigned int bus, uint32_t now,
                                 float speed, bool longitudinal_allowed) {
-  if (!tesla_preap_pedal_configured || (bus != tesla_preap_pedal_bus)) { return false; }
-  const unsigned int one = ((unsigned int)data[0] << 8U) | data[1];
-  const unsigned int two = ((unsigned int)data[2] << 8U) | data[3];
-  unsigned int sum = 0x51U + 5U;
-  for (unsigned int i = 0U; i < 5U; i++) { sum += data[i]; }
-  const bool enabled = (data[4] & 128U) != 0U;
-  const uint8_t counter = data[4] & 15U;
-  bool good = ((data[4] & 112U) == 0U) && ((sum & 255U) == data[5]) &&
-    (!tesla_preap_pedal_tx_seen || (counter == ((tesla_preap_pedal_tx_counter + 1U) & 15U)));
-  if (!enabled) {
-    // A valid zero/disabled command may resynchronize a dropped TX counter.
-    good = ((data[4] & 112U) == 0U) && ((sum & 255U) == data[5]) && (one == 0U) && (two == 0U);
-  } else {
-    const int difference = (int)one - (2 * (int)two);
-    const unsigned int previous = tesla_preap_pedal_tx_enabled ? tesla_preap_pedal_last_raw_one : tesla_preap_pedal_feedback_raw_one;
-    const unsigned int previous_two = tesla_preap_pedal_tx_enabled ? tesla_preap_pedal_last_raw_two : tesla_preap_pedal_feedback_raw_two;
-    good &= tesla_preap_pedal_tx_seen && longitudinal_allowed && tesla_preap_pedal_feedback_ready(now) &&
-      (tesla_preap_pedal_feedback_di <= 2.0F) && tesla_preap_pedal_finite(speed) && speed >= 0.0F &&
-      difference >= 0 && difference <= 1 && one < 65535U && two < 65535U &&
-      tesla_preap_pedal_channel_bounds(one, previous, 0.050796813F, speed) &&
-      tesla_preap_pedal_channel_bounds(two, previous_two, 0.101593626F, speed);
-    if (good) { tesla_preap_pedal_last_raw_one = one; tesla_preap_pedal_last_raw_two = two; }
+  bool good = false;
+  if (tesla_preap_pedal_configured && (bus == tesla_preap_pedal_bus)) {
+    static uint8_t tesla_preap_pedal_tx_counter = 0U;
+    const unsigned int one = ((unsigned int)data[0] << 8U) | data[1];
+    const unsigned int two = ((unsigned int)data[2] << 8U) | data[3];
+    unsigned int sum = 0x51U + 5U;
+    for (unsigned int i = 0U; i < 5U; i++) { sum += data[i]; }
+    const bool enabled = (data[4] & 128U) != 0U;
+    const uint8_t counter = data[4] & 15U;
+    good = ((data[4] & 112U) == 0U) && ((sum & 255U) == data[5]) &&
+      (!tesla_preap_pedal_tx_seen || (counter == ((tesla_preap_pedal_tx_counter + 1U) & 15U)));
+    if (!enabled) {
+      // A valid zero/disabled command may resynchronize a dropped TX counter.
+      good = ((data[4] & 112U) == 0U) && ((sum & 255U) == data[5]) && (one == 0U) && (two == 0U);
+    } else {
+      const int difference = (int)one - (2 * (int)two);
+      const unsigned int previous = tesla_preap_pedal_tx_enabled ? tesla_preap_pedal_last_raw_one : tesla_preap_pedal_feedback_raw_one;
+      const unsigned int previous_two = tesla_preap_pedal_tx_enabled ? tesla_preap_pedal_last_raw_two : tesla_preap_pedal_feedback_raw_two;
+      good &= tesla_preap_pedal_tx_seen && longitudinal_allowed && tesla_preap_pedal_feedback_ready(now) &&
+        (tesla_preap_pedal_feedback_di <= 2.0F) && tesla_preap_pedal_finite(speed) && (speed >= 0.0F) &&
+        (difference >= 0) && (difference <= 1) && (one < 65535U) && (two < 65535U) &&
+        tesla_preap_pedal_channel_bounds(one, previous, 0.050796813F, speed) &&
+        tesla_preap_pedal_channel_bounds(two, previous_two, 0.101593626F, speed);
+      if (good) { tesla_preap_pedal_last_raw_one = one; tesla_preap_pedal_last_raw_two = two; }
+    }
+    if (good) {
+      tesla_preap_pedal_tx_seen = true;
+      tesla_preap_pedal_tx_counter = counter;
+      tesla_preap_pedal_tx_enabled = enabled;
+    }
+    // Rejected TX does not alter the last accepted rate/counter baseline.
  }
-  if (good) {
-    tesla_preap_pedal_tx_seen = true;
-    tesla_preap_pedal_tx_counter = counter;
-    tesla_preap_pedal_tx_enabled = enabled;
-  }
-  // Rejected TX does not alter the last accepted rate/counter baseline.
  return good;
 }
@@ -0,0 +1,41 @@
+#pragma once
+#include "opendbc/safety/modes/tesla_preap_pedal.h"
+
+// Startup transport API: included by the actual Panda USB owner and native
+// library bridge. The CAN safety translation unit owns the shared state only.
+static inline bool tesla_preap_pedal_config_word(uint16_t index, uint16_t value, bool may_configure) {
+  bool accepted = false;
+  if (may_configure && !tesla_preap_pedal_configured) {
+    if (index == 0U) {
+      tesla_preap_pedal_startup.next_word = 0U;
+      tesla_preap_pedal_startup.staged_ready = false;
+    }
+    if (index == 0xFFFFU) {
+      tesla_preap_pedal_startup.staged_ready = (value == 0xC051U) &&
+        (tesla_preap_pedal_startup.next_word == TESLA_PREAP_PEDAL_CONFIG_WORDS) &&
+        tesla_preap_pedal_config_valid(tesla_preap_pedal_startup.staged);
+      accepted = tesla_preap_pedal_startup.staged_ready;
+    } else if ((index != tesla_preap_pedal_startup.next_word) || (index >= TESLA_PREAP_PEDAL_CONFIG_WORDS)) {
+      tesla_preap_pedal_startup.next_word = 0U;
+      tesla_preap_pedal_startup.staged_ready = false;
+    } else {
+      tesla_preap_pedal_startup.staged[index] = value;
+      tesla_preap_pedal_startup.next_word++;
+      accepted = true;
+    }
+  }
+  return accepted;
+}
+
+static inline unsigned int tesla_preap_pedal_config_read(uint8_t *out) {
+  unsigned int length = 0U;
+  if (tesla_preap_pedal_configured || tesla_preap_pedal_startup.staged_ready) {
+    const uint16_t *words = tesla_preap_pedal_configured ? tesla_preap_pedal_startup.active_words : tesla_preap_pedal_startup.staged;
+    for (unsigned int i = 0U; i < TESLA_PREAP_PEDAL_CONFIG_WORDS; i++) {
+      out[2U * i] = words[i] & 0xFFU;
+      out[(2U * i) + 1U] = words[i] >> 8U;
+    }
+    length = TESLA_PREAP_PEDAL_CONFIG_WORDS * 2U;
+  }
+  return length;
+}
@@ -1,6 +1,18 @@
 #pragma once
 #include "opendbc/safety/can_tx.h"

+static uint8_t tesla_preap_crc(const CANPacket_t *msg) {
+  unsigned int crc = 255U;
+  for (int i = 0; i < 7; i++) {
+    crc ^= msg->data[i];
+    for (int bit = 0; bit < 8; bit++) {
+      crc = ((crc & 128U) != 0U) ? ((crc << 1U) ^ 29U) : (crc << 1U);
+      crc &= 255U;
+    }
+  }
+  return (uint8_t)(crc ^ 255U);
+}
+
 static bool tesla_preap_radar_enabled = false;
 static bool tesla_preap_radar_nosecone = false;
 static const CANPacket_t *tesla_preap_radar_ticket = NULL;
@@ -80,7 +92,7 @@ static void tesla_preap_radar_rx(const CANPacket_t *msg) {
      uint32_t low = tesla_preap_radar_word(msg, 0U);
      uint32_t high = tesla_preap_radar_word(msg, 4U);
      if (((low >> 16U) & 0xFF3FU) == 0xFF3FU) {
-        low = (low & 0x00C0FFFFU) | (0x20U << 16U);
+        low = (low & 0x00C0FFFFU) | 0x00200000U;
        high = (high & 0x00FFFFF0U) | 4U;
        tesla_preap_radar_put(&packet, 0U, low);
        tesla_preap_radar_put(&packet, 4U, high);
@@ -102,9 +114,10 @@ static void tesla_preap_radar_rx(const CANPacket_t *msg) {
      const unsigned int raw_speed = (tesla_preap_radar_word(msg, 0U) >> 16U) & 4095U;
      uint32_t speed = 8191U;
      if (raw_speed != 4095U) {
-        const int mph_x100 = (int)raw_speed * 5 - 2500;
-        const int kph_x100 = mph_x100 * 1609 / 1000;
-        speed = (kph_x100 < 0) ? 0U : ((uint32_t)(kph_x100 / 4) & 8191U);
+        const int mph_x100 = ((int)raw_speed * 5) - 2500;
+        const int kph_x100 = (mph_x100 * 1609) / 1000;
+        const int scaled_speed = kph_x100 / 4;
+        speed = (kph_x100 < 0) ? 0U : ((uint32_t)scaled_speed & 8191U);
      }
      CANPacket_t packet = {0};
      packet.addr = 0x169U;
@@ -556,8 +556,8 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
        continue;
      }
      const bool preap_pedal_ready = ((param == 1U) || (param == 3U) || (param == 7U)) &&
-        tesla_preap_pedal_staged_ready && (tesla_preap_pedal_staged[2] == param) &&
-        tesla_preap_pedal_config_valid(tesla_preap_pedal_staged);
+        tesla_preap_pedal_startup.staged_ready && (tesla_preap_pedal_startup.staged[2] == param) &&
+        tesla_preap_pedal_config_valid(tesla_preap_pedal_startup.staged);
      if ((mode == SAFETY_TESLA_PREAP) && (param != 0U) && (param != 2U) && (param != 6U) && !preap_pedal_ready) {
        current_hooks = &nooutput_hooks;
        current_safety_mode = SAFETY_NOOUTPUT;
@@ -12,6 +12,7 @@ uint32_t microsecond_timer_get(void) {

 #include "opendbc/safety/can.h"
 #include "opendbc/safety/safety.h"
+#include "opendbc/safety/modes/tesla_preap_pedal_transport.h"
 #include "opendbc/safety/ignition.h"

 // Capture checked internally scheduled CAN frames for native safety tests.
@@ -1,14 +1,17 @@
 """Actual registered mode39 bounds; no forced controlsAllowed or native callback bypass."""
+import struct
+import unittest
+
 import pytest
 from opendbc.car.tesla.tests.preap_stream import PreAPStream
 from opendbc.car.tesla.preap.pedal_contract import PedalCalibration, PedalCodec
 from opendbc.safety.tests.libsafety import libsafety_py
-from openpilot.starpilot.car.tesla.tests.pedal_fixture import configure_native, feedback_frame
+from openpilot.starpilot.car.tesla.tests.pedal_fixture import configure_native, feedback_frame, native_words


-def warm(bus, word):
+def warm(bus, word, *, factor=1., zero=0., speed=80.):
  safety = libsafety_py.libsafety
-  calibration = PedalCalibration(1., 0., bus)
+  calibration = PedalCalibration(factor, zero, bus)
  configure_native(safety, calibration, word, b'actual-registered-profile-test')
  stream = PreAPStream()
  priming = PedalCodec(calibration)
@@ -17,7 +20,7 @@ def warm(bus, word):
    now = 1_000_000 + tick * 10000
    safety.set_timer(now)
    safety.set_aol_test_heartbeat(tick >= 65)
-    frames = stream.frames(tick, lever=2 if tick in (65, 75) else 0, cruise=1)
+    frames = stream.frames(tick, lever=2 if tick in (65, 75) else 0, cruise=1, speed=speed)
    if tick % 2 == 0:
      frames.append(feedback_frame(calibration, tick // 2))
    for address, data, source in frames:
@@ -84,3 +87,79 @@ def test_actual_feedback_fault_withdraws_then_needs_fresh_physical_rearm(fault):
  assert not (safety.aol_get_permission_mask() & 2)
  assert not transmit(safety, codec.command(0., enabled=True))
  assert transmit(safety, codec.command(enabled=False))
+
+
+class TestTeslaPreapPedalStartup(unittest.TestCase):
+  def setUp(self):
+    self.safety = libsafety_py.libsafety
+    self.safety.init_tests()
+    self.safety.set_safety_hooks(0, 0)
+    self.safety.set_aol_test_heartbeat(False)
+    self.safety.set_alternative_experience(32)
+
+  def read(self):
+    output = libsafety_py.ffi.new('uint8_t[48]')
+    length = self.safety.tesla_preap_test_config_read(output)
+    return bytes(libsafety_py.ffi.buffer(output, length))
+
+  def stage(self, words):
+    for index, value in enumerate(words):
+      self.assertTrue(self.safety.tesla_preap_test_config_word(index, value))
+    return bool(self.safety.tesla_preap_test_config_word(0xFFFF, 0xC051))
+
+  def test_startup_order_commit_ack_and_consume_once(self):
+    words = native_words(PedalCalibration(1., 0., 0), 1, b'startup-envelope')
+    self.assertEqual(self.read(), b'')
+    self.assertFalse(self.safety.tesla_preap_test_config_word(2, words[2]))
+    self.assertFalse(self.safety.tesla_preap_test_config_word(0xFFFF, 0xC051))
+    self.assertTrue(self.safety.tesla_preap_test_config_word(0, words[0]))
+    self.assertFalse(self.safety.tesla_preap_test_config_word(24, 0))
+    self.assertTrue(self.stage(words))
+    self.assertEqual(self.read(), struct.pack('<24H', *words))
+    self.assertFalse(self.safety.tesla_preap_test_config_word(0xFFFF, 0))
+    self.assertEqual(self.read(), b'')
+    self.assertTrue(self.stage(words))
+    self.assertNotEqual(self.safety.set_safety_hooks(39, 3), 0)
+    self.assertEqual(self.safety.get_current_safety_mode(), 19)  # rejected profile falls back to NoOutput
+    self.assertFalse(transmit(self.safety, PedalCodec(PedalCalibration(1., 0., 0)).command(enabled=False)))
+    self.assertEqual(self.read(), struct.pack('<24H', *words), 'a rejected profile cannot consume the matching capsule')
+    self.assertEqual(self.safety.set_safety_hooks(39, 1), 0)
+    self.assertTrue(transmit(self.safety, PedalCodec(PedalCalibration(1., 0., 0)).command(enabled=False)))
+    self.assertEqual(self.read(), struct.pack('<24H', *words))
+    self.assertFalse(self.safety.tesla_preap_test_config_word(0, 1))
+    self.safety.set_safety_hooks(39, 1)
+    self.assertFalse(transmit(self.safety, PedalCodec(PedalCalibration(1., 0., 0)).command(enabled=False)),
+                     'startup capsule is consumed once')
+    self.safety.set_safety_hooks(0, 0)
+    self.assertEqual(self.read(), b'')
+
+  def test_invalid_wire_calibration_never_commits(self):
+    valid = native_words(PedalCalibration(1., 0., 2), 7, b'admission-negatives')
+    cases = []
+    for index, value in ((0, 2), (1, 1), (2, 0), (3, 0)):
+      words = list(valid)
+      words[index] = value
+      cases.append(words)
+    for factor, zero in ((0., 0.), (1e-8, 0.), (float('nan'), 0.),
+                         (float('inf'), 0.), (1., float('nan')), (1., float('inf')), (1., 1e6)):
+      cases.append(native_words(PedalCalibration(factor, zero, 2), 7, b'admission-negatives'))
+    cases.append([*valid[:8], *([0] * 16)])
+    for words in cases:
+      with self.subTest(words=words[:8]):
+        self.assertFalse(self.stage(words))
+        self.assertEqual(self.read(), b'')
+
+  def test_negative_calibration_and_gas_override_preserve_native_guards(self):
+    safety, calibration, now = warm(2, 7, factor=-1., zero=100., speed=180.)
+    codec = PedalCodec(calibration)
+    self.assertTrue(transmit(safety, codec.command(enabled=False)))
+    self.assertTrue(transmit(safety, codec.command(4., enabled=True)))
+    unsafe = PedalCodec(calibration)
+    unsafe.counter = codec.counter
+    self.assertFalse(transmit(safety, unsafe.command(12., enabled=True)), 'rate limit applies to negative calibration')
+    safety.set_timer(now + 20_000)
+    address, data, bus = feedback_frame(calibration, 46, di=8.)
+    self.assertTrue(safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data)))
+    self.assertFalse(safety.aol_get_permission_mask() & 2)
+    self.assertFalse(transmit(safety, codec.command(4., enabled=True)))
+    self.assertTrue(transmit(safety, codec.command(enabled=False)))
@@ -153,3 +153,17 @@ class TestTeslaPreapRadar(unittest.TestCase):
    self.safety.safety_rx_hook(self.source(0x145))
    self.assertEqual(self.safety.get_recorded_can_count(), 1)
    self.assertFalse(self.safety.safety_tx_hook(self.recorded()[0]))
+
+  def test_invalid_steering_status_is_rewritten_with_literal_crc(self):
+    for parameter in (2, 6):
+      with self.subTest(parameter=parameter):
+        self.setUp()
+        self.safety.set_safety_hooks(39, parameter)
+        data = bytearray.fromhex('cdabffff78563400')
+        data[7] = stock_fixture.TestTeslaPreapStock.crc(data[:7])
+        self.safety.safety_rx_hook(make_msg(0, 0xE, dat=bytes(data)))
+        packets = self.recorded()
+        self.assertEqual(len(packets), 1)
+        self.assertEqual((packets[0][0].addr, packets[0][0].bus), (0x199, 1))
+        self.assertEqual(bytes(packets[0][0].data[0:8]), bytes.fromhex('cdabe0007456343d'))
+        self.assertFalse(self.safety.safety_tx_hook(packets[0]), 'scheduled ticket cannot be replayed by host')
@@ -1,4 +1,5 @@
 #pragma once
+#include "opendbc/safety/modes/tesla_preap_pedal_transport.h"

 static bool panda_ignition_line(void) {
  #ifdef PANDA_IGNORE_IGNITION_LINE
@@ -1,4 +1,4 @@
 #pragma once

 extern const uint8_t gitversion[29];
-const uint8_t gitversion[29] = "DEV-d429cc3c-Pa5919516-DEBUG";
+const uint8_t gitversion[29] = "DEV-33ca04f8-Pa5919516-DEBUG";
@@ -1 +1 @@
-DEV-d429cc3c-Pa5919516-DEBUG
+DEV-33ca04f8-Pa5919516-DEBUG
@@ -1 +1 @@
-{"protected_inventory_sha256": "4e3eefcc8319778d0158f10e442920761f14272888ed5d9bcae27b1ff3a3058d", "schema_version": 1}
+{"protected_inventory_sha256": "c8ef858fca1f6a622548736174a42cf9784fef1785e2ca1701523ba20e245ab4", "schema_version": 1}