/*
 * Copyright (c) 2018-2020, 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 <unistd.h>
#include "log_utils.h"
#include "cdi_max96712.h"
#include "cdi_max96712_priv.h"
#include "os_common.h"

/*
 * The following pointers may be used in the functions local to this file but they are checked for
 * NULL in the entry points for CDI functions.
 * DevBlkCDIDevice *handle
 */

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

    while (arrayData[0] != (uint8_t) 'e') {
        switch (arrayData[0]) {
            case 'w':
                status = DevBlkCDIDeviceWrite(handle,
                                              MAX96712_CDI_DEVICE_INDEX,
                                              arrayData[1],
                                              (uint8_t *) &arrayData[2]);
                if (status != NVMEDIA_STATUS_OK) {
                    if (arrayData[1] == 3u) {
                        LOG_ERR("MAX96712: I2C write 0x%02X%02X 0x%02X failed with status %d",
                                arrayData[2],
                                arrayData[3],
                                arrayData[4],
                                status);
                    } else {
                        /* All calls to WriteArray() have block length 3 so this path is not used */
                        LOG_ERR("MAX96712: I2C write failed with status %d", status);
                    }
                }
                arrayData += arrayData[1] + 2u;
                nvsleep(20);
                break;
            case 'd':
                nvsleep((arrayData[1] << 8u) + arrayData[2]);
                arrayData += 3u;
                break;
            default:
                LOG_ERR("MAX96712: Invalid array command");
                status = NVMEDIA_STATUS_BAD_PARAMETER;
                break;
        }

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

    return status;
}

static NvMediaStatus
WriteArray(
     DevBlkCDIDevice *handle,
     uint32_t arrayByteLength,
     const uint8_t *arrayData)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    const uint8_t *data = NULL;

    while (arrayByteLength) {
        data = GET_BLOCK_DATA(arrayData);
        status = DevBlkCDIDeviceWrite(handle,
                                      MAX96712_CDI_DEVICE_INDEX,
                                      GET_BLOCK_LENGTH(arrayData),
                                      data);
        if (status != NVMEDIA_STATUS_OK) {
            if (GET_BLOCK_LENGTH(arrayData) == 3u) {
                LOG_ERR("MAX96712: I2C write 0x%02X%02X 0x%02X failed with status %d",
                        data[0],
                        data[1],
                        data[2],
                        status);
            } else {
                /* All calls to WriteArray() have block length 3 so this path is not used */
                LOG_ERR("MAX96712: I2C write failed with status %d", status);
            }
            break;
        }

        arrayByteLength -= GET_BLOCK_LENGTH(arrayData) + 1u;
        SET_NEXT_BLOCK(arrayData);
        nvsleep(20);
    }

    return status;
}

static NvMediaStatus
AddToRegFieldQ(
    DevBlkCDIDevice *handle,
    RegBitField name,
    uint8_t val)
{
    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    uint8_t index = drvHandle->regBitFieldQ.numRegBitFieldArgs;

    if (index == MAX96712_REG_MAX_FIELDS_PER_REG) {
        LOG_ERR("MAX96712: RegFieldQ full. Failed to add %u", (uint32_t)name);
        return NVMEDIA_STATUS_ERROR;
    }

    LOG_DBG("MAX96712: Adding regField = %u, val = %u to index %u in RegFieldQ",
            name,
            val,
            index);

    drvHandle->regBitFieldQ.name[index] = name;
    drvHandle->regBitFieldQ.val[index] = val;
    drvHandle->regBitFieldQ.numRegBitFieldArgs = index + 1u;
    return NVMEDIA_STATUS_OK;
}

static void
ClearRegFieldQ(
    DevBlkCDIDevice *handle)
{
    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;

    LOG_DBG("MAX96712: Clearing RegFieldQ");
    drvHandle->regBitFieldQ.numRegBitFieldArgs = 0u;
}

static uint8_t
ReadFromRegFieldQ(
    DevBlkCDIDevice *handle,
    uint8_t index)
{
    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    uint8_t val = 0u;

    if (index >= drvHandle->regBitFieldQ.numRegBitFieldArgs) {
        LOG_ERR("MAX96712: Bad parameter. Invalid index %u", index);
        return 0u;
    }

    val = drvHandle->regBitFieldQ.val[index];

    LOG_DBG("MAX96712: Read index %u from RegFieldQ. Val = %u", index, val);
    return val;
}

/* Access register fields belonging to a same register.
 * REG_READ_MODE: Register is read and specified field vals are unpacked into regBitFieldArg array.
 * REG_WRITE_MODE: Specified field vals from regBitFieldArg array are packed and written to register.
 * REG_READ_MOD_WRITE_MODE: Register is read, specified field vals in regBitFieldArg are modified
 *                          and written to register */
static NvMediaStatus
sAccessRegFieldQ(
    DevBlkCDIDevice *handle,
    RegBitFieldAccessMode mode)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    RegBitFieldQ *regBitFieldQ = &(drvHandle->regBitFieldQ);
    uint8_t numFields = regBitFieldQ->numRegBitFieldArgs;
    uint16_t regAddr = regBitFieldProps[regBitFieldQ->name[0]].regAddr;
    const RegBitFieldProp *regBitFieldProp = NULL;
    uint8_t fieldMask = 0u;
    uint8_t regData[3] = {};
    uint8_t i = 0u;
    uint8_t loop = 0u;

    if (numFields == 0u) {
        LOG_DBG("MAX96712: Skipping sAccessRegFieldQ");
        return status;
    }

    /* TODO: Use this check in debug mode only */
//#ifdef DEBUG
    /* Check if all the supplied fields belongs to same register addr.
     * Check if msbPos and lsbPos are valid. */
    for (i = 0u; i < numFields; i++) {
        regBitFieldProp = &regBitFieldProps[regBitFieldQ->name[i]];
        if ((regAddr != regBitFieldProp->regAddr) ||
            (regBitFieldProp->lsbPos > regBitFieldProp->msbPos)) {
            LOG_ERR("MAX96712: Bad parameter");
            return NVMEDIA_STATUS_BAD_PARAMETER;
        }
    }
//#endif

    regData[0] = (regAddr >> 8u) & 0xFF;
    regData[1] = regAddr & 0xFF;

    if ((mode == REG_READ_MODE) || (mode == REG_READ_MOD_WRITE_MODE)) {
        for (loop = 0u; loop < 10u; loop++) {
            status = DevBlkCDIDeviceRead(handle,
                                         MAX96712_CDI_DEVICE_INDEX,
                                         MAX96712_NUM_ADDR_BYTES,
                                         &regData[0],
                                         1u,
                                         &regData[2]);
            if (status == NVMEDIA_STATUS_OK) {
                break;
            }
            nvsleep(10);
        }

        if (status != NVMEDIA_STATUS_OK) {
            LOG_ERR("MAX96712: Register 0x%04X I2C read failed with status %d", regAddr, status);
            return status;
        }

        nvsleep(20);
    }

    for (i = 0u; i < numFields; i++) {
        regBitFieldProp = &regBitFieldProps[regBitFieldQ->name[i]];
        fieldMask = (1u << (regBitFieldProp->msbPos + 1u)) - (1u << regBitFieldProp->lsbPos);
        if (mode == REG_READ_MODE) {
            /* Unpack fieldVals */
            regBitFieldQ->val[i] = ((regData[2] & fieldMask) >> (regBitFieldProp->lsbPos));
        } else {
            /* Pack fieldVals for write*/
            regData[2] &= ~fieldMask;
            regData[2] |= ((regBitFieldQ->val[i] << regBitFieldProp->lsbPos) & fieldMask);
        }
    }

    if (mode != REG_READ_MODE) {
        status = DevBlkCDIDeviceWrite(handle,
                                      MAX96712_CDI_DEVICE_INDEX,
                                      3u,
                                      regData);
        if (status != NVMEDIA_STATUS_OK) {
            LOG_ERR("MAX96712: Register 0x%04X I2C write failed with status %d", regAddr, status);
            return status;
        }
        nvsleep(20);
    }

    return status;
}

NvMediaStatus
MAX96712OneShotReset(
    DevBlkCDIDevice *handle,
    LinkMAX96712 link)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;

    ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_RESET_ONESHOT, (uint8_t)link, REG_WRITE_MODE);

    nvsleep(100000);

    return status;
}

static NvMediaStatus
EnableSpecificLinks(
    DevBlkCDIDevice *handle,
    LinkMAX96712 link)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t i = 0u;

    ClearRegFieldQ(handle);
    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_LINK_EN_A + i,
                                  MAX96712_IS_GMSL_LINK_SET(link, i) ? 1u : 0u);
    }
    ACCESS_REG_FIELD_RET_ERR(REG_READ_MOD_WRITE_MODE);

    nvsleep(25000);

    return status;
}

static NvMediaStatus
SetLinkMode(
    DevBlkCDIDevice *handle,
    LinkMAX96712 link)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    uint8_t i = 0u;
    bool triggerOneShotReset = false;

    /* Set GMSL mode */
    ClearRegFieldQ(handle);
    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
            if ((drvHandle->ctx.gmslModeUpdate & (1 << i)) == 0) {
                ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_LINK_GMSL2_A + i,
                                         (drvHandle->ctx.gmslMode[i] == CDI_MAX96712_GMSL1_MODE) ? 0u : 1u);
                drvHandle->ctx.gmslModeUpdate |= 1 << i;
                triggerOneShotReset = true;
            }
        }
    }
    if (triggerOneShotReset) {
        ACCESS_REG_FIELD_RET_ERR(REG_READ_MOD_WRITE_MODE);
    }

    /* Set Link speed */
    ClearRegFieldQ(handle);
    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
            if ((drvHandle->ctx.gmslModeUpdate & ((1 << i) << 4)) == 0) {
                ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_RX_RATE_PHY_A + i,
                                             (drvHandle->ctx.gmslMode[i] == CDI_MAX96712_GMSL1_MODE) ? 1u : 2u,
                                             REG_READ_MOD_WRITE_MODE);
                drvHandle->ctx.gmslModeUpdate |= (1 << i) << 4;
                triggerOneShotReset = true;
            }
        }
    }

    if (triggerOneShotReset) {
        status = MAX96712OneShotReset(handle, link);
    }

    return status;
}

static NvMediaStatus
EnablePeriodicAEQ(
    DevBlkCDIDevice *handle,
    LinkMAX96712 link)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t i = 0u;

    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
            ClearRegFieldQ(handle);
            ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_AEQ_PHY_A + i,
                                      1u);
            ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_PERIODIC_AEQ_PHY_A + i,
                                      1u);
            ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_EOM_PER_THR_PHY_A + i,
                                      0x10);
            ACCESS_REG_FIELD_RET_ERR(REG_WRITE_MODE);

            LOG_MSG("MAX96712: Enable periodic AEQ on Link %d\n", i);
            nvsleep(10000);
        }
    }

    return status;
}

static NvMediaStatus
SetDefaultGMSL1HIMEnabled(
    DevBlkCDIDevice *handle,
    LinkMAX96712 link,
    uint8_t step)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t max96712_defaults_HIM_step0[30] = {
        /* GMSL1 - Turn on HIM */
        'w', 3, 0x0B, 0x06, 0xEF,
        /* GMSL1 - Enable reverse channel cfg and turn on local I2C ack */
        'w', 3, 0x0B, 0x0D, 0x81,
        'e'
    };
    uint8_t max96712_defaults_HIM_step1[30] = {
        /* GMSL1 - Turn off HIM */
        'w', 3, 0x0B, 0x06, 0x6F,
        /* 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_step2[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;
    uint8_t i = 0u;

    if (step > 2u) {
        LOG_ERR("MAX96712: Bad parameter. Step must be either 0, 1 or 2.");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
            /* Update register offset */
            if (step == 0) {
                max96712_defaults_HIM_step0[2] = 0xB + i;
                max96712_defaults_HIM_step0[7] = 0xB + i;
            } else if (step == 1) {
                max96712_defaults_HIM_step1[2] = 0xB + i;
                max96712_defaults_HIM_step1[7] = 0x14 + i;
                max96712_defaults_HIM_step1[12] = 0x14 + i;
                max96712_defaults_HIM_step1[17] = 0x14 + i;
            } else {
                max96712_defaults_HIM_step2[2] = 0xB + i;
                max96712_defaults_HIM_step2[7] = 0x14 + i;
                max96712_defaults_HIM_step2[12] = 0x14 + i;
                max96712_defaults_HIM_step2[17] = 0x14 + i;
            }

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

            status = WriteArrayWithCommand(handle,
                                           stepHIM);
            if (status != NVMEDIA_STATUS_OK) {
                return status;
            }
        }
    }

    return status;
}

