/* Copyright (c) 2016-2019, NVIDIA CORPORATION.  All rights reserved.
 *
 * NVIDIA CORPORATION and its licensors retain all intellectual property
 * and proprietary rights in and to this software, related documentation
 * and any modifications thereto.  Any use, reproduction, disclosure or
 * distribution of this software and related documentation without an express
 * license agreement from NVIDIA CORPORATION is strictly prohibited.
 */

#include <stdbool.h>
#include "capture.h"
#include "save.h"

typedef struct {
    NvMediaICPCsiInputFrameError error;
    const char *errType;
    const char *errMsg;
} ICPCsiInputFrameErrLut;

typedef struct {
    NvMediaICPCsiInputStreamError error;
    const char *errType;
    const char *errMsg;
} ICPCsiInputStreamErrLut;

typedef struct {
    NvMediaICPPixelFaultError error;
    const char *errType;
    const char *errMsg;
} ICPViPixelFaultErrLut;

typedef struct {
    NvMediaICPFrameFaultError error;
    const char *errType;
    const char *errMsg;
} ICPViFrameFaultErrLut;

typedef struct {
    NvMediaICPMemoryWriteError error;
    const char *errType;
    const char *errMsg;
} ICPViMemoryWriteErrLut;

#define ARRAY_SIZE(a)       (sizeof(a) / sizeof((a)[0]))
#define ADD_ICP_ERROR_INFO(errStatus, type, desc) \
    { .error = errStatus, .errType = type, .errMsg = desc }

static const ICPCsiInputFrameErrLut gCaptureCsiInputFrameErrTbl[] = {
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_CSI_ERROR_FE_CSI_FAULT,
                       "CSI_FE_FAULT",
                       "A NVCSI fault received at frame end"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_CSI_ERROR_FS_FAULT,
                       "CSI_FS_FAULT",
                       "A frame start occurred before previous frame end"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_CSI_ERROR_FE_FRAMEID_FAULT,
                       "CSI_FE_FRAMEDID_FAULT",
                       "Frame ID for FE packet does not match that of FS packet"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_CSI_ERROR_FE_FORCED_FAULT,
                       "CSI_FE_FORCED_FAULT",
                       "A frame end was injected by the CSI hardware"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_CSI_ERROR_PXL_ENABLE_FAULT,
                       "CSI_PXL_ENABLE_FAULT",
                       "An illegal pixel encoding has been detected"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_CSI_ERROR_PXL_ENABLE_FAULT,
                       "CSI_PXL_ENABLE_FAULT",
                       "An illegal pixel encoding has been detected"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_CSI_ERROR_PP_FSM_TIMEOUT,
                       "CSI_FSM_TIMEOUT",
                       "PP FSM timeout"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_CSI_ERROR_PH_ECC_DPHY,
                       "CSI_PH_ECC_DPHY",
                       "PH ECC error for DPHY"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_CSI_ERROR_PAYLOAD_CRC,
                       "CSI_PAYLOAD_CRC",
                       "PAYLOAD CRC error"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_CSI_ERROR_LINE_SHORT,
                       "CSI_LINESHORT",
                       "Line Short Error"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_CSI_ERROR_PH_CRC_CPHY,
                       "CSI_PH_CRC_CPHY",
                       "PH ECC CRC error for CPHY"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_CSI_ERROR_EMB_CRC,
                       "CSI_EMB_CRC",
                       "Embedded Line CRC Error"),
};

static const ICPCsiInputStreamErrLut gCaptureCsiInputStreamErrTbl[] = {
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_CSI_ERROR_SPURIOUS_DATA,
                       "CSI_SPURIOUS_DATA",
                       "FIFO data was found in the gap between frame start and frame end"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_CSI_ERROR_FIFO_OVERFLOW,
                       "CSI_FIFO_OVERFLOW",
                       "An overflow occurred in a stream FIFO"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_CSI_ERROR_FIFO_LOF,
                       "CSI_FIFO_LOF",
                       "A loss of frame overflow occurred"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_CSI_ERROR_FIFO_BADPKT,
                       "CSI_FIFO_BADPKT",
                       "An unknown packet type has been received on a stream"),
};

static const ICPViPixelFaultErrLut gCaptureViPixelFaultErrTbl[] = {
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_FS_TIMEOUT,
                       "VI_FS_TIMEOUT",
                       "Frame Start Timeout Error"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_FE_TIMEOUT,
                       "VI_FE_TIMEOUT",
                       "Frame End Timeout Error"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_SHORT_FRAME,
                       "VI_SHORT_FRAME",
                       "FE packet arrived before the normal number of pixels has appeared"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_FAULT_FE,
                       "VI_FAULT_FE",
                       "Data specific fault occurred on a channel"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_PXL_MISSING_LE,
                       "VI_PXL_MISSING_LINE_END",
                       "Two LS packets were detected without a LE packet between them"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_PXL_RUNAWAY,
                       "VI_PXL_RUNAWAY",
                       "More lines were received than expected"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_PXL_SPURIOUS,
                       "VI_PXL_SPURIOUS",
                       "A pixel data packet was received without a LS packet first"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_PXL_LONG_LINE,
                       "VI_PXL_LONG_LINE",
                       "More pixels were received than expected in a line"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_PXL_SHORT_LINE,
                       "VI_PXL_SHORT_LINE",
                       "Fewer pixels were received than expected in a line"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_EMB_MISSING_LE,
                       "VI_EMB_MISSING_LINE_END",
                       "Two LS packets were detected without a LE packet between them in emb data"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_EMB_RUNAWAY,
                       "VI_EMB_RUNAWAY",
                       "More lines were received than expected in emb data"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_EMB_SPURIOUS,
                       "VI_EMB_SPURIOUS",
                       "A pixel data packet was received without a LS packet first in emb data"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_EMB_LONG_LINE,
                       "VI_EMB_LONG_LINE",
                       "More pixels were received than expected in a line in emb data"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_EMB_INFRINGE,
                       "VI_EMB_INFRINGE",
                       "Embedded data was received on a frame where no embedded data was expected"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_DTYPE_MISMATCH,
                       "VI_DTYPE_MISMATCH",
                       "The pixel datatype changed in the middle of the line"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_FORCED_FE,
                       "VI_FORCED_FE",
                       "FE didn't arrive within expected frame duration, capture engine injected FE"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_PXL_INCOMPLETE,
                       "VI_PXL_INCOMPLETE",
                       "Fewer lines of pixel are received than expected"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_EMB_INCOMPLETE,
                       "VI_EMB_INCOMPLETE",
                       "Fewer lines of emb data are received than expected"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_PFSD_FAULT,
                       "VI_PFSD_FAULT",
                       "Permanent Fault Software Diagnostic is detected from engine status"),
};

