Release 260111

system/camerad/cameras/camera_common.h

@@ -0,0 +1,46 @@
+#pragma once
+
+#include <memory>
+
+#include "cereal/messaging/messaging.h"
+#include "msgq/visionipc/visionipc_server.h"
+#include "common/util.h"
+
+
+const int VIPC_BUFFER_COUNT = 18;
+
+typedef struct FrameMetadata {
+  uint32_t frame_id;
+  uint32_t request_id;
+  uint64_t timestamp_sof;
+  uint64_t timestamp_eof;
+  float processing_time;
+} FrameMetadata;
+
+class SpectraCamera;
+
+class CameraBuf {
+private:
+  int frame_buf_count;
+
+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;
+  uint32_t 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);
+  void sendFrameToVipc();
+};
+
+void camerad_thread();
+kj::Array<uint8_t> get_raw_frame_image(const CameraBuf *b);
+float calculate_exposure_value(const CameraBuf *b, Rect ae_xywh, int x_skip, int y_skip);
+int open_v4l_by_name_and_index(const char name[], int index = 0, int flags = O_RDWR | O_NONBLOCK);

system/camerad/cameras/cdm.h

@@ -0,0 +1,79 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+#include <vector>
+#include <memory>
+
+// our helpers
+int write_random(uint8_t *dst, const std::vector<uint32_t> &vals);
+int write_cont(uint8_t *dst, uint32_t reg, const std::vector<uint32_t> &vals);
+int write_dmi(uint8_t *dst, uint64_t *addr, uint32_t length, uint32_t dmi_addr, uint8_t sel);
+
+// from drivers/media/platform/msm/camera/cam_cdm/cam_cdm_util.{c,h}
+
+enum cam_cdm_command {
+  CAM_CDM_CMD_UNUSED = 0x0,
+  CAM_CDM_CMD_DMI = 0x1,
+  CAM_CDM_CMD_NOT_DEFINED = 0x2,
+  CAM_CDM_CMD_REG_CONT = 0x3,
+  CAM_CDM_CMD_REG_RANDOM = 0x4,
+  CAM_CDM_CMD_BUFF_INDIRECT = 0x5,
+  CAM_CDM_CMD_GEN_IRQ = 0x6,
+  CAM_CDM_CMD_WAIT_EVENT = 0x7,
+  CAM_CDM_CMD_CHANGE_BASE = 0x8,
+  CAM_CDM_CMD_PERF_CTRL = 0x9,
+  CAM_CDM_CMD_DMI_32 = 0xa,
+  CAM_CDM_CMD_DMI_64 = 0xb,
+  CAM_CDM_CMD_PRIVATE_BASE = 0xc,
+  CAM_CDM_CMD_SWD_DMI_32 = (CAM_CDM_CMD_PRIVATE_BASE + 0x64),
+  CAM_CDM_CMD_SWD_DMI_64 = (CAM_CDM_CMD_PRIVATE_BASE + 0x65),
+  CAM_CDM_CMD_PRIVATE_BASE_MAX = 0x7F
+};
+
+/**
+ * struct cdm_regrandom_cmd - Definition for CDM random register command.
+ * @count: Number of register writes
+ * @reserved: reserved bits
+ * @cmd: Command ID (CDMCmd)
+ */
+struct cdm_regrandom_cmd {
+  unsigned int count    : 16;
+  unsigned int reserved : 8;
+  unsigned int cmd      : 8;
+} __attribute__((__packed__));
+
+/**
+ * struct cdm_regcontinuous_cmd - Definition for a CDM register range command.
+ * @count: Number of register writes
+ * @reserved0: reserved bits
+ * @cmd: Command ID (CDMCmd)
+ * @offset: Start address of the range of registers
+ * @reserved1: reserved bits
+ */
+struct cdm_regcontinuous_cmd {
+  unsigned int count     : 16;
+  unsigned int reserved0 : 8;
+  unsigned int cmd       : 8;
+  unsigned int offset    : 24;
+  unsigned int reserved1 : 8;
+} __attribute__((__packed__));
+
+/**
+ * struct cdm_dmi_cmd - Definition for a CDM DMI command.
+ * @length: Number of bytes in LUT - 1
+ * @reserved: reserved bits
+ * @cmd: Command ID (CDMCmd)
+ * @addr: Address of the LUT in memory
+ * @DMIAddr: Address of the target DMI config register
+ * @DMISel: DMI identifier
+ */
+struct cdm_dmi_cmd {
+  unsigned int length   : 16;
+  unsigned int reserved : 8;
+  unsigned int cmd      : 8;
+  unsigned int addr;
+  unsigned int DMIAddr  : 24;
+  unsigned int DMISel   : 8;
+} __attribute__((__packed__));

system/camerad/cameras/hw.h

@@ -0,0 +1,68 @@
+#pragma once
+
+#include "common/util.h"
+#include "cereal/gen/cpp/log.capnp.h"
+#include "msgq/visionipc/visionipc_server.h"
+
+#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 3X three camera platform
+
+struct CameraConfig {
+  int camera_num;
+  VisionStreamType stream_type;
+  float focal_len;  // millimeters
+  const char *publish_name;
+  cereal::FrameData::Builder (cereal::Event::Builder::*init_camera_state)();
+  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
+// If you don't do this, the strobe GPIO is an output (even in reset it seems!)
+const CameraConfig WIDE_ROAD_CAMERA_CONFIG = {
+  .camera_num = 0,
+  .stream_type = VISION_STREAM_WIDE_ROAD,
+  .focal_len = 1.71,
+  .publish_name = "wideRoadCameraState",
+  .init_camera_state = &cereal::Event::Builder::initWideRoadCameraState,
+  .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 = {
+  .camera_num = 1,
+  .stream_type = VISION_STREAM_ROAD,
+  .focal_len = 8.0,
+  .publish_name = "roadCameraState",
+  .init_camera_state = &cereal::Event::Builder::initRoadCameraState,
+  .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 = {
+  .camera_num = 2,
+  .stream_type = VISION_STREAM_DRIVER,
+  .focal_len = 1.71,
+  .publish_name = "driverCameraState",
+  .init_camera_state = &cereal::Event::Builder::initDriverCameraState,
+  .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/ife.h

@@ -0,0 +1,236 @@
+#pragma once
+
+#include "cdm.h"
+
+#include "system/camerad/cameras/hw.h"
+#include "system/camerad/sensors/sensor.h"
+
+int build_common_ife_bps(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std::vector<uint32_t> &patches, bool ife) {
+  uint8_t *start = dst;
+
+  /*
+    Common between IFE and BPS.
+  */
+
+  // IFE -> BPS addresses
+  /*
+  std::map<uint32_t, uint32_t> addrs = {
+    {0xf30, 0x3468},
+  };
+  */
+
+  // YUV
+  dst += write_cont(dst, ife ? 0xf30 : 0x3468, {
+    0x00680208,
+    0x00000108,
+    0x00400000,
+    0x03ff0000,
+    0x01c01ed8,
+    0x00001f68,
+    0x02000000,
+    0x03ff0000,
+    0x1fb81e88,
+    0x000001c0,
+    0x02000000,
+    0x03ff0000,
+  });
+
+  return dst - start;
+}
+
+int build_update(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std::vector<uint32_t> &patches) {
+  uint8_t *start = dst;
+
+  // init sequence
+  dst += write_random(dst, {
+    0x2c, 0xffffffff,
+    0x30, 0xffffffff,
+    0x34, 0xffffffff,
+    0x38, 0xffffffff,
+    0x3c, 0xffffffff,
+  });
+
+  // demux cfg
+  dst += write_cont(dst, 0x560, {
+    0x00000001,
+    0x04440444,
+    0x04450445,
+    0x04440444,
+    0x04450445,
+    0x000000ca,
+    0x0000009c,
+  });
+
+  // white balance
+  dst += write_cont(dst, 0x6fc, {
+    0x00800080,
+    0x00000080,
+    0x00000000,
+    0x00000000,
+  });
+
+  // module config/enables (e.g. enable debayer, white balance, etc.)
+  dst += write_cont(dst, 0x40, {
+    0x00000c06 | ((uint32_t)(cam.vignetting_correction) << 8),
+  });
+  dst += write_cont(dst, 0x44, {
+    0x00000000,
+  });
+  dst += write_cont(dst, 0x48, {
+    (1 << 3) | (1 << 1),
+  });
+  dst += write_cont(dst, 0x4c, {
+    0x00000019,
+  });
+  dst += write_cont(dst, 0xf00, {
+    0x00000000,
+  });
+
+  // cropping
+  dst += write_cont(dst, 0xe0c, {
+    0x00000e00,
+  });
+  dst += write_cont(dst, 0xe2c, {
+    0x00000e00,
+  });
+
+  // black level scale + offset
+  dst += write_cont(dst, 0x6b0, {
+    ((uint32_t)(1 << 11) << 0xf) | (s->black_level << (14 - s->bits_per_pixel)),
+    0x0,
+    0x0,
+  });
+
+  return dst - start;
+}
+
+
+int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std::vector<uint32_t> &patches, uint32_t out_width, uint32_t out_height) {
+  uint8_t *start = dst;
+
+  // start with the every frame config
+  dst += build_update(dst, cam, s, patches);
+
+  uint64_t addr;
+
+  // setup
+  dst += write_cont(dst, 0x478, {
+    0x00000004,
+    0x004000c0,
+  });
+  dst += write_cont(dst, 0x488, {
+    0x00000000,
+    0x00000000,
+    0x00000f0f,
+  });
+  dst += write_cont(dst, 0x49c, {
+    0x00000001,
+  });
+  dst += write_cont(dst, 0xce4, {
+    0x00000000,
+    0x00000000,
+  });
+
+  // linearization
+  dst += write_cont(dst, 0x4dc, {
+    0x00000000,
+  });
+  dst += write_cont(dst, 0x4e0, s->linearization_pts);
+  dst += write_cont(dst, 0x4f0, s->linearization_pts);
+  dst += write_cont(dst, 0x500, s->linearization_pts);
+  dst += write_cont(dst, 0x510, s->linearization_pts);
+  // TODO: this is DMI64 in the dump, does that matter?
+  dst += write_dmi(dst, &addr, s->linearization_lut.size()*sizeof(uint32_t), 0xc24, 9);
+  patches.push_back(addr - (uint64_t)start);
+
+  // vignetting correction
+  dst += write_cont(dst, 0x6bc, {
+    0x0b3c0000,
+    0x00670067,
+    0xd3b1300c,
+    0x13b1300c,
+  });
+  dst += write_cont(dst, 0x6d8, {
+    0xec4e4000,
+    0x0100c003,
+  });
+  dst += write_dmi(dst, &addr, s->vignetting_lut.size()*sizeof(uint32_t), 0xc24, 14); // GRR
+  patches.push_back(addr - (uint64_t)start);
+  dst += write_dmi(dst, &addr, s->vignetting_lut.size()*sizeof(uint32_t), 0xc24, 15); // GBB
+  patches.push_back(addr - (uint64_t)start);
+
+  // debayer
+  dst += write_cont(dst, 0x6f8, {
+    0x00000100,
+  });
+  dst += write_cont(dst, 0x71c, {
+    0x00008000,
+    0x08000066,
+  });
+
+  // color correction
+  dst += write_cont(dst, 0x760, s->color_correct_matrix);
+
+  // gamma
+  dst += write_cont(dst, 0x798, {
+    0x00000000,
+  });
+  dst += write_dmi(dst, &addr, s->gamma_lut_rgb.size()*sizeof(uint32_t), 0xc24, 26);  // G
+  patches.push_back(addr - (uint64_t)start);
+  dst += write_dmi(dst, &addr, s->gamma_lut_rgb.size()*sizeof(uint32_t), 0xc24, 28);  // B
+  patches.push_back(addr - (uint64_t)start);
+  dst += write_dmi(dst, &addr, s->gamma_lut_rgb.size()*sizeof(uint32_t), 0xc24, 30);  // R
+  patches.push_back(addr - (uint64_t)start);
+
+  // output size/scaling
+  dst += write_cont(dst, 0xa3c, {
+    0x00000003,
+    ((out_width - 1) << 16) | (s->frame_width - 1),
+    0x30036666,
+    0x00000000,
+    0x00000000,
+    s->frame_width - 1,
+    ((out_height - 1) << 16) | (s->frame_height - 1),
+    0x30036666,
+    0x00000000,
+    0x00000000,
+    s->frame_height - 1,
+  });
+  dst += write_cont(dst, 0xa68, {
+    0x00000003,
+    ((out_width / 2 - 1) << 16) | (s->frame_width - 1),
+    0x3006cccc,
+    0x00000000,
+    0x00000000,
+    s->frame_width - 1,
+    ((out_height / 2 - 1) << 16) | (s->frame_height - 1),
+    0x3006cccc,
+    0x00000000,
+    0x00000000,
+    s->frame_height - 1,
+  });
+
+  // cropping
+  dst += write_cont(dst, 0xe10, {
+    out_height - 1,
+    out_width - 1,
+  });
+  dst += write_cont(dst, 0xe30, {
+    out_height / 2 - 1,
+    out_width - 1,
+  });
+  dst += write_cont(dst, 0xe18, {
+    0x0ff00000,
+    0x00000016,
+  });
+  dst += write_cont(dst, 0xe38, {
+    0x0ff00000,
+    0x00000017,
+  });
+
+  dst += build_common_ife_bps(dst, cam, s, patches, true);
+
+  return dst - start;
+}
+
+

system/camerad/cameras/spectra.h

@@ -0,0 +1,219 @@
+#pragma once
+
+#include <sys/mman.h>
+#include <functional>
+#include <memory>
+#include <queue>
+#include <optional>
+#include <utility>
+
+#include "media/cam_req_mgr.h"
+
+#include "common/util.h"
+#include "common/swaglog.h"
+#include "system/camerad/cameras/hw.h"
+#include "system/camerad/cameras/camera_common.h"
+#include "system/camerad/sensors/sensor.h"
+
+#define MAX_IFE_BUFS 20
+
+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)
+#define CSLDeviceTypeIFE         (0x0F << 24)
+#define CSLDeviceTypeBPS         (0x10 << 24)
+#define OpcodesIFEInitialConfig  0x0
+#define OpcodesIFEUpdate         0x1
+
+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);
+int do_cam_control(int fd, int op_code, void *handle, int size);
+void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE,
+                      int mmu_hdl = 0, int mmu_hdl2 = 0);
+void release(int video0_fd, uint32_t handle);
+
+class MemoryManager {
+public:
+  void init(int _video0_fd) { video0_fd = _video0_fd; }
+  ~MemoryManager();
+
+  template <class T>
+  auto alloc(int len, uint32_t *handle) {
+    return std::unique_ptr<T, std::function<void(void *)>>((T*)alloc_buf(len, handle), [this](void *ptr) { this->free(ptr); });
+  }
+
+private:
+  void *alloc_buf(int len, uint32_t *handle);
+  void free(void *ptr);
+
+  std::map<void *, uint32_t> handle_lookup;
+  std::map<void *, int> size_lookup;
+  std::map<int, std::queue<void *> > cached_allocations;
+  int video0_fd;
+};
+
+class SpectraMaster {
+public:
+  void init();
+
+  unique_fd video0_fd;
+  unique_fd cam_sync_fd;
+  unique_fd isp_fd;
+  unique_fd icp_fd;
+  int device_iommu = -1;
+  int cdm_iommu = -1;
+  int icp_device_iommu = -1;
+  MemoryManager mem_mgr;
+};
+
+class SpectraBuf {
+public:
+  SpectraBuf() = default;
+
+  ~SpectraBuf() {
+    if (video_fd >= 0 && ptr) {
+      munmap(ptr, mmap_size);
+      release(video_fd, handle);
+    }
+  }
+
+  void init(SpectraMaster *m, int s, int a, bool shared_access, int mmu_hdl = 0, int mmu_hdl2 = 0, int count = 1) {
+    video_fd = m->video0_fd;
+    size = s;
+    alignment = a;
+    mmap_size = aligned_size() * count;
+
+    uint32_t flags = CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE;
+    if (shared_access) {
+      flags |= CAM_MEM_FLAG_HW_SHARED_ACCESS;
+    }
+
+    void *p = alloc_w_mmu_hdl(video_fd, mmap_size, (uint32_t*)&handle, alignment, flags, mmu_hdl, mmu_hdl2);
+    ptr = (unsigned char*)p;
+    assert(ptr != NULL);
+  };
+
+  uint32_t aligned_size() {
+    return ALIGNED_SIZE(size, alignment);
+  };
+
+  int video_fd = -1;
+  unsigned char *ptr = nullptr;
+  int size = 0, alignment = 0, handle = 0, mmap_size = 0;
+};
+
+class SpectraCamera {
+public:
+  SpectraCamera(SpectraMaster *master, const CameraConfig &config);
+  ~SpectraCamera();
+
+  void camera_open(VisionIpcServer *v, cl_device_id device_id, cl_context ctx);
+  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_frame(uint64_t request_id);
+
+  int sensors_init();
+  void sensors_start();
+  void sensors_poke(int request_id);
+  void sensors_i2c(const struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word);
+
+  bool openSensor();
+  void configISP();
+  void configICP();
+  void configCSIPHY();
+  void linkDevices();
+  void destroySyncObjectAt(int index);
+
+  // *** state ***
+
+  int ife_buf_depth = -1;
+  bool open = false;
+  bool enabled = true;
+  CameraConfig cc;
+  std::unique_ptr<const SensorInfo> sensor;
+
+  // YUV image size
+  uint32_t stride;
+  uint32_t y_height;
+  uint32_t uv_height;
+  uint32_t uv_offset;
+  uint32_t yuv_size;
+
+  unique_fd sensor_fd;
+  unique_fd csiphy_fd;
+
+  int32_t session_handle = -1;
+  int32_t sensor_dev_handle = -1;
+  int32_t isp_dev_handle = -1;
+  int32_t icp_dev_handle = -1;
+  int32_t csiphy_dev_handle = -1;
+
+  int32_t link_handle = -1;
+
+  SpectraBuf ife_cmd;
+  SpectraBuf ife_gamma_lut;
+  SpectraBuf ife_linearization_lut;
+  SpectraBuf ife_vignetting_lut;
+
+  SpectraBuf bps_cmd;
+  SpectraBuf bps_cdm_buffer;
+  SpectraBuf bps_cdm_program_array;
+  SpectraBuf bps_cdm_striping_bl;
+  SpectraBuf bps_iq;
+  SpectraBuf bps_striping;
+  SpectraBuf bps_linearization_lut;
+  std::vector<uint32_t> bps_lin_reg;
+  std::vector<uint32_t> bps_ccm_reg;
+
+  int buf_handle_yuv[MAX_IFE_BUFS] = {};
+  int buf_handle_raw[MAX_IFE_BUFS] = {};
+  int sync_objs_ife[MAX_IFE_BUFS] = {};
+  int sync_objs_bps[MAX_IFE_BUFS] = {};
+  uint64_t request_id_last = 0;
+  uint64_t last_requeue_ts = 0;
+  uint64_t frame_id_raw_last = 0;
+  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;
+    uint64_t frame_id_offset = 0;
+  };
+  inline static std::map<int, SyncData> camera_sync_data;
+  inline static bool first_frame_synced = false;
+
+  // a mode for stressing edge cases: realignment, sync failures, etc.
+  inline bool stress_test(std::string log) {
+    static double last_trigger = 0;
+    static double prob = std::stod(util::getenv("SPECTRA_ERROR_PROB", "-1"));
+    static double dt = std::stod(util::getenv("SPECTRA_ERROR_DT", "1"));
+    bool triggered = (prob > 0) && \
+                     ((static_cast<double>(rand()) / RAND_MAX) < prob) && \
+                     (millis_since_boot() - last_trigger) > dt;
+    if (triggered) {
+      last_trigger = millis_since_boot();
+      LOGE("stress test (cam %d): %s", cc.camera_num, log.c_str());
+    }
+    return triggered;
+  }
+};