static NvMediaStatus
EnablePacketBasedControlChannel(
    DevBlkCDIDevice *handle,
    LinkMAX96712 link,
    bool enable)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t data[4] = {3, 0x0B, 0x08, 0x25};
    uint8_t i = 0u;

    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
            data[1] = 0x0B + i;

            if (!enable) {
                data[3] = 0x21;
            }

            status = WriteArray(handle,
                                GET_SIZE(data),
                                data);
            if (status != NVMEDIA_STATUS_OK) {
                return status;
            }
            nvsleep(10000);
        }
    }

    return status;
}

static NvMediaStatus
EnableDoublePixelMode(
    DevBlkCDIDevice *handle,
    LinkMAX96712 link,
    DataTypeMAX96712 dataType,
    const bool embDataType)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = NULL;
    TxPortMAX96712 txPort;
    uint8_t disPktDetector[] = {3, 0x01, 0x00, 0x23};
    uint8_t altModeArr[] = {
        3, 0x09, 0x33, 0x01, /* ALT_MEM_MAP12 = 1 on Ctrl 0 */
    };
    uint8_t i = 0u;

    drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    txPort = drvHandle->ctx.txPort;

    ClearRegFieldQ(handle);
    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
            ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_BPP8DBL_5 + i,
                                      1u);
        }
    }
    ACCESS_REG_FIELD_RET_ERR(REG_READ_MOD_WRITE_MODE);

    ClearRegFieldQ(handle);
    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
            ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_BPP8DBL_MODE_5 + i,
                                      1u);
        }
    }
    ACCESS_REG_FIELD_RET_ERR(REG_READ_MOD_WRITE_MODE);

    altModeArr[2] = 0x33 + (txPort - CDI_MAX96712_TXPORT_PHY_C) * 0x40;


    if (dataType == CDI_MAX96712_DATA_TYPE_RAW12) {
        altModeArr[3] = 0x1;
    } else if (dataType == CDI_MAX96712_DATA_TYPE_RAW10) {
        altModeArr[3] = 0x4;
    }

    if (embDataType == true) {
        altModeArr[3] |= 0x2;
    }

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

    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
            disPktDetector[2] = 0x12 * i;
            status = WriteArray(handle,
                                GET_SIZE(disPktDetector),
                                disPktDetector);
            if (status != NVMEDIA_STATUS_OK) {
                return status;
            }

            disPktDetector[2] = 0x12 * (i + 4u) + ((i != 0u) ? 0x6 : 0u);
            status = WriteArray(handle,
                                GET_SIZE(disPktDetector),
                                disPktDetector);
            if (status != NVMEDIA_STATUS_OK) {
                return status;
            }
        }
    }

    return status;
}

/*
 * 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
 */
static NvMediaStatus
ConfigTxAmpTiming(
    DevBlkCDIDevice *handle,
    LinkMAX96712 link)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    GMSLModeMAX96712 gmslMode = CDI_MAX96712_GMSL_MODE_INVALID;
    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    RevisionMAX96712 rev = drvHandle->ctx.revision;
    uint8_t adjTxAmpAndTimingArr[] = {
        'w', 3, 0x14, 0x58, 0x28, /* vth1 : Error channel power-down */
        'd',    0x27, 0x01,       /* Delay 1ms */
        'w', 3, 0x14, 0x59, 0x68, /* vth0 : + 104 * 4.7mV = 488.8 mV */
        'd',    0x27, 0x01,       /* Delay 1ms */
        'w', 3, 0x14, 0x3E, 0xB3, /* Error channel phase secondary timing adjustment */
        'd',    0x27, 0x01,       /* Delay 1ms */
        'w', 3, 0x14, 0x3F, 0x72, /* Error channel phase primary timing adjustment */
        'd',    0x27, 0x01,       /* Delay 1ms */
        'w', 3, 0x14, 0x95, 0xD2, /* Reverse channel Tx amplitude to 180 mV */
        'd',    0x27, 0x01,       /* Delay 1ms */
        'e'
    };
    uint8_t i = 0u;

    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
            gmslMode = drvHandle->ctx.gmslMode[i];

            if (gmslMode != CDI_MAX96712_GMSL2_MODE) {
                LOG_INFO("MAX96712: Link %d: Tx amplitude is not required in GMSL2 mode\n", i);
                continue;
            }

            adjTxAmpAndTimingArr[2] = 0x14 + i;
            adjTxAmpAndTimingArr[10] = 0x14 + i;
            adjTxAmpAndTimingArr[18] = 0x14 + i;
            adjTxAmpAndTimingArr[26] = 0x14 + i;
            adjTxAmpAndTimingArr[34] = 0x14 + i;
            status = WriteArrayWithCommand(handle,
                                           adjTxAmpAndTimingArr);
            if (status != NVMEDIA_STATUS_OK) {
                LOG_INFO("MAX96712: Link %d: Failed to updte Tx amplitude\n", i);
                return status;
            }

            LOG_MSG("MAX96712 Rev %d: Link %d: ", rev, i);
            LOG_MSG("Tx amplitude 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x\n", adjTxAmpAndTimingArr[4],
                                                                             adjTxAmpAndTimingArr[12],
                                                                             adjTxAmpAndTimingArr[20],
                                                                             adjTxAmpAndTimingArr[28],
                                                                             adjTxAmpAndTimingArr[36]);
        }
    }

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
UpdateVGAHighGain(
    DevBlkCDIDevice *handle,
    LinkMAX96712 link,
    bool enable)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    GMSLModeMAX96712 gmslMode = CDI_MAX96712_GMSL_MODE_INVALID;
    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    uint8_t VGAHighGain[] = {
        'w', 3, 0x14, 0x18, 0x03,
        'e'
    };
    uint8_t i = 0u;

    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
            gmslMode = drvHandle->ctx.gmslMode[i];

            if (gmslMode != CDI_MAX96712_GMSL2_MODE) {
                LOG_INFO("MAX96712: Link %d: VGAHighGain is valid in ONLY GMSL2 mode\n", i);
                continue;
            }
            VGAHighGain[2] = 0x14 + i;
            VGAHighGain[4] = (enable) ? 0x07 : 0x03;

            status = WriteArrayWithCommand(handle,
                                           VGAHighGain);
            if (status != NVMEDIA_STATUS_OK) {
                LOG_INFO("MAX96712: Link %d: Failed to set VGAHighGain\n", i);
                return status;
            }
        }
    }

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
OverrideDataType(
    DevBlkCDIDevice *handle,
    LinkMAX96712 link,
    LinkPipelineMapMAX96712 *linkPipelineMap)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t bpp = 0u;
    uint8_t dataFormat = 0u;
    uint8_t i = 0u;

    /* Override is enabled only for pipes 0-3 */
    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i) &&
            linkPipelineMap[i].isDTOverride) {
            switch (linkPipelineMap[i].dataType) {
                case CDI_MAX96712_DATA_TYPE_RAW10:
                    bpp = 0xA;         /* 10 bits per pixel */
                    dataFormat = 0x2B; /* raw10 */
                    break;
                case CDI_MAX96712_DATA_TYPE_RAW12:
                    bpp = 0xC;         /* 12 bits per pixel */
                    dataFormat = 0x2C; /* raw12 */
                    break;
                case CDI_MAX96712_DATA_TYPE_RAW16:
                    bpp = 0x10;        /* 16 bits per pixel */
                    dataFormat = 0x2E; /* raw16 */
                    break;
                case CDI_MAX96712_DATA_TYPE_RGB:
                    bpp = 0x18;        /* 24 bits per pixel */
                    dataFormat = 0x24; /* RGB */
                    break;
                default:
                    LOG_ERR("MAX96712: Bad parameter: Invalid data type");
                    return NVMEDIA_STATUS_BAD_PARAMETER;
            }

            ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_SOFT_BPP_0 + i,
                                         bpp,
                                         REG_READ_MOD_WRITE_MODE);

            ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_SOFT_DT_0 + i,
                                         dataFormat,
                                         REG_READ_MOD_WRITE_MODE);


            if (i == 1u) {
                ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_SOFT_DT_1_H,
                                             (dataFormat >> 4u),
                                             REG_READ_MOD_WRITE_MODE);
            } else if (i == 2u) {
                ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_SOFT_BPP_2_H,
                                             (bpp >> 2u),
                                             REG_READ_MOD_WRITE_MODE);

                ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_SOFT_DT_2_H,
                                             (dataFormat >> 2u),
                                             REG_READ_MOD_WRITE_MODE);
            }

            ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_SOFT_OVR_0_EN + i,
                                         1u,
                                         REG_READ_MOD_WRITE_MODE);
        }
    }

    return status;
}

static NvMediaStatus
VideoPipelineSel(
    DevBlkCDIDevice *handle,
    LinkMAX96712 link,
    LinkPipelineMapMAX96712 *linkPipelineMap)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    uint8_t i = 0u;

    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
            if (drvHandle->ctx.gmslMode[i] == CDI_MAX96712_GMSL2_MODE) {
                ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_VIDEO_PIPE_SEL_0 + i,
                                             4u * i,
                                             REG_READ_MOD_WRITE_MODE);

                if (linkPipelineMap[i].isEmbDataType) {
                    ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_VIDEO_PIPE_SEL_4 + i,
                                                 (4u * i) + 1u,
                                                 REG_READ_MOD_WRITE_MODE);

                }
            }
        }
    }

    /* Enable Pipelines*/
    ClearRegFieldQ(handle);
    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
            ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_VIDEO_PIPE_EN_0 + i,
                                      1u);

            if (linkPipelineMap[i].isEmbDataType) {
                ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_VIDEO_PIPE_EN_4 + i,
                                          1u);
            }
        }
    }
    ACCESS_REG_FIELD_RET_ERR(REG_READ_MOD_WRITE_MODE);

    return status;
}

static NvMediaStatus
SetPipelineMap(
    DevBlkCDIDevice *handle,
    LinkMAX96712 link,
    LinkPipelineMapMAX96712 *linkPipelineMap)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    TxPortMAX96712 txPort = drvHandle->ctx.txPort;
    /* 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,
        3, 0x09, 0x10, 0x00,
        3, 0x09, 0x11, 0x01,
        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,
    };
    bool isEmbDataType = false;
    uint8_t vcID = 0u;
    uint8_t dataTypeVal = 0u;
    uint8_t i = 0u;
    uint8_t j = 0u;

    if (linkPipelineMap == NULL) {
        LOG_ERR("MAX96712: Bad parameter: Null ptr");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
            isEmbDataType = linkPipelineMap[i].isEmbDataType;
            vcID = linkPipelineMap[i].vcID;

            /* Update Tx Port */
            if (linkPipelineMap[i].isMapToUnusedCtrl) {
                if (drvHandle->ctx.mipiOutMode == CDI_MAX96712_MIPI_OUT_4x2) {
                    mappingRaw[7] = 0x15; /* controller 1 */
                    mappingEmb[7] = 0x15;
                } else {
                    /* 2x4 mode*/
                    mappingRaw[7] = 0x00; /* controller 0 */
                    mappingEmb[7] = 0x00;
                }
            } else {
                mappingRaw[7] = (txPort << 4u) | (txPort << 2u) | txPort;
                mappingEmb[7] = (txPort << 4u) | (txPort << 2u) | txPort;
            }

            if (isEmbDataType && drvHandle->ctx.gmslMode[i] != CDI_MAX96712_GMSL2_MODE) {
                LOG_ERR("MAX96712: Emb data type is valid for GMSL2 only");
                return NVMEDIA_STATUS_ERROR;
            }

            if (isEmbDataType && linkPipelineMap[i].isDTOverride) {
                LOG_ERR("MAX96712: Emb data type is not supported with dt override enabled");
                return NVMEDIA_STATUS_ERROR;
            }

            switch (linkPipelineMap[i].dataType) {
                case CDI_MAX96712_DATA_TYPE_RAW10:
                    dataTypeVal = 0x2B;
                    break;
                case CDI_MAX96712_DATA_TYPE_RAW12:
                    dataTypeVal = 0x2C;
                    break;
                case CDI_MAX96712_DATA_TYPE_RAW16:
                    dataTypeVal = 0x2E;
                    break;
                case CDI_MAX96712_DATA_TYPE_RGB:
                    dataTypeVal = 0x24;
                    break;
                default:
                    LOG_ERR("MAX96712: Bad parameter: Invalid data type");
                    return NVMEDIA_STATUS_BAD_PARAMETER;
            }

            /* update offset */
            /* 4 mapping for data and 4 mapping for emb */
            mappingRaw[11] = dataTypeVal;
            mappingRaw[15] = (vcID << 6u) | dataTypeVal;
            /* Change FS packet's DT to reserved for RAW pipeline if emb data is used */
            mappingRaw[23] = (vcID << 6u) | (isEmbDataType ? 2u : 0u);
            /* Change FE packet's DT to reserved for RAW pipeline if emb data is used */
            mappingRaw[31] = (vcID << 6u) | (isEmbDataType ? 3u : 1u);
            mappingEmb[15] = (vcID << 6u) | 0x12;
            mappingEmb[23] = (vcID << 6u) | 0x0;
            mappingEmb[31] = (vcID << 6u) | 0x1;

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

            if (isEmbDataType) {
                status = WriteArray(handle,
                                    GET_SIZE(mappingEmb),
                                    mappingEmb);
                if (status != NVMEDIA_STATUS_OK) {
                    return status;
                }
            }
        }
        for (j = 0u; j < 8u; j++) {
            mappingRaw[2u + 4u * j] += 0x40;
            mappingEmb[2u + 4u * j] += 0x40;
        }
    }

    return VideoPipelineSel(handle,
                            link,
                            linkPipelineMap);
}

