the morrow

2026-09-08 09:03:42 +08:00 · 2026-08-27 11:57:40 -05:00
parent 032f085500
commit 3c655e79a9
59 changed files with 1963 additions and 178 deletions
@@ -665,6 +665,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
    {"SwitchbackModeEnabled", {CLEAR_ON_OFFROAD_TRANSITION, BOOL, "0", "0"}},
    {"SubaruSNG", {PERSISTENT, BOOL, "1", "0", 2, SETTINGS_SIMPLE}},
    {"SubaruSNGManualParkingBrake", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
+    {"SubaruStopStartOff", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
    {"TacoTune", {PERSISTENT, BOOL, "0", "0", 2}},
    {"TeslaCoopSteering", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
    {"TestAlert", {CLEAR_ON_MANAGER_START, STRING, "", ""}},
@@ -180,6 +180,13 @@ def should_use_ev6_gt_line_stop_direct_tracking(ev6_gt_line: bool, stopping: boo
  return bool(ev6_gt_line and stopping and v_ego > EV6_GT_LINE_STOP_BRAKE_CAP_MAX_SPEED and accel_cmd < actual_accel)


+def apply_carnival_steering_override(car_fingerprint, steering_pressed: bool,
+                                     apply_steer_req: bool, apply_torque: int) -> tuple[bool, int]:
+  if car_fingerprint == CAR.KIA_CARNIVAL_2025 and steering_pressed:
+    return False, 0
+  return apply_steer_req, apply_torque
+
+
 def update_ev9_longitudinal_tuning(state: EV9LongitudinalTuningState, enabled: bool,
                                   stopping: bool, v_ego: float) -> EV9LongitudinalTuningState:
  if not enabled:
@@ -613,6 +620,10 @@ class CarController(CarControllerBase):
      if not CC.latActive:
        apply_torque = 0

+      apply_steer_req, apply_torque = apply_carnival_steering_override(
+        self.CP.carFingerprint, CS.out.steeringPressed, apply_steer_req, apply_torque,
+      )
+
      # Hold torque with induced temporary fault when cutting the actuation bit
      # FIXME: we don't use this with CAN FD?
      torque_fault = CC.latActive and not apply_steer_req
@@ -20,6 +20,7 @@ from opendbc.car.hyundai.carcontroller import CarController, Ioniq6LongitudinalT
                                             should_track_stop_accel_directly_for_car, \
                                             preserve_stock_canfd_lfa_status, \
                                             preserve_stock_canfd_lkas_status, \
+                                             apply_carnival_steering_override, \
                                             suppress_redundant_gv70_brake_cancel
 from opendbc.car.hyundai.carstate import CarState, decode_canfd_camera_lead, decode_ioniq_6_blindspot_radar_state, \
                                             get_canfd_cruise_available
@@ -208,6 +209,12 @@ class TestHyundaiFingerprint:
    assert parser.vl["LKAS_ALT"]["TORQUE_REQUEST"] == 123
    assert parser.vl["LKAS_ALT"]["STEER_REQ"] == 1

+  def test_carnival_steering_override_is_scoped_to_2025_platform(self):
+    assert apply_carnival_steering_override(CAR.KIA_CARNIVAL_2025, True, True, 123) == (False, 0)
+    assert apply_carnival_steering_override(CAR.KIA_CARNIVAL_2025, False, True, 123) == (True, 123)
+    assert apply_carnival_steering_override(CAR.KIA_CARNIVAL_HEV_4TH_GEN, True, True, 123) == (True, 123)
+    assert apply_carnival_steering_override(CAR.HYUNDAI_IONIQ_6, True, True, 123) == (True, 123)
+
  def test_canfd_torque_bsm_parser_registers_rear_blindspots(self):
    CP = CarParams.new_message()
    CP.carFingerprint = CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN
@@ -30,6 +30,10 @@ _ANGLE_RECLAIM_FRAMES = 36
 _ANGLE_RECLAIM_EXPONENT = 2.5
 _ANGLE_MADS_MIN_SPEED = 0.44704
 _ANGLE_MADS_MAX_STEER_ANGLE = 120.0
+_STOP_START_STARTUP_DELAY_FRAMES = 100
+_STOP_START_STARTUP_DEADLINE_FRAMES = 300
+_STOP_START_PULSE_FRAMES = 30
+_STOP_START_PULSE_PERIOD_FRAMES = 5


 def get_safety_CP():
@@ -73,6 +77,66 @@ class CarController(CarControllerBase):
    self.prev_close_distance = 0
    self.epb_resume_frames_remaining = -1
    self.last_standstill_frame = 0
+    self.stop_start_attempted = False
+    self.stop_start_request_started = False
+    self.stop_start_request_frame = 0
+    self.stop_start_initial_state = None
+    self.stop_start_counter = 0
+    self.stop_start_acknowledged = False
+
+  def _stop_start_off_request(self, CC, CS, starpilot_toggles):
+    """Send one bounded Outback Stop/Start OFF request after ignition.
+
+    This is intentionally opt-in and limited to a stationary vehicle in
+    Park/Neutral. A single ignition session gets at most one attempt.
+    """
+    if self.CP.carFingerprint != CAR.SUBARU_OUTBACK_2023 or \
+       not getattr(starpilot_toggles, "subaru_stop_start_off", False) or self.stop_start_attempted:
+      return None
+
+    if self.frame > _STOP_START_STARTUP_DEADLINE_FRAMES or getattr(CC, "enabled", False):
+      self.stop_start_attempted = True
+      return None
+
+    if self.frame < _STOP_START_STARTUP_DELAY_FRAMES or not getattr(getattr(CS, "out", None), "canValid", True):
+      return None
+
+    out = CS.out
+    if not getattr(out, "standstill", False) or out.gearShifter not in (
+      structs.CarState.GearShifter.park,
+      structs.CarState.GearShifter.neutral,
+    ):
+      return None
+
+    dashlights_msg = getattr(CS, "dashlights_msg", None)
+    if not dashlights_msg:
+      return None
+
+    if not self.stop_start_request_started:
+      self.stop_start_request_started = True
+      self.stop_start_request_frame = self.frame
+      self.stop_start_initial_state = getattr(CS, "stop_start_state", None)
+      self.stop_start_counter = (int(dashlights_msg.get("COUNTER", 0)) + 1) % 0x10
+
+    current_state = getattr(CS, "stop_start_state", None)
+    if self.stop_start_initial_state is not None and self.stop_start_initial_state != 3 and current_state == 3:
+      self.stop_start_attempted = True
+      self.stop_start_acknowledged = True
+      return None
+
+    elapsed = self.frame - self.stop_start_request_frame
+    if elapsed >= _STOP_START_PULSE_FRAMES:
+      self.stop_start_attempted = True
+      return None
+
+    if elapsed % _STOP_START_PULSE_PERIOD_FRAMES != 0:
+      return None
+
+    msg = subarucan.create_stop_start_control(
+      self.packer, dashlights_msg, counter=self.stop_start_counter, bus=self.main_bus,
+    )
+    self.stop_start_counter = (self.stop_start_counter + 1) % 0x10
+    return msg

  def _reset_legacy_2025_handoff(self):
    self.legacy_2025_handoff_active = False
@@ -87,7 +151,7 @@ class CarController(CarControllerBase):
      self._reset_legacy_2025_handoff()
      return False

-    if CS.out.steeringPressed:
+    if getattr(CS.out, "steeringPressed", False):
      self.legacy_2025_handoff_active = True
      self.legacy_2025_override_hold_frames = _LEGACY_2025_OVERRIDE_HOLD_FRAMES
      self.legacy_2025_reengage_settle_frames = 0
@@ -96,7 +160,7 @@ class CarController(CarControllerBase):
      return True

    if not self.legacy_2025_handoff_active and not self.legacy_2025_lkas_active and \
-       abs(CS.out.steeringRateDeg) > _LEGACY_2025_REENGAGE_MAX_STEER_RATE:
+       abs(getattr(CS.out, "steeringRateDeg", 0.0)) > _LEGACY_2025_REENGAGE_MAX_STEER_RATE:
      self.legacy_2025_handoff_active = True
      self.legacy_2025_reengage_reference_angle = CS.out.steeringAngleDeg

@@ -109,7 +173,7 @@ class CarController(CarControllerBase):
        self.legacy_2025_reengage_reference_angle = CS.out.steeringAngleDeg
      return True

-    wheel_stable = abs(CS.out.steeringRateDeg) <= _LEGACY_2025_REENGAGE_MAX_STEER_RATE and \
+    wheel_stable = abs(getattr(CS.out, "steeringRateDeg", 0.0)) <= _LEGACY_2025_REENGAGE_MAX_STEER_RATE and \
      abs(CS.out.steeringAngleDeg - self.legacy_2025_reengage_reference_angle) <= _LEGACY_2025_REENGAGE_MAX_ANGLE_DELTA
    if wheel_stable:
      self.legacy_2025_reengage_settle_frames += 1
@@ -150,7 +214,7 @@ class CarController(CarControllerBase):
      self._reset_angle_handoff()
      return False

-    if CS.out.steeringPressed:
+    if getattr(CS.out, "steeringPressed", False):
      self.angle_handoff_active = True
      self.angle_override_hold_frames = _ANGLE_OVERRIDE_HOLD_FRAMES
      self.angle_reengage_settle_frames = 0
@@ -159,7 +223,7 @@ class CarController(CarControllerBase):
      return True

    if not self.angle_handoff_active and not self.angle_lkas_active and \
-       abs(CS.out.steeringRateDeg) > _ANGLE_REENGAGE_MAX_STEER_RATE:
+       abs(getattr(CS.out, "steeringRateDeg", 0.0)) > _ANGLE_REENGAGE_MAX_STEER_RATE:
      self.angle_handoff_active = True
      self.angle_reengage_reference_angle = CS.out.steeringAngleDeg

@@ -172,7 +236,7 @@ class CarController(CarControllerBase):
        self.angle_reengage_reference_angle = CS.out.steeringAngleDeg
      return True

-    wheel_stable = abs(CS.out.steeringRateDeg) <= _ANGLE_REENGAGE_MAX_STEER_RATE and \
+    wheel_stable = abs(getattr(CS.out, "steeringRateDeg", 0.0)) <= _ANGLE_REENGAGE_MAX_STEER_RATE and \
      abs(CS.out.steeringAngleDeg - self.angle_reengage_reference_angle) <= _ANGLE_REENGAGE_MAX_ANGLE_DELTA
    if wheel_stable:
      self.angle_reengage_settle_frames += 1
@@ -267,9 +331,19 @@ class CarController(CarControllerBase):
    elif abs_torque < self.p.STEER_OVERRIDE_TORQUE_LOW:
      self.driver_override = False

-    lat_active = CC.latActive and not self.driver_override
+    mads_only = CC.latActive and not getattr(CC, "enabled", False)
+    mads_only_ok = CS.out.vEgoRaw > _ANGLE_MADS_MIN_SPEED and \
+      abs(CS.out.steeringAngleDeg) < _ANGLE_MADS_MAX_STEER_ANGLE
+    lkas_available = CC.latActive and (not mads_only or mads_only_ok) and \
+      getattr(CS.out, "gearShifter", structs.CarState.GearShifter.drive) == structs.CarState.GearShifter.drive and \
+      not getattr(CS.out, "standstill", False)
+    manual_handoff = self._angle_manual_handoff(CS, lkas_available)
+    lat_active = lkas_available and not self.driver_override and not manual_handoff
+    if lat_active and not self.angle_lkas_active:
+      self.apply_steer_last = CS.out.steeringAngleDeg
+    steer_target = self._angle_reclaim_target(CC.actuators.steeringAngleDeg) if lat_active else CC.actuators.steeringAngleDeg
    apply_steer = apply_steer_angle_limits_vm(
-      CC.actuators.steeringAngleDeg,
+      steer_target,
      self.apply_steer_last,
      CS.out.vEgoRaw,
      CS.out.steeringAngleDeg,
@@ -282,6 +356,7 @@ class CarController(CarControllerBase):
      apply_steer = CS.out.steeringAngleDeg

    self.apply_steer_last = apply_steer
+    self.angle_lkas_active = lat_active
    return subarucan.create_steering_control_angle(self.packer, apply_steer, lat_active, self.angle_bus)

  def lateral_torque(self, CC, CS):
@@ -316,6 +391,10 @@ class CarController(CarControllerBase):

    can_sends = []

+    stop_start_msg = self._stop_start_off_request(CC, CS, starpilot_toggles)
+    if stop_start_msg is not None:
+      can_sends.append(stop_start_msg)
+
    # *** steering ***
    if (self.frame % self.p.STEER_STEP) == 0:
      if self.CP.flags & SubaruFlags.LKAS_ANGLE:
@@ -4,7 +4,7 @@ from opendbc.can import CANDefine, CANParser
 from opendbc.car import Bus, structs
 from opendbc.car.common.conversions import Conversions as CV
 from opendbc.car.interfaces import CarStateBase
-from opendbc.car.subaru.values import DBC, CanBus, SubaruFlags
+from opendbc.car.subaru.values import CAR, DBC, CanBus, SubaruFlags
 from opendbc.car import CanSignalRateCalculator


@@ -15,6 +15,8 @@ class CarState(CarStateBase):
    self.shifter_values = can_define.dv["Transmission"]["Gear"]

    self.angle_rate_calulator = CanSignalRateCalculator(50)
+    self.dashlights_msg = {}
+    self.stop_start_state = 0

  def update(self, can_parsers, starpilot_toggles) -> structs.CarState:
    cp = can_parsers[Bus.pt]
@@ -24,6 +26,10 @@ class CarState(CarStateBase):
    cp_angle = cp_main if self.CP.flags & SubaruFlags.D_PLATFORM else cp
    ret = structs.CarState()

+    if self.CP.carFingerprint == CAR.SUBARU_OUTBACK_2023:
+      self.dashlights_msg = copy.copy(cp.vl["Dashlights"])
+      self.stop_start_state = cp.vl["Engine_Stop_Start"]["STOP_START_STATE"]
+
    throttle_msg = cp.vl["Throttle"] if not (self.CP.flags & SubaruFlags.HYBRID) else cp_alt.vl["Throttle_Hybrid"]
    ret.gasPressed = throttle_msg["Throttle_Pedal"] > 1e-5
    if self.CP.flags & SubaruFlags.PREGLOBAL:
@@ -40,6 +40,8 @@ class CarInterface(CarInterfaceBase):
        ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM.value
      if ret.flags & SubaruFlags.D_PLATFORM_CAMERA:
        ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM_CAMERA.value
+      if candidate == CAR.SUBARU_OUTBACK_2023:
+        ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.STOP_START_BUTTON.value
      if candidate in (CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023):
        ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.FIXED_ANGLE_LIMITS.value

@@ -182,6 +182,20 @@ def create_es_dashstatus(packer, frame, dashstatus_msg, enabled, long_enabled, l
  return packer.make_can_msg("ES_DashStatus", bus, values)


+def create_stop_start_control(packer, dashlights_msg, counter=None, bus=CanBus.alt):
+  """Create the Outback 2023-24 momentary Stop/Start button request.
+
+  Dashlights is a stock periodic message, so preserve the live frame and only
+  change the event bit. CANPacker calculates the Subaru checksum for us.
+  """
+  values = dict(dashlights_msg)
+  if counter is None:
+    counter = (int(values.get("COUNTER", 0)) + 1) % 0x10
+  values["COUNTER"] = counter % 0x10
+  values["STOP_START"] = 1
+  return packer.make_can_msg("Dashlights", bus, values)
+
+
 def create_es_brake(packer, frame, es_brake_msg, long_enabled, long_active, brake_value, bus=CanBus.main):
  values = {s: es_brake_msg[s] for s in [
    "CHECKSUM",
@@ -193,6 +193,7 @@ def test_outback_2023_uses_d_platform_bus_layout():

  assert CP.flags & SubaruFlags.D_PLATFORM
  assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM
+  assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.STOP_START_BUTTON
  assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.LEGACY_2025_ANGLE_LIMITS)
  assert CanBus.main_for_cp(CP) == CanBus.alt
  assert CanBus.angle_for_cp(CP) == CanBus.main
@@ -205,6 +206,52 @@ def test_outback_2023_uses_d_platform_bus_layout():
  assert CP.lateralSmoothSeconds == pytest.approx(0.4)


+def test_stop_start_request_is_bounded_and_uses_live_dashlights():
+  CP = CarInterface.get_non_essential_params(CAR.SUBARU_OUTBACK_2023)
+  controller = CarController({}, CP)
+  controller.frame = 101
+
+  class TestActuators:
+    steeringAngleDeg = 0.0
+
+    def as_builder(self):
+      return SimpleNamespace(steeringAngleDeg=self.steeringAngleDeg)
+
+  CC = SimpleNamespace(
+    enabled=False,
+    latActive=False,
+    longActive=False,
+    actuators=TestActuators(),
+    hudControl=SimpleNamespace(leadVisible=False),
+    cruiseControl=SimpleNamespace(cancel=False),
+  )
+  CS = SimpleNamespace(
+    canValid=True,
+    dashlights_msg={"COUNTER": 6, "STOP_START": 0},
+    stop_start_state=0,
+    out=SimpleNamespace(
+      standstill=True,
+      gearShifter=structs.CarState.GearShifter.park,
+    ),
+  )
+  toggles = SimpleNamespace(subaru_stop_start_off=True, subaru_sng=False)
+
+  _, can_sends = controller.update(CC, CS, 0, toggles)
+  stop_start_msgs = [msg for msg in can_sends if msg[0] == 0x390]
+  assert len(stop_start_msgs) == 1
+  assert stop_start_msgs[0][2] == CanBus.alt
+  parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("Dashlights", 0)], CanBus.alt)
+  parser.update([(1, [stop_start_msgs[0]])])
+  assert parser.vl["Dashlights"]["STOP_START"] == 1
+  assert parser.vl["Dashlights"]["COUNTER"] == 7
+
+  controller.frame = 103
+  CS.stop_start_state = 3
+  _, can_sends = controller.update(CC, CS, 0, toggles)
+  assert not any(msg[0] == 0x390 for msg in can_sends)
+  assert controller.stop_start_acknowledged
+
+
 def test_legacy_2025_uses_gen2_angle_bus_layout():
  CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
  parsers = CarState.get_can_parsers(CP)
@@ -448,6 +495,44 @@ def test_angle_controller_tracks_driver_override():
  assert not controller.driver_override


+def test_angle_controller_blocks_low_speed_mads_engagement():
+  CP = CarInterface.get_non_essential_params(CAR.SUBARU_CROSSTREK_2025)
+  controller = CarController({}, CP)
+  CC = SimpleNamespace(
+    enabled=False,
+    latActive=True,
+    actuators=SimpleNamespace(steeringAngleDeg=15.0),
+  )
+  CS = SimpleNamespace(out=SimpleNamespace(
+    vEgoRaw=0.3,
+    steeringAngleDeg=80.0,
+    steeringTorque=0.0,
+    gearShifter=structs.CarState.GearShifter.drive,
+    standstill=False,
+  ))
+  parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main)
+
+  msg = controller.lateral_angle(CC, CS)
+  parser.update([(1, [msg])])
+
+  assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
+  assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
+
+  CS.out.vEgoRaw = 1.0
+  CS.out.steeringAngleDeg = 130.0
+  msg = controller.lateral_angle(CC, CS)
+  parser.update([(2, [msg])])
+
+  assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
+  assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
+
+  CS.out.steeringAngleDeg = 0.0
+  msg = controller.lateral_angle(CC, CS)
+  parser.update([(3, [msg])])
+
+  assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
+
+
 def test_ascent_angle_controller_uses_fixed_angle_rate_limits():
  CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
  controller = CarController({}, CP)
@@ -88,6 +88,7 @@ class SubaruSafetyFlags(IntFlag):
  D_PLATFORM = 32
  D_PLATFORM_CAMERA = 64
  FIXED_ANGLE_LIMITS = 128
+  STOP_START_BUTTON = 256
  LEGACY_2025_ANGLE_LIMITS = FIXED_ANGLE_LIMITS


@@ -42,6 +42,7 @@ TOYOTA_NO_LEAD_CRUISE_SIGN_FLIP_MIN_SET_SPEED_ERROR = 0.35  # m/s
 # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
 MAX_STEER_RATE = 100  # deg/s
 MAX_STEER_RATE_FRAMES = 18  # tx control frames needed before torque can be cut
+TOYOTA_HIGHLANDER_TSS2_MAX_STEER_RATE_FRAMES = 8

 # EPS allows user torque above threshold for 50 frames before permanently faulting
 MAX_USER_TORQUE = 500
@@ -72,6 +73,11 @@ def should_bypass_toyota_long_pid(CP, starpilot_toggles=None) -> bool:
  ) or highlander_sdsu)


+def get_steer_rate_limit_frames(car_fingerprint) -> int:
+  return (TOYOTA_HIGHLANDER_TSS2_MAX_STEER_RATE_FRAMES
+          if car_fingerprint == CAR.TOYOTA_HIGHLANDER_TSS2 else MAX_STEER_RATE_FRAMES)
+
+
 def get_long_tune(CP, params):
  kiBP = [2., 5.]
  kiV = [0.5, 0.25]
@@ -223,6 +229,7 @@ class CarController(CarControllerBase):
    self.standstill_req = False
    self.permit_braking = True
    self.steer_rate_counter = 0
+    self.steer_rate_limit_frames = get_steer_rate_limit_frames(self.CP.carFingerprint)
    self.distance_button = 0

    # *** start long control state ***
@@ -345,8 +352,10 @@ class CarController(CarControllerBase):
    apply_torque = apply_meas_steer_torque_limits(new_torque, self.last_torque, CS.out.steeringTorqueEps, self.params)

    # >100 degree/sec steering fault prevention
-    self.steer_rate_counter, apply_steer_req = common_fault_avoidance(abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE, lat_active,
-                                                                      self.steer_rate_counter, MAX_STEER_RATE_FRAMES)
+    self.steer_rate_counter, apply_steer_req = common_fault_avoidance(
+      abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE, lat_active,
+      self.steer_rate_counter, self.steer_rate_limit_frames,
+    )

    if not lat_active:
      apply_torque = 0
@@ -10,6 +10,7 @@ from opendbc.car.fw_versions import build_fw_dict, match_fw_to_car
 from opendbc.car.toyota import toyotacan
 from opendbc.car.toyota.carcontroller import CarController, get_camry_hybrid_feedforward, get_long_tune, get_prius_feedforward, \
                                             get_prius_positive_feedforward_scale, \
+                                             get_steer_rate_limit_frames, \
                                             limit_interceptor_pcm_accel, \
                                             limit_interceptor_stopping_accel, limit_no_lead_cruise_sign_flip, \
                                             limit_prius_stopping_accel, should_bypass_toyota_long_pid, update_permit_braking
@@ -706,6 +707,10 @@ class TestToyotaFingerprint:


 class TestToyotaCarController:
+  def test_highlander_tss2_uses_early_steer_rate_fault_guard(self):
+    assert get_steer_rate_limit_frames(CAR.TOYOTA_HIGHLANDER_TSS2) == 8
+    assert get_steer_rate_limit_frames(CAR.TOYOTA_RAV4_TSS2) == 18
+
  @staticmethod
  def _make_controller(*, standstill_req=False, last_standstill=False):
    controller = CarController.__new__(CarController)
@@ -41,6 +41,7 @@
 #define MSG_SUBARU_ES_HighBeamAssist     0x121U
 #define MSG_SUBARU_ES_STATIC_1           0x22aU
 #define MSG_SUBARU_ES_STATIC_2           0x325U
+#define MSG_SUBARU_Dashlights            0x390U

 #define SUBARU_MAIN_BUS 0U
 #define SUBARU_ALT_BUS  1U
@@ -61,6 +62,9 @@
  {MSG_SUBARU_ES_LKAS_State,   bus, 8, .check_relay = true}, \
  {MSG_SUBARU_ES_Infotainment, bus, 8, .check_relay = true}, \

+#define SUBARU_STOP_START_TX_MSGS(bus) \
+  {MSG_SUBARU_Dashlights, bus, 8, .check_relay = false}, \
+
 #define SUBARU_COMMON_LONG_TX_MSGS(alt_bus) \
  {MSG_SUBARU_ES_Distance,       alt_bus,         8, .check_relay = true}, \
  {MSG_SUBARU_ES_Brake,          alt_bus,         8, .check_relay = true}, \
@@ -108,6 +112,7 @@ static bool subaru_stop_and_go = false;
 static bool subaru_lkas_angle = false;
 static bool subaru_d_platform = false;
 static bool subaru_fixed_angle_limits = false;
+static bool subaru_stop_start_button = false;

 static uint32_t subaru_get_checksum(const CANPacket_t *msg) {
  return (uint8_t)msg->data[0];
@@ -285,6 +290,13 @@ static bool subaru_tx_hook(const CANPacket_t *msg) {
    violation |= !(is_tester_present || is_button_rdbi);
  }

+  if (msg->addr == MSG_SUBARU_Dashlights) {
+    violation |= !subaru_stop_start_button;
+    violation |= msg->bus != SUBARU_ALT_BUS;
+    violation |= !GET_BIT(msg, 54U);
+    violation |= subaru_get_checksum(msg) != subaru_compute_checksum(msg);
+  }
+
  if (violation){
    tx = false;
  }
@@ -334,6 +346,12 @@ static safety_config subaru_init(uint16_t param) {
    SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
  };

+  static const CanMsg SUBARU_D_PLATFORM_ANGLE_STOP_START_MAIN_TX_MSGS[] = {
+    SUBARU_D_PLATFORM_ANGLE_TX_MSGS(SUBARU_MAIN_BUS)
+    SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
+    SUBARU_STOP_START_TX_MSGS(SUBARU_ALT_BUS)
+  };
+
  static const CanMsg SUBARU_D_PLATFORM_ANGLE_CAMERA_TX_MSGS[] = {
    SUBARU_D_PLATFORM_ANGLE_TX_MSGS(SUBARU_CAM_BUS)
    SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
@@ -378,6 +396,9 @@ static safety_config subaru_init(uint16_t param) {
  const uint16_t SUBARU_PARAM_FIXED_ANGLE_LIMITS = 128;
  subaru_fixed_angle_limits = GET_FLAG(param, SUBARU_PARAM_FIXED_ANGLE_LIMITS);

+  const uint16_t SUBARU_PARAM_STOP_START_BUTTON = 256;
+  subaru_stop_start_button = GET_FLAG(param, SUBARU_PARAM_STOP_START_BUTTON);
+
 #ifdef ALLOW_DEBUG
  const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
  subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL);
@@ -385,8 +406,9 @@ static safety_config subaru_init(uint16_t param) {

  safety_config ret;
  if (subaru_lkas_angle) {
-    ret = subaru_d_platform ? (subaru_d_platform_camera ? BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_CAMERA_TX_MSGS) : \
-                              BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_MAIN_TX_MSGS)) : \
+    ret = subaru_d_platform ? (subaru_stop_start_button ? BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_STOP_START_MAIN_TX_MSGS) : \
+                              (subaru_d_platform_camera ? BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_CAMERA_TX_MSGS) : \
+                              BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_MAIN_TX_MSGS))) : \
          subaru_gen2 ? BUILD_SAFETY_CFG(subaru_gen2_lkas_angle_rx_checks, SUBARU_GEN2_LKAS_ANGLE_TX_MSGS) : \
                        BUILD_SAFETY_CFG(subaru_lkas_angle_rx_checks, SUBARU_LKAS_ANGLE_TX_MSGS);
  } else if (subaru_gen2) {
@@ -1075,13 +1075,14 @@ class SafetyTest(SafetyTestBase):
              continue
            if attr.startswith('TestSubaruGen') and current_test.startswith('TestSubaruGen'):
              continue
-            if 'TestSubaruDPlatformAngleSafety' in {attr, current_test} and \
-                'Angle' in attr and 'Angle' in current_test:
+            if attr.startswith('TestSubaruDPlatform') and current_test.startswith('TestSubaruDPlatform'):
              continue
-            if 'TestSubaruDPlatformAngleSafety' in {attr, current_test}:
+            if attr.startswith('TestSubaruDPlatform'):
              # D-platform uses the same main-bus HUD messages as the other
              # Subaru modes, so those modes cannot be distinguished by ID.
-              tx = list(filter(lambda m: not (m[1] == 0 and m[0] in [0x321, 0x322, 0x323]), tx))
+              tx = list(filter(lambda m: not (m[1] == 0 and m[0] in [0x124, 0x321, 0x322, 0x323]), tx))
+            if current_test.startswith('TestSubaruDPlatform') and attr.startswith('TestSubaruGen'):
+              tx = list(filter(lambda m: not (m[1] == 0 and m[0] in [0x124, 0x321, 0x322, 0x323]), tx))
            if attr.startswith('TestSubaruPreglobal') and current_test.startswith('TestSubaruPreglobal'):
              continue
            if {attr, current_test}.issubset({'TestVolkswagenPqSafety', 'TestVolkswagenPqStockSafety', 'TestVolkswagenPqLongSafety'}):
@@ -36,6 +36,7 @@ class SubaruMsg(enum.IntEnum):
  ES_HighBeamAssist = 0x121
  ES_STATIC_1       = 0x22a
  ES_STATIC_2       = 0x325
+  Dashlights        = 0x390


 SUBARU_MAIN_BUS = 0
@@ -401,6 +402,21 @@ class TestSubaruDPlatformAngleSafety(TestSubaruStockLongitudinalSafetyBase, Test
    return self.packer.make_can_msg_safety("Steering_2", SUBARU_MAIN_BUS, {"Steering_Angle": angle})


+class TestSubaruDPlatformStopStartSafety(TestSubaruDPlatformAngleSafety):
+  FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.D_PLATFORM | \
+    SubaruSafetyFlags.STOP_START_BUTTON
+  TX_MSGS = TestSubaruDPlatformAngleSafety.TX_MSGS + [[SubaruMsg.Dashlights, SUBARU_ALT_BUS]]
+
+  def _stop_start_msg(self, pressed):
+    return self.packer.make_can_msg_safety(
+      "Dashlights", SUBARU_ALT_BUS, {"COUNTER": 0, "STOP_START": pressed},
+    )
+
+  def test_stop_start_tx_requires_pressed_bit(self):
+    self.assertTrue(self._tx(self._stop_start_msg(True)))
+    self.assertFalse(self._tx(self._stop_start_msg(False)))
+
+
 class TestSubaruDPlatformCameraAngleSafety(TestSubaruDPlatformAngleSafety):
  FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.D_PLATFORM | SubaruSafetyFlags.D_PLATFORM_CAMERA
  TX_MSGS = [[SubaruMsg.ES_LKAS_ANGLE, SUBARU_CAM_BUS],
@@ -1,2 +1,2 @@
 extern const uint8_t gitversion[19];
-const uint8_t gitversion[19] = "DEV-f8d47139-DEBUG";
+const uint8_t gitversion[19] = "DEV-5882bd23-DEBUG";
@@ -1 +1 @@
-DEV-f8d47139-DEBUG
+DEV-5882bd23-DEBUG
@@ -16,6 +16,8 @@ REPO_ROOT = Path(__file__).resolve().parents[1]
 if str(REPO_ROOT) not in sys.path:
  sys.path.insert(0, str(REPO_ROOT))

+from openpilot.starpilot.common.model_versions import UNIFIED_ARTIFACT_FORMAT
+
 DEFAULT_INPUT_ROOT = Path("/data/openpilot/uncompiledmodels")
 DEFAULT_OUTPUT_ROOT = Path("/data/openpilot/compiledmodels")
 COMPILE_SCRIPT = REPO_ROOT / "tinygrad_repo/examples/openpilot/compile3.py"
@@ -389,15 +391,33 @@ def multipart_output_paths(artifact: Path, output_dir: Path | None = None) -> li
  ]


-def install_local_artifact(artifact: Path, model_key: str, version: str) -> None:
+def _update_local_artifact_metadata(model_key: str, external_gpu: bool) -> None:
+  metadata_path = MODELS_PATH / ".model_artifacts.json"
+  try:
+    payload = json.loads(metadata_path.read_text()) if metadata_path.is_file() else {}
+    if not isinstance(payload, dict):
+      payload = {}
+    metadata = payload.get(model_key)
+    if not isinstance(metadata, dict):
+      metadata = {}
+    metadata["artifact_format"] = UNIFIED_ARTIFACT_FORMAT
+    metadata["uses_external_gpu"] = bool(external_gpu)
+    payload[model_key] = metadata
+    metadata_path.write_text(json.dumps(payload))
+  except Exception as error:
+    print(f"  WARN: could not update {metadata_path.name}: {error}")
+
+
+def install_local_artifact(artifact: Path, model_key: str, version: str, external_gpu: bool = False) -> None:
  """Copy a freshly compiled local- model into the runtime dir modeld loads from,
-  and ensure its <id>.json sidecar carries the correct version.
+  and ensure its <id>.json sidecar and artifact cache carry the correct metadata.

  The sidecar version is NOT cosmetic: without it _discover_local_models() records
  an empty version, which downstream parses on the wrong contract (a v15 model then
  drives like v11). Since we know the build version here, we write it so the local
-  install is correct by default. Local models must be a single is_file() in
-  /data/models to show in the picker. No-ops off-device (no /data/models).
+  install is correct by default. The GPU flag is equally important: it selects
+  the out-of-band loader and AMD queue at runtime. Local models must be a single
+  is_file() in /data/models to show in the picker. No-ops off-device.
  """
  if not MODELS_PATH.is_dir():
    print(f"  skipped auto-install: {MODELS_PATH} not present (not on device?)")
@@ -415,20 +435,19 @@ def install_local_artifact(artifact: Path, model_key: str, version: str) -> None
        info = loaded
    except Exception as error:
      print(f"  WARN: existing sidecar {sidecar.name} is malformed, rewriting: {error}")
-  if not version:
-    if not str(info.get("version") or "").strip():
-      print(f"  WARN: could not determine version -- set it by hand in {sidecar.name} "
-            "or the model may drive on the wrong version contract")
-    return
-  # keep any user-set name/series; only guarantee a correct, non-empty version
-  if str(info.get("version") or "").strip() == version and sidecar.is_file():
-    print(f"  sidecar ok: {sidecar.name} (version {version})")
-    return
-  info.setdefault("name", model_key[len("local-"):].replace("_", " ").replace("-", " ").strip())
-  info.setdefault("series", "Local")
-  info["version"] = version
-  sidecar.write_text(json.dumps(info, indent=2) + "\n")
-  print(f"  wrote sidecar {sidecar.name} (version {version})")
+  if version:
+    info.setdefault("name", model_key[len("local-"):].replace("_", " ").replace("-", " ").strip())
+    info.setdefault("series", "Local")
+    info["version"] = version
+  elif not str(info.get("version") or "").strip():
+    print(f"  WARN: could not determine version -- set it by hand in {sidecar.name} "
+          "or the model may drive on the wrong version contract")
+
+  info["uses_external_gpu"] = bool(external_gpu)
+  if version or sidecar.is_file() or external_gpu:
+    sidecar.write_text(json.dumps(info, indent=2) + "\n")
+    print(f"  wrote sidecar {sidecar.name} (gpu={bool(external_gpu)})")
+  _update_local_artifact_metadata(model_key, external_gpu)


 def split_oversized_artifact(
@@ -665,7 +684,7 @@ def main() -> int:
        print(f"  --no-split: kept one {size_mb:.1f} MB file; over 100 MB, so split it "
              "before committing to a repo (re-run without --no-split, or --split-artifact)")
    if is_local and not args.no_install:
-      install_local_artifact(output, model_key, version)
+      install_local_artifact(output, model_key, version, args.external_gpu)
  else:
    multipart_outputs = split_oversized_artifact(output)
    if multipart_outputs:
@@ -85,7 +85,8 @@ class Car:
  def __init__(self, CI=None, RI=None) -> None:
    self.can_sock = messaging.sub_sock('can', timeout=20)
    self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents', 'radarState', 'longitudinalPlan'])
-    self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks', 'gpsLocationExternal'])
+    self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'])
+    self.gps_pm = None

    self.can_rcv_cum_timeout_counter = 0
    self._last_car_gps_timestamp_nanos = 0
@@ -138,7 +139,10 @@ class Car:
      self.CI, self.CP, self.FPCP = CI, CI.CP, CI.FPCP
      self.RI = RI

-    self.params.put_bool("CarGpsAvailable", bool(getattr(self.CI.CS, 'car_gps_supported', False)))
+    car_gps_supported = bool(getattr(self.CI.CS, 'car_gps_supported', False))
+    self.params.put_bool("CarGpsAvailable", car_gps_supported)
+    if car_gps_supported:
+      self.gps_pm = messaging.PubMaster(['gpsLocationExternal'])

    interface_alternative_experience = self.CP.alternativeExperience
    self.CP.alternativeExperience = interface_alternative_experience
@@ -364,7 +368,8 @@ class Car:
      gps.speedAccuracy = car_gps['speedAccuracy']
      gps.hasFix = car_gps['hasFix']
      gps.satelliteCount = car_gps['satelliteCount']
-      self.pm.send('gpsLocationExternal', gps_send)
+      assert self.gps_pm is not None
+      self.gps_pm.send('gpsLocationExternal', gps_send)
      self._last_car_gps_publish_monotonic = now

    # carParams - logged every 50 seconds (> 1 per segment)
@@ -14,6 +14,7 @@ _MAX_OFFSET = 0.3
 _MIN_CENTER_TO_LINE = 1.1
 _MAX_RAW_CORRECTION = 0.004
 _MAX_GAIN = 0.30
+_VISUAL_CORRECTION_EPSILON = 1e-6
 _SMOOTH_TAU = 0.4
 _SIGNAL_RELEASE_TAU = 0.20
 _CONFIDENCE_RELEASE_TAU = 0.20
@@ -183,8 +184,9 @@ def get_lane_centering_visual_direction(model_v2, v_ego: float, offset: float, e
  )
  if not valid or not np.isfinite(correction):
    return 0
-  if correction == 0.0:
-    if applied_correction is None or not np.isfinite(applied_correction) or applied_correction == 0.0:
-      return 0
-    correction = applied_correction
+  if applied_correction is not None and np.isfinite(applied_correction) and \
+      abs(applied_correction) > _VISUAL_CORRECTION_EPSILON:
+    correction = float(applied_correction)
+  if abs(correction) <= _VISUAL_CORRECTION_EPSILON:
+    return 0
  return 1 if correction > 0.0 else -1
@@ -6,6 +6,8 @@ from openpilot.selfdrive.controls.lib.latcontrol import LatControl

 # TODO This is speed dependent
 STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
+FORD_ANGLE_CATCH_UP_HORIZON = 1.25  # Seconds
+FORD_ANGLE_RATE_FILTER_TIME_CONSTANT = 0.15  # Seconds

 _ASCENT_ANGLE_TRACKING_GAIN = 0.25
 _ASCENT_ANGLE_TRACKING_MAX_CORRECTION = 8.0
@@ -23,12 +25,32 @@ def _ascent_angle_tracking_target(target_angle: float, steering_angle: float,
  return target_angle + correction


+def _ford_angle_tracking_saturated(angle_error: float, steering_rate: float) -> bool:
+  """Only call a Ford angle request saturated when the EPS is not on track to catch it."""
+  catching_up = angle_error * steering_rate > 0.0
+  catching_up &= abs(angle_error) <= abs(steering_rate) * FORD_ANGLE_CATCH_UP_HORIZON
+  return abs(angle_error) > STEER_ANGLE_SATURATION_THRESHOLD and not catching_up
+
+
 class LatControlAngle(LatControl):
  def __init__(self, CP, CI, dt):
    super().__init__(CP, CI, dt)
    self.sat_check_min_speed = 5.
    self.use_steer_limited_by_safety = CP.brand in ("tesla", "hyundai")
    self.is_ascent = CP.carFingerprint == SUBARU_CAR.SUBARU_ASCENT_2023
+    self.is_ford = CP.brand == "ford"
+    self.measured_angle_last = None
+    self.measured_angle_rate = 0.0
+
+  def _update_measured_angle_rate(self, steering_angle: float, reset: bool) -> float:
+    if reset or self.measured_angle_last is None:
+      self.measured_angle_rate = 0.0
+    else:
+      raw_rate = (steering_angle - self.measured_angle_last) / self.dt
+      alpha = self.dt / (FORD_ANGLE_RATE_FILTER_TIME_CONSTANT + self.dt)
+      self.measured_angle_rate += alpha * (raw_rate - self.measured_angle_rate)
+    self.measured_angle_last = steering_angle
+    return self.measured_angle_rate

  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, calibrated_pose, model_data, starpilot_toggles):
    angle_log = log.ControlsState.LateralAngleState.new_message()
@@ -49,13 +71,21 @@ class LatControlAngle(LatControl):
          bool(getattr(CS, "steeringPressed", False)),
        )

+    ford_angle_mode = self.is_ford and getattr(starpilot_toggles, "ford_lateral_mode", -1) == 2
+    measured_angle_rate = self._update_measured_angle_rate(
+      float(CS.steeringAngleDeg), not active or bool(CS.steeringPressed) or not ford_angle_mode)
+
    if self.use_steer_limited_by_safety:
      # these cars' carcontrollers calculate max lateral accel and jerk, so we can rely on carOutput for saturation
      angle_control_saturated = steer_limited_by_safety
    else:
      # for cars which use a method of limiting torque such as a torque signal (Nissan and Toyota)
      # or relying on EPS (Ford Q3), carOutput does not capture maxing out torque  # TODO: this can be improved
-      angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
+      angle_error = angle_steers_des - CS.steeringAngleDeg
+      if ford_angle_mode:
+        angle_control_saturated = _ford_angle_tracking_saturated(angle_error, measured_angle_rate)
+      else:
+        angle_control_saturated = abs(angle_error) > STEER_ANGLE_SATURATION_THRESHOLD
    angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited))
    angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
    angle_log.steeringAngleDesiredDeg = angle_steers_des
