sw/device/examples/demo_hps_live/hps_demo_smc.c

/*
 * Copyright 2023 Google LLC
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

// ISP live stream mode with user defined resolution: 320 x 240 in grayscale.

#include "hw/top_matcha/ip/ml_top/data/ml_top_regs.h"  // Generated.
#include "hw/top_matcha/sw/autogen/top_matcha.h"
#include "sw/device/examples/testdata/kelvin_model_ml_bin.h"  // Generated.
#include "sw/device/lib/arch/device.h"
#include "sw/device/lib/camera_hm01b0.h"
#include "sw/device/lib/dif/dif_isp_wrapper.h"
#include "sw/device/lib/dif/dif_ml_top.h"
#include "sw/device/lib/dif/dif_rv_plic.h"
#include "sw/device/lib/dif/dif_rv_timer.h"
#include "sw/device/lib/dif/dif_uart.h"
#include "sw/device/lib/runtime/irq.h"
#include "sw/device/lib/runtime/log.h"
#include "sw/device/lib/runtime/print.h"
#include "sw/device/lib/spi_display/LCD_Driver.h"
#include "sw/device/lib/spi_display/spi_display.h"
#include "sw/device/lib/testing/test_framework/check.h"
#include "sw/device/lib/testing/test_framework/ottf_test_config.h"
#include "sw/device/lib/testing/test_framework/status.h"
#include "sw/device/lib/util.h"

#define TOP_MATCHA_RAM_ML_DMEM_IMG0_OFFSET_ADDR 0x00300000
#define TOP_MATCHA_RAM_ML_DMEM_IMG1_OFFSET_ADDR 0x00340000
#define TOP_MATCHA_RAM_ML_DMEM_OUT_OFFSET_ADDR 0x00380000
#define TOP_MATCHA_RAM_ML_DMEM_CMD_OFFSET_ADDR 0x003FF000
#define IMAGE_FRAME_SIZE 320 * 240

OTTF_DEFINE_TEST_CONFIG();

static dif_isp_wrapper_t isp_wrapper;
static dif_ml_top_t ml_top;
static dif_rv_plic_t plic_smc;
static dif_uart_t smc_uart;
static dif_spi_host_t spi_host2;
static spi_display_t spi_display;
static dif_tlul_mailbox_t tlul_mailbox;
static dif_rv_timer_t rv_timer;

static uint16_t rgb565_buffer[LCD_WIDTH * LCD_HEIGHT];
static PAINT paint_ctx;
static char paint_str[128];
const uint32_t delay_time_micros = 10;

static volatile bool isp_frame_done = false;
static uint32_t img_offset = TOP_MATCHA_RAM_ML_DMEM_IMG0_OFFSET_ADDR;

static volatile bool ml_top_finish_done = false;

static void handle_isp_wrapper_isp_irq() {
  sFONT *font = &Font16;
  // Set address of image buffer.
  mmio_region_t ml_dmem =
      mmio_region_from_addr(TOP_MATCHA_ML_TOP_DMEM_BASE_ADDR);
  mmio_region_write32(ml_dmem, TOP_MATCHA_RAM_ML_DMEM_CMD_OFFSET_ADDR,
                      img_offset);
  // Start up Kelvin core running the model
  ml_top_finish_done = false;
  CHECK_DIF_OK(dif_ml_top_release_ctrl_en(&ml_top));
  LOG_INFO("[SMC] Kelvin starts running model.");

  // Inform the host that a new frame is available.
  uint8_t *image_addr = (uint8_t *)(TOP_MATCHA_RAM_ML_DMEM_BASE_ADDR +
                                    img_offset);
  LOG_INFO("[SMC] Start of a frame, address %u.", image_addr);

  // Generate an RGB565 frame, and write it to the display.
  // We start drawing at an offset based on the height of our font,
  // so that scores can be displayed.
  for (int i = 0; i < LCD_WIDTH * LCD_HEIGHT; ++i) {
    uint8_t y = *(image_addr + i);
    uint16_t rgb565 =
        ((y >> 3) & 0x1F) << 11 | ((y >> 2) & 0x3F) << 5 | ((y >> 3) & 0x1F);
    rgb565_buffer[i] = __builtin_bswap16(rgb565);
  }
  LCD_ClearToBufferWindow(&spi_display,
                          rgb565_buffer + (LCD_WIDTH * font->Height), 0,
                          font->Height, LCD_WIDTH, LCD_HEIGHT);

  // Wait until Kelvin finishes. Reset Kelvin core.
  while (!ml_top_finish_done) {
    asm volatile("wfi");
  }
  LOG_INFO("[SMC] Kelvin finished running model.");

  // Log model score
  mmio_region_t ml_model_out_base =
      mmio_region_from_addr(TOP_MATCHA_RAM_ML_DMEM_BASE_ADDR +
                            TOP_MATCHA_RAM_ML_DMEM_OUT_OFFSET_ADDR);
  const int8_t model_out_val0 = mmio_region_read8(ml_model_out_base, 0);
  const int8_t model_out_val1 = mmio_region_read8(ml_model_out_base, 1);
  LOG_INFO("[SMC] Score 0: %d, Score 1: %d.", model_out_val0, model_out_val1);

  // Render an RGB565 buffer with the score text.
  memset(paint_ctx.Image, 0, LCD_WIDTH * font->Height * sizeof(uint16_t));
  memset(paint_str, 0, sizeof(paint_str));
  base_snprintf(paint_str, sizeof(paint_str), "Score 0: %d, Score 1: %d",
                model_out_val0, model_out_val1);
  Paint_DrawString_EN(&paint_ctx, 0, 0, paint_str, font, BLACK, WHITE);
  LCD_ClearToBufferWindow(&spi_display, paint_ctx.Image, 0, 0, LCD_WIDTH,
                          font->Height);

  if (img_offset == TOP_MATCHA_RAM_ML_DMEM_IMG0_OFFSET_ADDR) {
    img_offset = TOP_MATCHA_RAM_ML_DMEM_IMG1_OFFSET_ADDR;
  } else {
    img_offset = TOP_MATCHA_RAM_ML_DMEM_IMG0_OFFSET_ADDR;
  }
}

static void handle_isp_wrapper_isrs(const dif_rv_plic_irq_id_t interrupt_id) {
  // NOTE: This initialization is superfluous, since the `default` case below
  // is effectively noreturn, but the compiler is unable to prove this.
  switch (interrupt_id) {
    case kTopMatchaPlicIrqIdIspWrapperIsp:
      isp_frame_done = true;
      CHECK_DIF_OK(dif_isp_wrapper_isp_irq_done(&isp_wrapper));
      break;
    case kTopMatchaPlicIrqIdIspWrapperMi:
      CHECK_DIF_OK(dif_isp_wrapper_mi_irq_done(&isp_wrapper));
      break;
    default:
      LOG_FATAL("ISR is not implemented!");
  }
}

static void handle_ml_top_isr(const dif_rv_plic_irq_id_t interrupt_id) {
  switch (interrupt_id) {
    case kTopMatchaPlicIrqIdMlTopFinish:
      ml_top_finish_done = true;
      break;
    case kTopMatchaPlicIrqIdMlTopFinish | kTopMatchaPlicIrqIdMlTopFault:
      LOG_ERROR("ML core raised fault interrupt.");
      test_status_set(kTestStatusFailed);
    default:
      LOG_FATAL("ISR is not implemented!");
      test_status_set(kTestStatusFailed);
  }
  CHECK_DIF_OK(dif_ml_top_reset_ctrl_en(&ml_top));
  CHECK_DIF_OK(dif_ml_top_irq_acknowledge_all(&ml_top));
}

void ottf_external_isr(void) {
  // Claim the IRQ by reading the Ibex specific CC register.
  dif_rv_plic_irq_id_t interrupt_id;

  CHECK_DIF_OK(dif_rv_plic_irq_claim(&plic_smc, kTopMatchaPlicTargetIbex0Smc,
                                     &interrupt_id));

  // Check if the interrupted peripheral is ISP WRAPPER.
  top_matcha_plic_peripheral_smc_t peripheral_id =
      top_matcha_plic_interrupt_for_peripheral_smc[interrupt_id];
  CHECK(peripheral_id == kTopMatchaPlicPeripheralIspWrapper ||
            peripheral_id == kTopMatchaPlicPeripheralCamI2c ||
            peripheral_id == kTopMatchaPlicPeripheralSpiHost2 ||
            peripheral_id == kTopMatchaPlicPeripheralTlulMailboxSmc ||
            peripheral_id == kTopMatchaPlicPeripheralMlTop,
        "Unexpected peripheral in ISR: %d", peripheral_id);
  switch (peripheral_id) {
    case kTopMatchaPlicPeripheralCamI2c: {
      CHECK_DIF_OK(camera_hm01b0_irq_handler(interrupt_id));
      break;
    }
    case kTopMatchaPlicPeripheralIspWrapper:
      handle_isp_wrapper_isrs(interrupt_id);
      break;
    case kTopMatchaPlicPeripheralTlulMailboxSmc: {
      CHECK_DIF_OK(spi_display_smc_mailbox_irq_handler(&spi_display));
      break;
    }
    case kTopMatchaPlicPeripheralSpiHost2: {
      CHECK_DIF_OK(spi_display_spi_irq_handler(&spi_display));
      break;
    }
    case kTopMatchaPlicPeripheralMlTop: {
      handle_ml_top_isr(interrupt_id);
      break;
    }
    default:
      LOG_FATAL("Peripheral is not implemented!");
  }

  // Complete the IRQ by writing the IRQ source to the Ibex specific CC
  // register.
  CHECK_DIF_OK(dif_rv_plic_irq_complete(&plic_smc, kTopMatchaPlicTargetIbex0Smc,
                                        interrupt_id));
}

/*
 * Configures all the relevant interrupts in PLIC_SMC.
 */