static NvMediaStatus
ConfigPGSettings(
    DevBlkCDIDevice *handle)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    static const uint8_t configPGArrCmd[] = {
        /* 1920x1236, 30FPS */
        'w', 3, 0x10, 0x51, 0x20,
        'd',    0x03, 0xE8,       /* Delay 1ms. FIx for Getframe timeouts on QNX. Bug 2189783*/
        'w', 3, 0x10, 0x52, 0x00, /* VS_DLY  */
        'w', 3, 0x10, 0x53, 0x00,
        'w', 3, 0x10, 0x54, 0x00,
        'w', 3, 0x10, 0x55, 0x00, /* VS_HIGH */
        'w', 3, 0x10, 0x56, 0x3D,
        'w', 3, 0x10, 0x57, 0x68,
        'w', 3, 0x10, 0x58, 0x4B, /* VS_LOW */
        'w', 3, 0x10, 0x59, 0x9B,
        'w', 3, 0x10, 0x5A, 0x40,
        'w', 3, 0x10, 0x5B, 0x09, /* V2H */
        'w', 3, 0x10, 0x5C, 0xD3,
        'w', 3, 0x10, 0x5D, 0x50,
        'w', 3, 0x10, 0x5E, 0x0C, /* HS_HIGH */
        'w', 3, 0x10, 0x5F, 0x1C,
        'w', 3, 0x10, 0x60, 0x00, /* HS_LOW */
        'w', 3, 0x10, 0x61, 0x2C,
        'w', 3, 0x10, 0x62, 0x06, /* HS_CNT */
        'w', 3, 0x10, 0x63, 0x2D,
        'w', 3, 0x10, 0x64, 0x09, /* V2D */
        'w', 3, 0x10, 0x65, 0xD5,
        'w', 3, 0x10, 0x66, 0xA8,
        'w', 3, 0x10, 0x67, 0x07, /* DE_HIGH */
        'w', 3, 0x10, 0x68, 0x80,
        'w', 3, 0x10, 0x69, 0x04, /* DE_LOW */
        'w', 3, 0x10, 0x6A, 0xD8,
        'w', 3, 0x10, 0x6B, 0x04, /* DE_CNT */
        'w', 3, 0x10, 0x6C, 0xD4,
        'w', 3, 0x10, 0x6D, 0x03,
        'w', 3, 0x10, 0x6E, 0x80,
        'w', 3, 0x10, 0x6F, 0x00,
        'w', 3, 0x10, 0x70, 0x04,
        'w', 3, 0x10, 0x71, 0x00,
        'w', 3, 0x10, 0x72, 0x08,
        'w', 3, 0x10, 0x73, 0x80,
        'w', 3, 0x10, 0x74, 0x50,
        'w', 3, 0x10, 0x75, 0xA0,
        'w', 3, 0x10, 0x76, 0x50,
        'w', 3, 0x01, 0xDD, 0x1B,
        'w', 3, 0x01, 0xFD, 0x1B,
        'w', 3, 0x02, 0x1D, 0x1B,
        'w', 3, 0x02, 0x3D, 0x1B,
        'w', 3, 0x08, 0xA2, 0xF0,
        'w', 3, 0x00, 0x06, 0x00, /* disable all links */
        'w', 3, 0x00, 0x18, 0x0F,

        'w', 3, 0x00, 0x09, 0x02, /* Set pattern generator clock frequency to 150 MHz */

        /* Set pattern generator clock source for each pipeline to 150 MHz */
        'w', 3, 0x01, 0xDC, 0x00,
        'w', 3, 0x01, 0xFC, 0x00,
        'w', 3, 0x02, 0x1C, 0x00,
        'w', 3, 0x02, 0x3C, 0x00,
        'w', 3, 0x02, 0x5C, 0x00,
        'w', 3, 0x02, 0x7C, 0x00,
        'w', 3, 0x02, 0x9C, 0x00,
        'w', 3, 0x02, 0xBC, 0x00,
        'e'
    };

    status = WriteArrayWithCommand(handle,
                                   configPGArrCmd);
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_FORCE_CSI_OUT_EN,
                                 1u,
                                 REG_READ_MOD_WRITE_MODE);

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
EnablePG(
    DevBlkCDIDevice *handle)
{
    static const uint8_t enablePGArrCmd[] = {
        'w', 3, 0x10, 0x50, 0xF3,
        'e'
    };
    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    drvHandle->ctx.tpgEnabled = true;

    return WriteArrayWithCommand(handle,
                                 enablePGArrCmd);
}

static NvMediaStatus
SetTxSRCId(
    DevBlkCDIDevice *handle,
    LinkMAX96712 link)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t data[] = {
        3, 0x05, 0x03, 0x00,
    };
    uint8_t i = 0u;

    if (MAX96712_IS_MULTIPLE_GMSL_LINK_SET(link)) {
        LOG_ERR("MAX96712: Bad param: Multiple links specified");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
            data[2] += (i << 4); /* update the address */
            data[3] = i; /* 0 - link 0, 1 - link 1, so on */

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

    return status;
}

static NvMediaStatus
DisableAutoAck(
    DevBlkCDIDevice *handle,
    LinkMAX96712 link)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t data[] = {
        3, 0x0B, 0x0D, 0x00,
    };
    uint8_t i = 0u;

    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
            data[1] = 0xB + i; /* update the address */

            status = WriteArray(handle,
                                GET_SIZE(data),
                                data);
            nvsleep(25000);
        }
    }

    return status;
}

static NvMediaStatus
EnableBackTop(
    DevBlkCDIDevice *handle,
    bool enable)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;

    ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_BACKTOP_EN,
                                 (enable ? 1u : 0u),
                                 REG_READ_MOD_WRITE_MODE);

    return status;
}

static NvMediaStatus
TriggerDeskew(
    DevBlkCDIDevice *handle)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t i = 0u;
    uint8_t 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 = 0u; i < 2u; i++) {
        for (phy_num = 0u; phy_num < MAX96712_MAX_NUM_PHY; phy_num++) {
            data[2] = 0x40 * phy_num + 0x03; /* Update the register offset */
            status = DevBlkCDIDeviceRead(handle,
                                         MAX96712_CDI_DEVICE_INDEX,
                                         MAX96712_NUM_ADDR_BYTES,
                                         &data[1],
                                         1u,
                                         &data[3]);
            if (status != NVMEDIA_STATUS_OK) {
                return status;
            }

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

    return status;
}

static NvMediaStatus
SetI2CPort(
    DevBlkCDIDevice *handle)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    I2CPortMAX96712 i2cPort = drvHandle->ctx.i2cPort;
    uint8_t i = 0u;

    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (drvHandle->ctx.gmslMode[i] == CDI_MAX96712_GMSL1_MODE) {
            ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_I2C_PORT_GMSL1_PHY_A + i,
                                         (i2cPort == CDI_MAX96712_I2CPORT_0) ? 0u : 1u,
                                         REG_READ_MOD_WRITE_MODE);
        } else if (drvHandle->ctx.gmslMode[i] == CDI_MAX96712_GMSL2_MODE) {
            /* Disable connection from both port 0/1 */
            ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_DIS_REM_CC_A + i,
                                         0x3u,
                                         REG_READ_MOD_WRITE_MODE);

            /* Select port 0 or 1 over the link */
            ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_SEC_XOVER_SEL_PHY_A + i,
                                         (i2cPort == CDI_MAX96712_I2CPORT_0) ? 0u : 1u,
                                         REG_READ_MOD_WRITE_MODE);

            /* Enable connection from port 0 or 1 */
            ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_DIS_REM_CC_A + i,
                                         (i2cPort == CDI_MAX96712_I2CPORT_0) ? 2u : 1u,
                                         REG_READ_MOD_WRITE_MODE);
        }
    }

    return status;
}

static NvMediaStatus
SetFSYNCMode(
    DevBlkCDIDevice *handle,
    FSyncModeMAX96712 FSyncMode,
    uint32_t pclk,
    uint32_t fps,
    LinkMAX96712 link)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    GMSLModeMAX96712 gmslMode = CDI_MAX96712_GMSL_MODE_INVALID;
    uint8_t fsyncPeriod[] = {
        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 */
    };
    uint8_t setManualFsyncMode[] = {
        3, 0x04, 0xA2, 0xE0, /* video link for fsync generation */
        3, 0x04, 0xAA, 0x00, /* Disable overlap window */
        3, 0x04, 0xAB, 0x00, /* Disable overlap window */
        3, 0x04, 0xA8, 0x00, /* Disable error threshold */
        3, 0x04, 0xA9, 0x00, /* Disable error threshold */
        3, 0x04, 0xAF, 0x1F, /* Set FSYNC to GMSL1 type */
        3, 0x04, 0xA0, 0x10, /* Set FSYNC to manual mode */
    };
    uint8_t setAutoFsyncMode[] = {
        3, 0x04, 0xA2, 0xE1, /* video link for fsync generation */
        3, 0x04, 0xAA, 0x00, /* Disable overlap window */
        3, 0x04, 0xAB, 0x00, /* Disable overlap window */
        3, 0x04, 0xA8, 0x00, /* Disable error threshold */
        3, 0x04, 0xA9, 0x00, /* Disable error threshold */
        3, 0x04, 0xB1, 0x78, /* GPIO ID setup to output FSYNC. GPIO2 by default */
        3, 0x04, 0xA0, 0x12, /* Set FSYNC to auto mode */
    };
    uint8_t setOSCManualFsyncMode[] = {
        'w', 3, 0x04, 0xAF, 0x40, /* Set FSYNC to GMSL1 type */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x04, 0xA0, 0x04, /* Set FSYNC to manual mode */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x04, 0xA2, 0x00, /* Turn off auto master link selection */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x04, 0xAA, 0x00, /* Disable overlap window */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x04, 0xAB, 0x00, /* Disable overlap window */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x04, 0xA8, 0x00, /* Disable error threshold */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'w', 3, 0x04, 0xA9, 0x00, /* Disable error threshold */
        'd',    0x27, 0x10,       /* Delay 10ms */
        'e'
    };

    static const uint8_t setExtFsyncMode[] = {3, 0x04, 0xA0, 0x08};
    static const uint8_t setTxID[] = {3, 0x04, 0xB1, 0x10}; /* GPIO ID 2 for the input */
    static const uint8_t setGMSL2PerLinkExtFsyncMode[][4] = {
        {3, 0x03, 0x06, 0x83},
        {3, 0x03, 0x3D, 0x22},
        {3, 0x03, 0x74, 0x22},
        {3, 0x03, 0xAA, 0x22},
    };
    uint8_t enableGpiGpo[] = {3, 0x0B, 0x08, 0x00};
    uint8_t i = 0u;

    /* TODO: Handle GMSL1 + GMSL2 case */
    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
            gmslMode = drvHandle->ctx.gmslMode[i];
            break;
        }
    }

    if (FSyncMode == CDI_MAX96712_FSYNC_MANUAL) {
        /* Calculate FRSYNC period in manual mode based on PCLK */
        if (drvHandle->ctx.manualFSyncFPS != 0u) {
            if (drvHandle->ctx.manualFSyncFPS != fps) {
                /* Requested a new manual fsync frequency*/
                LOG_ERR("MAX96712: 2 different manual fsync frequencies requested");
                return NVMEDIA_STATUS_NOT_SUPPORTED;
            }
        } else {
            /* calculate frsync high period */
            fsyncPeriod[3] = (gmslMode == CDI_MAX96712_GMSL1_MODE) ? ((pclk / fps) >> 16u) & 0xFF : 0x25;
            /* calculate frsync middle period */
            fsyncPeriod[7] = (gmslMode == CDI_MAX96712_GMSL1_MODE) ? ((pclk / fps) >> 8u) & 0xFF : 0x4C;
            /* calculate frsync low period */
            fsyncPeriod[11] = (gmslMode == CDI_MAX96712_GMSL1_MODE) ? (pclk / fps) & 0xFF : 0x9C;

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

            if (gmslMode == CDI_MAX96712_GMSL2_MODE) {
                setManualFsyncMode[27] = 0x90; /* Set FSYNC to GMSL2 type */
            }

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

            nvsleep(10000);

            drvHandle->ctx.manualFSyncFPS = fps;
        }

        if (gmslMode == CDI_MAX96712_GMSL1_MODE) {
                enableGpiGpo[3] = 0x35;

                for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
                    if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
                        enableGpiGpo[1] = 0xB + i;

                        status = WriteArray(handle,
                                            GET_SIZE(enableGpiGpo),
                                            enableGpiGpo);
                        if (status != NVMEDIA_STATUS_OK) {
                            return status;
                        }
                        nvsleep(10000);
                    }
                }
        }
    } else if (FSyncMode == CDI_MAX96712_FSYNC_OSC_MANUAL) {
        /* Calculate FRSYNC period in manual with OSC mode */
        if (drvHandle->ctx.manualFSyncFPS != 0u) {
            if (drvHandle->ctx.manualFSyncFPS != fps) {
                /* Requested a new manual fsync frequency*/
                LOG_ERR("MAX96712: 2 different manual fsync frequencies requested");
                return NVMEDIA_STATUS_NOT_SUPPORTED;
            }
        }

        /* 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.
         */
        if (gmslMode == CDI_MAX96712_GMSL2_MODE) {
            setOSCManualFsyncMode[4] |= (1 << 7); /* Set FSYNC to GMSL2 type */
        }

        status = WriteArrayWithCommand(handle,
                                       setOSCManualFsyncMode);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }

        if (gmslMode == CDI_MAX96712_GMSL2_MODE) {
            status = WriteArray(handle,
                                GET_SIZE(setTxID),
                                setTxID);
            if (status != NVMEDIA_STATUS_OK) {
                return status;
            }
        }

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

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

        drvHandle->ctx.manualFSyncFPS = fps;

        if (gmslMode == CDI_MAX96712_GMSL1_MODE) {
                enableGpiGpo[3] = 0x35;

                for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
                    if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
                        enableGpiGpo[1] = 0xB + i;

                        status = WriteArray(handle,
                                            GET_SIZE(enableGpiGpo),
                                            enableGpiGpo);
                        if (status != NVMEDIA_STATUS_OK) {
                            return status;
                        }
                        nvsleep(10000);
                    }
                }
        }
    } else if (FSyncMode == CDI_MAX96712_FSYNC_EXTERNAL) {
        status = WriteArray(handle,
                            GET_SIZE(setExtFsyncMode),
                            setExtFsyncMode);

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

        for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
            if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
                if (gmslMode == CDI_MAX96712_GMSL1_MODE) {
                    enableGpiGpo[3] = 0x65;
                    enableGpiGpo[1] = 0xB + i;

                    status = WriteArray(handle,
                                        GET_SIZE(enableGpiGpo),
                                        enableGpiGpo);
                    if (status != NVMEDIA_STATUS_OK) {
                        return status;
                    }
                    nvsleep(10000);
                } else {
                    status = WriteArray(handle,
                                        GET_SIZE(setTxID),
                                        setTxID);
                    if (status != NVMEDIA_STATUS_OK) {
                        return status;
                    }

                    status = WriteArray(handle,
                                        GET_SIZE(setGMSL2PerLinkExtFsyncMode[i]),
                                        setGMSL2PerLinkExtFsyncMode[i]);
                    if (status != NVMEDIA_STATUS_OK) {
                        return status;
                    }
                    nvsleep(10000);
                }
            }
        }
    } else if (FSyncMode == CDI_MAX96712_FSYNC_AUTO) {
        status = WriteArray(handle,
                            GET_SIZE(setAutoFsyncMode),
                            setAutoFsyncMode);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }
    } else {
        LOG_ERR("MAX96712: Invalid param: FSyncMode");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    return status;
}

