From 4bb0dfd59cd4decdb9b75577454398c4299b7b18 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Wed, 5 Mar 2025 17:12:30 -0800 Subject: [PATCH] camerad: cleaner queueing logic (#34786) --- system/camerad/cameras/spectra.cc | 239 ++++++++++++++---------------- system/camerad/cameras/spectra.h | 15 +- 2 files changed, 122 insertions(+), 132 deletions(-) diff --git a/system/camerad/cameras/spectra.cc b/system/camerad/cameras/spectra.cc index 3e5941b13f..063420823c 100644 --- a/system/camerad/cameras/spectra.cc +++ b/system/camerad/cameras/spectra.cc @@ -249,6 +249,7 @@ SpectraCamera::~SpectraCamera() { } int SpectraCamera::clear_req_queue() { + // for "non-realtime" BPS if (icp_dev_handle > 0) { struct cam_flush_dev_cmd cmd = { .session_handle = session_handle, @@ -260,6 +261,7 @@ int SpectraCamera::clear_req_queue() { 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; @@ -304,14 +306,7 @@ void SpectraCamera::camera_open(VisionIpcServer *v, cl_device_id device_id, cl_c 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() { @@ -901,59 +896,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); - } - } - - if (ret == 0) { - // all good, hand off frame - frame_ready = true; - destroySyncObjectAt(i); - } else { - // need to start over on sync failures, - // otherwise future frames will tear - clear_req_queue(); - } - } +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 { @@ -985,8 +935,6 @@ 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; } void SpectraCamera::destroySyncObjectAt(int index) { @@ -1371,90 +1319,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; } - // 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; + // 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; + + // 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; + } - //LOGD("handle cam %d, request id %lu -> %lu, frame id raw %lu", cc.camera_num, request_id_last, request_id, frame_id_raw); + 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; +} - 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; +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++ > 10) { + LOGE("camera %d reset after half second of invalid requests", cc.camera_num); + clearAndRequeue(request_id_last + 1); + invalid_request_count = 0; } - - // 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)); + 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; } - // 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; + 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; +} - return false; +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::syncFirstFrame(int camera_id, uint64_t request_id, uint64_t raw_id, uint64_t timestamp) { - if (first_frame_synced) return true; +bool SpectraCamera::waitForFrameReady(uint64_t request_id) { + int buf_idx = request_id % ife_buf_depth; + assert(sync_objs_ife[buf_idx]); - // OX and OS cameras require a few frames for the FSIN to sync up - if (request_id < 3) { + 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; + // Store the frame data for this camera camera_sync_data[camera_id] = SyncData{timestamp, raw_id + 1}; @@ -1468,7 +1455,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; } } diff --git a/system/camerad/cameras/spectra.h b/system/camerad/cameras/spectra.h index d9abf884c1..2070363a16 100644 --- a/system/camerad/cameras/spectra.h +++ b/system/camerad/cameras/spectra.h @@ -128,8 +128,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,8 +189,8 @@ 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; + int invalid_request_count = 0; + bool skip_expected = true; SpectraOutputType output_type; @@ -199,6 +198,10 @@ public: 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 +211,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) { + inline bool stress_test(const char* log, float prob=0.02) { static bool enable = getenv("SPECTRA_STRESS_TEST") != nullptr; bool triggered = enable && ((static_cast(rand()) / RAND_MAX) < prob); if (triggered) { - LOGE("stress test: %s", log); + LOGE("stress test (cam %d): %s", cc.camera_num, log); } return triggered; }