IMX708.c

Go to the documentation of this file.

// Copyright (C) 2013-2026 Efinix Inc. All rights reserved.
// Full license header bsp/efinix/EfxSapphireSocRV64/include/LICENSE.MD
 
#include <stdio.h>
#include "bsp.h"
#include "cam/device/IMX708.h"
 

 
/*----------------------------------------------------------------------------*/
/* Register Definition                                                    */
/*----------------------------------------------------------------------------*/
 
#define IMX708_MODE_SELECT              0x0100
#define IMX708_ACTIVE                   0x01
#define IMX708_SLEEP                    0x00
 
/* Default initial pixel rate, will get updated for each mode. */
#define IMX708_cam_initIAL_PIXEL_RATE       590000000
 
#define IMX708_REG_BASE_SPC_GAINS_L     0x7b10
#define IMX708_REG_BASE_SPC_GAINS_R     0x7c00
 
/* QBC Re-mosaic broken line correction registers */
#define IMX708_LPF_INTENSITY_EN         0xC428
#define IMX708_LPF_INTENSITY_ENABLED    0x00
#define IMX708_LPF_INTENSITY_DISABLED   0x01
#define IMX708_LPF_INTENSITY            0xC429
 
/* Test Pattern Control */
#define IMX708_REG_TEST_PATTERN         0x0600
#define IMX708_TEST_PATTERN_DISABLE     0
#define IMX708_TEST_PATTERN_SOLID_COLOR 1
#define IMX708_TEST_PATTERN_COLOR_BARS  2
#define IMX708_TEST_PATTERN_GREY_COLOR  3
#define IMX708_TEST_PATTERN_PN9         4
 
/* Test pattern colour components */
#define IMX708_REG_TEST_PATTERN_R       0x0602
#define IMX708_REG_TEST_PATTERN_GR      0x0604
#define IMX708_REG_TEST_PATTERN_B       0x0606
#define IMX708_REG_TEST_PATTERN_GB      0x0608
#define IMX708_TEST_PATTERN_COLOUR_MIN  0
#define IMX708_TEST_PATTERN_COLOUR_MAX  0x0fff
#define IMX708_TEST_PATTERN_COLOUR_STEP 1
 
#define IMX708_REG_BASE_SPC_GAINS_L     0x7b10
#define IMX708_REG_BASE_SPC_GAINS_R     0x7c00
 
/* HDR exposure ratio (long:med == med:short) */
#define IMX708_HDR_EXPOSURE_RATIO       2
#define IMX708_REG_MID_EXPOSURE         0x3116
#define IMX708_REG_SHT_EXPOSURE         0x0224
#define IMX708_REG_MID_ANALOG_GAIN      0x3118
#define IMX708_REG_SHT_ANALOG_GAIN      0x0216
 
/* Exposure control */
#define IMX708_REG_EXPOSURE             0x0202
#define IMX708_EXPOSURE_OFFSET          48
#define IMX708_EXPOSURE_DEFAULT         0x640
#define IMX708_EXPOSURE_STEP            1
#define IMX708_EXPOSURE_MIN             1
#define IMX708_EXPOSURE_MAX             (IMX708_FRAME_LENGTH_MAX - IMX708_EXPOSURE_OFFSET)
 
/* Analog gain control */
#define IMX708_REG_ANALOG_GAIN          0x0204
#define IMX708_ANA_GAIN_MIN             112
#define IMX708_ANA_GAIN_MAX             960
#define IMX708_ANA_GAIN_STEP            1
#define IMX708_ANA_GAIN_DEFAULT         IMX708_ANA_GAIN_MIN
 
/* Digital gain control */
#define IMX708_REG_DIGITAL_GAIN         0x020e
#define IMX708_DGTL_GAIN_MIN            0x0100
#define IMX708_DGTL_GAIN_MAX            0xffff
#define IMX708_DGTL_GAIN_DEFAULT        0x0100
#define IMX708_DGTL_GAIN_STEP           1
 
 
struct imx708_reg {
    u16 address;
    u8 val;
};
 