static NvMediaStatus
ReadCtrlChnlCRCErr(
    DevBlkCDIDevice *handle,
    LinkMAX96712 link,
    uint8_t *errVal)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t i = 0u;

    if (errVal == NULL) {
        LOG_ERR("MAX96712: Bad parameter: Null ptr");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
            ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_CC_CRC_ERRCNT_A + i,
                                         0u,
                                         REG_READ_MODE);
            *errVal = ReadFromRegFieldQ(handle, 0u);
        }
    }

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
GetEnabledLinks(
    DevBlkCDIDevice *handle,
    LinkMAX96712 *link)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t i = 0u;

    *link = 0u;
    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        ClearRegFieldQ(handle);
        ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_LINK_EN_A + i,
                                  0u);
        ACCESS_REG_FIELD_RET_ERR(REG_READ_MODE);
        *link |= (ReadFromRegFieldQ(handle, 0u) << i);
    }

    return status;
}

static NvMediaStatus
ClearErrb(
    DevBlkCDIDevice *handle,
    LinkMAX96712 *link,
    uint8_t *errVal)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    ErrorStatusMAX96712 errorStatus;
    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    uint8_t regVal = 0u;

    if (drvHandle->ctx.tpgEnabled == true) {
        return NVMEDIA_STATUS_OK;
    }

    ClearRegFieldQ(handle);
    ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_ERRB,
                              0u);
    ACCESS_REG_FIELD_RET_ERR(REG_READ_MODE);
    if (ReadFromRegFieldQ(handle, 0u) == 1u) {
        LOG_ERR("MAX96712 ERRB was Set\n");
        status = MAX96712GetErrorStatus(handle,
                                        sizeof(errorStatus),
                                        &errorStatus);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }
    }

    ClearRegFieldQ(handle);
    ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_OVERFLOW,
                              0u);
    ACCESS_REG_FIELD_RET_ERR(REG_READ_MODE);
    regVal = ReadFromRegFieldQ(handle, 0u) ;
    if (regVal != 0u) {
        LOG_ERR("MAX96712 Overflow reported : 0x%02x\n", regVal);
        nvsleep(10000);
        return NVMEDIA_STATUS_ERROR;
    }

    return status;
}

static NvMediaStatus
EnableReplication(
    DevBlkCDIDevice *handle,
    bool enable)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    TxPortMAX96712 port = CDI_MAX96712_TXPORT_PHY_C;
    MipiOutModeMAX96712 mipiOutMode = CDI_MAX96712_MIPI_OUT_INVALID;
    PHYModeMAX96712 phyMode = CDI_MAX96712_PHY_MODE_INVALID;
    RevisionMAX96712 revision = CDI_MAX96712_REV_INVALID;
    NvMediaBool slaveEnabled;
    _DriverHandle *drvHandle = NULL;
    uint8_t 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 */
    };

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

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

    /* Replication is not supported on revision 1 in CPHY mode */
    if ((revision == CDI_MAX96712_REV_1) &&
        (phyMode == CDI_MAX96712_PHY_MODE_CPHY)) {
        LOG_ERR("MAX96712: Replication in CPHY mode is supported only "
                "on platforms with MAX96712 revision 2 or higher.");
        return NVMEDIA_STATUS_NOT_SUPPORTED;
    }

    if ((!slaveEnabled) && enable) { /* Update the replication but do NOT enable the replication */
        switch (port) {
            case CDI_MAX96712_TXPORT_PHY_C :
                if (mipiOutMode == CDI_MAX96712_MIPI_OUT_4x2) {
                    data[3] = MAX96712_REPLICATION(CDI_MAX96712_TXPORT_PHY_C, CDI_MAX96712_TXPORT_PHY_E);
                    data[7] = MAX96712_REPLICATION(CDI_MAX96712_TXPORT_PHY_C, CDI_MAX96712_TXPORT_PHY_F);
                    if (revision != CDI_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 (mipiOutMode == CDI_MAX96712_MIPI_OUT_2x4) {
                    data[3] = MAX96712_REPLICATION(CDI_MAX96712_TXPORT_PHY_D, CDI_MAX96712_TXPORT_PHY_E);
                } else {
                    return NVMEDIA_STATUS_BAD_PARAMETER;
                }
                break;
            case CDI_MAX96712_TXPORT_PHY_D :
                if ((mipiOutMode == CDI_MAX96712_MIPI_OUT_2x4) || (mipiOutMode == CDI_MAX96712_MIPI_OUT_4x2)) {
                    data[3] = MAX96712_REPLICATION(CDI_MAX96712_TXPORT_PHY_D, CDI_MAX96712_TXPORT_PHY_E);
                } else {
                    return NVMEDIA_STATUS_BAD_PARAMETER;
                }
                break;
            case CDI_MAX96712_TXPORT_PHY_E :
                if (mipiOutMode == CDI_MAX96712_MIPI_OUT_2x4) {
                    data[3] = MAX96712_REPLICATION(CDI_MAX96712_TXPORT_PHY_E, CDI_MAX96712_TXPORT_PHY_D);
                } else if (mipiOutMode == CDI_MAX96712_MIPI_OUT_4x2) {
                    data[3] = MAX96712_REPLICATION(CDI_MAX96712_TXPORT_PHY_E, CDI_MAX96712_TXPORT_PHY_C);
                } else {
                    return NVMEDIA_STATUS_BAD_PARAMETER;
                }
                break;
            default :
                data[3] = MAX96712_REPLICATION(CDI_MAX96712_TXPORT_PHY_C, CDI_MAX96712_TXPORT_PHY_E);
                break;
        }

        status = WriteArray(handle,
                            sizeof(data),
                            data);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }
    } else if (slaveEnabled) { /* Enable or disable the replication */
        if ((port == CDI_MAX96712_TXPORT_PHY_F) && (revision != CDI_MAX96712_REV_4)) {
            LOG_INFO("The replication mode is already enabled\n");
            return NVMEDIA_STATUS_OK;
        }

        for (i = 0u; i < 2u; i++) {
            status = DevBlkCDIDeviceRead(handle,
                                         MAX96712_CDI_DEVICE_INDEX,
                                         MAX96712_NUM_ADDR_BYTES,
                                         &data[1 + i * 4],
                                         1,
                                         &data[3 + i * 4]);
            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
ConfigureMIPIOutput(
    DevBlkCDIDevice *handle,
    uint8_t mipiSpeed,
    PHYModeMAX96712 phyMode)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    MipiOutModeMAX96712 mipiOutMode = drvHandle->ctx.mipiOutMode;
    uint8_t mipiOutModeVal = (mipiOutMode == CDI_MAX96712_MIPI_OUT_4x2) ? (1u << 0u) : (1u << 2u);
    uint8_t data[] = {3, 0x08, 0xA2, 0x00};
    uint8_t i = 0u;

    if ((phyMode != CDI_MAX96712_PHY_MODE_DPHY) &&
        (phyMode != CDI_MAX96712_PHY_MODE_CPHY)) {
        LOG_ERR("MAX96712: Invalid MIPI output port");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if ((mipiSpeed < 1u) || (mipiSpeed > 25u)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    /* Force DPHY0 clk enabled not needed for Rev 1 */
    if ((drvHandle->ctx.revision != CDI_MAX96712_REV_1) &&
        (phyMode == CDI_MAX96712_PHY_MODE_DPHY)) {
        mipiOutModeVal = mipiOutModeVal | (1u << 5u);
    }

    ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_MIPI_OUT_CFG,
                                 mipiOutModeVal,
                                 REG_READ_MOD_WRITE_MODE);

    /* Set prebegin phase, post length and prepare for CPHY mode
     * This is a requirement for CPHY periodic calibration */
    if (phyMode == CDI_MAX96712_PHY_MODE_CPHY) {
        /* t3_prebegin = (63 + 1) * 7 = 448 UI */
        ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_T_T3_PREBEGIN,
                                     0x3Fu,
                                     REG_READ_MOD_WRITE_MODE);

        /* Bit[6:2] = t3_post = (31 + 1) * 7 = 224 UI
         * Bit[1:0] = t3_prepare = 86.7ns */
        ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_T_T3_POST_PREP,
                                     0x7Fu,
                                     REG_READ_MOD_WRITE_MODE);
    }

    /* 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,
                        GET_SIZE(data),
                        data);
    if (status != NVMEDIA_STATUS_OK) {
        return status;
    }

    /* Set CSI2 lane count per Phy */
    /* Configure Lane count for Port A in x4 or C,D in x2 and config phyMode*/
    data[1] = 0x09;
    data[3] = (mipiOutMode == CDI_MAX96712_MIPI_OUT_4x2) ? 0x40 : 0xC0;
    data[3] |= (phyMode == CDI_MAX96712_PHY_MODE_CPHY) ? (1u << 5u) : 0u;
    for (i = 0u; i < 2u; i++) {
        data[2] = 0x0A + i * 0x40;
        status = WriteArray(handle,
                            GET_SIZE(data),
                            data);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }
    }
    /* Configure Lane count for Port B in x4 or E,F in x2 an config phyMode */
    for (i = 0u; i < 2u; i++) {
        data[2] = 0x8A + i * 0x40;
        status = WriteArray(handle,
                            GET_SIZE(data),
                            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 */
    data[3] = (mipiOutMode == CDI_MAX96712_MIPI_OUT_4x2) ? 0x44 : 0xE4;
    status = WriteArray(handle,
                        GET_SIZE(data),
                        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 */
    data[3] = (mipiOutMode == CDI_MAX96712_MIPI_OUT_4x2) ? 0x44 : 0xE4;
    status = WriteArray(handle,
                        GET_SIZE(data),
                        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 < MAX96712_MAX_NUM_LINK; i++) {
        data[1] = 0x1C + i;

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

    /* Set MIPI speed */
    data[1] = 0x04; /* high byte offset */
    data[2] = 0x15; /* low byte offset */
    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        data[2] = 0x15 + i * 0x3;
        status = DevBlkCDIDeviceRead(handle,
                             MAX96712_CDI_DEVICE_INDEX,
                             MAX96712_NUM_ADDR_BYTES,
                             &data[1],
                             1u,
                             &data[3]);
        if (status != NVMEDIA_STATUS_OK)
            return status;

        data[3] &= ~0x3F;
        data[3] |= ((1u << 5u) | mipiSpeed);
        status = WriteArray(handle,
                            GET_SIZE(data),
                            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 < MAX96712_MAX_NUM_LINK; i++) {
        data[1] = 0x1C + i;

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

    /* Deskew is enabled if MIPI speed is faster than or equal to 1.5GHz */
    if ((phyMode == CDI_MAX96712_PHY_MODE_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,
                                GET_SIZE(data),
                                data);
            if (status != NVMEDIA_STATUS_OK) {
                return status;
            }
        }
    }

    return status;
}

static NvMediaStatus
DisableDE(
    DevBlkCDIDevice *handle,
    LinkMAX96712 link)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t i = 0u;

    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
            ClearRegFieldQ(handle);

            ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_DE_EN_PHY_A + i,
                                      0u);
            ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_DE_PRBS_TYPE_PHY_A + i,
                                      1u);

            ACCESS_REG_FIELD_RET_ERR(REG_WRITE_MODE);
        }
    }

    return status;
}

