From 47c04210e2af8f03de51b13d56eb3316642039eb Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 22 Feb 2025 01:18:35 +0800 Subject: [PATCH] camerad: refactor frame processing to direct handling (#34655) refactor frame processing --- system/camerad/cameras/camera_common.cc | 18 +------ system/camerad/cameras/camera_common.h | 8 +-- system/camerad/cameras/camera_qcom2.cc | 7 +-- system/camerad/cameras/spectra.cc | 67 ++++++++++++++----------- system/camerad/cameras/spectra.h | 7 +-- 5 files changed, 49 insertions(+), 58 deletions(-) diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index f63a48b733..062647d66b 100644 --- a/system/camerad/cameras/camera_common.cc +++ b/system/camerad/cameras/camera_common.cc @@ -15,8 +15,6 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, SpectraCamera * const SensorInfo *sensor = cam->sensor.get(); is_raw = cam->output_type == ISP_RAW_OUTPUT; - frame_metadata = std::make_unique(frame_buf_count); - // RAW frames from ISP if (cam->output_type != ISP_IFE_PROCESSED) { camera_bufs_raw = std::make_unique(frame_buf_count); @@ -48,15 +46,9 @@ CameraBuf::~CameraBuf() { } } -bool CameraBuf::acquire() { - if (!safe_queue.try_pop(cur_buf_idx, 0)) return false; - - if (frame_metadata[cur_buf_idx].frame_id == -1) { - LOGE("no frame data? wtf"); - return false; - } +void CameraBuf::sendFrameToVipc() { + assert(cur_buf_idx >=0 && cur_buf_idx < frame_buf_count); - cur_frame_data = frame_metadata[cur_buf_idx]; if (camera_bufs_raw) { cur_camera_buf = &camera_bufs_raw[cur_buf_idx]; } @@ -71,12 +63,6 @@ bool CameraBuf::acquire() { }; cur_yuv_buf->set_frame_id(cur_frame_data.frame_id); vipc_server->send(cur_yuv_buf, &extra); - - return true; -} - -void CameraBuf::queue(size_t buf_idx) { - safe_queue.push(buf_idx); } // common functions diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h index 37689922f9..e6f5c3e4c4 100644 --- a/system/camerad/cameras/camera_common.h +++ b/system/camerad/cameras/camera_common.h @@ -4,7 +4,6 @@ #include "cereal/messaging/messaging.h" #include "msgq/visionipc/visionipc_server.h" -#include "common/queue.h" #include "common/util.h" @@ -24,8 +23,6 @@ class CameraState; class CameraBuf { private: - int cur_buf_idx; - SafeQueue safe_queue; int frame_buf_count; bool is_raw; @@ -33,18 +30,17 @@ public: VisionIpcServer *vipc_server; VisionStreamType stream_type; + int cur_buf_idx; FrameMetadata cur_frame_data; VisionBuf *cur_yuv_buf; VisionBuf *cur_camera_buf; std::unique_ptr camera_bufs_raw; - std::unique_ptr frame_metadata; int out_img_width, out_img_height; CameraBuf() = default; ~CameraBuf(); void init(cl_device_id device_id, cl_context context, SpectraCamera *cam, VisionIpcServer * v, int frame_cnt, VisionStreamType type); - bool acquire(); - void queue(size_t buf_idx); + void sendFrameToVipc(); }; void camerad_thread(); diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 4fb08f0e1e..d969d092e9 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -221,7 +221,7 @@ void CameraState::set_camera_exposure(float grey_frac) { } void CameraState::sendState() { - if (!camera.buf.acquire()) return; + camera.buf.sendFrameToVipc(); MessageBuilder msg; auto framed = (msg.initEvent().*camera.cc.init_camera_state)(); @@ -306,8 +306,9 @@ void camerad_thread() { for (auto &cam : cams) { if (event_data->session_hdl == cam->camera.session_handle) { - cam->camera.handle_camera_event(event_data); - cam->sendState(); + if (cam->camera.handle_camera_event(event_data)) { + cam->sendState(); + } break; } } diff --git a/system/camerad/cameras/spectra.cc b/system/camerad/cameras/spectra.cc index e7ee891a35..79dc83d9d6 100644 --- a/system/camerad/cameras/spectra.cc +++ b/system/camerad/cameras/spectra.cc @@ -306,14 +306,13 @@ 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, 0); + enqueue_req_multi(1, ife_buf_depth); } -void SpectraCamera::enqueue_req_multi(uint64_t start, int n, bool dp) { - for (uint64_t i = start; i < start + n; ++i) { - uint64_t idx = (i - 1) % ife_buf_depth; - request_ids[idx] = i; - enqueue_buffer(idx, dp); +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); } } @@ -904,9 +903,10 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) { assert(ret == 0); } -void SpectraCamera::enqueue_buffer(int i, bool dp) { +// 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; - uint64_t request_id = request_ids[i]; + bool frame_ready = false; // Before queuing up a new frame, wait for the // previous one in this slot (index) to come in. @@ -935,13 +935,9 @@ void SpectraCamera::enqueue_buffer(int i, bool dp) { } } - // in IFE_PROCESSED mode, we can't know the true EOF, so recover it with sensor readout time - buf.frame_metadata[i].timestamp_eof = buf.frame_metadata[i].timestamp_sof + sensor->readout_time_ns; - buf.frame_metadata[i].timestamp_end_of_isp = (uint64_t)nanos_since_boot(); - // all good, hand off frame - if (dp && ret == 0) { - buf.queue(i); + if (ret == 0) { + frame_ready = true; } if (ret != 0) { @@ -984,6 +980,8 @@ void SpectraCamera::enqueue_buffer(int i, bool dp) { // 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) { @@ -1368,8 +1366,9 @@ void SpectraCamera::camera_close() { LOGD("destroyed session %d: %d", cc.camera_num, ret); } -void SpectraCamera::handle_camera_event(const cam_req_mgr_message *event_data) { - if (!enabled) return; +// 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 (!enabled) return false; // ID from the qcom camera request manager uint64_t request_id = event_data->u.frame_msg.request_id; @@ -1378,13 +1377,11 @@ void SpectraCamera::handle_camera_event(const cam_req_mgr_message *event_data) { uint64_t frame_id_raw = event_data->u.frame_msg.frame_id; if (request_id != 0) { // next ready - int buf_idx = (request_id - 1) % ife_buf_depth; - // 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, 0); + 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; @@ -1393,37 +1390,47 @@ void SpectraCamera::handle_camera_event(const cam_req_mgr_message *event_data) { // 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), 0); + 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)) { - auto &meta_data = buf.frame_metadata[buf_idx]; - meta_data.frame_id = frame_id_raw - camera_sync_data[cc.camera_num].frame_id_offset; - meta_data.request_id = request_id; - meta_data.timestamp_sof = timestamp; - // wait for this frame's EOF, then queue up the next one - enqueue_req_multi(request_id + ife_buf_depth, 1, 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); + if (enqueue_buffer(buf_idx, request_id + ife_buf_depth)) { + // Frame is ready + 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, + // in IFE_PROCESSED mode, we can't know the true EOF, so recover it with sensor readout time + .timestamp_eof = timestamp + sensor->readout_time_ns, + .timestamp_end_of_isp = (uint64_t)nanos_since_boot(), + }; + 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, false); - //LOGW("camerad %d not synced req %d fid %d", cc.camera_num, (int)request_id, (int)frame_id_raw); + 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, 0); + enqueue_req_multi(request_id_last + 1, ife_buf_depth); frame_id_raw_last = frame_id_raw; skipped_last = true; } } + + return false; } bool SpectraCamera::syncFirstFrame(int camera_id, uint64_t request_id, uint64_t raw_id, uint64_t timestamp) { diff --git a/system/camerad/cameras/spectra.h b/system/camerad/cameras/spectra.h index bef82a798c..a606d2afa6 100644 --- a/system/camerad/cameras/spectra.h +++ b/system/camerad/cameras/spectra.h @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -121,15 +122,15 @@ public: ~SpectraCamera(); void camera_open(VisionIpcServer *v, cl_device_id device_id, cl_context ctx); - void handle_camera_event(const cam_req_mgr_message *event_data); + bool handle_camera_event(const cam_req_mgr_message *event_data); void camera_close(); void camera_map_bufs(); void config_bps(int idx, int request_id); void config_ife(int idx, int request_id, bool init=false); int clear_req_queue(); - void enqueue_buffer(int i, bool dp); - void enqueue_req_multi(uint64_t start, int n, bool dp); + bool enqueue_buffer(int i, uint64_t request_id); + void enqueue_req_multi(uint64_t start, int n); int sensors_init(); void sensors_start();