/*
 * Copyright (c) 2017-2019, NVIDIA CORPORATION.  All rights reserved. All
 * information contained herein is proprietary and confidential to NVIDIA
 * Corporation.  Any use, reproduction, or disclosure without the written
 * permission of NVIDIA Corporation is prohibited.
 */

#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <errno.h>
#include "log_utils.h"
#include "nvmedia_isc.h"
#include "isc_max96712.h"
#include "isc_max96712_pg_setting.h"
#include "os_common.h"

#define MAX96712_I2C_LOCAL_ACK_SHIFT            7
#define MAX96712_SET_I2C_LOCAL_ACK              1

#define MAX96712_CONCATENATION_SHIFT            7
#define MAX96712_ENABLE_CONCATENATION           1

#define MAX96712_MAX_CMD_SIZE                   100

#define MAX96712_REPLICATION(SRC, DST) ((DST << 5) | (SRC << 3))

#define REGISTER_ADDRESS_BYTES  2
#define REG_WRITE_BUFFER        32
#define MAX_NUM_GMSL_LINKS      4
#define COMMAND_SIZE_IN_BYTES   4
#define REG_LINK_ENABLED        0x06
#define REG_DEV_ID_ADDR         0x0D
#define MAX96712_DEV_ID         0xA0
#define REG_DEV_REV_ADDR        0x4C

#if !defined(__INTEGRITY)
#define MIN(a,b)            (((a) < (b)) ? (a) : (b))
#endif

#define GET_SIZE(x)         sizeof(x)
#define GET_BLOCK_LENGTH(x) x[0]
#define GET_BLOCK_DATA(x)   &x[1]
#define SET_NEXT_BLOCK(x)   (x += (x[0] + 1))
#define SET_BIT(val,pos)    (val |= (1<<pos))
#define RESET_BIT(val,pos)  (val &= (~(1<<pos)))

static const uint8_t SetGMSL1FSYNCMode[][MAX96712_MAX_CMD_SIZE] = {
    [ISC_CONFIG_MAX96712_MANUAL_SYNC] = { /* manual mode */
        'w', 3, 0x04, 0xA2, 0x00, /* video link for fsync generation */
        'w', 3, 0x04, 0xAA, 0x00, /* Disable overlap window */
        'w', 3, 0x04, 0xAB, 0x00, /* Disable overlap window */
        'w', 3, 0x04, 0xA8, 0x00, /* Disable error threshold */
        'w', 3, 0x04, 0xA9, 0x00, /* Disable error threshold */
        'w', 3, 0x04, 0xAF, 0x1F, /* Set FSYNC to GMSL1 type */
        'w', 3, 0x04, 0xA0, 0x00, /* Set FSYNC to manual mode */
        'w', 3, 0x0B, 0x08, 0x35, /* Enable FSYNC on Link0 */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x0C, 0x08, 0x35, /* Enable FSYNC on Link1 */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x0D, 0x08, 0x35, /* Enable FSYNC on Link2 */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x0E, 0x08, 0x35, /* Enable FSYNC on Link3 */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'e'
    },
    [ISC_CONFIG_MAX96712_OSC_MANUAL_SYNC] = { /* manual with OSC mode */
        'w', 3, 0x04, 0xAA, 0x00, /* Disable overlap window */
        'w', 3, 0x04, 0xAB, 0x00, /* Disable overlap window */
        'w', 3, 0x04, 0xA8, 0x00, /* Disable error threshold */
        'w', 3, 0x04, 0xA9, 0x00, /* Disable error threshold */
        'w', 3, 0x04, 0xAF, 0x4F, /* Set FSYNC to GMSL1 type */
        'w', 3, 0x04, 0xA0, 0x00, /* Set FSYNC to manual mode */
        'w', 3, 0x0B, 0x08, 0x35, /* Enable FSYNC on Link0 */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x0C, 0x08, 0x35, /* Enable FSYNC on Link1 */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x0D, 0x08, 0x35, /* Enable FSYNC on Link2 */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x0E, 0x08, 0x35, /* Enable FSYNC on Link3 */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'e'
    },
    [ISC_CONFIG_MAX96712_EXTERNAL_SYNC] =  { /* external sync */
        'w', 3, 0x04, 0xA0, 0x08,
        'w', 3, 0x0B, 0x08, 0x65,
        'w', 3, 0x0C, 0x08, 0x65,
        'w', 3, 0x0D, 0x08, 0x65,
        'w', 3, 0x0E, 0x08, 0x65,
        'e'
    },
};

static const uint8_t SetGMSL2FSYNCMode[][MAX96712_MAX_CMD_SIZE] = {
   [ISC_CONFIG_MAX96712_MANUAL_SYNC] = { /* manual */
        'w', 3, 0x04, 0xA2, 0x00, /* video link for fsync generation */
        'w', 3, 0x04, 0xAA, 0x00, /* Disable overlap window */
        'w', 3, 0x04, 0xAB, 0x00, /* Disable overlap window */
        'w', 3, 0x04, 0xA8, 0x00, /* Disable error threshold */
        'w', 3, 0x04, 0xA9, 0x00, /* Disable error threshold */
        'w', 3, 0x04, 0xB1, 0x08, /* GPIO ID setup to output FSYNC */
        'w', 3, 0x04, 0xAF, 0x90, /* Set FSYNC to GMSL2 type */
        'w', 3, 0x04, 0xA0, 0x00, /* Set FSYNC to manual mode */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'e'
    },
    [ISC_CONFIG_MAX96712_OSC_MANUAL_SYNC] = { /* manual with OSC mode */
        'w', 3, 0x04, 0xAA, 0x00, /* Disable overlap window */
        'w', 3, 0x04, 0xAB, 0x00, /* Disable overlap window */
        'w', 3, 0x04, 0xA8, 0x00, /* Disable error threshold */
        'w', 3, 0x04, 0xA9, 0x00, /* Disable error threshold */
        'w', 3, 0x04, 0xB1, 0x10, /* GPIO ID setup to output FSYNC */
        'w', 3, 0x04, 0xAF, 0xCF, /* Set FSYNC to GMSL2 type */
        'w', 3, 0x04, 0xA0, 0x00, /* Set FSYNC to manual mode */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'e'
    },
    [ISC_CONFIG_MAX96712_EXTERNAL_SYNC] = { /* External sync */
        'w', 3, 0x04, 0xA0, 0x08,
        'w', 3, 0x04, 0xB1, 0x10,
        'w', 3, 0x03, 0x06, 0x83,
        'w', 3, 0x03, 0x3D, 0x22,
        'w', 3, 0x03, 0x74, 0x22,
        'w', 3, 0x03, 0xAA, 0x22,
        'd',    0x27, 0x10,       /* Delay 10ms */
        'e'
    },
    [ISC_CONFIG_MAX96712_AUTO_SYNC] = { /* Auto sync */
        'w', 3, 0x04, 0xA2, 0x01,
        'w', 3, 0x04, 0xAA, 0x00,
        'w', 3, 0x04, 0xAB, 0x00,
        'w', 3, 0x04, 0xA8, 0x00,
        'w', 3, 0x04, 0xA9, 0x00,
        'w', 3, 0x04, 0xB1, 0x18,
        'w', 3, 0x04, 0xA0, 0x06,
        'd',    0x27, 0x10,       /* Delay 10ms */
        'e'
    },
};

static const uint8_t adjust_tx_amp_and_timing[] = {
    /* Bug 2182451 : the below errors were observed intermittently in GMSL2 6Gbps link speed.
    *               to resolve it, adjust the Tx amplitude and timing parameters
    * CSI error(short or long line) is seen
    * Decoding error is seen on the deserializer
    * Link margin becomes bad
    */
    /* Link 0 */
    'w', 3, 0x14, 0x58, 0x28, /* vth1 : Error channel power-down */
    'd',    0x27, 0x10,       /* Delay 10ms */
    'w', 3, 0x14, 0x59, 0x68, /* vth0 : + 104 * 4.7mV = 488.8 mV */
    'd',    0x27, 0x10,       /* Delay 10ms */
    'w', 3, 0x14, 0x3E, 0xB3, /* Error channel phase secondary timing adjustment */
    'd',    0x27, 0x10,       /* Delay 10ms */
    'w', 3, 0x14, 0x3F, 0x72, /* Error channel phase primary timing adjustment */
    'd',    0x27, 0x10,       /* Delay 10ms */
    'w', 3, 0x14, 0x95, 0xD2, /* Reverse channel Tx amplitude to 180 mV */
    'd',    0x27, 0x10,       /* Delay 10ms */
    /* Link 1 */
    'w', 3, 0x15, 0x58, 0x28, /* vth1 : Error channel power-down */
    'd',    0x27, 0x10,       /* Delay 10ms */
    'w', 3, 0x15, 0x59, 0x68, /* vth0 : + 104 * 4.7mV = 488.8 mV */
    'd',    0x27, 0x10,       /* Delay 10ms */
    'w', 3, 0x15, 0x3E, 0xB3, /* Error channel phase secondary timing adjustment */
    'd',    0x27, 0x10,       /* Delay 10ms */
    'w', 3, 0x15, 0x3F, 0x72, /* Error channel phase primary timing adjustment */
    'd',    0x27, 0x10,       /* Delay 10ms */
    'w', 3, 0x15, 0x95, 0xD2, /* Reverse channel Tx amplitude to 180 mV */
    'd',    0x27, 0x10,       /* Delay 10ms */
    /* Link 2 */
    'w', 3, 0x16, 0x58, 0x28, /* vth1 : Error channel power-down */
    'd',    0x27, 0x10,       /* Delay 10ms */
    'w', 3, 0x16, 0x59, 0x68, /* vth0 : + 104 * 4.7mV = 488.8 mV */
    'd',    0x27, 0x10,       /* Delay 10ms */
    'w', 3, 0x16, 0x3E, 0xB3, /* Error channel phase secondary timing adjustment */
    'd',    0x27, 0x10,       /* Delay 10ms */
    'w', 3, 0x16, 0x3F, 0x72, /* Error channel phase primary timing adjustment */
    'd',    0x27, 0x10,       /* Delay 10ms */
    'w', 3, 0x16, 0x95, 0xD2, /* Reverse channel Tx amplitude to 180 mV */
    'd',    0x27, 0x10,       /* Delay 10ms */
    /* Link 3 */
    'w', 3, 0x17, 0x58, 0x28, /* vth1 : Error channel power-down */
    'd',    0x27, 0x10,       /* Delay 10ms */
    'w', 3, 0x17, 0x59, 0x68, /* vth0 : + 104 * 4.7mV = 488.8 mV */
    'd',    0x27, 0x10,       /* Delay 10ms */
    'w', 3, 0x17, 0x3E, 0xB3, /* Error channel phase secondary timing adjustment */
    'd',    0x27, 0x10,       /* Delay 10ms */
    'w', 3, 0x17, 0x3F, 0x72, /* Error channel phase primary timing adjustment */
    'd',    0x27, 0x10,       /* Delay 10ms */
    'w', 3, 0x17, 0x95, 0xD2, /* Reverse channel Tx amplitude to 180 mV */
    'd',    0x27, 0x10,       /* Delay 10ms */
    'e'
};

typedef struct {
    ContextMAX96712 ctx;
} _DriverHandle;

typedef struct {
    RevisionMAX96712 revId;
    uint32_t revVal;
} Revision;

/* These values must include all of values in the RevisionMAX96712 enum */
static Revision supportedRevisions[] = {
    { ISC_MAX96712_REV_1, 0x1 },
    { ISC_MAX96712_REV_2, 0x2 },
    { ISC_MAX96712_REV_3, 0x3 },
    { ISC_MAX96712_REV_4, 0x4 },
};

static NvMediaStatus
WriteArrayWithCommand(
    NvMediaISCDevice *handle,
    const uint8_t *arrayData)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;

    if ((handle == NULL) || (arrayData == NULL)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    while(arrayData[0] != (uint8_t)'e') {
        switch (arrayData[0]) {
            case 'w':
                status = NvMediaISCDeviceWrite(handle, 0,
                                      arrayData[1],
                                      (uint8_t *)&arrayData[2]);
                                      arrayData += (arrayData[1] + 2u);
                break;
            case 'd':
                if (nvsleep((arrayData[1] << 8) + arrayData[2]) != 0) {
                    LOG_DBG("%s: nvsleep() failed\n", __func__);
                }
                arrayData += 3u;
                break;
            default:
                break;
        }

        if (status != NVMEDIA_STATUS_OK) {
            break;
        }
    }

    return status;
}

