AM62A7-Q1: tiovx issue

/*
 *
 * Copyright (c) 2024 Texas Instruments Incorporated
 *
 * All rights reserved not granted herein.
 *
 * Limited License.
 *
 * Texas Instruments Incorporated grants a world-wide, royalty-free, non-exclusive
 * license under copyrights and patents it now or hereafter owns or controls to make,
 * have made, use, import, offer to sell and sell ("Utilize") this software subject to the
 * terms herein.  With respect to the foregoing patent license, such license is granted
 * solely to the extent that any such patent is necessary to Utilize the software alone.
 * The patent license shall not apply to any combinations which include this software,
 * other than combinations with devices manufactured by or for TI ("TI Devices").
 * No hardware patent is licensed hereunder.
 *
 * Redistributions must preserve existing copyright notices and reproduce this license
 * (including the above copyright notice and the disclaimer and (if applicable) source
 * code license limitations below) in the documentation and/or other materials provided
 * with the distribution
 *
 * Redistribution and use in binary form, without modification, are permitted provided
 * that the following conditions are met:
 *
 * *       No reverse engineering, decompilation, or disassembly of this software is
 * permitted with respect to any software provided in binary form.
 *
 * *       any redistribution and use are licensed by TI for use only with TI Devices.
 *
 * *       Nothing shall obligate TI to provide you with source code for the software
 * licensed and provided to you in object code.
 *
 * If software source code is provided to you, modification and redistribution of the
 * source code are permitted provided that the following conditions are met:
 *
 * *       any redistribution and use of the source code, including any resulting derivative
 * works, are licensed by TI for use only with TI Devices.
 *
 * *       any redistribution and use of any object code compiled from the source code
 * and any resulting derivative works, are licensed by TI for use only with TI Devices.
 *
 * Neither the name of Texas Instruments Incorporated nor the names of its suppliers
 *
 * may be used to endorse or promote products derived from this software without
 * specific prior written permission.
 *
 * DISCLAIMER.
 *
 * THIS SOFTWARE IS PROVIDED BY TI AND TI'S LICENSORS "AS IS" AND ANY EXPRESS
 * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL TI AND TI'S LICENSORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
 * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
 * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 * OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include <tiovx_modules.h>
#include <tiovx_utils.h>
#include <v4l2_decode_module.h>
#include <kms_display_module.h>
#include <v4l2_capture_module.h>

#define APP_BUFQ_DEPTH      (3)
#define APP_NUM_CH          (1)
#define APP_NUM_ITERATIONS  (6000)

#define MIN(a, b) a<b?a:b

uint32_t tick()
{
  uint32_t u32TimeMs = 0;
  struct timespec time1 = {0, 0};
  if(clock_gettime(CLOCK_MONOTONIC, &time1)<0)
    return 0;
  u32TimeMs = time1.tv_sec * 1000 + time1.tv_nsec/1000000;
  return u32TimeMs;  
}

static vx_status ownCopyImage(vx_image input, vx_image output)
{
    vx_status status = VX_SUCCESS; // assume success until an error occurs.
    vx_uint32 p = 0;
    vx_uint32 y = 0;
    vx_uint32 len = 0;
    vx_size planes = 0;
    void* src;
    void* dst;
    vx_imagepatch_addressing_t src_addr;
    vx_imagepatch_addressing_t dst_addr;
    vx_rectangle_t rect;
    vx_map_id map_id1;
    vx_map_id map_id2;

    status |= vxQueryImage(input, VX_IMAGE_PLANES, &planes, sizeof(planes));
    status |= vxGetValidRegionImage(input, &rect);

    for (p = 0; p < planes && status == VX_SUCCESS; p++)
    {
        status = VX_SUCCESS;
        src = NULL;
        dst = NULL;

        status |= vxMapImagePatch(input, &rect, p, &map_id1, &src_addr, &src, VX_READ_ONLY, VX_MEMORY_TYPE_HOST, 0);
        status |= vxMapImagePatch(output, &rect, p, &map_id2, &dst_addr, &dst, VX_WRITE_ONLY, VX_MEMORY_TYPE_HOST, 0);

        for (y = 0; y < src_addr.dim_y && status == VX_SUCCESS; y += src_addr.step_y)
        {
            /*
            * in the case where the secondary planes are subsampled, the
            * scale will skip over the lines that are repeated.
            */
            vx_uint8* srcp = vxFormatImagePatchAddress2d(src, 0, y, &src_addr);
            vx_uint8* dstp = vxFormatImagePatchAddress2d(dst, 0, y, &dst_addr);

            len = (src_addr.stride_x * src_addr.dim_x * src_addr.scale_x) / VX_SCALE_UNITY;

            memcpy(dstp, srcp, len);
        }

        if (status == VX_SUCCESS)
        {
            status |= vxUnmapImagePatch(input, map_id1);
            status |= vxUnmapImagePatch(output, map_id2);
        }
    }

    return status;
}