static const struct imx708_reg mode_common_regs[] = {
    {0x0100, 0x00},
    {0x0136, 0x18},
    {0x0137, 0x00},
    {0x33F0, 0x02},
    {0x33F1, 0x05},
    {0x3062, 0x00},
    {0x3063, 0x12},
    {0x3068, 0x00},
    {0x3069, 0x12},
    {0x306A, 0x00},
    {0x306B, 0x30},
    {0x3076, 0x00},
    {0x3077, 0x30},
    {0x3078, 0x00},
    {0x3079, 0x30},
    {0x5E54, 0x0C},
    {0x6E44, 0x00},
    {0xB0B6, 0x01},
    {0xE829, 0x00},
    {0xF001, 0x08},
    {0xF003, 0x08},
    {0xF00D, 0x10},
    {0xF00F, 0x10},
    {0xF031, 0x08},
    {0xF033, 0x08},
    {0xF03D, 0x10},
    {0xF03F, 0x10},
    {0x0112, 0x0A},
    {0x0113, 0x0A},
    {0x0114, 0x01},
    {0x0B8E, 0x01},
    {0x0B8F, 0x00},
    {0x0B94, 0x01},
    {0x0B95, 0x00},
    {0x3400, 0x01},
    {0x3478, 0x01},
    {0x3479, 0x1c},
    {0x3091, 0x01},
    {0x3092, 0x00},
    {0x3419, 0x00},
    {0xBCF1, 0x02},
    {0x3094, 0x01},
    {0x3095, 0x01},
    {0x3362, 0x00},
    {0x3363, 0x00},
    {0x3364, 0x00},
    {0x3365, 0x00},
    {0x0138, 0x01},
};
 
static const struct imx708_reg mode_1920x1080_cropped_regs[] = {
    {0x0342, 0x1D}, // LINE_LENGTH
    {0x0343, 0x10}, // LINE_LENGTH
 
    // 50 FPS
//  {0x0340, 0x06}, // FRAME_LENGTH
//  {0x0341, 0x40}, // FRAME_LENGTH
 
    // 30 FPS
    {0x0340, 0x0A}, // FRAME_LENGTH
    {0x0341, 0x6B}, // FRAME_LENGTH
 
    {0x0344, 0x00}, // X_ADD_STAR
    {0x0345, 0x00}, // X_ADD_STAR
    {0x0346, 0x00}, // Y_ADD_STAR
    {0x0347, 0x00}, // Y_ADD_STAR
    {0x0348, 0x11}, // X_ADD_END_A
    {0x0349, 0xFF}, // X_ADD_END_A
    {0x034A, 0X0A}, // Y_ADD_END_A
    {0x034B, 0x1F}, // Y_ADD_END_A
    {0x0220, 0x62},
    {0x0222, 0x01},
    {0x0900, 0x00},
    {0x0901, 0x11},
    {0x0902, 0x0A},
    {0x3200, 0x01},
    {0x3201, 0x01},
    {0x32D5, 0x01},
    {0x32D6, 0x00},
    {0x32DB, 0x01},
    {0x32DF, 0x00},
    {0x350C, 0x00},
    {0x350D, 0x00},
    {0x0408, 0x00},
    {0x0409, 0x00},
    {0x040A, 0x00},
    {0x040B, 0x00},
    {0x040C, 0x07}, // X_OUTPUT_SIZE
    {0x040D, 0x80}, // X_OUTPUT_SIZE
    {0x040E, 0x04}, // Y_OUTPUT_SIZE
    {0x040F, 0x38}, // Y_OUTPUT_SIZE
    {0x034C, 0x07}, // X_OUTPUT_SIZE
    {0x034D, 0x80}, // X_OUTPUT_SIZE
    {0x034E, 0x04}, // Y_OUTPUT_SIZE
    {0x034F, 0x38}, // Y_OUTPUT_SIZE
    {0x0301, 0x05},
    {0x0303, 0x02},
    {0x0305, 0x02},
    {0x0306, 0x00},
    {0x0307, 0x7C},
    {0x030B, 0x02},
    {0x030D, 0x04},
    {0x0310, 0x01},
    {0x3CA0, 0x00},
    {0x3CA1, 0x64},
    {0x3CA4, 0x00},
    {0x3CA5, 0x00},
    {0x3CA6, 0x00},
    {0x3CA7, 0x00},
    {0x3CAA, 0x00},
    {0x3CAB, 0x00},
    {0x3CB8, 0x00},
    {0x3CB9, 0x08},
    {0x3CBA, 0x00},
    {0x3CBB, 0x00},
    {0x3CBC, 0x00},
    {0x3CBD, 0x3C},
    {0x3CBE, 0x00},
    {0x3CBF, 0x00},
    {0x0202, 0x03}, // EXPOSURE
    {0x0203, 0x20}, // EXPOSURE
    {0x0224, 0x01},
    {0x0225, 0xF4},
    {0x3116, 0x01},
    {0x3117, 0xF4},
    {0x0204, 0x03}, // ANALOGUE GAIN
    {0x0205, 0x20}, // ANALOGUE GAIN
    {0x0216, 0x00},
    {0x0217, 0x00},
    {0x0218, 0x01},
    {0x0219, 0x00},
    {0x020E, 0x03}, // DIGITAL GAIN
    {0x020F, 0x00}, // DIGITAL GAIN
    {0x3118, 0x00},
    {0x3119, 0x00},
    {0x311A, 0x01},
    {0x311B, 0x00},
    {0x341a, 0x00},
    {0x341b, 0x00},
    {0x341c, 0x00},
    {0x341d, 0x00},
    {0x341e, 0x01},
    {0x341f, 0x20},
    {0x3420, 0x00},
    {0x3421, 0xd8},
    {0x3366, 0x00},
    {0x3367, 0x00},
    {0x3368, 0x00},
    {0x3369, 0x00},
};
 