static NvMediaStatus
WriteArray(
     NvMediaISCDevice *handle,
     uint32_t arrayByteLength,
     const uint8_t *arrayData)
{
    NvMediaStatus status;

    if (!handle) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    while(arrayByteLength) {
        status = NvMediaISCDeviceWrite(handle, 0,              /* transaction */
                              GET_BLOCK_LENGTH(arrayData), /* dataLength */
                              GET_BLOCK_DATA(arrayData));  /* data */

        if (status != NVMEDIA_STATUS_OK) {
            LOG_ERR("%s: MAX96712: error: wri2c   0x%.2X    0x%.2X\n", __func__,
                                                            (uint32_t)arrayData[1],
                                                            (uint32_t)arrayData[2]);
            return status;
        }
        /* This SER-DES pair needs 20SCLK clocks or more timing for next I2C command so we set 100 us with margin */
        nvsleep(100);

        arrayByteLength -= GET_BLOCK_LENGTH(arrayData) + 1;
        SET_NEXT_BLOCK(arrayData);
    }
    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
SetDefaultGMSL1Legacy(
    NvMediaISCDevice *handle)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    /* GMSL1 Max9271 default */
    static const uint8_t max96712_defaults_legacy[] = {
        'w', 3, 0x00, 0x06, 0x00, /* Bus 200376711 : Disable all links. All links are in GMSL1 mode */
        'd',    0xC3, 0x50,       /* Delay 50ms */
        'w', 3, 0x00, 0x13, 0x75, /* RESET_REGISTER */
        'd',    0xC3, 0x50,       /* Delay 50ms */
        'd',    0xC3, 0x50,       /* Delay 50ms */
        'd',    0xC3, 0x50,       /* Delay 50ms */
        'd',    0xC3, 0x50,       /* Delay 50ms */
        'w', 3, 0x00, 0x06, 0x0F, /* Disable all links. All links are in GMSL1 mode */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x00, 0x10, 0x11, /* Link1 - 3Gbps, Link0 - 3Gbps */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x00, 0x11, 0x11, /* Link3 - 3Gbps, Link2 - 3Gbps */
        'd',    0x27, 0x10,       /* Delay 10ms */
        /* GMSL1 - Turn off HIM */
        'w', 3, 0x0B, 0x06, 0x6F, /* Link 0 */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x0C, 0x06, 0x6F, /* Link 1 */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x0D, 0x06, 0x6F, /* Link 2 */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x0E, 0x06, 0x6F, /* Link 3 */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x00, 0x18, 0x0F, /* Reset whole data path on Link 0/1/2/3 */
        'd',    0xC3, 0x50,       /* Delay 50ms */
        'd',    0xC3, 0x50,       /* Delay 50ms */
        /* GMSL1 - Enable reverse channel cfg and turn on local I2C ack */
        'w', 3, 0x0B, 0x0D, 0x81,
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x0C, 0x0D, 0x81,
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x0D, 0x0D, 0x81,
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x0E, 0x0D, 0x81,
        'd',    0x27, 0x10,       /* Delay 10ms */
        /* GMSL1 - Enable manual override of reverse channel pulse length */
        'w', 3, 0x14, 0xC5, 0xAA,
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x15, 0xC5, 0xAA,
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x16, 0xC5, 0xAA,
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x17, 0xC5, 0xAA,
        'd',    0x27, 0x10,       /* Delay 10ms */
        /* GMSL1 - Enable manual override of reverse channel rise fall time setting */
        'w', 3, 0x14, 0xC4, 0xA0,
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x15, 0xC4, 0xA0,
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x16, 0xC4, 0xA0,
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x17, 0xC4, 0xA0,
        'd',    0x27, 0x10,       /* Delay 10ms */
        /* GMSL1 - Tx amplitude manual override */
        'w', 3, 0x14, 0x95, 0x8B,
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x15, 0x95, 0x8B,
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x16, 0x95, 0x8B,
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x17, 0x95, 0x8B,
        'd',    0x27, 0x10,       /* Delay 10ms */
        /* GMSL1 - enable PRBS */
        'w', 3, 0x0B, 0x0F, 0x01,
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x0C, 0x0F, 0x01,
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x0D, 0x0F, 0x01,
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x0E, 0x0F, 0x01,
        'd',    0x27, 0x10,       /* Delay 10ms */
        /* CSI output speed : 2.5GMhz */
        'w', 3, 0x04, 0x15, 0x39,
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x04, 0x18, 0x39,
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x04, 0x1B, 0x39,
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x04, 0x1E, 0x39,
        'd',    0x27, 0x10,       /* Delay 10ms */
        /* GMSL1 - bpp/vc/dt setting */
        'w', 3, 0x04, 0x0B, 0x62,
        'w', 3, 0x04, 0x0C, 0x00,
        'w', 3, 0x04, 0x0D, 0x00,
        'w', 3, 0x04, 0x0E, 0xAC,
        'w', 3, 0x04, 0x0F, 0xBC,
        'w', 3, 0x04, 0x10, 0xB0,
        'w', 3, 0x04, 0x11, 0x6C,
        'w', 3, 0x04, 0x12, 0x30,
        'w', 3, 0x04, 0x15, 0xF9, /* Enable software override bpp/vc/dt for pipeline 1/0 */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x04, 0x18, 0xF9, /* Enable software override bpp/vc/dt for pipeline 3/2 */
        'd',    0x27, 0x10,       /* Delay 10ms */
        /* GMSL1 FRSYNC configuration */
        'w', 3, 0x04, 0xA2, 0x00, /* video link for fsync generation */
        'w', 3, 0x04, 0xAA, 0x00, /* Disable overlap window */
        'w', 3, 0x04, 0xAB, 0x00, /* Disable overlap window */
        'w', 3, 0x04, 0xA8, 0x00, /* Disable error threshold */
        'w', 3, 0x04, 0xA9, 0x00, /* Disable error threshold */
        'w', 3, 0x04, 0xAF, 0x1F, /* Set FSYNC to GMSL1 type */
        'w', 3, 0x04, 0xA0, 0x06, /* Set fsync to automatic mode */
        'w', 3, 0x0B, 0x08, 0x31, /* Enable FSYNC on Link0 */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x0C, 0x08, 0x31, /* Enable FSYNC on Link1 */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x0D, 0x08, 0x31, /* Enable FSYNC on Link2 */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x0E, 0x08, 0x31, /* Enable FSYNC on Link3 */
        'd',    0xC3, 0x50,       /* Delay 50ms */
        /* Disable AEQ(bit 7), AEQ periodic mode(bit 6), AEQ fine tune(bit 4) */
        'w', 3, 0x0B, 0x14, 0x00,
        'w', 3, 0x0C, 0x14, 0x00,
        'w', 3, 0x0D, 0x14, 0x00,
        'w', 3, 0x0E, 0x14, 0x00,
        'e'
    };

    if (!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;

    status = WriteArrayWithCommand(handle,
                                   max96712_defaults_legacy);
    if (status != NVMEDIA_STATUS_OK)
        return status;

    return status;
}

static NvMediaStatus
SetDefaultGMSL1HIMEnabled(
    NvMediaISCDevice *handle,
    uint8_t link,
    uint8_t step)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    /* GMSL1 Max96705 default step 1 - turn on HIM */
    uint8_t max96712_defaults_HIM_step1[30] = {
        /* GMSL1 - Turn on HIM */
        'w', 3, 0x0B, 0x06, 0xEF, /* Link 0 */
        /* GMSL1 - Enable reverse channel cfg and turn on local I2C ack */
        'w', 3, 0x0B, 0x0D, 0x81,
        'e'
    };
    uint8_t max96712_defaults_HIM_step2[30] = {
        /* GMSL1 - Turn off HIM */
        'w', 3, 0x0B, 0x06, 0x6F, /* Link 0 */
        /* GMSL1 - Enable manual override of reverse channel pulse length */
        'w', 3, 0x14, 0xC5, 0xAA,
        /* GMSL1 - Enable manual override of reverse channel rise fall time setting */
        'w', 3, 0x14, 0xC4, 0x8A,
        /* GMSL1 - Tx amplitude manual override */
        'w', 3, 0x14, 0x95, 0x88,
        'e'
    };
    uint8_t max96712_defaults_HIM_step3[30] = {
        /* Enable HIM */
        'w', 3, 0x0B, 0x06, 0xEF,
        /* Manual override of reverse channel pulse length */
        'w', 3, 0x14, 0xC5, 0x40,
        /* Manual override of reverse channel rise fall time setting */
        'w', 3, 0x14, 0xC4, 0x40,
        /* TxAmp manual override */
        'w', 3, 0x14, 0x95, 0x69,
        'e'
    };
    uint8_t *stepHIM = NULL;

    if (step > 2) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    /* Update register offset */
    if (step == 0) {
        max96712_defaults_HIM_step1[2] += link;
        max96712_defaults_HIM_step1[7] += link;
    } else if (step == 1) {
        max96712_defaults_HIM_step2[2] += link;
        max96712_defaults_HIM_step2[7] += link;
        max96712_defaults_HIM_step2[12] += link;
        max96712_defaults_HIM_step2[17] += link;
    } else {
        max96712_defaults_HIM_step3[2] += link;
        max96712_defaults_HIM_step3[7] += link;
        max96712_defaults_HIM_step3[12] += link;
        max96712_defaults_HIM_step3[17] += link;
    }

    stepHIM = (step == 0) ? max96712_defaults_HIM_step1 :
              ((step == 1) ? max96712_defaults_HIM_step2 : max96712_defaults_HIM_step3);

    status = WriteArrayWithCommand(handle,
                                   stepHIM);

    return status;
}

static NvMediaStatus
EnablePacketBasedControlChannel(
    NvMediaISCDevice *handle,
    uint32_t link)
{
    NvMediaStatus status;
    uint8_t data[4] = {3, 0x0B, 0x08, 0x25};

    data[1] = 0xB + link;

    status = WriteArray(handle,
                        COMMAND_SIZE_IN_BYTES,
                        data);
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    nvsleep(10000);

    return status;
}

static NvMediaStatus
SetTPGDefault(
    NvMediaISCDevice *handle)
{
    if (!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;

    return WriteArrayWithCommand(handle,
                                 max96712_pg_settings);
}

static NvMediaStatus
EnableTPG(
    NvMediaISCDevice *handle)
{
    if (!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;

    return WriteArrayWithCommand(handle,
                                 max96712_enable_pg);
}

static NvMediaStatus
DisablePacketDetector(
    NvMediaISCDevice *handle)
{
    static const uint8_t disable_pkt_detector[] = {
        'w', 3, 0x04, 0x34, 0xF0, /* bpp8dbl = 1 for pipe 4/5/6/7 */
        'w', 3, 0x04, 0x37, 0xF0, /* bpp8dbl_mode = 1 for pipe 4/5/6/7 */
        'w', 3, 0x09, 0x33, 0x03, /* ALT_MEM_MAP8/ALT_MEM_MAP12 = 1 on Ctrl 0 */
        'w', 3, 0x09, 0x73, 0x03, /* ALT_MEM_MAP8/ALT_MEM_MAP12 = 1 on Ctrl 1 */
        'w', 3, 0x09, 0xB3, 0x03, /* ALT_MEM_MAP8/ALT_MEM_MAP12 = 1 on Ctrl 2 */
        'w', 3, 0x09, 0xF3, 0x03, /* ALT_MEM_MAP8/ALT_MEM_MAP12 = 1 on Ctrl 3 */
        'w', 3, 0x01, 0x00, 0x23,
        'w', 3, 0x01, 0x12, 0x23,
        'w', 3, 0x01, 0x24, 0x23,
        'w', 3, 0x01, 0x36, 0x23,
        'w', 3, 0x01, 0x48, 0x23,
        'w', 3, 0x01, 0x60, 0x23,
        'w', 3, 0x01, 0x72, 0x23,
        'w', 3, 0x01, 0x84, 0x23,
        'e'
    };

   if (!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;

    return WriteArrayWithCommand(handle,
                                 disable_pkt_detector);
}

static NvMediaStatus
SetGMSLMode(
    NvMediaISCDevice *handle,
    uint8_t mode)
{
    uint8_t *setMode = NULL;
    uint8_t setGMSL1Mode[] = {
        'w', 3, 0x00, 0x06, 0x0F, /* Enable all links. All links are in GMSL1 mode */
        'w', 3, 0x00, 0x10, 0x11, /* Link1 - 3Gbps, Link0 - 3Gbps */
        'w', 3, 0x00, 0x11, 0x11, /* Link3 - 3Gbps, Link2 - 3Gbps */
        'w', 3, 0x00, 0x18, 0x0F, /* Perform one-shot reset for each link */
        'd',    0xC3, 0x50,       /* Delay 50ms */
        'd',    0xC3, 0x50,       /* Delay 50ms */
        /* GMSL1 - Turn on HIM */
        'e'
    };

    uint8_t setGMSL2Mode[] = {
        'w', 3, 0x00, 0x06, 0xFF, /* Enable all links. All links are in GMSL2 mode */
        'w', 3, 0x00, 0x10, 0x22, /* Link1 - 6Gbps, Link0 - 6Gbps */
        'w', 3, 0x00, 0x11, 0x22, /* Link3 - 6Gbps, Link2 - 6Gbps */
        'w', 3, 0x00, 0x18, 0x0F, /* Perform one-shot reset for each link */
        'd',    0xC3, 0x50,       /* Delay 50ms */
        'd',    0xC3, 0x50,       /* Delay 50ms */
        /* GMSL1 - Turn on HIM */
        'e'
    };

    setMode = (mode == 0) ? setGMSL1Mode : setGMSL2Mode;

    return WriteArrayWithCommand(handle,
                                 setMode);
}

static NvMediaStatus
SetMappingVCSetting(
    NvMediaISCDevice *handle,
    uint8_t embDataType,
    uint8_t vcID,
    DataTypeMAX96712 dataType,
    uint8_t usedPipeline)
{
    NvMediaStatus status;
    uint8_t i, txPort, mipiCfg;
    /* Two pipelines are one set to process raw12 and emb */
    uint8_t MappingRaw[] = {
        /* Send RAW12 FS and FE from X to Controller 1 */
        3, 0x09, 0x0B, 0x07,
        3, 0x09, 0x2D, 0x00,
        /* For the following MSB 2 bits = VC, LSB 6 bits = DT */
        3, 0x09, 0x0D, 0x2C,
        3, 0x09, 0x0E, 0x2C,
        3, 0x09, 0x0F, 0x00, /* Change FS packet's DT to reserved one */
        3, 0x09, 0x10, 0x00,
        3, 0x09, 0x11, 0x01, /* Change FE packet's DT to reserved one */
        3, 0x09, 0x12, 0x01,
    };

    uint8_t MappingEMB[] = {
        /* Send EMB8 from Y to Controller 1 with VC unchanged */
        3, 0x0A, 0x0B, 0x07,
        3, 0x0A, 0x2D, 0x00,
        /* For the following MSB 2 bits = VC, LSB 6 bits = DT */
        3, 0x0A, 0x0D, 0x12,
        3, 0x0A, 0x0E, 0x12,
        3, 0x0A, 0x0F, 0x00,
        3, 0x0A, 0x10, 0x00,
        3, 0x0A, 0x11, 0x01,
        3, 0x0A, 0x12, 0x01,
    };
    _DriverHandle *drvHandle = NULL;

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    txPort = drvHandle->ctx.txPort;
    mipiCfg = drvHandle->ctx.mipiCfg;

    /* Update Tx Port */
    if (usedPipeline == 1) {
        MappingRaw[7] = (txPort << 4) | (txPort << 2) | txPort;
        MappingEMB[7] = (txPort << 4) | (txPort << 2) | txPort;
    } else {
        if (mipiCfg == MAX96712_MIPI_OUT_4x2) {
            MappingRaw[7] = 0x15; /* controller 1 */
            MappingEMB[7] = 0x15;
        } else if (mipiCfg == MAX96712_MIPI_OUT_2x4) {
            MappingRaw[7] = 0x00; /* controller 0 */
            MappingEMB[7] = 0x00;
        } else {
            return NVMEDIA_STATUS_BAD_PARAMETER;
        }
    }

    /* Update data type */
    switch(dataType) {
        case ISC_MAX96712_DATA_TYPE_RAW10:
            MappingRaw[11] = 0x2B;
            MappingRaw[15] = 0x2B;
            break;
        case ISC_MAX96712_DATA_TYPE_RAW12:
            MappingRaw[11] = 0x2C;
            MappingRaw[15] = 0x2C;
            break;
        case ISC_MAX96712_DATA_TYPE_RAW16:
            MappingRaw[11] = 0x2E;
            MappingRaw[15] = 0x2E;
            break;
        case ISC_MAX96712_DATA_TYPE_RGB:
            MappingRaw[11] = 0x24;
            MappingRaw[15] = 0x24;
            break;
        default:
            return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    /* update offset */
    /* 4 mapping for data and 4 mapping for emb */
    MappingRaw[15] |= (vcID << 6);
    MappingRaw[23] |= (vcID << 6) | ((embDataType == 1) ? 2 : 0);
    MappingRaw[31] |= (vcID << 6) | ((embDataType == 1) ? 2 : 0);
    MappingEMB[15] |= (vcID << 6);
    MappingEMB[23] |= (vcID << 6);
    MappingEMB[31] |= (vcID << 6);

    for (i = 0; i < 8; i++) {
        MappingRaw[2 + 4 * i] += (0x40 * vcID);
        MappingEMB[2 + 4 * i] += (0x40 * vcID);
    }

    status = WriteArray(handle,
                        GET_SIZE(MappingRaw),
                        MappingRaw);
    if (status != NVMEDIA_STATUS_OK)
        return status;

    if (embDataType == 1) {
        status = WriteArray(handle,
                            GET_SIZE(MappingEMB),
                            MappingEMB);
    }
    return status;
}

static NvMediaStatus
SetI2CPort(
    NvMediaISCDevice *handle,
    uint8_t link)
{
    uint8_t data[3]  = {0x0B, 0x04, 0x00};
    uint8_t gmslMode, i2cPort;
    NvMediaStatus status;
    _DriverHandle *drvHandle = NULL;

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if (link > 4)
        return NVMEDIA_STATUS_BAD_PARAMETER;

    gmslMode = drvHandle->ctx.gmslMode;
    i2cPort = drvHandle->ctx.i2cPort;

    if (gmslMode == ISC_MAX96712_GMSL1_MODE) {
        data[0] += link;

        status = NvMediaISCDeviceRead(handle, 0,         /* transaction */
                             REGISTER_ADDRESS_BYTES, /* regLength */
                             data,                   /* regData */
                             1,                      /* dataLength */
                             &data[2]);              /* data */
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }

        if (i2cPort == ISC_MAX96712_I2CPORT_0)
            data[2] &= ~(1 << 3);
        else if (i2cPort == ISC_MAX96712_I2CPORT_1)
            data[2] |= (1 << 3);
        else
            return NVMEDIA_STATUS_BAD_PARAMETER;

        status = NvMediaISCDeviceWrite(handle, 0,
                              3,
                              data);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }
    } else if (gmslMode == ISC_MAX96712_GMSL2_MODE) {
        /* Disable connection from both port 0/1 */
        data[0] = 0x00;
        data[1] = 0x03;

        status = NvMediaISCDeviceRead(handle, 0,         /* transaction */
                             REGISTER_ADDRESS_BYTES, /* regLength */
                             data,                   /* regData */
                             1,                      /* dataLength */
                             &data[2]);              /* data */
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }

        data[2] |= 0x3 << (link * 2);

        status = NvMediaISCDeviceWrite(handle, 0,
                              3,
                              data);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }

        /* Select port 0 or 1 over the link */
        data[0] = 0x00;
        data[1] = 0x07;

        status = NvMediaISCDeviceRead(handle, 0,         /* transaction */
                             REGISTER_ADDRESS_BYTES, /* regLength */
                             data,                   /* regData */
                             1,                      /* dataLength */
                             &data[2]);              /* data */
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }

        if (i2cPort)
            data[2] |= ((1 << link) << 4);
        else
            data[2] &= ~((1 << link) << 4);

        status = NvMediaISCDeviceWrite(handle, 0,
                              3,
                              data);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }

        /* Enable connection from port 0 or 1 */
        data[0] = 0x00;
        data[1] = 0x03;

        status = NvMediaISCDeviceRead(handle, 0,         /* transaction */
                             REGISTER_ADDRESS_BYTES, /* regLength */
                             data,                   /* regData */
                             1,                      /* dataLength */
                             &data[2]);              /* data */
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }

        data[2] &= ~((1 << i2cPort) << (link * 2));

        status = NvMediaISCDeviceWrite(handle, 0,
                              3,
                              data);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }
    } else {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    return status;
}