system/camerad/sensors/ar0231_cl.h

@@ -0,0 +1,34 @@
+#if SENSOR_ID == 1
+
+#define VIGNETTE_PROFILE_8DT0MM
+
+#define BIT_DEPTH 12
+#define PV_MAX 4096
+#define BLACK_LVL 168
+
+float4 normalize_pv(int4 parsed, float vignette_factor) {
+  float4 pv = (convert_float4(parsed) - BLACK_LVL) / (PV_MAX - BLACK_LVL);
+  return clamp(pv*vignette_factor, 0.0, 1.0);
+}
+
+float3 color_correct(float3 rgb) {
+  float3 corrected = rgb.x * (float3)(1.82717181, -0.31231438, 0.07307673);
+  corrected += rgb.y * (float3)(-0.5743977, 1.36858544, -0.53183455);
+  corrected += rgb.z * (float3)(-0.25277411, -0.05627105, 1.45875782);
+  return corrected;
+}
+
+float3 apply_gamma(float3 rgb, int expo_time) {
+  // tone mapping params
+  const float gamma_k = 0.75;
+  const float gamma_b = 0.125;
+  const float mp = 0.01; // ideally midpoint should be adaptive
+  const float rk = 9 - 100*mp;
+
+  // poly approximation for s curve
+  return (rgb > mp) ?
+    ((rk * (rgb-mp) * (1-(gamma_k*mp+gamma_b)) * (1+1/(rk*(1-mp))) / (1+rk*(rgb-mp))) + gamma_k*mp + gamma_b) :
+    ((rk * (rgb-mp) * (gamma_k*mp+gamma_b) * (1+1/(rk*mp)) / (1-rk*(rgb-mp))) + gamma_k*mp + gamma_b);
+}
+
+#endif

system/camerad/sensors/ar0231_registers.h