vx_status manual_enqueue()
{
  vx_status status = VX_FAILURE;
  GraphObj graph;
  NodeObj *covert_node = NULL, *scaler_node = NULL;
  BufPool *covert_in_pool = NULL, *covert_out_pool = NULL, *msc_in_pool = NULL, *msc_out_pool[2] = {NULL};
  Buf *convert_inbuf = NULL, *convert_outbuf = NULL, *msc_inbuf = NULL, *msc_outbuf[2] = {NULL}, *tmp_buf;
  v4l2CaptureCfg v4l2_capture_cfg;
  v4l2CaptureHandle *v4l2_capture_handle;
  TIOVXColorConvertNodeCfg cfg;
  TIOVXMultiScalerNodeCfg msc_cfg;

  printf("manual_enqueue start\n");
  status = tiovx_modules_initialize_graph(&graph);
  graph.schedule_mode = VX_GRAPH_SCHEDULE_MODE_QUEUE_AUTO;

  //v4l2 capture
  v4l2_capture_init_cfg(&v4l2_capture_cfg);
  v4l2_capture_cfg.width = 1920;
  v4l2_capture_cfg.height = 1080;
  v4l2_capture_cfg.pix_format = V4L2_PIX_FMT_UYVY;
  v4l2_capture_cfg.bufq_depth = APP_BUFQ_DEPTH;
  sprintf(v4l2_capture_cfg.device, "/dev/video3");
  v4l2_capture_handle = v4l2_capture_create_handle(&v4l2_capture_cfg);

  //convert
  tiovx_color_convert_init_cfg(&cfg);
  cfg.width = 1920;
  cfg.height = 1080;
  cfg.input_cfg.color_format = VX_DF_IMAGE_UYVY;
  cfg.output_cfg.color_format = VX_DF_IMAGE_NV12;
  sprintf(cfg.target_string, TIVX_TARGET_DSP1);
  covert_node = tiovx_modules_add_node(&graph, TIOVX_COLOR_CONVERT, (void *)&cfg);
  covert_node->sinks[0].bufq_depth = APP_BUFQ_DEPTH;

  //MSC
  tiovx_multi_scaler_init_cfg(&msc_cfg);
  msc_cfg.color_format = VX_DF_IMAGE_NV12;
  msc_cfg.num_outputs = 2;
  msc_cfg.input_cfg.width = 1920;
  msc_cfg.input_cfg.height = 1080;
  msc_cfg.output_cfgs[0].width = 1920/4;
  msc_cfg.output_cfgs[0].height = 1080/4;
  msc_cfg.output_cfgs[1].width = 1920/2;
  msc_cfg.output_cfgs[1].height = 1080/2;
  sprintf(msc_cfg.target_string, TIVX_TARGET_VPAC_MSC1);
  tiovx_multi_scaler_module_crop_params_init(&msc_cfg);
  scaler_node = tiovx_modules_add_node(&graph, TIOVX_MULTI_SCALER, (void *)&msc_cfg);

//  status = tiovx_modules_link_pads(&covert_node->srcs[0], &scaler_node->sinks[0]);
  status = tiovx_modules_verify_graph(&graph);

  covert_in_pool = covert_node->sinks[0].buf_pool;
  covert_out_pool = covert_node->srcs[0].buf_pool;
  msc_in_pool = scaler_node->sinks[0].buf_pool;
  msc_out_pool[0] = scaler_node->srcs[0].buf_pool;
  msc_out_pool[1] = scaler_node->srcs[1].buf_pool;

  convert_inbuf = tiovx_modules_acquire_buf(covert_in_pool);
  v4l2_capture_enqueue_buf(v4l2_capture_handle, convert_inbuf);

  convert_outbuf = tiovx_modules_acquire_buf(covert_out_pool);
  tiovx_modules_enqueue_buf(convert_outbuf);

  msc_outbuf[0] = tiovx_modules_acquire_buf(msc_out_pool[0]);
  tiovx_modules_enqueue_buf(msc_outbuf[0]);

  msc_outbuf[1] = tiovx_modules_acquire_buf(msc_out_pool[1]);
  tiovx_modules_enqueue_buf(msc_outbuf[1]);

  printf("start tick[%d]\n", tick());
  v4l2_capture_start(v4l2_capture_handle);
  do {
    tmp_buf = v4l2_capture_dqueue_buf(v4l2_capture_handle);
  } while (tmp_buf == NULL);
  printf("capture success[%d]\n", tick());

  tiovx_modules_enqueue_buf(tmp_buf);
  tmp_buf = tiovx_modules_dequeue_buf(covert_out_pool);
  printf("covert_node dequeue success[%d]\n", tick());

  msc_inbuf = tiovx_modules_acquire_buf(msc_in_pool);
  ownCopyImage(tmp_buf->handle, msc_inbuf->handle);
  tiovx_modules_enqueue_buf(msc_inbuf);
  
  tmp_buf = tiovx_modules_dequeue_buf(msc_out_pool[0]);
  writeImage("/mnt/manual_msc_1.yuv", (vx_image)tmp_buf->handle);

  tmp_buf = tiovx_modules_dequeue_buf(msc_out_pool[1]);
  writeImage("/mnt/manual_msc_2.yuv", (vx_image)tmp_buf->handle);

  v4l2_capture_stop(v4l2_capture_handle);
  v4l2_capture_delete_handle(v4l2_capture_handle);

  tiovx_modules_release_buf(convert_inbuf);
  tiovx_modules_release_buf(convert_outbuf);
  tiovx_modules_release_buf(msc_inbuf);
  tiovx_modules_release_buf(msc_outbuf[0]);
  tiovx_modules_release_buf(msc_outbuf[1]);
  tiovx_modules_clean_graph(&graph);
  printf("manual_enqueue finish\n");
  return 0;
}

