/*
 * 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 "CNvMMax96712.hpp"
#include "platform/CNvMPlatform.hpp"

namespace nvsipl
{

CNvMMax96712::~CNvMMax96712()
{
}

SIPLStatus CNvMMax96712::SetConfig(const DeserInfo *deserInfo, DeserializerParams *params)
{
    SIPLStatus status = NVSIPL_STATUS_OK;

    if (m_eState != CREATED) {
        LOG_ERR("CDI invalid state\n");
        return NVSIPL_STATUS_INVALID_STATE;
    }

    status = CNvMDeserializer::SetConfig(deserInfo, params);
    CHK_NVSIPLEERROR_AND_RETURN(status, "CNvMDeserializer::SetConfig");

    /*! Get CDI Driver */
    m_pCDIDriver = GetMAX96712NewDriver();
    CHK_PTR_AND_RETURN(m_pCDIDriver, "GetMAX96712NewDriver()");

    /* Configure MAX96712 settings */
    status = SetMAX96712Ctx();
    CHK_NVSIPLEERROR_AND_RETURN(status, "CNvMMax96712::SetMAX96712Ctx");

    m_eState = CDI_DEVICE_CONFIG_SET;
    return NVSIPL_STATUS_OK;
}

SIPLStatus CNvMMax96712::CreateCDIDevice(DevBlkCDIRootDevice* cdiRoot)
{

    DevBlkCDIDevice* cdiDevice = NULL;
    std::uint32_t addressList[1] = {};
    DevBlkCDIAdvancedConfig *pCDIAdvConfig = NULL;

    if (m_eState != CDI_DEVICE_CONFIG_SET) {
        LOG_ERR("CDI device config is not set\n");
        return NVSIPL_STATUS_INVALID_STATE;
    }

    if (cdiRoot == NULL) {
        LOG_ERR("CDI root device is NULL\n");
        return NVSIPL_STATUS_BAD_ARGUMENT;
    }

    // Generate I2c address
    if (m_oDeviceParams.bUseNativeI2C) {
        m_i2CAddr = m_nativeI2CAddr;
    } else {
        m_i2CAddr = m_oDeviceParams.pI2CAddrMgr->GenerateI2CAddr(m_nativeI2CAddr);
    }

    if (m_oDeviceParams.bEnableSimulator ) {
        addressList[0] = DEVBLK_CDI_SIMULATOR_ADDRESS;
    } else {
        addressList[0] = m_i2CAddr;
    }

    CHK_PTR_AND_RETURN(addressList[0], "Failed to get I2C address");

    if (m_upDrvContext != NULL) {
        m_oAdvConfig.clientContext = m_upDrvContext->GetPtr();
        pCDIAdvConfig = &m_oAdvConfig;
    }

    // Create the CDI device
    cdiDevice = DevBlkCDIDeviceCreate(cdiRoot,
                                      addressList,
                                      1,
                                      m_pCDIDriver,
                                      pCDIAdvConfig);
    CHK_PTR_AND_RETURN(cdiDevice, "DevBlkCDIDeviceCreate in CNvMSensor::Init");
    m_upCDIDevice = std::unique_ptr<DevBlkCDIDevice, CloseDevBlkCDI>(
            cdiDevice);

    m_eState = CDI_DEVICE_CREATED;
    return NVSIPL_STATUS_OK;
}

