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

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include "log_utils.h"
#include "nvmedia_isc.h"
#include "isc_max9286.h"
#include "os_common.h"

#define REGISTER_ADDRESS_BYTES  1
#define REG_WRITE_BUFFER        32
#define MAX_NUM_GMSL_LINKS      4

#if !defined(__INTEGRITY)
#define MIN(a,b)            (((a) < (b)) ? (a) : (b))
#endif
#define GET_SIZE(x)         sizeof(x)
#define GET_BLOCK_LENGTH(x) x[0]
#define GET_BLOCK_DATA(x)   &x[1]
#define SET_NEXT_BLOCK(x)   x += (x[0] + 1)

// each 4bit 0 or 1 --> 1bit 0 or 1. eg. 0x1101 --> 0xD
#define CAMMAP_4BITSTO_1BIT(a) \
               ((a & 0x01) + ((a >> 3) & 0x02) + ((a >> 6) & 0x04) + ((a >> 9) & 0x08))
// each 4bit 0/1/2/3 --> 2bit 0/1/2/3. eg. 0x3210 --> 0xE4
#define CAMMAP_4BITSTO_2BITS(a) \
               ((a & 0x03) + ((a >> 2) & 0x0c) + ((a >> 4) & 0x30) + ((a >> 6) & 0xc0))

const uint8_t max9286_defaults[] = {
    2, 0x0A, 0xF0,  // Enable the forward control channels to make sure if they're disabled before starting a camera app
    2, 0x69, 0x30,  // Enable auto comeback and auto mask
};

const uint8_t max9286_enable_link_0[] = {
    2, 0x00, 0xE1  // Enable link 0
};

const uint8_t max9286_enable_links_01[] = {
    2, 0x00, 0xE3  // Enable links 0 and 1
};

const uint8_t max9286_enable_links_012[] = {
    2, 0x00, 0xE7  // Enable links 0, 1 and 2
};

const uint8_t max9286_enable_links_0123[] = {
    2, 0x00, 0xEF  // Enable all
};

const uint8_t max9286_disable_auto_ack[] = {
    2, 0x34, 0x36,  // Disable auto-ack
};

const uint8_t max9286_reverse_channel_reset[] = {
    2, 0x3f, 0x62, // Enable to change
    2, 0x3b, 0x24, // Set amplitude first
};

const uint8_t max9286_enable_auto_ack[] = {
    2, 0x34, 0xB6   // Reenable auto-ack
};

const uint8_t max9286_set_csi_timing[] = {
    2, 0x19, 0xA3  // Bug 1608443 for fixing CSI TEOT timing failure
};

const uint8_t max9286_swap_data_lanes[] = {
    2, 0x14, 0xE1,  // Swap data lanes D0 and D1 for e2580 platform
};

const uint8_t max9286_reverse_channel_enable[] = {
    2, 0x3f, 0x4F,   // Enable Rev channel;
};

const uint8_t max9286_reverse_channel_ampl_l[] = {
    2, 0x3b, 0x1E,   // Rev channel pulse length;
};

const uint8_t max9286_reverse_channel_ampl_h[] = {
    2, 0x3b, 0x19,  // Increase Rev channel amplitude to 170mv;
};

const uint8_t max9286_disable_ovlp_window[] = {
    2, 0x63, 0x00,
    2, 0x64, 0x00,
};

const uint8_t max9286_enable_hibw[] = {
    2, 0x1C, 0x06,  // HIBW =1
};

typedef struct {
    ContextMAX9286 ctx;
} _DriverHandle;

static uint8_t
max9286ErrRegAddress[ISC_MAX9286_NUM_ERR_REG] = {
    MAX9286_REG_DATA_OK,
    MAX9286_REG_VIDEO_LINK,
    MAX9286_REG_VSYNC_DET,
    MAX9286_REG_LINE_BUF_ERR,
    MAX9286_REG_FSYNC_LOCK,
};

static NvMediaStatus
EnableLinks(
    NvMediaISCDevice *handle,
    uint32_t links,
    NvMediaBool enable)
{
    uint8_t data[2];
    NvMediaStatus status;
    _DriverHandle *drvHandle = NULL;

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

    // Only 4 bits are used to represent 4 links
    if(links > 0xF)
        return NVMEDIA_STATUS_BAD_PARAMETER;

    // Invert bits if disabling links
    if(!enable)
        links = ~links;


    data[0] = MAX9286_REG_LINK_EN; // Register address

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

    // Clear last 4 bits
    data[1] &= 0xF0;

    // Set last 4 bits
    data[1] |=  links & 0xF;

    status = NvMediaISCDeviceWrite(handle, 0, // transaction
        2,           // dataLength
        data);       // data
    if(status != NVMEDIA_STATUS_OK)
        return status;

    // Update context with latest status of links
    drvHandle->ctx.gmslLinks = data[1] & 0x0F;

    return NVMEDIA_STATUS_OK;
}

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

    if(!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;

    drvHandle = calloc(1, sizeof(_DriverHandle));
    if(!drvHandle)
        return NVMEDIA_STATUS_OUT_OF_MEMORY;


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

    handle->deviceDriverHandle = (void *)drvHandle;

    return NVMEDIA_STATUS_OK;
}

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

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

    return NVMEDIA_STATUS_OK;
}