@@ -108,6 +108,7 @@ class LatControlTorque(LatControl):
    self.is_genesis_gv70 = CP.carFingerprint in GENESIS_GV70_CARS
    self.is_palisade = CP.carFingerprint in PALISADE_CARS
    self.is_prius = CP.carFingerprint in PRIUS_CARS
+    self.is_standard_prius = CP.carFingerprint == TOYOTA_CAR.TOYOTA_PRIUS
    self.is_camry = CP.carFingerprint in CAMRY_CARS
    self.is_rav4_tss2 = CP.carFingerprint in RAV4_TSS2_CARS
    self.is_rav4_prime = CP.carFingerprint in RAV4_PRIME_CARS
@@ -610,7 +611,9 @@ class LatControlTorque(LatControl):
        output_torque *= get_toyota_corolla_tss2_center_output_scale(setpoint, CS.vEgo)
      elif prius_active:
        output_torque *= prius_center_taper
-        output_torque *= get_prius_high_speed_output_taper_scale(setpoint, CS.vEgo)
+        prius_taper_max = (PRIUS_STANDARD_HIGH_SPEED_OUTPUT_TAPER_MAX if self.is_standard_prius
+                           else PRIUS_HIGH_SPEED_OUTPUT_TAPER_MAX)
+        output_torque *= get_prius_high_speed_output_taper_scale(setpoint, CS.vEgo, prius_taper_max)
      elif volt_standard_test_active:
        output_torque *= volt_standard_center_taper
      elif volt_plexy_test_active:
@@ -247,7 +247,7 @@ GENESIS_G70_FRICTION_JERK_DEADZONE_LAT = 0.30
 GENESIS_G70_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.08
 GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED = 12.0
 GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 3.5
-GENESIS_G70_CENTER_OUTPUT_TAPER_MAX = 0.14
+GENESIS_G70_CENTER_OUTPUT_TAPER_MAX = 0.16
 GENESIS_G70_CENTER_OUTPUT_TAPER_LAT = 0.30
 GENESIS_G70_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.10
 GENESIS_G70_CENTER_OUTPUT_TAPER_SPEED = 18.0
@@ -277,7 +277,7 @@ GENESIS_G70_CURVE_UNWIND_LAT = 0.25
 GENESIS_G70_CURVE_UNWIND_LAT_WIDTH = 0.12
 GENESIS_G70_CURVE_UNWIND_JERK = 0.08
 GENESIS_G70_CURVE_UNWIND_JERK_WIDTH = 0.08
-GENESIS_G70_UNWIND_FF_REDUCTION_MAX = 0.28
+GENESIS_G70_UNWIND_FF_REDUCTION_MAX = 0.32
 GENESIS_G70_UNWIND_FF_OVERSHOOT = 0.12
 GENESIS_G70_UNWIND_FF_OVERSHOOT_WIDTH = 0.12
 GENESIS_G70_UNWIND_FF_JERK = 0.10
@@ -1035,6 +1035,7 @@ PRIUS_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.07
 PRIUS_FRICTION_JERK_DEADZONE_SPEED = 18.0
 PRIUS_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 2.2
 PRIUS_HIGH_SPEED_OUTPUT_TAPER_MAX = 0.06
+PRIUS_STANDARD_HIGH_SPEED_OUTPUT_TAPER_MAX = 0.10
 PRIUS_HIGH_SPEED_OUTPUT_TAPER_LAT = 0.30
 PRIUS_HIGH_SPEED_OUTPUT_TAPER_LAT_WIDTH = 0.35
 PRIUS_HIGH_SPEED_OUTPUT_TAPER_SPEED = 22.0
@@ -1493,12 +1494,13 @@ def get_prius_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: float)
  return PRIUS_FRICTION_JERK_DEADZONE_MAX * speed_weight * center_weight


-def get_prius_high_speed_output_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
+def get_prius_high_speed_output_taper_scale(desired_lateral_accel: float, v_ego: float,
+                                            taper_max: float = PRIUS_HIGH_SPEED_OUTPUT_TAPER_MAX) -> float:
  speed_weight = _prius_sigmoid((v_ego - PRIUS_HIGH_SPEED_OUTPUT_TAPER_SPEED) /
                                PRIUS_HIGH_SPEED_OUTPUT_TAPER_SPEED_WIDTH)
  curve_weight = _prius_sigmoid((abs(desired_lateral_accel) - PRIUS_HIGH_SPEED_OUTPUT_TAPER_LAT) /
                                 PRIUS_HIGH_SPEED_OUTPUT_TAPER_LAT_WIDTH)
-  return 1.0 - PRIUS_HIGH_SPEED_OUTPUT_TAPER_MAX * speed_weight * curve_weight
+  return 1.0 - taper_max * speed_weight * curve_weight


 def get_camry_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0,
@@ -113,7 +113,9 @@ def is_radarless_matched_follow_window(v_ego: float, lead_distance: float, v_lea


 def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_gap: float, closing_speed: float,
-                                  v_cruise: float | None = None, y_rel: float | None = None) -> float:
+                                  v_cruise: float | None = None, y_rel: float | None = None,
+                                  min_headway_margin: float = TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN,
+                                  full_headway_margin: float = TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN) -> float:
  gap_error = lead_distance - desired_gap
  actual_hw = lead_distance / max(v_ego, 1e-3)
  desired_hw = desired_gap / max(v_ego, 1e-3)
@@ -137,8 +139,8 @@ def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_ga
  fade_end_margin = max(TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_END_MARGIN,
                        TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_END - desired_hw)
  entry_factor = _smoothstep(headway_margin,
-                             TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN,
-                             TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN)
+                             min_headway_margin,
+                             full_headway_margin)
  exit_factor = 1.0 - _smoothstep(headway_margin, fade_start_margin, fade_end_margin)

  closing_fade_end = max(2.5, 0.12 * v_ego)
@@ -924,7 +924,7 @@ class LongitudinalMpc:
             personality=log.LongitudinalPersonality.standard, tracking_lead=True,
             optional_far_lead_comfort=True, smooth_duplicate_vision=False,
             stop_x=None, silverado_early_follow=False, modelV2=None,
-             lead_obstacle_bias=(0.0, 0.0)):
+             lead_obstacle_bias=(0.0, 0.0), tracked_lead_catchup_headway_margins=None):
    v_ego = self.x0[1]
    lead_one = radarstate.leadOne
    lead_two = radarstate.leadTwo
@@ -972,6 +972,12 @@ class LongitudinalMpc:
      if optional_far_lead_comfort and tracking_lead and lead_one.status:
        desired_gap = desired_follow_distance(v_ego, lead_one.vLead, t_follow)
        closing_speed = max(0.0, v_ego - lead_one.vLead)
+        catchup_kwargs = {}
+        if tracked_lead_catchup_headway_margins is not None:
+          catchup_kwargs = {
+            "min_headway_margin": tracked_lead_catchup_headway_margins[0],
+            "full_headway_margin": tracked_lead_catchup_headway_margins[1],
+          }
        cruise_obstacle += get_tracked_lead_catchup_bias(
          v_ego,
          lead_one.dRel,
@@ -979,6 +985,7 @@ class LongitudinalMpc:
          closing_speed,
          v_cruise=v_cruise,
          y_rel=float(getattr(lead_one, "yRel", 0.0)),
+          **catchup_kwargs,
        )
      if optional_far_lead_comfort:
        cruise_obstacle += self.get_identical_radar_duplicate_cruise_bias(lead_one, lead_two, v_ego, t_follow)
@@ -37,8 +37,11 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
  get_honda_crv_5g_stopped_lead_obstacle_bias,
  get_honda_crv_5g_low_speed_stopped_lead_cap,
  allow_honda_crv_5g_vision_gap_settle,
+  get_honda_crv_5g_early_radar_follow_cap,
  get_standstill_gap_settle_max_extra_gap,
  get_standstill_stopped_lead_guard_distance_margin,
+  get_standstill_stopped_lead_guard_max_lead_speed,
+  get_tracked_lead_catchup_headway_margins,
 )
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
@@ -560,6 +563,7 @@ class LongitudinalPlanner:
    self.output_should_stop = False
    self.far_follow_brake_slew_rate, self.far_follow_release_slew_rate = get_far_follow_output_slew_rates(CP)
    self.untracked_slow_lead_decel_scale = get_untracked_slow_lead_decel_scale(CP)
+    self.tracked_lead_catchup_headway_margins = get_tracked_lead_catchup_headway_margins(CP)
    self.far_follow_output_slew_active = False
    self.model_launch_armed = False
    self.model_launch_stop_seen = False
@@ -1638,13 +1642,16 @@ class LongitudinalPlanner:

    lead_speed = max(float(getattr(lead, "vLead", 0.0)), 0.0)
    lead_delta = lead_speed - float(v_ego)
+    max_lead_speed = get_standstill_stopped_lead_guard_max_lead_speed(
+      self.CP, STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_SPEED,
+    )
    max_distance = max(
      STANDSTILL_STOPPED_LEAD_GUARD_MIN_DISTANCE,
      float(stop_distance) + get_standstill_stopped_lead_guard_distance_margin(self.CP),
    )
    if (
      float(getattr(lead, "dRel", float("inf"))) > max_distance or
-      lead_speed > STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_SPEED or
+      lead_speed > max_lead_speed or
      lead_delta > STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_DELTA
    ):
      return None
@@ -1652,7 +1659,7 @@ class LongitudinalPlanner:
    distance_factor = float(np.clip((max_distance - float(lead.dRel)) /
                                    max(max_distance - STANDSTILL_STOPPED_LEAD_GUARD_MIN_DISTANCE, 0.1),
                                    0.0, 1.0))
-    speed_factor = float(np.clip(lead_speed / max(STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_SPEED, 0.1), 0.0, 1.0))
+    speed_factor = float(np.clip(lead_speed / max(max_lead_speed, 0.1), 0.0, 1.0))
    delta_factor = float(np.clip((STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_DELTA - lead_delta) /
                                 max(STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_DELTA, 0.1),
                                 0.0, 1.0))
@@ -2261,7 +2268,8 @@ class LongitudinalPlanner:
                    stop_x=force_stop_x,
                    silverado_early_follow=early_truck_follow,
                    modelV2=sm['modelV2'],
-                    lead_obstacle_bias=stopped_lead_obstacle_bias)
+                    lead_obstacle_bias=stopped_lead_obstacle_bias,
+                    tracked_lead_catchup_headway_margins=self.tracked_lead_catchup_headway_margins)

    self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
    self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
@@ -2351,6 +2359,22 @@ class LongitudinalPlanner:
      if approach_lift_caps:
        raw_approach_lift_cap = min(approach_lift_caps)

+      if not experimental_mode:
+        early_radar_caps = [
+          cap for cap in (
+            get_honda_crv_5g_early_radar_follow_cap(
+              self.CP, self.lead_one, v_ego, vision_cap_accel_min,
+            ),
+            get_honda_crv_5g_early_radar_follow_cap(
+              self.CP, self.lead_two, v_ego, vision_cap_accel_min,
+            ),
+          ) if cap is not None
+        ]
+        if early_radar_caps:
+          early_radar_cap = min(early_radar_caps)
+          self.a_desired = min(self.a_desired, early_radar_cap)
+          output_a_target = min(output_a_target, early_radar_cap)
+
      pretracking_vision_caps = []
      for lead in (self.lead_one, self.lead_two):
        if lead.status and not bool(getattr(lead, "radar", False)):
@@ -14,6 +14,10 @@ GM_SILVERADO_EARLY_FOLLOW_MAX_LATERAL_OFFSET = 1.2
 DEFAULT_FOLLOW_PREBRAKE_MIN_HEADWAY = 1.25
 GM_SILVERADO_FOLLOW_PREBRAKE_MIN_HEADWAY = 1.25
 FORD_LIGHTNING_FOLLOW_PREBRAKE_MIN_HEADWAY = 1.0
+FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_MIN_HEADWAY_MARGIN = 0.20
+FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_FULL_HEADWAY_MARGIN = 0.45
+FORD_LIGHTNING_STANDSTILL_GUARD_DISTANCE_MARGIN = 5.0
+FORD_LIGHTNING_STANDSTILL_GUARD_MAX_LEAD_SPEED = 0.60
 TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_EGO_SPEED = 2.0
 TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_LEAD_SPEED = 0.45
 TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_LEAD_DELTA = 0.35
@@ -67,6 +71,14 @@ HONDA_CRV_5G_LOW_SPEED_STOP_MAX_DECEL = 0.45
 HONDA_CRV_5G_LOW_SPEED_STOP_MIN_DECEL = 0.12
 HONDA_CRV_5G_GAP_SETTLE_MAX_EXTRA_GAP = 7.0
 HONDA_CRV_5G_GUARD_DISTANCE_MARGIN = 1.5
+HONDA_CRV_5G_RADAR_EARLY_FOLLOW_MIN_EGO_SPEED = 8.0
+HONDA_CRV_5G_RADAR_EARLY_FOLLOW_MAX_EGO_SPEED = 22.0
+HONDA_CRV_5G_RADAR_EARLY_FOLLOW_MAX_LEAD_SPEED = 8.0
+HONDA_CRV_5G_RADAR_EARLY_FOLLOW_MIN_CLOSING_SPEED = 7.0
+HONDA_CRV_5G_RADAR_EARLY_FOLLOW_MAX_TTC = 8.0
+HONDA_CRV_5G_RADAR_EARLY_FOLLOW_MAX_DISTANCE_TIME = 5.0
+HONDA_CRV_5G_RADAR_EARLY_FOLLOW_MIN_MODEL_PROB = 0.80
+HONDA_CRV_5G_RADAR_EARLY_FOLLOW_MAX_LATERAL_OFFSET = 1.0
 TOYOTA_CAMRY_TSS2_FORCE_STOP_HANDOFF_M = 4.5
 # The Camry's force-stop path otherwise consumes the model endpoint before the
 # normal MPC stop-distance margin can be applied. Keep it within the forward
@@ -113,6 +125,50 @@ def is_honda_crv_5g(CP):
  )