static NvMediaStatus
SetDBL(
    DevBlkCDIDevice *handle,
    LinkMAX96712 link,
    bool enable)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t data[] = {3, 0x0B, 0x07, 0x8C};
    uint8_t i = 0u;

    if (enable == false) {
        data[3] = 0x0;
    }

    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
            data[1] = 0x0B + i;
            status = WriteArray(handle,
                                GET_SIZE(data),
                                data);
            if (status != NVMEDIA_STATUS_OK) {
                return status;
            }
            nvsleep(5000);
        }
    }

    return status;
}

static NvMediaStatus
ControlForwardChannels(
    DevBlkCDIDevice *handle,
    LinkMAX96712 link,
    bool enable)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    I2CPortMAX96712 i2cPort = drvHandle->ctx.i2cPort;
    uint8_t i = 0u;

    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
            ClearRegFieldQ(handle);
            /* Enable / disable the forward channel */
            ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_I2C_FWDCCEN_PHY_A + i,
                                      enable ? 1u : 0u);

            /* 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 */
            ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_I2C_REVCCEN_PHY_A + i,
                                      1u);

            /* Force set I2C Port */
            ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_I2C_PORT_GMSL1_PHY_A + i,
                                      (i2cPort == CDI_MAX96712_I2CPORT_1) ? 1u : 0u);

            ACCESS_REG_FIELD_RET_ERR(REG_READ_MOD_WRITE_MODE);
            nvsleep(10000);
        }
    }

    return status;
}

/*
 *  The functions defined below are the entry points when CDI functions are called.
 */

NvMediaStatus
MAX96712CheckLink(
    DevBlkCDIDevice *handle,
    LinkMAX96712 link,
    uint32_t linkType,
    bool display)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = NULL;
    GMSLModeMAX96712 gmslMode = CDI_MAX96712_GMSL_MODE_INVALID;
    uint8_t i = 0u, linkIndex = 0u, success = 0u;

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL)) {
        LOG_ERR("MAX96712: Bad parameter: Null ptr");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if (drvHandle->ctx.tpgEnabled == true) {
        return NVMEDIA_STATUS_OK;
    }

    for (linkIndex = 0u; linkIndex < MAX96712_MAX_NUM_LINK; linkIndex++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, linkIndex)) {
            gmslMode = drvHandle->ctx.gmslMode[linkIndex];
            /* Check lock for each link */
            switch (linkType) {
                case CDI_MAX96712_LINK_LOCK_GMSL1_CONFIG:
                    if (gmslMode != CDI_MAX96712_GMSL1_MODE) {
                        LOG_ERR("MAX96712: Link %u: Config link lock is only valid in GMSL1 mode", linkIndex);
                        return NVMEDIA_STATUS_ERROR;
                    }

                    /* Check for GMSL1 Link Lock.*/
                    ClearRegFieldQ(handle);
                    ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_GMSL1_LOCK_A + linkIndex,
                                              0u);
                    ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_GMSL1_CONFIG_LOCK_A + linkIndex,
                                              0u);

                    /* From Max96712 programming guide V1.1, typical link rebuilding time is 25 ~ 100ms
                     * check the link lock in 100ms periodically every 10ms */
                    for (i = 0u; i < 50u; i++) {
                        ACCESS_REG_FIELD_RET_ERR(REG_READ_MODE);
                        if ((ReadFromRegFieldQ(handle, 0u) == 1u) ||
                            (ReadFromRegFieldQ(handle, 1u) == 1u))  {
                            LOG_DBG("MAX96712: Link %u: GMSL1 config link lock after %u ms\n", linkIndex, (i * 10u));
                            success = 1;
                            break;
                        }
                        nvsleep(10000);
                    }
                    if (success == 1) {
                        success = 0;
                        break;
                    } else {
                        if (display) {
                            LOG_ERR("MAX96712: Link %u: GMSL1 config link lock not detected(%d)\n", linkIndex, i);
                        }
                        return NVMEDIA_STATUS_ERROR;
                    }
                case CDI_MAX96712_LINK_LOCK_GMSL2:
                    if (gmslMode != CDI_MAX96712_GMSL2_MODE) {
                        LOG_ERR("MAX96712: Link %u: GMSL2 link lock is only valid in GMSL2 mode", linkIndex);
                        return NVMEDIA_STATUS_ERROR;
                    }

                    /* Only register 0x001A is available on MAX96712 Rev 1 to check
                     * link lock in GMSL2 mode*/
                    if (drvHandle->ctx.revision == CDI_MAX96712_REV_1 && linkIndex > 0u) {
                        LOG_DBG("%s: GMSL2 link lock for link %u is not available on MAX96712 Rev 1\n",
                                 linkIndex);
                        return NVMEDIA_STATUS_OK;
                    }

                    /* From Max96712 programming guide V1.1, typical link rebuilding time is 25 ~ 100ms
                     * check the link lock in 100ms periodically
                     * TODO : Intermittently the link lock takes more than 100ms. Check it with MAXIM */
                    for (i = 0u; i < 50u; i++) {
                        ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_GMSL2_LOCK_A + linkIndex,
                                                     0u,
                                                     REG_READ_MODE);

                        if (ReadFromRegFieldQ(handle, 0) == 1u)  {
                            LOG_DBG("MAX96712: Link %u: GMSL2 link lock after %u ms", linkIndex, (i * 10u));
                            success = 1;
                            break;
                        }
                        nvsleep(10000);
                    }
                    if (success == 1) {
                        if (i > 10) {
                            LOG_INFO("MAX96712: GMSL2 Link time %d\n", i * 10);
                        }
                        success = 0;
                        break;
                    } else {
                        if (display) {
                            LOG_ERR("MAX96712: Link %u: GMSL2 link lock not detected", linkIndex);
                        }
                        return NVMEDIA_STATUS_ERROR;
                    }
                case CDI_MAX96712_LINK_LOCK_VIDEO:
                    if (gmslMode == CDI_MAX96712_GMSL1_MODE) {
                        for (i = 0u; i < 10u; i++) {
                            ClearRegFieldQ(handle);
                            ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_GMSL1_LOCK_A + linkIndex,
                                                      0u);
                            ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_GMSL1_VIDEO_LOCK_A + linkIndex,
                                                      0u);
                            ACCESS_REG_FIELD_RET_ERR(REG_READ_MODE);

                            if ((ReadFromRegFieldQ(handle, 0u) == 1u) &&
                                (ReadFromRegFieldQ(handle, 1u) == 1u))  {
                                LOG_DBG("MAX96712: Link %u: GMSL1 video lock after %u ms", linkIndex, (i * 10u));
                                success = 1;
                                break;
                            }
                            nvsleep(10000);
                        }
                        if (success == 1) {
                            success = 0;
                            break;
                        } else {
                            if (display) {
                                LOG_ERR("MAX96712: Link %u: GMSL1 video lock not detected", linkIndex);
                            }
                            return NVMEDIA_STATUS_ERROR;
                        }
                    } else if (gmslMode == CDI_MAX96712_GMSL2_MODE){
                        /* TODO: Check emb pipes if enabled */
                        for (i = 0u; i < 10u; i++) {
                            ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_VIDEO_LOCK_PIPE_0 + linkIndex,
                                                         0u,
                                                         REG_READ_MODE);

                            if (ReadFromRegFieldQ(handle, 0u) == 1u)  {
                                LOG_DBG("MAX96712: Link %u: GMSL2 video lock after %u ms", linkIndex, (i * 10u));
                                success = 1;
                                break;
                            }
                            nvsleep(10000);
                        }

                        if (success == 1) {
                            success = 0;
                            break;
                        } else {
                            if (display) {
                                LOG_ERR("MAX96712: Link %u: GMSL2 video lock not detected", linkIndex);
                            }
                            return NVMEDIA_STATUS_ERROR;
                        }
                    }
                    break;
                default:
                    LOG_ERR("MAX96712: Bad parameter: Invalid link type");
                    return NVMEDIA_STATUS_BAD_PARAMETER;
            }
        }
    }

    return NVMEDIA_STATUS_OK;
}

NvMediaStatus
MAX96712CheckPresence(
    DevBlkCDIDevice *handle)
{
    NvMediaStatus status = NVMEDIA_STATUS_ERROR;
    _DriverHandle *drvHandle = NULL;
    RevisionMAX96712 revision = CDI_MAX96712_REV_INVALID;
    uint8_t revisionVal = 0u;
    uint32_t numRev = sizeof(supportedRevisions) / sizeof(supportedRevisions[0]);
    uint8_t devID = 0u;
    uint32_t i = 0u;

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL)) {
        LOG_ERR("MAX96712: Bad parameter: Null ptr");
        status = NVMEDIA_STATUS_BAD_PARAMETER;
        goto done;
    }

    /* Check device ID */
    ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_DEV_ID,
                                 0u,
                                 REG_READ_MODE);
    devID = ReadFromRegFieldQ(handle, 0u);
    if (devID != MAX96712_DEV_ID) {
        LOG_ERR("MAX96712: Device ID mismatch. Expected: %u, Readval: %u", MAX96712_DEV_ID, devID);
        status = NVMEDIA_STATUS_ERROR;
        goto done;
    }

    /* Check revision ID */
    ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_DEV_REV,
                                 0u,
                                 REG_READ_MODE);
    revisionVal = ReadFromRegFieldQ(handle, 0u);
    for (i = 0u; i < numRev; i++) {
        if (revisionVal == supportedRevisions[i].revVal) {
            revision = supportedRevisions[i].revId;
            LOG_MSG("MAX96712: Revision %u detected\n", revision);

            if (revision == CDI_MAX96712_REV_1) {
                LOG_MSG("MAX96712: Warning: MAX96712 revision 1 detected. All features may not be supported\n"
                        "Please use a platform with MAX96712 revision 2 or higher for full support\n");
                LOG_MSG("And the below error can be observed"
                        "  - FE_FRAME_ID_FAULT on CSIMUX_FRAME : Frame IDs are mismatched between FS and FE packets\n");
            }
            drvHandle->ctx.revision = revision;
            status = NVMEDIA_STATUS_OK;
            break;
        }
    }

    if (status != NVMEDIA_STATUS_OK) {
        LOG_ERR("MAX96712: Unsupported MAX96712 revision %u detected! Supported revisions are:", revisionVal);
        for (i = 0u; i < numRev; i++) {
            LOG_MSG("MAX96712: Revision %u\n", supportedRevisions[i].revVal);
        }
        status = NVMEDIA_STATUS_NOT_SUPPORTED;
    }

done:
    return status;
}