static NvMediaStatus
VideoPipelineSel(
    NvMediaISCDevice *handle)
{
    NvMediaStatus status;
    uint8_t gmslMode;
    uint8_t VideoPipelines[] = {
        /* pipeline x in link 1 to video pipe 1, pipeline x in link 0 to video pipe 0 */
        3, 0x00, 0xF0, 0x40,
        /* pipeline x in link 3 to video pipe 3, pipeline x in link 2 to video pipe 2 */
        3, 0x00, 0xF1, 0xC8,
        /* pipeline y in link 1 to video pipe 5, pipeline y in link 0 to video pipe 4 */
        3, 0x00, 0xF2, 0x51,
        /* pipeline y in link 3 to video pipe 7, pipeline y in link 2 to video pipe 6 */
        3, 0x00, 0xF3, 0xD9,
        /* Enable pipelines */
        3, 0x00, 0xF4, 0xFF,
    };

    _DriverHandle *drvHandle = NULL;

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    gmslMode = drvHandle->ctx.gmslMode;

    /* pipeline 0/1/2/3 - raw12 in GMSL1, GMSL2
     * pipeline 4/5/6/7 - emb in GMSL2 */
    VideoPipelines[19] = (gmslMode == ISC_MAX96712_GMSL2_MODE) ? 0xFF : 0x0F;

    status = WriteArray(handle,
                        GET_SIZE(VideoPipelines),
                        VideoPipelines);

    return status;
}

static NvMediaStatus
SetFSYNCMode(
    NvMediaISCDevice *handle,
    uint8_t syncMode,
    uint32_t pclk,
    uint32_t fps)
{
    NvMediaStatus status;
    uint8_t gmslMode;
    uint8_t fsync_period[] = {
        3, 0x04, 0xA7, 0x00, /* Calculate FRSYNC period H. don't move position */
        3, 0x04, 0xA6, 0x00, /* Calculate FRSYNC period M. don't move position */
        3, 0x04, 0xA5, 0x00, /* Calculate FRSYNC period L. don't move position */
    };

    _DriverHandle *drvHandle = NULL;

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }
    gmslMode = drvHandle->ctx.gmslMode;

    if (syncMode == ISC_CONFIG_MAX96712_MANUAL_SYNC) {
        /* Calculate FRSYNC period in manual mode based on PCLK */
        /* calculate frsync high period */
        fsync_period[3] = (gmslMode == ISC_MAX96712_GMSL1_MODE) ? ((pclk / fps) >> 16) & 0xFF : 0x25;
        /* calculate frsync middle period */
        fsync_period[7] = (gmslMode == ISC_MAX96712_GMSL1_MODE) ? ((pclk / fps) >> 8) & 0xFF : 0x4C;
        /* calculate frsync low period */
        fsync_period[11] = (gmslMode == ISC_MAX96712_GMSL1_MODE) ? (pclk / fps) & 0xFF : 0x9C;

        status = WriteArray(handle,
                            GET_SIZE(fsync_period),
                            fsync_period);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }
    } else if (syncMode == ISC_CONFIG_MAX96712_OSC_MANUAL_SYNC) {
        /* MAXIM doesn't recommend to use auto or semi-auto mode for the safety concern.
         * If the master link is lost, the frame sync will be lost for other links in both modes.
         * Instead the manual mode with OSC in MAX96712 is recommended.
         * This is applicable after Rev B revision
         */

        if ((drvHandle->ctx.revision == ISC_MAX96712_REV_1) ||
            (drvHandle->ctx.revision == ISC_MAX96712_REV_2)) {
            return NVMEDIA_STATUS_NOT_SUPPORTED;
        }

        /* calculate frsync high period */
        fsync_period[3] = ((MAX96712_OSC_MHZ * 1000 * 1000 / fps) >> 16) & 0xFF;
        /* calculate frsync middle period */
        fsync_period[7] = ((MAX96712_OSC_MHZ * 1000 * 1000 / fps) >> 8) & 0xFF;
        /* calculate frsync low period */
        fsync_period[11] = (MAX96712_OSC_MHZ * 1000 * 1000 / fps) & 0xFF;

        status = WriteArray(handle,
                            GET_SIZE(fsync_period),
                            fsync_period);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }
    }

    if (gmslMode == ISC_MAX96712_GMSL1_MODE) {
        status = WriteArrayWithCommand(handle,
                                       SetGMSL1FSYNCMode[syncMode]);
    } else if (gmslMode == ISC_MAX96712_GMSL2_MODE) {
        status = WriteArrayWithCommand(handle,
                                       SetGMSL2FSYNCMode[syncMode]);
    }

    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
