camerad: get embedded statistics and data by embedding in pixel data (#24353)

* camerad: AR0231 read embedded data and statistics

* reorder

* remove unrelated camera config

selfdrive/camerad/cameras/camera_common.cc

@@ -36,10 +36,10 @@ public:
     hdr_ = ci->hdr;
     snprintf(args, sizeof(args),
              "-cl-fast-relaxed-math -cl-denorms-are-zero "
-             "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d "
+             "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d -DFRAME_OFFSET=%d "
              "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d "
              "-DBAYER_FLIP=%d -DHDR=%d -DCAM_NUM=%d",
-             ci->frame_width, ci->frame_height, ci->frame_stride,
+             ci->frame_width, ci->frame_height, ci->frame_stride, ci->frame_offset,
              b->rgb_width, b->rgb_height, b->rgb_stride,
              ci->bayer_flip, ci->hdr, s->camera_num);
     const char *cl_file = "cameras/real_debayer.cl";
@@ -81,7 +81,7 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s,
   frame_buf_count = frame_cnt;
 
   // RAW frame
-  const int frame_size = ci->frame_height * ci->frame_stride;
+  const int frame_size = (ci->frame_height + ci->extra_height) * ci->frame_stride;
   camera_bufs = std::make_unique<VisionBuf[]>(frame_buf_count);
   camera_bufs_metadata = std::make_unique<FrameMetadata[]>(frame_buf_count);
 

selfdrive/camerad/cameras/camera_common.h

@@ -52,11 +52,15 @@ const bool env_log_raw_frames = getenv("LOG_RAW_FRAMES") != NULL;
 typedef void (*release_cb)(void *cookie, int buf_idx);
 
 typedef struct CameraInfo {
-  int frame_width, frame_height;
-  int frame_stride;
+  uint32_t frame_width, frame_height;
+  uint32_t frame_stride;
   bool bayer;
   int bayer_flip;
   bool hdr;
+  uint32_t frame_offset = 0;
+  uint32_t extra_height = 0;
+  int registers_offset = -1;
+  int stats_offset = -1;
 } CameraInfo;
 
 typedef struct FrameMetadata {

selfdrive/camerad/cameras/camera_qcom2.cc

@@ -31,6 +31,9 @@ const size_t FRAME_WIDTH = 1928;
 const size_t FRAME_HEIGHT = 1208;
 const size_t FRAME_STRIDE = 2896;  // for 12 bit output. 1928 * 12 / 8 + 4 (alignment)
 
+const size_t AR0231_REGISTERS_HEIGHT = 2;
+const size_t AR0231_STATS_HEIGHT = 2;
+
 const int MIPI_SETTLE_CNT = 33;  // Calculated by camera_freqs.py
 
 CameraInfo cameras_supported[CAMERA_ID_MAX] = {
@@ -38,17 +41,24 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = {
     .frame_width = FRAME_WIDTH,
     .frame_height = FRAME_HEIGHT,
     .frame_stride = FRAME_STRIDE,
+    .extra_height = AR0231_REGISTERS_HEIGHT + AR0231_STATS_HEIGHT,
+
+    .registers_offset = 0,
+    .frame_offset = AR0231_REGISTERS_HEIGHT,
+    .stats_offset = AR0231_REGISTERS_HEIGHT + FRAME_HEIGHT,
+
     .bayer = true,
     .bayer_flip = 1,
-    .hdr = false
+    .hdr = false,
   },
   [CAMERA_ID_IMX390] = {
     .frame_width = FRAME_WIDTH,
     .frame_height = FRAME_HEIGHT,
     .frame_stride = FRAME_STRIDE,
+
     .bayer = true,
     .bayer_flip = 1,
-    .hdr = false
+    .hdr = false,
   },
 };
 
@@ -509,10 +519,10 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b
   if (io_mem_handle != 0) {
     io_cfg[0].mem_handle[0] = io_mem_handle;
 		io_cfg[0].planes[0] = (struct cam_plane_cfg){
-		 .width = FRAME_WIDTH,
-		 .height = FRAME_HEIGHT,
-		 .plane_stride = FRAME_STRIDE,
-		 .slice_height = FRAME_HEIGHT,
+		 .width = ci.frame_width,
+		 .height = ci.frame_height + ci.extra_height,
+		 .plane_stride = ci.frame_stride,
+		 .slice_height = ci.frame_height + ci.extra_height,
 		 .meta_stride = 0x0,    // YUV has meta(stride=0x400, size=0x5000)
 		 .meta_size = 0x0,
 		 .meta_offset = 0x0,
@@ -682,23 +692,23 @@ void CameraState::camera_open() {
       .lane_cfg = 0x3210,
 
       .vc = 0x0,
-      .dt = 0x2C,  // CSI_RAW12
+      .dt = 0x12, // Changing stats to 0x2C doesn't work, so change pixels to 0x12 instead
       .format = CAM_FORMAT_MIPI_RAW_12,
 
       .test_pattern = 0x2,  // 0x3?
       .usage_type = 0x0,
 
       .left_start = 0,
-      .left_stop = FRAME_WIDTH - 1,
-      .left_width = FRAME_WIDTH,
+      .left_stop = ci.frame_width - 1,
+      .left_width = ci.frame_width,
 
       .right_start = 0,
-      .right_stop = FRAME_WIDTH - 1,
-      .right_width = FRAME_WIDTH,
+      .right_stop = ci.frame_width - 1,
+      .right_width = ci.frame_width,
 
       .line_start = 0,
-      .line_stop = FRAME_HEIGHT - 1,
-      .height = FRAME_HEIGHT,
+      .line_stop = ci.frame_height + ci.extra_height - 1,
+      .height = ci.frame_height + ci.extra_height,
 
       .pixel_clk = 0x0,
       .batch_size = 0x0,
@@ -710,8 +720,8 @@ void CameraState::camera_open() {
       .data[0] = (struct cam_isp_out_port_info){
           .res_type = CAM_ISP_IFE_OUT_RES_RDI_0,
           .format = CAM_FORMAT_MIPI_RAW_12,
-          .width = FRAME_WIDTH,
-          .height = FRAME_HEIGHT,
+          .width = ci.frame_width,
+          .height = ci.frame_height + ci.extra_height,
           .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0,
       },
   };
@@ -935,6 +945,70 @@ void cameras_close(MultiCameraState *s) {
   delete s->pm;
 }
 
+std::map<uint16_t, std::pair<int, int>> CameraState::ar0231_build_register_lut(uint8_t *data) {
+  // This function builds a lookup table from register address, to a pair of indices in the
+  // buffer where to read this address. The buffer contains padding bytes,
+  // as well as markers to indicate the type of the next byte.
+  //
+  // 0xAA is used to indicate the MSB of the address, 0xA5 for the LSB of the address.
+  // Every byte of data (MSB and LSB) is preceded by 0x5A. Specifying an address is optional
+  // for contigous ranges. See page 27-29 of the AR0231 Developer guide for more information.
+
+  int max_i[] = {1828 / 2 * 3, 1500 / 2 * 3};
+  auto get_next_idx = [](int cur_idx) {
+    return (cur_idx % 3 == 1) ? cur_idx + 2 : cur_idx + 1; // Every third byte is padding
+  };
+
+  std::map<uint16_t, std::pair<int, int>> registers;
+  for (int register_row = 0; register_row < 2; register_row++) {
+    uint8_t *registers_raw = data + ci.frame_stride * register_row;
+    assert(registers_raw[0] == 0x0a); // Start of line
+
+    int value_tag_count = 0;
+    int first_val_idx = 0;
+    uint16_t cur_addr = 0;
+
+    for (int i = 1; i <= max_i[register_row]; i = get_next_idx(get_next_idx(i))) {
+      int val_idx = get_next_idx(i);
+
+      uint8_t tag = registers_raw[i];
+      uint16_t val = registers_raw[val_idx];
+
+      if (tag == 0xAA) { // Register MSB tag
+        cur_addr = val << 8;
+      } else if (tag == 0xA5) { // Register LSB tag
+        cur_addr |= val;
+        cur_addr -= 2; // Next value tag will increment address again
+      } else if (tag == 0x5A) { // Value tag
+
+        // First tag
+        if (value_tag_count % 2 == 0) {
+          cur_addr += 2;
+          first_val_idx = val_idx;
+        } else {
+          registers[cur_addr] = std::make_pair(first_val_idx + ci.frame_stride * register_row, val_idx + ci.frame_stride * register_row);
+        }
+
+        value_tag_count++;
+      }
+    }
+  }
+  return registers;
+}
+
+std::map<uint16_t, uint16_t> CameraState::ar0231_parse_registers(uint8_t *data, std::initializer_list<uint16_t> addrs) {
+  if (ar0231_register_lut.empty()) {
+    ar0231_register_lut = ar0231_build_register_lut(data);
+  }
+
+  std::map<uint16_t, uint16_t> registers;
+  for (uint16_t addr : addrs) {
+    auto offset = ar0231_register_lut[addr];
+    registers[addr] = ((uint16_t)data[offset.first] << 8) | data[offset.second];
+  }
+  return registers;
+}
+
 void CameraState::handle_camera_event(void *evdat) {
   if (!enabled) return;
   struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)evdat;
@@ -1114,6 +1188,25 @@ void camera_autoexposure(CameraState *s, float grey_frac) {
   s->set_camera_exposure(grey_frac);
 }
 
+static float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_reg) {
+  // See AR0231 Developer Guide - page 36
+  float slope = (125.0 - 55.0) / ((float)calib1 - (float)calib2);
+  float t0 = 55.0 - slope * (float)calib2;
+  return t0 + slope * (float)data_reg;
+}
+
+static void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed){
+  uint8_t *data = (uint8_t*)c->buf.cur_camera_buf->addr + c->ci.registers_offset;
+  auto registers = c->ar0231_parse_registers(data, {0x2000, 0x2002, 0x20b0, 0x20b2, 0x30c6, 0x30c8, 0x30ca, 0x30cc});
+
+  uint32_t frame_id = ((uint32_t)registers[0x2000] << 16) | registers[0x2002];
+  framed.setFrameIdSensor(frame_id);
+
+  float temp_0 = ar0231_parse_temp_sensor(registers[0x30c6], registers[0x30c8], registers[0x20b0]);
+  float temp_1 = ar0231_parse_temp_sensor(registers[0x30ca], registers[0x30cc], registers[0x20b2]);
+  framed.setTemperaturesC({temp_0, temp_1});
+}
+
 static void driver_cam_auto_exposure(CameraState *c, SubMaster &sm) {
   struct ExpRect {int x1, x2, x_skip, y1, y2, y_skip;};
   const CameraBuf *b = &c->buf;
@@ -1132,10 +1225,12 @@ static void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt)
   if (env_send_driver) {
     framed.setImage(get_frame_image(&c->buf));
   }
+  if (c->camera_id == CAMERA_ID_AR0231) {
+    ar0231_process_registers(s, c, framed);
+  }
   s->pm->send("driverCameraState", msg);
 }
 
-// called by processing_thread
 void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
   const CameraBuf *b = &c->buf;
 
@@ -1152,6 +1247,11 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
     framed.setTransform(b->yuv_transform.v);
     LOGT(c->buf.cur_frame_data.frame_id, "%s: Transformed", "RoadCamera");
   }
+
+  if (c->camera_id == CAMERA_ID_AR0231) {
+    ar0231_process_registers(s, c, framed);
+  }
+
   s->pm->send(c == &s->road_cam ? "roadCameraState" : "wideRoadCameraState", msg);
 
   const auto [x, y, w, h] = (c == &s->wide_road_cam) ? std::tuple(96, 250, 1734, 524) : std::tuple(96, 160, 1734, 986);
@@ -1221,3 +1321,4 @@ void cameras_run(MultiCameraState *s) {
 
   cameras_close(s);
 }
+

selfdrive/camerad/cameras/camera_qcom2.h

@@ -1,6 +1,8 @@
 #pragma once
 
 #include <cstdint>
+#include <map>
+#include <utility>
 
 #include <media/cam_req_mgr.h>
 
@@ -56,6 +58,8 @@ public:
   void camera_init(MultiCameraState *multi_cam_state, VisionIpcServer * v, int camera_id, int camera_num, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type, bool enabled);
   void camera_close();
 
+  std::map<uint16_t, uint16_t> ar0231_parse_registers(uint8_t *data, std::initializer_list<uint16_t> addrs);
+
   int32_t session_handle;
   int32_t sensor_dev_handle;
   int32_t isp_dev_handle;
@@ -75,6 +79,7 @@ public:
 
   CameraBuf buf;
   MemoryManager mm;
+
 private:
   void config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset);
   void enqueue_req_multi(int start, int n, bool dp);