static NvMediaStatus
DriverCreate(
    DevBlkCDIDevice *handle,
    void *clientContext)
{
    _DriverHandle *driverHandle = NULL;
    ContextMAX96712 *ctx = (ContextMAX96712 *) clientContext;
    uint8_t i = 0u;

    if ((handle == NULL) || (clientContext == NULL)) {
        LOG_ERR("MAX96712: Bad parameter: Null ptr");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    /* Check supplied context */
    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (ctx->gmslMode[i] != CDI_MAX96712_GMSL1_MODE &&
            ctx->gmslMode[i] != CDI_MAX96712_GMSL2_MODE &&
            ctx->gmslMode[i] != CDI_MAX96712_GMSL_MODE_UNUSED) {
            LOG_ERR("MAX96712: Invalid GMSL mode");
            return NVMEDIA_STATUS_BAD_PARAMETER;
        }
    }

    if (ctx->i2cPort != CDI_MAX96712_I2CPORT_0 &&
        ctx->i2cPort != CDI_MAX96712_I2CPORT_1) {
        LOG_ERR("MAX96712: Invalid I2C port");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if ((ctx->txPort != CDI_MAX96712_TXPORT_PHY_C) &&
        (ctx->txPort != CDI_MAX96712_TXPORT_PHY_D) &&
        (ctx->txPort != CDI_MAX96712_TXPORT_PHY_E) &&
        (ctx->txPort != CDI_MAX96712_TXPORT_PHY_F)) {
        LOG_ERR("MAX96712: Invalid Tx port");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if ((ctx->mipiOutMode != CDI_MAX96712_MIPI_OUT_4x2) &&
        (ctx->mipiOutMode != CDI_MAX96712_MIPI_OUT_2x4)) {
        LOG_ERR("MAX96712: Invalid MIPI output port");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    driverHandle = calloc(1, sizeof(_DriverHandle));
    if (driverHandle == NULL) {
        LOG_ERR("MAX96712: Memory allocation for context failed");
        return NVMEDIA_STATUS_OUT_OF_MEMORY;
    }

    memcpy(&driverHandle->ctx, ctx, sizeof(ContextMAX96712));
    driverHandle->ctx.revision = CDI_MAX96712_REV_INVALID;
    driverHandle->ctx.manualFSyncFPS = 0u;
    handle->deviceDriverHandle = (void *)driverHandle;

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
DriverDestroy(
    DevBlkCDIDevice *handle)
{
    if (handle == NULL) {
        LOG_ERR("MAX96712: Bad parameter: Null ptr");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if (handle->deviceDriverHandle != NULL) {
        free(handle->deviceDriverHandle);
        handle->deviceDriverHandle = NULL;
    }

    return NVMEDIA_STATUS_OK;
}

NvMediaStatus
MAX96712DumpRegisters(
    DevBlkCDIDevice *handle)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = NULL;
    uint8_t data[3] = {};
    uint32_t i = 0u;

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL)) {
        LOG_ERR("MAX96712: Bad parameter: Null ptr");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

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

        status = DevBlkCDIDeviceRead(handle,
                                     MAX96712_CDI_DEVICE_INDEX,
                                     MAX96712_NUM_ADDR_BYTES,
                                     &data[0],
                                     1u,
                                     &data[2]);
        if (status != NVMEDIA_STATUS_OK) {
            LOG_ERR("MAX96712: Register 0x%04X I2C read failed with status %d", i, status);
            return status;
        }

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

    return status;
}

NvMediaStatus
MAX96712GetErrorStatus(
    DevBlkCDIDevice *handle,
    uint32_t parameterSize,
    void *parameter)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = NULL;
    ErrorStatusMAX96712 *errorStatus = (ErrorStatusMAX96712 *) parameter;
    uint8_t linkNum = 0u;
    uint8_t i = 0u;

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL) ||
        (parameter == NULL)) {
        LOG_ERR("MAX96712: Bad parameter: Null ptr");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if (parameterSize != sizeof(ErrorStatusMAX96712)) {
        LOG_ERR("MAX96712: Bad parameter: Incorrect param size");
        return NVMEDIA_STATUS_BAD_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);

    for (linkNum = 0u; linkNum < MAX96712_MAX_NUM_LINK; linkNum++) {
        errorStatus->failureType[linkNum] = CDI_MAX96712_ERROR_NONE;

        ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_OVERFLOW,
                                     0u,
                                     REG_READ_MODE);
        if ((ReadFromRegFieldQ(handle, 0u) & (1 << linkNum)) ||
            ((ReadFromRegFieldQ(handle, 0u) >> 4) & (1 << linkNum))) {
            LOG_ERR("MAX96712: Link %u: overflow\n", linkNum);
            errorStatus->link |= (1 << linkNum);

            /* Check overflow status every 1ms periodically */
            for (i = 0u; i < 100u; i++) {
                ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_OVERFLOW,
                                             0u,
                                             REG_READ_MODE);
                if (!(ReadFromRegFieldQ(handle, 0u) & (1 << linkNum)) &&
                    !((ReadFromRegFieldQ(handle, 0u) >> 4) & (1 << linkNum))) {
                    LOG_ERR("MAX96712: Link %u: overflow disappear after %u ms\n", linkNum, i);
                    break;
                }
                nvsleep(1000);
            }
        }

        if (drvHandle->ctx.gmslMode[linkNum] == CDI_MAX96712_GMSL1_MODE) {
            ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_GMSL1_LOCK_A + linkNum,
                                         0u,
                                         REG_READ_MODE);
            if (ReadFromRegFieldQ(handle, 0u) != 1u) {
                LOG_ERR("MAX96712: Link %u: GMSL1 link unlocked", linkNum);
                errorStatus->failureType[linkNum] = CDI_MAX96712_ERROR_GMSL1_LINK_UNLOCK;
                errorStatus->regVal[linkNum] = ReadFromRegFieldQ(handle, 0u);
                errorStatus->link |= (1 << linkNum);
            }

            ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_GMSL1_DET_ERR_A + linkNum,
                                         0u,
                                         REG_READ_MODE);
            if ((ReadFromRegFieldQ(handle, 0u) != 0u) &&
                (errorStatus->failureType[linkNum] == CDI_MAX96712_ERROR_INVALID)) {
                LOG_ERR("MAX96712: Link %u: GMSL1 decoding error (%u)", linkNum,
                        ReadFromRegFieldQ(handle, 0u));
                errorStatus->failureType[linkNum] = CDI_MAX96712_GMSL1_DET_ERR;
                errorStatus->regVal[linkNum] = ReadFromRegFieldQ(handle, 0u);
                errorStatus->link |= (1 << linkNum);
            }

            ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_CC_CRC_ERRCNT_A + linkNum,
                                         0u,
                                         REG_READ_MODE);
            if ((ReadFromRegFieldQ(handle, 0u) != 0u) &&
                (errorStatus->failureType[linkNum] == CDI_MAX96712_ERROR_INVALID)) {
                LOG_ERR("MAX96712: Link %u: GMSL1 PKTCC CRC (%u)", linkNum,
                        ReadFromRegFieldQ(handle, 0u));
                errorStatus->failureType[linkNum] = CDI_MAX96712_GMSL1_PKTCC_CRC_ERR;
                errorStatus->regVal[linkNum] = ReadFromRegFieldQ(handle, 0u);
                errorStatus->link |= (1 << linkNum);
            }
        } else if (drvHandle->ctx.gmslMode[linkNum] == CDI_MAX96712_GMSL2_MODE) {
            ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_GMSL2_LOCK_A + linkNum,
                                         0u,
                                         REG_READ_MODE);
            if (ReadFromRegFieldQ(handle, 0u) == 0u) {
                LOG_ERR("MAX96712: Link %u: GMSL2 link unlocked", linkNum);
                errorStatus->failureType[linkNum] = CDI_MAX96712_ERROR_GMSL2_LINK_UNLOCK;
                errorStatus->regVal[linkNum] = ReadFromRegFieldQ(handle, 0u);
                errorStatus->link |= (1 << linkNum);
                continue;
            }

            ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_GMSL2_DEC_ERR_A + linkNum,
                                         0u,
                                         REG_READ_MODE);
            if ((ReadFromRegFieldQ(handle, 0u) != 0u) &&
                (errorStatus->failureType[linkNum] == CDI_MAX96712_ERROR_INVALID)) {
                LOG_ERR("MAX96712: Link %u: GMSL2 decoding error (%u)", linkNum,
                        ReadFromRegFieldQ(handle, 0u));
                errorStatus->failureType[linkNum] = CDI_MAX96712_GMSL2_DEC_ERR;
                errorStatus->regVal[linkNum] = ReadFromRegFieldQ(handle, 0u);
                errorStatus->link |= (1 << linkNum);
                continue;
            }

            ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_GMSL2_IDLE_ERR_A + linkNum,
                                         0u,
                                         REG_READ_MODE);
            if ((ReadFromRegFieldQ(handle, 0u) != 0u) &&
                (errorStatus->failureType[linkNum] == CDI_MAX96712_ERROR_INVALID)) {
                LOG_ERR("MAX96712: Link %u: GMSL2 idle error (%u)", linkNum,
                        ReadFromRegFieldQ(handle, 0u));
                errorStatus->failureType[linkNum] = CDI_MAX96712_GMSL2_IDLE_ERR;
                errorStatus->link |= (1 << linkNum);
                continue;
            }
        }
    }

    return NVMEDIA_STATUS_OK;
}

NvMediaStatus
MAX96712ReadParameters(
    DevBlkCDIDevice *handle,
    uint32_t parameterType,
    uint32_t parameterSize,
    void *parameter)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = NULL;
    bool isValidSize = false;
    ReadParametersParamMAX96712 *param = (ReadParametersParamMAX96712 *) parameter;
    static const char *cmdString[] = {
        [CDI_READ_PARAM_CMD_MAX96712_REV_ID] =
            "CDI_READ_PARAM_CMD_MAX96712_REV_ID",
        [CDI_READ_PARAM_CMD_MAX96712_CONTROL_CHANNEL_CRC_ERROR] =
            "CDI_READ_PARAM_CMD_MAX96712_CONTROL_CHANNEL_CRC_ERROR",
        [CDI_READ_PARAM_CMD_MAX96712_ENABLED_LINKS] =
            "CDI_READ_PARAM_CMD_MAX96712_ENABLED_LINKS",
        [CDI_READ_PARAM_CMD_MAX96712_ERRB] =
            "CDI_READ_PARAM_CMD_MAX96712_ERRB",
    };

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL) ||
        (parameter == NULL)) {
        LOG_ERR("MAX96712: Bad parameter: Null ptr");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if ((parameterType == CDI_READ_PARAM_CMD_MAX96712_INVALID) ||
        (parameterType >= CDI_READ_PARAM_CMD_MAX96712_NUM)) {
        LOG_ERR("MAX96712: Bad parameter: Invalid command");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    LOG_DBG("MAX96712: %s", cmdString[parameterType]);
    switch (parameterType) {
        case CDI_READ_PARAM_CMD_MAX96712_REV_ID:
            if (parameterSize == sizeof(param->revision)) {
                isValidSize = true;
                param->revision = drvHandle->ctx.revision;
                status = NVMEDIA_STATUS_OK;
            }
            break;
        case CDI_READ_PARAM_CMD_MAX96712_CONTROL_CHANNEL_CRC_ERROR:
            if (parameterSize == sizeof(param->ErrorStatus)) {
                isValidSize = true;
                status = ReadCtrlChnlCRCErr(handle,
                                            param->ErrorStatus.link,
                                            &param->ErrorStatus.errVal);
            }
            break;
        case CDI_READ_PARAM_CMD_MAX96712_ENABLED_LINKS:
            if (parameterSize == sizeof(param->link)) {
                isValidSize = true;
                status = GetEnabledLinks(handle,
                                         &param->link);
            }
            break;
        case CDI_READ_PARAM_CMD_MAX96712_ERRB:
            if (parameterSize == sizeof(param->ErrorStatus)) {
                isValidSize = true;
                status = ClearErrb(handle,
                                   &param->ErrorStatus.link,
                                   &param->ErrorStatus.errVal);
            }
            break;
        default:
            LOG_ERR("MAX96712: Bad parameter: Invalid command");
            isValidSize = true;
            status = NVMEDIA_STATUS_BAD_PARAMETER;
            break;
    }

    if (status != NVMEDIA_STATUS_OK) {
        LOG_ERR("MAX96712: %s failed", cmdString[parameterType]);
    }

    if (!isValidSize) {
        LOG_ERR("MAX96712: %s: Bad parameter: Invalid param size", cmdString[parameterType]);
        status = NVMEDIA_STATUS_BAD_PARAMETER;
    }

    return status;
}

NvMediaStatus
MAX96712ReadRegister(
    DevBlkCDIDevice *handle,
    uint32_t deviceIndex,
    uint32_t registerNum,
    uint32_t dataLength,
    uint8_t *dataBuff)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = NULL;
    uint8_t registerData[MAX96712_NUM_ADDR_BYTES] = {} ;

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL) ||
        (dataBuff == NULL)) {
        LOG_ERR("MAX96712: Bad parameter: Null ptr");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

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

    status = DevBlkCDIDeviceRead(handle,
                                 deviceIndex,
                                 MAX96712_NUM_ADDR_BYTES,
                                 registerData,
                                 dataLength,
                                 dataBuff);
    if (status != NVMEDIA_STATUS_OK) {
        LOG_ERR("MAX96712: Register 0x%04X I2C read failed with status %d", registerNum, status);
    }

    return status;
}