static const struct imx708_reg mode_2x2binned_1920x1080_regs[] = {
    {0x0342, 0x1D}, // LINE LENGTH
    {0x0343, 0x10}, // LINE LENGTH
 
//23 FPS
//  {0x0340, 0x0A}, // FRAME LENGTH
//  {0x0341, 0x3F}, // FRAME LENGTH
 
//50 FPS
    {0x0340, 0x05}, // FRAME LENGTH
    {0x0341, 0x20}, // FRAME LENGTH
    {0x0344, 0x01}, // X_START
    {0x0345, 0x80}, // X_START
    {0x0346, 0x00}, // Y_START
    {0x0347, 0xD8}, // Y_START
    {0x0348, 0x10}, // X_END
    {0x0349, 0x7F}, // X_END
    {0x034A, 0X09}, // Y_END
    {0x034B, 0x47}, // Y_END
    {0x0220, 0x62},
    {0x0222, 0x00},
    {0x0900, 0x01},
    {0x0901, 0x22},
    {0x0902, 0x08},
    {0x3200, 0x41},
    {0x3201, 0x41},
    {0x32D5, 0x00},
    {0x32D6, 0x00},
    {0x32DB, 0x01},
    {0x32DF, 0x00},
    {0x350C, 0x00},
    {0x350D, 0x00},
    {0x0408, 0x00},
    {0x0409, 0x00},
    {0x040A, 0x00},
    {0x040B, 0x00},
    {0x040C, 0x07}, // X_OUT_SIZE
    {0x040D, 0x80}, // X_OUT_SIZE
    {0x040E, 0x04}, // Y_OUT_SIZE
    {0x040F, 0x38}, // Y_OUT_SIZE
    {0x034C, 0x07}, // X_OUT_SIZE
    {0x034D, 0x80}, // X_OUT_SIZE
    {0x034E, 0x04}, // Y_OUT_SIZE
    {0x034F, 0x38}, // Y_OUT_SIZE
    {0x0301, 0x05},
    {0x0303, 0x02},
    {0x0305, 0x02},
    {0x0306, 0x00},
    {0x0307, 0x7A},
    {0x030B, 0x02},
    {0x030D, 0x04},
    {0x0310, 0x01},
    {0x3CA0, 0x00},
    {0x3CA1, 0x3C},
    {0x3CA4, 0x00},
    {0x3CA5, 0x3C},
    {0x3CA6, 0x00},
    {0x3CA7, 0x00},
    {0x3CAA, 0x00},
    {0x3CAB, 0x00},
    {0x3CB8, 0x00},
    {0x3CB9, 0x1C},
    {0x3CBA, 0x00},
    {0x3CBB, 0x08},
    {0x3CBC, 0x00},
    {0x3CBD, 0x1E},
    {0x3CBE, 0x00},
    {0x3CBF, 0x0A},
    {0x0202, 0x03}, // EXPOSURE
    {0x0203, 0x13}, // EXPOSURE
    {0x0224, 0x20},
    {0x0225, 0xF4},
    {0x3116, 0x01},
    {0x3117, 0xF4},
    {0x0204, 0x03}, // ANALOGUE GAIN
    {0x0205, 0x84}, // ANALOGUE GAIN
    {0x0216, 0x00},
    {0x0217, 0x70},
    {0x0218, 0x01},
    {0x0219, 0x00},
    {0x020E, 0x01}, // DIGITAL GAIN
    {0x020F, 0x00}, // DIGITAL GAIN
    {0x3118, 0x00},
    {0x3119, 0x70},
    {0x311A, 0x01},
    {0x311B, 0x00},
    {0x341a, 0x00},
    {0x341b, 0x00},
    {0x341c, 0x00},
    {0x341d, 0x00},
    {0x341e, 0x00},
    {0x341f, 0x90},
    {0x3420, 0x00},
    {0x3421, 0x6c},
    {0x3366, 0x00},
    {0x3367, 0x00},
    {0x3368, 0x00},
    {0x3369, 0x00},
};
 
