From e9f230e12f5813b5b3854c6a16b8612c6d380a42 Mon Sep 17 00:00:00 2001
From: Fanis Baikas <fan.baikas@gmail.com>
Date: Mon, 2 Sep 2024 12:46:04 +0100
Subject: [PATCH] Added Fast-kNN demo folder with updated implementation of kNN
 classifier

---
 nanosoc_board/C_Cxx/CMakeLists.txt            |   1 +
 .../C_Cxx/fast_knn_demo/CMakeLists.txt        |  40 +
 .../C_Cxx/fast_knn_demo/fast_knn_demo.cpp     | 860 ++++++++++++++++++
 .../C_Cxx/fast_knn_demo/fast_knn_driver.h     |  11 +
 .../C_Cxx/fast_knn_demo/hardware_config.c     |  73 ++
 5 files changed, 985 insertions(+)
 create mode 100644 nanosoc_board/C_Cxx/fast_knn_demo/CMakeLists.txt
 create mode 100644 nanosoc_board/C_Cxx/fast_knn_demo/fast_knn_demo.cpp
 create mode 100644 nanosoc_board/C_Cxx/fast_knn_demo/fast_knn_driver.h
 create mode 100644 nanosoc_board/C_Cxx/fast_knn_demo/hardware_config.c

diff --git a/nanosoc_board/C_Cxx/CMakeLists.txt b/nanosoc_board/C_Cxx/CMakeLists.txt
index 59ddded..af123cf 100644
--- a/nanosoc_board/C_Cxx/CMakeLists.txt
+++ b/nanosoc_board/C_Cxx/CMakeLists.txt
@@ -42,5 +42,6 @@ add_subdirectory(clock_scaling_power_measurement)
 add_subdirectory(nanosoc_demo)
 add_subdirectory(nanosoc_regression)
 add_subdirectory(fast_knn_demo2)
+add_subdirectory(fast_knn_demo)
 add_subdirectory(srimanth_demo)
 #add_subdirectory(FT1248)