OverrideDataType(
    NvMediaISCDevice *handle,
    DataTypeMAX96712 dataType)
{
    NvMediaStatus status;
    uint8_t bpp = 0, dataFormat = 0;
    uint8_t data[4] = {3, 0, 0, 0}, i;
    uint8_t dataTypeRegs[] = {
        3, 0x04, 0x0B, 0x62,
        3, 0x04, 0x0C, 0x00,
        3, 0x04, 0x0D, 0x00,
        3, 0x04, 0x0E, 0xAC,
        3, 0x04, 0x0F, 0xBC,
        3, 0x04, 0x10, 0xB0,
        3, 0x04, 0x11, 0x6C,
        3, 0x04, 0x12, 0x30,
    };

    if (!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;


    switch (dataType) {
        case ISC_MAX96712_DATA_TYPE_RAW10:
            bpp = 0xA; /* 10 bits per pixel */
            dataFormat = 0x2B; /* raw10 */
            break;
        case ISC_MAX96712_DATA_TYPE_RAW12:
            bpp = 0xC; /* 12 bits per pixel */
            dataFormat = 0x2C; /* raw12 */
            break;
        case ISC_MAX96712_DATA_TYPE_RAW16:
            bpp = 0x10; /* 16 bits per pixel */
            dataFormat = 0x2E; /* raw16 */
            break;
        case ISC_MAX96712_DATA_TYPE_RGB:
            bpp = 0x18; /* 24 bits per pixel */
            dataFormat = 0x24; /* RGB */
            break;
        default:
            return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    /* soft_bpp_1 */
    dataTypeRegs[3] &= ~(0x1F << 3);
    dataTypeRegs[3] |= (bpp & 0x1F) << 3;
    /* soft_bpp_2 */
    dataTypeRegs[27] = (bpp & 0x1F);
    /* soft_bpp_3 */
    dataTypeRegs[27] |= (bpp & 0x1C) << 3;
    dataTypeRegs[31] = (bpp & 0x3);
    /* soft_bpp_4 */
    dataTypeRegs[31] |= (bpp & 0x1F) << 2;

    /* soft_dt_1 */
    dataTypeRegs[15] = (dataFormat & 0x3F);
    /* soft_dt_2 */
    dataTypeRegs[15] |= (dataFormat & 0x30) << 2;
    dataTypeRegs[19] = (dataFormat & 0x0F);
    /* soft_dt_3 */
    dataTypeRegs[19] |= (dataFormat & 0x3C) << 2;
    dataTypeRegs[23] = (dataFormat & 0x3);
    /* soft_dt_4 */
    dataTypeRegs[23] |= (dataFormat & 0x3F) << 2;

    status = WriteArray(handle,
                        GET_SIZE(dataTypeRegs),
                        dataTypeRegs);
    if (status != NVMEDIA_STATUS_OK)
        return status;

    /* enable software override of BPP */
    data[1] = 0x04; /* high byte offset */
    data[2] = 0x15; /* low byte offset */

    for (i = 0; i < MAX_NUM_GMSL_LINKS / 2; i++) {
        data[2] = 0x15 + i * 0x3;
        status = NvMediaISCDeviceRead(handle, 0,         /* transaction */
                             REGISTER_ADDRESS_BYTES, /* regLength */
                             &data[1],               /* regData */
                             1,                      /* dataLength */
                             &data[3]);              /* data */
        if (status != NVMEDIA_STATUS_OK)
            return status;

        data[3] &= 0x3F;
        data[3] |= 0xC0;

        status = WriteArray(handle,
                            COMMAND_SIZE_IN_BYTES,
                            data);
        if (status != NVMEDIA_STATUS_OK)
            return status;
    }

    return status;
}

static NvMediaStatus
OverrideCSIPrebeginTime(
    NvMediaISCDevice *handle,
    uint8_t csiPrebeginTime)
{
    NvMediaStatus status;
    uint8_t data[4] = {3, 0x08, 0xAD, 0};

    if (!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;


    status = NvMediaISCDeviceRead(handle,0,
                         REGISTER_ADDRESS_BYTES,
                         &data[1],
                         1,
                         &data[3]);
    if (status != NVMEDIA_STATUS_OK)
        return status;

    data[3] &= ~0x3F;
    data[3] |= (csiPrebeginTime & 0x3F);

    return WriteArray(handle,
                      GET_SIZE(data),
                      data);
}

static NvMediaStatus
OverrideCSIPostLength(
    NvMediaISCDevice *handle,
    uint8_t csiPostLength)
{
    NvMediaStatus status;
    uint8_t data[4] = {3, 0x08, 0xAE, 0};

    if (!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;


    status = NvMediaISCDeviceRead(handle,0,
                         REGISTER_ADDRESS_BYTES,
                         &data[1],
                         1,
                         &data[3]);
    if (status != NVMEDIA_STATUS_OK)
        return status;

    data[3] &= ~(0x3F << 2);
    data[3] |= (csiPostLength & 0x3F) << 2;

    return WriteArray(handle,
                      GET_SIZE(data),
                      data);
}

static NvMediaStatus
EnableCSI(
    NvMediaISCDevice *handle,
    bool enable)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t max96712_enable_csi_out[3] = {
        0x04, 0x0B, 0x02   /* Enable CSI out */
    };

    if (!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;


    status = NvMediaISCDeviceRead(handle, 0,               /* transaction */
                         REGISTER_ADDRESS_BYTES,       /* regLength */
                         max96712_enable_csi_out,      /* regData */
                         1,                            /* dataLength */
                         &max96712_enable_csi_out[2]); /* data */
    if (status != NVMEDIA_STATUS_OK)
        return status;

    if (enable) {
        max96712_enable_csi_out[2] |= (1 << 1);
    } else {
        max96712_enable_csi_out[2] &= ~(1 << 1);
    }

    status = NvMediaISCDeviceWrite(handle, 0,
                          COMMAND_SIZE_IN_BYTES,
                          max96712_enable_csi_out);
    if (status != NVMEDIA_STATUS_OK)
        return status;

    return status;
}

static NvMediaStatus
CheckCSIPLLLock(
    NvMediaISCDevice *handle)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    uint8_t i = 0u, mipiCfg;
    uint8_t CSIPllLock[3] = {
        0x04, 0x00, 0x00
    };
    uint8_t forcePllLock[3] = {
        0x1C, 0x12, 0x4B
    };

    mipiCfg = drvHandle->ctx.mipiCfg;

    for (i = 0u; i < 20u; i++) {
        status = NvMediaISCDeviceRead(handle, 0u,              /* transaction */
                                      REGISTER_ADDRESS_BYTES,  /* regLength */
                                      CSIPllLock,              /* regData */
                                      1u,                      /* dataLength */
                                      &CSIPllLock[2]);         /* data */
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }

        if (((mipiCfg == MAX96712_MIPI_OUT_2x4) && ((CSIPllLock[2] & 0xF0) == 0x60)) ||
            ((mipiCfg == MAX96712_MIPI_OUT_4x2) && ((CSIPllLock[2] & 0xF0) == 0xF0))) {
            break;
        }
        nvsleep(10000);
    }

    for (i = 0u; i < 4u; i++) {
        if ((mipiCfg == MAX96712_MIPI_OUT_2x4) && ((i == 0u) || (i == 3u))) {
            continue;
        }

        /* Force to lock CSIPLL. This is not recommended but keep this logic as a backup plan */
        if (!(((CSIPllLock[2] >> 4) >> i) & 0x1)) {
            LOG_MSG("Warning : %s %d: CSIPLL Unlocked 0x%x\n", __func__, __LINE__, CSIPllLock[2]);
            forcePllLock[0] = 0x1C + i;
            status = NvMediaISCDeviceWrite(handle, 0u,
                                           3u,
                                           forcePllLock);
            if (status != NVMEDIA_STATUS_OK) {
                return status;
            }
        }
    }

    CSIPllLock[2] = 0u;
    status = NvMediaISCDeviceRead(handle, 0u,              /* transaction */
                                  REGISTER_ADDRESS_BYTES,  /* regLength */
                                  CSIPllLock,              /* regData */
                                  1u,                      /* dataLength */
                                  &CSIPllLock[2]);         /* data */
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    if (((mipiCfg == MAX96712_MIPI_OUT_2x4) && ((CSIPllLock[2] & 0xF0) != 0x60)) ||
        ((mipiCfg == MAX96712_MIPI_OUT_4x2) && ((CSIPllLock[2] & 0xF0) != 0xF0))) {
        LOG_ERR("%s: CSI PLL unlock 0x%x\n", __func__, CSIPllLock[2] & 0xF0);
        return NVMEDIA_STATUS_ERROR;
    }

    return status;
}

static NvMediaStatus
ControlControlChannel(
        NvMediaISCDevice *handle,
        uint32_t link,
        bool enable)
{
    uint8_t data[3]  = {0x0B, 0x04, 0x00};
    uint8_t gmslMode;
    NvMediaStatus status;
    _DriverHandle *drvHandle = NULL;

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if (link > 4)
        return NVMEDIA_STATUS_BAD_PARAMETER;

    gmslMode = drvHandle->ctx.gmslMode;

    if (gmslMode == ISC_MAX96712_GMSL1_MODE) {
        data[0] += link;

        status = NvMediaISCDeviceRead(handle, 0,         /* transaction */
                             REGISTER_ADDRESS_BYTES, /* regLength */
                             data,                   /* regData */
                             1,                      /* dataLength */
                             &data[2]);              /* data */
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }

        if (enable)
            data[2] |= (1 << 1);
        else if (!enable)
            data[2] &= ~(1 << 1);
        else
            return NVMEDIA_STATUS_BAD_PARAMETER;
    } else if (gmslMode == ISC_MAX96712_GMSL2_MODE) {
        data[0] = 0x00;
        data[1] = 0x03;

        status = NvMediaISCDeviceRead(handle, 0,          /* transaction */
                             REGISTER_ADDRESS_BYTES,  /* regLength */
                             data,                    /* regData */
                             1,                       /* dataLength */
                             &data[2]);               /* data */
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }

        if (enable)
            data[2] &= ~(3 << (link * 2));
        else if (!enable)
            data[2] |= (3 << (link * 2));
        else
            return NVMEDIA_STATUS_BAD_PARAMETER;
    } else {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    status = NvMediaISCDeviceWrite(handle, 0,
                          3,
                          data);
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    nvsleep(10000);

    return status;
}

static NvMediaStatus
EnableControlChannel(
        NvMediaISCDevice *handle,
        bool enable)
{
    uint8_t i;
    NvMediaStatus status;

    if (!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;

    for (i = 0; i < MAX_NUM_GMSL_LINKS; i++) {
        status = ControlControlChannel(handle, i, enable);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }
    }

    return status;
}

static NvMediaStatus
SwapBits(
        NvMediaISCDevice *handle,
        uint8_t link)
{
    NvMediaStatus status;
    uint8_t i;
    uint8_t swap[] = {
        3, 0x01, 0xC0, 0x00, /* CROSS_0 */
        3, 0x01, 0xC1, 0x00, /* CROSS_1 */
        3, 0x01, 0xC2, 0x00, /* CROSS_2 */
        3, 0x01, 0xC3, 0x00, /* CROSS_3 */
        3, 0x01, 0xC4, 0x00, /* CROSS_5 */
        3, 0x01, 0xC5, 0x00, /* CROSS_6 */
        3, 0x01, 0xC6, 0x00, /* CROSS_7 */
        3, 0x01, 0xC7, 0x00, /* CROSS_8 */
        3, 0x01, 0xC8, 0x00, /* CROSS_9 */
        3, 0x01, 0xC9, 0x00, /* CROSS_10 */
        3, 0x01, 0xCA, 0x00, /* CROSS_11 */
        3, 0x01, 0xCB, 0x00, /* CROSS_12 */
    };

    if (!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;

    /* Swap bit 0 <--> 11, 1 <--> 10, 2 <--> 9, etc */
    /* CROSS_0 <--> CROSS_11, CROSS_1 <--> CROSS_10, etc */
    for (i = 0; i < 12; i++) {
        swap[1 + 0x04 * i] = (link > 1) ? 0x02 : 0x01;
        swap[2 + 0x04 * i] += link * 0x20;
        swap[3 + 0x04 * i] = 11 - i;
    }

    status = WriteArray(handle,
                        GET_SIZE(swap),
                        swap);
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    return status;
}

static NvMediaStatus
ReadStatus(
    NvMediaISCDevice *handle,
    uint32_t parameterType,
    uint8_t link,
    uint8_t *errVal)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t data[4] = {3, 0x00, 0x00, 0x00};

    if (!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;


    switch (parameterType) {
        case ISC_READ_PARAM_CMD_MAX96712_CONTROL_CHANNEL_CRC_ERROR:
            data[1] = 0x0B + link;
            data[2] = 0x19;

            status = NvMediaISCDeviceRead(handle,0,
                                 REGISTER_ADDRESS_BYTES,
                                 &data[1],
                                 1,
                                 &data[3]);
            if (status != NVMEDIA_STATUS_OK)
                return status;

            *errVal = data[3];
            break;
        default:
            return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    return status;
}

static NvMediaStatus
TriggerDeskew(
    NvMediaISCDevice *handle)
{
    NvMediaStatus status;
    uint8_t i = 0u, phy_num = 0u;
    uint8_t data[] = {
        3, 0x09, 0x03, 0x00,
    };

    /* Trigger the initial deskew patterns two times
     * to make sure Rx device recevies the patterns */
    for (i = 0; i < 2u; i++) {
        for (phy_num = 0; phy_num < MAX96712_MAX_NUM_PHY; phy_num++) {
            data[2] = 0x40 * phy_num + 0x03; /* Update the register offset */
            status = NvMediaISCDeviceRead(handle,0,
                                          REGISTER_ADDRESS_BYTES,
                                          &data[1],
                                          1,
                                          &data[3]);
            if (status != NVMEDIA_STATUS_OK) {
                return status;
            }

            data[3] ^= (1 << 5);
            status = WriteArray(handle,
                                COMMAND_SIZE_IN_BYTES,
                                data);
            if (status != NVMEDIA_STATUS_OK) {
                return status;
            }
        }
        nvsleep((i == 0) ? 10000 : 0);
    }

    return status;
}

static NvMediaStatus
EnableConcatenation(
    NvMediaISCDevice *handle,
    uint8_t numSensor)
{
    NvMediaStatus status;
    uint8_t data[4] = {3, 0x09, 0x31, 0x00};
    uint8_t txPort;
    _DriverHandle *drvHandle = NULL;

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if (numSensor == 0)
        return NVMEDIA_STATUS_BAD_PARAMETER;

    txPort = drvHandle->ctx.txPort;

    /* Update the base address for Tx port; Phy C/D/E/F */
    data[2] += (txPort * 0x40);

    /* Update the number of sensors to be concatenated */
    data[3] = (numSensor == 1) ? 0x1 :
              (numSensor == 2) ? 0x3 :
              (numSensor == 3) ? 0x7 : 0xF;
    data[3] |= (MAX96712_ENABLE_CONCATENATION << MAX96712_CONCATENATION_SHIFT);

    status = WriteArray(handle,
                        COMMAND_SIZE_IN_BYTES,
                        data);
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    return status;
}

static NvMediaStatus
EnableMemoryECC(
    NvMediaISCDevice *handle,
    bool enable2bitReport,
    bool enable1bitReport)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    uint8_t data[4] = {3, 0x00, 0x44, 0x0F};

    if ((drvHandle->ctx.revision != ISC_MAX96712_REV_3) &&
        (drvHandle->ctx.revision != ISC_MAX96712_REV_4)) {
        return NVMEDIA_STATUS_NOT_SUPPORTED;
    }

    if (enable2bitReport) {
        data[3] |= (1 << 7);
    }
    if (enable1bitReport) {
        data[3] |= (1 << 6);
    }

    status = WriteArray(handle,
                        COMMAND_SIZE_IN_BYTES,
                        data);
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    return status;
}

static NvMediaStatus
ConfigureMIPIOutput(
    NvMediaISCDevice *handle)
{
    NvMediaStatus status;
    uint8_t data[4] = {3, 0x08, 0xA0, 0x00}, i = 0;
    RevisionMAX96712 rev;
    uint8_t mipiCfg;
    PHYModeMax96712 phyMode = ISC_MAX96712_DPHY;
    uint8_t mipiSpeed = ISC_MAX96712_MIPI_SPEED_2500_MHZ;
    _DriverHandle *drvHandle = NULL;

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    mipiCfg = drvHandle->ctx.mipiCfg;
    rev = drvHandle->ctx.revision;
    phyMode = drvHandle->ctx.phyMode;
    mipiSpeed = drvHandle->ctx.mipiSpeed;

    /* Revision 2 and 3 specific settings */
    if (((rev == ISC_MAX96712_REV_2) || (rev == ISC_MAX96712_REV_3) ||
         (rev == ISC_MAX96712_REV_4)) &&
        (phyMode == ISC_MAX96712_DPHY)) {
        mipiCfg |= (1 << 5); /* Force DPHY0 clk enalbed */
    }

    status = NvMediaISCDeviceRead(handle,0,
                         REGISTER_ADDRESS_BYTES,
                         &data[1],
                         1,
                         &data[3]);
    if (status != NVMEDIA_STATUS_OK)
        return status;

    /* MIPI Phy configuration */
    data[3] &= ~0x3F;
    data[3] |= mipiCfg;

    status = WriteArray(handle,
                        COMMAND_SIZE_IN_BYTES,
                        data);
    if (status != NVMEDIA_STATUS_OK)
        return status;

    /* Set prebegin phase, post length and prepare for CPHY mode
     * This is a requirement for CPHY periodic calibration */
    if (phyMode == ISC_MAX96712_CPHY) {
        data[1] = 0x08;
        data[2] = 0xAD;
        /* t3_prebegin = (63 + 1) * 7 = 448 UI */
        data[3] = 0x3F;

        status = WriteArray(handle,
                            COMMAND_SIZE_IN_BYTES,
                            data);
        if (status != NVMEDIA_STATUS_OK)
            return status;

        data[2] = 0xAE;
        /* Bit[6:2] = t3_post = (31 + 1) * 7 = 224 UI
         * Bit[1:0] = t3_prepare = 86.7ns */
        data[3] = 0x7F;

        status = WriteArray(handle,
                            COMMAND_SIZE_IN_BYTES,
                            data);
        if (status != NVMEDIA_STATUS_OK)
            return status;
    }

    /* Put all Phys in standby mode */
    data[1] = 0x08;
    data[2] = 0xA2;
    data[3] = 0xF4; /* Bug 200383247 : t_lpx 106.7 ns */

    status = WriteArray(handle,
                        COMMAND_SIZE_IN_BYTES,
                        data);
    if (status != NVMEDIA_STATUS_OK)
        return status;

    /* Set CSI2 lane count per Phy */
    data[1] = 0x09; /* high byte offset */
    data[2] = 0x0A; /* low byte offset */

    /* Configure Lane count for Port A in x4 or C,D in x2 */
    switch(drvHandle->ctx.mipiCfg) {
        case MAX96712_MIPI_OUT_4x2:
        case MAX96712_MIPI_OUT_4Bx22A:
            data[3] = 0x40; /* 2 data lanes */
            break;
        default:
            data[3] = 0xC0; /* 4 data lanes */
    }

    /* Update CPHY mode if neeeded */
    data[3] |= (phyMode << 5);

    for (i = 0; i < 2; i++) {
        data[2] = 0x0A + i * 0x40;
        status = WriteArray(handle,
                            COMMAND_SIZE_IN_BYTES,
                            data);
        if (status != NVMEDIA_STATUS_OK)
            return status;
    }

    /* Configure Lane count for Port B in x4 or E,F in x2 */
    switch(drvHandle->ctx.mipiCfg) {
        case MAX96712_MIPI_OUT_4x2:
        case MAX96712_MIPI_OUT_4Ax22B:
            data[3] = 0x40; /* 2 data lanes */
            break;
        default:
            data[3] = 0xC0; /* 4 data lanes */
    }

    /* Update CPHY mode if neeeded */
    data[3] |= (phyMode << 5);

    for (i = 0; i < 2; i++) {
        data[2] = 0x8A + i * 0x40;
        status = WriteArray(handle,
                            COMMAND_SIZE_IN_BYTES,
                            data);
        if (status != NVMEDIA_STATUS_OK)
            return status;
    }

    /* Mapping data lanes Port A */
    data[1] = 0x08; /* high byte offset */
    data[2] = 0xA3; /* low byte offset */

    switch(drvHandle->ctx.mipiCfg) {
        case MAX96712_MIPI_OUT_4x2:
        case MAX96712_MIPI_OUT_4Bx22A:
            data[3] = 0x44; /* 2 data lanes */
            break;
        default:
            data[3] = 0xE4; /* 4 data lanes */
    }

    status = WriteArray(handle,
                        COMMAND_SIZE_IN_BYTES,
                        data);
    if (status != NVMEDIA_STATUS_OK)
        return status;

    /* Mapping data lanes Port B */
    data[1] = 0x08; /* high byte offset */
    data[2] = 0xA4; /* low byte offset */

    switch(drvHandle->ctx.mipiCfg) {
        case MAX96712_MIPI_OUT_4x2:
        case MAX96712_MIPI_OUT_4Ax22B:
            data[3] = 0x44; /* 2 data lanes */
            break;
        default:
            data[3] = 0xE4; /* 4 data lanes */
    }

    status = WriteArray(handle,
                        COMMAND_SIZE_IN_BYTES,
                        data);
    if (status != NVMEDIA_STATUS_OK)
        return status;

    /* deactive DPLL */
    data[1] = 0x1C; /* high byte offset */
    data[2] = 0x00; /* low byte offset */
    data[3] = 0xF4;

    for (i = 0; i < MAX_NUM_GMSL_LINKS; i++) {
        data[1] = 0x1C + i;

        status = WriteArray(handle,
                            COMMAND_SIZE_IN_BYTES,
                            data);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }
    }

    /* Set MIPI speed : 2.5GHz */
    data[1] = 0x04; /* high byte offset */
    data[2] = 0x15; /* low byte offset */

    for (i = 0; i < MAX_NUM_GMSL_LINKS; i++) {
        data[2] = 0x15 + i * 0x3;
        status = NvMediaISCDeviceRead(handle,0,
                             REGISTER_ADDRESS_BYTES,
                             &data[1],
                             1,
                             &data[3]);
        if (status != NVMEDIA_STATUS_OK)
            return status;

        data[3] &= ~0x3F;
        data[3] |= (1 << 5) | mipiSpeed;

        status = WriteArray(handle,
                            COMMAND_SIZE_IN_BYTES,
                            data);
        if (status != NVMEDIA_STATUS_OK)
            return status;
    }

    /* active DPLL */
    data[1] = 0x1C; /* high byte offset */
    data[2] = 0x00; /* low byte offset */
    data[3] = 0xF5;

    for (i = 0; i < MAX_NUM_GMSL_LINKS; i++) {
        data[1] = 0x1C + i;

        status = WriteArray(handle,
                            COMMAND_SIZE_IN_BYTES,
                            data);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }
    }

    /* Deskew is enabled if MIPI speed is faster than or equal to 1.5GHz */
    if ((phyMode == ISC_MAX96712_DPHY) && (mipiSpeed >= 15)) {
        data[1] = 0x09; /* high byte offset */
        data[2] = 0x03; /* low byte offset */
        data[3] = 0x97; /* enable the initial deskew with 8 * 32K UI */
        for (i = 0; i < MAX96712_MAX_NUM_PHY; i++) {
            data[2] += 0x40;
            status = WriteArray(handle,
                                COMMAND_SIZE_IN_BYTES,
                                data);
            if (status != NVMEDIA_STATUS_OK)
                return status;
        }
    }

    return status;
}

