camerad: refactor frame processing to direct handling (#34655)

refactor frame processing

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<FrameMetadata[]>(frame_buf_count);
-
   // RAW frames from ISP
   if (cam->output_type != ISP_IFE_PROCESSED) {
     camera_bufs_raw = std::make_unique<VisionBuf[]>(frame_buf_count);
@@ -48,15 +46,9 @@ CameraBuf::~CameraBuf() {
   }
 }
 
-bool CameraBuf::acquire() {
-  if (!safe_queue.try_pop(cur_buf_idx, 0)) return false;
+void CameraBuf::sendFrameToVipc() {
+  assert(cur_buf_idx >=0 && cur_buf_idx < frame_buf_count);
 
-  if (frame_metadata[cur_buf_idx].frame_id == -1) {
-    LOGE("no frame data? wtf");
-    return false;
-  }
-
-  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

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<int> 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<VisionBuf[]> camera_bufs_raw;
-  std::unique_ptr<FrameMetadata[]> 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();

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;
           }
         }

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) {

system/camerad/cameras/spectra.h

@@ -4,6 +4,7 @@
 #include <functional>
 #include <memory>
 #include <mutex>
+#include <queue>
 #include <optional>
 #include <utility>
 
@@ -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();