static const ICPViFrameFaultErrLut gCaptureViFrameFaultErrTbl[] = {
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_SHORT_FRAME,
                       "VI_SHORT_FRAME",
                       "FE packet arrived before the normal number of pixels has appeared"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_SHORT_EMB,
                       "VI_SHORT_EMB",
                       "FE of EMB Data packet arrived before the normal number of pixels has appeared"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_PFSD_FAULT,
                       "VI_PFSD_FAULT",
                       "Permanent Fault Software Diagnostic is detected"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_FAULT_FE,
                       "VI_FAULT_FE",
                       "FE is inserted due to a stream reset or early abort before"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_NOMATCH,
                       "VI_NOMATCH",
                       "CSI packets are discarged due to no matched capture channels"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_COLLISION,
                       "VI_COLLISION",
                       "An FS packet matches a channel which is not done processing a previously matching frame"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_LOAD_FRAMED,
                       "VI_LOAD_FRAMED",
                       "FE is inserted due to a stream reset or early abort"),
};

static const ICPViMemoryWriteErrLut gCaptureViMemoryWriteErrTbl[] = {
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_PACKET_OVERFLOW,
                       "VI_PACKET_OVERFLOW",
                       "The FIFO for a surface packer has overflowed"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_FRAME_TRUNCATED,
                       "VI_PACKET_TRUNCATED",
                       "A packer has dropped a partial frame"),
    ADD_ICP_ERROR_INFO(NVMEDIA_ICP_VI_ERROR_FRAME_TOSSED,
                       "VI_FRAME_TOSSED",
                       "A packer has dropped an entire frame"),
};