// Set Max96712 context
SIPLStatus CNvMMax96712::SetMAX96712Ctx()
{
    std::string platformName = CNvMPlatform::GetInstance()->GetPlatformName();
    ContextMAX96712 *ctx = NULL;
    DriverContextImpl<ContextMAX96712> *driverContext = new DriverContextImpl<ContextMAX96712>();
    CHK_PTR_AND_RETURN(driverContext, "Failed to create CDI driver context");

    m_upDrvContext.reset(driverContext);
    ctx = &driverContext->m_Context;

    for (std::uint8_t i = 0u; i < MAX96712_MAX_NUM_LINK; i++) {
        ctx->gmslMode[i] = CDI_MAX96712_GMSL_MODE_UNUSED;
        ctx->longCables[i] = m_longCables[i];
    }

    for (auto & item : m_ovLinkModes) {
        if (item.linkMode == LinkMode::LINK_MODE_GMSL2) {
            ctx->gmslMode[item.linkIndex] = CDI_MAX96712_GMSL2_MODE;
        } else if (item.linkMode == LinkMode::LINK_MODE_GMSL1) {
            ctx->gmslMode[item.linkIndex] = CDI_MAX96712_GMSL1_MODE;
        } else {
            return NVSIPL_STATUS_NOT_SUPPORTED;
        }
    }

    // Select I2C port
    if (platformName == "ddpx-a" or platformName == "p3479" or platformName == "p3570-a") {
        ctx->i2cPort = CDI_MAX96712_I2CPORT_0;
    } else if (platformName == "ddpx-b" or platformName == "ddpx-c" or platformName == "p3570-b") {
        ctx->i2cPort = CDI_MAX96712_I2CPORT_1;
    } else {
        return NVSIPL_STATUS_NOT_SUPPORTED;
    }

    switch (m_eInterface) {
        case NVMEDIA_IMAGE_CAPTURE_CSI_INTERFACE_TYPE_CSI_A:
        case NVMEDIA_IMAGE_CAPTURE_CSI_INTERFACE_TYPE_CSI_C:
        case NVMEDIA_IMAGE_CAPTURE_CSI_INTERFACE_TYPE_CSI_E:
        case NVMEDIA_IMAGE_CAPTURE_CSI_INTERFACE_TYPE_CSI_G:
            ctx->txPort = (ctx->i2cPort == CDI_MAX96712_I2CPORT_0) ? CDI_MAX96712_TXPORT_PHY_C :
                                                                     CDI_MAX96712_TXPORT_PHY_E;
            ctx->mipiOutMode = CDI_MAX96712_MIPI_OUT_4x2;
            break;
        case NVMEDIA_IMAGE_CAPTURE_CSI_INTERFACE_TYPE_CSI_B:
        case NVMEDIA_IMAGE_CAPTURE_CSI_INTERFACE_TYPE_CSI_D:
        case NVMEDIA_IMAGE_CAPTURE_CSI_INTERFACE_TYPE_CSI_F:
        case NVMEDIA_IMAGE_CAPTURE_CSI_INTERFACE_TYPE_CSI_H:
              ctx->txPort = (ctx->i2cPort == CDI_MAX96712_I2CPORT_0) ? CDI_MAX96712_TXPORT_PHY_D :
                                                                       CDI_MAX96712_TXPORT_PHY_F;
              ctx->mipiOutMode = CDI_MAX96712_MIPI_OUT_4x2;
            break;
        case NVMEDIA_IMAGE_CAPTURE_CSI_INTERFACE_TYPE_CSI_AB:
        case NVMEDIA_IMAGE_CAPTURE_CSI_INTERFACE_TYPE_CSI_CD:
        case NVMEDIA_IMAGE_CAPTURE_CSI_INTERFACE_TYPE_CSI_EF:
        case NVMEDIA_IMAGE_CAPTURE_CSI_INTERFACE_TYPE_CSI_GH:
            ctx->txPort = (ctx->i2cPort == CDI_MAX96712_I2CPORT_0) ? CDI_MAX96712_TXPORT_PHY_D :
                                                                     CDI_MAX96712_TXPORT_PHY_E;
            ctx->mipiOutMode = CDI_MAX96712_MIPI_OUT_2x4;

            break;
        default:
            return NVSIPL_STATUS_NOT_SUPPORTED;
    }

    /* Set CSI output frequency */
    if (m_ePhyMode == NVMEDIA_ICP_CSI_DPHY_MODE) {
        m_uMipiSpeed = 2500000u;
    } else if (m_ePhyMode == NVMEDIA_ICP_CSI_CPHY_MODE) {
        if (ctx->mipiOutMode == CDI_MAX96712_MIPI_OUT_2x4) {
            m_uMipiSpeed = 1700000u;
        } else if (ctx->mipiOutMode == CDI_MAX96712_MIPI_OUT_4x2) {
            if (platformName == "ddpx-a" or platformName == "ddpx-b" or platformName == "ddpx-c") {
                m_uMipiSpeed = 2000000u;
            } else {
                m_uMipiSpeed = 2500000u;
            }
        }
    }

    /* Set link mask to be enabled */
    ctx->linkMask = m_linkMask;

    /* Set slave mode */
    ctx->slaveEnabled = m_oDeviceParams.bSlave;

    /* Set py mode */
    ctx->phyMode = (m_ePhyMode == NVMEDIA_ICP_CSI_CPHY_MODE) ? CDI_MAX96712_PHY_MODE_CPHY : CDI_MAX96712_PHY_MODE_DPHY;

    return NVSIPL_STATUS_OK;
}

