From 84560ccd55bc87bb9615176d69a5b8afc40660e1 Mon Sep 17 00:00:00 2001 From: George Hotz Date: Fri, 17 Jan 2020 10:52:42 -0800 Subject: [PATCH] selfdrive/camerad --- selfdrive/camerad/SConscript | 19 + selfdrive/camerad/__init__.py | 0 selfdrive/camerad/bufs.h | 8 + selfdrive/camerad/cameras/camera_common.h | 47 + .../camerad/cameras/camera_frame_stream.cc | 166 ++ .../camerad/cameras/camera_frame_stream.h | 58 + selfdrive/camerad/cameras/camera_qcom.c | 2214 +++++++++++++++++ selfdrive/camerad/cameras/camera_qcom.h | 131 + selfdrive/camerad/cameras/debayer.cl | 130 + selfdrive/camerad/cameras/sensor_i2c.h | 1830 ++++++++++++++ selfdrive/camerad/include/msm_cam_sensor.h | 829 ++++++ selfdrive/camerad/include/msm_camsensor_sdk.h | 386 +++ selfdrive/camerad/include/msmb_camera.h | 220 ++ selfdrive/camerad/include/msmb_isp.h | 880 +++++++ selfdrive/camerad/include/msmb_ispif.h | 125 + selfdrive/camerad/main.cc | 1128 +++++++++ selfdrive/camerad/snapshot/__init__.py | 0 selfdrive/camerad/snapshot/snapshot.py | 72 + selfdrive/camerad/snapshot/visionipc.py | 93 + selfdrive/camerad/test/camera/test.c | 48 + selfdrive/camerad/test/camera/test.sh | 10 + selfdrive/camerad/test/frame_test.py | 40 + selfdrive/camerad/test/stress_restart.sh | 9 + selfdrive/camerad/test/yuv_bench/Makefile | 57 + selfdrive/camerad/test/yuv_bench/cnv.py | 19 + selfdrive/camerad/test/yuv_bench/yuv.cl | 116 + selfdrive/camerad/test/yuv_bench/yuv_bench.cc | 145 ++ selfdrive/camerad/transforms/rgb_to_yuv.c | 53 + selfdrive/camerad/transforms/rgb_to_yuv.cl | 127 + selfdrive/camerad/transforms/rgb_to_yuv.h | 32 + .../camerad/transforms/rgb_to_yuv_test.cc | 201 ++ 31 files changed, 9193 insertions(+) create mode 100644 selfdrive/camerad/SConscript create mode 100644 selfdrive/camerad/__init__.py create mode 100644 selfdrive/camerad/bufs.h create mode 100644 selfdrive/camerad/cameras/camera_common.h create mode 100644 selfdrive/camerad/cameras/camera_frame_stream.cc create mode 100644 selfdrive/camerad/cameras/camera_frame_stream.h create mode 100644 selfdrive/camerad/cameras/camera_qcom.c create mode 100644 selfdrive/camerad/cameras/camera_qcom.h create mode 100644 selfdrive/camerad/cameras/debayer.cl create mode 100644 selfdrive/camerad/cameras/sensor_i2c.h create mode 100644 selfdrive/camerad/include/msm_cam_sensor.h create mode 100644 selfdrive/camerad/include/msm_camsensor_sdk.h create mode 100644 selfdrive/camerad/include/msmb_camera.h create mode 100644 selfdrive/camerad/include/msmb_isp.h create mode 100644 selfdrive/camerad/include/msmb_ispif.h create mode 100644 selfdrive/camerad/main.cc create mode 100644 selfdrive/camerad/snapshot/__init__.py create mode 100755 selfdrive/camerad/snapshot/snapshot.py create mode 100644 selfdrive/camerad/snapshot/visionipc.py create mode 100644 selfdrive/camerad/test/camera/test.c create mode 100755 selfdrive/camerad/test/camera/test.sh create mode 100755 selfdrive/camerad/test/frame_test.py create mode 100755 selfdrive/camerad/test/stress_restart.sh create mode 100644 selfdrive/camerad/test/yuv_bench/Makefile create mode 100644 selfdrive/camerad/test/yuv_bench/cnv.py create mode 100644 selfdrive/camerad/test/yuv_bench/yuv.cl create mode 100644 selfdrive/camerad/test/yuv_bench/yuv_bench.cc create mode 100644 selfdrive/camerad/transforms/rgb_to_yuv.c create mode 100644 selfdrive/camerad/transforms/rgb_to_yuv.cl create mode 100644 selfdrive/camerad/transforms/rgb_to_yuv.h create mode 100644 selfdrive/camerad/transforms/rgb_to_yuv_test.cc diff --git a/selfdrive/camerad/SConscript b/selfdrive/camerad/SConscript new file mode 100644 index 000000000..12e1afe5a --- /dev/null +++ b/selfdrive/camerad/SConscript @@ -0,0 +1,19 @@ +Import('env', 'arch', 'messaging', 'common', 'gpucommon', 'visionipc', 'cereal') + +libs = ['m', 'pthread', common, 'jpeg', 'json', cereal, 'OpenCL', messaging, 'czmq', 'zmq', 'capnp', 'kj', 'capnp_c', visionipc, gpucommon] + +if arch == "aarch64": + libs += ['gsl', 'CB', 'adreno_utils', 'EGL', 'GLESv3', 'cutils', 'ui'] + cameras = ['cameras/camera_qcom.c'] +else: + libs += [] + cameras = ['cameras/camera_frame_stream.cc'] + +env.SharedLibrary('snapshot/visionipc', + ["#selfdrive/common/visionipc.c", "#selfdrive/common/ipc.c"]) + +env.Program('camerad', [ + 'main.cc', + 'transforms/rgb_to_yuv.c', + cameras, + ], LIBS=libs) diff --git a/selfdrive/camerad/__init__.py b/selfdrive/camerad/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/camerad/bufs.h b/selfdrive/camerad/bufs.h new file mode 100644 index 000000000..71930baed --- /dev/null +++ b/selfdrive/camerad/bufs.h @@ -0,0 +1,8 @@ +#ifndef _SELFDRIVE_VISIOND_VISIOND_H_ +#define _SELFDRIVE_VISIOND_VISIOND_H_ + +#include + +typedef struct { uint8_t *y, *u, *v; } YUVBuf; + +#endif // _SELFDRIVE_VISIOND_VISIOND_H_ diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h new file mode 100644 index 000000000..187952e22 --- /dev/null +++ b/selfdrive/camerad/cameras/camera_common.h @@ -0,0 +1,47 @@ +#ifndef CAMERA_COMMON_H +#define CAMERA_COMMON_H + +#include +#include + +#define CAMERA_ID_IMX298 0 +#define CAMERA_ID_IMX179 1 +#define CAMERA_ID_S5K3P8SP 2 +#define CAMERA_ID_OV8865 3 +#define CAMERA_ID_IMX298_FLIPPED 4 +#define CAMERA_ID_OV10640 5 +#define CAMERA_ID_MAX 6 + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct CameraInfo { + const char* name; + int frame_width, frame_height; + int frame_stride; + bool bayer; + int bayer_flip; + bool hdr; +} CameraInfo; + +typedef struct FrameMetadata { + uint32_t frame_id; + uint64_t timestamp_eof; + unsigned int frame_length; + unsigned int integ_lines; + unsigned int global_gain; + unsigned int lens_pos; + float lens_sag; + float lens_err; + float lens_true_pos; + float gain_frac; +} FrameMetadata; + +extern CameraInfo cameras_supported[CAMERA_ID_MAX]; + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/selfdrive/camerad/cameras/camera_frame_stream.cc b/selfdrive/camerad/cameras/camera_frame_stream.cc new file mode 100644 index 000000000..1a9f8a931 --- /dev/null +++ b/selfdrive/camerad/cameras/camera_frame_stream.cc @@ -0,0 +1,166 @@ +#include "camera_frame_stream.h" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include "cereal/gen/cpp/log.capnp.h" +#include "messaging.hpp" + +#include "common/util.h" +#include "common/timing.h" +#include "common/swaglog.h" +#include "buffering.h" + +extern "C" { +#include +} + +extern volatile sig_atomic_t do_exit; + +#define FRAME_WIDTH 1164 +#define FRAME_HEIGHT 874 + +namespace { +void camera_open(CameraState *s, VisionBuf *camera_bufs, bool rear) { + assert(camera_bufs); + s->camera_bufs = camera_bufs; +} + +void camera_close(CameraState *s) { + tbuffer_stop(&s->camera_tb); +} + +void camera_release_buffer(void *cookie, int buf_idx) { + CameraState *s = static_cast(cookie); +} + +void camera_init(CameraState *s, int camera_id, unsigned int fps) { + assert(camera_id < ARRAYSIZE(cameras_supported)); + s->ci = cameras_supported[camera_id]; + assert(s->ci.frame_width != 0); + + s->frame_size = s->ci.frame_height * s->ci.frame_stride; + s->fps = fps; + + tbuffer_init2(&s->camera_tb, FRAME_BUF_COUNT, "frame", camera_release_buffer, s); +} + +void run_frame_stream(DualCameraState *s) { + int err; + Context * context = Context::create(); + SubSocket * recorder_sock = SubSocket::create(context, "frame"); + assert(recorder_sock != NULL); + + CameraState *const rear_camera = &s->rear; + auto *tb = &rear_camera->camera_tb; + + while (!do_exit) { + Message * msg = recorder_sock->receive(); + + auto amsg = kj::heapArray((msg->getSize() / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), msg->getData(), msg->getSize()); + + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::Event::Reader event = cmsg.getRoot(); + auto frame = event.getFrame(); + + const int buf_idx = tbuffer_select(tb); + rear_camera->camera_bufs_metadata[buf_idx] = { + .frame_id = frame.getFrameId(), + .timestamp_eof = frame.getTimestampEof(), + .frame_length = static_cast(frame.getFrameLength()), + .integ_lines = static_cast(frame.getIntegLines()), + .global_gain = static_cast(frame.getGlobalGain()), + }; + + cl_command_queue q = rear_camera->camera_bufs[buf_idx].copy_q; + cl_mem yuv_cl = rear_camera->camera_bufs[buf_idx].buf_cl; + cl_event map_event; + void *yuv_buf = (void *)clEnqueueMapBuffer(q, yuv_cl, CL_TRUE, + CL_MAP_WRITE, 0, frame.getImage().size(), + 0, NULL, &map_event, &err); + assert(err == 0); + clWaitForEvents(1, &map_event); + clReleaseEvent(map_event); + memcpy(yuv_buf, frame.getImage().begin(), frame.getImage().size()); + + clEnqueueUnmapMemObject(q, yuv_cl, yuv_buf, 0, NULL, &map_event); + clWaitForEvents(1, &map_event); + clReleaseEvent(map_event); + tbuffer_dispatch(tb, buf_idx); + delete msg; + + } + delete recorder_sock; + delete context; +} + +} // namespace + +CameraInfo cameras_supported[CAMERA_ID_MAX] = { + [CAMERA_ID_IMX298] = { + .frame_width = FRAME_WIDTH, + .frame_height = FRAME_HEIGHT, + .frame_stride = FRAME_WIDTH*3, + .bayer = false, + .bayer_flip = false, + }, + [CAMERA_ID_OV8865] = { + .frame_width = 1632, + .frame_height = 1224, + .frame_stride = 2040, // seems right + .bayer = false, + .bayer_flip = 3, + .hdr = false + }, +}; + +void cameras_init(DualCameraState *s) { + memset(s, 0, sizeof(*s)); + + camera_init(&s->rear, CAMERA_ID_IMX298, 20); + s->rear.transform = (mat3){{ + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0, + }}; + + camera_init(&s->front, CAMERA_ID_OV8865, 10); + s->front.transform = (mat3){{ + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0, + }}; +} + +void camera_autoexposure(CameraState *s, float grey_frac) {} + +void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, + VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, + VisionBuf *camera_bufs_front) { + assert(camera_bufs_rear); + assert(camera_bufs_front); + int err; + + // LOG("*** open front ***"); + camera_open(&s->front, camera_bufs_front, false); + + // LOG("*** open rear ***"); + camera_open(&s->rear, camera_bufs_rear, true); +} + +void cameras_close(DualCameraState *s) { + camera_close(&s->rear); +} + +void cameras_run(DualCameraState *s) { + set_thread_name("frame_streaming"); + run_frame_stream(s); + cameras_close(s); +} diff --git a/selfdrive/camerad/cameras/camera_frame_stream.h b/selfdrive/camerad/cameras/camera_frame_stream.h new file mode 100644 index 000000000..5cb7e27c5 --- /dev/null +++ b/selfdrive/camerad/cameras/camera_frame_stream.h @@ -0,0 +1,58 @@ +#ifndef CAMERA_FRAME_STREAM_H +#define CAMERA_FRAME_STREAM_H + +#include + +#ifdef __APPLE__ +#include +#else +#include +#endif + +#include "common/mat.h" + +#include "buffering.h" +#include "common/visionbuf.h" +#include "camera_common.h" + +#define FRAME_BUF_COUNT 16 + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct CameraState { + int camera_id; + CameraInfo ci; + int frame_size; + + VisionBuf *camera_bufs; + FrameMetadata camera_bufs_metadata[FRAME_BUF_COUNT]; + TBuffer camera_tb; + + int fps; + float digital_gain; + + float cur_gain_frac; + + mat3 transform; +} CameraState; + + +typedef struct DualCameraState { + int ispif_fd; + + CameraState rear; + CameraState front; +} DualCameraState; + +void cameras_init(DualCameraState *s); +void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front); +void cameras_run(DualCameraState *s); +void cameras_close(DualCameraState *s); +void camera_autoexposure(CameraState *s, float grey_frac); +#ifdef __cplusplus +} // extern "C" +#endif + +#endif diff --git a/selfdrive/camerad/cameras/camera_qcom.c b/selfdrive/camerad/cameras/camera_qcom.c new file mode 100644 index 000000000..c0f10f13b --- /dev/null +++ b/selfdrive/camerad/cameras/camera_qcom.c @@ -0,0 +1,2214 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include + +#include "msmb_isp.h" +#include "msmb_ispif.h" +#include "msmb_camera.h" +#include "msm_cam_sensor.h" + +#include "common/util.h" +#include "common/timing.h" +#include "common/swaglog.h" +#include "common/params.h" + +#include "cereal/gen/c/log.capnp.h" + +#include "sensor_i2c.h" + +#include "camera_qcom.h" + + +// enable this to run the camera at 60fps and sample every third frame +// supposed to reduce 33ms of lag, but no results +//#define HIGH_FPS + +#define CAMERA_MSG_AUTOEXPOSE 0 + +typedef struct CameraMsg { + int type; + int camera_num; + + float grey_frac; +} CameraMsg; + +extern volatile sig_atomic_t do_exit; + +CameraInfo cameras_supported[CAMERA_ID_MAX] = { + [CAMERA_ID_IMX298] = { + .frame_width = 2328, + .frame_height = 1748, + .frame_stride = 2912, + .bayer = true, + .bayer_flip = 0, + .hdr = true + }, + [CAMERA_ID_IMX179] = { + .frame_width = 3280, + .frame_height = 2464, + .frame_stride = 4104, + .bayer = true, + .bayer_flip = 0, + .hdr = false + }, + [CAMERA_ID_S5K3P8SP] = { + .frame_width = 2304, + .frame_height = 1728, + .frame_stride = 2880, + .bayer = true, + .bayer_flip = 1, + .hdr = false + }, + [CAMERA_ID_OV8865] = { + .frame_width = 1632, + .frame_height = 1224, + .frame_stride = 2040, // seems right + .bayer = true, + .bayer_flip = 3, + .hdr = false + }, + // this exists to get the kernel to build for the LeEco in release + [CAMERA_ID_IMX298_FLIPPED] = { + .frame_width = 2328, + .frame_height = 1748, + .frame_stride = 2912, + .bayer = true, + .bayer_flip = 3, + .hdr = true + }, + [CAMERA_ID_OV10640] = { + .frame_width = 1280, + .frame_height = 1080, + .frame_stride = 2040, + .bayer = true, + .bayer_flip = 0, + .hdr = true + }, +}; + +static void camera_release_buffer(void* cookie, int buf_idx) { + CameraState *s = cookie; + // printf("camera_release_buffer %d\n", buf_idx); + s->ss[0].qbuf_info[buf_idx].dirty_buf = 1; + ioctl(s->isp_fd, VIDIOC_MSM_ISP_ENQUEUE_BUF, &s->ss[0].qbuf_info[buf_idx]); +} + +static void camera_init(CameraState *s, int camera_id, int camera_num, + uint32_t pixel_clock, uint32_t line_length_pclk, + unsigned int max_gain, unsigned int fps) { + memset(s, 0, sizeof(*s)); + + s->camera_num = camera_num; + s->camera_id = camera_id; + + assert(camera_id < ARRAYSIZE(cameras_supported)); + s->ci = cameras_supported[camera_id]; + assert(s->ci.frame_width != 0); + s->frame_size = s->ci.frame_height * s->ci.frame_stride; + + s->pixel_clock = pixel_clock; + s->line_length_pclk = line_length_pclk; + s->max_gain = max_gain; + s->fps = fps; + + zsock_t *ops_sock = zsock_new_push(">inproc://cameraops"); + assert(ops_sock); + s->ops_sock = zsock_resolve(ops_sock); + + tbuffer_init2(&s->camera_tb, FRAME_BUF_COUNT, "frame", + camera_release_buffer, s); + + pthread_mutex_init(&s->frame_info_lock, NULL); +} + + +int sensor_write_regs(CameraState *s, struct msm_camera_i2c_reg_array* arr, size_t size, int data_type) { + struct msm_camera_i2c_reg_setting out_settings = { + .reg_setting = arr, + .size = size, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .data_type = data_type, + .delay = 0, + }; + struct sensorb_cfg_data cfg_data = {0}; + cfg_data.cfgtype = CFG_WRITE_I2C_ARRAY; + cfg_data.cfg.setting = &out_settings; + return ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &cfg_data); +} + +static int imx298_apply_exposure(CameraState *s, int gain, int integ_lines, int frame_length) { + int err; + + int analog_gain = min(gain, 448); + + if (gain > 448) { + s->digital_gain = (512.0/(512-(gain))) / 8.0; + } else { + s->digital_gain = 1.0; + } + + //printf("%5d/%5d %5d %f\n", s->cur_integ_lines, s->cur_frame_length, analog_gain, s->digital_gain); + + int digital_gain = 0x100; + + float white_balance[] = {0.4609375, 1.0, 0.546875}; + //float white_balance[] = {1.0, 1.0, 1.0}; + + int digital_gain_gr = digital_gain / white_balance[1]; + int digital_gain_gb = digital_gain / white_balance[1]; + int digital_gain_r = digital_gain / white_balance[0]; + int digital_gain_b = digital_gain / white_balance[2]; + + struct msm_camera_i2c_reg_array reg_array[] = { + // REG_HOLD + {0x104,0x1,0}, + {0x3002,0x0,0}, // long autoexposure off + + // FRM_LENGTH + {0x340, frame_length >> 8, 0}, {0x341, frame_length & 0xff, 0}, + // INTEG_TIME aka coarse_int_time_addr aka shutter speed + {0x202, integ_lines >> 8, 0}, {0x203, integ_lines & 0xff,0}, + // global_gain_addr + // if you assume 1x gain is 32, 448 is 14x gain, aka 2^14=16384 + {0x204, analog_gain >> 8, 0}, {0x205, analog_gain & 0xff,0}, + + // digital gain for colors: gain_greenR, gain_red, gain_blue, gain_greenB + /*{0x20e, digital_gain_gr >> 8, 0}, {0x20f,digital_gain_gr & 0xFF,0}, + {0x210, digital_gain_r >> 8, 0}, {0x211,digital_gain_r & 0xFF,0}, + {0x212, digital_gain_b >> 8, 0}, {0x213,digital_gain_b & 0xFF,0}, + {0x214, digital_gain_gb >> 8, 0}, {0x215,digital_gain_gb & 0xFF,0},*/ + + // REG_HOLD + {0x104,0x0,0}, + }; + + err = sensor_write_regs(s, reg_array, ARRAYSIZE(reg_array), MSM_CAMERA_I2C_BYTE_DATA); + if (err != 0) { + LOGE("apply_exposure err %d", err); + } + return err; +} + +static inline int ov8865_get_coarse_gain(int gain) { + static const int gains[] = {0, 256, 384, 448, 480}; + int i; + + for (i = 1; i < ARRAYSIZE(gains); i++) { + if (gain >= gains[i - 1] && gain < gains[i]) + break; + } + + return i - 1; +} + +static int ov8865_apply_exposure(CameraState *s, int gain, int integ_lines, int frame_length) { + //printf("front camera: %d %d %d\n", gain, integ_lines, frame_length); + int err, gain_bitmap; + gain_bitmap = (1 << ov8865_get_coarse_gain(gain)) - 1; + integ_lines *= 16; // The exposure value in reg is in 16ths of a line + struct msm_camera_i2c_reg_array reg_array[] = { + //{0x104,0x1,0}, + + // FRM_LENGTH + {0x380e, frame_length >> 8, 0}, {0x380f, frame_length & 0xff, 0}, + // AEC EXPO + {0x3500, integ_lines >> 16, 0}, {0x3501, integ_lines >> 8, 0}, {0x3502, integ_lines & 0xff,0}, + // AEC MANUAL + {0x3503, 0x4, 0}, + // AEC GAIN + {0x3508, gain_bitmap, 0}, {0x3509, 0xf8, 0}, + + //{0x104,0x0,0}, + }; + err = sensor_write_regs(s, reg_array, ARRAYSIZE(reg_array), MSM_CAMERA_I2C_BYTE_DATA); + if (err != 0) { + LOGE("apply_exposure err %d", err); + } + return err; +} + +static int imx179_s5k3p8sp_apply_exposure(CameraState *s, int gain, int integ_lines, int frame_length) { + //printf("front camera: %d %d %d\n", gain, integ_lines, frame_length); + int err; + + if (gain > 448) { + s->digital_gain = (512.0/(512-(gain))) / 8.0; + } else { + s->digital_gain = 1.0; + } + + struct msm_camera_i2c_reg_array reg_array[] = { + {0x104,0x1,0}, + + // FRM_LENGTH + {0x340, frame_length >> 8, 0}, {0x341, frame_length & 0xff, 0}, + // coarse_int_time + {0x202, integ_lines >> 8, 0}, {0x203, integ_lines & 0xff,0}, + // global_gain + {0x204, gain >> 8, 0}, {0x205, gain & 0xff,0}, + + {0x104,0x0,0}, + }; + err = sensor_write_regs(s, reg_array, ARRAYSIZE(reg_array), MSM_CAMERA_I2C_BYTE_DATA); + if (err != 0) { + LOGE("apply_exposure err %d", err); + } + return err; +} + +void cameras_init(DualCameraState *s) { + memset(s, 0, sizeof(*s)); + + char project_name[1024] = {0}; + property_get("ro.boot.project_name", project_name, ""); + + char product_name[1024] = {0}; + property_get("ro.product.name", product_name, ""); + + if (strlen(project_name) == 0) { + LOGD("LePro 3 op system detected"); + s->device = DEVICE_LP3; + + // sensor is flipped in LP3 + // IMAGE_ORIENT = 3 + init_array_imx298[0].reg_data = 3; + cameras_supported[CAMERA_ID_IMX298].bayer_flip = 3; + } else if (strcmp(product_name, "OnePlus3") == 0 && strcmp(project_name, "15811") != 0) { + // no more OP3 support + s->device = DEVICE_OP3; + assert(false); + } else if (strcmp(product_name, "OnePlus3") == 0 && strcmp(project_name, "15811") == 0) { + // only OP3T support + s->device = DEVICE_OP3T; + } else { + assert(false); + } + + // 0 = ISO 100 + // 256 = ISO 200 + // 384 = ISO 400 + // 448 = ISO 800 + // 480 = ISO 1600 + // 496 = ISO 3200 + // 504 = ISO 6400, 8x digital gain + // 508 = ISO 12800, 16x digital gain + // 510 = ISO 25600, 32x digital gain + + camera_init(&s->rear, CAMERA_ID_IMX298, 0, + /*pixel_clock=*/600000000, /*line_length_pclk=*/5536, + /*max_gain=*/510, //0 (ISO 100)- 448 (ISO 800, max analog gain) - 511 (super noisy) +#ifdef HIGH_FPS + /*fps*/60 +#else + /*fps*/20 +#endif + ); + s->rear.apply_exposure = imx298_apply_exposure; + + if (s->device == DEVICE_OP3T) { + camera_init(&s->front, CAMERA_ID_S5K3P8SP, 1, + /*pixel_clock=*/561000000, /*line_length_pclk=*/5120, + /*max_gain=*/510, 10); + s->front.apply_exposure = imx179_s5k3p8sp_apply_exposure; + } else if (s->device == DEVICE_LP3) { + camera_init(&s->front, CAMERA_ID_OV8865, 1, + /*pixel_clock=*/251200000, /*line_length_pclk=*/7000, + /*max_gain=*/510, 10); + s->front.apply_exposure = ov8865_apply_exposure; + } else { + camera_init(&s->front, CAMERA_ID_IMX179, 1, + /*pixel_clock=*/251200000, /*line_length_pclk=*/3440, + /*max_gain=*/224, 20); + s->front.apply_exposure = imx179_s5k3p8sp_apply_exposure; + } + + // assume the device is upside-down (not anymore) + s->rear.transform = (mat3){{ + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0, + }}; + + // probably wrong + s->front.transform = (mat3){{ + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0, + }}; + + s->rear.device = s->device; + s->front.device = s->device; +} + +static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) { + int err = 0; + + unsigned int frame_length = s->pixel_clock / s->line_length_pclk / s->fps; + + unsigned int gain = s->cur_gain; + unsigned int integ_lines = s->cur_integ_lines; + + if (exposure_frac >= 0) { + exposure_frac = clamp(exposure_frac, 2.0 / frame_length, 1.0); + integ_lines = frame_length * exposure_frac; + + // See page 79 of the datasheet, this is the max allowed (-1 for phase adjust) + integ_lines = min(integ_lines, frame_length-11); + } + + if (gain_frac >= 0) { + // ISO200 is minimum gain + gain_frac = clamp(gain_frac, 1.0/64, 1.0); + + // linearize gain response + // TODO: will be wrong for front camera + // 0.125 -> 448 + // 0.25 -> 480 + // 0.5 -> 496 + // 1.0 -> 504 + // 512 - 512/(128*gain_frac) + gain = (s->max_gain/510) * (512 - 512/(256*gain_frac)); + } + + if (gain != s->cur_gain + || integ_lines != s->cur_integ_lines + || frame_length != s->cur_frame_length) { + + if (s->apply_exposure) { + err = s->apply_exposure(s, gain, integ_lines, frame_length); + } + + if (err == 0) { + pthread_mutex_lock(&s->frame_info_lock); + s->cur_gain = gain; + s->cur_integ_lines = integ_lines; + s->cur_frame_length = frame_length; + pthread_mutex_unlock(&s->frame_info_lock); + } + } + + if (err == 0) { + s->cur_exposure_frac = exposure_frac; + s->cur_gain_frac = gain_frac; + } + + //LOGD("set exposure: %f %f - %d", exposure_frac, gain_frac, err); +} + +static void do_autoexposure(CameraState *s, float grey_frac) { + const float target_grey = 0.3; + + float new_exposure = s->cur_exposure_frac; + new_exposure *= pow(1.05, (target_grey - grey_frac) / 0.05 ); + //LOGD("diff %f: %f to %f", target_grey - grey_frac, s->cur_exposure_frac, new_exposure); + + float new_gain = s->cur_gain_frac; + if (new_exposure < 0.10) { + new_gain *= 0.95; + } else if (new_exposure > 0.40) { + new_gain *= 1.05; + } + + set_exposure(s, new_exposure, new_gain); +} + +void camera_autoexposure(CameraState *s, float grey_frac) { + CameraMsg msg = { + .type = CAMERA_MSG_AUTOEXPOSE, + .camera_num = s->camera_num, + .grey_frac = grey_frac, + }; + + zmq_send(s->ops_sock, &msg, sizeof(msg), ZMQ_DONTWAIT); +} + +static uint8_t* get_eeprom(int eeprom_fd, size_t *out_len) { + int err; + + struct msm_eeprom_cfg_data cfg = {0}; + cfg.cfgtype = CFG_EEPROM_GET_CAL_DATA; + err = ioctl(eeprom_fd, VIDIOC_MSM_EEPROM_CFG, &cfg); + assert(err >= 0); + + uint32_t num_bytes = cfg.cfg.get_data.num_bytes; + assert(num_bytes > 100); + + uint8_t* buffer = malloc(num_bytes); + assert(buffer); + memset(buffer, 0, num_bytes); + + cfg.cfgtype = CFG_EEPROM_READ_CAL_DATA; + cfg.cfg.read_data.num_bytes = num_bytes; + cfg.cfg.read_data.dbuffer = buffer; + err = ioctl(eeprom_fd, VIDIOC_MSM_EEPROM_CFG, &cfg); + assert(err >= 0); + + *out_len = num_bytes; + return buffer; +} + +static void imx298_ois_calibration(int ois_fd, uint8_t* eeprom) { + int err; + + const int ois_registers[][2] = { + // == SET_FADJ_PARAM() == (factory adjustment) + + // Set Hall Current DAC + {0x8230, *(uint16_t*)(eeprom+0x102)}, //_P_30_ADC_CH0 (CURDAT) + + // Set Hall PreAmp Offset + {0x8231, *(uint16_t*)(eeprom+0x104)}, //_P_31_ADC_CH1 (HALOFS_X) + {0x8232, *(uint16_t*)(eeprom+0x106)}, //_P_32_ADC_CH2 (HALOFS_Y) + + // Set Hall-X/Y PostAmp Offset + {0x841e, *(uint16_t*)(eeprom+0x108)}, //_M_X_H_ofs + {0x849e, *(uint16_t*)(eeprom+0x10a)}, //_M_Y_H_ofs + + // Set Residual Offset + {0x8239, *(uint16_t*)(eeprom+0x10c)}, //_P_39_Ch3_VAL_1 (PSTXOF) + {0x823b, *(uint16_t*)(eeprom+0x10e)}, //_P_3B_Ch3_VAL_3 (PSTYOF) + + // DIGITAL GYRO OFFSET + {0x8406, *(uint16_t*)(eeprom+0x110)}, //_M_Kgx00 + {0x8486, *(uint16_t*)(eeprom+0x112)}, //_M_Kgy00 + {0x846a, *(uint16_t*)(eeprom+0x120)}, //_M_TMP_X_ + {0x846b, *(uint16_t*)(eeprom+0x122)}, //_M_TMP_Y_ + + // HALLSENSE + // Set Hall Gain + {0x8446, *(uint16_t*)(eeprom+0x114)}, //_M_KgxHG + {0x84c6, *(uint16_t*)(eeprom+0x116)}, //_M_KgyHG + // Set Cross Talk Canceller + {0x8470, *(uint16_t*)(eeprom+0x124)}, //_M_KgxH0 + {0x8472, *(uint16_t*)(eeprom+0x126)}, //_M_KgyH0 + + // LOOPGAIN + {0x840f, *(uint16_t*)(eeprom+0x118)}, //_M_KgxG + {0x848f, *(uint16_t*)(eeprom+0x11a)}, //_M_KgyG + + // Position Servo ON ( OIS OFF ) + {0x847f, 0x0c0c}, //_M_EQCTL + }; + + + struct msm_ois_cfg_data cfg = {0}; + struct msm_camera_i2c_seq_reg_array ois_reg_settings[ARRAYSIZE(ois_registers)] = {{0}}; + for (int i=0; i> 8) & 0xff; + ois_reg_settings[i].reg_data_size = 2; + } + struct msm_camera_i2c_seq_reg_setting ois_reg_setting = { + .reg_setting = &ois_reg_settings[0], + .size = ARRAYSIZE(ois_reg_settings), + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .delay = 0, + }; + cfg.cfgtype = CFG_OIS_I2C_WRITE_SEQ_TABLE; + cfg.cfg.settings = &ois_reg_setting; + err = ioctl(ois_fd, VIDIOC_MSM_OIS_CFG, &cfg); + LOG("ois reg calibration: %d", err); +} + + + + +static void sensors_init(DualCameraState *s) { + int err; + + int sensorinit_fd = -1; + if (s->device == DEVICE_LP3) { + sensorinit_fd = open("/dev/v4l-subdev11", O_RDWR | O_NONBLOCK); + } else { + sensorinit_fd = open("/dev/v4l-subdev12", O_RDWR | O_NONBLOCK); + } + assert(sensorinit_fd >= 0); + + struct sensor_init_cfg_data sensor_init_cfg = {0}; + + // init rear sensor + + struct msm_camera_sensor_slave_info slave_info = {0}; + if (s->device == DEVICE_LP3) { + slave_info = (struct msm_camera_sensor_slave_info){ + .sensor_name = "imx298", + .eeprom_name = "sony_imx298", + .actuator_name = "dw9800w", + .ois_name = "", + .flash_name = "pmic", + .camera_id = 0, + .slave_addr = 32, + .i2c_freq_mode = 1, + .addr_type = 2, + .sensor_id_info = { + .sensor_id_reg_addr = 22, + .sensor_id = 664, + .sensor_id_mask = 0, + .module_id = 9, + .vcm_id = 6, + }, + .power_setting_array = { + .power_setting_a = { + { + .seq_type = 1, + .seq_val = 0, + .config_val = 0, + .delay = 1, + },{ + .seq_type = 2, + .seq_val = 2, + .config_val = 0, + .delay = 0, + },{ + .seq_type = 1, + .seq_val = 5, + .config_val = 2, + .delay = 0, + },{ + .seq_type = 2, + .seq_val = 1, + .config_val = 0, + .delay = 0, + },{ + .seq_type = 2, + .seq_val = 3, + .config_val = 0, + .delay = 1, + },{ + .seq_type = 0, + .seq_val = 0, + .config_val = 24000000, + .delay = 1, + },{ + .seq_type = 1, + .seq_val = 0, + .config_val = 2, + .delay = 10, + }, + }, + .size = 7, + .power_down_setting_a = { + { + .seq_type = 0, + .seq_val = 0, + .config_val = 0, + .delay = 1, + },{ + .seq_type = 1, + .seq_val = 0, + .config_val = 0, + .delay = 1, + },{ + .seq_type = 2, + .seq_val = 1, + .config_val = 0, + .delay = 0, + },{ + .seq_type = 1, + .seq_val = 5, + .config_val = 0, + .delay = 0, + },{ + .seq_type = 2, + .seq_val = 2, + .config_val = 0, + .delay = 0, + },{ + .seq_type = 2, + .seq_val = 3, + .config_val = 0, + .delay = 1, + }, + }, + .size_down = 6, + }, + .is_init_params_valid = 0, + .sensor_init_params = { + .modes_supported = 1, + .position = 0, + .sensor_mount_angle = 90, + }, + .output_format = 0, + }; + } else { + slave_info = (struct msm_camera_sensor_slave_info){ + .sensor_name = "imx298", + .eeprom_name = "sony_imx298", + .actuator_name = "rohm_bu63165gwl", + .ois_name = "rohm_bu63165gwl", + .camera_id = 0, + .slave_addr = 52, + .i2c_freq_mode = 2, + .addr_type = 2, + .sensor_id_info = { + .sensor_id_reg_addr = 22, + .sensor_id = 664, + .sensor_id_mask = 0, + }, + .power_setting_array = { + .power_setting_a = { + { + .seq_type = 1, + .seq_val = 0, + .config_val = 0, + .delay = 2, + },{ + .seq_type = 2, + .seq_val = 2, + .config_val = 0, + .delay = 2, + },{ + .seq_type = 2, + .seq_val = 0, + .config_val = 0, + .delay = 2, + },{ + .seq_type = 2, + .seq_val = 1, + .config_val = 0, + .delay = 2, + },{ + .seq_type = 1, + .seq_val = 6, + .config_val = 2, + .delay = 0, + },{ + .seq_type = 2, + .seq_val = 3, + .config_val = 0, + .delay = 5, + },{ + .seq_type = 2, + .seq_val = 4, + .config_val = 0, + .delay = 5, + },{ + .seq_type = 0, + .seq_val = 0, + .config_val = 24000000, + .delay = 2, + },{ + .seq_type = 1, + .seq_val = 0, + .config_val = 2, + .delay = 2, + }, + }, + .size = 9, + .power_down_setting_a = { + { + .seq_type = 1, + .seq_val = 0, + .config_val = 0, + .delay = 10, + },{ + .seq_type = 0, + .seq_val = 0, + .config_val = 0, + .delay = 1, + },{ + .seq_type = 2, + .seq_val = 4, + .config_val = 0, + .delay = 0, + },{ + .seq_type = 2, + .seq_val = 3, + .config_val = 0, + .delay = 1, + },{ + .seq_type = 1, + .seq_val = 6, + .config_val = 0, + .delay = 0, + },{ + .seq_type = 2, + .seq_val = 1, + .config_val = 0, + .delay = 0, + },{ + .seq_type = 2, + .seq_val = 0, + .config_val = 0, + .delay = 0, + },{ + .seq_type = 2, + .seq_val = 2, + .config_val = 0, + .delay = 0, + }, + }, + .size_down = 8, + }, + .is_init_params_valid = 0, + .sensor_init_params = { + .modes_supported = 1, + .position = 0, + .sensor_mount_angle = 360, + }, + .output_format = 0, + }; + } + slave_info.power_setting_array.power_setting = + (struct msm_sensor_power_setting *)&slave_info.power_setting_array.power_setting_a[0]; + slave_info.power_setting_array.power_down_setting = + (struct msm_sensor_power_setting *)&slave_info.power_setting_array.power_down_setting_a[0]; + sensor_init_cfg.cfgtype = CFG_SINIT_PROBE; + sensor_init_cfg.cfg.setting = &slave_info; + err = ioctl(sensorinit_fd, VIDIOC_MSM_SENSOR_INIT_CFG, &sensor_init_cfg); + LOG("sensor init cfg (rear): %d", err); + assert(err >= 0); + + + struct msm_camera_sensor_slave_info slave_info2 = {0}; + if (s->device == DEVICE_LP3) { + slave_info2 = (struct msm_camera_sensor_slave_info){ + .sensor_name = "ov8865_sunny", + .eeprom_name = "ov8865_plus", + .actuator_name = "", + .ois_name = "", + .flash_name = "", + .camera_id = 2, + .slave_addr = 108, + .i2c_freq_mode = 1, + .addr_type = 2, + .sensor_id_info = { + .sensor_id_reg_addr = 12299, + .sensor_id = 34917, + .sensor_id_mask = 0, + .module_id = 2, + .vcm_id = 0, + }, + .power_setting_array = { + .power_setting_a = { + { + .seq_type = 1, + .seq_val = 0, + .config_val = 0, + .delay = 5, + },{ + .seq_type = 2, + .seq_val = 1, + .config_val = 0, + .delay = 0, + },{ + .seq_type = 2, + .seq_val = 2, + .config_val = 0, + .delay = 0, + },{ + .seq_type = 2, + .seq_val = 0, + .config_val = 0, + .delay = 0, + },{ + .seq_type = 0, + .seq_val = 0, + .config_val = 24000000, + .delay = 1, + },{ + .seq_type = 1, + .seq_val = 0, + .config_val = 2, + .delay = 1, + }, + }, + .size = 6, + .power_down_setting_a = { + { + .seq_type = 1, + .seq_val = 0, + .config_val = 0, + .delay = 5, + },{ + .seq_type = 0, + .seq_val = 0, + .config_val = 0, + .delay = 1, + },{ + .seq_type = 2, + .seq_val = 0, + .config_val = 0, + .delay = 0, + },{ + .seq_type = 2, + .seq_val = 1, + .config_val = 0, + .delay = 0, + },{ + .seq_type = 2, + .seq_val = 2, + .config_val = 0, + .delay = 1, + }, + }, + .size_down = 5, + }, + .is_init_params_valid = 0, + .sensor_init_params = { + .modes_supported = 1, + .position = 1, + .sensor_mount_angle = 270, + }, + .output_format = 0, + }; + } else if (s->front.camera_id == CAMERA_ID_S5K3P8SP) { + // init front camera + slave_info2 = (struct msm_camera_sensor_slave_info){ + .sensor_name = "s5k3p8sp", + .eeprom_name = "s5k3p8sp_m24c64s", + .actuator_name = "", + .ois_name = "", + .camera_id = 1, + .slave_addr = 32, + .i2c_freq_mode = 1, + .addr_type = 2, + .sensor_id_info = { + .sensor_id_reg_addr = 0, + .sensor_id = 12552, + .sensor_id_mask = 0, + }, + .power_setting_array = { + .power_setting_a = { + { + .seq_type = 1, + .seq_val = 0, + .config_val = 0, + .delay = 1, + },{ + .seq_type = 2, + .seq_val = 2, + .config_val = 0, + .delay = 1, + },{ + .seq_type = 2, + .seq_val = 1, + .config_val = 0, + .delay = 1, + },{ + .seq_type = 2, + .seq_val = 0, + .config_val = 0, + .delay = 1, + },{ + .seq_type = 0, + .seq_val = 0, + .config_val = 24000000, + .delay = 1, + },{ + .seq_type = 1, + .seq_val = 0, + .config_val = 2, + .delay = 1, + }, + }, + .size = 6, + .power_down_setting_a = { + { + .seq_type = 0, + .seq_val = 0, + .config_val = 0, + .delay = 1, + },{ + .seq_type = 1, + .seq_val = 0, + .config_val = 0, + .delay = 1, + },{ + .seq_type = 2, + .seq_val = 0, + .config_val = 0, + .delay = 1, + },{ + .seq_type = 2, + .seq_val = 1, + .config_val = 0, + .delay = 1, + },{ + .seq_type = 2, + .seq_val = 2, + .config_val = 0, + .delay = 1, + }, + }, + .size_down = 5, + }, + .is_init_params_valid = 0, + .sensor_init_params = { + .modes_supported = 1, + .position = 1, + .sensor_mount_angle = 270, + }, + .output_format = 0, + }; + } else { + // init front camera + slave_info2 = (struct msm_camera_sensor_slave_info){ + .sensor_name = "imx179", + .eeprom_name = "sony_imx179", + .actuator_name = "", + .ois_name = "", + .camera_id = 1, + .slave_addr = 32, + .i2c_freq_mode = 1, + .addr_type = 2, + .sensor_id_info = { + .sensor_id_reg_addr = 2, + .sensor_id = 377, + .sensor_id_mask = 4095, + }, + .power_setting_array = { + .power_setting_a = { + { + .seq_type = 2, + .seq_val = 2, + .config_val = 0, + .delay = 0, + },{ + .seq_type = 2, + .seq_val = 1, + .config_val = 0, + .delay = 0, + },{ + .seq_type = 2, + .seq_val = 0, + .config_val = 0, + .delay = 0, + },{ + .seq_type = 1, + .seq_val = 0, + .config_val = 2, + .delay = 0, + },{ + .seq_type = 0, + .seq_val = 0, + .config_val = 24000000, + .delay = 0, + }, + }, + .size = 5, + .power_down_setting_a = { + { + .seq_type = 0, + .seq_val = 0, + .config_val = 0, + .delay = 0, + },{ + .seq_type = 1, + .seq_val = 0, + .config_val = 0, + .delay = 1, + },{ + .seq_type = 2, + .seq_val = 0, + .config_val = 0, + .delay = 2, + },{ + .seq_type = 2, + .seq_val = 1, + .config_val = 0, + .delay = 0, + },{ + .seq_type = 2, + .seq_val = 2, + .config_val = 0, + .delay = 0, + }, + }, + .size_down = 5, + }, + .is_init_params_valid = 0, + .sensor_init_params = { + .modes_supported = 1, + .position = 1, + .sensor_mount_angle = 270, + }, + .output_format = 0, + }; + } + slave_info2.power_setting_array.power_setting = + (struct msm_sensor_power_setting *)&slave_info2.power_setting_array.power_setting_a[0]; + slave_info2.power_setting_array.power_down_setting = + (struct msm_sensor_power_setting *)&slave_info2.power_setting_array.power_down_setting_a[0]; + sensor_init_cfg.cfgtype = CFG_SINIT_PROBE; + sensor_init_cfg.cfg.setting = &slave_info2; + err = ioctl(sensorinit_fd, VIDIOC_MSM_SENSOR_INIT_CFG, &sensor_init_cfg); + LOG("sensor init cfg (front): %d", err); + assert(err >= 0); +} + +static void camera_open(CameraState *s, bool rear) { + int err; + + struct sensorb_cfg_data sensorb_cfg_data = {0}; + struct csid_cfg_data csid_cfg_data = {0}; + struct csiphy_cfg_data csiphy_cfg_data = {0}; + struct msm_camera_csiphy_params csiphy_params = {0}; + struct msm_camera_csid_params csid_params = {0}; + struct msm_vfe_input_cfg input_cfg = {0}; + struct msm_vfe_axi_stream_update_cmd update_cmd = {0}; + struct v4l2_event_subscription sub = {0}; + struct ispif_cfg_data ispif_cfg_data = {0}; + struct msm_vfe_cfg_cmd_list cfg_cmd_list = {0}; + + struct msm_actuator_cfg_data actuator_cfg_data = {0}; + struct msm_ois_cfg_data ois_cfg_data = {0}; + + // open devices + char *sensor_dev; + if (rear) { + s->csid_fd = open("/dev/v4l-subdev3", O_RDWR | O_NONBLOCK); + assert(s->csid_fd >= 0); + s->csiphy_fd = open("/dev/v4l-subdev0", O_RDWR | O_NONBLOCK); + assert(s->csiphy_fd >= 0); + if (s->device == DEVICE_LP3) { + sensor_dev = "/dev/v4l-subdev17"; + } else { + sensor_dev = "/dev/v4l-subdev18"; + } + if (s->device == DEVICE_LP3) { + s->isp_fd = open("/dev/v4l-subdev13", O_RDWR | O_NONBLOCK); + } else { + s->isp_fd = open("/dev/v4l-subdev14", O_RDWR | O_NONBLOCK); + } + assert(s->isp_fd >= 0); + s->eeprom_fd = open("/dev/v4l-subdev8", O_RDWR | O_NONBLOCK); + assert(s->eeprom_fd >= 0); + + s->actuator_fd = open("/dev/v4l-subdev7", O_RDWR | O_NONBLOCK); + assert(s->actuator_fd >= 0); + + if (s->device != DEVICE_LP3) { + s->ois_fd = open("/dev/v4l-subdev10", O_RDWR | O_NONBLOCK); + assert(s->ois_fd >= 0); + } + } else { + s->csid_fd = open("/dev/v4l-subdev5", O_RDWR | O_NONBLOCK); + assert(s->csid_fd >= 0); + s->csiphy_fd = open("/dev/v4l-subdev2", O_RDWR | O_NONBLOCK); + assert(s->csiphy_fd >= 0); + if (s->device == DEVICE_LP3) { + sensor_dev = "/dev/v4l-subdev18"; + } else { + sensor_dev = "/dev/v4l-subdev19"; + } + if (s->device == DEVICE_LP3) { + s->isp_fd = open("/dev/v4l-subdev14", O_RDWR | O_NONBLOCK); + } else { + s->isp_fd = open("/dev/v4l-subdev15", O_RDWR | O_NONBLOCK); + } + assert(s->isp_fd >= 0); + s->eeprom_fd = open("/dev/v4l-subdev9", O_RDWR | O_NONBLOCK); + assert(s->eeprom_fd >= 0); + } + + // wait for sensor device + // on first startup, these devices aren't present yet + for (int i = 0; i < 10; i++) { + s->sensor_fd = open(sensor_dev, O_RDWR | O_NONBLOCK); + if (s->sensor_fd >= 0) break; + LOGW("waiting for sensors..."); + sleep(1); + } + assert(s->sensor_fd >= 0); + + // *** SHUTDOWN ALL *** + + // CSIPHY: release csiphy + struct msm_camera_csi_lane_params csi_lane_params = {0}; + csi_lane_params.csi_lane_mask = 0x1f; + csiphy_cfg_data.cfg.csi_lane_params = &csi_lane_params; + csiphy_cfg_data.cfgtype = CSIPHY_RELEASE; + err = ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data); + LOG("release csiphy: %d", err); + + // CSID: release csid + csid_cfg_data.cfgtype = CSID_RELEASE; + err = ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data); + LOG("release csid: %d", err); + + // SENSOR: send power down + memset(&sensorb_cfg_data, 0, sizeof(sensorb_cfg_data)); + sensorb_cfg_data.cfgtype = CFG_POWER_DOWN; + err = ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data); + LOG("sensor power down: %d", err); + + if (rear && s->device != DEVICE_LP3) { + // ois powerdown + ois_cfg_data.cfgtype = CFG_OIS_POWERDOWN; + err = ioctl(s->ois_fd, VIDIOC_MSM_OIS_CFG, &ois_cfg_data); + LOG("ois powerdown: %d", err); + } + + // actuator powerdown + actuator_cfg_data.cfgtype = CFG_ACTUATOR_POWERDOWN; + err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); + LOG("actuator powerdown: %d", err); + + // reset isp + // struct msm_vfe_axi_halt_cmd halt_cmd = { + // .stop_camif = 1, + // .overflow_detected = 1, + // .blocking_halt = 1, + // }; + // err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_AXI_HALT, &halt_cmd); + // printf("axi halt: %d\n", err); + + // struct msm_vfe_axi_reset_cmd reset_cmd = { + // .blocking = 1, + // .frame_id = 1, + // }; + // err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_AXI_RESET, &reset_cmd); + // printf("axi reset: %d\n", err); + + // struct msm_vfe_axi_restart_cmd restart_cmd = { + // .enable_camif = 1, + // }; + // err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_AXI_RESTART, &restart_cmd); + // printf("axi restart: %d\n", err); + + // **** GO GO GO **** + LOG("******************** GO GO GO ************************"); + + s->eeprom = get_eeprom(s->eeprom_fd, &s->eeprom_size); + + // printf("eeprom:\n"); + // for (int i=0; ieeprom_size; i++) { + // printf("%02x", s->eeprom[i]); + // } + // printf("\n"); + + // CSID: init csid + csid_cfg_data.cfgtype = CSID_INIT; + err = ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data); + LOG("init csid: %d", err); + + // CSIPHY: init csiphy + memset(&csiphy_cfg_data, 0, sizeof(csiphy_cfg_data)); + csiphy_cfg_data.cfgtype = CSIPHY_INIT; + err = ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data); + LOG("init csiphy: %d", err); + + // SENSOR: stop stream + struct msm_camera_i2c_reg_setting stop_settings = { + .reg_setting = stop_reg_array, + .size = ARRAYSIZE(stop_reg_array), + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .data_type = MSM_CAMERA_I2C_BYTE_DATA, + .delay = 0 + }; + sensorb_cfg_data.cfgtype = CFG_SET_STOP_STREAM_SETTING; + sensorb_cfg_data.cfg.setting = &stop_settings; + err = ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data); + LOG("stop stream: %d", err); + + // SENSOR: send power up + memset(&sensorb_cfg_data, 0, sizeof(sensorb_cfg_data)); + sensorb_cfg_data.cfgtype = CFG_POWER_UP; + err = ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data); + LOG("sensor power up: %d", err); + + // **** configure the sensor **** + + // SENSOR: send i2c configuration + if (s->camera_id == CAMERA_ID_IMX298) { + err = sensor_write_regs(s, init_array_imx298, ARRAYSIZE(init_array_imx298), MSM_CAMERA_I2C_BYTE_DATA); + } else if (s->camera_id == CAMERA_ID_S5K3P8SP) { + err = sensor_write_regs(s, init_array_s5k3p8sp, ARRAYSIZE(init_array_s5k3p8sp), MSM_CAMERA_I2C_WORD_DATA); + } else if (s->camera_id == CAMERA_ID_IMX179) { + err = sensor_write_regs(s, init_array_imx179, ARRAYSIZE(init_array_imx179), MSM_CAMERA_I2C_BYTE_DATA); + } else if (s->camera_id == CAMERA_ID_OV8865) { + err = sensor_write_regs(s, init_array_ov8865, ARRAYSIZE(init_array_ov8865), MSM_CAMERA_I2C_BYTE_DATA); + } else { + assert(false); + } + LOG("sensor init i2c: %d", err); + + if (rear) { + // init the actuator + actuator_cfg_data.cfgtype = CFG_ACTUATOR_POWERUP; + err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); + LOG("actuator powerup: %d", err); + + actuator_cfg_data.cfgtype = CFG_ACTUATOR_INIT; + err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); + LOG("actuator init: %d", err); + + + // no OIS in LP3 + if (s->device != DEVICE_LP3) { + // see sony_imx298_eeprom_format_afdata in libmmcamera_sony_imx298_eeprom.so + const float far_margin = -0.28; + uint16_t macro_dac = *(uint16_t*)(s->eeprom + 0x24); + s->infinity_dac = *(uint16_t*)(s->eeprom + 0x26); + LOG("macro_dac: %d infinity_dac: %d", macro_dac, s->infinity_dac); + + int dac_range = macro_dac - s->infinity_dac; + s->infinity_dac += far_margin * dac_range; + + LOG(" -> macro_dac: %d infinity_dac: %d", macro_dac, s->infinity_dac); + + struct msm_actuator_reg_params_t actuator_reg_params[] = { + { + .reg_write_type = MSM_ACTUATOR_WRITE_DAC, + .hw_mask = 0, + .reg_addr = 240, + .hw_shift = 0, + .data_type = 10, + .addr_type = 4, + .reg_data = 0, + .delay = 0, + }, { + .reg_write_type = MSM_ACTUATOR_WRITE_DAC, + .hw_mask = 0, + .reg_addr = 241, + .hw_shift = 0, + .data_type = 10, + .addr_type = 4, + .reg_data = 0, + .delay = 0, + }, { + .reg_write_type = MSM_ACTUATOR_WRITE_DAC, + .hw_mask = 0, + .reg_addr = 242, + .hw_shift = 0, + .data_type = 10, + .addr_type = 4, + .reg_data = 0, + .delay = 0, + }, { + .reg_write_type = MSM_ACTUATOR_WRITE_DAC, + .hw_mask = 0, + .reg_addr = 243, + .hw_shift = 0, + .data_type = 10, + .addr_type = 4, + .reg_data = 0, + .delay = 0, + }, + }; + + //... + struct reg_settings_t actuator_init_settings[1] = {0}; + + struct region_params_t region_params[] = { + { + .step_bound = {512, 0,}, + .code_per_step = 118, + .qvalue = 128, + }, + }; + + actuator_cfg_data.cfgtype = CFG_SET_ACTUATOR_INFO; + actuator_cfg_data.cfg.set_info = (struct msm_actuator_set_info_t){ + .actuator_params = { + .act_type = ACTUATOR_VCM, + .reg_tbl_size = 4, + .data_size = 10, + .init_setting_size = 0, + .i2c_freq_mode = I2C_CUSTOM_MODE, + .i2c_addr = 28, + .i2c_addr_type = MSM_ACTUATOR_BYTE_ADDR, + .i2c_data_type = MSM_ACTUATOR_BYTE_DATA, + .reg_tbl_params = &actuator_reg_params[0], + .init_settings = &actuator_init_settings[0], + .park_lens = { + .damping_step = 1023, + .damping_delay = 15000, + .hw_params = 58404, + .max_step = 20, + } + }, + .af_tuning_params = { + .initial_code = s->infinity_dac, + .pwd_step = 0, + .region_size = 1, + .total_steps = 512, + .region_params = ®ion_params[0], + }, + }; + err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); + LOG("actuator set info: %d", err); + + // power up ois + ois_cfg_data.cfgtype = CFG_OIS_POWERUP; + err = ioctl(s->ois_fd, VIDIOC_MSM_OIS_CFG, &ois_cfg_data); + LOG("ois powerup: %d", err); + + ois_cfg_data.cfgtype = CFG_OIS_INIT; + err = ioctl(s->ois_fd, VIDIOC_MSM_OIS_CFG, &ois_cfg_data); + LOG("ois init: %d", err); + + ois_cfg_data.cfgtype = CFG_OIS_CONTROL; + ois_cfg_data.cfg.set_info.ois_params = (struct msm_ois_params_t){ + // .data_size = 26312, + .setting_size = 120, + .i2c_addr = 28, + .i2c_freq_mode = I2C_CUSTOM_MODE, + // .i2c_addr_type = wtf + // .i2c_data_type = wtf + .settings = &ois_init_settings[0], + }; + err = ioctl(s->ois_fd, VIDIOC_MSM_OIS_CFG, &ois_cfg_data); + LOG("ois init settings: %d", err); + } else { + // leeco actuator + // from sniff + s->infinity_dac = 364; + + struct msm_actuator_reg_params_t actuator_reg_params[] = { + { + .reg_write_type = MSM_ACTUATOR_WRITE_DAC, + .hw_mask = 0, + .reg_addr = 3, + .hw_shift = 0, + .data_type = 9, + .addr_type = 4, + .reg_data = 0, + .delay = 0, + }, + }; + + struct reg_settings_t actuator_init_settings[] = { + { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=1, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 }, + { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=0, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 2 }, + { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=2, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 2 }, + { .reg_addr=6, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=64, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 }, + { .reg_addr=7, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=113, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 }, + }; + + struct region_params_t region_params[] = { + { + .step_bound = {238, 0,}, + .code_per_step = 235, + .qvalue = 128, + }, + }; + + actuator_cfg_data.cfgtype = CFG_SET_ACTUATOR_INFO; + actuator_cfg_data.cfg.set_info = (struct msm_actuator_set_info_t){ + .actuator_params = { + .act_type = ACTUATOR_BIVCM, + .reg_tbl_size = 1, + .data_size = 10, + .init_setting_size = 5, + .i2c_freq_mode = I2C_STANDARD_MODE, + .i2c_addr = 24, + .i2c_addr_type = MSM_ACTUATOR_BYTE_ADDR, + .i2c_data_type = MSM_ACTUATOR_WORD_DATA, + .reg_tbl_params = &actuator_reg_params[0], + .init_settings = &actuator_init_settings[0], + .park_lens = { + .damping_step = 1023, + .damping_delay = 14000, + .hw_params = 11, + .max_step = 20, + } + }, + .af_tuning_params = { + .initial_code = s->infinity_dac, + .pwd_step = 0, + .region_size = 1, + .total_steps = 238, + .region_params = ®ion_params[0], + }, + }; + + err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); + LOG("actuator set info: %d", err); + } + } + + if (s->camera_id == CAMERA_ID_IMX298) { + err = sensor_write_regs(s, mode_setting_array_imx298, ARRAYSIZE(mode_setting_array_imx298), MSM_CAMERA_I2C_BYTE_DATA); + LOG("sensor setup: %d", err); + } + + // CSIPHY: configure csiphy + if (s->camera_id == CAMERA_ID_IMX298) { + csiphy_params.lane_cnt = 4; + csiphy_params.settle_cnt = 14; + csiphy_params.lane_mask = 0x1f; + csiphy_params.csid_core = 0; + } else if (s->camera_id == CAMERA_ID_S5K3P8SP) { + csiphy_params.lane_cnt = 4; + csiphy_params.settle_cnt = 24; + csiphy_params.lane_mask = 0x1f; + csiphy_params.csid_core = 0; + } else if (s->camera_id == CAMERA_ID_IMX179) { + csiphy_params.lane_cnt = 4; + csiphy_params.settle_cnt = 11; + csiphy_params.lane_mask = 0x1f; + csiphy_params.csid_core = 2; + } else if (s->camera_id == CAMERA_ID_OV8865) { + // guess! + csiphy_params.lane_cnt = 4; + csiphy_params.settle_cnt = 24; + csiphy_params.lane_mask = 0x1f; + csiphy_params.csid_core = 2; + } + csiphy_cfg_data.cfgtype = CSIPHY_CFG; + csiphy_cfg_data.cfg.csiphy_params = &csiphy_params; + err = ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data); + LOG("csiphy configure: %d", err); + + // CSID: configure csid + csid_params.lane_cnt = 4; + csid_params.lane_assign = 0x4320; + if (rear) { + csid_params.phy_sel = 0; + } else { + csid_params.phy_sel = 2; + } + csid_params.lut_params.num_cid = rear ? 3 : 1; + +#define CSI_STATS 0x35 +#define CSI_PD 0x36 + + csid_params.lut_params.vc_cfg_a[0].cid = 0; + csid_params.lut_params.vc_cfg_a[0].dt = CSI_RAW10; + csid_params.lut_params.vc_cfg_a[0].decode_format = CSI_DECODE_10BIT; + csid_params.lut_params.vc_cfg_a[1].cid = 1; + csid_params.lut_params.vc_cfg_a[1].dt = CSI_PD; + csid_params.lut_params.vc_cfg_a[1].decode_format = CSI_DECODE_10BIT; + csid_params.lut_params.vc_cfg_a[2].cid = 2; + csid_params.lut_params.vc_cfg_a[2].dt = CSI_STATS; + csid_params.lut_params.vc_cfg_a[2].decode_format = CSI_DECODE_10BIT; + + csid_params.lut_params.vc_cfg[0] = &csid_params.lut_params.vc_cfg_a[0]; + csid_params.lut_params.vc_cfg[1] = &csid_params.lut_params.vc_cfg_a[1]; + csid_params.lut_params.vc_cfg[2] = &csid_params.lut_params.vc_cfg_a[2]; + + csid_cfg_data.cfgtype = CSID_CFG; + csid_cfg_data.cfg.csid_params = &csid_params; + err = ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data); + LOG("csid configure: %d", err); + + // ISP: SMMU_ATTACH + struct msm_vfe_smmu_attach_cmd smmu_attach_cmd = { + .security_mode = 0, + .iommu_attach_mode = IOMMU_ATTACH + }; + err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_SMMU_ATTACH, &smmu_attach_cmd); + LOG("isp smmu attach: %d", err); + + // ******************* STREAM RAW ***************************** + + // configure QMET input + for (int i = 0; i < (rear ? 3 : 1); i++) { + StreamState *ss = &s->ss[i]; + + memset(&input_cfg, 0, sizeof(struct msm_vfe_input_cfg)); + input_cfg.input_src = VFE_RAW_0+i; + input_cfg.input_pix_clk = s->pixel_clock; + input_cfg.d.rdi_cfg.cid = i; + input_cfg.d.rdi_cfg.frame_based = 1; + err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_INPUT_CFG, &input_cfg); + LOG("configure input(%d): %d", i, err); + + // ISP: REQUEST_STREAM + ss->stream_req.axi_stream_handle = 0; + if (rear) { + ss->stream_req.session_id = 2; + ss->stream_req.stream_id = /*ISP_META_CHANNEL_BIT | */ISP_NATIVE_BUF_BIT | (1+i); + } else { + ss->stream_req.session_id = 3; + ss->stream_req.stream_id = ISP_NATIVE_BUF_BIT | 1; + } + + if (i == 0) { + ss->stream_req.output_format = v4l2_fourcc('R', 'G', '1', '0'); + } else { + ss->stream_req.output_format = v4l2_fourcc('Q', 'M', 'E', 'T'); + } + ss->stream_req.stream_src = RDI_INTF_0+i; + +#ifdef HIGH_FPS + if (rear) { + ss->stream_req.frame_skip_pattern = EVERY_3FRAME; + } +#endif + + ss->stream_req.frame_base = 1; + ss->stream_req.buf_divert = 1; //i == 0; + + // setup stream plane. doesn't even matter? + /*s->stream_req.plane_cfg[0].output_plane_format = Y_PLANE; + s->stream_req.plane_cfg[0].output_width = s->ci.frame_width; + s->stream_req.plane_cfg[0].output_height = s->ci.frame_height; + s->stream_req.plane_cfg[0].output_stride = s->ci.frame_width; + s->stream_req.plane_cfg[0].output_scan_lines = s->ci.frame_height; + s->stream_req.plane_cfg[0].rdi_cid = 0;*/ + + err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_REQUEST_STREAM, &ss->stream_req); + LOG("isp request stream: %d -> 0x%x", err, ss->stream_req.axi_stream_handle); + + // ISP: REQUEST_BUF + ss->buf_request.session_id = ss->stream_req.session_id; + ss->buf_request.stream_id = ss->stream_req.stream_id; + ss->buf_request.num_buf = FRAME_BUF_COUNT; + ss->buf_request.buf_type = ISP_PRIVATE_BUF; + ss->buf_request.handle = 0; + err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_REQUEST_BUF, &ss->buf_request); + LOG("isp request buf: %d", err); + LOG("got buf handle: 0x%x", ss->buf_request.handle); + + // ENQUEUE all buffers + for (int j = 0; j < ss->buf_request.num_buf; j++) { + ss->qbuf_info[j].handle = ss->buf_request.handle; + ss->qbuf_info[j].buf_idx = j; + ss->qbuf_info[j].buffer.num_planes = 1; + ss->qbuf_info[j].buffer.planes[0].addr = ss->bufs[j].fd; + ss->qbuf_info[j].buffer.planes[0].length = ss->bufs[j].len; + err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_ENQUEUE_BUF, &ss->qbuf_info[j]); + } + + // ISP: UPDATE_STREAM + update_cmd.num_streams = 1; + update_cmd.update_info[0].user_stream_id = ss->stream_req.stream_id; + update_cmd.update_info[0].stream_handle = ss->stream_req.axi_stream_handle; + update_cmd.update_type = UPDATE_STREAM_ADD_BUFQ; + err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_UPDATE_STREAM, &update_cmd); + LOG("isp update stream: %d", err); + } + + LOG("******** START STREAMS ********"); + + sub.id = 0; + sub.type = 0x1ff; + err = ioctl(s->isp_fd, VIDIOC_SUBSCRIBE_EVENT, &sub); + LOG("isp subscribe: %d", err); + + // ISP: START_STREAM + s->stream_cfg.cmd = START_STREAM; + s->stream_cfg.num_streams = rear ? 3 : 1; + for (int i = 0; i < s->stream_cfg.num_streams; i++) { + s->stream_cfg.stream_handle[i] = s->ss[i].stream_req.axi_stream_handle; + } + err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_CFG_STREAM, &s->stream_cfg); + LOG("isp start stream: %d", err); +} + + +static struct damping_params_t actuator_ringing_params = { + .damping_step = 1023, + .damping_delay = 15000, + .hw_params = 0x0000e422, +}; + +static void rear_start(CameraState *s) { + int err; + + struct msm_actuator_cfg_data actuator_cfg_data = {0}; + + set_exposure(s, 1.0, 1.0); + + err = sensor_write_regs(s, start_reg_array, ARRAYSIZE(start_reg_array), MSM_CAMERA_I2C_BYTE_DATA); + LOG("sensor start regs: %d", err); + + // focus on infinity assuming phone is perpendicular + int inf_step; + + if (s->device != DEVICE_LP3) { + imx298_ois_calibration(s->ois_fd, s->eeprom); + inf_step = 332 - s->infinity_dac; + + // initial guess + s->lens_true_pos = 300; + } else { + // default is OP3, this is for LeEco + actuator_ringing_params.damping_step = 1023; + actuator_ringing_params.damping_delay = 20000; + actuator_ringing_params.hw_params = 13; + + inf_step = 512 - s->infinity_dac; + + // initial guess + s->lens_true_pos = 400; + } + + // reset lens position + memset(&actuator_cfg_data, 0, sizeof(actuator_cfg_data)); + actuator_cfg_data.cfgtype = CFG_SET_POSITION; + actuator_cfg_data.cfg.setpos = (struct msm_actuator_set_position_t){ + .number_of_steps = 1, + .hw_params = (s->device != DEVICE_LP3) ? 0x0000e424 : 7, + .pos = {s->infinity_dac, 0}, + .delay = {0,} + }; + err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); + LOG("actuator set pos: %d", err); + + // TODO: confirm this isn't needed + /*memset(&actuator_cfg_data, 0, sizeof(actuator_cfg_data)); + actuator_cfg_data.cfgtype = CFG_MOVE_FOCUS; + actuator_cfg_data.cfg.move = (struct msm_actuator_move_params_t){ + .dir = 0, + .sign_dir = 1, + .dest_step_pos = inf_step, + .num_steps = inf_step, + .curr_lens_pos = 0, + .ringing_params = &actuator_ringing_params, + }; + err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); // should be ~332 at startup ? + LOG("init actuator move focus: %d", err);*/ + //actuator_cfg_data.cfg.move.curr_lens_pos; + + s->cur_lens_pos = 0; + s->cur_step_pos = inf_step; + + actuator_move(s, s->cur_lens_pos); + + LOG("init lens pos: %d", s->cur_lens_pos); +} + +void actuator_move(CameraState *s, uint16_t target) { + int err; + + int step = target - s->cur_lens_pos; + // LP3 moves only on even positions. TODO: use proper sensor params + if (s->device == DEVICE_LP3) { + step /= 2; + } + + int dest_step_pos = s->cur_step_pos + step; + dest_step_pos = clamp(dest_step_pos, 0, 255); + + struct msm_actuator_cfg_data actuator_cfg_data = {0}; + actuator_cfg_data.cfgtype = CFG_MOVE_FOCUS; + actuator_cfg_data.cfg.move = (struct msm_actuator_move_params_t){ + .dir = (step > 0) ? 0 : 1, + .sign_dir = (step > 0) ? 1 : -1, + .dest_step_pos = dest_step_pos, + .num_steps = abs(step), + .curr_lens_pos = s->cur_lens_pos, + .ringing_params = &actuator_ringing_params, + }; + err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); + //LOGD("actuator move focus: %d", err); + + s->cur_step_pos = dest_step_pos; + s->cur_lens_pos = actuator_cfg_data.cfg.move.curr_lens_pos; + + //LOGD("step %d target: %d lens pos: %d", dest_step_pos, target, s->cur_lens_pos); +} + +static void parse_autofocus(CameraState *s, uint8_t *d) { + int good_count = 0; + int16_t max_focus = -32767; + int avg_focus = 0; + + /*printf("FOCUS: "); + for (int i = 0; i < 0x10; i++) { + printf("%2.2X ", d[i]); + }*/ + + for (int i = 0; i < NUM_FOCUS; i++) { + int doff = i*5+5; + s->confidence[i] = d[doff]; + int16_t focus_t = (d[doff+1] << 3) | (d[doff+2] >> 5); + if (focus_t >= 1024) focus_t = -(2048-focus_t); + s->focus[i] = focus_t; + //printf("%x->%d ", d[doff], focus_t); + if (s->confidence[i] > 0x20) { + good_count++; + max_focus = max(max_focus, s->focus[i]); + avg_focus += s->focus[i]; + } + } + + //printf("\n"); + if (good_count < 4) { + s->focus_err = nan(""); + return; + } + + avg_focus /= good_count; + + // outlier rejection + if (abs(avg_focus - max_focus) > 200) { + s->focus_err = nan(""); + return; + } + + s->focus_err = max_focus*1.0; +} + +static void do_autofocus(CameraState *s) { + // params for focus PI controller + const float focus_kp = 0.005; + + float err = s->focus_err; + float offset = 0; + float sag = (s->last_sag_acc_z/9.8) * 128; + + const int dac_up = s->device == DEVICE_LP3? 634:456; + const int dac_down = s->device == DEVICE_LP3? 366:224; + + if (!isnan(err)) { + // learn lens_true_pos + s->lens_true_pos -= err*focus_kp; + } + + // stay off the walls + s->lens_true_pos = clamp(s->lens_true_pos, dac_down, dac_up); + + int target = clamp(s->lens_true_pos - sag, dac_down, dac_up); + + /*char debug[4096]; + char *pdebug = debug; + pdebug += sprintf(pdebug, "focus "); + for (int i = 0; i < NUM_FOCUS; i++) pdebug += sprintf(pdebug, "%2x(%4d) ", s->confidence[i], s->focus[i]); + pdebug += sprintf(pdebug, " err: %7.2f offset: %6.2f sag: %6.2f lens_true_pos: %6.2f cur_lens_pos: %4d->%4d", err * focus_kp, offset, sag, s->lens_true_pos, s->cur_lens_pos, target); + LOGD(debug);*/ + + actuator_move(s, target); +} + + +static void front_start(CameraState *s) { + int err; + + set_exposure(s, 1.0, 1.0); + + err = sensor_write_regs(s, start_reg_array, ARRAYSIZE(start_reg_array), MSM_CAMERA_I2C_BYTE_DATA); + LOG("sensor start regs: %d", err); +} + + + +void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front) { + int err; + + struct ispif_cfg_data ispif_cfg_data = {0}; + + struct msm_ispif_param_data ispif_params = {0}; + ispif_params.num = 4; + // rear camera + ispif_params.entries[0].vfe_intf = 0; + ispif_params.entries[0].intftype = RDI0; + ispif_params.entries[0].num_cids = 1; + ispif_params.entries[0].cids[0] = 0; + ispif_params.entries[0].csid = 0; + // front camera + ispif_params.entries[1].vfe_intf = 1; + ispif_params.entries[1].intftype = RDI0; + ispif_params.entries[1].num_cids = 1; + ispif_params.entries[1].cids[0] = 0; + ispif_params.entries[1].csid = 2; + // rear camera (focus) + ispif_params.entries[2].vfe_intf = 0; + ispif_params.entries[2].intftype = RDI1; + ispif_params.entries[2].num_cids = 1; + ispif_params.entries[2].cids[0] = 1; + ispif_params.entries[2].csid = 0; + // rear camera (stats, for AE) + ispif_params.entries[3].vfe_intf = 0; + ispif_params.entries[3].intftype = RDI2; + ispif_params.entries[3].num_cids = 1; + ispif_params.entries[3].cids[0] = 2; + ispif_params.entries[3].csid = 0; + + assert(camera_bufs_rear); + assert(camera_bufs_front); + + int msmcfg_fd = open("/dev/media0", O_RDWR | O_NONBLOCK); + assert(msmcfg_fd >= 0); + + sensors_init(s); + + int v4l_fd = open("/dev/video0", O_RDWR | O_NONBLOCK); + assert(v4l_fd >= 0); + + if (s->device == DEVICE_LP3) { + s->ispif_fd = open("/dev/v4l-subdev15", O_RDWR | O_NONBLOCK); + } else { + s->ispif_fd = open("/dev/v4l-subdev16", O_RDWR | O_NONBLOCK); + } + assert(s->ispif_fd >= 0); + + // ISPIF: stop + // memset(&ispif_cfg_data, 0, sizeof(ispif_cfg_data)); + // ispif_cfg_data.cfg_type = ISPIF_STOP_FRAME_BOUNDARY; + // ispif_cfg_data.params = ispif_params; + // err = ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data); + // LOG("ispif stop: %d", err); + + LOG("*** open front ***"); + s->front.ss[0].bufs = camera_bufs_front; + camera_open(&s->front, false); + + LOG("*** open rear ***"); + s->rear.ss[0].bufs = camera_bufs_rear; + s->rear.ss[1].bufs = camera_bufs_focus; + s->rear.ss[2].bufs = camera_bufs_stats; + camera_open(&s->rear, true); + + if (getenv("CAMERA_TEST")) { + cameras_close(s); + exit(0); + } + + // ISPIF: set vfe info + memset(&ispif_cfg_data, 0, sizeof(ispif_cfg_data)); + ispif_cfg_data.cfg_type = ISPIF_SET_VFE_INFO; + ispif_cfg_data.vfe_info.num_vfe = 2; + err = ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data); + LOG("ispif set vfe info: %d", err); + + // ISPIF: setup + memset(&ispif_cfg_data, 0, sizeof(ispif_cfg_data)); + ispif_cfg_data.cfg_type = ISPIF_INIT; + ispif_cfg_data.csid_version = 0x30050000; //CSID_VERSION_V35 + err = ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data); + LOG("ispif setup: %d", err); + + memset(&ispif_cfg_data, 0, sizeof(ispif_cfg_data)); + ispif_cfg_data.cfg_type = ISPIF_CFG; + ispif_cfg_data.params = ispif_params; + + err = ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data); + LOG("ispif cfg: %d", err); + + ispif_cfg_data.cfg_type = ISPIF_START_FRAME_BOUNDARY; + err = ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data); + LOG("ispif start_frame_boundary: %d", err); + + front_start(&s->front); + rear_start(&s->rear); +} + + +static void camera_close(CameraState *s) { + int err; + + tbuffer_stop(&s->camera_tb); + + // ISP: STOP_STREAM + s->stream_cfg.cmd = STOP_STREAM; + err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_CFG_STREAM, &s->stream_cfg); + LOG("isp stop stream: %d", err); + + for (int i = 0; i < 3; i++) { + StreamState *ss = &s->ss[i]; + if (ss->stream_req.axi_stream_handle != 0) { + err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_RELEASE_BUF, &ss->buf_request); + LOG("isp release buf: %d", err); + + struct msm_vfe_axi_stream_release_cmd stream_release = { + .stream_handle = ss->stream_req.axi_stream_handle, + }; + err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_RELEASE_STREAM, &stream_release); + LOG("isp release stream: %d", err); + } + } + + free(s->eeprom); +} + + +const char* get_isp_event_name(unsigned int type) { + switch (type) { + case ISP_EVENT_REG_UPDATE: return "ISP_EVENT_REG_UPDATE"; + case ISP_EVENT_EPOCH_0: return "ISP_EVENT_EPOCH_0"; + case ISP_EVENT_EPOCH_1: return "ISP_EVENT_EPOCH_1"; + case ISP_EVENT_START_ACK: return "ISP_EVENT_START_ACK"; + case ISP_EVENT_STOP_ACK: return "ISP_EVENT_STOP_ACK"; + case ISP_EVENT_IRQ_VIOLATION: return "ISP_EVENT_IRQ_VIOLATION"; + case ISP_EVENT_STATS_OVERFLOW: return "ISP_EVENT_STATS_OVERFLOW"; + case ISP_EVENT_ERROR: return "ISP_EVENT_ERROR"; + case ISP_EVENT_SOF: return "ISP_EVENT_SOF"; + case ISP_EVENT_EOF: return "ISP_EVENT_EOF"; + case ISP_EVENT_BUF_DONE: return "ISP_EVENT_BUF_DONE"; + case ISP_EVENT_BUF_DIVERT: return "ISP_EVENT_BUF_DIVERT"; + case ISP_EVENT_STATS_NOTIFY: return "ISP_EVENT_STATS_NOTIFY"; + case ISP_EVENT_COMP_STATS_NOTIFY: return "ISP_EVENT_COMP_STATS_NOTIFY"; + case ISP_EVENT_FE_READ_DONE: return "ISP_EVENT_FE_READ_DONE"; + case ISP_EVENT_IOMMU_P_FAULT: return "ISP_EVENT_IOMMU_P_FAULT"; + case ISP_EVENT_HW_FATAL_ERROR: return "ISP_EVENT_HW_FATAL_ERROR"; + case ISP_EVENT_PING_PONG_MISMATCH: return "ISP_EVENT_PING_PONG_MISMATCH"; + case ISP_EVENT_REG_UPDATE_MISSING: return "ISP_EVENT_REG_UPDATE_MISSING"; + case ISP_EVENT_BUF_FATAL_ERROR: return "ISP_EVENT_BUF_FATAL_ERROR"; + case ISP_EVENT_STREAM_UPDATE_DONE: return "ISP_EVENT_STREAM_UPDATE_DONE"; + default: return "unknown"; + } +} + +static FrameMetadata get_frame_metadata(CameraState *s, uint32_t frame_id) { + pthread_mutex_lock(&s->frame_info_lock); + for (int i=0; iframe_metadata[i].frame_id == frame_id) { + pthread_mutex_unlock(&s->frame_info_lock); + return s->frame_metadata[i]; + } + } + pthread_mutex_unlock(&s->frame_info_lock); + + // should never happen + return (FrameMetadata){ + .frame_id = -1, + }; +} + +static bool acceleration_from_sensor_sock(void* sock, float* vs) { + int err; + + zmq_msg_t msg; + err = zmq_msg_init(&msg); + assert(err == 0); + + err = zmq_msg_recv(&msg, sock, 0); + assert(err >= 0); + + struct capn ctx; + capn_init_mem(&ctx, zmq_msg_data(&msg), zmq_msg_size(&msg), 0); + + cereal_Event_ptr eventp; + eventp.p = capn_getp(capn_root(&ctx), 0, 1); + struct cereal_Event eventd; + cereal_read_Event(&eventd, eventp); + + bool ret = false; + + if (eventd.which == cereal_Event_sensorEvents) { + cereal_SensorEventData_list lst = eventd.sensorEvents; + int len = capn_len(lst); + for (int i=0; iinproc://cameraops"); + assert(ops_sock); + + CameraMsg msg = {.type = -1}; + zmq_send(zsock_resolve(ops_sock), &msg, sizeof(msg), ZMQ_DONTWAIT); + + zsock_destroy(&ops_sock); +} + +static void* ops_thread(void* arg) { + int err; + DualCameraState *s = (DualCameraState*)arg; + + set_thread_name("camera_settings"); + + zsock_t *cameraops = zsock_new_pull("@inproc://cameraops"); + assert(cameraops); + + zsock_t *sensor_sock = zsock_new_sub(">tcp://127.0.0.1:8003", ""); + assert(sensor_sock); + + zsock_t *terminate = zsock_new_sub(">inproc://terminate", ""); + assert(terminate); + + zpoller_t *poller = zpoller_new(cameraops, sensor_sock, terminate, NULL); + assert(poller); + + while (!do_exit) { + + zsock_t *which = (zsock_t*)zpoller_wait(poller, -1); + if (which == terminate || which == NULL) { + break; + } + void* sockraw = zsock_resolve(which); + + if (which == cameraops) { + zmq_msg_t msg; + err = zmq_msg_init(&msg); + assert(err == 0); + + err = zmq_msg_recv(&msg, sockraw, 0); + assert(err >= 0); + + CameraMsg cmsg; + if (zmq_msg_size(&msg) == sizeof(cmsg)) { + memcpy(&cmsg, zmq_msg_data(&msg), zmq_msg_size(&msg)); + + //LOGD("cameraops %d", cmsg.type); + + if (cmsg.type == CAMERA_MSG_AUTOEXPOSE) { + if (cmsg.camera_num == 0) { + do_autoexposure(&s->rear, cmsg.grey_frac); + do_autofocus(&s->rear); + } else { + do_autoexposure(&s->front, cmsg.grey_frac); + } + } else if (cmsg.type == -1) { + break; + } + } + + zmq_msg_close(&msg); + + } else if (which == sensor_sock) { + float vs[3] = {0.0}; + bool got_accel = acceleration_from_sensor_sock(sockraw, vs); + + uint64_t ts = nanos_since_boot(); + if (got_accel && ts - s->rear.last_sag_ts > 10000000) { // 10 ms + s->rear.last_sag_ts = ts; + s->rear.last_sag_acc_z = -vs[2]; + } + } + } + + zpoller_destroy(&poller); + zsock_destroy(&cameraops); + zsock_destroy(&sensor_sock); + zsock_destroy(&terminate); + + return NULL; +} + +void cameras_run(DualCameraState *s) { + int err; + + pthread_t ops_thread_handle; + err = pthread_create(&ops_thread_handle, NULL, + ops_thread, s); + assert(err == 0); + + CameraState* cameras[2] = {&s->rear, &s->front}; + + while (!do_exit) { + struct pollfd fds[2] = {{0}}; + + fds[0].fd = cameras[0]->isp_fd; + fds[0].events = POLLPRI; + + fds[1].fd = cameras[1]->isp_fd; + fds[1].events = POLLPRI; + + int ret = poll(fds, ARRAYSIZE(fds), 1000); + if (ret <= 0) { + LOGE("poll failed (%d)", ret); + break; + } + + // process cameras + for (int i=0; i<2; i++) { + if (!fds[i].revents) continue; + + CameraState *c = cameras[i]; + + struct v4l2_event ev; + ret = ioctl(c->isp_fd, VIDIOC_DQEVENT, &ev); + struct msm_isp_event_data *isp_event_data = (struct msm_isp_event_data *)ev.u.data; + unsigned int event_type = ev.type; + + uint64_t timestamp = (isp_event_data->mono_timestamp.tv_sec*1000000000ULL + + isp_event_data->mono_timestamp.tv_usec*1000); + + int buf_idx = isp_event_data->u.buf_done.buf_idx; + int stream_id = isp_event_data->u.buf_done.stream_id; + int buffer = (stream_id&0xFFFF) - 1; + + uint64_t t = nanos_since_boot(); + + /*if (i == 1) { + printf("%10.2f: VIDIOC_DQEVENT: %d type:%X (%s)\n", t*1.0/1e6, ret, event_type, get_isp_event_name(event_type)); + }*/ + + // printf("%d: %s\n", i, get_isp_event_name(event_type)); + + switch (event_type) { + case ISP_EVENT_BUF_DIVERT: + + /*if (c->is_samsung) { + printf("write %d\n", c->frame_size); + FILE *f = fopen("/tmp/test", "wb"); + fwrite((void*)c->camera_bufs[i].addr, 1, c->frame_size, f); + fclose(f); + }*/ + //printf("divert: %d %d %d\n", i, buffer, buf_idx); + + if (buffer == 0) { + c->camera_bufs_metadata[buf_idx] = get_frame_metadata(c, isp_event_data->frame_id); + tbuffer_dispatch(&c->camera_tb, buf_idx); + } else { + uint8_t *d = c->ss[buffer].bufs[buf_idx].addr; + if (buffer == 1) { + parse_autofocus(c, d); + } + c->ss[buffer].qbuf_info[buf_idx].dirty_buf = 1; + ioctl(c->isp_fd, VIDIOC_MSM_ISP_ENQUEUE_BUF, &c->ss[buffer].qbuf_info[buf_idx]); + } + break; + case ISP_EVENT_EOF: + // printf("ISP_EVENT_EOF delta %f\n", (t-last_t)/1e6); + c->last_t = t; + + pthread_mutex_lock(&c->frame_info_lock); + c->frame_metadata[c->frame_metadata_idx] = (FrameMetadata){ + .frame_id = isp_event_data->frame_id, + .timestamp_eof = timestamp, + .frame_length = c->cur_frame_length, + .integ_lines = c->cur_integ_lines, + .global_gain = c->cur_gain, + .lens_pos = c->cur_lens_pos, + .lens_sag = c->last_sag_acc_z, + .lens_err = c->focus_err, + .lens_true_pos = c->lens_true_pos, + .gain_frac = c->cur_gain_frac, + }; + c->frame_metadata_idx = (c->frame_metadata_idx+1)%METADATA_BUF_COUNT; + pthread_mutex_unlock(&c->frame_info_lock); + + break; + case ISP_EVENT_ERROR: + LOGE("ISP_EVENT_ERROR! err type: 0x%08x", isp_event_data->u.error_info.err_type); + break; + } + } + } + + LOG(" ************** STOPPING **************"); + + ops_term(); + err = pthread_join(ops_thread_handle, NULL); + assert(err == 0); + + cameras_close(s); +} + +void cameras_close(DualCameraState *s) { + camera_close(&s->rear); + camera_close(&s->front); +} diff --git a/selfdrive/camerad/cameras/camera_qcom.h b/selfdrive/camerad/cameras/camera_qcom.h new file mode 100644 index 000000000..1a9c31baa --- /dev/null +++ b/selfdrive/camerad/cameras/camera_qcom.h @@ -0,0 +1,131 @@ +#ifndef CAMERA_H +#define CAMERA_H + +#include +#include +#include + +#include "msmb_isp.h" +#include "msmb_ispif.h" +#include "msmb_camera.h" +#include "msm_cam_sensor.h" + +#include "common/mat.h" +#include "common/visionbuf.h" +#include "common/buffering.h" + +#include "camera_common.h" + +#define FRAME_BUF_COUNT 4 +#define METADATA_BUF_COUNT 4 + +#define DEVICE_OP3 0 +#define DEVICE_OP3T 1 +#define DEVICE_LP3 2 + +#define NUM_FOCUS 8 + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct CameraState CameraState; + +typedef int (*camera_apply_exposure_func)(CameraState *s, int gain, int integ_lines, int frame_length); + +typedef struct StreamState { + struct msm_isp_buf_request buf_request; + struct msm_vfe_axi_stream_request_cmd stream_req; + struct msm_isp_qbuf_info qbuf_info[FRAME_BUF_COUNT]; + VisionBuf *bufs; +} StreamState; + +typedef struct CameraState { + int camera_num; + int camera_id; + CameraInfo ci; + int frame_size; + + int device; + + void* ops_sock; + + uint32_t pixel_clock; + uint32_t line_length_pclk; + unsigned int max_gain; + + int csid_fd; + int csiphy_fd; + int sensor_fd; + int isp_fd; + int eeprom_fd; + // rear only + int ois_fd, actuator_fd; + uint16_t infinity_dac; + + struct msm_vfe_axi_stream_cfg_cmd stream_cfg; + + size_t eeprom_size; + uint8_t *eeprom; + + // uint32_t camera_bufs_ids[FRAME_BUF_COUNT]; + FrameMetadata camera_bufs_metadata[FRAME_BUF_COUNT]; + TBuffer camera_tb; + + pthread_mutex_t frame_info_lock; + FrameMetadata frame_metadata[METADATA_BUF_COUNT]; + int frame_metadata_idx; + float cur_exposure_frac; + float cur_gain_frac; + int cur_gain; + int cur_frame_length; + int cur_integ_lines; + + float digital_gain; + + StreamState ss[3]; + + uint64_t last_t; + + camera_apply_exposure_func apply_exposure; + + int16_t focus[NUM_FOCUS]; + uint8_t confidence[NUM_FOCUS]; + + float focus_err; + + uint16_t cur_step_pos; + uint16_t cur_lens_pos; + uint64_t last_sag_ts; + float last_sag_acc_z; + float lens_true_pos; + + int fps; + + mat3 transform; +} CameraState; + + +typedef struct DualCameraState { + int device; + + int ispif_fd; + + CameraState rear; + CameraState front; +} DualCameraState; + +void cameras_init(DualCameraState *s); +void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front); +void cameras_run(DualCameraState *s); +void cameras_close(DualCameraState *s); + +void camera_autoexposure(CameraState *s, float grey_frac); +void actuator_move(CameraState *s, uint16_t target); +int sensor_write_regs(CameraState *s, struct msm_camera_i2c_reg_array* arr, size_t size, int data_type); + +#ifdef __cplusplus +} // extern "C" +#endif + +#endif diff --git a/selfdrive/camerad/cameras/debayer.cl b/selfdrive/camerad/cameras/debayer.cl new file mode 100644 index 000000000..344ead035 --- /dev/null +++ b/selfdrive/camerad/cameras/debayer.cl @@ -0,0 +1,130 @@ +const __constant float3 color_correction[3] = { + // Matrix from WBraw -> sRGBD65 (normalized) + (float3)( 1.62393627, -0.2092988, 0.00119886), + (float3)(-0.45734315, 1.5534676, -0.59296798), + (float3)(-0.16659312, -0.3441688, 1.59176912), +}; + +float3 color_correct(float3 x) { + float3 ret = (0,0,0); + + // white balance of daylight + x /= (float3)(0.4609375, 1.0, 0.546875); + x = max(0.0, min(1.0, x)); + + // fix up the colors + ret += x.x * color_correction[0]; + ret += x.y * color_correction[1]; + ret += x.z * color_correction[2]; + return ret; +} + +float3 srgb_gamma(float3 p) { + // go all out and add an sRGB gamma curve + const float3 ph = (1.0f + 0.055f)*pow(p, 1/2.4f) - 0.055f; + const float3 pl = p*12.92f; + return select(ph, pl, islessequal(p, 0.0031308f)); +} + +__constant int dpcm_lookup[512] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 0, -1, -2, -3, -4, -5, -6, -7, -8, -9, -10, -11, -12, -13, -14, -15, -16, -17, -18, -19, -20, -21, -22, -23, -24, -25, -26, -27, -28, -29, -30, -31, 935, 951, 967, 983, 999, 1015, 1031, 1047, 1063, 1079, 1095, 1111, 1127, 1143, 1159, 1175, 1191, 1207, 1223, 1239, 1255, 1271, 1287, 1303, 1319, 1335, 1351, 1367, 1383, 1399, 1415, 1431, -935, -951, -967, -983, -999, -1015, -1031, -1047, -1063, -1079, -1095, -1111, -1127, -1143, -1159, -1175, -1191, -1207, -1223, -1239, -1255, -1271, -1287, -1303, -1319, -1335, -1351, -1367, -1383, -1399, -1415, -1431, 419, 427, 435, 443, 451, 459, 467, 475, 483, 491, 499, 507, 515, 523, 531, 539, 547, 555, 563, 571, 579, 587, 595, 603, 611, 619, 627, 635, 643, 651, 659, 667, 675, 683, 691, 699, 707, 715, 723, 731, 739, 747, 755, 763, 771, 779, 787, 795, 803, 811, 819, 827, 835, 843, 851, 859, 867, 875, 883, 891, 899, 907, 915, 923, -419, -427, -435, -443, -451, -459, -467, -475, -483, -491, -499, -507, -515, -523, -531, -539, -547, -555, -563, -571, -579, -587, -595, -603, -611, -619, -627, -635, -643, -651, -659, -667, -675, -683, -691, -699, -707, -715, -723, -731, -739, -747, -755, -763, -771, -779, -787, -795, -803, -811, -819, -827, -835, -843, -851, -859, -867, -875, -883, -891, -899, -907, -915, -923, 161, 165, 169, 173, 177, 181, 185, 189, 193, 197, 201, 205, 209, 213, 217, 221, 225, 229, 233, 237, 241, 245, 249, 253, 257, 261, 265, 269, 273, 277, 281, 285, 289, 293, 297, 301, 305, 309, 313, 317, 321, 325, 329, 333, 337, 341, 345, 349, 353, 357, 361, 365, 369, 373, 377, 381, 385, 389, 393, 397, 401, 405, 409, 413, -161, -165, -169, -173, -177, -181, -185, -189, -193, -197, -201, -205, -209, -213, -217, -221, -225, -229, -233, -237, -241, -245, -249, -253, -257, -261, -265, -269, -273, -277, -281, -285, -289, -293, -297, -301, -305, -309, -313, -317, -321, -325, -329, -333, -337, -341, -345, -349, -353, -357, -361, -365, -369, -373, -377, -381, -385, -389, -393, -397, -401, -405, -409, -413, 32, 34, 36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58, 60, 62, 64, 66, 68, 70, 72, 74, 76, 78, 80, 82, 84, 86, 88, 90, 92, 94, 96, 98, 100, 102, 104, 106, 108, 110, 112, 114, 116, 118, 120, 122, 124, 126, 128, 130, 132, 134, 136, 138, 140, 142, 144, 146, 148, 150, 152, 154, 156, 158, -32, -34, -36, -38, -40, -42, -44, -46, -48, -50, -52, -54, -56, -58, -60, -62, -64, -66, -68, -70, -72, -74, -76, -78, -80, -82, -84, -86, -88, -90, -92, -94, -96, -98, -100, -102, -104, -106, -108, -110, -112, -114, -116, -118, -120, -122, -124, -126, -128, -130, -132, -134, -136, -138, -140, -142, -144, -146, -148, -150, -152, -154, -156, -158}; + +inline uint4 decompress(uint4 p, uint4 pl) { + uint4 r1 = (pl + (uint4)(dpcm_lookup[p.s0], dpcm_lookup[p.s1], dpcm_lookup[p.s2], dpcm_lookup[p.s3])); + uint4 r2 = ((p-0x200)<<5) | 0xF; + r2 += select((uint4)(0,0,0,0), (uint4)(1,1,1,1), r2 <= pl); + return select(r2, r1, p < 0x200); +} + +__kernel void debayer10(__global uchar const * const in, + __global uchar * out, float digital_gain) +{ + const int oy = get_global_id(0); + if (oy >= RGB_HEIGHT) return; + const int iy = oy * 2; + + uint4 pint_last; + for (int ox = 0; ox < RGB_WIDTH; ox += 2) { + const int ix = (ox/2) * 5; + + // TODO: why doesn't this work for the frontview + /*const uchar8 v1 = vload8(0, &in[iy * FRAME_STRIDE + ix]); + const uchar ex1 = v1.s4; + const uchar8 v2 = vload8(0, &in[(iy+1) * FRAME_STRIDE + ix]); + const uchar ex2 = v2.s4;*/ + + const uchar4 v1 = vload4(0, &in[iy * FRAME_STRIDE + ix]); + const uchar ex1 = in[iy * FRAME_STRIDE + ix + 4]; + const uchar4 v2 = vload4(0, &in[(iy+1) * FRAME_STRIDE + ix]); + const uchar ex2 = in[(iy+1) * FRAME_STRIDE + ix + 4]; + + uint4 pinta[2]; + pinta[0] = (uint4)( + (((uint)v1.s0 << 2) + ( (ex1 >> 0) & 3)), + (((uint)v1.s1 << 2) + ( (ex1 >> 2) & 3)), + (((uint)v2.s0 << 2) + ( (ex2 >> 0) & 3)), + (((uint)v2.s1 << 2) + ( (ex2 >> 2) & 3))); + pinta[1] = (uint4)( + (((uint)v1.s2 << 2) + ( (ex1 >> 4) & 3)), + (((uint)v1.s3 << 2) + ( (ex1 >> 6) & 3)), + (((uint)v2.s2 << 2) + ( (ex2 >> 4) & 3)), + (((uint)v2.s3 << 2) + ( (ex2 >> 6) & 3))); + + #pragma unroll + for (uint px = 0; px < 2; px++) { + uint4 pint = pinta[px]; + +#if HDR + // decompress HDR + pint = (ox == 0 && px == 0) ? ((pint<<4) | 8) : decompress(pint, pint_last); + pint_last = pint; +#endif + + float4 p = convert_float4(pint); + + // 64 is the black level of the sensor, remove + // (changed to 56 for HDR) + const float black_level = 56.0f; + p = (p - black_level); + + // correct vignetting (no pow function?) + // see https://www.eecis.udel.edu/~jye/lab_research/09/JiUp.pdf the A (4th order) + const float r = ((oy - RGB_HEIGHT/2)*(oy - RGB_HEIGHT/2) + (ox - RGB_WIDTH/2)*(ox - RGB_WIDTH/2)); + const float fake_f = 700.0f; // should be 910, but this fits... + const float lil_a = (1.0f + r/(fake_f*fake_f)); + p = p * lil_a * lil_a; + + // rescale to 1.0 +#if HDR + p /= (16384.0f-black_level); +#else + p /= (1024.0f-black_level); +#endif + + // digital gain + p *= digital_gain; + + // use both green channels +#if BAYER_FLIP == 3 + float3 c1 = (float3)(p.s3, (p.s1+p.s2)/2.0f, p.s0); +#elif BAYER_FLIP == 2 + float3 c1 = (float3)(p.s2, (p.s0+p.s3)/2.0f, p.s1); +#elif BAYER_FLIP == 1 + float3 c1 = (float3)(p.s1, (p.s0+p.s3)/2.0f, p.s2); +#elif BAYER_FLIP == 0 + float3 c1 = (float3)(p.s0, (p.s1+p.s2)/2.0f, p.s3); +#endif + + // color correction + c1 = color_correct(c1); + +#if HDR + // srgb gamma isn't right for YUV, so it's disabled for now + c1 = srgb_gamma(c1); +#endif + + // output BGR + const int ooff = oy * RGB_STRIDE/3 + ox; + vstore3(convert_uchar3_sat(c1.zyx * 255.0f), ooff+px, out); + } + } +} diff --git a/selfdrive/camerad/cameras/sensor_i2c.h b/selfdrive/camerad/cameras/sensor_i2c.h new file mode 100644 index 000000000..c33bc3468 --- /dev/null +++ b/selfdrive/camerad/cameras/sensor_i2c.h @@ -0,0 +1,1830 @@ +static struct msm_camera_i2c_reg_array init_array_imx298[] = { + {0x101,0x0,0}, // IMAGE_ORIENT + {0x601,0x0,0}, // test pattern + //{0xb02,0,0}, // green correction? + // external clock setting + {0x136,0x18,0}, {0x137,0x0,0}, // EXCK_FREQ = Extclk_frequency_mhz + // global setting? + {0x30f4,0x1,0}, + {0x30f5,0x7a,0}, + {0x30f6,0x0,0}, + {0x30f7,0xec,0}, + {0x30fc,0x1,0}, + {0x3101,0x1,0}, + {0x5b2f,0x8,0}, + {0x5d32,0x5,0}, + {0x5d7c,0x0,0}, + {0x5d7d,0x0,0}, + {0x5db9,0x1,0}, + {0x5e43,0x0,0}, + {0x6300,0x0,0}, + {0x6301,0xea,0}, + {0x6302,0x0,0}, + {0x6303,0xb4,0}, + {0x6564,0x0,0}, + {0x6565,0xb6,0}, + {0x6566,0x0,0}, + {0x6567,0xe6,0}, + {0x6714,0x1,0}, + {0x6758,0xb,0}, + {0x6910,0x4,0}, + {0x6916,0x1,0}, + {0x6918,0x4,0}, + {0x691e,0x1,0}, + {0x6931,0x1,0}, + {0x6937,0x2,0}, + {0x693b,0x2,0}, + {0x6d00,0x4a,0}, + {0x6d01,0x41,0}, + {0x6d02,0x23,0}, + {0x6d05,0x4c,0}, + {0x6d06,0x10,0}, + {0x6d08,0x30,0}, + {0x6d09,0x38,0}, + {0x6d0a,0x2c,0}, + {0x6d0b,0x2d,0}, + {0x6d0c,0x34,0}, + {0x6d0d,0x42,0}, + {0x6d19,0x1c,0}, + {0x6d1a,0x71,0}, + {0x6d1b,0xc6,0}, + {0x6d1c,0x94,0}, + {0x6d24,0xe4,0}, + {0x6d30,0xa,0}, + {0x6d31,0x1,0}, + {0x6d33,0xb,0}, + {0x6d34,0x5,0}, + {0x6d35,0x0,0}, + {0x83c2,0x3,0}, + {0x83c3,0x8,0}, + {0x83c4,0x48,0}, + {0x83c7,0x8,0}, + {0x83cb,0x0,0}, + {0xb101,0xff,0}, + {0xb103,0xff,0}, + {0xb105,0xff,0}, + {0xb107,0xff,0}, + {0xb109,0xff,0}, + {0xb10b,0xff,0}, + {0xb10d,0xff,0}, + {0xb10f,0xff,0}, + {0xb111,0xff,0}, + {0xb163,0x3c,0}, + {0xc2a0,0x8,0}, + {0xc2a3,0x3,0}, + {0xc2a5,0x8,0}, + {0xc2a6,0x48,0}, + {0xc2a9,0x0,0}, + {0xf800,0x5e,0}, + {0xf801,0x5e,0}, + {0xf802,0xcd,0}, + {0xf803,0x20,0}, + {0xf804,0x55,0}, + {0xf805,0xd4,0}, + {0xf806,0x1f,0}, + {0xf808,0xf8,0}, + {0xf809,0x3a,0}, + {0xf80a,0xf1,0}, + {0xf80b,0x7e,0}, + {0xf80c,0x55,0}, + {0xf80d,0x38,0}, + {0xf80e,0xe3,0}, + {0xf810,0x74,0}, + {0xf811,0x41,0}, + {0xf812,0xbf,0}, + {0xf844,0x40,0}, + {0xf845,0xba,0}, + {0xf846,0x70,0}, + {0xf847,0x47,0}, + {0xf848,0xc0,0}, + {0xf849,0xba,0}, + {0xf84a,0x70,0}, + {0xf84b,0x47,0}, + {0xf84c,0x82,0}, + {0xf84d,0xf6,0}, + {0xf84e,0x32,0}, + {0xf84f,0xfd,0}, + {0xf851,0xf0,0}, + {0xf852,0x2,0}, + {0xf853,0xf8,0}, + {0xf854,0x81,0}, + {0xf855,0xf6,0}, + {0xf856,0xc0,0}, + {0xf857,0xff,0}, + {0xf858,0x10,0}, + {0xf859,0xb5,0}, + {0xf85a,0xd,0}, + {0xf85b,0x48,0}, + {0xf85c,0x40,0}, + {0xf85d,0x7a,0}, + {0xf85e,0x1,0}, + {0xf85f,0x28,0}, + {0xf860,0x15,0}, + {0xf861,0xd1,0}, + {0xf862,0xc,0}, + {0xf863,0x49,0}, + {0xf864,0xc,0}, + {0xf865,0x46,0}, + {0xf866,0x40,0}, + {0xf867,0x3c,0}, + {0xf868,0x48,0}, + {0xf869,0x8a,0}, + {0xf86a,0x62,0}, + {0xf86b,0x8a,0}, + {0xf86c,0x80,0}, + {0xf86d,0x1a,0}, + {0xf86e,0x8a,0}, + {0xf86f,0x89,0}, + {0xf871,0xb2,0}, + {0xf872,0x10,0}, + {0xf873,0x18,0}, + {0xf874,0xa,0}, + {0xf875,0x46,0}, + {0xf876,0x20,0}, + {0xf877,0x32,0}, + {0xf878,0x12,0}, + {0xf879,0x88,0}, + {0xf87a,0x90,0}, + {0xf87b,0x42,0}, + {0xf87d,0xda,0}, + {0xf87e,0x10,0}, + {0xf87f,0x46,0}, + {0xf880,0x80,0}, + {0xf881,0xb2,0}, + {0xf882,0x88,0}, + {0xf883,0x81,0}, + {0xf884,0x84,0}, + {0xf885,0xf6,0}, + {0xf886,0xd2,0}, + {0xf887,0xf9,0}, + {0xf888,0xe0,0}, + {0xf889,0x67,0}, + {0xf88a,0x85,0}, + {0xf88b,0xf6,0}, + {0xf88c,0xa1,0}, + {0xf88d,0xfc,0}, + {0xf88e,0x10,0}, + {0xf88f,0xbd,0}, + {0xf891,0x18,0}, + {0xf892,0x21,0}, + {0xf893,0x24,0}, + {0xf895,0x18,0}, + {0xf896,0x19,0}, + {0xf897,0xb4,0}, + {0x4e29,0x1,0}, + // PDAF stuff + {0x3166,0x1,0}, //AREA_EN_0 + {0x3167,0x1,0}, + {0x3168,0x1,0}, + {0x3169,0x1,0}, + {0x316a,0x1,0}, + {0x316b,0x1,0}, + {0x316c,0x1,0}, + {0x316d,0x1,0}, + {0x3158,0x2,0}, + {0x3159,0x2,0}, + {0x315a,0x2,0}, + {0x315b,0x3,0}, + {0x3013,0x7,0}, //RMSC_NR_MODE + {0x3035,0x1,0}, + {0x3051,0x0,0}, + {0x3056,0x2,0}, + {0x3057,0x1,0}, + {0x3060,0x0,0}, + {0x8435,0x0,0}, + {0x8455,0x0,0}, + {0x847c,0x0,0}, + {0x84fb,0x1,0}, + {0x9619,0xa0,0}, + {0x961b,0xa0,0}, + {0x961d,0xa0,0}, + {0x961f,0x20,0}, + {0x9621,0x20,0}, + {0x9623,0x20,0}, + {0x9625,0xa0,0}, + {0x9627,0xa0,0}, + {0x9629,0xa0,0}, + {0x962b,0x20,0}, + {0x962d,0x20,0}, + {0x962f,0x20,0}, + {0x9901,0x35,0}, + {0x9903,0x23,0}, + {0x9905,0x23,0}, + {0x9906,0x0,0}, + {0x9907,0x31,0}, + {0x9908,0x0,0}, + {0x9909,0x1b,0}, + {0x990a,0x0,0}, + {0x990b,0x15,0}, + {0x990d,0x3f,0}, + {0x990f,0x3f,0}, + {0x9911,0x3f,0}, + {0x9913,0x64,0}, + {0x9915,0x64,0}, + {0x9917,0x64,0}, + {0x9919,0x50,0}, + {0x991b,0x60,0}, + {0x991d,0x65,0}, + {0x991f,0x1,0}, + {0x9921,0x1,0}, + {0x9923,0x1,0}, + {0x9925,0x23,0}, + {0x9927,0x23,0}, + {0x9929,0x23,0}, + {0x992b,0x2f,0}, + {0x992d,0x1a,0}, + {0x992f,0x14,0}, + {0x9931,0x3f,0}, + {0x9933,0x3f,0}, + {0x9935,0x3f,0}, + {0x9937,0x6b,0}, + {0x9939,0x7c,0}, + {0x993b,0x81,0}, + {0x9943,0xf,0}, + {0x9945,0xf,0}, + {0x9947,0xf,0}, + {0x9949,0xf,0}, + {0x994b,0xf,0}, + {0x994d,0xf,0}, + {0x994f,0x42,0}, + {0x9951,0xf,0}, + {0x9953,0xb,0}, + {0x9955,0x5a,0}, + {0x9957,0x13,0}, + {0x9959,0xc,0}, + {0x995a,0x0,0}, + {0x995b,0x0,0}, + {0x995c,0x0,0}, + {0x996b,0x0,0}, + {0x996d,0x10,0}, + {0x996f,0x10,0}, + {0x9971,0xc8,0}, + {0x9973,0x32,0}, + {0x9975,0x4,0}, + {0x9976,0xa,0}, + {0x9977,0xa,0}, + {0x9978,0xa,0}, + {0x99a4,0x2f,0}, + {0x99a5,0x2f,0}, + {0x99a6,0x2f,0}, + {0x99a7,0xa,0}, + {0x99a8,0xa,0}, + {0x99a9,0xa,0}, + {0x99aa,0x2f,0}, + {0x99ab,0x2f,0}, + {0x99ac,0x2f,0}, + {0x99ad,0x0,0}, + {0x99ae,0x0,0}, + {0x99af,0x0,0}, + {0x99b0,0x40,0}, + {0x99b1,0x40,0}, + {0x99b2,0x40,0}, + {0x99b3,0x30,0}, + {0x99b4,0x30,0}, + {0x99b5,0x30,0}, + {0x99bb,0xa,0}, + {0x99bd,0xa,0}, + {0x99bf,0xa,0}, + {0x99c0,0x9,0}, + {0x99c1,0x9,0}, + {0x99c2,0x9,0}, + {0x99c6,0x3c,0}, + {0x99c7,0x3c,0}, + {0x99c8,0x3c,0}, + {0x99c9,0xff,0}, + {0x99ca,0xff,0}, + {0x99cb,0xff,0}, + {0x9b01,0x35,0}, + {0x9b03,0x14,0}, + {0x9b05,0x14,0}, + {0x9b07,0x31,0}, + {0x9b09,0x1b,0}, + {0x9b0b,0x15,0}, + {0x9b0d,0x1e,0}, + {0x9b0f,0x1e,0}, + {0x9b11,0x1e,0}, + {0x9b13,0x64,0}, + {0x9b15,0x64,0}, + {0x9b17,0x64,0}, + {0x9b19,0x50,0}, + {0x9b1b,0x60,0}, + {0x9b1d,0x65,0}, + {0x9b1f,0x1,0}, + {0x9b21,0x1,0}, + {0x9b23,0x1,0}, + {0x9b25,0x14,0}, + {0x9b27,0x14,0}, + {0x9b29,0x14,0}, + {0x9b2b,0x2f,0}, + {0x9b2d,0x1a,0}, + {0x9b2f,0x14,0}, + {0x9b31,0x1e,0}, + {0x9b33,0x1e,0}, + {0x9b35,0x1e,0}, + {0x9b37,0x6b,0}, + {0x9b39,0x7c,0}, + {0x9b3b,0x81,0}, + {0x9b43,0xf,0}, + {0x9b45,0xf,0}, + {0x9b47,0xf,0}, + {0x9b49,0xf,0}, + {0x9b4b,0xf,0}, + {0x9b4d,0xf,0}, + {0x9b4f,0x2d,0}, + {0x9b51,0xb,0}, + {0x9b53,0x8,0}, + {0x9b55,0x40,0}, + {0x9b57,0xd,0}, + {0x9b59,0x8,0}, + {0x9b5a,0x0,0}, + {0x9b5b,0x0,0}, + {0x9b5c,0x0,0}, + {0x9b6b,0x0,0}, + {0x9b6d,0x10,0}, + {0x9b6f,0x10,0}, + {0x9b71,0xc8,0}, + {0x9b73,0x32,0}, + {0x9b75,0x4,0}, + {0x9bb0,0x40,0}, + {0x9bb1,0x40,0}, + {0x9bb2,0x40,0}, + {0x9bb3,0x30,0}, + {0x9bb4,0x30,0}, + {0x9bb5,0x30,0}, + {0x9bbb,0xa,0}, + {0x9bbd,0xa,0}, + {0x9bbf,0xa,0}, + {0x9bc0,0x9,0}, + {0x9bc1,0x9,0}, + {0x9bc2,0x9,0}, + {0x9bc6,0x18,0}, + {0x9bc7,0x18,0}, + {0x9bc8,0x18,0}, + {0x9bc9,0xff,0}, + {0x9bca,0xff,0}, + {0x9bcb,0xff,0}, + {0xb2b2,0x1,0}, +}; + +static struct msm_camera_i2c_reg_array mode_setting_array_imx298[] = { +// i2c settings for mode 3 +// { +// .x_output = 2328, +// .y_output = 1748, +// .line_length_pclk = 5536, +// .frame_length_lines = 1802, +// .vt_pixel_clk = 299300000, +// .op_pixel_clk = 299300000, +// .binning_factor = 2, +// .min_fps = 15.000000, +// .max_fps = 30.020000, +// .mode = 1, +// .offset_x = 0, +// .offset_y = 0, +// .scale_factor = 1.000000, +// .is_pdaf_supported = 1, +// } + +// mode settings + +// hdr settings +{0x0114, 0x03, 0}, // CSI_LANE_MODE = 4-lane +/*{0x0220, 0x00, 0}, // HDR_MODE = disable +{0x0221, 0x11, 0}, // HDR_RESO_REDU_H/V = Full Pixel +{0x0222, 0x10, 0}, // EXPO_RATIO = 16*/ +{0x0220, 0x01, 0}, // HDR_MODE = enable with combined gain and 16x ratio +{0x0221, 0x22, 0}, // HDR_RESO_REDU_H/V = 2 binning +{0x0222, 0x10, 0}, // EXPO_RATIO = 16 + +{0x0340, 0x07, 0}, {0x0341, 0x0a, 0}, // FRM_LENGTH = frame_length_lines = 1802 +{0x0342, 0x15, 0}, {0x0343, 0xa0, 0}, // LINE_LENGTH = line_length_pclk = 5536 +{0x0344, 0x00, 0}, {0x0345, 0x00, 0}, // x_addr_start +{0x0346, 0x00, 0}, {0x0347, 0x00, 0}, // y_addr_start +{0x0348, 0x12, 0}, {0x0349, 0x2f, 0}, // x_addr_end +{0x034a, 0x0d, 0}, {0x034b, 0xa7, 0}, // y_addr_end +{0x0381, 0x01, 0}, // x_even_inc +{0x0383, 0x01, 0}, // x_odd_inc +{0x0385, 0x01, 0}, // y_even_inc +{0x0387, 0x01, 0}, // y_odd_inc +{0x0900, 0x01, 0}, // BINNING_MODE = enable +{0x0901, 0x22, 0}, // BINING_TYPE_H/V = 2binning +{0x0902, 0x00, 0}, // binning_weighting = average + +{0x0b06, 1, 0}, // SING_DEF_CORR_EN +{0x0b0a, 1, 0}, // combined defect correct + +{0x3010, 0x66, 0}, // HDR_OUTPUT_CTRL = ATR + HDR compose + DPC1D + DCP2D +{0x3011, 0x01, 0}, // HDR_OUTPUT_CTRL2 = PD enable +{0x30c0, 0x11, 0}, // RED_GAIN_CB? +{0x300d, 0x00, 0}, // FORCE_FDSUM = disable +{0x30fd, 0x00, 0}, +{0x8493, 0x00, 0}, +{0x8863, 0x00, 0}, +{0x90d7, 0x19, 0}, + +// set black level +{0x3090, 1, 0}, +{0x3092, 0, 0}, +{0x3093, 0x38, 0}, + +// output size settings +{0x0112, 0x0a, 0}, {0x0113, 0x0a, 0}, // CS_DT_FMT_H = 0x0a0a (RAW10 output) +{0x034c, 0x09, 0}, {0x034d, 0x18, 0}, // X_OUT_SIZE = 2328 (1164*2) +{0x034e, 0x06, 0}, {0x034f, 0xd4, 0}, // Y_OUT_SIZE = 1748 (874*2) +{0x0401, 0x00, 0}, // SCALING_MODE +{0x0404, 0x00, 0}, {0x0405, 0x10, 0}, // SCALE_M +{0x0408, 0x00, 0}, {0x0409, 0x00, 0}, // DCROP_XOFS +{0x040a, 0x00, 0}, {0x040b, 0x00, 0}, // DCROP_YOFS +{0x040c, 0x09, 0}, {0x040d, 0x18, 0}, // DCROP_WIDTH +{0x040e, 0x06, 0}, {0x040f, 0xd4, 0}, // DCROP_HIGT + +// clock settings +// 299300000 +/* +{0x0301, 0x05, 0}, // VT_PIX_CLK_DIV +{0x0303, 0x02, 0}, // VT_SYS_CLK_DIV +{0x0305, 0x04, 0}, // PRE_PLL_CLK_DIV +{0x0306, 0x00, 0}, {0x0307, 0x7d, 0}, // PLL_MULTIPLIER . mode 1: 0xf6 +{0x0309, 0x0a, 0}, // OP_PIX_CLK_DIV +{0x030b, 0x01, 0}, // OP_SYS_CLK_DIV +{0x030d, 0x0f, 0}, // PREPLLCK_OP_DIV +{0x030e, 0x03, 0}, {0x030f, 0x41, 0}, // PLL_OP_MPY +{0x0310, 0x00, 0}, // PLL_MULT_DRIV +*/ +// 600000000 +{0x0301, 0x05, 0}, +{0x0303, 0x02, 0}, +{0x0305, 0x04, 0}, +{0x0306, 0x00, 0}, +{0x0307, 0xfa, 0}, +{0x0309, 0x0a, 0}, +{0x030b, 0x01, 0}, +{0x030d, 0x0f, 0}, +{0x030e, 0x03, 0}, {0x030f, 0x41, 0}, +{0x0310, 0x00, 0}, + +// data rate settings +/*{0x0820, 0x0b, 0}, // requested_link_bit_rate_mbps = 3000 +{0x0821, 0xb8, 0}, +{0x0822, 0x00, 0}, +{0x0823, 0x00, 0},*/ +{0x0820, 0x17, 0}, // requested_link_bit_rate_mbps = 6000 +{0x0821, 0x70, 0}, +{0x0822, 0x00, 0}, +{0x0823, 0x00, 0}, + +//integration time settings +{0x0202, 0x07, 0}, {0x0203, 0x00, 0}, // INTEG_TIME = 1792 +{0x0224, 0x01, 0}, {0x0225, 0xf4, 0}, // ST_COARSE_INTEG_TIME = 506 + +// gain settings +{0x0204, 0x00, 0}, {0x0205, 0x00, 0}, // ANA_GAIN_GLOBAL = 512 / (512 - X) +{0x0216, 0x00, 0}, {0x0217, 0x00, 0}, // ST_ANA_GAIN_GLOBAL[8] +{0x020e, 0x01, 0}, {0x020f, 0x00, 0}, // DIG_GAIN_GR +{0x0210, 0x01, 0}, {0x0211, 0x00, 0}, // DIG_GAIN_R +{0x0212, 0x01, 0}, {0x0213, 0x00, 0}, // DIG_GAIN_B +{0x0214, 0x01, 0}, {0x0215, 0x00, 0}, // DIG_GAIN_GB + +// HDR white balance settings (ABS_GAIN) +{0xb8e, 0x01, 0}, {0xb8f, 0x00, 0}, // GR +{0xb90, 0x02, 0}, {0xb91, 0x2b, 0}, // R +{0xb92, 0x01, 0}, {0xb93, 0xd4, 0}, // B +{0xb94, 0x01, 0}, {0xb95, 0x00, 0}, // GB + +// phase detection settings +{0x3058, 0x00, 0}, // NML_NR_EN +{0x3103, 0x01, 0}, // NML_PD_CAL_ENABLE = enable +{0x3108, 0x00, 0}, {0x3109, 0x2c, 0}, //PD_AREA_X_OFFSET +{0x310a, 0x00, 0}, {0x310b, 0x24, 0}, //PD_AREA_Y_OFFSET +{0x310c, 0x01, 0}, {0x310d, 0xa4, 0}, //PD_AREA_WIDTH +{0x310e, 0x01, 0}, {0x310f, 0xa4, 0}, //PD_AREA_HEIGHT +// whole size is 0x918 x 0x6d4 +{0x3110, 0x03, 0}, // PD_AREA_0 = 0x375-0x4d1, 0x258-0x3b6 +{0x3111, 0x75, 0}, +{0x3112, 0x02, 0}, +{0x3113, 0x58, 0}, +{0x3114, 0x04, 0}, +{0x3115, 0xd1, 0}, +{0x3116, 0x03, 0}, +{0x3117, 0xb6, 0}, +{0x3118, 0x04, 0}, // PD_AREA_1 = 0x446-0x5a2, 0x258-0x3b6 +{0x3119, 0x46, 0}, +{0x311a, 0x02, 0}, +{0x311b, 0x58, 0}, +{0x311c, 0x05, 0}, +{0x311d, 0xa2, 0}, +{0x311e, 0x03, 0}, +{0x311f, 0xb6, 0}, +{0x3120, 0x03, 0}, // PD_AREA_2 = 0x375-0x4d1, 0x32a-0x488 +{0x3121, 0x75, 0}, +{0x3122, 0x03, 0}, +{0x3123, 0x2a, 0}, +{0x3124, 0x04, 0}, +{0x3125, 0xd1, 0}, +{0x3126, 0x04, 0}, +{0x3127, 0x88, 0}, +{0x3128, 0x04, 0}, // PD_AREA_3 = 0x446-0x5a2, 0x32a-0x488 +{0x3129, 0x46, 0}, +{0x312a, 0x03, 0}, +{0x312b, 0x2a, 0}, +{0x312c, 0x05, 0}, +{0x312d, 0xa2, 0}, +{0x312e, 0x04, 0}, +{0x312f, 0x88, 0}, +{0x3130, 0x03, 0}, // PD_AREA_4 = 0x375-0x5a2, 0x258-0x488 +{0x3131, 0x75, 0}, +{0x3132, 0x02, 0}, +{0x3133, 0x58, 0}, +{0x3134, 0x05, 0}, +{0x3135, 0xa2, 0}, +{0x3136, 0x04, 0}, +{0x3137, 0x88, 0}, +{0x3138, 0x02, 0}, // PD_AREA_5 = 0x2ba-0x65d, 0x210-0x4d0 +{0x3139, 0xba, 0}, +{0x313a, 0x02, 0}, +{0x313b, 0x10, 0}, +{0x313c, 0x06, 0}, +{0x313d, 0x5d, 0}, +{0x313e, 0x04, 0}, +{0x313f, 0xd0, 0}, +{0x3140, 0x00, 0}, // PD_AREA_6 = 0xa1-0x876, 0x6b-0x676 +{0x3141, 0xa1, 0}, +{0x3142, 0x00, 0}, +{0x3143, 0x6b, 0}, +{0x3144, 0x08, 0}, +{0x3145, 0x76, 0}, +{0x3146, 0x06, 0}, +{0x3147, 0x76, 0}, +{0x3148, 0x00, 0}, // PD_AREA_7 = 0xa1-0x876, 0x5e-0x34c +{0x3149, 0xa1, 0}, +{0x314a, 0x00, 0}, +{0x314b, 0x5e, 0}, +{0x314c, 0x08, 0}, +{0x314d, 0x76, 0}, +{0x314e, 0x03, 0}, +{0x314f, 0x4c, 0}, +{0x3165, 0x02, 0}, // AREA_EN_0 = free area +}; + +// static struct msm_camera_i2c_reg_array reg_array3[] = { +// // REG_HOLD ON +// {0x104,0x1,0}, +// // from regression +// {0x3002,0x0,0}, +// // FRM_LENGTH, 1802 vs 3554 +// // {0x340,0x7,0}, {0x341,0xa,0}, // camera start {0x340,0xd,0}, {0x341,0xe2,0}, +// // INTEG_TIME aka coarse_int_time_addr aka shutter speed +// {0x202,0x03,0}, {0x203,0xda,0}, +// // global_gain_addr +// {0x204,0x0,0}, {0x205,0x0,0}, + +// //?? +// {0x20e,0x1,0}, {0x20f,0x0,0}, +// {0x210,0x1,0}, {0x211,0x0,0}, +// {0x212,0x1,0}, {0x213,0x0,0}, +// {0x214,0x1,0}, {0x215,0x0,0}, + +// // REG_HOLD: mode setting +// {0x104,0x0,0}, +// }; + +// start, remove standby mode +static struct msm_camera_i2c_reg_array start_reg_array[] = {{0x100,0x1,0}}; + +// stop, enable standby mode +static struct msm_camera_i2c_reg_array stop_reg_array[] = {{0x100,0x0,0}}; + + +/////////////////// + + +static struct msm_camera_i2c_reg_array init_array_imx179[] = { + { 0x100, 0x0, 0}, // MODE_SELECT + { 0x101, 0x0, 0}, // IMAGE_ORIENT + { 0x202, 0x9, 0}, { 0x203, 0xd2, 0}, // COARSE_INTEGRATION_TIME + { 0x301, 0x5, 0}, // vt_pix_clk_div + { 0x303, 0x1, 0}, // vt_sys_clk_div + { 0x305, 0x6, 0}, // pre_pll_clk_div + { 0x309, 0x5, 0}, // op_pix_clk_div + { 0x30b, 0x1, 0}, // op_sys_clk_div + { 0x30c, 0x0, 0}, { 0x30d, 0x9d, 0}, + + { 0x340, 0x9, 0}, { 0x341, 0xd6, 0}, // frame_length_lines + { 0x342, 0xd, 0}, { 0x343, 0x70, 0}, // line_length_pclk + { 0x344, 0x0, 0}, { 0x345, 0x0, 0}, // x_addr_start + { 0x346, 0x0, 0}, { 0x347, 0x0, 0}, // y_addr_start + { 0x348, 0xc, 0}, { 0x349, 0xcf, 0}, // last_pixel / x_addr_end + { 0x34a, 0x9, 0}, { 0x34b, 0x9f, 0}, // last_line / y_addr_end + { 0x34c, 0xc, 0}, { 0x34d, 0xd0, 0}, // pixels_per_line / x_output_size + { 0x34e, 0x9, 0}, { 0x34f, 0xa0, 0}, // lines_per_frame / y_output_size + { 0x383, 0x1, 0}, // x_odd_inc + { 0x387, 0x1, 0}, // y_odd_inc + { 0x390, 0x0, 0}, // binning_mode + { 0x401, 0x0, 0}, // SCALING_MODE + { 0x405, 0x10, 0}, // SCALE_M + + {0x3020, 0x10, 0}, + {0x3041, 0x15, 0}, // READ_MODE? + {0x3042, 0x87, 0}, + {0x3089, 0x4f, 0}, + {0x3309, 0x9a, 0}, + {0x3344, 0x57, 0}, + {0x3345, 0x1f, 0}, + {0x3362, 0xa, 0}, + {0x3363, 0xa, 0}, + {0x3364, 0x0, 0}, + {0x3368, 0x18, 0}, + {0x3369, 0x0, 0}, + {0x3370, 0x6f, 0}, + {0x3371, 0x27, 0}, + {0x3372, 0x4f, 0}, + {0x3373, 0x2f, 0}, + {0x3374, 0x27, 0}, + {0x3375, 0x2f, 0}, + {0x3376, 0x97, 0}, + {0x3377, 0x37, 0}, + {0x33c8, 0x0, 0}, + {0x33d4, 0xc, 0}, + {0x33d5, 0xd0, 0}, + {0x33d6, 0x9, 0}, + {0x33d7, 0xa0, 0}, + // znr + {0x4100, 0xe, 0}, + {0x4108, 0x1, 0}, + {0x4109, 0x7c, 0}, +}; + + + +/////////////// ois stuff /////////////// + +/* +#define _OP_FIRM_DWNLD 0x80 +#define _OP_Periphe_RW 0x82 +#define _OP_Memory__RW 0x84 +#define _OP_AD_TRNSFER 0x86 +#define _OP_COEF_DWNLD 0x88 +#define _OP_PrgMem__RD 0x8A +#define _OP_SpecialCMD 0x8C +*/ + +static struct reg_settings_ois_t ois_init_settings[] = { + { + .reg_addr = 0x8262, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0xbf03, + .data_type = MSM_CAMERA_I2C_WORD_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "", + .reg_data_seq_size = 0, + },{ + .reg_addr = 0x8263, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x9f05, + .data_type = MSM_CAMERA_I2C_WORD_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "", + .reg_data_seq_size = 0, + },{ + .reg_addr = 0x8264, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x6040, + .data_type = MSM_CAMERA_I2C_WORD_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "", + .reg_data_seq_size = 0, + },{ + .reg_addr = 0x8260, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x1130, + .data_type = MSM_CAMERA_I2C_WORD_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "", + .reg_data_seq_size = 0, + },{ + .reg_addr = 0x8265, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x8000, + .data_type = MSM_CAMERA_I2C_WORD_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "", + .reg_data_seq_size = 0, + },{ + .reg_addr = 0x8261, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0280, + .data_type = MSM_CAMERA_I2C_WORD_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "", + .reg_data_seq_size = 0, + },{ + .reg_addr = 0x8261, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0380, + .data_type = MSM_CAMERA_I2C_WORD_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "", + .reg_data_seq_size = 0, + },{ + .reg_addr = 0x8261, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0988, + .data_type = MSM_CAMERA_I2C_WORD_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "", + .reg_data_seq_size = 0, + },{ + .reg_addr = 0x8000, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x34\x84\x00\x03\xff\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8000, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8000, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8000, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8000, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8000, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8000, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8000, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8000, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8000, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8000, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8000, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x00\x00\x03\x00\x10\x7e\x84\x50\x00\x08\x40\x7e\xa0\x00\x03\x00\x10\x7e\x84\x60\x00\x08\x40\x7e\xa0\x00\x03\x00\x90\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8084, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x10\x08\x80\x00\xa0\x10\x00\x08\x7f\xff\x11\x8f\x02\x07\x80\x00\x11\x40\xff\xa0\x90\x01\x84\x20\x8f\x08\x40\xfe\x90\x40\xf5", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x80a0, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x08\x80\x01\xa0\x00\x01\x11\x8f\x02\x07\xff\xff\x11\x08\x00\x20\x50\x12\x07\x00\x10\x08\x80\x00\xa0\x10\xff\x84\x20\x0a", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8008, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x7f\xff\x21\x08\xfe\x84\x00\x04\x07\x20\x0c\x08\x7f\xff\x21\x08\xfe\x84\x00\x03\x00\x90\x17\x84\x20\x1f\x08\x80\x17\xa0\x10\x10", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8008, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x7f\xff\x21\x10\x00\x08\x01\x00\x11\x40\x51\xa0\x90\x17\x84\x20\x0f\x08\x80\x47\xa0\x8d\x0c\x07\x00\x00\x11\x30\x03\x07\x80\x41", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8090, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x50\x00\x08\x40\xfc\x90\x88\x2f\x84\x00\x00\x11\x30\x02\x07\x40\xff\x90\x50\x00\x08\x40\xfd\x90\x40\x7f\xa0\x10\xff\x84\x20\x2c", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8008, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x7f\xff\x11\x20\x0d\x08\x80\x0f\x90\x80\x26\xa0\x90\x2e\x84\x00\x10\x08\x90\x26\x84\x00\x10\x08\x80\x1f\xa0\x20\x2e\x08\x40\xed", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8090, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x20\x0f\x08\x80\x0e\x90\x00\x00\x21\x30\x02\x07\x40\xeb\xa0\x50\x00\x08\x40\xfe\x90\x40\x7f\xa0\x04\xeb\x84\x10\x00\x20\x20\x0f", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8008, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x80\x00\x21\x60\x04\x07\x40\xff\xa0\x10\x00\x08\x40\xea\x90\x10\x00\x20\x20\x0f\x08\x80\x00\x11\x00\x0b\x07\x08\x00\x20\x60\x0d", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8007, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x10\x00\x08\x40\xea\x90\x8f\x06\x07\x04\xff\x84\x20\x09\x60\x10\xfc\x84\x08\xfd\x84\x04\xfe\x84\x00\x03\x00\x00\x10\x08\x80\x37", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x80a0, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x90\x69\x84\x00\x10\x08\x80\x69\xa0\x20\x1a\x08\x40\x64\xa0\x10\x10\x08\x80\x4f\xa0\x20\x1d\x08\x40\x5f\xa0\x20\x1e\x08\x40\x5d", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x80a0, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x10\x00\x08\x80\x68\x90\x40\xfe\xa0\x20\x0e\x07\x50\x00\x08\x40\x7f\xa0\x04\xfe\x84\x00\x04\x00\x10\xf0\x44\x50\x00\x08\x00\x7f", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8011, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x20\xf0\x60\x04\x5c\x84\x04\x61\x84\x04\x57\x84\x00\x00\x21\x04\x74\x84\x00\x40\x21\x00\x0b\x07\x04\x74\x84\x00\x00\x21\x10\x57", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8084, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x30\x20\x08\x00\x04\x11\x50\x00\x08\x03\xff\x11\x20\xfa\x60\x10\xf0\x44\x50\x20\x08\x00\x7f\x11\x60\x20\x08\x40\x00\x08\x00\x08", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8011, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x30\x1a\x07\x50\x00\x08\x07\x00\x11\x9f\x1d\x07\x8b\x1e\x07\x9c\x15\x07\x20\xf8\x60\x10\x28\x44\x60\x00\x08\x01\x00\x11\x20\x28", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8060, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x87\x1b\x07\x40\xf7\xa0\x81\x27\x07\x20\x48\x60\x10\x0a\x44\x10\xf4\x84\x3b\xd4\x00\xe0\x14\x43\x08\x00\x20\x00\x06\x07\x89\x02", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8007, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x40\xae\x90\x81\x04\x07\x40\x2e\x90\x40\x7f\xa0\x10\x2e\x44\x20\x2f\x08\x9f\x02\x07\x00\x00\x11\x10\x00\x20\x00\x00\x08\x40\x67", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x80a0, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x11\x84\x02\x07\x20\xf0\x60\x9f\x03\x07\x40\x7e\xa0\x40\x65\x90\x10\x62\x84\x10\x10\x08\x40\x61\xa0\x20\x1d\x08\x40\x5f", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x80a0, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x20\x1e\x08\x40\x5d\xa0\x10\x00\x08\x40\x5e\x90\x20\x30\x60\x10\xcf\x84\x20\x1c\x08\x7f\xff\x21\x3b\xf7\x00\xc8\x14\x43\x10\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8020, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x08\x40\xb5\x90\x40\xad\xa0\x00\x0a\x07\x10\x4f\x84\x20\x1c\x08\x7f\xff\x21\x3c\x01\x00\x48\x14\x43\x10\x00\x20\x00\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8008, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x40\x35\x90\x40\x2d\xa0\x00\x14\x07\x80\x0c\x07\x82\x03\x07\x04\x50\x84\x10\x00\x20\x20\x02\x07\x00\x08\x21\x40\x00\x08\x00\x01", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8011, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x40\x50\xa0\x00\x04\x00\x20\x7f\x00\x80\x14\x43\x01\x00\x01\x04\x00\x11\x02\x00\x21\x10\x9f\x84\x10\x20\x08\x40\x97\x90\x10\xbf", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8084, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x20\x1d\x08\x40\xc6\xa0\x20\x8e\x08\x40\x72\x90\x40\x16\xa0\x10\x96\x84\x10\x00\x08\x40\x9e\x90\x20\x36\x60\x20\x91\x00\x00\x14", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8043, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x01\x01\x00\x04\x11\x00\x02\x21\x10\x1f\x84\x10\x20\x08\x40\x17\x90\x10\x3f\x84\x20\x1d\x08\x40\x46\xa0\x20\x8e\x08\x40\x70", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8090, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x40\x96\xa0\x10\x16\x84\x10\x00\x08\x40\x1e\x90\x20\x34\x60\x10\x65\x84\x50\x10\x08\x00\x00\x21\x84\x03\x07\x20\xf0\x60\x9f\x03", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8007, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x40\x7e\xa0\x00\x10\x08\x40\x65\xa0\x20\x0d\x08\x40\x64\x90\x40\x62\xa0\x00\x04\x00\x04\x1c\x44\x04\x1b\x44\xc3\xff\x21\x87\x04", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8007, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x40\x7f\xa0\x10\x42\x84\x20\x1e\x08\x7f\xff\x21\x10\x20\x08\x40\x1c\x90\x10\x07\x84\x10\x00\x08\x40\x06\x90\x40\x55\xa0\x10\xc2", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8084, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x20\x1e\x08\x7f\xff\x21\x10\x20\x08\x40\x9c\x90\x10\x87\x84\x10\x00\x08\x40\x86\x90\x40\x56\xa0\x10\x56\x84\x20\x0f\x08\x40\x7b", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8090, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x22\x00\x60\x40\x00\x08\x40\x79\xa0\x00\x08\x11\x10\x55\x84\x20\x0f\x08\x40\x7a\x90\x22\x00\x60\x50\x00\x08\x40\x79\xa0\x00\xff", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8011, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x04\x6f\x84\x00\x05\x21\x21\x39\x00\x04\x00\x11\x10\xc1\x84\x20\x0f\x08\x70\x07\x07\x10\x10\x08\x40\xc0\xa0\x20\x0f\x08\x7f\xff", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8011, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x8f\x02\x07\x80\x00\x11\x08\x00\x20\x08\xc1\x84\x40\xbd\x90\x9e\x02\x07\x40\x7f\xa0\x40\xec\x90\x04\x6e\x84\x00\x05\x21\x21\x4c", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8000, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x04\x11\x10\x41\x84\x20\x0f\x08\x70\x07\x07\x10\x10\x08\x40\x40\xa0\x20\x0f\x08\x7f\xff\x11\x8f\x02\x07\x80\x00\x11\x08\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8020, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x08\x41\x84\x40\x3d\x90\x9e\x02\x07\x40\x7f\xa0\x40\x6d\x90\x10\x2d\x44\x20\xe7\x00\x08\x00\x11\x20\x32\x60\x80\x14\x43\x10\x2c", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8044, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x20\xec\x00\x00\x08\x11\x20\x31\x60\x00\x14\x43\x04\x34\x44\x40\x5b\xa0\x00\x04\x00\x21\x6a\x00\x80\x00\x11\x21\x70\x00\x3f\xff", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8011, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x08\xe8\x84\x20\x32\x50\x08\x68\x84\x20\x31\x50\x08\x5e\x84\x20\x30\x50\x0f\xc4\x07\x10\x34\x44\x10\x5b\x84\x6f\xf3\x07\x00\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8008, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x02\x00\x21\x40\xfc\x90\x3d\xd4\x00\x04\xfc\x84\x00\x00\x21\x04\xfd\x84\x00\x08\x21\x00\x04\x00\x21\x81\x00\xc0\x00\x11\x04\x5b", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8084, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x02\x00\x21\x70\x06\x07\x10\xfe\x84\x30\x20\x08\x00\x03\x11\x10\x00\x08\x40\x59\x90\x04\xff\x84\x40\x58\xa0\x10\x59\x84\x20\x0f", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8008, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x40\x59\xa0\x20\x00\x11\x70\x1e\x07\x10\x00\x08\x00\x0b\x11\x0f\xe4\x07\x10\x59\x84\x00\x00\x08\x20\x30\x50\x40\x59\xa0\x70\x06", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8007, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x10\x00\x08\x00\x0a\x11\x0f\xec\x07\x08\x34\x44\x02\x10\x11\x10\x58\x84\x20\x0f\x08\x40\x58\xa0\x20\x00\x11\x30\x08\x07\x0f\xf4", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8007, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x10\x58\x84\x00\x00\x08\x20\x30\x50\x40\x58\xa0\x70\x06\x07\x10\x00\x08\x00\x05\x11\x00\x04\x00\x10\x5a\x84\x00\x00\x08\x00\x01", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8011, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x40\x5a\xa0\x08\x34\x44\x02\x00\x11\x30\x08\x07\x60\x00\x08\x00\x00\x11\x40\x5a\xa0\x8f\x4f\x07\x40\x7e\xa0\x00\x04\x00\x3c\xdd", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8000, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x10\x00\x20\xa8\x14\x43\x04\xa7\x84\x20\x8e\x08\x40\xc4\x90\x40\xaf\xa0\x20\x0f\x08\x40\xc5\x90\x40\xa7\xa0\x3c\xe7\x00\x10\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8020, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x28\x14\x43\x04\x27\x84\x20\x8e\x08\x40\x44\x90\x40\x2f\xa0\x20\x0f\x08\x40\x45\x90\x40\x27\xa0\x00\x04\x00\x10\xf0\x44\x50\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8008, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\xff\xf7\x11\x93\x04\x07\x20\xf0\x60\x04\x61\x84\x04\x5c\x84\x40\x57\xa0\x00\x04\x00\x10\x61\x84\x00\x20\x08\x20\x1f\x08\x40\xff", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x80a0, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x10\x00\x08\x40\x5c\x90\x40\x57\xa0\x04\x74\x84\x10\x00\x20\x00\x00\x08\x40\x74\xa0\x00\x01\x11\x20\x05\x07\x00\x40\x21\x70\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8008, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x7f\xff\x21\x08\xff\x84\x42\x00\x90\x85\x02\x07\x20\xf0\x60\x7f\xff\x11\x60\x20\x08\x00\xd0\x11\x40\x00\x08\x00\x02\x11\x86\x1b", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8007, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x40\x74\xa0\x08\x6f\x84\x00\x00\x11\x04\xf1\x84\x00\x40\x21\x21\xf7\x00\x02\x00\x11\x60\x07\x07\x10\x20\x08\x40\xc3\x90\x20\x2f", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8008, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x80\x00\x11\x70\x02\x07\x7f\xff\x11\x10\x00\x08\x40\x8d\x90\x40\x85\xa0\x08\x6e\x84\x00\x00\x11\x04\xf0\x84\x00\x40\x21\x22\x07", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8000, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x02\x11\x60\x07\x07\x10\x20\x08\x40\x43\x90\x20\x2f\x08\x80\x00\x11\x70\x02\x07\x7f\xff\x11\x10\x00\x08\x40\x0d\x90\x40\x05", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x80a0, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x10\xec\x84\x00\x10\x08\x10\xe7\x84\x40\xe7\xa0\x20\x0f\x08\x40\xef\xa0\x10\x6d\x84\x00\x10\x08\x10\x6c\x84\x40\x6c\xa0\x20\x0f", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8008, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x40\x7d\xa0\x7f\xff\x11\x00\x04\x00\x10\xef\x84\x00\x10\x08\x10\xee\x84\x40\xee\xa0\x20\x0f\x08\x40\xbd\xa0\x10\x7d\x84\x00\x10", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8008, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x10\x7c\x84\x40\x7c\xa0\x20\x0f\x08\x40\x3d\xa0\x40\x00\x11\x3e\x9b\x00\xfb\xff\x21\x6f\x14\x43\x3e\x9e\x00\xff\xfb\x21\x6e\x14", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8043, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x3d\x57\x00\x40\xaf\xa0\xa0\x14\x43\x3d\x5a\x00\x40\x2f\xa0\x20\x14\x43\x00\x04\x00\x3d\x5e\x00\x40\xa5\xa0\xb0\x14\x43\x3d\x61", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8000, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x40\x25\xa0\x30\x14\x43\x00\x00\x00\x22\x28\x00\x00\x02\x07\x22\x26\x00\x8a\x03\x07\x89\x04\x07\x40\x7e\xa0\x40\xbd\x90\x98\x14", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8043, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x10\x9c\x84\x20\x8f\x08\x40\x9a\x90\x40\x99\xa0\x20\x2e\x08\x40\xbd\x90\x00\x10\x08\x40\x9b\xa0\x20\x0e\x08\x7f\xff\x21\x40\xf5", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8090, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x00\x22\x3c\x00\x00\x02\x07\x22\x3a\x00\x82\x03\x07\x81\x04\x07\x40\x7e\xa0\x40\x3d\x90\x18\x14\x43\x10\x1c\x84\x20\x8f", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8008, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x40\x1a\x90\x40\x19\xa0\x20\x2e\x08\x40\x3d\x90\x00\x10\x08\x40\x1b\xa0\x20\x0e\x08\x7f\xff\x21\x40\xf5\x90\x3d\x8c\x00\xb8\x14", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8043, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x40\xc2\xa0\x3d\x8f\x00\x38\x14\x43\x40\x42\xa0\x00\x04\x00\x10\x51\x84\x20\x0f\x08\x7f\xc0\x11\x40\x51\xa0\x08\xbb\x84\x08\x3b", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8084, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\xcb\x90\x11\x9e\x02\x07\xc7\x00\x11\x40\x7f\xa0\x08\xba\x84\x08\x3a\x84\x74\x70\x11\x9e\x02\x07\x59\x00\x11\x40\x7f\xa0\x3e\xed", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8000, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\xfd\xff\x21\xf1\x14\x43\x3e\xf0\x00\xff\xfd\x21\xf0\x14\x43\x00\x04\x00\x04\xb9\x84\x40\xb6\xa0\x04\x39\x84\x40\x36\xa0\x00\x04", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8000, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x3e\xc3\x00\x40\x87\xa0\x80\x14\x43\x3e\xc6\x00\x40\x07\xa0\x00\x14\x43\x00\x04\x00\x00\x00\x00\x04\xf5\x84\x00\x00\x21\x70\x03", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8007, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x10\xf5\x84\x10\x00\x08\x40\xf3\x90\x40\xf5\xa0\x3e\xd2\x00\x40\x85\xa0\x88\x14\x43\x3e\xd5\x00\x40\x05\xa0\x08\x14\x43\x00\x04", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8000, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x01\x07\x00\x02\x07\x00\x03\x07\x00\x04\x07\x00\x06\x07\x00\x16\x07\x00\x1e\x07\x00\x24\x07\x00\x3c\x07\x00\x72\x07\x00\x8d", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8007, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\xe0\x07\x00\xf6\x07\x01\x9f\x07\x01\xd1\x07\x00\x10\x07\x1f\xff\x07\x20\x03\x60\x0f\x5f\x07\x08\xfa\x84\x04\x11\x44\xff\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8011, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x2f\x63\x07\x70\x10\x08\x00\xf0\x21\x50\x00\x08\x00\xf0\x21\x40\xf9\x90\x00\x08\x07\x3e\x79\x00\x0f\x6b\x07\x90\x01\x07\x40\xf9", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x80a0, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x3f\x6e\x07\x70\x00\x08\x00\x7f\x11\x40\xf8\xa0\x9d\x07\x07\x20\x12\x60\x08\xfa\x84\x04\x11\x44\x3e\x54\x00\x00\x16\x07\x48\x80", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8011, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x08\x09\x44\x40\x75\x90\x7f\xef\x07\x10\x00\x20\x00\x10\x08\xff\xff\x21\x06\x00\x84\x40\xfb\xa0\x60\x00\x08\x00\x00\x21\x40\xff", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8090, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x10\xfb\x84\x60\x20\x08\x40\xfb\x90\x30\x00\x08\x00\x08\x11\x3d\xea\x00\x04\xfb\x84\x3d\xec\x00\x04\xff\x84\x40\xf8\xa0\x04\x11", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8044, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x8f\xff\x07\x20\x19\x60\x3e\xd2\x00\x00\x31\x07\x00\x01\x00\x00\x33\x07\x00\x02\x00\x27\x18\x43\x75\x00\x43\x8f\xff\x07\x20\x19", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8060, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x04\xf5\x84\x40\xf2\xa0\x04\x51\x84\x7f\xff\x21\x04\xf5\x84\x00\x00\x21\x04\xf3\x84\x00\x10\x21\x04\xf2\x84\x0c\x00\x21\x04\x72", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8044, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x77\x21\x3f\xfa\x07\x70\x20\x08\x00\x00\x11\x10\x2d\x44\x10\x2c\x44\x00\x20\x08\x10\x00\x11\x00\x00\x08\x00\x00\x11\x80\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8021, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x04\x73\x44\x07\x07\x21\x04\x72\x44\x00\x07\x21\x04\x73\x44\x07\x77\x21\x04\x73\x44\x00\x70\x21\x04\x2e\x44\x04\x2d\x44\x04\x2c", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8044, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x80\x00\x21\x04\x1c\x44\x04\x1b\x44\x0d\x00\x21\x04\x1c\x44\x04\x1b\x44\x0c\x80\x21\x04\x1c\x44\x04\x1b\x44\x1f\x08\x21\x0f\x18", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8043, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x08\x28\x44\x01\x11\x11\x08\x5d\x84\x10\x00\x10\x10\x00\x08\x40\x00\x21\x30\x04\x07\x40\xff\x90\x50\x20\x08\xc0\x00\x11\x10\xff", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8084, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x20\x08\x00\x40\x11\x40\x00\x08\x00\x01\x11\x20\x51\x60\x10\x00\x44\x60\x00\x08\x40\x54\x90\x20\x00\x60\x00\x79\x07\x81\x48", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8007, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x80\x03\x07\x40\xf8\xa0\x48\xc0\x11\x00\x77\x07\x00\x78\x07\x00\x79\x07\x00\x04\x07\x00\x7b\x07\x00\x7c\x07\x00\x7d\x07\x00\x55", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8007, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x7f\x07\x00\x80\x07\x00\x81\x07\x00\x74\x07\x00\x83\x07\x00\x82\x07\x00\x85\x07\x00\x86\x07\x0f\xef\x07\x1f\xff\x07\xff\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8011, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x10\x00\x20\x30\x10\x08\x00\x01\x21\x50\x00\x08\x00\x0f\x21\x40\xf9\x90\x30\x90\x07\x70\x10\x08\x00\x80\x21\x50\x00\x08\x00\xf0", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8021, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x08\xf9\x84\x04\xf8\x84\x2f\xff\x07\x3e\x59\x00\x04\xf1\x84\x04\xf0\x84\x04\x5a\x84\x00\x00\x21\x04\xf6\x84\x00\x02\x21\x04\xfa", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8084, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x40\x00\x21\x04\xf7\x84\x01\x04\x21\x04\x1c\x44\x04\x1b\x44\x0b\x23\x21\x0f\x18\x43\x04\x70\x44\x06\x66\x21\x04\x3b\x44\x04\x39", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8044, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x80\x21\x04\x3c\x44\x2a\x0a\x21\x04\x2f\x44\x00\x44\x21\x04\x29\x44\x00\x00\x21\x03\x19\x42\x4e\x19\x43\x0c\x59\x43\x27\x18", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8043, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x15\x00\x43\x04\x22\x44\x03\x07\x21\x04\x21\x44\x11\x11\x21\x04\x20\x44\x33\x33\x21\x00\x00\x42\xc0\x3d\x42\x80\x3d\x43\x04\x35", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8044, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x21\x04\x05\x43\x42\xcd\x00", + .reg_data_seq_size = 9, + },{ + .reg_addr = 0x88ef, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x00\x20\x00\x18\x00\x00\x00\x00\xab\x0a\x00\x40\x00\x40\x00\x00\x00\x00\x00\x00\x00\x80\x00\x00\x27\x83\x64\x7e\xca\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x88b2, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x7f\x59\x7f\xbe\x7f\xfe\x7f\xfe\x7f\xee\x73\x54\x4c\x70\x5a\x00\x6b\x0e\x6b\x1c\x5b\xcb\x32\x8c\x0b\xd2\x13\x07\x28\x01\x49\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8800, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x00\x00\xff\x7f\x0b\x97\x14\xae\xf7\x53\x26\x63\x07\x46\x00\x20\x00\x89\x00\x40\x80\x41\x00\x7f\xff\x7f\xd3\x09\x00\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x88ff, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x7f\x00\x00\x00\x00\x90\xcb\x70\x74\xfe\x7f\x52\x09\x00\x00\xfe\x7f\x00\x00\x00\x00\x00\xf2\x00\x40\xf0\x7f\x00\x0c\x00\x00\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8800, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x00\x00\x00\x4a\x00\x4a\x00\x24\xff\x7f\x00\x00\x00\x00\x00\x00\x00\x00\xff\x7f\x00\x00\x00\x50\x00\x30\x00\x00\x00\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8800, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x00\xa0\x00\x00\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x20\xff\x7f\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8800, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x00\x00\xff\x7f\x00\x78\x00\x08\x00\x00\x10\x00\x00\x00\x00\x00\x00\x00\xff\x7f\x00\x40\x00\x40\x00\x00\x00\x00\x00\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8800, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x80\xff\x7f\x1c\x1b\xc5\x07\xc0\x20\x12\xf0\x00\x10\x10\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8800, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x00\x00\x00\x00\x00\x00\x00\xcc\x59\x00\x00\x00\x40\x00\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8800, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x0a\x00\x00\x00\x00\x00\x00\x08\x00\x00\x00\x00\x00\x00\x00\xff\x7f\x0b", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8897, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x14\xae\xf7\x53\x26\x63\x07\x46\x00\x20\x00\x89\x00\x40\x80\x41\x00\x7f\xff\x7f\xd3\x09\x00\x00\xff\x7f\x00\x00\x00\x00\x90\xcb", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8870, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x74\xfe\x7f\x52\x09\x00\x00\xfe\x7f\x00\x00\x00\x00\x00\xf2\x00\x40\xf0\x7f\x00\x0c\x00\x00\x00\x00\x00\x00\x00\x00\x00\x4a\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x884a, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x24\xff\x7f\x00\x00\x00\x00\x00\x00\x00\x00\xff\x7f\x00\x00\x00\x50\x00\x30\x00\x00\x00\x00\x00\x00\x00\x00\xa0\x00\x00\x03", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8800, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x20\xff\x7f\x00\x00\x00\x00\x00\x00\xff\x7f\x00", + .reg_data_seq_size = 32, + },{ + .reg_addr = 0x8878, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0000, + .data_type = MSM_CAMERA_I2C_SEQ_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "\x00\x08\x00\x00\x10\x00\x00\x00\x00\x00\x00\x00\xff\x7f\x00\x40\x00\x40", + .reg_data_seq_size = 18, + },{ + .reg_addr = 0x8205, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0c00, + .data_type = MSM_CAMERA_I2C_WORD_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "", + .reg_data_seq_size = 0, + },{ + .reg_addr = 0x8205, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0d00, + .data_type = MSM_CAMERA_I2C_WORD_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "", + .reg_data_seq_size = 0, + },{ + .reg_addr = 0x8c01, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, + .reg_data = 0x0001, + .data_type = MSM_CAMERA_I2C_BYTE_DATA, + .i2c_operation = MSM_OIS_WRITE, + .delay = 0, + .reg_data_seq = "", + .reg_data_seq_size = 0, + } +}; + + +/* +// still mode settings: + {0x847f, 0x0c0c,}, //_M_EQCTL + + {0x8436, 0xfd7f,}, //_M_Kgxdr + {0x8440, 0xf07f,}, //_M_X_LMT + {0x8443, 0xb41e,}, //_M_X_TGT + {0x841b, 0x4001,}, //_M_Kgx10 + + {0x84b6, 0xfd7f,}, //_M_Kgydr + {0x84c0, 0xf07f,}, //_M_Y_LMT + {0x84c3, 0xb41e,}, //_M_Y_TGT + {0x849b, 0x4001,}, //_M_Kgy10 + + {0x8438, 0x2d0f,}, //_M_Kgx11 + {0x84b8, 0x2d0f,}, //_M_Kgy11 + {0x8447, 0x002b,}, //_M_KgxTG + {0x84c7, 0x002b,}, //_M_KgyTG + + {0x847f, 0x0d0d,}, //_M_EQCTL +*/ + +static struct msm_camera_i2c_reg_array init_array_s5k3p8sp[] = { + {0x6028,0x2000,0}, {0x6214,0x7971,0}, {0x6218,0x7150,0}, {0x30e,0x3d,0}, + {0x6028,0x2000,0}, {0x602a,0x2f38,0}, {0x6f12,0x88,0}, {0x6f12,0xd70,0}, + {0x344,0x18,0}, + {0x348,0x1217,0}, // last_pixel = 0x90C*2 + {0x346,0x18,0}, + {0x34a,0xd97,0}, // last_line = 0x6CC*2 + {0x34c,0x900,0}, // width? + {0x34e,0x6c0,0}, // height? + {0x342,0x1400,0}, // line_length_pclk + {0x340,0xe3b,0}, // frame_length_lines + {0x202,0x200,0}, // integ_time + {0x200,0x618,0}, + {0x900,0x122,0}, {0x380,0x1,0}, {0x382,0x3,0}, {0x384,0x3,0}, {0x386,0x1,0}, {0x400,0x0,0}, {0x404,0x10,0}, + {0x3604,0x2,0}, {0x3606,0x103,0}, {0xf496,0x48,0}, {0xf470,0x20,0}, {0xf43a,0x15,0}, {0xf484,0x6,0}, {0xf440,0xaf,0}, {0xf442,0x44c6,0}, + {0xf408,0xfff7,0}, {0x3664,0x19,0}, {0xf494,0x1010,0}, {0x367a,0x100,0}, {0x362a,0x104,0}, {0x362e,0x404,0}, {0x32b2,0x8,0}, {0x3286,0x3,0}, {0x328a,0x5,0}, + {0xf47c,0x1f,0}, {0xf62e,0xc5,0}, {0xf630,0xcd,0}, {0xf632,0xdd,0}, {0xf634,0xe5,0}, {0xf636,0xf5,0}, {0xf638,0xfd,0}, {0xf63a,0x10d,0}, {0xf63c,0x115,0}, {0xf63e,0x125,0}, {0xf640,0x12d,0}, + {0x6028,0x2000,0}, {0x602a,0x1704,0}, {0x6f12,0x8011,0}, {0x3070,0x0,0}, {0xb0e,0x0,0}, {0x317a,0x7,0}, {0x31c0,0xc8,0}, {0x1006,0x4,0}, {0x31a4,0x102,0}, +}; + +static struct msm_camera_i2c_reg_array init_array_ov8865[] = { +// round 1 +//{0x103,0x1,0}, // software reset +{0x100,0x0,0}, // standby on +{0x3638,0xff,0}, +{0x302,0x1e,0}, {0x303,0x0,0}, {0x304,0x3,0}, {0x30d,0x1e,0}, {0x30e,0x0,0}, {0x30f,0x9,0}, {0x312,0x1,0}, {0x31e,0xc,0}, // PLL control +{0x3015,0x1,0}, {0x3018,0x72,0}, {0x3020,0x93,0}, {0x3022,0x1,0}, +{0x3031,0xa,0}, // 10-bit mode +{0x3106,0x1,0}, {0x3305,0xf1,0}, {0x3308,0x0,0}, {0x3309,0x28,0}, {0x330a,0x0,0}, {0x330b,0x20,0}, {0x330c,0x0,0}, {0x330d,0x0,0}, {0x330e,0x0,0}, {0x330f,0x40,0}, {0x3307,0x4,0}, {0x3604,0x4,0}, {0x3602,0x30,0}, {0x3605,0x0,0}, {0x3607,0x20,0}, {0x3608,0x11,0}, {0x3609,0x68,0}, {0x360a,0x40,0}, {0x360c,0xdd,0}, {0x360e,0xc,0}, {0x3610,0x7,0}, {0x3612,0x86,0}, {0x3613,0x58,0}, {0x3614,0x28,0}, {0x3617,0x40,0}, {0x3618,0x5a,0}, {0x3619,0x9b,0}, {0x361c,0x0,0}, {0x361d,0x60,0}, {0x3631,0x60,0}, {0x3633,0x10,0}, {0x3634,0x10,0}, {0x3635,0x10,0}, {0x3636,0x10,0}, {0x3641,0x55,0}, {0x3646,0x86,0}, {0x3647,0x27,0}, {0x364a,0x1b,0}, {0x3500,0x0,0}, {0x3501,0x4c,0}, {0x3502,0x0,0}, {0x3503,0x0,0}, {0x3508,0x2,0}, +{0x3509,0x0,0}, // AEC GAIN +{0x3700,0x24,0}, {0x3701,0xc,0}, {0x3702,0x28,0}, {0x3703,0x19,0}, {0x3704,0x14,0}, {0x3705,0x0,0}, {0x3706,0x38,0}, {0x3707,0x4,0}, {0x3708,0x24,0}, {0x3709,0x40,0}, {0x370a,0x0,0}, {0x370b,0xb8,0}, {0x370c,0x4,0}, {0x3718,0x12,0}, {0x3719,0x31,0}, {0x3712,0x42,0}, {0x3714,0x12,0}, {0x371e,0x19,0}, {0x371f,0x40,0}, {0x3720,0x5,0}, {0x3721,0x5,0}, {0x3724,0x2,0}, {0x3725,0x2,0}, {0x3726,0x6,0}, {0x3728,0x5,0}, {0x3729,0x2,0}, {0x372a,0x3,0}, {0x372b,0x53,0}, {0x372c,0xa3,0}, {0x372d,0x53,0}, {0x372e,0x6,0}, {0x372f,0x10,0}, {0x3730,0x1,0}, {0x3731,0x6,0}, {0x3732,0x14,0}, {0x3733,0x10,0}, {0x3734,0x40,0}, {0x3736,0x20,0}, {0x373a,0x2,0}, {0x373b,0xc,0}, {0x373c,0xa,0}, {0x373e,0x3,0}, {0x3755,0x40,0}, {0x3758,0x0,0}, {0x3759,0x4c,0}, {0x375a,0x6,0}, {0x375b,0x13,0}, {0x375c,0x40,0}, {0x375d,0x2,0}, {0x375e,0x0,0}, {0x375f,0x14,0}, {0x3767,0x1c,0}, {0x3768,0x4,0}, {0x3769,0x20,0}, {0x376c,0xc0,0}, {0x376d,0xc0,0}, {0x376a,0x8,0}, {0x3761,0x0,0}, {0x3762,0x0,0}, {0x3763,0x0,0}, {0x3766,0xff,0}, {0x376b,0x42,0}, {0x3772,0x23,0}, {0x3773,0x2,0}, {0x3774,0x16,0}, {0x3775,0x12,0}, {0x3776,0x8,0}, {0x37a0,0x44,0}, {0x37a1,0x3d,0}, {0x37a2,0x3d,0}, {0x37a3,0x1,0}, {0x37a4,0x0,0}, {0x37a5,0x8,0}, {0x37a6,0x0,0}, {0x37a7,0x44,0}, {0x37a8,0x58,0}, {0x37a9,0x58,0}, {0x3760,0x0,0}, {0x376f,0x1,0}, {0x37aa,0x44,0}, {0x37ab,0x2e,0}, {0x37ac,0x2e,0}, {0x37ad,0x33,0}, {0x37ae,0xd,0}, {0x37af,0xd,0}, {0x37b0,0x0,0}, {0x37b1,0x0,0}, {0x37b2,0x0,0}, {0x37b3,0x42,0}, {0x37b4,0x42,0}, {0x37b5,0x33,0}, {0x37b6,0x0,0}, {0x37b7,0x0,0}, {0x37b8,0x0,0}, {0x37b9,0xff,0}, {0x3800,0x0,0}, {0x3801,0xc,0}, {0x3802,0x0,0}, {0x3803,0xc,0}, +{0x3804,0xc,0}, {0x3805,0xd3,0}, // 3283 +{0x3806,0x9,0}, {0x3807,0xa3,0}, // 2467 +{0x3808,0x6,0}, {0x3809,0x60,0}, // 0x660 = 1632 (width) +{0x380a,0x4,0}, {0x380b,0xc8,0}, // 0x4c8 = 1224 (height) +{0x380c,0x7,0}, {0x380d,0x83,0}, // 0x783 = 1923 (stride) +{0x380e,0x4,0}, {0x380f,0xe0,0}, // 0x4e0 = 1248 (vstride) +{0x3810,0x0,0}, {0x3811,0x4,0}, {0x3813,0x4,0}, +{0x3814,0x3,0}, {0x3815,0x1,0}, // H-binning +{0x3820,0x6,0}, // format1 +{0x3821,0x40,0}, // format2 +{0x382a,0x3,0}, {0x382b,0x1,0}, // V-binning +{0x382d,0x7f,0}, {0x3830,0x8,0}, {0x3836,0x2,0}, {0x3837,0x18,0}, +{0x3841,0xff,0}, +{0x3846,0x88,0}, {0x3d85,0x6,0}, {0x3d8c,0x75,0}, {0x3d8d,0xef,0}, {0x3f08,0xb,0}, {0x4000,0xf1,0}, {0x4001,0x14,0}, {0x4005,0x10,0}, {0x4006,0x1,0}, {0x4007,0x1,0}, {0x400b,0xc,0}, {0x400d,0x10,0}, {0x401b,0x0,0}, {0x401d,0x0,0}, {0x4020,0x0,0}, {0x4021,0x0,0}, {0x4022,0x4,0}, {0x4023,0x1f,0}, {0x4024,0x6,0}, {0x4025,0x20,0}, {0x4026,0x6,0}, {0x4027,0x4f,0}, {0x4028,0x0,0}, {0x4029,0x2,0}, {0x402a,0x4,0}, {0x402b,0x4,0}, {0x402c,0x2,0}, {0x402d,0x2,0}, {0x402e,0x8,0}, {0x402f,0x2,0}, {0x401f,0x0,0}, {0x4034,0x3f,0}, {0x4300,0xff,0}, {0x4301,0x0,0}, {0x4302,0xf,0}, {0x4500,0x40,0}, {0x4503,0x10,0}, {0x4601,0x74,0}, {0x481f,0x32,0}, {0x4837,0x15,0}, {0x4850,0x10,0}, {0x4851,0x32,0}, {0x4b00,0x2a,0}, {0x4b0d,0x0,0}, {0x4d00,0x4,0}, {0x4d01,0x18,0}, {0x4d02,0xc3,0}, {0x4d03,0xff,0}, {0x4d04,0xff,0}, {0x4d05,0xff,0}, {0x5000,0x96,0}, {0x5001,0x1,0}, {0x5002,0x8,0}, {0x5901,0x0,0}, +{0x5e00,0x0,0}, +//{0x5e00,0x80,0}, +{0x5e01,0x41,0}, {0x5b00,0x2,0}, {0x5b01,0xd0,0}, {0x5b02,0x3,0}, {0x5b03,0xff,0}, {0x5b05,0x6c,0}, {0x5780,0xfc,0}, {0x5781,0xdf,0}, {0x5782,0x3f,0}, {0x5783,0x8,0}, {0x5784,0xc,0}, {0x5786,0x20,0}, {0x5787,0x40,0}, {0x5788,0x8,0}, {0x5789,0x8,0}, {0x578a,0x2,0}, {0x578b,0x1,0}, {0x578c,0x1,0}, {0x578d,0xc,0}, {0x578e,0x2,0}, {0x578f,0x1,0}, {0x5790,0x1,0}, {0x5800,0x1d,0}, {0x5801,0xe,0}, {0x5802,0xc,0}, {0x5803,0xc,0}, {0x5804,0xf,0}, {0x5805,0x22,0}, {0x5806,0xa,0}, {0x5807,0x6,0}, {0x5808,0x5,0}, {0x5809,0x5,0}, {0x580a,0x7,0}, {0x580b,0xa,0}, {0x580c,0x6,0}, {0x580d,0x2,0}, {0x580e,0x0,0}, {0x580f,0x0,0}, {0x5810,0x3,0}, {0x5811,0x7,0}, {0x5812,0x6,0}, {0x5813,0x2,0}, {0x5814,0x0,0}, {0x5815,0x0,0}, {0x5816,0x3,0}, {0x5817,0x7,0}, {0x5818,0x9,0}, {0x5819,0x6,0}, {0x581a,0x4,0}, {0x581b,0x4,0}, {0x581c,0x6,0}, {0x581d,0xa,0}, {0x581e,0x19,0}, {0x581f,0xd,0}, {0x5820,0xb,0}, {0x5821,0xb,0}, {0x5822,0xe,0}, {0x5823,0x22,0}, {0x5824,0x23,0}, {0x5825,0x28,0}, {0x5826,0x29,0}, {0x5827,0x27,0}, {0x5828,0x13,0}, {0x5829,0x26,0}, {0x582a,0x33,0}, {0x582b,0x32,0}, {0x582c,0x33,0}, {0x582d,0x16,0}, {0x582e,0x14,0}, {0x582f,0x30,0}, {0x5830,0x31,0}, {0x5831,0x30,0}, {0x5832,0x15,0}, {0x5833,0x26,0}, {0x5834,0x23,0}, {0x5835,0x21,0}, {0x5836,0x23,0}, {0x5837,0x5,0}, {0x5838,0x36,0}, {0x5839,0x27,0}, {0x583a,0x28,0}, {0x583b,0x26,0}, {0x583c,0x24,0}, {0x583d,0xdf,0}, +//{0x100,0x1,0}, +// round 2 (color calibration) +{0x7010,0x0,0}, {0x7011,0x0,0}, {0x7012,0x0,0}, {0x7013,0x0,0}, {0x7014,0x0,0}, {0x7015,0x0,0}, {0x7016,0x0,0}, {0x7017,0x0,0}, {0x7018,0x0,0}, {0x7019,0x0,0}, {0x701a,0x0,0}, {0x701b,0x0,0}, {0x701c,0x0,0}, {0x701d,0x0,0}, {0x701e,0x0,0}, {0x701f,0x0,0}, {0x7020,0x0,0}, {0x7021,0x0,0}, {0x7022,0x0,0}, {0x7023,0x0,0}, {0x7024,0x0,0}, {0x7025,0x0,0}, {0x7026,0x0,0}, {0x7027,0x0,0}, {0x7028,0x0,0}, {0x7029,0x0,0}, {0x702a,0x0,0}, {0x702b,0x0,0}, {0x702c,0x0,0}, {0x702d,0x0,0}, {0x702e,0x0,0}, {0x702f,0x0,0}, {0x7030,0x0,0}, {0x7031,0x0,0}, {0x7032,0x0,0}, {0x7033,0x0,0}, {0x7034,0x0,0}, {0x7035,0x0,0}, {0x7036,0x0,0}, {0x7037,0x0,0}, {0x7038,0x0,0}, {0x7039,0x0,0}, {0x703a,0x0,0}, {0x703b,0x0,0}, {0x703c,0x0,0}, {0x703d,0x0,0}, {0x703e,0x0,0}, {0x703f,0x0,0}, {0x7040,0x0,0}, {0x7041,0x0,0}, {0x7042,0x0,0}, {0x7043,0x0,0}, {0x7044,0x0,0}, {0x7045,0x0,0}, {0x7046,0x0,0}, {0x7047,0x0,0}, {0x7048,0x0,0}, {0x7049,0x0,0}, {0x704a,0x0,0}, {0x704b,0x0,0}, {0x704c,0x0,0}, {0x704d,0x0,0}, {0x704e,0x0,0}, {0x704f,0x0,0}, {0x7050,0x0,0}, {0x7051,0x0,0}, {0x7052,0x0,0}, {0x7053,0x0,0}, {0x7054,0x0,0}, {0x7055,0x0,0}, {0x7056,0x0,0}, {0x7057,0x0,0}, {0x7058,0x0,0}, {0x7059,0x0,0}, {0x705a,0x0,0}, {0x705b,0x0,0}, {0x705c,0x0,0}, {0x705d,0x0,0}, {0x705e,0x0,0}, {0x705f,0x0,0}, {0x7060,0x0,0}, {0x7061,0x0,0}, {0x7062,0x0,0}, {0x7063,0x0,0}, {0x7064,0x0,0}, {0x7065,0x0,0}, {0x7066,0x0,0}, {0x7067,0x0,0}, {0x7068,0x0,0}, {0x7069,0x0,0}, {0x706a,0x0,0}, {0x706b,0x0,0}, {0x706c,0x0,0}, {0x706d,0x0,0}, {0x706e,0x0,0}, {0x706f,0x0,0}, {0x7070,0x0,0}, {0x7071,0x0,0}, {0x7072,0x0,0}, {0x7073,0x0,0}, {0x7074,0x0,0}, {0x7075,0x0,0}, {0x7076,0x0,0}, {0x7077,0x0,0}, {0x7078,0x0,0}, {0x7079,0x0,0}, {0x707a,0x0,0}, {0x707b,0x0,0}, {0x707c,0x0,0}, {0x707d,0x0,0}, {0x707e,0x0,0}, {0x707f,0x0,0}, {0x7080,0x0,0}, {0x7081,0x0,0}, {0x7082,0x0,0}, {0x7083,0x0,0}, {0x7084,0x0,0}, {0x7085,0x0,0}, {0x7086,0x0,0}, {0x7087,0x0,0}, {0x7088,0x0,0}, {0x7089,0x0,0}, {0x708a,0x0,0}, {0x708b,0x0,0}, {0x708c,0x0,0}, {0x708d,0x0,0}, {0x708e,0x0,0}, {0x708f,0x0,0}, {0x7090,0x0,0}, {0x7091,0x0,0}, {0x7092,0x0,0}, {0x7093,0x0,0}, {0x7094,0x0,0}, {0x7095,0x0,0}, {0x7096,0x0,0}, {0x7097,0x0,0}, {0x7098,0x0,0}, {0x7099,0x0,0}, {0x709a,0x0,0}, {0x709b,0x0,0}, {0x709c,0x0,0}, {0x709d,0x0,0}, {0x709e,0x0,0}, {0x709f,0x0,0}, {0x70a0,0x0,0}, {0x70a1,0x0,0}, {0x70a2,0x0,0}, {0x70a3,0x0,0}, {0x70a4,0x0,0}, {0x70a5,0x0,0}, {0x70a6,0x0,0}, {0x70a7,0x0,0}, {0x70a8,0x0,0}, {0x70a9,0x0,0}, {0x70aa,0x0,0}, {0x70ab,0x0,0}, {0x70ac,0x0,0}, {0x70ad,0x0,0}, {0x70ae,0x0,0}, {0x70af,0x0,0}, {0x70b0,0x0,0}, {0x70b1,0x0,0}, {0x70b2,0x0,0}, {0x70b3,0x0,0}, {0x70b4,0x0,0}, {0x70b5,0x0,0}, {0x70b6,0x0,0}, {0x70b7,0x0,0}, {0x70b8,0x0,0}, {0x70b9,0x0,0}, {0x70ba,0x0,0}, {0x70bb,0x0,0}, {0x70bc,0x0,0}, {0x70bd,0x0,0}, {0x70be,0x0,0}, {0x70bf,0x0,0}, {0x70c0,0x0,0}, {0x70c1,0x0,0}, {0x70c2,0x0,0}, {0x70c3,0x0,0}, {0x70c4,0x0,0}, {0x70c5,0x0,0}, {0x70c6,0x0,0}, {0x70c7,0x0,0}, {0x70c8,0x0,0}, {0x70c9,0x0,0}, {0x70ca,0x0,0}, {0x70cb,0x0,0}, {0x70cc,0x0,0}, {0x70cd,0x0,0}, {0x70ce,0x0,0}, {0x70cf,0x0,0}, {0x70d0,0x0,0}, {0x70d1,0x0,0}, {0x70d2,0x0,0}, {0x70d3,0x0,0}, {0x70d4,0x0,0}, {0x70d5,0x0,0}, {0x70d6,0x0,0}, {0x70d7,0x0,0}, {0x70d8,0x0,0}, {0x70d9,0x0,0}, {0x70da,0x0,0}, {0x70db,0x0,0}, {0x70dc,0x0,0}, {0x70dd,0x0,0}, {0x70de,0x0,0}, {0x70df,0x0,0}, {0x70e0,0x0,0}, {0x70e1,0x0,0}, {0x70e2,0x0,0}, {0x70e3,0x0,0}, {0x70e4,0x0,0}, {0x70e5,0x0,0}, {0x70e6,0x0,0}, {0x70e7,0x0,0}, {0x70e8,0x0,0}, {0x70e9,0x0,0}, {0x70ea,0x0,0}, {0x70eb,0x0,0}, {0x70ec,0x0,0}, {0x70ed,0x0,0}, {0x70ee,0x0,0}, {0x70ef,0x0,0}, {0x70f0,0x0,0}, {0x70f1,0x0,0}, {0x70f2,0x0,0}, {0x70f3,0x0,0}, {0x70f4,0x0,0}, {0x70f5,0x0,0}, {0x70f6,0x0,0}, {0x70f7,0x0,0}, {0x70f8,0x0,0}, {0x70f9,0x0,0}, {0x70fa,0x0,0}, {0x70fb,0x0,0}, {0x70fc,0x0,0}, {0x70fd,0x0,0}, {0x70fe,0x0,0}, {0x70ff,0x0,0}, {0x7100,0x0,0}, {0x7101,0x0,0}, {0x7102,0x0,0}, {0x7103,0x0,0}, {0x7104,0x0,0}, {0x7105,0x0,0}, {0x7106,0x0,0}, {0x7107,0x0,0}, {0x7108,0x0,0}, {0x7109,0x0,0}, {0x710a,0x0,0}, {0x710b,0x0,0}, {0x710c,0x0,0}, {0x710d,0x0,0}, {0x710e,0x0,0}, {0x710f,0x0,0}, {0x7110,0x0,0}, {0x7111,0x0,0}, {0x7112,0x0,0}, {0x7113,0x0,0}, {0x7114,0x0,0}, {0x7115,0x0,0}, {0x7116,0x0,0}, {0x7117,0x0,0}, {0x7118,0x0,0}, {0x7119,0x0,0}, {0x711a,0x0,0}, {0x711b,0x0,0}, {0x711c,0x0,0}, {0x711d,0x0,0}, {0x711e,0x0,0}, {0x711f,0x0,0}, {0x7120,0x0,0}, {0x7121,0x0,0}, {0x7122,0x0,0}, {0x7123,0x0,0}, {0x7124,0x0,0}, {0x7125,0x0,0}, {0x7126,0x0,0}, {0x7127,0x0,0}, {0x7128,0x0,0}, {0x7129,0x0,0}, {0x712a,0x0,0}, {0x712b,0x0,0}, {0x712c,0x0,0}, {0x712d,0x0,0}, {0x712e,0x0,0}, {0x712f,0x0,0}, {0x7130,0x0,0}, {0x7131,0x0,0}, {0x7132,0x0,0}, {0x501a,0x10,0}, {0x501b,0xd,0}, {0x501c,0x10,0}, {0x501d,0x13,0}, {0x5000,0x96,0}, {0x5800,0x14,0}, {0x5801,0xd,0}, {0x5802,0xa,0}, {0x5803,0xa,0}, {0x5804,0xd,0}, {0x5805,0x13,0}, {0x5806,0xa,0}, {0x5807,0x5,0}, {0x5808,0x3,0}, {0x5809,0x3,0}, {0x580a,0x5,0}, {0x580b,0x9,0}, {0x580c,0x6,0}, {0x580d,0x2,0}, {0x580e,0x0,0}, {0x580f,0x0,0}, {0x5810,0x2,0}, {0x5811,0x5,0}, {0x5812,0x6,0}, {0x5813,0x2,0}, {0x5814,0x0,0}, {0x5815,0x0,0}, {0x5816,0x2,0}, {0x5817,0x5,0}, {0x5818,0xb,0}, {0x5819,0x6,0}, {0x581a,0x3,0}, {0x581b,0x3,0}, {0x581c,0x5,0}, {0x581d,0xa,0}, {0x581e,0x16,0}, {0x581f,0xf,0}, {0x5820,0xb,0}, {0x5821,0xb,0}, {0x5822,0xf,0}, {0x5823,0x15,0}, {0x5824,0x32,0}, {0x5825,0x23,0}, {0x5826,0x23,0}, {0x5827,0x23,0}, {0x5828,0x22,0}, {0x5829,0x21,0}, {0x582a,0x21,0}, {0x582b,0x22,0}, {0x582c,0x21,0},{0x582d,0x11,0}, {0x582e,0x22,0}, {0x582f,0x31,0}, {0x5830,0x41,0}, {0x5831,0x31,0}, {0x5832,0x1,0}, {0x5833,0x21,0}, {0x5834,0x21,0}, {0x5835,0x21,0}, {0x5836,0x11,0}, {0x5837,0x11,0}, {0x5838,0x22,0}, {0x5839,0x22,0}, {0x583a,0x12,0}, {0x583b,0x22,0}, {0x583c,0x22,0}, {0x583d,0xdf,0}, +}; + diff --git a/selfdrive/camerad/include/msm_cam_sensor.h b/selfdrive/camerad/include/msm_cam_sensor.h new file mode 100644 index 000000000..c75d1fd52 --- /dev/null +++ b/selfdrive/camerad/include/msm_cam_sensor.h @@ -0,0 +1,829 @@ +#ifndef __LINUX_MSM_CAM_SENSOR_H +#define __LINUX_MSM_CAM_SENSOR_H + +#ifdef MSM_CAMERA_BIONIC +#include +#endif + +//#include +#include "msm_camsensor_sdk.h" + +#include +#include +#ifdef CONFIG_COMPAT +#include +#endif + +#define I2C_SEQ_REG_SETTING_MAX 5 + +#define MSM_SENSOR_MCLK_8HZ 8000000 +#define MSM_SENSOR_MCLK_16HZ 16000000 +#define MSM_SENSOR_MCLK_24HZ 24000000 + +#define MAX_SENSOR_NAME 32 +#define MAX_ACTUATOR_AF_TOTAL_STEPS 1024 + +#define MAX_OIS_MOD_NAME_SIZE 32 +#define MAX_OIS_NAME_SIZE 32 +#define MAX_OIS_REG_SETTINGS 800 + +#define MOVE_NEAR 0 +#define MOVE_FAR 1 + +#define MSM_ACTUATOR_MOVE_SIGNED_FAR -1 +#define MSM_ACTUATOR_MOVE_SIGNED_NEAR 1 + +#define MAX_ACTUATOR_REGION 5 + +#define MAX_EEPROM_NAME 32 + +#define MAX_AF_ITERATIONS 3 +#define MAX_NUMBER_OF_STEPS 47 +#define MAX_REGULATOR 5 + +#define MSM_V4L2_PIX_FMT_META v4l2_fourcc('M', 'E', 'T', 'A') /* META */ +#define MSM_V4L2_PIX_FMT_SBGGR14 v4l2_fourcc('B', 'G', '1', '4') + /* 14 BGBG.. GRGR.. */ +#define MSM_V4L2_PIX_FMT_SGBRG14 v4l2_fourcc('G', 'B', '1', '4') + /* 14 GBGB.. RGRG.. */ +#define MSM_V4L2_PIX_FMT_SGRBG14 v4l2_fourcc('B', 'A', '1', '4') + /* 14 GRGR.. BGBG.. */ +#define MSM_V4L2_PIX_FMT_SRGGB14 v4l2_fourcc('R', 'G', '1', '4') + /* 14 RGRG.. GBGB.. */ + +enum flash_type { + LED_FLASH = 1, + STROBE_FLASH, + GPIO_FLASH +}; + +enum msm_sensor_resolution_t { + MSM_SENSOR_RES_FULL, + MSM_SENSOR_RES_QTR, + MSM_SENSOR_RES_2, + MSM_SENSOR_RES_3, + MSM_SENSOR_RES_4, + MSM_SENSOR_RES_5, + MSM_SENSOR_RES_6, + MSM_SENSOR_RES_7, + MSM_SENSOR_INVALID_RES, +}; + +enum msm_camera_stream_type_t { + MSM_CAMERA_STREAM_PREVIEW, + MSM_CAMERA_STREAM_SNAPSHOT, + MSM_CAMERA_STREAM_VIDEO, + MSM_CAMERA_STREAM_INVALID, +}; + +enum sensor_sub_module_t { + SUB_MODULE_SENSOR, + SUB_MODULE_CHROMATIX, + SUB_MODULE_ACTUATOR, + SUB_MODULE_EEPROM, + SUB_MODULE_LED_FLASH, + SUB_MODULE_STROBE_FLASH, + SUB_MODULE_CSID, + SUB_MODULE_CSID_3D, + SUB_MODULE_CSIPHY, + SUB_MODULE_CSIPHY_3D, + SUB_MODULE_OIS, + SUB_MODULE_EXT, + SUB_MODULE_MAX, +}; + +enum { + MSM_CAMERA_EFFECT_MODE_OFF, + MSM_CAMERA_EFFECT_MODE_MONO, + MSM_CAMERA_EFFECT_MODE_NEGATIVE, + MSM_CAMERA_EFFECT_MODE_SOLARIZE, + MSM_CAMERA_EFFECT_MODE_SEPIA, + MSM_CAMERA_EFFECT_MODE_POSTERIZE, + MSM_CAMERA_EFFECT_MODE_WHITEBOARD, + MSM_CAMERA_EFFECT_MODE_BLACKBOARD, + MSM_CAMERA_EFFECT_MODE_AQUA, + MSM_CAMERA_EFFECT_MODE_EMBOSS, + MSM_CAMERA_EFFECT_MODE_SKETCH, + MSM_CAMERA_EFFECT_MODE_NEON, + MSM_CAMERA_EFFECT_MODE_MAX +}; + +enum { + MSM_CAMERA_WB_MODE_AUTO, + MSM_CAMERA_WB_MODE_CUSTOM, + MSM_CAMERA_WB_MODE_INCANDESCENT, + MSM_CAMERA_WB_MODE_FLUORESCENT, + MSM_CAMERA_WB_MODE_WARM_FLUORESCENT, + MSM_CAMERA_WB_MODE_DAYLIGHT, + MSM_CAMERA_WB_MODE_CLOUDY_DAYLIGHT, + MSM_CAMERA_WB_MODE_TWILIGHT, + MSM_CAMERA_WB_MODE_SHADE, + MSM_CAMERA_WB_MODE_OFF, + MSM_CAMERA_WB_MODE_MAX +}; + +enum { + MSM_CAMERA_SCENE_MODE_OFF, + MSM_CAMERA_SCENE_MODE_AUTO, + MSM_CAMERA_SCENE_MODE_LANDSCAPE, + MSM_CAMERA_SCENE_MODE_SNOW, + MSM_CAMERA_SCENE_MODE_BEACH, + MSM_CAMERA_SCENE_MODE_SUNSET, + MSM_CAMERA_SCENE_MODE_NIGHT, + MSM_CAMERA_SCENE_MODE_PORTRAIT, + MSM_CAMERA_SCENE_MODE_BACKLIGHT, + MSM_CAMERA_SCENE_MODE_SPORTS, + MSM_CAMERA_SCENE_MODE_ANTISHAKE, + MSM_CAMERA_SCENE_MODE_FLOWERS, + MSM_CAMERA_SCENE_MODE_CANDLELIGHT, + MSM_CAMERA_SCENE_MODE_FIREWORKS, + MSM_CAMERA_SCENE_MODE_PARTY, + MSM_CAMERA_SCENE_MODE_NIGHT_PORTRAIT, + MSM_CAMERA_SCENE_MODE_THEATRE, + MSM_CAMERA_SCENE_MODE_ACTION, + MSM_CAMERA_SCENE_MODE_AR, + MSM_CAMERA_SCENE_MODE_FACE_PRIORITY, + MSM_CAMERA_SCENE_MODE_BARCODE, + MSM_CAMERA_SCENE_MODE_HDR, + MSM_CAMERA_SCENE_MODE_MAX +}; + +enum csid_cfg_type_t { + CSID_INIT, + CSID_CFG, + CSID_TESTMODE_CFG, + CSID_RELEASE, +}; + +enum csiphy_cfg_type_t { + CSIPHY_INIT, + CSIPHY_CFG, + CSIPHY_RELEASE, +}; + +enum camera_vreg_type { + VREG_TYPE_DEFAULT, + VREG_TYPE_CUSTOM, +}; + +enum sensor_af_t { + SENSOR_AF_FOCUSSED, + SENSOR_AF_NOT_FOCUSSED, +}; + +enum cci_i2c_master_t { + MASTER_0, + MASTER_1, + MASTER_MAX, +}; + +struct msm_camera_i2c_array_write_config { + struct msm_camera_i2c_reg_setting conf_array; + uint16_t slave_addr; +}; + +struct msm_camera_i2c_read_config { + uint16_t slave_addr; + uint16_t reg_addr; + enum msm_camera_i2c_reg_addr_type addr_type; + enum msm_camera_i2c_data_type data_type; + uint16_t data; +}; + +struct msm_camera_csi2_params { + struct msm_camera_csid_params csid_params; + struct msm_camera_csiphy_params csiphy_params; + uint8_t csi_clk_scale_enable; +}; + +struct msm_camera_csi_lane_params { + uint16_t csi_lane_assign; + uint16_t csi_lane_mask; +}; + +struct csi_lane_params_t { + uint16_t csi_lane_assign; + uint8_t csi_lane_mask; + uint8_t csi_if; + int8_t csid_core[2]; + uint8_t csi_phy_sel; +}; + +struct msm_sensor_info_t { + char sensor_name[MAX_SENSOR_NAME]; + uint32_t session_id; + int32_t subdev_id[SUB_MODULE_MAX]; + int32_t subdev_intf[SUB_MODULE_MAX]; + uint8_t is_mount_angle_valid; + uint32_t sensor_mount_angle; + int modes_supported; + enum camb_position_t position; +}; + +struct camera_vreg_t { + const char *reg_name; + int min_voltage; + int max_voltage; + int op_mode; + uint32_t delay; + const char *custom_vreg_name; + enum camera_vreg_type type; +}; + +struct sensorb_cfg_data { + int cfgtype; + union { + struct msm_sensor_info_t sensor_info; + struct msm_sensor_init_params sensor_init_params; + void *setting; + struct msm_sensor_i2c_sync_params sensor_i2c_sync_params; + } cfg; +}; + +struct csid_cfg_data { + enum csid_cfg_type_t cfgtype; + union { + uint32_t csid_version; + struct msm_camera_csid_params *csid_params; + struct msm_camera_csid_testmode_parms *csid_testmode_params; + } cfg; +}; + +struct csiphy_cfg_data { + enum csiphy_cfg_type_t cfgtype; + union { + struct msm_camera_csiphy_params *csiphy_params; + struct msm_camera_csi_lane_params *csi_lane_params; + } cfg; +}; + +enum eeprom_cfg_type_t { + CFG_EEPROM_GET_INFO, + CFG_EEPROM_GET_CAL_DATA, + CFG_EEPROM_READ_CAL_DATA, + CFG_EEPROM_WRITE_DATA, + CFG_EEPROM_GET_MM_INFO, + CFG_EEPROM_INIT, +}; + +struct eeprom_get_t { + uint32_t num_bytes; +}; + +struct eeprom_read_t { + uint8_t *dbuffer; + uint32_t num_bytes; +}; + +struct eeprom_write_t { + uint8_t *dbuffer; + uint32_t num_bytes; +}; + +struct eeprom_get_cmm_t { + uint32_t cmm_support; + uint32_t cmm_compression; + uint32_t cmm_size; +}; + +struct msm_eeprom_info_t { + struct msm_sensor_power_setting_array *power_setting_array; + enum i2c_freq_mode_t i2c_freq_mode; + struct msm_eeprom_memory_map_array *mem_map_array; +}; + +struct msm_eeprom_cfg_data { + enum eeprom_cfg_type_t cfgtype; + uint8_t is_supported; + union { + char eeprom_name[MAX_SENSOR_NAME]; + struct eeprom_get_t get_data; + struct eeprom_read_t read_data; + struct eeprom_write_t write_data; + struct eeprom_get_cmm_t get_cmm_data; + struct msm_eeprom_info_t eeprom_info; + } cfg; +}; + +#ifdef CONFIG_COMPAT +struct msm_sensor_power_setting32 { + enum msm_sensor_power_seq_type_t seq_type; + uint16_t seq_val; + compat_uint_t config_val; + uint16_t delay; + compat_uptr_t data[10]; +}; + +struct msm_sensor_power_setting_array32 { + struct msm_sensor_power_setting32 power_setting_a[MAX_POWER_CONFIG]; + compat_uptr_t power_setting; + uint16_t size; + struct msm_sensor_power_setting32 + power_down_setting_a[MAX_POWER_CONFIG]; + compat_uptr_t power_down_setting; + uint16_t size_down; +}; + +struct msm_camera_sensor_slave_info32 { + char sensor_name[32]; + char eeprom_name[32]; + char actuator_name[32]; + char ois_name[32]; + char flash_name[32]; + enum msm_sensor_camera_id_t camera_id; + uint16_t slave_addr; + enum i2c_freq_mode_t i2c_freq_mode; + enum msm_camera_i2c_reg_addr_type addr_type; + struct msm_sensor_id_info_t sensor_id_info; + struct msm_sensor_power_setting_array32 power_setting_array; + uint8_t is_init_params_valid; + struct msm_sensor_init_params sensor_init_params; + enum msm_sensor_output_format_t output_format; +}; + +struct msm_camera_csid_lut_params32 { + uint8_t num_cid; + struct msm_camera_csid_vc_cfg vc_cfg_a[MAX_CID]; + compat_uptr_t vc_cfg[MAX_CID]; +}; + +struct msm_camera_csid_params32 { + uint8_t lane_cnt; + uint16_t lane_assign; + uint8_t phy_sel; + uint32_t csi_clk; + struct msm_camera_csid_lut_params32 lut_params; + uint8_t csi_3p_sel; +}; + +struct msm_camera_csi2_params32 { + struct msm_camera_csid_params32 csid_params; + struct msm_camera_csiphy_params csiphy_params; + uint8_t csi_clk_scale_enable; +}; + +struct csid_cfg_data32 { + enum csid_cfg_type_t cfgtype; + union { + uint32_t csid_version; + compat_uptr_t csid_params; + compat_uptr_t csid_testmode_params; + } cfg; +}; + +struct eeprom_read_t32 { + compat_uptr_t dbuffer; + uint32_t num_bytes; +}; + +struct eeprom_write_t32 { + compat_uptr_t dbuffer; + uint32_t num_bytes; +}; + +struct msm_eeprom_info_t32 { + compat_uptr_t power_setting_array; + enum i2c_freq_mode_t i2c_freq_mode; + compat_uptr_t mem_map_array; +}; + +struct msm_eeprom_cfg_data32 { + enum eeprom_cfg_type_t cfgtype; + uint8_t is_supported; + union { + char eeprom_name[MAX_SENSOR_NAME]; + struct eeprom_get_t get_data; + struct eeprom_read_t32 read_data; + struct eeprom_write_t32 write_data; + struct msm_eeprom_info_t32 eeprom_info; + } cfg; +}; + +struct msm_camera_i2c_seq_reg_setting32 { + compat_uptr_t reg_setting; + uint16_t size; + enum msm_camera_i2c_reg_addr_type addr_type; + uint16_t delay; +}; +#endif + +enum msm_sensor_cfg_type_t { + CFG_SET_SLAVE_INFO, + CFG_SLAVE_READ_I2C, + CFG_WRITE_I2C_ARRAY, + CFG_SLAVE_WRITE_I2C_ARRAY, + CFG_WRITE_I2C_SEQ_ARRAY, + CFG_POWER_UP, + CFG_POWER_DOWN, + CFG_SET_STOP_STREAM_SETTING, + CFG_GET_SENSOR_INFO, + CFG_GET_SENSOR_INIT_PARAMS, + CFG_SET_INIT_SETTING, + CFG_SET_RESOLUTION, + CFG_SET_STOP_STREAM, + CFG_SET_START_STREAM, + CFG_SET_SATURATION, + CFG_SET_CONTRAST, + CFG_SET_SHARPNESS, + CFG_SET_ISO, + CFG_SET_EXPOSURE_COMPENSATION, + CFG_SET_ANTIBANDING, + CFG_SET_BESTSHOT_MODE, + CFG_SET_EFFECT, + CFG_SET_WHITE_BALANCE, + CFG_SET_AUTOFOCUS, + CFG_CANCEL_AUTOFOCUS, + CFG_SET_STREAM_TYPE, + CFG_SET_I2C_SYNC_PARAM, + CFG_WRITE_I2C_ARRAY_ASYNC, + CFG_WRITE_I2C_ARRAY_SYNC, + CFG_WRITE_I2C_ARRAY_SYNC_BLOCK, +}; + +enum msm_actuator_cfg_type_t { + CFG_GET_ACTUATOR_INFO, + CFG_SET_ACTUATOR_INFO, + CFG_SET_DEFAULT_FOCUS, + CFG_MOVE_FOCUS, + CFG_SET_POSITION, + CFG_ACTUATOR_POWERDOWN, + CFG_ACTUATOR_POWERUP, + CFG_ACTUATOR_INIT, +}; + +enum msm_ois_cfg_type_t { + CFG_OIS_INIT, + CFG_OIS_POWERDOWN, + CFG_OIS_POWERUP, + CFG_OIS_CONTROL, + CFG_OIS_I2C_WRITE_SEQ_TABLE, +}; + +enum msm_ois_i2c_operation { + MSM_OIS_WRITE = 0, + MSM_OIS_POLL, +}; + +struct reg_settings_ois_t { + uint16_t reg_addr; + enum msm_camera_i2c_reg_addr_type addr_type; + uint32_t reg_data; + enum msm_camera_i2c_data_type data_type; + enum msm_ois_i2c_operation i2c_operation; + uint32_t delay; +#define OIS_REG_DATA_SEQ_MAX 128 + unsigned char reg_data_seq[OIS_REG_DATA_SEQ_MAX]; + uint32_t reg_data_seq_size; +}; + +struct msm_ois_params_t { + uint16_t data_size; + uint16_t setting_size; + uint32_t i2c_addr; + enum i2c_freq_mode_t i2c_freq_mode; + enum msm_camera_i2c_reg_addr_type i2c_addr_type; + enum msm_camera_i2c_data_type i2c_data_type; + struct reg_settings_ois_t *settings; +}; + +struct msm_ois_set_info_t { + struct msm_ois_params_t ois_params; +}; + +struct msm_actuator_move_params_t { + int8_t dir; + int8_t sign_dir; + int16_t dest_step_pos; + int32_t num_steps; + uint16_t curr_lens_pos; + struct damping_params_t *ringing_params; +}; + +struct msm_actuator_tuning_params_t { + int16_t initial_code; + uint16_t pwd_step; + uint16_t region_size; + uint32_t total_steps; + struct region_params_t *region_params; +}; + +struct park_lens_data_t { + uint32_t damping_step; + uint32_t damping_delay; + uint32_t hw_params; + uint32_t max_step; +}; + +struct msm_actuator_params_t { + enum actuator_type act_type; + uint8_t reg_tbl_size; + uint16_t data_size; + uint16_t init_setting_size; + uint32_t i2c_addr; + enum i2c_freq_mode_t i2c_freq_mode; + enum msm_actuator_addr_type i2c_addr_type; + enum msm_actuator_data_type i2c_data_type; + struct msm_actuator_reg_params_t *reg_tbl_params; + struct reg_settings_t *init_settings; + struct park_lens_data_t park_lens; +}; + +struct msm_actuator_set_info_t { + struct msm_actuator_params_t actuator_params; + struct msm_actuator_tuning_params_t af_tuning_params; +}; + +struct msm_actuator_get_info_t { + uint32_t focal_length_num; + uint32_t focal_length_den; + uint32_t f_number_num; + uint32_t f_number_den; + uint32_t f_pix_num; + uint32_t f_pix_den; + uint32_t total_f_dist_num; + uint32_t total_f_dist_den; + uint32_t hor_view_angle_num; + uint32_t hor_view_angle_den; + uint32_t ver_view_angle_num; + uint32_t ver_view_angle_den; +}; + +enum af_camera_name { + ACTUATOR_MAIN_CAM_0, + ACTUATOR_MAIN_CAM_1, + ACTUATOR_MAIN_CAM_2, + ACTUATOR_MAIN_CAM_3, + ACTUATOR_MAIN_CAM_4, + ACTUATOR_MAIN_CAM_5, + ACTUATOR_WEB_CAM_0, + ACTUATOR_WEB_CAM_1, + ACTUATOR_WEB_CAM_2, +}; + +struct msm_ois_cfg_data { + int cfgtype; + union { + struct msm_ois_set_info_t set_info; + struct msm_camera_i2c_seq_reg_setting *settings; + } cfg; +}; + +struct msm_actuator_set_position_t { + uint16_t number_of_steps; + uint32_t hw_params; + uint16_t pos[MAX_NUMBER_OF_STEPS]; + uint16_t delay[MAX_NUMBER_OF_STEPS]; +}; + +struct msm_actuator_cfg_data { + int cfgtype; + uint8_t is_af_supported; + union { + struct msm_actuator_move_params_t move; + struct msm_actuator_set_info_t set_info; + struct msm_actuator_get_info_t get_info; + struct msm_actuator_set_position_t setpos; + enum af_camera_name cam_name; + } cfg; +}; + +enum msm_camera_led_config_t { + MSM_CAMERA_LED_OFF, + MSM_CAMERA_LED_LOW, + MSM_CAMERA_LED_HIGH, + MSM_CAMERA_LED_INIT, + MSM_CAMERA_LED_RELEASE, +}; + +struct msm_camera_led_cfg_t { + enum msm_camera_led_config_t cfgtype; + int32_t torch_current[MAX_LED_TRIGGERS]; + int32_t flash_current[MAX_LED_TRIGGERS]; + int32_t flash_duration[MAX_LED_TRIGGERS]; +}; + +struct msm_flash_init_info_t { + enum msm_flash_driver_type flash_driver_type; + uint32_t slave_addr; + enum i2c_freq_mode_t i2c_freq_mode; + struct msm_sensor_power_setting_array *power_setting_array; + struct msm_camera_i2c_reg_setting_array *settings; +}; + +struct msm_flash_cfg_data_t { + enum msm_flash_cfg_type_t cfg_type; + int32_t flash_current[MAX_LED_TRIGGERS]; + int32_t flash_duration[MAX_LED_TRIGGERS]; + union { + struct msm_flash_init_info_t *flash_init_info; + struct msm_camera_i2c_reg_setting_array *settings; + } cfg; +}; + +/* sensor init structures and enums */ +enum msm_sensor_init_cfg_type_t { + CFG_SINIT_PROBE, + CFG_SINIT_PROBE_DONE, + CFG_SINIT_PROBE_WAIT_DONE, +}; + +struct sensor_init_cfg_data { + enum msm_sensor_init_cfg_type_t cfgtype; + struct msm_sensor_info_t probed_info; + char entity_name[MAX_SENSOR_NAME]; + union { + void *setting; + } cfg; +}; + +#define VIDIOC_MSM_SENSOR_CFG \ + _IOWR('V', BASE_VIDIOC_PRIVATE + 1, struct sensorb_cfg_data) + +#define VIDIOC_MSM_SENSOR_RELEASE \ + _IO('V', BASE_VIDIOC_PRIVATE + 2) + +#define VIDIOC_MSM_SENSOR_GET_SUBDEV_ID \ + _IOWR('V', BASE_VIDIOC_PRIVATE + 3, uint32_t) + +#define VIDIOC_MSM_CSIPHY_IO_CFG \ + _IOWR('V', BASE_VIDIOC_PRIVATE + 4, struct csiphy_cfg_data) + +#define VIDIOC_MSM_CSID_IO_CFG \ + _IOWR('V', BASE_VIDIOC_PRIVATE + 5, struct csid_cfg_data) + +#define VIDIOC_MSM_ACTUATOR_CFG \ + _IOWR('V', BASE_VIDIOC_PRIVATE + 6, struct msm_actuator_cfg_data) + +#define VIDIOC_MSM_FLASH_LED_DATA_CFG \ + _IOWR('V', BASE_VIDIOC_PRIVATE + 7, struct msm_camera_led_cfg_t) + +#define VIDIOC_MSM_EEPROM_CFG \ + _IOWR('V', BASE_VIDIOC_PRIVATE + 8, struct msm_eeprom_cfg_data) + +#define VIDIOC_MSM_SENSOR_GET_AF_STATUS \ + _IOWR('V', BASE_VIDIOC_PRIVATE + 9, uint32_t) + +#define VIDIOC_MSM_SENSOR_INIT_CFG \ + _IOWR('V', BASE_VIDIOC_PRIVATE + 10, struct sensor_init_cfg_data) + +#define VIDIOC_MSM_OIS_CFG \ + _IOWR('V', BASE_VIDIOC_PRIVATE + 11, struct msm_ois_cfg_data) + +#define VIDIOC_MSM_FLASH_CFG \ + _IOWR('V', BASE_VIDIOC_PRIVATE + 13, struct msm_flash_cfg_data_t) + +#ifdef CONFIG_COMPAT +struct msm_camera_i2c_reg_setting32 { + compat_uptr_t reg_setting; + uint16_t size; + enum msm_camera_i2c_reg_addr_type addr_type; + enum msm_camera_i2c_data_type data_type; + uint16_t delay; +}; + +struct msm_camera_i2c_array_write_config32 { + struct msm_camera_i2c_reg_setting32 conf_array; + uint16_t slave_addr; +}; + +struct msm_actuator_tuning_params_t32 { + int16_t initial_code; + uint16_t pwd_step; + uint16_t region_size; + uint32_t total_steps; + compat_uptr_t region_params; +}; + +struct msm_actuator_params_t32 { + enum actuator_type act_type; + uint8_t reg_tbl_size; + uint16_t data_size; + uint16_t init_setting_size; + uint32_t i2c_addr; + enum i2c_freq_mode_t i2c_freq_mode; + enum msm_actuator_addr_type i2c_addr_type; + enum msm_actuator_data_type i2c_data_type; + compat_uptr_t reg_tbl_params; + compat_uptr_t init_settings; + struct park_lens_data_t park_lens; +}; + +struct msm_actuator_set_info_t32 { + struct msm_actuator_params_t32 actuator_params; + struct msm_actuator_tuning_params_t32 af_tuning_params; +}; + +struct sensor_init_cfg_data32 { + enum msm_sensor_init_cfg_type_t cfgtype; + struct msm_sensor_info_t probed_info; + char entity_name[MAX_SENSOR_NAME]; + union { + compat_uptr_t setting; + } cfg; +}; + +struct msm_actuator_move_params_t32 { + int8_t dir; + int8_t sign_dir; + int16_t dest_step_pos; + int32_t num_steps; + uint16_t curr_lens_pos; + compat_uptr_t ringing_params; +}; + +struct msm_actuator_cfg_data32 { + int cfgtype; + uint8_t is_af_supported; + union { + struct msm_actuator_move_params_t32 move; + struct msm_actuator_set_info_t32 set_info; + struct msm_actuator_get_info_t get_info; + struct msm_actuator_set_position_t setpos; + enum af_camera_name cam_name; + } cfg; +}; + +struct csiphy_cfg_data32 { + enum csiphy_cfg_type_t cfgtype; + union { + compat_uptr_t csiphy_params; + compat_uptr_t csi_lane_params; + } cfg; +}; + +struct sensorb_cfg_data32 { + int cfgtype; + union { + struct msm_sensor_info_t sensor_info; + struct msm_sensor_init_params sensor_init_params; + compat_uptr_t setting; + struct msm_sensor_i2c_sync_params sensor_i2c_sync_params; + } cfg; +}; + +struct msm_ois_params_t32 { + uint16_t data_size; + uint16_t setting_size; + uint32_t i2c_addr; + enum i2c_freq_mode_t i2c_freq_mode; + enum msm_camera_i2c_reg_addr_type i2c_addr_type; + enum msm_camera_i2c_data_type i2c_data_type; + compat_uptr_t settings; +}; + +struct msm_ois_set_info_t32 { + struct msm_ois_params_t32 ois_params; +}; + +struct msm_ois_cfg_data32 { + int cfgtype; + union { + struct msm_ois_set_info_t32 set_info; + compat_uptr_t settings; + } cfg; +}; + +struct msm_flash_init_info_t32 { + enum msm_flash_driver_type flash_driver_type; + uint32_t slave_addr; + enum i2c_freq_mode_t i2c_freq_mode; + compat_uptr_t power_setting_array; + compat_uptr_t settings; +}; + +struct msm_flash_cfg_data_t32 { + enum msm_flash_cfg_type_t cfg_type; + int32_t flash_current[MAX_LED_TRIGGERS]; + int32_t flash_duration[MAX_LED_TRIGGERS]; + union { + compat_uptr_t flash_init_info; + compat_uptr_t settings; + } cfg; +}; + +#define VIDIOC_MSM_ACTUATOR_CFG32 \ + _IOWR('V', BASE_VIDIOC_PRIVATE + 6, struct msm_actuator_cfg_data32) + +#define VIDIOC_MSM_SENSOR_INIT_CFG32 \ + _IOWR('V', BASE_VIDIOC_PRIVATE + 10, struct sensor_init_cfg_data32) + +#define VIDIOC_MSM_CSIPHY_IO_CFG32 \ + _IOWR('V', BASE_VIDIOC_PRIVATE + 4, struct csiphy_cfg_data32) + +#define VIDIOC_MSM_SENSOR_CFG32 \ + _IOWR('V', BASE_VIDIOC_PRIVATE + 1, struct sensorb_cfg_data32) + +#define VIDIOC_MSM_EEPROM_CFG32 \ + _IOWR('V', BASE_VIDIOC_PRIVATE + 8, struct msm_eeprom_cfg_data32) + +#define VIDIOC_MSM_OIS_CFG32 \ + _IOWR('V', BASE_VIDIOC_PRIVATE + 11, struct msm_ois_cfg_data32) + +#define VIDIOC_MSM_CSID_IO_CFG32 \ + _IOWR('V', BASE_VIDIOC_PRIVATE + 5, struct csid_cfg_data32) + +#define VIDIOC_MSM_FLASH_CFG32 \ + _IOWR('V', BASE_VIDIOC_PRIVATE + 13, struct msm_flash_cfg_data_t32) +#endif + +#endif /* __LINUX_MSM_CAM_SENSOR_H */ diff --git a/selfdrive/camerad/include/msm_camsensor_sdk.h b/selfdrive/camerad/include/msm_camsensor_sdk.h new file mode 100644 index 000000000..62a4da253 --- /dev/null +++ b/selfdrive/camerad/include/msm_camsensor_sdk.h @@ -0,0 +1,386 @@ +#ifndef __LINUX_MSM_CAMSENSOR_SDK_H +#define __LINUX_MSM_CAMSENSOR_SDK_H + +#define KVERSION 0x1 + +#define MAX_POWER_CONFIG 12 +#define GPIO_OUT_LOW (0 << 1) +#define GPIO_OUT_HIGH (1 << 1) +#define CSI_EMBED_DATA 0x12 +#define CSI_RESERVED_DATA_0 0x13 +#define CSI_YUV422_8 0x1E +#define CSI_RAW8 0x2A +#define CSI_RAW10 0x2B +#define CSI_RAW12 0x2C +#define CSI_DECODE_6BIT 0 +#define CSI_DECODE_8BIT 1 +#define CSI_DECODE_10BIT 2 +#define CSI_DECODE_12BIT 3 +#define CSI_DECODE_DPCM_10_8_10 5 +#define MAX_CID 16 +#define I2C_SEQ_REG_DATA_MAX 1024 +#define I2C_REG_DATA_MAX (8*1024) + +#define MSM_V4L2_PIX_FMT_META v4l2_fourcc('M', 'E', 'T', 'A') /* META */ +#define MSM_V4L2_PIX_FMT_SBGGR14 v4l2_fourcc('B', 'G', '1', '4') + /* 14 BGBG.. GRGR.. */ +#define MSM_V4L2_PIX_FMT_SGBRG14 v4l2_fourcc('G', 'B', '1', '4') + /* 14 GBGB.. RGRG.. */ +#define MSM_V4L2_PIX_FMT_SGRBG14 v4l2_fourcc('B', 'A', '1', '4') + /* 14 GRGR.. BGBG.. */ +#define MSM_V4L2_PIX_FMT_SRGGB14 v4l2_fourcc('R', 'G', '1', '4') + /* 14 RGRG.. GBGB.. */ + +#define MAX_ACTUATOR_REG_TBL_SIZE 8 +#define MAX_ACTUATOR_REGION 5 +#define NUM_ACTUATOR_DIR 2 +#define MAX_ACTUATOR_SCENARIO 8 +#define MAX_ACT_MOD_NAME_SIZE 32 +#define MAX_ACT_NAME_SIZE 32 +#define MAX_ACTUATOR_INIT_SET 120 +#define MAX_I2C_REG_SET 12 + +#define MAX_LED_TRIGGERS 3 + +#define MSM_EEPROM_MEMORY_MAP_MAX_SIZE 80 +#define MSM_EEPROM_MAX_MEM_MAP_CNT 8 + +enum msm_sensor_camera_id_t { + CAMERA_0, + CAMERA_1, + CAMERA_2, + CAMERA_3, + MAX_CAMERAS, +}; + +enum i2c_freq_mode_t { + I2C_STANDARD_MODE, + I2C_FAST_MODE, + I2C_CUSTOM_MODE, + I2C_FAST_PLUS_MODE, + I2C_MAX_MODES, +}; + +enum camb_position_t { + BACK_CAMERA_B, + FRONT_CAMERA_B, + AUX_CAMERA_B = 0x100, + INVALID_CAMERA_B, +}; + +enum msm_sensor_power_seq_type_t { + SENSOR_CLK, + SENSOR_GPIO, + SENSOR_VREG, + SENSOR_I2C_MUX, + SENSOR_I2C, +}; + +enum msm_camera_i2c_reg_addr_type { + MSM_CAMERA_I2C_BYTE_ADDR = 1, + MSM_CAMERA_I2C_WORD_ADDR, + MSM_CAMERA_I2C_3B_ADDR, + MSM_CAMERA_I2C_ADDR_TYPE_MAX, +}; + +enum msm_camera_i2c_data_type { + MSM_CAMERA_I2C_BYTE_DATA = 1, + MSM_CAMERA_I2C_WORD_DATA, + MSM_CAMERA_I2C_DWORD_DATA, + MSM_CAMERA_I2C_SET_BYTE_MASK, + MSM_CAMERA_I2C_UNSET_BYTE_MASK, + MSM_CAMERA_I2C_SET_WORD_MASK, + MSM_CAMERA_I2C_UNSET_WORD_MASK, + MSM_CAMERA_I2C_SET_BYTE_WRITE_MASK_DATA, + MSM_CAMERA_I2C_SEQ_DATA, + MSM_CAMERA_I2C_DATA_TYPE_MAX, +}; + +enum msm_sensor_power_seq_gpio_t { + SENSOR_GPIO_RESET, + SENSOR_GPIO_STANDBY, + SENSOR_GPIO_AF_PWDM, + SENSOR_GPIO_VIO, + SENSOR_GPIO_VANA, + SENSOR_GPIO_VDIG, + SENSOR_GPIO_VAF, + SENSOR_GPIO_FL_EN, + SENSOR_GPIO_FL_NOW, + SENSOR_GPIO_FL_RESET, + SENSOR_GPIO_CUSTOM1, + SENSOR_GPIO_CUSTOM2, + SENSOR_GPIO_MAX, +}; + +enum msm_camera_vreg_name_t { + CAM_VDIG, + CAM_VIO, + CAM_VANA, + CAM_VAF, + CAM_V_CUSTOM1, + CAM_V_CUSTOM2, + CAM_VREG_MAX, +}; + +enum msm_sensor_clk_type_t { + SENSOR_CAM_MCLK, + SENSOR_CAM_CLK, + SENSOR_CAM_CLK_MAX, +}; + +enum camerab_mode_t { + CAMERA_MODE_2D_B = (1<<0), + CAMERA_MODE_3D_B = (1<<1), + CAMERA_MODE_INVALID = (1<<2), +}; + +enum msm_actuator_data_type { + MSM_ACTUATOR_BYTE_DATA = 1, + MSM_ACTUATOR_WORD_DATA, +}; + +enum msm_actuator_addr_type { + MSM_ACTUATOR_BYTE_ADDR = 1, + MSM_ACTUATOR_WORD_ADDR, +}; + +enum msm_actuator_write_type { + MSM_ACTUATOR_WRITE_HW_DAMP, + MSM_ACTUATOR_WRITE_DAC, + MSM_ACTUATOR_WRITE, + MSM_ACTUATOR_WRITE_DIR_REG, + MSM_ACTUATOR_POLL, + MSM_ACTUATOR_READ_WRITE, +}; + +enum msm_actuator_i2c_operation { + MSM_ACT_WRITE = 0, + MSM_ACT_POLL, +}; + +enum actuator_type { + ACTUATOR_VCM, + ACTUATOR_PIEZO, + ACTUATOR_HVCM, + ACTUATOR_BIVCM, +}; + +enum msm_flash_driver_type { + FLASH_DRIVER_PMIC, + FLASH_DRIVER_I2C, + FLASH_DRIVER_GPIO, + FLASH_DRIVER_DEFAULT +}; + +enum msm_flash_cfg_type_t { + CFG_FLASH_INIT, + CFG_FLASH_RELEASE, + CFG_FLASH_OFF, + CFG_FLASH_LOW, + CFG_FLASH_HIGH, +}; + +enum msm_sensor_output_format_t { + MSM_SENSOR_BAYER, + MSM_SENSOR_YCBCR, + MSM_SENSOR_META, +}; + +struct msm_sensor_power_setting { + enum msm_sensor_power_seq_type_t seq_type; + unsigned short seq_val; + long config_val; + unsigned short delay; + void *data[10]; +}; + +struct msm_sensor_power_setting_array { + struct msm_sensor_power_setting power_setting_a[MAX_POWER_CONFIG]; + struct msm_sensor_power_setting *power_setting; + unsigned short size; + struct msm_sensor_power_setting power_down_setting_a[MAX_POWER_CONFIG]; + struct msm_sensor_power_setting *power_down_setting; + unsigned short size_down; +}; + +enum msm_camera_i2c_operation { + MSM_CAM_WRITE = 0, + MSM_CAM_POLL, + MSM_CAM_READ, +}; + +struct msm_sensor_i2c_sync_params { + unsigned int cid; + int csid; + unsigned short line; + unsigned short delay; +}; + +struct msm_camera_reg_settings_t { + uint16_t reg_addr; + enum msm_camera_i2c_reg_addr_type addr_type; + uint16_t reg_data; + enum msm_camera_i2c_data_type data_type; + enum msm_camera_i2c_operation i2c_operation; + uint16_t delay; +}; + +struct msm_eeprom_mem_map_t { + int slave_addr; + struct msm_camera_reg_settings_t + mem_settings[MSM_EEPROM_MEMORY_MAP_MAX_SIZE]; + int memory_map_size; +}; + +struct msm_eeprom_memory_map_array { + struct msm_eeprom_mem_map_t memory_map[MSM_EEPROM_MAX_MEM_MAP_CNT]; + uint32_t msm_size_of_max_mappings; +}; + +struct msm_sensor_init_params { + /* mask of modes supported: 2D, 3D */ + int modes_supported; + /* sensor position: front, back */ + enum camb_position_t position; + /* sensor mount angle */ + unsigned int sensor_mount_angle; +}; + +struct msm_sensor_id_info_t { + unsigned short sensor_id_reg_addr; + unsigned short sensor_id; + unsigned short sensor_id_mask; + // added in LeEco + unsigned char module_id; + unsigned char vcm_id; +}; + +struct msm_camera_sensor_slave_info { + char sensor_name[32]; + char eeprom_name[32]; + char actuator_name[32]; + char ois_name[32]; + char flash_name[32]; + enum msm_sensor_camera_id_t camera_id; + unsigned short slave_addr; + enum i2c_freq_mode_t i2c_freq_mode; + enum msm_camera_i2c_reg_addr_type addr_type; + struct msm_sensor_id_info_t sensor_id_info; + struct msm_sensor_power_setting_array power_setting_array; + unsigned char is_init_params_valid; + struct msm_sensor_init_params sensor_init_params; + enum msm_sensor_output_format_t output_format; +}; + +struct msm_camera_i2c_reg_array { + unsigned short reg_addr; + unsigned short reg_data; + unsigned int delay; +}; + +struct msm_camera_i2c_reg_setting { + struct msm_camera_i2c_reg_array *reg_setting; + unsigned short size; + enum msm_camera_i2c_reg_addr_type addr_type; + enum msm_camera_i2c_data_type data_type; + unsigned short delay; +}; + +struct msm_camera_csid_vc_cfg { + unsigned char cid; + unsigned char dt; + unsigned char decode_format; +}; + +struct msm_camera_csid_lut_params { + unsigned char num_cid; + struct msm_camera_csid_vc_cfg vc_cfg_a[MAX_CID]; + struct msm_camera_csid_vc_cfg *vc_cfg[MAX_CID]; +}; + +struct msm_camera_csid_params { + unsigned char lane_cnt; + unsigned short lane_assign; + unsigned char phy_sel; + unsigned int csi_clk; + struct msm_camera_csid_lut_params lut_params; + unsigned char csi_3p_sel; +}; + +struct msm_camera_csid_testmode_parms { + unsigned int num_bytes_per_line; + unsigned int num_lines; + unsigned int h_blanking_count; + unsigned int v_blanking_count; + unsigned int payload_mode; +}; + +struct msm_camera_csiphy_params { + unsigned char lane_cnt; + unsigned char settle_cnt; + unsigned short lane_mask; + unsigned char combo_mode; + unsigned char csid_core; + unsigned int csiphy_clk; + unsigned char csi_3phase; +}; + +struct msm_camera_i2c_seq_reg_array { + unsigned short reg_addr; + unsigned char reg_data[I2C_SEQ_REG_DATA_MAX]; + unsigned short reg_data_size; +}; + +struct msm_camera_i2c_seq_reg_setting { + struct msm_camera_i2c_seq_reg_array *reg_setting; + unsigned short size; + enum msm_camera_i2c_reg_addr_type addr_type; + unsigned short delay; +}; + +struct msm_actuator_reg_params_t { + enum msm_actuator_write_type reg_write_type; + unsigned int hw_mask; + unsigned short reg_addr; + unsigned short hw_shift; + unsigned short data_shift; + unsigned short data_type; + unsigned short addr_type; + unsigned short reg_data; + unsigned short delay; +}; + + +struct damping_params_t { + unsigned int damping_step; + unsigned int damping_delay; + unsigned int hw_params; +}; + +struct region_params_t { + /* [0] = ForwardDirection Macro boundary + [1] = ReverseDirection Inf boundary + */ + unsigned short step_bound[2]; + unsigned short code_per_step; + /* qvalue for converting float type numbers to integer format */ + unsigned int qvalue; +}; + +struct reg_settings_t { + unsigned short reg_addr; + enum msm_actuator_addr_type addr_type; + unsigned short reg_data; + enum msm_actuator_data_type data_type; + enum msm_actuator_i2c_operation i2c_operation; + unsigned int delay; +}; + +struct msm_camera_i2c_reg_setting_array { + struct msm_camera_i2c_reg_array reg_setting_a[MAX_I2C_REG_SET]; + unsigned short size; + enum msm_camera_i2c_reg_addr_type addr_type; + enum msm_camera_i2c_data_type data_type; + unsigned short delay; +}; +#endif /* __LINUX_MSM_CAM_SENSOR_H */ diff --git a/selfdrive/camerad/include/msmb_camera.h b/selfdrive/camerad/include/msmb_camera.h new file mode 100644 index 000000000..f4c4b3e18 --- /dev/null +++ b/selfdrive/camerad/include/msmb_camera.h @@ -0,0 +1,220 @@ +#ifndef __LINUX_MSMB_CAMERA_H +#define __LINUX_MSMB_CAMERA_H + +#include +#include +#include + +#define MSM_CAM_LOGSYNC_FILE_NAME "logsync" +#define MSM_CAM_LOGSYNC_FILE_BASEDIR "camera" + +#define MSM_CAM_V4L2_IOCTL_NOTIFY \ + _IOW('V', BASE_VIDIOC_PRIVATE + 30, struct msm_v4l2_event_data) + +#define MSM_CAM_V4L2_IOCTL_NOTIFY_META \ + _IOW('V', BASE_VIDIOC_PRIVATE + 31, struct msm_v4l2_event_data) + +#define MSM_CAM_V4L2_IOCTL_CMD_ACK \ + _IOW('V', BASE_VIDIOC_PRIVATE + 32, struct msm_v4l2_event_data) + +#define MSM_CAM_V4L2_IOCTL_NOTIFY_ERROR \ + _IOW('V', BASE_VIDIOC_PRIVATE + 33, struct msm_v4l2_event_data) + +#define MSM_CAM_V4L2_IOCTL_NOTIFY_DEBUG \ + _IOW('V', BASE_VIDIOC_PRIVATE + 34, struct msm_v4l2_event_data) + +#ifdef CONFIG_COMPAT +#define MSM_CAM_V4L2_IOCTL_NOTIFY32 \ + _IOW('V', BASE_VIDIOC_PRIVATE + 30, struct v4l2_event32) + +#define MSM_CAM_V4L2_IOCTL_NOTIFY_META32 \ + _IOW('V', BASE_VIDIOC_PRIVATE + 31, struct v4l2_event32) + +#define MSM_CAM_V4L2_IOCTL_CMD_ACK32 \ + _IOW('V', BASE_VIDIOC_PRIVATE + 32, struct v4l2_event32) + +#define MSM_CAM_V4L2_IOCTL_NOTIFY_ERROR32 \ + _IOW('V', BASE_VIDIOC_PRIVATE + 33, struct v4l2_event32) + +#define MSM_CAM_V4L2_IOCTL_NOTIFY_DEBUG32 \ + _IOW('V', BASE_VIDIOC_PRIVATE + 34, struct v4l2_event32) + +#endif + +#define QCAMERA_DEVICE_GROUP_ID 1 +#define QCAMERA_VNODE_GROUP_ID 2 +#define MSM_CAMERA_NAME "msm_camera" +#define MSM_CONFIGURATION_NAME "msm_config" + +#define MSM_CAMERA_SUBDEV_CSIPHY 0 +#define MSM_CAMERA_SUBDEV_CSID 1 +#define MSM_CAMERA_SUBDEV_ISPIF 2 +#define MSM_CAMERA_SUBDEV_VFE 3 +#define MSM_CAMERA_SUBDEV_AXI 4 +#define MSM_CAMERA_SUBDEV_VPE 5 +#define MSM_CAMERA_SUBDEV_SENSOR 6 +#define MSM_CAMERA_SUBDEV_ACTUATOR 7 +#define MSM_CAMERA_SUBDEV_EEPROM 8 +#define MSM_CAMERA_SUBDEV_CPP 9 +#define MSM_CAMERA_SUBDEV_CCI 10 +#define MSM_CAMERA_SUBDEV_LED_FLASH 11 +#define MSM_CAMERA_SUBDEV_STROBE_FLASH 12 +#define MSM_CAMERA_SUBDEV_BUF_MNGR 13 +#define MSM_CAMERA_SUBDEV_SENSOR_INIT 14 +#define MSM_CAMERA_SUBDEV_OIS 15 +#define MSM_CAMERA_SUBDEV_FLASH 16 +#define MSM_CAMERA_SUBDEV_EXT 17 + +#define MSM_MAX_CAMERA_SENSORS 5 + +/* The below macro is defined to put an upper limit on maximum + * number of buffer requested per stream. In case of extremely + * large value for number of buffer due to data structure corruption + * we return error to avoid integer overflow. Group processing + * can have max of 9 groups of 8 bufs each. This value may be + * configured in future*/ +#define MSM_CAMERA_MAX_STREAM_BUF 72 + +/* Max batch size of processing */ +#define MSM_CAMERA_MAX_USER_BUFF_CNT 16 + +/* featur base */ +#define MSM_CAMERA_FEATURE_BASE 0x00010000 +#define MSM_CAMERA_FEATURE_SHUTDOWN (MSM_CAMERA_FEATURE_BASE + 1) + +#define MSM_CAMERA_STATUS_BASE 0x00020000 +#define MSM_CAMERA_STATUS_FAIL (MSM_CAMERA_STATUS_BASE + 1) +#define MSM_CAMERA_STATUS_SUCCESS (MSM_CAMERA_STATUS_BASE + 2) + +/* event type */ +#define MSM_CAMERA_V4L2_EVENT_TYPE (V4L2_EVENT_PRIVATE_START + 0x00002000) + +/* event id */ +#define MSM_CAMERA_EVENT_MIN 0 +#define MSM_CAMERA_NEW_SESSION (MSM_CAMERA_EVENT_MIN + 1) +#define MSM_CAMERA_DEL_SESSION (MSM_CAMERA_EVENT_MIN + 2) +#define MSM_CAMERA_SET_PARM (MSM_CAMERA_EVENT_MIN + 3) +#define MSM_CAMERA_GET_PARM (MSM_CAMERA_EVENT_MIN + 4) +#define MSM_CAMERA_MAPPING_CFG (MSM_CAMERA_EVENT_MIN + 5) +#define MSM_CAMERA_MAPPING_SES (MSM_CAMERA_EVENT_MIN + 6) +#define MSM_CAMERA_MSM_NOTIFY (MSM_CAMERA_EVENT_MIN + 7) +#define MSM_CAMERA_EVENT_MAX (MSM_CAMERA_EVENT_MIN + 8) + +/* data.command */ +#define MSM_CAMERA_PRIV_S_CROP (V4L2_CID_PRIVATE_BASE + 1) +#define MSM_CAMERA_PRIV_G_CROP (V4L2_CID_PRIVATE_BASE + 2) +#define MSM_CAMERA_PRIV_G_FMT (V4L2_CID_PRIVATE_BASE + 3) +#define MSM_CAMERA_PRIV_S_FMT (V4L2_CID_PRIVATE_BASE + 4) +#define MSM_CAMERA_PRIV_TRY_FMT (V4L2_CID_PRIVATE_BASE + 5) +#define MSM_CAMERA_PRIV_METADATA (V4L2_CID_PRIVATE_BASE + 6) +#define MSM_CAMERA_PRIV_QUERY_CAP (V4L2_CID_PRIVATE_BASE + 7) +#define MSM_CAMERA_PRIV_STREAM_ON (V4L2_CID_PRIVATE_BASE + 8) +#define MSM_CAMERA_PRIV_STREAM_OFF (V4L2_CID_PRIVATE_BASE + 9) +#define MSM_CAMERA_PRIV_NEW_STREAM (V4L2_CID_PRIVATE_BASE + 10) +#define MSM_CAMERA_PRIV_DEL_STREAM (V4L2_CID_PRIVATE_BASE + 11) +#define MSM_CAMERA_PRIV_SHUTDOWN (V4L2_CID_PRIVATE_BASE + 12) +#define MSM_CAMERA_PRIV_STREAM_INFO_SYNC \ + (V4L2_CID_PRIVATE_BASE + 13) +#define MSM_CAMERA_PRIV_G_SESSION_ID (V4L2_CID_PRIVATE_BASE + 14) +#define MSM_CAMERA_PRIV_CMD_MAX 20 + +/* data.status - success */ +#define MSM_CAMERA_CMD_SUCESS 0x00000001 +#define MSM_CAMERA_BUF_MAP_SUCESS 0x00000002 + +/* data.status - error */ +#define MSM_CAMERA_ERR_EVT_BASE 0x00010000 +#define MSM_CAMERA_ERR_CMD_FAIL (MSM_CAMERA_ERR_EVT_BASE + 1) +#define MSM_CAMERA_ERR_MAPPING (MSM_CAMERA_ERR_EVT_BASE + 2) +#define MSM_CAMERA_ERR_DEVICE_BUSY (MSM_CAMERA_ERR_EVT_BASE + 3) + +/* The msm_v4l2_event_data structure should match the + * v4l2_event.u.data field. + * should not exceed 16 elements */ +struct msm_v4l2_event_data { + /*word 0*/ + unsigned int command; + /*word 1*/ + unsigned int status; + /*word 2*/ + unsigned int session_id; + /*word 3*/ + unsigned int stream_id; + /*word 4*/ + unsigned int map_op; + /*word 5*/ + unsigned int map_buf_idx; + /*word 6*/ + unsigned int notify; + /*word 7*/ + unsigned int arg_value; + /*word 8*/ + unsigned int ret_value; + /*word 9*/ + unsigned int v4l2_event_type; + /*word 10*/ + unsigned int v4l2_event_id; + /*word 11*/ + unsigned int handle; + /*word 12*/ + unsigned int nop6; + /*word 13*/ + unsigned int nop7; + /*word 14*/ + unsigned int nop8; + /*word 15*/ + unsigned int nop9; +}; + +/* map to v4l2_format.fmt.raw_data */ +struct msm_v4l2_format_data { + enum v4l2_buf_type type; + unsigned int width; + unsigned int height; + unsigned int pixelformat; /* FOURCC */ + unsigned char num_planes; + unsigned int plane_sizes[VIDEO_MAX_PLANES]; +}; + +/* MSM Four-character-code (FOURCC) */ +#define msm_v4l2_fourcc(a, b, c, d)\ + ((__u32)(a) | ((__u32)(b) << 8) | ((__u32)(c) << 16) |\ + ((__u32)(d) << 24)) + +/* Composite stats */ +#define MSM_V4L2_PIX_FMT_STATS_COMB v4l2_fourcc('S', 'T', 'C', 'M') +/* AEC stats */ +#define MSM_V4L2_PIX_FMT_STATS_AE v4l2_fourcc('S', 'T', 'A', 'E') +/* AF stats */ +#define MSM_V4L2_PIX_FMT_STATS_AF v4l2_fourcc('S', 'T', 'A', 'F') +/* AWB stats */ +#define MSM_V4L2_PIX_FMT_STATS_AWB v4l2_fourcc('S', 'T', 'W', 'B') +/* IHIST stats */ +#define MSM_V4L2_PIX_FMT_STATS_IHST v4l2_fourcc('I', 'H', 'S', 'T') +/* Column count stats */ +#define MSM_V4L2_PIX_FMT_STATS_CS v4l2_fourcc('S', 'T', 'C', 'S') +/* Row count stats */ +#define MSM_V4L2_PIX_FMT_STATS_RS v4l2_fourcc('S', 'T', 'R', 'S') +/* Bayer Grid stats */ +#define MSM_V4L2_PIX_FMT_STATS_BG v4l2_fourcc('S', 'T', 'B', 'G') +/* Bayer focus stats */ +#define MSM_V4L2_PIX_FMT_STATS_BF v4l2_fourcc('S', 'T', 'B', 'F') +/* Bayer hist stats */ +#define MSM_V4L2_PIX_FMT_STATS_BHST v4l2_fourcc('B', 'H', 'S', 'T') + +enum smmu_attach_mode { + NON_SECURE_MODE = 0x01, + SECURE_MODE = 0x02, + MAX_PROTECTION_MODE = 0x03, +}; + +struct msm_camera_smmu_attach_type { + enum smmu_attach_mode attach; +}; + +struct msm_camera_user_buf_cont_t { + unsigned int buf_cnt; + unsigned int buf_idx[MSM_CAMERA_MAX_USER_BUFF_CNT]; +}; + +#endif /* __LINUX_MSMB_CAMERA_H */ diff --git a/selfdrive/camerad/include/msmb_isp.h b/selfdrive/camerad/include/msmb_isp.h new file mode 100644 index 000000000..35589e050 --- /dev/null +++ b/selfdrive/camerad/include/msmb_isp.h @@ -0,0 +1,880 @@ +#ifndef __MSMB_ISP__ +#define __MSMB_ISP__ + +#include + +#define MAX_PLANES_PER_STREAM 3 +#define MAX_NUM_STREAM 7 + +#define ISP_VERSION_47 47 +#define ISP_VERSION_46 46 +#define ISP_VERSION_44 44 +#define ISP_VERSION_40 40 +#define ISP_VERSION_32 32 +#define ISP_NATIVE_BUF_BIT (0x10000 << 0) +#define ISP0_BIT (0x10000 << 1) +#define ISP1_BIT (0x10000 << 2) +#define ISP_META_CHANNEL_BIT (0x10000 << 3) +#define ISP_SCRATCH_BUF_BIT (0x10000 << 4) +#define ISP_OFFLINE_STATS_BIT (0x10000 << 5) +#define ISP_STATS_STREAM_BIT 0x80000000 + +struct msm_vfe_cfg_cmd_list; + +enum ISP_START_PIXEL_PATTERN { + ISP_BAYER_RGRGRG, + ISP_BAYER_GRGRGR, + ISP_BAYER_BGBGBG, + ISP_BAYER_GBGBGB, + ISP_YUV_YCbYCr, + ISP_YUV_YCrYCb, + ISP_YUV_CbYCrY, + ISP_YUV_CrYCbY, + ISP_PIX_PATTERN_MAX +}; + +enum msm_vfe_plane_fmt { + Y_PLANE, + CB_PLANE, + CR_PLANE, + CRCB_PLANE, + CBCR_PLANE, + VFE_PLANE_FMT_MAX +}; + +enum msm_vfe_input_src { + VFE_PIX_0, + VFE_RAW_0, + VFE_RAW_1, + VFE_RAW_2, + VFE_SRC_MAX, +}; + +enum msm_vfe_axi_stream_src { + PIX_ENCODER, + PIX_VIEWFINDER, + PIX_VIDEO, + CAMIF_RAW, + IDEAL_RAW, + RDI_INTF_0, + RDI_INTF_1, + RDI_INTF_2, + VFE_AXI_SRC_MAX +}; + +enum msm_vfe_frame_skip_pattern { + NO_SKIP, + EVERY_2FRAME, + EVERY_3FRAME, + EVERY_4FRAME, + EVERY_5FRAME, + EVERY_6FRAME, + EVERY_7FRAME, + EVERY_8FRAME, + EVERY_16FRAME, + EVERY_32FRAME, + SKIP_ALL, + SKIP_RANGE, + MAX_SKIP, +}; + +/* + * Define an unused period. When this period is set it means that the stream is + * stopped(i.e the pattern is 0). We don't track the current pattern, just the + * period defines what the pattern is, if period is this then pattern is 0 else + * pattern is 1 + */ +#define MSM_VFE_STREAM_STOP_PERIOD 15 + +enum msm_isp_stats_type { + MSM_ISP_STATS_AEC, /* legacy based AEC */ + MSM_ISP_STATS_AF, /* legacy based AF */ + MSM_ISP_STATS_AWB, /* legacy based AWB */ + MSM_ISP_STATS_RS, /* legacy based RS */ + MSM_ISP_STATS_CS, /* legacy based CS */ + MSM_ISP_STATS_IHIST, /* legacy based HIST */ + MSM_ISP_STATS_SKIN, /* legacy based SKIN */ + MSM_ISP_STATS_BG, /* Bayer Grids */ + MSM_ISP_STATS_BF, /* Bayer Focus */ + MSM_ISP_STATS_BE, /* Bayer Exposure*/ + MSM_ISP_STATS_BHIST, /* Bayer Hist */ + MSM_ISP_STATS_BF_SCALE, /* Bayer Focus scale */ + MSM_ISP_STATS_HDR_BE, /* HDR Bayer Exposure */ + MSM_ISP_STATS_HDR_BHIST, /* HDR Bayer Hist */ + MSM_ISP_STATS_AEC_BG, /* AEC BG */ + MSM_ISP_STATS_MAX /* MAX */ +}; + +/* + * @stats_type_mask: Stats type mask (enum msm_isp_stats_type). + * @stream_src_mask: Stream src mask (enum msm_vfe_axi_stream_src) + * @skip_mode: skip pattern, if skip mode is range only then min/max is used + * @min_frame_id: minimum frame id (valid only if skip_mode = RANGE) + * @max_frame_id: maximum frame id (valid only if skip_mode = RANGE) +*/ +struct msm_isp_sw_framskip { + uint32_t stats_type_mask; + uint32_t stream_src_mask; + enum msm_vfe_frame_skip_pattern skip_mode; + uint32_t min_frame_id; + uint32_t max_frame_id; +}; + +enum msm_vfe_testgen_color_pattern { + COLOR_BAR_8_COLOR, + UNICOLOR_WHITE, + UNICOLOR_YELLOW, + UNICOLOR_CYAN, + UNICOLOR_GREEN, + UNICOLOR_MAGENTA, + UNICOLOR_RED, + UNICOLOR_BLUE, + UNICOLOR_BLACK, + MAX_COLOR, +}; + +enum msm_vfe_camif_input { + CAMIF_DISABLED, + CAMIF_PAD_REG_INPUT, + CAMIF_MIDDI_INPUT, + CAMIF_MIPI_INPUT, +}; + +struct msm_vfe_fetch_engine_cfg { + uint32_t input_format; + uint32_t buf_width; + uint32_t buf_height; + uint32_t fetch_width; + uint32_t fetch_height; + uint32_t x_offset; + uint32_t y_offset; + uint32_t buf_stride; +}; + +enum msm_vfe_camif_output_format { + CAMIF_QCOM_RAW, + CAMIF_MIPI_RAW, + CAMIF_PLAIN_8, + CAMIF_PLAIN_16, + CAMIF_MAX_FORMAT, +}; + +/* + * Camif output general configuration + */ +struct msm_vfe_camif_subsample_cfg { + uint32_t irq_subsample_period; + uint32_t irq_subsample_pattern; + uint32_t sof_counter_step; + uint32_t pixel_skip; + uint32_t line_skip; + uint32_t first_line; + uint32_t last_line; + uint32_t first_pixel; + uint32_t last_pixel; + enum msm_vfe_camif_output_format output_format; +}; + +/* + * Camif frame and window configuration + */ +struct msm_vfe_camif_cfg { + uint32_t lines_per_frame; + uint32_t pixels_per_line; + uint32_t first_pixel; + uint32_t last_pixel; + uint32_t first_line; + uint32_t last_line; + uint32_t epoch_line0; + uint32_t epoch_line1; + uint32_t is_split; + enum msm_vfe_camif_input camif_input; + struct msm_vfe_camif_subsample_cfg subsample_cfg; +}; + +struct msm_vfe_testgen_cfg { + uint32_t lines_per_frame; + uint32_t pixels_per_line; + uint32_t v_blank; + uint32_t h_blank; + enum ISP_START_PIXEL_PATTERN pixel_bayer_pattern; + uint32_t rotate_period; + enum msm_vfe_testgen_color_pattern color_bar_pattern; + uint32_t burst_num_frame; +}; + +enum msm_vfe_inputmux { + CAMIF, + TESTGEN, + EXTERNAL_READ, +}; + +enum msm_vfe_stats_composite_group { + STATS_COMPOSITE_GRP_NONE, + STATS_COMPOSITE_GRP_1, + STATS_COMPOSITE_GRP_2, + STATS_COMPOSITE_GRP_MAX, +}; + +enum msm_vfe_hvx_streaming_cmd { + HVX_DISABLE, + HVX_ONE_WAY, + HVX_ROUND_TRIP +}; + +struct msm_vfe_pix_cfg { + struct msm_vfe_camif_cfg camif_cfg; + struct msm_vfe_testgen_cfg testgen_cfg; + struct msm_vfe_fetch_engine_cfg fetch_engine_cfg; + enum msm_vfe_inputmux input_mux; + enum ISP_START_PIXEL_PATTERN pixel_pattern; + uint32_t input_format; + enum msm_vfe_hvx_streaming_cmd hvx_cmd; + uint32_t is_split; +}; + +struct msm_vfe_rdi_cfg { + uint8_t cid; + uint8_t frame_based; +}; + +struct msm_vfe_input_cfg { + union { + struct msm_vfe_pix_cfg pix_cfg; + struct msm_vfe_rdi_cfg rdi_cfg; + } d; + enum msm_vfe_input_src input_src; + uint32_t input_pix_clk; +}; + +struct msm_vfe_fetch_eng_start { + uint32_t session_id; + uint32_t stream_id; + uint32_t buf_idx; + uint8_t offline_mode; + uint32_t fd; + uint32_t buf_addr; + uint32_t frame_id; +}; + +struct msm_vfe_axi_plane_cfg { + uint32_t output_width; /*Include padding*/ + uint32_t output_height; + uint32_t output_stride; + uint32_t output_scan_lines; + uint32_t output_plane_format; /*Y/Cb/Cr/CbCr*/ + uint32_t plane_addr_offset; + uint8_t csid_src; /*RDI 0-2*/ + uint8_t rdi_cid;/*CID 1-16*/ +}; + +enum msm_stream_memory_input_t { + MEMORY_INPUT_DISABLED, + MEMORY_INPUT_ENABLED +}; + +struct msm_vfe_axi_stream_request_cmd { + uint32_t session_id; + uint32_t stream_id; + uint32_t vt_enable; + uint32_t output_format;/*Planar/RAW/Misc*/ + enum msm_vfe_axi_stream_src stream_src; /*CAMIF/IDEAL/RDIs*/ + struct msm_vfe_axi_plane_cfg plane_cfg[MAX_PLANES_PER_STREAM]; + + uint32_t burst_count; + uint32_t hfr_mode; + uint8_t frame_base; + + uint32_t init_frame_drop; /*MAX 31 Frames*/ + enum msm_vfe_frame_skip_pattern frame_skip_pattern; + uint8_t buf_divert; /* if TRUE no vb2 buf done. */ + /*Return values*/ + uint32_t axi_stream_handle; + uint32_t controllable_output; + uint32_t burst_len; + /* Flag indicating memory input stream */ + enum msm_stream_memory_input_t memory_input; +}; + +struct msm_vfe_axi_stream_release_cmd { + uint32_t stream_handle; +}; + +enum msm_vfe_axi_stream_cmd { + STOP_STREAM, + START_STREAM, + STOP_IMMEDIATELY, +}; + +struct msm_vfe_axi_stream_cfg_cmd { + uint8_t num_streams; + uint32_t stream_handle[VFE_AXI_SRC_MAX]; + enum msm_vfe_axi_stream_cmd cmd; + uint8_t sync_frame_id_src; +}; + +enum msm_vfe_axi_stream_update_type { + ENABLE_STREAM_BUF_DIVERT, + DISABLE_STREAM_BUF_DIVERT, + UPDATE_STREAM_FRAMEDROP_PATTERN, + UPDATE_STREAM_STATS_FRAMEDROP_PATTERN, + UPDATE_STREAM_AXI_CONFIG, + UPDATE_STREAM_REQUEST_FRAMES, + UPDATE_STREAM_ADD_BUFQ, + UPDATE_STREAM_REMOVE_BUFQ, + UPDATE_STREAM_SW_FRAME_DROP, +}; + +enum msm_vfe_iommu_type { + IOMMU_ATTACH, + IOMMU_DETACH, +}; + +enum msm_vfe_buff_queue_id { + VFE_BUF_QUEUE_DEFAULT, + VFE_BUF_QUEUE_SHARED, + VFE_BUF_QUEUE_MAX, +}; + +struct msm_vfe_axi_stream_cfg_update_info { + uint32_t stream_handle; + uint32_t output_format; + uint32_t user_stream_id; + uint32_t frame_id; + enum msm_vfe_frame_skip_pattern skip_pattern; + struct msm_vfe_axi_plane_cfg plane_cfg[MAX_PLANES_PER_STREAM]; + struct msm_isp_sw_framskip sw_skip_info; +}; + +struct msm_vfe_axi_halt_cmd { + uint32_t stop_camif; + uint32_t overflow_detected; + uint32_t blocking_halt; +}; + +struct msm_vfe_axi_reset_cmd { + uint32_t blocking; + uint32_t frame_id; +}; + +struct msm_vfe_axi_restart_cmd { + uint32_t enable_camif; +}; + +struct msm_vfe_axi_stream_update_cmd { + uint32_t num_streams; + enum msm_vfe_axi_stream_update_type update_type; + struct msm_vfe_axi_stream_cfg_update_info + update_info[MSM_ISP_STATS_MAX]; +}; + +struct msm_vfe_smmu_attach_cmd { + uint32_t security_mode; + uint32_t iommu_attach_mode; +}; + +struct msm_vfe_stats_stream_request_cmd { + uint32_t session_id; + uint32_t stream_id; + enum msm_isp_stats_type stats_type; + uint32_t composite_flag; + uint32_t framedrop_pattern; + uint32_t init_frame_drop; /*MAX 31 Frames*/ + uint32_t irq_subsample_pattern; + uint32_t buffer_offset; + uint32_t stream_handle; +}; + +struct msm_vfe_stats_stream_release_cmd { + uint32_t stream_handle; +}; +struct msm_vfe_stats_stream_cfg_cmd { + uint8_t num_streams; + uint32_t stream_handle[MSM_ISP_STATS_MAX]; + uint8_t enable; + uint32_t stats_burst_len; +}; + +enum msm_vfe_reg_cfg_type { + VFE_WRITE, + VFE_WRITE_MB, + VFE_READ, + VFE_CFG_MASK, + VFE_WRITE_DMI_16BIT, + VFE_WRITE_DMI_32BIT, + VFE_WRITE_DMI_64BIT, + VFE_READ_DMI_16BIT, + VFE_READ_DMI_32BIT, + VFE_READ_DMI_64BIT, + GET_MAX_CLK_RATE, + GET_CLK_RATES, + GET_ISP_ID, + VFE_HW_UPDATE_LOCK, + VFE_HW_UPDATE_UNLOCK, + SET_WM_UB_SIZE, + SET_UB_POLICY, +}; + +struct msm_vfe_cfg_cmd2 { + uint16_t num_cfg; + uint16_t cmd_len; + void __user *cfg_data; + void __user *cfg_cmd; +}; + +struct msm_vfe_cfg_cmd_list { + struct msm_vfe_cfg_cmd2 cfg_cmd; + struct msm_vfe_cfg_cmd_list *next; + uint32_t next_size; +}; + +struct msm_vfe_reg_rw_info { + uint32_t reg_offset; + uint32_t cmd_data_offset; + uint32_t len; +}; + +struct msm_vfe_reg_mask_info { + uint32_t reg_offset; + uint32_t mask; + uint32_t val; +}; + +struct msm_vfe_reg_dmi_info { + uint32_t hi_tbl_offset; /*Optional*/ + uint32_t lo_tbl_offset; /*Required*/ + uint32_t len; +}; + +struct msm_vfe_reg_cfg_cmd { + union { + struct msm_vfe_reg_rw_info rw_info; + struct msm_vfe_reg_mask_info mask_info; + struct msm_vfe_reg_dmi_info dmi_info; + } u; + + enum msm_vfe_reg_cfg_type cmd_type; +}; + +enum vfe_sd_type { + VFE_SD_0 = 0, + VFE_SD_1, + VFE_SD_COMMON, + VFE_SD_MAX, +}; + +/* When you change the value below, check for the sof event_data size. + * V4l2 limits payload to 64 bytes */ +#define MS_NUM_SLAVE_MAX 1 + +/* Usecases when 2 HW need to be related or synced */ +enum msm_vfe_dual_hw_type { + DUAL_NONE = 0, + DUAL_HW_VFE_SPLIT = 1, + DUAL_HW_MASTER_SLAVE = 2, +}; + +/* Type for 2 INTF when used in Master-Slave mode */ +enum msm_vfe_dual_hw_ms_type { + MS_TYPE_NONE, + MS_TYPE_MASTER, + MS_TYPE_SLAVE, +}; + +struct msm_isp_set_dual_hw_ms_cmd { + uint8_t num_src; + /* Each session can be only one type but multiple intf if YUV cam */ + enum msm_vfe_dual_hw_ms_type dual_hw_ms_type; + /* Primary intf is mostly associated with preview. + * This primary intf SOF frame_id and timestamp is tracked + * and used to calculate delta */ + enum msm_vfe_input_src primary_intf; + /* input_src array indicates other input INTF that may be Master/Slave. + * For these additional intf, frame_id and timestamp are not saved. + * However, if these are slaves then they will still get their + * frame_id from Master */ + enum msm_vfe_input_src input_src[VFE_SRC_MAX]; + uint32_t sof_delta_threshold; /* In milliseconds. Sent for Master */ +}; + +enum msm_isp_buf_type { + ISP_PRIVATE_BUF, + ISP_SHARE_BUF, + MAX_ISP_BUF_TYPE, +}; + +struct msm_isp_unmap_buf_req { + uint32_t fd; +}; + +struct msm_isp_buf_request { + uint32_t session_id; + uint32_t stream_id; + uint8_t num_buf; + uint32_t handle; + enum msm_isp_buf_type buf_type; +}; + +struct msm_isp_qbuf_plane { + uint32_t addr; + uint32_t offset; + uint32_t length; +}; + +struct msm_isp_qbuf_buffer { + struct msm_isp_qbuf_plane planes[MAX_PLANES_PER_STREAM]; + uint32_t num_planes; +}; + +struct msm_isp_qbuf_info { + uint32_t handle; + int32_t buf_idx; + /*Only used for prepare buffer*/ + struct msm_isp_qbuf_buffer buffer; + /*Only used for diverted buffer*/ + uint32_t dirty_buf; +}; + +struct msm_isp_clk_rates { + uint32_t svs_rate; + uint32_t nominal_rate; + uint32_t high_rate; +}; + +struct msm_vfe_axi_src_state { + enum msm_vfe_input_src input_src; + uint32_t src_active; + uint32_t src_frame_id; +}; + +enum msm_isp_event_mask_index { + ISP_EVENT_MASK_INDEX_STATS_NOTIFY = 0, + ISP_EVENT_MASK_INDEX_ERROR = 1, + ISP_EVENT_MASK_INDEX_IOMMU_P_FAULT = 2, + ISP_EVENT_MASK_INDEX_STREAM_UPDATE_DONE = 3, + ISP_EVENT_MASK_INDEX_REG_UPDATE = 4, + ISP_EVENT_MASK_INDEX_SOF = 5, + ISP_EVENT_MASK_INDEX_BUF_DIVERT = 6, + ISP_EVENT_MASK_INDEX_COMP_STATS_NOTIFY = 7, + ISP_EVENT_MASK_INDEX_MASK_FE_READ_DONE = 8, + ISP_EVENT_MASK_INDEX_BUF_DONE = 9, + ISP_EVENT_MASK_INDEX_REG_UPDATE_MISSING = 10, + ISP_EVENT_MASK_INDEX_PING_PONG_MISMATCH = 11, + ISP_EVENT_MASK_INDEX_BUF_FATAL_ERROR = 12, +}; + + +#define ISP_EVENT_SUBS_MASK_NONE 0 + +#define ISP_EVENT_SUBS_MASK_STATS_NOTIFY \ + (1 << ISP_EVENT_MASK_INDEX_STATS_NOTIFY) + +#define ISP_EVENT_SUBS_MASK_ERROR \ + (1 << ISP_EVENT_MASK_INDEX_ERROR) + +#define ISP_EVENT_SUBS_MASK_IOMMU_P_FAULT \ + (1 << ISP_EVENT_MASK_INDEX_IOMMU_P_FAULT) + +#define ISP_EVENT_SUBS_MASK_STREAM_UPDATE_DONE \ + (1 << ISP_EVENT_MASK_INDEX_STREAM_UPDATE_DONE) + +#define ISP_EVENT_SUBS_MASK_REG_UPDATE \ + (1 << ISP_EVENT_MASK_INDEX_REG_UPDATE) + +#define ISP_EVENT_SUBS_MASK_SOF \ + (1 << ISP_EVENT_MASK_INDEX_SOF) + +#define ISP_EVENT_SUBS_MASK_BUF_DIVERT \ + (1 << ISP_EVENT_MASK_INDEX_BUF_DIVERT) + +#define ISP_EVENT_SUBS_MASK_COMP_STATS_NOTIFY \ + (1 << ISP_EVENT_MASK_INDEX_COMP_STATS_NOTIFY) + +#define ISP_EVENT_SUBS_MASK_FE_READ_DONE \ + (1 << ISP_EVENT_MASK_INDEX_MASK_FE_READ_DONE) + +#define ISP_EVENT_SUBS_MASK_BUF_DONE \ + (1 << ISP_EVENT_MASK_INDEX_BUF_DONE) + +#define ISP_EVENT_SUBS_MASK_REG_UPDATE_MISSING \ + (1 << ISP_EVENT_MASK_INDEX_REG_UPDATE_MISSING) + +#define ISP_EVENT_SUBS_MASK_PING_PONG_MISMATCH \ + (1 << ISP_EVENT_MASK_INDEX_PING_PONG_MISMATCH) + +#define ISP_EVENT_SUBS_MASK_BUF_FATAL_ERROR \ + (1 << ISP_EVENT_MASK_INDEX_BUF_FATAL_ERROR) + +enum msm_isp_event_idx { + ISP_REG_UPDATE = 0, + ISP_EPOCH_0 = 1, + ISP_EPOCH_1 = 2, + ISP_START_ACK = 3, + ISP_STOP_ACK = 4, + ISP_IRQ_VIOLATION = 5, + ISP_STATS_OVERFLOW = 6, + ISP_BUF_DONE = 7, + ISP_FE_RD_DONE = 8, + ISP_IOMMU_P_FAULT = 9, + ISP_ERROR = 10, + ISP_HW_FATAL_ERROR = 11, + ISP_PING_PONG_MISMATCH = 12, + ISP_REG_UPDATE_MISSING = 13, + ISP_BUF_FATAL_ERROR = 14, + ISP_EVENT_MAX = 15 +}; + +#define ISP_EVENT_OFFSET 8 +#define ISP_EVENT_BASE (V4L2_EVENT_PRIVATE_START) +#define ISP_BUF_EVENT_BASE (ISP_EVENT_BASE + (1 << ISP_EVENT_OFFSET)) +#define ISP_STATS_EVENT_BASE (ISP_EVENT_BASE + (2 << ISP_EVENT_OFFSET)) +#define ISP_CAMIF_EVENT_BASE (ISP_EVENT_BASE + (3 << ISP_EVENT_OFFSET)) +#define ISP_STREAM_EVENT_BASE (ISP_EVENT_BASE + (4 << ISP_EVENT_OFFSET)) +#define ISP_EVENT_REG_UPDATE (ISP_EVENT_BASE + ISP_REG_UPDATE) +#define ISP_EVENT_EPOCH_0 (ISP_EVENT_BASE + ISP_EPOCH_0) +#define ISP_EVENT_EPOCH_1 (ISP_EVENT_BASE + ISP_EPOCH_1) +#define ISP_EVENT_START_ACK (ISP_EVENT_BASE + ISP_START_ACK) +#define ISP_EVENT_STOP_ACK (ISP_EVENT_BASE + ISP_STOP_ACK) +#define ISP_EVENT_IRQ_VIOLATION (ISP_EVENT_BASE + ISP_IRQ_VIOLATION) +#define ISP_EVENT_STATS_OVERFLOW (ISP_EVENT_BASE + ISP_STATS_OVERFLOW) +#define ISP_EVENT_ERROR (ISP_EVENT_BASE + ISP_ERROR) +#define ISP_EVENT_SOF (ISP_CAMIF_EVENT_BASE) +#define ISP_EVENT_EOF (ISP_CAMIF_EVENT_BASE + 1) +#define ISP_EVENT_BUF_DONE (ISP_EVENT_BASE + ISP_BUF_DONE) +#define ISP_EVENT_BUF_DIVERT (ISP_BUF_EVENT_BASE) +#define ISP_EVENT_STATS_NOTIFY (ISP_STATS_EVENT_BASE) +#define ISP_EVENT_COMP_STATS_NOTIFY (ISP_EVENT_STATS_NOTIFY + MSM_ISP_STATS_MAX) +#define ISP_EVENT_FE_READ_DONE (ISP_EVENT_BASE + ISP_FE_RD_DONE) +#define ISP_EVENT_IOMMU_P_FAULT (ISP_EVENT_BASE + ISP_IOMMU_P_FAULT) +#define ISP_EVENT_HW_FATAL_ERROR (ISP_EVENT_BASE + ISP_HW_FATAL_ERROR) +#define ISP_EVENT_PING_PONG_MISMATCH (ISP_EVENT_BASE + ISP_PING_PONG_MISMATCH) +#define ISP_EVENT_REG_UPDATE_MISSING (ISP_EVENT_BASE + ISP_REG_UPDATE_MISSING) +#define ISP_EVENT_BUF_FATAL_ERROR (ISP_EVENT_BASE + ISP_BUF_FATAL_ERROR) +#define ISP_EVENT_STREAM_UPDATE_DONE (ISP_STREAM_EVENT_BASE) + +/* The msm_v4l2_event_data structure should match the + * v4l2_event.u.data field. + * should not exceed 64 bytes */ + +struct msm_isp_buf_event { + uint32_t session_id; + uint32_t stream_id; + uint32_t handle; + uint32_t output_format; + int8_t buf_idx; +}; +struct msm_isp_fetch_eng_event { + uint32_t session_id; + uint32_t stream_id; + uint32_t handle; + uint32_t fd; + int8_t buf_idx; + int8_t offline_mode; +}; +struct msm_isp_stats_event { + uint32_t stats_mask; /* 4 bytes */ + uint8_t stats_buf_idxs[MSM_ISP_STATS_MAX]; /* 11 bytes */ +}; + +struct msm_isp_stream_ack { + uint32_t session_id; + uint32_t stream_id; + uint32_t handle; +}; + +enum msm_vfe_error_type { + ISP_ERROR_NONE, + ISP_ERROR_CAMIF, + ISP_ERROR_BUS_OVERFLOW, + ISP_ERROR_RETURN_EMPTY_BUFFER, + ISP_ERROR_FRAME_ID_MISMATCH, + ISP_ERROR_MAX, +}; + +struct msm_isp_error_info { + enum msm_vfe_error_type err_type; + uint32_t session_id; + uint32_t stream_id; + uint32_t stream_id_mask; +}; + +/* This structure reports delta between master and slave */ +struct msm_isp_ms_delta_info { + uint8_t num_delta_info; + uint32_t delta[MS_NUM_SLAVE_MAX]; +}; + +/* This is sent in EPOCH irq */ +struct msm_isp_output_info { + uint8_t regs_not_updated; + /* mask with bufq_handle for regs not updated or return empty */ + uint16_t output_err_mask; + /* mask with stream_idx for get_buf failed */ + uint8_t stream_framedrop_mask; + /* mask with stats stream_idx for get_buf failed */ + uint16_t stats_framedrop_mask; + /* delta between master and slave */ +}; + +/* This structure is piggybacked with SOF event */ +struct msm_isp_sof_info { + uint8_t regs_not_updated; + /* mask with AXI_SRC for regs not updated */ + uint16_t reg_update_fail_mask; + /* mask with bufq_handle for get_buf failed */ + uint32_t stream_get_buf_fail_mask; + /* mask with stats stream_idx for get_buf failed */ + uint16_t stats_get_buf_fail_mask; + /* delta between master and slave */ + struct msm_isp_ms_delta_info ms_delta_info; +}; + +struct msm_isp_event_data { + /*Wall clock except for buffer divert events + *which use monotonic clock + */ + struct timeval timestamp; + /* Monotonic timestamp since bootup */ + struct timeval mono_timestamp; + uint32_t frame_id; + union { + /* Sent for Stats_Done event */ + struct msm_isp_stats_event stats; + /* Sent for Buf_Divert event */ + struct msm_isp_buf_event buf_done; + /* Sent for offline fetch done event */ + struct msm_isp_fetch_eng_event fetch_done; + /* Sent for Error_Event */ + struct msm_isp_error_info error_info; + /* + * This struct needs to be removed once + * userspace switches to sof_info + */ + struct msm_isp_output_info output_info; + /* Sent for SOF event */ + struct msm_isp_sof_info sof_info; + } u; /* union can have max 52 bytes */ +}; + +#ifdef CONFIG_COMPAT +struct msm_isp_event_data32 { + struct compat_timeval timestamp; + struct compat_timeval mono_timestamp; + uint32_t frame_id; + union { + struct msm_isp_stats_event stats; + struct msm_isp_buf_event buf_done; + struct msm_isp_fetch_eng_event fetch_done; + struct msm_isp_error_info error_info; + struct msm_isp_output_info output_info; + struct msm_isp_sof_info sof_info; + } u; +}; +#endif + +#define V4L2_PIX_FMT_QBGGR8 v4l2_fourcc('Q', 'B', 'G', '8') +#define V4L2_PIX_FMT_QGBRG8 v4l2_fourcc('Q', 'G', 'B', '8') +#define V4L2_PIX_FMT_QGRBG8 v4l2_fourcc('Q', 'G', 'R', '8') +#define V4L2_PIX_FMT_QRGGB8 v4l2_fourcc('Q', 'R', 'G', '8') +#define V4L2_PIX_FMT_QBGGR10 v4l2_fourcc('Q', 'B', 'G', '0') +#define V4L2_PIX_FMT_QGBRG10 v4l2_fourcc('Q', 'G', 'B', '0') +#define V4L2_PIX_FMT_QGRBG10 v4l2_fourcc('Q', 'G', 'R', '0') +#define V4L2_PIX_FMT_QRGGB10 v4l2_fourcc('Q', 'R', 'G', '0') +#define V4L2_PIX_FMT_QBGGR12 v4l2_fourcc('Q', 'B', 'G', '2') +#define V4L2_PIX_FMT_QGBRG12 v4l2_fourcc('Q', 'G', 'B', '2') +#define V4L2_PIX_FMT_QGRBG12 v4l2_fourcc('Q', 'G', 'R', '2') +#define V4L2_PIX_FMT_QRGGB12 v4l2_fourcc('Q', 'R', 'G', '2') +#define V4L2_PIX_FMT_QBGGR14 v4l2_fourcc('Q', 'B', 'G', '4') +#define V4L2_PIX_FMT_QGBRG14 v4l2_fourcc('Q', 'G', 'B', '4') +#define V4L2_PIX_FMT_QGRBG14 v4l2_fourcc('Q', 'G', 'R', '4') +#define V4L2_PIX_FMT_QRGGB14 v4l2_fourcc('Q', 'R', 'G', '4') +#define V4L2_PIX_FMT_P16BGGR10 v4l2_fourcc('P', 'B', 'G', '0') +#define V4L2_PIX_FMT_P16GBRG10 v4l2_fourcc('P', 'G', 'B', '0') +#define V4L2_PIX_FMT_P16GRBG10 v4l2_fourcc('P', 'G', 'R', '0') +#define V4L2_PIX_FMT_P16RGGB10 v4l2_fourcc('P', 'R', 'G', '0') +#define V4L2_PIX_FMT_NV14 v4l2_fourcc('N', 'V', '1', '4') +#define V4L2_PIX_FMT_NV41 v4l2_fourcc('N', 'V', '4', '1') +#define V4L2_PIX_FMT_META v4l2_fourcc('Q', 'M', 'E', 'T') +#define V4L2_PIX_FMT_SBGGR14 v4l2_fourcc('B', 'G', '1', '4') /* 14 BGBG.GRGR.*/ +#define V4L2_PIX_FMT_SGBRG14 v4l2_fourcc('G', 'B', '1', '4') /* 14 GBGB.RGRG.*/ +#define V4L2_PIX_FMT_SGRBG14 v4l2_fourcc('B', 'A', '1', '4') /* 14 GRGR.BGBG.*/ +#define V4L2_PIX_FMT_SRGGB14 v4l2_fourcc('R', 'G', '1', '4') /* 14 RGRG.GBGB.*/ + +#define VIDIOC_MSM_VFE_REG_CFG \ + _IOWR('V', BASE_VIDIOC_PRIVATE, struct msm_vfe_cfg_cmd2) + +#define VIDIOC_MSM_ISP_REQUEST_BUF \ + _IOWR('V', BASE_VIDIOC_PRIVATE+1, struct msm_isp_buf_request) + +#define VIDIOC_MSM_ISP_ENQUEUE_BUF \ + _IOWR('V', BASE_VIDIOC_PRIVATE+2, struct msm_isp_qbuf_info) + +#define VIDIOC_MSM_ISP_RELEASE_BUF \ + _IOWR('V', BASE_VIDIOC_PRIVATE+3, struct msm_isp_buf_request) + +#define VIDIOC_MSM_ISP_REQUEST_STREAM \ + _IOWR('V', BASE_VIDIOC_PRIVATE+4, struct msm_vfe_axi_stream_request_cmd) + +#define VIDIOC_MSM_ISP_CFG_STREAM \ + _IOWR('V', BASE_VIDIOC_PRIVATE+5, struct msm_vfe_axi_stream_cfg_cmd) + +#define VIDIOC_MSM_ISP_RELEASE_STREAM \ + _IOWR('V', BASE_VIDIOC_PRIVATE+6, struct msm_vfe_axi_stream_release_cmd) + +#define VIDIOC_MSM_ISP_INPUT_CFG \ + _IOWR('V', BASE_VIDIOC_PRIVATE+7, struct msm_vfe_input_cfg) + +#define VIDIOC_MSM_ISP_SET_SRC_STATE \ + _IOWR('V', BASE_VIDIOC_PRIVATE+8, struct msm_vfe_axi_src_state) + +#define VIDIOC_MSM_ISP_REQUEST_STATS_STREAM \ + _IOWR('V', BASE_VIDIOC_PRIVATE+9, \ + struct msm_vfe_stats_stream_request_cmd) + +#define VIDIOC_MSM_ISP_CFG_STATS_STREAM \ + _IOWR('V', BASE_VIDIOC_PRIVATE+10, struct msm_vfe_stats_stream_cfg_cmd) + +#define VIDIOC_MSM_ISP_RELEASE_STATS_STREAM \ + _IOWR('V', BASE_VIDIOC_PRIVATE+11, \ + struct msm_vfe_stats_stream_release_cmd) + +#define VIDIOC_MSM_ISP_REG_UPDATE_CMD \ + _IOWR('V', BASE_VIDIOC_PRIVATE+12, enum msm_vfe_input_src) + +#define VIDIOC_MSM_ISP_UPDATE_STREAM \ + _IOWR('V', BASE_VIDIOC_PRIVATE+13, struct msm_vfe_axi_stream_update_cmd) + +#define VIDIOC_MSM_VFE_REG_LIST_CFG \ + _IOWR('V', BASE_VIDIOC_PRIVATE+14, struct msm_vfe_cfg_cmd_list) + +#define VIDIOC_MSM_ISP_SMMU_ATTACH \ + _IOWR('V', BASE_VIDIOC_PRIVATE+15, struct msm_vfe_smmu_attach_cmd) + +#define VIDIOC_MSM_ISP_UPDATE_STATS_STREAM \ + _IOWR('V', BASE_VIDIOC_PRIVATE+16, struct msm_vfe_axi_stream_update_cmd) + +#define VIDIOC_MSM_ISP_AXI_HALT \ + _IOWR('V', BASE_VIDIOC_PRIVATE+17, struct msm_vfe_axi_halt_cmd) + +#define VIDIOC_MSM_ISP_AXI_RESET \ + _IOWR('V', BASE_VIDIOC_PRIVATE+18, struct msm_vfe_axi_reset_cmd) + +#define VIDIOC_MSM_ISP_AXI_RESTART \ + _IOWR('V', BASE_VIDIOC_PRIVATE+19, struct msm_vfe_axi_restart_cmd) + +#define VIDIOC_MSM_ISP_FETCH_ENG_START \ + _IOWR('V', BASE_VIDIOC_PRIVATE+20, struct msm_vfe_fetch_eng_start) + +#define VIDIOC_MSM_ISP_DEQUEUE_BUF \ + _IOWR('V', BASE_VIDIOC_PRIVATE+21, struct msm_isp_qbuf_info) + +#define VIDIOC_MSM_ISP_SET_DUAL_HW_MASTER_SLAVE \ + _IOWR('V', BASE_VIDIOC_PRIVATE+22, struct msm_isp_set_dual_hw_ms_cmd) + +#define VIDIOC_MSM_ISP_MAP_BUF_START_FE \ + _IOWR('V', BASE_VIDIOC_PRIVATE+23, struct msm_vfe_fetch_eng_start) + +#define VIDIOC_MSM_ISP_UNMAP_BUF \ + _IOWR('V', BASE_VIDIOC_PRIVATE+24, struct msm_isp_unmap_buf_req) + +#endif /* __MSMB_ISP__ */ diff --git a/selfdrive/camerad/include/msmb_ispif.h b/selfdrive/camerad/include/msmb_ispif.h new file mode 100644 index 000000000..2564c33b7 --- /dev/null +++ b/selfdrive/camerad/include/msmb_ispif.h @@ -0,0 +1,125 @@ +#ifndef MSM_CAM_ISPIF_H +#define MSM_CAM_ISPIF_H + +#define CSID_VERSION_V20 0x02000011 +#define CSID_VERSION_V22 0x02001000 +#define CSID_VERSION_V30 0x30000000 +#define CSID_VERSION_V3 0x30000000 + +enum msm_ispif_vfe_intf { + VFE0, + VFE1, + VFE_MAX +}; +#define VFE0_MASK (1 << VFE0) +#define VFE1_MASK (1 << VFE1) + +enum msm_ispif_intftype { + PIX0, + RDI0, + PIX1, + RDI1, + RDI2, + INTF_MAX +}; +#define MAX_PARAM_ENTRIES (INTF_MAX * 2) +#define MAX_CID_CH 8 + +#define PIX0_MASK (1 << PIX0) +#define PIX1_MASK (1 << PIX1) +#define RDI0_MASK (1 << RDI0) +#define RDI1_MASK (1 << RDI1) +#define RDI2_MASK (1 << RDI2) + + +enum msm_ispif_vc { + VC0, + VC1, + VC2, + VC3, + VC_MAX +}; + +enum msm_ispif_cid { + CID0, + CID1, + CID2, + CID3, + CID4, + CID5, + CID6, + CID7, + CID8, + CID9, + CID10, + CID11, + CID12, + CID13, + CID14, + CID15, + CID_MAX +}; + +enum msm_ispif_csid { + CSID0, + CSID1, + CSID2, + CSID3, + CSID_MAX +}; + +struct msm_ispif_params_entry { + enum msm_ispif_vfe_intf vfe_intf; + enum msm_ispif_intftype intftype; + int num_cids; + enum msm_ispif_cid cids[3]; + enum msm_ispif_csid csid; + int crop_enable; + uint16_t crop_start_pixel; + uint16_t crop_end_pixel; +}; + +struct msm_ispif_param_data { + uint32_t num; + struct msm_ispif_params_entry entries[MAX_PARAM_ENTRIES]; +}; + +struct msm_isp_info { + uint32_t max_resolution; + uint32_t id; + uint32_t ver; +}; + +struct msm_ispif_vfe_info { + int num_vfe; + struct msm_isp_info info[VFE_MAX]; +}; + +enum ispif_cfg_type_t { + ISPIF_CLK_ENABLE, + ISPIF_CLK_DISABLE, + ISPIF_INIT, + ISPIF_CFG, + ISPIF_START_FRAME_BOUNDARY, + ISPIF_RESTART_FRAME_BOUNDARY, + ISPIF_STOP_FRAME_BOUNDARY, + ISPIF_STOP_IMMEDIATELY, + ISPIF_RELEASE, + ISPIF_ENABLE_REG_DUMP, + ISPIF_SET_VFE_INFO, +}; + +struct ispif_cfg_data { + enum ispif_cfg_type_t cfg_type; + union { + int reg_dump; /* ISPIF_ENABLE_REG_DUMP */ + uint32_t csid_version; /* ISPIF_INIT */ + struct msm_ispif_vfe_info vfe_info; /* ISPIF_SET_VFE_INFO */ + struct msm_ispif_param_data params; /* CFG, START, STOP */ + }; +}; + +#define VIDIOC_MSM_ISPIF_CFG \ + _IOWR('V', BASE_VIDIOC_PRIVATE, struct ispif_cfg_data) + +#endif /* MSM_CAM_ISPIF_H */ diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc new file mode 100644 index 000000000..896a4cac6 --- /dev/null +++ b/selfdrive/camerad/main.cc @@ -0,0 +1,1128 @@ +#include +#include +#include + +#ifdef QCOM +#include "cameras/camera_qcom.h" +#else +#include "cameras/camera_frame_stream.h" +#endif + +#include "common/util.h" +#include "common/swaglog.h" + +#include "common/visionipc.h" +#include "common/visionbuf.h" +#include "common/visionimg.h" + +#include "messaging.hpp" + +#include "transforms/rgb_to_yuv.h" + +#include "clutil.h" +#include "bufs.h" + +#include +#include +#include +#include +#include "cereal/gen/cpp/log.capnp.h" + +#define UI_BUF_COUNT 4 +#define YUV_COUNT 40 +#define MAX_CLIENTS 5 + +extern "C" { +volatile sig_atomic_t do_exit = 0; +} + +void set_do_exit(int sig) { + do_exit = 1; +} + +struct VisionState; + +struct VisionClientState { + VisionState *s; + int fd; + pthread_t thread_handle; + bool running; +}; + +struct VisionClientStreamState { + bool subscribed; + int bufs_outstanding; + bool tb; + TBuffer* tbuffer; + PoolQueue* queue; +}; + +struct VisionState { + int frame_width, frame_height; + int frame_stride; + int frame_size; + + int ion_fd; + + // cl state + cl_device_id device_id; + cl_context context; + + cl_program prg_debayer_rear; + cl_program prg_debayer_front; + cl_kernel krnl_debayer_rear; + cl_kernel krnl_debayer_front; + + // processing + TBuffer ui_tb; + TBuffer ui_front_tb; + + mat3 yuv_transform; + TBuffer *yuv_tb; + TBuffer *yuv_front_tb; + + // TODO: refactor for both cameras? + Pool yuv_pool; + VisionBuf yuv_ion[YUV_COUNT]; + cl_mem yuv_cl[YUV_COUNT]; + YUVBuf yuv_bufs[YUV_COUNT]; + FrameMetadata yuv_metas[YUV_COUNT]; + size_t yuv_buf_size; + int yuv_width, yuv_height; + RGBToYUVState rgb_to_yuv_state; + + // for front camera recording + Pool yuv_front_pool; + VisionBuf yuv_front_ion[YUV_COUNT]; + cl_mem yuv_front_cl[YUV_COUNT]; + YUVBuf yuv_front_bufs[YUV_COUNT]; + FrameMetadata yuv_front_metas[YUV_COUNT]; + size_t yuv_front_buf_size; + int yuv_front_width, yuv_front_height; + RGBToYUVState front_rgb_to_yuv_state; + + size_t rgb_buf_size; + int rgb_width, rgb_height, rgb_stride; + VisionBuf rgb_bufs[UI_BUF_COUNT]; + cl_mem rgb_bufs_cl[UI_BUF_COUNT]; + + size_t rgb_front_buf_size; + int rgb_front_width, rgb_front_height, rgb_front_stride; + VisionBuf rgb_front_bufs[UI_BUF_COUNT]; + cl_mem rgb_front_bufs_cl[UI_BUF_COUNT]; + int front_meteringbox_xmin, front_meteringbox_xmax; + int front_meteringbox_ymin, front_meteringbox_ymax; + + + cl_mem camera_bufs_cl[FRAME_BUF_COUNT]; + VisionBuf camera_bufs[FRAME_BUF_COUNT]; + VisionBuf focus_bufs[FRAME_BUF_COUNT]; + VisionBuf stats_bufs[FRAME_BUF_COUNT]; + + cl_mem front_camera_bufs_cl[FRAME_BUF_COUNT]; + VisionBuf front_camera_bufs[FRAME_BUF_COUNT]; + + DualCameraState cameras; + + zsock_t *terminate_pub; + + Context * msg_context; + PubSocket *frame_sock; + PubSocket *front_frame_sock; + PubSocket *thumbnail_sock; + + pthread_mutex_t clients_lock; + VisionClientState clients[MAX_CLIENTS]; +}; + +// frontview thread +void* frontview_thread(void *arg) { + int err; + VisionState *s = (VisionState*)arg; + + set_thread_name("frontview"); + + s->msg_context = Context::create(); + + // we subscribe to this for placement of the AE metering box + // TODO: the loop is bad, ideally models shouldn't affect sensors + Context *msg_context = Context::create(); + SubSocket *monitoring_sock = SubSocket::create(msg_context, "driverMonitoring", "127.0.0.1", true); + assert(monitoring_sock != NULL); + + cl_command_queue q = clCreateCommandQueue(s->context, s->device_id, 0, &err); + assert(err == 0); + + for (int cnt = 0; !do_exit; cnt++) { + int buf_idx = tbuffer_acquire(&s->cameras.front.camera_tb); + if (buf_idx < 0) { + break; + } + + int ui_idx = tbuffer_select(&s->ui_front_tb); + FrameMetadata frame_data = s->cameras.front.camera_bufs_metadata[buf_idx]; + + double t1 = millis_since_boot(); + + err = clSetKernelArg(s->krnl_debayer_front, 0, sizeof(cl_mem), &s->front_camera_bufs_cl[buf_idx]); + assert(err == 0); + err = clSetKernelArg(s->krnl_debayer_front, 1, sizeof(cl_mem), &s->rgb_front_bufs_cl[ui_idx]); + assert(err == 0); + float digital_gain = 1.0; + err = clSetKernelArg(s->krnl_debayer_front, 2, sizeof(float), &digital_gain); + assert(err == 0); + + cl_event debayer_event; + const size_t debayer_work_size = s->rgb_front_height; + const size_t debayer_local_work_size = 128; + err = clEnqueueNDRangeKernel(q, s->krnl_debayer_front, 1, NULL, + &debayer_work_size, &debayer_local_work_size, 0, 0, &debayer_event); + assert(err == 0); + clWaitForEvents(1, &debayer_event); + clReleaseEvent(debayer_event); + + tbuffer_release(&s->cameras.front.camera_tb, buf_idx); + + visionbuf_sync(&s->rgb_front_bufs[ui_idx], VISIONBUF_SYNC_FROM_DEVICE); + + // set front camera metering target + Message *msg = monitoring_sock->receive(true); + if (msg != NULL) { + auto amsg = kj::heapArray((msg->getSize() / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), msg->getData(), msg->getSize()); + + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::Event::Reader event = cmsg.getRoot(); + + float face_prob = event.getDriverMonitoring().getFaceProb(); + float face_position[2]; + face_position[0] = event.getDriverMonitoring().getFacePosition()[0]; + face_position[1] = event.getDriverMonitoring().getFacePosition()[1]; + + // set front camera metering target + if (face_prob > 0.4) + { + int x_offset = s->rgb_front_width - 0.5 * s->rgb_front_height; + s->front_meteringbox_xmin = x_offset + (face_position[0] + 0.5) * (0.5 * s->rgb_front_height) - 72; + s->front_meteringbox_xmax = x_offset + (face_position[0] + 0.5) * (0.5 * s->rgb_front_height) + 72; + s->front_meteringbox_ymin = (face_position[1] + 0.5) * (s->rgb_front_height) - 72; + s->front_meteringbox_ymax = (face_position[1] + 0.5) * (s->rgb_front_height) + 72; + } + else // use default setting if no face + { + s->front_meteringbox_ymin = s->rgb_front_height * 1 / 3; + s->front_meteringbox_ymax = s->rgb_front_height * 1; + s->front_meteringbox_xmin = s->rgb_front_width * 3 / 5; + s->front_meteringbox_xmax = s->rgb_front_width; + } + + delete msg; + } + + // auto exposure + const uint8_t *bgr_front_ptr = (const uint8_t*)s->rgb_front_bufs[ui_idx].addr; +#ifndef DEBUG_DRIVER_MONITOR + if (cnt % 3 == 0) +#endif + { + // use driver face crop for AE + int x_start; + int x_end; + int y_start; + int y_end; + + if (s->front_meteringbox_xmax > 0) + { + x_start = s->front_meteringbox_xmin<0 ? 0:s->front_meteringbox_xmin; + x_end = s->front_meteringbox_xmax>=s->rgb_front_width ? s->rgb_front_width-1:s->front_meteringbox_xmax; + y_start = s->front_meteringbox_ymin<0 ? 0:s->front_meteringbox_ymin; + y_end = s->front_meteringbox_ymax>=s->rgb_front_height ? s->rgb_front_height-1:s->front_meteringbox_ymax; + } + else + { + y_start = s->rgb_front_height * 1 / 3; + y_end = s->rgb_front_height * 1; + x_start = s->rgb_front_width * 3 / 5; + x_end = s->rgb_front_width; + } + + uint32_t lum_binning[256] = {0,}; + for (int y = y_start; y < y_end; ++y) { + for (int x = x_start; x < x_end; x += 2) { // every 2nd col + const uint8_t *pix = &bgr_front_ptr[y * s->rgb_front_stride + x * 3]; + unsigned int lum = (unsigned int)pix[0] + pix[1] + pix[2]; +#ifdef DEBUG_DRIVER_MONITOR + uint8_t *pix_rw = (uint8_t *)pix; + + // set all the autoexposure pixels to pure green (pixel format is bgr) + pix_rw[0] = pix_rw[2] = 0; + pix_rw[1] = 0xff; +#endif + lum_binning[std::min(lum / 3, 255u)]++; + } + } + const unsigned int lum_total = (y_end - y_start) * (x_end - x_start)/2; + unsigned int lum_cur = 0; + int lum_med = 0; + for (lum_med=0; lum_med<256; lum_med++) { + lum_cur += lum_binning[lum_med]; + if (lum_cur >= lum_total / 2) { + break; + } + } + camera_autoexposure(&s->cameras.front, lum_med / 256.0); + } + + // push YUV buffer + int yuv_idx = pool_select(&s->yuv_front_pool); + s->yuv_front_metas[yuv_idx] = frame_data; + + rgb_to_yuv_queue(&s->front_rgb_to_yuv_state, q, s->rgb_front_bufs_cl[ui_idx], s->yuv_front_cl[yuv_idx]); + visionbuf_sync(&s->yuv_front_ion[yuv_idx], VISIONBUF_SYNC_FROM_DEVICE); + s->yuv_front_metas[yuv_idx] = frame_data; + + // no reference required cause we don't use this in visiond + //pool_acquire(&s->yuv_front_pool, yuv_idx); + pool_push(&s->yuv_front_pool, yuv_idx); + //pool_release(&s->yuv_front_pool, yuv_idx); + + // send frame event + { + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + + auto framed = event.initFrontFrame(); + framed.setFrameId(frame_data.frame_id); + framed.setEncodeId(cnt); + framed.setTimestampEof(frame_data.timestamp_eof); + framed.setFrameLength(frame_data.frame_length); + framed.setIntegLines(frame_data.integ_lines); + framed.setGlobalGain(frame_data.global_gain); + framed.setLensPos(frame_data.lens_pos); + framed.setLensSag(frame_data.lens_sag); + framed.setLensErr(frame_data.lens_err); + framed.setLensTruePos(frame_data.lens_true_pos); + framed.setGainFrac(frame_data.gain_frac); + framed.setFrameType(cereal::FrameData::FrameType::FRONT); + + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + s->front_frame_sock->send((char*)bytes.begin(), bytes.size()); + } + + /*FILE *f = fopen("/tmp/test2", "wb"); + printf("%d %d\n", s->rgb_front_height, s->rgb_front_stride); + fwrite(bgr_front_ptr, 1, s->rgb_front_stride * s->rgb_front_height, f); + fclose(f);*/ + + tbuffer_dispatch(&s->ui_front_tb, ui_idx); + + double t2 = millis_since_boot(); + + //LOGD("front process: %.2fms", t2-t1); + } + + delete monitoring_sock; + + return NULL; +} +// processing +void* processing_thread(void *arg) { + int err; + VisionState *s = (VisionState*)arg; + + set_thread_name("processing"); + + err = set_realtime_priority(1); + LOG("setpriority returns %d", err); + + // init cl stuff + const cl_queue_properties props[] = {0}; //CL_QUEUE_PRIORITY_KHR, CL_QUEUE_PRIORITY_HIGH_KHR, 0}; + cl_command_queue q = clCreateCommandQueueWithProperties(s->context, s->device_id, props, &err); + assert(err == 0); + + // init the net + LOG("processing start!"); + + for (int cnt = 0; !do_exit; cnt++) { + int buf_idx = tbuffer_acquire(&s->cameras.rear.camera_tb); + // int buf_idx = camera_acquire_buffer(s); + if (buf_idx < 0) { + break; + } + + double t1 = millis_since_boot(); + + FrameMetadata frame_data = s->cameras.rear.camera_bufs_metadata[buf_idx]; + uint32_t frame_id = frame_data.frame_id; + + if (frame_id == -1) { + LOGE("no frame data? wtf"); + tbuffer_release(&s->cameras.rear.camera_tb, buf_idx); + continue; + } + + int ui_idx = tbuffer_select(&s->ui_tb); + int rgb_idx = ui_idx; + + cl_event debayer_event; + if (s->cameras.rear.ci.bayer) { + err = clSetKernelArg(s->krnl_debayer_rear, 0, sizeof(cl_mem), &s->camera_bufs_cl[buf_idx]); + cl_check_error(err); + err = clSetKernelArg(s->krnl_debayer_rear, 1, sizeof(cl_mem), &s->rgb_bufs_cl[rgb_idx]); + cl_check_error(err); + err = clSetKernelArg(s->krnl_debayer_rear, 2, sizeof(float), &s->cameras.rear.digital_gain); + assert(err == 0); + + const size_t debayer_work_size = s->rgb_height; // doesn't divide evenly, is this okay? + const size_t debayer_local_work_size = 128; + err = clEnqueueNDRangeKernel(q, s->krnl_debayer_rear, 1, NULL, + &debayer_work_size, &debayer_local_work_size, 0, 0, &debayer_event); + assert(err == 0); + } else { + assert(s->rgb_buf_size >= s->frame_size); + assert(s->rgb_stride == s->frame_stride); + err = clEnqueueCopyBuffer(q, s->camera_bufs_cl[buf_idx], s->rgb_bufs_cl[rgb_idx], + 0, 0, s->rgb_buf_size, 0, 0, &debayer_event); + assert(err == 0); + } + + clWaitForEvents(1, &debayer_event); + clReleaseEvent(debayer_event); + + tbuffer_release(&s->cameras.rear.camera_tb, buf_idx); + + visionbuf_sync(&s->rgb_bufs[rgb_idx], VISIONBUF_SYNC_FROM_DEVICE); + + + double t2 = millis_since_boot(); + + uint8_t *bgr_ptr = (uint8_t*)s->rgb_bufs[rgb_idx].addr; + + double yt1 = millis_since_boot(); + + int yuv_idx = pool_select(&s->yuv_pool); + + s->yuv_metas[yuv_idx] = frame_data; + + uint8_t* yuv_ptr_y = s->yuv_bufs[yuv_idx].y; + uint8_t* yuv_ptr_u = s->yuv_bufs[yuv_idx].u; + uint8_t* yuv_ptr_v = s->yuv_bufs[yuv_idx].v; + cl_mem yuv_cl = s->yuv_cl[yuv_idx]; + rgb_to_yuv_queue(&s->rgb_to_yuv_state, q, s->rgb_bufs_cl[rgb_idx], yuv_cl); + visionbuf_sync(&s->yuv_ion[yuv_idx], VISIONBUF_SYNC_FROM_DEVICE); + + double yt2 = millis_since_boot(); + + // keep another reference around till were done processing + pool_acquire(&s->yuv_pool, yuv_idx); + pool_push(&s->yuv_pool, yuv_idx); + + // send frame event + { + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + + auto framed = event.initFrame(); + framed.setFrameId(frame_data.frame_id); + framed.setEncodeId(cnt); + framed.setTimestampEof(frame_data.timestamp_eof); + framed.setFrameLength(frame_data.frame_length); + framed.setIntegLines(frame_data.integ_lines); + framed.setGlobalGain(frame_data.global_gain); + framed.setLensPos(frame_data.lens_pos); + framed.setLensSag(frame_data.lens_sag); + framed.setLensErr(frame_data.lens_err); + framed.setLensTruePos(frame_data.lens_true_pos); + framed.setGainFrac(frame_data.gain_frac); + +#ifndef QCOM + framed.setImage(kj::arrayPtr((const uint8_t*)s->yuv_ion[yuv_idx].addr, s->yuv_buf_size)); +#endif + + kj::ArrayPtr transform_vs(&s->yuv_transform.v[0], 9); + framed.setTransform(transform_vs); + + if (s->frame_sock != NULL) { + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + s->frame_sock->send((char*)bytes.begin(), bytes.size()); + } + } + + // one thumbnail per 5 seconds (instead of %5 == 0 posenet) + if (cnt % 100 == 3) { + uint8_t* thumbnail_buffer = NULL; + uint64_t thumbnail_len = 0; + + unsigned char *row = (unsigned char *)malloc(s->rgb_width/4*3); + + struct jpeg_compress_struct cinfo; + struct jpeg_error_mgr jerr; + + cinfo.err = jpeg_std_error(&jerr); + jpeg_create_compress(&cinfo); + jpeg_mem_dest(&cinfo, &thumbnail_buffer, &thumbnail_len); + + cinfo.image_width = s->rgb_width / 4; + cinfo.image_height = s->rgb_height / 4; + cinfo.input_components = 3; + cinfo.in_color_space = JCS_RGB; + + jpeg_set_defaults(&cinfo); + jpeg_set_quality(&cinfo, 50, true); + jpeg_start_compress(&cinfo, true); + + JSAMPROW row_pointer[1]; + for (int i = 0; i < s->rgb_height - 4; i+=4) { + for (int j = 0; j < s->rgb_width*3; j+=12) { + for (int k = 0; k < 3; k++) { + uint16_t dat = 0; + dat += bgr_ptr[s->rgb_stride*i + j + k]; + dat += bgr_ptr[s->rgb_stride*i + j+3 + k]; + dat += bgr_ptr[s->rgb_stride*(i+1) + j + k]; + dat += bgr_ptr[s->rgb_stride*(i+1) + j+3 + k]; + dat += bgr_ptr[s->rgb_stride*(i+2) + j + k]; + dat += bgr_ptr[s->rgb_stride*(i+2) + j+3 + k]; + dat += bgr_ptr[s->rgb_stride*(i+3) + j + k]; + dat += bgr_ptr[s->rgb_stride*(i+3) + j+3 + k]; + + row[(j/4) + (2-k)] = dat/8; + } + } + row_pointer[0] = row; + jpeg_write_scanlines(&cinfo, row_pointer, 1); + } + free(row); + jpeg_finish_compress(&cinfo); + + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + + auto thumbnaild = event.initThumbnail(); + thumbnaild.setFrameId(frame_data.frame_id); + thumbnaild.setTimestampEof(frame_data.timestamp_eof); + thumbnaild.setThumbnail(kj::arrayPtr((const uint8_t*)thumbnail_buffer, thumbnail_len)); + + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + if (s->thumbnail_sock != NULL) { + s->thumbnail_sock->send((char*)bytes.begin(), bytes.size()); + } + + free(thumbnail_buffer); + } + + tbuffer_dispatch(&s->ui_tb, ui_idx); + + // auto exposure over big box + const int exposure_x = 290; + const int exposure_y = 282 + 40; + const int exposure_height = 314; + const int exposure_width = 560; + if (cnt % 3 == 0) { + // find median box luminance for AE + uint32_t lum_binning[256] = {0,}; + for (int y=0; yyuv_width) + exposure_x + x]; + lum_binning[lum]++; + } + } + const unsigned int lum_total = exposure_height * exposure_width; + unsigned int lum_cur = 0; + int lum_med = 0; + for (lum_med=0; lum_med<256; lum_med++) { + // shouldn't be any values less than 16 - yuv footroom + lum_cur += lum_binning[lum_med]; + if (lum_cur >= lum_total / 2) { + break; + } + } + // double avg = (double)acc / (big_box_width * big_box_height) - 16; + // printf("avg %d\n", lum_med); + + camera_autoexposure(&s->cameras.rear, lum_med / 256.0); + } + + pool_release(&s->yuv_pool, yuv_idx); + double t5 = millis_since_boot(); + LOGD("queued: %.2fms, yuv: %.2f, | processing: %.3fms", (t2-t1), (yt2-yt1), (t5-t1)); + } + + return NULL; +} + +// visionserver +void* visionserver_client_thread(void* arg) { + int err; + VisionClientState *client = (VisionClientState*)arg; + VisionState *s = client->s; + int fd = client->fd; + + set_thread_name("clientthread"); + + zsock_t *terminate = zsock_new_sub(">inproc://terminate", ""); + assert(terminate); + void* terminate_raw = zsock_resolve(terminate); + + VisionClientStreamState streams[VISION_STREAM_MAX] = {{0}}; + + LOGW("client start fd %d", fd); + + while (true) { + zmq_pollitem_t polls[2+VISION_STREAM_MAX] = {{0}}; + polls[0].socket = terminate_raw; + polls[0].events = ZMQ_POLLIN; + polls[1].fd = fd; + polls[1].events = ZMQ_POLLIN; + + int poll_to_stream[2+VISION_STREAM_MAX] = {0}; + int num_polls = 2; + for (int i=0; i= 2) { + continue; + } + if (streams[i].tb) { + polls[num_polls].fd = tbuffer_efd(streams[i].tbuffer); + } else { + polls[num_polls].fd = poolq_efd(streams[i].queue); + } + poll_to_stream[num_polls] = i; + num_polls++; + } + int ret = zmq_poll(polls, num_polls, -1); + if (ret < 0) { + if (errno == EINTR) continue; + LOGE("poll failed (%d)", ret); + break; + } + if (polls[0].revents) { + break; + } else if (polls[1].revents) { + VisionPacket p; + err = vipc_recv(fd, &p); + // printf("recv %d\n", p.type); + if (err <= 0) { + break; + } else if (p.type == VIPC_STREAM_SUBSCRIBE) { + VisionStreamType stream_type = p.d.stream_sub.type; + VisionPacket rep = { + .type = VIPC_STREAM_BUFS, + .d = { .stream_bufs = { .type = stream_type }, }, + }; + + VisionClientStreamState *stream = &streams[stream_type]; + stream->tb = p.d.stream_sub.tbuffer; + + VisionStreamBufs *stream_bufs = &rep.d.stream_bufs; + if (stream_type == VISION_STREAM_RGB_BACK) { + stream_bufs->width = s->rgb_width; + stream_bufs->height = s->rgb_height; + stream_bufs->stride = s->rgb_stride; + stream_bufs->buf_len = s->rgb_bufs[0].len; + rep.num_fds = UI_BUF_COUNT; + for (int i=0; irgb_bufs[i].fd; + } + if (stream->tb) { + stream->tbuffer = &s->ui_tb; + } else { + assert(false); + } + } else if (stream_type == VISION_STREAM_RGB_FRONT) { + stream_bufs->width = s->rgb_front_width; + stream_bufs->height = s->rgb_front_height; + stream_bufs->stride = s->rgb_front_stride; + stream_bufs->buf_len = s->rgb_front_bufs[0].len; + rep.num_fds = UI_BUF_COUNT; + for (int i=0; irgb_front_bufs[i].fd; + } + if (stream->tb) { + stream->tbuffer = &s->ui_front_tb; + } else { + assert(false); + } + } else if (stream_type == VISION_STREAM_YUV) { + stream_bufs->width = s->yuv_width; + stream_bufs->height = s->yuv_height; + stream_bufs->stride = s->yuv_width; + stream_bufs->buf_len = s->yuv_buf_size; + rep.num_fds = YUV_COUNT; + for (int i=0; iyuv_ion[i].fd; + } + if (stream->tb) { + stream->tbuffer = s->yuv_tb; + } else { + stream->queue = pool_get_queue(&s->yuv_pool); + } + } else if (stream_type == VISION_STREAM_YUV_FRONT) { + stream_bufs->width = s->yuv_front_width; + stream_bufs->height = s->yuv_front_height; + stream_bufs->stride = s->yuv_front_width; + stream_bufs->buf_len = s->yuv_front_buf_size; + rep.num_fds = YUV_COUNT; + for (int i=0; iyuv_front_ion[i].fd; + } + if (stream->tb) { + stream->tbuffer = s->yuv_front_tb; + } else { + stream->queue = pool_get_queue(&s->yuv_front_pool); + } + } else { + assert(false); + } + vipc_send(fd, &rep); + streams[stream_type].subscribed = true; + } else if (p.type == VIPC_STREAM_RELEASE) { + // printf("client release f %d %d\n", p.d.stream_rel.type, p.d.stream_rel.idx); + int si = p.d.stream_rel.type; + assert(si < VISION_STREAM_MAX); + if (streams[si].tb) { + tbuffer_release(streams[si].tbuffer, p.d.stream_rel.idx); + } else { + poolq_release(streams[si].queue, p.d.stream_rel.idx); + } + streams[p.d.stream_rel.type].bufs_outstanding--; + } else { + assert(false); + } + } else { + int stream_i = VISION_STREAM_MAX; + for (int i=2; iyuv_metas[idx].frame_id; + rep.d.stream_acq.extra.timestamp_eof = s->yuv_metas[idx].timestamp_eof; + } else if (stream_i == VISION_STREAM_YUV_FRONT) { + rep.d.stream_acq.extra.frame_id = s->yuv_front_metas[idx].frame_id; + rep.d.stream_acq.extra.timestamp_eof = s->yuv_front_metas[idx].timestamp_eof; + } + vipc_send(fd, &rep); + } + } + } + + LOGW("client end fd %d", fd); + + for (int i=0; iclients_lock); + client->running = false; + pthread_mutex_unlock(&s->clients_lock); + + return NULL; +} + +void* visionserver_thread(void* arg) { + int err; + VisionState *s = (VisionState*)arg; + + set_thread_name("visionserver"); + + zsock_t *terminate = zsock_new_sub(">inproc://terminate", ""); + assert(terminate); + void* terminate_raw = zsock_resolve(terminate); + + unlink(VIPC_SOCKET_PATH); + + int sock = socket(AF_UNIX, SOCK_SEQPACKET, 0); + struct sockaddr_un addr = { + .sun_family = AF_UNIX, + .sun_path = VIPC_SOCKET_PATH, + }; + err = bind(sock, (struct sockaddr *)&addr, sizeof(addr)); + assert(err == 0); + + err = listen(sock, 3); + assert(err == 0); + + // printf("waiting\n"); + + while (!do_exit) { + zmq_pollitem_t polls[2] = {{0}}; + polls[0].socket = terminate_raw; + polls[0].events = ZMQ_POLLIN; + polls[1].fd = sock; + polls[1].events = ZMQ_POLLIN; + + int ret = zmq_poll(polls, ARRAYSIZE(polls), -1); + if (ret < 0) { + LOGE("poll failed (%d)", ret); + break; + } + if (polls[0].revents) { + break; + } else if (!polls[1].revents) { + continue; + } + + int fd = accept(sock, NULL, NULL); + assert(fd >= 0); + + pthread_mutex_lock(&s->clients_lock); + + int client_idx = 0; + for (; client_idx < MAX_CLIENTS; client_idx++) { + if (!s->clients[client_idx].running) break; + } + + if (client_idx >= MAX_CLIENTS) { + LOG("ignoring visionserver connection, max clients connected"); + close(fd); + + pthread_mutex_unlock(&s->clients_lock); + continue; + } + + VisionClientState *client = &s->clients[client_idx]; + client->s = s; + client->fd = fd; + client->running = true; + + err = pthread_create(&client->thread_handle, NULL, + visionserver_client_thread, client); + assert(err == 0); + + pthread_mutex_unlock(&s->clients_lock); + } + + for (int i=0; iclients_lock); + bool running = s->clients[i].running; + pthread_mutex_unlock(&s->clients_lock); + if (running) { + err = pthread_join(s->clients[i].thread_handle, NULL); + assert(err == 0); + } + } + + close(sock); + zsock_destroy(&terminate); + + return NULL; +} + + +////////// cl stuff + +cl_program build_debayer_program(VisionState *s, + int frame_width, int frame_height, int frame_stride, + int rgb_width, int rgb_height, int rgb_stride, + int bayer_flip, int hdr) { + assert(rgb_width == frame_width/2); + assert(rgb_height == frame_height/2); + + char args[4096]; + snprintf(args, sizeof(args), + "-cl-fast-relaxed-math -cl-denorms-are-zero " + "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d " + "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d " + "-DBAYER_FLIP=%d -DHDR=%d", + frame_width, frame_height, frame_stride, + rgb_width, rgb_height, rgb_stride, + bayer_flip, hdr); + return CLU_LOAD_FROM_FILE(s->context, s->device_id, "cameras/debayer.cl", args); +} + +void cl_init(VisionState *s) { + int err; + cl_platform_id platform_id = NULL; + cl_uint num_devices; + cl_uint num_platforms; + + err = clGetPlatformIDs(1, &platform_id, &num_platforms); + assert(err == 0); + err = clGetDeviceIDs(platform_id, CL_DEVICE_TYPE_DEFAULT, 1, + &s->device_id, &num_devices); + assert(err == 0); + + cl_print_info(platform_id, s->device_id); + printf("\n"); + + s->context = clCreateContext(NULL, 1, &s->device_id, NULL, NULL, &err); + assert(err == 0); +} + +void cl_free(VisionState *s) { + int err; + + err = clReleaseContext(s->context); + assert(err == 0); +} + +void init_buffers(VisionState *s) { + int err; + + // allocate camera buffers + + for (int i=0; icamera_bufs[i] = visionbuf_allocate_cl(s->frame_size, s->device_id, s->context, + &s->camera_bufs_cl[i]); + // TODO: make lengths correct + s->focus_bufs[i] = visionbuf_allocate(0xb80); + s->stats_bufs[i] = visionbuf_allocate(0xb80); + } + + for (int i=0; ifront_camera_bufs[i] = visionbuf_allocate_cl(s->cameras.front.frame_size, + s->device_id, s->context, + &s->front_camera_bufs_cl[i]); + } + + // processing buffers + if (s->cameras.rear.ci.bayer) { + s->rgb_width = s->frame_width/2; + s->rgb_height = s->frame_height/2; + } else { + s->rgb_width = s->frame_width; + s->rgb_height = s->frame_height; + } + + for (int i=0; irgb_width, s->rgb_height, &s->rgb_bufs[i]); + s->rgb_bufs_cl[i] = visionbuf_to_cl(&s->rgb_bufs[i], s->device_id, s->context); + if (i == 0){ + s->rgb_stride = img.stride; + s->rgb_buf_size = img.size; + } + } + tbuffer_init(&s->ui_tb, UI_BUF_COUNT, "rgb"); + + //assert(s->cameras.front.ci.bayer); + s->rgb_front_width = s->cameras.front.ci.frame_width/2; + s->rgb_front_height = s->cameras.front.ci.frame_height/2; + + for (int i=0; irgb_front_width, s->rgb_front_height, &s->rgb_front_bufs[i]); + s->rgb_front_bufs_cl[i] = visionbuf_to_cl(&s->rgb_front_bufs[i], s->device_id, s->context); + if (i == 0){ + s->rgb_front_stride = img.stride; + s->rgb_front_buf_size = img.size; + } + } + tbuffer_init(&s->ui_front_tb, UI_BUF_COUNT, "frontrgb"); + + // yuv back for recording and orbd + pool_init(&s->yuv_pool, YUV_COUNT); + s->yuv_tb = pool_get_tbuffer(&s->yuv_pool); //only for visionserver... + + s->yuv_width = s->rgb_width; + s->yuv_height = s->rgb_height; + s->yuv_buf_size = s->rgb_width * s->rgb_height * 3 / 2; + + for (int i=0; iyuv_ion[i] = visionbuf_allocate_cl(s->yuv_buf_size, s->device_id, s->context, &s->yuv_cl[i]); + s->yuv_bufs[i].y = (uint8_t*)s->yuv_ion[i].addr; + s->yuv_bufs[i].u = s->yuv_bufs[i].y + (s->yuv_width * s->yuv_height); + s->yuv_bufs[i].v = s->yuv_bufs[i].u + (s->yuv_width/2 * s->yuv_height/2); + } + + // yuv front for recording + pool_init(&s->yuv_front_pool, YUV_COUNT); + s->yuv_front_tb = pool_get_tbuffer(&s->yuv_front_pool); + + s->yuv_front_width = s->rgb_front_width; + s->yuv_front_height = s->rgb_front_height; + s->yuv_front_buf_size = s->rgb_front_width * s->rgb_front_height * 3 / 2; + + for (int i=0; iyuv_front_ion[i] = visionbuf_allocate_cl(s->yuv_front_buf_size, s->device_id, s->context, &s->yuv_front_cl[i]); + s->yuv_front_bufs[i].y = (uint8_t*)s->yuv_front_ion[i].addr; + s->yuv_front_bufs[i].u = s->yuv_front_bufs[i].y + (s->yuv_front_width * s->yuv_front_height); + s->yuv_front_bufs[i].v = s->yuv_front_bufs[i].u + (s->yuv_front_width/2 * s->yuv_front_height/2); + } + + if (s->cameras.rear.ci.bayer) { + // debayering does a 2x downscale + s->yuv_transform = transform_scale_buffer(s->cameras.rear.transform, 0.5); + } else { + s->yuv_transform = s->cameras.rear.transform; + } + + if (s->cameras.rear.ci.bayer) { + s->prg_debayer_rear = build_debayer_program(s, s->cameras.rear.ci.frame_width, s->cameras.rear.ci.frame_height, + s->cameras.rear.ci.frame_stride, + s->rgb_width, s->rgb_height, s->rgb_stride, + s->cameras.rear.ci.bayer_flip, s->cameras.rear.ci.hdr); + s->krnl_debayer_rear = clCreateKernel(s->prg_debayer_rear, "debayer10", &err); + assert(err == 0); + } + + if (s->cameras.front.ci.bayer) { + s->prg_debayer_front = build_debayer_program(s, s->cameras.front.ci.frame_width, s->cameras.front.ci.frame_height, + s->cameras.front.ci.frame_stride, + s->rgb_front_width, s->rgb_front_height, s->rgb_front_stride, + s->cameras.front.ci.bayer_flip, s->cameras.front.ci.hdr); + + s->krnl_debayer_front = clCreateKernel(s->prg_debayer_front, "debayer10", &err); + assert(err == 0); + } + + rgb_to_yuv_init(&s->rgb_to_yuv_state, s->context, s->device_id, s->yuv_width, s->yuv_height, s->rgb_stride); + rgb_to_yuv_init(&s->front_rgb_to_yuv_state, s->context, s->device_id, s->yuv_front_width, s->yuv_front_height, s->rgb_front_stride); +} + +void free_buffers(VisionState *s) { + // free bufs + for (int i=0; icamera_bufs[i]); + visionbuf_free(&s->focus_bufs[i]); + visionbuf_free(&s->stats_bufs[i]); + } + + for (int i=0; ifront_camera_bufs[i]); + } + + for (int i=0; irgb_bufs[i]); + } + + for (int i=0; irgb_front_bufs[i]); + } + + for (int i=0; iyuv_ion[i]); + } +} + +void party(VisionState *s) { + int err; + + s->terminate_pub = zsock_new_pub("@inproc://terminate"); + assert(s->terminate_pub); + + pthread_t visionserver_thread_handle; + err = pthread_create(&visionserver_thread_handle, NULL, + visionserver_thread, s); + assert(err == 0); + + pthread_t proc_thread_handle; + err = pthread_create(&proc_thread_handle, NULL, + processing_thread, s); + assert(err == 0); + + pthread_t frontview_thread_handle; + err = pthread_create(&frontview_thread_handle, NULL, + frontview_thread, s); + assert(err == 0); + + // priority for cameras + err = set_realtime_priority(1); + LOG("setpriority returns %d", err); + + cameras_run(&s->cameras); + + tbuffer_stop(&s->ui_tb); + tbuffer_stop(&s->ui_front_tb); + pool_stop(&s->yuv_pool); + pool_stop(&s->yuv_front_pool); + + zsock_signal(s->terminate_pub, 0); + + LOG("joining frontview_thread"); + err = pthread_join(frontview_thread_handle, NULL); + assert(err == 0); + + LOG("joining visionserver_thread"); + err = pthread_join(visionserver_thread_handle, NULL); + assert(err == 0); + + LOG("joining proc_thread"); + err = pthread_join(proc_thread_handle, NULL); + assert(err == 0); + + zsock_destroy (&s->terminate_pub); +} + +int main(int argc, char *argv[]) { + int err; + set_realtime_priority(1); + + zsys_handler_set(NULL); + signal(SIGINT, (sighandler_t)set_do_exit); + signal(SIGTERM, (sighandler_t)set_do_exit); + + VisionState state = {0}; + VisionState *s = &state; + + clu_init(); + cl_init(s); + + cameras_init(&s->cameras); + + s->frame_width = s->cameras.rear.ci.frame_width; + s->frame_height = s->cameras.rear.ci.frame_height; + s->frame_stride = s->cameras.rear.ci.frame_stride; + s->frame_size = s->cameras.rear.frame_size; + + init_buffers(s); + +#ifdef QCOM + s->msg_context = Context::create(); + s->frame_sock = PubSocket::create(s->msg_context, "frame"); + s->front_frame_sock = PubSocket::create(s->msg_context, "frontFrame"); + s->thumbnail_sock = PubSocket::create(s->msg_context, "thumbnail"); + assert(s->frame_sock != NULL); + assert(s->front_frame_sock != NULL); + assert(s->thumbnail_sock != NULL); +#endif + + cameras_open(&s->cameras, &s->camera_bufs[0], &s->focus_bufs[0], &s->stats_bufs[0], &s->front_camera_bufs[0]); + + party(s); + +#ifdef QCOM + delete s->frame_sock; + delete s->front_frame_sock; + delete s->thumbnail_sock; + delete s->msg_context; +#endif + + free_buffers(s); + cl_free(s); +} diff --git a/selfdrive/camerad/snapshot/__init__.py b/selfdrive/camerad/snapshot/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/camerad/snapshot/snapshot.py b/selfdrive/camerad/snapshot/snapshot.py new file mode 100755 index 000000000..0da8d6797 --- /dev/null +++ b/selfdrive/camerad/snapshot/snapshot.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 +import os +import json +import signal +import subprocess +import time +from PIL import Image +from common.basedir import BASEDIR +from common.params import Params +from selfdrive.camerad.snapshot.visionipc import VisionIPC + +with open(BASEDIR + "/selfdrive/controls/lib/alerts_offroad.json") as json_file: + OFFROAD_ALERTS = json.load(json_file) + + +def jpeg_write(fn, dat): + img = Image.fromarray(dat) + img.save(fn, "JPEG") + + +def snapshot(): + params = Params() + front_camera_allowed = int(params.get("RecordFront")) + + params.put("IsTakingSnapshot", "1") + params.put("Offroad_IsTakingSnapshot", json.dumps(OFFROAD_ALERTS["Offroad_IsTakingSnapshot"])) + time.sleep(2.0) # Give thermald time to read the param, or if just started give camerad time to start + + # Check if camerad is already started + ps = subprocess.Popen("ps | grep camerad", shell=True, stdout=subprocess.PIPE) + ret = list(filter(lambda x: 'grep ' not in x, ps.communicate()[0].decode('utf-8').strip().split("\n"))) + if len(ret) > 0: + params.put("IsTakingSnapshot", "0") + params.delete("Offroad_IsTakingSnapshot") + return None + + proc = subprocess.Popen(os.path.join(BASEDIR, "selfdrive/camerad/camerad"), cwd=os.path.join(BASEDIR, "selfdrive/camerad")) + time.sleep(3.0) + + ret = None + start_time = time.time() + while time.time() - start_time < 5.0: + try: + ipc = VisionIPC() + pic = ipc.get() + del ipc + + if front_camera_allowed: + ipc_front = VisionIPC(front=True) + fpic = ipc_front.get() + del ipc_front + else: + fpic = None + + ret = pic, fpic + break + except Exception: + time.sleep(1) + + proc.send_signal(signal.SIGINT) + proc.communicate() + + params.put("IsTakingSnapshot", "0") + params.delete("Offroad_IsTakingSnapshot") + return ret + + +if __name__ == "__main__": + pic, fpic = snapshot() + print(pic.shape) + jpeg_write("/tmp/back.jpg", pic) + jpeg_write("/tmp/front.jpg", fpic) diff --git a/selfdrive/camerad/snapshot/visionipc.py b/selfdrive/camerad/snapshot/visionipc.py new file mode 100644 index 000000000..816db4120 --- /dev/null +++ b/selfdrive/camerad/snapshot/visionipc.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python3 +import os +from cffi import FFI + +import numpy as np + +gf_dir = os.path.dirname(os.path.abspath(__file__)) + +ffi = FFI() +ffi.cdef(""" + +typedef enum VisionStreamType { + VISION_STREAM_RGB_BACK, + VISION_STREAM_RGB_FRONT, + VISION_STREAM_YUV, + VISION_STREAM_YUV_FRONT, + VISION_STREAM_MAX, +} VisionStreamType; + +typedef struct VisionUIInfo { + int big_box_x, big_box_y; + int big_box_width, big_box_height; + int transformed_width, transformed_height; + + int front_box_x, front_box_y; + int front_box_width, front_box_height; +} VisionUIInfo; + +typedef struct VisionStreamBufs { + VisionStreamType type; + + int width, height, stride; + size_t buf_len; + + union { + VisionUIInfo ui_info; + } buf_info; +} VisionStreamBufs; + +typedef struct VIPCBuf { + int fd; + size_t len; + void* addr; +} VIPCBuf; + +typedef struct VIPCBufExtra { + // only for yuv + uint32_t frame_id; + uint64_t timestamp_eof; +} VIPCBufExtra; + +typedef struct VisionStream { + int ipc_fd; + int last_idx; + int last_type; + int num_bufs; + VisionStreamBufs bufs_info; + VIPCBuf *bufs; +} VisionStream; + +int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info); +VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra); +void visionstream_destroy(VisionStream *s); + +""" +) + + +class VisionIPCError(Exception): + pass + + +class VisionIPC(): + def __init__(self, front=False): + self.clib = ffi.dlopen(os.path.join(gf_dir, "libvisionipc.so")) + + self.s = ffi.new("VisionStream*") + self.buf_info = ffi.new("VisionStreamBufs*") + + err = self.clib.visionstream_init(self.s, self.clib.VISION_STREAM_RGB_FRONT if front else self.clib.VISION_STREAM_RGB_BACK, True, self.buf_info) + + if err != 0: + self.clib.visionstream_destroy(self.s) + raise VisionIPCError + + def __del__(self): + self.clib.visionstream_destroy(self.s) + + def get(self): + buf = self.clib.visionstream_get(self.s, ffi.NULL) + pbuf = ffi.buffer(buf.addr, buf.len) + ret = np.frombuffer(pbuf, dtype=np.uint8).reshape((-1, self.buf_info.stride//3, 3)) + return ret[:self.buf_info.height, :self.buf_info.width, [2,1,0]] diff --git a/selfdrive/camerad/test/camera/test.c b/selfdrive/camerad/test/camera/test.c new file mode 100644 index 000000000..8a7d44517 --- /dev/null +++ b/selfdrive/camerad/test/camera/test.c @@ -0,0 +1,48 @@ +#include +#include +#include + +#include "camera_qcom.h" + +bool do_exit = false; + +void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func, + const char* fmt, ...) { + va_list args; + va_start(args, fmt); + vprintf(fmt, args); + printf("\n"); +} + +void set_thread_name(const char* name) { +} + +// tbuffers + +void tbuffer_init2(TBuffer *tb, int num_bufs, const char* name, + void (*release_cb)(void* c, int idx), + void* cb_cookie) { + printf("tbuffer_init2\n"); +} + +void tbuffer_dispatch(TBuffer *tb, int idx) { + printf("tbuffer_dispatch\n"); +} + +void tbuffer_stop(TBuffer *tb) { + printf("tbuffer_stop\n"); +} + +int main() { + DualCameraState s; + cameras_init(&s); + VisionBuf camera_bufs_rear[0x10] = {0}; + VisionBuf camera_bufs_focus[0x10] = {0}; + VisionBuf camera_bufs_stats[0x10] = {0}; + VisionBuf camera_bufs_front[0x10] = {0}; + cameras_open(&s, + camera_bufs_rear, camera_bufs_focus, + camera_bufs_stats, camera_bufs_front); + cameras_close(&s); +} + diff --git a/selfdrive/camerad/test/camera/test.sh b/selfdrive/camerad/test/camera/test.sh new file mode 100755 index 000000000..80faa4767 --- /dev/null +++ b/selfdrive/camerad/test/camera/test.sh @@ -0,0 +1,10 @@ +#!/bin/sh + +gcc -DQCOM -I ~/one -I ~/one/selfdrive -I ../../include \ + -I ~/one/phonelibs/android_system_core/include -I ~/one/phonelibs/opencl/include \ + -I ~/one/selfdrive/visiond/cameras \ + test.c ../../cameras/camera_qcom.c \ + -l:libczmq.a -l:libzmq.a -lgnustl_shared -lm -llog -lcutils \ + -l:libcapn.a -l:libcapnp.a -l:libkj.a \ + ~/one/cereal/gen/c/log.capnp.o + diff --git a/selfdrive/camerad/test/frame_test.py b/selfdrive/camerad/test/frame_test.py new file mode 100755 index 000000000..802471ea9 --- /dev/null +++ b/selfdrive/camerad/test/frame_test.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python3 +import time +import numpy as np +import cereal.messaging as messaging +from PIL import ImageFont, ImageDraw, Image + +font = ImageFont.truetype("arial", size=72) +def get_frame(idx): + img = np.zeros((874, 1164, 3), np.uint8) + img[100:400, 100:100+(idx%10)*100] = 255 + + # big number + im2 = Image.new("RGB", (200,200)) + draw = ImageDraw.Draw(im2) + draw.text((10, 100), "%02d" % idx, font=font) + img[400:600, 400:600] = np.array(im2.getdata()).reshape((200,200,3)) + return img.tostring() + +if __name__ == "__main__": + from common.realtime import Ratekeeper + rk = Ratekeeper(20) + + pm = messaging.PubMaster(['frame']) + frm = [get_frame(x) for x in range(30)] + idx = 0 + while 1: + print("send %d" % idx) + dat = messaging.new_message() + dat.init('frame') + dat.valid = True + dat.frame = { + "frameId": idx, + "image": frm[idx%len(frm)], + } + pm.send('frame', dat) + + idx += 1 + rk.keep_time() + #time.sleep(1.0) + diff --git a/selfdrive/camerad/test/stress_restart.sh b/selfdrive/camerad/test/stress_restart.sh new file mode 100755 index 000000000..0445dcba7 --- /dev/null +++ b/selfdrive/camerad/test/stress_restart.sh @@ -0,0 +1,9 @@ +#!/bin/sh +cd .. +while :; do + ./camerad & + pid="$!" + sleep 2 + kill -2 $pid + wait $pid +done diff --git a/selfdrive/camerad/test/yuv_bench/Makefile b/selfdrive/camerad/test/yuv_bench/Makefile new file mode 100644 index 000000000..4a47356d1 --- /dev/null +++ b/selfdrive/camerad/test/yuv_bench/Makefile @@ -0,0 +1,57 @@ +CC = clang +CXX = clang++ + +PHONELIBS = ../../../../phonelibs + +WARN_FLAGS = -Werror=implicit-function-declaration \ + -Werror=incompatible-pointer-types \ + -Werror=int-conversion \ + -Werror=return-type \ + -Werror=format-extra-args \ + -Wno-deprecated-declarations + +CFLAGS = -std=gnu11 -fPIC -O2 $(WARN_FLAGS) + +UNAME_M := $(shell uname -m) +ifeq ($(UNAME_M),x86_64) + OPENCL_LIBS = -framework OpenCL +else + OPENCL_FLAGS = -I$(PHONELIBS)/opencl/include + OPENCL_LIBS = -L/system/vendor/lib64 -lgsl -lCB -lOpenCL +endif + +OBJS += yuv_bench.o \ + ../../../common/util.o \ + ../../clutil.o + +OUTPUT = yuv + +.PHONY: all +all: $(OUTPUT) + +$(OUTPUT): $(OBJS) + @echo "[ LINK ] $@" + $(CXX) -fPIC -o '$@' $^ \ + -L/usr/lib \ + $(OPENCL_LIBS) + + +%.o: %.cc + @echo "[ CXX ] $@" + $(CXX) $(CXXFLAGS) -MMD \ + -I../.. -I../../.. -I ../../../.. \ + $(OPENCL_FLAGS) \ + -c -o '$@' '$<' + + +%.o: %.c + @echo "[ CC ] $@" + $(CC) $(CFLAGS) -MMD \ + -I../.. -I../../.. -I ../../../.. \ + $(OPENCL_FLAGS) \ + -c -o '$@' '$<' + +.PHONY: clean +clean: + rm -f $(OUTPUT) $(OBJS) $(DEPS) + diff --git a/selfdrive/camerad/test/yuv_bench/cnv.py b/selfdrive/camerad/test/yuv_bench/cnv.py new file mode 100644 index 000000000..86fd0c998 --- /dev/null +++ b/selfdrive/camerad/test/yuv_bench/cnv.py @@ -0,0 +1,19 @@ +import numpy as np +import cv2 + +# img_bgr = np.zeros((874, 1164, 3), dtype=np.uint8) +# for y in range(874): +# for k in range(1164*3): +# img_bgr[y, k//3, k%3] = k ^ y + +# cv2.imwrite("img_rgb.png", img_bgr) + + +cl = np.fromstring(open("out_cl.bin", "rb").read(), dtype=np.uint8) + +cl_r = cl.reshape(874 * 3 // 2, -1) + +cv2.imwrite("out_y.png", cl_r[:874]) + +cl_bgr = cv2.cvtColor(cl_r, cv2.COLOR_YUV2BGR_I420) +cv2.imwrite("out_cl.png", cl_bgr) diff --git a/selfdrive/camerad/test/yuv_bench/yuv.cl b/selfdrive/camerad/test/yuv_bench/yuv.cl new file mode 100644 index 000000000..03ce340a3 --- /dev/null +++ b/selfdrive/camerad/test/yuv_bench/yuv.cl @@ -0,0 +1,116 @@ + +#define PIX_PER_WI_X 1 +#define PIX_PER_WI_Y 1 + +#define scn 3 +#define bidx 2 +#define uidx 0 + +#define R_COMP x +#define G_COMP y +#define B_COMP z + +__constant float c_RGB2YUVCoeffs_420[8] = { 0.256999969f, 0.50399971f, 0.09799957f, -0.1479988098f, -0.2909994125f, + 0.438999176f, -0.3679990768f, -0.0709991455f }; + +__kernel void RGB2YUV_YV12_IYUV(__global const uchar* srcptr, int src_step, int src_offset, + __global uchar* dstptr, int dst_step, int dst_offset, + int rows, int cols) +{ + int x = get_global_id(0) * PIX_PER_WI_X; + int y = get_global_id(1) * PIX_PER_WI_Y; + + if (x < cols/2) + { + int src_index = mad24(y << 1, src_step, mad24(x << 1, scn, src_offset)); + int ydst_index = mad24(y << 1, dst_step, (x << 1) + dst_offset); + int y_rows = rows / 3 * 2; + int vsteps[2] = { cols >> 1, dst_step - (cols >> 1)}; + __constant float* coeffs = c_RGB2YUVCoeffs_420; + + #pragma unroll + for (int cy = 0; cy < PIX_PER_WI_Y; ++cy) + { + if (y < rows / 3) + { + __global const uchar* src1 = srcptr + src_index; + __global const uchar* src2 = src1 + src_step; + __global uchar* ydst1 = dstptr + ydst_index; + __global uchar* ydst2 = ydst1 + dst_step; + + __global uchar* udst = dstptr + mad24(y_rows + (y>>1), dst_step, dst_offset + (y%2)*(cols >> 1) + x); + __global uchar* vdst = udst + mad24(y_rows >> 2, dst_step, y_rows % 4 ? vsteps[y%2] : 0); + +#if PIX_PER_WI_X == 2 + int s11 = *((__global const int*) src1); + int s12 = *((__global const int*) src1 + 1); + int s13 = *((__global const int*) src1 + 2); +#if scn == 4 + int s14 = *((__global const int*) src1 + 3); +#endif + int s21 = *((__global const int*) src2); + int s22 = *((__global const int*) src2 + 1); + int s23 = *((__global const int*) src2 + 2); +#if scn == 4 + int s24 = *((__global const int*) src2 + 3); +#endif + float src_pix1[scn * 4], src_pix2[scn * 4]; + + *((float4*) src_pix1) = convert_float4(as_uchar4(s11)); + *((float4*) src_pix1 + 1) = convert_float4(as_uchar4(s12)); + *((float4*) src_pix1 + 2) = convert_float4(as_uchar4(s13)); +#if scn == 4 + *((float4*) src_pix1 + 3) = convert_float4(as_uchar4(s14)); +#endif + *((float4*) src_pix2) = convert_float4(as_uchar4(s21)); + *((float4*) src_pix2 + 1) = convert_float4(as_uchar4(s22)); + *((float4*) src_pix2 + 2) = convert_float4(as_uchar4(s23)); +#if scn == 4 + *((float4*) src_pix2 + 3) = convert_float4(as_uchar4(s24)); +#endif + uchar4 y1, y2; + y1.x = convert_uchar_sat(fma(coeffs[0], src_pix1[ 2-bidx], fma(coeffs[1], src_pix1[ 1], fma(coeffs[2], src_pix1[ bidx], 16.5f)))); + y1.y = convert_uchar_sat(fma(coeffs[0], src_pix1[ scn+2-bidx], fma(coeffs[1], src_pix1[ scn+1], fma(coeffs[2], src_pix1[ scn+bidx], 16.5f)))); + y1.z = convert_uchar_sat(fma(coeffs[0], src_pix1[2*scn+2-bidx], fma(coeffs[1], src_pix1[2*scn+1], fma(coeffs[2], src_pix1[2*scn+bidx], 16.5f)))); + y1.w = convert_uchar_sat(fma(coeffs[0], src_pix1[3*scn+2-bidx], fma(coeffs[1], src_pix1[3*scn+1], fma(coeffs[2], src_pix1[3*scn+bidx], 16.5f)))); + y2.x = convert_uchar_sat(fma(coeffs[0], src_pix2[ 2-bidx], fma(coeffs[1], src_pix2[ 1], fma(coeffs[2], src_pix2[ bidx], 16.5f)))); + y2.y = convert_uchar_sat(fma(coeffs[0], src_pix2[ scn+2-bidx], fma(coeffs[1], src_pix2[ scn+1], fma(coeffs[2], src_pix2[ scn+bidx], 16.5f)))); + y2.z = convert_uchar_sat(fma(coeffs[0], src_pix2[2*scn+2-bidx], fma(coeffs[1], src_pix2[2*scn+1], fma(coeffs[2], src_pix2[2*scn+bidx], 16.5f)))); + y2.w = convert_uchar_sat(fma(coeffs[0], src_pix2[3*scn+2-bidx], fma(coeffs[1], src_pix2[3*scn+1], fma(coeffs[2], src_pix2[3*scn+bidx], 16.5f)))); + + *((__global int*) ydst1) = as_int(y1); + *((__global int*) ydst2) = as_int(y2); + + float uv[4] = { fma(coeffs[3], src_pix1[ 2-bidx], fma(coeffs[4], src_pix1[ 1], fma(coeffs[5], src_pix1[ bidx], 128.5f))), + fma(coeffs[5], src_pix1[ 2-bidx], fma(coeffs[6], src_pix1[ 1], fma(coeffs[7], src_pix1[ bidx], 128.5f))), + fma(coeffs[3], src_pix1[2*scn+2-bidx], fma(coeffs[4], src_pix1[2*scn+1], fma(coeffs[5], src_pix1[2*scn+bidx], 128.5f))), + fma(coeffs[5], src_pix1[2*scn+2-bidx], fma(coeffs[6], src_pix1[2*scn+1], fma(coeffs[7], src_pix1[2*scn+bidx], 128.5f))) }; + + udst[0] = convert_uchar_sat(uv[uidx] ); + vdst[0] = convert_uchar_sat(uv[1 - uidx]); + udst[1] = convert_uchar_sat(uv[2 + uidx]); + vdst[1] = convert_uchar_sat(uv[3 - uidx]); +#else + float4 src_pix1 = convert_float4(vload4(0, src1)); + float4 src_pix2 = convert_float4(vload4(0, src1+scn)); + float4 src_pix3 = convert_float4(vload4(0, src2)); + float4 src_pix4 = convert_float4(vload4(0, src2+scn)); + + ydst1[0] = convert_uchar_sat(fma(coeffs[0], src_pix1.R_COMP, fma(coeffs[1], src_pix1.G_COMP, fma(coeffs[2], src_pix1.B_COMP, 16.5f)))); + ydst1[1] = convert_uchar_sat(fma(coeffs[0], src_pix2.R_COMP, fma(coeffs[1], src_pix2.G_COMP, fma(coeffs[2], src_pix2.B_COMP, 16.5f)))); + ydst2[0] = convert_uchar_sat(fma(coeffs[0], src_pix3.R_COMP, fma(coeffs[1], src_pix3.G_COMP, fma(coeffs[2], src_pix3.B_COMP, 16.5f)))); + ydst2[1] = convert_uchar_sat(fma(coeffs[0], src_pix4.R_COMP, fma(coeffs[1], src_pix4.G_COMP, fma(coeffs[2], src_pix4.B_COMP, 16.5f)))); + + float uv[2] = { fma(coeffs[3], src_pix1.R_COMP, fma(coeffs[4], src_pix1.G_COMP, fma(coeffs[5], src_pix1.B_COMP, 128.5f))), + fma(coeffs[5], src_pix1.R_COMP, fma(coeffs[6], src_pix1.G_COMP, fma(coeffs[7], src_pix1.B_COMP, 128.5f))) }; + + udst[0] = convert_uchar_sat(uv[uidx] ); + vdst[0] = convert_uchar_sat(uv[1-uidx]); +#endif + ++y; + src_index += 2*src_step; + ydst_index += 2*dst_step; + } + } + } +} diff --git a/selfdrive/camerad/test/yuv_bench/yuv_bench.cc b/selfdrive/camerad/test/yuv_bench/yuv_bench.cc new file mode 100644 index 000000000..22e71c412 --- /dev/null +++ b/selfdrive/camerad/test/yuv_bench/yuv_bench.cc @@ -0,0 +1,145 @@ +#include +#include +#include + +#include +#include + +// #include + +#ifdef __APPLE__ +#include +#else +#include +#endif + +#include "common/util.h" +#include "common/timing.h" +#include "common/mat.h" +#include "clutil.h" + +int main() { + + int rgb_width = 1164; + int rgb_height = 874; + + int rgb_stride = rgb_width*3; + + size_t out_size = rgb_width*rgb_height*3/2; + + uint8_t* rgb_buf = (uint8_t*)calloc(1, rgb_width*rgb_height*3); + uint8_t* out = (uint8_t*)calloc(1, out_size); + + for (int y=0; y +#include + +#include "clutil.h" + +#include "rgb_to_yuv.h" + +void rgb_to_yuv_init(RGBToYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height, int rgb_stride) { + int err = 0; + memset(s, 0, sizeof(*s)); + assert(width % 2 == 0); + assert(height % 2 == 0); + s->width = width; + s->height = height; + char args[1024]; + snprintf(args, sizeof(args), + "-cl-fast-relaxed-math -cl-denorms-are-zero " +#ifdef CL_DEBUG + "-DCL_DEBUG " +#endif + "-DWIDTH=%d -DHEIGHT=%d -DUV_WIDTH=%d -DUV_HEIGHT=%d -DRGB_STRIDE=%d -DRGB_SIZE=%d", + width, height, width/ 2, height / 2, rgb_stride, width * height); + cl_program prg = CLU_LOAD_FROM_FILE(ctx, device_id, "transforms/rgb_to_yuv.cl", args); + + s->rgb_to_yuv_krnl = clCreateKernel(prg, "rgb_to_yuv", &err); + assert(err == 0); + // done with this + err = clReleaseProgram(prg); + assert(err == 0); +} + +void rgb_to_yuv_destroy(RGBToYUVState* s) { + int err = 0; + err = clReleaseKernel(s->rgb_to_yuv_krnl); + assert(err == 0); +} + +void rgb_to_yuv_queue(RGBToYUVState* s, cl_command_queue q, cl_mem rgb_cl, cl_mem yuv_cl) { + int err = 0; + err = clSetKernelArg(s->rgb_to_yuv_krnl, 0, sizeof(cl_mem), &rgb_cl); + assert(err == 0); + err = clSetKernelArg(s->rgb_to_yuv_krnl, 1, sizeof(cl_mem), &yuv_cl); + assert(err == 0); + const size_t work_size[2] = { + (size_t)(s->width + (s->width % 4 == 0 ? 0 : (4 - s->width % 4))) / 4, + (size_t)(s->height + (s->height % 4 == 0 ? 0 : (4 - s->height % 4))) / 4 + }; + cl_event event; + err = clEnqueueNDRangeKernel(q, s->rgb_to_yuv_krnl, 2, NULL, &work_size[0], NULL, 0, 0, &event); + assert(err == 0); + clWaitForEvents(1, &event); + clReleaseEvent(event); +} diff --git a/selfdrive/camerad/transforms/rgb_to_yuv.cl b/selfdrive/camerad/transforms/rgb_to_yuv.cl new file mode 100644 index 000000000..60dbdb4d5 --- /dev/null +++ b/selfdrive/camerad/transforms/rgb_to_yuv.cl @@ -0,0 +1,127 @@ +#define RGB_TO_Y(r, g, b) ((((mul24(b, 13) + mul24(g, 65) + mul24(r, 33)) + 64) >> 7) + 16) +#define RGB_TO_U(r, g, b) ((mul24(b, 56) - mul24(g, 37) - mul24(r, 19) + 0x8080) >> 8) +#define RGB_TO_V(r, g, b) ((mul24(r, 56) - mul24(g, 47) - mul24(b, 9) + 0x8080) >> 8) +#define AVERAGE(x, y, z, w) ((convert_ushort(x) + convert_ushort(y) + convert_ushort(z) + convert_ushort(w) + 1) >> 1) + +inline void convert_2_ys(__global uchar * out_yuv, int yi, const uchar8 rgbs1) { + uchar2 yy = (uchar2)( + RGB_TO_Y(rgbs1.s2, rgbs1.s1, rgbs1.s0), + RGB_TO_Y(rgbs1.s5, rgbs1.s4, rgbs1.s3) + ); +#ifdef CL_DEBUG + if(yi >= RGB_SIZE) + printf("Y vector2 overflow, %d > %d\n", yi, RGB_SIZE); +#endif + vstore2(yy, 0, out_yuv + yi); +} + +inline void convert_4_ys(__global uchar * out_yuv, int yi, const uchar8 rgbs1, const uchar8 rgbs3) { + const uchar4 yy = (uchar4)( + RGB_TO_Y(rgbs1.s2, rgbs1.s1, rgbs1.s0), + RGB_TO_Y(rgbs1.s5, rgbs1.s4, rgbs1.s3), + RGB_TO_Y(rgbs3.s0, rgbs1.s7, rgbs1.s6), + RGB_TO_Y(rgbs3.s3, rgbs3.s2, rgbs3.s1) + ); +#ifdef CL_DEBUG + if(yi > RGB_SIZE - 4) + printf("Y vector4 overflow, %d > %d\n", yi, RGB_SIZE - 4); +#endif + vstore4(yy, 0, out_yuv + yi); +} + +inline void convert_uv(__global uchar * out_yuv, int ui, int vi, + const uchar8 rgbs1, const uchar8 rgbs2) { + // U & V: average of 2x2 pixels square + const short ab = AVERAGE(rgbs1.s0, rgbs1.s3, rgbs2.s0, rgbs2.s3); + const short ag = AVERAGE(rgbs1.s1, rgbs1.s4, rgbs2.s1, rgbs2.s4); + const short ar = AVERAGE(rgbs1.s2, rgbs1.s5, rgbs2.s2, rgbs2.s5); +#ifdef CL_DEBUG + if(ui >= RGB_SIZE + RGB_SIZE / 4) + printf("U overflow, %d >= %d\n", ui, RGB_SIZE + RGB_SIZE / 4); + if(vi >= RGB_SIZE + RGB_SIZE / 2) + printf("V overflow, %d >= %d\n", vi, RGB_SIZE + RGB_SIZE / 2); +#endif + out_yuv[ui] = RGB_TO_U(ar, ag, ab); + out_yuv[vi] = RGB_TO_V(ar, ag, ab); +} + +inline void convert_2_uvs(__global uchar * out_yuv, int ui, int vi, + const uchar8 rgbs1, const uchar8 rgbs2, const uchar8 rgbs3, const uchar8 rgbs4) { + // U & V: average of 2x2 pixels square + const short ab1 = AVERAGE(rgbs1.s0, rgbs1.s3, rgbs2.s0, rgbs2.s3); + const short ag1 = AVERAGE(rgbs1.s1, rgbs1.s4, rgbs2.s1, rgbs2.s4); + const short ar1 = AVERAGE(rgbs1.s2, rgbs1.s5, rgbs2.s2, rgbs2.s5); + const short ab2 = AVERAGE(rgbs1.s6, rgbs3.s1, rgbs2.s6, rgbs4.s1); + const short ag2 = AVERAGE(rgbs1.s7, rgbs3.s2, rgbs2.s7, rgbs4.s2); + const short ar2 = AVERAGE(rgbs3.s0, rgbs3.s3, rgbs4.s0, rgbs4.s3); + uchar2 u2 = (uchar2)( + RGB_TO_U(ar1, ag1, ab1), + RGB_TO_U(ar2, ag2, ab2) + ); + uchar2 v2 = (uchar2)( + RGB_TO_V(ar1, ag1, ab1), + RGB_TO_V(ar2, ag2, ab2) + ); +#ifdef CL_DEBUG1 + if(ui > RGB_SIZE + RGB_SIZE / 4 - 2) + printf("U 2 overflow, %d >= %d\n", ui, RGB_SIZE + RGB_SIZE / 4 - 2); + if(vi > RGB_SIZE + RGB_SIZE / 2 - 2) + printf("V 2 overflow, %d >= %d\n", vi, RGB_SIZE + RGB_SIZE / 2 - 2); +#endif + vstore2(u2, 0, out_yuv + ui); + vstore2(v2, 0, out_yuv + vi); +} + +__kernel void rgb_to_yuv(__global uchar const * const rgb, + __global uchar * out_yuv) +{ + const int dx = get_global_id(0); + const int dy = get_global_id(1); + const int col = mul24(dx, 4); // Current column in rgb image + const int row = mul24(dy, 4); // Current row in rgb image + const int bgri_start = mad24(row, RGB_STRIDE, mul24(col, 3)); // Start offset of rgb data being converted + const int yi_start = mad24(row, WIDTH, col); // Start offset in the target yuv buffer + int ui = mad24(row / 2, UV_WIDTH, RGB_SIZE + col / 2); + int vi = mad24(row / 2 , UV_WIDTH, RGB_SIZE + UV_WIDTH * UV_HEIGHT + col / 2); + int num_col = min(WIDTH - col, 4); + int num_row = min(HEIGHT - row, 4); + if(num_row == 4) { + const uchar8 rgbs0_0 = vload8(0, rgb + bgri_start); + const uchar8 rgbs0_1 = vload8(0, rgb + bgri_start + 8); + const uchar8 rgbs1_0 = vload8(0, rgb + bgri_start + RGB_STRIDE); + const uchar8 rgbs1_1 = vload8(0, rgb + bgri_start + RGB_STRIDE + 8); + const uchar8 rgbs2_0 = vload8(0, rgb + bgri_start + RGB_STRIDE * 2); + const uchar8 rgbs2_1 = vload8(0, rgb + bgri_start + RGB_STRIDE * 2 + 8); + const uchar8 rgbs3_0 = vload8(0, rgb + bgri_start + RGB_STRIDE * 3); + const uchar8 rgbs3_1 = vload8(0, rgb + bgri_start + RGB_STRIDE * 3 + 8); + if(num_col == 4) { + convert_4_ys(out_yuv, yi_start, rgbs0_0, rgbs0_1); + convert_4_ys(out_yuv, yi_start + WIDTH, rgbs1_0, rgbs1_1); + convert_4_ys(out_yuv, yi_start + WIDTH * 2, rgbs2_0, rgbs2_1); + convert_4_ys(out_yuv, yi_start + WIDTH * 3, rgbs3_0, rgbs3_1); + convert_2_uvs(out_yuv, ui, vi, rgbs0_0, rgbs1_0, rgbs0_1, rgbs1_1); + convert_2_uvs(out_yuv, ui + UV_WIDTH, vi + UV_WIDTH, rgbs2_0, rgbs3_0, rgbs2_1, rgbs3_1); + } else if(num_col == 2) { + convert_2_ys(out_yuv, yi_start, rgbs0_0); + convert_2_ys(out_yuv, yi_start + WIDTH, rgbs1_0); + convert_2_ys(out_yuv, yi_start + WIDTH * 2, rgbs2_0); + convert_2_ys(out_yuv, yi_start + WIDTH * 3, rgbs3_0); + convert_uv(out_yuv, ui, vi, rgbs0_0, rgbs1_0); + convert_uv(out_yuv, ui + UV_WIDTH, vi + UV_WIDTH, rgbs2_0, rgbs3_0); + } + } else { + const uchar8 rgbs0_0 = vload8(0, rgb + bgri_start); + const uchar8 rgbs0_1 = vload8(0, rgb + bgri_start + 8); + const uchar8 rgbs1_0 = vload8(0, rgb + bgri_start + RGB_STRIDE); + const uchar8 rgbs1_1 = vload8(0, rgb + bgri_start + RGB_STRIDE + 8); + if(num_col == 4) { + convert_4_ys(out_yuv, yi_start, rgbs0_0, rgbs0_1); + convert_4_ys(out_yuv, yi_start + WIDTH, rgbs1_0, rgbs1_1); + convert_2_uvs(out_yuv, ui, vi, rgbs0_0, rgbs1_0, rgbs0_1, rgbs1_1); + } else if(num_col == 2) { + convert_2_ys(out_yuv, yi_start, rgbs0_0); + convert_2_ys(out_yuv, yi_start + WIDTH, rgbs1_0); + convert_uv(out_yuv, ui, vi, rgbs0_0, rgbs1_0); + } + } +} diff --git a/selfdrive/camerad/transforms/rgb_to_yuv.h b/selfdrive/camerad/transforms/rgb_to_yuv.h new file mode 100644 index 000000000..798958034 --- /dev/null +++ b/selfdrive/camerad/transforms/rgb_to_yuv.h @@ -0,0 +1,32 @@ +#ifndef RGB_TO_YUV_H +#define RGB_TO_YUV_H + +#include +#include + +#ifdef __APPLE__ +#include +#else +#include +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct { + int width, height; + cl_kernel rgb_to_yuv_krnl; +} RGBToYUVState; + +void rgb_to_yuv_init(RGBToYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height, int rgb_stride); + +void rgb_to_yuv_destroy(RGBToYUVState* s); + +void rgb_to_yuv_queue(RGBToYUVState* s, cl_command_queue q, cl_mem rgb_cl, cl_mem yuv_cl); + +#ifdef __cplusplus +} +#endif + +#endif // RGB_TO_YUV_H diff --git a/selfdrive/camerad/transforms/rgb_to_yuv_test.cc b/selfdrive/camerad/transforms/rgb_to_yuv_test.cc new file mode 100644 index 000000000..c8b875105 --- /dev/null +++ b/selfdrive/camerad/transforms/rgb_to_yuv_test.cc @@ -0,0 +1,201 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef ANDROID + +#define MAXE 0 +#include + +#else +// The libyuv implementation on ARM is slightly different than on x86 +// Our implementation matches the ARM version, so accept errors of 1 +#define MAXE 1 + +#endif + +#include + +#include + +#include "clutil.h" +#include "rgb_to_yuv.h" + + +static inline double millis_since_boot() { + struct timespec t; + clock_gettime(CLOCK_BOOTTIME, &t); + return t.tv_sec * 1000.0 + t.tv_nsec * 1e-6; +} + +void cl_init(cl_device_id &device_id, cl_context &context) { + int err; + cl_platform_id platform_id = NULL; + cl_uint num_devices; + cl_uint num_platforms; + + err = clGetPlatformIDs(1, &platform_id, &num_platforms); + err = clGetDeviceIDs(platform_id, CL_DEVICE_TYPE_DEFAULT, 1, + &device_id, &num_devices); + cl_print_info(platform_id, device_id); + context = clCreateContext(NULL, 1, &device_id, NULL, NULL, &err); +} + + +bool compare_results(uint8_t *a, uint8_t *b, int len, int stride, int width, int height, uint8_t *rgb) { + int min_diff = 0., max_diff = 0., max_e = 0.; + int e1 = 0, e0 = 0; + int e0y = 0, e0u = 0, e0v = 0, e1y = 0, e1u = 0, e1v = 0; + int max_e_i = 0; + for (int i = 0;i < len;i++) { + int e = ((int)a[i]) - ((int)b[i]); + if(e < min_diff) { + min_diff = e; + } + if(e > max_diff) { + max_diff = e; + } + int e_abs = std::abs(e); + if(e_abs > max_e) { + max_e = e_abs; + max_e_i = i; + } + if(e_abs < 1) { + e0++; + if(i < stride * height) + e0y++; + else if(i < stride * height + stride * height / 4) + e0u++; + else + e0v++; + } else { + e1++; + if(i < stride * height) + e1y++; + else if(i < stride * height + stride * height / 4) + e1u++; + else + e1v++; + } + } + //printf("max diff : %d, min diff : %d, e < 1: %d, e >= 1: %d\n", max_diff, min_diff, e0, e1); + //printf("Y: e < 1: %d, e >= 1: %d, U: e < 1: %d, e >= 1: %d, V: e < 1: %d, e >= 1: %d\n", e0y, e1y, e0u, e1u, e0v, e1v); + if(max_e <= MAXE) { + return true; + } + int row = max_e_i / stride; + if(row < height) { + printf("max error is Y: %d = (libyuv: %u - cl: %u), row: %d, col: %d\n", max_e, a[max_e_i], b[max_e_i], row, max_e_i % stride); + } else if(row >= height && row < (height + height / 4)) { + printf("max error is U: %d = %u - %u, row: %d, col: %d\n", max_e, a[max_e_i], b[max_e_i], (row - height) / 2, max_e_i % stride / 2); + } else { + printf("max error is V: %d = %u - %u, row: %d, col: %d\n", max_e, a[max_e_i], b[max_e_i], (row - height - height / 4) / 2, max_e_i % stride / 2); + } + return false; +} + +int main(int argc, char** argv) { + srand(1337); + + clu_init(); + cl_device_id device_id; + cl_context context; + cl_init(device_id, context) ; + + int err; + const cl_queue_properties props[] = {0}; //CL_QUEUE_PRIORITY_KHR, CL_QUEUE_PRIORITY_HIGH_KHR, 0}; + cl_command_queue q = clCreateCommandQueueWithProperties(context, device_id, props, &err); + if(err != 0) { + std::cout << "clCreateCommandQueueWithProperties error: " << err << std::endl; + } + + int width = 1164; + int height = 874; + + int opt = 0; + while ((opt = getopt(argc, argv, "f")) != -1) + { + switch (opt) + { + case 'f': + std::cout << "Using front camera dimensions" << std::endl; + int width = 1152; + int height = 846; + } + } + + std::cout << "Width: " << width << " Height: " << height << std::endl; + uint8_t *rgb_frame = new uint8_t[width * height * 3]; + + + RGBToYUVState rgb_to_yuv_state; + rgb_to_yuv_init(&rgb_to_yuv_state, context, device_id, width, height, width * 3); + + int frame_yuv_buf_size = width * height * 3 / 2; + cl_mem yuv_cl = clCreateBuffer(context, CL_MEM_READ_WRITE, frame_yuv_buf_size, (void*)NULL, &err); + uint8_t *frame_yuv_buf = new uint8_t[frame_yuv_buf_size]; + uint8_t *frame_yuv_ptr_y = frame_yuv_buf; + uint8_t *frame_yuv_ptr_u = frame_yuv_buf + (width * height); + uint8_t *frame_yuv_ptr_v = frame_yuv_ptr_u + ((width/2) * (height/2)); + + cl_mem rgb_cl = clCreateBuffer(context, CL_MEM_READ_WRITE, width * height * 3, (void*)NULL, &err); + int mismatched = 0; + int counter = 0; + srand (time(NULL)); + + for (int i = 0; i < 100; i++){ + for (int i = 0; i < width * height * 3; i++){ + rgb_frame[i] = (uint8_t)rand(); + } + + double t1 = millis_since_boot(); + libyuv::RGB24ToI420((uint8_t*)rgb_frame, width * 3, + frame_yuv_ptr_y, width, + frame_yuv_ptr_u, width/2, + frame_yuv_ptr_v, width/2, + width, height); + double t2 = millis_since_boot(); + //printf("Libyuv: rgb to yuv: %.2fms\n", t2-t1); + + clEnqueueWriteBuffer(q, rgb_cl, CL_TRUE, 0, width * height * 3, (void *)rgb_frame, 0, NULL, NULL); + t1 = millis_since_boot(); + rgb_to_yuv_queue(&rgb_to_yuv_state, q, rgb_cl, yuv_cl); + t2 = millis_since_boot(); + + //printf("OpenCL: rgb to yuv: %.2fms\n", t2-t1); + uint8_t *yyy = (uint8_t *)clEnqueueMapBuffer(q, yuv_cl, CL_TRUE, + CL_MAP_READ, 0, frame_yuv_buf_size, + 0, NULL, NULL, &err); + if(!compare_results(frame_yuv_ptr_y, yyy, frame_yuv_buf_size, width, width, height, (uint8_t*)rgb_frame)) + mismatched++; + clEnqueueUnmapMemObject(q, yuv_cl, yyy, 0, NULL, NULL); + + // std::this_thread::sleep_for(std::chrono::milliseconds(20)); + if(counter++ % 100 == 0) + printf("Matched: %d, Mismatched: %d\n", counter - mismatched, mismatched); + + } + printf("Matched: %d, Mismatched: %d\n", counter - mismatched, mismatched); + + delete[] frame_yuv_buf; + rgb_to_yuv_destroy(&rgb_to_yuv_state); + clReleaseContext(context); + delete[] rgb_frame; + + if (mismatched == 0) + return 0; + else + return -1; +}