bps is up

system/camerad/.gitignore

@@ -0,0 +1,3 @@
+bps.h
+blob.h
+/includes/

system/camerad/SConscript

@@ -1,5 +1,7 @@
 Import('env', 'arch', 'messaging', 'common', 'gpucommon', 'visionipc')
 
+env['CPPPATH'].append("includes/")
+
 libs = ['pthread', common, 'jpeg', 'OpenCL', messaging, visionipc, gpucommon]
 
 camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/spectra.cc',

system/camerad/cameras/spectra.cc

@@ -19,6 +19,8 @@
 #include "system/camerad/cameras/spectra.h"
 #include "third_party/linux/include/msm_media_info.h"
 
+#include "blob.h"
+
 // For debugging:
 // echo "4294967295" > /sys/module/cam_debug_util/parameters/debug_mdl
 
@@ -196,9 +198,9 @@ void SpectraMaster::init() {
   assert(isp_fd >= 0);
   LOGD("opened isp");
 
-  //icp_fd = open_v4l_by_name_and_index("cam-icp");
-  //assert(icp_fd >= 0);
-  //LOGD("opened icp");
+  icp_fd = open_v4l_by_name_and_index("cam-icp");
+  assert(icp_fd >= 0);
+  LOGD("opened icp");
 
   // query ISP for MMU handles
   LOG("-- Query for MMU handles");
@@ -215,15 +217,13 @@ void SpectraMaster::init() {
   cdm_iommu = isp_query_cap_cmd.cdm_iommu.non_secure;
 
   // query ICP for MMU handles
-  /*
   struct cam_icp_query_cap_cmd icp_query_cap_cmd = {0};
   query_cap_cmd.caps_handle = (uint64_t)&icp_query_cap_cmd;
   query_cap_cmd.size = sizeof(icp_query_cap_cmd);
   ret = do_cam_control(icp_fd, CAM_QUERY_CAP, &query_cap_cmd, sizeof(query_cap_cmd));
   assert(ret == 0);
   LOGD("using ICP MMU handle: %x", icp_query_cap_cmd.dev_iommu_handle.non_secure);
   icp_device_iommu = icp_query_cap_cmd.dev_iommu_handle.non_secure;
-  */
 
   // subscribe
   LOG("-- Subscribing");
@@ -283,7 +283,7 @@ void SpectraCamera::camera_open(VisionIpcServer *v, cl_device_id device_id, cl_c
 
   open = true;
   configISP();
-  //configICP();  // needs the new AGNOS kernel
+  configICP();
   configCSIPHY();
   linkDevices();
 
@@ -453,8 +453,185 @@ void SpectraCamera::config_bps(int idx, int request_id) {
     Handles per-frame BPS config.
     * BPS = Bayer Processing Segment
   */
-  (void)idx;
-  (void)request_id;
+  int size = sizeof(struct cam_packet) + sizeof(struct cam_cmd_buf_desc)*2 + sizeof(struct cam_buf_io_cfg)*2;
+  size += sizeof(struct cam_patch_desc)*8;
+
+  uint32_t cam_packet_handle = 0;
+  auto pkt = mm.alloc<struct cam_packet>(size, &cam_packet_handle);
+
+  pkt->header.op_code = 0x10000001;
+  pkt->header.request_id = request_id;
+  pkt->header.size = size;
+
+  // *** cmd buf ***
+  struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload;
+  {
+    pkt->num_cmd_buf = 2;
+    pkt->kmd_cmd_buf_index = -1;
+    pkt->kmd_cmd_buf_offset = 0;
+
+    buf_desc[0].meta_data = 0;
+    buf_desc[0].mem_handle = bps_cmd.handle;
+    buf_desc[0].type = CAM_CMD_BUF_FW;
+    buf_desc[0].offset = ALIGNED_SIZE(464, 0x20)*idx;
+
+    buf_desc[0].length = sizeof(BpsFrameProcess) + sizeof(CDMProgramArray) + sizeof(CdmProgram)*10;
+
+    // rest gets patched in
+    BpsFrameProcess *fp = (BpsFrameProcess *)((unsigned char *)bps_cmd.ptr + buf_desc[0].offset);
+    fp->userArg = (uint64_t)icp_dev_handle;
+    fp->cmdData.cdmBufferSize = 65216;   // this comes from the striping lib create call
+    fp->cmdData.requestId = 0;  // why always 0?
+
+    CDMProgramArray *pa = (CDMProgramArray *)((unsigned char *)fp + sizeof(BpsFrameProcess));
+    pa->allocator = 0;
+    pa->numPrograms = 1;
+    pa->programs[0].programType = 20;  // GENERIC
+    pa->programs[0].uID = 0;
+    pa->programs[0].hasSingleReg = 0;
+    pa->programs[0].bufferAllocatedInternally = 0;
+    pa->programs[0].cdmBaseAndLength.bitfields.LEN = 39;  // TODO
+    pa->programs[0].cdmBaseAndLength.bitfields.RESERVED = 0;
+    pa->programs[0].cdmBaseAndLength.bitfields.BASE = 0;  // this gets patched
+
+    // *** second command ***
+    // parsed by cam_icp_packet_generic_blob_handler
+    struct icp_clk {
+      uint64_t budgetNS;
+      uint32_t frameCycles;
+      uint32_t realtimeFlag;
+      uint64_t unCompressedBW;
+      uint64_t compressedBW;
+    } __attribute__((packed));
+    struct isp_packet {
+      uint32_t header;
+      struct icp_clk clk;
+    } __attribute__((packed)) tmp;
+    memset(&tmp, 0, sizeof(tmp));
+
+    tmp.header = CAM_ICP_CMD_GENERIC_BLOB_CLK; // CSLICPGenericBlobCmdBufferClk
+    tmp.header |= (sizeof(icp_clk)) << 8;
+    tmp.clk = {
+      .budgetNS = 0x1fca058,
+      .frameCycles = 0x12e8c6,
+      .realtimeFlag = 0x0,  // TODO: does this work? camx is always 0
+      .unCompressedBW = 0xeac57500,
+      .compressedBW = 0xeac5751c,
+    };
+
+    buf_desc[1].size = sizeof(tmp);
+    buf_desc[1].offset = 0;
+    buf_desc[1].length = buf_desc[1].size - buf_desc[1].offset;
+    buf_desc[1].type = CAM_CMD_BUF_GENERIC;
+    buf_desc[1].meta_data = 1;
+    auto buf2 = mm.alloc<uint32_t>(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle);
+    memcpy(buf2.get(), &tmp, sizeof(tmp));
+  }
+
+  // *** io config ***
+  pkt->num_io_configs = 2;
+  pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf;
+  struct cam_buf_io_cfg *io_cfg = (struct cam_buf_io_cfg *)((char*)&pkt->payload + pkt->io_configs_offset);
+  {
+    // input frame
+    io_cfg[0].offsets[0] = 0;
+    io_cfg[0].mem_handle[0] = buf_handle_raw[idx];
+
+    io_cfg[0].planes[0] = (struct cam_plane_cfg){
+      .width = sensor->frame_width,
+      .height = sensor->frame_height + sensor->extra_height,
+      .plane_stride = sensor->frame_stride,
+      .slice_height = sensor->frame_height + sensor->extra_height,
+    };
+    io_cfg[0].format = sensor->mipi_format;
+    io_cfg[0].color_space = CAM_COLOR_SPACE_BASE;
+    io_cfg[0].color_pattern = 0x5;
+    io_cfg[0].bpp = (sensor->mipi_format == CAM_FORMAT_MIPI_RAW_10 ? 0xa : 0xc);
+    io_cfg[0].resource_type = 0;
+    io_cfg[0].fence = sync_objs[idx];
+    io_cfg[0].direction = CAM_BUF_INPUT;
+    io_cfg[0].subsample_pattern = 0x1;
+    io_cfg[0].framedrop_pattern = 0x1;
+
+    // output frame
+    io_cfg[1].mem_handle[0] = buf_handle_yuv[idx];
+    io_cfg[1].mem_handle[1] = buf_handle_yuv[idx];
+    io_cfg[1].planes[0] = (struct cam_plane_cfg){
+      .width = sensor->frame_width,
+      .height = sensor->frame_height,
+      .plane_stride = ALIGNED_SIZE(sensor->frame_width, 32),
+      .slice_height = ALIGNED_SIZE(sensor->frame_height, 32),
+    };
+    io_cfg[1].planes[1] = (struct cam_plane_cfg){
+      .width = sensor->frame_width,
+      .height = sensor->frame_height/2,
+      .plane_stride = ALIGNED_SIZE(sensor->frame_width, 32),
+      .slice_height = ALIGNED_SIZE(sensor->frame_height/2, 32),
+    };
+    io_cfg[1].offsets[1] = ALIGNED_SIZE(io_cfg[1].planes[0].plane_stride*io_cfg[1].planes[0].slice_height, 0x1000);
+
+    io_cfg[1].format = CAM_FORMAT_NV21;  // TODO: why is this 21 in the dump? should be 12
+    io_cfg[1].color_space = CAM_COLOR_SPACE_BT601_FULL;
+    io_cfg[1].color_pattern = 0;
+    io_cfg[1].bpp = 0;
+    io_cfg[1].resource_type = 1;
+    io_cfg[1].fence = sync_objs_bps_out[idx];
+    io_cfg[1].direction = CAM_BUF_OUTPUT;
+    io_cfg[1].subsample_pattern = 0x1;
+    io_cfg[1].framedrop_pattern = 0x1;
+  }
+
+  // *** patches ***
+  {
+    pkt->num_patches = 8;
+    pkt->patch_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf + sizeof(struct cam_buf_io_cfg)*pkt->num_io_configs;
+    for (int i = 0; i < pkt->num_patches; i++) {
+      struct cam_patch_desc *patch = (struct cam_patch_desc *)((char*)&pkt->payload + pkt->patch_offset + sizeof(cam_patch_desc)*i);
+      patch->dst_buf_hdl = bps_cmd.handle;
+      patch->dst_offset = buf_desc[0].offset + (uint32_t[]){0x0, 0x10, 0x14, 0xa8, 0xb0, 0xc8, 0xac, 0xa0}[i];
+
+      if (i == 0) {
+        // input frame
+        patch->src_buf_hdl = buf_handle_raw[idx];
+        patch->src_offset = 0;
+      } else if ((i == 1) || (i == 2)) {
+        // output frame
+        patch->src_buf_hdl = buf_handle_yuv[idx];
+        patch->src_offset = (i == 1) ? 0 : io_cfg[1].offsets[1];
+      } else if (i == 3) {
+        // this is type BpsIQSettings, not sure we need to set anything?
+        patch->src_buf_hdl = bps_iq.handle;
+        BpsIQSettings *iq = (BpsIQSettings *)bps_iq.ptr;
+        memset(iq, 0, sizeof(BpsIQSettings));
+        iq->demosaicParameters.moduleCfg.EN = 1;
+      } else if (i == 4) {
+        // CDMProgramArray
+        patch->src_buf_hdl = bps_cmd.handle;
+        patch->src_offset = 0xc0;
+      } else if (i == 5) {
+        patch->src_buf_hdl = bps_cdm_program_array.handle;
+        int offset = 0;
+        offset += write_cont((unsigned char *)bps_cdm_program_array.ptr + offset, 0x2868, {
+          0x069d0400,
+          0x00000610,
+          0x00000000,
+          0x00000000,
+        });
+        assert(offset == 24);
+        offset += write_cont((unsigned char *)bps_cdm_program_array.ptr + offset, 0x2878, {
+          0x00000080,
+          0x00800066,
+        });
+      } else if (i == 6) {
+        patch->src_buf_hdl = bps_striping.handle;
+      } else if (i == 7) {
+        patch->src_buf_hdl = bps_cdm_striping_bl.handle;
+      }
+    }
+  }
+
+  int ret = device_config(m->icp_fd, session_handle, icp_dev_handle, cam_packet_handle);
+  assert(ret == 0);
 }
 
 void SpectraCamera::config_ife(int idx, int request_id, bool init) {
@@ -583,7 +760,6 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) {
     pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf;
 
     struct cam_buf_io_cfg *io_cfg = (struct cam_buf_io_cfg *)((char*)&pkt->payload + pkt->io_configs_offset);
-
     if (is_raw) {
       io_cfg[0].mem_handle[0] = buf_handle_raw[idx];
       io_cfg[0].planes[0] = (struct cam_plane_cfg){
@@ -647,7 +823,7 @@ void SpectraCamera::enqueue_buffer(int i, bool dp) {
   if (buf_handle_raw[i] && sync_objs[i]) {
     // wait
     struct cam_sync_wait sync_wait = {0};
-    sync_wait.sync_obj = sync_objs[i];
+    sync_wait.sync_obj = sync_objs_bps_out[i];
     sync_wait.timeout_ms = 50; // max dt tolerance, typical should be 23
     ret = do_sync_control(m->cam_sync_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait));
     if (ret != 0) {
@@ -679,13 +855,11 @@ void SpectraCamera::enqueue_buffer(int i, bool dp) {
   }
   sync_objs[i] = sync_create.sync_obj;
 
-  /*
   ret = do_cam_control(m->cam_sync_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create));
   if (ret != 0) {
     LOGE("failed to create fence: %d %d", ret, sync_create.sync_obj);
   }
   sync_objs_bps_out[i] = sync_create.sync_obj;
-  */
 
   // schedule request with camera request manager
   struct cam_req_mgr_sched_request req_mgr_sched_request = {0};
@@ -711,9 +885,8 @@ void SpectraCamera::camera_map_bufs() {
     // configure ISP to put the image in place
     struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0};
     mem_mgr_map_cmd.mmu_hdls[0] = m->device_iommu;
-    //mem_mgr_map_cmd.mmu_hdls[1] = m->icp_device_iommu;
-    //mem_mgr_map_cmd.num_hdl = 2;
-    mem_mgr_map_cmd.num_hdl = 1;
+    mem_mgr_map_cmd.mmu_hdls[1] = m->icp_device_iommu;
+    mem_mgr_map_cmd.num_hdl = 2;
     mem_mgr_map_cmd.flags = CAM_MEM_FLAG_HW_READ_WRITE;
 
     // RAW bayer images
@@ -851,11 +1024,25 @@ void SpectraCamera::configICP() {
     Configures both the ICP and BPS.
   */
 
+  int cfg_handle;
+  BpsCfg *cfg = (BpsCfg *)alloc_w_mmu_hdl(m->video0_fd, sizeof(BpsCfg), (uint32_t*)&cfg_handle, 0x1,
+                                                    CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_HW_SHARED_ACCESS,
+                                                    m->icp_device_iommu);
+  memset(cfg, 0, sizeof(BpsCfg));
+
+  cfg->cmdData.images[0].info.format = IMAGE_FORMAT_MIPI_10;
+  cfg->cmdData.images[0].info.bayerOrder = FIRST_PIXEL_B;
+  cfg->cmdData.images[0].info.dimensions.widthPixels = sensor->frame_width;
+  cfg->cmdData.images[0].info.dimensions.heightLines = sensor->frame_height;
+  cfg->cmdData.images[1].info.format = IMAGE_FORMAT_LINEAR_NV12;
+  cfg->cmdData.images[1].info.dimensions.widthPixels = sensor->frame_width;
+  cfg->cmdData.images[1].info.dimensions.heightLines = sensor->frame_height;
+
   struct cam_icp_acquire_dev_info icp_info = {
     .scratch_mem_size = 0x0,
     .dev_type = 0x1,  // BPS
-    .io_config_cmd_size = 0,
-    .io_config_cmd_handle = 0,
+    .io_config_cmd_size = sizeof(BpsCfg),
+    .io_config_cmd_handle = cfg_handle,
     .secure_mode = 0,
     .num_out_res = 1,
     .in_res = (struct cam_icp_res_info){
@@ -876,8 +1063,9 @@ void SpectraCamera::configICP() {
   icp_dev_handle = *h;
   LOGD("acquire icp dev");
 
+  release(m->video0_fd, cfg_handle);
+
   // BPS CMD buffer
-  unsigned char striping_out[] = "\x00";
   bps_cmd.init(m, FRAME_BUF_COUNT*ALIGNED_SIZE(464, 0x20), 0x20,
                CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE | CAM_MEM_FLAG_HW_SHARED_ACCESS,
                m->icp_device_iommu);

system/camerad/intercept.sh

@@ -0,0 +1,2 @@
+#!/usr/bin/bash
+DISABLE_ROAD=1 DISABLE_WIDE_ROAD=1 DEBUG_FRAMES=1 LOGPRINT=debug LD_PRELOAD=/data/tici_test_scripts/isp/interceptor/tmpioctl.so ./camerad

system/camerad/test/debug.sh

@@ -10,7 +10,7 @@ echo 0 | sudo tee /sys/module/cam_debug_util/parameters/debug_mdl
 sudo dmesg -C
 scons -u -j8 --minimal .
 export DEBUG_FRAMES=1
-#export DISABLE_ROAD=1 DISABLE_WIDE_ROAD=1
-export DISABLE_DRIVER=1
-#export LOGPRINT=debug
+export DISABLE_ROAD=1 DISABLE_WIDE_ROAD=1
+#export DISABLE_DRIVER=1
+export LOGPRINT=debug
 ./camerad

system/camerad/test/icp_debug.sh

@@ -0,0 +1,13 @@
+#!/usr/bin/env bash
+set -e
+
+cd /sys/kernel/debug/tracing
+echo "" > trace
+echo 1 > tracing_on
+#echo Y > /sys/kernel/debug/camera_icp/a5_debug_q
+echo 0x1 > /sys/kernel/debug/camera_icp/a5_debug_type
+echo 1 > /sys/kernel/debug/tracing/events/camera/enable
+echo 0xff > /sys/kernel/debug/camera_icp/a5_debug_lvl
+echo 1 > /sys/kernel/debug/tracing/events/camera/cam_icp_fw_dbg/enable
+
+cat /sys/kernel/debug/tracing/trace_pipe