\ No newline at end of file
diff --git a/nanosoc_board/C_Cxx/fast_knn_demo/CMakeLists.txt b/nanosoc_board/C_Cxx/fast_knn_demo/CMakeLists.txt
new file mode 100644
index 0000000..33b51a4
--- /dev/null
+++ b/nanosoc_board/C_Cxx/fast_knn_demo/CMakeLists.txt
@@ -0,0 +1,40 @@
+if (TARGET tinyusb_device)
+    set(PROJECT_NAME fast_knn_demo) 
+
+    # Add your source files
+    add_executable(${PROJECT_NAME}
+        fast_knn_demo.cpp 
+        hardware_config.c
+    )
+    #target_compile_definitions(fast_knn_demo PRIVATE
+    ##        PICO_DEFAULT_UART=0
+    #        PICO_DEFAULT_UART_TX_PIN=28
+    #        PICO_DEFAULT_UART_RX_PIN=29
+    #        PICO_DEFAULT_UART_BAUD_RATE=9600
+    #        )
+    pico_enable_stdio_usb(${PROJECT_NAME} 1)
+    pico_enable_stdio_uart(${PROJECT_NAME} 0)
+
+    pico_generate_pio_header(${PROJECT_NAME}  
+        ${SOCLABS_PIO_PATH}/ft1248x1/ft1248x1_sm.pio
+    )
+
+    # Don't forget to link the libraries you need!
+    target_link_libraries(${PROJECT_NAME}
+        pico_explorer
+        hardware_pio
+        hardware_i2c
+        nanosoc_graphics
+        nanosoc_board_system
+        pico_multicore
+        I2C_device_bus
+        hardware_clocks
+        FatFs_SPI
+        )
+
+
+    # create map/bin/hex file etc.
+    pico_add_extra_outputs(${PROJECT_NAME})
+elseif(PICO_ON_DEVICE)
+    message(WARNING "not building hello_usb because TinyUSB submodule is not initialized in the SDK")
+endif()
diff --git a/nanosoc_board/C_Cxx/fast_knn_demo/fast_knn_demo.cpp b/nanosoc_board/C_Cxx/fast_knn_demo/fast_knn_demo.cpp
new file mode 100644
index 0000000..e4b11c8
--- /dev/null
+++ b/nanosoc_board/C_Cxx/fast_knn_demo/fast_knn_demo.cpp
@@ -0,0 +1,860 @@
+#include <string>
+#include <math.h>
+#include <vector>
+#include <stdlib.h>
+#include "ft1248x1_sm.pio.h"
+#include "hardware/pio.h"
+#include "hardware/pll.h"
+#include "hardware/clocks.h"
+#include "hardware/structs/pll.h"
+#include "hardware/structs/clocks.h"
+#include "pico/stdlib.h"
+#include "hardware/i2c.h"
+#include "ff.h"
+#include "f_util.h"
+#include "diskio.h" /* Declarations of disk functions */
+#include "sd_card.h"
+#include "hw_config.h" 
+#include "nanosoc_graphics.hpp"
+
+#include "pico/multicore.h"
+#include <stdio.h>
+#include <cmath>
+
+extern "C"{
+#include "nanosoc_board_system.h"
+#include "I2C_device_bus.h"
+}
+
+#include "fast_knn_driver.h"
+
+#include "libraries/pico_display_2/pico_display_2.hpp"
+#include "drivers/st7789/st7789.hpp"
+#include "libraries/pico_graphics/pico_graphics.hpp"
+
+using namespace pimoroni;
+
+// Variables/structs for Graphics control of Pico Display
+ST7789 st7789(320, 240, ROTATE_270, false, get_spi_pins(BG_SPI_FRONT));
+PicoGraphics_PenRGB332 graphics(st7789.width, st7789.height, nullptr);
+Pen BG = graphics.create_pen(0, 0, 0);
+Pen WHITE = graphics.create_pen(255, 255, 255);
+Pen RED = graphics.create_pen(255, 0, 0);
+bool BUTTON_Y_pressed = false;
+bool BUTTON_X_pressed = false;
+int x=0;
+int y=0;
+int xtext=0;
+int ytext=0;
+
+// Variables for screen/graphics state
+const uint8_t CONSOLE_FIFO_DEPTH=32;
+std::string console[CONSOLE_FIFO_DEPTH];
+
+// GPIO Pins for Valid-Ready Handshake 
+#define DATA_REQ_GPIO_PIN 2
+#define DATA_SENT_GPIO_PIN 3
+
+#define NUM_UNLABELLED_IMGS 36
+#define NUM_LABELLED_IMGS 130
+
+#define NUM_OF_CLASSES 10
+#define NUM_LABELLED_IMAGES_PER_CLASS  NUM_LABELLED_IMGS/NUM_OF_CLASSES
+#define kNN_k 5
+
+std::string labelled_files[NUM_OF_CLASSES][NUM_LABELLED_IMAGES_PER_CLASS];
+std::string unlabelled_files[NUM_UNLABELLED_IMGS];
+
+uint8_t unlabelled_buffer[NUM_UNLABELLED_IMGS][784];
+uint8_t labelled_buffer[NUM_LABELLED_IMGS][784];
+uint8_t labelled_imgs_labels[NUM_LABELLED_IMGS];
+
+// Nanosoc image buffer address
+int nanosoc_img_buffer_addr;
+
+// Zero-filled buffer for clearing accelerator registers
+uint8_t logic_zero[4] = {0x00, 0x00, 0x00, 0x00};
+
+uintptr_t unlab_img_dot_prod_addr = FAST_KNN_REGS_BASE + offsetof(FAST_KNN_regs_typedef, UNLAB_IMG_DOT_PROD);
+uintptr_t lab_img_dot_prod_addr = FAST_KNN_REGS_BASE + offsetof(FAST_KNN_regs_typedef, LAB_IMG_DOT_PROD);
+uintptr_t comb_dot_prod_addr = FAST_KNN_REGS_BASE + offsetof(FAST_KNN_regs_typedef, COMB_DOT_PROD);
+uintptr_t byte_counter_reg_addr = FAST_KNN_REGS_BASE + offsetof(FAST_KNN_regs_typedef, BYTE_COUNTER_REG);
+uintptr_t priming_mode_reg_addr = FAST_KNN_REGS_BASE + offsetof(FAST_KNN_regs_typedef, PRIMING_MODE_REG);
+uintptr_t sw_reset_reg_addr = FAST_KNN_REGS_BASE + offsetof(FAST_KNN_regs_typedef, SW_RESET_REG);
+
+// Path to labelled images 
+std::string labelled_image_dirs[10] = {
+    "./data/labelled_images/image0",
+    "./data/labelled_images/image1",
+    "./data/labelled_images/image2",
+    "./data/labelled_images/image3",
+    "./data/labelled_images/image4",
+    "./data/labelled_images/image5",
+    "./data/labelled_images/image6",
+    "./data/labelled_images/image7",
+    "./data/labelled_images/image8",
+    "./data/labelled_images/image9"};
+
+// Path to unlabelled images
+std::string unlabelled_image_dir = "0:/data/unlabelled_images";
+
+std::string classes[] = {
+    "T-Shirt",
+    "Trousers",
+    "Pullover",
+    "Dress",
+    "Coat",
+    "Sandal",
+    "Shirt",
+    "Sneaker",
+    "Bag",
+    "Boot"};
+
+Point console_text_location(5,30);
+uint8_t console_addr_screen = 0;
+uint8_t console_addr=0;
+
+volatile uint current_unlab_img=0;
+volatile uint8_t predicted_label;
+
+// Enumerator for screen state, this allows for different 'pages' on the screen
+typedef enum{
+    HOME,
+    CONSOLE,
+    DEMO,
+    POWER
+} screen_state;
+
+screen_state current_screen_state=HOME;
+screen_state last_screen_state=POWER;
+
+// Global variables for Core 0 program flow
+bool DEMO_RUN = false;
+bool KNN_done = false;
+bool KNN_start = false;
+bool nanosoc_console = false;
+bool nanosoc_console_draw = false;
+i2c_inst_t *i2c = i2c1;
+
+static spi_t spis[] = {{
+    .hw_inst = spi0,
+    .miso_gpio = 0,
+    .mosi_gpio = 19,
+    .sck_gpio  = 18,
+    .baud_rate = 12500 * 1000
+}};
+
+// Hardware Configuration of the SD Card "objects"
+static sd_card_t sd_cards[] = {{
+    .pcName = "0:",
+    .spi = &spis[0],
+    .ss_gpio = 1,
+    .use_card_detect = false
+}};
+
+int img_x_grid[] = {3, 83, 163};
+int img_y_grid[] = {30, 118, 206};
+
+// Button IRQ (for controlling screen state)
+void BUTTON_irq(uint gpio, uint32_t events) {
+    if(gpio==BUTTON_A){
+        // if button A is pressed: increment through screen states
+        switch(current_screen_state) {
+            case HOME:
+                current_screen_state=DEMO;
+                break;
+            // case CONSOLE:
+            //     current_screen_state=DEMO;
+            //     break;
+            case DEMO:
+                current_screen_state=POWER;
+                break;
+            case POWER:
+                current_screen_state=HOME;
+                break;
+            default:
+                current_screen_state = HOME;
+        }
+  
+    }
+    else if (gpio==BUTTON_B){
+        // if button B is pressed: decrement through screen states
+        switch(current_screen_state) {
+            case HOME:
+                current_screen_state=POWER;
+                break;
+            // case CONSOLE:
+            //     current_screen_state=HOME;
+            //     break;
+            case DEMO:
+                current_screen_state=HOME;
+                break;
+            case POWER:
+                current_screen_state=DEMO;
+                break;
+            default:
+                current_screen_state = HOME;
+        }
+    }
+    else if (gpio == BUTTON_X){
+        BUTTON_X_pressed = true;
+    } 
+    else if (gpio == BUTTON_Y){
+        BUTTON_Y_pressed = true;
+    }
+
+}
+
+// Screen refresh, clears the screen and sets the header and footer bar
+void screen_refresh(){
+    graphics.set_pen(BG);
+    graphics.clear();
+    Rect blank(0,302,240,18);
+    graphics.set_pen(WHITE);
+    graphics.rectangle(blank);
+    graphics.set_pen(WHITE);
+    Rect blank1(0,0,240,18);
+    graphics.rectangle(blank1);
+}
+
+// Routine for the HOME page: just displays a welcome message
+void HOME_ROUTINE(){
+    Point title_location(0, 2);
+    Point text_location(10,50);
+    if(current_screen_state!=last_screen_state){
+        graphics.set_pen(WHITE);
+        Rect blank(0,0,240,18);
+        graphics.rectangle(blank);
+        graphics.set_pen(BG);
+        graphics.text("      <-    HOME     ->", title_location, 600);
+        graphics.set_pen(WHITE);
+        graphics.text("Welcome to Fast-KNN!",text_location, 230);
+        text_location.y+=30;
+        graphics.text("Fast-kNN is a k-Nearest Neighbour algorithm within the nanoSoC re-usable SoC framework. To use the testboard- press the A button to go one screen forward and B button to go back a screen", text_location, 225);
+        last_screen_state = HOME;
+        nanosoc_console = false;
+    }
+    if(BUTTON_X_pressed){
+        BUTTON_X_pressed=false;
+    }
+    if(BUTTON_Y_pressed){
+        BUTTON_Y_pressed=false;
+    }
+
+}
+
+// Routine for CONSOLE Page, prints out everything in the console FIFO to screen and clears when it gets to the end
+void CONSOLE_ROUTINE(){
+    Point title_location(0, 2);
+    Point footer_location(10,304);
+    Rect console_BG(0,18,240,284);
+    uint32_t data;
+    nanosoc_console = true;
+    if(current_screen_state!=last_screen_state){
+        console_text_location.x = 5;
+        console_text_location.y = 30;
+        graphics.set_pen(BG);
+        graphics.text("      <-   CONSOLE   ->", title_location, 600);
+        graphics.text("   Reset          Clear  ",footer_location,600);
+        last_screen_state = CONSOLE;
+    } 
+    if(current_screen_state==last_screen_state){
+        if(nanosoc_console_draw){
+            if (console_text_location.y > 298){
+                console_text_location.y = 30;
+                graphics.set_pen(BG);
+                graphics.rectangle(console_BG);
+            }
+
+            graphics.set_pen(WHITE);
+            graphics.text(console[console_addr_screen], console_text_location, 1000);
+            console_text_location.y += 16;
+
+            // update screen
+            //st7789.update(&graphics);
+            console[console_addr_screen].clear();
+            console_addr_screen++;
+            if(console_addr_screen == CONSOLE_FIFO_DEPTH){
+                console_addr_screen=0;
+            }
+            if(console[console_addr_screen].empty()){
+                nanosoc_console_draw=false;
+            }
+        }
+    }
+    if(BUTTON_X_pressed){
+        // Clear console and fifo
+        graphics.set_pen(BG);
+        graphics.rectangle(console_BG);
+        console_text_location.y=30;
+        for(int i = 0; i<CONSOLE_FIFO_DEPTH; i++){
+            console[i].clear();
+        }
+        console_addr = 0;
+        console_addr_screen=0;
+        BUTTON_X_pressed=false;
+    }
+    if(BUTTON_Y_pressed){
+        nanosoc_reset(i2c);
+        BUTTON_Y_pressed=false;
+    }
+}
+
+// Routine for Demo Page
+// Draws the unlabelled image that is currently being processed and once finished prints the classification label
+void DEMO_ROUTINE(){
+    Point title_location(0, 2);
+    Point result_location;
+    Point footer_location(10,304);
+    int x_plot;
+    int y_plot;
+    if(current_screen_state!=last_screen_state){
+        graphics.set_pen(BG);
+        graphics.text("     <- Fast KNN DEMO ->", title_location, 600);
+        graphics.text("               Start   ",footer_location,600);
+        last_screen_state = DEMO;
+        nanosoc_console = false;
+    }
+    if(current_screen_state==last_screen_state){
+        if(KNN_start){
+            x_plot = img_x_grid[x];
+            y_plot = img_y_grid[y];
+
+            screen_draw_vector(graphics,st7789,&unlabelled_buffer[current_unlab_img][0],x_plot, y_plot,28,28,2);
+
+            result_location.x = img_x_grid[xtext];
+            result_location.y = img_y_grid[ytext]+60;
+            // Clear text area
+            graphics.set_pen(BG);
+            Rect blank(result_location.x-2, result_location.y-4, 80, 22);
+            graphics.rectangle(blank);
+
+            x++;
+            if(x>=3){
+                x=0;
+                y++;
+                if(y>=3){
+                    y=0;
+                }
+            }
+            KNN_start=false;
+            printf("printed image\n");
+        }
+        if(KNN_done){
+            result_location.x = img_x_grid[xtext];
+            result_location.y = img_y_grid[ytext]+60;
+
+            // KNN finished, plot the result
+            graphics.set_pen(WHITE);
+            graphics.text(classes[predicted_label], result_location, result_location.x+56);
+            xtext++;
+            if(xtext>=3){
+                xtext=0;
+                ytext++;
+                if(ytext>=3){
+                    ytext=0;
+                }
+            }
+            printf("printed result\n");
+            KNN_done=false;
+        }
+    }
+    if(BUTTON_X_pressed){
+        DEMO_RUN=true;
+        BUTTON_X_pressed=false;
+    }
+    if(BUTTON_Y_pressed){
+        BUTTON_Y_pressed=false;
+    }
+}
+
+void POWER_ROUTINE(){
+    Point title_location(0, 2);
+    Point footer_location(10,304);
+    Point text_location(5,50);
+    Point power_stat_location(185,50);
+    Rect power_stat_bg(183,48,100,115);
+    float result;
+    char print_result[10];
+    std::string print_buf;
+    if(current_screen_state!=last_screen_state){
+        graphics.set_pen(BG);
+        graphics.text("      <-    POWER    ->", title_location, 600);
+        graphics.text("                    ",footer_location,600);
+        graphics.set_pen(WHITE);
+        graphics.text("Core Voltage  (V)= ",text_location,600);
+        text_location.y+=16;
+        graphics.text("Core Current (mA)= ",text_location,600);
+        text_location.y+=16;
+        graphics.text("Core Power   (mW)= ",text_location,600);
+        text_location.y+=16;
+        text_location.y+=16;
+        graphics.text("Accel Voltage (V)= ",text_location,600);
+        text_location.y+=16;
+        graphics.text("Accel Current(mA)= ",text_location,600);
+        text_location.y+=16;
+        graphics.text("Accel Power  (mW)= ",text_location,600);
+        last_screen_state = POWER;
+        nanosoc_console = false;
+    } 
+    if(current_screen_state==last_screen_state){
+
+        power_stat_location.y=50;
+
+        graphics.set_pen(BG);
+        graphics.rectangle(power_stat_bg);
+        graphics.set_pen(WHITE);
+
+        result = nanosoc_read_bus_voltage(i2c, VDD_MONITOR);
+        sprintf(print_result, "%1.3f",result);
+        print_buf = print_result;
+        graphics.text(print_buf, power_stat_location, 600);
+        power_stat_location.y+=16;
+
+        result = 1e3*nanosoc_read_current(i2c, VDD_MONITOR);
+        sprintf(print_result, "%1.3f",result);
+        print_buf = print_result;
+        graphics.text(print_buf, power_stat_location, 600);
+        power_stat_location.y+=16;
+
+        result = 1e3*nanosoc_read_power(i2c, VDD_MONITOR);
+        sprintf(print_result, "%1.3f",result);
+        print_buf = print_result;
+        graphics.text(print_buf, power_stat_location, 600);
+        power_stat_location.y+=16;
+        power_stat_location.y+=16;
+
+        result = nanosoc_read_bus_voltage(i2c, VDDACC_MONITOR);
+        sprintf(print_result, "%1.3f",result);
+        print_buf = print_result;
+        graphics.text(print_buf, power_stat_location, 600);
+        power_stat_location.y+=16;
+
+        result = 1e3*nanosoc_read_current(i2c, VDDACC_MONITOR);
+        sprintf(print_result, "%1.3f",result);
+        print_buf = print_result;
+        graphics.text(print_buf, power_stat_location, 600);
+        power_stat_location.y+=16;
+
+        result = 1e3*nanosoc_read_power(i2c, VDDACC_MONITOR);
+        sprintf(print_result, "%1.3f",result);
+        print_buf = print_result;
+        graphics.text(print_buf, power_stat_location, 600);
+        power_stat_location.y+=16;
+    }
+    if(BUTTON_X_pressed){
+        BUTTON_X_pressed=false;
+    }
+    if(BUTTON_Y_pressed){
+        BUTTON_Y_pressed=false;
+    }
+}
+
+void core1_entry(){
+    // Set GPIO for buttons
+    gpio_init(BUTTON_A);
+    gpio_set_dir(BUTTON_A, GPIO_IN);
+    gpio_pull_up(BUTTON_A);
+    gpio_init(BUTTON_B);
+    gpio_set_dir(BUTTON_B, GPIO_IN);
+    gpio_pull_up(BUTTON_B);
+    gpio_init(BUTTON_X);
+    gpio_set_dir(BUTTON_X, GPIO_IN);
+    gpio_pull_up(BUTTON_X);
+    gpio_init(BUTTON_Y);
+    gpio_set_dir(BUTTON_Y, GPIO_IN);
+    gpio_pull_up(BUTTON_Y);
+
+    // Setup interrupts for GPIO 
+    gpio_set_irq_enabled_with_callback(BUTTON_A, GPIO_IRQ_EDGE_RISE, true, &BUTTON_irq);
+    gpio_set_irq_enabled(BUTTON_B, GPIO_IRQ_EDGE_RISE, true);
+    gpio_set_irq_enabled(BUTTON_X, GPIO_IRQ_EDGE_RISE, true);
+    gpio_set_irq_enabled(BUTTON_Y, GPIO_IRQ_EDGE_RISE, true);
+
+
+    // Set screen backlight (0-255)
+    st7789.set_backlight(255);
+
+
+    Point title_location(100, 0);
+
+    graphics.set_pen(BG);
+    graphics.clear();
+    graphics.set_pen(WHITE);
+    graphics.set_font(&font8);
+    while(1){
+        if(current_screen_state!=last_screen_state){
+            screen_refresh();
+        }
+
+        switch(current_screen_state) {
+            case HOME:
+                HOME_ROUTINE();
+                break;
+            // case CONSOLE:
+            //     CONSOLE_ROUTINE();
+            //     break;
+            case DEMO:
+                DEMO_ROUTINE();
+                break;
+            case POWER:
+                POWER_ROUTINE();
+                break;
+            default:
+                current_screen_state = HOME;
+        }
+        st7789.update(&graphics);
+    }
+}
+
+void send_data_to_accelerator(PIO *pio, uint *sm, int *nanosoc_img_buffer_addr, uint8_t *data);
+int compare_indices(void *arr, const void *a, const void *b);
+void sort_indices(uint32_t *array, int *indices, size_t size);
+uint8_t predict_label(int *sorting_indices, int k);
+uint8_t find_max_index(uint8_t *array, size_t size);
+
+int main() {
+    // Initialize GPIO pins
+    gpio_init(DATA_REQ_GPIO_PIN);
+    gpio_init(DATA_SENT_GPIO_PIN);
+
+    // Set DATA_REQ_GPIO_PIN as input and DATA_SENT_GPIO_PIN as output
+    gpio_set_dir(DATA_REQ_GPIO_PIN, GPIO_IN);
+    gpio_set_dir(DATA_SENT_GPIO_PIN, GPIO_OUT);
+
+    // Pull DATA_SENT_GPIO_PIN LOW
+    gpio_put(DATA_SENT_GPIO_PIN, 0);
+
+    printf("DATA_REQ_GPIO_PIN direction: %d\n", gpio_is_dir_out(DATA_REQ_GPIO_PIN));
+    printf("DATA_SENT_GPIO_PIN direction: %d\n", gpio_is_dir_out(DATA_SENT_GPIO_PIN));
+
+    // Check pull-up and pull-downs for DATA_REQ_GPIO_PIN
+    gpio_set_pulls(DATA_REQ_GPIO_PIN, false, false);
+    printf("DATA_REQ_GPIO_PIN pulled down: %d\n", gpio_is_pulled_down(DATA_REQ_GPIO_PIN));
+    printf("DATA_REQ_GPIO_PIN pulled high: %d\n", gpio_is_pulled_up(DATA_REQ_GPIO_PIN));
+
+    // Check pull-up and pull-downs for DATA_SENT_GPIO_PIN
+    gpio_set_pulls(DATA_SENT_GPIO_PIN, false, false);
+    printf("DATA_SENT_GPIO_PIN pulled down: %d\n", gpio_is_pulled_down(DATA_SENT_GPIO_PIN));
+    printf("DATA_SENT_GPIO_PIN pulled high: %d\n", gpio_is_pulled_up(DATA_SENT_GPIO_PIN));
+
+
+    stdio_init_all();
+    //while(!stdio_usb_connected()){;}
+
+    sleep_ms(500);
+
+    // Setup nanosoc
+    nanosoc_i2c_init(i2c);
+
+    PIO pio = pio0;
+	uint offset = pio_add_program(pio, &ft1248x1_sm_program);
+	uint sm = pio_claim_unused_sm(pio, true);
+
+	ft1248x1_sm_program_init(pio, sm, offset);
+	pio_sm_clear_fifos(pio, sm);
+	pio_sm_restart(pio, sm);
+    sleep_ms(500);  
+
+    char buf[128];
+    int len;
+
+
+    // Mount SD card
+    sd_card_t *pSD = &sd_cards[0];
+    FRESULT fr = f_mount(&pSD->fatfs, pSD->pcName, 1);
+    if (FR_OK != fr) {
+        printf("f_mount error: %s (%d)\n", FRESULT_str(fr), fr);
+    }
+    else{
+        printf("SD Card mounted\n");
+    }
+
+    // Read contents of labelled image directory from SD card
+    DIR dir;
+    static FILINFO fno;
+    uint i;
+    FRESULT res;
+    printf("Read names of labelled image files from SD card\n");
+    for (int i = 0; i < NUM_OF_CLASSES; i++) {
+        res = f_opendir(&dir, labelled_image_dirs[i].c_str());
+        if (res == FR_OK) {
+            for (int j = 0; j < NUM_LABELLED_IMAGES_PER_CLASS; j++) {
+                res = f_readdir(&dir, &fno);                    /* Read a directory item */
+                if (res != FR_OK || fno.fname[0] == 0) break;   /* Break on error or end of dir */
+                else {                                          /* It is a file. */
+                    labelled_files[i][j] = fno.fname;
+                    labelled_imgs_labels[j+i*NUM_LABELLED_IMAGES_PER_CLASS] = i;
+                    printf("Loaded file: %s, label: %d\n", labelled_files[i][j].c_str(), i);
+                }
+            }
+            f_closedir(&dir);
+        }
+    }
+
+    // Read contents of unlabelled image directory from SD card
+    printf("Read names of unlabelled image files from SD card\n");
+    res = f_opendir(&dir, unlabelled_image_dir.c_str());
+    if (res == FR_OK) {
+        for (int i = 0; i < NUM_UNLABELLED_IMGS; i++) {
+            res = f_readdir(&dir, &fno);                   /* Read a directory item */
+            if (res != FR_OK || fno.fname[0] == 0) break;  /* Break on error or end of dir */
+            else {                                         /* It is a file. */
+                unlabelled_files[i] = fno.fname;
+                printf("Loaded file: %s\n", unlabelled_files[i].c_str());
+            }
+        }
+        f_closedir(&dir);
+    }
+
+    // Clear FT1248 buffer
+    while(!pio_sm_is_rx_fifo_empty(pio,sm)){
+        len = pio_ft1248_read8_blocking(pio, sm, &buf[0]);
+        for (int i = 0; i < len; i++)
+        {
+            printf("%c", (buf[i]));
+        }
+    }
+
+    // Load labelled images to buffer
+    FIL fil;
+    for(int i = 0; i < NUM_OF_CLASSES; i++){
+        f_chdir(labelled_image_dirs[i].c_str());
+        for(int j = 0; j < NUM_LABELLED_IMAGES_PER_CLASS; j++){
+            fr = f_open(&fil, labelled_files[i][j].c_str(), FA_READ);
+            if (FR_OK != fr && FR_EXIST != fr){
+                printf("f_open(%s) error: %s (%d)\n", labelled_files[i][j].c_str(), FRESULT_str(fr), fr);
+            }
+            char fil_buf[8]="";
+            char * pEnd;
+            int k=0;
+            while(f_gets(fil_buf, 8, &fil)){
+                labelled_buffer[j+i*NUM_LABELLED_IMAGES_PER_CLASS][k] = strtol(fil_buf,&pEnd,16);
+                k++;
+            }
+            fr = f_close(&fil);
+            if (FR_OK != fr) {
+                printf("f_close error: %s (%d)\n", FRESULT_str(fr), fr);
+            }
+        }
+        f_chdir("/");
+    }
+
+    // Load unlabelled images to buffer
+    f_chdir(unlabelled_image_dir.c_str());
+    for(int i = 0; i < NUM_UNLABELLED_IMGS; i++){
+        fr = f_open(&fil, unlabelled_files[i].c_str(), FA_READ);
+        if (FR_OK != fr && FR_EXIST != fr){
+            printf("f_open(%s) error: %s (%d)\n", unlabelled_files[i].c_str(), FRESULT_str(fr), fr);
+        }
+        char fil_buf[8]="";
+        char * pEnd;
+        int k=0;
+        printf("File name: %s, Unlabelled img %u\n",  unlabelled_files[i].c_str(), i);
+        while(f_gets(fil_buf, 8, &fil)){
+            unlabelled_buffer[i][k] = strtol(fil_buf,&pEnd,16);
+            // printf("0x%02x ", unlabelled_buffer[j][k]);
+            k++;
+            if (k % 16 == 0) {
+                // printf("\n");
+            }
+        }
+        fr = f_close(&fil);
+        if (FR_OK != fr) {
+            printf("f_close error: %s (%d)\n", FRESULT_str(fr), fr);
+        }
+
+    }
+    f_chdir("/");
+
+    // Download program to nanosoc
+    f_chdir("./programs");
+    fr = f_open(&fil, "test_gpio.hex", FA_READ);
+    if (FR_OK != fr && FR_EXIST != fr){
+       printf("f_open(test_gpio.hex) error: %s (%d)\n", FRESULT_str(fr), fr);
+       printf("Can't open demo program. Is SD card inserted?\n");
+       return 1;
+    }
+
+    nanosoc_download_program(pio, sm, fil);
+    
+    fr = f_close(&fil);
+    if (FR_OK != fr) {
+       printf("f_close error: %s (%d)\n", FRESULT_str(fr), fr);
+    }
+    f_chdir("/");
+    f_unmount(pSD->pcName);
+
+    // Reset nanosoc for program start
+    nanosoc_reset(i2c);
+    sleep_ms(100);
+
+    // Launch Core 1 to handle graphics
+    multicore_launch_core1(core1_entry);
+    
+    // Read the address of the nanosoc image buffer
+    nanosoc_img_buffer_addr = nanosoc_read_reg32(pio, sm, 0x80000000);
+    printf("img_buffer_addr = %08x\n", nanosoc_img_buffer_addr);
+
+    // Allocate memory for the array using calloc
+    uint32_t *dist = (uint32_t *)calloc(NUM_LABELLED_IMGS, sizeof(uint32_t));
+    int indices[NUM_LABELLED_IMGS];
+
+    // Main loop for Core 0
+    while(1){
+        if (DEMO_RUN && !KNN_done) {                
+            // Reset accelerator
+            nanosoc_download_buffer(pio, sm, &logic_zero[0], sw_reset_reg_addr, 4);
+
+            // Send unlabelled image i
+            send_data_to_accelerator(&pio, &sm, &nanosoc_img_buffer_addr, &unlabelled_buffer[current_unlab_img][0]);
+
+            KNN_start=true;
+
+            for (int j = 0; j < NUM_LABELLED_IMGS; j++) {
+                // Send labelled image j
+                send_data_to_accelerator(&pio, &sm, &nanosoc_img_buffer_addr, &labelled_buffer[j][0]);
+
+                // Set priming mode to 0
+                nanosoc_download_buffer(pio, sm, &logic_zero[0], priming_mode_reg_addr, 4);
+
+                uint32_t unlab_img_dot_prod = nanosoc_read_reg32(pio, sm, unlab_img_dot_prod_addr);
+                uint32_t lab_img_dot_prod = nanosoc_read_reg32(pio, sm, lab_img_dot_prod_addr);
+                uint32_t comb_dot_prod = nanosoc_read_reg32(pio, sm, comb_dot_prod_addr);
+                uint32_t priming_mode = nanosoc_read_reg32(pio, sm, priming_mode_reg_addr);
+
+                // printf("\nUNLAB_IMG_DOT_PROD: %u\n", unlab_img_dot_prod);
+                // printf("LAB_IMG_DOT_PROD: %u\n", lab_img_dot_prod);
+                // printf("COMB_DOT_PROD: %u\n", comb_dot_prod);
+                // printf("PRIMING MODE: %u\n\n", priming_mode);
+
+                // Compute distance between unlabelled image i and labelled image j
+                dist[j] = unlab_img_dot_prod + lab_img_dot_prod - 2*comb_dot_prod;
+
+                // Clear lab_img_dot_prod and comb_dot_prod_reg
+                nanosoc_download_buffer(pio, sm, &logic_zero[0], lab_img_dot_prod_addr, 4);
+                nanosoc_download_buffer(pio, sm, &logic_zero[0], comb_dot_prod_addr, 4);
+            }
+
+
+            sort_indices(dist, indices, NUM_LABELLED_IMGS);
+            printf("kNN labels: ");
+            for (size_t i = 0; i < kNN_k; i++) {
+                printf("%d ", labelled_imgs_labels[indices[i]]);
+            }
+            printf("\n");
+
+            predicted_label = predict_label(&indices[0], kNN_k);
+            printf("Unlabelled image %s, Predicted label: %s\n", unlabelled_files[i].c_str(), classes[predicted_label].c_str());
+
+            KNN_done = true;
+            current_unlab_img++;
+
+            if(current_unlab_img >= NUM_UNLABELLED_IMGS) {
+                current_unlab_img=0;
+            }
+        }
+
+        // Always read in and store STDIO from nanosoc
+        while(!pio_sm_is_rx_fifo_empty(pio,sm)){
+            len = pio_ft1248_readline(pio, sm, buf);
+
+            for (int i =0; i <len; i++){
+                console[console_addr] += buf[i];
+            }
+            if(current_screen_state!=CONSOLE){
+                 printf(console[console_addr].c_str());
+                 console[console_addr].clear();
+            } else{
+                console_addr++;
+                if(console_addr == CONSOLE_FIFO_DEPTH){
+                    console_addr = 0;
+                }
+                nanosoc_console_draw = true;
+                if(nanosoc_console!=true){
+                    break;
+                }
+            }
+        }
+        
+    }
+    return 0;
+}
+
+void send_data_to_accelerator(PIO *pio, uint *sm, int *nanosoc_img_buffer_addr, uint8_t *data) {
+    // Wait for DATA_REQ signal
+    // printf("Waiting for DATA_REQ signal...\n");
+    char buf[128];
+    int len;
+
+    while(gpio_get(DATA_REQ_GPIO_PIN) == false) {
+        if(!pio_sm_is_rx_fifo_empty(*pio, *sm))
+            len = pio_ft1248_readline(*pio, *sm, buf);
+    };
+    // printf("DATA_REQ signal received. Sending data...\n");
+
+    // Send data
+    nanosoc_download_buffer(*pio, *sm, data, *nanosoc_img_buffer_addr, 784);
+
+    // Pull DATA_SENT_GPIO_PIN HIGH
+    gpio_put(DATA_SENT_GPIO_PIN, true);
+    // printf("DATA_SENT signal pulled HIGH.\n");
+    // printf("DATA_SENT_GPIO_PIN level: %d\n", gpio_get_out_level(DATA_SENT_GPIO_PIN));
+
+
+    // Wait for DATA_REQ signal to be pulled LOW 
+    // printf("Waiting for DATA_REQ signal to be pulled LOW...\n");
+    while(gpio_get(DATA_REQ_GPIO_PIN) == true) {
+        if(!pio_sm_is_rx_fifo_empty(*pio, *sm))
+            len = pio_ft1248_readline(*pio, *sm, buf);
+    };
+    // printf("DATA_REQ signal pulled LOW.\n");
+
+    // Pull DATA_SENT_GPIO_PIN LOW
+    gpio_put(DATA_SENT_GPIO_PIN, false);
+    // printf("DATA_SENT signal pulled LOW.\n");
+    // printf("DATA_SENT_GPIO_PIN level: %d\n", gpio_get_out_level(DATA_SENT_GPIO_PIN));
+}
+
+int compare_indices(void *arr, const void *a, const void *b) {
+    uint32_t *array = (uint32_t *)arr;
+    int idx1 = *(const int *)a;
+    int idx2 = *(const int *)b;
+    if (array[idx1] < array[idx2]) return -1;
+    if (array[idx1] > array[idx2]) return 1;
+    return 0;
+}
+
+void sort_indices(uint32_t *array, int *indices, size_t size) {
+    for (size_t i = 0; i < size; i++) {
+        indices[i] = i;
+    }
+    qsort_r(indices, size, sizeof(int), (void *) array, compare_indices);
+}
+
+uint8_t predict_label(int *sorting_indices, int k){
+    uint8_t *label_counts = (uint8_t *)calloc(NUM_OF_CLASSES, sizeof(uint8_t));
+    for (int i = 0; i < k; i++) {
+        uint8_t label = labelled_imgs_labels[sorting_indices[i]];
+        label_counts[label]++;
+    }
+
+    for (int i = 0; i < NUM_OF_CLASSES; i++) {
+        printf("label %d: %u \n", i, label_counts[i]);
+    }
+
+    uint8_t max_index = find_max_index(&label_counts[0], NUM_OF_CLASSES);
+    return max_index;
+}
+
+// Function to find the index of the maximum value in an array
+uint8_t find_max_index(uint8_t *array, size_t size) {
+    uint8_t max_index = 0; // Initialize the maximum index to the first element
+    for (size_t i = 1; i < size; i++) {
+        if (array[i] > array[max_index]) {
+            max_index = i; // Update the maximum index if a larger value is found
+        }
+    }
+
+    return max_index;
+}
diff --git a/nanosoc_board/C_Cxx/fast_knn_demo/fast_knn_driver.h b/nanosoc_board/C_Cxx/fast_knn_demo/fast_knn_driver.h
new file mode 100644
index 0000000..a057fb0
--- /dev/null
+++ b/nanosoc_board/C_Cxx/fast_knn_demo/fast_knn_driver.h
@@ -0,0 +1,11 @@
+// Accelerator engine registers
+#define FAST_KNN_REGS_BASE (0x60008000UL)
+ 
+typedef struct {
+    uint32_t UNLAB_IMG_DOT_PROD;	//ADDR offset 0x00
+    uint32_t LAB_IMG_DOT_PROD;		//ADDR offset 0x04
+    uint32_t COMB_DOT_PROD;			//ADDR offset 0x08
+    uint32_t BYTE_COUNTER_REG;		//ADDR offset 0x0C
+    uint32_t PRIMING_MODE_REG;		//ADDR offset 0x10
+    uint32_t SW_RESET_REG;			//ADDR offset 0x14
+} FAST_KNN_regs_typedef;
\ No newline at end of file
diff --git a/nanosoc_board/C_Cxx/fast_knn_demo/hardware_config.c b/nanosoc_board/C_Cxx/fast_knn_demo/hardware_config.c
new file mode 100644
index 0000000..f581d40
--- /dev/null
+++ b/nanosoc_board/C_Cxx/fast_knn_demo/hardware_config.c
@@ -0,0 +1,73 @@
+/* hw_config.c
+Copyright 2021 Carl John Kugler III
+
+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.
+*/
+/*
+
+This file should be tailored to match the hardware design.
+
+There should be one element of the spi[] array for each hardware SPI used.
+
+There should be one element of the sd_cards[] array for each SD card slot.
+The name is should correspond to the FatFs "logical drive" identifier.
+(See http://elm-chan.org/fsw/ff/doc/filename.html#vol)
+The rest of the constants will depend on the type of
+socket, which SPI it is driven by, and how it is wired.
+
+*/
+
+#include <string.h>
+//
+#include "my_debug.h"
+//
+#include "hw_config.h"
+//
+#include "ff.h" /* Obtains integer types */
+//
+#include "diskio.h" /* Declarations of disk functions */
+
+static spi_t spis[] = {{
+    .hw_inst = spi0,
+    .miso_gpio = 0,
+    .mosi_gpio = 19,
+    .sck_gpio  = 18,
+    .baud_rate = 12500 * 1000
+}};
+
+
+// Hardware Configuration of the SD Card "objects"
+static sd_card_t sd_cards[] = {{
+    .pcName = "0:",
+    .spi = &spis[0],
+    .ss_gpio = 1,
+    .use_card_detect = false
+}};
+
+/* ********************************************************************** */
+size_t sd_get_num() { return count_of(sd_cards); }
+sd_card_t *sd_get_by_num(size_t num) {
+    if (num <= sd_get_num()) {
+        return &sd_cards[num];
+    } else {
+        return NULL;
+    }
+}
+size_t spi_get_num() { return count_of(spis); }
+spi_t *spi_get_by_num(size_t num) {
+    if (num <= sd_get_num()) {
+        return &spis[num];
+    } else {
+        return NULL;
+    }
+}
+
+/* [] END OF FILE */
-- 
GitLab