static NvMediaStatus
SetTxSRCId(
    NvMediaISCDevice *handle)
{
    NvMediaStatus status;
    uint8_t data[] = {
        3, 0x05, 0x03, 0x00,
        3, 0x05, 0x13, 0x01,
        3, 0x05, 0x23, 0x02,
        3, 0x05, 0x33, 0x03,
    };

    if (NULL == handle) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    /* Write the array */
    status = WriteArray(handle,
                        GET_SIZE(data),
                        data);
    return status;
}

static NvMediaStatus
SetI2CLocalACK(
    NvMediaISCDevice *handle)
{
    NvMediaStatus status;
    uint8_t data[4] = {3, 0x0B, 0x0D, 0}, i = 0;

    if (!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;


    for (i = 0; i < MAX_NUM_GMSL_LINKS; i++) {
        data[1] = 0xB + i;
        status = NvMediaISCDeviceRead(handle,0,
                             REGISTER_ADDRESS_BYTES,
                             &data[1],
                             1,
                             &data[3]);
        if (status != NVMEDIA_STATUS_OK)
            return status;

        /* I2C-to-I2C slave generates local ack */
        data[3] |= (MAX96712_SET_I2C_LOCAL_ACK
                    << MAX96712_I2C_LOCAL_ACK_SHIFT);

        status = WriteArray(handle,
                            COMMAND_SIZE_IN_BYTES,
                            data);
        if (status != NVMEDIA_STATUS_OK)
            return status;
    }

    return status;
}

static NvMediaStatus
DisableDE(
    NvMediaISCDevice *handle,
    uint32_t link)
{
    NvMediaStatus status;
    uint8_t data[4] = {3, 0x0B, 0x0F, 0x01};

    data[1] = 0x0B + link;

    status = WriteArray(handle,
                        COMMAND_SIZE_IN_BYTES,
                        data);
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    return status;
}

static NvMediaStatus
EnableSpecificPipelines(
    NvMediaISCDevice *handle,
    uint32_t numPipelines)
{
    NvMediaStatus status;
    uint8_t data[] = {
        3, 0x00, 0xF4, 0x00,
    };

    if (NULL == handle) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if (numPipelines > MAX96712_NUM_VIDEO_PIPELINES) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    data[3] = ((1u << numPipelines) - 1u);

    status = WriteArray(handle,
                        GET_SIZE(data),
                        data);

    nvsleep(10000);
    return status;

}

static NvMediaStatus
SetReset(
    NvMediaISCDevice *handle)
{
    NvMediaStatus status;
    uint8_t data[4] = {3, 0x00, 0x13, 0x75};

    if (!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;

    status = WriteArray(handle,
                        COMMAND_SIZE_IN_BYTES,
                        data);

    nvsleep(10000);

    return status;
}

static NvMediaStatus
SetDBL(
    NvMediaISCDevice *handle,
    uint8_t link)
{
    NvMediaStatus status;
    uint8_t data[4] = {3, 0x0B, 0x07, 0x8C};

    if (!handle) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    data[1] = 0x0B + link;

    status = WriteArray(handle,
                        COMMAND_SIZE_IN_BYTES,
                        data);

    nvsleep(10000);
    return status;
}

static NvMediaStatus
EnableLinks(
    NvMediaISCDevice *handle,
    uint8_t links)
{
    NvMediaStatus status;

    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    uint8_t gmslMode = drvHandle->ctx.gmslMode;
    uint8_t data[4] = {3, 0x00, 0x06, 0};

    if (gmslMode == ISC_MAX96712_GMSL2_MODE) {
        data[3] = 0xF0;
    }

    switch(links) {
        case ISC_CONFIG_MAX96712_ENABLE_LINKS_0123:
            data[3] |= (1 << 3);
        case ISC_CONFIG_MAX96712_ENABLE_LINKS_012:
            data[3] |= (1 << 2);
        case ISC_CONFIG_MAX96712_ENABLE_LINKS_01:
            data[3] |= (1 << 1);
        case ISC_CONFIG_MAX96712_ENABLE_LINK_0:
            data[3] |= (1 << 0);
            break;
        case ISC_CONFIG_MAX96712_ENABLE_LINKS_23:
            data[3] |= (1 << 3);
        case ISC_CONFIG_MAX96712_ENABLE_LINK_2:
            data[3] |= (1 << 2);
            break;
        case ISC_CONFIG_MAX96712_ENABLE_LINKS_02:
            data[3] |= (1 << 0);
            data[3] |= (1 << 2);
            break;
        case ISC_CONFIG_MAX96712_DISABLE_LINKS_0123:
            data[3] &= 0xF0;
            break;
        default:
            return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    status = WriteArray(handle,
                        COMMAND_SIZE_IN_BYTES,
                        data);
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    /* From Max96712 programming guide V1.1, typical link rebuilding time is 25 ~ 100ms
     * Add the minimum delay
     */
    nvsleep(25000);

    return status;
}

static NvMediaStatus
EnableSpecificLink(
    NvMediaISCDevice *handle,
    uint8_t link)
{
    NvMediaStatus status;
    uint8_t data[4] = {3, 0x00, 0x06, 0};
    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    uint8_t gmslMode = drvHandle->ctx.gmslMode;

    data[3] = (gmslMode == ISC_MAX96712_GMSL1_MODE) ? 0x00 : 0xF0;
    data[3] |= (1 << link);

    status = WriteArray(handle,
                        COMMAND_SIZE_IN_BYTES,
                        data);
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    /* From Max96712 programming guide V1.1, typical link rebuilding time is 25 ~ 100ms
     * Add the minimum delay
     */
    nvsleep(25000);

    return status;
}

static NvMediaStatus
EnablePeriodicAEQ(
    NvMediaISCDevice *handle,
    uint8_t link)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t data[4] = {3, 0x0B, 0x14, 0xD0};

    data[1] += link;

    status = WriteArray(handle,
                        COMMAND_SIZE_IN_BYTES,
                        data);
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    LOG_MSG("MAX96712: Enable periodic AEQ on Link %d\n", link);

    return status;
}

static NvMediaStatus
EnableReplication(
    NvMediaISCDevice *handle,
    bool enable)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t port, mipiCfg, i = 0u;
    uint8_t data[] = {
        3, 0x08, 0xA9, 0, /* For the replication from Tegra A to Tegra B */
        3, 0x08, 0xAA, 0  /* For the replication from Tegra A to Tegra C */
    };
    RevisionMAX96712 rev;
    PHYModeMax96712 phyMode;
    NvMediaBool slaveEnabled;
    _DriverHandle *drvHandle = NULL;

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    port = drvHandle->ctx.txPort;
    mipiCfg = drvHandle->ctx.mipiCfg;
    rev = drvHandle->ctx.revision;
    phyMode = drvHandle->ctx.phyMode;
    slaveEnabled = drvHandle->ctx.slaveEnabled;

    /* Replication is not supported on Rev 1 in CPHY mode */
    if ((rev == ISC_MAX96712_REV_1) && (phyMode == ISC_MAX96712_CPHY)) {
        LOG_ERR("%s: Replication is not supported on MAX96712 Rev 1 in CPHY mode. "
                "Please use a platform with MAX96712 Rev 2 for this feature\n",
                __func__);
        return NVMEDIA_STATUS_ERROR;
    }

    if ((!slaveEnabled) && enable) { /* Update the replication but do NOT enable the replication */
        switch(port) {
            case ISC_MAX96712_TXPORT_PHY_C :
                if ((mipiCfg == MAX96712_MIPI_OUT_4x2) || (mipiCfg == MAX96712_MIPI_OUT_4Ax22B)) {
                    data[3] = MAX96712_REPLICATION(ISC_MAX96712_TXPORT_PHY_C, ISC_MAX96712_TXPORT_PHY_E);
                    data[7] = MAX96712_REPLICATION(ISC_MAX96712_TXPORT_PHY_C, ISC_MAX96712_TXPORT_PHY_F);
                    if (rev != ISC_MAX96712_REV_4) {
                        /* 3rd I2C port connected to 3rd Xavier is enabled by default only in MAX96712 Rev D(4)
                         * For other revisions, the replication from PHY C to PHY F is enabled by the master
                         */
                        data[7] |= (1 << 7);
                    }
                } else if (mipiCfg == MAX96712_MIPI_OUT_2x4) {
                    data[3] = MAX96712_REPLICATION(ISC_MAX96712_TXPORT_PHY_D, ISC_MAX96712_TXPORT_PHY_E);
                } else {
                    return NVMEDIA_STATUS_BAD_PARAMETER;
                }
                break;
            case ISC_MAX96712_TXPORT_PHY_D :
                if ((mipiCfg == MAX96712_MIPI_OUT_2x4) || (mipiCfg == MAX96712_MIPI_OUT_4x2)) {
                    data[3] = MAX96712_REPLICATION(ISC_MAX96712_TXPORT_PHY_D, ISC_MAX96712_TXPORT_PHY_E);
                } else {
                    return NVMEDIA_STATUS_BAD_PARAMETER;
                }
                break;
            case ISC_MAX96712_TXPORT_PHY_E :
                if (mipiCfg == MAX96712_MIPI_OUT_2x4) {
                    data[3] = MAX96712_REPLICATION(ISC_MAX96712_TXPORT_PHY_E, ISC_MAX96712_TXPORT_PHY_D);
                } else if (mipiCfg == MAX96712_MIPI_OUT_4x2) {
                    data[3] = MAX96712_REPLICATION(ISC_MAX96712_TXPORT_PHY_E, ISC_MAX96712_TXPORT_PHY_C);
                } else {
                    return NVMEDIA_STATUS_BAD_PARAMETER;
                }
                break;
            default :
                data[3] = MAX96712_REPLICATION(ISC_MAX96712_TXPORT_PHY_C, ISC_MAX96712_TXPORT_PHY_E);
                break;
        }
        status = WriteArray(handle,
                            sizeof(data),
                            data);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }
    } else if (slaveEnabled) {
        if ((port == ISC_MAX96712_TXPORT_PHY_F) && (rev != ISC_MAX96712_REV_4)) {
	    LOG_INFO("The replication mode is already enabled\n");
            return NVMEDIA_STATUS_OK;
	}

        for (i = 0u; i < 2u; i++) {
            status = NvMediaISCDeviceRead(handle, 0,
                                          REGISTER_ADDRESS_BYTES, /* regLength */
                                          &data[1 + i * 4],       /* regData */
                                          1,                      /* dataLength */
                                          &data[3 + i * 4]);      /* data */
            if (status != NVMEDIA_STATUS_OK) {
                return status;
            }

            if (((data[3 + i * 4] >> 5) & 3) == port) { /* if the destination is same as port */
                if (enable) {
                    data[3 + i * 4] |= (1 << 7); /* Enable the replication */
                } else {
                    data[3 + i * 4] &= ~(1 << 7); /* Disable the replication */
                }
            }
        }

        status = WriteArray(handle,
                            sizeof(data),
                            data);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }
    }

    return status;
}