static const struct imx708_reg mode_hdr_1920x1080_regs[] = {
    {0x0342, 0x14}, // LINE LENGTH
    {0x0343, 0x60}, // LINE LENGTH
    {0x0340, 0x09}, // FRAME LENGTH
    {0x0341, 0xB5}, // FRAME LENGTH
    {0x0344, 0x01}, // X_START
    {0x0345, 0x80}, // X_START
    {0x0346, 0x00}, // Y_START
    {0x0347, 0xD8}, // Y_START
    {0x0348, 0x10}, // X_END
    {0x0349, 0x7F}, // X_END
    {0x034A, 0X09}, // Y_END
    {0x034B, 0x47}, // Y_END
    {0x0220, 0x01},
    {0x0222, IMX708_HDR_EXPOSURE_RATIO},
    {0x0900, 0x00},
    {0x0901, 0x11},
    {0x0902, 0x0A},
    {0x3200, 0x01},
    {0x3201, 0x01},
    {0x32D5, 0x00},
    {0x32D6, 0x00},
    {0x32DB, 0x01},
    {0x32DF, 0x00},
    {0x350C, 0x00},
    {0x350D, 0x00},
    {0x0408, 0x00},
    {0x0409, 0x00},
    {0x040A, 0x00},
    {0x040B, 0x00},
    {0x040C, 0x07}, // X_OUT_SIZE
    {0x040D, 0x80}, // X_OUT_SIZE
    {0x040E, 0x04}, // Y_OUT_SIZE
    {0x040F, 0x38}, // Y_OUT_SIZE
    {0x034C, 0x07}, // X_OUT_SIZE
    {0x034D, 0x80}, // X_OUT_SIZE
    {0x034E, 0x04}, // Y_OUT_SIZE
    {0x034F, 0x38}, // Y_OUT_SIZE
    {0x0301, 0x05},
    {0x0303, 0x02},
    {0x0305, 0x02},
    {0x0306, 0x00},
    {0x0307, 0xA2},
    {0x030B, 0x02},
    {0x030D, 0x04},
    {0x030E, 0x01},
    {0x030F, 0x2C},
    {0x0310, 0x01},
    {0x3CA0, 0x00},
    {0x3CA1, 0x00},
    {0x3CA4, 0x00},
    {0x3CA5, 0x00},
    {0x3CA6, 0x00},
    {0x3CA7, 0x28},
    {0x3CAA, 0x00},
    {0x3CAB, 0x00},
    {0x3CB8, 0x00},
    {0x3CB9, 0x30},
    {0x3CBA, 0x00},
    {0x3CBB, 0x00},
    {0x3CBC, 0x00},
    {0x3CBD, 0x32},
    {0x3CBE, 0x00},
    {0x3CBF, 0x00},
    {0x0202, 0x0B}, // EXPOSURE
    {0x0203, 0xA4}, // EXPOSURE
    {0x0224, 0x02}, // SHORT EXPOSURE
    {0x0225, 0xE9}, // SHORT EXPOSURE
    {0x3116, 0x05}, // MID EXPOSURE
    {0x3117, 0xD2}, // MID EXPOSURE
    {0x0204, 0x03}, // ANALOGUE GAIN
    {0x0205, 0x84}, // ANALOGUE GAIN
    {0x0216, 0x03}, // SHORT EXPOSURE ANALOG GAIN
    {0x0217, 0x84}, // SHORT EXPOSURE ANALOG GAIN
    {0x0218, 0x01},
    {0x0219, 0x00},
    {0x020E, 0x01}, // DIGITAL GAIN
    {0x020F, 0x00}, // DIGITAL GAIN
    {0x3118, 0x03}, // MID EXPOSURE ANALOG GAIN
    {0x3119, 0x84}, // MID EXPOSURE ANALOG GAIN
    {0x311A, 0x01},
    {0x311B, 0x00},
    {0x341a, 0x00},
    {0x341b, 0x00},
    {0x341c, 0x00},
    {0x341d, 0x00},
    {0x341e, 0x00},
    {0x341f, 0x90},
    {0x3420, 0x00},
    {0x3421, 0x6c},
    {0x3360, 0x01},
    {0x3361, 0x01},
    {0x3366, 0x09},
    {0x3367, 0x00},
    {0x3368, 0x05},
    {0x3369, 0x10},
};
 