static NvMediaStatus
_SetExtImgDevParameters(NvCaptureContext *ctx,
                        CaptureConfigParams *captureParams,
                        ExtImgDevParam *configParam)
{
    unsigned int i;

    configParam->desAddr = captureParams->desAddr;
    configParam->brdcstSerAddr = captureParams->brdcstSerAddr;
    configParam->brdcstSensorAddr = captureParams->brdcstSensorAddr;
    configParam->e2pPhysicalAddr = captureParams->e2pPhysicalAddr;
    for (i = 0; i < ctx->numSensors; i++) {
        configParam->serAddr[i] = captureParams->serAddr[i];
        configParam->sensorAddr[i] = captureParams->sensorAddr[i];
        configParam->e2pAddr[i] = captureParams->e2pAddr[i];
        configParam->sensorIdentifier[i] = captureParams->sensorIdentifier[i];
    }
    configParam->i2cDevice = captureParams->i2cDevice;
    configParam->moduleName = captureParams->inputDevice;
    configParam->board = captureParams->board;
    configParam->resolution = captureParams->resolution;
    configParam->camMap = &ctx->testArgs->camMap;
    configParam->sensorsNum = ctx->numSensors;
    configParam->inputFormat = captureParams->inputFormat;
    configParam->interface = captureParams->interface;
    configParam->enableEmbLines =
        (captureParams->embeddedDataLinesTop || captureParams->embeddedDataLinesBottom) ?
            NVMEDIA_TRUE : NVMEDIA_FALSE;
    configParam->initialized = NVMEDIA_FALSE;
    configParam->enableSimulator = NVMEDIA_FALSE;
    configParam->slave = ctx->testArgs->slaveTegra;
    configParam->enableVirtualChannels = (ctx->testArgs->useVirtualChannels)? NVMEDIA_TRUE : NVMEDIA_FALSE;
    configParam->enableExtSync = ctx->testArgs->enableExtSync;
    configParam->dutyRatio =
        ((ctx->testArgs->dutyRatio < 0.0) || (ctx->testArgs->dutyRatio >= 1.0)) ?
            0.25 : ctx->testArgs->dutyRatio;

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
_SetICPSettings(CaptureThreadCtx *ctx,
                NvMediaICPSettings *icpSettings,
                ExtImgDevProperty *property,
                CaptureConfigParams *captureParams,
                TestArgs *testArgs)
{
    char *surfaceFormat = captureParams->surfaceFormat;
    NvMediaBool useCaptureS16Format = NVMEDIA_FALSE;

    NVM_SURF_FMT_DEFINE_ATTR(surfFormatAttrs);

    if ((strncasecmp(captureParams->board, "e2379", strlen("e2379")) == 0) ||
        (strncasecmp(captureParams->board, "p2382", strlen("p2382")) == 0) ||
        (strncasecmp(captureParams->board, "p3407", strlen("p3407")) == 0)) {
        useCaptureS16Format = NVMEDIA_TRUE;
    }

    /* Set surface format */
    if (!strcasecmp(surfaceFormat, "yv16")) {
        NVM_SURF_FMT_SET_ATTR_YUV(surfFormatAttrs,YUV,422,PLANAR,UINT,8,PL);
        ctx->rawBytesPerPixel = 0;
    } else if (!strcasecmp(surfaceFormat, "rgb")) {
        NVM_SURF_FMT_SET_ATTR_RGBA(surfFormatAttrs,RGBA,UINT,8,PL);
        ctx->rawBytesPerPixel = 0;
    } else if (!strcasecmp(surfaceFormat, "raw8")) {
        if (useCaptureS16Format == NVMEDIA_TRUE) {
            NVM_SURF_FMT_SET_ATTR_RAW(surfFormatAttrs,RGGB,INT,8,PL);
        } else {
            NVM_SURF_FMT_SET_ATTR_RAW(surfFormatAttrs,RGGB,UINT,8,PL);
        }
        surfFormatAttrs[NVM_SURF_ATTR_COMPONENT_ORDER].value +=  property->pixelOrder;
        ctx->rawBytesPerPixel = 1;
        ctx->pixelOrder =  property->pixelOrder;
    } else if  (!strcasecmp(surfaceFormat, "raw10")) {
        if (useCaptureS16Format == NVMEDIA_TRUE) {
            NVM_SURF_FMT_SET_ATTR_RAW(surfFormatAttrs,RGGB,INT,10,PL);
        } else {
            NVM_SURF_FMT_SET_ATTR_RAW(surfFormatAttrs,RGGB,UINT,10,PL);
        }
        surfFormatAttrs[NVM_SURF_ATTR_COMPONENT_ORDER].value +=  property->pixelOrder;
        ctx->rawBytesPerPixel = 2;
        ctx->pixelOrder =  property->pixelOrder;
    } else if (!strcasecmp(surfaceFormat, "raw12")) {
        if (useCaptureS16Format == NVMEDIA_TRUE) {
            NVM_SURF_FMT_SET_ATTR_RAW(surfFormatAttrs,RGGB,INT,12,PL);
        } else {
            NVM_SURF_FMT_SET_ATTR_RAW(surfFormatAttrs,RGGB,UINT,12,PL);
        }
        surfFormatAttrs[NVM_SURF_ATTR_COMPONENT_ORDER].value +=  property->pixelOrder;
        ctx->rawBytesPerPixel = 2;
        ctx->pixelOrder =  property->pixelOrder;
    } else if (!strcasecmp(surfaceFormat, "raw16")) {
        if (useCaptureS16Format == NVMEDIA_TRUE) {
            NVM_SURF_FMT_SET_ATTR_RAW(surfFormatAttrs,RGGB,INT,16,PL);
        } else {
            NVM_SURF_FMT_SET_ATTR_RAW(surfFormatAttrs,RGGB,UINT,16,PL);
        }
        surfFormatAttrs[NVM_SURF_ATTR_COMPONENT_ORDER].value +=  property->pixelOrder;
        ctx->rawBytesPerPixel = 2;
        ctx->pixelOrder =  property->pixelOrder;
    } else if (!strcasecmp(surfaceFormat, "raw20")) {
        if (useCaptureS16Format == NVMEDIA_TRUE) {
            NVM_SURF_FMT_SET_ATTR_RAW(surfFormatAttrs,RGGB,INT,20,PL);
        } else {
            NVM_SURF_FMT_SET_ATTR_RAW(surfFormatAttrs,RGGB,UINT,20,PL);
        }
        surfFormatAttrs[NVM_SURF_ATTR_COMPONENT_ORDER].value +=  property->pixelOrder;
        ctx->rawBytesPerPixel = 4;
        ctx->pixelOrder =  property->pixelOrder;
    } else {
        LOG_ERR("%s: Bad surface format specified: %s \n",
                __func__, surfaceFormat);
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }
    ctx->surfType = NvMediaSurfaceFormatGetType(surfFormatAttrs, NVM_SURF_FMT_ATTR_MAX);

    /* Set NvMediaICPSettings */
    icpSettings->interfaceType = property->interface;
    icpSettings->inputFormat.inputFormatType = property->inputFormatType;
    icpSettings->inputFormat.bitsPerPixel = property->bitsPerPixel;
    icpSettings->tpgEnable = property->tpgEnable;
    icpSettings->surfaceType = ctx->surfType;
    icpSettings->width = (testArgs->useVirtualChannels)?
                          property->width : property->width*testArgs->numSensors;
    icpSettings->height = property->height;
    icpSettings->startX = 0;
    icpSettings->startY = 0;
    icpSettings->embeddedDataType = property->embDataType;
    icpSettings->embeddedDataLines = property->embLinesTop + property->embLinesBottom;
    icpSettings->interfaceLanes = captureParams->csiLanes;
    /* pixel frequency is from imgDevPropery, it is calculated by (VTS * HTS * FrameRate) * n sensors */
    icpSettings->pixelFrequency = property->pixelFrequency;
    icpSettings->phyMode = property->phyMode;
    icpSettings->mipiSpeed = property->mipiSpeed[property->phyMode];
    /* set default frame rate as 30 fps */
    icpSettings->frameRate = 30;

    /* Set SurfaceAllocAttrs */
    ctx->surfAllocAttrs[0].type = NVM_SURF_ATTR_WIDTH;
    ctx->surfAllocAttrs[0].value = icpSettings->width;
    ctx->surfAllocAttrs[1].type = NVM_SURF_ATTR_HEIGHT;
    ctx->surfAllocAttrs[1].value = icpSettings->height;
    ctx->surfAllocAttrs[2].type = NVM_SURF_ATTR_EMB_LINES_TOP;
    ctx->surfAllocAttrs[2].value = property->embLinesTop;
    ctx->surfAllocAttrs[3].type = NVM_SURF_ATTR_EMB_LINES_BOTTOM;
    ctx->surfAllocAttrs[3].value = property->embLinesBottom;
    ctx->surfAllocAttrs[4].type = NVM_SURF_ATTR_CPU_ACCESS;
    ctx->surfAllocAttrs[4].value = NVM_SURF_ATTR_CPU_ACCESS_CACHED;
    ctx->surfAllocAttrs[5].type = NVM_SURF_ATTR_ALLOC_TYPE;
    ctx->surfAllocAttrs[5].value = NVM_SURF_ATTR_ALLOC_ISOCHRONOUS;
    ctx->numSurfAllocAttrs = 6;

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
_CreateImageQueue(NvMediaDevice *device,
                  NvMediaICP *icpCtx,
                  NvQueue **queue,
                  uint32_t queueSize,
                  uint32_t imagesPerGroup,
                  uint32_t width,
                  uint32_t height,
                  NvMediaSurfaceType surfType,
                  NvMediaSurfAllocAttr *surfAllocAttrs,
                  uint32_t numSurfAllocAttrs)
{
    uint32_t i = 0, j = 0;
    NvMediaImageGroup *imageGroup = NULL;
    NvMediaStatus status = NVMEDIA_STATUS_OK;

    if (NvQueueCreate(queue,
                      queueSize,
                      sizeof(NvMediaImageGroup *)) != NVMEDIA_STATUS_OK) {
       LOG_ERR("%s: Failed to create image Queue \n", __func__);
       goto failed;
    }

    for (i = 0; i < queueSize; i++) {
        imageGroup = calloc(1, sizeof(NvMediaImageGroup));
        if (imageGroup == NULL) {
            LOG_ERR("%s: Failed to create image group %u\n", __func__, i);
            status = NVMEDIA_STATUS_OUT_OF_MEMORY;
            goto failed;
        }

        for (j = 0; j < imagesPerGroup; j++) {
            imageGroup->imageList[j] =  NvMediaImageCreateNew(device,              // device
                                                              surfType,            // NvMediaSurfaceType type
                                                              surfAllocAttrs,      // surf allocation attrs
                                                              numSurfAllocAttrs,   // num attrs
                                                              0);                  // flags
            if (!imageGroup->imageList[j]) {
                LOG_ERR("%s: NvMediaImageCreate failed for image %u in group %u",
                        __func__, j, i);
                status = NVMEDIA_STATUS_ERROR;
                goto failed;
            }
            imageGroup->imageList[j]->tag = imageGroup;
        }

        imageGroup->numImages = imagesPerGroup;
        imageGroup->tag =  *queue;

        if (IsFailed(NvQueuePut(*queue,
                                (void *)&imageGroup,
                                NV_TIMEOUT_INFINITE))) {
            LOG_ERR("%s: Pushing image group %u to image queue failed\n",
                    __func__, i);
            status = NVMEDIA_STATUS_ERROR;
            goto failed;
        }
        status = NvMediaICPRegisterImageGroup(icpCtx, imageGroup);
        if(status != NVMEDIA_STATUS_OK) {
            LOG_ERR("%s: Pinning failed for camera %d \n", __func__, i);
            goto failed;
        }
        imageGroup = NULL;
    }

    return NVMEDIA_STATUS_OK;
failed:
    /*imageGroup's already pushed in the Queue will be cleaned up in CaptureFini*/
    if(imageGroup) {
        for (j = 0; j < imagesPerGroup; j++) {
            if(imageGroup->imageList[j]) {
                NvMediaImageDestroy(imageGroup->imageList[j]);
                imageGroup->imageList[j] = NULL;
            }
        }
        free(imageGroup);
        imageGroup = NULL;
    }
    return status;
}

static void
_PrintICPErrorCsiInputFrameInfo(NvMediaICPErrorInfo *errInfo)
{
    uint32_t i = 0, size = ARRAY_SIZE(gCaptureCsiInputFrameErrTbl);
    uint32_t status = 0;

    for (i = 0; i < size; i++) {
        if (errInfo->errorData & (1 << gCaptureCsiInputFrameErrTbl[i].error)) {
            status = 1;
            LOG_ERR("In CSI-Stream:%u, CSI-FrameId:%u, CSI-VC:%u, error Type:%s(%s)\n",
                    errInfo->csiStreamId,
                    errInfo->csiFrameId, errInfo->virtualChannelId,
                    gCaptureCsiInputFrameErrTbl[i].errType,
                    gCaptureCsiInputFrameErrTbl[i].errMsg);
        }
    }

    if (status == 0) {
        LOG_ERR("%s: Invalid ICP Error reported \n", __func__);
    }
}

static void
_PrintICPErrorCsiInputStreamInfo(NvMediaICPErrorInfo *errInfo)
{
    uint32_t i = 0, size = ARRAY_SIZE(gCaptureCsiInputStreamErrTbl);
    uint32_t status = 0;

    for (i = 0; i < size; i++) {
        if (errInfo->errorData & (1 << gCaptureCsiInputStreamErrTbl[i].error)) {
            status = 1;
            LOG_ERR("In CSI-Stream:%u, CSI-FrameId:%u, CSI-VC:%u, error Type:%s(%s)\n",
                    errInfo->csiStreamId,
                    errInfo->csiFrameId, errInfo->virtualChannelId,
                    gCaptureCsiInputStreamErrTbl[i].errType,
                    gCaptureCsiInputStreamErrTbl[i].errMsg);
        }
    }

    if (status == 0) {
        LOG_ERR("%s: Invalid ICP Error reported \n", __func__);
    }
}

static void
_PrintICPErrorPixelFaultInfo(NvMediaICPErrorInfo *errInfo)
{
    uint32_t i = 0, size = ARRAY_SIZE(gCaptureViPixelFaultErrTbl);
    uint32_t status = 0;

    for (i = 0; i < size; i++) {
        if (errInfo->errorData & (1 << gCaptureViPixelFaultErrTbl[i].error)) {
            status = 1;
            LOG_ERR("In CSI-Stream:%u, CSI-FrameId:%u, CSI-VC:%u, error Type:%s(%s)\n",
                    errInfo->csiStreamId,
                    errInfo->csiFrameId, errInfo->virtualChannelId,
                    gCaptureViPixelFaultErrTbl[i].errType,
                    gCaptureViPixelFaultErrTbl[i].errMsg);
        }
    }

    if (status == 0) {
        LOG_ERR("%s: Invalid ICP Error reported %x: %d \n", __func__, errInfo->errorData, size);
    }
}

static void
_PrintICPErrorFrameFaultInfo(NvMediaICPErrorInfo *errInfo, bool isNotifiedBits)
{
    uint32_t i = 0, size = ARRAY_SIZE(gCaptureViFrameFaultErrTbl);
    uint32_t status = 0;

    for (i = 0; i < size; i++) {
        if (isNotifiedBits == true) {
            if ((errInfo->errorData & (1 << gCaptureViFrameFaultErrTbl[i].error)) != 0) {
                status = 1;
                LOG_ERR("In CSI-Stream:%u, CSI-FrameId:%u, CSI-VC:%u, error Type:%s(%s)\n",
                        errInfo->csiStreamId,
                        errInfo->csiFrameId, errInfo->virtualChannelId,
                        gCaptureViFrameFaultErrTbl[i].errType,
                        gCaptureViFrameFaultErrTbl[i].errMsg);
            }
        } else {
            if (errInfo->errorData == gCaptureViFrameFaultErrTbl[i].error) {
                status = 1;
                LOG_ERR("In CSI-Stream:%u, CSI-FrameId:%u, CSI-VC:%u, error Type:%s(%s)\n",
                        errInfo->csiStreamId,
                        errInfo->csiFrameId, errInfo->virtualChannelId,
                        gCaptureViFrameFaultErrTbl[i].errType,
                        gCaptureViFrameFaultErrTbl[i].errMsg);
            }
        }
    }

    if ((errInfo->errorData != 0) && (status == 0)) {
        LOG_ERR("%s: Invalid ICP Error reported %x: %d \n", __func__, errInfo->errorData, size);
    }
}

static void
_PrintICPErrorMemoryWriteInfo(NvMediaICPErrorInfo *errInfo, bool isNotifiedBits)
{
    uint32_t i = 0, size = ARRAY_SIZE(gCaptureViMemoryWriteErrTbl);
    uint32_t status = 0;

    for (i = 0; i < size; i++) {
        if (isNotifiedBits == true) {
            if ((errInfo->errorData & (1 << gCaptureViMemoryWriteErrTbl[i].error)) != 0) {
                status = 1;
                LOG_ERR("In CSI-Stream:%u, CSI-FrameId:%u, CSI-VC:%u, error Type:%s(%s)\n",
                        errInfo->csiStreamId,
                        errInfo->csiFrameId, errInfo->virtualChannelId,
                        gCaptureViFrameFaultErrTbl[i].errType,
                        gCaptureViFrameFaultErrTbl[i].errMsg);
            }
        } else {
            if (errInfo->errorData == gCaptureViMemoryWriteErrTbl[i].error) {
                status = 1;
                LOG_ERR("In CSI-Stream:%u, CSI-FrameId:%u, CSI-VC:%u, error Type:%s(%s)\n",
                        errInfo->csiStreamId,
                        errInfo->csiFrameId, errInfo->virtualChannelId,
                        gCaptureViMemoryWriteErrTbl[i].errType,
                        gCaptureViMemoryWriteErrTbl[i].errMsg);
            }
        }
    }

    if ((errInfo->errorData != 0) && (status == 0)) {
        LOG_ERR("%s: Invalid ICP Error reported %x: %d \n", __func__, errInfo->errorData, size);
    }
}

static void
_PrintICPErrorInfo(NvMediaICPErrorInfo *errInfo, bool isNotifiedBits)
{
    switch (errInfo->errorStatus) {
        case NVMEDIA_ICP_ERROR_STATUS_CSI_INPUTFRAME:
             _PrintICPErrorCsiInputFrameInfo(errInfo);
             break;
        case NVMEDIA_ICP_ERROR_STATUS_CSI_INPUTSTREAM:
             _PrintICPErrorCsiInputStreamInfo(errInfo);
              break;
        case NVMEDIA_ICP_ERROR_STATUS_PIXEL_FAULT:
             _PrintICPErrorPixelFaultInfo(errInfo);
             break;
        case NVMEDIA_ICP_ERROR_STATUS_FRAME_FAULT:
             _PrintICPErrorFrameFaultInfo(errInfo, isNotifiedBits);
             break;
        case NVMEDIA_ICP_ERROR_STATUS_MEMORYWRITE:
             _PrintICPErrorMemoryWriteInfo(errInfo, isNotifiedBits);
             break;
        default:
                break;
    }
}

static void
_PrintICPNotifiedErrorInfo(NvMediaICPErrorInfo *errInfo)
{
    NvMediaICPErrorInfo notifiedInfo = *errInfo;
    uint32_t i;

    LOG_ERR("reporting all notified error\n");
    for (i = NVMEDIA_ICP_ERROR_STATUS_CSI_INPUTFRAME;
         i <= NVMEDIA_ICP_ERROR_STATUS_MEMORYWRITE; i++) {
        notifiedInfo.errorData = errInfo->notifiedErrors[i];
        notifiedInfo.errorStatus = i;
        if (notifiedInfo.errorData) {

            _PrintICPErrorInfo(&notifiedInfo, true);
        }
    }
}

static uint32_t
_CaptureThreadFunc(void *data)
{
    CaptureThreadCtx *threadCtx = (CaptureThreadCtx *)data;
    uint32_t i = 0, totalCapturedFrames = 0, lastCapturedFrame = 0;
    NvMediaImageGroup *capturedImageGroup = NULL;
    NvMediaImageGroup *feedImageGroup = NULL;
    NvMediaStatus status;
    uint64_t tbegin = 0, tend = 0, fps;
    NvMediaICP *icpInst = NULL;
    uint32_t retry = 0;
    NvMediaICPErrorInfo icpErrInfo;
    uint64_t td;
    NvMediaGlobalTime globalTimeStamp;

    for (i = 0; i < threadCtx->icpExCtx->numVirtualGroups; i++) {
        if (threadCtx->icpExCtx->icp[i].virtualGroupId == threadCtx->virtualGroupIndex) {
            icpInst = NVMEDIA_ICP_HANDLER(threadCtx->icpExCtx, i);
            break;
        }
    }
    if (!icpInst) {
        LOG_ERR("%s: Failed to get icpInst for virtual channel %d\n", __func__,
                threadCtx->virtualGroupIndex);
        goto done;
    }
    while (!(*threadCtx->quit) && !(*threadCtx->suspend)) {

        /* Set current frame to be an offset by frames to skip */
        threadCtx->currentFrame = i;

        /* Feed all images to image capture object from the input Queue */
        while (NvQueueGet(threadCtx->inputQueue,
                          &feedImageGroup,
                          0) == NVMEDIA_STATUS_OK) {
            status = NvMediaICPFeedImageGroup(icpInst,
                                         feedImageGroup,
                                         CAPTURE_FEED_FRAME_TIMEOUT);
            if (status != NVMEDIA_STATUS_OK) {
                LOG_ERR("%s: %d: NvMediaICPFeedImageGroup failed\n", __func__, __LINE__);
                if (NvQueuePut((NvQueue *)feedImageGroup->tag,
                               (void *)&feedImageGroup,
                               0) != NVMEDIA_STATUS_OK) {
                    LOG_ERR("%s: Failed to put image group back into capture input queue", __func__);
                    *threadCtx->quit = NVMEDIA_TRUE;
                    status = NVMEDIA_STATUS_ERROR;
                    goto done;
                }
                feedImageGroup = NULL;
                *threadCtx->quit = NVMEDIA_TRUE;
                goto done;
            }
        }

        /* Get captured frame */
        status = NvMediaICPGetImageGroup(icpInst,
                                         CAPTURE_GET_FRAME_TIMEOUT,
                                         &capturedImageGroup);
        switch (status) {
            case NVMEDIA_STATUS_OK:
                retry = 0;
                break;
            case NVMEDIA_STATUS_TIMED_OUT:
                LOG_WARN("%s: NvMediaICPGetImageGroup timed out\n", __func__);
                if (++retry > CAPTURE_MAX_RETRY) {
                    LOG_ERR("%s: keep failing at NvMediaICPGetImageGroup for %d times\n", __func__, retry);
                    *threadCtx->quit = NVMEDIA_TRUE;
                    goto done;
                }
                continue;
            case NVMEDIA_STATUS_NONE_PENDING:
                LOG_WARN("%s: NvMediaICPGetImageGroup failed as no frame buffers are available for capture."
                         "Please feed frames using NvMediaICPFeedImageGroup\n", __func__);
                continue;
            case NVMEDIA_STATUS_ERROR:
                status = NvMediaICPGetErrorInfo(icpInst, &icpErrInfo);
                if (status == NVMEDIA_STATUS_OK) {
                    _PrintICPErrorInfo(&icpErrInfo, false);
                    _PrintICPNotifiedErrorInfo(&icpErrInfo);
                    /* continue on capture */
                    break;
                }
                LOG_ERR("%s: NvMediaICPGetImageGroup failed with getting error info\n", __func__);
                *threadCtx->quit = NVMEDIA_TRUE;
                goto done;
            case NVMEDIA_STATUS_PFSD_ERROR:
                /* app needs to be stopped */
                LOG_ERR("%s: NvMediaICPGetImageGroup failed with PFSD error\n", __func__);
                *threadCtx->quit = NVMEDIA_TRUE;
                goto done;
            case NVMEDIA_STATUS_UNDEFINED_STATE:
                /* app needs to be stopped */
                LOG_ERR("%s: NvMediaICPGetImageGroup failed from sys call\n", __func__);
                *threadCtx->quit = NVMEDIA_TRUE;
                goto done;
            default:
                LOG_ERR("%s: NvMediaICPGetImageGroup failed\n", __func__);
                *threadCtx->quit = NVMEDIA_TRUE;
                goto done;
        }

        if (capturedImageGroup != NULL) {
            if (threadCtx->enableTimeStamp) {
                (void)NvMediaImageGetGlobalTimeStamp(capturedImageGroup->imageList[0], &globalTimeStamp);
                printf("capture TSC is %lu, global timestamp is %lu\n",
                        capturedImageGroup->captureTimeStamp[0], globalTimeStamp);
            }
        } else {
            LOG_ERR("%s: NvMediaICPGetImageGroup returned NULL\n", __func__);
            *threadCtx->quit = NVMEDIA_TRUE;
            goto done;
        }

        GetTimeMicroSec(&tend);
        td = tend - tbegin;
        if (td > 3000000) {
            fps = (int)(totalCapturedFrames-lastCapturedFrame)*(1000000.0/td);

            tbegin = tend;
            lastCapturedFrame = totalCapturedFrames;
            LOG_INFO("%s: VC:%d FPS=%d delta=%lld", __func__,
                     threadCtx->virtualGroupIndex, fps, td);
        }

        /* Push the captured image onto output queue
         * TODO: Currently puts only the first image from the group onto
         * output for saving/displaying */
        status = NvQueuePut(threadCtx->outputQueue,
                            (void *)&capturedImageGroup->imageList[0],
                            CAPTURE_ENQUEUE_TIMEOUT);
        if (status != NVMEDIA_STATUS_OK) {
            LOG_INFO("%s: Failed to put image onto capture output queue", __func__);
            goto done;
        }

        totalCapturedFrames++;

        capturedImageGroup = NULL;
done:
        if (capturedImageGroup) {
            status = NvQueuePut((NvQueue *)capturedImageGroup->tag,
                                (void *)&capturedImageGroup,
                                0);
            if (status != NVMEDIA_STATUS_OK) {
                LOG_ERR("%s: Failed to put image group back into capture input queue", __func__);
                *threadCtx->quit = NVMEDIA_TRUE;
            }
            capturedImageGroup = NULL;
        }
        i++;

        /* To stop capturing if specified number of frames are captured */
        if (threadCtx->numFramesToCapture &&
           (totalCapturedFrames == threadCtx->numFramesToCapture))
            break;
    }

    /* Release all the frames which are fed */
    while (NvMediaICPReleaseImageGroup(icpInst, &capturedImageGroup) == NVMEDIA_STATUS_OK) {
        if (capturedImageGroup) {
            status = NvQueuePut((NvQueue *)capturedImageGroup->tag,
                                (void *)&capturedImageGroup,
                                0);
            if (status != NVMEDIA_STATUS_OK) {
                LOG_ERR("%s: Failed to put image group back into input queue", __func__);
                break;
            }
        }
        capturedImageGroup = NULL;
    }

    LOG_INFO("%s: Capture thread exited\n", __func__);
    threadCtx->exitedFlag = NVMEDIA_TRUE;
    return NVMEDIA_STATUS_OK;
}

NvMediaStatus
CaptureInit(NvMainContext *mainCtx)
{
    NvCaptureContext *captureCtx = NULL;
    NvMediaStatus status;
    TestArgs *testArgs = mainCtx->testArgs;
    ExtImgDevParam *extImgDevParam;
    uint32_t defaultCaptureSet = 0;
    uint32_t i = 0;

    if (!mainCtx) {
        LOG_ERR("%s: Bad parameter", __func__);
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    mainCtx->ctxs[CAPTURE_ELEMENT]= malloc(sizeof(NvCaptureContext));
    if (!mainCtx->ctxs[CAPTURE_ELEMENT]) {
        LOG_ERR("%s: Failed to allocate memory for capture context\n", __func__);
        return NVMEDIA_STATUS_OUT_OF_MEMORY;
    }

    captureCtx = mainCtx->ctxs[CAPTURE_ELEMENT];
    memset(captureCtx, 0, sizeof(NvCaptureContext));

    /* initialize capture context */
    captureCtx->quit = &mainCtx->quit;
    captureCtx->suspend = &mainCtx->suspend;
    captureCtx->testArgs  = testArgs;
    captureCtx->numSensors = testArgs->numSensors;
    captureCtx->numVirtualChannels = testArgs->numVirtualChannels;
    extImgDevParam = &captureCtx->extImgDevParam;

    for (i = 0; i < captureCtx->numVirtualChannels; i++) {
        captureCtx->captureParams[i] = &testArgs->captureConfigCollection[
                            testArgs->config[i].uIntValue];
    }

    /* TBD: Currently, all cameras created using same ISC configuration.
       Need to change this once HW supports capture with various camera
       configurations */
    for (i = 0; i < captureCtx->numVirtualChannels; i++) {
        if (testArgs->config[i].isUsed) {
            defaultCaptureSet = i;
            break;
        }
    }

    /* Set ExtImgDev params */
    status = _SetExtImgDevParameters(captureCtx,
                captureCtx->captureParams[defaultCaptureSet], extImgDevParam);
    if (status != NVMEDIA_STATUS_OK) {
        LOG_ERR("%s: Failed to set ISC device parameters\n", __func__);
        goto failed;
    }

    /* Create ExtImgDev object */
    captureCtx->extImgDevice = ExtImgDevInit(extImgDevParam);
    if (!captureCtx->extImgDevice) {
        LOG_ERR("%s: Failed to initialize ISC devices\n", __func__);
        status = NVMEDIA_STATUS_ERROR;
        goto failed;
    }

    /* Create NvMedia Device */
    captureCtx->device = NvMediaDeviceCreate();
    if (!captureCtx->device) {
        status = NVMEDIA_STATUS_ERROR;
        LOG_ERR("%s: Failed to create NvMedia device\n", __func__);
        goto failed;
    }

    /* Set NvMediaICPSettingsEx */
    captureCtx->icpSettingsEx.numVirtualGroups = captureCtx->numVirtualChannels;
    captureCtx->icpSettingsEx.interfaceType = captureCtx->extImgDevice->property.interface;
    captureCtx->icpSettingsEx.interfaceLanes = captureCtx->captureParams[defaultCaptureSet]->csiLanes;
    captureCtx->icpSettingsEx.phyMode = captureCtx->extImgDevice->property.phyMode;

    for (i = 0; i < captureCtx->icpSettingsEx.numVirtualGroups; i++) {
        status = _SetICPSettings(&captureCtx->threadCtx[i],
                                 NVMEDIA_ICP_SETTINGS_HANDLER(captureCtx->icpSettingsEx, i, 0),
                                 &captureCtx->extImgDevice->property,
                                 captureCtx->captureParams[defaultCaptureSet],
                                 testArgs);
        if (status != NVMEDIA_STATUS_OK) {
            LOG_ERR("%s: Failed to set ICP settings\n", __func__);
            status = NVMEDIA_STATUS_ERROR;
            goto failed;
        }

        captureCtx->icpSettingsEx.virtualGroups[i].numVirtualChannels = 1;
        if (captureCtx->extImgDevice->property.tpgEnable)
            captureCtx->icpSettingsEx.virtualGroups[i].virtualChannels[0].virtualChannelIndex = captureCtx->extImgDevice->property.vcId[i] + testArgs->tpgChannelOffset;
        else
            captureCtx->icpSettingsEx.virtualGroups[i].virtualChannels[0].virtualChannelIndex = captureCtx->extImgDevice->property.vcId[i];
    }

    /* Create NvMediaICPEx object */
    captureCtx->icpExCtx = NvMediaICPCreateEx(&captureCtx->icpSettingsEx);
    if (!captureCtx->icpExCtx) {
        LOG_ERR("%s: NvMediaICPCreateEx failed\n", __func__);
        status = NVMEDIA_STATUS_ERROR;
        goto failed;
    }


    /* Create Input Queues and set data for capture threads */
    for (i = 0; i < captureCtx->numVirtualChannels; i++) {
        captureCtx->threadCtx[i].icpExCtx = captureCtx->icpExCtx;
        captureCtx->threadCtx[i].quit = captureCtx->quit;
        captureCtx->threadCtx[i].suspend = captureCtx->suspend;
        captureCtx->threadCtx[i].exitedFlag = NVMEDIA_TRUE;
        captureCtx->threadCtx[i].virtualGroupIndex = i;
        captureCtx->threadCtx[i].width  = NVMEDIA_ICP_SETTINGS_HANDLER(captureCtx->icpSettingsEx,i, 0)->width;
        captureCtx->threadCtx[i].height = NVMEDIA_ICP_SETTINGS_HANDLER(captureCtx->icpSettingsEx,i, 0)->height;
        captureCtx->threadCtx[i].settings = NVMEDIA_ICP_SETTINGS_HANDLER(captureCtx->icpSettingsEx,i, 0);
        captureCtx->threadCtx[i].numFramesToCapture = testArgs->numFrames.uIntValue;
        captureCtx->threadCtx[i].numBuffers = CAPTURE_INPUT_QUEUE_SIZE;
        captureCtx->threadCtx[i].enableTimeStamp = testArgs->enableTimeStamp;

        /* Create inputQueue for storing captured Images */
        status = _CreateImageQueue(captureCtx->device,
                                   NVMEDIA_ICP_HANDLER(captureCtx->icpExCtx, i),
                                   &captureCtx->threadCtx[i].inputQueue,
                                   CAPTURE_INPUT_QUEUE_SIZE,
                                   CAPTURE_IMAGES_PER_GROUP,
                                   captureCtx->threadCtx[i].width,
                                   captureCtx->threadCtx[i].height,
                                   captureCtx->threadCtx[i].surfType,
                                   captureCtx->threadCtx[i].surfAllocAttrs,
                                   captureCtx->threadCtx[i].numSurfAllocAttrs);
        if (status != NVMEDIA_STATUS_OK) {
            LOG_ERR("%s: capture InputQueue %d creation failed\n", __func__, i);
            goto failed;
        }

        LOG_DBG("%s: Capture Input Queue %d: %ux%u, images: %u \n",
                __func__, i, captureCtx->threadCtx[i].width,
                captureCtx->threadCtx[i].height,
                CAPTURE_INPUT_QUEUE_SIZE);
    }

    /* Start ExtImgDevice */
    if (captureCtx->extImgDevice)
        ExtImgDevStart(captureCtx->extImgDevice);

    return NVMEDIA_STATUS_OK;
failed:
    LOG_ERR("%s: Failed to initialize Capture\n", __func__);
    return status;
}

NvMediaStatus
CaptureFini(NvMainContext *mainCtx)
{
    NvCaptureContext *captureCtx = NULL;
    NvMediaImageGroup *imageGroup = NULL;
    NvMediaStatus status;
    uint32_t i = 0, j = 0;

    if (!mainCtx)
        return NVMEDIA_STATUS_OK;

    captureCtx = mainCtx->ctxs[CAPTURE_ELEMENT];
    if (!captureCtx)
        return NVMEDIA_STATUS_OK;

    /* Destroy input queues */
    for (i = 0; i < captureCtx->numVirtualChannels; i++) {
        if (captureCtx->threadCtx[i].inputQueue) {
            while ((NvQueueGet(captureCtx->threadCtx[i].inputQueue, &imageGroup,
                        0)) == NVMEDIA_STATUS_OK) {
                if (imageGroup) {
                    status = NvMediaICPUnregisterImageGroup(
                                 NVMEDIA_ICP_HANDLER(captureCtx->threadCtx[i].icpExCtx, i),
                                 imageGroup
                                 );
                    if(status != NVMEDIA_STATUS_OK) {
                        LOG_ERR("%s: UnPinning failed for camera %d \n", __func__, i);
                    }
                    for (j = 0; j < imageGroup->numImages; j++) {
                        NvMediaImageDestroy(imageGroup->imageList[j]);
                        imageGroup->imageList[j] = NULL;
                    }
                    free(imageGroup);
                    imageGroup = NULL;
                }
            }
            LOG_DBG("%s: Destroying capture input queue %d \n", __func__, i);
            NvQueueDestroy(captureCtx->threadCtx[i].inputQueue);
        }
    }

    /* Destroy capture contexts */
    if (captureCtx->icpExCtx) {
        NvMediaICPDestroyEx(captureCtx->icpExCtx);
        captureCtx->icpExCtx = NULL;
    }

    if (captureCtx->device)
        NvMediaDeviceDestroy(captureCtx->device);

    if (captureCtx->extImgDevice) {
        ExtImgDevDeinit(captureCtx->extImgDevice);
        captureCtx->extImgDevice = NULL;
    }

    if (captureCtx)
        free(captureCtx);

    LOG_INFO("%s: CaptureFini done\n", __func__);

    return NVMEDIA_STATUS_OK;
}

NvMediaStatus
CaptureProc(NvMainContext *mainCtx)
{
    NvMediaStatus status;
    uint32_t i=0;

    if (!mainCtx) {
        LOG_ERR("%s: Bad parameter\n", __func__);
        return NVMEDIA_STATUS_ERROR;
    }

    NvCaptureContext *captureCtx  = mainCtx->ctxs[CAPTURE_ELEMENT];
    NvSaveContext    *saveCtx     = mainCtx->ctxs[SAVE_ELEMENT];

    /* Setting the queues */
    for (i = 0; i < captureCtx->numVirtualChannels; i++) {
        CaptureThreadCtx *threadCtx = &captureCtx->threadCtx[i];
        if (threadCtx)
            threadCtx->outputQueue = saveCtx->threadCtx[i].inputQueue;
    }

    /* Create capture threads */
    for (i = 0; i < captureCtx->numVirtualChannels; i++) {
        CaptureThreadCtx *threadCtx = &captureCtx->threadCtx[i];
        if (threadCtx) {
            threadCtx->exitedFlag = NVMEDIA_FALSE;
            status = NvThreadCreate(&captureCtx->captureThread[i],
                                    &_CaptureThreadFunc,
                                    (void *)threadCtx,
                                    NV_THREAD_PRIORITY_NORMAL);
            if (status != NVMEDIA_STATUS_OK) {
                LOG_ERR("%s: Failed to create captureThread %d\n",
                        __func__, i);
                threadCtx->exitedFlag = NVMEDIA_TRUE;
                goto failed;
            }
        }
    }
failed:
    return status;
}

NvMediaStatus
CaptureSuspend(NvMainContext *mainCtx)
{
    NvCaptureContext *captureCtx = NULL;
    NvMediaStatus status;
    uint32_t i = 0;

    if (!mainCtx)
        return NVMEDIA_STATUS_OK;

    captureCtx = mainCtx->ctxs[CAPTURE_ELEMENT];
    if (!captureCtx)
        return NVMEDIA_STATUS_OK;

    /* Wait for threads to exit */
    for (i = 0; i < captureCtx->numVirtualChannels; i++) {
        if (captureCtx->captureThread[i]) {
            while (!captureCtx->threadCtx[i].exitedFlag) {
                LOG_DBG("%s: Waiting for capture thread %d to quit\n",
                        __func__, i);
            }
        }
    }

    /* Destroy threads */
    for (i = 0; i < captureCtx->numVirtualChannels; i++) {
        if (captureCtx->captureThread[i]) {
            status = NvThreadDestroy(captureCtx->captureThread[i]);
            captureCtx->captureThread[i] = NULL;
            if (status != NVMEDIA_STATUS_OK)
                LOG_ERR("%s: Failed to destroy capture thread %d\n",
                        __func__, i);
        }
    }
    if(!mainCtx->quit) {
        /* Destroy capture contexts */
        if (captureCtx->icpExCtx) {
            NvMediaICPDestroyEx(captureCtx->icpExCtx);
            captureCtx->icpExCtx = NULL;
        }

        if (captureCtx->extImgDevice) {
            ExtImgDevDeinit(captureCtx->extImgDevice);
            captureCtx->extImgDevice = NULL;
        }
    }

    LOG_INFO("%s: done\n", __func__);
    return NVMEDIA_STATUS_OK;
}

NvMediaStatus
CaptureResume(NvMainContext *mainCtx)
{
    NvCaptureContext *captureCtx = NULL;
    NvSaveContext    *saveCtx = NULL;
    NvMediaStatus status;
    TestArgs *testArgs;
    ExtImgDevParam *extImgDevParam;
    uint32_t defaultCaptureSet = 0;
    uint32_t i = 0;

    if (!mainCtx) {
        LOG_ERR("%s: Bad parameter", __func__);
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    saveCtx = mainCtx->ctxs[SAVE_ELEMENT];
    testArgs = mainCtx->testArgs;
    captureCtx = mainCtx->ctxs[CAPTURE_ELEMENT];

    /* initialize capture context */
    captureCtx->quit = &mainCtx->quit;
    captureCtx->suspend = &mainCtx->suspend;
    captureCtx->testArgs  = testArgs;
    captureCtx->numSensors = testArgs->numSensors;
    captureCtx->numVirtualChannels = testArgs->numVirtualChannels;
    extImgDevParam = &captureCtx->extImgDevParam;

    for (i = 0; i < captureCtx->numVirtualChannels; i++) {
        captureCtx->captureParams[i] = &testArgs->captureConfigCollection[
                            testArgs->config[i].uIntValue];
    }

    /* TBD: Currently, all cameras created using same ISC configuration.
       Need to change this once HW supports capture with various camera
       configurations */
    for (i = 0; i < captureCtx->numVirtualChannels; i++) {
        if (testArgs->config[i].isUsed) {
            defaultCaptureSet = i;
            break;
        }
    }

    /* Set ExtImgDev params */
    status = _SetExtImgDevParameters(captureCtx,
                captureCtx->captureParams[defaultCaptureSet], extImgDevParam);
    if (status != NVMEDIA_STATUS_OK) {
        LOG_ERR("%s: Failed to set ISC device parameters\n", __func__);
        goto failed;
    }

    /* Create ExtImgDev object */
    captureCtx->extImgDevice = ExtImgDevInit(extImgDevParam);
    if (!captureCtx->extImgDevice) {
        LOG_ERR("%s: Failed to initialize ISC devices\n", __func__);
        status = NVMEDIA_STATUS_ERROR;
        goto failed;
    }

    /* Set NvMediaICPSettingsEx */
    captureCtx->icpSettingsEx.numVirtualGroups = captureCtx->numVirtualChannels;
    captureCtx->icpSettingsEx.interfaceType = captureCtx->extImgDevice->property.interface;
    captureCtx->icpSettingsEx.interfaceLanes = captureCtx->captureParams[defaultCaptureSet]->csiLanes;
    captureCtx->icpSettingsEx.phyMode = captureCtx->extImgDevice->property.phyMode;

    for (i = 0; i < captureCtx->icpSettingsEx.numVirtualGroups; i++) {
        status = _SetICPSettings(&captureCtx->threadCtx[i],
                                 NVMEDIA_ICP_SETTINGS_HANDLER(captureCtx->icpSettingsEx, i, 0),
                                 &captureCtx->extImgDevice->property,
                                 captureCtx->captureParams[defaultCaptureSet],
                                 testArgs);
        if (status != NVMEDIA_STATUS_OK) {
            LOG_ERR("%s: Failed to set ICP settings\n", __func__);
            status = NVMEDIA_STATUS_ERROR;
            goto failed;
        }

        captureCtx->icpSettingsEx.virtualGroups[i].numVirtualChannels = 1;
        captureCtx->icpSettingsEx.virtualGroups[i].virtualChannels[0].virtualChannelIndex =
                                                    captureCtx->extImgDevice->property.vcId[i];
    }

    /* Create NvMediaICPEx object */
    captureCtx->icpExCtx = NvMediaICPCreateEx(&captureCtx->icpSettingsEx);
    if (!captureCtx->icpExCtx) {
        LOG_ERR("%s: NvMediaICPCreateEx failed\n", __func__);
        status = NVMEDIA_STATUS_ERROR;
        goto failed;
    }

    /* Create Input Queues and set data for capture threads */
    for (i = 0; i < captureCtx->numVirtualChannels; i++) {
        captureCtx->threadCtx[i].icpExCtx = captureCtx->icpExCtx;
        captureCtx->threadCtx[i].quit = captureCtx->quit;
        captureCtx->threadCtx[i].suspend = captureCtx->suspend;
        captureCtx->threadCtx[i].exitedFlag = NVMEDIA_TRUE;
        captureCtx->threadCtx[i].virtualGroupIndex = i;
        captureCtx->threadCtx[i].width  = NVMEDIA_ICP_SETTINGS_HANDLER(captureCtx->icpSettingsEx,i, 0)->width;
        captureCtx->threadCtx[i].height = NVMEDIA_ICP_SETTINGS_HANDLER(captureCtx->icpSettingsEx,i, 0)->height;
        captureCtx->threadCtx[i].settings = NVMEDIA_ICP_SETTINGS_HANDLER(captureCtx->icpSettingsEx,i, 0);
        captureCtx->threadCtx[i].numFramesToCapture = testArgs->numFrames.uIntValue;
        captureCtx->threadCtx[i].numBuffers = CAPTURE_INPUT_QUEUE_SIZE;

    }
    /* Start ExtImgDevice */
    if (captureCtx->extImgDevice)
        ExtImgDevStart(captureCtx->extImgDevice);

    /* Setting the queues */
    for (i = 0; i < captureCtx->numVirtualChannels; i++) {
        CaptureThreadCtx *threadCtx = &captureCtx->threadCtx[i];
        if (threadCtx)
            threadCtx->outputQueue = saveCtx->threadCtx[i].inputQueue;
    }

    /* Create capture threads */
    for (i = 0; i < captureCtx->numVirtualChannels; i++) {
        CaptureThreadCtx *threadCtx = &captureCtx->threadCtx[i];
        if (threadCtx) {
            threadCtx->exitedFlag = NVMEDIA_FALSE;
            status = NvThreadCreate(&captureCtx->captureThread[i],
                                    &_CaptureThreadFunc,
                                    (void *)threadCtx,
                                    NV_THREAD_PRIORITY_NORMAL);
            if (status != NVMEDIA_STATUS_OK) {
                LOG_ERR("%s: Failed to create captureThread %d\n",
                        __func__, i);
                threadCtx->exitedFlag = NVMEDIA_TRUE;
                goto failed;
            }
        }
    }
failed:
    return status;
}