@@ -0,0 +1,121 @@
+#pragma once
+
+const struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}};
+const struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}};
+
+const struct i2c_random_wr_payload init_array_ar0231[] = {
+  {0x301A, 0x0018}, // RESET_REGISTER
+
+  // **NOTE**: if this is changed, readout_time_ns must be updated in the Sensor config
+
+  // CLOCK Settings
+  // input clock is 19.2 / 2 * 0x37 = 528 MHz
+  // pixclk is 528 / 6 = 88 MHz
+  // full roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*FRAME_LENGTH_LINES)) = 39.99 ms
+  // img  roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*Y_OUTPUT_CONTROL))   = 22.85 ms
+  {0x302A, 0x0006}, // VT_PIX_CLK_DIV
+  {0x302C, 0x0001}, // VT_SYS_CLK_DIV
+  {0x302E, 0x0002}, // PRE_PLL_CLK_DIV
+  {0x3030, 0x0037}, // PLL_MULTIPLIER
+  {0x3036, 0x000C}, // OP_PIX_CLK_DIV
+  {0x3038, 0x0001}, // OP_SYS_CLK_DIV
+
+  // FORMAT
+  {0x3040, 0xC000}, // READ_MODE
+  {0x3004, 0x0000}, // X_ADDR_START_
+  {0x3008, 0x0787}, // X_ADDR_END_
+  {0x3002, 0x0000}, // Y_ADDR_START_
+  {0x3006, 0x04B7}, // Y_ADDR_END_
+  {0x3032, 0x0000}, // SCALING_MODE
+  {0x30A2, 0x0001}, // X_ODD_INC_
+  {0x30A6, 0x0001}, // Y_ODD_INC_
+  {0x3402, 0x0788}, // X_OUTPUT_CONTROL
+  {0x3404, 0x04B8}, // Y_OUTPUT_CONTROL
+  {0x3064, 0x1982}, // SMIA_TEST
+  {0x30BA, 0x11F2}, // DIGITAL_CTRL
+
+  // Enable external trigger and disable GPIO outputs
+  {0x30CE, 0x0120}, // SLAVE_SH_SYNC_MODE | FRAME_START_MODE
+  {0x340A, 0xE0},   // GPIO3_INPUT_DISABLE | GPIO2_INPUT_DISABLE | GPIO1_INPUT_DISABLE
+  {0x340C, 0x802},  // GPIO_HIDRV_EN | GPIO0_ISEL=2
+
+  // Readout timing
+  {0x300C, 0x0672}, // LINE_LENGTH_PCK (valid for 3-exposure HDR)
+  {0x300A, 0x0855}, // FRAME_LENGTH_LINES
+  {0x3042, 0x0000}, // EXTRA_DELAY
+
+  // Readout Settings
+  {0x31AE, 0x0204}, // SERIAL_FORMAT, 4-lane MIPI
+  {0x31AC, 0x0C0C}, // DATA_FORMAT_BITS, 12 -> 12
+  {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
+  {0x3350, 0x0311}, // MIPI_F4_VDT_VC
+  {0x31B0, 0x0053}, // FRAME_PREAMBLE
+  {0x31B2, 0x003B}, // LINE_PREAMBLE
+  {0x301A, 0x001C}, // RESET_REGISTER
+
+  // Noise Corrections
+  {0x3092, 0x0C24}, // ROW_NOISE_CONTROL
+  {0x337A, 0x0C80}, // DBLC_SCALE0
+  {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},
+
+  // HDR Settings
+  {0x3082, 0x0004}, // OPERATION_MODE_CTRL
+  {0x3238, 0x0444}, // EXPOSURE_RATIO
+
+  {0x1008, 0x0361}, // FINE_INTEGRATION_TIME_MIN
+  {0x100C, 0x0589}, // FINE_INTEGRATION_TIME2_MIN
+  {0x100E, 0x07B1}, // FINE_INTEGRATION_TIME3_MIN
+  {0x1010, 0x0139}, // FINE_INTEGRATION_TIME4_MIN
+
+  // TODO: do these have to be lower than LINE_LENGTH_PCK?
+  {0x3014, 0x08CB}, // FINE_INTEGRATION_TIME_
+  {0x321E, 0x0894}, // FINE_INTEGRATION_TIME2
+
+  {0x31D0, 0x0000}, // COMPANDING, no good in 10 bit?
+  {0x33DA, 0x0000}, // COMPANDING
+  {0x318E, 0x0200}, // PRE_HDR_GAIN_EN
+
+  // DLO Settings
+  {0x3100, 0x4000}, // DLO_CONTROL0
+  {0x3280, 0x0CCC}, // T1 G1
+  {0x3282, 0x0CCC}, // T1 R
+  {0x3284, 0x0CCC}, // T1 B
+  {0x3286, 0x0CCC}, // T1 G2
+  {0x3288, 0x0FA0}, // T2 G1
+  {0x328A, 0x0FA0}, // T2 R
+  {0x328C, 0x0FA0}, // T2 B
+  {0x328E, 0x0FA0}, // T2 G2
+
+   // Initial Gains
+  {0x3022, 0x0001}, // GROUPED_PARAMETER_HOLD_
+  {0x3366, 0xFF77}, // ANALOG_GAIN (1x)
+
+  {0x3060, 0x3333}, // ANALOG_COLOR_GAIN
+
+  {0x3362, 0x0000}, // DC GAIN
+
+  {0x305A, 0x00F8}, // red gain
+  {0x3058, 0x0122}, // blue gain
+  {0x3056, 0x009A}, // g1 gain
+  {0x305C, 0x009A}, // g2 gain
+
+  {0x3022, 0x0000}, // GROUPED_PARAMETER_HOLD_
+
+  // Initial Integration Time
+  {0x3012, 0x0005},
+};

system/camerad/sensors/os04c10_cl.h

@@ -0,0 +1,58 @@
+#if SENSOR_ID == 3
+
+#define BGGR
+#define VIGNETTE_PROFILE_4DT6MM
+
+#define BIT_DEPTH 12
+#define PV_MAX10 1023
+#define PV_MAX12 4095
+#define PV_MAX16 65536 // gamma curve is calibrated to 16bit
+#define BLACK_LVL 48
+
+float combine_dual_pvs(float lv, float sv, int expo_time) {
+  float svc = fmax(sv * expo_time, (float)(64 * (PV_MAX10 - BLACK_LVL)));
+  float svd = sv * fmin(expo_time, 8.0) / 8;
+
+  if (expo_time > 64) {
+    if (lv < PV_MAX10 - BLACK_LVL) {
+      return lv / (PV_MAX16 - BLACK_LVL);
+    } else {
+      return (svc / 64) / (PV_MAX16 - BLACK_LVL);
+    }
+  } else {
+    if (lv > 32) {
+      return (lv * 64 / fmax(expo_time, 8.0)) / (PV_MAX16 - BLACK_LVL);
+    } else {
+      return svd / (PV_MAX16 - BLACK_LVL);
+    }
+  }
+}
+
+float4 normalize_pv_hdr(int4 parsed, int4 short_parsed, float vignette_factor, int expo_time) {
+  float4 pl = convert_float4(parsed - BLACK_LVL);
+  float4 ps = convert_float4(short_parsed - BLACK_LVL);
+  float4 pv;
+  pv.s0 = combine_dual_pvs(pl.s0, ps.s0, expo_time);
+  pv.s1 = combine_dual_pvs(pl.s1, ps.s1, expo_time);
+  pv.s2 = combine_dual_pvs(pl.s2, ps.s2, expo_time);
+  pv.s3 = combine_dual_pvs(pl.s3, ps.s3, expo_time);
+  return clamp(pv*vignette_factor, 0.0, 1.0);
+}
+
+float4 normalize_pv(int4 parsed, float vignette_factor) {
+  float4 pv = (convert_float4(parsed) - BLACK_LVL) / (PV_MAX12 - BLACK_LVL);
+  return clamp(pv*vignette_factor, 0.0, 1.0);
+}
+
+float3 color_correct(float3 rgb) {
+  float3 corrected = rgb.x * (float3)(1.55361989, -0.268894615, -0.000593219);
+  corrected += rgb.y * (float3)(-0.421217301, 1.51883144, -0.69760146);
+  corrected += rgb.z * (float3)(-0.132402589, -0.249936825, 1.69819468);
+  return corrected;
+}
+
+float3 apply_gamma(float3 rgb, int expo_time) {
+  return (10 * rgb) / (1 + 9 * rgb);
+}
+
+#endif

system/camerad/sensors/os04c10_registers.h

@@ -0,0 +1,352 @@
+#pragma once
+
+const struct i2c_random_wr_payload start_reg_array_os04c10[] = {{0x100, 1}};
+const struct i2c_random_wr_payload stop_reg_array_os04c10[] = {{0x100, 0}};
+
+const struct i2c_random_wr_payload init_array_os04c10[] = {
+  // DP_2688X1520_NEWSTG_MIPI0776Mbps_30FPS_10BIT_FOURLANE
+  {0x0103, 0x01},
+
+  // PLL
+  {0x0301, 0xe4},
+  {0x0303, 0x01},
+  {0x0305, 0xb6},
+  {0x0306, 0x01},
+  {0x0307, 0x17},
+  {0x0323, 0x04},
+  {0x0324, 0x01},
+  {0x0325, 0x62},
+
+  {0x3012, 0x06},
+  {0x3013, 0x02},
+  {0x3016, 0x72},
+  {0x3021, 0x03},
+  {0x3106, 0x21},
+  {0x3107, 0xa1},
+
+  // ?
+  {0x3624, 0x00},
+  {0x3625, 0x4c},
+  {0x3660, 0x04},
+  {0x3666, 0xa5},
+  {0x3667, 0xa5},
+  {0x366a, 0x50},
+  {0x3673, 0x0d},
+  {0x3672, 0x0d},
+  {0x3671, 0x0d},
+  {0x3670, 0x0d},
+  {0x3685, 0x00},
+  {0x3694, 0x0d},
+  {0x3693, 0x0d},
+  {0x3692, 0x0d},
+  {0x3691, 0x0d},
+  {0x3696, 0x4c},
+  {0x3697, 0x4c},
+  {0x3698, 0x00},
+  {0x3699, 0x80},
+  {0x369a, 0x80},
+  {0x369b, 0x1f},
+  {0x369c, 0x1f},
+  {0x369d, 0x80},
+  {0x369e, 0x40},
+  {0x369f, 0x21},
+  {0x36a0, 0x12},
+  {0x36a1, 0xdd},
+  {0x36a2, 0x66},
+  {0x370a, 0x02},
+  {0x370e, 0x00},
+  {0x3710, 0x00},
+  {0x3713, 0x04},
+  {0x3725, 0x02},
+  {0x372a, 0x03},
+  {0x3738, 0xce},
+  {0x3748, 0x02},
+  {0x374a, 0x02},
+  {0x374c, 0x02},
+  {0x374e, 0x02},
+  {0x3756, 0x00},
+  {0x3757, 0x00},
+  {0x3767, 0x00},
+  {0x3771, 0x00},
+  {0x377b, 0x28},
+  {0x377c, 0x00},
+  {0x377d, 0x0c},
+  {0x3781, 0x03},
+  {0x3782, 0x00},
+  {0x3789, 0x14},
+  {0x3795, 0x02},
+  {0x379c, 0x00},
+  {0x379d, 0x00},
+  {0x37b8, 0x04},
+  {0x37ba, 0x03},
+  {0x37bb, 0x00},
+  {0x37bc, 0x04},
+  {0x37be, 0x26},
+  {0x37c4, 0x11},
+  {0x37c5, 0x80},
+  {0x37c6, 0x14},
+  {0x37c7, 0xa8},
+  {0x37da, 0x11},
+  {0x381f, 0x08},
+  {0x3881, 0x00},
+  {0x3888, 0x04},
+  {0x388b, 0x00},
+  {0x3c80, 0x10},
+  {0x3c86, 0x00},
+  {0x3c8c, 0x20},
+  {0x3c9f, 0x01},
+  {0x3d85, 0x1b},
+  {0x3d8c, 0x71},
+  {0x3d8d, 0xe2},
+  {0x3f00, 0x0b},
+  {0x3f06, 0x04},
+
+  // BLC
+  {0x400a, 0x01},
+  {0x400b, 0x50},
+  {0x400e, 0x08},
+  {0x4043, 0x7e},
+  {0x4045, 0x7e},
+  {0x4047, 0x7e},
+  {0x4049, 0x7e},
+  {0x4090, 0x04},
+  {0x40b0, 0x00},
+  {0x40b1, 0x00},
+  {0x40b2, 0x00},
+  {0x40b3, 0x00},
+  {0x40b4, 0x00},
+  {0x40b5, 0x00},
+  {0x40b7, 0x00},
+  {0x40b8, 0x00},
+  {0x40b9, 0x00},
+  {0x40ba, 0x01},
+
+  {0x4301, 0x00},
+  {0x4303, 0x00},
+  {0x4502, 0x04},
+  {0x4503, 0x00},
+  {0x4504, 0x06},
+  {0x4506, 0x00},
+  {0x4507, 0x47},
+  {0x4803, 0x00},
+  {0x480c, 0x32},
+  {0x480e, 0x04},
+  {0x4813, 0xe4},
+  {0x4819, 0x70},
+  {0x481f, 0x30},
+  {0x4823, 0x3f},
+  {0x4825, 0x30},
+  {0x4833, 0x10},
+  {0x484b, 0x27},
+  {0x488b, 0x00},
+  {0x4d00, 0x04},
+  {0x4d01, 0xad},
+  {0x4d02, 0xbc},
+  {0x4d03, 0xa1},
+  {0x4d04, 0x1f},
+  {0x4d05, 0x4c},
+  {0x4d0b, 0x01},
+  {0x4e00, 0x2a},
+  {0x4e0d, 0x00},
+
+  // ISP
+  {0x5001, 0x09},
+  {0x5004, 0x00},
+  {0x5080, 0x04},
+  {0x5036, 0x80},
+  {0x5180, 0x70},
+  {0x5181, 0x10},
+
+  // DPC
+  {0x520a, 0x03},
+  {0x520b, 0x06},
+  {0x520c, 0x0c},
+
+  {0x580b, 0x0f},
+  {0x580d, 0x00},
+  {0x580f, 0x00},
+  {0x5820, 0x00},
+  {0x5821, 0x00},
+
+  {0x301c, 0xf8},
+  {0x301e, 0xb4},
+  {0x301f, 0xf0},
+  {0x3022, 0x61},
+  {0x3109, 0xe7},
+  {0x3600, 0x00},
+  {0x3610, 0x65},
+  {0x3611, 0x85},
+  {0x3613, 0x3a},
+  {0x3615, 0x60},
+  {0x3621, 0xb0},
+  {0x3620, 0x0c},
+  {0x3629, 0x00},
+  {0x3661, 0x04},
+  {0x3664, 0x70},
+  {0x3665, 0x00},
+  {0x3681, 0x80},
+  {0x3682, 0x40},
+  {0x3683, 0x21},
+  {0x3684, 0x12},
+  {0x3700, 0x2a},
+  {0x3701, 0x12},
+  {0x3703, 0x28},
+  {0x3704, 0x0e},
+  {0x3706, 0x9d},
+  {0x3709, 0x4a},
+  {0x370b, 0x48},
+  {0x370c, 0x01},
+  {0x370f, 0x00},
+  {0x3714, 0x28},
+  {0x3716, 0x04},
+  {0x3719, 0x11},
+  {0x371a, 0x1e},
+  {0x3720, 0x00},
+  {0x3724, 0x13},
+  {0x373f, 0xb0},
+  {0x3741, 0x9d},
+  {0x3743, 0x9d},
+  {0x3745, 0x9d},
+  {0x3747, 0x9d},
+  {0x3749, 0x48},
+  {0x374b, 0x48},
+  {0x374d, 0x48},
+  {0x374f, 0x48},
+  {0x3755, 0x10},
+  {0x376c, 0x00},
+  {0x378d, 0x3c},
+  {0x3790, 0x01},
+  {0x3791, 0x01},
+  {0x3798, 0x40},
+  {0x379e, 0x00},
+  {0x379f, 0x04},
+  {0x37a1, 0x10},
+  {0x37a2, 0x1e},
+  {0x37a8, 0x10},
+  {0x37a9, 0x1e},
+  {0x37ac, 0xa0},
+  {0x37b9, 0x01},
+  {0x37bd, 0x01},
+  {0x37bf, 0x26},
+  {0x37c0, 0x11},
+  {0x37c2, 0x14},
+  {0x37cd, 0x19},
+  {0x37e0, 0x08},
+  {0x37e6, 0x04},
+  {0x37e5, 0x02},
+  {0x37e1, 0x0c},
+  {0x3737, 0x04},
+  {0x37d8, 0x02},
+  {0x37e2, 0x10},
+  {0x3739, 0x10},
+  {0x3662, 0x08},
+  {0x37e4, 0x20},
+  {0x37e3, 0x08},
+  {0x37d9, 0x04},
+  {0x4040, 0x00},
+  {0x4041, 0x03},
+  {0x4008, 0x01},
+  {0x4009, 0x06},
+
+  // FSIN
+  {0x3002, 0x22},
+  {0x3663, 0x22},
+  {0x368a, 0x04},
+  {0x3822, 0x44},
+  {0x3823, 0x00},
+  {0x3829, 0x03},
+  {0x3832, 0xf8},
+  {0x382c, 0x00},
+  {0x3844, 0x06},
+  {0x3843, 0x00},
+  {0x382a, 0x00},
+  {0x382b, 0x0c},
+
+  // 2704x1536 -> 2688x1520 out
+  {0x3800, 0x00}, {0x3801, 0x00},
+  {0x3802, 0x00}, {0x3803, 0x00},
+  {0x3804, 0x0a}, {0x3805, 0x8f},
+  {0x3806, 0x05}, {0x3807, 0xff},
+  {0x3808, 0x05}, {0x3809, 0x40},
+  {0x380a, 0x02}, {0x380b, 0xf8},
+  {0x3811, 0x08},
+  {0x3813, 0x08},
+  {0x3814, 0x03},
+  {0x3815, 0x01},
+  {0x3816, 0x03},
+  {0x3817, 0x01},
+
+  {0x380c, 0x0b}, {0x380d, 0xac}, // HTS
+  {0x380e, 0x06}, {0x380f, 0x9c}, // VTS
+
+  {0x3820, 0xb3},
+  {0x3821, 0x01},
+  {0x3880, 0x00},
+  {0x3882, 0x20},
+  {0x3c91, 0x0b},
+  {0x3c94, 0x45},
+  {0x3cad, 0x00},
+  {0x3cae, 0x00},
+  {0x4000, 0xf3},
+  {0x4001, 0x60},
+  {0x4003, 0x40},
+  {0x4300, 0xff},
+  {0x4302, 0x0f},
+  {0x4305, 0x83},
+  {0x4505, 0x84},
+  {0x4809, 0x0e},
+  {0x480a, 0x04},
+  {0x4837, 0x15},
+  {0x4c00, 0x08},
+  {0x4c01, 0x08},
+  {0x4c04, 0x00},
+  {0x4c05, 0x00},
+  {0x5000, 0xf9},
+  // {0x0100, 0x01},
+  // {0x320d, 0x00},
+  // {0x3208, 0xa0},
+
+  // initialize exposure
+  {0x3503, 0x88},
+
+  // long
+  {0x3500, 0x00}, {0x3501, 0x00}, {0x3502, 0x10},
+  {0x3508, 0x00}, {0x3509, 0x80},
+  {0x350a, 0x04}, {0x350b, 0x00},
+
+  // short
+  {0x3510, 0x00}, {0x3511, 0x00}, {0x3512, 0x40},
+  {0x350c, 0x00}, {0x350d, 0x80},
+  {0x350e, 0x04}, {0x350f, 0x00},
+
+  // wb
+  // b
+  {0x5100, 0x06}, {0x5101, 0x7e},
+  {0x5140, 0x06}, {0x5141, 0x7e},
+  // g
+  {0x5102, 0x04}, {0x5103, 0x00},
+  {0x5142, 0x04}, {0x5143, 0x00},
+  // r
+  {0x5104, 0x08}, {0x5105, 0xd6},
+  {0x5144, 0x08}, {0x5145, 0xd6},
+};
+
+const struct i2c_random_wr_payload ife_downscale_override_array_os04c10[] = {
+  // OS04C10_AA_00_02_17_wAO_2688x1524_MIPI728Mbps_Linear12bit_20FPS_4Lane_MCLK24MHz
+  {0x3c8c, 0x40},
+  {0x3714, 0x24},
+  {0x37c2, 0x04},
+  {0x3662, 0x10},
+  {0x37d9, 0x08},
+  {0x4041, 0x07},
+  {0x4008, 0x02},
+  {0x4009, 0x0d},
+  {0x3808, 0x0a}, {0x3809, 0x80},
+  {0x380a, 0x05}, {0x380b, 0xf0},
+  {0x3814, 0x01},
+  {0x3816, 0x01},
+  {0x380c, 0x08}, {0x380d, 0x5c}, // HTS
+  {0x380e, 0x09}, {0x380f, 0x38}, // VTS
+  {0x3820, 0xb0},
+  {0x3821, 0x00},
+};

system/camerad/sensors/ox03c10_cl.h

@@ -0,0 +1,47 @@
+#if SENSOR_ID == 2
+
+#define VIGNETTE_PROFILE_8DT0MM
+
+#define BIT_DEPTH 12
+#define BLACK_LVL 64
+
+float ox_lut_func(int x) {
+  if (x < 512) {
+    return x * 5.94873e-8;
+  } else if (512 <= x && x < 768) {
+    return 3.0458e-05 + (x-512) * 1.19913e-7;
+  } else if (768 <= x && x < 1536) {
+    return 6.1154e-05 + (x-768) * 2.38493e-7;
+  } else if (1536 <= x && x < 1792) {
+    return 0.0002448 + (x-1536) * 9.56930e-7;
+  } else if (1792 <= x && x < 2048) {
+    return 0.00048977 + (x-1792) * 1.91441e-6;
+  } else if (2048 <= x && x < 2304) {
+    return 0.00097984 + (x-2048) * 3.82937e-6;
+  } else if (2304 <= x && x < 2560) {
+    return 0.0019601 + (x-2304) * 7.659055e-6;
+  } else if (2560 <= x && x < 2816) {
+    return 0.0039207 + (x-2560) * 1.525e-5;
+  } else {
+    return 0.0078421 + (exp((x-2816)/273.0) - 1) * 0.0092421;
+  }
+}
+
+float4 normalize_pv(int4 parsed, float vignette_factor) {
+  // PWL
+  float4 pv = {ox_lut_func(parsed.s0), ox_lut_func(parsed.s1), ox_lut_func(parsed.s2), ox_lut_func(parsed.s3)};
+  return clamp(pv*vignette_factor*256.0, 0.0, 1.0);
+}
+
+float3 color_correct(float3 rgb) {
+  float3 corrected = rgb.x * (float3)(1.5664815, -0.29808738, -0.03973474);
+  corrected += rgb.y * (float3)(-0.48672447, 1.41914433, -0.40295248);
+  corrected += rgb.z * (float3)(-0.07975703, -0.12105695, 1.44268722);
+  return corrected;
+}
+
+float3 apply_gamma(float3 rgb, int expo_time) {
+  return -0.507089*exp(-12.54124638*rgb) + 0.9655*powr(rgb, 0.5) - 0.472597*rgb + 0.507089;
+}
+
+#endif

system/camerad/sensors/ox03c10_registers.h

@@ -0,0 +1,751 @@
+#pragma once
+
+const struct i2c_random_wr_payload start_reg_array_ox03c10[] = {{0x100, 1}};
+const struct i2c_random_wr_payload stop_reg_array_ox03c10[] = {{0x100, 0}};
+
+const struct i2c_random_wr_payload init_array_ox03c10[] = {
+  {0x103, 1},
+  {0x107, 1},
+
+  // X3C_1920x1280_60fps_HDR4_LFR_PWL12_mipi1200
+
+  // TPM
+  {0x4d5a, 0x1a}, {0x4d09, 0xff}, {0x4d09, 0xdf},
+
+  /*)
+  // group 4
+  {0x3208, 0x04},
+  {0x4620, 0x04},
+  {0x3208, 0x14},
+
+  // group 5
+  {0x3208, 0x05},
+  {0x4620, 0x04},
+  {0x3208, 0x15},
+
+  // group 2
+  {0x3208, 0x02},
+  {0x3507, 0x00},
+  {0x3208, 0x12},
+
+  // delay launch group 2
+  {0x3208, 0xa2},*/
+
+  // **NOTE**: if this is changed, readout_time_ns must be updated in the Sensor config
+  // PLL setup
+  {0x0301, 0xc8}, // pll1_divs, pll1_predivp, pll1_divpix
+  {0x0303, 0x01}, // pll1_prediv
+  {0x0304, 0x01}, {0x0305, 0x2c}, // pll1_loopdiv = 300
+  {0x0306, 0x04}, // pll1_divmipi = 4
+  {0x0307, 0x01}, // pll1_divm = 1
+  {0x0316, 0x00},
+  {0x0317, 0x00},
+  {0x0318, 0x00},
+  {0x0323, 0x05}, // pll2_prediv
+  {0x0324, 0x01}, {0x0325, 0x2c}, // pll2_divp = 300
+
+  // SCLK/PCLK
+  {0x0400, 0xe0}, {0x0401, 0x80},
+  {0x0403, 0xde}, {0x0404, 0x34},
+  {0x0405, 0x3b}, {0x0406, 0xde},
+  {0x0407, 0x08},
+  {0x0408, 0xe0}, {0x0409, 0x7f},
+  {0x040a, 0xde}, {0x040b, 0x34},
+  {0x040c, 0x47}, {0x040d, 0xd8},
+  {0x040e, 0x08},
+
+  // xchk
+  {0x2803, 0xfe}, {0x280b, 0x00}, {0x280c, 0x79},
+
+  // SC ctrl
+  {0x3001, 0x03}, // io_pad_oen
+  {0x3002, 0xfc}, // io_pad_oen
+  {0x3005, 0x80}, // io_pad_out
+  {0x3007, 0x01}, // io_pad_sel
+  {0x3008, 0x80}, // io_pad_sel
+
+  // FSIN (frame sync) with external pulses
+  {0x3009, 0x2},
+  {0x3015, 0x2},
+  {0x383E, 0x80},
+  {0x3881, 0x4},
+  {0x3882, 0x8}, {0x3883, 0x0D},
+  {0x3836, 0x1F}, {0x3837, 0x40},
+
+  // causes issues on some devices
+  //{0x3822, 0x33},  // wait for pulse before first frame
+
+  {0x3892, 0x44},
+  {0x3823, 0x41},
+
+  {0x3012, 0x41}, // SC_PHY_CTRL = 4 lane MIPI
+  {0x3020, 0x05}, // SC_CTRL_20
+
+  // this is not in the datasheet, listed as RSVD
+  // but the camera doesn't work without it
+  {0x3700, 0x28}, {0x3701, 0x15}, {0x3702, 0x19}, {0x3703, 0x23},
+  {0x3704, 0x0a}, {0x3705, 0x00}, {0x3706, 0x3e}, {0x3707, 0x0d},
+  {0x3708, 0x50}, {0x3709, 0x5a}, {0x370a, 0x00}, {0x370b, 0x96},
+  {0x3711, 0x11}, {0x3712, 0x13}, {0x3717, 0x02}, {0x3718, 0x73},
+  {0x372c, 0x40}, {0x3733, 0x01}, {0x3738, 0x36}, {0x3739, 0x36},
+  {0x373a, 0x25}, {0x373b, 0x25}, {0x373f, 0x21}, {0x3740, 0x21},
+  {0x3741, 0x21}, {0x3742, 0x21}, {0x3747, 0x28}, {0x3748, 0x28},
+  {0x3749, 0x19}, {0x3755, 0x1a}, {0x3756, 0x0a}, {0x3757, 0x1c},
+  {0x3765, 0x19}, {0x3766, 0x05}, {0x3767, 0x05}, {0x3768, 0x13},
+  {0x376c, 0x07}, {0x3778, 0x20}, {0x377c, 0xc8}, {0x3781, 0x02},
+  {0x3783, 0x02}, {0x379c, 0x58}, {0x379e, 0x00}, {0x379f, 0x00},
+  {0x37a0, 0x00}, {0x37bc, 0x22}, {0x37c0, 0x01}, {0x37c4, 0x3e},
+  {0x37c5, 0x3e}, {0x37c6, 0x2a}, {0x37c7, 0x28}, {0x37c8, 0x02},
+  {0x37c9, 0x12}, {0x37cb, 0x29}, {0x37cd, 0x29}, {0x37d2, 0x00},
+  {0x37d3, 0x73}, {0x37d6, 0x00}, {0x37d7, 0x6b}, {0x37dc, 0x00},
+  {0x37df, 0x54}, {0x37e2, 0x00}, {0x37e3, 0x00}, {0x37f8, 0x00},
+  {0x37f9, 0x01}, {0x37fa, 0x00}, {0x37fb, 0x19},
+
+  // also RSVD
+  {0x3c03, 0x01}, {0x3c04, 0x01}, {0x3c06, 0x21}, {0x3c08, 0x01},
+  {0x3c09, 0x01}, {0x3c0a, 0x01}, {0x3c0b, 0x21}, {0x3c13, 0x21},
+  {0x3c14, 0x82}, {0x3c16, 0x13}, {0x3c21, 0x00}, {0x3c22, 0xf3},
+  {0x3c37, 0x12}, {0x3c38, 0x31}, {0x3c3c, 0x00}, {0x3c3d, 0x03},
+  {0x3c44, 0x16}, {0x3c5c, 0x8a}, {0x3c5f, 0x03}, {0x3c61, 0x80},
+  {0x3c6f, 0x2b}, {0x3c70, 0x5f}, {0x3c71, 0x2c}, {0x3c72, 0x2c},
+  {0x3c73, 0x2c}, {0x3c76, 0x12},
+
+  // PEC checks
+  {0x3182, 0x12},
+
+  {0x320e, 0x00}, {0x320f, 0x00}, // RSVD
+  {0x3211, 0x61},
+  {0x3215, 0xcd},
+  {0x3219, 0x08},
+
+  {0x3506, 0x20}, {0x3507, 0x00}, // hcg fine exposure
+  {0x350a, 0x01}, {0x350b, 0x00}, {0x350c, 0x00}, // hcg digital gain
+
+  {0x3586, 0x40}, {0x3587, 0x00}, // lcg fine exposure
+  {0x358a, 0x01}, {0x358b, 0x00}, {0x358c, 0x00}, // lcg digital gain
+
+  {0x3546, 0x20}, {0x3547, 0x00}, // spd fine exposure
+  {0x354a, 0x01}, {0x354b, 0x00}, {0x354c, 0x00}, // spd digital gain
+
+  {0x35c6, 0xb0}, {0x35c7, 0x00}, // vs fine exposure
+  {0x35ca, 0x01}, {0x35cb, 0x00}, {0x35cc, 0x00}, // vs digital gain
+
+  // also RSVD
+  {0x3600, 0x8f}, {0x3605, 0x16}, {0x3609, 0xf0}, {0x360a, 0x01},
+  {0x360e, 0x1d}, {0x360f, 0x10}, {0x3610, 0x70}, {0x3611, 0x3a},
+  {0x3612, 0x28}, {0x361a, 0x29}, {0x361b, 0x6c}, {0x361c, 0x0b},
+  {0x361d, 0x00}, {0x361e, 0xfc}, {0x362a, 0x00}, {0x364d, 0x0f},
+  {0x364e, 0x18}, {0x364f, 0x12}, {0x3653, 0x1c}, {0x3654, 0x00},
+  {0x3655, 0x1f}, {0x3656, 0x1f}, {0x3657, 0x0c}, {0x3658, 0x0a},
+  {0x3659, 0x14}, {0x365a, 0x18}, {0x365b, 0x14}, {0x365c, 0x10},
+  {0x365e, 0x12}, {0x3674, 0x08}, {0x3677, 0x3a}, {0x3678, 0x3a},
+  {0x3679, 0x19},
+
+  // Y_ADDR_START = 4
+  {0x3802, 0x00}, {0x3803, 0x04},
+  // Y_ADDR_END = 0x50b
+  {0x3806, 0x05}, {0x3807, 0x0b},
+
+  // X_OUTPUT_SIZE = 0x780 = 1920 (changed to 1928)
+  {0x3808, 0x07}, {0x3809, 0x88},
+
+  // Y_OUTPUT_SIZE = 0x500 = 1280 (changed to 1208)
+  {0x380a, 0x04}, {0x380b, 0xb8},
+
+  // horizontal timing 0x447
+  {0x380c, 0x04}, {0x380d, 0x47},
+
+  // rows per frame (was 0x2ae)
+  // 0x8ae = 53.65 ms
+  {0x380e, 0x08}, {0x380f, 0x15},
+  // this should be triggered by FSIN, not free running
+
+  {0x3810, 0x00}, {0x3811, 0x08}, // x cutoff
+  {0x3812, 0x00}, {0x3813, 0x04}, // y cutoff
+  {0x3816, 0x01},
+  {0x3817, 0x01},
+  {0x381c, 0x18},
+  {0x381e, 0x01},
+  {0x381f, 0x01},
+
+  // don't mirror, just flip
+  {0x3820, 0x04},
+
+  {0x3821, 0x19},
+  {0x3832, 0xF0},
+  {0x3834, 0xF0},
+  {0x384c, 0x02},
+  {0x384d, 0x0d},
+  {0x3850, 0x00},
+  {0x3851, 0x42},
+  {0x3852, 0x00},
+  {0x3853, 0x40},
+  {0x3858, 0x04},
+  {0x388c, 0x02},
+  {0x388d, 0x2b},
+
+  // APC
+  {0x3b40, 0x05}, {0x3b41, 0x40}, {0x3b42, 0x00}, {0x3b43, 0x90},
+  {0x3b44, 0x00}, {0x3b45, 0x20}, {0x3b46, 0x00}, {0x3b47, 0x20},
+  {0x3b48, 0x19}, {0x3b49, 0x12}, {0x3b4a, 0x16}, {0x3b4b, 0x2e},
+  {0x3b4c, 0x00}, {0x3b4d, 0x00},
+  {0x3b86, 0x00}, {0x3b87, 0x34}, {0x3b88, 0x00}, {0x3b89, 0x08},
+  {0x3b8a, 0x05}, {0x3b8b, 0x00}, {0x3b8c, 0x07}, {0x3b8d, 0x80},
+  {0x3b8e, 0x00}, {0x3b8f, 0x00}, {0x3b92, 0x05}, {0x3b93, 0x00},
+  {0x3b94, 0x07}, {0x3b95, 0x80}, {0x3b9e, 0x09},
+
+  // OTP
+  {0x3d82, 0x73},
+  {0x3d85, 0x05},
+  {0x3d8a, 0x03},
+  {0x3d8b, 0xff},
+  {0x3d99, 0x00},
+  {0x3d9a, 0x9f},
+  {0x3d9b, 0x00},
+  {0x3d9c, 0xa0},
+  {0x3da4, 0x00},
+  {0x3da7, 0x50},
+
+  // DTR
+  {0x420e, 0x6b},
+  {0x420f, 0x6e},
+  {0x4210, 0x06},
+  {0x4211, 0xc1},
+  {0x421e, 0x02},
+  {0x421f, 0x45},
+  {0x4220, 0xe1},
+  {0x4221, 0x01},
+  {0x4301, 0xff},
+  {0x4307, 0x03},
+  {0x4308, 0x13},
+  {0x430a, 0x13},
+  {0x430d, 0x93},
+  {0x430f, 0x57},
+  {0x4310, 0x95},
+  {0x4311, 0x16},
+  {0x4316, 0x00},
+
+  {0x4317, 0x38}, // both embedded rows are enabled
+
+  {0x4319, 0x03}, // spd dcg
+  {0x431a, 0x00}, // 8 bit mipi
+  {0x431b, 0x00},
+  {0x431d, 0x2a},
+  {0x431e, 0x11},
+
+  {0x431f, 0x20}, // enable PWL (pwl0_en), 12 bits
+  //{0x431f, 0x00}, // disable PWL
+
+  {0x4320, 0x19},
+  {0x4323, 0x80},
+  {0x4324, 0x00},
+  {0x4503, 0x4e},
+  {0x4505, 0x00},
+  {0x4509, 0x00},
+  {0x450a, 0x00},
+  {0x4580, 0xf8},
+  {0x4583, 0x07},
+  {0x4584, 0x6a},
+  {0x4585, 0x08},
+  {0x4586, 0x05},
+  {0x4587, 0x04},
+  {0x4588, 0x73},
+  {0x4589, 0x05},
+  {0x458a, 0x1f},
+  {0x458b, 0x02},
+  {0x458c, 0xdc},
+  {0x458d, 0x03},
+  {0x458e, 0x02},
+  {0x4597, 0x07},
+  {0x4598, 0x40},
+  {0x4599, 0x0e},
+  {0x459a, 0x0e},
+  {0x459b, 0xfb},
+  {0x459c, 0xf3},
+  {0x4602, 0x00},
+  {0x4603, 0x13},
+  {0x4604, 0x00},
+  {0x4609, 0x0a},
+  {0x460a, 0x30},
+  {0x4610, 0x00},
+  {0x4611, 0x70},
+  {0x4612, 0x01},
+  {0x4613, 0x00},
+  {0x4614, 0x00},
+  {0x4615, 0x70},
+  {0x4616, 0x01},
+  {0x4617, 0x00},
+
+  {0x4800, 0x04}, // invert output PCLK
+  {0x480a, 0x22},
+  {0x4813, 0xe4},
+
+  // mipi
+  {0x4814, 0x2a},
+  {0x4837, 0x0d},
+  {0x484b, 0x47},
+  {0x484f, 0x00},
+  {0x4887, 0x51},
+  {0x4d00, 0x4a},
+  {0x4d01, 0x18},
+  {0x4d05, 0xff},
+  {0x4d06, 0x88},
+  {0x4d08, 0x63},
+  {0x4d09, 0xdf},
+  {0x4d15, 0x7d},
+  {0x4d1a, 0x20},
+  {0x4d30, 0x0a},
+  {0x4d31, 0x00},
+  {0x4d34, 0x7d},
+  {0x4d3c, 0x7d},
+  {0x4f00, 0x00},
+  {0x4f01, 0x00},
+  {0x4f02, 0x00},
+  {0x4f03, 0x20},
+  {0x4f04, 0xe0},
+  {0x6a00, 0x00},
+  {0x6a01, 0x20},
+  {0x6a02, 0x00},
+  {0x6a03, 0x20},
+  {0x6a04, 0x02},
+  {0x6a05, 0x80},
+  {0x6a06, 0x01},
+  {0x6a07, 0xe0},
+  {0x6a08, 0xcf},
+  {0x6a09, 0x01},
+  {0x6a0a, 0x40},
+  {0x6a20, 0x00},
+  {0x6a21, 0x02},
+  {0x6a22, 0x00},
+  {0x6a23, 0x00},
+  {0x6a24, 0x00},
+  {0x6a25, 0x00},
+  {0x6a26, 0x00},
+  {0x6a27, 0x00},
+  {0x6a28, 0x00},
+
+  // isp
+  {0x5000, 0x8f},
+  {0x5001, 0x75},
+  {0x5002, 0x7f}, // PWL0
+  //{0x5002, 0x3f}, // PWL disable
+  {0x5003, 0x7a},
+
+  {0x5004, 0x3e},
+  {0x5005, 0x1e},
+  {0x5006, 0x1e},
+  {0x5007, 0x1e},
+
+  {0x5008, 0x00},
+  {0x500c, 0x00},
+  {0x502c, 0x00},
+  {0x502e, 0x00},
+  {0x502f, 0x00},
+  {0x504b, 0x00},
+  {0x5053, 0x00},
+  {0x505b, 0x00},
+  {0x5063, 0x00},
+  {0x5070, 0x00},
+  {0x5074, 0x04},
+  {0x507a, 0x04},
+  {0x507b, 0x09},
+  {0x5500, 0x02},
+  {0x5700, 0x02},
+  {0x5900, 0x02},
+  {0x6007, 0x04},
+  {0x6008, 0x05},
+  {0x6009, 0x02},
+  {0x600b, 0x08},
+  {0x600c, 0x07},
+  {0x600d, 0x88},
+  {0x6016, 0x00},
+  {0x6027, 0x04},
+  {0x6028, 0x05},
+  {0x6029, 0x02},
+  {0x602b, 0x08},
+  {0x602c, 0x07},
+  {0x602d, 0x88},
+  {0x6047, 0x04},
+  {0x6048, 0x05},
+  {0x6049, 0x02},
+  {0x604b, 0x08},
+  {0x604c, 0x07},
+  {0x604d, 0x88},
+  {0x6067, 0x04},
+  {0x6068, 0x05},
+  {0x6069, 0x02},
+  {0x606b, 0x08},
+  {0x606c, 0x07},
+  {0x606d, 0x88},
+  {0x6087, 0x04},
+  {0x6088, 0x05},
+  {0x6089, 0x02},
+  {0x608b, 0x08},
+  {0x608c, 0x07},
+  {0x608d, 0x88},
+
+  // 12-bit PWL0
+  {0x5e00, 0x00},
+
+  // m_ndX_exp[0:32]
+  // 9*2+0xa*3+0xb*2+0xc*2+0xd*2+0xe*2+0xf*2+0x10*2+0x11*2+0x12*4+0x13*3+0x14*3+0x15*3+0x16 = 518
+  {0x5e01, 0x09},
+  {0x5e02, 0x09},
+  {0x5e03, 0x0a},
+  {0x5e04, 0x0a},
+  {0x5e05, 0x0a},
+  {0x5e06, 0x0b},
+  {0x5e07, 0x0b},
+  {0x5e08, 0x0c},
+  {0x5e09, 0x0c},
+  {0x5e0a, 0x0d},
+  {0x5e0b, 0x0d},
+  {0x5e0c, 0x0e},
+  {0x5e0d, 0x0e},
+  {0x5e0e, 0x0f},
+  {0x5e0f, 0x0f},
+  {0x5e10, 0x10},
+  {0x5e11, 0x10},
+  {0x5e12, 0x11},
+  {0x5e13, 0x11},
+  {0x5e14, 0x12},
+  {0x5e15, 0x12},
+  {0x5e16, 0x12},
+  {0x5e17, 0x12},
+  {0x5e18, 0x13},
+  {0x5e19, 0x13},
+  {0x5e1a, 0x13},
+  {0x5e1b, 0x14},
+  {0x5e1c, 0x14},
+  {0x5e1d, 0x14},
+  {0x5e1e, 0x15},
+  {0x5e1f, 0x15},
+  {0x5e20, 0x15},
+  {0x5e21, 0x16},
+
+  // m_ndY_val[0:32]
+  // 0x200+0xff+0x100*3+0x80*12+0x40*16 = 4095
+  {0x5e22, 0x00}, {0x5e23, 0x02}, {0x5e24, 0x00},
+  {0x5e25, 0x00}, {0x5e26, 0x00}, {0x5e27, 0xff},
+  {0x5e28, 0x00}, {0x5e29, 0x01}, {0x5e2a, 0x00},
+  {0x5e2b, 0x00}, {0x5e2c, 0x01}, {0x5e2d, 0x00},
+  {0x5e2e, 0x00}, {0x5e2f, 0x01}, {0x5e30, 0x00},
+  {0x5e31, 0x00}, {0x5e32, 0x00}, {0x5e33, 0x80},
+  {0x5e34, 0x00}, {0x5e35, 0x00}, {0x5e36, 0x80},
+  {0x5e37, 0x00}, {0x5e38, 0x00}, {0x5e39, 0x80},
+  {0x5e3a, 0x00}, {0x5e3b, 0x00}, {0x5e3c, 0x80},
+  {0x5e3d, 0x00}, {0x5e3e, 0x00}, {0x5e3f, 0x80},
+  {0x5e40, 0x00}, {0x5e41, 0x00}, {0x5e42, 0x80},
+  {0x5e43, 0x00}, {0x5e44, 0x00}, {0x5e45, 0x80},
+  {0x5e46, 0x00}, {0x5e47, 0x00}, {0x5e48, 0x80},
+  {0x5e49, 0x00}, {0x5e4a, 0x00}, {0x5e4b, 0x80},
+  {0x5e4c, 0x00}, {0x5e4d, 0x00}, {0x5e4e, 0x80},
+  {0x5e4f, 0x00}, {0x5e50, 0x00}, {0x5e51, 0x80},
+  {0x5e52, 0x00}, {0x5e53, 0x00}, {0x5e54, 0x80},
+  {0x5e55, 0x00}, {0x5e56, 0x00}, {0x5e57, 0x40},
+  {0x5e58, 0x00}, {0x5e59, 0x00}, {0x5e5a, 0x40},
+  {0x5e5b, 0x00}, {0x5e5c, 0x00}, {0x5e5d, 0x40},
+  {0x5e5e, 0x00}, {0x5e5f, 0x00}, {0x5e60, 0x40},
+  {0x5e61, 0x00}, {0x5e62, 0x00}, {0x5e63, 0x40},
+  {0x5e64, 0x00}, {0x5e65, 0x00}, {0x5e66, 0x40},
+  {0x5e67, 0x00}, {0x5e68, 0x00}, {0x5e69, 0x40},
+  {0x5e6a, 0x00}, {0x5e6b, 0x00}, {0x5e6c, 0x40},
+  {0x5e6d, 0x00}, {0x5e6e, 0x00}, {0x5e6f, 0x40},
+  {0x5e70, 0x00}, {0x5e71, 0x00}, {0x5e72, 0x40},
+  {0x5e73, 0x00}, {0x5e74, 0x00}, {0x5e75, 0x40},
+  {0x5e76, 0x00}, {0x5e77, 0x00}, {0x5e78, 0x40},
+  {0x5e79, 0x00}, {0x5e7a, 0x00}, {0x5e7b, 0x40},
+  {0x5e7c, 0x00}, {0x5e7d, 0x00}, {0x5e7e, 0x40},
+  {0x5e7f, 0x00}, {0x5e80, 0x00}, {0x5e81, 0x40},
+  {0x5e82, 0x00}, {0x5e83, 0x00}, {0x5e84, 0x40},
+
+  // disable PWL
+  /*{0x5e01, 0x18}, {0x5e02, 0x00}, {0x5e03, 0x00}, {0x5e04, 0x00},
+  {0x5e05, 0x00}, {0x5e06, 0x00}, {0x5e07, 0x00}, {0x5e08, 0x00},
+  {0x5e09, 0x00}, {0x5e0a, 0x00}, {0x5e0b, 0x00}, {0x5e0c, 0x00},
+  {0x5e0d, 0x00}, {0x5e0e, 0x00}, {0x5e0f, 0x00}, {0x5e10, 0x00},
+  {0x5e11, 0x00}, {0x5e12, 0x00}, {0x5e13, 0x00}, {0x5e14, 0x00},
+  {0x5e15, 0x00}, {0x5e16, 0x00}, {0x5e17, 0x00}, {0x5e18, 0x00},
+  {0x5e19, 0x00}, {0x5e1a, 0x00}, {0x5e1b, 0x00}, {0x5e1c, 0x00},
+  {0x5e1d, 0x00}, {0x5e1e, 0x00}, {0x5e1f, 0x00}, {0x5e20, 0x00},
+  {0x5e21, 0x00},
+
+  {0x5e22, 0x00}, {0x5e23, 0x0f}, {0x5e24, 0xFF},*/
+
+  {0x4001, 0x2b},  // BLC_CTRL_1
+  {0x4008, 0x02}, {0x4009, 0x03},
+  {0x4018, 0x12},
+  {0x4022, 0x40},
+  {0x4023, 0x20},
+
+  // all black level targets are 0x40
+  {0x4026, 0x00}, {0x4027, 0x40},
+  {0x4028, 0x00}, {0x4029, 0x40},
+  {0x402a, 0x00}, {0x402b, 0x40},
+  {0x402c, 0x00}, {0x402d, 0x40},
+
+  {0x407e, 0xcc},
+  {0x407f, 0x18},
+  {0x4080, 0xff},
+  {0x4081, 0xff},
+  {0x4082, 0x01},
+  {0x4083, 0x53},
+  {0x4084, 0x01},
+  {0x4085, 0x2b},
+  {0x4086, 0x00},
+  {0x4087, 0xb3},
+
+  {0x4640, 0x40},
+  {0x4641, 0x11},
+  {0x4642, 0x0e},
+  {0x4643, 0xee},
+  {0x4646, 0x0f},
+  {0x4648, 0x00},
+  {0x4649, 0x03},
+
+  {0x4f00, 0x00},
+  {0x4f01, 0x00},
+  {0x4f02, 0x80},
+  {0x4f03, 0x2c},
+  {0x4f04, 0xf8},
+
+  {0x4d09, 0xff},
+  {0x4d09, 0xdf},
+
+  {0x5003, 0x7a},
+  {0x5b80, 0x08},
+  {0x5c00, 0x08},
+  {0x5c80, 0x00},
+  {0x5bbe, 0x12},
+  {0x5c3e, 0x12},
+  {0x5cbe, 0x12},
+  {0x5b8a, 0x80},
+  {0x5b8b, 0x80},
+  {0x5b8c, 0x80},
+  {0x5b8d, 0x80},
+  {0x5b8e, 0x60},
+  {0x5b8f, 0x80},
+  {0x5b90, 0x80},
+  {0x5b91, 0x80},
+  {0x5b92, 0x80},
+  {0x5b93, 0x20},
+  {0x5b94, 0x80},
+  {0x5b95, 0x80},
+  {0x5b96, 0x80},
+  {0x5b97, 0x20},
+  {0x5b98, 0x00},
+  {0x5b99, 0x80},
+  {0x5b9a, 0x40},
+  {0x5b9b, 0x20},
+  {0x5b9c, 0x00},
+  {0x5b9d, 0x00},
+  {0x5b9e, 0x80},
+  {0x5b9f, 0x00},
+  {0x5ba0, 0x00},
+  {0x5ba1, 0x00},
+  {0x5ba2, 0x00},
+  {0x5ba3, 0x00},
+  {0x5ba4, 0x00},
+  {0x5ba5, 0x00},
+  {0x5ba6, 0x00},
+  {0x5ba7, 0x00},
+  {0x5ba8, 0x02},
+  {0x5ba9, 0x00},
+  {0x5baa, 0x02},
+  {0x5bab, 0x76},
+  {0x5bac, 0x03},
+  {0x5bad, 0x08},
+  {0x5bae, 0x00},
+  {0x5baf, 0x80},
+  {0x5bb0, 0x00},
+  {0x5bb1, 0xc0},
+  {0x5bb2, 0x01},
+  {0x5bb3, 0x00},
+
+  // m_nNormCombineWeight
+  {0x5c0a, 0x80}, {0x5c0b, 0x80}, {0x5c0c, 0x80}, {0x5c0d, 0x80}, {0x5c0e, 0x60},
+  {0x5c0f, 0x80}, {0x5c10, 0x80}, {0x5c11, 0x80}, {0x5c12, 0x60}, {0x5c13, 0x20},
+  {0x5c14, 0x80}, {0x5c15, 0x80}, {0x5c16, 0x80}, {0x5c17, 0x20}, {0x5c18, 0x00},
+  {0x5c19, 0x80}, {0x5c1a, 0x40}, {0x5c1b, 0x20}, {0x5c1c, 0x00}, {0x5c1d, 0x00},
+  {0x5c1e, 0x80}, {0x5c1f, 0x00}, {0x5c20, 0x00}, {0x5c21, 0x00}, {0x5c22, 0x00},
+  {0x5c23, 0x00}, {0x5c24, 0x00}, {0x5c25, 0x00}, {0x5c26, 0x00}, {0x5c27, 0x00},
+
+  // m_nCombinThreL
+  {0x5c28, 0x02}, {0x5c29, 0x00},
+  {0x5c2a, 0x02}, {0x5c2b, 0x76},
+  {0x5c2c, 0x03}, {0x5c2d, 0x08},
+
+  // m_nCombinThreS
+  {0x5c2e, 0x00}, {0x5c2f, 0x80},
+  {0x5c30, 0x00}, {0x5c31, 0xc0},
+  {0x5c32, 0x01}, {0x5c33, 0x00},
+
+  // m_nNormCombineWeight
+  {0x5c8a, 0x80}, {0x5c8b, 0x80}, {0x5c8c, 0x80}, {0x5c8d, 0x80}, {0x5c8e, 0x80},
+  {0x5c8f, 0x80}, {0x5c90, 0x80}, {0x5c91, 0x80}, {0x5c92, 0x80}, {0x5c93, 0x60},
+  {0x5c94, 0x80}, {0x5c95, 0x80}, {0x5c96, 0x80}, {0x5c97, 0x60}, {0x5c98, 0x40},
+  {0x5c99, 0x80}, {0x5c9a, 0x80}, {0x5c9b, 0x80}, {0x5c9c, 0x40}, {0x5c9d, 0x00},
+  {0x5c9e, 0x80}, {0x5c9f, 0x80}, {0x5ca0, 0x80}, {0x5ca1, 0x20}, {0x5ca2, 0x00},
+  {0x5ca3, 0x80}, {0x5ca4, 0x80}, {0x5ca5, 0x00}, {0x5ca6, 0x00}, {0x5ca7, 0x00},
+
+  {0x5ca8, 0x01}, {0x5ca9, 0x00},
+  {0x5caa, 0x02}, {0x5cab, 0x00},
+  {0x5cac, 0x03}, {0x5cad, 0x08},
+
+  {0x5cae, 0x01}, {0x5caf, 0x00},
+  {0x5cb0, 0x02}, {0x5cb1, 0x00},
+  {0x5cb2, 0x03}, {0x5cb3, 0x08},
+
+  // combine ISP
+  {0x5be7, 0x80},
+  {0x5bc9, 0x80},
+  {0x5bca, 0x80},
+  {0x5bcb, 0x80},
+  {0x5bcc, 0x80},
+  {0x5bcd, 0x80},
+  {0x5bce, 0x80},
+  {0x5bcf, 0x80},
+  {0x5bd0, 0x80},
+  {0x5bd1, 0x80},
+  {0x5bd2, 0x20},
+  {0x5bd3, 0x80},
+  {0x5bd4, 0x40},
+  {0x5bd5, 0x20},
+  {0x5bd6, 0x00},
+  {0x5bd7, 0x00},
+  {0x5bd8, 0x00},
+  {0x5bd9, 0x00},
+  {0x5bda, 0x00},
+  {0x5bdb, 0x00},
+  {0x5bdc, 0x00},
+  {0x5bdd, 0x00},
+  {0x5bde, 0x00},
+  {0x5bdf, 0x00},
+  {0x5be0, 0x00},
+  {0x5be1, 0x00},
+  {0x5be2, 0x00},
+  {0x5be3, 0x00},
+  {0x5be4, 0x00},
+  {0x5be5, 0x00},
+  {0x5be6, 0x00},
+
+  // m_nSPDCombineWeight
+  {0x5c49, 0x80}, {0x5c4a, 0x80}, {0x5c4b, 0x80}, {0x5c4c, 0x80}, {0x5c4d, 0x40},
+  {0x5c4e, 0x80}, {0x5c4f, 0x80}, {0x5c50, 0x80}, {0x5c51, 0x60}, {0x5c52, 0x20},
+  {0x5c53, 0x80}, {0x5c54, 0x80}, {0x5c55, 0x80}, {0x5c56, 0x20}, {0x5c57, 0x00},
+  {0x5c58, 0x80}, {0x5c59, 0x40}, {0x5c5a, 0x20}, {0x5c5b, 0x00}, {0x5c5c, 0x00},
+  {0x5c5d, 0x80}, {0x5c5e, 0x00}, {0x5c5f, 0x00}, {0x5c60, 0x00}, {0x5c61, 0x00},
+  {0x5c62, 0x00}, {0x5c63, 0x00}, {0x5c64, 0x00}, {0x5c65, 0x00}, {0x5c66, 0x00},
+
+  // m_nSPDCombineWeight
+  {0x5cc9, 0x80}, {0x5cca, 0x80}, {0x5ccb, 0x80}, {0x5ccc, 0x80}, {0x5ccd, 0x80},
+  {0x5cce, 0x80}, {0x5ccf, 0x80}, {0x5cd0, 0x80}, {0x5cd1, 0x80}, {0x5cd2, 0x60},
+  {0x5cd3, 0x80}, {0x5cd4, 0x80}, {0x5cd5, 0x80}, {0x5cd6, 0x60}, {0x5cd7, 0x40},
+  {0x5cd8, 0x80}, {0x5cd9, 0x80}, {0x5cda, 0x80}, {0x5cdb, 0x40}, {0x5cdc, 0x20},
+  {0x5cdd, 0x80}, {0x5cde, 0x80}, {0x5cdf, 0x80}, {0x5ce0, 0x20}, {0x5ce1, 0x00},
+  {0x5ce2, 0x80}, {0x5ce3, 0x80}, {0x5ce4, 0x80}, {0x5ce5, 0x00}, {0x5ce6, 0x00},
+
+  {0x5d74, 0x01},
+  {0x5d75, 0x00},
+
+  {0x5d1f, 0x81},
+  {0x5d11, 0x00},
+  {0x5d12, 0x10},
+  {0x5d13, 0x10},
+  {0x5d15, 0x05},
+  {0x5d16, 0x05},
+  {0x5d17, 0x05},
+  {0x5d08, 0x03},
+  {0x5d09, 0xb6},
+  {0x5d0a, 0x03},
+  {0x5d0b, 0xb6},
+  {0x5d18, 0x03},
+  {0x5d19, 0xb6},
+  {0x5d62, 0x01},
+  {0x5d40, 0x02},
+  {0x5d41, 0x01},
+  {0x5d63, 0x1f},
+  {0x5d64, 0x00},
+  {0x5d65, 0x80},
+  {0x5d56, 0x00},
+  {0x5d57, 0x20},
+  {0x5d58, 0x00},
+  {0x5d59, 0x20},
+  {0x5d5a, 0x00},
+  {0x5d5b, 0x0c},
+  {0x5d5c, 0x02},
+  {0x5d5d, 0x40},
+  {0x5d5e, 0x02},
+  {0x5d5f, 0x40},
+  {0x5d60, 0x03},
+  {0x5d61, 0x40},
+  {0x5d4a, 0x02},
+  {0x5d4b, 0x40},
+  {0x5d4c, 0x02},
+  {0x5d4d, 0x40},
+  {0x5d4e, 0x02},
+  {0x5d4f, 0x40},
+  {0x5d50, 0x18},
+  {0x5d51, 0x80},
+  {0x5d52, 0x18},
+  {0x5d53, 0x80},
+  {0x5d54, 0x18},
+  {0x5d55, 0x80},
+  {0x5d46, 0x20},
+  {0x5d47, 0x00},
+  {0x5d48, 0x22},
+  {0x5d49, 0x00},
+  {0x5d42, 0x20},
+  {0x5d43, 0x00},
+  {0x5d44, 0x22},
+  {0x5d45, 0x00},
+
+  {0x5004, 0x1e},
+  {0x4221, 0x03},  // this is changed from 1 -> 3
+
+  // DCG exposure coarse
+  // {0x3501, 0x01}, {0x3502, 0xc8},
+  // SPD exposure coarse
+  // {0x3541, 0x01}, {0x3542, 0xc8},
+  // VS exposure coarse
+  // {0x35c1, 0x00}, {0x35c2, 0x01},
+
+  // crc reference
+  {0x420e, 0x66}, {0x420f, 0x5d}, {0x4210, 0xa8}, {0x4211, 0x55},
+  // crc stat check
+  {0x507a, 0x5f}, {0x507b, 0x46},
+
+  // watchdog control
+  {0x4f00, 0x00}, {0x4f01, 0x01}, {0x4f02, 0x80}, {0x4f04, 0x2c},
+
+  // color balance gains
+  // blue
+  {0x5280, 0x06}, {0x5281, 0xCB}, // hcg
+  {0x5480, 0x06}, {0x5481, 0xCB}, // lcg
+  {0x5680, 0x06}, {0x5681, 0xCB}, // spd
+  {0x5880, 0x06}, {0x5881, 0xCB}, // vs
+
+  // green(blue)
+  {0x5282, 0x04}, {0x5283, 0x00},
+  {0x5482, 0x04}, {0x5483, 0x00},
+  {0x5682, 0x04}, {0x5683, 0x00},
+  {0x5882, 0x04}, {0x5883, 0x00},
+
+  // green(red)
+  {0x5284, 0x04}, {0x5285, 0x00},
+  {0x5484, 0x04}, {0x5485, 0x00},
+  {0x5684, 0x04}, {0x5685, 0x00},
+  {0x5884, 0x04}, {0x5885, 0x00},
+
+  // red
+  {0x5286, 0x08}, {0x5287, 0xDE},
+  {0x5486, 0x08}, {0x5487, 0xDE},
+  {0x5686, 0x08}, {0x5687, 0xDE},
+  {0x5886, 0x08}, {0x5887, 0xDE},
+
+  // fixed gains
+  {0x3588, 0x01}, {0x3589, 0x00},
+  {0x35c8, 0x01}, {0x35c9, 0x00},
+  {0x3548, 0x0F}, {0x3549, 0x00},
+  {0x35c1, 0x00},
+};

system/camerad/sensors/sensor.h

@@ -0,0 +1,117 @@
+#pragma once
+
+#include <cassert>
+#include <cstdint>
+#include <map>
+#include <utility>
+#include <vector>
+
+#include "media/cam_isp.h"
+#include "media/cam_sensor.h"
+
+#include "cereal/gen/cpp/log.capnp.h"
+#include "system/camerad/sensors/ar0231_registers.h"
+#include "system/camerad/sensors/ox03c10_registers.h"
+#include "system/camerad/sensors/os04c10_registers.h"
+
+#define ANALOG_GAIN_MAX_CNT 55
+
+class SensorInfo {
+public:
+  SensorInfo() = default;
+  virtual std::vector<i2c_random_wr_payload> getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const { return {}; }
+  virtual float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const {return 0; }
+  virtual int getSlaveAddress(int port) const { assert(0); }
+
+  cereal::FrameData::ImageSensor image_sensor = cereal::FrameData::ImageSensor::UNKNOWN;
+  float pixel_size_mm;
+  uint32_t frame_width, frame_height;
+  uint32_t frame_stride;
+  uint32_t frame_offset = 0;
+  uint32_t extra_height = 0;
+  int out_scale = 1;
+  int registers_offset = -1;
+  int stats_offset = -1;
+  int hdr_offset = -1;
+
+  int exposure_time_min;
+  int exposure_time_max;
+
+  float dc_gain_factor;
+  int dc_gain_min_weight;
+  int dc_gain_max_weight;
+  float dc_gain_on_grey;
+  float dc_gain_off_grey;
+
+  float ev_scale = 1.0;
+  float sensor_analog_gains[ANALOG_GAIN_MAX_CNT];
+  int analog_gain_min_idx;
+  int analog_gain_max_idx;
+  int analog_gain_rec_idx;
+  int analog_gain_cost_delta;
+  float analog_gain_cost_low;
+  float analog_gain_cost_high;
+  float target_grey_factor;
+  float min_ev;
+  float max_ev;
+
+  bool data_word;
+  uint32_t probe_reg_addr;
+  uint32_t probe_expected_data;
+  std::vector<i2c_random_wr_payload> start_reg_array;
+  std::vector<i2c_random_wr_payload> init_reg_array;
+
+  uint32_t bits_per_pixel;
+  uint32_t bayer_pattern;
+  uint32_t mipi_format;
+  uint32_t mclk_frequency;
+  uint32_t frame_data_type;
+
+  uint32_t readout_time_ns;  // used to recover EOF from SOF
+
+  // ISP image processing params
+  uint32_t black_level;
+  std::vector<uint32_t> color_correct_matrix;  // 3x3
+  std::vector<uint32_t> gamma_lut_rgb;         // gamma LUTs are length 64 * sizeof(uint32_t); same for r/g/b here
+  void prepare_gamma_lut() {
+    for (int i = 0; i < 64; i++) {
+      gamma_lut_rgb[i] |= ((uint32_t)(gamma_lut_rgb[i+1] - gamma_lut_rgb[i]) << 10);
+    }
+    gamma_lut_rgb.pop_back();
+  }
+  std::vector<uint32_t> linearization_lut;     // length 36
+  std::vector<uint32_t> linearization_pts;     // length 4
+  std::vector<uint32_t> vignetting_lut;        // length 221
+
+  const int num() const {
+    return static_cast<int>(image_sensor);
+  };
+};
+
+class AR0231 : public SensorInfo {
+public:
+  AR0231();
+  std::vector<i2c_random_wr_payload> getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override;
+  float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override;
+  int getSlaveAddress(int port) const override;
+
+private:
+  mutable std::map<uint16_t, std::pair<int, int>> ar0231_register_lut;
+};
+
+class OX03C10 : public SensorInfo {
+public:
+  OX03C10();
+  std::vector<i2c_random_wr_payload> getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override;
+  float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override;
+  int getSlaveAddress(int port) const override;
+};
+
+class OS04C10 : public SensorInfo {
+public:
+  OS04C10();
+  void ife_downscale_configure();
+  std::vector<i2c_random_wr_payload> getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override;
+  float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override;
+  int getSlaveAddress(int port) const override;
+};

system/camerad/snapshot/snapshot.py

@@ -0,0 +1,125 @@
+#!/usr/bin/env python3
+import subprocess
+import time
+
+import numpy as np
+from PIL import Image
+
+import cereal.messaging as messaging
+from msgq.visionipc import VisionIpcClient, VisionStreamType
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.system.hardware import PC
+from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
+from openpilot.system.manager.process_config import managed_processes
+
+
+VISION_STREAMS = {
+  "roadCameraState": VisionStreamType.VISION_STREAM_ROAD,
+  "driverCameraState": VisionStreamType.VISION_STREAM_DRIVER,
+  "wideRoadCameraState": VisionStreamType.VISION_STREAM_WIDE_ROAD,
+}
+
+
+def jpeg_write(fn, dat):
+  img = Image.fromarray(dat)
+  img.save(fn, "JPEG")
+
+
+def yuv_to_rgb(y, u, v):
+  ul = np.repeat(np.repeat(u, 2).reshape(u.shape[0], y.shape[1]), 2, axis=0).reshape(y.shape)
+  vl = np.repeat(np.repeat(v, 2).reshape(v.shape[0], y.shape[1]), 2, axis=0).reshape(y.shape)
+
+  yuv = np.dstack((y, ul, vl)).astype(np.int16)
+  yuv[:, :, 1:] -= 128
+
+  m = np.array([
+    [1.00000,  1.00000, 1.00000],
+    [0.00000, -0.39465, 2.03211],
+    [1.13983, -0.58060, 0.00000],
+  ])
+  rgb = np.dot(yuv, m).clip(0, 255)
+  return rgb.astype(np.uint8)
+
+
+def extract_image(buf):
+  y = np.array(buf.data[:buf.uv_offset], dtype=np.uint8).reshape((-1, buf.stride))[:buf.height, :buf.width]
+  u = np.array(buf.data[buf.uv_offset::2], dtype=np.uint8).reshape((-1, buf.stride//2))[:buf.height//2, :buf.width//2]
+  v = np.array(buf.data[buf.uv_offset+1::2], dtype=np.uint8).reshape((-1, buf.stride//2))[:buf.height//2, :buf.width//2]
+
+  return yuv_to_rgb(y, u, v)
+
+
+def get_snapshots(frame="roadCameraState", front_frame="driverCameraState"):
+  sockets = [s for s in (frame, front_frame) if s is not None]
+  sm = messaging.SubMaster(sockets)
+  vipc_clients = {s: VisionIpcClient("camerad", VISION_STREAMS[s], True) for s in sockets}
+
+  # wait 4 sec from camerad startup for focus and exposure
+  while sm[sockets[0]].frameId < int(4. / DT_MDL):
+    sm.update()
+
+  for client in vipc_clients.values():
+    client.connect(True)
+
+  # grab images
+  rear, front = None, None
+  if frame is not None:
+    c = vipc_clients[frame]
+    rear = extract_image(c.recv())
+  if front_frame is not None:
+    c = vipc_clients[front_frame]
+    front = extract_image(c.recv())
+  return rear, front
+
+
+def snapshot():
+  params = Params()
+
+  if (not params.get_bool("IsOffroad")) or params.get_bool("IsTakingSnapshot"):
+    print("Already taking snapshot")
+    return None, None
+
+  front_camera_allowed = params.get_bool("RecordFront")
+  params.put_bool("IsTakingSnapshot", True)
+  set_offroad_alert("Offroad_IsTakingSnapshot", True)
+  time.sleep(2.0)  # Give hardwared time to read the param, or if just started give camerad time to start
+
+  # Check if camerad is already started
+  try:
+    subprocess.check_call(["pgrep", "camerad"])
+    print("Camerad already running")
+    params.put_bool("IsTakingSnapshot", False)
+    params.remove("Offroad_IsTakingSnapshot")
+    return None, None
+  except subprocess.CalledProcessError:
+    pass
+
+  try:
+    # Allow testing on replay on PC
+    if not PC:
+      managed_processes['camerad'].start()
+
+    frame = "wideRoadCameraState"
+    front_frame = "driverCameraState" if front_camera_allowed else None
+    rear, front = get_snapshots(frame, front_frame)
+  finally:
+    managed_processes['camerad'].stop()
+    params.put_bool("IsTakingSnapshot", False)
+    set_offroad_alert("Offroad_IsTakingSnapshot", False)
+
+  if not front_camera_allowed:
+    front = None
+
+  return rear, front
+
+
+if __name__ == "__main__":
+  pic, fpic = snapshot()
+  if pic is not None:
+    print(pic.shape)
+    jpeg_write("/tmp/back.jpg", pic)
+    if fpic is not None:
+      jpeg_write("/tmp/front.jpg", fpic)
+  else:
+    print("Error taking snapshot")

system/camerad/test/check_skips.py

@@ -0,0 +1,27 @@
+#!/usr/bin/env python3
+# type: ignore
+import cereal.messaging as messaging
+
+all_sockets = ['roadCameraState', 'driverCameraState', 'wideRoadCameraState']
+prev_id = [None,None,None]
+this_id = [None,None,None]
+dt = [None,None,None]
+num_skipped = [0,0,0]
+
+if __name__ == "__main__":
+  sm = messaging.SubMaster(all_sockets)
+  while True:
+    sm.update()
+
+    for i in range(len(all_sockets)):
+      if not sm.updated[all_sockets[i]]:
+        continue
+      this_id[i] = sm[all_sockets[i]].frameId
+      if prev_id[i] is None:
+        prev_id[i] = this_id[i]
+        continue
+      dt[i] = this_id[i] - prev_id[i]
+      if dt[i] != 1:
+        num_skipped[i] += dt[i] - 1
+        print(all_sockets[i] ,dt[i] - 1, num_skipped[i])
+      prev_id[i] = this_id[i]

system/camerad/test/debug.sh

@@ -0,0 +1,16 @@
+#!/usr/bin/env bash
+set -e
+
+#echo 4294967295 | sudo tee /sys/module/cam_debug_util/parameters/debug_mdl
+
+# no CCI and UTIL, very spammy
+echo 0xfffdbfff | sudo tee /sys/module/cam_debug_util/parameters/debug_mdl
+#echo 0 | sudo tee /sys/module/cam_debug_util/parameters/debug_mdl
+
+sudo dmesg -C
+scons -u -j8 --minimal .
+export DEBUG_FRAMES=1
+export DISABLE_ROAD=1 DISABLE_WIDE_ROAD=1
+#export DISABLE_DRIVER=1
+export LOGPRINT=debug
+./camerad

system/camerad/test/get_thumbnails_for_segment.py

@@ -0,0 +1,24 @@
+#!/usr/bin/env python3
+import argparse
+import os
+from tqdm import tqdm
+
+from openpilot.tools.lib.logreader import LogReader
+
+if __name__ == "__main__":
+  parser = argparse.ArgumentParser()
+  parser.add_argument("route", help="The route name")
+  args = parser.parse_args()
+
+  out_path = os.path.join("jpegs", f"{args.route.replace('|', '_').replace('/', '_')}")
+  os.makedirs(out_path, exist_ok=True)
+
+  lr = LogReader(args.route)
+
+  for msg in tqdm(lr):
+    if msg.which() == 'thumbnail':
+      with open(os.path.join(out_path, f"{msg.thumbnail.frameId}.jpg"), 'wb') as f:
+        f.write(msg.thumbnail.thumbnail)
+    elif msg.which() == 'navThumbnail':
+      with open(os.path.join(out_path, f"nav_{msg.navThumbnail.frameId}.jpg"), 'wb') as f:
+        f.write(msg.navThumbnail.thumbnail)

system/camerad/test/icp_debug.sh

@@ -0,0 +1,13 @@
+#!/usr/bin/env bash
+set -e
+
+cd /sys/kernel/debug/tracing
+echo "" > trace
+echo 1 > tracing_on
+#echo Y > /sys/kernel/debug/camera_icp/a5_debug_q
+echo 0x1 > /sys/kernel/debug/camera_icp/a5_debug_type
+echo 1 > /sys/kernel/debug/tracing/events/camera/enable
+echo 0xffffffff > /sys/kernel/debug/camera_icp/a5_debug_lvl
+echo 1 > /sys/kernel/debug/tracing/events/camera/cam_icp_fw_dbg/enable
+
+cat /sys/kernel/debug/tracing/trace_pipe

system/camerad/test/intercept.sh

@@ -0,0 +1,2 @@
+#!/usr/bin/env bash
+DISABLE_ROAD=1 DISABLE_WIDE_ROAD=1 DEBUG_FRAMES=1 LOGPRINT=debug LD_PRELOAD=/data/tici_test_scripts/isp/interceptor/tmpioctl.so ./camerad

system/camerad/test/stress_restart.sh

@@ -0,0 +1,9 @@
+#!/bin/sh
+cd ..
+while :; do
+  ./camerad &
+  pid="$!"
+  sleep 2
+  kill -2 $pid
+  wait $pid
+done

system/camerad/test/test_camerad.py

@@ -0,0 +1,98 @@
+import os
+import time
+import pytest
+import numpy as np
+
+import cereal.messaging as messaging
+from cereal.services import SERVICE_LIST
+from openpilot.system.manager.process_config import managed_processes
+from openpilot.tools.lib.log_time_series import msgs_to_time_series
+
+TEST_TIMESPAN = 10
+CAMERAS = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState')
+
+
+def run_and_log(procs, services, duration):
+  logs = []
+
+  try:
+    for p in procs:
+      managed_processes[p].start()
+    socks = [messaging.sub_sock(s, conflate=False, timeout=100) for s in services]
+
+    start_time = time.monotonic()
+    while time.monotonic() - start_time < duration:
+      for s in socks:
+        logs.extend(messaging.drain_sock(s))
+    for p in procs:
+      assert managed_processes[p].proc.is_alive()
+  finally:
+    for p in procs:
+      managed_processes[p].stop()
+
+  return logs
+
+@pytest.fixture(scope="module")
+def logs():
+  logs = run_and_log(["camerad", ], CAMERAS, TEST_TIMESPAN)
+  ts = msgs_to_time_series(logs)
+
+  for cam in CAMERAS:
+    expected_frames = SERVICE_LIST[cam].frequency * TEST_TIMESPAN
+    cnt = len(ts[cam]['t'])
+    assert expected_frames*0.8 < cnt < expected_frames*1.2, f"unexpected frame count {cam}: {expected_frames=}, got {cnt}"
+
+    dts = np.abs(np.diff([ts[cam]['timestampSof']/1e6]) - 1000/SERVICE_LIST[cam].frequency)
+    assert (dts < 1.0).all(), f"{cam} dts(ms) out of spec: max diff {dts.max()}, 99 percentile {np.percentile(dts, 99)}"
+  return ts
+
+@pytest.mark.tici
+class TestCamerad:
+  def test_frame_skips(self, logs):
+    for c in CAMERAS:
+      assert set(np.diff(logs[c]['frameId'])) == {1, }, f"{c} has frame skips"
+
+  def test_frame_sync(self, logs):
+    n = range(len(logs['roadCameraState']['t'][:-10]))
+
+    frame_ids = {i: [logs[cam]['frameId'][i] for cam in CAMERAS] for i in n}
+    assert all(len(set(v)) == 1 for v in frame_ids.values()), "frame IDs not aligned"
+
+    frame_times = {i: [logs[cam]['timestampSof'][i] for cam in CAMERAS] for i in n}
+    diffs = {i: (max(ts) - min(ts))/1e6 for i, ts in frame_times.items()}
+
+    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=}"
+
+  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/camerad/test/test_exposure.py

@@ -0,0 +1,51 @@
+import time
+import numpy as np
+import pytest
+
+from openpilot.selfdrive.test.helpers import with_processes
+from openpilot.system.camerad.snapshot.snapshot import get_snapshots
+
+TEST_TIME = 45
+REPEAT = 5
+
+@pytest.mark.tici
+class TestCamerad:
+  @classmethod
+  def setup_class(cls):
+    pass
+
+  def _numpy_rgb2gray(self, im):
+    ret = np.clip(im[:,:,2] * 0.114 + im[:,:,1] * 0.587 + im[:,:,0] * 0.299, 0, 255).astype(np.uint8)
+    return ret
+
+  def _is_exposure_okay(self, i, med_mean=None):
+    if med_mean is None:
+      med_mean = np.array([[0.2,0.4],[0.2,0.6]])
+    h, w = i.shape[:2]
+    i = i[h//10:9*h//10,w//10:9*w//10]
+    med_ex, mean_ex = med_mean
+    i = self._numpy_rgb2gray(i)
+    i_median = np.median(i) / 255.
+    i_mean = np.mean(i) / 255.
+    print([i_median, i_mean])
+    return med_ex[0] < i_median < med_ex[1] and mean_ex[0] < i_mean < mean_ex[1]
+
+  @with_processes(['camerad'])
+  def test_camera_operation(self):
+    passed = 0
+    start = time.time()
+    while time.time() - start < TEST_TIME and passed < REPEAT:
+      rpic, dpic = get_snapshots(frame="roadCameraState", front_frame="driverCameraState")
+      wpic, _ = get_snapshots(frame="wideRoadCameraState")
+
+      res = self._is_exposure_okay(rpic)
+      res = res and self._is_exposure_okay(dpic)
+      res = res and self._is_exposure_okay(wpic)
+
+      if passed > 0 and not res:
+        passed = -passed # fails test if any failure after first sus
+        break
+
+      passed += int(res)
+      time.sleep(2)
+    assert passed >= REPEAT