@@ -84,6 +89,10 @@ private:
   int sensors_init();
   void sensors_poke(int request_id);
   void sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word);
+
+  // Register parsing
+  std::map<uint16_t, std::pair<int, int>> ar0231_register_lut;
+  std::map<uint16_t, std::pair<int, int>> ar0231_build_register_lut(uint8_t *data);
 };
 
 typedef struct MultiCameraState {

selfdrive/camerad/cameras/camera_replay.cc

@@ -30,9 +30,9 @@ void camera_init(VisionIpcServer *v, CameraState *s, int camera_id, unsigned int
   }
 
   CameraInfo ci = {
-      .frame_width = s->frame->width,
-      .frame_height = s->frame->height,
-      .frame_stride = s->frame->width * 3,
+      .frame_width = (uint32_t)s->frame->width,
+      .frame_height = (uint32_t)s->frame->height,
+      .frame_stride = (uint32_t)s->frame->width * 3,
   };
   s->ci = ci;
   s->camera_num = camera_id;

selfdrive/camerad/cameras/real_debayer.cl

@@ -42,7 +42,7 @@ half3 color_correct(half3 rgb) {
 
 inline half val_from_10(const uchar * source, int gx, int gy, half black_level) {
   // parse 12bit
-  int start = gy * FRAME_STRIDE + (3 * (gx / 2));
+  int start = gy * FRAME_STRIDE + (3 * (gx / 2)) + (FRAME_STRIDE * FRAME_OFFSET);
   int offset = gx % 2;
   uint major = (uint)source[start + offset] << 4;
   uint minor = (source[start + 2] >> (4 * offset)) & 0xf;

selfdrive/camerad/cameras/sensor2_i2c.h

@@ -7,7 +7,7 @@ struct i2c_random_wr_payload init_array_imx390[] = {
   {0x2008, 0xd0}, {0x2009, 0x07}, {0x200a, 0x00}, // MODE_VMAX = time between frames
   {0x200C, 0xe4}, {0x200D, 0x0c},  // MODE_HMAX
 
-  // crop 
+  // crop
   {0x3410, 0x88}, {0x3411, 0x7},     // CROP_H_SIZE
   {0x3418, 0xb8}, {0x3419, 0x4},     // CROP_V_SIZE
   {0x0078, 1}, {0x03c0, 1},
@@ -67,7 +67,7 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
   {0x30A6, 0x0001}, // Y_ODD_INC_
   {0x3402, 0x0788}, // X_OUTPUT_CONTROL
   {0x3404, 0x04B8}, // Y_OUTPUT_CONTROL
-  {0x3064, 0x1802}, // SMIA_TEST
+  {0x3064, 0x1982}, // SMIA_TEST
   {0x30BA, 0x11F2}, // DIGITAL_CTRL
 
   // Enable external trigger and disable GPIO outputs
@@ -83,10 +83,10 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
   // Readout Settings
   {0x31AE, 0x0204}, // SERIAL_FORMAT, 4-lane MIPI
   {0x31AC, 0x0C0C}, // DATA_FORMAT_BITS, 12 -> 12
-  {0x3342, 0x122C}, // MIPI_F1_PDT_EDT
-  {0x3346, 0x122C}, // MIPI_F2_PDT_EDT
-  {0x334A, 0x122C}, // MIPI_F3_PDT_EDT
-  {0x334E, 0x122C}, // MIPI_F4_PDT_EDT
+  {0x3342, 0x1212}, // MIPI_F1_PDT_EDT
+  {0x3346, 0x1212}, // MIPI_F2_PDT_EDT
+  {0x334A, 0x1212}, // MIPI_F3_PDT_EDT
+  {0x334E, 0x1212}, // MIPI_F4_PDT_EDT
   {0x3344, 0x0011}, // MIPI_F1_VDT_VC
   {0x3348, 0x0111}, // MIPI_F2_VDT_VC
   {0x334C, 0x0211}, // MIPI_F3_VDT_VC
@@ -101,6 +101,10 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
   {0x3370, 0x03B1}, // DBLC
   {0x3044, 0x0400}, // DARK_CONTROL
 
+  // Enable temperature sensor
+  {0x30B4, 0x0007}, // TEMPSENS0_CTRL_REG
+  {0x30B8, 0x0007}, // TEMPSENS1_CTRL_REG
+
   // Enable dead pixel correction using
   // the 1D line correction scheme
   {0x31E0, 0x0003},
@@ -114,9 +118,7 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
   {0x100E, 0x07B1}, // FINE_INTEGRATION_TIME3_MIN
   {0x1010, 0x0139}, // FINE_INTEGRATION_TIME4_MIN
   {0x3014, 0x08CB}, // FINE_INTEGRATION_TIME_
-  {0x321E, 0x08CB}, // FINE_INTEGRATION_TIME2
-  {0x321E, 0x08CB}, // FINE_INTEGRATION_TIME3
-  {0x321E, 0x0894}, // FINE_INTEGRATION_TIME4
+  {0x321E, 0x0894}, // FINE_INTEGRATION_TIME2
 
   {0x31D0, 0x0000}, // COMPANDING, no good in 10 bit?
   {0x33DA, 0x0000}, // COMPANDING