static NvMediaStatus
GMSL2LinkAdaptation(
    DevBlkCDIDevice *handle,
    LinkMAX96712 link)
{
    NvMediaStatus status;
    GMSLModeMAX96712 gmslMode = CDI_MAX96712_GMSL_MODE_INVALID;
    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    RevisionMAX96712 rev = drvHandle->ctx.revision;
    uint8_t regVal = 0u, i = 0u, loop = 0u;

    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if (MAX96712_IS_GMSL_LINK_SET(link, i)) {
            gmslMode = drvHandle->ctx.gmslMode[i];

            if (gmslMode != CDI_MAX96712_GMSL2_MODE) {
                LOG_INFO("MAX96712: Link %d: adaptation is required only in GMSL2 mode\n", i);
                continue;
            }

            /* Disable OSN */
            ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_ENABLE_OSN_0 + i,
                                         0u,
                                         REG_READ_MOD_WRITE_MODE);

            /* Reseed and set to default value 31 */
            ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_OSN_COEFF_MANUAL_SEED_0 + i,
                                         1u,
                                         REG_READ_MOD_WRITE_MODE);

            nvsleep(10000);

            for (loop = 0; loop < 100; loop++) {
                /* Read back OSN value */
                ACCESS_ONE_REG_FIELD_RET_ERR(REG_FIELD_OSN_COEFFICIENT_0 + i, 0u, REG_READ_MODE);
                regVal = ReadFromRegFieldQ(handle, 0u);
                if (regVal == 31) {
                    break;
                }
                nvsleep(1000);
            }

            LOG_MSG("MAX96712 Rev %d manual adaptation on the link %d (%d)\n", rev,
                                                                               i,
                                                                               regVal);
        }
    }

    return status;
}

static NvMediaStatus
EnableMemoryECC(
    DevBlkCDIDevice *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 != CDI_MAX96712_REV_3) &&
        (drvHandle->ctx.revision != CDI_MAX96712_REV_4)) {
        return NVMEDIA_STATUS_NOT_SUPPORTED;
    }

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

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

    return status;
}

static NvMediaStatus
CheckCSIPLLLock(
    DevBlkCDIDevice *handle)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    NvMediaBool slaveEnabled = drvHandle->ctx.slaveEnabled;

    uint8_t i = 0u;
    MipiOutModeMAX96712 mipiOutMode;
    uint8_t CSIPllLock[3] = {
        0x04, 0x00, 0x00
    };
    uint8_t forcePllLock[3] = {
        0x1C, 0x12, 0x4B
    };

    mipiOutMode = drvHandle->ctx.mipiOutMode;

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

            if (((mipiOutMode == CDI_MAX96712_MIPI_OUT_2x4) && ((CSIPllLock[2] & 0xF0) == 0x60)) ||
                ((mipiOutMode == CDI_MAX96712_MIPI_OUT_4x2) && ((CSIPllLock[2] & 0xF0) == 0xF0))) {
                break;
            }
            nvsleep(10000);
        }

        for (i = 0u; i < 4u; i++) {
            if ((mipiOutMode == CDI_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 = DevBlkCDIDeviceWrite(handle, 0u,
                                              3u,
                                              forcePllLock);
                if (status != NVMEDIA_STATUS_OK) {
                    return status;
                }
            }
        }

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

        if (((mipiOutMode == CDI_MAX96712_MIPI_OUT_2x4) && ((CSIPllLock[2] & 0xF0) != 0x60)) ||
            ((mipiOutMode == CDI_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
GMSL2PHYOptimizationRevE(
    DevBlkCDIDevice *handle,
    LinkMAX96712 link)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = (_DriverHandle *)handle->deviceDriverHandle;
    uint8_t i = 0u;
    uint8_t increaseCMUOutVoltage[] = {
        'w', 3, 0x06, 0xC2, 0x10, /* Increase CMU regulator output voltage (bit 4) */
        'e'
    };

    uint8_t vgaHiGain_Init[] = {
        'w', 3, 0x14, 0xD1, 0x03, /* Set VgaHiGain_Init_6G (bit 1) and VgaHiGain_Init_3G (bit 0) */
        'e'
    };

    status = WriteArrayWithCommand(handle,
                                   increaseCMUOutVoltage);
    if (status != NVMEDIA_STATUS_OK) {
        LOG_ERR("MAX96712: Failed to increase CMU output voltage\n");
        return status;
    }

    for (i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        if ((MAX96712_IS_GMSL_LINK_SET(drvHandle->ctx.linkMask, i)) &&
            (drvHandle->ctx.gmslMode[i] == CDI_MAX96712_GMSL2_MODE)) {
            vgaHiGain_Init[2] = 0x14 + i;
            status = WriteArrayWithCommand(handle,
                                           vgaHiGain_Init);
            if (status != NVMEDIA_STATUS_OK) {
                LOG_ERR("MAX96712: Link %d: Failed to set VgaHighGain_Init\n", i);
                return status;
            }

            LOG_MSG("MAX96712 Link %d: PHY optimization was enabled\n", i);
        }
    }

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
GMSL2PHYOptimization(
    DevBlkCDIDevice *handle)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = NULL;
    uint8_t i = 0u, isOSNDone = 0u;

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL)) {
        LOG_ERR("MAX96712: Bad parameter: Null ptr");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if ((drvHandle->ctx.revision == CDI_MAX96712_REV_2) ||
        (drvHandle->ctx.revision == CDI_MAX96712_REV_3)){
        // If any link is configured in GMSL2 mode, execute the link adaptation
        for (i = 0; i < MAX96712_MAX_NUM_LINK; i++) {
            if ((MAX96712_IS_GMSL_LINK_SET(drvHandle->ctx.linkMask, i)) &&
                (drvHandle->ctx.gmslMode[i] == CDI_MAX96712_GMSL2_MODE)) {
                status = EnableSpecificLinks(handle, (LinkMAX96712)(1 << i));
                if (status != NVMEDIA_STATUS_OK) {
                    return status;
                }

                status = MAX96712CheckLink(handle, (LinkMAX96712)(1 << i), CDI_MAX96712_LINK_LOCK_GMSL2, true);
                if (status != NVMEDIA_STATUS_OK) {
                    return status;
                }

                status = ConfigTxAmpTiming(handle, (LinkMAX96712)(1 << i));
                if (status != NVMEDIA_STATUS_OK) {
                    return status;
                }

                status = GMSL2LinkAdaptation(handle, (LinkMAX96712)(1 << i));
                if (status != NVMEDIA_STATUS_OK) {
                    return status;
                }

                status = MAX96712OneShotReset(handle, (LinkMAX96712)(1 << i));
                if (status != NVMEDIA_STATUS_OK) {
                    return status;
                }

                status = MAX96712CheckLink(handle, (LinkMAX96712)(1 << i), CDI_MAX96712_LINK_LOCK_GMSL2, true);
                if (status != NVMEDIA_STATUS_OK) {
                    return status;
                }
                isOSNDone = 1u;
            }
        }

        if (isOSNDone == 1u) {
            status = EnableSpecificLinks(handle, drvHandle->ctx.linkMask);
            if (status != NVMEDIA_STATUS_OK) {
                return status;
            }

            for (i = 0; i < MAX96712_MAX_NUM_LINK; i++) {
                if (MAX96712_IS_GMSL_LINK_SET(drvHandle->ctx.linkMask, i)) {
                    if (drvHandle->ctx.gmslMode[i] == CDI_MAX96712_GMSL1_MODE) {
                        /* HIM mode is not enabled yet so the link lock will not be set
                         * Instead use sleep function */
                        nvsleep(100000);
                    } else {
                        status = MAX96712CheckLink(handle, drvHandle->ctx.linkMask,CDI_MAX96712_LINK_LOCK_GMSL2, true);
                        if (status != NVMEDIA_STATUS_OK) {
                            return status;
                        }
                    }
                }
            }
        }
    } else if (drvHandle->ctx.revision == CDI_MAX96712_REV_5) {
        status = GMSL2PHYOptimizationRevE(handle, drvHandle->ctx.linkMask);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }
    }

    return status;
}

NvMediaStatus
MAX96712SetDefaults(
    DevBlkCDIDevice *handle)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = NULL;
    uint8_t i = 0u;

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL)) {
        LOG_ERR("MAX96712: Bad parameter: Null ptr");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if (drvHandle->ctx.revision == CDI_MAX96712_REV_3) {
        /* Bug 2446492: Disable 2-bit ECC error reporting as spurious ECC errors are
         * intermittently observed on Rev C of MAX96712
         * Disable reporting 2-bit ECC errors to ERRB
         */
        status = EnableMemoryECC(handle, false, false);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }
    } else if (drvHandle->ctx.revision == CDI_MAX96712_REV_4) {
        status = EnableMemoryECC(handle, true, true);
        if (status != NVMEDIA_STATUS_OK) {
            return status;
        }
    }

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

    for (i = 0; i < MAX96712_MAX_NUM_LINK; i++) {
        if ((drvHandle->ctx.gmslMode[i] == CDI_MAX96712_GMSL2_MODE) &&
            (drvHandle->ctx.longCables[i] == true)) {
            status = UpdateVGAHighGain(handle, (LinkMAX96712)(1 << i), drvHandle->ctx.longCables[i]);
            if (status != NVMEDIA_STATUS_OK) {
                return status;
            }
        }
    }

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

    /* Disable all pipelines*/
    ClearRegFieldQ(handle);
    for (i = 0u; i < 8; i++) {
            ADD_ONE_REG_FIELD_RET_ERR(REG_FIELD_VIDEO_PIPE_EN_0 + i,
                                      0u);
    }
    ACCESS_REG_FIELD_RET_ERR(REG_WRITE_MODE);

    return NVMEDIA_STATUS_OK;
}