struct PiCamV3_mode {
    struct imx708_reg *regs;
    u32 output_width;
    u32 output_height;
    u32 line_length_pix;
    u32 vblank_min;
    u32 vblank_default;
    u32 pxiel_rate;
    u32 exposure_lines_min;
    u32 exposure_lines_step;
};
 
/* Default PDAF pixel correction gains */
static const u8 pdaf_gains[2][9] = {
    { 0x4c, 0x4c, 0x4c, 0x46, 0x3e, 0x38, 0x35, 0x35, 0x35 },
    { 0x35, 0x35, 0x35, 0x38, 0x3e, 0x46, 0x4c, 0x4c, 0x4c }
};
 
/* 450MHz is the nominal "default" link frequency */
static const struct imx708_reg link_450Mhz_regs[] = {
    {0x030E, 0x01},
    {0x030F, 0x2c},
};
 
static const struct imx708_reg link_447Mhz_regs[] = {
    {0x030E, 0x01},
    {0x030F, 0x2a},
};
 
static const struct imx708_reg link_453Mhz_regs[] = {
    {0x030E, 0x01},
    {0x030F, 0x2e},
};
 
#define DW9807_I2C_ADDRESS 0x0C << 1
 
#define DW9807_MAX_FOCUS_POS    1023
/*
 * This sets the minimum granularity for the focus positions.
 * A value of 1 gives maximum accuracy for a desired focus position.
 */
#define DW9807_FOCUS_STEPS  1
/*
 * This acts as the minimum granularity of lens movement.
 * Keep this value power of 2, so the control steps can be
 * uniformly adjusted for gradual lens movement, with desired
 * number of control steps.
 */
#define DW9807_CTRL_STEPS   16
#define DW9807_CTRL_DELAY_US    1000
 
#define DW9807_CTL_ADDR     0x02
/*
 * DW9807 separates two registers to control the VCM position.
 * One for MSB value, another is LSB value.
 */
#define DW9807_MSB_ADDR     0x03
#define DW9807_LSB_ADDR     0x04
#define DW9807_STATUS_ADDR  0x05
#define DW9807_MODE_ADDR    0x06
#define DW9807_RESONANCE_ADDR   0x07
 
#define DW9807_PW_MIN_DELAY_US      100
#define DW9807_PW_DELAY_RANGE_US    10
 
#define DW9807_ACTIVE 0x00
#define DW9807_SLEEP 0x01
 
/* -----------------------------------------------------------------------------*/
/* CAM: Plug & Play Driver                                          
/* -----------------------------------------------------------------------------*/
const cam_api_t IMX708_DRIVER =
{
    .cam_init                   = IMX708_cam_init,
    .cam_startStreaming         = IMX708_startStreaming,
    .cam_stopStreaming          = IMX708_stopStreaming,
};
 
 
 