SIPLStatus CNvMMax96712::Init()
{
    NvMediaStatus nvmStatus = NVMEDIA_STATUS_OK;
    WriteParametersParamMAX96712 writeParamsMAX96712 = {};
    ReadParametersParamMAX96712 readParamsMAX96712 = {};

    if (m_eState != CDI_DEVICE_CREATED) {
        LOG_ERR("CDI device invalid state\n");
        return NVSIPL_STATUS_INVALID_STATE;
    }

    if (m_oDeviceParams.bEnableSimulator or m_oDeviceParams.bSlave) {
        m_eState = INIT_1_DONE;
        return NVSIPL_STATUS_OK;
    }

    /*! Check deserializer is present */
    LOG_INFO("Check deserializer is present\n");
    nvmStatus = MAX96712CheckPresence(m_upCDIDevice.get());
    CHK_NVMERROR_CONVERT_AND_RETURN(nvmStatus, "MAX96712CheckPresence");

    LOG_INFO("Set deserializer defaults\n");
    nvmStatus = MAX96712SetDefaults(m_upCDIDevice.get());
    CHK_NVMERROR_CONVERT_AND_RETURN(nvmStatus, "MAX96712SetDefaults");

    /* Get deserializer revision */
    LOG_INFO("Get deserializer revision\n");
    nvmStatus = MAX96712ReadParameters(m_upCDIDevice.get(),
                                       CDI_READ_PARAM_CMD_MAX96712_REV_ID,
                                       sizeof(readParamsMAX96712.revision),
                                       &readParamsMAX96712);
    CHK_NVMERROR_CONVERT_AND_RETURN(nvmStatus, "MAX96712ReadParameters(MAX96712, CDI_READ_PARAM_CMD_MAX96712_REV_ID");
    m_eRevision = readParamsMAX96712.revision;

    /* Set MIPI output mode */
    LOG_INFO("Set MIPI output mode\n");
    writeParamsMAX96712.MipiSettings.mipiSpeed = m_uMipiSpeed / 100000u;

    writeParamsMAX96712.MipiSettings.phyMode = (m_ePhyMode == NVMEDIA_ICP_CSI_CPHY_MODE) ? CDI_MAX96712_PHY_MODE_CPHY : CDI_MAX96712_PHY_MODE_DPHY;
    nvmStatus = MAX96712WriteParameters(m_upCDIDevice.get(),
                                        CDI_WRITE_PARAM_CMD_MAX96712_SET_MIPI,
                                        sizeof(writeParamsMAX96712.MipiSettings),
                                        &writeParamsMAX96712);
    CHK_NVMERROR_CONVERT_AND_RETURN(nvmStatus, "MAX96712WriteParameters(MAX96712, CDI_WRITE_PARAM_CMD_MAX96712_SET_MIPI");

    /* Disable BACKTOP */
    nvmStatus = MAX96712SetDeviceConfig(m_upCDIDevice.get(), CDI_CONFIG_MAX96712_DISABLE_BACKTOP);
    CHK_NVMERROR_CONVERT_AND_RETURN(nvmStatus, "CDI_CONFIG_MAX96712_DISABLE_BACKTOP");

    m_eState = INIT_1_DONE;
    return NVSIPL_STATUS_OK;
}