static void plic_smc_configure_irqs(dif_rv_plic_t *plic) {
  CHECK_DIF_OK(camera_hm01b0_irq_init(plic, kTopMatchaPlicTargetIbex0Smc));
  // Set IRQ priorities to MAX
  CHECK_DIF_OK(dif_rv_plic_irq_set_priority(
      plic, kTopMatchaPlicIrqIdIspWrapperIsp, kDifRvPlicMaxPriority));
  CHECK_DIF_OK(dif_rv_plic_irq_set_priority(
      plic, kTopMatchaPlicIrqIdIspWrapperMi, kDifRvPlicMaxPriority));
  CHECK_DIF_OK(dif_rv_plic_irq_set_priority(
      plic, kTopMatchaPlicIrqIdMlTopFinish, kDifRvPlicMaxPriority));
  CHECK_DIF_OK(dif_rv_plic_irq_set_priority(plic, kTopMatchaPlicIrqIdMlTopFault,
                                            kDifRvPlicMaxPriority));

  // Set Ibex IRQ priority threshold level
  CHECK_DIF_OK(dif_rv_plic_target_set_threshold(
      plic, kTopMatchaPlicTargetIbex0Smc, kDifRvPlicMinPriority));

  // Enable IRQs in PLIC
  // Enable ISP IRQs
  CHECK_DIF_OK(dif_rv_plic_irq_set_enabled(
      plic, kTopMatchaPlicIrqIdIspWrapperIsp, kTopMatchaPlicTargetIbex0Smc,
      kDifToggleEnabled));
  CHECK_DIF_OK(dif_rv_plic_irq_set_enabled(
      plic, kTopMatchaPlicIrqIdIspWrapperMi, kTopMatchaPlicTargetIbex0Smc,
      kDifToggleEnabled));
  // Enable Mailbox IRQs
  CHECK_DIF_OK(dif_rv_plic_irq_set_enabled(
      plic, kTopMatchaPlicIrqIdTlulMailboxSmcRtirq,
      kTopMatchaPlicTargetIbex0Smc, kDifToggleEnabled));
  CHECK_DIF_OK(dif_rv_plic_irq_set_priority(
      plic, kTopMatchaPlicIrqIdTlulMailboxSmcRtirq, 1));
  // Enable ML core IRQs
  CHECK_DIF_OK(dif_rv_plic_irq_set_enabled(plic, kTopMatchaPlicIrqIdMlTopFinish,
                                           kTopMatchaPlicTargetIbex0Smc,
                                           kDifToggleEnabled));
  CHECK_DIF_OK(dif_rv_plic_irq_set_enabled(plic, kTopMatchaPlicIrqIdMlTopFault,
                                           kTopMatchaPlicTargetIbex0Smc,
                                           kDifToggleEnabled));
}