static NvMediaStatus
SetVideoPipelineMapping(
    NvMediaISCDevice *handle,
    const VideoPipelineMappingMAX96712 *pipelineMappings,
    uint8_t numPipelines)
{
    NvMediaStatus status;
    uint8_t i, arrayIndex, offset, value;
    uint8_t data[] = {
        3, 0x00, 0xF0, 0x00,
        3, 0x00, 0xF1, 0x00,
        3, 0x00, 0xF2, 0x00,
        3, 0x00, 0xF3, 0x00,
        3, 0x00, 0xF4, 0x00,
    };

    if (NULL == handle) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if (numPipelines > MAX96712_NUM_VIDEO_PIPELINES) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    for (i = 0; i < numPipelines; i++) {
        arrayIndex = ((i / 2) * 4) + 3;
        offset = ((i % 2) == 0) ? 0 : 4;
        value = (pipelineMappings[i].link & 0x3) << 2;
        value |= (pipelineMappings[i].serializerStream & 0x3);
        data[arrayIndex] |= (value << offset);
    }

    data[19] = ((1 << numPipelines) - 1);

    status = WriteArray(handle,
                        GET_SIZE(data),
                        data);

    nvsleep(10000);
    return status;

}

static NvMediaStatus
GetRevId (
    NvMediaISCDevice *handle,
    RevisionMAX96712 *rev)
{
    NvMediaStatus status;
    uint8_t readBuff[1] = {0u};
    uint32_t revision;
    uint32_t i = 0, numRev = sizeof(supportedRevisions) / sizeof(supportedRevisions[0]);

    if ((handle == NULL)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }
    status = MAX96712ReadRegister(handle, 0,
                                  REG_DEV_REV_ADDR,
                                  1,
                                  readBuff);
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    revision = readBuff[0] & 0x0F;

    for (i = 0; i < numRev; i++) {
        if (revision == supportedRevisions[i].revVal) {
            *rev = supportedRevisions[i].revId;
            LOG_MSG("Max96712 Rev %u detected!\n", revision);
            return NVMEDIA_STATUS_OK;
        }
    }

    *rev = ISC_MAX96712_INVALID_REV;
    LOG_MSG("Unsupported MAX96712 revision %u detected! Supported revisions are:\n",
            revision);
    for (i = 0; i < numRev; i++) {
        LOG_MSG("MAX96712 Rev%u\n", supportedRevisions[i].revVal);
    }

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
SetPGClkFreq(
    NvMediaISCDevice *handle,
    PGClockFreqMAX96712 pgClkFreq)
{
    uint8_t data[4] = {3, 0x00, 0x09, 0};

    if (!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;

    if (pgClkFreq >= ISC_MAX96712_NUM_PG_CLK_SETTINGS) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    data[3] = (uint8_t)pgClkFreq;

    return WriteArray(handle,
                      COMMAND_SIZE_IN_BYTES,
                      data);
}

static NvMediaStatus
SetPGPipelineClkSrc(
    NvMediaISCDevice *handle,
    PGClockFreqMAX96712 *pgPipelineClkSrc)
{
    NvMediaStatus status;
    uint8_t data[4] = {3, 0x01, 0xDC, 0};
    uint16_t regAddr = 0;
    uint32_t i = 0;

    if (!handle || !pgPipelineClkSrc)
        return NVMEDIA_STATUS_BAD_PARAMETER;

    for (i = 0; i < MAX96712_NUM_VIDEO_PIPELINES; i++) {
        regAddr = 0x01DC + (i * 0x20);
        data[1] = (uint8_t)(regAddr >> 8);
        data[2] = (uint8_t)(regAddr & 0xFF);

        status = MAX96712ReadRegister(handle, 0,
                                      regAddr,
                                      1,
                                      &data[3]);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }

        switch (pgPipelineClkSrc[i]) {
            case ISC_MAX96712_PG_CLK_150_MHZ:
                /* Clear bit 7 */
                data[3] &= ~0x80;
                break;
            case ISC_MAX96712_PG_CLK_375_MHZ:
                /* Set bit 7 */
                data[3] |= 0x80;
                break;
            default:
                return NVMEDIA_STATUS_BAD_PARAMETER;
        }
        status = WriteArray(handle,
                            sizeof(data),
                            data);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }
    }

    return status;
}

NvMediaStatus
MAX96712OneShotReset(
    NvMediaISCDevice *handle,
    uint32_t linkMask)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t data[4] = {3, 0x00, 0x18, 0x00};

    data[3] = linkMask & 0x0F;

    status = WriteArray(handle,
                        sizeof(data),
                        data);
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    nvsleep(100000);

    return status;
}