vx_status link()
{
  vx_status status = VX_FAILURE;
  GraphObj graph;
  NodeObj *covert_node = NULL, *scaler_node = NULL, *mosaic_node = NULL;
  BufPool *in_buf_pool = NULL, *out_buf_pool[2] = {NULL};
  Buf *inbuf = NULL, *outbuf[2] = {NULL};
  kmsDisplayCfg kms_display_cfg;
  kmsDisplayHandle *kms_display_handle;
  v4l2CaptureCfg v4l2_capture_cfg;
  v4l2CaptureHandle *v4l2_capture_handle;
  TIOVXColorConvertNodeCfg cfg;
  TIOVXMultiScalerNodeCfg msc_cfg;

  printf("link test start\n");
  status = tiovx_modules_initialize_graph(&graph);
  graph.schedule_mode = VX_GRAPH_SCHEDULE_MODE_QUEUE_AUTO;

  //v4l2 capture
  v4l2_capture_init_cfg(&v4l2_capture_cfg);
  v4l2_capture_cfg.width = 1920;
  v4l2_capture_cfg.height = 1080;
  v4l2_capture_cfg.pix_format = V4L2_PIX_FMT_UYVY;
  v4l2_capture_cfg.bufq_depth = APP_BUFQ_DEPTH;
  sprintf(v4l2_capture_cfg.device, "/dev/video3");
  v4l2_capture_handle = v4l2_capture_create_handle(&v4l2_capture_cfg);

  //convert
  tiovx_color_convert_init_cfg(&cfg);
  cfg.width = 1920;
  cfg.height = 1080;
  cfg.input_cfg.color_format = VX_DF_IMAGE_UYVY;
  cfg.output_cfg.color_format = VX_DF_IMAGE_NV12;
  sprintf(cfg.target_string, TIVX_TARGET_DSP1);
  covert_node = tiovx_modules_add_node(&graph, TIOVX_COLOR_CONVERT, (void *)&cfg);
  covert_node->sinks[0].bufq_depth = APP_BUFQ_DEPTH;

  //MSC
  tiovx_multi_scaler_init_cfg(&msc_cfg);
  msc_cfg.color_format = VX_DF_IMAGE_NV12;
  msc_cfg.num_outputs = 2;
  msc_cfg.input_cfg.width = 1920;
  msc_cfg.input_cfg.height = 1080;
  msc_cfg.output_cfgs[0].width = 1920/4;
  msc_cfg.output_cfgs[0].height = 1080/4;
  msc_cfg.output_cfgs[1].width = 1920/2;
  msc_cfg.output_cfgs[1].height = 1080/2;
  sprintf(msc_cfg.target_string, TIVX_TARGET_VPAC_MSC1);
  tiovx_multi_scaler_module_crop_params_init(&msc_cfg);
  scaler_node = tiovx_modules_add_node(&graph, TIOVX_MULTI_SCALER, (void *)&msc_cfg);

  status = tiovx_modules_link_pads(&covert_node->srcs[0], &scaler_node->sinks[0]);
  status = tiovx_modules_verify_graph(&graph);

  in_buf_pool = covert_node->sinks[0].buf_pool;
  out_buf_pool[0] = scaler_node->srcs[0].buf_pool;
  out_buf_pool[1] = scaler_node->srcs[1].buf_pool;

  inbuf = tiovx_modules_acquire_buf(in_buf_pool);
  v4l2_capture_enqueue_buf(v4l2_capture_handle, inbuf);

  outbuf[0] = tiovx_modules_acquire_buf(out_buf_pool[0]);
  tiovx_modules_enqueue_buf(outbuf[0]);

  outbuf[1] = tiovx_modules_acquire_buf(out_buf_pool[1]);
  tiovx_modules_enqueue_buf(outbuf[1]);

  printf("start tick[%d]\n", tick());
  v4l2_capture_start(v4l2_capture_handle);
  do {
    inbuf = v4l2_capture_dqueue_buf(v4l2_capture_handle);
  } while (inbuf == NULL);
  printf("capture success[%d]\n", tick());

  tiovx_modules_enqueue_buf(inbuf);
  outbuf[0] = tiovx_modules_dequeue_buf(out_buf_pool[0]);
  printf("tiovx_modules_dequeue_buf[0] success[%d]\n", tick());
  writeImage("/mnt/link_msc_1.yuv", (vx_image)outbuf[0]->handle);
  
  outbuf[1] = tiovx_modules_dequeue_buf(out_buf_pool[1]);
  printf("tiovx_modules_dequeue_buf[1] success[%d]\n", tick());
  writeImage("/mnt/link_msc_2.yuv", (vx_image)outbuf[0]->handle);

  v4l2_capture_stop(v4l2_capture_handle);
  v4l2_capture_delete_handle(v4l2_capture_handle);

  tiovx_modules_release_buf(inbuf);
  tiovx_modules_release_buf(outbuf[0]);
  tiovx_modules_release_buf(outbuf[1]);
  tiovx_modules_clean_graph(&graph);
  printf("link test finish\n");
  return 0;
}