void _ottf_main(void) {
  test_status_set(kTestStatusInTest);

  CHECK_DIF_OK(dif_isp_wrapper_init(
      mmio_region_from_addr(TOP_MATCHA_ISP_WRAPPER_BASE_ADDR), &isp_wrapper));
  CHECK_DIF_OK(dif_rv_plic_init(
      mmio_region_from_addr(TOP_MATCHA_RV_PLIC_SMC_BASE_ADDR), &plic_smc));
  LOG_INFO("Configure isp into CAM live mode.");
  CHECK_DIF_OK(dif_uart_init(
      mmio_region_from_addr(TOP_MATCHA_SMC_UART_BASE_ADDR), &smc_uart));
  CHECK_DIF_OK(
      dif_uart_configure(&smc_uart, (dif_uart_config_t){
                                        .baudrate = kUartBaudrate,
                                        .clk_freq_hz = kClockFreqPeripheralHz,
                                        .parity_enable = kDifToggleDisabled,
                                        .parity = kDifUartParityEven,
                                        .tx_enable = kDifToggleEnabled,
                                        .rx_enable = kDifToggleDisabled,
                                    }));
  base_uart_stdout(&smc_uart);
  LOG_INFO("[SMC] ISP_WRAPPER test start.");

  // Configure Mailbox.
  CHECK_DIF_OK(dif_tlul_mailbox_init(
      mmio_region_from_addr(TOP_MATCHA_TLUL_MAILBOX_SMC_BASE_ADDR),
      &tlul_mailbox));
  CHECK_DIF_OK(dif_tlul_mailbox_irq_set_enabled(
      &tlul_mailbox, kDifTlulMailboxIrqRtirq, kDifToggleEnabled));
  CHECK_DIF_OK(dif_tlul_mailbox_irq_set_enabled(
      &tlul_mailbox, kDifTlulMailboxIrqWtirq, kDifToggleEnabled));
  CHECK_DIF_OK(dif_tlul_mailbox_irq_set_enabled(
      &tlul_mailbox, kDifTlulMailboxIrqEirq, kDifToggleEnabled));

  // Init ML_TOP
  CHECK_DIF_OK(dif_ml_top_init(
      mmio_region_from_addr(TOP_MATCHA_ML_TOP_CORE_BASE_ADDR), &ml_top));
  CHECK_DIF_OK(dif_ml_top_irq_set_enabled(&ml_top, kDifMlTopIrqFinish,
                                          kDifToggleEnabled));
  CHECK_DIF_OK(dif_ml_top_irq_set_enabled(&ml_top, kDifMlTopIrqFault,
                                          kDifToggleEnabled));
  dif_ml_top_reset_ctrl_en(&ml_top);
  LOG_INFO("[SMC] ML_TOP INIT.");

  // Fill the memory with zeroes.
  mmio_region_t ml_dmem =
      mmio_region_from_addr(TOP_MATCHA_ML_TOP_DMEM_BASE_ADDR);
  for (int i = 0; i < TOP_MATCHA_ML_TOP_DMEM_SIZE_BYTES / sizeof(uint32_t);
       ++i) {
    mmio_region_write32(ml_dmem, i * sizeof(uint32_t), 0);
  }
  LOG_INFO("[SMC] Finished initializing ML DMEM");

  // Load Kelvin binary.
  // TODO: update kelvin binary from greg
  uint32_t *kelvin_bin_uint32 = (uint32_t *)kelvin_bin;
  for (int i = 0; i < kelvin_bin_len / sizeof(uint32_t); ++i) {
    mmio_region_write32(ml_dmem, i * sizeof(uint32_t), kelvin_bin_uint32[i]);
  }
  LOG_INFO("[SMC] Finished loading Kelvin binary");
  mmio_region_write32(ml_dmem, TOP_MATCHA_RAM_ML_DMEM_CMD_OFFSET_ADDR,
                      TOP_MATCHA_RAM_ML_DMEM_IMG0_OFFSET_ADDR);
  mmio_region_write32(ml_dmem,
                      TOP_MATCHA_RAM_ML_DMEM_CMD_OFFSET_ADDR + sizeof(uint32_t),
                      TOP_MATCHA_RAM_ML_DMEM_OUT_OFFSET_ADDR);
  LOG_INFO("[SMC] Finished setting Kelvin input/output addresses");

  plic_smc_configure_irqs(&plic_smc);
  irq_global_ctrl(true);
  irq_external_ctrl(true);
  // Initialize the SPI host and display.
  CHECK_DIF_OK(dif_spi_host_init(
      mmio_region_from_addr(TOP_MATCHA_SPI_HOST2_BASE_ADDR), &spi_host2));
  dif_spi_host_config_t config = {
      .spi_clock = kClockFreqSpiDisplayHz,
      .peripheral_clock_freq_hz = kClockFreqCpuHz,
  };
  CHECK_DIF_OK(dif_spi_host_configure(&spi_host2, config));
  CHECK_DIF_OK(dif_spi_host_output_set_enabled(&spi_host2, /*enabled=*/true));
  CHECK_DIF_OK(spi_display_init(&spi_display, &spi_host2, &tlul_mailbox));
  CHECK_DIF_OK(spi_display_irq_init(&spi_display, &plic_smc,
                                    kTopMatchaPlicIrqIdSpiHost2SpiEvent,
                                    kTopMatchaPlicTargetIbex0Smc));
  LOG_INFO("ISP interrupt finshed setup!");

  // Initialize camera
  CHECK_DIF_OK(camera_hm01b0_init());
  // Initialize the written address of the frame
  memset((void *)TOP_MATCHA_RAM_ML_DMEM_BASE_ADDR +
             TOP_MATCHA_RAM_ML_DMEM_IMG0_OFFSET_ADDR,
         0, IMAGE_FRAME_SIZE);
  memset((void *)TOP_MATCHA_RAM_ML_DMEM_BASE_ADDR +
             TOP_MATCHA_RAM_ML_DMEM_IMG1_OFFSET_ADDR,
         0, IMAGE_FRAME_SIZE);
  CHECK_DIF_OK(camera_hm01b0_set_default_registers(
      kCameraHm01b0Resolution320x240, kCameraHm01b0FrameRate60Fps));
  CHECK_DIF_OK(camera_hm01b0_set_test_pattern(kCameraHm01b0TestPatternNone));
  // Set ISP into 320x320 mode and start camera
  CHECK_DIF_OK(dif_isp_wrapper_set_ml_mem_byp_320x240_en(&isp_wrapper));

  LOG_INFO("ISP finshed setup Cam Ctrl!");

  Paint_NewImage(&paint_ctx, LCD_WIDTH, LCD_HEIGHT, ROTATE_0, MIRROR_NONE);

  dif_rv_timer_tick_params_t tick_params;
  CHECK_DIF_OK(dif_rv_timer_init(
      mmio_region_from_addr(TOP_MATCHA_RV_TIMER_SMC_BASE_ADDR), &rv_timer));
  CHECK_DIF_OK(dif_rv_timer_approximate_tick_params(kClockFreqPeripheralHz,
                                                    1000, &tick_params));
  CHECK_DIF_OK(dif_rv_timer_set_tick_params(&rv_timer, 0, tick_params));
  CHECK_DIF_OK(
      dif_rv_timer_counter_set_enabled(&rv_timer, 0, kDifToggleEnabled));
  while (true) {
    uint64_t counter_start, counter_end;
    CHECK_DIF_OK(dif_rv_timer_counter_read(&rv_timer, 0, &counter_start));
    CHECK_DIF_OK(camera_hm01b0_set_frame_count(1));
    CHECK_DIF_OK(
        camera_hm01b0_set_streaming_mode(kCameraHm01b0StreamingModeCounter));
    while (!isp_frame_done) {
      asm volatile("wfi");
    }
    isp_frame_done = false;
    handle_isp_wrapper_isp_irq();
    CHECK_DIF_OK(dif_rv_timer_counter_read(&rv_timer, 0, &counter_end));
    LOG_INFO("Capture + eval + render took %d ms",
             (uint32_t)(counter_end - counter_start));
  }
}