SIPLStatus CNvMMax96712::Deinit()
{
    NvMediaStatus nvmStatus = NVMEDIA_STATUS_OK;

    /* Disable the replication */
    nvmStatus = MAX96712SetDeviceConfig(m_upCDIDevice.get(), CDI_CONFIG_MAX96712_DISABLE_REPLICATION);
    CHK_NVMERROR_CONVERT_AND_RETURN(nvmStatus, "CDI_CONFIG_MAX96712_DISABLE_REPLICATION");

    m_upCDIDevice.reset();

    m_eState = DEINIT;
    return NVSIPL_STATUS_OK;
}

SIPLStatus CNvMMax96712::EnableLinks(uint8_t linkMask)
{
    NvMediaStatus nvmStatus = NVMEDIA_STATUS_OK;
    ReadParametersParamMAX96712 readParamsMAX96712 = {};
    WriteParametersParamMAX96712 writeParamsMAX96712 = {};
    uint8_t link = 0u;

    if (m_oDeviceParams.bEnableSimulator or m_oDeviceParams.bSlave) {
        return NVSIPL_STATUS_OK;
    }

    /* Get the links currently enabled */
    nvmStatus = MAX96712ReadParameters(m_upCDIDevice.get(),
                                       CDI_READ_PARAM_CMD_MAX96712_ENABLED_LINKS,
                                       sizeof(readParamsMAX96712.link),
                                       &readParamsMAX96712);
    CHK_NVMERROR_CONVERT_AND_RETURN(nvmStatus, "MAX96712ReadParameters(CDI_READ_PARAM_CMD_MAX96712_ENABLED_LINKS)");
    link = readParamsMAX96712.link;

    if (link != linkMask) {
        LOG_INFO("Enabling links\n");
        writeParamsMAX96712.link = (LinkMAX96712) linkMask;
        nvmStatus = MAX96712WriteParameters(m_upCDIDevice.get(),
                                            CDI_WRITE_PARAM_CMD_MAX96712_ENABLE_SPECIFIC_LINKS,
                                            sizeof(writeParamsMAX96712.link),
                                            &writeParamsMAX96712);
        CHK_NVMERROR_CONVERT_AND_RETURN(nvmStatus, "MAX96712WriteParameters(MAX96712, CDI_WRITE_PARAM_CMD_MAX96712_ENABLE_SPECIFIC_LINK");

        SLEEP_MS(5);
    }

    return NVSIPL_STATUS_OK;
}