void IMX708_ConfigCommon(cam_instance_t *cam)
{
    for (int i = 0; i < sizeof(mode_common_regs)/sizeof(mode_common_regs[0]); i++) {
        cam_writeReg(cam,mode_common_regs[i].address,mode_common_regs[i].val);
    }
}
 
void IMX708_ConfigFormat(cam_instance_t *cam, u8 mode)
{
    //  MODE
    //  0 : 1920 x 1080 cropped, 50FPS
    //  1 : 1920 x 1080 2x2 binned, 60 FPS
    //  2 : 1920 x 1080 HDR, 50 FPS
 
    if (mode == 0) {
        for (int i = 0; i < sizeof(mode_1920x1080_cropped_regs)/sizeof(mode_1920x1080_cropped_regs[0]); i++) {
            cam_writeReg(cam,mode_1920x1080_cropped_regs[i].address,mode_1920x1080_cropped_regs[i].val);
        }
    }
 
    else if (mode == 1) {
        for (int i = 0; i < sizeof(mode_2x2binned_1920x1080_regs)/sizeof(mode_2x2binned_1920x1080_regs[0]); i++) {
            cam_writeReg(cam,mode_2x2binned_1920x1080_regs[i].address,mode_2x2binned_1920x1080_regs[i].val);
        }
    }
 
    else if (mode == 2) {
        for (int i = 0; i < sizeof(mode_hdr_1920x1080_regs)/sizeof(mode_hdr_1920x1080_regs[0]); i++) {
            cam_writeReg(cam,mode_hdr_1920x1080_regs[i].address,mode_hdr_1920x1080_regs[i].val);
        }
    }
 
}
 
void IMX708_ConfigLinkFreq(cam_instance_t *cam)
{
    for (int i = 0; i < sizeof(link_450Mhz_regs)/sizeof(link_450Mhz_regs[0]); i++) {
        cam_writeReg(cam,link_450Mhz_regs[i].address,link_450Mhz_regs[i].val);
    }
}
 
void IMX708_ConfigQuadBayerRemosaicAdjustment(cam_instance_t *cam)
{
    cam_writeReg(cam,IMX708_LPF_INTENSITY_EN, IMX708_LPF_INTENSITY_ENABLED);
    cam_writeReg(cam,IMX708_LPF_INTENSITY, 0x04);
}
 
void IMX708_SetPdafGain(cam_instance_t *cam)
{
    for (int i = 0; i < 54 ;i++) {
        cam_writeReg(cam,IMX708_REG_BASE_SPC_GAINS_L + i, pdaf_gains[0][i%9]);
        cam_writeReg(cam,IMX708_REG_BASE_SPC_GAINS_R + i, pdaf_gains[1][i%9]);
    }
}
 
void IMX708_SetExposure(cam_instance_t *cam ,u16 val)
{
    cam_writeReg(cam,IMX708_REG_EXPOSURE, (val & 0xFF00) >> 8);
    cam_writeReg(cam,IMX708_REG_EXPOSURE + 1, val & 0xFF);
}
 
void IMX708_SetAnalogueGain(cam_instance_t *cam, u16 val)
{
    if (val > IMX708_ANA_GAIN_MAX)
        val = IMX708_ANA_GAIN_MAX;
 
    if (val < IMX708_ANA_GAIN_MIN)
        val = IMX708_ANA_GAIN_MIN;
 
    cam_writeReg(cam,IMX708_REG_ANALOG_GAIN , (val & 0xFF00) >> 8);
    cam_writeReg(cam,IMX708_REG_ANALOG_GAIN + 1 , val & 0xFF);
 
}
 