NvMediaStatus
MAX96712CheckLink(
    NvMediaISCDevice *handle,
    uint32_t linkIndex,
    uint32_t linkType,
    NvMediaBool display)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    RevisionMAX96712 rev;
    uint8_t data[4] = {3, 0x00, 0x00, 0x00};
    uint8_t gmsl2LinkLockRegs[4] = {0x1A, 0x0A, 0x0B, 0x0C};
    uint16_t gmsl2VideoLockRegs[4] = {0x01DC, 0x01FC, 0x021C, 0x023C};
    uint8_t gmslMode, loop = 0, lockSuccess = 0;
    _DriverHandle *drvHandle = NULL;

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if (linkIndex >= MAX_NUM_GMSL_LINKS) {
        LOG_ERR("%s: Invalid link index %u provided\n",  __func__, linkIndex);
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    rev = drvHandle->ctx.revision;
    gmslMode = drvHandle->ctx.gmslMode;

    /* Check lock for each link */
    switch (linkType) {
        case ISC_MAX96712_GMSL1_CONFIG_LINK_LOCK:
            if (gmslMode != ISC_MAX96712_GMSL1_MODE) {
                LOG_ERR("%s: Config link lock is only valid in GMSL1 mode\n",
                        __func__);
                return NVMEDIA_STATUS_BAD_PARAMETER;
            }

            data[1] = 0x0B + linkIndex;
            data[2] = 0xCB;

            /* From Max96712 programming guide V1.1, typical link rebuilding time is 25 ~ 100ms
             * check the link lock in 100ms periodically */
            for (loop = 0; loop < 10; loop++) {
                status = NvMediaISCDeviceRead(handle,0,
                                     REGISTER_ADDRESS_BYTES,
                                     &data[1],
                                     1,
                                     &data[3]);
                if (status != NVMEDIA_STATUS_OK) {
                    return status;
                }

                if (((data[3] >> 7) & 0x1) == 1) {
                    lockSuccess = 1;
                    break;
                }
                nvsleep(10000);
            }

            if (lockSuccess == 0) {
                if (display == NVMEDIA_TRUE) {
                    LOG_ERR("%s: Config link(%u) is not locked\n", __func__, linkIndex);
                }
                return NVMEDIA_STATUS_ERROR;
            }
            LOG_DBG("%s: Config link locked for link %u\n", __func__, linkIndex);
            break;
        case ISC_MAX96712_GMSL2_LINK_LOCK:
            if (gmslMode != ISC_MAX96712_GMSL2_MODE) {
                LOG_ERR("%s: GMSL2 link lock is only valid in GMSL2 mode\n",
                        __func__);
                return NVMEDIA_STATUS_BAD_PARAMETER;
            }

            /* Only register 0x001A is available on MAX96712 Rev 1 to check
             * link lock in GMSL2 mode*/
            if (rev == ISC_MAX96712_REV_1 && linkIndex > 0) {
                LOG_DBG("%s: GMSL2 link lock for link %u is not available on MAX96712 Rev 1\n",
                         __func__, linkIndex);
                break;
            }

            data[1] = 0x0;
            data[2] = gmsl2LinkLockRegs[linkIndex];

            /* From Max96712 programming guide V1.1, typical link rebuilding time is 25 ~ 100ms
             * check the link lock in 100ms periodically */
            for (loop = 0; loop < 10; loop++) {
                status = NvMediaISCDeviceRead(handle,0,
                                     REGISTER_ADDRESS_BYTES,
                                     &data[1],
                                     1,
                                     &data[3]);
                if (status != NVMEDIA_STATUS_OK)
                    return status;

                if (((data[3] >> 3) & 0x1) == 1) {
                    lockSuccess = 1;
                    break;
                }
                nvsleep(10000);
            }

            if (lockSuccess == 0) {
                if (display == NVMEDIA_TRUE) {
                    LOG_ERR("%s: GMSL2 Link(%u) is not locked\n", __func__, linkIndex);
                }
                return NVMEDIA_STATUS_ERROR;
            }
            LOG_DBG("%s: GMSL2 link locked for link %u\n", __func__, linkIndex);
            break;
        case ISC_MAX96712_VIDEO_LINK_LOCK:
            if (gmslMode == ISC_MAX96712_GMSL1_MODE) {
                data[1] = 0x0B + linkIndex;
                data[2] = 0xCB;

                for (loop = 0; loop < 30; loop++) {
                    status = NvMediaISCDeviceRead(handle,0,
                                         REGISTER_ADDRESS_BYTES,
                                         &data[1],
                                         1,
                                         &data[3]);
                    if (status != NVMEDIA_STATUS_OK) {
                        return status;
                    }

                    if (((data[3] & 0x1) == 1) && ((data[3] >> 6) & 1) == 1) {
                        LOG_DBG("%s: GMSL1 video on link %d is locked after %d ms\n", __func__, linkIndex, loop * 10u);
                        lockSuccess = 1;
                        break;
                    }
                    nvsleep(10000);
                }

                if (lockSuccess == 0) {
                    if (display == NVMEDIA_TRUE) {
                        LOG_ERR("%s: GMSL1 video on link(%u) is not locked\n", __func__, linkIndex);
                    }
                    return NVMEDIA_STATUS_ERROR;
                }
                LOG_DBG("%s: GMSL1 video on link %u is locked\n", __func__, linkIndex);
            } else if (gmslMode == ISC_MAX96712_GMSL2_MODE) {
                data[1] = (uint8_t)((gmsl2VideoLockRegs[linkIndex] >> 8) & 0xFF);
                data[2] = (uint8_t)(gmsl2VideoLockRegs[linkIndex] & 0xFF);


                for (loop = 0; loop < 30; loop++) {
                    status = NvMediaISCDeviceRead(handle,0,
                                                  REGISTER_ADDRESS_BYTES,
                                                  &data[1],
                                                  1,
                                                  &data[3]);
                    if (status != NVMEDIA_STATUS_OK) {
                        return status;
                    }

                    if ((data[3] & 1) == 1) {
                        LOG_DBG("%s: GMSL2 video on link %d is locked after %d ms\n", __func__, linkIndex, loop * 10u);
                        lockSuccess = 1;
                        break;
                    }
                    nvsleep(10000);
                }


                if (lockSuccess == 0) {
                    if (display == NVMEDIA_TRUE) {
                        LOG_ERR("%s: GMSL2 video on link(%u) is not locked\n", __func__, linkIndex);
                    }
                    return NVMEDIA_STATUS_ERROR;
                }
                LOG_DBG("%s: GMSL2 video on link %u is locked\n", __func__, linkIndex);
            }
            break;
        default:
            LOG_ERR("%s: Invalid link type provided\n", __func__);
            return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
GMSL2LinkAdaptation(
    NvMediaISCDevice *handle,
    uint32_t enumeratedDeviceConfig)
{
    NvMediaStatus status;
    RevisionMAX96712 rev;
    _DriverHandle *drvHandle = NULL;
    uint8_t osnCoff[3] = {0x14, 0x2E, 0x00};
    uint8_t osnDisAdap[3] = {0x14, 0x32, 0x00};
    uint8_t osnReset[3] = {0x14, 0x57, 0x00};
    uint8_t link = 0, gmslMode, loop = 0u;


    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    rev = drvHandle->ctx.revision;
    gmslMode = drvHandle->ctx.gmslMode;

    if (gmslMode != ISC_MAX96712_GMSL2_MODE) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    switch (enumeratedDeviceConfig) {
        case ISC_CONFIG_MAX96712_GMSL2_LINK_ADAPTATION_LINK0:
            link = 0;
            break;
        case ISC_CONFIG_MAX96712_GMSL2_LINK_ADAPTATION_LINK1:
            link = 1;
            break;
        case ISC_CONFIG_MAX96712_GMSL2_LINK_ADAPTATION_LINK2:
            link = 2;
            break;
        case ISC_CONFIG_MAX96712_GMSL2_LINK_ADAPTATION_LINK3:
            link = 3;
            break;
        default:
            return NVMEDIA_STATUS_BAD_PARAMETER;
    };

    /* Update the register offset per link */
    osnCoff[0] += link;
    osnDisAdap[0] += link;
    osnReset[0] += link;

    /* Disable OSN */
    status = NvMediaISCDeviceRead(handle, 0,
                                  REGISTER_ADDRESS_BYTES, /* regLength */
                                  osnDisAdap,             /* regData */
                                  1,                      /* dataLength */
                                  &osnDisAdap[2]);        /* data */
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    osnDisAdap[2] &= ~(1 << 6);
    status = NvMediaISCDeviceWrite(handle, 0,
                                   3,
                                   osnDisAdap);
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    /* Reseed and set to default value 31 */
    status = NvMediaISCDeviceRead(handle, 0,
                                  REGISTER_ADDRESS_BYTES, /* regLength */
                                  osnReset,               /* regData */
                                  1,                      /* dataLength */
                                  &osnReset[2]);          /* data */
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    nvsleep(10000);

    osnReset[2] |= 1;
    status = NvMediaISCDeviceWrite(handle, 0,
                                   3,
                                   osnReset);
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    for (loop = 0; loop < 100; loop++) {
        /* Read back OSN value */
        status = NvMediaISCDeviceRead(handle, 0,
                                      REGISTER_ADDRESS_BYTES, /* regLength */
                                      osnCoff,                /* regData */
                                      1,                      /* dataLength */
                                      &osnCoff[2]);           /* data */
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }

        osnCoff[2] &= 0x3F;
        if (osnCoff[2] == 31) {
            break;
        }
        nvsleep(1000);
    }

    /* Manual adaptation completed */
    LOG_MSG("MAX96712 Rev %d manual adaptation on the link %d(%d)\n", rev,
                                                                      link,
                                                                      osnCoff[2]);

    return status;
}

static NvMediaStatus
ControlAllForwardChannels(
    NvMediaISCDevice *handle,
    uint32_t link,
    NvMediaBool enable)
{
    NvMediaStatus status;
    uint8_t data[3]  = {0x0B, 0x04, 0x00};
    uint8_t i2cPort;
    _DriverHandle *drvHandle = NULL;

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    i2cPort = drvHandle->ctx.i2cPort;

    data[0] = 0x0B + link;

    status = NvMediaISCDeviceRead(handle, 0,         /* transaction */
                                  REGISTER_ADDRESS_BYTES, /* regLength */
                                  data,                   /* regData */
                                  1,                      /* dataLength */
                                  &data[2]);              /* data */
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    /* Enable / Disable the forward channel */
    if (enable == NVMEDIA_TRUE) {
        data[2] |= (1 << 0);
    } else {
        data[2] &= ~(1 << 0);
    }

    /* WAR Bug 2411206: Sometimes when reading the 0x0B04/0x0C04/0x0D04/0x0E04 registers,
       0x00 is returned, regardless of what was written to the register.  To get around this
       the reverse channel and I2CPort bits must be set before the register is rewritten
    */

    /* Force enable reverse channel */
    data[2] |= (1 << 1);

    /* Force set I2C Port */
    if (i2cPort == ISC_MAX96712_I2CPORT_0) {
        data[2] &= ~(1 << 3);
    } else if (i2cPort == ISC_MAX96712_I2CPORT_1) {
        data[2] |= (1 << 3);
    } else {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    status = NvMediaISCDeviceWrite(handle, 0,
                                   3,
                                   data);
    if (status != NVMEDIA_STATUS_OK) {
       return status;
    }

    nvsleep(10000);

    return status;
}

static NvMediaStatus
DriverCreate(
    NvMediaISCDevice *handle,
    void *clientContext)
{
    _DriverHandle *drvHandle = NULL;

    if (!handle) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    drvHandle = calloc(1, sizeof(_DriverHandle));

    if (!drvHandle) {
        return NVMEDIA_STATUS_OUT_OF_MEMORY;
    }

    if (clientContext) {
        memcpy(&drvHandle->ctx, clientContext, sizeof(ContextMAX96712));
    }

    handle->deviceDriverHandle = (void *)drvHandle;

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
DriverDestroy(
    NvMediaISCDevice *handle)
{
    if (!handle) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }
    if (handle->deviceDriverHandle != NULL) {
        free(handle->deviceDriverHandle);
        handle->deviceDriverHandle = NULL;
    }

    return NVMEDIA_STATUS_OK;
}

NvMediaStatus
MAX96712SetDeviceConfig(
    NvMediaISCDevice *handle,
    uint32_t enumeratedDeviceConfig)
{
    if (!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;

    switch(enumeratedDeviceConfig) {
        case ISC_CONFIG_MAX96712_ENABLE_CSI_OUT:
            return EnableCSI(handle,
                             NVMEDIA_TRUE);
        case ISC_CONFIG_MAX96712_DISABLE_CSI_OUT:
            return EnableCSI(handle,
                             NVMEDIA_FALSE);
        case ISC_CONFIG_MAX96712_SET_I2C_LOCAL_ACK:
            return SetI2CLocalACK(handle);
        case ISC_CONFIG_MAX96712_DISABLE_LINKS_0123:
        case ISC_CONFIG_MAX96712_ENABLE_LINK_0:
        case ISC_CONFIG_MAX96712_ENABLE_LINKS_01:
        case ISC_CONFIG_MAX96712_ENABLE_LINKS_012:
        case ISC_CONFIG_MAX96712_ENABLE_LINKS_0123:
        case ISC_CONFIG_MAX96712_ENABLE_LINK_2:
        case ISC_CONFIG_MAX96712_ENABLE_LINKS_23:
        case ISC_CONFIG_MAX96712_ENABLE_LINKS_02:
            return EnableLinks(handle, enumeratedDeviceConfig);
        case ISC_CONFIG_MAX96712_SET_RESET:
            return SetReset(handle);
        case ISC_CONFIG_MAX96712_SET_VIDEO_PIPELINES_SELECT:
            return VideoPipelineSel(handle);
        case ISC_CONFIG_MAX96712_SET_DEFAULT_GMSL1_LEGACY:
            return SetDefaultGMSL1Legacy(handle);
        case ISC_CONFIG_MAX96712_GMSL2_LINK_ADAPTATION_LINK0:
        case ISC_CONFIG_MAX96712_GMSL2_LINK_ADAPTATION_LINK1:
        case ISC_CONFIG_MAX96712_GMSL2_LINK_ADAPTATION_LINK2:
        case ISC_CONFIG_MAX96712_GMSL2_LINK_ADAPTATION_LINK3:
            return GMSL2LinkAdaptation(handle, enumeratedDeviceConfig);
        case ISC_CONFIG_MAX96712_ENABLE_REPLICATION:
            return EnableReplication(handle, true);
        case ISC_CONFIG_MAX96712_DISABLE_REPLICATION:
            return EnableReplication(handle, false);
        case ISC_CONFIG_MAX96712_SET_PG_1920x1236:
            return SetTPGDefault(handle);
        case ISC_CONFIG_MAX96712_ENABLE_PG:
            return EnableTPG(handle);
        case ISC_CONFIG_MAX96712_DISABLE_PACKET_DETECTOR:
            return DisablePacketDetector(handle);
        case ISC_CONFIG_MAX96712_SET_GMSL1_MODE:
            return SetGMSLMode(handle, 0);
        case ISC_CONFIG_MAX96712_SET_GMSL2_MODE:
            return SetGMSLMode(handle, 1);
        case ISC_CONFIG_MAX96712_ENABLE_REVERSE_CHANNELS_0123:
            return EnableControlChannel(handle,
                                        true);
        case ISC_CONFIG_MAX96712_DISABLE_REVERSE_CHANNELS_0123:
            return EnableControlChannel(handle,
                                        false);
        case ISC_CONFIG_MAX96712_ADJUST_TX_AMP_AND_TIMING:
            return WriteArrayWithCommand(handle,
                                         adjust_tx_amp_and_timing);
        case ISC_CONFIG_MAX96712_CONFIG_MIPI_OUT_MODE:
            return ConfigureMIPIOutput(handle);
        case ISC_CONFIG_MAX96712_ENABLE_MEMORY_ECC_ERRB:
            return EnableMemoryECC(handle, true, true);
        case ISC_CONFIG_MAX96712_DISABLE_MEMORY_ECC_ERRB:
            return EnableMemoryECC(handle, false, false);
        case ISC_CONFIG_MAX96712_SET_TX_SRC_ID:
            return SetTxSRCId(handle);
        case ISC_CONFIG_MAX96712_TRIGGER_DESKEW:
            return TriggerDeskew(handle);
        case ISC_CONFIG_MAX96712_CHECK_CSIPLL_LOCK:
            return CheckCSIPLLLock(handle);
        default:
            break;
    }
    return NVMEDIA_STATUS_ERROR;
}

NvMediaStatus
MAX96712ReadRegister(
    NvMediaISCDevice *handle,
    uint32_t deviceIndex,
    uint32_t registerNum,
    uint32_t dataLength,
    uint8_t *dataBuff)
{
    uint8_t registerData[REGISTER_ADDRESS_BYTES];
    NvMediaStatus status;

    if (!handle || !dataBuff) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    registerData[0] = (registerNum >> 8) & 0xFF;
    registerData[1] = registerNum & 0xFF;

    status = NvMediaISCDeviceRead(handle,            /* transaction */
                         deviceIndex,            /* device index */
                         REGISTER_ADDRESS_BYTES, /* regLength */
                         registerData,           /* regData */
                         dataLength,             /* dataLength */
                         dataBuff);              /* data */
    return status;
}

NvMediaStatus
MAX96712WriteRegister(
    NvMediaISCDevice *handle,
    uint32_t deviceIndex,
    uint32_t registerNum,
    uint32_t dataLength,
    uint8_t *dataBuff)
{
    uint8_t data[REGISTER_ADDRESS_BYTES + REG_WRITE_BUFFER];
    NvMediaStatus status;

    if (!handle || !dataBuff) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    data[0] = registerNum & 0xFF;
    memcpy(&data[1], dataBuff, MIN(REG_WRITE_BUFFER, dataLength));

    status = NvMediaISCDeviceWrite(handle,                         /* transaction */
                          deviceIndex,                         /* device index */
                          dataLength + REGISTER_ADDRESS_BYTES, /* dataLength */
                          data);                               /* data */
    return status;
}

NvMediaStatus
MAX96712ReadParameters(
    NvMediaISCDevice *handle,
    uint32_t parameterType,
    uint32_t parameterSize,
    void *parameter)
{
    ReadParametersParamMAX96712 *param = parameter;
    _DriverHandle *drvHandle = NULL;

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL) ||
        (parameter == NULL)   ) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    switch(parameterType) {
        case ISC_READ_PARAM_CMD_MAX96712_REV_ID:
            if (parameterSize != sizeof(param->rev))
                return NVMEDIA_STATUS_BAD_PARAMETER;
            param->rev = drvHandle->ctx.revision;
            return NVMEDIA_STATUS_OK;
        case ISC_READ_PARAM_CMD_MAX96712_CONTROL_CHANNEL_CRC_ERROR:
            if (parameterSize != sizeof(param->ErrorStatus))
                return NVMEDIA_STATUS_BAD_PARAMETER;
            return ReadStatus(handle, parameterType,
                              param->ErrorStatus.link,
                              &param->ErrorStatus.errVal);
        default:
            break;
    }
    return NVMEDIA_STATUS_ERROR;
}

NvMediaStatus
MAX96712WriteParameters(
    NvMediaISCDevice *handle,
    uint32_t parameterType,
    uint32_t parameterSize,
    void *parameter)
{
    WriteParametersParamMAX96712 *param = parameter;

    if (!handle || !parameter) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    switch(parameterType) {
        case ISC_WRITE_PARAM_CMD_MAX96712_PIPELINE_MAPPING:
            if (parameterSize != sizeof(param->VC))
                return NVMEDIA_STATUS_BAD_PARAMETER;
            return SetMappingVCSetting(handle,
                                       param->VC.embDataType,
                                       param->VC.link,
                                       param->VC.dataType,
                                       1);
        case ISC_WRITE_PARAM_CMD_MAX96712_UNUSED_PIPELINE_MAPPING:
            if (parameterSize != sizeof(param->VC))
                return NVMEDIA_STATUS_BAD_PARAMETER;
            return SetMappingVCSetting(handle,
                                       param->VC.embDataType,
                                       param->VC.link,
                                       param->VC.dataType,
                                       0);
        case ISC_WRITE_PARAM_CMD_MAX96712_SET_I2C_PORT:
            if (parameterSize != sizeof(param->I2CPort))
                return NVMEDIA_STATUS_BAD_PARAMETER;
            return SetI2CPort(handle,
                              param->I2CPort.link);
        case ISC_WRITE_PARAM_CMD_MAX96712_ENABLE_CONCATENATION:
            if (parameterSize != sizeof(param->ConcatenateVideoStream))
                return NVMEDIA_STATUS_BAD_PARAMETER;
            return EnableConcatenation(handle,
                                       param->ConcatenateVideoStream.numSensor);
        case ISC_WRITE_PARAM_CMD_MAX96712_SWAP:
            if (parameterSize != sizeof(param->ConfigLink))
                return NVMEDIA_STATUS_BAD_PARAMETER;
            return SwapBits(handle,
                            param->ConfigLink.link);
        case ISC_WRITE_PARAM_CMD_MAX96712_FSYNC_MODE:
            if (parameterSize != sizeof(param->FRSYNCMode))
                return NVMEDIA_STATUS_BAD_PARAMETER;
            return SetFSYNCMode(handle,
                                param->FRSYNCMode.syncMode,
                                param->FRSYNCMode.pclk,
                                param->FRSYNCMode.fps);
        case ISC_WRITE_PARAM_CMD_MAX96712_SET_DATA_TYPE:
            if (parameterSize != sizeof(param->dataType))
                return NVMEDIA_STATUS_BAD_PARAMETER;
            return OverrideDataType(handle,
                                    param->dataType);
        case ISC_WRITE_PARAM_CMD_MAX96712_SET_CSI_PREBEGIN_TIME:
            if (parameterSize != sizeof(param->csiPrebeginTime))
                return NVMEDIA_STATUS_BAD_PARAMETER;
            return OverrideCSIPrebeginTime(handle,
                                           param->csiPrebeginTime);
        case ISC_WRITE_PARAM_CMD_MAX96712_SET_CSI_POST_LENGTH:
            if (parameterSize != sizeof(param->csiPostLength))
                return NVMEDIA_STATUS_BAD_PARAMETER;
            return OverrideCSIPostLength(handle,
                                         param->csiPostLength);
        case ISC_WRITE_PARAM_CMD_MAX96712_SET_VIDEO_PIPELINE_MAPPING:
            if (parameterSize != sizeof(param->VideoPipelineMapping))
                return NVMEDIA_STATUS_BAD_PARAMETER;
            return SetVideoPipelineMapping(handle,
                                           param->VideoPipelineMapping.pipelineMapping,
                                           param->VideoPipelineMapping.numPipelines);
        case ISC_WRITE_PARAM_CMD_MAX96712_SET_PG_CLK_FREQ:
            if (parameterSize != sizeof(param->pgClkFreq))
                return NVMEDIA_STATUS_BAD_PARAMETER;
            return SetPGClkFreq(handle,
                                param->pgClkFreq);
        case ISC_WRITE_PARAM_CMD_MAX96712_SET_PG_PIPELINE_CLK_SRC:
            if (parameterSize != sizeof(param->pgPipelineClkSrc))
                return NVMEDIA_STATUS_BAD_PARAMETER;
            return SetPGPipelineClkSrc(handle,
                                       param->pgPipelineClkSrc);
        case ISC_WRITE_PARAM_CMD_MAX96712_ENABLE_SPECIFIC_PIPELINES:
            if (parameterSize != sizeof(param->numPipelines)) {
                return NVMEDIA_STATUS_BAD_PARAMETER;
            }
            return EnableSpecificPipelines(handle, param->numPipelines);
        case ISC_WRITE_PARAM_CMD_MAX96712_ENABLE_SPECIFIC_LINK:
            if (parameterSize != sizeof(param->SpecificLink))
                return NVMEDIA_STATUS_BAD_PARAMETER;
            return EnableSpecificLink(handle, param->SpecificLink);
        case ISC_WRITE_PARAM_CMD_MAX96712_ENABLE_PERIODIC_AEQ:
            if (parameterSize != sizeof(param->SpecificLink)) {
                return NVMEDIA_STATUS_BAD_PARAMETER;
            }
            return EnablePeriodicAEQ(handle,
                                     param->SpecificLink);
        case ISC_WRITE_PARAM_CMD_MAX96712_GMSL1_HIM_ENABLED:
            if (parameterSize != sizeof(param->stepHIM)) {
                return NVMEDIA_STATUS_BAD_PARAMETER;
            }
            return SetDefaultGMSL1HIMEnabled(handle, param->stepHIM.link, param->stepHIM.step);
        case ISC_WRITE_PARAM_CMD_MAX96712_ENABLE_FORWARD_CHANNEL:
            if (parameterSize != sizeof(param->SpecificLink)) {
                return NVMEDIA_STATUS_BAD_PARAMETER;
            }
            return ControlAllForwardChannels(handle, param->SpecificLink, NVMEDIA_TRUE);
        case ISC_WRITE_PARAM_CMD_MAX96712_DISABLE_FORWARD_CHANNEL:
            if (parameterSize != sizeof(param->SpecificLink)) {
                return NVMEDIA_STATUS_BAD_PARAMETER;
            }
            return ControlAllForwardChannels(handle, param->SpecificLink, NVMEDIA_FALSE);
        case ISC_WRITE_PARAM_CMD_MAX96712_ENABLE_PACKET_BASED_CONTROL_CHANNEL:
            if (parameterSize != sizeof(param->SpecificLink)) {
                return NVMEDIA_STATUS_BAD_PARAMETER;
            }
            return EnablePacketBasedControlChannel(handle, param->SpecificLink);
        case ISC_WRITE_PARAM_CMD_MAX96712_DISABLE_DE:
            if (parameterSize != sizeof(param->SpecificLink)) {
                return NVMEDIA_STATUS_BAD_PARAMETER;
            }
            return DisableDE(handle, param->SpecificLink);
        case ISC_WRITE_PARAM_CMD_MAX96712_SET_DBL:
            if (parameterSize != sizeof(param->SpecificLink)) {
                return NVMEDIA_STATUS_BAD_PARAMETER;
            }
            return SetDBL(handle, param->SpecificLink);
        default:
            break;
    }
    return NVMEDIA_STATUS_ERROR;
}

static NvMediaStatus
GetErrorStatusGMSL1(
    NvMediaISCDevice *handle,
    uint32_t parameterSize,
    void *parameter,
    uint8_t link,
    uint8_t *regValues)
{
    uint8_t i = 0u;
    ErrorStatusMAX96712 *errorStatus = NULL;
    MAX96712ErrLut ErrorLut[ISC_MAX96712_GMSL1_NUM_ERR_REG] = {
        [ISC_MAX96712_REG_GMSL1_LINK_A] =
            { .link = 0u,
              .mask = MAX96712_GMSL1_LINK_LOCK,
              .result = MAX96712_GMSL1_LINK_LOCK,
              .errType = ISC_MAX96712_GMSL1_LINK_UNLOCK },
        [ISC_MAX96712_REG_GMSL1_LINK_B] =
            { .link = 1u,
              .mask = MAX96712_GMSL1_LINK_LOCK,
              .result = MAX96712_GMSL1_LINK_LOCK,
              .errType = ISC_MAX96712_GMSL1_LINK_UNLOCK },
        [ISC_MAX96712_REG_GMSL1_LINK_C] =
            { .link = 2u,
              .mask = MAX96712_GMSL1_LINK_LOCK,
              .result = MAX96712_GMSL1_LINK_LOCK,
              .errType = ISC_MAX96712_GMSL1_LINK_UNLOCK },
        [ISC_MAX96712_REG_GMSL1_LINK_D] =
            { .link = 3u,
              .mask = MAX96712_GMSL1_LINK_LOCK,
              .result = MAX96712_GMSL1_LINK_LOCK,
              .errType = ISC_MAX96712_GMSL1_LINK_UNLOCK },
        [ISC_MAX96712_REG_GMSL1_DET_ERR_A] =
            { .link = 0u,
              .mask = 0xFF,
              .result = 0u,
              .errType = ISC_MAX96712_GMSL1_DET_ERR},
        [ISC_MAX96712_REG_GMSL1_DET_ERR_B] =
            { .link = 1u,
              .mask = 0xFF,
              .result = 0u,
              .errType = ISC_MAX96712_GMSL1_DET_ERR},
        [ISC_MAX96712_REG_GMSL1_DET_ERR_C] =
            { .link = 2u,
              .mask = 0xFF,
              .result = 0u,
              .errType = ISC_MAX96712_GMSL1_DET_ERR},
        [ISC_MAX96712_REG_GMSL1_DET_ERR_D] =
            { .link = 3u,
              .mask = 0xFF,
              .result = 0u,
              .errType = ISC_MAX96712_GMSL1_DET_ERR},
    };

    if ((handle == NULL) || (parameter == NULL)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if (parameterSize != sizeof(ErrorStatusMAX96712)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    errorStatus = (ErrorStatusMAX96712 *)parameter;

    for (i = 0u; i < ISC_MAX96712_GMSL1_NUM_ERR_REG; i++) {
        if ((ErrorLut[i].link == link) &&
            ((regValues[i] & ErrorLut[i].mask) != ErrorLut[i].result)) {
            errorStatus->link += (1 << link);
            errorStatus->regVal[link] = regValues[i];
            errorStatus->failureType[link] = ErrorLut[i].errType;
            break;
        }
    }

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
GetErrorStatusGMSL2(
    NvMediaISCDevice *handle,
    uint32_t parameterSize,
    void *parameter,
    uint8_t link,
    uint8_t *regValues)
{
    uint8_t i = 0u;
    ErrorStatusMAX96712 *errorStatus = NULL;
    MAX96712ErrLut ErrorLut[ISC_MAX96712_GMSL2_NUM_ERR_REG] = {
        [ISC_MAX96712_REG_GMSL2_LINK_A] =
            { .link = 0u,
              .mask = MAX96712_GMSL2_LINK_LOCK,
              .result = MAX96712_GMSL2_LINK_LOCK,
              .errType = ISC_MAX96712_GMSL2_LINK_UNLOCK },
            { .link = 1u,
              .mask = MAX96712_GMSL2_LINK_LOCK,
              .result = MAX96712_GMSL2_LINK_LOCK,
              .errType = ISC_MAX96712_GMSL2_LINK_UNLOCK },
            { .link = 2u,
              .mask = MAX96712_GMSL2_LINK_LOCK,
              .result = MAX96712_GMSL2_LINK_LOCK,
              .errType = ISC_MAX96712_GMSL2_LINK_UNLOCK },
            { .link = 3u,
              .mask = MAX96712_GMSL2_LINK_LOCK,
              .result = MAX96712_GMSL2_LINK_LOCK,
              .errType = ISC_MAX96712_GMSL2_LINK_UNLOCK },
            { .link = 0u,
              .mask = 0xFF,
              .result = 0u,
              .errType = ISC_MAX96712_GMSL2_DEC_ERR},
            { .link = 1u,
              .mask = 0xFF,
              .result = 0u,
              .errType = ISC_MAX96712_GMSL2_DEC_ERR},
            { .link = 2u,
              .mask = 0xFF,
              .result = 0u,
              .errType = ISC_MAX96712_GMSL2_DEC_ERR},
            { .link = 3u,
              .mask = 0xFF,
              .result = 0u,
              .errType = ISC_MAX96712_GMSL2_DEC_ERR},
            { .link = 0u,
              .mask = 0xFF,
              .result = 0u,
              .errType = ISC_MAX96712_GMSL2_IDLE_ERR},
            { .link = 1u,
              .mask = 0xFF,
              .result = 0u,
              .errType = ISC_MAX96712_GMSL2_IDLE_ERR},
            { .link = 2u,
              .mask = 0xFF,
              .result = 0u,
              .errType = ISC_MAX96712_GMSL2_IDLE_ERR},
            { .link = 3u,
              .mask = 0xFF,
              .result = 0u,
              .errType = ISC_MAX96712_GMSL2_IDLE_ERR},
    };

    if ((handle == NULL) || (parameter == NULL)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if (parameterSize != sizeof(ErrorStatusMAX96712)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    errorStatus = (ErrorStatusMAX96712 *)parameter;

    for (i = 0u; i < ISC_MAX96712_GMSL2_NUM_ERR_REG; i++) {
        if ((ErrorLut[i].link == link) &&
            ((regValues[i] & ErrorLut[i].mask) != ErrorLut[i].result)) {
            errorStatus->link += (1 << link);
            errorStatus->regVal[link] = regValues[i];
            errorStatus->failureType[link] = ErrorLut[i].errType;
            break;
        }
    }

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
ReadStatusRegisters(
    NvMediaISCDevice *handle,
    uint8_t *regValues)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t i = 0u;
    uint16_t regAddr[ISC_MAX96712_GMSL1_NUM_ERR_REG + ISC_MAX96712_GMSL2_NUM_ERR_REG] = {
        /* GMSL1 Error status registers */
        MAX96712_REG_GMSL1_LINK_A,
        MAX96712_REG_GMSL1_LINK_B,
        MAX96712_REG_GMSL1_LINK_C,
        MAX96712_REG_GMSL1_LINK_D,
        MAX96712_REG_GMSL1_DET_ERR_LINK_A,
        MAX96712_REG_GMSL1_DET_ERR_LINK_B,
        MAX96712_REG_GMSL1_DET_ERR_LINK_C,
        MAX96712_REG_GMSL1_DET_ERR_LINK_D,
        /* GMSL2 Error status registers */
        MAX96712_REG_GMSL2_LINK_A,
        MAX96712_REG_GMSL2_LINK_B,
        MAX96712_REG_GMSL2_LINK_C,
        MAX96712_REG_GMSL2_LINK_D,
        MAX96712_REG_GMSL2_DEC_ERR_LINK_A,
        MAX96712_REG_GMSL2_DEC_ERR_LINK_B,
        MAX96712_REG_GMSL2_DEC_ERR_LINK_C,
        MAX96712_REG_GMSL2_DEC_ERR_LINK_D,
        MAX96712_REG_GMSL2_IDLE_ERR_LINK_A,
        MAX96712_REG_GMSL2_IDLE_ERR_LINK_B,
        MAX96712_REG_GMSL2_IDLE_ERR_LINK_C,
        MAX96712_REG_GMSL2_IDLE_ERR_LINK_D,
    };

    if ((handle == NULL) || (regValues == NULL)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    for (i = 0u; i < (ISC_MAX96712_GMSL1_NUM_ERR_REG + ISC_MAX96712_GMSL2_NUM_ERR_REG); i++) {
        status = MAX96712ReadRegister(handle, 0u,
                                      regAddr[i],
                                      1u,
                                      &regValues[i]);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }
    }

    return status;
}

NvMediaStatus
MAX96712GetErrorStatus(
    NvMediaISCDevice *handle,
    uint32_t parameterSize,
    void *parameter)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t linkEnabled = 0u, link = 0u;
    uint8_t gmslMode;
    ErrorStatusMAX96712 *errorStatus = NULL;
    uint8_t regValues[ISC_MAX96712_GMSL1_NUM_ERR_REG + ISC_MAX96712_GMSL2_NUM_ERR_REG] = {0u};
    static NvMediaStatus (*func[ISC_MAX96712_MAX_GMSL_MODE])(NvMediaISCDevice *,
                                                             uint32_t,
                                                             void *,
                                                             uint8_t,
                                                             uint8_t *);
    _DriverHandle *drvHandle = NULL;

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if (parameterSize != sizeof(ErrorStatusMAX96712)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    errorStatus = (ErrorStatusMAX96712 *)parameter;
    memset(errorStatus, 0u, sizeof(ErrorStatusMAX96712));

    /* MAX96712_REG_GMSL1_LINK_A read back as 0 without this delay when any link is powered down */
    nvsleep(5000);

    /* Initialize GetErrorStatus API for each GMSL mode */
    func[ISC_MAX96712_GMSL1_MODE] = GetErrorStatusGMSL1;
    func[ISC_MAX96712_GMSL2_MODE] = GetErrorStatusGMSL2;

    /* Get enblaed links */
    status = MAX96712ReadRegister(handle, 0u,
                                  REG_LINK_ENABLED,
                                  1u,
                                  &linkEnabled);
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }
    linkEnabled &= 0x0F;

    /* Read status registers */
    status = ReadStatusRegisters(handle, regValues);
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    gmslMode = drvHandle->ctx.gmslMode;

    /* Check the error related to GMSL mode */
    for (link = 0u; (link < MAX96712_MAX_NUM_LINK) && ((linkEnabled & (1 << link)) != 0u); link++) {
        status = func[gmslMode](handle,
                                parameterSize,
                                parameter,
                                link,
                                regValues + gmslMode * ISC_MAX96712_GMSL1_NUM_ERR_REG);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }
    }

    return status;
}

NvMediaStatus
MAX96712DumpRegisters(
    NvMediaISCDevice *handle)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t data[4] = {3, 0x00, 0x00, 0};
    uint32_t i = 0;

    if ((handle == NULL)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }


    for (i = 0; i <= MAX96712_MAX_REG_ADDRESS; i++) {
        data[1] = i / 256;
        data[2] = i % 256;

        status = NvMediaISCDeviceRead(handle,0,
                             REGISTER_ADDRESS_BYTES,
                             &data[1],
                             1,
                             &data[3]);
        if (status != NVMEDIA_STATUS_OK)
            return status;

        printf(" Max96712 : 0x%02x%02x - 0x%02x\n", data[1], data[2], data[3]);
    }

    return status;
}

NvMediaStatus
MAX96712CheckPresence(
    NvMediaISCDevice *handle)
{
    NvMediaStatus status;
    uint8_t readBuff[1] = {0u};
    RevisionMAX96712 rev = ISC_MAX96712_INVALID_REV;
    _DriverHandle *drvHandle = NULL;

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    nvsleep(20000); /* Link lock time 20ms, I2C wake time is typical 1.1ms after PWDN released */

    status = MAX96712ReadRegister(handle, 0,
                                  REG_DEV_ID_ADDR,
                                  1,
                                  readBuff);
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    if (readBuff[0] != ((uint32_t)MAX96712_DEV_ID & (uint32_t)0xff)) {
        return NVMEDIA_STATUS_ERROR;
    }

    status = GetRevId(handle,
                      &rev);
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    switch (rev) {
        case ISC_MAX96712_REV_1:
            LOG_MSG("Warning: MAX96712 Rev 1 detected. All features may not be supported."
                    " Please use a platform with MAX96712 Rev 2 or higher for full support.\n");
            LOG_MSG("And the below error can be observed\n"
                    "  - FE_FRAME_ID_FAULT on CSIMUX_FRAME : Frame IDs are mismatched between FS and FE packets\n");
            break;
        case ISC_MAX96712_REV_2:
        case ISC_MAX96712_REV_3:
        case ISC_MAX96712_REV_4:
            break;
        default:
            return NVMEDIA_STATUS_NOT_SUPPORTED;
    }

    drvHandle->ctx.revision = rev;

    return status;
}

static NvMediaISCDeviceDriver deviceDriver = {
    .deviceName = "Maxim 96712 Aggregator",
    .regLength = 2,
    .dataLength = 1,
    .DriverCreate = DriverCreate,
    .DriverDestroy = DriverDestroy,
};

NvMediaISCDeviceDriver *
GetMAX96712Driver(void)
{
    return &deviceDriver;
}