SIPLStatus CNvMMax96712::ControlLinks(const std::vector<LinkAction>& linkActions)
{
    NvMediaStatus nvmStatus = NVMEDIA_STATUS_OK;
    ReadParametersParamMAX96712 readParamsMAX96712 = {};
    WriteParametersParamMAX96712 writeParamsMAX96712 = {};
    uint8_t link = 0u;

    if (m_oDeviceParams.bEnableSimulator or m_oDeviceParams.bSlave) {
        return NVSIPL_STATUS_OK;
    }

    /* Get the links currently enabled */
    nvmStatus = MAX96712ReadParameters(m_upCDIDevice.get(),
                                       CDI_READ_PARAM_CMD_MAX96712_ENABLED_LINKS,
                                       sizeof(readParamsMAX96712.link),
                                       &readParamsMAX96712);
    CHK_NVMERROR_CONVERT_AND_RETURN(nvmStatus, "MAX96712ReadParameters(CDI_READ_PARAM_CMD_MAX96712_ENABLED_LINKS)");
    link = readParamsMAX96712.link;

    for (std::uint32_t i = 0; i < linkActions.size(); i++) {
        auto& item = linkActions[i];

        if (item.linkIdx > MAX96712_MAX_NUM_LINK) {
            return NVSIPL_STATUS_ERROR;
        }

        if (item.action == LinkAction::LINK_ENABLE) {
            link |= (1 << item.linkIdx);
        } else if (item.action == LinkAction::LINK_DISABLE) {
            link &= ((~(1 << item.linkIdx)) & 0x0F);
        } else {
            continue;
        }
    }

    writeParamsMAX96712.link = static_cast<LinkMAX96712>(link);
    nvmStatus = MAX96712WriteParameters(m_upCDIDevice.get(),
                                        CDI_WRITE_PARAM_CMD_MAX96712_ENABLE_SPECIFIC_LINKS,
                                        sizeof(writeParamsMAX96712.link),
                                        &writeParamsMAX96712);
    CHK_NVMERROR_CONVERT_AND_RETURN(nvmStatus, "MAX96712WriteParameters(MAX96712, CDI_WRITE_PARAM_CMD_MAX96712_ENABLE_SPECIFIC_LINK");

    return NVSIPL_STATUS_OK;
}

SIPLStatus CNvMMax96712::CheckLinkLock(uint8_t linkMask) {
    NvMediaStatus nvmStatus = NVMEDIA_STATUS_OK;

    if (m_oDeviceParams.bEnableSimulator or m_oDeviceParams.bSlave) {
        return NVSIPL_STATUS_OK;
    }

    for(uint8_t linkIndex = 0; linkIndex < m_ovLinkModes.size(); linkIndex++) {
        auto& item = m_ovLinkModes[linkIndex];

        if ((linkMask & (1 << linkIndex)) == 0 ) {
            continue;
        }

        if (item.linkMode == LinkMode::LINK_MODE_GMSL2) {
            LOG_INFO("Check config link lock\n");
            nvmStatus = MAX96712CheckLink(m_upCDIDevice.get(),
                                            GetMAX96712Link(linkIndex),
                                            CDI_MAX96712_LINK_LOCK_GMSL2,
                                            false);
            if (nvmStatus != NVMEDIA_STATUS_OK) {
                nvmStatus = MAX96712OneShotReset(m_upCDIDevice.get(), (LinkMAX96712)linkMask);
                CHK_NVMERROR_CONVERT_AND_RETURN(nvmStatus, "MAX96712OneShotReset");
                nvmStatus = MAX96712CheckLink(m_upCDIDevice.get(),
                                              GetMAX96712Link(linkIndex),
                                              CDI_MAX96712_LINK_LOCK_GMSL2,
                                              true);
                CHK_NVMERROR_CONVERT_AND_RETURN(nvmStatus, "MAX96712CheckLink(CDI_MAX96712_GMSL2_LINK_LOCK)");
            }
        } else if (item.linkMode == LinkMode::LINK_MODE_GMSL1) {
            LOG_INFO("Check config link lock \n");
            nvmStatus = MAX96712CheckLink(m_upCDIDevice.get(),
                                          GetMAX96712Link(linkIndex),
                                          CDI_MAX96712_LINK_LOCK_GMSL1_CONFIG,
                                          false);
            if (nvmStatus != NVMEDIA_STATUS_OK) {
                nvmStatus = MAX96712OneShotReset(m_upCDIDevice.get(), (LinkMAX96712)linkMask);
                CHK_NVMERROR_CONVERT_AND_RETURN(nvmStatus, "MAX96712OneShotReset");
                nvmStatus = MAX96712CheckLink(m_upCDIDevice.get(),
                                              GetMAX96712Link(linkIndex),
                                              CDI_MAX96712_LINK_LOCK_GMSL1_CONFIG,
                                              true);
                CHK_NVMERROR_CONVERT_AND_RETURN(nvmStatus, "MAX96712CheckLink(CDI_MAX96712_GMSL1_CONFIG_LINK_LOCK)");
            }
        } else {
            return NVSIPL_STATUS_NOT_SUPPORTED;
        }
    }

    return NVSIPL_STATUS_OK;
}