+def is_honda_crv_5g_early_radar_follow_lead(CP, lead, v_ego):
+  """Admit a credible, rapidly closing CR-V radar lead before model tracking catches up."""
+  if (
+    not is_honda_crv_5g(CP) or
+    lead is None or not bool(getattr(lead, "status", False)) or
+    not bool(getattr(lead, "radar", False)) or
+    not HONDA_CRV_5G_RADAR_EARLY_FOLLOW_MIN_EGO_SPEED <= float(v_ego) <= \
+      HONDA_CRV_5G_RADAR_EARLY_FOLLOW_MAX_EGO_SPEED or
+    float(getattr(lead, "modelProb", 0.0)) < HONDA_CRV_5G_RADAR_EARLY_FOLLOW_MIN_MODEL_PROB or
+    abs(float(getattr(lead, "yRel", 0.0))) > HONDA_CRV_5G_RADAR_EARLY_FOLLOW_MAX_LATERAL_OFFSET
+  ):
+    return False
+
+  lead_speed = max(float(getattr(lead, "vLead", 0.0)), 0.0)
+  closing_speed = float(v_ego) - lead_speed
+  distance = float(getattr(lead, "dRel", float("inf")))
+  if (
+    lead_speed > HONDA_CRV_5G_RADAR_EARLY_FOLLOW_MAX_LEAD_SPEED or
+    closing_speed < HONDA_CRV_5G_RADAR_EARLY_FOLLOW_MIN_CLOSING_SPEED or
+    distance <= 0.0 or
+    distance > HONDA_CRV_5G_RADAR_EARLY_FOLLOW_MAX_DISTANCE_TIME * float(v_ego)
+  ):
+    return False
+
+  return distance / max(closing_speed, 0.1) <= HONDA_CRV_5G_RADAR_EARLY_FOLLOW_MAX_TTC
+
+
+def get_honda_crv_5g_early_radar_follow_cap(CP, lead, v_ego, accel_min):
+  """Start a mild CR-V radar response before the normal lead handoff."""
+  if not is_honda_crv_5g_early_radar_follow_lead(CP, lead, v_ego):
+    return None
+
+  lead_speed = max(float(getattr(lead, "vLead", 0.0)), 0.0)
+  closing_speed = float(v_ego) - lead_speed
+  distance = float(getattr(lead, "dRel", float("inf")))
+  ttc = distance / max(closing_speed, 0.1)
+  urgency = float(np.clip(
+    (HONDA_CRV_5G_RADAR_EARLY_FOLLOW_MAX_TTC - ttc) / 4.0,
+    0.0, 1.0,
+  ))
+  cap = 0.25 + 0.25 * urgency
+  return max(float(accel_min), -cap)
+
+
 def get_honda_crv_5g_stopped_lead_obstacle_bias(CP, lead, v_ego):
  """Bring the CR-V's vision stopped-lead target in without changing stops."""
  if (
@@ -189,9 +245,33 @@ def get_standstill_gap_settle_max_extra_gap(CP):
 def get_standstill_stopped_lead_guard_distance_margin(CP):
  if is_honda_crv_5g(CP):
    return HONDA_CRV_5G_GUARD_DISTANCE_MARGIN
+  if is_ford_f150_lightning(CP):
+    return FORD_LIGHTNING_STANDSTILL_GUARD_DISTANCE_MARGIN
  return 3.0


+def get_standstill_stopped_lead_guard_max_lead_speed(CP, default):
+  if is_ford_f150_lightning(CP):
+    return FORD_LIGHTNING_STANDSTILL_GUARD_MAX_LEAD_SPEED
+  return float(default)
+
+
+def get_tracked_lead_catchup_headway_margins(CP):
+  if is_ford_f150_lightning(CP):
+    return (
+      FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_MIN_HEADWAY_MARGIN,
+      FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_FULL_HEADWAY_MARGIN,
+    )
+  return None
+
+
+def is_ford_f150_lightning(CP):
+  return (
+    getattr(CP, "brand", "") == "ford" and
+    str(getattr(CP, "carFingerprint", "")) == "FORD_F_150_LIGHTNING_MK1"
+  )
+
+
 def is_toyota_rav4_tss2_post_departure_tune(CP):
  """Identify RAV4 TSS2 variants that need normal catch-up caps after departure."""
  return (
@@ -191,3 +191,8 @@ def test_visual_direction_requires_both_primary_lane_lines():
 def test_visual_direction_uses_filtered_correction_in_deadband():
  model = _model()
  assert get_lane_centering_visual_direction(model, _V_EGO, 0.0, 0.0, True, True, applied_correction=0.001) == 1
+
+
+def test_visual_direction_follows_applied_correction():
+  model = _model(left=-1.5, right=2.1)
+  assert get_lane_centering_visual_direction(model, _V_EGO, 0.0, 0.0, True, True, applied_correction=-0.001) == -1
@@ -1,3 +1,4 @@
+import math
 import pytest
 from parameterized import parameterized
 from types import SimpleNamespace
@@ -17,7 +18,11 @@ from opendbc.car.hyundai.values import CAR as HYUNDAI
 from opendbc.car.subaru.values import CAR as SUBARU
 from opendbc.car.vehicle_model import VehicleModel
 from openpilot.common.realtime import DT_CTRL
-from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, _ascent_angle_tracking_target
+from openpilot.selfdrive.controls.lib.latcontrol_angle import (
+  LatControlAngle,
+  _ascent_angle_tracking_target,
+  _ford_angle_tracking_saturated,
+)
 from openpilot.selfdrive.controls.lib.latcontrol_pid import (
  LatControlPID,
  get_civic_bosch_modified_pid_output_alpha,
@@ -193,6 +198,41 @@ class TestLatControl:
    assert _ascent_angle_tracking_target(10.0, 0.0, 4.0, False) == pytest.approx(10.0)
    assert _ascent_angle_tracking_target(10.0, 0.0, 20.0, True) == pytest.approx(10.0)

+  def test_ford_angle_tracking_does_not_report_a_responsive_eps_as_saturated(self):
+    assert not _ford_angle_tracking_saturated(12.0, 12.0)
+    assert not _ford_angle_tracking_saturated(-12.0, -12.0)
+    assert _ford_angle_tracking_saturated(16.0, 12.0)
+    assert _ford_angle_tracking_saturated(12.0, -12.0)
+
+  def test_ford_angle_tracking_still_reports_a_stalled_eps(self):
+    assert _ford_angle_tracking_saturated(3.0, 0.0)
+    assert not _ford_angle_tracking_saturated(2.5, 0.0)
+
+  def test_ford_angle_handoff_saturation_waits_for_eps_response(self):
+    CP = SimpleNamespace(
+      steerLimitTimer=1.0,
+      brand="ford",
+      carFingerprint="FORD_MUSTANG_MACH_E_MK1",
+    )
+    controller = LatControlAngle(CP, None, DT_CTRL)
+    target = [12.0]
+    VM = SimpleNamespace(get_steer_from_curvature=lambda *_args: math.radians(target[0]))
+    CS = car.CarState.new_message(vEgo=10.0, steeringPressed=False)
+    params = log.LiveParametersData.new_message(angleOffsetDeg=0.0, roll=0.0)
+    toggles = SimpleNamespace(ford_lateral_mode=2)
+
+    for frame in range(round(2.0 / DT_CTRL)):
+      CS.steeringAngleDeg = frame * 12.0 * DT_CTRL
+      target[0] = CS.steeringAngleDeg + 12.0
+      _, _, angle_log = controller.update(
+        True, CS, VM, params, False, 0.0, False, 0.0, None, None, toggles)
+      assert not angle_log.saturated
+
+    for _ in range(round(2.0 / DT_CTRL)):
+      _, _, angle_log = controller.update(
+        True, CS, VM, params, False, 0.0, False, 0.0, None, None, toggles)
+    assert angle_log.saturated
+
  def test_torque_log_exposes_friction_controller_state(self):
    controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_BOLT_ACC_2022_2023)

@@ -17,6 +17,30 @@ def test_tracked_lead_catchup_bias_ignores_near_desired_gap():
  assert bias == 0.0


+def test_tracked_lead_catchup_bias_corrects_lightning_aggressive_follow_bookmark():
+  default_bias = get_tracked_lead_catchup_bias(
+    31.75,
+    46.2,
+    34.0,
+    0.0,
+    v_cruise=37.55,
+    y_rel=0.1,
+  )
+  bias = get_tracked_lead_catchup_bias(
+    31.75,
+    46.2,
+    34.0,
+    0.0,
+    v_cruise=37.55,
+    y_rel=0.1,
+    min_headway_margin=0.20,
+    full_headway_margin=0.45,
+  )
+
+  assert default_bias == 0.0
+  assert bias > 2.0
+
+
 def test_tracked_lead_catchup_bias_ignores_very_far_gap():
  bias = get_tracked_lead_catchup_bias(31.4, 110.0, 38.0, 0.1)
  assert bias == 0.0
@@ -10,6 +10,8 @@ from cereal import log
 from opendbc.car.honda.interface import CarInterface
 from opendbc.car.honda.values import CAR
 from opendbc.car.gm.values import CAR as GM_CAR, GMFlags
+from opendbc.car.ford.interface import CarInterface as FordCarInterface
+from opendbc.car.ford.values import CAR as FORD_CAR
 from opendbc.car.toyota.interface import CarInterface as ToyotaCarInterface
 from opendbc.car.toyota.values import CAR as TOYOTA_CAR
 import openpilot.selfdrive.controls.lib.longitudinal_planner as longitudinal_planner_module
@@ -31,7 +33,12 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
  get_honda_crv_5g_stopped_lead_obstacle_bias,
  get_honda_crv_5g_low_speed_stopped_lead_cap,
  allow_honda_crv_5g_vision_gap_settle,
+  is_honda_crv_5g_early_radar_follow_lead,
+  get_honda_crv_5g_early_radar_follow_cap,
  get_standstill_gap_settle_max_extra_gap,
+  get_standstill_stopped_lead_guard_distance_margin,
+  get_standstill_stopped_lead_guard_max_lead_speed,
+  get_tracked_lead_catchup_headway_margins,
  get_toyota_prius_stopped_lead_obstacle_bias,
  get_toyota_rav4_tss2_lead_departure_tune,
  get_toyota_rav4_tss2_early_lead_cap,
@@ -139,6 +146,43 @@ def test_honda_crv_5g_stopped_lead_tune_is_vehicle_specific():
  assert get_standstill_gap_settle_max_extra_gap(crv) > get_standstill_gap_settle_max_extra_gap(civic)


+def test_honda_crv_5g_early_radar_follow_admits_high_closing_centered_lead():
+  crv = CarInterface.get_non_essential_params(CAR.HONDA_CRV_5G)
+  lead = make_lead(
+    status=True, d_rel=77.0, v_lead=2.8, radar=True, model_prob=0.87, y_rel=0.56,
+  )
+
+  assert is_honda_crv_5g_early_radar_follow_lead(crv, lead, v_ego=16.2)
+  assert get_honda_crv_5g_early_radar_follow_cap(
+    crv, lead, v_ego=16.2, accel_min=-3.5,
+  ) == pytest.approx(-0.39, abs=0.01)
+
+
+@pytest.mark.parametrize("kwargs", [
+  {"radar": False},
+  {"model_prob": 0.79},
+  {"y_rel": 1.01},
+  {"v_lead": 8.1},
+  {"d_rel": 90.0},
+])
+def test_honda_crv_5g_early_radar_follow_rejects_unreliable_or_nonurgent_lead(kwargs):
+  crv = CarInterface.get_non_essential_params(CAR.HONDA_CRV_5G)
+  lead_kwargs = {
+    "d_rel": 77.0, "v_lead": 2.8, "radar": True, "model_prob": 0.87, "y_rel": 0.56,
+  }
+  lead_kwargs.update(kwargs)
+  lead = make_lead(status=True, **lead_kwargs)
+
+  assert not is_honda_crv_5g_early_radar_follow_lead(crv, lead, v_ego=16.2)
+
+
+def test_honda_crv_5g_early_radar_follow_is_vehicle_specific():
+  civic = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  lead = make_lead(status=True, d_rel=77.0, v_lead=2.8, radar=True, model_prob=0.87, y_rel=0.56)
+
+  assert not is_honda_crv_5g_early_radar_follow_lead(civic, lead, v_ego=16.2)
+
+
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
 def test_honda_crv_5g_vision_lead_gap_settle_is_bounded(model_version):
  CP = CarInterface.get_non_essential_params(CAR.HONDA_CRV_5G)
@@ -679,6 +723,18 @@ def test_prebrake_floor_is_vehicle_specific():
  assert get_follow_prebrake_min_headway(honda, 1.0) == pytest.approx(1.25)


+def test_lightning_stopped_lead_guard_tune_is_vehicle_specific():
+  lightning = FordCarInterface.get_non_essential_params(FORD_CAR.FORD_F_150_LIGHTNING_MK1)
+  civic = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+
+  assert get_standstill_stopped_lead_guard_distance_margin(lightning) == pytest.approx(5.0)
+  assert get_standstill_stopped_lead_guard_distance_margin(civic) == pytest.approx(3.0)
+  assert get_standstill_stopped_lead_guard_max_lead_speed(lightning, 0.45) == pytest.approx(0.60)
+  assert get_standstill_stopped_lead_guard_max_lead_speed(civic, 0.45) == pytest.approx(0.45)
+  assert get_tracked_lead_catchup_headway_margins(lightning) == pytest.approx((0.20, 0.45))
+  assert get_tracked_lead_catchup_headway_margins(civic) is None
+
+
 def test_silverado_vision_follow_hold_survives_nonurgent_far_lead_crossover():
  v_ego = 32.0
  t_follow = 1.0
@@ -2332,6 +2388,37 @@ def test_standstill_stopped_lead_guard_blocks_false_release_at_longer_gap(model_
  assert planner.output_a_target <= 0.0


+def test_lightning_stopped_lead_guard_holds_ambiguous_creep_then_releases():
+  CP = FordCarInterface.get_non_essential_params(FORD_CAR.FORD_F_150_LIGHTNING_MK1)
+  planner = LongitudinalPlanner(CP, init_v=0.17)
+  sm = make_sm(
+    0.17,
+    desired_accel=0.17,
+    min_accel=-0.5,
+    experimental_mode=False,
+    tracking_lead=True,
+    lead_one=make_lead(status=True, d_rel=9.5, v_lead=0.46, a_lead=0.02, radar=True, model_prob=1.0),
+  )
+  sm["starpilotPlan"].vCruise = 10.0
+  sm["modelV2"].action.shouldStop = False
+
+  planner.update(sm, make_toggles("v15"))
+
+  assert planner.output_should_stop
+  assert planner.output_a_target <= 0.0
+
+  sm["carState"].vEgo = 0.0
+  sm["carState"].vEgoCluster = 0.0
+  sm["carState"].standstill = True
+  sm["radarState"].leadOne.dRel = 9.9
+  sm["radarState"].leadOne.vLead = 0.71
+  sm["radarState"].leadOne.vLeadK = 0.71
+  sm["radarState"].leadOne.aLeadK = 0.22
+  planner.update(sm, make_toggles("v15"))
+
+  assert not planner.output_should_stop
+
+
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
 def test_standstill_stopped_lead_guard_does_not_block_radar_depart_at_longer_gap(model_version):
  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
@@ -77,9 +77,6 @@ def _should_publish_model_output(model_output, vipc_dropped_frames: int, externa
 MIN_LAT_CONTROL_SPEED = 0.3
 BIG_MODEL_LOAD_WAIT_TIMEOUT_MS = 30000
 BIG_MODEL_RUN_WAIT_TIMEOUT_MS = 3000
-EXTERNAL_GPU_POWER_READY_MV = 13000
-EXTERNAL_GPU_POWER_STABLE_SECONDS = 3.0
-EXTERNAL_GPU_POWER_LOG_INTERVAL_SECONDS = 10.0
 LAT_SMOOTH_BP = [2.0, 8.0]


@@ -92,40 +89,6 @@ def _set_hcq_wait_timeout(timeout_ms: int) -> None:
  getenv.cache_clear()


-def _external_gpu_power_ready(panda_states, now: float, stable_since: float | None) -> tuple[bool, float | None, int | None]:
-  voltages = [
-    int(state.voltage) for state in panda_states
-    if state.pandaType != log.PandaState.PandaType.unknown and int(state.voltage) > 0
-  ]
-  voltage = max(voltages, default=None)
-  if voltage is None or voltage < EXTERNAL_GPU_POWER_READY_MV:
-    return False, None, voltage
-
-  stable_since = now if stable_since is None else stable_since
-  return now - stable_since >= EXTERNAL_GPU_POWER_STABLE_SECONDS, stable_since, voltage
-
-
-def wait_for_external_gpu_power_ready() -> None:
-  """Wait until the vehicle's 12 V rail is in its post-start charging state."""
-  sm = SubMaster(["pandaStates"])
-  stable_since = None
-  last_log = 0.0
-
-  while True:
-    sm.update(1000)
-    now = time.monotonic()
-    ready, stable_since, voltage = _external_gpu_power_ready(sm["pandaStates"], now, stable_since)
-    if ready:
-      cloudlog.warning(f"vehicle power stable at {voltage / 1000:.2f} V; starting external GPU load")
-      return
-
-    if now - last_log >= EXTERNAL_GPU_POWER_LOG_INTERVAL_SECONDS:
-      detail = "unavailable" if voltage is None else f"{voltage / 1000:.2f} V"
-      cloudlog.warning(f"external GPU load deferred: vehicle power is {detail}; waiting for " +
-                       f"{EXTERNAL_GPU_POWER_READY_MV / 1000:.1f} V to remain stable")
-      last_log = now
-
-
 def get_lateral_smooth_seconds(v_ego: float, maximum: float = 0.0) -> float:
  return float(np.interp(v_ego, LAT_SMOOTH_BP, [maximum, 0.0]))

@@ -666,14 +629,10 @@ def _load_model_state(cam_w: int, cam_h: int, selected_model: str, external_gpu_
    return ModelState(cam_w, cam_h, False)


-def _load_external_gpu_model(cam_w: int, cam_h: int, selected_model: str,
-                             demo: bool = False) -> ModelState | None:
+def _load_external_gpu_model(cam_w: int, cam_h: int, selected_model: str) -> ModelState | None:
  """Load and warm the USB-GPU model without running another tinygrad model concurrently."""
  candidate = None
  try:
-    if not demo:
-      wait_for_external_gpu_power_ready()
-
    _set_hcq_wait_timeout(BIG_MODEL_LOAD_WAIT_TIMEOUT_MS)
    wait_usbgpu_link()
    candidate = ModelState(
@@ -748,7 +707,6 @@ def main(demo=False):
      vipc_client_main.width,
      vipc_client_main.height,
      selected_model,
-      demo,
    )

    small_model = ModelState(
@@ -69,46 +69,6 @@ def test_native_amd_signal_keeps_existing_short_wait_behavior():
  assert sleeps == [200]


-def test_external_gpu_power_must_be_stable_after_vehicle_start():
-  panda_type = modeld.log.PandaState.PandaType.tres
-
-  def panda_state(voltage):
-    return SimpleNamespace(pandaType=panda_type, voltage=voltage)
-
-  ready, stable_since, voltage = modeld._external_gpu_power_ready([panda_state(12800)], 10.0, None)
-  assert not ready
-  assert stable_since is None
-  assert voltage == 12800
-
-  ready, stable_since, voltage = modeld._external_gpu_power_ready([panda_state(14100)], 11.0, stable_since)
-  assert not ready
-  assert stable_since == 11.0
-  assert voltage == 14100
-
-  ready, stable_since, _ = modeld._external_gpu_power_ready([panda_state(14100)], 13.9, stable_since)
-  assert not ready
-  assert stable_since == 11.0
-
-  ready, stable_since, _ = modeld._external_gpu_power_ready([panda_state(11900)], 14.0, stable_since)
-  assert not ready
-  assert stable_since is None
-
-  ready, stable_since, _ = modeld._external_gpu_power_ready([panda_state(14100)], 15.0, stable_since)
-  assert not ready
-  ready, stable_since, _ = modeld._external_gpu_power_ready([panda_state(14100)], 18.0, stable_since)
-  assert ready
-  assert stable_since == 15.0
-
-
-def test_external_gpu_power_ignores_unknown_pandas():
-  panda_states = [
-    SimpleNamespace(pandaType=modeld.log.PandaState.PandaType.unknown, voltage=15000),
-    SimpleNamespace(pandaType=modeld.log.PandaState.PandaType.tres, voltage=0),
-  ]
-
-  assert modeld._external_gpu_power_ready(panda_states, 10.0, None) == (False, None, None)
-
-
 def test_external_gpu_wait_timeout_updates_tinygrad_cache(monkeypatch):
  from tinygrad.helpers import getenv

@@ -178,7 +138,6 @@ def test_external_gpu_load_finishes_before_native_model_can_start(monkeypatch):
    def warmup(self):
      calls.append("warmup")

-  monkeypatch.setattr(modeld, "wait_for_external_gpu_power_ready", lambda: calls.append("power"))
  monkeypatch.setattr(modeld, "wait_usbgpu_link", lambda: calls.append("link"))
  monkeypatch.setattr(modeld, "_set_hcq_wait_timeout", lambda timeout: calls.append(("timeout", timeout)))
  monkeypatch.setattr(modeld, "_close_tinygrad_disk_cache_connection", lambda: calls.append("close_cache"))
@@ -193,7 +152,6 @@ def test_external_gpu_load_finishes_before_native_model_can_start(monkeypatch):

  assert isinstance(loaded, FakeModelState)
  assert calls == [
-    "power",
    ("timeout", modeld.BIG_MODEL_LOAD_WAIT_TIMEOUT_MS),
    "link",
    ("model", 1928, 1208, True, "big-model", False),
@@ -365,12 +365,13 @@ class ModelRenderer(Widget):
  def _lane_centering_direction(self) -> int:
    toggles = ui_state.starpilot_toggles
    sm = ui_state.sm
-    if not sm.valid.get("modelV2", False) or not sm.valid.get("carState", False):
+    if (sm.recv_frame.get("modelV2", 0) < ui_state.started_frame or
+        sm.recv_frame.get("carState", 0) < ui_state.started_frame):
      return 0

    car_state = sm["carState"]
    applied_correction = None
-    if sm.valid.get("controlsState", False):
+    if sm.recv_frame.get("controlsState", 0) >= ui_state.started_frame:
      try:
        applied_correction = sm["controlsState"].desiredCurvature - sm["modelV2"].action.desiredCurvature
      except (AttributeError, TypeError, ValueError):
@@ -373,12 +373,13 @@ class ModelRenderer(Widget):
  def _lane_centering_direction(self) -> int:
    toggles = ui_state.starpilot_toggles
    sm = ui_state.sm
-    if not sm.valid.get("modelV2", False) or not sm.valid.get("carState", False):
+    if (sm.recv_frame.get("modelV2", 0) < ui_state.started_frame or
+        sm.recv_frame.get("carState", 0) < ui_state.started_frame):
      return 0

    car_state = sm["carState"]
    applied_correction = None
-    if sm.valid.get("controlsState", False):
+    if sm.recv_frame.get("controlsState", 0) >= ui_state.started_frame:
      try:
        applied_correction = sm["controlsState"].desiredCurvature - sm["modelV2"].action.desiredCurvature
      except (AttributeError, TypeError, ValueError):
@@ -11,7 +11,12 @@ DEEP_GLIDE_BORDER_COLOR = rl.Color(24, 72, 150, 255)

 def get_pulse_glide_border_color(sm, default_color: rl.Color) -> rl.Color:
  """Use a deep-blue outer border only while developer P&G is gliding."""
-  if not sm.valid.get("starpilotCarState", False) or not sm.valid.get("starpilotPlan", False):
+  if (
+    not sm.valid.get("starpilotCarState", False) or
+    not sm.valid.get("starpilotPlan", False) or
+    not sm.valid.get("carControl", False) or
+    not bool(getattr(sm["carControl"], "longActive", False))
+  ):
    return default_color

  car_state = sm["starpilotCarState"]
@@ -571,6 +571,7 @@ class ModelManager:
        "released": str(info.get("released") or "2100-01-01").strip(),
        "community_favorite": False,
        "artifact_format": UNIFIED_ARTIFACT_FORMAT,
+        "uses_external_gpu": bool(info.get("uses_external_gpu", False)),
      }

    return list(discovered.values())
@@ -64,6 +64,32 @@ def test_external_gpu_requirement_is_cached_from_manifest(tmp_path, monkeypatch)
  assert not model_manager.model_uses_external_gpu("missing")


+def test_local_gpu_compile_persists_runtime_metadata(tmp_path, monkeypatch):
+  models_path = tmp_path / "models"
+  compiled_path = tmp_path / "compiled" / "local-large_driving_tinygrad.pkl"
+  models_path.mkdir()
+  compiled_path.parent.mkdir()
+  compiled_path.write_bytes(b"artifact")
+  monkeypatch.setattr(model_compiler, "MODELS_PATH", models_path)
+
+  model_compiler.install_local_artifact(compiled_path, "local-large", "v16", external_gpu=True)
+
+  sidecar = json.loads((models_path / "local-large.json").read_text())
+  metadata = json.loads((models_path / model_manager.ARTIFACT_METADATA_CACHE).read_text())
+  assert sidecar["uses_external_gpu"] is True
+  assert metadata["local-large"]["uses_external_gpu"] is True
+
+  monkeypatch.setattr(model_manager, "MODELS_PATH", models_path)
+  manager = object.__new__(ModelManager)
+  assert manager._discover_local_models()[0]["uses_external_gpu"] is True
+
+  model_compiler.install_local_artifact(compiled_path, "local-large", "v16", external_gpu=False)
+  sidecar = json.loads((models_path / "local-large.json").read_text())
+  metadata = json.loads((models_path / model_manager.ARTIFACT_METADATA_CACHE).read_text())
+  assert sidecar["uses_external_gpu"] is False
+  assert metadata["local-large"]["uses_external_gpu"] is False
+
+
 def test_external_gpu_compilation_is_opt_in(tmp_path, monkeypatch):
  invocations = []
  monkeypatch.setattr(model_compiler, "build_compile_env", lambda **_: {
@@ -499,7 +499,7 @@
        "ui_type": "numeric",
        "min": 0.0,
        "max": 1.0,
-        "step": 0.05,
+        "step": 0.01,
        "precision": 2,
        "parent_key": "FordLateralMode",
        "visible_when_key": "FordLateralMode",
@@ -520,7 +520,7 @@
        "ui_type": "numeric",
        "min": 0.5,
        "max": 1.5,
-        "step": 0.05,
+        "step": 0.01,
        "precision": 2,
        "parent_key": "FordLateralMode",
        "visible_when_key": "FordLateralMode",
@@ -541,7 +541,7 @@
        "ui_type": "numeric",
        "min": 0.5,
        "max": 1.5,
-        "step": 0.05,
+        "step": 0.01,
        "precision": 2,
        "parent_key": "FordLateralMode",
        "visible_when_key": "FordLateralMode",
@@ -562,7 +562,7 @@
        "ui_type": "numeric",
        "min": 0.25,
        "max": 1.25,
-        "step": 0.05,
+        "step": 0.01,
        "precision": 2,
        "parent_key": "FordLateralMode",
        "visible_when_key": "FordLateralMode",
@@ -583,7 +583,7 @@
        "ui_type": "numeric",
        "min": 0.5,
        "max": 1.5,
-        "step": 0.05,
+        "step": 0.01,
        "precision": 2,
        "parent_key": "FordLateralMode",
        "visible_when_key": "FordLateralMode",
@@ -3379,6 +3379,16 @@
        "ui_type": "toggle",
        "settings_tier": "simple"
      },
+      {
+        "key": "SubaruStopStartOff",
+        "label": "Stop/Start Off at Startup",
+        "description": "For Subaru Outback 2023-24, send one momentary Stop/Start request after ignition while the vehicle is stationary and in Park or Neutral.",
+        "picker_description": "Requests Stop/Start OFF once after ignition on supported Outbacks.",
+        "data_type": "bool",
+        "ui_type": "toggle",
+        "galaxy_only": true,
+        "settings_tier": "simple"
+      },
      {
        "key": "ClusterOffset",
        "label": "Dashboard Speed Offset",
@@ -191,6 +191,7 @@ SAFE_MODE_MANAGED_KEYS = (
  "ToyotaAutoHold",
  "SubaruSNG",
  "SubaruSNGManualParkingBrake",
+  "SubaruStopStartOff",
  "VoltSNG",
  "JeepBrakeHold",
  "GMAutoHold",
@@ -208,6 +209,7 @@ SAFE_MODE_FIXED_VALUES = {
  "ExperimentalMode": False,
  "LongitudinalPersonality": int(log.LongitudinalPersonality.relaxed),
  "UseAutoSteerDelay": True,
+  "SubaruStopStartOff": False,
 }

 SAFE_MODE_STOCK_PARAM_MAP = {
@@ -20,7 +20,7 @@ from opendbc.car.gm.values import CAR as GM_CAR, EV_CAR as GM_EV_CAR, GMFlags
 from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR, EV_CAR as HYUNDAI_EV_CAR, HyundaiFlags, HyundaiStarPilotSafetyFlags
 from opendbc.car.interfaces import TORQUE_SUBSTITUTE_PATH, CarInterfaceBase, GearShifter
 from opendbc.car.mock.values import CAR as MOCK
-from opendbc.car.subaru.values import SubaruFlags
+from opendbc.car.subaru.values import CAR as SUBARU_CAR, SubaruFlags
 from opendbc.car.tesla.values import CAR as TESLA_CAR
 from opendbc.car.toyota.values import CAR as TOYOTA_CAR, ToyotaStarPilotFlags
 from openpilot.common.basedir import BASEDIR
@@ -621,6 +621,14 @@ class StarPilotVariables:

    alpha_longitudinal = CP.alphaLongitudinalAvailable
    toggle.car_make = CP.brand
+    toggle.ford_lateral_mode = self.get_value(
+      "FordLateralMode",
+      cast=int,
+      condition=toggle.car_make == "ford",
+      default=1,
+      min=0,
+      max=2,
+    )
    migrate_ford_lkas_button_default(toggle.car_make, self.params)
    toggle.car_model = CP.carFingerprint
    toggle.disable_openpilot_long = self.get_value("DisableOpenpilotLongitudinal", condition=not alpha_longitudinal)
@@ -1479,6 +1487,9 @@ class StarPilotVariables:
    toggle.subaru_sng = self.get_value("SubaruSNG", condition=toggle.car_make == "subaru" and
                                       not (CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID | SubaruFlags.LKAS_ANGLE)))
    toggle.subaru_sng_manual_parking_brake = self.get_value("SubaruSNGManualParkingBrake", condition=toggle.subaru_sng)
+    toggle.subaru_stop_start_off = self.get_value(
+      "SubaruStopStartOff", condition=toggle.car_model == SUBARU_CAR.SUBARU_OUTBACK_2023,
+    )

    toggle.jeep_brake_hold = self.get_value(
      "JeepBrakeHold",
@@ -86,7 +86,7 @@ class StarPilotCard:
    elif getattr(starpilot_toggles, f"force_coast_via_{key}"):
      self.force_coast = not self.force_coast
    elif getattr(starpilot_toggles, f"pulse_and_glide_via_{key}"):
-      if getattr(sm["carControl"], "longActive", False):
+      if getattr(sm["carControl"], "longActive", False) or self.pulse_and_glide:
        self.pulse_and_glide = not self.pulse_and_glide
        return True
    elif getattr(starpilot_toggles, f"pause_lateral_via_{key}"):
@@ -137,9 +137,12 @@ class StarPilotCard:
    self.switchback_mode_enabled = self.params_memory.get_bool("SwitchbackModeEnabled")
    self._handle_favorite_traffic_mode_action(sm)

-    pulse_glide_cancel_override = bool(getattr(sm["carControl"], "longActive", False)) and any(
-      getattr(starpilot_toggles, f"pulse_and_glide_via_cancel{suffix}", False)
-      for suffix in ("", "_long", "_very_long")
+    pulse_glide_cancel_override = (
+      (bool(getattr(sm["carControl"], "longActive", False)) or self.pulse_and_glide) and
+      any(
+        getattr(starpilot_toggles, f"pulse_and_glide_via_cancel{suffix}", False)
+        for suffix in ("", "_long", "_very_long")
+      )
    )
    cancel_pressed = bool(getattr(starpilotCarState, "cancelPressed", False))
    if pulse_glide_cancel_override:
@@ -155,8 +158,9 @@ class StarPilotCard:
      self._button_type_raw(be) == int(ButtonType.lkas) and be.pressed
      for be in carState.buttonEvents
    )
-    pulse_glide_lkas_override = bool(getattr(sm["carControl"], "longActive", False)) and getattr(
-      starpilot_toggles, "pulse_and_glide_via_lkas", False
+    pulse_glide_lkas_override = (
+      (bool(getattr(sm["carControl"], "longActive", False)) or self.pulse_and_glide) and
+      getattr(starpilot_toggles, "pulse_and_glide_via_lkas", False)
    )
    if pulse_glide_lkas_override:
      carState.buttonEvents = [
@@ -368,8 +372,6 @@ class StarPilotCard:

    if not getattr(starpilot_toggles, "pulse_and_glide_available", False):
      self.pulse_and_glide = False
-    self.pulse_and_glide &= bool(getattr(sm["carControl"], "longActive", False))
-    self.pulse_and_glide &= not (carState.brakePressed or carState.gasPressed)
    self.force_coast &= not (carState.brakePressed or carState.gasPressed)

    starpilotCarState.accelPressed = self.accel_pressed
@@ -123,7 +123,38 @@ def test_pulse_and_glide_requires_developer_access_and_active_longitudinal(monke

  car_state = make_car_state(gas_pressed=True)
  result = card.update(car_state, starpilot_car_state, sm, toggles)
+  assert result.pulseAndGlide is True
+
+
+def test_pulse_and_glide_survives_temporary_disengagement(monkeypatch, tmp_path):
+  monkeypatch.setattr(spc, "Params", FakeParams)
+  monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)
+
+  card = spc.StarPilotCard(SimpleNamespace(brand="gm"), SimpleNamespace(alternativeExperience=0))
+  sm = make_sm()
+  sm["carControl"].longActive = True
+  toggles = make_toggles(
+    pulse_and_glide_available=True,
+    pulse_and_glide_via_lkas=True,
+  )
+  starpilot_car_state = SimpleNamespace(distancePressed=False)
+
+  card.handle_button_event("lkas", sm, toggles)
+  assert card.pulse_and_glide is True
+
+  sm["carControl"].longActive = False
+  result = card.update(make_car_state(brake_pressed=True), starpilot_car_state, sm, toggles)
+  assert result.pulseAndGlide is True
+
+  sm["carControl"].longActive = True
+  result = card.update(make_car_state(), starpilot_car_state, sm, toggles)
+  assert result.pulseAndGlide is True
+
+  sm["carControl"].longActive = False
+  off_press = make_car_state(button_events=[SimpleNamespace(type=spc.ButtonType.lkas, pressed=True)])
+  result = card.update(off_press, starpilot_car_state, sm, toggles)
  assert result.pulseAndGlide is False
+  assert off_press.buttonEvents == []


 def test_pulse_and_glide_consumes_native_cancel_when_mapped(monkeypatch, tmp_path):
@@ -39,6 +39,7 @@ const VEHICLE_SETTING_MAKES = {
  JeepBrakeHold: ["Jeep"],
  SubaruSNG: ["Subaru"],
  SubaruSNGManualParkingBrake: ["Subaru"],
+  SubaruStopStartOff: ["Subaru"],
  ClusterOffset: ["Lexus", "Toyota"],
  SNGHack: ["Lexus", "Toyota"],
  ToyotaAutoHold: ["Lexus", "Toyota"],
@@ -255,6 +255,22 @@ def test_honda_pid_scale_controls_use_galaxy_fine_granularity():
    assert setting["settings_tier"] == "advanced"


+def test_ford_angle_controls_use_galaxy_fine_granularity():
+  lateral = _params_by_section(_layout())["Lateral (Steering)"]
+
+  for key in (
+    "FordAngleBlend",
+    "FordAngleLowSpeedFactor",
+    "FordAngleHighSpeedFactor",
+    "FordAngleHighSpeedDamping",
+    "FordAngleLaneChangeFactor",
+  ):
+    setting = lateral[key]
+    assert setting["step"] == 0.01
+    assert setting["precision"] == 2
+    assert setting["galaxy_only"]
+
+
 def test_hidden_feature_defaults_remain_enabled():
  assert _declared_default("GalaxyDeveloperMode") == "0"
  assert _declared_default("NavDesiresAllowed") == "1"
@@ -5,21 +5,25 @@ import base64
 import hashlib
 import io
 import json
+import math
 import os
 import queue
 import random
+import re
 import select
 import socket
+import subprocess
 import sys
 import tempfile
 import threading
 import time
+from contextlib import suppress
 from dataclasses import asdict, dataclass, replace
 from datetime import datetime
 from functools import partial, total_ordering
 from queue import Queue
 from typing import cast
-from collections.abc import Callable
+from collections.abc import Callable, Iterable

 import requests
 from requests.adapters import HTTPAdapter, DEFAULT_POOLBLOCK
@@ -31,15 +35,18 @@ import cereal.messaging as messaging
 from cereal import car, log
 from cereal.services import SERVICE_LIST
 from openpilot.common.api import Api, get_key_pair
+from openpilot.common.basedir import BASEDIR
 from openpilot.common.utils import CallbackReader, get_upload_stream
 from openpilot.common.params import Params
 from openpilot.common.realtime import set_core_affinity
 from openpilot.system.hardware import HARDWARE, PC
+from openpilot.system.loggerd.config import CAMERA_FPS, SEGMENT_LENGTH
 from openpilot.system.loggerd.xattr_cache import getxattr, setxattr
 from openpilot.common.swaglog import cloudlog
 from openpilot.system.athena.registration import UNREGISTERED_DONGLE_ID
 from openpilot.system.version import get_build_metadata
 from openpilot.system.hardware.hw import Paths
+from openpilot.tools.lib.helpers import RE

 from openpilot.starpilot.common.starpilot_utilities import use_konik_server

@@ -58,6 +65,7 @@ MAX_AGE = 31 * 24 * 3600  # seconds
 WS_FRAME_SIZE = 4096
 DEVICE_STATE_UPDATE_INTERVAL = 1.0  # in seconds
 DEFAULT_UPLOAD_PRIORITY = 99  # higher number = lower priority
+CLIP_CHUNK_SIZE = 512 * 1024

 # https://bytesolutions.com/dscp-tos-cos-precedence-conversion-chart,
 # https://en.wikipedia.org/wiki/Differentiated_services
@@ -398,6 +406,217 @@ def listDataDirectory(prefix='') -> list[str]:
  return scan_dir(Paths.log_root(), prefix)


+class VideoClips:
+  @dataclass
+  class Clip:
+    route: str
+    camera: str
+    source_start_time: float
+    source_end_time: float
+    bitrate: int
+    speedup: int
+    filename: str
+    requested_at: float
+
+  def __init__(self):
+    self.clip_path = os.path.join(Paths.log_root(), "clips")
+    self.lock = threading.Condition()
+    self.clips: dict[str, VideoClips.Clip] = {}
+    self.transcode_proc: tuple[str, subprocess.Popen] | None = None
+    threading.Thread(target=self._worker, name="video_clip", daemon=True).start()
+
+  def _encode(self, clip: Clip, inputs: Iterable[str], output_path: str, start_time: float, duration: float) -> None:
+    inputs = list(inputs)
+    metadata = json.dumps(asdict(clip), separators=(',', ':'))
+    if PC:
+      command = [
+        "ffmpeg", "-hide_banner", "-loglevel", "error", "-nostdin", "-y",
+        "-r", str(CAMERA_FPS * clip.speedup), "-f", "concat", "-safe", "0", "-protocol_whitelist", "file,pipe", "-c:v", "hevc",
+        "-i", "pipe:0", "-ss", str(start_time / clip.speedup), "-t", str(duration / clip.speedup),
+        "-map", "0:v:0", "-an", "-r", str(CAMERA_FPS), "-c:v", "libx264", "-preset", "veryfast",
+        "-b:v", f"{clip.bitrate}M", "-pix_fmt", "yuv420p", "-movflags", "+faststart+use_metadata_tags",
+        "-metadata", f"ai.comma.clip.settings={metadata}", output_path,
+      ]
+    else:
+      command = [os.path.join(BASEDIR, "openpilot/system/loggerd/encoderd"), "--clip", output_path,
+                 str(start_time), str(duration), "--bitrate", str(clip.bitrate * 1_000_000),
+                 "--speedup", str(clip.speedup), "--metadata", metadata, "--", *inputs]
+
+    with self.lock:
+      if self.clips.get(clip.filename) is not clip:
+        return
+      process = subprocess.Popen(command, stdin=subprocess.PIPE if PC else subprocess.DEVNULL,
+                                 stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, text=True)
+      self.transcode_proc = (clip.filename, process)
+    try:
+      if PC:
+        if process.stdin is None:
+          raise RuntimeError("ffmpeg stdin is unavailable")
+        process.stdin.write("ffconcat version 1.0\n")
+        for path in inputs:
+          escaped_path = path.replace("'", "'\\''")
+          process.stdin.write(f"file 'file:{escaped_path}'\noption framerate {CAMERA_FPS}\nduration {SEGMENT_LENGTH}\n")
+        process.stdin.close()
+      process.wait()
+      if process.returncode != 0:
+        raise RuntimeError(f"clip encoder exited with code {process.returncode}")
+    finally:
+      with suppress(OSError):
+        if process.stdin is not None:
+          process.stdin.close()
+      if process.poll() is None:
+        process.terminate()
+        process.wait()
+      with self.lock:
+        if self.transcode_proc is not None and self.transcode_proc[0] == clip.filename:
+          self.transcode_proc = None
+
+  def _worker(self) -> None:
+    while True:
+      with self.lock:
+        while not self.clips:
+          self.lock.wait()
+        clip = next(iter(self.clips.values()))
+      temporary_path = ""
+      try:
+        with self.lock:
+          if self.clips.get(clip.filename) is not clip:
+            continue
+        first_segment = math.floor(clip.source_start_time / SEGMENT_LENGTH)
+        inputs = (
+          os.path.join(Paths.log_root(), f"{clip.route}--{segment}", clip.camera)
+          for segment in range(first_segment, math.ceil(clip.source_end_time / SEGMENT_LENGTH))
+        )
+        os.makedirs(self.clip_path, exist_ok=True)
+        temporary_path = os.path.join(self.clip_path, f".{clip.filename}")
+        output_path = os.path.join(self.clip_path, clip.filename)
+        self._encode(clip, inputs, temporary_path, clip.source_start_time - first_segment * SEGMENT_LENGTH,
+                     clip.source_end_time - clip.source_start_time)
+        with self.lock:
+          if self.clips.get(clip.filename) is clip:
+            os.replace(temporary_path, output_path)
+            del self.clips[clip.filename]
+      except Exception:
+        with self.lock:
+          failed = self.clips.get(clip.filename) is clip
+          if failed:
+            del self.clips[clip.filename]
+        if failed:
+          cloudlog.exception("athena.video_clip.failed")
+      finally:
+        with suppress(OSError):
+          if temporary_path:
+            os.unlink(temporary_path)
+
+  def _on_disk(self) -> dict[str, dict]:
+    clips = {}
+    try:
+      entries = os.scandir(self.clip_path)
+    except FileNotFoundError:
+      return clips
+    with entries:
+      for entry in entries:
+        if entry.name.startswith(".") or not entry.is_file():
+          continue
+        probe = subprocess.run(["ffprobe", "-v", "error", "-show_entries", "format_tags=ai.comma.clip.settings",
+                                "-of", "json", entry.path], capture_output=True, text=True)
+        if probe.returncode != 0:
+          continue
+        try:
+          metadata = json.loads(json.loads(probe.stdout)["format"]["tags"]["ai.comma.clip.settings"])
+          size = entry.stat().st_size
+        except (FileNotFoundError, KeyError, TypeError, json.JSONDecodeError):
+          continue
+        if not isinstance(metadata, dict) or not isinstance(metadata.get("requested_at"), (int, float)):
+          continue
+        clips[entry.name] = {**metadata, "filename": entry.name, "status": "ready",
+                             "fn": os.path.relpath(entry.path, Paths.log_root()), "size": size}
+    return clips
+
+  def _available_ranges(self, route: str) -> dict:
+    cameras: dict[str, list[int]] = {}
+    try:
+      with os.scandir(Paths.log_root()) as entries:
+        for entry in entries:
+          entry_route, _, segment = entry.name.rpartition("--")
+          if entry_route != route or not segment.isdigit() or not entry.is_dir():
+            continue
+          with os.scandir(entry.path) as files:
+            for camera in files:
+              if camera.is_file() and camera.name.endswith("camera.hevc"):
+                cameras.setdefault(camera.name, []).append(int(segment))
+    except OSError:
+      return {}
+
+    available = {}
+    for camera, camera_segments in cameras.items():
+      ranges: list[list[int]] = []
+      for segment in sorted(camera_segments):
+        if ranges and ranges[-1][1] == segment * SEGMENT_LENGTH:
+          ranges[-1][1] += SEGMENT_LENGTH
+        else:
+          ranges.append([segment * SEGMENT_LENGTH, (segment + 1) * SEGMENT_LENGTH])
+      available[camera] = {"available_ranges": ranges}
+    return available
+
+  def createClip(self, route: str, source_start_time: float, source_end_time: float, clip: dict):
+    if not PC and not Params().get_bool("IsOffroad"):
+      raise RuntimeError("video clips can only be created while offroad")
+    route_match = re.fullmatch(RE.ROUTE_NAME, route)
+    assert route_match is not None, "invalid route"
+    route_name = route_match.group("log_id")
+    camera = clip["camera"]
+    filename = clip["filename"]
+    assert camera == os.path.basename(camera) and camera.endswith("camera.hevc"), "invalid camera filename"
+    assert filename == os.path.basename(filename), "invalid filename"
+    with self.lock:
+      self.clips[filename] = self.Clip(route_name, camera, source_start_time, source_end_time, clip["bitrate"], clip["speedup"],
+                                        filename, datetime.now().timestamp())
+      self.lock.notify()
+
+  def getClipState(self, route: str | None = None) -> dict:
+    route_match = re.search(RE.ROUTE_NAME, route or "")
+    with self.lock:
+      transcode_filename = self.transcode_proc[0] if self.transcode_proc is not None else None
+      active_clips = {clip.filename: {**asdict(clip), "status": "encoding" if clip.filename == transcode_filename else "queued"}
+                      for clip in self.clips.values()}
+    clips = self._on_disk()
+    clips.update(active_clips)
+    state = {"clips": sorted(clips.values(), key=lambda clip: clip["requested_at"], reverse=True)}
+    if route_match is not None:
+      route_name = route_match.group("log_id")
+      state.update({"route": route_name, "cameras": self._available_ranges(route_name)})
+    return state
+
+  def deleteClip(self, filename: str) -> None:
+    assert filename == os.path.basename(filename), "invalid filename"
+    with self.lock:
+      self.clips.pop(filename, None)
+      output_path = os.path.join(self.clip_path, filename)
+      if self.transcode_proc is not None and self.transcode_proc[0] == filename:
+        self.transcode_proc[1].terminate()
+      if os.path.exists(output_path):
+        os.unlink(output_path)
+
+  def getClipChunk(self, filename: str, offset: int) -> dict:
+    assert filename == os.path.basename(filename) and not filename.startswith("."), "invalid filename"
+    assert isinstance(offset, int) and offset >= 0, "invalid offset"
+    path = os.path.join(self.clip_path, filename)
+    size = os.path.getsize(path)
+    assert offset <= size, "offset past end of file"
+    with open(path, "rb") as f:
+      f.seek(offset)
+      data = f.read(CLIP_CHUNK_SIZE)
+    return {"data": base64.b64encode(data).decode(), "offset": offset, "size": size}
+
+
+video_clips = VideoClips()
+dispatcher.add_method(video_clips.createClip)
+dispatcher.add_method(video_clips.getClipState)
+dispatcher.add_method(video_clips.deleteClip)
+dispatcher.add_method(video_clips.getClipChunk)
+
+
@dispatcher.add_method
 def uploadFileToUrl(fn: str, url: str, headers: dict[str, str]) -> UploadFilesToUrlResponse:
  # this is because mypy doesn't understand that the decorator doesn't change the return type
@@ -104,12 +104,23 @@ class TestAthenadMethods:
      f.write(data)
    return fn

+  @staticmethod
+  def _video_clips(clip):
+    clips = object.__new__(athenad.VideoClips)
+    clips.lock = threading.Condition()
+    clips.clips = {clip.filename: clip}
+    clips.transcode_proc = None
+    return clips
+

  # *** test cases ***

  def test_echo(self):
    assert dispatcher["echo"]("bob") == "bob"

+  def test_video_clip_methods_registered(self):
+    assert {"createClip", "getClipState", "deleteClip", "getClipChunk"}.issubset(dispatcher)
+
  def test_get_message(self):
    with pytest.raises(TimeoutError) as _:
      dispatcher["getMessage"]("controlsState")
@@ -174,6 +185,44 @@ class TestAthenadMethods:
    assert resp, 'list empty!'
    assert len(resp) == len(expected)

+  def test_video_clip_hardware_encoder(self, mocker):
+    clip = athenad.VideoClips.Clip("route", "fcamera.hevc", 10, 130, 2, 4, "clip.mp4", 123)
+    clips = self._video_clips(clip)
+    process = mocker.Mock(stdin=None, returncode=0)
+    process.poll.return_value = 0
+    popen = mocker.patch("openpilot.system.athena.athenad.subprocess.Popen", return_value=process)
+    mocker.patch.object(athenad, "PC", False)
+
+    clips._encode(clip, ["segment0", "segment1"], "output.mp4", 10, 120)
+
+    metadata = json.dumps(asdict(clip), separators=(',', ':'))
+    assert popen.call_args.args[0] == [
+      os.path.join(athenad.BASEDIR, "openpilot/system/loggerd/encoderd"), "--clip", "output.mp4", "10", "120",
+      "--bitrate", "2000000", "--speedup", "4", "--metadata", metadata, "--", "segment0", "segment1",
+    ]
+    assert popen.call_args.kwargs["stdin"] == athenad.subprocess.DEVNULL
+    assert clips.transcode_proc is None
+
+  def test_video_clip_software_fallback(self, mocker):
+    clip = athenad.VideoClips.Clip("route", "fcamera.hevc", 10, 30, 3, 2, "clip.mp4", 123)
+    clips = self._video_clips(clip)
+    stdin = mocker.Mock()
+    process = mocker.Mock(stdin=stdin, returncode=0)
+    process.poll.return_value = 0
+    popen = mocker.patch("openpilot.system.athena.athenad.subprocess.Popen", return_value=process)
+    mocker.patch.object(athenad, "PC", True)
+
+    clips._encode(clip, ["segment'0", "segment1"], "output.mp4", 10, 20)
+
+    command = popen.call_args.args[0]
+    assert ["-r", "40"] == command[command.index("-r"):command.index("-r") + 2]
+    assert ["-ss", "5.0"] == command[command.index("-ss"):command.index("-ss") + 2]
+    assert ["-t", "10.0"] == command[command.index("-t"):command.index("-t") + 2]
+    assert ["-b:v", "3M"] == command[command.index("-b:v"):command.index("-b:v") + 2]
+    writes = [call.args[0] for call in stdin.write.call_args_list]
+    assert "file 'file:segment'\\''0'\n" in writes[1]
+    assert writes[-1].startswith("file 'file:segment1'")
+
  def test_strip_extension(self):
    # any requested log file with an invalid extension won't return as existing
    fn = self._create_file('qlog.bz2')
@@ -5,7 +5,9 @@ libs = [common, messaging, visionipc,
        'pthread', 'z', 'm', 'zstd']

 src = ['logger.cc', 'zstd_writer.cc', 'video_writer.cc', 'encoder/encoder.cc', 'encoder/v4l_encoder.cc', 'encoder/jpeg_encoder.cc']
-if arch != "larch64":
+if arch == "larch64":
+  src += ['clip_encoder.cc', 'encoder/v4l_decoder.cc']
+else:
  src += ['encoder/ffmpeg_encoder.cc']
  libs += ['yuv']

@@ -0,0 +1,270 @@
+#include "system/loggerd/clip_encoder.h"
+
+#include <algorithm>
+#include <array>
+#include <cmath>
+#include <exception>
+#include <filesystem>
+#include <thread>
+#include <unistd.h>
+#include <utility>
+#include <vector>
+
+extern "C" {
+#include <libavcodec/avcodec.h>
+#include <libavformat/avformat.h>
+}
+
+#include "common/swaglog.h"
+#include "system/loggerd/encoder/v4l_decoder.h"
+#include "system/loggerd/encoder/v4l_encoder.h"
+#include "system/loggerd/loggerd.h"
+#include "system/loggerd/video_writer.h"
+
+namespace {
+
+constexpr double SEGMENT_DURATION = 60.0;
+constexpr int CLIP_FPS = 20;
+constexpr double PARALLEL_CLIP_MIN_DURATION = 2 * SEGMENT_DURATION;
+
+const EncoderInfo clip_encoder_info = {
+  .publish_name = "livestreamRoadEncodeData",
+  .record = false,
+  .fps = CLIP_FPS,
+  .get_settings = [](int) { return EncoderSettings::StreamEncoderSettings(); },
+  INIT_ENCODE_FUNCTIONS(LivestreamRoadEncode),
+};
+
+bool open_input(const std::string &path, AVFormatContext **ctx, int *stream_index) {
+  if (avformat_open_input(ctx, path.c_str(), nullptr, nullptr) < 0 ||
+      avformat_find_stream_info(*ctx, nullptr) < 0 ||
+      (*stream_index = av_find_best_stream(*ctx, AVMEDIA_TYPE_VIDEO, -1, -1, nullptr, 0)) < 0) {
+    LOGE("failed to open clip input %s", path.c_str());
+    avformat_close_input(ctx);
+    return false;
+  }
+  return true;
+}
+
+void remove_file(const std::string &path) {
+  std::error_code error;
+  std::filesystem::remove(path, error);
+}
+
+int encode_clip_worker(const std::vector<std::string> &inputs, int width, int height,
+                       double start_time, double duration, int bitrate, int speedup,
+                       int64_t frame_offset, int64_t *encoded_frames,
+                       V4LEncoder::PacketCallback packet_callback) try {
+  EncoderInfo encoder_info = clip_encoder_info;
+  encoder_info.get_settings = [bitrate](int) {
+    return EncoderSettings{.encode_type = cereal::EncodeIndex::Type::QCAMERA_H264,
+                           .bitrate = bitrate, .gop_size = 5};
+  };
+  V4LDecoder decoder;
+  V4LEncoder::Options options = {
+    .packet_callback = std::move(packet_callback),
+    .input_format = V4L2_PIX_FMT_NV12_UBWC,
+    .input_done_callback = [&decoder](VisionBuf *buf) { decoder.releaseFrame(buf); },
+    .max_performance = true,
+  };
+  V4LEncoder encoder(encoder_info, width, height, std::move(options));
+  encoder.encoder_open();
+  if (!decoder.init(V4LDecoder::DEVICE, width, height, V4L2_PIX_FMT_HEVC, true, V4L2_PIX_FMT_NV12_UBWC)) return 1;
+
+  const int64_t first_frame = std::floor(start_time * CLIP_FPS);
+  const int64_t end_frame = std::ceil((start_time + duration) * CLIP_FPS);
+  const int64_t source_frames = end_frame - first_frame;
+  const int64_t first_output_frame = (speedup - frame_offset % speedup) % speedup;
+  const int64_t expected_output_frames = first_output_frame < source_frames ?
+    1 + (source_frames - first_output_frame - 1) / speedup : 0;
+  int64_t input_frame = 0;
+  int64_t output_frame = 0;
+  int64_t received_frames = 0;
+  bool failed = false;
+  auto pump_decoder = [&](int timeout_ms) {
+    V4LDecodedFrame frame;
+    if (!decoder.pump(frame, timeout_ms)) return false;
+    if (!frame.buf) return true;
+    ++received_frames;
+    const int64_t source_frame = (int64_t)frame.token - 1;
+    if (source_frame < first_frame) {
+      decoder.releaseFrame(frame.buf);
+      return true;
+    }
+    if ((frame_offset + source_frame - first_frame) % speedup != 0) {
+      decoder.releaseFrame(frame.buf);
+      return true;
+    }
+
+    VisionIpcBufExtra extra = {};
+    extra.frame_id = output_frame;
+    extra.timestamp_sof = output_frame * 1000000000ULL / CLIP_FPS;
+    extra.timestamp_eof = extra.timestamp_sof;
+    if (encoder.encode_frame(frame.buf, &extra) < 0) {
+      decoder.releaseFrame(frame.buf);
+      return false;
+    }
+
+    ++output_frame;
+    return true;
+  };
+
+  for (const std::string &input : inputs) {
+    AVFormatContext *ctx = nullptr;
+    int stream_index = -1;
+    if (!open_input(input, &ctx, &stream_index)) { failed = true; break; }
+    AVPacket packet = {};
+    while (input_frame < end_frame && av_read_frame(ctx, &packet) >= 0) {
+      if (packet.stream_index != stream_index) {
+        av_packet_unref(&packet);
+        continue;
+      }
+      if (packet.size <= 0 || (size_t)packet.size > decoder.maxPacketSize()) {
+        LOGE("decoder packet too large: %d > %zu", packet.size, decoder.maxPacketSize());
+        av_packet_unref(&packet);
+        failed = true;
+        break;
+      }
+
+      while (!decoder.queuePacket(&packet, input_frame + 1)) {
+        if (!pump_decoder(-1)) {
+          failed = true;
+          break;
+        }
+      }
+      av_packet_unref(&packet);
+      if (failed) break;
+
+      ++input_frame;
+    }
+    av_packet_unref(&packet);
+    avformat_close_input(&ctx);
+    if (failed || input_frame >= end_frame) break;
+  }
+
+  if (!failed) decoder.sendEOS();
+  for (int empty_polls = 0; !failed && received_frames < input_frame;) {
+    const int64_t before = received_frames;
+    failed = !pump_decoder(1000);
+    empty_polls = received_frames == before ? empty_polls + 1 : 0;
+    if (empty_polls == 5) failed = true;
+  }
+
+  encoder.encoder_close();
+  if (failed || input_frame < end_frame || output_frame != expected_output_frames) {
+    LOGE("clip failed: input=%lld/%lld decoded=%lld encoded=%lld/%lld",
+         (long long)input_frame, (long long)end_frame, (long long)received_frames,
+         (long long)output_frame, (long long)expected_output_frames);
+    return 1;
+  }
+  *encoded_frames = output_frame;
+  return 0;
+} catch (const std::exception &e) {
+  LOGE("clip worker failed: %s", e.what());
+  return 1;
+}
+
+struct SpoolPacket {
+  uint32_t size;
+  int64_t timestamp;
+  bool keyframe;
+};
+
+}  // namespace
+
+int encode_clip(const std::vector<std::string> &inputs, const std::string &output,
+                double start_time, double duration, int bitrate, int speedup,
+                const std::string &metadata) {
+  if (inputs.empty() || !std::isfinite(start_time) || !std::isfinite(duration) ||
+      start_time < 0 || duration <= 0 || bitrate <= 0 || speedup <= 0) {
+    return 1;
+  }
+
+  const double available_duration = inputs.size() * SEGMENT_DURATION;
+  if (start_time >= available_duration || duration > available_duration - start_time) return 1;
+  const size_t skipped_segments = start_time / SEGMENT_DURATION;
+  const std::vector<std::string> clip_inputs(inputs.begin() + skipped_segments, inputs.end());
+  const double local_start = start_time - skipped_segments * SEGMENT_DURATION;
+
+  AVFormatContext *ctx = nullptr;
+  int stream = -1;
+  if (!open_input(clip_inputs.front(), &ctx, &stream)) return 1;
+  AVCodecParameters *codec = ctx->streams[stream]->codecpar;
+  const int width = codec->width, height = codec->height;
+  const bool valid_codec = codec->codec_id == AV_CODEC_ID_HEVC && width > 0 && height > 0;
+  avformat_close_input(&ctx);
+  if (!valid_codec) return 1;
+
+  std::filesystem::path output_path(output);
+  const std::string output_dir = output_path.has_parent_path() ? output_path.parent_path() : ".";
+  VideoWriter writer(output_dir.c_str(), output_path.filename().c_str(), true,
+                     width, height, CLIP_FPS, cereal::EncodeIndex::Type::QCAMERA_H264);
+  if (!metadata.empty()) writer.set_metadata("ai.comma.clip.settings", metadata.c_str());
+  V4LEncoder::PacketCallback write_packet = [&writer](uint8_t *data, size_t size, int64_t timestamp,
+                                                      bool config, bool keyframe) {
+    writer.write(data, size, timestamp, config, keyframe);
+  };
+
+  if (clip_inputs.size() < 2 || duration < PARALLEL_CLIP_MIN_DURATION) {
+    int64_t encoded_frames = 0;
+    const bool success = encode_clip_worker(clip_inputs, width, height, local_start, duration,
+                                            bitrate, speedup, 0, &encoded_frames, write_packet) == 0;
+    if (!success) remove_file(output);
+    return success ? 0 : 1;
+  }
+
+  const size_t split = std::clamp<size_t>(std::llround((local_start + duration / 2) / SEGMENT_DURATION),
+                                           1, clip_inputs.size() - 1);
+  const double split_time = split * SEGMENT_DURATION;
+  const std::array<std::vector<std::string>, 2> shard_inputs = {
+    std::vector<std::string>(clip_inputs.begin(), clip_inputs.begin() + split),
+    std::vector<std::string>(clip_inputs.begin() + split, clip_inputs.end()),
+  };
+  const std::array<double, 2> shard_starts = {local_start, 0};
+  const std::array<double, 2> shard_durations = {
+    split_time - local_start, local_start + duration - split_time,
+  };
+  const std::string spool_path = output + ".encoderd-" + std::to_string(getpid()) + ".tmp";
+  FILE *spool = fopen(spool_path.c_str(), "w+b");
+  if (!spool) {
+    remove_file(output);
+    return 1;
+  }
+  remove_file(spool_path);
+  bool spool_ok = true;
+  V4LEncoder::PacketCallback spool_packet = [&](uint8_t *data, size_t size, int64_t timestamp,
+                                                bool config, bool keyframe) {
+    if (config) return;
+    const SpoolPacket packet = {(uint32_t)size, timestamp, keyframe};
+    spool_ok &= fwrite(&packet, sizeof(packet), 1, spool) == 1 && fwrite(data, 1, size, spool) == size;
+  };
+  std::array<int, 2> results = {1, 1};
+  std::array<int64_t, 2> encoded_frames = {};
+  const std::array<int64_t, 2> frame_offsets = {
+    0, (int64_t)std::llround(split_time * CLIP_FPS) - (int64_t)std::floor(local_start * CLIP_FPS),
+  };
+  std::array<std::thread, 2> workers;
+
+  for (size_t i = 0; i < workers.size(); ++i) {
+    workers[i] = std::thread([&, i]() {
+      results[i] = encode_clip_worker(shard_inputs[i], width, height, shard_starts[i], shard_durations[i],
+                                      bitrate, speedup, frame_offsets[i], &encoded_frames[i],
+                                      i == 0 ? write_packet : spool_packet);
+    });
+  }
+  for (std::thread &worker : workers) worker.join();
+
+  rewind(spool);
+  SpoolPacket packet;
+  std::vector<uint8_t> data;
+  const int64_t timestamp_offset = encoded_frames[0] * 1000000 / CLIP_FPS;
+  while (spool_ok && fread(&packet, sizeof(packet), 1, spool) == 1) {
+    data.resize(packet.size);
+    spool_ok = fread(data.data(), 1, data.size(), spool) == data.size();
+    if (spool_ok) writer.write(data.data(), data.size(), packet.timestamp + timestamp_offset, false, packet.keyframe);
+  }
+  fclose(spool);
+  bool success = results[0] == 0 && results[1] == 0 && spool_ok;
+  if (!success) remove_file(output);
+  return success ? 0 : 1;
+}
@@ -0,0 +1,10 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+// inputs are consecutive 60-second loggerd HEVC segments; start_time is
+// relative to the beginning of the first input.
+int encode_clip(const std::vector<std::string> &inputs, const std::string &output,
+                double start_time, double duration, int bitrate = 5'000'000,
+                int speedup = 1, const std::string &metadata = {});
@@ -0,0 +1,390 @@
+#include "system/loggerd/encoder/v4l_decoder.h"
+
+#include <assert.h>
+#include <cerrno>
+#include <climits>
+#include "third_party/linux/include/v4l2-controls.h"
+#include <linux/videodev2.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+
+
+#include "common/swaglog.h"
+#include "common/util.h"
+
+constexpr int OFFLINE_CORE_PLACEMENT_RATE = 80 << 16;
+
+// echo "0xFFFF" > /sys/kernel/debug/msm_vidc/debug_level
+
+static void copyBuffer(VisionBuf *src_buf, VisionBuf *dst_buf) {
+  // Copy Y plane
+  memcpy(dst_buf->y, src_buf->y, src_buf->height * src_buf->stride);
+  // Copy UV plane
+  memcpy(dst_buf->uv, src_buf->uv, src_buf->height / 2 * src_buf->stride);
+}
+
+static void request_buffers(int fd, v4l2_buf_type buf_type, unsigned int count) {
+  struct v4l2_requestbuffers reqbuf = {
+    .count = count,
+    .type = buf_type,
+    .memory = V4L2_MEMORY_USERPTR
+  };
+  util::safe_ioctl(fd, VIDIOC_REQBUFS, &reqbuf, "VIDIOC_REQBUFS failed");
+}
+
+V4LDecoder::~V4LDecoder() {
+  if (fd > 0) {
+    close(fd);
+  }
+}
+
+bool V4LDecoder::init(const char* dev, size_t width, size_t height, uint64_t codec,
+                   bool direct_mode, uint32_t capture_fourcc) {
+  LOG("Initializing msm_vidc device %s", dev);
+  this->w = width;
+  this->h = height;
+  this->direct = direct_mode;
+  this->capture_format = capture_fourcc;
+  this->fd = open(dev, O_RDWR | O_NONBLOCK, 0);
+  if (fd < 0) {
+    LOGE("failed to open video device %s", dev);
+    return false;
+  }
+  subscribeEvents();
+  v4l2_buf_type out_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+  setPlaneFormat(out_type, codec); // Also allocates the output buffers
+  setFPS(FPS);
+  if (direct) {
+    struct v4l2_control ctrls[] = {
+      { .id = V4L2_CID_MPEG_VIDC_VIDEO_OPERATING_RATE, .value = OFFLINE_CORE_PLACEMENT_RATE },
+      { .id = V4L2_CID_MPEG_VIDC_VIDEO_PRIORITY, .value = V4L2_MPEG_VIDC_VIDEO_PRIORITY_REALTIME_ENABLE },
+    };
+    for (auto ctrl : ctrls) {
+      util::safe_ioctl(fd, VIDIOC_S_CTRL, &ctrl, "VIDIOC_S_CTRL offline decode failed");
+    }
+  }
+  request_buffers(fd, out_type, OUTPUT_BUFFER_COUNT);
+  util::safe_ioctl(fd, VIDIOC_STREAMON, &out_type, "VIDIOC_STREAMON OUTPUT failed");
+  restartCapture();
+  pfd = {fd, POLLIN | POLLOUT | POLLWRNORM | POLLRDNORM | POLLPRI, 0};
+
+  this->initialized = true;
+  return true;
+}
+
+VisionBuf* V4LDecoder::decodeFrame(AVPacket *pkt, VisionBuf *buf) {
+  assert(initialized && !direct && pkt != nullptr && buf != nullptr);
+  bool queued = false;
+  while (true) {
+    if (!queued) queued = queuePacket(pkt, 0);
+    V4LDecodedFrame frame;
+    if (!pump(frame, -1)) return nullptr;
+    if (!frame.buf) continue;
+
+    VisionBuf *decoded = frame.buf;
+    copyBuffer(decoded, buf);
+    releaseFrame(decoded);
+    return buf;
+  }
+}
+
+void V4LDecoder::releaseFrame(VisionBuf *buf) {
+  assert(buf >= cap_bufs && buf < cap_bufs + CAPTURE_BUFFER_COUNT);
+  queueCaptureBuffer(buf - cap_bufs);
+}
+
+bool V4LDecoder::queuePacket(const AVPacket *pkt, uint64_t token) {
+  int buf_index = getBufferUnlocked();
+  return buf_index >= 0 && sendPacket(buf_index, pkt, token);
+}
+
+bool V4LDecoder::pump(V4LDecodedFrame &frame, int timeout_ms) {
+  frame = {};
+  int rc;
+  while (true) {
+    rc = poll(&pfd, 1, timeout_ms);
+    if (rc < 0) {
+      if (errno == EINTR) continue;
+      LOGE("poll() error: %d", errno);
+      return false;
+    }
+    break;
+  }
+
+  if (rc == 0) return true;
+
+  int result;
+
+  while ((result = handleEvent()) > 0) {}
+  if (result < 0) return false;
+
+  while ((result = handleOutput()) > 0) {}
+  if (result < 0) return false;
+
+  result = handleCapture(&frame);
+  return result >= 0;
+}
+
+int V4LDecoder::handleCapture(V4LDecodedFrame *frame) {
+  struct v4l2_buffer buf = {0};
+  struct v4l2_plane planes[1] = {0};
+  buf.type          = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+  buf.memory        = V4L2_MEMORY_USERPTR;
+  buf.m.planes      = planes;
+  buf.length        = 1;
+  int err = HANDLE_EINTR(ioctl(this->fd, VIDIOC_DQBUF, &buf));
+  if (err < 0 && errno == EAGAIN) return 0;
+  if (err < 0) {
+    LOGE("VIDIOC_DQBUF CAPTURE failed: %d", errno);
+    return -1;
+  }
+
+  const bool has_payload = buf.m.planes[0].bytesused != 0;
+  const bool eos = (buf.flags & V4L2_QCOM_BUF_FLAG_EOS) != 0;
+
+  frame->buf = nullptr;
+  if (!reconfigure_pending && has_payload) {
+    frame->buf = &cap_bufs[buf.index];
+    frame->token = (uint64_t)buf.timestamp.tv_sec * 1000000ULL + buf.timestamp.tv_usec;
+  } else if (!reconfigure_pending && !eos) {
+    queueCaptureBuffer(buf.index);
+  }
+
+  return 1;
+}
+
+bool V4LDecoder::subscribeEvents() {
+  for (uint32_t event : subscriptions) {
+    struct v4l2_event_subscription sub = { .type = event};
+    util::safe_ioctl(fd, VIDIOC_SUBSCRIBE_EVENT, &sub, "VIDIOC_SUBSCRIBE_EVENT failed");
+  }
+  return true;
+}
+
+bool V4LDecoder::setPlaneFormat(enum v4l2_buf_type type, uint32_t fourcc) {
+  struct v4l2_format fmt = {.type = type};
+  struct v4l2_pix_format_mplane *pix = &fmt.fmt.pix_mp;
+  *pix = {
+    .width = (__u32)this->w,
+    .height = (__u32)this->h,
+    .pixelformat = fourcc
+  };
+  util::safe_ioctl(fd, VIDIOC_S_FMT, &fmt, "VIDIOC_S_FMT failed");
+  if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+    this->out_buf_size = pix->plane_fmt[0].sizeimage;
+    for (int i = 0; i < OUTPUT_BUFFER_COUNT; i++) {
+      this->out_bufs[i].allocate(this->out_buf_size);
+      this->out_buf_flag[i] = false;
+    }
+    LOGD("Set output buffer size to %d, count %d, addr %p", this->out_buf_size, OUTPUT_BUFFER_COUNT, this->out_bufs[0].addr);
+  } else if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+    request_buffers(this->fd, type, CAPTURE_BUFFER_COUNT);
+    util::safe_ioctl(fd, VIDIOC_G_FMT, &fmt, "VIDIOC_G_FMT failed");
+    const __u32 y_size    = pix->plane_fmt[0].sizeimage;
+    const __u32 y_stride  = pix->plane_fmt[0].bytesperline;
+    for (size_t i = 0; i < CAPTURE_BUFFER_COUNT; i++) {
+      size_t uv_offset = (size_t)y_stride * pix->height;
+      size_t required = uv_offset + (y_stride * pix->height / 2); // enough for Y + UV. For linear NV12, UV plane starts at y_stride * height.
+      size_t alloc_size = std::max<size_t>(y_size, required);
+      this->cap_bufs[i].allocate(alloc_size);
+      this->cap_bufs[i].init_yuv(pix->width, pix->height, y_stride, uv_offset);
+    }
+    LOGD("Set capture buffer size to %d, count %d, addr %p, extradata size %d",
+      pix->plane_fmt[0].sizeimage, CAPTURE_BUFFER_COUNT, this->cap_bufs[0].addr, pix->plane_fmt[1].sizeimage);
+  }
+  return true;
+}
+
+bool V4LDecoder::setFPS(uint32_t fps) {
+  struct v4l2_streamparm streamparam = {
+    .type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+  };
+  streamparam.parm.output.timeperframe = {1, fps};
+  util::safe_ioctl(fd, VIDIOC_S_PARM, &streamparam, "VIDIOC_S_PARM failed");
+  return true;
+}
+
+bool V4LDecoder::restartCapture() {
+  // stop if already initialized
+  enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+  if (this->initialized) {
+    LOGD("Restarting capture, flushing buffers...");
+    util::safe_ioctl(this->fd, VIDIOC_STREAMOFF, &type, "VIDIOC_STREAMOFF CAPTURE failed");
+    struct v4l2_requestbuffers reqbuf = {.type = type, .memory = V4L2_MEMORY_USERPTR};
+    util::safe_ioctl(this->fd, VIDIOC_REQBUFS, &reqbuf, "VIDIOC_REQBUFS failed");
+    for (size_t i = 0; i < CAPTURE_BUFFER_COUNT; ++i) {
+      this->cap_bufs[i].free();
+      cap_bufs[i].~VisionBuf();
+      new (&cap_bufs[i]) VisionBuf();
+    }
+  }
+  // setup, start and queue capture buffers
+  setDBP();
+  setPlaneFormat(type, capture_format);
+  if (direct) {
+    struct v4l2_control ctrl = {
+      .id = V4L2_CID_MPEG_VIDC_VIDEO_OPERATING_RATE,
+      .value = OFFLINE_CORE_PLACEMENT_RATE,
+    };
+    util::safe_ioctl(fd, VIDIOC_S_CTRL, &ctrl, "VIDIOC_S_CTRL placement decode failed");
+  }
+  util::safe_ioctl(this->fd, VIDIOC_STREAMON, &type, "VIDIOC_STREAMON CAPTURE failed");
+  for (size_t i = 0; i < CAPTURE_BUFFER_COUNT; ++i) {
+    queueCaptureBuffer(i);
+  }
+  if (direct) {
+    struct v4l2_control ctrl = {
+      .id = V4L2_CID_MPEG_VIDC_VIDEO_OPERATING_RATE,
+      .value = INT_MAX,
+    };
+    util::safe_ioctl(fd, VIDIOC_S_CTRL, &ctrl, "VIDIOC_S_CTRL turbo decode failed");
+  }
+
+  return true;
+}
+
+bool V4LDecoder::queueCaptureBuffer(int i) {
+  struct v4l2_buffer buf = {0};
+  struct v4l2_plane planes[1] = {0};
+
+  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+  buf.memory = V4L2_MEMORY_USERPTR;
+  buf.index = i;
+  buf.m.planes = planes;
+  buf.length = 1;
+  // decoded frame plane
+  planes[0].m.userptr     = (unsigned long)this->cap_bufs[i].addr; // no security
+  planes[0].length        = this->cap_bufs[i].len;
+  planes[0].reserved[0]   = this->cap_bufs[i].fd; // ION fd
+  planes[0].reserved[1]   = 0;
+  planes[0].bytesused     = this->cap_bufs[i].len;
+  planes[0].data_offset   = 0;
+  util::safe_ioctl(this->fd, VIDIOC_QBUF, &buf, "VIDIOC_QBUF failed");
+  return true;
+}
+
+bool V4LDecoder::queueOutputBuffer(int i, size_t size, uint64_t token) {
+  struct v4l2_buffer buf = {0};
+  struct v4l2_plane planes[1] = {0};
+
+  buf.type                = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+  buf.memory              = V4L2_MEMORY_USERPTR;
+  buf.index               = i;
+  buf.flags               = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+  buf.timestamp.tv_sec    = token / 1000000ULL;
+  buf.timestamp.tv_usec   = token % 1000000ULL;
+  buf.m.planes            = planes;
+  buf.length              = 1;
+  // decoded frame plane
+  planes[0].m.userptr     = (unsigned long)this->out_bufs[i].addr;
+  planes[0].length        = this->out_buf_size;
+  planes[0].reserved[0]   = this->out_bufs[i].fd; // ION fd
+  planes[0].reserved[1]   = 0;
+  planes[0].bytesused     = size;
+  planes[0].data_offset   = 0;
+  assert(this->out_buf_size % 4096 == 0);               // ditto for size
+
+  util::safe_ioctl(this->fd, VIDIOC_QBUF, &buf, "VIDIOC_QBUF failed");
+  this->out_buf_flag[i] = true; // mark as queued
+  return true;
+}
+
+bool V4LDecoder::setDBP() {
+  struct v4l2_ext_control control[2] = {0};
+  struct v4l2_ext_controls controls = {0};
+  control[0].id           = V4L2_CID_MPEG_VIDC_VIDEO_STREAM_OUTPUT_MODE;
+  control[0].value        = 1; // V4L2_CID_MPEG_VIDC_VIDEO_STREAM_OUTPUT_SECONDARY
+  control[1].id           = V4L2_CID_MPEG_VIDC_VIDEO_DPB_COLOR_FORMAT;
+  control[1].value        = 0; // V4L2_MPEG_VIDC_VIDEO_DPB_COLOR_FMT_NONE
+  controls.count          = 2;
+  controls.ctrl_class     = V4L2_CTRL_CLASS_MPEG;
+  controls.controls       = control;
+  util::safe_ioctl(fd, VIDIOC_S_EXT_CTRLS, &controls, "VIDIOC_S_EXT_CTRLS failed");
+  return true;
+}
+
+bool V4LDecoder::sendPacket(int buf_index, const AVPacket *pkt, uint64_t token) {
+  assert(buf_index >= 0 && buf_index < OUTPUT_BUFFER_COUNT);
+  assert(pkt != nullptr && pkt->data != nullptr && pkt->size > 0);
+  assert((size_t)pkt->size <= (size_t)this->out_buf_size);
+  // Prepare output buffer
+  uint8_t * data = (uint8_t *)this->out_bufs[buf_index].addr;
+  memcpy(data, pkt->data, pkt->size);
+  queueOutputBuffer(buf_index, pkt->size, token);
+  return true;
+}
+
+int V4LDecoder::getBufferUnlocked() {
+  for (int i = 0; i < OUTPUT_BUFFER_COUNT; i++) {
+    if (!out_buf_flag[i]) {
+      return i;
+    }
+  }
+  return -1;
+}
+
+
+int V4LDecoder::handleOutput() {
+  struct v4l2_buffer buf = {0};
+  struct v4l2_plane planes[1];
+  buf.type      = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+  buf.memory    = V4L2_MEMORY_USERPTR;
+  buf.m.planes  = planes;
+  buf.length    = 1;
+  int err = HANDLE_EINTR(ioctl(this->fd, VIDIOC_DQBUF, &buf));
+  if (err < 0 && errno == EAGAIN) return 0;
+  if (err < 0) {
+    LOGE("VIDIOC_DQBUF OUTPUT failed: %d", errno);
+    return -1;
+  }
+  this->out_buf_flag[buf.index] = false; // mark as not queued
+  return 1;
+}
+
+int V4LDecoder::handleEvent() {
+  // dequeue event
+  struct v4l2_event event = {0};
+  int err = HANDLE_EINTR(ioctl(this->fd, VIDIOC_DQEVENT, &event));
+  if (err < 0 && (errno == EAGAIN || errno == ENOENT)) return 0;
+  if (err < 0) {
+    LOGE("VIDIOC_DQEVENT failed: %d", errno);
+    return -1;
+  }
+  switch (event.type) {
+    case V4L2_EVENT_MSM_VIDC_PORT_SETTINGS_CHANGED_INSUFFICIENT: {
+      unsigned int *ptr     = (unsigned int *)event.u.data;
+      unsigned int height   = ptr[0];
+      unsigned int width    = ptr[1];
+      this->w               = width;
+      this->h               = height;
+      LOGD("Port Reconfig received insufficient, new size %ux%u, flushing capture bufs...", width, height); // This is normal
+      struct v4l2_decoder_cmd dec;
+      dec.flags = V4L2_QCOM_CMD_FLUSH_CAPTURE;
+      dec.cmd = V4L2_QCOM_CMD_FLUSH;
+      util::safe_ioctl(this->fd, VIDIOC_DECODER_CMD, &dec, "VIDIOC_DECODER_CMD FLUSH_CAPTURE failed");
+      this->reconfigure_pending = true;
+      LOGD("Waiting for flush done event to reconfigure capture queue");
+      break;
+    }
+
+    case V4L2_EVENT_MSM_VIDC_FLUSH_DONE: {
+      unsigned int *ptr   = (unsigned int *)event.u.data;
+      unsigned int flags  = ptr[0];
+      if (flags & V4L2_QCOM_CMD_FLUSH_CAPTURE) {
+        if (this->reconfigure_pending) {
+          this->restartCapture();
+          this->reconfigure_pending = false;
+        }
+      }
+      break;
+    }
+    default:
+      break;
+  }
+  return 1;
+}
+
+void V4LDecoder::sendEOS() {
+  struct v4l2_decoder_cmd command = { .cmd = V4L2_DEC_CMD_STOP };
+  util::safe_ioctl(fd, VIDIOC_DECODER_CMD, &command, "VIDIOC_DECODER_CMD STOP failed");
+}
@@ -0,0 +1,105 @@
+#pragma once
+
+#include <linux/videodev2.h>
+#include <poll.h>
+
+#include "msgq/visionipc/visionbuf.h"
+
+extern "C" {
+  #include <libavcodec/avcodec.h>
+  #include <libavformat/avformat.h>
+}
+
+#define V4L2_EVENT_MSM_VIDC_START (V4L2_EVENT_PRIVATE_START + 0x00001000)
+#define V4L2_EVENT_MSM_VIDC_FLUSH_DONE (V4L2_EVENT_MSM_VIDC_START + 1)
+#define V4L2_EVENT_MSM_VIDC_PORT_SETTINGS_CHANGED_INSUFFICIENT (V4L2_EVENT_MSM_VIDC_START + 3)
+#ifndef V4L2_CID_MPEG_MSM_VIDC_BASE
+#define V4L2_CID_MPEG_MSM_VIDC_BASE 0x00992000
+#endif
+#ifndef V4L2_CID_MPEG_VIDC_VIDEO_DPB_COLOR_FORMAT
+#define V4L2_CID_MPEG_VIDC_VIDEO_DPB_COLOR_FORMAT (V4L2_CID_MPEG_MSM_VIDC_BASE + 44)
+#endif
+#ifndef V4L2_CID_MPEG_VIDC_VIDEO_STREAM_OUTPUT_MODE
+#define V4L2_CID_MPEG_VIDC_VIDEO_STREAM_OUTPUT_MODE (V4L2_CID_MPEG_MSM_VIDC_BASE + 22)
+#endif
+#ifndef V4L2_PIX_FMT_NV12_UBWC
+#define V4L2_PIX_FMT_NV12_UBWC v4l2_fourcc('Q', '1', '2', '8')
+#endif
+#ifndef V4L2_CID_MPEG_VIDC_VIDEO_PRIORITY
+#define V4L2_CID_MPEG_VIDC_VIDEO_PRIORITY (V4L2_CID_MPEG_MSM_VIDC_BASE + 52)
+#define V4L2_MPEG_VIDC_VIDEO_PRIORITY_REALTIME_ENABLE 0
+#define V4L2_MPEG_VIDC_VIDEO_PRIORITY_REALTIME_DISABLE 1
+#endif
+#ifndef V4L2_CID_MPEG_VIDC_VIDEO_OPERATING_RATE
+#define V4L2_CID_MPEG_VIDC_VIDEO_OPERATING_RATE (V4L2_CID_MPEG_MSM_VIDC_BASE + 53)
+#endif
+#define V4L2_QCOM_CMD_FLUSH_CAPTURE (1 << 1)
+#define V4L2_QCOM_CMD_FLUSH (4)
+#ifndef V4L2_QCOM_BUF_FLAG_EOS
+#define V4L2_QCOM_BUF_FLAG_EOS 0x02000000
+#endif
+
+#define OUTPUT_BUFFER_COUNT 	8
+#define CAPTURE_BUFFER_COUNT 	16
+#define FPS 									20
+
+struct V4LDecodedFrame {
+  VisionBuf *buf = nullptr;
+  uint64_t token = 0;
+};
+
+class V4LDecoder {
+public:
+  static constexpr const char *DEVICE = "/dev/video32";
+
+  V4LDecoder() = default;
+  ~V4LDecoder();
+
+  bool init(const char* dev, size_t width, size_t height, uint64_t codec,
+            bool direct_mode = false, uint32_t capture_fourcc = V4L2_PIX_FMT_NV12);
+  VisionBuf* decodeFrame(AVPacket* pkt, VisionBuf* buf);
+  // queuePacket() and pump() are single-threaded. releaseFrame() may be called
+  // from a consumer thread after a direct capture surface is no longer needed.
+  bool queuePacket(const AVPacket *pkt, uint64_t token);
+  bool pump(V4LDecodedFrame &frame, int timeout_ms);
+  void releaseFrame(VisionBuf *buf);
+  void sendEOS();
+  size_t maxPacketSize() const { return out_buf_size; }
+
+  AVFormatContext* avctx = nullptr;
+  int fd = 0;
+
+private:
+  bool initialized = false;
+  bool reconfigure_pending = false;
+  bool direct = false;
+  uint32_t capture_format = V4L2_PIX_FMT_NV12;
+
+  VisionBuf out_bufs[OUTPUT_BUFFER_COUNT];    // Distinct dma-buf per in-flight packet
+  VisionBuf cap_bufs[CAPTURE_BUFFER_COUNT];   // Capture (output) buffers
+
+  size_t w = 0, h = 0;
+  int out_buf_size = 0;
+
+  bool out_buf_flag[OUTPUT_BUFFER_COUNT] = {false};
+
+  const int subscriptions[2] = {
+    V4L2_EVENT_MSM_VIDC_FLUSH_DONE,
+    V4L2_EVENT_MSM_VIDC_PORT_SETTINGS_CHANGED_INSUFFICIENT
+  };
+
+  struct pollfd pfd = {};
+
+  bool subscribeEvents();
+  bool setPlaneFormat(v4l2_buf_type type, uint32_t fourcc);
+  bool setFPS(uint32_t fps);
+  bool restartCapture();
+  bool queueCaptureBuffer(int i);
+  bool queueOutputBuffer(int i, size_t size, uint64_t token);
+  bool setDBP();
+  bool sendPacket(int buf_index, const AVPacket* pkt, uint64_t token);
+  int getBufferUnlocked();
+  int handleCapture(V4LDecodedFrame *frame);
+  int handleOutput();
+  int handleEvent();
+};
@@ -2,6 +2,7 @@
 #include <string>
 #include <sys/ioctl.h>
 #include <poll.h>
+#include <utility>

 #include "system/loggerd/encoder/v4l_encoder.h"
 #include "common/util.h"
@@ -119,12 +120,17 @@ void V4LEncoder::dequeue_handler(V4LEncoder *e) {
      } else if (flags & V4L2_QCOM_BUF_FLAG_CODECCONFIG) {
        // save header
        header = kj::heapArray<capnp::byte>(buf, bytesused);
+        if (e->packet_callback) e->packet_callback(header.begin(), header.size(), ts, true, false);
      } else {
        VisionIpcBufExtra extra = e->extras.pop();
        assert(extra.timestamp_eof/1000 == ts); // stay in sync
        frame_id = extra.frame_id;
        ++idx;
-        e->publisher_publish(e->segment_num, idx, extra, flags, header, kj::arrayPtr<capnp::byte>(buf, bytesused));
+        if (e->packet_callback) {
+          e->packet_callback(buf, bytesused, ts, false, flags & V4L2_BUF_FLAG_KEYFRAME);
+        } else {
+          e->publisher_publish(e->segment_num, idx, extra, flags, header, kj::arrayPtr<capnp::byte>(buf, bytesused));
+        }
      }

      if (env_debug_encoder) {
@@ -139,13 +145,19 @@ void V4LEncoder::dequeue_handler(V4LEncoder *e) {
    if (pfd.revents & POLLOUT) {
      unsigned int index;
      dequeue_buffer(e->fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, &index);
+      VisionBuf *input_buf = e->input_bufs[index].exchange(nullptr);
+      if (input_buf && e->input_done_callback) e->input_done_callback(input_buf);
      e->free_buf_in.push(index);
    }
  }
 }

 V4LEncoder::V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_height)
-    : VideoEncoder(encoder_info, in_width, in_height) {
+    : V4LEncoder(encoder_info, in_width, in_height, Options{}) {}
+
+V4LEncoder::V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_height, Options options)
+    : VideoEncoder(encoder_info, in_width, in_height), packet_callback(std::move(options.packet_callback)),
+      input_done_callback(std::move(options.input_done_callback)) {
  fd = HANDLE_EINTR(open("/dev/v4l/by-path/platform-aa00000.qcom_vidc-video-index1", O_RDWR|O_NONBLOCK));
  assert(fd >= 0);

@@ -194,7 +206,7 @@ V4LEncoder::V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_hei
      .pix_mp = {
        .width = (unsigned int)in_width,
        .height = (unsigned int)in_height,
-        .pixelformat = V4L2_PIX_FMT_NV12,
+        .pixelformat = options.input_format,
        .field = V4L2_FIELD_ANY,
        .colorspace = V4L2_COLORSPACE_470_SYSTEM_BG,
      }
@@ -221,6 +233,13 @@ V4LEncoder::V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_hei
      util::safe_ioctl(fd, VIDIOC_S_CTRL, &ctrl, "VIDIOC_S_CTRL failed");
    }
  }
+  if (options.max_performance) {
+    struct v4l2_control ctrl = {
+      .id = V4L2_CID_MPEG_VIDC_VIDEO_PRIORITY,
+      .value = V4L2_MPEG_VIDC_VIDEO_PRIORITY_REALTIME_ENABLE,
+    };
+    util::safe_ioctl(fd, VIDIOC_S_CTRL, &ctrl, "VIDIOC_S_CTRL offline encode failed");
+  }

  if (is_h265) {
    struct v4l2_control ctrls[] = {
@@ -282,6 +301,7 @@ int V4LEncoder::encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) {

  // reserve buffer
  int buffer_in = free_buf_in.pop();
+  input_bufs[buffer_in].store(buf);

  // push buffer
  extras.push(*extra);
@@ -1,14 +1,27 @@
 #pragma once

+#include <atomic>
+#include <functional>
+
 #include "common/queue.h"
 #include "system/loggerd/encoder/encoder.h"

-#define BUF_IN_COUNT 7
+#define BUF_IN_COUNT 9
 #define BUF_OUT_COUNT 6

 class V4LEncoder : public VideoEncoder {
 public:
+  using PacketCallback = std::function<void(uint8_t *, size_t, int64_t, bool, bool)>;
+  using InputDoneCallback = std::function<void(VisionBuf *)>;
+  struct Options {
+    PacketCallback packet_callback;
+    uint32_t input_format = V4L2_PIX_FMT_NV12;
+    InputDoneCallback input_done_callback;
+    bool max_performance = false;
+  };
+
  V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_height);
+  V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_height, Options options);
  ~V4LEncoder();
  int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra);
  void encoder_open();
@@ -25,10 +38,13 @@ private:
  int current_bitrate = -1;

  SafeQueue<VisionIpcBufExtra> extras;
+  PacketCallback packet_callback;
+  InputDoneCallback input_done_callback;

  static void dequeue_handler(V4LEncoder *e);
  std::thread dequeue_handler_thread;

  VisionBuf buf_out[BUF_OUT_COUNT];
+  std::atomic<VisionBuf *> input_bufs[BUF_IN_COUNT] = {};
  SafeQueue<unsigned int> free_buf_in;
 };
@@ -1,5 +1,12 @@
 #include <cassert>
+#ifdef __TICI__
+#include <exception>
+#include <stdexcept>
+#endif

+#ifdef __TICI__
+#include "system/loggerd/clip_encoder.h"
+#endif
 #include "system/loggerd/loggerd.h"
 #include "system/loggerd/encoder/jpeg_encoder.h"

@@ -170,6 +177,37 @@ void encoderd_thread(const LogCameraInfo (&cameras)[N]) {
 }

 int main(int argc, char* argv[]) {
+#ifdef __TICI__
+  if (argc > 1 && std::string(argv[1]) == "--clip") {
+    if (argc < 6) {
+      fprintf(stderr, "usage: encoderd --clip OUTPUT START DURATION [--bitrate BPS] [--speedup N] "
+                      "[--metadata JSON] SEGMENT [SEGMENT ...]\n");
+      return 2;
+    }
+    try {
+      int bitrate = 5'000'000;
+      int speedup = 1;
+      std::string metadata;
+      int input_arg = 5;
+      while (input_arg < argc && std::string(argv[input_arg]).rfind("--", 0) == 0) {
+        const std::string option = argv[input_arg++];
+        if (option == "--") break;
+        if (input_arg == argc) throw std::invalid_argument("missing clip option value");
+        if (option == "--bitrate") bitrate = std::stoi(argv[input_arg++]);
+        else if (option == "--speedup") speedup = std::stoi(argv[input_arg++]);
+        else if (option == "--metadata") metadata = argv[input_arg++];
+        else throw std::invalid_argument("unknown clip option: " + option);
+      }
+      if (input_arg == argc) throw std::invalid_argument("missing clip input");
+      std::vector<std::string> inputs(argv + input_arg, argv + argc);
+      return encode_clip(inputs, argv[2], std::stod(argv[3]), std::stod(argv[4]),
+                         bitrate, speedup, metadata);
+    } catch (const std::exception &e) {
+      fprintf(stderr, "clip encoding failed: %s\n", e.what());
+      return 1;
+    }
+  }
+#endif
  if (!Hardware::PC()) {
    int ret;
    ret = util::set_realtime_priority(52);
@@ -50,6 +50,11 @@ VideoWriter::VideoWriter(const char *path, const char *filename, bool remuxing,
  }
 }

+void VideoWriter::set_metadata(const char *key, const char *value) {
+  assert(remuxing && !header_written);
+  av_dict_set(&ofmt_ctx->metadata, key, value, 0);
+}
+
 void VideoWriter::initialize_audio(int sample_rate) {
  assert(this->ofmt_ctx->oformat->audio_codec != AV_CODEC_ID_NONE); // check output format supports audio streams
  const AVCodec *audio_avcodec = avcodec_find_encoder(AV_CODEC_ID_AAC);
@@ -110,7 +115,10 @@ void VideoWriter::write(uint8_t *data, int len, long long timestamp, bool codecc
      int err = avcodec_parameters_from_context(out_stream->codecpar, codec_ctx);
      assert(err >= 0);
      // if there is an audio stream, it must be initialized before this point
-      err = avformat_write_header(ofmt_ctx, NULL);
+      AVDictionary *options = nullptr;
+      if (ofmt_ctx->metadata) av_dict_set(&options, "movflags", "+faststart+use_metadata_tags", 0);
+      err = avformat_write_header(ofmt_ctx, &options);
+      av_dict_free(&options);
      assert(err >= 0);
      header_written = true;
    } else {
@@ -13,6 +13,7 @@ extern "C" {
 class VideoWriter {
 public:
  VideoWriter(const char *path, const char *filename, bool remuxing, int width, int height, int fps, cereal::EncodeIndex::Type codec);
+  void set_metadata(const char *key, const char *value);
  void write(uint8_t *data, int len, long long timestamp, bool codecconfig, bool keyframe);
  void write_audio(uint8_t *data, int len, long long timestamp, int sample_rate);

@@ -32,20 +32,23 @@ def ublox_available() -> bool:
  return os.path.exists('/dev/ttyHS0') and not os.path.exists('/persist/comma/use-quectel-gps')


-def update_car_gps_param(params: Params, CP: car.CarParams) -> bool:
-  available = car_gps_available(CP)
+def update_car_gps_param(params: Params) -> bool | None:
+  car_params = params.get("CarParams")
+  if car_params is None:
+    return None
+
+  with car.CarParams.from_bytes(car_params) as CP:
+    available = car_gps_available(CP)
  if available != params.get_bool("CarGpsAvailable"):
    params.put_bool("CarGpsAvailable", available)
  return available

 def ublox(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool:
-  car_gps = update_car_gps_param(params, CP)
+  car_gps = update_car_gps_param(params)
  use_ublox = ublox_available()
  if use_ublox != params.get_bool("UbloxAvailable"):
    params.put_bool("UbloxAvailable", use_ublox)
-  # The Mach-E's CAN GPS is the preferred external source for now. Do not let
-  # the no-fix comma GNSS publisher race it on gpsLocationExternal.
-  return started and use_ublox and not car_gps
+  return started and use_ublox and car_gps is False

 def joystick(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool:
  return started and params.get_bool("JoystickDebugMode")
@@ -63,7 +66,7 @@ def not_long_maneuver(started: bool, params: Params, CP: car.CarParams, starpilo
  return started and not params.get_bool("LongitudinalManeuverMode")

 def qcomgps(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool:
-  update_car_gps_param(params, CP)
+  update_car_gps_param(params)
  return started and not ublox_available()

 def always_run(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool:
@@ -3,7 +3,8 @@ from types import SimpleNamespace
 import pytest

 from cereal import car
-from openpilot.system.manager.process_config import allow_uploads, camera_run, managed_processes, sentry_mode
+from opendbc.car.ford.values import CAR as FORD_CAR
+from openpilot.system.manager.process_config import allow_uploads, camera_run, managed_processes, sentry_mode, ublox


 class FakeParams:
@@ -71,3 +72,43 @@ class SentryParams:
@pytest.mark.parametrize("started,enabled,expected", [(True, True, False), (False, True, True), (False, False, False)])
 def test_sentry_process_is_offroad_only(started, enabled, expected):
  assert sentry_mode(started, SentryParams(enabled), car.CarParams.new_message(), SimpleNamespace()) is expected
+
+
+class GpsParams:
+  def __init__(self, CP=None):
+    self.values = {}
+    if CP is not None:
+      self.values["CarParams"] = CP.to_bytes()
+
+  def get(self, key: str):
+    return self.values.get(key)
+
+  def get_bool(self, key: str) -> bool:
+    return bool(self.values.get(key, False))
+
+  def put_bool(self, key: str, value: bool):
+    self.values[key] = value
+
+
+def test_ublox_waits_for_current_carparams(monkeypatch):
+  monkeypatch.setattr("openpilot.system.manager.process_config.ublox_available", lambda: True)
+  params = GpsParams()
+
+  assert not ublox(True, params, car.CarParams.new_message(), SimpleNamespace())
+  assert params.get_bool("UbloxAvailable")
+
+
+@pytest.mark.parametrize("car_gps,expected", [(False, True), (True, False)])
+def test_ublox_has_single_external_gps_publisher(monkeypatch, car_gps, expected):
+  monkeypatch.setattr("openpilot.system.manager.process_config.ublox_available", lambda: True)
+  CP = car.CarParams.new_message()
+  if car_gps:
+    CP.brand = "ford"
+    CP.carFingerprint = FORD_CAR.FORD_MUSTANG_MACH_E_MK1
+  else:
+    CP.brand = "mock"
+    CP.carFingerprint = "mock"
+  params = GpsParams(CP)
+
+  assert ublox(True, params, car.CarParams.new_message(), SimpleNamespace()) is expected
+  assert params.get_bool("CarGpsAvailable") is car_gps
@@ -353,6 +353,7 @@ StoppingDecelRate
 StoppingDecelRateStock
 SubaruSNG
 SubaruSNGManualParkingBrake
+SubaruStopStartOff
 SwitchbackModeCooldown
 SwitchbackModeEnabled
 TacoTune