NvMediaStatus
MAX9286CheckPresence(
    NvMediaISCDevice *handle)
{
    uint8_t data[2] = {0};
    NvMediaStatus status;
    _DriverHandle *drvHandle = NULL;

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

    // Read the chip revision number
    data[0] = MAX9286_REG_CHIP_REVISION;

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

    drvHandle->ctx.revision = data[1] & MAX9286_CHIP_REVISION_MASK;

    if(drvHandle->ctx.disableBackChannelCtrl) {
        if(drvHandle->ctx.revision != MAX9286_CHIP_REVISION_8 &&
           drvHandle->ctx.revision != MAX9286_CHIP_REVISION_9) {
            return NVMEDIA_STATUS_NOT_SUPPORTED;
        } else {
            return NVMEDIA_STATUS_OK;
        }
    } else {
        return NVMEDIA_STATUS_OK;
    }
}

NvMediaStatus
MAX9286CheckLink(
    NvMediaISCDevice *handle,
    uint32_t instanceNumber,
    uint32_t linkType)
{
    uint8_t address;
    NvMediaStatus status;
    uint8_t data = 0;

    if(!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;
    if(instanceNumber > 4)
        return NVMEDIA_STATUS_BAD_PARAMETER;


    address = MAX9286_REG_CONFIG_LINK_DETECT;

    status = NvMediaISCDeviceRead(handle, 0,
        1,
        &address,
        1,
        &data);

    if(status != NVMEDIA_STATUS_OK)
        return status;

    switch (linkType) {
        case ISC_MAX9286_CONFIG_LINK:
            if(!((data >> (4 + instanceNumber)) & 1)) {
                status = NVMEDIA_STATUS_ERROR;
            } else {
                status = NVMEDIA_STATUS_OK;
            }
            break;
        case ISC_MAX9286_VIDEO_LINK:
            if(!((data >> instanceNumber) & 1)) {
                status = NVMEDIA_STATUS_ERROR;
            } else {
                status = NVMEDIA_STATUS_OK;
            }
            break;
        default:
            status = NVMEDIA_STATUS_NOT_SUPPORTED;
            break;
    }

    return status;
}

static NvMediaStatus
SetIgnoreError(
        NvMediaISCDevice *handle,
        bool enable
)
{
    uint8_t data[2];
    NvMediaStatus status;

    if(!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;

    if(enable) {
        /* set err threshold to be maximum */
        data[0] = 0x10;
        data[1] = 0xff;
        status = NvMediaISCDeviceWrite(handle, 0,                       // transaction
            2,                                 // dataLength
            &data[0]);                         // data
        if(status != NVMEDIA_STATUS_OK)
            return status;

        /* set error reset automatically */
        data[0] = 0x0f;
        data[1] = 0x2b;
        status = NvMediaISCDeviceWrite(handle, 0,                       // transaction
            2,                                 // dataLength
            &data[0]);                         // data
        if(status != NVMEDIA_STATUS_OK)
            return status;

    } else { /* unmask error */
        /* read operation resets sync error count before enabling error */
        data[0] = 0x5e;
        status = NvMediaISCDeviceRead(handle, 0,            // transaction
            REGISTER_ADDRESS_BYTES, // regLength
            data,                   // regData
            1,                      // dataLength
            &data[1]);              // data
        if(status != NVMEDIA_STATUS_OK)
            return status;

        /* disable automatically clearing error */
        data[0] = 0x0f;
        data[1] = 0x0b;
        status = NvMediaISCDeviceWrite(handle, 0,                       // transaction
            2,                                 // dataLength
            &data[0]);                         // data
        if(status != NVMEDIA_STATUS_OK)
            return status;

        /* set error threshold to be 0 to detect any error from MAX9286 */
        data[0] = 0x10;
        data[1] = 0x00;
        status = NvMediaISCDeviceWrite(handle, 0,                       // transaction
            2,                                 // dataLength
            &data[0]);                         // data
        if(status != NVMEDIA_STATUS_OK)
            return status;
    }

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
SetPixelInfo(
    NvMediaISCDevice *handle,
    ConfigurePixelInfoMAX9286 *pixelInfo)
{
    NvMediaStatus status;
    uint8_t pixel_info[3] = {
        2, 0x12, 0xf7  // 4-lane, YUV422 8-bit, double pixel per word
    };

    if(!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;


    pixel_info[2] =  (uint8_t)pixelInfo->byte;

    status = NvMediaISCDeviceWrite(handle, 0,                  // transaction
        pixel_info[0],    // dataLength
        &pixel_info[1]);  // data

    if(status != NVMEDIA_STATUS_OK)
            return status;

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
SetSyncMode(
    NvMediaISCDevice *handle,
    SetFsyncModeMAX9286 mode,
    int32_t margin)
{
    NvMediaStatus status;
    uint8_t set_val[4];
    uint8_t max9286_set_sync_mode[] = {
        2, 0x01, 0x21,  // FSYNC settings
    };

    if(!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;


    switch (mode) {
    case ISC_SET_FSYNC_MAX9286_FSYNC_MANUAL:
        max9286_set_sync_mode[2] = 0x00;
        status = NvMediaISCDeviceWrite(handle, 0,                    // transaction
           max9286_set_sync_mode[0],       // dataLength
           &max9286_set_sync_mode[1]);     // data
        if(status != NVMEDIA_STATUS_OK)
            return status;

        //set number of pclk of fsync
        set_val[0] = MAX9286_REG_FSYNC_PERIOD;
        set_val[1] = margin & 0xff;
        set_val[2] = (margin >> 8) & 0xff;
        set_val[3] = (margin >> 16) & 0xff;
        status = NvMediaISCDeviceWrite(handle, 0,         // transaction
            4,                   // dataLength
            &set_val[0]);        // data
        if(status != NVMEDIA_STATUS_OK)
            return status;

        return NVMEDIA_STATUS_OK;
    case ISC_SET_FSYNC_MAX9286_FSYNC_AUTO:
        mode = ISC_SET_FSYNC_MAX9286_INTERNAL_PIN_HIGH_Z;
        max9286_set_sync_mode[2] &= 0x3c;
        max9286_set_sync_mode[2] |= ((mode << 6) & 0xc0) | 0x2;

        set_val[0] = MAX9286_REG_SET_SYNC_MARGIN;
        set_val[1] = 1; /*default margin */
        status = NvMediaISCDeviceWrite(handle, 0,      // transaction
            2,                // dataLength
            &set_val[0]);     // data
        if(status != NVMEDIA_STATUS_OK)
            return status;

        status = NvMediaISCDeviceWrite(handle, 0,                 // transaction
            max9286_set_sync_mode[0],    // dataLength
            &max9286_set_sync_mode[1]);  // data
        if(status != NVMEDIA_STATUS_OK)
            return status;

        return status;
    case ISC_SET_FSYNC_MAX9286_FSYNC_SEMI_AUTO:
        mode = ISC_SET_FSYNC_MAX9286_INTERNAL_PIN_HIGH_Z;
        max9286_set_sync_mode[2] &= 0x3c;
        max9286_set_sync_mode[2] |= ((mode << 6) & 0xc0) | 0x1;
        set_val[0] = MAX9286_REG_SET_SYNC_MARGIN;
        set_val[1] = (margin > 0)? (uint8_t)(margin & 0xf) :
                                   (uint8_t)((1 << 4) | (margin & 0xf));
        status = NvMediaISCDeviceWrite(handle, 0,      // transaction
            2,                // dataLength
            &set_val[0]);     // data
        if(status != NVMEDIA_STATUS_OK)
            return status;
        status = NvMediaISCDeviceWrite(handle, 0,                 // transaction
            max9286_set_sync_mode[0],    // dataLength
            &max9286_set_sync_mode[1]);  // data

        if(status != NVMEDIA_STATUS_OK)
            return status;
        return status;
    case ISC_SET_FSYNC_MAX9286_DISABLE_SYNC:
        max9286_set_sync_mode[2] = 0xc0;
        set_val[0] = MAX9286_REG_SET_SYNC_MARGIN;
        set_val[1] = 1; /* default margin */
        status = NvMediaISCDeviceWrite(handle, 0,      // transaction
            2,                // dataLength
            &set_val[0]);     // data

        if(status != NVMEDIA_STATUS_OK)
            return status;
        return NvMediaISCDeviceWrite(handle, 0,                 // transaction
            max9286_set_sync_mode[0],    // dataLength
            &max9286_set_sync_mode[1]);  // data
    case ISC_SET_FSYNC_MAX9286_EXTERNAL_FROM_ECU:
        mode = ISC_SET_FSYNC_MAX9286_EXTERNAL_FROM_ECU;
        max9286_set_sync_mode[2] &= 0x3c;
        max9286_set_sync_mode[2] |= ((mode << 6) & 0xc0);
        set_val[0] = MAX9286_REG_SET_SYNC_MARGIN;
        set_val[1] = (margin > 0)? (uint8_t)(margin & 0xf) :
                                   (uint8_t)((1 << 4) | (margin & 0xf));
        status = NvMediaISCDeviceWrite(handle, 0,      // transaction
            2,                // dataLength
            &set_val[0]);     // data
        if(status != NVMEDIA_STATUS_OK)
            return status;

        return NvMediaISCDeviceWrite(handle, 0,                 // transaction
            max9286_set_sync_mode[0],    // dataLength
            &max9286_set_sync_mode[1]);  // data
    default:
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }
}

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

    if(!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;


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

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

    return NVMEDIA_STATUS_OK;
}

NvMediaStatus
MAX9286SetDefaults(
    NvMediaISCDevice *handle)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t offset[1];
    uint8_t dataBuff[1];

    if(!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;


    offset[0] = 0x00;

    status = NvMediaISCDeviceRead(handle, 0,    // transaction
        REGISTER_ADDRESS_BYTES, // regLength
        offset,   // regData
        1,     // dataLength
        dataBuff);      // data

    if(status != NVMEDIA_STATUS_OK)
        return status;

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

    nvsleep(3000);

    return status;
}

static NvMediaStatus
SetReverseChannelAmplitude(
        NvMediaISCDevice *handle,
        uint32_t amplitude,
        uint32_t transitionTime)
{
    uint8_t rev_amp = 0;
    uint8_t rev_trf = 0;
    uint8_t reverse_channel_setting[] = {
        2, 0x3b, 0x1b, // Set amplitude first
        2, 0x3f, 0x62, // Enable to change
    };

    if(amplitude > MAX9286_MAX_REVERSE_CHANNEL_AMP)
        return NVMEDIA_STATUS_NOT_SUPPORTED;

    if((amplitude > MAX9286_REVERSE_CHANNEL_BOOT_AMP) &&
        (amplitude <= MAX9286_MAX_REVERSE_CHANNEL_AMP)) {
        rev_amp = (((amplitude - (MAX9286_REVERSE_CHANNEL_BOOT_AMP +
                                     MAX9286_MIN_REVERSE_CHANNEL_AMP)) /
                       MAX9286_REVERSE_CHANNEL_STEP_SIZE) << MAX9286_REVERSE_CHANNEL_AMP_REG_SHIFT) |
                   (1 << MAX9286_REVERSE_CHANNEL_AMP_BOOST_REG_SHIFT);
        reverse_channel_setting[2] &= 0xf0;
        reverse_channel_setting[2] |= (rev_amp << 0);

    } else if(amplitude >= 30) {
        rev_amp = (((amplitude - MAX9286_MIN_REVERSE_CHANNEL_AMP) /
                       MAX9286_REVERSE_CHANNEL_STEP_SIZE) << MAX9286_REVERSE_CHANNEL_AMP_REG_SHIFT);
        reverse_channel_setting[2] &= 0xf0;
        reverse_channel_setting[2] |= (rev_amp << 0);
    }

    if((transitionTime >= 100) && (transitionTime <= 400)) {
        rev_trf = (transitionTime / 100) - 1;
        reverse_channel_setting[2] &= 0xcf;
        reverse_channel_setting[2] |= (rev_trf << 4);
    }

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

static NvMediaStatus
EnableControlChannels(
        NvMediaISCDevice *handle,
        NvMediaBool enable)
{
    uint8_t max9286_enable_ctrl_channel[2] = {
        0x0a, 0xff,   // Enable forward & reverse control channels
    };
    NvMediaStatus status;

    if(!handle) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }


    if(!enable) {
        max9286_enable_ctrl_channel[1] = 0x00;
    }

    status = NvMediaISCDeviceWrite(handle, 0,
        2,
        max9286_enable_ctrl_channel);
    if(status != NVMEDIA_STATUS_OK) {
        return status;
    }

    // 3ms needed by MAX9286 to lock the link
    nvsleep(3000);

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
ControlReverseChannel(
        NvMediaISCDevice *handle,
        uint32_t id,
        bool enable,
        bool individualControl)
{
    uint8_t data[2];
    NvMediaStatus status;

    if(!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;
    if(id > 4)
        return NVMEDIA_STATUS_BAD_PARAMETER;


    data[0] = MAX9286_REG_CONTROL_CHANNEL;

    status = NvMediaISCDeviceRead(handle, 0,
        1,
        &data[0],
        1,
        &data[1]);

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

    if(individualControl && enable)
        data[1] |= (1 << id);
    else if(individualControl && !enable)
        data[1] &= ~(1 << id);
    else if(!individualControl && enable)
        data[1] |= 0x0f;
    else
        data[1] &= 0xf0;

    return NvMediaISCDeviceWrite(handle, 0,
        2,
        data);
}

static NvMediaStatus
PowerupReverseChannelTransmitter(
        NvMediaISCDevice *handle,
        NvMediaBool powerup)
{
    NvMediaStatus status;
    uint8_t max9286_power_up_reverse_channel_transmitter[2] = {
        0x43, 0xb0,    // power up reverse channel transmitter
    };
    _DriverHandle *drvHandle = NULL;

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

    // The new aggregator should power up the reverse channel transmitter before
    // programming the serializer and the camera sensor
    if(drvHandle->ctx.revision == MAX9286_CHIP_REVISION_8 ||
       drvHandle->ctx.revision == MAX9286_CHIP_REVISION_9) {

        // Invert a bit if powering down reverse channel transmitter
        if(!powerup) {
            max9286_power_up_reverse_channel_transmitter[1] &= ~MAX9286_REVERSE_CHANNEL_TRANSMITTER;
        }

        status = NvMediaISCDeviceWrite(handle, 0, // transaction
            2,           // dataLength
            max9286_power_up_reverse_channel_transmitter);
        if(status != NVMEDIA_STATUS_OK) {
            return status;
        }

        // Wait for reverse channel transmitter stable
        nvsleep(2000);
    } else if(drvHandle->ctx.revision == 0) {
        LOG_ERR("%s: CheckPresence() should be called before this function called\n", __func__);
        return NVMEDIA_STATUS_ERROR;
    } else {
        return NVMEDIA_STATUS_OK;
    }

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
CameraMapping(
    NvMediaISCDevice *handle,
    CamMap *camMap,
    NvMediaBool enable)
{
    uint8_t data[2];
    NvMediaStatus status;

    if(!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;


    if(enable) {
        // set video link enable
        status = EnableLinks (handle, CAMMAP_4BITSTO_1BIT(camMap->enable), NVMEDIA_TRUE);
        if(status != NVMEDIA_STATUS_OK)
            return status;

        // set video link mask
        data[0] = MAX9286_REG_MASK_LINK; // Register address
        status = NvMediaISCDeviceRead(handle, 0,            // transaction
            REGISTER_ADDRESS_BYTES, // regLength
            data,                   // regData
            1,                      // dataLength
            &data[1]);              // data
        if(status != NVMEDIA_STATUS_OK)
            return status;

        data[1] &= 0xC0; // disable auto mask and auto comeback
        data[1] |= CAMMAP_4BITSTO_1BIT(camMap->mask);
        status = NvMediaISCDeviceWrite(handle, 0, // transaction
            2,           // dataLength
            data);       // data
        if(status != NVMEDIA_STATUS_OK)
            return status;

        // set csi out mapping
        data[0] = MAX9286_REG_LINK_OUT_ORDER; // Register addres
        data[1] = CAMMAP_4BITSTO_2BITS(camMap->csiOut);
        status = NvMediaISCDeviceWrite(handle, 0, // transaction
            2,           // dataLength
            data);       // data
    } else {
        // clear video link mask
        data[0] = MAX9286_REG_MASK_LINK;
        data[1] = 0;
        status = NvMediaISCDeviceWrite(handle, 0, // transaction
            2,           // dataLength
            data);       // data
        if(status != NVMEDIA_STATUS_OK)
            return status;
        // clear link order
        data[0] = MAX9286_REG_LINK_OUT_ORDER;
        data[1] = 0xe4; /* default value */
        status = NvMediaISCDeviceWrite(handle, 0, // transaction
            2,           // dataLength
            data);       // data
    }

    return status;
}

static NvMediaStatus
EnableVirtualChannels(
    NvMediaISCDevice *handle)
{
    uint8_t data[2];
    NvMediaStatus status;
    uint8_t max9286_enable_vc[2] = {
        0x15, 0x90    // Select W x N*H for combined camera line format for CSI2 output & set VC according to the link number
    };

    if(!handle)
        return NVMEDIA_STATUS_BAD_PARAMETER;


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

    // Enable virtual channels
    max9286_enable_vc[1] |= (data[1] & 0x0F);

    return NvMediaISCDeviceWrite(handle, 0,
        2,
        &max9286_enable_vc[0]);
}

static NvMediaStatus
EnableCSI(
    NvMediaISCDevice *handle,
    bool enable)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    uint8_t data[2];
    NvMediaBool enSwapBitOrder;
    uint8_t max9286_enable_csi_out[] = {
        2, 0x15, 0x0B   // Enable CSI out
    };
    _DriverHandle *drvHandle = NULL;

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

    enSwapBitOrder = drvHandle->ctx.enSwapBitOrder;

    if(enable) {
        /* csi_timing doesn't need to be programmed for disabling CSI */
        status = NvMediaISCDeviceWrite(handle, 0,
            2,
            &max9286_set_csi_timing[1]);
        if(status != NVMEDIA_STATUS_OK)
            return status;

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

        max9286_enable_csi_out[2] |= (data[1] & 0xF0);
        max9286_enable_csi_out[2] |= (1 << 3) | (enSwapBitOrder << 2);

        status = NvMediaISCDeviceWrite(handle, 0,
            2,
            &max9286_enable_csi_out[1]);

        if(status != NVMEDIA_STATUS_OK)
            return status;

        /* revert ignoring error */
        status = SetIgnoreError(
            handle,
            false);
    } else {
        /* disable csi out */
        data[0] = max9286_enable_csi_out[1]; // Register address
        status = NvMediaISCDeviceRead(handle, 0,            // transaction
            REGISTER_ADDRESS_BYTES, // regLength
            data,                   // regData
            1,                      // dataLength
            &data[1]);              // data
        if(status != NVMEDIA_STATUS_OK)
            return status;

        max9286_enable_csi_out[2] |= (data[1] & 0xF0);
        max9286_enable_csi_out[2] &= ~(1 << 3);

        status = NvMediaISCDeviceWrite(handle, 0,
            2,
            &max9286_enable_csi_out[1]);

        if(status != NVMEDIA_STATUS_OK)
            return status;

        /* enable ignoring error while csi out is off */
        status = SetIgnoreError(
            handle,
            true);
    }

    return status;
}

NvMediaStatus
MAX9286SetDeviceConfig(
        NvMediaISCDevice *handle,
        uint32_t enumeratedDeviceConfig)
{
    _DriverHandle *drvHandle = NULL;

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

    switch(enumeratedDeviceConfig) {
        case ISC_CONFIG_MAX9286_DEFAULT:
            return WriteArray(
                handle,
                GET_SIZE(max9286_defaults),
                max9286_defaults);
        case ISC_CONFIG_MAX9286_DISABLE_AUTO_ACK:
            return WriteArray(
                handle,
                GET_SIZE(max9286_disable_auto_ack),
                max9286_disable_auto_ack);
        case ISC_CONFIG_MAX9286_ENABLE_AUTO_ACK:
            return WriteArray(
                handle,
                GET_SIZE(max9286_enable_auto_ack),
                max9286_enable_auto_ack);
        case ISC_CONFIG_MAX9286_ENABLE_CSI_OUT:
            return EnableCSI(handle,
                true);
        case ISC_CONFIG_MAX9286_ENABLE_LINK_0:
            return EnableLinks(
                handle,
                MAX9286_GMSL_LINK(0),
                NVMEDIA_TRUE);
        case ISC_CONFIG_MAX9286_ENABLE_LINKS_01:
            return EnableLinks(
                handle,
                MAX9286_GMSL_LINK(0) | MAX9286_GMSL_LINK(1),
                NVMEDIA_TRUE);
        case ISC_CONFIG_MAX9286_ENABLE_LINKS_012:
            return EnableLinks(
                handle,
                MAX9286_GMSL_LINK(0) | MAX9286_GMSL_LINK(1) |
                MAX9286_GMSL_LINK(2),
                NVMEDIA_TRUE);
        case ISC_CONFIG_MAX9286_ENABLE_LINKS_0123:
            return EnableLinks(
                handle,
                MAX9286_GMSL_LINK(0) | MAX9286_GMSL_LINK(1) |
                MAX9286_GMSL_LINK(2) | MAX9286_GMSL_LINK(3),
                NVMEDIA_TRUE);
        case ISC_CONFIG_MAX9286_REVERSE_CHANNEL_SETTING:
            return SetReverseChannelAmplitude(
                handle,
                drvHandle->ctx.reverseChannelAmp,
                drvHandle->ctx.reverseChannelTrf);
        case ISC_CONFIG_MAX9286_ENABLE_REVERSE_CHANNEL_0123:
            return ControlReverseChannel(
                handle,
                0,
                true,
                false);
        case ISC_CONFIG_MAX9286_DISABLE_REVERSE_CHANNEL_0123:
            return ControlReverseChannel(
                handle,
                0,
                false,
                false);
        case ISC_CONFIG_MAX9286_DISABLE_ALL_CONTROL_CHANNEL:
            return EnableControlChannels(
                handle,
                NVMEDIA_FALSE);
        case ISC_CONFIG_MAX9286_REVERSE_CHANNEL_RESET:
            return WriteArray(
                handle,
                GET_SIZE(max9286_reverse_channel_reset),
                max9286_reverse_channel_reset);
        case ISC_CONFIG_MAX9286_POWER_UP_REVERSE_CHANNEL_TRANSMITTER:
            return PowerupReverseChannelTransmitter(
                handle,
                NVMEDIA_TRUE);
        case ISC_CONFIG_MAX9286_SWAP_DATA_LANES:
            return WriteArray(
                handle,
                GET_SIZE(max9286_swap_data_lanes),
                max9286_swap_data_lanes);
        case ISC_CONFIG_MAX9286_CAMERA_MAPPING:
            return CameraMapping(
                handle,
                &drvHandle->ctx.camMap, NVMEDIA_TRUE);
        case ISC_CONFIG_MAX9286_SWAP_12BIT_ORDER:
            drvHandle->ctx.enSwapBitOrder = NVMEDIA_TRUE;
            return NVMEDIA_STATUS_OK;
        case ISC_CONFIG_MAX9286_REVERSE_CHANNEL_ENABLE:
            return WriteArray(
                handle,
                GET_SIZE(max9286_reverse_channel_enable),
                max9286_reverse_channel_enable);
        case ISC_CONFIG_MAX9286_REVERSE_CHANNEL_AMPL_L:
            return WriteArray(
                handle,
                GET_SIZE(max9286_reverse_channel_ampl_l),
                max9286_reverse_channel_ampl_l);
        case ISC_CONFIG_MAX9286_REVERSE_CHANNEL_AMPL_H:
            return WriteArray(
                handle,
                GET_SIZE(max9286_reverse_channel_ampl_h),
                max9286_reverse_channel_ampl_h);
        case ISC_CONFIG_MAX9286_DISABLE_CSI_OUT:
            return EnableCSI(handle,
                false);
        case ISC_CONFIG_MAX9286_DISABLE_OVLP_WINDOW:
            return WriteArray(
                handle,
                GET_SIZE(max9286_disable_ovlp_window),
                max9286_disable_ovlp_window);
        case ISC_CONFIG_MAX9286_ENABLE_HIBW:
            /* enable to ignore error, changing bws value causes error */
            if(SetIgnoreError(
                handle,
                true) != NVMEDIA_STATUS_OK)
                return NVMEDIA_STATUS_ERROR;
            /* set bws */
            if(NvMediaISCDeviceWrite(handle, 0,
                max9286_enable_hibw[0],
                &max9286_enable_hibw[1]) != NVMEDIA_STATUS_OK)
                return NVMEDIA_STATUS_ERROR;
            /* wait for lock again after changing bws */
            nvsleep(6000);
            /* revert ingnoring error */
            if(SetIgnoreError(
                handle,
                false) != NVMEDIA_STATUS_OK)
                return NVMEDIA_STATUS_ERROR;
            return NVMEDIA_STATUS_OK;
        case ISC_CONFIG_MAX9286_ENABLE_VC:
            return EnableVirtualChannels(
                handle);
        default:
            break;
    }

    return NVMEDIA_STATUS_OK;
}

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

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


    registerData[0] = registerNum & 0xFF;
    status = NvMediaISCDeviceRead(handle,    // transaction
        deviceIndex,    // device index
        REGISTER_ADDRESS_BYTES, // regLength
        registerData,   // regData
        dataLength,     // dataLength
        dataBuff);      // data

    return status;
}

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

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


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

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

    return status;
}

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

    if (!param)
        return NVMEDIA_STATUS_BAD_PARAMETER;

    switch(parameterType) {
        case ISC_WRITE_PARAM_CMD_MAX9286_ENABLE_REVERSE_CHANNEL:
            if(parameterSize != sizeof(param->EnableReverseChannel))
                return NVMEDIA_STATUS_BAD_PARAMETER;
            return ControlReverseChannel(
                handle,
                param->EnableReverseChannel.id,
                true,
                true);
        case ISC_WRITE_PARAM_CMD_MAX9286_DISABLE_REVERSE_CHANNEL:
            if(parameterSize != sizeof(param->DisableReverseChannel))
                return NVMEDIA_STATUS_BAD_PARAMETER;
            return ControlReverseChannel(
                handle,
                param->DisableReverseChannel.id,
                false,
                true);
        case ISC_WRITE_PARAM_CMD_MAX9286_SET_PIXEL_INFO:
            if(parameterSize != sizeof(param->SetPixelInfo))
                return NVMEDIA_STATUS_BAD_PARAMETER;
            return SetPixelInfo(
                handle,
                param->SetPixelInfo.pixelInfo);
        case ISC_WRITE_PARAM_CMD_MAX9286_SET_SYNC_MODE:
            if(parameterSize != sizeof(param->SetFsyncMode))
                return NVMEDIA_STATUS_BAD_PARAMETER;
            return SetSyncMode(
                handle,
                param->SetFsyncMode.syncMode,
                param->SetFsyncMode.k_val);
        default:
            break;
    }

    return NVMEDIA_STATUS_OK;
}

NvMediaStatus
MAX9286DumpRegisters(
    NvMediaISCDevice *handle)
{
    return NVMEDIA_STATUS_NOT_SUPPORTED;
}

NvMediaStatus
MAX9286GetErrorStatus (
    NvMediaISCDevice *handle,
    uint32_t parameterSize,
    void *parameter)
{
    NvMediaStatus status;
    uint8_t regValues[ISC_MAX9286_NUM_ERR_REG] = {0};
    uint8_t gmslLinks;
    uint32_t i;
    ErrorStatusMAX9286 *errorStatus;
    _DriverHandle *drvHandle = NULL;

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

    if(parameterSize != sizeof(ErrorStatusMAX9286))
        return NVMEDIA_STATUS_BAD_PARAMETER;

    errorStatus = (ErrorStatusMAX9286 *)parameter;
    gmslLinks = drvHandle->ctx.gmslLinks;

    memset(errorStatus, 0, sizeof(ErrorStatusMAX9286));

    for(i = 0; i < ISC_MAX9286_NUM_ERR_REG; i++) {
        status = NvMediaISCDeviceRead(handle, 0,
            1,
            &max9286ErrRegAddress[i],
            1,
            &regValues[i]);
        if(status != NVMEDIA_STATUS_OK) {
            return status;
        }
    }

    // Check for data activity
    for(i = 0; i < MAX_NUM_GMSL_LINKS; i++) {
        if(MAX9286_GMSL_LINK(i) & gmslLinks) {
            if(!(regValues[ISC_MAX9286_REG_DATA_OK] & MAX9286_GMSL_LINK(i))) {
                errorStatus->link |= MAX9286_GMSL_LINK(i);
                errorStatus->failureType = ISC_MAX9286_NO_DATA_ACTIVITY;
            }
        }
    }
    if(errorStatus->failureType != ISC_MAX9286_NO_ERROR)
        goto done;

    // Check for video link failure
    for(i = 0; i < MAX_NUM_GMSL_LINKS; i++) {
        if(MAX9286_GMSL_LINK(i) & gmslLinks) {
            if(!(regValues[ISC_MAX9286_REG_VIDEO_LINK] & MAX9286_GMSL_LINK(i))) {
                errorStatus->link |= MAX9286_GMSL_LINK(i);
                errorStatus->failureType = ISC_MAX9286_VIDEO_LINK_ERROR;
            }
        }
    }
    if(errorStatus->failureType != ISC_MAX9286_NO_ERROR)
        goto done;

    // Check for vsync detection
    for(i = 0; i < MAX_NUM_GMSL_LINKS; i++) {
        if(MAX9286_GMSL_LINK(i) & gmslLinks) {
            if(!(regValues[ISC_MAX9286_REG_VSYNC_DET] & MAX9286_GMSL_LINK(i))) {
                errorStatus->link |= MAX9286_GMSL_LINK(i);
                errorStatus->failureType = ISC_MAX9286_VSYNC_DETECT_FAILURE;
            }
        }
    }
    if(errorStatus->failureType != ISC_MAX9286_NO_ERROR)
        goto done;

    // Check for FSYNC loss of lock
    if(regValues[ISC_MAX9286_REG_FSYNC_LOCK] & MAX9286_FSYNC_LOSS_OF_LOCK) {
        errorStatus->failureType = ISC_MAX9286_FSYNC_LOSS_OF_LOCK;
        for(i = 0; i < MAX_NUM_GMSL_LINKS; i++) {
            if(MAX9286_GMSL_LINK(i) & gmslLinks) {
                errorStatus->link |= MAX9286_GMSL_LINK(i);
            }
        }
    }
    if(errorStatus->failureType != ISC_MAX9286_NO_ERROR)
        goto done;

    // Check for line length error
    for(i = 0; i < MAX_NUM_GMSL_LINKS; i++) {
        if(MAX9286_GMSL_LINK(i) & gmslLinks) {
            if(regValues[ISC_MAX9286_REG_LINE_BUF_ERR] & MAX9286_GMSL_LINK(i)) {
                errorStatus->link |= MAX9286_GMSL_LINK(i);
                errorStatus->failureType = ISC_MAX9286_LINE_LENGTH_ERROR;
            }
        }
    }
    if(errorStatus->failureType != ISC_MAX9286_NO_ERROR)
        goto done;

    // Check for line buffer overflow
    for(i = 0; i < MAX_NUM_GMSL_LINKS; i++) {
        if(MAX9286_GMSL_LINK(i) & gmslLinks) {
            if((regValues[ISC_MAX9286_REG_LINE_BUF_ERR] >> 4) & MAX9286_GMSL_LINK(i)) {
                errorStatus->link |= MAX9286_GMSL_LINK(i);
                errorStatus->failureType = ISC_MAX9286_LINE_BUFFER_OVERFLOW;
            }
        }
    }

done:
    return NVMEDIA_STATUS_OK;
}

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

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