SIPLStatus CNvMMax96712::Start()
{
    NvMediaStatus nvmStatus = NVMEDIA_STATUS_OK;
    SIPLStatus status = NVSIPL_STATUS_OK;
    ReadParametersParamMAX96712 readParamsMAX96712 = {};

    if (m_oDeviceParams.bEnableSimulator) {
        m_eState = STARTED;
        return NVSIPL_STATUS_OK;
    }

    /* Enable the replication */
    nvmStatus = MAX96712SetDeviceConfig(m_upCDIDevice.get(), CDI_CONFIG_MAX96712_ENABLE_REPLICATION);
    CHK_NVMERROR_CONVERT_AND_RETURN(nvmStatus, "CDI_CONFIG_MAX96712_ENABLE_REPLICATION");

    /* Check CSIPLL lock */
    nvmStatus = MAX96712SetDeviceConfig(m_upCDIDevice.get(), CDI_CONFIG_MAX96712_CHECK_CSIPLL_LOCK);
    CHK_NVMERROR(nvmStatus, "CDI_CONFIG_MAX96712_CHECK_CSIPLL_LOCK");

    /* Trigger the initial deskew */
    if (m_ePhyMode == NVMEDIA_ICP_CSI_DPHY_MODE and m_uMipiSpeed >= 1500000u) {
        nvmStatus = MAX96712SetDeviceConfig(m_upCDIDevice.get(), CDI_CONFIG_MAX96712_TRIGGER_DESKEW);
        CHK_NVMERROR_CONVERT_AND_RETURN(nvmStatus, "CDI_CONFIG_MAX96712_TRIGGER_DESKEW");
    }

    if (m_oDeviceParams.bSlave) {
        m_eState = STARTED;
        return NVSIPL_STATUS_OK;
    }

    status = CheckLinkLock(m_linkMask);
    if (status != NVSIPL_STATUS_OK) {
        nvmStatus = MAX96712OneShotReset(m_upCDIDevice.get(), (LinkMAX96712)m_linkMask);
        CHK_NVMERROR_CONVERT_AND_RETURN(nvmStatus, "MAX96712OneShotReset");
    }

    /* Enable BACKTOP */
    nvmStatus = MAX96712SetDeviceConfig(m_upCDIDevice.get(), CDI_CONFIG_MAX96712_ENABLE_BACKTOP);
    CHK_NVMERROR_CONVERT_AND_RETURN(nvmStatus, "CDI_CONFIG_MAX96712_ENABLE_BACKTOP");

    /* Check & Clear if ERRB set */
    nvmStatus = MAX96712ReadParameters(m_upCDIDevice.get(),
                                       CDI_READ_PARAM_CMD_MAX96712_ERRB,
                                       sizeof(readParamsMAX96712.ErrorStatus),
                                       &readParamsMAX96712);
    CHK_NVMERROR_CONVERT_AND_RETURN(nvmStatus, "DevBlkCDIReadParameters(CDI_READ_PARAM_CMD_MAX96712_ERRB)");

    m_eState = STARTED;

    return NVSIPL_STATUS_OK;
}

SIPLStatus CNvMMax96712::DetectErrors(std::uint8_t& cameraModules)
{
    ErrorStatusMAX96712 errorStatus;

    SLEEP_MS(30);

    auto nvmStatus = MAX96712GetErrorStatus(m_upCDIDevice.get(),
                                              sizeof(errorStatus),
                                              &errorStatus);
    CHK_NVMERROR_CONVERT_AND_RETURN(nvmStatus, "MAX96712GetErrorStatus");

    cameraModules = errorStatus.link;

    return NVSIPL_STATUS_OK;
}

} // end of nvsipl