void IMX708_SetDigitalGain(cam_instance_t *cam, u16 val)
{
    if (val > IMX708_DGTL_GAIN_MAX)
        val = IMX708_DGTL_GAIN_MAX;
 
    if (val < IMX708_DGTL_GAIN_MIN)
            val = IMX708_DGTL_GAIN_MIN;
 
    cam_writeReg(cam,IMX708_REG_DIGITAL_GAIN , (val & 0xFF00) >> 8);
    cam_writeReg(cam,IMX708_REG_DIGITAL_GAIN + 1 , val & 0xFF);
 
}
 
 
void IMX708_OnActuator(cam_instance_t *cam)
{
    i2c_instance_t *inst = cam->inst;
    // Turn on actuator
    i2c_startMasterBlocking(inst);
    i2c_txByte(inst, DW9807_I2C_ADDRESS);
    i2c_txNackBlocking(inst);
    BSP_ASSERT(i2c_rxAck(inst),"Failed to Assert!\n");
    i2c_txByte(inst, DW9807_CTL_ADDR);
    i2c_txNackBlocking(inst);
    BSP_ASSERT(i2c_rxAck(inst),"Failed to Assert!\n");
    i2c_txByte(inst, DW9807_ACTIVE);
    i2c_txNackBlocking(inst);
    i2c_stopMasterBlocking(inst);
}
 
 
void IMX708_OffActuator(cam_instance_t *cam)
{
    i2c_instance_t *inst = cam->inst;
    // Turn off actuator
    i2c_startMasterBlocking(inst);
    i2c_txByte(inst, DW9807_I2C_ADDRESS);
    i2c_txNackBlocking(inst);
    BSP_ASSERT(i2c_rxAck(inst),"Failed to Assert!\n");
    i2c_txByte(inst, DW9807_CTL_ADDR);
    i2c_txNackBlocking(inst);
    BSP_ASSERT(i2c_rxAck(inst),"Failed to Assert!\n");
    i2c_txByte(inst, DW9807_SLEEP);
    i2c_txNackBlocking(inst);
    i2c_stopMasterBlocking(inst);
}
 
void IMX708_SetFocusStep(cam_instance_t *cam ,u32 focus_step)
{
    i2c_instance_t *inst = cam->inst;
    if (focus_step >= DW9807_MAX_FOCUS_POS)
        focus_step = DW9807_MAX_FOCUS_POS;
    else if (focus_step <= 0)
        focus_step = 0;
 
    i2c_startMasterBlocking(inst);
    i2c_txByte(inst, DW9807_I2C_ADDRESS);
    i2c_txNackBlocking(inst);
    BSP_ASSERT(i2c_rxAck(inst),"Failed to Assert!\n");
    i2c_txByte(inst, DW9807_MSB_ADDR);
    i2c_txNackBlocking(inst);
    BSP_ASSERT(i2c_rxAck(inst),"Failed to Assert!\n");
    i2c_txByte(inst, (focus_step >> 8) & 0x03);
    i2c_txNackBlocking(inst);
    BSP_ASSERT(i2c_rxAck(inst),"Failed to Assert!\n");
    i2c_stopMasterBlocking(inst);
 
    i2c_startMasterBlocking(inst);
    i2c_txByte(inst, DW9807_I2C_ADDRESS);
    i2c_txNackBlocking(inst);
    BSP_ASSERT(i2c_rxAck(inst),"Failed to Assert!\n");
    i2c_txByte(inst, DW9807_LSB_ADDR);
    i2c_txNackBlocking(inst);
    BSP_ASSERT(i2c_rxAck(inst),"Failed to Assert!\n");
    i2c_txByte(inst, focus_step & 0xFF);
    i2c_txNackBlocking(inst);
    BSP_ASSERT(i2c_rxAck(inst),"Failed to Assert!\n");
    i2c_stopMasterBlocking(inst);
}
 
cam_status_t IMX708_startStreaming(cam_instance_t *cam)
{
    cam_writeReg(cam,IMX708_MODE_SELECT, IMX708_ACTIVE);
    return CAM_OK;
}
 
cam_status_t IMX708_stopStreaming(cam_instance_t *cam)
{
    cam_writeReg(cam,IMX708_MODE_SELECT, IMX708_SLEEP);
    return CAM_OK;
}
 
cam_status_t IMX708_cam_init(cam_instance_t *cam)
{
 
    IMX708_stopStreaming(cam);
 
    IMX708_ConfigCommon(cam);
 
    IMX708_SetPdafGain(cam);
 
    IMX708_ConfigFormat(cam, 1);
 
    IMX708_ConfigLinkFreq(cam);
 
    IMX708_ConfigQuadBayerRemosaicAdjustment(cam);
 
    IMX708_OnActuator(cam);
 
    IMX708_SetFocusStep(cam ,700);
 
    IMX708_OffActuator(cam);
 
    LOG_INFO(DBG_MOD_CAM, "Camera Init Done !\n");
    return CAM_OK;
}