vx_status ldc_link()
{
  vx_status status = VX_FAILURE;
  GraphObj graph;
  NodeObj *ldc_node = NULL, *scaler_node = NULL, *mosaic_node = NULL;
  BufPool *in_buf_pool = NULL, *out_buf_pool[2] = {NULL};
  Buf *inbuf = NULL, *outbuf[2] = {NULL};
  kmsDisplayCfg kms_display_cfg;
  kmsDisplayHandle *kms_display_handle;
  v4l2CaptureCfg v4l2_capture_cfg;
  v4l2CaptureHandle *v4l2_capture_handle;
  TIOVXMultiScalerNodeCfg msc_cfg;
  TIOVXLdcNodeCfg ldc_cfg;

  printf("link test start\n");
  status = tiovx_modules_initialize_graph(&graph);
  graph.schedule_mode = VX_GRAPH_SCHEDULE_MODE_QUEUE_AUTO;

  //v4l2 capture
  v4l2_capture_init_cfg(&v4l2_capture_cfg);
  v4l2_capture_cfg.width = 1920;
  v4l2_capture_cfg.height = 1080;
  v4l2_capture_cfg.pix_format = V4L2_PIX_FMT_UYVY;
  v4l2_capture_cfg.bufq_depth = APP_BUFQ_DEPTH;
  sprintf(v4l2_capture_cfg.device, "/dev/video3");
  v4l2_capture_handle = v4l2_capture_create_handle(&v4l2_capture_cfg);

  //LDC
  tiovx_ldc_init_cfg(&ldc_cfg);
  ldc_cfg.input_cfg.color_format = VX_DF_IMAGE_UYVY;
  ldc_cfg.input_cfg.width = 1920;
  ldc_cfg.input_cfg.height = 1080;

  ldc_cfg.output_cfgs[0].color_format = VX_DF_IMAGE_NV12;
  ldc_cfg.output_cfgs[0].width = 1920;
  ldc_cfg.output_cfgs[0].height = 1080;

  sprintf(ldc_cfg.target_string, TIVX_TARGET_VPAC_LDC1);
  ldc_node = tiovx_modules_add_node(&graph, TIOVX_LDC, (void *)&ldc_cfg);

  //MSC
  tiovx_multi_scaler_init_cfg(&msc_cfg);
  msc_cfg.color_format = VX_DF_IMAGE_NV12;
  msc_cfg.num_outputs = 2;
  msc_cfg.input_cfg.width = 1920;
  msc_cfg.input_cfg.height = 1080;
  msc_cfg.output_cfgs[0].width = 1920/4;
  msc_cfg.output_cfgs[0].height = 1080/4;
  msc_cfg.output_cfgs[1].width = 1920/2;
  msc_cfg.output_cfgs[1].height = 1080/2;
  sprintf(msc_cfg.target_string, TIVX_TARGET_VPAC_MSC1);
  tiovx_multi_scaler_module_crop_params_init(&msc_cfg);
  scaler_node = tiovx_modules_add_node(&graph, TIOVX_MULTI_SCALER, (void *)&msc_cfg);

  status = tiovx_modules_link_pads(&ldc_node->srcs[0], &scaler_node->sinks[0]);
  status = tiovx_modules_verify_graph(&graph);

  in_buf_pool = ldc_node->sinks[0].buf_pool;
  out_buf_pool[0] = scaler_node->srcs[0].buf_pool;
  out_buf_pool[1] = scaler_node->srcs[1].buf_pool;

  inbuf = tiovx_modules_acquire_buf(in_buf_pool);
  v4l2_capture_enqueue_buf(v4l2_capture_handle, inbuf);

  outbuf[0] = tiovx_modules_acquire_buf(out_buf_pool[0]);
  tiovx_modules_enqueue_buf(outbuf[0]);

  outbuf[1] = tiovx_modules_acquire_buf(out_buf_pool[1]);
  tiovx_modules_enqueue_buf(outbuf[1]);

  printf("start tick[%d]\n", tick());
  v4l2_capture_start(v4l2_capture_handle);
  do {
    inbuf = v4l2_capture_dqueue_buf(v4l2_capture_handle);
  } while (inbuf == NULL);
  printf("capture success[%d]\n", tick());

  tiovx_modules_enqueue_buf(inbuf);
  outbuf[0] = tiovx_modules_dequeue_buf(out_buf_pool[0]);
  printf("tiovx_modules_dequeue_buf[0] success[%d]\n", tick());
  writeImage("/mnt/ldc_msc_1.yuv", (vx_image)outbuf[0]->handle);
  
  outbuf[1] = tiovx_modules_dequeue_buf(out_buf_pool[1]);
  printf("tiovx_modules_dequeue_buf[1] success[%d]\n", tick());
  writeImage("/mnt/ldc_msc_2.yuv", (vx_image)outbuf[1]->handle);

  v4l2_capture_stop(v4l2_capture_handle);
  v4l2_capture_delete_handle(v4l2_capture_handle);

  tiovx_modules_release_buf(inbuf);
  tiovx_modules_release_buf(outbuf[0]);
  tiovx_modules_release_buf(outbuf[1]);
  tiovx_modules_clean_graph(&graph);
  printf("link test finish\n");
  return 0;
}

vx_status app_modules_linux_decode_display_test(vx_int32 argc, vx_char* argv[])
{
  manual_enqueue();
  sleep(1);
  ldc_link();
  return 0;
}