NvMediaStatus
MAX96712SetDeviceConfig(
    DevBlkCDIDevice *handle,
    uint32_t enumeratedDeviceConfig)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = NULL;
    static const char *cmdString[] = {
        [CDI_CONFIG_MAX96712_SET_PG_1920x1236] =
            "CDI_CONFIG_MAX96712_SET_PG_1920x1236",
        [CDI_CONFIG_MAX96712_ENABLE_PG] =
            "CDI_CONFIG_MAX96712_ENABLE_PG",
        [CDI_CONFIG_MAX96712_ENABLE_BACKTOP] =
            "CDI_CONFIG_MAX96712_ENABLE_BACKTOP",
        [CDI_CONFIG_MAX96712_DISABLE_BACKTOP] =
            "CDI_CONFIG_MAX96712_DISABLE_BACKTOP",
        [CDI_CONFIG_MAX96712_TRIGGER_DESKEW] =
            "CDI_CONFIG_MAX96712_TRIGGER_DESKEW",
        [CDI_CONFIG_MAX96712_CHECK_CSIPLL_LOCK] =
            "CDI_CONFIG_MAX96712_CHECK_CSIPLL_LOCK",
        [CDI_CONFIG_MAX96712_ENABLE_REPLICATION] =
            "CDI_CONFIG_MAX96712_ENABLE_REPLICATION",
        [CDI_CONFIG_MAX96712_DISABLE_REPLICATION] =
            "CDI_CONFIG_MAX96712_DISABLE_REPLICATION",
    };

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL)) {
        LOG_ERR("MAX96712: Bad parameter: Null ptr");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if ((enumeratedDeviceConfig == CDI_CONFIG_MAX96712_INVALID) ||
        (enumeratedDeviceConfig >= CDI_CONFIG_MAX96712_NUM)) {
        LOG_ERR("MAX96712: Bad parameter: Invalid command");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    LOG_DBG("MAX96712: %s", cmdString[enumeratedDeviceConfig]);
    switch (enumeratedDeviceConfig) {
        case CDI_CONFIG_MAX96712_SET_PG_1920x1236:
            status = ConfigPGSettings(handle);
            break;
        case CDI_CONFIG_MAX96712_ENABLE_PG:
            status = EnablePG(handle);
            break;
        case CDI_CONFIG_MAX96712_ENABLE_BACKTOP:
            status = EnableBackTop(handle,
                                   true);
            break;
        case CDI_CONFIG_MAX96712_DISABLE_BACKTOP:
            status = EnableBackTop(handle,
                                   false);
            break;
        case CDI_CONFIG_MAX96712_TRIGGER_DESKEW:
            status = TriggerDeskew(handle);
            break;
        case CDI_CONFIG_MAX96712_CHECK_CSIPLL_LOCK:
            status = CheckCSIPLLLock(handle);
            break;
        case CDI_CONFIG_MAX96712_ENABLE_REPLICATION:
            status = EnableReplication(handle, true);
            break;
        case CDI_CONFIG_MAX96712_DISABLE_REPLICATION:
            status = EnableReplication(handle, false);
            break;
        default:
            LOG_ERR("MAX96712: Bad parameter: Invalid command");
            status = NVMEDIA_STATUS_BAD_PARAMETER;
            break;
    }

    if (status != NVMEDIA_STATUS_OK) {
        LOG_ERR("MAX9295: %s failed", cmdString[enumeratedDeviceConfig]);
    }

    return status;
}

NvMediaStatus
MAX96712WriteRegister(
    DevBlkCDIDevice *handle,
    uint32_t deviceIndex,
    uint32_t registerNum,
    uint32_t dataLength,
    uint8_t *dataBuff)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t data[MAX96712_NUM_ADDR_BYTES + REG_WRITE_BUFFER_BYTES] = {};

    if ((handle == NULL) || (dataBuff == NULL)) {
        LOG_ERR("MAX96712: Bad parameter: Null ptr");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if (dataLength > REG_WRITE_BUFFER_BYTES) {
        LOG_ERR("MAX96712: Insufficient buffering");
        return NVMEDIA_STATUS_INSUFFICIENT_BUFFERING;
    }

    data[1] = registerNum & 0xFF;
    data[0] = (registerNum >> 8u) & 0xFF;
    memcpy(&data[2], dataBuff, dataLength);

    status = DevBlkCDIDeviceWrite(handle,
                             deviceIndex,
                             dataLength + MAX96712_NUM_ADDR_BYTES,
                             data);
    if (status != NVMEDIA_STATUS_OK) {
        LOG_ERR("MAX96712: Register 0x%04X I2C write failed with status %d", registerNum, status);
    }

    return status;
}

NvMediaStatus
MAX96712WriteParameters(
    DevBlkCDIDevice *handle,
    uint32_t parameterType,
    uint32_t parameterSize,
    void *parameter)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = NULL;
    WriteParametersParamMAX96712 *param = (WriteParametersParamMAX96712 *) parameter;
    bool isValidSize = false;
    static const char *cmdString[] = {
        [CDI_WRITE_PARAM_CMD_MAX96712_ENABLE_SPECIFIC_LINKS] =
            "CDI_WRITE_PARAM_CMD_MAX96712_ENABLE_SPECIFIC_LINKS",
        [CDI_WRITE_PARAM_CMD_MAX96712_DISABLE_FORWARD_CHANNELS] =
            "CDI_WRITE_PARAM_CMD_MAX96712_DISABLE_FORWARD_CHANNELS",
        [CDI_WRITE_PARAM_CMD_MAX96712_ENABLE_FORWARD_CHANNELS] =
            "CDI_WRITE_PARAM_CMD_MAX96712_ENABLE_FORWARD_CHANNELS",
        [CDI_WRITE_PARAM_CMD_MAX96712_ENABLE_PACKET_BASED_CONTROL_CHANNEL] =
            "CDI_WRITE_PARAM_CMD_MAX96712_ENABLE_PACKET_BASED_CONTROL_CHANNEL",
        [CDI_WRITE_PARAM_CMD_MAX96712_DISABLE_DE] =
            "CDI_WRITE_PARAM_CMD_MAX96712_DISABLE_DE",
        [CDI_WRITE_PARAM_CMD_MAX96712_SET_DEFAULT_GMSL1_HIM_ENABLED] =
            "CDI_WRITE_PARAM_CMD_MAX96712_SET_DEFAULT_GMSL1_HIM_ENABLED",
        [CDI_WRITE_PARAM_CMD_MAX96712_SET_DBL] =
            "CDI_WRITE_PARAM_CMD_MAX96712_SET_DBL",
        [CDI_WRITE_PARAM_CMD_MAX96712_SET_FSYNC] =
            "CDI_WRITE_PARAM_CMD_MAX96712_SET_FSYNC",
        [CDI_WRITE_PARAM_CMD_MAX96712_ENABLE_DOUBLE_PIXEL_MODE] =
            "CDI_WRITE_PARAM_CMD_MAX96712_ENABLE_DOUBLE_PIXEL_MODE",
        [CDI_WRITE_PARAM_CMD_MAX96712_PIPELINE_MAPPING] =
            "CDI_WRITE_PARAM_CMD_MAX96712_PIPELINE_MAPPING",
        [CDI_WRITE_PARAM_CMD_MAX96712_OVERRIDE_DATATYPE] =
            "CDI_WRITE_PARAM_CMD_MAX96712_OVERRIDE_DATATYPE",
        [CDI_WRITE_PARAM_CMD_MAX96712_SET_MIPI] =
            "CDI_WRITE_PARAM_CMD_MAX96712_SET_MIPI",
        [CDI_WRITE_PARAM_CMD_MAX96712_SET_TX_SRC_ID] =
            "CDI_WRITE_PARAM_CMD_MAX96712_SET_TX_SRC_ID",
        [CDI_WRITE_PARAM_CMD_MAX96712_UNSET_DBL] =
            "CDI_WRITE_PARAM_CMD_MAX96712_UNSET_DBL",
        [CDI_WRITE_PARAM_CMD_MAX96712_DISABLE_PACKET_BASED_CONTROL_CHANNEL] =
            "CDI_WRITE_PARAM_CMD_MAX96712_DISABLE_PACKET_BASED_CONTROL_CHANNEL",
        [CDI_WRITE_PARAM_CMD_MAX96712_SET_LINK_MODE] =
            "CDI_WRITE_PARAM_CMD_MAX96712_SET_LINK_MODE",
        [CDI_WRITE_PARAM_CMD_MAX96712_ENABLE_PERIODIC_AEQ] =
            "CDI_WRITE_PARAM_CMD_MAX96712_ENABLE_PERIODIC_AEQ",
        [CDI_WRITE_PARAM_CMD_MAX96712_DISABLE_AUTO_ACK] =
            "CDI_WRITE_PARAM_CMD_MAX96712_DISABLE_AUTO_ACK",
    };

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL) ||
        (parameter == NULL)) {
        LOG_ERR("MAX96712: Bad parameter: Null ptr");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if ((parameterType == CDI_WRITE_PARAM_CMD_MAX96712_INVALID) ||
        (parameterType >= CDI_WRITE_PARAM_CMD_MAX96712_NUM)) {
        LOG_ERR("MAX96712: Bad parameter: Invalid command");
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    LOG_DBG("MAX96712: %s", cmdString[parameterType]);
    switch (parameterType) {
        case CDI_WRITE_PARAM_CMD_MAX96712_PIPELINE_MAPPING:
            if (parameterSize == sizeof(param->PipelineMapping)) {
                isValidSize = true;
                status = SetPipelineMap(handle,
                                        param->PipelineMapping.link,
                                        param->PipelineMapping.linkPipelineMap);
            }
            break;
        case CDI_WRITE_PARAM_CMD_MAX96712_OVERRIDE_DATATYPE:
            if (parameterSize == sizeof(param->PipelineMapping)) {
                isValidSize = true;
                status = OverrideDataType(handle,
                                          param->PipelineMapping.link,
                                          param->PipelineMapping.linkPipelineMap);
            }
            break;
        case CDI_WRITE_PARAM_CMD_MAX96712_SET_FSYNC:
            if (parameterSize == sizeof(param->FSyncSettings)) {
                isValidSize = true;
                status = SetFSYNCMode(handle,
                                      param->FSyncSettings.FSyncMode,
                                      param->FSyncSettings.pclk,
                                      param->FSyncSettings.fps,
                                      param->FSyncSettings.link);
            }
            break;
        case CDI_WRITE_PARAM_CMD_MAX96712_ENABLE_SPECIFIC_LINKS:
            if (parameterSize == sizeof(param->link)) {
                isValidSize = true;
                status = EnableSpecificLinks(handle,
                                             param->link);
            }
            break;
        case CDI_WRITE_PARAM_CMD_MAX96712_DISABLE_FORWARD_CHANNELS:
            if (parameterSize == sizeof(param->link)) {
                isValidSize = true;
                status = ControlForwardChannels(handle,
                                                   param->link,
                                                   false);
            }
            break;
        case CDI_WRITE_PARAM_CMD_MAX96712_ENABLE_FORWARD_CHANNELS:
            if (parameterSize == sizeof(param->link)) {
                isValidSize = true;
                status = ControlForwardChannels(handle,
                                                   param->link,
                                                   true);
            }
            break;
        case CDI_WRITE_PARAM_CMD_MAX96712_ENABLE_PACKET_BASED_CONTROL_CHANNEL:
            if (parameterSize == sizeof(param->link)) {
                isValidSize = true;
                status = EnablePacketBasedControlChannel(handle,
                                                         param->link,
                                                         true);
            }
            break;
        case CDI_WRITE_PARAM_CMD_MAX96712_DISABLE_DE:
            if (parameterSize == sizeof(param->link)) {
                isValidSize = true;
                status = DisableDE(handle,
                                   param->link);
            }
            break;
        case CDI_WRITE_PARAM_CMD_MAX96712_SET_DEFAULT_GMSL1_HIM_ENABLED:
            if (parameterSize == sizeof(param->GMSL1HIMEnabled)) {
                isValidSize = true;
                status = SetDefaultGMSL1HIMEnabled(handle,
                                                   param->GMSL1HIMEnabled.link,
                                                   param->GMSL1HIMEnabled.step);
            }
            break;
        case CDI_WRITE_PARAM_CMD_MAX96712_SET_DBL:
            if (parameterSize == sizeof(param->link)) {
                isValidSize = true;
                status = SetDBL(handle,
                                param->link,
                                true);
            }
            break;
        case CDI_WRITE_PARAM_CMD_MAX96712_ENABLE_DOUBLE_PIXEL_MODE:
            if (parameterSize == sizeof(param->DoublePixelMode)) {
                isValidSize = true;
                status = EnableDoublePixelMode(handle,
                                               param->DoublePixelMode.link,
                                               param->DoublePixelMode.dataType,
                                               param->DoublePixelMode.embDataType);
            }
            break;
        case CDI_WRITE_PARAM_CMD_MAX96712_SET_MIPI:
            if (parameterSize == sizeof(param->MipiSettings)) {
                isValidSize = true;
                status = ConfigureMIPIOutput(handle,
                                             param->MipiSettings.mipiSpeed,
                                             param->MipiSettings.phyMode);
            }
            break;
        case CDI_WRITE_PARAM_CMD_MAX96712_SET_TX_SRC_ID:
            if (parameterSize == sizeof(param->link)) {
                isValidSize = true;
                status = SetTxSRCId(handle,
                                    param->link);
            }
            break;
        case CDI_WRITE_PARAM_CMD_MAX96712_UNSET_DBL:
            if (parameterSize == sizeof(param->link)) {
                isValidSize = true;
                status = SetDBL(handle,
                                param->link,
                                false);
            }
            break;
        case CDI_WRITE_PARAM_CMD_MAX96712_DISABLE_PACKET_BASED_CONTROL_CHANNEL:
            if (parameterSize == sizeof(param->link)) {
                isValidSize = true;
                status = EnablePacketBasedControlChannel(handle,
                                                         param->link,
                                                         false);
            }
            break;
        case CDI_WRITE_PARAM_CMD_MAX96712_SET_LINK_MODE:
            if (parameterSize == sizeof(param->link)) {
                isValidSize = true;
                status = SetLinkMode(handle,
                                     param->link);
            }
            break;
        case CDI_WRITE_PARAM_CMD_MAX96712_ENABLE_PERIODIC_AEQ:
            if (parameterSize == sizeof(param->link)) {
                isValidSize = true;
                status = EnablePeriodicAEQ(handle,
                                           param->link);
            }
            break;
        case CDI_WRITE_PARAM_CMD_MAX96712_DISABLE_AUTO_ACK:
            if (parameterSize == sizeof(param->link)) {
                isValidSize = true;
                status = DisableAutoAck(handle,
                                        param->link);
            }
            break;
        default:
            LOG_ERR("MAX96712: Bad parameter: Invalid command");
            isValidSize = true;
            status = NVMEDIA_STATUS_BAD_PARAMETER;
            break;
    }

    if (status != NVMEDIA_STATUS_OK) {
        LOG_ERR("MAX96712: %s failed", cmdString[parameterType]);
    }

    if (!isValidSize) {
        LOG_ERR("MAX96712: %s: Bad parameter: Invalid param size", cmdString[parameterType]);
        status = NVMEDIA_STATUS_BAD_PARAMETER;
    }

    return status;
}

LinkMAX96712
GetMAX96712Link(
    uint8_t linkNum)
{
    switch (linkNum) {
        case 0u:
            return CDI_MAX96712_LINK_0;
        case 1u:
            return CDI_MAX96712_LINK_1;
        case 2u:
            return CDI_MAX96712_LINK_2;
        case 3u:
            return CDI_MAX96712_LINK_3;
        default:
            return CDI_MAX96712_LINK_NONE;
    }
}

static DevBlkCDIDeviceDriver deviceDriver = {
    .deviceName = "Maxim 96712 Deserializer",
    .regLength = MAX96712_NUM_ADDR_BYTES,
    .dataLength = MAX96712_NUM_DATA_BYTES,
    .DriverCreate = DriverCreate,
    .DriverDestroy = DriverDestroy,
};

DevBlkCDIDeviceDriver *
GetMAX96712NewDriver(
    void)
{
    return &deviceDriver;
}
