Merge remote-tracking branch 'comma/master' into sync-20250309

# Conflicts:
#	.github/workflows/ui_preview.yaml
#	common/params_keys.h
#	msgq_repo
#	opendbc_repo
#	panda
#	selfdrive/ui/qt/offroad/firehose.h
#	selfdrive/ui/tests/test_ui/run.py
#	system/manager/process_config.py

system/athena/athenad.py

@@ -523,10 +523,6 @@ def getSshAuthorizedKeys() -> str:
 def getGithubUsername() -> str:
   return Params().get("GithubUsername", encoding='utf8') or ''
 
-@dispatcher.add_method
-def getFirehoseMode() -> bool:
-  return Params().get_bool("FirehoseMode") or False
-
 @dispatcher.add_method
 def getSimInfo():
   return HARDWARE.get_sim_info()

system/camerad/cameras/camera_common.cc

@@ -15,7 +15,7 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, SpectraCamera *
   const SensorInfo *sensor = cam->sensor.get();
 
   // RAW frames from ISP
-  if (cam->output_type != ISP_IFE_PROCESSED) {
+  if (cam->cc.output_type != ISP_IFE_PROCESSED) {
     camera_bufs_raw = std::make_unique<VisionBuf[]>(frame_buf_count);
 
     const int raw_frame_size = (sensor->frame_height + sensor->extra_height) * sensor->frame_stride;

system/camerad/cameras/camera_qcom2.cc

@@ -55,7 +55,7 @@ public:
   float fl_pix = 0;
   std::unique_ptr<PubMaster> pm;
 
-  CameraState(SpectraMaster *master, const CameraConfig &config) : camera(master, config, config.stream_type == VISION_STREAM_DRIVER ? ISP_BPS_PROCESSED : ISP_IFE_PROCESSED) {};
+  CameraState(SpectraMaster *master, const CameraConfig &config) : camera(master, config) {};
   ~CameraState();
   void init(VisionIpcServer *v, cl_device_id device_id, cl_context ctx);
   void update_exposure_score(float desired_ev, int exp_t, int exp_g_idx, float exp_gain);
@@ -268,7 +268,7 @@ void camerad_thread() {
 
   // *** per-cam init ***
   std::vector<std::unique_ptr<CameraState>> cams;
-  for (const auto &config : {WIDE_ROAD_CAMERA_CONFIG, ROAD_CAMERA_CONFIG, DRIVER_CAMERA_CONFIG}) {
+  for (const auto &config : ALL_CAMERA_CONFIGS) {
     auto cam = std::make_unique<CameraState>(&m, config);
     cam->init(&v, device_id, ctx);
     cams.emplace_back(std::move(cam));

system/camerad/cameras/hw.h

@@ -6,6 +6,13 @@
 
 #include "media/cam_isp_ife.h"
 
+
+typedef enum {
+  ISP_RAW_OUTPUT,   // raw frame from sensor
+  ISP_IFE_PROCESSED,  // fully processed image through the IFE
+  ISP_BPS_PROCESSED,  // fully processed image through the BPS
+} SpectraOutputType;
+
 // For the comma 3/3X three camera platform
 
 struct CameraConfig {
@@ -17,6 +24,7 @@ struct CameraConfig {
   bool enabled;
   uint32_t phy;
   bool vignetting_correction;
+  SpectraOutputType output_type;
 };
 
 // NOTE: to be able to disable road and wide road, we still have to configure the sensor over i2c
@@ -30,6 +38,7 @@ const CameraConfig WIDE_ROAD_CAMERA_CONFIG = {
   .enabled = !getenv("DISABLE_WIDE_ROAD"),
   .phy = CAM_ISP_IFE_IN_RES_PHY_0,
   .vignetting_correction = false,
+  .output_type = ISP_IFE_PROCESSED,
 };
 
 const CameraConfig ROAD_CAMERA_CONFIG = {
@@ -41,6 +50,7 @@ const CameraConfig ROAD_CAMERA_CONFIG = {
   .enabled = !getenv("DISABLE_ROAD"),
   .phy = CAM_ISP_IFE_IN_RES_PHY_1,
   .vignetting_correction = true,
+  .output_type = ISP_IFE_PROCESSED,
 };
 
 const CameraConfig DRIVER_CAMERA_CONFIG = {
@@ -52,6 +62,7 @@ const CameraConfig DRIVER_CAMERA_CONFIG = {
   .enabled = !getenv("DISABLE_DRIVER"),
   .phy = CAM_ISP_IFE_IN_RES_PHY_2,
   .vignetting_correction = false,
+  .output_type = ISP_BPS_PROCESSED,
 };
 
 const CameraConfig ALL_CAMERA_CONFIGS[] = {WIDE_ROAD_CAMERA_CONFIG, ROAD_CAMERA_CONFIG, DRIVER_CAMERA_CONFIG};

system/camerad/cameras/spectra.cc

@@ -233,12 +233,11 @@ void SpectraMaster::init() {
 
 // *** SpectraCamera ***
 
-SpectraCamera::SpectraCamera(SpectraMaster *master, const CameraConfig &config, SpectraOutputType out)
+SpectraCamera::SpectraCamera(SpectraMaster *master, const CameraConfig &config)
   : m(master),
     enabled(config.enabled),
-    cc(config),
-    output_type(out) {
-  ife_buf_depth = (out == ISP_RAW_OUTPUT) ? 4 : VIPC_BUFFER_COUNT;
+    cc(config) {
+  ife_buf_depth = (cc.output_type == ISP_RAW_OUTPUT) ? 4 : VIPC_BUFFER_COUNT;
   assert(ife_buf_depth < MAX_IFE_BUFS);
 }
 
@@ -249,13 +248,7 @@ SpectraCamera::~SpectraCamera() {
 }
 
 int SpectraCamera::clear_req_queue() {
-  struct cam_req_mgr_flush_info req_mgr_flush_request = {0};
-  req_mgr_flush_request.session_hdl = session_handle;
-  req_mgr_flush_request.link_hdl = link_handle;
-  req_mgr_flush_request.flush_type = CAM_REQ_MGR_FLUSH_TYPE_ALL;
-  int ret = do_cam_control(m->video0_fd, CAM_REQ_MGR_FLUSH_REQ, &req_mgr_flush_request, sizeof(req_mgr_flush_request));
-  LOGD("flushed all req: %d", ret);
-
+  // for "non-realtime" BPS
   if (icp_dev_handle > 0) {
     struct cam_flush_dev_cmd cmd = {
       .session_handle = session_handle,
@@ -264,8 +257,17 @@ int SpectraCamera::clear_req_queue() {
     };
     int err = do_cam_control(m->icp_fd, CAM_FLUSH_REQ, &cmd, sizeof(cmd));
     assert(err == 0);
+    LOGD("flushed bps: %d", err);
   }
 
+  // for "realtime" devices
+  struct cam_req_mgr_flush_info req_mgr_flush_request = {0};
+  req_mgr_flush_request.session_hdl = session_handle;
+  req_mgr_flush_request.link_hdl = link_handle;
+  req_mgr_flush_request.flush_type = CAM_REQ_MGR_FLUSH_TYPE_ALL;
+  int ret = do_cam_control(m->video0_fd, CAM_REQ_MGR_FLUSH_REQ, &req_mgr_flush_request, sizeof(req_mgr_flush_request));
+  LOGD("flushed all req: %d", ret);
+
   for (int i = 0; i < MAX_IFE_BUFS; ++i) {
     destroySyncObjectAt(i);
   }
@@ -287,7 +289,7 @@ void SpectraCamera::camera_open(VisionIpcServer *v, cl_device_id device_id, cl_c
   uv_height = VENUS_UV_SCANLINES(COLOR_FMT_NV12, sensor->frame_height);
   uv_offset = stride*y_height;
   yuv_size = uv_offset + stride*uv_height;
-  if (output_type != ISP_RAW_OUTPUT) {
+  if (cc.output_type != ISP_RAW_OUTPUT) {
     uv_offset = ALIGNED_SIZE(uv_offset, 0x1000);
     yuv_size = uv_offset + ALIGNED_SIZE(stride*uv_height, 0x1000);
   }
@@ -296,21 +298,14 @@ void SpectraCamera::camera_open(VisionIpcServer *v, cl_device_id device_id, cl_c
 
   open = true;
   configISP();
-  if (output_type == ISP_BPS_PROCESSED) configICP();
+  if (cc.output_type == ISP_BPS_PROCESSED) configICP();
   configCSIPHY();
   linkDevices();
 
   LOGD("camera init %d", cc.camera_num);
   buf.init(device_id, ctx, this, v, ife_buf_depth, cc.stream_type);
   camera_map_bufs();
-  enqueue_req_multi(1, ife_buf_depth);
-}
-
-void SpectraCamera::enqueue_req_multi(uint64_t start, int n) {
-  for (uint64_t request_id = start; request_id < start + n; ++request_id) {
-    uint64_t idx = (request_id - 1) % ife_buf_depth;
-    enqueue_buffer(idx, request_id);
-  }
+  clearAndRequeue(1);
 }
 
 void SpectraCamera::sensors_start() {
@@ -740,7 +735,7 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) {
     buf_desc[0].offset = ife_cmd.aligned_size()*idx;
 
     // stream of IFE register writes
-    bool is_raw = output_type != ISP_IFE_PROCESSED;
+    bool is_raw = cc.output_type != ISP_IFE_PROCESSED;
     if (!is_raw) {
       if (init) {
         buf_desc[0].length = build_initial_config((unsigned char*)ife_cmd.ptr + buf_desc[0].offset, cc, sensor.get(), patches);
@@ -829,7 +824,7 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) {
     pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf;
 
     struct cam_buf_io_cfg *io_cfg = (struct cam_buf_io_cfg *)((char*)&pkt->payload + pkt->io_configs_offset);
-    if (output_type != ISP_IFE_PROCESSED) {
+    if (cc.output_type != ISP_IFE_PROCESSED) {
       io_cfg[0].mem_handle[0] = buf_handle_raw[idx];
       io_cfg[0].planes[0] = (struct cam_plane_cfg){
         .width = sensor->frame_width,
@@ -900,60 +895,14 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) {
   assert(ret == 0);
 }
 
-// Enqueue buffer for the given index and return true if the frame is ready
-bool SpectraCamera::enqueue_buffer(int i, uint64_t request_id) {
-  int ret;
-  bool frame_ready = false;
-
-  // Before queuing up a new frame, wait for the
-  // previous one in this slot (index) to come in.
-  if (sync_objs_ife[i]) {
-    // TODO: write a test to stress test w/ a low timeout and check camera frame ids match
-
-    struct cam_sync_wait sync_wait = {0};
-
-    // *** Wait for IFE ***
-    // in RAW_OUTPUT mode, this is just the frame readout from the sensor
-    // in IFE_PROCESSED mode, this is both frame readout and image processing (~1ms)
-    sync_wait.sync_obj = sync_objs_ife[i];
-    sync_wait.timeout_ms = 100;
-    if (stress_test("IFE sync")) {
-      sync_wait.timeout_ms = 1;
-    }
-    ret = do_sync_control(m->cam_sync_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait));
-    if (ret != 0) {
-      LOGE("failed to wait for IFE sync: %d %d", ret, sync_wait.sync_obj);
-    }
-
-    // *** Wait for BPS ***
-    if (ret == 0 && sync_objs_bps[i]) {
-      sync_wait.sync_obj = sync_objs_bps[i];
-      sync_wait.timeout_ms = 50; // typically 7ms
-      if (stress_test("BPS sync")) {
-        sync_wait.timeout_ms = 1;
-      }
-      ret = do_sync_control(m->cam_sync_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait));
-      if (ret != 0) {
-        LOGE("failed to wait for BPS sync: %d %d", ret, sync_wait.sync_obj);
-      }
-    }
-
-    // all good, hand off frame
-    if (ret == 0) {
-      frame_ready = true;
-    }
-
-    if (ret != 0) {
-      clear_req_queue();
-    }
-
-    destroySyncObjectAt(i);
-  }
+void SpectraCamera::enqueue_frame(uint64_t request_id) {
+  int i = request_id % ife_buf_depth;
+  assert(sync_objs_ife[i] == 0);
 
   // create output fences
   struct cam_sync_info sync_create = {0};
   strcpy(sync_create.name, "NodeOutputPortFence");
-  ret = do_sync_control(m->cam_sync_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create));
+  int ret = do_sync_control(m->cam_sync_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create));
   if (ret != 0) {
     LOGE("failed to create fence: %d %d", ret, sync_create.sync_obj);
   } else {
@@ -984,9 +933,7 @@ bool SpectraCamera::enqueue_buffer(int i, uint64_t request_id) {
 
   // submit request to IFE and BPS
   config_ife(i, request_id);
-  if (output_type == ISP_BPS_PROCESSED) config_bps(i, request_id);
-
-  return frame_ready;
+  if (cc.output_type == ISP_BPS_PROCESSED) config_bps(i, request_id);
 }
 
 void SpectraCamera::destroySyncObjectAt(int index) {
@@ -1019,7 +966,7 @@ void SpectraCamera::camera_map_bufs() {
       mem_mgr_map_cmd.mmu_hdls[1] = m->icp_device_iommu;
     }
 
-    if (output_type != ISP_IFE_PROCESSED) {
+    if (cc.output_type != ISP_IFE_PROCESSED) {
       // RAW bayer images
       mem_mgr_map_cmd.fd = buf.camera_bufs_raw[i].fd;
       ret = do_cam_control(m->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd));
@@ -1028,7 +975,7 @@ void SpectraCamera::camera_map_bufs() {
       buf_handle_raw[i] = mem_mgr_map_cmd.out.buf_handle;
     }
 
-    if (output_type != ISP_RAW_OUTPUT) {
+    if (cc.output_type != ISP_RAW_OUTPUT) {
       // final processed images
       VisionBuf *vb = buf.vipc_server->get_buffer(buf.stream_type, i);
       mem_mgr_map_cmd.fd = vb->fd;
@@ -1125,7 +1072,7 @@ void SpectraCamera::configISP() {
     },
   };
 
-  if (output_type != ISP_IFE_PROCESSED) {
+  if (cc.output_type != ISP_IFE_PROCESSED) {
     in_port_info.line_start = 0;
     in_port_info.line_stop = sensor->frame_height + sensor->extra_height - 1;
     in_port_info.height = sensor->frame_height + sensor->extra_height;
@@ -1148,7 +1095,7 @@ void SpectraCamera::configISP() {
 
   // allocate IFE memory, then configure it
   ife_cmd.init(m, 67984, 0x20, false, m->device_iommu, m->cdm_iommu, ife_buf_depth);
-  if (output_type == ISP_IFE_PROCESSED) {
+  if (cc.output_type == ISP_IFE_PROCESSED) {
     assert(sensor->gamma_lut_rgb.size() == 64);
     ife_gamma_lut.init(m, sensor->gamma_lut_rgb.size()*sizeof(uint32_t), 0x20, false, m->device_iommu, m->cdm_iommu, 3); // 3 for RGB
     for (int i = 0; i < 3; i++) {
@@ -1300,7 +1247,7 @@ void SpectraCamera::linkDevices() {
   ret = device_control(m->isp_fd, CAM_START_DEV, session_handle, isp_dev_handle);
   LOGD("start isp: %d", ret);
   assert(ret == 0);
-  if (output_type == ISP_BPS_PROCESSED) {
+  if (cc.output_type == ISP_BPS_PROCESSED) {
     ret = device_control(m->icp_fd, CAM_START_DEV, session_handle, icp_dev_handle);
     LOGD("start icp: %d", ret);
     assert(ret == 0);
@@ -1315,7 +1262,7 @@ void SpectraCamera::camera_close() {
     // LOGD("stop sensor: %d", ret);
     int ret = device_control(m->isp_fd, CAM_STOP_DEV, session_handle, isp_dev_handle);
     LOGD("stop isp: %d", ret);
-    if (output_type == ISP_BPS_PROCESSED) {
+    if (cc.output_type == ISP_BPS_PROCESSED) {
       ret = device_control(m->icp_fd, CAM_STOP_DEV, session_handle, icp_dev_handle);
       LOGD("stop icp: %d", ret);
     }
@@ -1344,7 +1291,7 @@ void SpectraCamera::camera_close() {
     LOGD("-- Release devices");
     ret = device_control(m->isp_fd, CAM_RELEASE_DEV, session_handle, isp_dev_handle);
     LOGD("release isp: %d", ret);
-    if (output_type == ISP_BPS_PROCESSED) {
+    if (cc.output_type == ISP_BPS_PROCESSED) {
       ret = device_control(m->icp_fd, CAM_RELEASE_DEV, session_handle, icp_dev_handle);
       LOGD("release icp: %d", ret);
     }
@@ -1371,90 +1318,129 @@ void SpectraCamera::camera_close() {
   LOGD("destroyed session %d: %d", cc.camera_num, ret);
 }
 
-// Processes camera events and returns true if the frame is ready for further processing
 bool SpectraCamera::handle_camera_event(const cam_req_mgr_message *event_data) {
-  if (stress_test("skipping handling camera event")) {
-    LOGW("skipping event");
+  /*
+    Handles camera SOF event. Returns true if the frame is valid for publishing.
+  */
+
+  uint64_t request_id = event_data->u.frame_msg.request_id;  // ID from the camera request manager
+  uint64_t frame_id_raw = event_data->u.frame_msg.frame_id;  // raw as opposed to our re-indexed frame ID
+  uint64_t timestamp = event_data->u.frame_msg.timestamp;    // timestamped in the kernel's SOF IRQ callback
+  //LOGD("handle cam %d ts %lu req id %lu frame id %lu", cc.camera_num, timestamp, request_id, frame_id_raw);
+
+  if (stress_test("skipping SOF event")) return false;
+
+  if (!validateEvent(request_id, frame_id_raw)) {
     return false;
   }
 
-  if (!enabled) return false;
+  // Update tracking variables
+  if (request_id == request_id_last + 1) {
+    skip_expected = false;
+  }
+  frame_id_raw_last = frame_id_raw;
+  request_id_last = request_id;
 
-  // ID from the qcom camera request manager
-  uint64_t request_id = event_data->u.frame_msg.request_id;
-
-  // raw as opposed to our re-indexed frame ID
-  uint64_t frame_id_raw = event_data->u.frame_msg.frame_id;
-
-  if (request_id != 0) { // next ready
-    // check for skipped_last frames
-    if (frame_id_raw > frame_id_raw_last + 1 && !skipped_last) {
-      LOGE("camera %d realign", cc.camera_num);
-      clear_req_queue();
-      enqueue_req_multi(request_id + 1, ife_buf_depth - 1);
-      skipped_last = true;
-    } else if (frame_id_raw == frame_id_raw_last + 1) {
-      skipped_last = false;
-    }
-
-    // check for dropped requests
-    if (request_id > request_id_last + 1) {
-      LOGE("camera %d dropped requests %ld %ld", cc.camera_num, request_id, request_id_last);
-      enqueue_req_multi(request_id_last + 1 + ife_buf_depth, request_id - (request_id_last + 1));
-    }
-
-    // metas
-    frame_id_raw_last = frame_id_raw;
-    request_id_last = request_id;
-
-    int buf_idx = (request_id - 1) % ife_buf_depth;
-    uint64_t timestamp = event_data->u.frame_msg.timestamp;  // this is timestamped in the kernel's SOF IRQ callback
-    if (syncFirstFrame(cc.camera_num, request_id, frame_id_raw, timestamp)) {
-      // wait for this frame's EOF, then queue up the next one
-      if (enqueue_buffer(buf_idx, request_id + ife_buf_depth)) {
-        // Frame is ready
-
-        // in IFE_PROCESSED mode, we can't know the true EOF, so recover it with sensor readout time
-        uint64_t timestamp_eof = timestamp + sensor->readout_time_ns;
-
-        // Update buffer and frame data
-        buf.cur_buf_idx = buf_idx;
-        buf.cur_frame_data = {
-          .frame_id = (uint32_t)(frame_id_raw - camera_sync_data[cc.camera_num].frame_id_offset),
-          .request_id = (uint32_t)request_id,
-          .timestamp_sof = timestamp,
-          .timestamp_eof = timestamp_eof,
-          .processing_time = float((nanos_since_boot() - timestamp_eof) * 1e-9)
-        };
-        return true;
-      }
-      // LOGW("camerad %d synced req %d fid %d, publishing ts %.2f cereal_frame_id %d", cc.camera_num, (int)request_id, (int)frame_id_raw, (double)(timestamp)*1e-6, meta_data.frame_id);
-    } else {
-      // Frames not yet synced
-      enqueue_req_multi(request_id + ife_buf_depth, 1);
-      // LOGW("camerad %d not synced req %d fid %d", cc.camera_num, (int)request_id, (int)frame_id_raw);
-    }
-  } else { // not ready
-    if (frame_id_raw > frame_id_raw_last + 10) {
-      LOGE("camera %d reset after half second of no response", cc.camera_num);
-      clear_req_queue();
-      enqueue_req_multi(request_id_last + 1, ife_buf_depth);
-      frame_id_raw_last = frame_id_raw;
-      skipped_last = true;
-    }
+  // Wait until frame's fully read out and processed
+  if (!waitForFrameReady(request_id)) {
+    // Reset queue on sync failure to prevent frame tearing
+    LOGE("camera %d sync failure %ld %ld ", cc.camera_num, request_id, frame_id_raw);
+    clearAndRequeue(request_id + 1);
+    return false;
   }
 
-  return false;
+  int buf_idx = request_id % ife_buf_depth;
+  bool ret = processFrame(buf_idx, request_id, frame_id_raw, timestamp);
+  destroySyncObjectAt(buf_idx);
+  enqueue_frame(request_id + ife_buf_depth);  // request next frame for this slot
+  return ret;
+}
+
+bool SpectraCamera::validateEvent(uint64_t request_id, uint64_t frame_id_raw) {
+  // check if the request ID is even valid. this happens after queued
+  // requests are cleared. unclear if it happens any other time.
+  if (request_id == 0) {
+    if (invalid_request_count++ > ife_buf_depth+2) {
+      LOGE("camera %d reset after half second of invalid requests", cc.camera_num);
+      clearAndRequeue(request_id_last + 1);
+      invalid_request_count = 0;
+    }
+    return false;
+  }
+  invalid_request_count = 0;
+
+  // check for skips in frame_id or request_id
+  if (!skip_expected) {
+    if (frame_id_raw != frame_id_raw_last + 1) {
+      LOGE("camera %d frame ID skipped, %lu -> %lu", cc.camera_num, frame_id_raw_last, frame_id_raw);
+      clearAndRequeue(request_id + 1);
+      return false;
+    }
+
+    if (request_id != request_id_last + 1) {
+      LOGE("camera %d requests skipped %ld -> %ld", cc.camera_num, request_id_last, request_id);
+      clearAndRequeue(request_id_last + 1);
+      return false;
+    }
+  }
+  return true;
+}
+
+void SpectraCamera::clearAndRequeue(uint64_t from_request_id) {
+  // clear everything, then queue up a fresh set of frames
+  LOGW("clearing and requeuing camera %d from %lu", cc.camera_num, from_request_id);
+  clear_req_queue();
+  for (uint64_t id = from_request_id; id < from_request_id + ife_buf_depth; ++id) {
+    enqueue_frame(id);
+  }
+  skip_expected = true;
+}
+
+bool SpectraCamera::waitForFrameReady(uint64_t request_id) {
+  int buf_idx = request_id % ife_buf_depth;
+  assert(sync_objs_ife[buf_idx]);
+
+  auto waitForSync = [&](uint32_t sync_obj, int timeout_ms, const char *sync_type) {
+    struct cam_sync_wait sync_wait = {};
+    sync_wait.sync_obj = sync_obj;
+    sync_wait.timeout_ms = stress_test(sync_type) ? 1 : timeout_ms;
+    return do_sync_control(m->cam_sync_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait)) == 0;
+  };
+
+  // wait for frame from IFE
+  // - in RAW_OUTPUT mode, this time is just the frame readout from the sensor
+  // - in IFE_PROCESSED mode, this time also includes image processing (~1ms)
+  bool success = waitForSync(sync_objs_ife[buf_idx], 100, "IFE sync");
+  if (success && sync_objs_bps[buf_idx]) {
+    // BPS is typically 7ms
+    success = waitForSync(sync_objs_bps[buf_idx], 50, "BPS sync");
+  }
+  return success;
+}
+
+bool SpectraCamera::processFrame(int buf_idx, uint64_t request_id, uint64_t frame_id_raw, uint64_t timestamp) {
+  if (!syncFirstFrame(cc.camera_num, request_id, frame_id_raw, timestamp)) {
+    return false;
+  }
+
+  // in IFE_PROCESSED mode, we can't know the true EOF, so recover it with sensor readout time
+  uint64_t timestamp_eof = timestamp + sensor->readout_time_ns;
+
+  // Update buffer and frame data
+  buf.cur_buf_idx = buf_idx;
+  buf.cur_frame_data = {
+    .frame_id = (uint32_t)(frame_id_raw - camera_sync_data[cc.camera_num].frame_id_offset),
+    .request_id = (uint32_t)request_id,
+    .timestamp_sof = timestamp,
+    .timestamp_eof = timestamp_eof,
+    .processing_time = float((nanos_since_boot() - timestamp_eof) * 1e-9)
+  };
+  return true;
 }
 
 bool SpectraCamera::syncFirstFrame(int camera_id, uint64_t request_id, uint64_t raw_id, uint64_t timestamp) {
   if (first_frame_synced) return true;
 
-  // OX and OS cameras require a few frames for the FSIN to sync up
-  if (request_id < 3) {
-    return false;
-  }
-
   // Store the frame data for this camera
   camera_sync_data[camera_id] = SyncData{timestamp, raw_id + 1};
 
@@ -1468,7 +1454,7 @@ bool SpectraCamera::syncFirstFrame(int camera_id, uint64_t request_id, uint64_t
   for (const auto &[_, sync_data] : camera_sync_data) {
     uint64_t diff = std::max(timestamp, sync_data.timestamp) -
                     std::min(timestamp, sync_data.timestamp);
-    if (diff > 0.5*1e6) {  // within 0.5ms
+    if (diff > 0.2*1e6) { // milliseconds
       all_cams_synced = false;
     }
   }

system/camerad/cameras/spectra.h

@@ -22,7 +22,6 @@ const int MIPI_SETTLE_CNT = 33;  // Calculated by camera_freqs.py
 // For use with the Titan 170 ISP in the SDM845
 // https://github.com/commaai/agnos-kernel-sdm845
 
-
 // CSLDeviceType/CSLPacketOpcodesIFE from camx
 // cam_packet_header.op_code = (device << 24) | (opcode);
 #define CSLDeviceTypeImageSensor (0x01 << 24)
@@ -31,12 +30,6 @@ const int MIPI_SETTLE_CNT = 33;  // Calculated by camera_freqs.py
 #define OpcodesIFEInitialConfig  0x0
 #define OpcodesIFEUpdate         0x1
 
-typedef enum {
-  ISP_RAW_OUTPUT,   // raw frame from sensor
-  ISP_IFE_PROCESSED,  // fully processed image through the IFE
-  ISP_BPS_PROCESSED,  // fully processed image through the BPS
-} SpectraOutputType;
-
 std::optional<int32_t> device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources=1);
 int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle);
 int device_control(int fd, int op_code, int session_handle, int dev_handle);
@@ -117,7 +110,7 @@ public:
 
 class SpectraCamera {
 public:
-  SpectraCamera(SpectraMaster *master, const CameraConfig &config, SpectraOutputType out);
+  SpectraCamera(SpectraMaster *master, const CameraConfig &config);
   ~SpectraCamera();
 
   void camera_open(VisionIpcServer *v, cl_device_id device_id, cl_context ctx);
@@ -128,8 +121,7 @@ public:
   void config_ife(int idx, int request_id, bool init=false);
 
   int clear_req_queue();
-  bool enqueue_buffer(int i, uint64_t request_id);
-  void enqueue_req_multi(uint64_t start, int n);
+  void enqueue_frame(uint64_t request_id);
 
   int sensors_init();
   void sensors_start();
@@ -190,15 +182,17 @@ public:
   int sync_objs_bps[MAX_IFE_BUFS] = {};
   uint64_t request_id_last = 0;
   uint64_t frame_id_raw_last = 0;
-  int64_t frame_id_offset = 0;
-  bool skipped_last = true;
-
-  SpectraOutputType output_type;
+  int invalid_request_count = 0;
+  bool skip_expected = true;
 
   CameraBuf buf;
   SpectraMaster *m;
 
 private:
+  void clearAndRequeue(uint64_t from_request_id);
+  bool validateEvent(uint64_t request_id, uint64_t frame_id_raw);
+  bool waitForFrameReady(uint64_t request_id);
+  bool processFrame(int buf_idx, uint64_t request_id, uint64_t frame_id_raw, uint64_t timestamp);
   static bool syncFirstFrame(int camera_id, uint64_t request_id, uint64_t raw_id, uint64_t timestamp);
   struct SyncData {
     uint64_t timestamp;
@@ -208,11 +202,11 @@ private:
   inline static bool first_frame_synced = false;
 
   // a mode for stressing edge cases: realignment, sync failures, etc.
-  inline bool stress_test(const char* log, float prob=0.01) {
-    static bool enable = getenv("SPECTRA_STRESS_TEST") != nullptr;
-    bool triggered = enable && ((static_cast<double>(rand()) / RAND_MAX) < prob);
+  inline bool stress_test(const char* log) {
+    static double prob = std::stod(util::getenv("SPECTRA_ERROR_PROB", "-1"));;
+    bool triggered = (prob > 0) && ((static_cast<double>(rand()) / RAND_MAX) < prob);
     if (triggered) {
-      LOGE("stress test: %s", log);
+      LOGE("stress test (cam %d): %s", cc.camera_num, log);
     }
     return triggered;
   }

system/camerad/sensors/os04c10.cc

@@ -43,6 +43,9 @@ OS04C10::OS04C10() {
   frame_data_type = 0x2c;
   mclk_frequency = 24000000; // Hz
 
+  // TODO: this was set from logs. actually calculate it out
+  readout_time_ns = 11000000;
+
   ev_scale = 150.0;
   dc_gain_factor = 1;
   dc_gain_min_weight = 1;  // always on is fine

system/camerad/test/test_camerad.py

@@ -64,7 +64,35 @@ class TestCamerad:
     laggy_frames = {k: v for k, v in diffs.items() if v > 1.1}
     assert len(laggy_frames) == 0, f"Frames not synced properly: {laggy_frames=}"
 
-  @pytest.mark.skip("TODO: enable this")
-  def test_stress_test(self, logs):
-    os.environ['SPECTRA_STRESS_TEST'] = '1'
-    run_and_log(["camerad", ], CAMERAS, 5)
+  def test_sanity_checks(self, logs):
+    self._sanity_checks(logs)
+
+  def _sanity_checks(self, ts):
+    for c in CAMERAS:
+      assert c in ts
+      assert len(ts[c]['t']) > 20
+
+      # not a valid request id
+      assert 0 not in ts[c]['requestId']
+
+      # should monotonically increase
+      assert np.all(np.diff(ts[c]['frameId']) >= 1)
+      assert np.all(np.diff(ts[c]['requestId']) >= 1)
+
+      # EOF > SOF
+      assert np.all((ts[c]['timestampEof'] - ts[c]['timestampSof']) > 0)
+
+      # logMonoTime > SOF
+      assert np.all((ts[c]['t'] - ts[c]['timestampSof']/1e9) > 1e-7)
+      assert np.all((ts[c]['t'] - ts[c]['timestampEof']/1e9) > 1e-7)
+
+  def test_stress_test(self):
+    os.environ['SPECTRA_ERROR_PROB'] = '0.008'
+    logs = run_and_log(["camerad", ], CAMERAS, 10)
+    ts = msgs_to_time_series(logs)
+
+    # we should see some jumps from introduced errors
+    assert np.max([ np.max(np.diff(ts[c]['frameId'])) for c in CAMERAS ]) > 1
+    assert np.max([ np.max(np.diff(ts[c]['requestId'])) for c in CAMERAS ]) > 1
+
+    self._sanity_checks(ts)

system/hardware/base.h

@@ -34,8 +34,6 @@ public:
   static bool get_ssh_enabled() { return false; }
   static void set_ssh_enabled(bool enabled) {}
 
-  static void config_cpu_rendering(bool offscreen);
-
   static bool PC() { return false; }
   static bool TICI() { return false; }
   static bool AGNOS() { return false; }

system/hardware/pc/hardware.h

@@ -12,12 +12,4 @@ public:
   static bool PC() { return true; }
   static bool TICI() { return util::getenv("TICI", 0) == 1; }
   static bool AGNOS() { return util::getenv("TICI", 0) == 1; }
-
-  static void config_cpu_rendering(bool offscreen) {
-    if (offscreen) {
-      setenv("QT_QPA_PLATFORM", "offscreen", 1);
-    }
-    setenv("__GLX_VENDOR_LIBRARY_NAME", "mesa", 1);
-    setenv("LP_NUM_THREADS", "0", 1); // disable threading so we stay on our assigned CPU
-  }
 };

system/hardware/tici/agnos.json

@@ -56,28 +56,28 @@
   },
   {
     "name": "boot",
-    "url": "https://commadist.azureedge.net/agnosupdate/boot-c7cba1ce64bf85384241fa063971855c5ffcb4d90a0d07bed4733d46b94d4170.img.xz",
-    "hash": "c7cba1ce64bf85384241fa063971855c5ffcb4d90a0d07bed4733d46b94d4170",
-    "hash_raw": "c7cba1ce64bf85384241fa063971855c5ffcb4d90a0d07bed4733d46b94d4170",
+    "url": "https://commadist.azureedge.net/agnosupdate/boot-bca7573652def58a0afc40bbdd550d63dc08ed2e925ace69032aef84bb9dc4ba.img.xz",
+    "hash": "bca7573652def58a0afc40bbdd550d63dc08ed2e925ace69032aef84bb9dc4ba",
+    "hash_raw": "bca7573652def58a0afc40bbdd550d63dc08ed2e925ace69032aef84bb9dc4ba",
     "size": 18475008,
     "sparse": false,
     "full_check": true,
     "has_ab": true,
-    "ondevice_hash": "6a9a71bf01f2013f35bda9594cefe3cb4a3835402a6cb0e95306fe4decf261c5"
+    "ondevice_hash": "8907415564e8a242548e871b534dcd53240fe4e4517700c6c85b5637e365f0b0"
   },
   {
     "name": "system",
-    "url": "https://commadist.azureedge.net/agnosupdate/system-cd03486f6c7333dee21f59af771f9992ea90a9a04271c1506a663d658f391625.img.xz",
-    "hash": "990ff7005a5bee8e759c96ddba23f1258f043fb038cf74083bf7d2d9c9a29e39",
-    "hash_raw": "cd03486f6c7333dee21f59af771f9992ea90a9a04271c1506a663d658f391625",
+    "url": "https://commadist.azureedge.net/agnosupdate/system-5612484e7f255659c0845de620e7c733afd2e1b939f9464f5ef039721bb7cba9.img.xz",
+    "hash": "e4872f4132111b7b28586d978dd01bb48ffa031e103d029ebede7613c1bc2aa6",
+    "hash_raw": "5612484e7f255659c0845de620e7c733afd2e1b939f9464f5ef039721bb7cba9",
     "size": 4404019200,
     "sparse": true,
     "full_check": false,
     "has_ab": true,
-    "ondevice_hash": "d922fffe1b5f02898465a2d6625294abb70d22643ebf2d6a94f5d7512291d1a4",
+    "ondevice_hash": "4e5e680b4ac387ddc974b32dd3d5ec1d76282511eab974866b3b72399034985e",
     "alt": {
-      "hash": "cd03486f6c7333dee21f59af771f9992ea90a9a04271c1506a663d658f391625",
-      "url": "https://commadist.azureedge.net/agnosupdate/system-cd03486f6c7333dee21f59af771f9992ea90a9a04271c1506a663d658f391625.img",
+      "hash": "5612484e7f255659c0845de620e7c733afd2e1b939f9464f5ef039721bb7cba9",
+      "url": "https://commadist.azureedge.net/agnosupdate/system-5612484e7f255659c0845de620e7c733afd2e1b939f9464f5ef039721bb7cba9.img",
       "size": 4404019200
     }
   }

system/hardware/tici/all-partitions.json

@@ -339,62 +339,62 @@
   },
   {
     "name": "boot",
-    "url": "https://commadist.azureedge.net/agnosupdate/boot-c7cba1ce64bf85384241fa063971855c5ffcb4d90a0d07bed4733d46b94d4170.img.xz",
-    "hash": "c7cba1ce64bf85384241fa063971855c5ffcb4d90a0d07bed4733d46b94d4170",
-    "hash_raw": "c7cba1ce64bf85384241fa063971855c5ffcb4d90a0d07bed4733d46b94d4170",
+    "url": "https://commadist.azureedge.net/agnosupdate/boot-bca7573652def58a0afc40bbdd550d63dc08ed2e925ace69032aef84bb9dc4ba.img.xz",
+    "hash": "bca7573652def58a0afc40bbdd550d63dc08ed2e925ace69032aef84bb9dc4ba",
+    "hash_raw": "bca7573652def58a0afc40bbdd550d63dc08ed2e925ace69032aef84bb9dc4ba",
     "size": 18475008,
     "sparse": false,
     "full_check": true,
     "has_ab": true,
-    "ondevice_hash": "6a9a71bf01f2013f35bda9594cefe3cb4a3835402a6cb0e95306fe4decf261c5"
+    "ondevice_hash": "8907415564e8a242548e871b534dcd53240fe4e4517700c6c85b5637e365f0b0"
   },
   {
     "name": "system",
-    "url": "https://commadist.azureedge.net/agnosupdate/system-cd03486f6c7333dee21f59af771f9992ea90a9a04271c1506a663d658f391625.img.xz",
-    "hash": "990ff7005a5bee8e759c96ddba23f1258f043fb038cf74083bf7d2d9c9a29e39",
-    "hash_raw": "cd03486f6c7333dee21f59af771f9992ea90a9a04271c1506a663d658f391625",
+    "url": "https://commadist.azureedge.net/agnosupdate/system-5612484e7f255659c0845de620e7c733afd2e1b939f9464f5ef039721bb7cba9.img.xz",
+    "hash": "e4872f4132111b7b28586d978dd01bb48ffa031e103d029ebede7613c1bc2aa6",
+    "hash_raw": "5612484e7f255659c0845de620e7c733afd2e1b939f9464f5ef039721bb7cba9",
     "size": 4404019200,
     "sparse": true,
     "full_check": false,
     "has_ab": true,
-    "ondevice_hash": "d922fffe1b5f02898465a2d6625294abb70d22643ebf2d6a94f5d7512291d1a4",
+    "ondevice_hash": "4e5e680b4ac387ddc974b32dd3d5ec1d76282511eab974866b3b72399034985e",
     "alt": {
-      "hash": "cd03486f6c7333dee21f59af771f9992ea90a9a04271c1506a663d658f391625",
-      "url": "https://commadist.azureedge.net/agnosupdate/system-cd03486f6c7333dee21f59af771f9992ea90a9a04271c1506a663d658f391625.img",
+      "hash": "5612484e7f255659c0845de620e7c733afd2e1b939f9464f5ef039721bb7cba9",
+      "url": "https://commadist.azureedge.net/agnosupdate/system-5612484e7f255659c0845de620e7c733afd2e1b939f9464f5ef039721bb7cba9.img",
       "size": 4404019200
     }
   },
   {
     "name": "userdata_90",
-    "url": "https://commadist.azureedge.net/agnosupdate/userdata_90-554a22697b356cb150c2c803b4cb1de79403849e9be451c844d218d38b5bc236.img.xz",
-    "hash": "4f0a862e3aff4980e697ece63afaef6f9869d013ac2ca4c45193e41089ee4f5c",
-    "hash_raw": "554a22697b356cb150c2c803b4cb1de79403849e9be451c844d218d38b5bc236",
+    "url": "https://commadist.azureedge.net/agnosupdate/userdata_90-16c037fa42ee99bc6ec92909efc8a8075a0e8a0232a7d90e39e7d40a7bd0ee8e.img.xz",
+    "hash": "c6fb215f2b297f7ff5b8f133bc5d687772b37f2fee42a44aa730e37a84a14e52",
+    "hash_raw": "16c037fa42ee99bc6ec92909efc8a8075a0e8a0232a7d90e39e7d40a7bd0ee8e",
     "size": 96636764160,
     "sparse": true,
     "full_check": true,
     "has_ab": false,
-    "ondevice_hash": "5d780092d51c569e6b8a28ab73f4029bcc3517698f496af5801f863c9865060f"
+    "ondevice_hash": "7199262f209abbb07be5eece505ac4d4c7ba8957f2d9ff7b1ac1ef2063461665"
   },
   {
     "name": "userdata_89",
-    "url": "https://commadist.azureedge.net/agnosupdate/userdata_89-6045b9a3f1ae6e0ee09b95da039b697ab8d8447cdd0796fa31afa9c7d324ee80.img.xz",
-    "hash": "ba49fef48573e9befdfb6334181753855cb83d3e1d8c6d4d83d510ea0141ec3f",
-    "hash_raw": "6045b9a3f1ae6e0ee09b95da039b697ab8d8447cdd0796fa31afa9c7d324ee80",
+    "url": "https://commadist.azureedge.net/agnosupdate/userdata_89-62c2c41470282b581ec1bbbe0375fb3b6c66df2f4bc3dc6c6fdf796f1797f136.img.xz",
+    "hash": "d66f894436fa11d4ff00f8a84e54d9e23a6492b0087f69bb958d2ab0bdc6dfba",
+    "hash_raw": "62c2c41470282b581ec1bbbe0375fb3b6c66df2f4bc3dc6c6fdf796f1797f136",
     "size": 95563022336,
     "sparse": true,
     "full_check": true,
     "has_ab": false,
-    "ondevice_hash": "19b63b184063c25ff2fc9f1c8d1c919c140c3e1295ebac6cb66e64c5cb609abe"
+    "ondevice_hash": "f1d3685618f6d1bde24ce6109284c5d30ece2f4fd015be67e8b52ef7e06067a4"
   },
   {
     "name": "userdata_30",
-    "url": "https://commadist.azureedge.net/agnosupdate/userdata_30-498358c1e5347dc3f8c369523bd77b94ef73d6c6729b40f376209d0d32b356fe.img.xz",
-    "hash": "828d0911713af02de7fae4c583a2274783e344939f296c7c016866a8cf2cb63f",
-    "hash_raw": "498358c1e5347dc3f8c369523bd77b94ef73d6c6729b40f376209d0d32b356fe",
+    "url": "https://commadist.azureedge.net/agnosupdate/userdata_30-3e71d8804c90a6dff5048edb976149f9ea177efa139dea47cb585cef76b26f6e.img.xz",
+    "hash": "1b201ecbd0e1573777811bf18fa90cb080bfbccb34a6dcfd39b412632e7ca699",
+    "hash_raw": "3e71d8804c90a6dff5048edb976149f9ea177efa139dea47cb585cef76b26f6e",
     "size": 32212254720,
     "sparse": true,
     "full_check": true,
     "has_ab": false,
-    "ondevice_hash": "4beb60ffceff88b05daa014db50cb2f4e9acec3ed7ae1b89a8d3c84cfc3c1c92"
+    "ondevice_hash": "47c28e63209556442cd9d2fd06a6c0e7fcf35c8a4e4fbcc550b9c089429ad0e0"
   }
 ]

system/hardware/tici/hardware.h

@@ -109,11 +109,4 @@ public:
 
   static bool get_ssh_enabled() { return Params().getBool("SshEnabled"); }
   static void set_ssh_enabled(bool enabled) { Params().putBool("SshEnabled", enabled); }
-
-  static void config_cpu_rendering(bool offscreen) {
-    if (offscreen) {
-      setenv("QT_QPA_PLATFORM", "eglfs", 1); // offscreen doesn't work with EGL/GLES
-    }
-    setenv("LP_NUM_THREADS", "0", 1); // disable threading so we stay on our assigned CPU
-  }
 };

system/loggerd/deleter.py

@@ -22,9 +22,9 @@ def has_preserve_xattr(d: str) -> bool:
   return getxattr(os.path.join(Paths.log_root(), d), PRESERVE_ATTR_NAME) == PRESERVE_ATTR_VALUE
 
 
-def get_preserved_segments(dirs_by_creation: list[str]) -> list[str]:
+def get_preserved_segments(dirs_by_creation: list[str]) -> set[str]:
   # skip deleting most recent N preserved segments (and their prior segment)
-  preserved = []
+  preserved = set()
   for n, d in enumerate(filter(has_preserve_xattr, reversed(dirs_by_creation))):
     if n == PRESERVE_COUNT:
       break
@@ -40,7 +40,7 @@ def get_preserved_segments(dirs_by_creation: list[str]) -> list[str]:
 
     # preserve segment and two prior
     for _seg_num in range(max(0, seg_num - 2), seg_num + 1):
-      preserved.append(f"{date_str}--{_seg_num}")
+      preserved.add(f"{date_str}--{_seg_num}")
 
   return preserved
 

system/loggerd/logger.cc

@@ -50,6 +50,10 @@ kj::Array<capnp::word> logger_build_init_data() {
   init.setPassive(false);
   init.setDongleId(params_map["DongleId"]);
 
+  // for prebuilt branches
+  init.setGitSrcCommit(util::read_file("../../git_src_commit"));
+  init.setGitSrcCommitDate(util::read_file("../../git_src_commit_date"));
+
   auto lparams = init.initParams().initEntries(params_map.size());
   int j = 0;
   for (auto& [key, value] : params_map) {

system/loggerd/loggerd.cc

@@ -64,6 +64,58 @@ struct RemoteEncoder {
   bool seen_first_packet = false;
 };
 
+size_t write_encode_data(LoggerdState *s, cereal::Event::Reader event, RemoteEncoder &re, const EncoderInfo &encoder_info) {
+  auto edata = (event.*(encoder_info.get_encode_data_func))();
+  auto idx = edata.getIdx();
+  auto flags = idx.getFlags();
+
+  // if we aren't recording yet, try to start, since we are in the correct segment
+  if (!re.recording) {
+    if (flags & V4L2_BUF_FLAG_KEYFRAME) {
+      // only create on iframe
+      if (re.dropped_frames) {
+        // this should only happen for the first segment, maybe
+        LOGW("%s: dropped %d non iframe packets before init", encoder_info.publish_name, re.dropped_frames);
+        re.dropped_frames = 0;
+      }
+      // if we aren't actually recording, don't create the writer
+      if (encoder_info.record) {
+        assert(encoder_info.filename != NULL);
+        re.writer.reset(new VideoWriter(s->logger.segmentPath().c_str(),
+                                        encoder_info.filename, idx.getType() != cereal::EncodeIndex::Type::FULL_H_E_V_C,
+                                        edata.getWidth(), edata.getHeight(), encoder_info.fps, idx.getType()));
+        // write the header
+        auto header = edata.getHeader();
+        re.writer->write((uint8_t *)header.begin(), header.size(), idx.getTimestampEof() / 1000, true, false);
+      }
+      re.recording = true;
+    } else {
+      // this is a sad case when we aren't recording, but don't have an iframe
+      // nothing we can do but drop the frame
+      ++re.dropped_frames;
+      return 0;
+    }
+  }
+
+  // we have to be recording if we are here
+  assert(re.recording);
+
+  // if we are actually writing the video file, do so
+  if (re.writer) {
+    auto data = edata.getData();
+    re.writer->write((uint8_t *)data.begin(), data.size(), idx.getTimestampEof() / 1000, false, flags & V4L2_BUF_FLAG_KEYFRAME);
+  }
+
+  // put it in log stream as the idx packet
+  MessageBuilder bmsg;
+  auto evt = bmsg.initEvent(event.getValid());
+  evt.setLogMonoTime(event.getLogMonoTime());
+  (evt.*(encoder_info.set_encode_idx_func))(idx);
+  auto new_msg = bmsg.toBytes();
+  s->logger.write((uint8_t *)new_msg.begin(), new_msg.size(), true);  // always in qlog?
+  return new_msg.size();
+}
+
 int handle_encoder_msg(LoggerdState *s, Message *msg, std::string &name, struct RemoteEncoder &re, const EncoderInfo &encoder_info) {
   int bytes_count = 0;
 
@@ -72,7 +124,6 @@ int handle_encoder_msg(LoggerdState *s, Message *msg, std::string &name, struct
   auto event = cmsg.getRoot<cereal::Event>();
   auto edata = (event.*(encoder_info.get_encode_data_func))();
   auto idx = edata.getIdx();
-  auto flags = idx.getFlags();
 
   // encoderd can have started long before loggerd
   if (!re.seen_first_packet) {
@@ -95,61 +146,15 @@ int handle_encoder_msg(LoggerdState *s, Message *msg, std::string &name, struct
       re.marked_ready_to_rotate = false;
       // we are in this segment now, process any queued messages before this one
       if (!re.q.empty()) {
-        for (auto &qmsg : re.q) {
-          bytes_count += handle_encoder_msg(s, qmsg, name, re, encoder_info);
+        for (auto qmsg : re.q) {
+          capnp::FlatArrayMessageReader reader({(capnp::word *)qmsg->getData(), qmsg->getSize() / sizeof(capnp::word)});
+          bytes_count += write_encode_data(s, reader.getRoot<cereal::Event>(), re, encoder_info);
+          delete qmsg;
         }
         re.q.clear();
       }
     }
-
-    // if we aren't recording yet, try to start, since we are in the correct segment
-    if (!re.recording) {
-      if (flags & V4L2_BUF_FLAG_KEYFRAME) {
-        // only create on iframe
-        if (re.dropped_frames) {
-          // this should only happen for the first segment, maybe
-          LOGW("%s: dropped %d non iframe packets before init", name.c_str(), re.dropped_frames);
-          re.dropped_frames = 0;
-        }
-        // if we aren't actually recording, don't create the writer
-        if (encoder_info.record) {
-          assert(encoder_info.filename != NULL);
-          re.writer.reset(new VideoWriter(s->logger.segmentPath().c_str(),
-            encoder_info.filename, idx.getType() != cereal::EncodeIndex::Type::FULL_H_E_V_C,
-            edata.getWidth(), edata.getHeight(), encoder_info.fps, idx.getType()));
-          // write the header
-          auto header = edata.getHeader();
-          re.writer->write((uint8_t *)header.begin(), header.size(), idx.getTimestampEof()/1000, true, false);
-        }
-        re.recording = true;
-      } else {
-        // this is a sad case when we aren't recording, but don't have an iframe
-        // nothing we can do but drop the frame
-        delete msg;
-        ++re.dropped_frames;
-        return bytes_count;
-      }
-    }
-
-    // we have to be recording if we are here
-    assert(re.recording);
-
-    // if we are actually writing the video file, do so
-    if (re.writer) {
-      auto data = edata.getData();
-      re.writer->write((uint8_t *)data.begin(), data.size(), idx.getTimestampEof()/1000, false, flags & V4L2_BUF_FLAG_KEYFRAME);
-    }
-
-    // put it in log stream as the idx packet
-    MessageBuilder bmsg;
-    auto evt = bmsg.initEvent(event.getValid());
-    evt.setLogMonoTime(event.getLogMonoTime());
-    (evt.*(encoder_info.set_encode_idx_func))(idx);
-    auto new_msg = bmsg.toBytes();
-    s->logger.write((uint8_t *)new_msg.begin(), new_msg.size(), true);   // always in qlog?
-    bytes_count += new_msg.size();
-
-    // free the message, we used it
+    bytes_count += write_encode_data(s, event, re, encoder_info);
     delete msg;
   } else if (offset_segment_num > s->logger.segment()) {
     // encoderd packet has a newer segment, this means encoderd has rolled over

system/loggerd/tests/test_zstd_writer.cc

@@ -18,15 +18,21 @@ TEST_CASE("ZstdFileWriter writes and compresses data correctly in loops", "[Zstd
   // Step 1: Write compressed data to file in a loop
   {
     ZstdFileWriter writer(filename, LOG_COMPRESSION_LEVEL);
+    // Write various data sizes including edge cases
+    std::vector<size_t> testSizes = {dataSize, 1, 0, dataSize * 2};  // Normal, minimal, empty, large
     for (int i = 0; i < iterations; ++i) {
-      std::string testData = util::random_string(dataSize);
+      size_t currentSize = testSizes[i % testSizes.size()];
+      std::string testData = util::random_string(currentSize);
       totalTestData.append(testData);
+
       writer.write((void *)testData.c_str(), testData.size());
     }
   }
 
   // Step 2: Decompress the file and verify the data
   auto compressedContent = util::read_file(filename);
+  REQUIRE(compressedContent.size() > 0);
+  REQUIRE(compressedContent.size() < totalTestData.size());
   std::string decompressedData = zstd_decompress(compressedContent);
 
   // Step 3: Verify that the decompressed data matches the original accumulated data

system/manager/process_config.py

@@ -105,10 +105,11 @@ procs = [
   PythonProcess("micd", "system.micd", iscar),
   PythonProcess("timed", "system.timed", always_run, enabled=not PC),
 
-  # TODO Make python process once TG allows opening QCOM from child proc
-  NativeProcess("dmonitoringmodeld", "selfdrive/modeld", ["./dmonitoringmodeld"], driverview, enabled=(WEBCAM or not PC)),
-  # TODO Make python process once TG allows opening QCOM from child proc
-  NativeProcess("modeld", "selfdrive/modeld", ["./modeld"], and_(only_onroad, is_stock_model)),
+  # TODO: Make python process once TG allows opening QCOM from child pro
+  # https://github.com/tinygrad/tinygrad/blob/ac9c96dae1656dc220ee4acc39cef4dd449aa850/tinygrad/device.py#L26
+  NativeProcess("modeld", "selfdrive/modeld", ["./modeld.py"], and_(only_onroad, is_stock_model)),
+  NativeProcess("dmonitoringmodeld", "selfdrive/modeld", ["./dmonitoringmodeld.py"], driverview, enabled=(WEBCAM or not PC)),
+
   NativeProcess("sensord", "system/sensord", ["./sensord"], only_onroad, enabled=not PC),
   NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)),
   PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad),

system/ui/lib/application.py

@@ -15,6 +15,7 @@ DEBUG_FPS = os.getenv("DEBUG_FPS") == '1'
 STRICT_MODE = os.getenv("STRICT_MODE") == '1'
 
 DEFAULT_TEXT_SIZE = 60
+DEFAULT_TEXT_COLOR = rl.Color(200, 200, 200, 255)
 FONT_DIR = os.path.join(BASEDIR, "selfdrive/assets/fonts")
 
 
@@ -118,7 +119,7 @@ class GuiApplication:
     rl.gui_set_style(rl.GuiControl.DEFAULT, rl.GuiControlProperty.BORDER_WIDTH, 0)
     rl.gui_set_style(rl.GuiControl.DEFAULT, rl.GuiDefaultProperty.TEXT_SIZE, DEFAULT_TEXT_SIZE)
     rl.gui_set_style(rl.GuiControl.DEFAULT, rl.GuiDefaultProperty.BACKGROUND_COLOR, rl.color_to_int(rl.BLACK))
-    rl.gui_set_style(rl.GuiControl.DEFAULT, rl.GuiControlProperty.TEXT_COLOR_NORMAL, rl.color_to_int(rl.Color(200, 200, 200, 255)))
+    rl.gui_set_style(rl.GuiControl.DEFAULT, rl.GuiControlProperty.TEXT_COLOR_NORMAL, rl.color_to_int(DEFAULT_TEXT_COLOR))
     rl.gui_set_style(rl.GuiControl.DEFAULT, rl.GuiDefaultProperty.BACKGROUND_COLOR, rl.color_to_int(rl.Color(30, 30, 30, 255)))
     rl.gui_set_style(rl.GuiControl.DEFAULT, rl.GuiControlProperty.BASE_COLOR_NORMAL, rl.color_to_int(rl.Color(50, 50, 50, 255)))
 

system/ui/lib/button.py

@@ -54,6 +54,7 @@ def gui_button(
   if button_style != ButtonStyle.TRANSPARENT:
     rl.draw_rectangle_rounded(rect, roundness, 20, bg_color)
   else:
+    rl.draw_rectangle_rounded(rect, roundness, 20, rl.BLACK)
     rl.draw_rectangle_rounded_lines_ex(rect, roundness, 20, 2, rl.WHITE)
 
   font = gui_app.font(font_weight)

system/ui/lib/label.py

@@ -1,11 +1,55 @@
 import pyray as rl
+from openpilot.system.ui.lib.application import gui_app, FontWeight, DEFAULT_TEXT_SIZE, DEFAULT_TEXT_COLOR
 from openpilot.system.ui.lib.utils import GuiStyleContext
 
-def gui_label(rect, text, font_size):
+
+def gui_label(
+  rect: rl.Rectangle,
+  text: str,
+  font_size: int = DEFAULT_TEXT_SIZE,
+  color: rl.Color = DEFAULT_TEXT_COLOR,
+  font_weight: FontWeight = FontWeight.NORMAL,
+  alignment: int = rl.GuiTextAlignment.TEXT_ALIGN_LEFT,
+  alignment_vertical: int = rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE
+):
+  # Set font based on the provided weight
+  font = gui_app.font(font_weight)
+
+  # Measure text size
+  text_size = rl.measure_text_ex(font, text, font_size, 0)
+
+  # Calculate horizontal position based on alignment
+  text_x = rect.x + {
+    rl.GuiTextAlignment.TEXT_ALIGN_LEFT: 0,
+    rl.GuiTextAlignment.TEXT_ALIGN_CENTER: (rect.width - text_size.x) / 2,
+    rl.GuiTextAlignment.TEXT_ALIGN_RIGHT: rect.width - text_size.x,
+  }.get(alignment, 0)
+
+  # Calculate vertical position based on alignment
+  text_y = rect.y + {
+    rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP: 0,
+    rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE: (rect.height - text_size.y) / 2,
+    rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM: rect.height - text_size.y,
+  }.get(alignment_vertical, 0)
+
+  # Draw the text in the specified rectangle
+  rl.draw_text_ex(font, text, rl.Vector2(text_x, text_y), font_size, 0, color)
+
+
+def gui_text_box(
+  rect: rl.Rectangle,
+  text: str,
+  font_size: int = DEFAULT_TEXT_SIZE,
+  color: rl.Color = DEFAULT_TEXT_COLOR,
+  alignment: int = rl.GuiTextAlignment.TEXT_ALIGN_LEFT,
+  alignment_vertical: int = rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP
+):
   styles = [
+    (rl.GuiControl.DEFAULT, rl.GuiControlProperty.TEXT_COLOR_NORMAL, rl.color_to_int(color)),
     (rl.GuiControl.DEFAULT, rl.GuiDefaultProperty.TEXT_SIZE, font_size),
     (rl.GuiControl.DEFAULT, rl.GuiDefaultProperty.TEXT_LINE_SPACING, font_size),
-    (rl.GuiControl.DEFAULT, rl.GuiDefaultProperty.TEXT_ALIGNMENT_VERTICAL, rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP),
+    (rl.GuiControl.DEFAULT, rl.GuiControlProperty.TEXT_ALIGNMENT, alignment),
+    (rl.GuiControl.DEFAULT, rl.GuiDefaultProperty.TEXT_ALIGNMENT_VERTICAL, alignment_vertical),
     (rl.GuiControl.DEFAULT, rl.GuiDefaultProperty.TEXT_WRAP_MODE, rl.GuiTextWrapMode.TEXT_WRAP_WORD)
   ]
 

system/ui/reset.py

@@ -7,7 +7,7 @@ from enum import IntEnum
 
 from openpilot.system.ui.lib.application import gui_app, FontWeight
 from openpilot.system.ui.lib.button import gui_button, ButtonStyle
-from openpilot.system.ui.lib.label import gui_label
+from openpilot.system.ui.lib.label import gui_label, gui_text_box
 
 NVME = "/dev/nvme0n1"
 USERDATA = "/dev/disk/by-partlabel/userdata"
@@ -51,13 +51,11 @@ class Reset:
     threading.Timer(0.1, self.do_reset).start()
 
   def render(self, rect: rl.Rectangle):
-    rl.gui_set_font(gui_app.font(FontWeight.BOLD))
-    label_rect = rl.Rectangle(rect.x + 140, rect.y, rect.width - 280, rect.height)
-    gui_label(label_rect, "System Reset", 90)
-    rl.gui_set_font(gui_app.font(FontWeight.NORMAL))
+    label_rect = rl.Rectangle(rect.x + 140, rect.y, rect.width - 280, 100)
+    gui_label(label_rect, "System Reset", 100, font_weight=FontWeight.BOLD)
 
-    label_rect.y += 150
-    gui_label(label_rect, self.get_body_text(), 80)
+    text_rect = rl.Rectangle(rect.x + 140, rect.y + 140, rect.width - 280, rect.height - 90 - 100)
+    gui_text_box(text_rect, self.get_body_text(), 90)
 
     button_height = 160
     button_spacing = 50

system/ui/text.py

@@ -39,7 +39,7 @@ def main():
 
   text_content = sys.argv[1] if len(sys.argv) > 1 else DEMO_TEXT
 
-  textarea_rect = rl.Rectangle(MARGIN, MARGIN, gui_app.width - MARGIN * 2, gui_app.height - MARGIN * 2 - BUTTON_SIZE.y - SPACING)
+  textarea_rect = rl.Rectangle(MARGIN, MARGIN, gui_app.width - MARGIN * 2, gui_app.height - MARGIN * 2)
   wrapped_lines = wrap_text(text_content, FONT_SIZE, textarea_rect.width - 20)
   content_rect = rl.Rectangle(0, 0, textarea_rect.width - 20, len(wrapped_lines) * LINE_HEIGHT)
   scroll_panel = GuiScrollPanel(show_vertical_scroll_bar=True)