support J60-10， J60-6 for GD32f425ret6 from Zhang_chaojie

it is OK

.gitignore

@@ -3,6 +3,7 @@
 *libmusl.a
 *liblwip.a
 .DS_Store
+*.pyc
 
 .cache/
 compile_commands.json
@@ -11,3 +12,4 @@ Ubiquitous/XiZi_AIoT/services/app/bin/*
 Ubiquitous/XiZi_AIoT/build/*
 Ubiquitous/XiZi_AIoT/services/app/fs.img
 Ubiquitous/XiZi_AIoT/services/tools/mkfs/mkfs
+APP_Framework/Framework/knowing/micropython/build/*

APP_Framework/Applications/app_test/Kconfig

@@ -307,5 +307,14 @@ menu "test app"
                 bool "Config test usb camera"
                 default n
 
+            menuconfig USER_TEST_MPU
+                bool "Config test MPU fault (Task Isolation)"
+                default n
+                depends on TASK_ISOLATION
+                help
+                    Enable MPU fault testing for task isolation feature.
+                    This test creates user tasks that trigger controlled MPU violations
+                    to verify memory protection is working correctly.
+
         endif
 endmenu

APP_Framework/Applications/app_test/Makefile

@@ -175,5 +175,9 @@ ifeq ($(CONFIG_ADD_XIZI_FEATURES),y)
         SRC_FILES += test_ftpclient_final/test_ftpclient_final.c test_ftpclient_final/ftp_client/ftp_client.c test_ftpclient_final/ftp_client/my_socket.c
     endif   
 
+    ifeq ($(CONFIG_USER_TEST_MPU),y)
+        SRC_FILES += test_mpu_fault.c
+    endif
+
     include $(KERNEL_ROOT)/compiler.mk
 endif

APP_Framework/Applications/app_test/test_mpu_fault.c

@@ -0,0 +1,313 @@
+/*
+* Copyright (c) 2020 AIIT XUOS Lab
+* XiUOS is licensed under Mulan PSL v2.
+* You can use this software according to the terms and conditions of the Mulan PSL v2.
+* You may obtain a copy of Mulan PSL v2 at:
+*        http://license.coscl.org.cn/MulanPSL2
+* THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
+* EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
+* MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
+* See the Mulan PSL v2 for more details.
+*/
+
+/**
+* @file:    test_mpu_fault.c
+* @brief:   MPU fault test - trigger controlled MPU violations (User Space Version)
+* @version: 1.0
+* @author:  AIIT XUOS Lab
+* @date:    2025/11/18
+*/
+
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <transform.h>
+
+#ifdef TASK_ISOLATION
+/* Shared memory region for kernel-userspace communication */
+#define MPU_TEST_STATUS_ADDR    0x20051100
+#define MPU_TEST_FAULT_COUNT    0x20051104
+
+typedef enum {
+    MPU_TEST_IDLE = 0,
+    MPU_TEST_RUNNING = 1,
+    MPU_TEST_FAULT_EXPECTED = 2,
+    MPU_TEST_FAULT_TRIGGERED = 3,
+    MPU_TEST_PASSED = 4,
+    MPU_TEST_FAILED = 5
+} MpuTestStatus;
+
+/* Access shared memory variables via fixed addresses */
+#define test_status (*(volatile uint32_t *)MPU_TEST_STATUS_ADDR)
+#define fault_count (*(volatile uint32_t *)MPU_TEST_FAULT_COUNT)
+
+/**
+ * @brief Test 1: Try to read kernel flash (should work - we have RO permission)
+ * @note Use a safe address, not 0x00000000 which is the reset vector
+ */
+void TestMpuReadKernelFlash(void)
+{
+    printf("   NOTE: Reading from known good kernel code address\n");
+    printf("   (Not 0x00000000 - that's the reset vector)\n\n");
+    
+    test_status = MPU_TEST_RUNNING;
+    
+    // Use a known good kernel address - start of flash
+    volatile uint32_t *kernel_addr = (volatile uint32_t *)0x00000100;
+    volatile uint32_t value = *kernel_addr;
+    
+    printf(" Read succeeded from 0x%08X: 0x%08X\n", (uint32_t)kernel_addr, (unsigned int)value);
+    printf("  (This proves kernel flash is readable)\n");
+    test_status = MPU_TEST_PASSED;
+}
+
+/**
+ * @brief User task wrapper for Test 1: Read kernel flash
+ */
+void MpuTestReadKernelFlashTask(void *parameter)
+{    
+    printf("\n====== Test 1: Read Kernel Flash ======\n");
+    TestMpuReadKernelFlash();   
+    printf("\n====== Test 1 Complete ======\n");
+}
+
+void TestMpuReadKernelFlashShell(int argc, char *argv[])
+{
+    UtaskType task;
+    task.prio = 31;
+    task.stack_size = 4096;
+    task.func_param = NULL;
+    task.func_entry = MpuTestReadKernelFlashTask;
+    strncpy(task.name, "mpu_test1", NAME_NUM_MAX);
+    
+    int32_t task_id = UserTaskCreate(task);
+    if (task_id < 0) {
+        printf("ERROR: Failed to create test task!\n");
+        return;
+    }
+    UserTaskStartup(task_id);
+    printf("Test task started (ID=%d)\n", (int)task_id);
+}
+PRIV_SHELL_CMD_FUNCTION(TestMpuReadKernelFlashShell, a MPU test to read kernel flash, PRIV_SHELL_CMD_MAIN_ATTR);
+
+/**
+ * @brief Test 2: Try to write to kernel flash (should FAULT - flash is read-only)
+ */
+void TestMpuWriteKernelFlash(void)
+{
+    printf("Attempting to write to 0x00000100 (kernel flash)...\n");
+    printf("  This SHOULD trigger a MemManage fault!\n\n");
+    
+    test_status = MPU_TEST_FAULT_EXPECTED;
+    
+    // This should trigger MemManage fault - flash is read-only
+    volatile uint32_t *kernel_addr = (volatile uint32_t *)0x00000100;
+    *kernel_addr = 0xDEADBEEF;
+    
+    test_status = MPU_TEST_PASSED;
+}
+
+/**
+ * @brief User task wrapper for Test 2: Write to kernel flash
+ */
+void MpuTestWriteKernelFlashTask(void *parameter)
+{
+    printf("\n====== Test 2: Write to Kernel Flash ======\n");
+    TestMpuWriteKernelFlash();
+    printf("\n====== Test 2 Complete ======\n");
+}
+
+void TestMpuWriteKernelFlashShell(int argc, char *argv[])
+{
+    UtaskType task;
+    task.prio = 31;
+    task.stack_size = 4096;
+    task.func_param = NULL;
+    task.func_entry = MpuTestWriteKernelFlashTask;
+    strncpy(task.name, "mpu_test2", NAME_NUM_MAX);
+    
+    int32_t task_id = UserTaskCreate(task);
+    if (task_id < 0) {
+        printf("ERROR: Failed to create test task!\n");
+        return;
+    }
+    UserTaskStartup(task_id);
+    printf("Test task started (ID=%d)\n", (int)task_id);
+}
+PRIV_SHELL_CMD_FUNCTION(TestMpuWriteKernelFlashShell, a MPU test to write kernel flash, PRIV_SHELL_CMD_MAIN_ATTR);
+
+/**
+ * @brief Test 3: Try to write to user flash (should FAULT - flash is read-only)
+ */
+void TestMpuWriteUserFlash(void)
+{
+    printf("Attempting to write to 0x00100000 (user flash)...\n");
+    printf("  This SHOULD trigger a MemManage fault!\n\n");
+
+    test_status = MPU_TEST_FAULT_EXPECTED;
+    
+    // This should trigger MemManage fault - flash is read-only
+    volatile uint32_t *flash_addr = (volatile uint32_t *)0x00100000;
+    *flash_addr = 0xCAFEBABE;
+    test_status = MPU_TEST_PASSED;
+}
+
+/**
+ * @brief User task wrapper for Test 3: Write to user flash
+ */
+void MpuTestWriteUserFlashTask(void *parameter)
+{
+    printf("\n====== Test 3: Write to User Flash ======\n");    
+    TestMpuWriteUserFlash();
+    printf("\n====== Test 3 Complete ======\n");
+}
+
+void TestMpuWriteUserFlashShell(int argc, char *argv[])
+{
+    UtaskType task;
+    task.prio = 31;
+    task.stack_size = 4096;
+    task.func_param = NULL;
+    task.func_entry = MpuTestWriteUserFlashTask;
+    strncpy(task.name, "mpu_test3", NAME_NUM_MAX);
+    
+    int32_t task_id = UserTaskCreate(task);
+    if (task_id < 0) {
+        printf("ERROR: Failed to create test task!\n");
+        return;
+    }
+    UserTaskStartup(task_id);
+    printf("Test task started (ID=%d)\n", (int)task_id);
+}
+PRIV_SHELL_CMD_FUNCTION(TestMpuWriteUserFlashShell, a MPU test to write user flash, PRIV_SHELL_CMD_MAIN_ATTR);
+
+/**
+ * @brief Test 4: Try to access invalid memory region (should FAULT)
+ */
+void TestMpuAccessInvalidRegion(void)
+{
+    /* Check CONTROL register BEFORE any printf */
+    uint32_t control_before, control_after_print;
+    __asm volatile ("MRS %0, CONTROL" : "=r" (control_before));
+    
+    printf("====== Test 4: Access Invalid Region (User Task) ======\n");
+    
+    /* Check CONTROL register AFTER printf */
+    __asm volatile ("MRS %0, CONTROL" : "=r" (control_after_print));
+    
+    printf("CONTROL before printf: 0x%08X (nPRIV=%d)\n", 
+           (unsigned int)control_before, (control_before & 0x01) ? 1 : 0);
+    printf("CONTROL after printf:  0x%08X (nPRIV=%d)\n", 
+           (unsigned int)control_after_print, (control_after_print & 0x01) ? 1 : 0);
+    
+    if ((control_after_print & 0x01) == 0) {
+        printf("WARNING: Task lost unprivileged mode after printf!\n");
+        printf("  This means printf/syscall is not restoring CONTROL correctly.\n\n");
+    }
+    
+    printf("Attempting to access invalid memory at 0x01A00000\n");
+    printf("  This SHOULD trigger a MemManage fault!\n\n");
+
+    test_status = MPU_TEST_FAULT_EXPECTED;
+    volatile uint32_t *invalid_addr = (volatile uint32_t *)0x01A00000;
+    volatile uint32_t value = *invalid_addr;
+    test_status = MPU_TEST_PASSED;
+}
+
+/**
+ * @brief User task wrapper for Test 4: Access invalid region
+ */
+void MpuTestAccessInvalidTask(void *parameter)
+{
+    /* Check CONTROL at the VERY START of user task, before any other code */
+    uint32_t control_entry;
+    __asm volatile (
+        "MRS %0, CONTROL\n"
+        : "=r" (control_entry)
+        :
+        : "memory"
+    );
+    
+    printf("\n====== Test 4: Access Invalid Region (User Task) ======\n");
+    printf("CONTROL at task entry: 0x%08X (nPRIV=%d)\n", 
+           (unsigned int)control_entry, (control_entry & 0x01) ? 1 : 0);
+    
+    TestMpuAccessInvalidRegion();
+    printf("\n====== Test 4 Complete ======\n");
+}
+
+void TestMpuAccessInvalidShell(int argc, char *argv[])
+{
+    UtaskType task;
+    task.prio = 31;
+    task.stack_size = 4096;
+    task.func_param = NULL;
+    task.func_entry = MpuTestAccessInvalidTask;
+    strncpy(task.name, "mpu_test4", NAME_NUM_MAX);
+    
+    int32_t task_id = UserTaskCreate(task);
+    if (task_id < 0) {
+        printf("ERROR: Failed to create test task!\n");
+        return;
+    }
+    UserTaskStartup(task_id);
+    printf("Test task started (ID=%d)\n", (int)task_id);
+}
+PRIV_SHELL_CMD_FUNCTION(TestMpuAccessInvalidShell, a MPU test to access invalid region, PRIV_SHELL_CMD_MAIN_ATTR);
+
+/**
+ * @brief Test 5: Normal user SRAM access (should work)
+ */
+void TestMpuUserSramAccess(void)
+{
+    printf("Attempting to read/write user SRAM...\n");
+    
+    test_status = MPU_TEST_RUNNING;
+    
+    // This should work - user SRAM is RW
+    volatile uint32_t test_var = 0x12345678;
+    uint32_t read_value = test_var;
+    
+    printf("Write succeeded: 0x%08X\n", 0x12345678);
+    printf("Read succeeded:  0x%08X\n", (unsigned int)read_value);
+    
+    if (read_value == 0x12345678) {
+        printf("Test PASSED - user SRAM is accessible\n");
+        test_status = MPU_TEST_PASSED;
+    } else {
+        printf("Test FAILED - data mismatch!\n");
+        test_status = MPU_TEST_FAILED;
+    }
+}
+
+/**
+ * @brief User task wrapper for Test 5: User SRAM access
+ */
+void MpuTestUserSramAccessTask(void *parameter)
+{
+    printf("\n=== Test 5: User SRAM Access ===\n");
+    
+    TestMpuUserSramAccess();
+    printf("\n=== Test 5: User SRAM Access Complete ===\n");
+}
+
+void TestMpuUserSramAccessShell(int argc, char *argv[])
+{
+    UtaskType task;
+    task.prio = 31;
+    task.stack_size = 4096;
+    task.func_param = NULL;
+    task.func_entry = MpuTestUserSramAccessTask;
+    strncpy(task.name, "mpu_test5", NAME_NUM_MAX);
+    
+    int32_t task_id = UserTaskCreate(task);
+    if (task_id < 0) {
+        printf("ERROR: Failed to create test task!\n");
+        return;
+    }
+    UserTaskStartup(task_id);
+    printf("Test task started (ID=%d)\n", (int)task_id);
+}
+PRIV_SHELL_CMD_FUNCTION(TestMpuUserSramAccessShell, a MPU test for user SRAM access, PRIV_SHELL_CMD_MAIN_ATTR);
+
+#endif /* TASK_ISOLATION */

APP_Framework/Applications/robot_app/Makefile

@@ -0,0 +1,3 @@
+SRC_FILES := main.c
+
+include $(KERNEL_ROOT)/compiler.mk

APP_Framework/Applications/robot_app/README.md

@@ -0,0 +1,34 @@
+# 构建矽璓工业物联操作系统：机器人控制代码
+
+
+## 目录结构
+
+| 名称 | 说明 |
+| -- | -- |
+| configure | 电机配置 |
+| motor_control_algo | 算法包 |
+| protocol | 上层接口协议 |
+
+### 电机配置：
+
+|  目前支持电机型号：|
+| -- |
+|  云深处J60-6 |
+|  云深处J60-10 |
+|  智元R86-3 with STM32F405外挂板 |
+
+### 算法包：
+
+|  目前支持的电机算法：|
+| -- |
+|  自研FOC |
+|  ODRIVE |
+
+### 上层接口协议
+|  目前支持的协议：|
+| -- |
+|  云深处协议 |
+
+## 使用方法
+
+TODO：

APP_Framework/Applications/robot_app/configure/J60/connect_motor.c

@@ -0,0 +1,366 @@
+/* USER CODE BEGIN Header */
+/**
+  ******************************************************************************
+  * @file           : main.c
+  * @brief          : Main program body
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2025 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+  ******************************************************************************
+  */
+/* USER CODE END Header */
+/* Includes ------------------------------------------------------------------*/
+#include "xizi.h"
+/* Private includes ----------------------------------------------------------*/
+/* USER CODE BEGIN Includes */
+#include "foc.h"
+#include "conf.h"
+#include "arm_math.h"
+
+
+struct MotorDriver motor_driver;
+
+void set_pwm_duty(float d_u, float d_v, float d_w)
+{
+   // duty 限幅 [0, 0.9]
+    d_u = min(d_u, 0.9);
+    d_v = min(d_v, 0.9);
+    d_w = min(d_w, 0.9);
+    
+    struct BusBlockWriteParam write_param;
+    float duty[3] = {d_u, d_v, d_w};
+    write_param.buffer = (void *)duty;
+    BusDevWriteData(motor_driver.motor_control.dev, &write_param);
+}
+
+uint16_t angles[1000] = {0};
+static int speed_calc_task = -1;
+void speed_calc_entry(void *parameter)
+{
+    // 10 ticks = 1 ms
+    static uint16_t motor_speed_calc_delay = 5000;
+    static uint16_t motor_speed_calc_freq = 1000;
+    // static float last_position = 0;
+    while (1)
+    {
+        x_ticks_t start = CurrentTicksGain();
+        struct BusBlockReadParam read_param;
+        uint16_t read_data;
+        read_param.buffer = (void *)&read_data;
+        read_param.size = 1;
+        BusDevReadData(motor_driver.motor_encoder.dev, &read_param);
+        g_encoder_angle = (read_data) * RAW_TO_RAD;
+        static float encoder_angle_last = 0;
+        static int once = 1;
+        if (once) {
+            once = 0;
+            encoder_angle_last = g_encoder_angle;
+        }
+        float diff_angle = cycle_diff(g_encoder_angle - encoder_angle_last, 2 * PI);
+        encoder_angle_last = g_encoder_angle;
+
+        // 更新电机逻辑角度
+        g_motor_logic_angle = cycle_diff(g_motor_logic_angle + diff_angle, mult_position_cycle);
+
+        float _motor_speed = diff_angle * motor_speed_calc_freq;
+        float filter_alpha_speed = 0.1f;
+        g_motor_logic_speed = low_pass_filter(_motor_speed, g_motor_logic_speed, filter_alpha_speed);
+        g_motor_speed = g_motor_logic_speed / Reduction_Ratio;
+
+        // if ((int)last_position != (int)g_motor_logic_angle)
+        // {
+        //     last_position = g_motor_logic_angle;
+        //     KPrintf("logic angle: %d \r\n", (int)(g_motor_logic_angle * 100));
+        //     /* code */
+        // }
+
+        x_ticks_t end = CurrentTicksGain();
+        MdelayKTask(motor_speed_calc_delay - (end - start));
+    }
+    
+    
+    // static float last_angle = 0;
+    // float angle_diff = cycle_diff(g_motor_logic_angle - last_angle, 2 * PI);
+    // g_motor_speed = angle_diff / (SAMPLE_TIME_MS / 1000.0f); // rad/s
+    // last_angle = g_motor_logic_angle;
+}
+
+void __FOC_START(void) {
+    BusDevOpen(motor_driver.motor_control.dev);
+    if(speed_calc_task == -1)
+        speed_calc_task = KTaskCreate("speed_calc", speed_calc_entry, NONE, 2048, 30);
+    if (speed_calc_task != -1) 
+        StartupKTask(speed_calc_task);
+}
+
+void __FOC_STOP(void) {
+    BusDevClose(motor_driver.motor_control.dev);
+    if (speed_calc_task != NONE)
+        KTaskDelete(speed_calc_task);
+        speed_calc_task = -1;
+}
+
+void __FOC_BREAK(void) {
+   
+}
+
+
+//todo 角度计算
+void motor_angle_calc(){
+
+}
+
+
+
+// 上电校准函数
+void calibrate_current_offset(void)
+{
+    uint32_t sum_u = 0, sum_v = 0, sum_w = 0;
+    int SAMPLE_COUNT = 10;
+
+    for (int i = 0; i < SAMPLE_COUNT; i++)
+    {
+        // 启动 ADC 注入转换
+        adc_software_trigger_enable(ADC0, ADC_INSERTED_CHANNEL);
+        adc_software_trigger_enable(ADC1, ADC_INSERTED_CHANNEL);
+        adc_software_trigger_enable(ADC2, ADC_INSERTED_CHANNEL);
+
+        // 等待转换完成（可用中断方式，这里简单轮询）
+        while(!adc_flag_get(ADC0, ADC_FLAG_EOIC));
+        while(!adc_flag_get(ADC1, ADC_FLAG_EOIC));
+        while(!adc_flag_get(ADC2, ADC_FLAG_EOIC));
+
+        // 读取注入通道数据
+        uint16_t adc_u = adc_inserted_data_read(ADC0, ADC_INSERTED_CHANNEL_0);  
+        uint16_t adc_v = adc_inserted_data_read(ADC1, ADC_INSERTED_CHANNEL_0);  
+        uint16_t adc_w = adc_inserted_data_read(ADC2, ADC_INSERTED_CHANNEL_0);  
+
+        sum_u += adc_u;
+        sum_v += adc_v;
+        sum_w += adc_w;
+    }
+
+    // 求平均
+    float avg_u = sum_u / (float)SAMPLE_COUNT;
+    float avg_v = sum_v / (float)SAMPLE_COUNT;
+    float avg_w = sum_w / (float)SAMPLE_COUNT;
+
+    // 转换为电压偏置
+    i_offset_u1 = avg_u / ((1 << ADC_BITS) - 1) * ADC_REFERENCE_VOLT;
+    i_offset_u2 = avg_v / ((1 << ADC_BITS) - 1) * ADC_REFERENCE_VOLT;
+    i_offset_u3 = avg_w / ((1 << ADC_BITS) - 1) * ADC_REFERENCE_VOLT;
+
+    // printf("i_offset_u1 = %.3f\r\n", i_offset_u1);
+    // printf("i_offset_u2 = %.3f\r\n", i_offset_u2);
+    // printf("i_offset_u3 = %.3f\r\n", i_offset_u3);
+}
+
+
+// adc callback in interrupt
+void foc_loop(){
+    float u_1 = (ADC_REFERENCE_VOLT * (float)adc_inserted_data_read(ADC0, ADC_INSERTED_CHANNEL_0) / ((1 << ADC_BITS) - 1)) - i_offset_u1;
+    float u_2 = (ADC_REFERENCE_VOLT * (float)adc_inserted_data_read(ADC1, ADC_INSERTED_CHANNEL_0) / ((1 << ADC_BITS) - 1)) - i_offset_u2;
+    float u_3 = (ADC_REFERENCE_VOLT * (float)adc_inserted_data_read(ADC2, ADC_INSERTED_CHANNEL_0) / ((1 << ADC_BITS) - 1)) - i_offset_u3;
+
+    float i_1 = u_1 / (R_SHUNT * OP_GAIN);
+    float i_2 = u_2 / (R_SHUNT * OP_GAIN);
+    float i_3 = u_3 / (R_SHUNT * OP_GAIN);
+
+    // Clarke 输入：i_u, i_v
+    g_motor_i_u = -i_2;
+    g_motor_i_v = -i_3;
+    g_motor_i_w = (i_2 + i_3);
+    //printf("ADC_IRQHandler\r\n");
+    // Clarke 变换
+    float i_alpha = g_motor_i_u;
+    float i_beta  = (g_motor_i_u + 2.0f * g_motor_i_v) * 0.57735026919f; // 1/sqrt(3)
+
+    // Park 变换
+    float sin_value = sinf(rotor_logic_angle);
+    float cos_value = cosf(rotor_logic_angle);
+    float w_e = g_motor_logic_speed * POLE_PAIRS;  // rad/s 机械转速=>电角速度
+    float _motor_i_d =  i_alpha * cos_value + i_beta * sin_value;
+    float _motor_i_q = -i_alpha * sin_value + i_beta * cos_value;
+
+    // 归一化
+    float motor_i_d_norm = _motor_i_d;
+    float motor_i_q_norm = _motor_i_q;
+    
+    static float i_d_hat = 0.0f;
+    static float i_q_hat = 0.0f;
+    float i_d_dot = (_motor_i_d - 0.54 * i_d_hat + w_e * 0.00034 * i_q_hat) / 0.00034;
+    float i_q_dot = (_motor_i_q - 0.54 * i_q_hat - w_e * 0.00034 * i_d_hat - w_e * 0.1283) / 0.00034;
+    // 更新观测电流
+    i_d_hat += 1.0f/20000.0f*5.0f * (i_d_dot + 200.0f * (0 - i_d_hat));   // 无电流传感器 → 期望电流为0的补偿
+    i_q_hat += 1.0f/20000.0f*5.0f * (i_q_dot + 200.0f * (0 - i_q_hat));
+    
+    
+    
+    // 一阶低通滤波
+    float filter_alpha_i_d = 0.3f;
+    float filter_alpha_i_q = 0.3f;
+    g_motor_i_d = low_pass_filter(motor_i_d_norm, g_motor_i_d, filter_alpha_i_d);
+    g_motor_i_q = low_pass_filter(motor_i_q_norm, g_motor_i_q, filter_alpha_i_q);
+
+    
+    float motor_control_torque_i = g_motor_i_q * (1.3473f * MAX_CURRENT * 1.3);
+    
+    float motor_control_torque_o = -1.5f * POLE_PAIRS * 0.1283 * i_q_hat / 28;
+    
+    //motor_control_torque = fabs(motor_control_torque_i) > fabs(motor_control_torque_o) ? motor_control_torque_i : 0;
+    g_motor_control_torque = motor_control_torque_i;
+    // g_motor_control_torque = addValue(&ADC_Iq_filter, g_motor_control_torque);
+    g_motor_i_q = g_motor_control_torque / (1.3473f * MAX_CURRENT);
+    
+    
+    // 控制类型调度
+    switch (motor_control_context.type)
+    {
+        case control_type_position:
+            lib_position_control(motor_control_context.position);
+            break;
+        case control_type_speed:
+            lib_speed_control(motor_control_context.speed);
+            break;
+        case control_type_torque:
+            lib_torque_control(motor_control_context.torque_norm_d, motor_control_context.torque_norm_q);
+            break;
+        case control_type_speed_torque:
+            lib_speed_torque_control(motor_control_context.speed);
+            break;
+        case control_type_position_speed_torque:
+            lib_position_speed_torque_control(motor_control_context.position);
+            break;
+        case control_type_mit_control:
+//                lib_mit_control(0, 0, 0, 0, 1.5);
+            // lib_mit_control(
+            //     ctrl_params.position,
+            //     ctrl_params.speed,
+            //     ctrl_params.kp,
+            //     ctrl_params.kd,
+            //     ctrl_params.torque
+            // );
+            break;
+        default:
+            break;
+    }
+}
+
+
+
+void CanProtocalHandler(void){
+
+}
+
+
+void CanMsgReceived(void){
+    struct BusBlockReadParam read_param;
+    struct BusBlockWriteParam write_param;
+    uint32_t can_id = 4;
+    uint8_t recv_buf[8];
+    *(uint32_t *)recv_buf = can_id; //temp id
+    read_param.buffer = (void *)recv_buf;
+    read_param.size = 8;
+    BusDevReadData(motor_driver.motor_encoder.dev, &read_param);
+
+    // process can message
+    uint8_t send_buf[12];
+    uint8 can_dlc;
+    write_param.buffer = (void *)send_buf;
+    handleCanMessage(can_id, read_param.size, recv_buf, &can_id, send_buf + 4, &can_dlc);
+    *(uint32_t *)send_buf = can_id;
+    write_param.size = can_dlc + 4;
+    BusDevWriteData(motor_driver.motor_can_protocal.dev, &write_param);
+}
+
+
+
+
+int InitHwMotor(void)
+{
+    motor_driver.motor_control.bus = BusFind(HWTIMER_BUS_NAME);
+    motor_driver.motor_control.dev = BusFindDevice(motor_driver.motor_control.bus, HWTIMER_DEVICE1_NAME);
+    motor_driver.motor_control.drv = BusFindDriver(motor_driver.motor_control.bus, HWTIMER_DRIVER_NAME);
+
+    struct BusConfigureInfo configure_info;
+    configure_info.configure_cmd = OPE_INT;
+    BusDrvConfigure(motor_driver.motor_control.drv, &configure_info);
+
+    struct BusBlockReadParam read_param;
+
+    motor_driver.motor_encoder.bus = BusFind(SPI_BUS_NAME);
+    motor_driver.motor_encoder.dev = BusFindDevice(motor_driver.motor_encoder.bus, SPI_DEVICE_NAME);
+    motor_driver.motor_encoder.drv = BusFindDriver(motor_driver.motor_encoder.bus, SPI_DRV_NAME);
+
+    // struct BusConfigureInfo configure_info;
+    configure_info.configure_cmd = OPE_INT;
+    BusDrvConfigure(motor_driver.motor_encoder.drv, &configure_info);
+
+    // read once encoder data
+    BusDevOpen(motor_driver.motor_encoder.dev);
+    uint16_t read_data = 0;
+    read_param.buffer = (void *)&read_data;
+    read_param.size = 1;
+    BusDevReadData(motor_driver.motor_encoder.dev, &read_param);
+
+    motor_driver.motor_can_protocal.bus = BusFind(CAN_BUS_NAME);
+    motor_driver.motor_can_protocal.dev = BusFindDevice(motor_driver.motor_can_protocal.bus, CAN_DEVICE_NAME);
+    motor_driver.motor_can_protocal.drv = BusFindDriver(motor_driver.motor_can_protocal.bus, CAN_DRIVER_NAME);
+
+    BusDevOpen(motor_driver.motor_can_protocal.dev);
+
+    struct CanDriverConfigure can_config;
+    can_config.brp = 5;
+    can_config.mode = 0;
+    can_config.tbs1 = 4;
+    can_config.tbs2 = 3;
+    can_config.tsjw = 0;
+
+    configure_info.configure_cmd = OPE_INT;
+    configure_info.private_data = (void *)&can_config;
+    BusDrvConfigure(motor_driver.motor_can_protocal.drv, &configure_info);
+}
+
+
+
+int MotorStart(void)
+{
+    set_motor_pid(
+      2, 0, 0,
+      0, 0, 0,
+      0, 0, 0,
+      0, 0, 0);
+    foc_start();
+
+    g_encoder_init_angle = g_encoder_angle;
+    set_pwm_duty(0.2, 0, 0); // d轴强拖，形成SVPWM模型中的基础矢量1，即对应转子零度位置
+
+    
+    MdelayKTask(1000); // 等待电机转到零位
+    g_rotor_zero_angle = g_encoder_angle;
+
+    set_pwm_duty(0, 0, 0); // 松开电机
+
+    MdelayKTask(1000); // 等待电机转到零位
+    //   delay_1ms(100);
+
+
+    motor_control_context.type = control_type_mit_control;
+
+    x_err_t flag;
+    MdelayKTask(1);
+}
+
+SHELL_EXPORT_CMD(SHELL_CMD_PERMISSION(0)|SHELL_CMD_TYPE(SHELL_TYPE_CMD_MAIN),
+                    m, MotorStart, motor start function);
+
+
+SHELL_EXPORT_CMD(SHELL_CMD_PERMISSION(0)|SHELL_CMD_TYPE(SHELL_TYPE_CMD_MAIN),
+                    s, foc_stop, motor stop function);

APP_Framework/Applications/robot_app/configure/J60/connect_motor.h

@@ -0,0 +1,53 @@
+/*
+* Copyright (c) 2020 AIIT XUOS Lab
+* XiUOS is licensed under Mulan PSL v2.
+* You can use this software according to the terms and conditions of the Mulan PSL v2.
+* You may obtain a copy of Mulan PSL v2 at:
+*        http://license.coscl.org.cn/MulanPSL2
+* THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
+* EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
+* MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
+* See the Mulan PSL v2 for more details.
+*/
+ 
+/**
+* @file connect_adc.h
+* @brief define stm32f446ret6 adc function and struct
+* @version 1.0
+* @author AIIT XUOS Lab
+* @date 2025-7-31
+*/
+
+#ifndef CONNECT_MOTOR_H
+#define CONNECT_MOTOR_H
+
+#include <device.h>
+
+
+struct XiZiCommonDevice
+{
+    struct Bus *bus;
+    struct HardwareDev *dev;
+    struct Driver *drv;
+};
+
+struct MotorDriver
+{
+    struct XiZiCommonDevice motor_control;
+    struct XiZiCommonDevice motor_encoder;
+    struct XiZiCommonDevice motor_can_protocal;
+    
+    int type; // 控制类型
+    float position; // 位置控制目标 (rad)
+    float speed;    // 速度控制目标 (rad/s)
+    float torque_norm_d; // 力矩控制目标 d 轴分量 (归一化)
+    float torque_norm_q; // 力矩控制目标 q 轴分量 (归一化)
+    float mit_torque;    // MIT 控制目标力矩 (Nm)
+};
+
+
+int InitHwMotor(void);
+int MotorStart(void);
+void CanMsgReceived(void);
+
+#endif

APP_Framework/Applications/robot_app/main.c

@@ -0,0 +1,22 @@
+/*
+* Copyright (c) 2020 AIIT XUOS Lab
+* XiUOS is licensed under Mulan PSL v2.
+* You can use this software according to the terms and conditions of the Mulan PSL v2.
+* You may obtain a copy of Mulan PSL v2 at:
+*        http://license.coscl.org.cn/MulanPSL2
+* THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
+* EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
+* MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
+* See the Mulan PSL v2 for more details.
+*/
+
+#include <stdio.h>
+#include <string.h>
+
+int main(void)
+{
+    printf("\nHello, Robot!\n");
+    RobotInit();
+
+    return 0;
+}

APP_Framework/Applications/robot_app/motor_control_algo/algo.c

@@ -0,0 +1,9 @@
+#include <stdio.h>
+#include <string.h>
+
+
+
+int algo_register()
+{
+
+}

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/foc.c

@@ -0,0 +1,400 @@
+#include "foc.h"
+#include "motor_runtime_param.h"
+#include <stdbool.h>
+#include "math.h"
+
+
+void arm_inv_park_f32( float Id,
+                        float Iq,
+                        float * pIalpha,
+                        float * pIbeta,
+                        float sinVal,
+                        float cosVal)
+{
+    /* Calculate pIalpha using the equation, pIalpha = Id * cosVal - Iq * sinVal */
+    *pIalpha = Id * cosVal - Iq * sinVal;
+
+    /* Calculate pIbeta using the equation, pIbeta = Id * sinVal + Iq * cosVal */
+    *pIbeta = Id * sinVal + Iq * cosVal;
+}
+
+
+motor_control_context_t motor_control_context;
+motor_status_e motor_status = motor_idle;
+
+//interface function weak implement
+__attribute__((weak)) void set_pwm_duty(float d_u, float d_v, float d_w)
+{
+    while (1)
+        ;
+}
+
+__attribute__((weak)) void __FOC_START()
+{
+    while (1)
+        ;
+}
+__attribute__((weak)) void __FOC_STOP()
+{
+    while (1)
+        ;
+}
+__attribute__((weak)) void __FOC_BREAK()
+{
+    while (1)
+        ;
+}
+
+void foc_start(){
+    __FOC_START();
+    motor_status = motor_running;
+}
+
+void foc_stop(){
+    __FOC_STOP();
+    motor_status = motor_idle;
+}
+
+void foc_break(){
+    __FOC_BREAK();
+    motor_status = motor_break;
+}
+
+static void svpwm(float phi, float d, float q, float *d_u, float *d_v, float *d_w)
+{
+    d = min(d, 1);
+    d = max(d, -1);
+    q = min(q, 1);
+    q = max(q, -1);
+    const int v[6][3] = {{1, 0, 0}, {1, 1, 0}, {0, 1, 0}, {0, 1, 1}, {0, 0, 1}, {1, 0, 1}};
+    const int K_to_sector[] = {4, 6, 5, 5, 3, 1, 2, 2};
+    float sin_phi = sinf(phi);
+    float cos_phi = cosf(phi);
+    float alpha = 0;
+    float beta = 0;
+    arm_inv_park_f32(d, q, &alpha, &beta, sin_phi, cos_phi);
+
+    bool A = beta > 0;
+    bool B = fabs(beta) > SQRT3 * fabs(alpha);
+    bool C = alpha > 0;
+
+    int K = 4 * A + 2 * B + C;
+    int sector = K_to_sector[K];
+
+    float t_m = sinf(sector * rad60) * alpha - cosf(sector * rad60) * beta;
+    float t_n = beta * cosf(sector * rad60 - rad60) - alpha * sinf(sector * rad60 - rad60);
+    float t_0 = 1 - t_m - t_n;
+
+    *d_u = t_m * v[sector - 1][0] + t_n * v[sector % 6][0] + t_0 / 2;
+    *d_v = t_m * v[sector - 1][1] + t_n * v[sector % 6][1] + t_0 / 2;
+    *d_w = t_m * v[sector - 1][2] + t_n * v[sector % 6][2] + t_0 / 2;
+    motor_control_context.pwm_u = *d_u;
+}
+
+void foc_forward(float d, float q, float rotor_rad)
+{
+    float d_u = 0;
+    float d_v = 0;
+    float d_w = 0;
+    svpwm(rotor_rad, d, q, &d_u, &d_v, &d_w);
+    set_pwm_duty(d_u, d_v, d_w);
+}
+
+float cycle_diff(float diff, float cycle)
+{
+    if (diff > (cycle / 2))
+        diff -= cycle;
+    else if (diff < (-cycle / 2))
+        diff += cycle;
+    return diff;
+}
+
+float low_pass_filter(float input, float last_output, float alpha)
+{
+    return alpha * input + (1.0 - alpha) * last_output;
+}
+
+// pid control implementation
+PID_t pid_position;
+PID_t pid_speed;
+PID_t pid_torque_d;
+PID_t pid_torque_q;
+void set_motor_pid(
+    float position_p, float position_i, float position_d,
+    float speed_p, float speed_i, float speed_d,
+    float torque_d_p, float torque_d_i, float torque_d_d,
+    float torque_q_p, float torque_q_i, float torque_q_d)
+{
+    PID_Init(&pid_position, position_p, position_i, position_d, MAX_SPEED);
+    PID_Init(&pid_speed, speed_p, speed_i, speed_d,  MAX_CURRENT);
+    PID_Init(&pid_torque_d, torque_d_p, torque_d_i, torque_d_d, 1.0f);
+    PID_Init(&pid_torque_q, torque_q_p, torque_q_i, torque_q_d, 1.0f);
+}
+
+static float position_loop(float rad)
+{
+    float diff = rad - g_motor_logic_angle;
+    return PID_Update(&pid_position, diff);
+}
+
+void lib_position_control(float rad)
+{
+    float d = 0;
+    float q = position_loop(rad);
+    foc_forward(d, q, rotor_logic_angle);
+}
+
+
+static float speed_loop(float speed_rad)
+{
+    float diff = speed_rad - g_motor_speed;
+    return PID_Update(&pid_speed, diff);
+}
+
+
+void lib_speed_control(float speed)
+{
+    float d = 0;
+    float q = speed_loop(speed);
+    foc_forward(d, q, rotor_logic_angle);
+}
+
+
+static float torque_d_loop(float d)
+{
+    float diff = d - g_motor_i_d / MAX_CURRENT;
+    return PID_Update(&pid_torque_d, diff);
+}
+
+static float torque_q_loop(float q)
+{
+    float diff = q - g_motor_i_q / Nm_PER_A / MAX_CURRENT;
+    return PID_Update(&pid_torque_q, diff);
+}
+
+
+void lib_torque_control(float torque_norm_d, float torque_norm_q)
+{
+    float torque = torque_norm_q;
+    float diff = torque - g_motor_i_q * Nm_PER_A * MAX_CURRENT;
+    float q = g_motor_i_q + PID_Update(&pid_torque_q, diff);
+    float d = 0;
+    foc_forward(d, q, rotor_logic_angle);
+}
+
+
+
+
+void lib_speed_torque_control(float speed_rad)
+{
+    float torque_cmd = speed_loop(speed_rad);
+    float d = 0;
+    float q = torque_q_loop(torque_cmd);
+    foc_forward(d, q, rotor_logic_angle);
+}
+
+
+void lib_position_speed_torque_control(float position_rad)
+{
+    float speed_cmd = position_loop(position_rad);
+    float torque_cmd = speed_loop(speed_cmd);
+    float d = 0;
+    float q = torque_q_loop(torque_cmd);
+    foc_forward(d, q, rotor_logic_angle);
+}
+
+
+
+void lib_position_to_target(float target_rad, float max_speed)
+{
+    float speed_cmd = position_loop(target_rad);
+
+    if (speed_cmd > max_speed) speed_cmd = max_speed;
+    else if (speed_cmd < -max_speed) speed_cmd = -max_speed;
+
+    float torque_cmd = speed_loop(speed_cmd);
+
+    float d = 0;
+    float q = PID_Update(&pid_torque_q, torque_cmd - g_motor_i_q);
+    foc_forward(d, q, rotor_logic_angle);
+}
+
+
+void PID_Init(PID_t *pid, float kp, float ki, float kd, float limit) {
+    pid->kp = kp;
+    pid->ki = ki;
+    pid->kd = kd;
+    pid->integral = 0.0f;
+    pid->prev_error = 0.0f;
+    pid->output_limit = limit;
+}
+
+float PID_Update(PID_t *pid, float error) {
+    pid->integral += error * dt;
+    float derivative = (error - pid->prev_error) / dt;
+    float output = pid->kp * error + pid->ki * pid->integral + pid->kd * derivative;
+
+    // <20>޷<EFBFBD>
+    if (output > pid->output_limit) output = pid->output_limit;
+    else if (output < -pid->output_limit) output = -pid->output_limit;
+
+    pid->prev_error = error;
+    return output;
+}
+
+/*************************************************************
+ * MIT 控制器 (Impedance Control)
+ * 公式: τ_cmd = Kp * (q_d - q) + Kd * (qd_d - qd) + τ_ff
+ * 其中：
+ *   q_d   = 目标位置 (rad)
+ *   q     = 当前电机位置 (rad)
+ *   qd_d  = 目标速度 (rad/s)
+ *   qd    = 当前电机速度 (rad/s)
+ *   τ_ff  = 力矩前馈 (Nm)
+ *   τ_cmd = 目标力矩 (Nm)
+ *
+ * 核心流程：
+ *   1. 读当前位置/速度
+ *   2. 计算位置误差、速度误差
+ *   3. 阻抗模型生成目标力矩 τ_cmd
+ *   4. τ_cmd -> 目标电流 Iq -> 归一化 [-1,1]
+ *   5. 走 FOC：svpwm -> set_pwm_duty
+ *************************************************************/
+
+typedef struct {
+    float Kt_Nm_per_A;    // 电机力矩常数 (Nm/A)，电机参数，例：0.08
+    float torque_limit;   // 力矩限幅 (Nm)，防止电机过载
+    float iq_norm_limit;  // 归一化电流限幅 [-1,1]，通常 <=1.0
+    float vel_limit;      // 速度限幅 (rad/s)，保护电机
+    float pos_cycle;      // 位置一圈的周期 (rad)，通常 2π
+    float friction_comp;  // 摩擦补偿力矩 (Nm)，可选
+    float out_pos_limit;  //输出软限位
+    float current_loop_Ts;
+} MIT_Param_t;
+
+// 默认参数（可以运行时通过 MIT_SetParams() 修改）
+static MIT_Param_t mit_param = {
+    .Kt_Nm_per_A   = 1.3473f,        // 电机力矩常数 Nm/A
+    .torque_limit  = 9.58f,         // 峰值扭矩 (Nm)
+    .iq_norm_limit = 1.0f,           // 电流归一化上限 (假设 1.0 = 最大电流 30A)
+    .vel_limit     = 40.0f,         // 最大机械角速度 (rad/s)
+    .pos_cycle     = 2.0f * PI * Reduction_Ratio,      // 一圈 2π rad  1：18的减速比
+    .friction_comp = 0.0f,            // 暂时不用摩擦补偿
+    .out_pos_limit = 40.0f * PI,        // 输出软限位
+    .current_loop_Ts = 0.00004                 //ADC采样周期
+};
+/*************************************************************
+ * 参数配置接口
+ *************************************************************/
+void MIT_SetParams(float Kt, float torque_lim, float iq_norm_lim,
+                   float vel_lim, float pos_cycle, float fric_comp)
+{
+    mit_param.Kt_Nm_per_A   = Kt;
+    mit_param.torque_limit  = torque_lim;
+    mit_param.iq_norm_limit = (iq_norm_lim > 1.0f) ? 1.0f :
+                              (iq_norm_lim < 0.0f ? 0.0f : iq_norm_lim);
+    mit_param.vel_limit     = vel_lim;
+    mit_param.pos_cycle     = pos_cycle;
+    mit_param.friction_comp = fric_comp;
+}
+
+/*************************************************************
+ * MIT 控制函数
+ *
+ * 输入：
+ *   pos_des   = 目标角度 (rad)
+ *   vel_des   = 目标速度 (rad/s)
+ *   kp        = 位置刚度 (Nm/rad)
+ *   kd        = 速度阻尼 (Nm/(rad/s))
+ *   tau_ff    = 力矩前馈 (Nm)
+ *
+ * 注意：
+ *   1. 该函数会直接调用 foc_forward() 输出 PWM
+ *   2. motor_logic_angle, motor_speed 需在主程序中更新
+ *   3. motor_status == motor_running 时才执行
+ *************************************************************/
+void lib_mit_control(float pos_des, float vel_des,
+                     float kp, float kd, float tau_ff)
+{
+    /************* 1. 读取当前状态 *************/
+    float pos_meas = g_motor_logic_angle;   // 电机内圈角度 (rad)
+    float vel_meas = g_motor_speed;         // 当前电机速度 (rad/s)
+
+    // 速度限幅
+    if (fabsf(vel_meas) > mit_param.vel_limit) {
+        vel_meas = (vel_meas > 0) ? mit_param.vel_limit : -mit_param.vel_limit;
+    }
+
+    // 角度限幅
+    float outer_meas = pos_meas / Reduction_Ratio; //电机外圈角度
+    if (outer_meas > mit_param.out_pos_limit) outer_meas = mit_param.out_pos_limit;
+    if (outer_meas < -mit_param.out_pos_limit) outer_meas = -mit_param.out_pos_limit;
+    pos_meas = outer_meas;
+    
+    float outer_des = pos_des / Reduction_Ratio; //电机外圈角度
+    if (outer_des > mit_param.out_pos_limit) outer_des = mit_param.out_pos_limit;
+    if (outer_des < -mit_param.out_pos_limit) outer_des = -mit_param.out_pos_limit;
+    pos_des = outer_des;
+
+    /************* 2. 计算误差 *************/
+    float pos_err = (pos_des - pos_meas);
+    float vel_err = vel_des - vel_meas;
+
+    if (fabsf(pos_err) <= 0.01f)
+        motor_control_context.position_reached_flag = 1;
+    else
+        motor_control_context.position_reached_flag = 0;
+
+    /************* 3. 力矩前馈（位置/速度环输出） *************/
+    float tau_cmd = kp * pos_err + kd * vel_err + tau_ff;
+
+    // 摩擦补偿
+    if (mit_param.friction_comp > 0.0f) {
+        if (vel_meas > 1e-4f) tau_cmd += mit_param.friction_comp;
+        if (vel_meas < -1e-4f) tau_cmd -= mit_param.friction_comp;
+    }
+
+    // 力矩限幅
+    if (tau_cmd > mit_param.torque_limit) tau_cmd = mit_param.torque_limit;
+    if (tau_cmd < -mit_param.torque_limit) tau_cmd = -mit_param.torque_limit;
+
+    /************* 4. 电流参考 (归一化) *************/
+    float iq_ref = tau_cmd / (mit_param.Kt_Nm_per_A + 1e-9f) / MAX_CURRENT / 1.3;
+
+    // 限幅
+    if (iq_ref > mit_param.iq_norm_limit) iq_ref = mit_param.iq_norm_limit;
+    if (iq_ref < -mit_param.iq_norm_limit) iq_ref = -mit_param.iq_norm_limit;
+
+    /************* 5. Q轴 PI 控制 *************/
+    static float iq_integral = 0;       // 静态变量保存积分
+    float iq_meas = g_motor_i_q;          // 归一化 q 轴电流
+    float iq_err = iq_ref - iq_meas;
+    // PI 增益
+    float Kp_iq = 2.0f;                 // P 增益，可调
+    float Ki_iq = 0.5f;                // I 增益，可调
+    float Ts = mit_param.current_loop_Ts > 0 ? mit_param.current_loop_Ts : 0.00004f; // 电流环采样周期
+
+    // 积分更新
+    iq_integral += Ki_iq * iq_err * Ts;
+    if (iq_integral > 0.9f) iq_integral = 0.9f;
+    if (iq_integral < -0.9f) iq_integral = -0.9f;
+
+    // 输出电压归一化 [-vdq_max, vdq_max]
+    float vdq_max = 0.95f;               
+    float v_q = 0;
+    if(fabs(tau_ff) > 2.5){
+        v_q = Kp_iq * iq_err + iq_integral;
+    } else {
+        v_q = iq_ref;
+    }
+//    float v_q = iq_ref;
+
+    if (v_q > vdq_max) v_q = vdq_max;
+    if (v_q < -vdq_max) v_q = -vdq_max;
+    /************* 6. D轴保持 0 *************/
+    float v_d = 0.0f;
+    
+    /************* 7. 发送到 FOC *************/
+    foc_forward(v_d, v_q, rotor_logic_angle);
+}
+

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/foc_math.c

@@ -0,0 +1,329 @@
+#include "controller_functions.h"
+#include "fast_math_functions.h"
+
+float32_t sinTable_f32[FAST_MATH_TABLE_SIZE + 1] = {
+   0.00000000f, 0.01227154f, 0.02454123f, 0.03680722f, 0.04906767f, 0.06132074f,
+   0.07356456f, 0.08579731f, 0.09801714f, 0.11022221f, 0.12241068f, 0.13458071f,
+   0.14673047f, 0.15885814f, 0.17096189f, 0.18303989f, 0.19509032f, 0.20711138f,
+   0.21910124f, 0.23105811f, 0.24298018f, 0.25486566f, 0.26671276f, 0.27851969f,
+   0.29028468f, 0.30200595f, 0.31368174f, 0.32531029f, 0.33688985f, 0.34841868f,
+   0.35989504f, 0.37131719f, 0.38268343f, 0.39399204f, 0.40524131f, 0.41642956f,
+   0.42755509f, 0.43861624f, 0.44961133f, 0.46053871f, 0.47139674f, 0.48218377f,
+   0.49289819f, 0.50353838f, 0.51410274f, 0.52458968f, 0.53499762f, 0.54532499f,
+   0.55557023f, 0.56573181f, 0.57580819f, 0.58579786f, 0.59569930f, 0.60551104f,
+   0.61523159f, 0.62485949f, 0.63439328f, 0.64383154f, 0.65317284f, 0.66241578f,
+   0.67155895f, 0.68060100f, 0.68954054f, 0.69837625f, 0.70710678f, 0.71573083f,
+   0.72424708f, 0.73265427f, 0.74095113f, 0.74913639f, 0.75720885f, 0.76516727f,
+   0.77301045f, 0.78073723f, 0.78834643f, 0.79583690f, 0.80320753f, 0.81045720f,
+   0.81758481f, 0.82458930f, 0.83146961f, 0.83822471f, 0.84485357f, 0.85135519f,
+   0.85772861f, 0.86397286f, 0.87008699f, 0.87607009f, 0.88192126f, 0.88763962f,
+   0.89322430f, 0.89867447f, 0.90398929f, 0.90916798f, 0.91420976f, 0.91911385f,
+   0.92387953f, 0.92850608f, 0.93299280f, 0.93733901f, 0.94154407f, 0.94560733f,
+   0.94952818f, 0.95330604f, 0.95694034f, 0.96043052f, 0.96377607f, 0.96697647f,
+   0.97003125f, 0.97293995f, 0.97570213f, 0.97831737f, 0.98078528f, 0.98310549f,
+   0.98527764f, 0.98730142f, 0.98917651f, 0.99090264f, 0.99247953f, 0.99390697f,
+   0.99518473f, 0.99631261f, 0.99729046f, 0.99811811f, 0.99879546f, 0.99932238f,
+   0.99969882f, 0.99992470f, 1.00000000f, 0.99992470f, 0.99969882f, 0.99932238f,
+   0.99879546f, 0.99811811f, 0.99729046f, 0.99631261f, 0.99518473f, 0.99390697f,
+   0.99247953f, 0.99090264f, 0.98917651f, 0.98730142f, 0.98527764f, 0.98310549f,
+   0.98078528f, 0.97831737f, 0.97570213f, 0.97293995f, 0.97003125f, 0.96697647f,
+   0.96377607f, 0.96043052f, 0.95694034f, 0.95330604f, 0.94952818f, 0.94560733f,
+   0.94154407f, 0.93733901f, 0.93299280f, 0.92850608f, 0.92387953f, 0.91911385f,
+   0.91420976f, 0.90916798f, 0.90398929f, 0.89867447f, 0.89322430f, 0.88763962f,
+   0.88192126f, 0.87607009f, 0.87008699f, 0.86397286f, 0.85772861f, 0.85135519f,
+   0.84485357f, 0.83822471f, 0.83146961f, 0.82458930f, 0.81758481f, 0.81045720f,
+   0.80320753f, 0.79583690f, 0.78834643f, 0.78073723f, 0.77301045f, 0.76516727f,
+   0.75720885f, 0.74913639f, 0.74095113f, 0.73265427f, 0.72424708f, 0.71573083f,
+   0.70710678f, 0.69837625f, 0.68954054f, 0.68060100f, 0.67155895f, 0.66241578f,
+   0.65317284f, 0.64383154f, 0.63439328f, 0.62485949f, 0.61523159f, 0.60551104f,
+   0.59569930f, 0.58579786f, 0.57580819f, 0.56573181f, 0.55557023f, 0.54532499f,
+   0.53499762f, 0.52458968f, 0.51410274f, 0.50353838f, 0.49289819f, 0.48218377f,
+   0.47139674f, 0.46053871f, 0.44961133f, 0.43861624f, 0.42755509f, 0.41642956f,
+   0.40524131f, 0.39399204f, 0.38268343f, 0.37131719f, 0.35989504f, 0.34841868f,
+   0.33688985f, 0.32531029f, 0.31368174f, 0.30200595f, 0.29028468f, 0.27851969f,
+   0.26671276f, 0.25486566f, 0.24298018f, 0.23105811f, 0.21910124f, 0.20711138f,
+   0.19509032f, 0.18303989f, 0.17096189f, 0.15885814f, 0.14673047f, 0.13458071f,
+   0.12241068f, 0.11022221f, 0.09801714f, 0.08579731f, 0.07356456f, 0.06132074f,
+   0.04906767f, 0.03680722f, 0.02454123f, 0.01227154f, 0.00000000f, -0.01227154f,
+   -0.02454123f, -0.03680722f, -0.04906767f, -0.06132074f, -0.07356456f,
+   -0.08579731f, -0.09801714f, -0.11022221f, -0.12241068f, -0.13458071f,
+   -0.14673047f, -0.15885814f, -0.17096189f, -0.18303989f, -0.19509032f,
+   -0.20711138f, -0.21910124f, -0.23105811f, -0.24298018f, -0.25486566f,
+   -0.26671276f, -0.27851969f, -0.29028468f, -0.30200595f, -0.31368174f,
+   -0.32531029f, -0.33688985f, -0.34841868f, -0.35989504f, -0.37131719f,
+   -0.38268343f, -0.39399204f, -0.40524131f, -0.41642956f, -0.42755509f,
+   -0.43861624f, -0.44961133f, -0.46053871f, -0.47139674f, -0.48218377f,
+   -0.49289819f, -0.50353838f, -0.51410274f, -0.52458968f, -0.53499762f,
+   -0.54532499f, -0.55557023f, -0.56573181f, -0.57580819f, -0.58579786f,
+   -0.59569930f, -0.60551104f, -0.61523159f, -0.62485949f, -0.63439328f,
+   -0.64383154f, -0.65317284f, -0.66241578f, -0.67155895f, -0.68060100f,
+   -0.68954054f, -0.69837625f, -0.70710678f, -0.71573083f, -0.72424708f,
+   -0.73265427f, -0.74095113f, -0.74913639f, -0.75720885f, -0.76516727f,
+   -0.77301045f, -0.78073723f, -0.78834643f, -0.79583690f, -0.80320753f,
+   -0.81045720f, -0.81758481f, -0.82458930f, -0.83146961f, -0.83822471f,
+   -0.84485357f, -0.85135519f, -0.85772861f, -0.86397286f, -0.87008699f,
+   -0.87607009f, -0.88192126f, -0.88763962f, -0.89322430f, -0.89867447f,
+   -0.90398929f, -0.90916798f, -0.91420976f, -0.91911385f, -0.92387953f,
+   -0.92850608f, -0.93299280f, -0.93733901f, -0.94154407f, -0.94560733f,
+   -0.94952818f, -0.95330604f, -0.95694034f, -0.96043052f, -0.96377607f,
+   -0.96697647f, -0.97003125f, -0.97293995f, -0.97570213f, -0.97831737f,
+   -0.98078528f, -0.98310549f, -0.98527764f, -0.98730142f, -0.98917651f,
+   -0.99090264f, -0.99247953f, -0.99390697f, -0.99518473f, -0.99631261f,
+   -0.99729046f, -0.99811811f, -0.99879546f, -0.99932238f, -0.99969882f,
+   -0.99992470f, -1.00000000f, -0.99992470f, -0.99969882f, -0.99932238f,
+   -0.99879546f, -0.99811811f, -0.99729046f, -0.99631261f, -0.99518473f,
+   -0.99390697f, -0.99247953f, -0.99090264f, -0.98917651f, -0.98730142f,
+   -0.98527764f, -0.98310549f, -0.98078528f, -0.97831737f, -0.97570213f,
+   -0.97293995f, -0.97003125f, -0.96697647f, -0.96377607f, -0.96043052f,
+   -0.95694034f, -0.95330604f, -0.94952818f, -0.94560733f, -0.94154407f,
+   -0.93733901f, -0.93299280f, -0.92850608f, -0.92387953f, -0.91911385f,
+   -0.91420976f, -0.90916798f, -0.90398929f, -0.89867447f, -0.89322430f,
+   -0.88763962f, -0.88192126f, -0.87607009f, -0.87008699f, -0.86397286f,
+   -0.85772861f, -0.85135519f, -0.84485357f, -0.83822471f, -0.83146961f,
+   -0.82458930f, -0.81758481f, -0.81045720f, -0.80320753f, -0.79583690f,
+   -0.78834643f, -0.78073723f, -0.77301045f, -0.76516727f, -0.75720885f,
+   -0.74913639f, -0.74095113f, -0.73265427f, -0.72424708f, -0.71573083f,
+   -0.70710678f, -0.69837625f, -0.68954054f, -0.68060100f, -0.67155895f,
+   -0.66241578f, -0.65317284f, -0.64383154f, -0.63439328f, -0.62485949f,
+   -0.61523159f, -0.60551104f, -0.59569930f, -0.58579786f, -0.57580819f,
+   -0.56573181f, -0.55557023f, -0.54532499f, -0.53499762f, -0.52458968f,
+   -0.51410274f, -0.50353838f, -0.49289819f, -0.48218377f, -0.47139674f,
+   -0.46053871f, -0.44961133f, -0.43861624f, -0.42755509f, -0.41642956f,
+   -0.40524131f, -0.39399204f, -0.38268343f, -0.37131719f, -0.35989504f,
+   -0.34841868f, -0.33688985f, -0.32531029f, -0.31368174f, -0.30200595f,
+   -0.29028468f, -0.27851969f, -0.26671276f, -0.25486566f, -0.24298018f,
+   -0.23105811f, -0.21910124f, -0.20711138f, -0.19509032f, -0.18303989f,
+   -0.17096189f, -0.15885814f, -0.14673047f, -0.13458071f, -0.12241068f,
+   -0.11022221f, -0.09801714f, -0.08579731f, -0.07356456f, -0.06132074f,
+   -0.04906767f, -0.03680722f, -0.02454123f, -0.01227154f, -0.00000000f
+};
+/**
+  @addtogroup PID
+  @{
+ */
+
+/**
+  @brief         Initialization function for the floating-point PID Control.
+  @param[in,out] S               points to an instance of the PID structure
+  @param[in]     resetStateFlag
+                   - value = 0: no change in state
+                   - value = 1: reset state
+  @return        none
+
+  @par           Details
+                   The <code>resetStateFlag</code> specifies whether to set state to zero or not. \n
+                   The function computes the structure fields: <code>A0</code>, <code>A1</code> <code>A2</code>
+                   using the proportional gain( \c Kp), integral gain( \c Ki) and derivative gain( \c Kd)
+                   also sets the state variables to all zeros.
+ */
+
+void arm_pid_init_f32(arm_pid_instance_f32 * S,int32_t resetStateFlag)
+{
+  /* Derived coefficient A0 */
+  S->A0 = S->Kp + S->Ki + S->Kd;
+
+  /* Derived coefficient A1 */
+  S->A1 = (-S->Kp) - ((float32_t) 2.0f * S->Kd);
+
+  /* Derived coefficient A2 */
+  S->A2 = S->Kd;
+
+  /* Check whether state needs reset or not */
+  if (resetStateFlag)
+  {
+    /* Reset state to zero, The size will be always 3 samples */
+    memset(S->state, 0, 3U * sizeof(float32_t));
+  }
+
+}
+
+/**
+  @} end of PID group
+ */
+
+
+
+/**
+  @ingroup groupFastMath
+ */
+
+/**
+  @defgroup sin Sine
+
+  Computes the trigonometric sine function using a combination of table lookup
+  and linear interpolation.  There are separate functions for
+  Q15, Q31, and floating-point data types.
+  The input to the floating-point version is in radians while the
+  fixed-point Q15 and Q31 have a scaled input with the range
+  [0 +0.9999] mapping to [0 2*pi).  The fixed-point range is chosen so that a
+  value of 2*pi wraps around to 0.
+
+  The implementation is based on table lookup using 512 values together with linear interpolation.
+  The steps used are:
+   -# Calculation of the nearest integer table index
+   -# Compute the fractional portion (fract) of the table index.
+   -# The final result equals <code>(1.0f-fract)*a + fract*b;</code>
+
+  where
+  <pre>
+     b = Table[index];
+     c = Table[index+1];
+  </pre>
+ */
+
+/**
+  @addtogroup sin
+  @{
+ */
+
+/**
+  @brief         Fast approximation to the trigonometric sine function for floating-point data.
+  @param[in]     x  input value in radians.
+  @return        sin(x)
+ */
+
+float32_t arm_sin_f32(float32_t x)
+{
+  float32_t sinVal, fract, in;                   /* Temporary input, output variables */
+  uint16_t index;                                /* Index variable */
+  float32_t a, b;                                /* Two nearest output values */
+  int32_t n;
+  float32_t findex;
+
+  /* input x is in radians */
+  /* Scale input to [0 1] range from [0 2*PI] , divide input by 2*pi */
+  in = x * 0.159154943092f;
+
+  /* Calculation of floor value of input */
+  n = (int32_t) in;
+
+  /* Make negative values towards -infinity */
+  if (in < 0.0f)
+  {
+    n--;
+  }
+
+  /* Map input value to [0 1] */
+  in = in - (float32_t) n;
+
+  /* Calculation of index of the table */
+  findex = (float32_t)FAST_MATH_TABLE_SIZE * in;
+  index = (uint16_t)findex;
+
+  /* when "in" is exactly 1, we need to rotate the index down to 0 */
+  if (index >= FAST_MATH_TABLE_SIZE) {
+    index = 0;
+    findex -= (float32_t)FAST_MATH_TABLE_SIZE;
+  }
+
+  /* fractional value calculation */
+  fract = findex - (float32_t) index;
+
+  /* Read two nearest values of input value from the sin table */
+  a = sinTable_f32[index];
+  b = sinTable_f32[index+1];
+
+  /* Linear interpolation process */
+  sinVal = (1.0f - fract) * a + fract * b;
+
+  /* Return output value */
+  return (sinVal);
+}
+
+/**
+  @} end of sin group
+ */
+
+
+
+
+
+/**
+  @ingroup groupFastMath
+ */
+
+/**
+  @defgroup cos Cosine
+
+  Computes the trigonometric cosine function using a combination of table lookup
+  and linear interpolation.  There are separate functions for
+  Q15, Q31, and floating-point data types.
+  The input to the floating-point version is in radians while the
+  fixed-point Q15 and Q31 have a scaled input with the range
+  [0 +0.9999] mapping to [0 2*pi).  The fixed-point range is chosen so that a
+  value of 2*pi wraps around to 0.
+
+  The implementation is based on table lookup using 512 values together with linear interpolation.
+  The steps used are:
+   -# Calculation of the nearest integer table index
+   -# Compute the fractional portion (fract) of the table index.
+   -# The final result equals <code>(1.0f-fract)*a + fract*b;</code>
+
+  where
+  <pre>
+     a = Table[index];
+     b = Table[index+1];
+  </pre>
+ */
+
+/**
+  @addtogroup cos
+  @{
+ */
+
+/**
+  @brief         Fast approximation to the trigonometric cosine function for floating-point data.
+  @param[in]     x  input value in radians
+  @return        cos(x)
+ */
+float32_t arm_cos_f32(float32_t x)
+{
+  float32_t cosVal, fract, in;                   /* Temporary input, output variables */
+  uint16_t index;                                /* Index variable */
+  float32_t a, b;                                /* Two nearest output values */
+  int32_t n;
+  float32_t findex;
+
+  /* input x is in radians */
+  /* Scale input to [0 1] range from [0 2*PI] , divide input by 2*pi, add 0.25 (pi/2) to read sine table */
+  in = x * 0.159154943092f + 0.25f;
+
+  /* Calculation of floor value of input */
+  n = (int32_t) in;
+
+  /* Make negative values towards -infinity */
+  if (in < 0.0f)
+  {
+    n--;
+  }
+
+  /* Map input value to [0 1] */
+  in = in - (float32_t) n;
+
+  /* Calculation of index of the table */
+  findex = (float32_t)FAST_MATH_TABLE_SIZE * in;
+  index = (uint16_t)findex;
+
+  /* when "in" is exactly 1, we need to rotate the index down to 0 */
+  if (index >= FAST_MATH_TABLE_SIZE) {
+    index = 0;
+    findex -= (float32_t)FAST_MATH_TABLE_SIZE;
+  }
+
+  /* fractional value calculation */
+  fract = findex - (float32_t) index;
+
+  /* Read two nearest values of input value from the cos table */
+  a = sinTable_f32[index];
+  b = sinTable_f32[index+1];
+
+  /* Linear interpolation process */
+  cosVal = (1.0f - fract) * a + fract * b;
+
+  /* Return output value */
+  return (cosVal);
+}
+
+/**
+  @} end of cos group
+ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/conf.h

@@ -0,0 +1,31 @@
+#pragma once
+
+#define POLE_PAIRS 7
+
+#define R_SHUNT 0.02
+#define OP_GAIN 50
+#define I_RMS_CONT      5.6f
+#define I_PEAK_CONT     (I_RMS_CONT * 1.41421356f)
+#define MAX_CURRENT     I_PEAK_CONT
+#define Nm_PER_A 1.3473
+#define ADC_REFERENCE_VOLT 3.3
+#define ADC_BITS 12
+#define MAX_SPEED 15.49
+#define Reduction_Ratio 18
+
+#define motor_pwm_freq 20000
+// #define motor_speed_calc_freq 1000 
+#define dt 0.001
+
+#define position_cycle Reduction_Ratio*2 *3.14159265358979
+#define mult_position_cycle  3*Reduction_Ratio*2 *3.14159265358979
+#ifndef PI
+#define PI 3.14159265358979
+#endif
+#define deg2rad(a) (PI * (a) / 180)
+#define rad2deg(a) (180 * (a) / PI)
+#define max(a, b) ((a) > (b) ? (a) : (b))
+#define min(a, b) ((a) < (b) ? (a) : (b))
+
+#define RAW_TO_RAD (2.0f * PI / 65536.0f)
+#define RAW_TO_GRE (360.0f / 65536.0f)

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/basic_math_functions.h

@@ -0,0 +1,880 @@
+/******************************************************************************
+ * @file     basic_math_functions.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _BASIC_MATH_FUNCTIONS_H_
+#define _BASIC_MATH_FUNCTIONS_H_
+
+#include "arm_math_types.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * @defgroup groupMath Basic Math Functions
+ */
+
+ /**
+   * @brief Q7 vector multiplication.
+   * @param[in]  pSrcA      points to the first input vector
+   * @param[in]  pSrcB      points to the second input vector
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in each vector
+   */
+  void arm_mult_q7(
+  const q7_t * pSrcA,
+  const q7_t * pSrcB,
+        q7_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief Q15 vector multiplication.
+   * @param[in]  pSrcA      points to the first input vector
+   * @param[in]  pSrcB      points to the second input vector
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in each vector
+   */
+  void arm_mult_q15(
+  const q15_t * pSrcA,
+  const q15_t * pSrcB,
+        q15_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief Q31 vector multiplication.
+   * @param[in]  pSrcA      points to the first input vector
+   * @param[in]  pSrcB      points to the second input vector
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in each vector
+   */
+  void arm_mult_q31(
+  const q31_t * pSrcA,
+  const q31_t * pSrcB,
+        q31_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief Floating-point vector multiplication.
+   * @param[in]  pSrcA      points to the first input vector
+   * @param[in]  pSrcB      points to the second input vector
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in each vector
+   */
+  void arm_mult_f32(
+  const float32_t * pSrcA,
+  const float32_t * pSrcB,
+        float32_t * pDst,
+        uint32_t blockSize);
+
+
+
+/**
+ * @brief Floating-point vector multiplication.
+ * @param[in]  pSrcA      points to the first input vector
+ * @param[in]  pSrcB      points to the second input vector
+ * @param[out] pDst       points to the output vector
+ * @param[in]  blockSize  number of samples in each vector
+ */
+void arm_mult_f64(
+const float64_t * pSrcA,
+const float64_t * pSrcB,
+	  float64_t * pDst,
+	  uint32_t blockSize);
+
+
+
+ /**
+   * @brief Floating-point vector addition.
+   * @param[in]  pSrcA      points to the first input vector
+   * @param[in]  pSrcB      points to the second input vector
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in each vector
+   */
+  void arm_add_f32(
+  const float32_t * pSrcA,
+  const float32_t * pSrcB,
+        float32_t * pDst,
+        uint32_t blockSize);
+
+
+
+/**
+  * @brief Floating-point vector addition.
+  * @param[in]  pSrcA      points to the first input vector
+  * @param[in]  pSrcB      points to the second input vector
+  * @param[out] pDst       points to the output vector
+  * @param[in]  blockSize  number of samples in each vector
+  */
+ void arm_add_f64(
+ const float64_t * pSrcA,
+ const float64_t * pSrcB,
+	   float64_t * pDst,
+	   uint32_t blockSize);
+
+
+
+  /**
+   * @brief Q7 vector addition.
+   * @param[in]  pSrcA      points to the first input vector
+   * @param[in]  pSrcB      points to the second input vector
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in each vector
+   */
+  void arm_add_q7(
+  const q7_t * pSrcA,
+  const q7_t * pSrcB,
+        q7_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief Q15 vector addition.
+   * @param[in]  pSrcA      points to the first input vector
+   * @param[in]  pSrcB      points to the second input vector
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in each vector
+   */
+  void arm_add_q15(
+  const q15_t * pSrcA,
+  const q15_t * pSrcB,
+        q15_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief Q31 vector addition.
+   * @param[in]  pSrcA      points to the first input vector
+   * @param[in]  pSrcB      points to the second input vector
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in each vector
+   */
+  void arm_add_q31(
+  const q31_t * pSrcA,
+  const q31_t * pSrcB,
+        q31_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief Floating-point vector subtraction.
+   * @param[in]  pSrcA      points to the first input vector
+   * @param[in]  pSrcB      points to the second input vector
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in each vector
+   */
+  void arm_sub_f32(
+  const float32_t * pSrcA,
+  const float32_t * pSrcB,
+        float32_t * pDst,
+        uint32_t blockSize);
+
+
+
+  /**
+   * @brief Floating-point vector subtraction.
+   * @param[in]  pSrcA      points to the first input vector
+   * @param[in]  pSrcB      points to the second input vector
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in each vector
+   */
+  void arm_sub_f64(
+  const float64_t * pSrcA,
+  const float64_t * pSrcB,
+        float64_t * pDst,
+        uint32_t blockSize);
+
+
+
+  /**
+   * @brief Q7 vector subtraction.
+   * @param[in]  pSrcA      points to the first input vector
+   * @param[in]  pSrcB      points to the second input vector
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in each vector
+   */
+  void arm_sub_q7(
+  const q7_t * pSrcA,
+  const q7_t * pSrcB,
+        q7_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief Q15 vector subtraction.
+   * @param[in]  pSrcA      points to the first input vector
+   * @param[in]  pSrcB      points to the second input vector
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in each vector
+   */
+  void arm_sub_q15(
+  const q15_t * pSrcA,
+  const q15_t * pSrcB,
+        q15_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief Q31 vector subtraction.
+   * @param[in]  pSrcA      points to the first input vector
+   * @param[in]  pSrcB      points to the second input vector
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in each vector
+   */
+  void arm_sub_q31(
+  const q31_t * pSrcA,
+  const q31_t * pSrcB,
+        q31_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief Multiplies a floating-point vector by a scalar.
+   * @param[in]  pSrc       points to the input vector
+   * @param[in]  scale      scale factor to be applied
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in the vector
+   */
+  void arm_scale_f32(
+  const float32_t * pSrc,
+        float32_t scale,
+        float32_t * pDst,
+        uint32_t blockSize);
+
+
+
+  /**
+   * @brief Multiplies a floating-point vector by a scalar.
+   * @param[in]  pSrc       points to the input vector
+   * @param[in]  scale      scale factor to be applied
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in the vector
+   */
+  void arm_scale_f64(
+  const float64_t * pSrc,
+        float64_t scale,
+        float64_t * pDst,
+        uint32_t blockSize);
+
+
+
+  /**
+   * @brief Multiplies a Q7 vector by a scalar.
+   * @param[in]  pSrc        points to the input vector
+   * @param[in]  scaleFract  fractional portion of the scale value
+   * @param[in]  shift       number of bits to shift the result by
+   * @param[out] pDst        points to the output vector
+   * @param[in]  blockSize   number of samples in the vector
+   */
+  void arm_scale_q7(
+  const q7_t * pSrc,
+        q7_t scaleFract,
+        int8_t shift,
+        q7_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief Multiplies a Q15 vector by a scalar.
+   * @param[in]  pSrc        points to the input vector
+   * @param[in]  scaleFract  fractional portion of the scale value
+   * @param[in]  shift       number of bits to shift the result by
+   * @param[out] pDst        points to the output vector
+   * @param[in]  blockSize   number of samples in the vector
+   */
+  void arm_scale_q15(
+  const q15_t * pSrc,
+        q15_t scaleFract,
+        int8_t shift,
+        q15_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief Multiplies a Q31 vector by a scalar.
+   * @param[in]  pSrc        points to the input vector
+   * @param[in]  scaleFract  fractional portion of the scale value
+   * @param[in]  shift       number of bits to shift the result by
+   * @param[out] pDst        points to the output vector
+   * @param[in]  blockSize   number of samples in the vector
+   */
+  void arm_scale_q31(
+  const q31_t * pSrc,
+        q31_t scaleFract,
+        int8_t shift,
+        q31_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief Q7 vector absolute value.
+   * @param[in]  pSrc       points to the input buffer
+   * @param[out] pDst       points to the output buffer
+   * @param[in]  blockSize  number of samples in each vector
+   */
+  void arm_abs_q7(
+  const q7_t * pSrc,
+        q7_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief Floating-point vector absolute value.
+   * @param[in]  pSrc       points to the input buffer
+   * @param[out] pDst       points to the output buffer
+   * @param[in]  blockSize  number of samples in each vector
+   */
+  void arm_abs_f32(
+  const float32_t * pSrc,
+        float32_t * pDst,
+        uint32_t blockSize);
+
+
+
+/**
+ * @brief Floating-point vector absolute value.
+ * @param[in]  pSrc       points to the input buffer
+ * @param[out] pDst       points to the output buffer
+ * @param[in]  blockSize  number of samples in each vector
+ */
+void arm_abs_f64(
+const float64_t * pSrc,
+	  float64_t * pDst,
+	  uint32_t blockSize);
+
+
+
+  /**
+   * @brief Q15 vector absolute value.
+   * @param[in]  pSrc       points to the input buffer
+   * @param[out] pDst       points to the output buffer
+   * @param[in]  blockSize  number of samples in each vector
+   */
+  void arm_abs_q15(
+  const q15_t * pSrc,
+        q15_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief Q31 vector absolute value.
+   * @param[in]  pSrc       points to the input buffer
+   * @param[out] pDst       points to the output buffer
+   * @param[in]  blockSize  number of samples in each vector
+   */
+  void arm_abs_q31(
+  const q31_t * pSrc,
+        q31_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief Dot product of floating-point vectors.
+   * @param[in]  pSrcA      points to the first input vector
+   * @param[in]  pSrcB      points to the second input vector
+   * @param[in]  blockSize  number of samples in each vector
+   * @param[out] result     output result returned here
+   */
+  void arm_dot_prod_f32(
+  const float32_t * pSrcA,
+  const float32_t * pSrcB,
+        uint32_t blockSize,
+        float32_t * result);
+
+
+
+/**
+ * @brief Dot product of floating-point vectors.
+ * @param[in]  pSrcA      points to the first input vector
+ * @param[in]  pSrcB      points to the second input vector
+ * @param[in]  blockSize  number of samples in each vector
+ * @param[out] result     output result returned here
+ */
+void arm_dot_prod_f64(
+const float64_t * pSrcA,
+const float64_t * pSrcB,
+	  uint32_t blockSize,
+	  float64_t * result);
+
+
+
+  /**
+   * @brief Dot product of Q7 vectors.
+   * @param[in]  pSrcA      points to the first input vector
+   * @param[in]  pSrcB      points to the second input vector
+   * @param[in]  blockSize  number of samples in each vector
+   * @param[out] result     output result returned here
+   */
+  void arm_dot_prod_q7(
+  const q7_t * pSrcA,
+  const q7_t * pSrcB,
+        uint32_t blockSize,
+        q31_t * result);
+
+
+  /**
+   * @brief Dot product of Q15 vectors.
+   * @param[in]  pSrcA      points to the first input vector
+   * @param[in]  pSrcB      points to the second input vector
+   * @param[in]  blockSize  number of samples in each vector
+   * @param[out] result     output result returned here
+   */
+  void arm_dot_prod_q15(
+  const q15_t * pSrcA,
+  const q15_t * pSrcB,
+        uint32_t blockSize,
+        q63_t * result);
+
+
+  /**
+   * @brief Dot product of Q31 vectors.
+   * @param[in]  pSrcA      points to the first input vector
+   * @param[in]  pSrcB      points to the second input vector
+   * @param[in]  blockSize  number of samples in each vector
+   * @param[out] result     output result returned here
+   */
+  void arm_dot_prod_q31(
+  const q31_t * pSrcA,
+  const q31_t * pSrcB,
+        uint32_t blockSize,
+        q63_t * result);
+
+
+  /**
+   * @brief  Shifts the elements of a Q7 vector a specified number of bits.
+   * @param[in]  pSrc       points to the input vector
+   * @param[in]  shiftBits  number of bits to shift.  A positive value shifts left; a negative value shifts right.
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in the vector
+   */
+  void arm_shift_q7(
+  const q7_t * pSrc,
+        int8_t shiftBits,
+        q7_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Shifts the elements of a Q15 vector a specified number of bits.
+   * @param[in]  pSrc       points to the input vector
+   * @param[in]  shiftBits  number of bits to shift.  A positive value shifts left; a negative value shifts right.
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in the vector
+   */
+  void arm_shift_q15(
+  const q15_t * pSrc,
+        int8_t shiftBits,
+        q15_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Shifts the elements of a Q31 vector a specified number of bits.
+   * @param[in]  pSrc       points to the input vector
+   * @param[in]  shiftBits  number of bits to shift.  A positive value shifts left; a negative value shifts right.
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in the vector
+   */
+  void arm_shift_q31(
+  const q31_t * pSrc,
+        int8_t shiftBits,
+        q31_t * pDst,
+        uint32_t blockSize);
+
+
+/**
+ * @brief  Adds a constant offset to a floating-point vector.
+ * @param[in]  pSrc       points to the input vector
+ * @param[in]  offset     is the offset to be added
+ * @param[out] pDst       points to the output vector
+ * @param[in]  blockSize  number of samples in the vector
+ */
+void arm_offset_f64(
+const float64_t * pSrc,
+	  float64_t offset,
+	  float64_t * pDst,
+	  uint32_t blockSize);
+
+
+
+  /**
+   * @brief  Adds a constant offset to a floating-point vector.
+   * @param[in]  pSrc       points to the input vector
+   * @param[in]  offset     is the offset to be added
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in the vector
+   */
+  void arm_offset_f32(
+  const float32_t * pSrc,
+        float32_t offset,
+        float32_t * pDst,
+        uint32_t blockSize);
+
+
+
+  /**
+   * @brief  Adds a constant offset to a Q7 vector.
+   * @param[in]  pSrc       points to the input vector
+   * @param[in]  offset     is the offset to be added
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in the vector
+   */
+  void arm_offset_q7(
+  const q7_t * pSrc,
+        q7_t offset,
+        q7_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Adds a constant offset to a Q15 vector.
+   * @param[in]  pSrc       points to the input vector
+   * @param[in]  offset     is the offset to be added
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in the vector
+   */
+  void arm_offset_q15(
+  const q15_t * pSrc,
+        q15_t offset,
+        q15_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Adds a constant offset to a Q31 vector.
+   * @param[in]  pSrc       points to the input vector
+   * @param[in]  offset     is the offset to be added
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in the vector
+   */
+  void arm_offset_q31(
+  const q31_t * pSrc,
+        q31_t offset,
+        q31_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Negates the elements of a floating-point vector.
+   * @param[in]  pSrc       points to the input vector
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in the vector
+   */
+  void arm_negate_f32(
+  const float32_t * pSrc,
+        float32_t * pDst,
+        uint32_t blockSize);
+
+
+
+/**
+ * @brief  Negates the elements of a floating-point vector.
+ * @param[in]  pSrc       points to the input vector
+ * @param[out] pDst       points to the output vector
+ * @param[in]  blockSize  number of samples in the vector
+ */
+void arm_negate_f64(
+const float64_t * pSrc,
+	  float64_t * pDst,
+	  uint32_t blockSize);
+
+
+
+  /**
+   * @brief  Negates the elements of a Q7 vector.
+   * @param[in]  pSrc       points to the input vector
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in the vector
+   */
+  void arm_negate_q7(
+  const q7_t * pSrc,
+        q7_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Negates the elements of a Q15 vector.
+   * @param[in]  pSrc       points to the input vector
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in the vector
+   */
+  void arm_negate_q15(
+  const q15_t * pSrc,
+        q15_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Negates the elements of a Q31 vector.
+   * @param[in]  pSrc       points to the input vector
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in the vector
+   */
+  void arm_negate_q31(
+  const q31_t * pSrc,
+        q31_t * pDst,
+        uint32_t blockSize);
+
+/**
+   * @brief         Compute the logical bitwise AND of two fixed-point vectors.
+   * @param[in]     pSrcA      points to input vector A
+   * @param[in]     pSrcB      points to input vector B
+   * @param[out]    pDst       points to output vector
+   * @param[in]     blockSize  number of samples in each vector
+   * @return        none
+   */
+  void arm_and_u16(
+    const uint16_t * pSrcA,
+    const uint16_t * pSrcB,
+          uint16_t * pDst,
+          uint32_t blockSize);
+
+  /**
+   * @brief         Compute the logical bitwise AND of two fixed-point vectors.
+   * @param[in]     pSrcA      points to input vector A
+   * @param[in]     pSrcB      points to input vector B
+   * @param[out]    pDst       points to output vector
+   * @param[in]     blockSize  number of samples in each vector
+   * @return        none
+   */
+  void arm_and_u32(
+    const uint32_t * pSrcA,
+    const uint32_t * pSrcB,
+          uint32_t * pDst,
+          uint32_t blockSize);
+
+  /**
+   * @brief         Compute the logical bitwise AND of two fixed-point vectors.
+   * @param[in]     pSrcA      points to input vector A
+   * @param[in]     pSrcB      points to input vector B
+   * @param[out]    pDst       points to output vector
+   * @param[in]     blockSize  number of samples in each vector
+   * @return        none
+   */
+  void arm_and_u8(
+    const uint8_t * pSrcA,
+    const uint8_t * pSrcB,
+          uint8_t * pDst,
+          uint32_t blockSize);
+
+  /**
+   * @brief         Compute the logical bitwise OR of two fixed-point vectors.
+   * @param[in]     pSrcA      points to input vector A
+   * @param[in]     pSrcB      points to input vector B
+   * @param[out]    pDst       points to output vector
+   * @param[in]     blockSize  number of samples in each vector
+   * @return        none
+   */
+  void arm_or_u16(
+    const uint16_t * pSrcA,
+    const uint16_t * pSrcB,
+          uint16_t * pDst,
+          uint32_t blockSize);
+
+  /**
+   * @brief         Compute the logical bitwise OR of two fixed-point vectors.
+   * @param[in]     pSrcA      points to input vector A
+   * @param[in]     pSrcB      points to input vector B
+   * @param[out]    pDst       points to output vector
+   * @param[in]     blockSize  number of samples in each vector
+   * @return        none
+   */
+  void arm_or_u32(
+    const uint32_t * pSrcA,
+    const uint32_t * pSrcB,
+          uint32_t * pDst,
+          uint32_t blockSize);
+
+  /**
+   * @brief         Compute the logical bitwise OR of two fixed-point vectors.
+   * @param[in]     pSrcA      points to input vector A
+   * @param[in]     pSrcB      points to input vector B
+   * @param[out]    pDst       points to output vector
+   * @param[in]     blockSize  number of samples in each vector
+   * @return        none
+   */
+  void arm_or_u8(
+    const uint8_t * pSrcA,
+    const uint8_t * pSrcB,
+          uint8_t * pDst,
+          uint32_t blockSize);
+
+  /**
+   * @brief         Compute the logical bitwise NOT of a fixed-point vector.
+   * @param[in]     pSrc       points to input vector 
+   * @param[out]    pDst       points to output vector
+   * @param[in]     blockSize  number of samples in each vector
+   * @return        none
+   */
+  void arm_not_u16(
+    const uint16_t * pSrc,
+          uint16_t * pDst,
+          uint32_t blockSize);
+
+  /**
+   * @brief         Compute the logical bitwise NOT of a fixed-point vector.
+   * @param[in]     pSrc       points to input vector 
+   * @param[out]    pDst       points to output vector
+   * @param[in]     blockSize  number of samples in each vector
+   * @return        none
+   */
+  void arm_not_u32(
+    const uint32_t * pSrc,
+          uint32_t * pDst,
+          uint32_t blockSize);
+
+  /**
+   * @brief         Compute the logical bitwise NOT of a fixed-point vector.
+   * @param[in]     pSrc       points to input vector 
+   * @param[out]    pDst       points to output vector
+   * @param[in]     blockSize  number of samples in each vector
+   * @return        none
+   */
+  void arm_not_u8(
+    const uint8_t * pSrc,
+          uint8_t * pDst,
+          uint32_t blockSize);
+
+/**
+   * @brief         Compute the logical bitwise XOR of two fixed-point vectors.
+   * @param[in]     pSrcA      points to input vector A
+   * @param[in]     pSrcB      points to input vector B
+   * @param[out]    pDst       points to output vector
+   * @param[in]     blockSize  number of samples in each vector
+   * @return        none
+   */
+  void arm_xor_u16(
+    const uint16_t * pSrcA,
+    const uint16_t * pSrcB,
+          uint16_t * pDst,
+          uint32_t blockSize);
+
+  /**
+   * @brief         Compute the logical bitwise XOR of two fixed-point vectors.
+   * @param[in]     pSrcA      points to input vector A
+   * @param[in]     pSrcB      points to input vector B
+   * @param[out]    pDst       points to output vector
+   * @param[in]     blockSize  number of samples in each vector
+   * @return        none
+   */
+  void arm_xor_u32(
+    const uint32_t * pSrcA,
+    const uint32_t * pSrcB,
+          uint32_t * pDst,
+          uint32_t blockSize);
+
+  /**
+   * @brief         Compute the logical bitwise XOR of two fixed-point vectors.
+   * @param[in]     pSrcA      points to input vector A
+   * @param[in]     pSrcB      points to input vector B
+   * @param[out]    pDst       points to output vector
+   * @param[in]     blockSize  number of samples in each vector
+   * @return        none
+   */
+  void arm_xor_u8(
+    const uint8_t * pSrcA,
+    const uint8_t * pSrcB,
+          uint8_t * pDst,
+    uint32_t blockSize);
+
+  /**
+  @brief         Elementwise floating-point clipping
+  @param[in]     pSrc          points to input values
+  @param[out]    pDst          points to output clipped values
+  @param[in]     low           lower bound
+  @param[in]     high          higher bound
+  @param[in]     numSamples    number of samples to clip
+  @return        none
+ */
+
+void arm_clip_f32(const float32_t * pSrc, 
+  float32_t * pDst, 
+  float32_t low, 
+  float32_t high, 
+  uint32_t numSamples);
+
+  /**
+  @brief         Elementwise fixed-point clipping
+  @param[in]     pSrc          points to input values
+  @param[out]    pDst          points to output clipped values
+  @param[in]     low           lower bound
+  @param[in]     high          higher bound
+  @param[in]     numSamples    number of samples to clip
+  @return        none
+ */
+
+void arm_clip_q31(const q31_t * pSrc, 
+  q31_t * pDst, 
+  q31_t low, 
+  q31_t high, 
+  uint32_t numSamples);
+
+  /**
+  @brief         Elementwise fixed-point clipping
+  @param[in]     pSrc          points to input values
+  @param[out]    pDst          points to output clipped values
+  @param[in]     low           lower bound
+  @param[in]     high          higher bound
+  @param[in]     numSamples    number of samples to clip
+  @return        none
+ */
+
+void arm_clip_q15(const q15_t * pSrc, 
+  q15_t * pDst, 
+  q15_t low, 
+  q15_t high, 
+  uint32_t numSamples);
+
+  /**
+  @brief         Elementwise fixed-point clipping
+  @param[in]     pSrc          points to input values
+  @param[out]    pDst          points to output clipped values
+  @param[in]     low           lower bound
+  @param[in]     high          higher bound
+  @param[in]     numSamples    number of samples to clip
+  @return        none
+ */
+
+void arm_clip_q7(const q7_t * pSrc, 
+  q7_t * pDst, 
+  q7_t low, 
+  q7_t high, 
+  uint32_t numSamples);
+
+
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _BASIC_MATH_FUNCTIONS_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/basic_math_functions_f16.h

@@ -0,0 +1,168 @@
+/******************************************************************************
+ * @file     basic_math_functions_f16.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _BASIC_MATH_FUNCTIONS_F16_H_
+#define _BASIC_MATH_FUNCTIONS_F16_H_
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+#include "arm_math_types_f16.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+
+
+#if defined(ARM_FLOAT16_SUPPORTED)
+
+
+  /**
+   * @brief Floating-point vector addition.
+   * @param[in]  pSrcA      points to the first input vector
+   * @param[in]  pSrcB      points to the second input vector
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in each vector
+   */
+  void arm_add_f16(
+  const float16_t * pSrcA,
+  const float16_t * pSrcB,
+        float16_t * pDst,
+        uint32_t blockSize);
+
+  /**
+   * @brief Floating-point vector subtraction.
+   * @param[in]  pSrcA      points to the first input vector
+   * @param[in]  pSrcB      points to the second input vector
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in each vector
+   */
+  void arm_sub_f16(
+  const float16_t * pSrcA,
+  const float16_t * pSrcB,
+        float16_t * pDst,
+        uint32_t blockSize);
+
+    /**
+   * @brief Multiplies a floating-point vector by a scalar.
+   * @param[in]  pSrc       points to the input vector
+   * @param[in]  scale      scale factor to be applied
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in the vector
+   */
+  void arm_scale_f16(
+  const float16_t * pSrc,
+        float16_t scale,
+        float16_t * pDst,
+        uint32_t blockSize);
+
+    /**
+   * @brief Floating-point vector absolute value.
+   * @param[in]  pSrc       points to the input buffer
+   * @param[out] pDst       points to the output buffer
+   * @param[in]  blockSize  number of samples in each vector
+   */
+  void arm_abs_f16(
+  const float16_t * pSrc,
+        float16_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Adds a constant offset to a floating-point vector.
+   * @param[in]  pSrc       points to the input vector
+   * @param[in]  offset     is the offset to be added
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in the vector
+   */
+  void arm_offset_f16(
+  const float16_t * pSrc,
+        float16_t offset,
+        float16_t * pDst,
+        uint32_t blockSize);
+
+  /**
+   * @brief Dot product of floating-point vectors.
+   * @param[in]  pSrcA      points to the first input vector
+   * @param[in]  pSrcB      points to the second input vector
+   * @param[in]  blockSize  number of samples in each vector
+   * @param[out] result     output result returned here
+   */
+  void arm_dot_prod_f16(
+  const float16_t * pSrcA,
+  const float16_t * pSrcB,
+        uint32_t blockSize,
+        float16_t * result);
+
+  /**
+   * @brief Floating-point vector multiplication.
+   * @param[in]  pSrcA      points to the first input vector
+   * @param[in]  pSrcB      points to the second input vector
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in each vector
+   */
+  void arm_mult_f16(
+  const float16_t * pSrcA,
+  const float16_t * pSrcB,
+        float16_t * pDst,
+        uint32_t blockSize);
+
+  /**
+   * @brief  Negates the elements of a floating-point vector.
+   * @param[in]  pSrc       points to the input vector
+   * @param[out] pDst       points to the output vector
+   * @param[in]  blockSize  number of samples in the vector
+   */
+  void arm_negate_f16(
+  const float16_t * pSrc,
+        float16_t * pDst,
+        uint32_t blockSize);
+
+  /**
+  @brief         Elementwise floating-point clipping
+  @param[in]     pSrc          points to input values
+  @param[out]    pDst          points to output clipped values
+  @param[in]     low           lower bound
+  @param[in]     high          higher bound
+  @param[in]     numSamples    number of samples to clip
+  @return        none
+ */
+
+void arm_clip_f16(const float16_t * pSrc, 
+  float16_t * pDst, 
+  float16_t low, 
+  float16_t high, 
+  uint32_t numSamples);
+
+#endif /* defined(ARM_FLOAT16_SUPPORTED)*/
+
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _BASIC_MATH_FUNCTIONS_F16_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/bayes_functions.h

@@ -0,0 +1,89 @@
+/******************************************************************************
+ * @file     bayes_functions.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _BAYES_FUNCTIONS_H_
+#define _BAYES_FUNCTIONS_H_
+
+#include "arm_math_types.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+
+#include "dsp/statistics_functions.h"
+
+/**
+ * @defgroup groupBayes Bayesian estimators
+ *
+ * Implement the naive gaussian Bayes estimator.
+ * The training must be done from scikit-learn.
+ *
+ * The parameters can be easily
+ * generated from the scikit-learn object. Some examples are given in
+ * DSP/Testing/PatternGeneration/Bayes.py
+ */
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * @brief Instance structure for Naive Gaussian Bayesian estimator.
+ */
+typedef struct
+{
+  uint32_t vectorDimension;  /**< Dimension of vector space */
+  uint32_t numberOfClasses;  /**< Number of different classes  */
+  const float32_t *theta;          /**< Mean values for the Gaussians */
+  const float32_t *sigma;          /**< Variances for the Gaussians */
+  const float32_t *classPriors;    /**< Class prior probabilities */
+  float32_t epsilon;         /**< Additive value to variances */
+} arm_gaussian_naive_bayes_instance_f32;
+
+/**
+ * @brief Naive Gaussian Bayesian Estimator
+ *
+ * @param[in]  S                        points to a naive bayes instance structure
+ * @param[in]  in                       points to the elements of the input vector.
+ * @param[out] *pOutputProbabilities    points to a buffer of length numberOfClasses containing estimated probabilities
+ * @param[out] *pBufferB                points to a temporary buffer of length numberOfClasses
+ * @return The predicted class
+ *
+ */
+
+
+uint32_t arm_gaussian_naive_bayes_predict_f32(const arm_gaussian_naive_bayes_instance_f32 *S, 
+   const float32_t * in, 
+   float32_t *pOutputProbabilities,
+   float32_t *pBufferB);
+
+
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _BAYES_FUNCTIONS_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/bayes_functions_f16.h

@@ -0,0 +1,80 @@
+/******************************************************************************
+ * @file     bayes_functions_f16.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _BAYES_FUNCTIONS_F16_H_
+#define _BAYES_FUNCTIONS_F16_H_
+
+#include "arm_math_types_f16.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+
+#include "dsp/statistics_functions_f16.h"
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+#if defined(ARM_FLOAT16_SUPPORTED)
+
+/**
+ * @brief Instance structure for Naive Gaussian Bayesian estimator.
+ */
+typedef struct
+{
+  uint32_t vectorDimension;  /**< Dimension of vector space */
+  uint32_t numberOfClasses;  /**< Number of different classes  */
+  const float16_t *theta;          /**< Mean values for the Gaussians */
+  const float16_t *sigma;          /**< Variances for the Gaussians */
+  const float16_t *classPriors;    /**< Class prior probabilities */
+  float16_t epsilon;         /**< Additive value to variances */
+} arm_gaussian_naive_bayes_instance_f16;
+
+/**
+ * @brief Naive Gaussian Bayesian Estimator
+ *
+ * @param[in]  S                        points to a naive bayes instance structure
+ * @param[in]  in                       points to the elements of the input vector.
+ * @param[out] *pOutputProbabilities    points to a buffer of length numberOfClasses containing estimated probabilities
+ * @param[out] *pBufferB                points to a temporary buffer of length numberOfClasses
+ * @return The predicted class
+ *
+ */
+
+
+uint32_t arm_gaussian_naive_bayes_predict_f16(const arm_gaussian_naive_bayes_instance_f16 *S, 
+   const float16_t * in, 
+   float16_t *pOutputProbabilities,
+   float16_t *pBufferB);
+
+#endif /*defined(ARM_FLOAT16_SUPPORTED)*/
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _BAYES_FUNCTIONS_F16_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/cmsis_compiler.h

@@ -0,0 +1,322 @@
+/**************************************************************************//**
+ * @file     cmsis_compiler.h
+ * @brief    CMSIS compiler generic header file
+ * @version  V5.0.4
+ * @date     10. January 2018
+ ******************************************************************************/
+/*
+ * Copyright (c) 2009-2018 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+#ifndef __CMSIS_COMPILER_H
+#define __CMSIS_COMPILER_H
+
+#include <stdint.h>
+
+/*
+ * Arm Compiler 4/5
+ */
+#if   defined ( __CC_ARM )
+  #include "cmsis_armcc.h"
+
+
+/*
+ * Arm Compiler 6 (armclang)
+ */
+#elif defined (__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050)
+  #include "cmsis_armclang.h"
+
+
+/*
+ * GNU Compiler
+ */
+#elif defined ( __GNUC__ )
+  #include "core_cm4.h"
+  #ifndef   __ASM
+    #define __ASM                                  __asm
+  #endif
+  #ifndef   __INLINE
+    #define __INLINE                               inline
+  #endif
+  #ifndef   __STATIC_INLINE
+    #define __STATIC_INLINE                        static inline
+  #endif
+  #ifndef   __STATIC_FORCEINLINE
+    #define __STATIC_FORCEINLINE                   __STATIC_INLINE
+  #endif
+  #ifndef   __NO_RETURN
+    #define __NO_RETURN                            __attribute__((noreturn))
+  #endif
+  #ifndef   __USED
+    #define __USED                                 __attribute__((used))
+  #endif
+  #ifndef   __WEAK
+    #define __WEAK                                 __attribute__((weak))
+  #endif
+  #ifndef   __PACKED
+    #define __PACKED                               __attribute__((packed))
+  #endif
+  #ifndef   __PACKED_STRUCT
+    #define __PACKED_STRUCT                        struct __attribute__((packed))
+  #endif
+  #ifndef   __PACKED_UNION
+    #define __PACKED_UNION                         union __attribute__((packed))
+  #endif
+  #ifndef   __UNALIGNED_UINT32        /* deprecated */
+    struct __attribute__((packed)) T_UINT32 { uint32_t v; };
+    #define __UNALIGNED_UINT32(x)                  (((struct T_UINT32 *)(x))->v)
+  #endif
+  #ifndef   __UNALIGNED_UINT16_WRITE
+    __PACKED_STRUCT T_UINT16_WRITE { uint16_t v; };
+    #define __UNALIGNED_UINT16_WRITE(addr, val)    (void)((((struct T_UINT16_WRITE *)(void*)(addr))->v) = (val))
+  #endif
+  #ifndef   __UNALIGNED_UINT16_READ
+    __PACKED_STRUCT T_UINT16_READ { uint16_t v; };
+    #define __UNALIGNED_UINT16_READ(addr)          (((const struct T_UINT16_READ *)(const void *)(addr))->v)
+  #endif
+  #ifndef   __UNALIGNED_UINT32_WRITE
+    __PACKED_STRUCT T_UINT32_WRITE { uint32_t v; };
+    #define __UNALIGNED_UINT32_WRITE(addr, val)    (void)((((struct T_UINT32_WRITE *)(void *)(addr))->v) = (val))
+  #endif
+  #ifndef   __UNALIGNED_UINT32_READ
+    __PACKED_STRUCT T_UINT32_READ { uint32_t v; };
+    #define __UNALIGNED_UINT32_READ(addr)          (((const struct T_UINT32_READ *)(const void *)(addr))->v)
+  #endif
+  #ifndef   __ALIGNED
+    #define __ALIGNED(x)                           __attribute__((aligned(x)))
+  #endif
+  #ifndef   __RESTRICT
+    //#warning No compiler specific solution for __RESTRICT. __RESTRICT is ignored.
+    #define __RESTRICT
+  #endif
+
+/*
+ * IAR Compiler
+ */
+#elif defined ( __ICCARM__ )
+  #include <cmsis_iccarm.h>
+
+
+/*
+ * TI Arm Compiler
+ */
+#elif defined ( __TI_ARM__ )
+  #include <cmsis_ccs.h>
+
+  #ifndef   __ASM
+    #define __ASM                                  __asm
+  #endif
+  #ifndef   __INLINE
+    #define __INLINE                               inline
+  #endif
+  #ifndef   __STATIC_INLINE
+    #define __STATIC_INLINE                        static inline
+  #endif
+  #ifndef   __STATIC_FORCEINLINE
+    #define __STATIC_FORCEINLINE                   __STATIC_INLINE
+  #endif
+  #ifndef   __NO_RETURN
+    #define __NO_RETURN                            __attribute__((noreturn))
+  #endif
+  #ifndef   __USED
+    #define __USED                                 __attribute__((used))
+  #endif
+  #ifndef   __WEAK
+    #define __WEAK                                 __attribute__((weak))
+  #endif
+  #ifndef   __PACKED
+    #define __PACKED                               __attribute__((packed))
+  #endif
+  #ifndef   __PACKED_STRUCT
+    #define __PACKED_STRUCT                        struct __attribute__((packed))
+  #endif
+  #ifndef   __PACKED_UNION
+    #define __PACKED_UNION                         union __attribute__((packed))
+  #endif
+  #ifndef   __UNALIGNED_UINT32        /* deprecated */
+    struct __attribute__((packed)) T_UINT32 { uint32_t v; };
+    #define __UNALIGNED_UINT32(x)                  (((struct T_UINT32 *)(x))->v)
+  #endif
+  #ifndef   __UNALIGNED_UINT16_WRITE
+    __PACKED_STRUCT T_UINT16_WRITE { uint16_t v; };
+    #define __UNALIGNED_UINT16_WRITE(addr, val)    (void)((((struct T_UINT16_WRITE *)(void*)(addr))->v) = (val))
+  #endif
+  #ifndef   __UNALIGNED_UINT16_READ
+    __PACKED_STRUCT T_UINT16_READ { uint16_t v; };
+    #define __UNALIGNED_UINT16_READ(addr)          (((const struct T_UINT16_READ *)(const void *)(addr))->v)
+  #endif
+  #ifndef   __UNALIGNED_UINT32_WRITE
+    __PACKED_STRUCT T_UINT32_WRITE { uint32_t v; };
+    #define __UNALIGNED_UINT32_WRITE(addr, val)    (void)((((struct T_UINT32_WRITE *)(void *)(addr))->v) = (val))
+  #endif
+  #ifndef   __UNALIGNED_UINT32_READ
+    __PACKED_STRUCT T_UINT32_READ { uint32_t v; };
+    #define __UNALIGNED_UINT32_READ(addr)          (((const struct T_UINT32_READ *)(const void *)(addr))->v)
+  #endif
+  #ifndef   __ALIGNED
+    #define __ALIGNED(x)                           __attribute__((aligned(x)))
+  #endif
+  #ifndef   __RESTRICT
+    #warning No compiler specific solution for __RESTRICT. __RESTRICT is ignored.
+    #define __RESTRICT
+  #endif
+
+
+/*
+ * TASKING Compiler
+ */
+#elif defined ( __TASKING__ )
+  /*
+   * The CMSIS functions have been implemented as intrinsics in the compiler.
+   * Please use "carm -?i" to get an up to date list of all intrinsics,
+   * Including the CMSIS ones.
+   */
+
+  #ifndef   __ASM
+    #define __ASM                                  __asm
+  #endif
+  #ifndef   __INLINE
+    #define __INLINE                               inline
+  #endif
+  #ifndef   __STATIC_INLINE
+    #define __STATIC_INLINE                        static inline
+  #endif
+  #ifndef   __STATIC_FORCEINLINE
+    #define __STATIC_FORCEINLINE                   __STATIC_INLINE
+  #endif
+  #ifndef   __NO_RETURN
+    #define __NO_RETURN                            __attribute__((noreturn))
+  #endif
+  #ifndef   __USED
+    #define __USED                                 __attribute__((used))
+  #endif
+  #ifndef   __WEAK
+    #define __WEAK                                 __attribute__((weak))
+  #endif
+  #ifndef   __PACKED
+    #define __PACKED                               __packed__
+  #endif
+  #ifndef   __PACKED_STRUCT
+    #define __PACKED_STRUCT                        struct __packed__
+  #endif
+  #ifndef   __PACKED_UNION
+    #define __PACKED_UNION                         union __packed__
+  #endif
+  #ifndef   __UNALIGNED_UINT32        /* deprecated */
+    struct __packed__ T_UINT32 { uint32_t v; };
+    #define __UNALIGNED_UINT32(x)                  (((struct T_UINT32 *)(x))->v)
+  #endif
+  #ifndef   __UNALIGNED_UINT16_WRITE
+    __PACKED_STRUCT T_UINT16_WRITE { uint16_t v; };
+    #define __UNALIGNED_UINT16_WRITE(addr, val)    (void)((((struct T_UINT16_WRITE *)(void *)(addr))->v) = (val))
+  #endif
+  #ifndef   __UNALIGNED_UINT16_READ
+    __PACKED_STRUCT T_UINT16_READ { uint16_t v; };
+    #define __UNALIGNED_UINT16_READ(addr)          (((const struct T_UINT16_READ *)(const void *)(addr))->v)
+  #endif
+  #ifndef   __UNALIGNED_UINT32_WRITE
+    __PACKED_STRUCT T_UINT32_WRITE { uint32_t v; };
+    #define __UNALIGNED_UINT32_WRITE(addr, val)    (void)((((struct T_UINT32_WRITE *)(void *)(addr))->v) = (val))
+  #endif
+  #ifndef   __UNALIGNED_UINT32_READ
+    __PACKED_STRUCT T_UINT32_READ { uint32_t v; };
+    #define __UNALIGNED_UINT32_READ(addr)          (((const struct T_UINT32_READ *)(const void *)(addr))->v)
+  #endif
+  #ifndef   __ALIGNED
+    #define __ALIGNED(x)              __align(x)
+  #endif
+  #ifndef   __RESTRICT
+    #warning No compiler specific solution for __RESTRICT. __RESTRICT is ignored.
+    #define __RESTRICT
+  #endif
+
+
+/*
+ * COSMIC Compiler
+ */
+#elif defined ( __CSMC__ )
+   #include <cmsis_csm.h>
+
+ #ifndef   __ASM
+    #define __ASM                                  _asm
+  #endif
+  #ifndef   __INLINE
+    #define __INLINE                               inline
+  #endif
+  #ifndef   __STATIC_INLINE
+    #define __STATIC_INLINE                        static inline
+  #endif
+  #ifndef   __STATIC_FORCEINLINE
+    #define __STATIC_FORCEINLINE                   __STATIC_INLINE
+  #endif
+  #ifndef   __NO_RETURN
+    // NO RETURN is automatically detected hence no warning here
+    #define __NO_RETURN
+  #endif
+  #ifndef   __USED
+    #warning No compiler specific solution for __USED. __USED is ignored.
+    #define __USED
+  #endif
+  #ifndef   __WEAK
+    #define __WEAK                                 __weak
+  #endif
+  #ifndef   __PACKED
+    #define __PACKED                               @packed
+  #endif
+  #ifndef   __PACKED_STRUCT
+    #define __PACKED_STRUCT                        @packed struct
+  #endif
+  #ifndef   __PACKED_UNION
+    #define __PACKED_UNION                         @packed union
+  #endif
+  #ifndef   __UNALIGNED_UINT32        /* deprecated */
+    @packed struct T_UINT32 { uint32_t v; };
+    #define __UNALIGNED_UINT32(x)                  (((struct T_UINT32 *)(x))->v)
+  #endif
+  #ifndef   __UNALIGNED_UINT16_WRITE
+    __PACKED_STRUCT T_UINT16_WRITE { uint16_t v; };
+    #define __UNALIGNED_UINT16_WRITE(addr, val)    (void)((((struct T_UINT16_WRITE *)(void *)(addr))->v) = (val))
+  #endif
+  #ifndef   __UNALIGNED_UINT16_READ
+    __PACKED_STRUCT T_UINT16_READ { uint16_t v; };
+    #define __UNALIGNED_UINT16_READ(addr)          (((const struct T_UINT16_READ *)(const void *)(addr))->v)
+  #endif
+  #ifndef   __UNALIGNED_UINT32_WRITE
+    __PACKED_STRUCT T_UINT32_WRITE { uint32_t v; };
+    #define __UNALIGNED_UINT32_WRITE(addr, val)    (void)((((struct T_UINT32_WRITE *)(void *)(addr))->v) = (val))
+  #endif
+  #ifndef   __UNALIGNED_UINT32_READ
+    __PACKED_STRUCT T_UINT32_READ { uint32_t v; };
+    #define __UNALIGNED_UINT32_READ(addr)          (((const struct T_UINT32_READ *)(const void *)(addr))->v)
+  #endif
+  #ifndef   __ALIGNED
+    #warning No compiler specific solution for __ALIGNED. __ALIGNED is ignored.
+    #define __ALIGNED(x)
+  #endif
+  #ifndef   __RESTRICT
+    #warning No compiler specific solution for __RESTRICT. __RESTRICT is ignored.
+    #define __RESTRICT
+  #endif
+
+
+#else
+  #error Unknown compiler.
+#endif
+
+
+#endif /* __CMSIS_COMPILER_H */
+

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/complex_math_functions.h

@@ -0,0 +1,345 @@
+/******************************************************************************
+ * @file     complex_math_functions.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _COMPLEX_MATH_FUNCTIONS_H_
+#define _COMPLEX_MATH_FUNCTIONS_H_
+
+#include "arm_math_types.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+#include "dsp/fast_math_functions.h"
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * @defgroup groupCmplxMath Complex Math Functions
+ * This set of functions operates on complex data vectors.
+ * The data in the complex arrays is stored in an interleaved fashion
+ * (real, imag, real, imag, ...).
+ * In the API functions, the number of samples in a complex array refers
+ * to the number of complex values; the array contains twice this number of
+ * real values.
+ */
+
+ /**
+   * @brief  Floating-point complex conjugate.
+   * @param[in]  pSrc        points to the input vector
+   * @param[out] pDst        points to the output vector
+   * @param[in]  numSamples  number of complex samples in each vector
+   */
+  void arm_cmplx_conj_f32(
+  const float32_t * pSrc,
+        float32_t * pDst,
+        uint32_t numSamples);
+
+  /**
+   * @brief  Q31 complex conjugate.
+   * @param[in]  pSrc        points to the input vector
+   * @param[out] pDst        points to the output vector
+   * @param[in]  numSamples  number of complex samples in each vector
+   */
+  void arm_cmplx_conj_q31(
+  const q31_t * pSrc,
+        q31_t * pDst,
+        uint32_t numSamples);
+
+
+  /**
+   * @brief  Q15 complex conjugate.
+   * @param[in]  pSrc        points to the input vector
+   * @param[out] pDst        points to the output vector
+   * @param[in]  numSamples  number of complex samples in each vector
+   */
+  void arm_cmplx_conj_q15(
+  const q15_t * pSrc,
+        q15_t * pDst,
+        uint32_t numSamples);
+
+
+  /**
+   * @brief  Floating-point complex magnitude squared
+   * @param[in]  pSrc        points to the complex input vector
+   * @param[out] pDst        points to the real output vector
+   * @param[in]  numSamples  number of complex samples in the input vector
+   */
+  void arm_cmplx_mag_squared_f32(
+  const float32_t * pSrc,
+        float32_t * pDst,
+        uint32_t numSamples);
+
+
+  /**
+   * @brief  Floating-point complex magnitude squared
+   * @param[in]  pSrc        points to the complex input vector
+   * @param[out] pDst        points to the real output vector
+   * @param[in]  numSamples  number of complex samples in the input vector
+   */
+  void arm_cmplx_mag_squared_f64(
+  const float64_t * pSrc,
+        float64_t * pDst,
+        uint32_t numSamples);
+
+
+  /**
+   * @brief  Q31 complex magnitude squared
+   * @param[in]  pSrc        points to the complex input vector
+   * @param[out] pDst        points to the real output vector
+   * @param[in]  numSamples  number of complex samples in the input vector
+   */
+  void arm_cmplx_mag_squared_q31(
+  const q31_t * pSrc,
+        q31_t * pDst,
+        uint32_t numSamples);
+
+
+  /**
+   * @brief  Q15 complex magnitude squared
+   * @param[in]  pSrc        points to the complex input vector
+   * @param[out] pDst        points to the real output vector
+   * @param[in]  numSamples  number of complex samples in the input vector
+   */
+  void arm_cmplx_mag_squared_q15(
+  const q15_t * pSrc,
+        q15_t * pDst,
+        uint32_t numSamples);
+
+
+/**
+   * @brief  Floating-point complex magnitude
+   * @param[in]  pSrc        points to the complex input vector
+   * @param[out] pDst        points to the real output vector
+   * @param[in]  numSamples  number of complex samples in the input vector
+   */
+  void arm_cmplx_mag_f32(
+  const float32_t * pSrc,
+        float32_t * pDst,
+        uint32_t numSamples);
+
+
+/**
+   * @brief  Floating-point complex magnitude
+   * @param[in]  pSrc        points to the complex input vector
+   * @param[out] pDst        points to the real output vector
+   * @param[in]  numSamples  number of complex samples in the input vector
+   */
+  void arm_cmplx_mag_f64(
+  const float64_t * pSrc,
+        float64_t * pDst,
+        uint32_t numSamples);
+
+
+  /**
+   * @brief  Q31 complex magnitude
+   * @param[in]  pSrc        points to the complex input vector
+   * @param[out] pDst        points to the real output vector
+   * @param[in]  numSamples  number of complex samples in the input vector
+   */
+  void arm_cmplx_mag_q31(
+  const q31_t * pSrc,
+        q31_t * pDst,
+        uint32_t numSamples);
+
+
+  /**
+   * @brief  Q15 complex magnitude
+   * @param[in]  pSrc        points to the complex input vector
+   * @param[out] pDst        points to the real output vector
+   * @param[in]  numSamples  number of complex samples in the input vector
+   */
+  void arm_cmplx_mag_q15(
+  const q15_t * pSrc,
+        q15_t * pDst,
+        uint32_t numSamples);
+
+  /**
+   * @brief  Q15 complex magnitude
+   * @param[in]  pSrc        points to the complex input vector
+   * @param[out] pDst        points to the real output vector
+   * @param[in]  numSamples  number of complex samples in the input vector
+   */
+  void arm_cmplx_mag_fast_q15(
+  const q15_t * pSrc,
+        q15_t * pDst,
+        uint32_t numSamples);
+
+
+  /**
+   * @brief  Q15 complex dot product
+   * @param[in]  pSrcA       points to the first input vector
+   * @param[in]  pSrcB       points to the second input vector
+   * @param[in]  numSamples  number of complex samples in each vector
+   * @param[out] realResult  real part of the result returned here
+   * @param[out] imagResult  imaginary part of the result returned here
+   */
+  void arm_cmplx_dot_prod_q15(
+  const q15_t * pSrcA,
+  const q15_t * pSrcB,
+        uint32_t numSamples,
+        q31_t * realResult,
+        q31_t * imagResult);
+
+
+  /**
+   * @brief  Q31 complex dot product
+   * @param[in]  pSrcA       points to the first input vector
+   * @param[in]  pSrcB       points to the second input vector
+   * @param[in]  numSamples  number of complex samples in each vector
+   * @param[out] realResult  real part of the result returned here
+   * @param[out] imagResult  imaginary part of the result returned here
+   */
+  void arm_cmplx_dot_prod_q31(
+  const q31_t * pSrcA,
+  const q31_t * pSrcB,
+        uint32_t numSamples,
+        q63_t * realResult,
+        q63_t * imagResult);
+
+
+  /**
+   * @brief  Floating-point complex dot product
+   * @param[in]  pSrcA       points to the first input vector
+   * @param[in]  pSrcB       points to the second input vector
+   * @param[in]  numSamples  number of complex samples in each vector
+   * @param[out] realResult  real part of the result returned here
+   * @param[out] imagResult  imaginary part of the result returned here
+   */
+  void arm_cmplx_dot_prod_f32(
+  const float32_t * pSrcA,
+  const float32_t * pSrcB,
+        uint32_t numSamples,
+        float32_t * realResult,
+        float32_t * imagResult);
+
+
+  /**
+   * @brief  Q15 complex-by-real multiplication
+   * @param[in]  pSrcCmplx   points to the complex input vector
+   * @param[in]  pSrcReal    points to the real input vector
+   * @param[out] pCmplxDst   points to the complex output vector
+   * @param[in]  numSamples  number of samples in each vector
+   */
+  void arm_cmplx_mult_real_q15(
+  const q15_t * pSrcCmplx,
+  const q15_t * pSrcReal,
+        q15_t * pCmplxDst,
+        uint32_t numSamples);
+
+
+  /**
+   * @brief  Q31 complex-by-real multiplication
+   * @param[in]  pSrcCmplx   points to the complex input vector
+   * @param[in]  pSrcReal    points to the real input vector
+   * @param[out] pCmplxDst   points to the complex output vector
+   * @param[in]  numSamples  number of samples in each vector
+   */
+  void arm_cmplx_mult_real_q31(
+  const q31_t * pSrcCmplx,
+  const q31_t * pSrcReal,
+        q31_t * pCmplxDst,
+        uint32_t numSamples);
+
+
+  /**
+   * @brief  Floating-point complex-by-real multiplication
+   * @param[in]  pSrcCmplx   points to the complex input vector
+   * @param[in]  pSrcReal    points to the real input vector
+   * @param[out] pCmplxDst   points to the complex output vector
+   * @param[in]  numSamples  number of samples in each vector
+   */
+  void arm_cmplx_mult_real_f32(
+  const float32_t * pSrcCmplx,
+  const float32_t * pSrcReal,
+        float32_t * pCmplxDst,
+        uint32_t numSamples);
+
+  /**
+   * @brief  Q15 complex-by-complex multiplication
+   * @param[in]  pSrcA       points to the first input vector
+   * @param[in]  pSrcB       points to the second input vector
+   * @param[out] pDst        points to the output vector
+   * @param[in]  numSamples  number of complex samples in each vector
+   */
+  void arm_cmplx_mult_cmplx_q15(
+  const q15_t * pSrcA,
+  const q15_t * pSrcB,
+        q15_t * pDst,
+        uint32_t numSamples);
+
+
+  /**
+   * @brief  Q31 complex-by-complex multiplication
+   * @param[in]  pSrcA       points to the first input vector
+   * @param[in]  pSrcB       points to the second input vector
+   * @param[out] pDst        points to the output vector
+   * @param[in]  numSamples  number of complex samples in each vector
+   */
+  void arm_cmplx_mult_cmplx_q31(
+  const q31_t * pSrcA,
+  const q31_t * pSrcB,
+        q31_t * pDst,
+        uint32_t numSamples);
+
+
+  /**
+   * @brief  Floating-point complex-by-complex multiplication
+   * @param[in]  pSrcA       points to the first input vector
+   * @param[in]  pSrcB       points to the second input vector
+   * @param[out] pDst        points to the output vector
+   * @param[in]  numSamples  number of complex samples in each vector
+   */
+  void arm_cmplx_mult_cmplx_f32(
+  const float32_t * pSrcA,
+  const float32_t * pSrcB,
+        float32_t * pDst,
+        uint32_t numSamples);
+
+
+
+/**
+ * @brief  Floating-point complex-by-complex multiplication
+ * @param[in]  pSrcA       points to the first input vector
+ * @param[in]  pSrcB       points to the second input vector
+ * @param[out] pDst        points to the output vector
+ * @param[in]  numSamples  number of complex samples in each vector
+ */
+void arm_cmplx_mult_cmplx_f64(
+const float64_t * pSrcA,
+const float64_t * pSrcB,
+	  float64_t * pDst,
+	  uint32_t numSamples);
+
+
+
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _COMPLEX_MATH_FUNCTIONS_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/complex_math_functions_f16.h

@@ -0,0 +1,123 @@
+/******************************************************************************
+ * @file     complex_math_functions_f16.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _COMPLEX_MATH_FUNCTIONS_F16_H_
+#define _COMPLEX_MATH_FUNCTIONS_F16_H_
+
+#include "arm_math_types_f16.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+#include "dsp/fast_math_functions_f16.h"
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+#if defined(ARM_FLOAT16_SUPPORTED)
+
+ /**
+   * @brief  Floating-point complex conjugate.
+   * @param[in]  pSrc        points to the input vector
+   * @param[out] pDst        points to the output vector
+   * @param[in]  numSamples  number of complex samples in each vector
+   */
+  void arm_cmplx_conj_f16(
+  const float16_t * pSrc,
+        float16_t * pDst,
+        uint32_t numSamples);
+
+ /**
+   * @brief  Floating-point complex magnitude squared
+   * @param[in]  pSrc        points to the complex input vector
+   * @param[out] pDst        points to the real output vector
+   * @param[in]  numSamples  number of complex samples in the input vector
+   */
+  void arm_cmplx_mag_squared_f16(
+  const float16_t * pSrc,
+        float16_t * pDst,
+        uint32_t numSamples);
+
+  /**
+   * @brief  Floating-point complex magnitude
+   * @param[in]  pSrc        points to the complex input vector
+   * @param[out] pDst        points to the real output vector
+   * @param[in]  numSamples  number of complex samples in the input vector
+   */
+  void arm_cmplx_mag_f16(
+  const float16_t * pSrc,
+        float16_t * pDst,
+        uint32_t numSamples);
+
+  /**
+   * @brief  Floating-point complex dot product
+   * @param[in]  pSrcA       points to the first input vector
+   * @param[in]  pSrcB       points to the second input vector
+   * @param[in]  numSamples  number of complex samples in each vector
+   * @param[out] realResult  real part of the result returned here
+   * @param[out] imagResult  imaginary part of the result returned here
+   */
+  void arm_cmplx_dot_prod_f16(
+  const float16_t * pSrcA,
+  const float16_t * pSrcB,
+        uint32_t numSamples,
+        float16_t * realResult,
+        float16_t * imagResult);
+
+   /**
+   * @brief  Floating-point complex-by-real multiplication
+   * @param[in]  pSrcCmplx   points to the complex input vector
+   * @param[in]  pSrcReal    points to the real input vector
+   * @param[out] pCmplxDst   points to the complex output vector
+   * @param[in]  numSamples  number of samples in each vector
+   */
+  void arm_cmplx_mult_real_f16(
+  const float16_t * pSrcCmplx,
+  const float16_t * pSrcReal,
+        float16_t * pCmplxDst,
+        uint32_t numSamples);
+
+  /**
+   * @brief  Floating-point complex-by-complex multiplication
+   * @param[in]  pSrcA       points to the first input vector
+   * @param[in]  pSrcB       points to the second input vector
+   * @param[out] pDst        points to the output vector
+   * @param[in]  numSamples  number of complex samples in each vector
+   */
+  void arm_cmplx_mult_cmplx_f16(
+  const float16_t * pSrcA,
+  const float16_t * pSrcB,
+        float16_t * pDst,
+        uint32_t numSamples);
+
+#endif /*defined(ARM_FLOAT16_SUPPORTED)*/
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _COMPLEX_MATH_FUNCTIONS_F16_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/controller_functions.h

@@ -0,0 +1,791 @@
+/******************************************************************************
+ * @file     controller_functions.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _CONTROLLER_FUNCTIONS_H_
+#define _CONTROLLER_FUNCTIONS_H_
+
+#include "arm_math_types.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+  /**
+   * @brief Macros required for SINE and COSINE Controller functions
+   */
+
+#define CONTROLLER_Q31_SHIFT  (32 - 9)
+  /* 1.31(q31) Fixed value of 2/360 */
+  /* -1 to +1 is divided into 360 values so total spacing is (2/360) */
+#define INPUT_SPACING         0xB60B61
+  
+/**
+ * @defgroup groupController Controller Functions
+ */
+
+
+ /**
+   * @ingroup groupController
+   */
+
+  /**
+   * @addtogroup SinCos
+   * @{
+   */
+
+/**
+   * @brief  Floating-point sin_cos function.
+   * @param[in]  theta   input value in degrees
+   * @param[out] pSinVal  points to the processed sine output.
+   * @param[out] pCosVal  points to the processed cos output.
+   */
+  void arm_sin_cos_f32(
+        float32_t theta,
+        float32_t * pSinVal,
+        float32_t * pCosVal);
+
+
+  /**
+   * @brief  Q31 sin_cos function.
+   * @param[in]  theta    scaled input value in degrees
+   * @param[out] pSinVal  points to the processed sine output.
+   * @param[out] pCosVal  points to the processed cosine output.
+   */
+  void arm_sin_cos_q31(
+        q31_t theta,
+        q31_t * pSinVal,
+        q31_t * pCosVal);
+
+  /**
+   * @} end of SinCos group
+   */
+
+ /**
+   * @ingroup groupController
+   */
+
+/**
+   * @defgroup PID PID Motor Control
+   *
+   * A Proportional Integral Derivative (PID) controller is a generic feedback control
+   * loop mechanism widely used in industrial control systems.
+   * A PID controller is the most commonly used type of feedback controller.
+   *
+   * This set of functions implements (PID) controllers
+   * for Q15, Q31, and floating-point data types.  The functions operate on a single sample
+   * of data and each call to the function returns a single processed value.
+   * <code>S</code> points to an instance of the PID control data structure.  <code>in</code>
+   * is the input sample value. The functions return the output value.
+   *
+   * \par Algorithm:
+   * <pre>
+   *    y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2]
+   *    A0 = Kp + Ki + Kd
+   *    A1 = (-Kp ) - (2 * Kd )
+   *    A2 = Kd
+   * </pre>
+   *
+   * \par
+   * where \c Kp is proportional constant, \c Ki is Integral constant and \c Kd is Derivative constant
+   *
+   * \par
+   * \image html PID.gif "Proportional Integral Derivative Controller"
+   *
+   * \par
+   * The PID controller calculates an "error" value as the difference between
+   * the measured output and the reference input.
+   * The controller attempts to minimize the error by adjusting the process control inputs.
+   * The proportional value determines the reaction to the current error,
+   * the integral value determines the reaction based on the sum of recent errors,
+   * and the derivative value determines the reaction based on the rate at which the error has been changing.
+   *
+   * \par Instance Structure
+   * The Gains A0, A1, A2 and state variables for a PID controller are stored together in an instance data structure.
+   * A separate instance structure must be defined for each PID Controller.
+   * There are separate instance structure declarations for each of the 3 supported data types.
+   *
+   * \par Reset Functions
+   * There is also an associated reset function for each data type which clears the state array.
+   *
+   * \par Initialization Functions
+   * There is also an associated initialization function for each data type.
+   * The initialization function performs the following operations:
+   * - Initializes the Gains A0, A1, A2 from Kp,Ki, Kd gains.
+   * - Zeros out the values in the state buffer.
+   *
+   * \par
+   * Instance structure cannot be placed into a const data section and it is recommended to use the initialization function.
+   *
+   * \par Fixed-Point Behavior
+   * Care must be taken when using the fixed-point versions of the PID Controller functions.
+   * In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.
+   * Refer to the function specific documentation below for usage guidelines.
+   */
+
+
+  /**
+   * @brief Instance structure for the Q15 PID Control.
+   */
+  typedef struct
+  {
+          q15_t A0;           /**< The derived gain, A0 = Kp + Ki + Kd . */
+#if !defined (ARM_MATH_DSP)
+          q15_t A1;           /**< The derived gain A1 = -Kp - 2Kd */
+          q15_t A2;           /**< The derived gain A1 = Kd. */
+#else
+          q31_t A1;           /**< The derived gain A1 = -Kp - 2Kd | Kd.*/
+#endif
+          q15_t state[3];     /**< The state array of length 3. */
+          q15_t Kp;           /**< The proportional gain. */
+          q15_t Ki;           /**< The integral gain. */
+          q15_t Kd;           /**< The derivative gain. */
+  } arm_pid_instance_q15;
+
+  /**
+   * @brief Instance structure for the Q31 PID Control.
+   */
+  typedef struct
+  {
+          q31_t A0;            /**< The derived gain, A0 = Kp + Ki + Kd . */
+          q31_t A1;            /**< The derived gain, A1 = -Kp - 2Kd. */
+          q31_t A2;            /**< The derived gain, A2 = Kd . */
+          q31_t state[3];      /**< The state array of length 3. */
+          q31_t Kp;            /**< The proportional gain. */
+          q31_t Ki;            /**< The integral gain. */
+          q31_t Kd;            /**< The derivative gain. */
+  } arm_pid_instance_q31;
+
+  /**
+   * @brief Instance structure for the floating-point PID Control.
+   */
+  typedef struct
+  {
+          float32_t A0;          /**< The derived gain, A0 = Kp + Ki + Kd . */
+          float32_t A1;          /**< The derived gain, A1 = -Kp - 2Kd. */
+          float32_t A2;          /**< The derived gain, A2 = Kd . */
+          float32_t state[3];    /**< The state array of length 3. */
+          float32_t Kp;          /**< The proportional gain. */
+          float32_t Ki;          /**< The integral gain. */
+          float32_t Kd;          /**< The derivative gain. */
+  } arm_pid_instance_f32;
+
+
+
+  /**
+   * @brief  Initialization function for the floating-point PID Control.
+   * @param[in,out] S               points to an instance of the PID structure.
+   * @param[in]     resetStateFlag  flag to reset the state. 0 = no change in state 1 = reset the state.
+   */
+  void arm_pid_init_f32(
+        arm_pid_instance_f32 * S,
+        int32_t resetStateFlag);
+
+
+  /**
+   * @brief  Reset function for the floating-point PID Control.
+   * @param[in,out] S  is an instance of the floating-point PID Control structure
+   */
+  void arm_pid_reset_f32(
+        arm_pid_instance_f32 * S);
+
+
+  /**
+   * @brief  Initialization function for the Q31 PID Control.
+   * @param[in,out] S               points to an instance of the Q15 PID structure.
+   * @param[in]     resetStateFlag  flag to reset the state. 0 = no change in state 1 = reset the state.
+   */
+  void arm_pid_init_q31(
+        arm_pid_instance_q31 * S,
+        int32_t resetStateFlag);
+
+
+  /**
+   * @brief  Reset function for the Q31 PID Control.
+   * @param[in,out] S   points to an instance of the Q31 PID Control structure
+   */
+
+  void arm_pid_reset_q31(
+        arm_pid_instance_q31 * S);
+
+
+  /**
+   * @brief  Initialization function for the Q15 PID Control.
+   * @param[in,out] S               points to an instance of the Q15 PID structure.
+   * @param[in]     resetStateFlag  flag to reset the state. 0 = no change in state 1 = reset the state.
+   */
+  void arm_pid_init_q15(
+        arm_pid_instance_q15 * S,
+        int32_t resetStateFlag);
+
+
+  /**
+   * @brief  Reset function for the Q15 PID Control.
+   * @param[in,out] S  points to an instance of the q15 PID Control structure
+   */
+  void arm_pid_reset_q15(
+        arm_pid_instance_q15 * S);
+
+
+
+  /**
+   * @addtogroup PID
+   * @{
+   */
+
+  /**
+   * @brief         Process function for the floating-point PID Control.
+   * @param[in,out] S   is an instance of the floating-point PID Control structure
+   * @param[in]     in  input sample to process
+   * @return        processed output sample.
+   */
+  __STATIC_FORCEINLINE float32_t arm_pid_f32(
+  arm_pid_instance_f32 * S,
+  float32_t in)
+  {
+    float32_t out;
+
+    /* y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2]  */
+    out = (S->A0 * in) +
+      (S->A1 * S->state[0]) + (S->A2 * S->state[1]) + (S->state[2]);
+
+    /* Update state */
+    S->state[1] = S->state[0];
+    S->state[0] = in;
+    S->state[2] = out;
+
+    /* return to application */
+    return (out);
+
+  }
+
+/**
+  @brief         Process function for the Q31 PID Control.
+  @param[in,out] S  points to an instance of the Q31 PID Control structure
+  @param[in]     in  input sample to process
+  @return        processed output sample.
+
+  \par Scaling and Overflow Behavior
+         The function is implemented using an internal 64-bit accumulator.
+         The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.
+         Thus, if the accumulator result overflows it wraps around rather than clip.
+         In order to avoid overflows completely the input signal must be scaled down by 2 bits as there are four additions.
+         After all multiply-accumulates are performed, the 2.62 accumulator is truncated to 1.32 format and then saturated to 1.31 format.
+ */
+__STATIC_FORCEINLINE q31_t arm_pid_q31(
+  arm_pid_instance_q31 * S,
+  q31_t in)
+  {
+    q63_t acc;
+    q31_t out;
+
+    /* acc = A0 * x[n]  */
+    acc = (q63_t) S->A0 * in;
+
+    /* acc += A1 * x[n-1] */
+    acc += (q63_t) S->A1 * S->state[0];
+
+    /* acc += A2 * x[n-2]  */
+    acc += (q63_t) S->A2 * S->state[1];
+
+    /* convert output to 1.31 format to add y[n-1] */
+    out = (q31_t) (acc >> 31U);
+
+    /* out += y[n-1] */
+    out += S->state[2];
+
+    /* Update state */
+    S->state[1] = S->state[0];
+    S->state[0] = in;
+    S->state[2] = out;
+
+    /* return to application */
+    return (out);
+  }
+
+
+/**
+  @brief         Process function for the Q15 PID Control.
+  @param[in,out] S   points to an instance of the Q15 PID Control structure
+  @param[in]     in  input sample to process
+  @return        processed output sample.
+
+  \par Scaling and Overflow Behavior
+         The function is implemented using a 64-bit internal accumulator.
+         Both Gains and state variables are represented in 1.15 format and multiplications yield a 2.30 result.
+         The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.
+         There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.
+         After all additions have been performed, the accumulator is truncated to 34.15 format by discarding low 15 bits.
+         Lastly, the accumulator is saturated to yield a result in 1.15 format.
+ */
+__STATIC_FORCEINLINE q15_t arm_pid_q15(
+  arm_pid_instance_q15 * S,
+  q15_t in)
+  {
+    q63_t acc;
+    q15_t out;
+
+#if defined (ARM_MATH_DSP)
+    /* Implementation of PID controller */
+
+    /* acc = A0 * x[n]  */
+    acc = (q31_t) __SMUAD((uint32_t)S->A0, (uint32_t)in);
+
+    /* acc += A1 * x[n-1] + A2 * x[n-2]  */
+    acc = (q63_t)__SMLALD((uint32_t)S->A1, (uint32_t)read_q15x2 (S->state), (uint64_t)acc);
+#else
+    /* acc = A0 * x[n]  */
+    acc = ((q31_t) S->A0) * in;
+
+    /* acc += A1 * x[n-1] + A2 * x[n-2]  */
+    acc += (q31_t) S->A1 * S->state[0];
+    acc += (q31_t) S->A2 * S->state[1];
+#endif
+
+    /* acc += y[n-1] */
+    acc += (q31_t) S->state[2] << 15;
+
+    /* saturate the output */
+    out = (q15_t) (__SSAT((q31_t)(acc >> 15), 16));
+
+    /* Update state */
+    S->state[1] = S->state[0];
+    S->state[0] = in;
+    S->state[2] = out;
+
+    /* return to application */
+    return (out);
+  }
+
+  /**
+   * @} end of PID group
+   */
+
+  /**
+   * @ingroup groupController
+   */
+
+  /**
+   * @defgroup park Vector Park Transform
+   *
+   * Forward Park transform converts the input two-coordinate vector to flux and torque components.
+   * The Park transform can be used to realize the transformation of the <code>Ialpha</code> and the <code>Ibeta</code> currents
+   * from the stationary to the moving reference frame and control the spatial relationship between
+   * the stator vector current and rotor flux vector.
+   * If we consider the d axis aligned with the rotor flux, the diagram below shows the
+   * current vector and the relationship from the two reference frames:
+   * \image html park.gif "Stator current space vector and its component in (a,b) and in the d,q rotating reference frame"
+   *
+   * The function operates on a single sample of data and each call to the function returns the processed output.
+   * The library provides separate functions for Q31 and floating-point data types.
+   * \par Algorithm
+   * \image html parkFormula.gif
+   * where <code>Ialpha</code> and <code>Ibeta</code> are the stator vector components,
+   * <code>pId</code> and <code>pIq</code> are rotor vector components and <code>cosVal</code> and <code>sinVal</code> are the
+   * cosine and sine values of theta (rotor flux position).
+   * \par Fixed-Point Behavior
+   * Care must be taken when using the Q31 version of the Park transform.
+   * In particular, the overflow and saturation behavior of the accumulator used must be considered.
+   * Refer to the function specific documentation below for usage guidelines.
+   */
+
+  /**
+   * @addtogroup park
+   * @{
+   */
+
+  /**
+   * @brief Floating-point Park transform
+   * @param[in]  Ialpha  input two-phase vector coordinate alpha
+   * @param[in]  Ibeta   input two-phase vector coordinate beta
+   * @param[out] pId     points to output   rotor reference frame d
+   * @param[out] pIq     points to output   rotor reference frame q
+   * @param[in]  sinVal  sine value of rotation angle theta
+   * @param[in]  cosVal  cosine value of rotation angle theta
+   * @return     none
+   *
+   * The function implements the forward Park transform.
+   *
+   */
+  __STATIC_FORCEINLINE void arm_park_f32(
+  float32_t Ialpha,
+  float32_t Ibeta,
+  float32_t * pId,
+  float32_t * pIq,
+  float32_t sinVal,
+  float32_t cosVal)
+  {
+    /* Calculate pId using the equation, pId = Ialpha * cosVal + Ibeta * sinVal */
+    *pId = Ialpha * cosVal + Ibeta * sinVal;
+
+    /* Calculate pIq using the equation, pIq = - Ialpha * sinVal + Ibeta * cosVal */
+    *pIq = -Ialpha * sinVal + Ibeta * cosVal;
+  }
+
+
+/**
+  @brief  Park transform for Q31 version
+  @param[in]  Ialpha  input two-phase vector coordinate alpha
+  @param[in]  Ibeta   input two-phase vector coordinate beta
+  @param[out] pId     points to output rotor reference frame d
+  @param[out] pIq     points to output rotor reference frame q
+  @param[in]  sinVal  sine value of rotation angle theta
+  @param[in]  cosVal  cosine value of rotation angle theta
+  @return     none
+
+  \par Scaling and Overflow Behavior
+         The function is implemented using an internal 32-bit accumulator.
+         The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
+         There is saturation on the addition and subtraction, hence there is no risk of overflow.
+ */
+__STATIC_FORCEINLINE void arm_park_q31(
+  q31_t Ialpha,
+  q31_t Ibeta,
+  q31_t * pId,
+  q31_t * pIq,
+  q31_t sinVal,
+  q31_t cosVal)
+  {
+    q31_t product1, product2;                    /* Temporary variables used to store intermediate results */
+    q31_t product3, product4;                    /* Temporary variables used to store intermediate results */
+
+    /* Intermediate product is calculated by (Ialpha * cosVal) */
+    product1 = (q31_t) (((q63_t) (Ialpha) * (cosVal)) >> 31);
+
+    /* Intermediate product is calculated by (Ibeta * sinVal) */
+    product2 = (q31_t) (((q63_t) (Ibeta) * (sinVal)) >> 31);
+
+
+    /* Intermediate product is calculated by (Ialpha * sinVal) */
+    product3 = (q31_t) (((q63_t) (Ialpha) * (sinVal)) >> 31);
+
+    /* Intermediate product is calculated by (Ibeta * cosVal) */
+    product4 = (q31_t) (((q63_t) (Ibeta) * (cosVal)) >> 31);
+
+    /* Calculate pId by adding the two intermediate products 1 and 2 */
+    *pId = __QADD(product1, product2);
+
+    /* Calculate pIq by subtracting the two intermediate products 3 from 4 */
+    *pIq = __QSUB(product4, product3);
+  }
+
+  /**
+   * @} end of park group
+   */
+
+
+  /**
+   * @ingroup groupController
+   */
+
+  /**
+   * @defgroup inv_park Vector Inverse Park transform
+   * Inverse Park transform converts the input flux and torque components to two-coordinate vector.
+   *
+   * The function operates on a single sample of data and each call to the function returns the processed output.
+   * The library provides separate functions for Q31 and floating-point data types.
+   * \par Algorithm
+   * \image html parkInvFormula.gif
+   * where <code>pIalpha</code> and <code>pIbeta</code> are the stator vector components,
+   * <code>Id</code> and <code>Iq</code> are rotor vector components and <code>cosVal</code> and <code>sinVal</code> are the
+   * cosine and sine values of theta (rotor flux position).
+   * \par Fixed-Point Behavior
+   * Care must be taken when using the Q31 version of the Park transform.
+   * In particular, the overflow and saturation behavior of the accumulator used must be considered.
+   * Refer to the function specific documentation below for usage guidelines.
+   */
+
+  /**
+   * @addtogroup inv_park
+   * @{
+   */
+
+   /**
+   * @brief  Floating-point Inverse Park transform
+   * @param[in]  Id       input coordinate of rotor reference frame d
+   * @param[in]  Iq       input coordinate of rotor reference frame q
+   * @param[out] pIalpha  points to output two-phase orthogonal vector axis alpha
+   * @param[out] pIbeta   points to output two-phase orthogonal vector axis beta
+   * @param[in]  sinVal   sine value of rotation angle theta
+   * @param[in]  cosVal   cosine value of rotation angle theta
+   * @return     none
+   */
+  __STATIC_FORCEINLINE void arm_inv_park_f32(
+  float32_t Id,
+  float32_t Iq,
+  float32_t * pIalpha,
+  float32_t * pIbeta,
+  float32_t sinVal,
+  float32_t cosVal)
+  {
+    /* Calculate pIalpha using the equation, pIalpha = Id * cosVal - Iq * sinVal */
+    *pIalpha = Id * cosVal - Iq * sinVal;
+
+    /* Calculate pIbeta using the equation, pIbeta = Id * sinVal + Iq * cosVal */
+    *pIbeta = Id * sinVal + Iq * cosVal;
+  }
+
+
+/**
+  @brief  Inverse Park transform for   Q31 version
+  @param[in]  Id       input coordinate of rotor reference frame d
+  @param[in]  Iq       input coordinate of rotor reference frame q
+  @param[out] pIalpha  points to output two-phase orthogonal vector axis alpha
+  @param[out] pIbeta   points to output two-phase orthogonal vector axis beta
+  @param[in]  sinVal   sine value of rotation angle theta
+  @param[in]  cosVal   cosine value of rotation angle theta
+  @return     none
+
+  @par Scaling and Overflow Behavior
+         The function is implemented using an internal 32-bit accumulator.
+         The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
+         There is saturation on the addition, hence there is no risk of overflow.
+ */
+__STATIC_FORCEINLINE void arm_inv_park_q31(
+  q31_t Id,
+  q31_t Iq,
+  q31_t * pIalpha,
+  q31_t * pIbeta,
+  q31_t sinVal,
+  q31_t cosVal)
+  {
+    q31_t product1, product2;                    /* Temporary variables used to store intermediate results */
+    q31_t product3, product4;                    /* Temporary variables used to store intermediate results */
+
+    /* Intermediate product is calculated by (Id * cosVal) */
+    product1 = (q31_t) (((q63_t) (Id) * (cosVal)) >> 31);
+
+    /* Intermediate product is calculated by (Iq * sinVal) */
+    product2 = (q31_t) (((q63_t) (Iq) * (sinVal)) >> 31);
+
+
+    /* Intermediate product is calculated by (Id * sinVal) */
+    product3 = (q31_t) (((q63_t) (Id) * (sinVal)) >> 31);
+
+    /* Intermediate product is calculated by (Iq * cosVal) */
+    product4 = (q31_t) (((q63_t) (Iq) * (cosVal)) >> 31);
+
+    /* Calculate pIalpha by using the two intermediate products 1 and 2 */
+    *pIalpha = __QSUB(product1, product2);
+
+    /* Calculate pIbeta by using the two intermediate products 3 and 4 */
+    *pIbeta = __QADD(product4, product3);
+  }
+
+  /**
+   * @} end of Inverse park group
+   */
+
+/**
+   * @ingroup groupController
+   */
+
+  /**
+   * @defgroup clarke Vector Clarke Transform
+   * Forward Clarke transform converts the instantaneous stator phases into a two-coordinate time invariant vector.
+   * Generally the Clarke transform uses three-phase currents <code>Ia, Ib and Ic</code> to calculate currents
+   * in the two-phase orthogonal stator axis <code>Ialpha</code> and <code>Ibeta</code>.
+   * When <code>Ialpha</code> is superposed with <code>Ia</code> as shown in the figure below
+   * \image html clarke.gif Stator current space vector and its components in (a,b).
+   * and <code>Ia + Ib + Ic = 0</code>, in this condition <code>Ialpha</code> and <code>Ibeta</code>
+   * can be calculated using only <code>Ia</code> and <code>Ib</code>.
+   *
+   * The function operates on a single sample of data and each call to the function returns the processed output.
+   * The library provides separate functions for Q31 and floating-point data types.
+   * \par Algorithm
+   * \image html clarkeFormula.gif
+   * where <code>Ia</code> and <code>Ib</code> are the instantaneous stator phases and
+   * <code>pIalpha</code> and <code>pIbeta</code> are the two coordinates of time invariant vector.
+   * \par Fixed-Point Behavior
+   * Care must be taken when using the Q31 version of the Clarke transform.
+   * In particular, the overflow and saturation behavior of the accumulator used must be considered.
+   * Refer to the function specific documentation below for usage guidelines.
+   */
+
+  /**
+   * @addtogroup clarke
+   * @{
+   */
+
+  /**
+   *
+   * @brief  Floating-point Clarke transform
+   * @param[in]  Ia       input three-phase coordinate <code>a</code>
+   * @param[in]  Ib       input three-phase coordinate <code>b</code>
+   * @param[out] pIalpha  points to output two-phase orthogonal vector axis alpha
+   * @param[out] pIbeta   points to output two-phase orthogonal vector axis beta
+   * @return        none
+   */
+  __STATIC_FORCEINLINE void arm_clarke_f32(
+  float32_t Ia,
+  float32_t Ib,
+  float32_t * pIalpha,
+  float32_t * pIbeta)
+  {
+    /* Calculate pIalpha using the equation, pIalpha = Ia */
+    *pIalpha = Ia;
+
+    /* Calculate pIbeta using the equation, pIbeta = (1/sqrt(3)) * Ia + (2/sqrt(3)) * Ib */
+    *pIbeta = (0.57735026919f * Ia + 1.15470053838f * Ib);
+  }
+
+
+/**
+  @brief  Clarke transform for Q31 version
+  @param[in]  Ia       input three-phase coordinate <code>a</code>
+  @param[in]  Ib       input three-phase coordinate <code>b</code>
+  @param[out] pIalpha  points to output two-phase orthogonal vector axis alpha
+  @param[out] pIbeta   points to output two-phase orthogonal vector axis beta
+  @return     none
+
+  \par Scaling and Overflow Behavior
+         The function is implemented using an internal 32-bit accumulator.
+         The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
+         There is saturation on the addition, hence there is no risk of overflow.
+ */
+__STATIC_FORCEINLINE void arm_clarke_q31(
+  q31_t Ia,
+  q31_t Ib,
+  q31_t * pIalpha,
+  q31_t * pIbeta)
+  {
+    q31_t product1, product2;                    /* Temporary variables used to store intermediate results */
+
+    /* Calculating pIalpha from Ia by equation pIalpha = Ia */
+    *pIalpha = Ia;
+
+    /* Intermediate product is calculated by (1/(sqrt(3)) * Ia) */
+    product1 = (q31_t) (((q63_t) Ia * 0x24F34E8B) >> 30);
+
+    /* Intermediate product is calculated by (2/sqrt(3) * Ib) */
+    product2 = (q31_t) (((q63_t) Ib * 0x49E69D16) >> 30);
+
+    /* pIbeta is calculated by adding the intermediate products */
+    *pIbeta = __QADD(product1, product2);
+  }
+
+  /**
+   * @} end of clarke group
+   */
+
+
+  /**
+   * @ingroup groupController
+   */
+
+  /**
+   * @defgroup inv_clarke Vector Inverse Clarke Transform
+   * Inverse Clarke transform converts the two-coordinate time invariant vector into instantaneous stator phases.
+   *
+   * The function operates on a single sample of data and each call to the function returns the processed output.
+   * The library provides separate functions for Q31 and floating-point data types.
+   * \par Algorithm
+   * \image html clarkeInvFormula.gif
+   * where <code>pIa</code> and <code>pIb</code> are the instantaneous stator phases and
+   * <code>Ialpha</code> and <code>Ibeta</code> are the two coordinates of time invariant vector.
+   * \par Fixed-Point Behavior
+   * Care must be taken when using the Q31 version of the Clarke transform.
+   * In particular, the overflow and saturation behavior of the accumulator used must be considered.
+   * Refer to the function specific documentation below for usage guidelines.
+   */
+
+  /**
+   * @addtogroup inv_clarke
+   * @{
+   */
+
+   /**
+   * @brief  Floating-point Inverse Clarke transform
+   * @param[in]  Ialpha  input two-phase orthogonal vector axis alpha
+   * @param[in]  Ibeta   input two-phase orthogonal vector axis beta
+   * @param[out] pIa     points to output three-phase coordinate <code>a</code>
+   * @param[out] pIb     points to output three-phase coordinate <code>b</code>
+   * @return     none
+   */
+  __STATIC_FORCEINLINE void arm_inv_clarke_f32(
+  float32_t Ialpha,
+  float32_t Ibeta,
+  float32_t * pIa,
+  float32_t * pIb)
+  {
+    /* Calculating pIa from Ialpha by equation pIa = Ialpha */
+    *pIa = Ialpha;
+
+    /* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */
+    *pIb = -0.5f * Ialpha + 0.8660254039f * Ibeta;
+  }
+
+
+/**
+  @brief  Inverse Clarke transform for Q31 version
+  @param[in]  Ialpha  input two-phase orthogonal vector axis alpha
+  @param[in]  Ibeta   input two-phase orthogonal vector axis beta
+  @param[out] pIa     points to output three-phase coordinate <code>a</code>
+  @param[out] pIb     points to output three-phase coordinate <code>b</code>
+  @return     none
+
+  \par Scaling and Overflow Behavior
+         The function is implemented using an internal 32-bit accumulator.
+         The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
+         There is saturation on the subtraction, hence there is no risk of overflow.
+ */
+__STATIC_FORCEINLINE void arm_inv_clarke_q31(
+  q31_t Ialpha,
+  q31_t Ibeta,
+  q31_t * pIa,
+  q31_t * pIb)
+  {
+    q31_t product1, product2;                    /* Temporary variables used to store intermediate results */
+
+    /* Calculating pIa from Ialpha by equation pIa = Ialpha */
+    *pIa = Ialpha;
+
+    /* Intermediate product is calculated by (1/(2*sqrt(3)) * Ia) */
+    product1 = (q31_t) (((q63_t) (Ialpha) * (0x40000000)) >> 31);
+
+    /* Intermediate product is calculated by (1/sqrt(3) * pIb) */
+    product2 = (q31_t) (((q63_t) (Ibeta) * (0x6ED9EBA1)) >> 31);
+
+    /* pIb is calculated by subtracting the products */
+    *pIb = __QSUB(product2, product1);
+  }
+
+  /**
+   * @} end of inv_clarke group
+   */
+
+
+
+  
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _CONTROLLER_FUNCTIONS_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/controller_functions_f16.h

@@ -0,0 +1,41 @@
+/******************************************************************************
+ * @file     controller_functions_f16.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _CONTROLLER_FUNCTIONS_F16_H_
+#define _CONTROLLER_FUNCTIONS_F16_H_
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+#if defined(ARM_FLOAT16_SUPPORTED)
+#endif /*defined(ARM_FLOAT16_SUPPORTED)*/
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _CONTROLLER_FUNCTIONS_F16_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/distance_functions.h

@@ -0,0 +1,341 @@
+/******************************************************************************
+ * @file     distance_functions.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _DISTANCE_FUNCTIONS_H_
+#define _DISTANCE_FUNCTIONS_H_
+
+#include "arm_math_types.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+
+#include "dsp/statistics_functions.h"
+#include "dsp/basic_math_functions.h"
+#include "dsp/fast_math_functions.h"
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+
+/**
+ * @defgroup groupDistance Distance functions
+ *
+ * Distance functions for use with clustering algorithms.
+ * There are distance functions for float vectors and boolean vectors.
+ *
+ */
+
+/* 6.14 bug */
+#if defined (__ARMCC_VERSION) && (__ARMCC_VERSION >= 6100100) && (__ARMCC_VERSION < 6150001)
+ 
+__attribute__((weak)) float __powisf2(float a, int b);
+
+#endif 
+
+/**
+ * @brief        Euclidean distance between two vectors
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+
+float32_t arm_euclidean_distance_f32(const float32_t *pA,const float32_t *pB, uint32_t blockSize);
+
+/**
+ * @brief        Euclidean distance between two vectors
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+
+float64_t arm_euclidean_distance_f64(const float64_t *pA,const float64_t *pB, uint32_t blockSize);
+
+/**
+ * @brief        Bray-Curtis distance between two vectors
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+float32_t arm_braycurtis_distance_f32(const float32_t *pA,const float32_t *pB, uint32_t blockSize);
+
+/**
+ * @brief        Canberra distance between two vectors
+ *
+ * This function may divide by zero when samples pA[i] and pB[i] are both zero.
+ * The result of the computation will be correct. So the division per zero may be
+ * ignored.
+ *
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+float32_t arm_canberra_distance_f32(const float32_t *pA,const float32_t *pB, uint32_t blockSize);
+
+
+/**
+ * @brief        Chebyshev distance between two vectors
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+float32_t arm_chebyshev_distance_f32(const float32_t *pA,const float32_t *pB, uint32_t blockSize);
+
+
+/**
+ * @brief        Chebyshev distance between two vectors
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+float64_t arm_chebyshev_distance_f64(const float64_t *pA,const float64_t *pB, uint32_t blockSize);
+
+
+/**
+ * @brief        Cityblock (Manhattan) distance between two vectors
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+float32_t arm_cityblock_distance_f32(const float32_t *pA,const float32_t *pB, uint32_t blockSize);
+
+/**
+ * @brief        Cityblock (Manhattan) distance between two vectors
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+float64_t arm_cityblock_distance_f64(const float64_t *pA,const float64_t *pB, uint32_t blockSize);
+
+/**
+ * @brief        Correlation distance between two vectors
+ *
+ * The input vectors are modified in place !
+ *
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+float32_t arm_correlation_distance_f32(float32_t *pA,float32_t *pB, uint32_t blockSize);
+
+/**
+ * @brief        Cosine distance between two vectors
+ *
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+
+float32_t arm_cosine_distance_f32(const float32_t *pA,const float32_t *pB, uint32_t blockSize);
+
+/**
+ * @brief        Cosine distance between two vectors
+ *
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+
+float64_t arm_cosine_distance_f64(const float64_t *pA,const float64_t *pB, uint32_t blockSize);
+
+/**
+ * @brief        Jensen-Shannon distance between two vectors
+ *
+ * This function is assuming that elements of second vector are > 0
+ * and 0 only when the corresponding element of first vector is 0.
+ * Otherwise the result of the computation does not make sense
+ * and for speed reasons, the cases returning NaN or Infinity are not
+ * managed.
+ *
+ * When the function is computing x log (x / y) with x 0 and y 0,
+ * it will compute the right value (0) but a division per zero will occur
+ * and shoudl be ignored in client code.
+ *
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+
+float32_t arm_jensenshannon_distance_f32(const float32_t *pA,const float32_t *pB,uint32_t blockSize);
+
+/**
+ * @brief        Minkowski distance between two vectors
+ *
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    n          Norm order (>= 2)
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+
+
+
+float32_t arm_minkowski_distance_f32(const float32_t *pA,const float32_t *pB, int32_t order, uint32_t blockSize);
+
+/**
+ * @brief        Dice distance between two vectors
+ *
+ * @param[in]    pA              First vector of packed booleans
+ * @param[in]    pB              Second vector of packed booleans
+ * @param[in]    order           Distance order
+ * @param[in]    blockSize       Number of samples
+ * @return distance
+ *
+ */
+
+
+float32_t arm_dice_distance(const uint32_t *pA, const uint32_t *pB, uint32_t numberOfBools);
+
+/**
+ * @brief        Hamming distance between two vectors
+ *
+ * @param[in]    pA              First vector of packed booleans
+ * @param[in]    pB              Second vector of packed booleans
+ * @param[in]    numberOfBools   Number of booleans
+ * @return distance
+ *
+ */
+
+float32_t arm_hamming_distance(const uint32_t *pA, const uint32_t *pB, uint32_t numberOfBools);
+
+/**
+ * @brief        Jaccard distance between two vectors
+ *
+ * @param[in]    pA              First vector of packed booleans
+ * @param[in]    pB              Second vector of packed booleans
+ * @param[in]    numberOfBools   Number of booleans
+ * @return distance
+ *
+ */
+
+float32_t arm_jaccard_distance(const uint32_t *pA, const uint32_t *pB, uint32_t numberOfBools);
+
+/**
+ * @brief        Kulsinski distance between two vectors
+ *
+ * @param[in]    pA              First vector of packed booleans
+ * @param[in]    pB              Second vector of packed booleans
+ * @param[in]    numberOfBools   Number of booleans
+ * @return distance
+ *
+ */
+
+float32_t arm_kulsinski_distance(const uint32_t *pA, const uint32_t *pB, uint32_t numberOfBools);
+
+/**
+ * @brief        Roger Stanimoto distance between two vectors
+ *
+ * @param[in]    pA              First vector of packed booleans
+ * @param[in]    pB              Second vector of packed booleans
+ * @param[in]    numberOfBools   Number of booleans
+ * @return distance
+ *
+ */
+
+float32_t arm_rogerstanimoto_distance(const uint32_t *pA, const uint32_t *pB, uint32_t numberOfBools);
+
+/**
+ * @brief        Russell-Rao distance between two vectors
+ *
+ * @param[in]    pA              First vector of packed booleans
+ * @param[in]    pB              Second vector of packed booleans
+ * @param[in]    numberOfBools   Number of booleans
+ * @return distance
+ *
+ */
+
+float32_t arm_russellrao_distance(const uint32_t *pA, const uint32_t *pB, uint32_t numberOfBools);
+
+/**
+ * @brief        Sokal-Michener distance between two vectors
+ *
+ * @param[in]    pA              First vector of packed booleans
+ * @param[in]    pB              Second vector of packed booleans
+ * @param[in]    numberOfBools   Number of booleans
+ * @return distance
+ *
+ */
+
+float32_t arm_sokalmichener_distance(const uint32_t *pA, const uint32_t *pB, uint32_t numberOfBools);
+
+/**
+ * @brief        Sokal-Sneath distance between two vectors
+ *
+ * @param[in]    pA              First vector of packed booleans
+ * @param[in]    pB              Second vector of packed booleans
+ * @param[in]    numberOfBools   Number of booleans
+ * @return distance
+ *
+ */
+
+float32_t arm_sokalsneath_distance(const uint32_t *pA, const uint32_t *pB, uint32_t numberOfBools);
+
+/**
+ * @brief        Yule distance between two vectors
+ *
+ * @param[in]    pA              First vector of packed booleans
+ * @param[in]    pB              Second vector of packed booleans
+ * @param[in]    numberOfBools   Number of booleans
+ * @return distance
+ *
+ */
+
+float32_t arm_yule_distance(const uint32_t *pA, const uint32_t *pB, uint32_t numberOfBools);
+
+
+
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _DISTANCE_FUNCTIONS_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/distance_functions_f16.h

@@ -0,0 +1,180 @@
+/******************************************************************************
+ * @file     distance_functions_f16.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _DISTANCE_FUNCTIONS_F16_H_
+#define _DISTANCE_FUNCTIONS_F16_H_
+
+#include "arm_math_types_f16.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+
+/* 6.14 bug */
+#if defined (__ARMCC_VERSION) && (__ARMCC_VERSION >= 6100100) && (__ARMCC_VERSION < 6150001)
+/* Defined in minkowski_f32 */
+__attribute__((weak)) float __powisf2(float a, int b);
+#endif 
+
+#include "dsp/statistics_functions_f16.h"
+#include "dsp/basic_math_functions_f16.h"
+
+#include "dsp/fast_math_functions_f16.h"
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+#if defined(ARM_FLOAT16_SUPPORTED)
+
+/**
+ * @brief        Euclidean distance between two vectors
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+
+float16_t arm_euclidean_distance_f16(const float16_t *pA,const float16_t *pB, uint32_t blockSize);
+
+/**
+ * @brief        Bray-Curtis distance between two vectors
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+float16_t arm_braycurtis_distance_f16(const float16_t *pA,const float16_t *pB, uint32_t blockSize);
+
+/**
+ * @brief        Canberra distance between two vectors
+ *
+ * This function may divide by zero when samples pA[i] and pB[i] are both zero.
+ * The result of the computation will be correct. So the division per zero may be
+ * ignored.
+ *
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+float16_t arm_canberra_distance_f16(const float16_t *pA,const float16_t *pB, uint32_t blockSize);
+
+
+/**
+ * @brief        Chebyshev distance between two vectors
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+float16_t arm_chebyshev_distance_f16(const float16_t *pA,const float16_t *pB, uint32_t blockSize);
+
+
+/**
+ * @brief        Cityblock (Manhattan) distance between two vectors
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+float16_t arm_cityblock_distance_f16(const float16_t *pA,const float16_t *pB, uint32_t blockSize);
+
+/**
+ * @brief        Correlation distance between two vectors
+ *
+ * The input vectors are modified in place !
+ *
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+float16_t arm_correlation_distance_f16(float16_t *pA,float16_t *pB, uint32_t blockSize);
+
+/**
+ * @brief        Cosine distance between two vectors
+ *
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+
+float16_t arm_cosine_distance_f16(const float16_t *pA,const float16_t *pB, uint32_t blockSize);
+
+/**
+ * @brief        Jensen-Shannon distance between two vectors
+ *
+ * This function is assuming that elements of second vector are > 0
+ * and 0 only when the corresponding element of first vector is 0.
+ * Otherwise the result of the computation does not make sense
+ * and for speed reasons, the cases returning NaN or Infinity are not
+ * managed.
+ *
+ * When the function is computing x log (x / y) with x 0 and y 0,
+ * it will compute the right value (0) but a division per zero will occur
+ * and shoudl be ignored in client code.
+ *
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+
+float16_t arm_jensenshannon_distance_f16(const float16_t *pA,const float16_t *pB,uint32_t blockSize);
+
+/**
+ * @brief        Minkowski distance between two vectors
+ *
+ * @param[in]    pA         First vector
+ * @param[in]    pB         Second vector
+ * @param[in]    n          Norm order (>= 2)
+ * @param[in]    blockSize  vector length
+ * @return distance
+ *
+ */
+
+
+
+float16_t arm_minkowski_distance_f16(const float16_t *pA,const float16_t *pB, int32_t order, uint32_t blockSize);
+
+
+#endif /*defined(ARM_FLOAT16_SUPPORTED)*/
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _DISTANCE_FUNCTIONS_F16_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/fast_math_functions.h

@@ -0,0 +1,389 @@
+/******************************************************************************
+ * @file     fast_math_functions.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _FAST_MATH_FUNCTIONS_H_
+#define _FAST_MATH_FUNCTIONS_H_
+
+#include "arm_math_types.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+
+#include "dsp/basic_math_functions.h"
+
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+  /**
+   * @brief Macros required for SINE and COSINE Fast math approximations
+   */
+
+#define FAST_MATH_TABLE_SIZE  512
+#define FAST_MATH_Q31_SHIFT   (32 - 10)
+#define FAST_MATH_Q15_SHIFT   (16 - 10)
+  
+#ifndef PI
+  #define PI               3.14159265358979f
+#endif
+
+
+/**
+ * @defgroup groupFastMath Fast Math Functions
+ * This set of functions provides a fast approximation to sine, cosine, and square root.
+ * As compared to most of the other functions in the CMSIS math library, the fast math functions
+ * operate on individual values and not arrays.
+ * There are separate functions for Q15, Q31, and floating-point data.
+ *
+ */
+
+  /**
+   * @ingroup groupFastMath
+   */
+
+
+/**
+  @addtogroup sin
+  @{
+ */
+
+/**
+   * @brief  Fast approximation to the trigonometric sine function for floating-point data.
+   * @param[in] x  input value in radians.
+   * @return  sin(x).
+   */
+  float32_t arm_sin_f32(
+  float32_t x);
+
+
+  /**
+   * @brief  Fast approximation to the trigonometric sine function for Q31 data.
+   * @param[in] x  Scaled input value in radians.
+   * @return  sin(x).
+   */
+  q31_t arm_sin_q31(
+  q31_t x);
+
+
+  /**
+   * @brief  Fast approximation to the trigonometric sine function for Q15 data.
+   * @param[in] x  Scaled input value in radians.
+   * @return  sin(x).
+   */
+  q15_t arm_sin_q15(
+  q15_t x);
+
+/**
+  @} end of sin group
+ */
+
+/**
+  @addtogroup cos
+  @{
+ */
+
+  /**
+   * @brief  Fast approximation to the trigonometric cosine function for floating-point data.
+   * @param[in] x  input value in radians.
+   * @return  cos(x).
+   */
+  float32_t arm_cos_f32(
+  float32_t x);
+
+
+  /**
+   * @brief Fast approximation to the trigonometric cosine function for Q31 data.
+   * @param[in] x  Scaled input value in radians.
+   * @return  cos(x).
+   */
+  q31_t arm_cos_q31(
+  q31_t x);
+
+
+  /**
+   * @brief  Fast approximation to the trigonometric cosine function for Q15 data.
+   * @param[in] x  Scaled input value in radians.
+   * @return  cos(x).
+   */
+  q15_t arm_cos_q15(
+  q15_t x);
+
+/**
+  @} end of cos group
+ */
+
+
+/**
+  @brief         Floating-point vector of log values.
+  @param[in]     pSrc       points to the input vector
+  @param[out]    pDst       points to the output vector
+  @param[in]     blockSize  number of samples in each vector
+  @return        none
+ */
+  void arm_vlog_f32(
+  const float32_t * pSrc,
+        float32_t * pDst,
+        uint32_t blockSize);
+
+
+
+/**
+  @brief         Floating-point vector of log values.
+  @param[in]     pSrc       points to the input vector
+  @param[out]    pDst       points to the output vector
+  @param[in]     blockSize  number of samples in each vector
+  @return        none
+ */
+  void arm_vlog_f64(
+  const float64_t * pSrc,
+		float64_t * pDst,
+		uint32_t blockSize);
+
+
+
+  /**
+   * @brief  q31 vector of log values.
+   * @param[in]     pSrc       points to the input vector in q31
+   * @param[out]    pDst       points to the output vector in q5.26
+   * @param[in]     blockSize  number of samples in each vector
+   * @return        none
+   */
+  void arm_vlog_q31(const q31_t * pSrc,
+        q31_t * pDst,
+        uint32_t blockSize);
+
+  /**
+   * @brief  q15 vector of log values.
+   * @param[in]     pSrc       points to the input vector in q15
+   * @param[out]    pDst       points to the output vector in q4.11
+   * @param[in]     blockSize  number of samples in each vector
+   * @return        none
+   */
+  void arm_vlog_q15(const q15_t * pSrc,
+        q15_t * pDst,
+        uint32_t blockSize);
+
+
+
+/**
+  @brief         Floating-point vector of exp values.
+  @param[in]     pSrc       points to the input vector
+  @param[out]    pDst       points to the output vector
+  @param[in]     blockSize  number of samples in each vector
+  @return        none
+ */
+  void arm_vexp_f32(
+  const float32_t * pSrc,
+        float32_t * pDst,
+        uint32_t blockSize);
+
+
+
+/**
+  @brief         Floating-point vector of exp values.
+  @param[in]     pSrc       points to the input vector
+  @param[out]    pDst       points to the output vector
+  @param[in]     blockSize  number of samples in each vector
+  @return        none
+ */
+  void arm_vexp_f64(
+  const float64_t * pSrc,
+		float64_t * pDst,
+		uint32_t blockSize);
+
+
+
+ /**
+   * @defgroup SQRT Square Root
+   *
+   * Computes the square root of a number.
+   * There are separate functions for Q15, Q31, and floating-point data types.
+   * The square root function is computed using the Newton-Raphson algorithm.
+   * This is an iterative algorithm of the form:
+   * <pre>
+   *      x1 = x0 - f(x0)/f'(x0)
+   * </pre>
+   * where <code>x1</code> is the current estimate,
+   * <code>x0</code> is the previous estimate, and
+   * <code>f'(x0)</code> is the derivative of <code>f()</code> evaluated at <code>x0</code>.
+   * For the square root function, the algorithm reduces to:
+   * <pre>
+   *     x0 = in/2                         [initial guess]
+   *     x1 = 1/2 * ( x0 + in / x0)        [each iteration]
+   * </pre>
+   */
+
+
+  /**
+   * @addtogroup SQRT
+   * @{
+   */
+
+/**
+  @brief         Floating-point square root function.
+  @param[in]     in    input value
+  @param[out]    pOut  square root of input value
+  @return        execution status
+                   - \ref ARM_MATH_SUCCESS        : input value is positive
+                   - \ref ARM_MATH_ARGUMENT_ERROR : input value is negative; *pOut is set to 0
+ */
+__STATIC_FORCEINLINE arm_status arm_sqrt_f32(
+  const float32_t in,
+  float32_t * pOut)
+  {
+    if (in >= 0.0f)
+    {
+#if defined ( __CC_ARM )
+  #if defined __TARGET_FPU_VFP
+      *pOut = __sqrtf(in);
+  #else
+      *pOut = sqrtf(in);
+  #endif
+
+#elif defined ( __ICCARM__ )
+  #if defined __ARMVFP__
+      __ASM("VSQRT.F32 %0,%1" : "=t"(*pOut) : "t"(in));
+  #else
+      *pOut = sqrtf(in);
+  #endif
+
+#else
+      *pOut = sqrtf(in);
+#endif
+
+      return (ARM_MATH_SUCCESS);
+    }
+    else
+    {
+      *pOut = 0.0f;
+      return (ARM_MATH_ARGUMENT_ERROR);
+    }
+  }
+
+
+/**
+  @brief         Q31 square root function.
+  @param[in]     in    input value.  The range of the input value is [0 +1) or 0x00000000 to 0x7FFFFFFF
+  @param[out]    pOut  points to square root of input value
+  @return        execution status
+                   - \ref ARM_MATH_SUCCESS        : input value is positive
+                   - \ref ARM_MATH_ARGUMENT_ERROR : input value is negative; *pOut is set to 0
+ */
+arm_status arm_sqrt_q31(
+  q31_t in,
+  q31_t * pOut);
+
+
+/**
+  @brief         Q15 square root function.
+  @param[in]     in    input value.  The range of the input value is [0 +1) or 0x0000 to 0x7FFF
+  @param[out]    pOut  points to square root of input value
+  @return        execution status
+                   - \ref ARM_MATH_SUCCESS        : input value is positive
+                   - \ref ARM_MATH_ARGUMENT_ERROR : input value is negative; *pOut is set to 0
+ */
+arm_status arm_sqrt_q15(
+  q15_t in,
+  q15_t * pOut);
+
+
+
+  /**
+   * @} end of SQRT group
+   */
+
+  /**
+  @brief         Fixed point division
+  @param[in]     numerator    Numerator
+  @param[in]     denominator  Denominator
+  @param[out]    quotient     Quotient value normalized between -1.0 and 1.0
+  @param[out]    shift        Shift left value to get the unnormalized quotient
+  @return        error status
+
+  When dividing by 0, an error ARM_MATH_NANINF is returned. And the quotient is forced
+  to the saturated negative or positive value.
+ */
+
+arm_status arm_divide_q15(q15_t numerator,
+  q15_t denominator,
+  q15_t *quotient,
+  int16_t *shift);
+
+  /**
+  @brief         Fixed point division
+  @param[in]     numerator    Numerator
+  @param[in]     denominator  Denominator
+  @param[out]    quotient     Quotient value normalized between -1.0 and 1.0
+  @param[out]    shift        Shift left value to get the unnormalized quotient
+  @return        error status
+
+  When dividing by 0, an error ARM_MATH_NANINF is returned. And the quotient is forced
+  to the saturated negative or positive value.
+ */
+
+arm_status arm_divide_q31(q31_t numerator,
+  q31_t denominator,
+  q31_t *quotient,
+  int16_t *shift);
+
+
+
+  /**
+     @brief  Arc tangent in radian of y/x using sign of x and y to determine right quadrant.
+     @param[in]   y  y coordinate
+     @param[in]   x  x coordinate
+     @param[out]  result  Result
+     @return  error status.
+   */
+  arm_status arm_atan2_f32(float32_t y,float32_t x,float32_t *result);
+
+
+  /**
+     @brief  Arc tangent in radian of y/x using sign of x and y to determine right quadrant.
+     @param[in]   y  y coordinate
+     @param[in]   x  x coordinate
+     @param[out]  result  Result in Q2.29
+     @return  error status.
+   */
+  arm_status arm_atan2_q31(q31_t y,q31_t x,q31_t *result);
+
+  /**
+     @brief  Arc tangent in radian of y/x using sign of x and y to determine right quadrant.
+     @param[in]   y  y coordinate
+     @param[in]   x  x coordinate
+     @param[out]  result  Result in Q2.13
+     @return  error status.
+   */
+  arm_status arm_atan2_q15(q15_t y,q15_t x,q15_t *result);
+
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _FAST_MATH_FUNCTIONS_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/fast_math_functions_f16.h

@@ -0,0 +1,125 @@
+/******************************************************************************
+ * @file     fast_math_functions_f16.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _FAST_MATH_FUNCTIONS_F16_H_
+#define _FAST_MATH_FUNCTIONS_F16_H_
+
+#include "arm_math_types_f16.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+
+/* For sqrt_f32 */
+#include "dsp/fast_math_functions.h"
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+#if defined(ARM_FLOAT16_SUPPORTED)
+
+ /**
+   * @addtogroup SQRT
+   * @{
+   */
+
+/**
+  @brief         Floating-point square root function.
+  @param[in]     in    input value
+  @param[out]    pOut  square root of input value
+  @return        execution status
+                   - \ref ARM_MATH_SUCCESS        : input value is positive
+                   - \ref ARM_MATH_ARGUMENT_ERROR : input value is negative; *pOut is set to 0
+ */
+__STATIC_FORCEINLINE arm_status arm_sqrt_f16(
+  float16_t in,
+  float16_t * pOut)
+  {
+    float32_t r;
+    arm_status status;
+    status=arm_sqrt_f32((float32_t)in,&r);
+    *pOut=(float16_t)r;
+    return(status);
+  }
+
+
+/**
+  @} end of SQRT group
+ */
+  
+/**
+  @brief         Floating-point vector of log values.
+  @param[in]     pSrc       points to the input vector
+  @param[out]    pDst       points to the output vector
+  @param[in]     blockSize  number of samples in each vector
+  @return        none
+ */
+  void arm_vlog_f16(
+  const float16_t * pSrc,
+        float16_t * pDst,
+        uint32_t blockSize);
+
+/**
+  @brief         Floating-point vector of exp values.
+  @param[in]     pSrc       points to the input vector
+  @param[out]    pDst       points to the output vector
+  @param[in]     blockSize  number of samples in each vector
+  @return        none
+ */
+  void arm_vexp_f16(
+  const float16_t * pSrc,
+        float16_t * pDst,
+        uint32_t blockSize);
+
+  /**
+  @brief         Floating-point vector of inverse values.
+  @param[in]     pSrc       points to the input vector
+  @param[out]    pDst       points to the output vector
+  @param[in]     blockSize  number of samples in each vector
+  @return        none
+ */
+  void arm_vinverse_f16(
+  const float16_t * pSrc,
+        float16_t * pDst,
+        uint32_t blockSize);
+
+  /**
+     @brief  Arc tangent in radian of y/x using sign of x and y to determine right quadrant.
+     @param[in]   y  y coordinate
+     @param[in]   x  x coordinate
+     @param[out]  result  Result
+     @return  error status.
+   */
+  arm_status arm_atan2_f16(float16_t y,float16_t x,float16_t *result);
+
+#endif /*defined(ARM_FLOAT16_SUPPORTED)*/
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _FAST_MATH_FUNCTIONS_F16_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/filtering_functions_f16.h

@@ -0,0 +1,237 @@
+/******************************************************************************
+ * @file     filtering_functions_f16.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _FILTERING_FUNCTIONS_F16_H_
+#define _FILTERING_FUNCTIONS_F16_H_
+
+#include "arm_math_types_f16.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+#if defined(ARM_FLOAT16_SUPPORTED)
+
+ /**
+   * @brief Instance structure for the floating-point FIR filter.
+   */
+  typedef struct
+  {
+          uint16_t numTaps;     /**< number of filter coefficients in the filter. */
+          float16_t *pState;    /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+    const float16_t *pCoeffs;   /**< points to the coefficient array. The array is of length numTaps. */
+  } arm_fir_instance_f16;
+
+  /**
+   * @brief  Initialization function for the floating-point FIR filter.
+   * @param[in,out] S          points to an instance of the floating-point FIR filter structure.
+   * @param[in]     numTaps    Number of filter coefficients in the filter.
+   * @param[in]     pCoeffs    points to the filter coefficients.
+   * @param[in]     pState     points to the state buffer.
+   * @param[in]     blockSize  number of samples that are processed at a time.
+   */
+  void arm_fir_init_f16(
+        arm_fir_instance_f16 * S,
+        uint16_t numTaps,
+  const float16_t * pCoeffs,
+        float16_t * pState,
+        uint32_t blockSize);
+
+  /**
+   * @brief Processing function for the floating-point FIR filter.
+   * @param[in]  S          points to an instance of the floating-point FIR structure.
+   * @param[in]  pSrc       points to the block of input data.
+   * @param[out] pDst       points to the block of output data.
+   * @param[in]  blockSize  number of samples to process.
+   */
+  void arm_fir_f16(
+  const arm_fir_instance_f16 * S,
+  const float16_t * pSrc,
+        float16_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief Instance structure for the floating-point Biquad cascade filter.
+   */
+  typedef struct
+  {
+          uint32_t numStages;      /**< number of 2nd order stages in the filter.  Overall order is 2*numStages. */
+          float16_t *pState;       /**< Points to the array of state coefficients.  The array is of length 4*numStages. */
+    const float16_t *pCoeffs;      /**< Points to the array of coefficients.  The array is of length 5*numStages. */
+  } arm_biquad_casd_df1_inst_f16;
+
+#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE)
+  /**
+   * @brief Instance structure for the modified Biquad coefs required by vectorized code.
+   */
+  typedef struct
+  {
+      float16_t coeffs[12][8]; /**< Points to the array of modified coefficients.  The array is of length 32. There is one per stage */
+  } arm_biquad_mod_coef_f16;
+#endif 
+
+  /**
+   * @brief Processing function for the floating-point Biquad cascade filter.
+   * @param[in]  S          points to an instance of the floating-point Biquad cascade structure.
+   * @param[in]  pSrc       points to the block of input data.
+   * @param[out] pDst       points to the block of output data.
+   * @param[in]  blockSize  number of samples to process.
+   */
+  void arm_biquad_cascade_df1_f16(
+  const arm_biquad_casd_df1_inst_f16 * S,
+  const float16_t * pSrc,
+        float16_t * pDst,
+        uint32_t blockSize);
+
+#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE)
+  void arm_biquad_cascade_df1_mve_init_f16(
+      arm_biquad_casd_df1_inst_f16 * S,
+      uint8_t numStages,
+      const float16_t * pCoeffs, 
+      arm_biquad_mod_coef_f16 * pCoeffsMod, 
+      float16_t * pState);
+#endif
+
+  void arm_biquad_cascade_df1_init_f16(
+        arm_biquad_casd_df1_inst_f16 * S,
+        uint8_t numStages,
+  const float16_t * pCoeffs,
+        float16_t * pState);
+
+  /**
+   * @brief Instance structure for the floating-point transposed direct form II Biquad cascade filter.
+   */
+  typedef struct
+  {
+          uint8_t numStages;         /**< number of 2nd order stages in the filter.  Overall order is 2*numStages. */
+          float16_t *pState;         /**< points to the array of state coefficients.  The array is of length 2*numStages. */
+    const float16_t *pCoeffs;        /**< points to the array of coefficients.  The array is of length 5*numStages. */
+  } arm_biquad_cascade_df2T_instance_f16;
+
+  /**
+   * @brief Instance structure for the floating-point transposed direct form II Biquad cascade filter.
+   */
+  typedef struct
+  {
+          uint8_t numStages;         /**< number of 2nd order stages in the filter.  Overall order is 2*numStages. */
+          float16_t *pState;         /**< points to the array of state coefficients.  The array is of length 4*numStages. */
+    const float16_t *pCoeffs;        /**< points to the array of coefficients.  The array is of length 5*numStages. */
+  } arm_biquad_cascade_stereo_df2T_instance_f16;
+
+  /**
+   * @brief Processing function for the floating-point transposed direct form II Biquad cascade filter.
+   * @param[in]  S          points to an instance of the filter data structure.
+   * @param[in]  pSrc       points to the block of input data.
+   * @param[out] pDst       points to the block of output data
+   * @param[in]  blockSize  number of samples to process.
+   */
+  void arm_biquad_cascade_df2T_f16(
+  const arm_biquad_cascade_df2T_instance_f16 * S,
+  const float16_t * pSrc,
+        float16_t * pDst,
+        uint32_t blockSize);
+
+  /**
+   * @brief Processing function for the floating-point transposed direct form II Biquad cascade filter. 2 channels
+   * @param[in]  S          points to an instance of the filter data structure.
+   * @param[in]  pSrc       points to the block of input data.
+   * @param[out] pDst       points to the block of output data
+   * @param[in]  blockSize  number of samples to process.
+   */
+  void arm_biquad_cascade_stereo_df2T_f16(
+  const arm_biquad_cascade_stereo_df2T_instance_f16 * S,
+  const float16_t * pSrc,
+        float16_t * pDst,
+        uint32_t blockSize);
+
+  /**
+   * @brief  Initialization function for the floating-point transposed direct form II Biquad cascade filter.
+   * @param[in,out] S          points to an instance of the filter data structure.
+   * @param[in]     numStages  number of 2nd order stages in the filter.
+   * @param[in]     pCoeffs    points to the filter coefficients.
+   * @param[in]     pState     points to the state buffer.
+   */
+  void arm_biquad_cascade_df2T_init_f16(
+        arm_biquad_cascade_df2T_instance_f16 * S,
+        uint8_t numStages,
+  const float16_t * pCoeffs,
+        float16_t * pState);
+
+  /**
+   * @brief  Initialization function for the floating-point transposed direct form II Biquad cascade filter.
+   * @param[in,out] S          points to an instance of the filter data structure.
+   * @param[in]     numStages  number of 2nd order stages in the filter.
+   * @param[in]     pCoeffs    points to the filter coefficients.
+   * @param[in]     pState     points to the state buffer.
+   */
+  void arm_biquad_cascade_stereo_df2T_init_f16(
+        arm_biquad_cascade_stereo_df2T_instance_f16 * S,
+        uint8_t numStages,
+  const float16_t * pCoeffs,
+        float16_t * pState);
+
+  /**
+   * @brief Correlation of floating-point sequences.
+   * @param[in]  pSrcA    points to the first input sequence.
+   * @param[in]  srcALen  length of the first input sequence.
+   * @param[in]  pSrcB    points to the second input sequence.
+   * @param[in]  srcBLen  length of the second input sequence.
+   * @param[out] pDst     points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.
+   */
+  void arm_correlate_f16(
+  const float16_t * pSrcA,
+        uint32_t srcALen,
+  const float16_t * pSrcB,
+        uint32_t srcBLen,
+        float16_t * pDst);
+
+
+/**
+  @brief         Levinson Durbin
+  @param[in]     phi      autocovariance vector starting with lag 0 (length is nbCoefs + 1)
+  @param[out]    a        autoregressive coefficients
+  @param[out]    err      prediction error (variance)
+  @param[in]     nbCoefs  number of autoregressive coefficients
+  @return        none
+ */
+void arm_levinson_durbin_f16(const float16_t *phi,
+  float16_t *a, 
+  float16_t *err,
+  int nbCoefs);
+
+#endif /*defined(ARM_FLOAT16_SUPPORTED)*/
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _FILTERING_FUNCTIONS_F16_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/interpolation_functions.h

@@ -0,0 +1,319 @@
+/******************************************************************************
+ * @file     interpolation_functions.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _INTERPOLATION_FUNCTIONS_H_
+#define _INTERPOLATION_FUNCTIONS_H_
+
+#include "arm_math_types.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+
+/**
+ * @defgroup groupInterpolation Interpolation Functions
+ * These functions perform 1- and 2-dimensional interpolation of data.
+ * Linear interpolation is used for 1-dimensional data and
+ * bilinear interpolation is used for 2-dimensional data.
+ */
+
+
+  /**
+   * @brief Instance structure for the floating-point Linear Interpolate function.
+   */
+  typedef struct
+  {
+          uint32_t nValues;           /**< nValues */
+          float32_t x1;               /**< x1 */
+          float32_t xSpacing;         /**< xSpacing */
+          float32_t *pYData;          /**< pointer to the table of Y values */
+  } arm_linear_interp_instance_f32;
+
+  /**
+   * @brief Instance structure for the floating-point bilinear interpolation function.
+   */
+  typedef struct
+  {
+          uint16_t numRows;   /**< number of rows in the data table. */
+          uint16_t numCols;   /**< number of columns in the data table. */
+          float32_t *pData;   /**< points to the data table. */
+  } arm_bilinear_interp_instance_f32;
+
+   /**
+   * @brief Instance structure for the Q31 bilinear interpolation function.
+   */
+  typedef struct
+  {
+          uint16_t numRows;   /**< number of rows in the data table. */
+          uint16_t numCols;   /**< number of columns in the data table. */
+          q31_t *pData;       /**< points to the data table. */
+  } arm_bilinear_interp_instance_q31;
+
+   /**
+   * @brief Instance structure for the Q15 bilinear interpolation function.
+   */
+  typedef struct
+  {
+          uint16_t numRows;   /**< number of rows in the data table. */
+          uint16_t numCols;   /**< number of columns in the data table. */
+          q15_t *pData;       /**< points to the data table. */
+  } arm_bilinear_interp_instance_q15;
+
+   /**
+   * @brief Instance structure for the Q15 bilinear interpolation function.
+   */
+  typedef struct
+  {
+          uint16_t numRows;   /**< number of rows in the data table. */
+          uint16_t numCols;   /**< number of columns in the data table. */
+          q7_t *pData;        /**< points to the data table. */
+  } arm_bilinear_interp_instance_q7;
+
+
+  /**
+   * @brief Struct for specifying cubic spline type
+   */
+  typedef enum
+  {
+    ARM_SPLINE_NATURAL = 0,           /**< Natural spline */
+    ARM_SPLINE_PARABOLIC_RUNOUT = 1   /**< Parabolic runout spline */
+  } arm_spline_type;
+
+  /**
+   * @brief Instance structure for the floating-point cubic spline interpolation.
+   */
+  typedef struct
+  {
+    arm_spline_type type;      /**< Type (boundary conditions) */
+    const float32_t * x;       /**< x values */
+    const float32_t * y;       /**< y values */
+    uint32_t n_x;              /**< Number of known data points */
+    float32_t * coeffs;        /**< Coefficients buffer (b,c, and d) */
+  } arm_spline_instance_f32;
+
+
+
+
+  /**
+   * @ingroup groupInterpolation
+   */
+
+  /**
+   * @addtogroup SplineInterpolate
+   * @{
+   */
+
+  
+  /**
+   * @brief Processing function for the floating-point cubic spline interpolation.
+   * @param[in]  S          points to an instance of the floating-point spline structure.
+   * @param[in]  xq         points to the x values ot the interpolated data points.
+   * @param[out] pDst       points to the block of output data.
+   * @param[in]  blockSize  number of samples of output data.
+   */
+  void arm_spline_f32(
+        arm_spline_instance_f32 * S, 
+  const float32_t * xq,
+        float32_t * pDst,
+        uint32_t blockSize);
+
+  /**
+   * @brief Initialization function for the floating-point cubic spline interpolation.
+   * @param[in,out] S        points to an instance of the floating-point spline structure.
+   * @param[in]     type     type of cubic spline interpolation (boundary conditions)
+   * @param[in]     x        points to the x values of the known data points.
+   * @param[in]     y        points to the y values of the known data points.
+   * @param[in]     n        number of known data points.
+   * @param[in]     coeffs   coefficients array for b, c, and d
+   * @param[in]     tempBuffer   buffer array for internal computations
+   */
+  void arm_spline_init_f32(
+          arm_spline_instance_f32 * S,
+          arm_spline_type type,
+    const float32_t * x,
+    const float32_t * y,
+          uint32_t n, 
+          float32_t * coeffs,
+          float32_t * tempBuffer);
+
+
+  /**
+   * @} end of SplineInterpolate group
+   */
+
+
+  
+  /**
+   * @addtogroup LinearInterpolate
+   * @{
+   */
+
+    /**
+   * @brief  Process function for the floating-point Linear Interpolation Function.
+   * @param[in,out] S  is an instance of the floating-point Linear Interpolation structure
+   * @param[in]     x  input sample to process
+   * @return y processed output sample.
+   *
+   */
+  float32_t arm_linear_interp_f32(
+  arm_linear_interp_instance_f32 * S,
+  float32_t x);
+
+   /**
+   *
+   * @brief  Process function for the Q31 Linear Interpolation Function.
+   * @param[in] pYData   pointer to Q31 Linear Interpolation table
+   * @param[in] x        input sample to process
+   * @param[in] nValues  number of table values
+   * @return y processed output sample.
+   *
+   * \par
+   * Input sample <code>x</code> is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.
+   * This function can support maximum of table size 2^12.
+   *
+   */
+  q31_t arm_linear_interp_q31(
+  const q31_t * pYData,
+  q31_t x,
+  uint32_t nValues);
+
+  /**
+   *
+   * @brief  Process function for the Q15 Linear Interpolation Function.
+   * @param[in] pYData   pointer to Q15 Linear Interpolation table
+   * @param[in] x        input sample to process
+   * @param[in] nValues  number of table values
+   * @return y processed output sample.
+   *
+   * \par
+   * Input sample <code>x</code> is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.
+   * This function can support maximum of table size 2^12.
+   *
+   */
+  q15_t arm_linear_interp_q15(
+  const q15_t * pYData,
+  q31_t x,
+  uint32_t nValues);
+
+  /**
+   *
+   * @brief  Process function for the Q7 Linear Interpolation Function.
+   * @param[in] pYData   pointer to Q7 Linear Interpolation table
+   * @param[in] x        input sample to process
+   * @param[in] nValues  number of table values
+   * @return y processed output sample.
+   *
+   * \par
+   * Input sample <code>x</code> is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.
+   * This function can support maximum of table size 2^12.
+   */
+q7_t arm_linear_interp_q7(
+  const q7_t * pYData,
+  q31_t x,
+  uint32_t nValues);
+
+  /**
+   * @} end of LinearInterpolate group
+   */
+
+  
+
+
+  /**
+   * @ingroup groupInterpolation
+   */
+
+
+  /**
+   * @addtogroup BilinearInterpolate
+   * @{
+   */
+
+  /**
+  * @brief  Floating-point bilinear interpolation.
+  * @param[in,out] S  points to an instance of the interpolation structure.
+  * @param[in]     X  interpolation coordinate.
+  * @param[in]     Y  interpolation coordinate.
+  * @return out interpolated value.
+  */
+  float32_t arm_bilinear_interp_f32(
+  const arm_bilinear_interp_instance_f32 * S,
+  float32_t X,
+  float32_t Y);
+
+  /**
+  * @brief  Q31 bilinear interpolation.
+  * @param[in,out] S  points to an instance of the interpolation structure.
+  * @param[in]     X  interpolation coordinate in 12.20 format.
+  * @param[in]     Y  interpolation coordinate in 12.20 format.
+  * @return out interpolated value.
+  */
+  q31_t arm_bilinear_interp_q31(
+  arm_bilinear_interp_instance_q31 * S,
+  q31_t X,
+  q31_t Y);
+
+
+  /**
+  * @brief  Q15 bilinear interpolation.
+  * @param[in,out] S  points to an instance of the interpolation structure.
+  * @param[in]     X  interpolation coordinate in 12.20 format.
+  * @param[in]     Y  interpolation coordinate in 12.20 format.
+  * @return out interpolated value.
+  */
+  q15_t arm_bilinear_interp_q15(
+  arm_bilinear_interp_instance_q15 * S,
+  q31_t X,
+  q31_t Y);
+
+  /**
+  * @brief  Q7 bilinear interpolation.
+  * @param[in,out] S  points to an instance of the interpolation structure.
+  * @param[in]     X  interpolation coordinate in 12.20 format.
+  * @param[in]     Y  interpolation coordinate in 12.20 format.
+  * @return out interpolated value.
+  */
+  q7_t arm_bilinear_interp_q7(
+  arm_bilinear_interp_instance_q7 * S,
+  q31_t X,
+  q31_t Y);
+  /**
+   * @} end of BilinearInterpolate group
+   */
+
+
+
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _INTERPOLATION_FUNCTIONS_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/interpolation_functions_f16.h

@@ -0,0 +1,107 @@
+/******************************************************************************
+ * @file     interpolation_functions_f16.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _INTERPOLATION_FUNCTIONS_F16_H_
+#define _INTERPOLATION_FUNCTIONS_F16_H_
+
+#include "arm_math_types_f16.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+#if defined(ARM_FLOAT16_SUPPORTED)
+
+typedef struct
+{
+    uint32_t  nValues;        /**< nValues */
+    float16_t x1;             /**< x1 */
+    float16_t xSpacing;       /**< xSpacing */
+    float16_t *pYData;        /**< pointer to the table of Y values */
+} arm_linear_interp_instance_f16;
+
+/**
+ * @brief Instance structure for the floating-point bilinear interpolation function.
+ */
+typedef struct
+{
+    uint16_t  numRows;/**< number of rows in the data table. */
+    uint16_t  numCols;/**< number of columns in the data table. */
+    float16_t *pData; /**< points to the data table. */
+} arm_bilinear_interp_instance_f16;
+
+  /**
+   * @addtogroup LinearInterpolate
+   * @{
+   */
+
+    /**
+   * @brief  Process function for the floating-point Linear Interpolation Function.
+   * @param[in,out] S  is an instance of the floating-point Linear Interpolation structure
+   * @param[in]     x  input sample to process
+   * @return y processed output sample.
+   *
+   */
+  float16_t arm_linear_interp_f16(
+  arm_linear_interp_instance_f16 * S,
+  float16_t x);
+
+    /**
+   * @} end of LinearInterpolate group
+   */
+
+/**
+   * @addtogroup BilinearInterpolate
+   * @{
+   */
+
+  /**
+  * @brief  Floating-point bilinear interpolation.
+  * @param[in,out] S  points to an instance of the interpolation structure.
+  * @param[in]     X  interpolation coordinate.
+  * @param[in]     Y  interpolation coordinate.
+  * @return out interpolated value.
+  */
+  float16_t arm_bilinear_interp_f16(
+  const arm_bilinear_interp_instance_f16 * S,
+  float16_t X,
+  float16_t Y);
+
+
+  /**
+   * @} end of BilinearInterpolate group
+   */
+#endif /*defined(ARM_FLOAT16_SUPPORTED)*/
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _INTERPOLATION_FUNCTIONS_F16_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/matrix_functions.h

@@ -0,0 +1,757 @@
+/******************************************************************************
+ * @file     matrix_functions.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _MATRIX_FUNCTIONS_H_
+#define _MATRIX_FUNCTIONS_H_
+
+#include "arm_math_types.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * @defgroup groupMatrix Matrix Functions
+ *
+ * This set of functions provides basic matrix math operations.
+ * The functions operate on matrix data structures.  For example,
+ * the type
+ * definition for the floating-point matrix structure is shown
+ * below:
+ * <pre>
+ *     typedef struct
+ *     {
+ *       uint16_t numRows;     // number of rows of the matrix.
+ *       uint16_t numCols;     // number of columns of the matrix.
+ *       float32_t *pData;     // points to the data of the matrix.
+ *     } arm_matrix_instance_f32;
+ * </pre>
+ * There are similar definitions for Q15 and Q31 data types.
+ *
+ * The structure specifies the size of the matrix and then points to
+ * an array of data.  The array is of size <code>numRows X numCols</code>
+ * and the values are arranged in row order.  That is, the
+ * matrix element (i, j) is stored at:
+ * <pre>
+ *     pData[i*numCols + j]
+ * </pre>
+ *
+ * \par Init Functions
+ * There is an associated initialization function for each type of matrix
+ * data structure.
+ * The initialization function sets the values of the internal structure fields.
+ * Refer to \ref arm_mat_init_f32(), \ref arm_mat_init_q31() and \ref arm_mat_init_q15()
+ * for floating-point, Q31 and Q15 types,  respectively.
+ *
+ * \par
+ * Use of the initialization function is optional. However, if initialization function is used
+ * then the instance structure cannot be placed into a const data section.
+ * To place the instance structure in a const data
+ * section, manually initialize the data structure.  For example:
+ * <pre>
+ * <code>arm_matrix_instance_f32 S = {nRows, nColumns, pData};</code>
+ * <code>arm_matrix_instance_q31 S = {nRows, nColumns, pData};</code>
+ * <code>arm_matrix_instance_q15 S = {nRows, nColumns, pData};</code>
+ * </pre>
+ * where <code>nRows</code> specifies the number of rows, <code>nColumns</code>
+ * specifies the number of columns, and <code>pData</code> points to the
+ * data array.
+ *
+ * \par Size Checking
+ * By default all of the matrix functions perform size checking on the input and
+ * output matrices. For example, the matrix addition function verifies that the
+ * two input matrices and the output matrix all have the same number of rows and
+ * columns. If the size check fails the functions return:
+ * <pre>
+ *     ARM_MATH_SIZE_MISMATCH
+ * </pre>
+ * Otherwise the functions return
+ * <pre>
+ *     ARM_MATH_SUCCESS
+ * </pre>
+ * There is some overhead associated with this matrix size checking.
+ * The matrix size checking is enabled via the \#define
+ * <pre>
+ *     ARM_MATH_MATRIX_CHECK
+ * </pre>
+ * within the library project settings.  By default this macro is defined
+ * and size checking is enabled. By changing the project settings and
+ * undefining this macro size checking is eliminated and the functions
+ * run a bit faster. With size checking disabled the functions always
+ * return <code>ARM_MATH_SUCCESS</code>.
+ */
+
+  /**
+   * @brief Instance structure for the floating-point matrix structure.
+   */
+  typedef struct
+  {
+    uint16_t numRows;     /**< number of rows of the matrix.     */
+    uint16_t numCols;     /**< number of columns of the matrix.  */
+    float32_t *pData;     /**< points to the data of the matrix. */
+  } arm_matrix_instance_f32;
+ 
+ /**
+   * @brief Instance structure for the floating-point matrix structure.
+   */
+  typedef struct
+  {
+    uint16_t numRows;     /**< number of rows of the matrix.     */
+    uint16_t numCols;     /**< number of columns of the matrix.  */
+    float64_t *pData;     /**< points to the data of the matrix. */
+  } arm_matrix_instance_f64;
+
+ /**
+   * @brief Instance structure for the Q7 matrix structure.
+   */
+  typedef struct
+  {
+    uint16_t numRows;     /**< number of rows of the matrix.     */
+    uint16_t numCols;     /**< number of columns of the matrix.  */
+    q7_t *pData;         /**< points to the data of the matrix. */
+  } arm_matrix_instance_q7;
+
+  /**
+   * @brief Instance structure for the Q15 matrix structure.
+   */
+  typedef struct
+  {
+    uint16_t numRows;     /**< number of rows of the matrix.     */
+    uint16_t numCols;     /**< number of columns of the matrix.  */
+    q15_t *pData;         /**< points to the data of the matrix. */
+  } arm_matrix_instance_q15;
+
+  /**
+   * @brief Instance structure for the Q31 matrix structure.
+   */
+  typedef struct
+  {
+    uint16_t numRows;     /**< number of rows of the matrix.     */
+    uint16_t numCols;     /**< number of columns of the matrix.  */
+    q31_t *pData;         /**< points to the data of the matrix. */
+  } arm_matrix_instance_q31;
+
+  /**
+   * @brief Floating-point matrix addition.
+   * @param[in]  pSrcA  points to the first input matrix structure
+   * @param[in]  pSrcB  points to the second input matrix structure
+   * @param[out] pDst   points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_add_f32(
+  const arm_matrix_instance_f32 * pSrcA,
+  const arm_matrix_instance_f32 * pSrcB,
+        arm_matrix_instance_f32 * pDst);
+
+  /**
+   * @brief Q15 matrix addition.
+   * @param[in]   pSrcA  points to the first input matrix structure
+   * @param[in]   pSrcB  points to the second input matrix structure
+   * @param[out]  pDst   points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_add_q15(
+  const arm_matrix_instance_q15 * pSrcA,
+  const arm_matrix_instance_q15 * pSrcB,
+        arm_matrix_instance_q15 * pDst);
+
+  /**
+   * @brief Q31 matrix addition.
+   * @param[in]  pSrcA  points to the first input matrix structure
+   * @param[in]  pSrcB  points to the second input matrix structure
+   * @param[out] pDst   points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_add_q31(
+  const arm_matrix_instance_q31 * pSrcA,
+  const arm_matrix_instance_q31 * pSrcB,
+        arm_matrix_instance_q31 * pDst);
+
+  /**
+   * @brief Floating-point, complex, matrix multiplication.
+   * @param[in]  pSrcA  points to the first input matrix structure
+   * @param[in]  pSrcB  points to the second input matrix structure
+   * @param[out] pDst   points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_cmplx_mult_f32(
+  const arm_matrix_instance_f32 * pSrcA,
+  const arm_matrix_instance_f32 * pSrcB,
+        arm_matrix_instance_f32 * pDst);
+
+  /**
+   * @brief Q15, complex,  matrix multiplication.
+   * @param[in]  pSrcA  points to the first input matrix structure
+   * @param[in]  pSrcB  points to the second input matrix structure
+   * @param[out] pDst   points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_cmplx_mult_q15(
+  const arm_matrix_instance_q15 * pSrcA,
+  const arm_matrix_instance_q15 * pSrcB,
+        arm_matrix_instance_q15 * pDst,
+        q15_t * pScratch);
+
+  /**
+   * @brief Q31, complex, matrix multiplication.
+   * @param[in]  pSrcA  points to the first input matrix structure
+   * @param[in]  pSrcB  points to the second input matrix structure
+   * @param[out] pDst   points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_cmplx_mult_q31(
+  const arm_matrix_instance_q31 * pSrcA,
+  const arm_matrix_instance_q31 * pSrcB,
+        arm_matrix_instance_q31 * pDst);
+
+  /**
+   * @brief Floating-point matrix transpose.
+   * @param[in]  pSrc  points to the input matrix
+   * @param[out] pDst  points to the output matrix
+   * @return    The function returns either  <code>ARM_MATH_SIZE_MISMATCH</code>
+   * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_trans_f32(
+  const arm_matrix_instance_f32 * pSrc,
+        arm_matrix_instance_f32 * pDst);
+
+/**
+   * @brief Floating-point matrix transpose.
+   * @param[in]  pSrc  points to the input matrix
+   * @param[out] pDst  points to the output matrix
+   * @return    The function returns either  <code>ARM_MATH_SIZE_MISMATCH</code>
+   * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_trans_f64(
+  const arm_matrix_instance_f64 * pSrc,
+        arm_matrix_instance_f64 * pDst);
+
+  /**
+   * @brief Floating-point complex matrix transpose.
+   * @param[in]  pSrc  points to the input matrix
+   * @param[out] pDst  points to the output matrix
+   * @return    The function returns either  <code>ARM_MATH_SIZE_MISMATCH</code>
+   * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_cmplx_trans_f32(
+  const arm_matrix_instance_f32 * pSrc,
+  arm_matrix_instance_f32 * pDst);
+
+
+  /**
+   * @brief Q15 matrix transpose.
+   * @param[in]  pSrc  points to the input matrix
+   * @param[out] pDst  points to the output matrix
+   * @return    The function returns either  <code>ARM_MATH_SIZE_MISMATCH</code>
+   * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_trans_q15(
+  const arm_matrix_instance_q15 * pSrc,
+        arm_matrix_instance_q15 * pDst);
+
+  /**
+   * @brief Q15 complex matrix transpose.
+   * @param[in]  pSrc  points to the input matrix
+   * @param[out] pDst  points to the output matrix
+   * @return    The function returns either  <code>ARM_MATH_SIZE_MISMATCH</code>
+   * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_cmplx_trans_q15(
+  const arm_matrix_instance_q15 * pSrc,
+  arm_matrix_instance_q15 * pDst);
+
+  /**
+   * @brief Q7 matrix transpose.
+   * @param[in]  pSrc  points to the input matrix
+   * @param[out] pDst  points to the output matrix
+   * @return    The function returns either  <code>ARM_MATH_SIZE_MISMATCH</code>
+   * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_trans_q7(
+  const arm_matrix_instance_q7 * pSrc,
+        arm_matrix_instance_q7 * pDst);
+
+  /**
+   * @brief Q31 matrix transpose.
+   * @param[in]  pSrc  points to the input matrix
+   * @param[out] pDst  points to the output matrix
+   * @return    The function returns either  <code>ARM_MATH_SIZE_MISMATCH</code>
+   * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_trans_q31(
+  const arm_matrix_instance_q31 * pSrc,
+        arm_matrix_instance_q31 * pDst);
+
+  /**
+   * @brief Q31 complex matrix transpose.
+   * @param[in]  pSrc  points to the input matrix
+   * @param[out] pDst  points to the output matrix
+   * @return    The function returns either  <code>ARM_MATH_SIZE_MISMATCH</code>
+   * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_cmplx_trans_q31(
+  const arm_matrix_instance_q31 * pSrc,
+  arm_matrix_instance_q31 * pDst);
+
+  /**
+   * @brief Floating-point matrix multiplication
+   * @param[in]  pSrcA  points to the first input matrix structure
+   * @param[in]  pSrcB  points to the second input matrix structure
+   * @param[out] pDst   points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_mult_f32(
+  const arm_matrix_instance_f32 * pSrcA,
+  const arm_matrix_instance_f32 * pSrcB,
+        arm_matrix_instance_f32 * pDst);
+
+  /**
+   * @brief Floating-point matrix multiplication
+   * @param[in]  pSrcA  points to the first input matrix structure
+   * @param[in]  pSrcB  points to the second input matrix structure
+   * @param[out] pDst   points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_mult_f64(
+  const arm_matrix_instance_f64 * pSrcA,
+  const arm_matrix_instance_f64 * pSrcB,
+        arm_matrix_instance_f64 * pDst);
+
+  /**
+   * @brief Floating-point matrix and vector multiplication
+   * @param[in]  pSrcMat  points to the input matrix structure
+   * @param[in]  pVec     points to vector
+   * @param[out] pDst     points to output vector
+   */
+void arm_mat_vec_mult_f32(
+  const arm_matrix_instance_f32 *pSrcMat, 
+  const float32_t *pVec, 
+  float32_t *pDst);
+
+  /**
+   * @brief Q7 matrix multiplication
+   * @param[in]  pSrcA   points to the first input matrix structure
+   * @param[in]  pSrcB   points to the second input matrix structure
+   * @param[out] pDst    points to output matrix structure
+   * @param[in]  pState  points to the array for storing intermediate results
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_mult_q7(
+  const arm_matrix_instance_q7 * pSrcA,
+  const arm_matrix_instance_q7 * pSrcB,
+        arm_matrix_instance_q7 * pDst,
+        q7_t * pState);
+
+  /**
+   * @brief Q7 matrix and vector multiplication
+   * @param[in]  pSrcMat  points to the input matrix structure
+   * @param[in]  pVec     points to vector
+   * @param[out] pDst     points to output vector
+   */
+void arm_mat_vec_mult_q7(
+  const arm_matrix_instance_q7 *pSrcMat, 
+  const q7_t *pVec, 
+  q7_t *pDst);
+
+  /**
+   * @brief Q15 matrix multiplication
+   * @param[in]  pSrcA   points to the first input matrix structure
+   * @param[in]  pSrcB   points to the second input matrix structure
+   * @param[out] pDst    points to output matrix structure
+   * @param[in]  pState  points to the array for storing intermediate results
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_mult_q15(
+  const arm_matrix_instance_q15 * pSrcA,
+  const arm_matrix_instance_q15 * pSrcB,
+        arm_matrix_instance_q15 * pDst,
+        q15_t * pState);
+
+  /**
+   * @brief Q15 matrix and vector multiplication
+   * @param[in]  pSrcMat  points to the input matrix structure
+   * @param[in]  pVec     points to vector
+   * @param[out] pDst     points to output vector
+   */
+void arm_mat_vec_mult_q15(
+  const arm_matrix_instance_q15 *pSrcMat, 
+  const q15_t *pVec, 
+  q15_t *pDst);
+
+  /**
+   * @brief Q15 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4
+   * @param[in]  pSrcA   points to the first input matrix structure
+   * @param[in]  pSrcB   points to the second input matrix structure
+   * @param[out] pDst    points to output matrix structure
+   * @param[in]  pState  points to the array for storing intermediate results
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_mult_fast_q15(
+  const arm_matrix_instance_q15 * pSrcA,
+  const arm_matrix_instance_q15 * pSrcB,
+        arm_matrix_instance_q15 * pDst,
+        q15_t * pState);
+
+  /**
+   * @brief Q31 matrix multiplication
+   * @param[in]  pSrcA  points to the first input matrix structure
+   * @param[in]  pSrcB  points to the second input matrix structure
+   * @param[out] pDst   points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_mult_q31(
+  const arm_matrix_instance_q31 * pSrcA,
+  const arm_matrix_instance_q31 * pSrcB,
+        arm_matrix_instance_q31 * pDst);
+
+  /**
+   * @brief Q31 matrix multiplication
+   * @param[in]  pSrcA  points to the first input matrix structure
+   * @param[in]  pSrcB  points to the second input matrix structure
+   * @param[out] pDst   points to output matrix structure
+   * @param[in]  pState  points to the array for storing intermediate results
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_mult_opt_q31(
+  const arm_matrix_instance_q31 * pSrcA,
+  const arm_matrix_instance_q31 * pSrcB,
+        arm_matrix_instance_q31 * pDst,
+        q31_t *pState);
+
+  /**
+   * @brief Q31 matrix and vector multiplication
+   * @param[in]  pSrcMat  points to the input matrix structure
+   * @param[in]  pVec     points to vector
+   * @param[out] pDst     points to output vector
+   */
+void arm_mat_vec_mult_q31(
+  const arm_matrix_instance_q31 *pSrcMat, 
+  const q31_t *pVec, 
+  q31_t *pDst);
+
+  /**
+   * @brief Q31 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4
+   * @param[in]  pSrcA  points to the first input matrix structure
+   * @param[in]  pSrcB  points to the second input matrix structure
+   * @param[out] pDst   points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_mult_fast_q31(
+  const arm_matrix_instance_q31 * pSrcA,
+  const arm_matrix_instance_q31 * pSrcB,
+        arm_matrix_instance_q31 * pDst);
+
+  /**
+   * @brief Floating-point matrix subtraction
+   * @param[in]  pSrcA  points to the first input matrix structure
+   * @param[in]  pSrcB  points to the second input matrix structure
+   * @param[out] pDst   points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_sub_f32(
+  const arm_matrix_instance_f32 * pSrcA,
+  const arm_matrix_instance_f32 * pSrcB,
+        arm_matrix_instance_f32 * pDst);
+
+  /**
+   * @brief Floating-point matrix subtraction
+   * @param[in]  pSrcA  points to the first input matrix structure
+   * @param[in]  pSrcB  points to the second input matrix structure
+   * @param[out] pDst   points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_sub_f64(
+  const arm_matrix_instance_f64 * pSrcA,
+  const arm_matrix_instance_f64 * pSrcB,
+        arm_matrix_instance_f64 * pDst);
+
+  /**
+   * @brief Q15 matrix subtraction
+   * @param[in]  pSrcA  points to the first input matrix structure
+   * @param[in]  pSrcB  points to the second input matrix structure
+   * @param[out] pDst   points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_sub_q15(
+  const arm_matrix_instance_q15 * pSrcA,
+  const arm_matrix_instance_q15 * pSrcB,
+        arm_matrix_instance_q15 * pDst);
+
+  /**
+   * @brief Q31 matrix subtraction
+   * @param[in]  pSrcA  points to the first input matrix structure
+   * @param[in]  pSrcB  points to the second input matrix structure
+   * @param[out] pDst   points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_sub_q31(
+  const arm_matrix_instance_q31 * pSrcA,
+  const arm_matrix_instance_q31 * pSrcB,
+        arm_matrix_instance_q31 * pDst);
+
+  /**
+   * @brief Floating-point matrix scaling.
+   * @param[in]  pSrc   points to the input matrix
+   * @param[in]  scale  scale factor
+   * @param[out] pDst   points to the output matrix
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_scale_f32(
+  const arm_matrix_instance_f32 * pSrc,
+        float32_t scale,
+        arm_matrix_instance_f32 * pDst);
+
+  /**
+   * @brief Q15 matrix scaling.
+   * @param[in]  pSrc        points to input matrix
+   * @param[in]  scaleFract  fractional portion of the scale factor
+   * @param[in]  shift       number of bits to shift the result by
+   * @param[out] pDst        points to output matrix
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_scale_q15(
+  const arm_matrix_instance_q15 * pSrc,
+        q15_t scaleFract,
+        int32_t shift,
+        arm_matrix_instance_q15 * pDst);
+
+  /**
+   * @brief Q31 matrix scaling.
+   * @param[in]  pSrc        points to input matrix
+   * @param[in]  scaleFract  fractional portion of the scale factor
+   * @param[in]  shift       number of bits to shift the result by
+   * @param[out] pDst        points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_scale_q31(
+  const arm_matrix_instance_q31 * pSrc,
+        q31_t scaleFract,
+        int32_t shift,
+        arm_matrix_instance_q31 * pDst);
+
+  /**
+   * @brief  Q31 matrix initialization.
+   * @param[in,out] S         points to an instance of the floating-point matrix structure.
+   * @param[in]     nRows     number of rows in the matrix.
+   * @param[in]     nColumns  number of columns in the matrix.
+   * @param[in]     pData     points to the matrix data array.
+   */
+void arm_mat_init_q31(
+        arm_matrix_instance_q31 * S,
+        uint16_t nRows,
+        uint16_t nColumns,
+        q31_t * pData);
+
+  /**
+   * @brief  Q15 matrix initialization.
+   * @param[in,out] S         points to an instance of the floating-point matrix structure.
+   * @param[in]     nRows     number of rows in the matrix.
+   * @param[in]     nColumns  number of columns in the matrix.
+   * @param[in]     pData     points to the matrix data array.
+   */
+void arm_mat_init_q15(
+        arm_matrix_instance_q15 * S,
+        uint16_t nRows,
+        uint16_t nColumns,
+        q15_t * pData);
+
+  /**
+   * @brief  Floating-point matrix initialization.
+   * @param[in,out] S         points to an instance of the floating-point matrix structure.
+   * @param[in]     nRows     number of rows in the matrix.
+   * @param[in]     nColumns  number of columns in the matrix.
+   * @param[in]     pData     points to the matrix data array.
+   */
+void arm_mat_init_f32(
+        arm_matrix_instance_f32 * S,
+        uint16_t nRows,
+        uint16_t nColumns,
+        float32_t * pData);
+
+
+
+  /**
+   * @brief Floating-point matrix inverse.
+   * @param[in]  src   points to the instance of the input floating-point matrix structure.
+   * @param[out] dst   points to the instance of the output floating-point matrix structure.
+   * @return The function returns ARM_MATH_SIZE_MISMATCH, if the dimensions do not match.
+   * If the input matrix is singular (does not have an inverse), then the algorithm terminates and returns error status ARM_MATH_SINGULAR.
+   */
+  arm_status arm_mat_inverse_f32(
+  const arm_matrix_instance_f32 * src,
+  arm_matrix_instance_f32 * dst);
+
+
+  /**
+   * @brief Floating-point matrix inverse.
+   * @param[in]  src   points to the instance of the input floating-point matrix structure.
+   * @param[out] dst   points to the instance of the output floating-point matrix structure.
+   * @return The function returns ARM_MATH_SIZE_MISMATCH, if the dimensions do not match.
+   * If the input matrix is singular (does not have an inverse), then the algorithm terminates and returns error status ARM_MATH_SINGULAR.
+   */
+  arm_status arm_mat_inverse_f64(
+  const arm_matrix_instance_f64 * src,
+  arm_matrix_instance_f64 * dst);
+
+ /**
+   * @brief Floating-point Cholesky decomposition of Symmetric Positive Definite Matrix.
+   * @param[in]  src   points to the instance of the input floating-point matrix structure.
+   * @param[out] dst   points to the instance of the output floating-point matrix structure.
+   * @return The function returns ARM_MATH_SIZE_MISMATCH, if the dimensions do not match.
+   * If the input matrix does not have a decomposition, then the algorithm terminates and returns error status ARM_MATH_DECOMPOSITION_FAILURE.
+   * If the matrix is ill conditioned or only semi-definite, then it is better using the LDL^t decomposition.
+   * The decomposition is returning a lower triangular matrix.
+   */
+  arm_status arm_mat_cholesky_f64(
+  const arm_matrix_instance_f64 * src,
+  arm_matrix_instance_f64 * dst);
+
+ /**
+   * @brief Floating-point Cholesky decomposition of Symmetric Positive Definite Matrix.
+   * @param[in]  src   points to the instance of the input floating-point matrix structure.
+   * @param[out] dst   points to the instance of the output floating-point matrix structure.
+   * @return The function returns ARM_MATH_SIZE_MISMATCH, if the dimensions do not match.
+   * If the input matrix does not have a decomposition, then the algorithm terminates and returns error status ARM_MATH_DECOMPOSITION_FAILURE.
+   * If the matrix is ill conditioned or only semi-definite, then it is better using the LDL^t decomposition.
+   * The decomposition is returning a lower triangular matrix.
+   */
+  arm_status arm_mat_cholesky_f32(
+  const arm_matrix_instance_f32 * src,
+  arm_matrix_instance_f32 * dst);
+
+  /**
+   * @brief Solve UT . X = A where UT is an upper triangular matrix
+   * @param[in]  ut  The upper triangular matrix
+   * @param[in]  a  The matrix a
+   * @param[out] dst The solution X of UT . X = A
+   * @return The function returns ARM_MATH_SINGULAR, if the system can't be solved.
+  */
+  arm_status arm_mat_solve_upper_triangular_f32(
+  const arm_matrix_instance_f32 * ut,
+  const arm_matrix_instance_f32 * a,
+  arm_matrix_instance_f32 * dst);
+
+ /**
+   * @brief Solve LT . X = A where LT is a lower triangular matrix
+   * @param[in]  lt  The lower triangular matrix
+   * @param[in]  a  The matrix a
+   * @param[out] dst The solution X of LT . X = A
+   * @return The function returns ARM_MATH_SINGULAR, if the system can't be solved.
+   */
+  arm_status arm_mat_solve_lower_triangular_f32(
+  const arm_matrix_instance_f32 * lt,
+  const arm_matrix_instance_f32 * a,
+  arm_matrix_instance_f32 * dst);
+
+
+  /**
+   * @brief Solve UT . X = A where UT is an upper triangular matrix
+   * @param[in]  ut  The upper triangular matrix
+   * @param[in]  a  The matrix a
+   * @param[out] dst The solution X of UT . X = A
+   * @return The function returns ARM_MATH_SINGULAR, if the system can't be solved.
+  */
+  arm_status arm_mat_solve_upper_triangular_f64(
+  const arm_matrix_instance_f64 * ut,
+  const arm_matrix_instance_f64 * a,
+  arm_matrix_instance_f64 * dst);
+
+ /**
+   * @brief Solve LT . X = A where LT is a lower triangular matrix
+   * @param[in]  lt  The lower triangular matrix
+   * @param[in]  a  The matrix a
+   * @param[out] dst The solution X of LT . X = A
+   * @return The function returns ARM_MATH_SINGULAR, if the system can't be solved.
+   */
+  arm_status arm_mat_solve_lower_triangular_f64(
+  const arm_matrix_instance_f64 * lt,
+  const arm_matrix_instance_f64 * a,
+  arm_matrix_instance_f64 * dst);
+
+
+  /**
+   * @brief Floating-point LDL decomposition of Symmetric Positive Semi-Definite Matrix.
+   * @param[in]  src   points to the instance of the input floating-point matrix structure.
+   * @param[out] l   points to the instance of the output floating-point triangular matrix structure.
+   * @param[out] d   points to the instance of the output floating-point diagonal matrix structure.
+   * @param[out] p   points to the instance of the output floating-point permutation vector.
+   * @return The function returns ARM_MATH_SIZE_MISMATCH, if the dimensions do not match.
+   * If the input matrix does not have a decomposition, then the algorithm terminates and returns error status ARM_MATH_DECOMPOSITION_FAILURE.
+   * The decomposition is returning a lower triangular matrix.
+   */
+  arm_status arm_mat_ldlt_f32(
+  const arm_matrix_instance_f32 * src,
+  arm_matrix_instance_f32 * l,
+  arm_matrix_instance_f32 * d,
+  uint16_t * pp);
+
+ /**
+   * @brief Floating-point LDL decomposition of Symmetric Positive Semi-Definite Matrix.
+   * @param[in]  src   points to the instance of the input floating-point matrix structure.
+   * @param[out] l   points to the instance of the output floating-point triangular matrix structure.
+   * @param[out] d   points to the instance of the output floating-point diagonal matrix structure.
+   * @param[out] p   points to the instance of the output floating-point permutation vector.
+   * @return The function returns ARM_MATH_SIZE_MISMATCH, if the dimensions do not match.
+   * If the input matrix does not have a decomposition, then the algorithm terminates and returns error status ARM_MATH_DECOMPOSITION_FAILURE.
+   * The decomposition is returning a lower triangular matrix.
+   */
+  arm_status arm_mat_ldlt_f64(
+  const arm_matrix_instance_f64 * src,
+  arm_matrix_instance_f64 * l,
+  arm_matrix_instance_f64 * d,
+  uint16_t * pp);
+
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _MATRIX_FUNCTIONS_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/matrix_functions_f16.h

@@ -0,0 +1,221 @@
+/******************************************************************************
+ * @file     matrix_functions_f16.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _MATRIX_FUNCTIONS_F16_H_
+#define _MATRIX_FUNCTIONS_F16_H_
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+
+#include "arm_math_types_f16.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+    
+#if defined(ARM_FLOAT16_SUPPORTED)
+
+ /**
+   * @brief Instance structure for the floating-point matrix structure.
+   */
+  typedef struct
+  {
+    uint16_t numRows;     /**< number of rows of the matrix.     */
+    uint16_t numCols;     /**< number of columns of the matrix.  */
+    float16_t *pData;     /**< points to the data of the matrix. */
+  } arm_matrix_instance_f16;
+
+ /**
+   * @brief Floating-point matrix addition.
+   * @param[in]  pSrcA  points to the first input matrix structure
+   * @param[in]  pSrcB  points to the second input matrix structure
+   * @param[out] pDst   points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_add_f16(
+  const arm_matrix_instance_f16 * pSrcA,
+  const arm_matrix_instance_f16 * pSrcB,
+        arm_matrix_instance_f16 * pDst);
+
+  /**
+   * @brief Floating-point, complex, matrix multiplication.
+   * @param[in]  pSrcA  points to the first input matrix structure
+   * @param[in]  pSrcB  points to the second input matrix structure
+   * @param[out] pDst   points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_cmplx_mult_f16(
+  const arm_matrix_instance_f16 * pSrcA,
+  const arm_matrix_instance_f16 * pSrcB,
+        arm_matrix_instance_f16 * pDst);
+
+  /**
+   * @brief Floating-point matrix transpose.
+   * @param[in]  pSrc  points to the input matrix
+   * @param[out] pDst  points to the output matrix
+   * @return    The function returns either  <code>ARM_MATH_SIZE_MISMATCH</code>
+   * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_trans_f16(
+  const arm_matrix_instance_f16 * pSrc,
+        arm_matrix_instance_f16 * pDst);
+
+  /**
+   * @brief Floating-point complex matrix transpose.
+   * @param[in]  pSrc  points to the input matrix
+   * @param[out] pDst  points to the output matrix
+   * @return    The function returns either  <code>ARM_MATH_SIZE_MISMATCH</code>
+   * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_cmplx_trans_f16(
+  const arm_matrix_instance_f16 * pSrc,
+  arm_matrix_instance_f16 * pDst);
+
+  /**
+   * @brief Floating-point matrix multiplication
+   * @param[in]  pSrcA  points to the first input matrix structure
+   * @param[in]  pSrcB  points to the second input matrix structure
+   * @param[out] pDst   points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_mult_f16(
+  const arm_matrix_instance_f16 * pSrcA,
+  const arm_matrix_instance_f16 * pSrcB,
+        arm_matrix_instance_f16 * pDst);
+  /**
+   * @brief Floating-point matrix and vector multiplication
+   * @param[in]  pSrcMat  points to the input matrix structure
+   * @param[in]  pVec     points to vector
+   * @param[out] pDst     points to output vector
+   */
+void arm_mat_vec_mult_f16(
+  const arm_matrix_instance_f16 *pSrcMat, 
+  const float16_t *pVec, 
+  float16_t *pDst);
+
+  /**
+   * @brief Floating-point matrix subtraction
+   * @param[in]  pSrcA  points to the first input matrix structure
+   * @param[in]  pSrcB  points to the second input matrix structure
+   * @param[out] pDst   points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_sub_f16(
+  const arm_matrix_instance_f16 * pSrcA,
+  const arm_matrix_instance_f16 * pSrcB,
+        arm_matrix_instance_f16 * pDst);
+
+  /**
+   * @brief Floating-point matrix scaling.
+   * @param[in]  pSrc   points to the input matrix
+   * @param[in]  scale  scale factor
+   * @param[out] pDst   points to the output matrix
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+arm_status arm_mat_scale_f16(
+  const arm_matrix_instance_f16 * pSrc,
+        float16_t scale,
+        arm_matrix_instance_f16 * pDst);
+
+  /**
+   * @brief  Floating-point matrix initialization.
+   * @param[in,out] S         points to an instance of the floating-point matrix structure.
+   * @param[in]     nRows     number of rows in the matrix.
+   * @param[in]     nColumns  number of columns in the matrix.
+   * @param[in]     pData     points to the matrix data array.
+   */
+void arm_mat_init_f16(
+        arm_matrix_instance_f16 * S,
+        uint16_t nRows,
+        uint16_t nColumns,
+        float16_t * pData);
+
+
+  /**
+   * @brief Floating-point matrix inverse.
+   * @param[in]  src   points to the instance of the input floating-point matrix structure.
+   * @param[out] dst   points to the instance of the output floating-point matrix structure.
+   * @return The function returns ARM_MATH_SIZE_MISMATCH, if the dimensions do not match.
+   * If the input matrix is singular (does not have an inverse), then the algorithm terminates and returns error status ARM_MATH_SINGULAR.
+   */
+  arm_status arm_mat_inverse_f16(
+  const arm_matrix_instance_f16 * src,
+  arm_matrix_instance_f16 * dst);
+
+
+ /**
+   * @brief Floating-point Cholesky decomposition of Symmetric Positive Definite Matrix.
+   * @param[in]  src   points to the instance of the input floating-point matrix structure.
+   * @param[out] dst   points to the instance of the output floating-point matrix structure.
+   * @return The function returns ARM_MATH_SIZE_MISMATCH, if the dimensions do not match.
+   * If the input matrix does not have a decomposition, then the algorithm terminates and returns error status ARM_MATH_DECOMPOSITION_FAILURE.
+   * If the matrix is ill conditioned or only semi-definite, then it is better using the LDL^t decomposition.
+   * The decomposition is returning a lower triangular matrix.
+   */
+  arm_status arm_mat_cholesky_f16(
+  const arm_matrix_instance_f16 * src,
+  arm_matrix_instance_f16 * dst);
+
+ /**
+   * @brief Solve UT . X = A where UT is an upper triangular matrix
+   * @param[in]  ut  The upper triangular matrix
+   * @param[in]  a  The matrix a
+   * @param[out] dst The solution X of UT . X = A
+   * @return The function returns ARM_MATH_SINGULAR, if the system can't be solved.
+  */
+  arm_status arm_mat_solve_upper_triangular_f16(
+  const arm_matrix_instance_f16 * ut,
+  const arm_matrix_instance_f16 * a,
+  arm_matrix_instance_f16 * dst);
+
+ /**
+   * @brief Solve LT . X = A where LT is a lower triangular matrix
+   * @param[in]  lt  The lower triangular matrix
+   * @param[in]  a  The matrix a
+   * @param[out] dst The solution X of LT . X = A
+   * @return The function returns ARM_MATH_SINGULAR, if the system can't be solved.
+   */
+  arm_status arm_mat_solve_lower_triangular_f16(
+  const arm_matrix_instance_f16 * lt,
+  const arm_matrix_instance_f16 * a,
+  arm_matrix_instance_f16 * dst);
+
+
+
+#endif /*defined(ARM_FLOAT16_SUPPORTED)*/
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _MATRIX_FUNCTIONS_F16_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/none.h

@@ -0,0 +1,576 @@
+/******************************************************************************
+ * @file     none.h
+ * @brief    Intrinsincs when no DSP extension available
+ * @version  V1.9.0
+ * @date     20. July 2020
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+/*
+
+Definitions in this file are allowing to reuse some versions of the
+CMSIS-DSP to build on a core (M0 for instance) or a host where
+DSP extension are not available.
+
+Ideally a pure C version should have been used instead.
+But those are not always available or use a restricted set
+of intrinsics.
+
+*/
+ 
+#ifndef _NONE_H_
+#define _NONE_H_
+
+#include "arm_math_types.h"
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+ 
+
+/*
+
+Normally those kind of definitions are in a compiler file
+in Core or Core_A.
+
+But for MSVC compiler it is a bit special. The goal is very specific
+to CMSIS-DSP and only to allow the use of this library from other
+systems like Python or Matlab.
+
+MSVC is not going to be used to cross-compile to ARM. So, having a MSVC
+compiler file in Core or Core_A would not make sense.
+
+*/
+#if defined ( _MSC_VER ) || defined(__GNUC_PYTHON__) || defined(__APPLE_CC__)
+    __STATIC_FORCEINLINE uint8_t __CLZ(uint32_t data)
+    {
+      if (data == 0U) { return 32U; }
+
+      uint32_t count = 0U;
+      uint32_t mask = 0x80000000U;
+
+      while ((data & mask) == 0U)
+      {
+        count += 1U;
+        mask = mask >> 1U;
+      }
+      return count;
+    }
+
+  __STATIC_FORCEINLINE int32_t __SSAT(int32_t val, uint32_t sat)
+  {
+    if ((sat >= 1U) && (sat <= 32U))
+    {
+      const int32_t max = (int32_t)((1U << (sat - 1U)) - 1U);
+      const int32_t min = -1 - max ;
+      if (val > max)
+      {
+        return max;
+      }
+      else if (val < min)
+      {
+        return min;
+      }
+    }
+    return val;
+  }
+
+  __STATIC_FORCEINLINE uint32_t __USAT(int32_t val, uint32_t sat)
+  {
+    if (sat <= 31U)
+    {
+      const uint32_t max = ((1U << sat) - 1U);
+      if (val > (int32_t)max)
+      {
+        return max;
+      }
+      else if (val < 0)
+      {
+        return 0U;
+      }
+    }
+    return (uint32_t)val;
+  }
+
+ /**
+  \brief   Rotate Right in unsigned value (32 bit)
+  \details Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.
+  \param [in]    op1  Value to rotate
+  \param [in]    op2  Number of Bits to rotate
+  \return               Rotated value
+ */
+__STATIC_FORCEINLINE uint32_t __ROR(uint32_t op1, uint32_t op2)
+{
+  op2 %= 32U;
+  if (op2 == 0U)
+  {
+    return op1;
+  }
+  return (op1 >> op2) | (op1 << (32U - op2));
+}
+
+
+#endif
+
+/**
+   * @brief Clips Q63 to Q31 values.
+   */
+  __STATIC_FORCEINLINE q31_t clip_q63_to_q31(
+  q63_t x)
+  {
+    return ((q31_t) (x >> 32) != ((q31_t) x >> 31)) ?
+      ((0x7FFFFFFF ^ ((q31_t) (x >> 63)))) : (q31_t) x;
+  }
+
+  /**
+   * @brief Clips Q63 to Q15 values.
+   */
+  __STATIC_FORCEINLINE q15_t clip_q63_to_q15(
+  q63_t x)
+  {
+    return ((q31_t) (x >> 32) != ((q31_t) x >> 31)) ?
+      ((0x7FFF ^ ((q15_t) (x >> 63)))) : (q15_t) (x >> 15);
+  }
+
+  /**
+   * @brief Clips Q31 to Q7 values.
+   */
+  __STATIC_FORCEINLINE q7_t clip_q31_to_q7(
+  q31_t x)
+  {
+    return ((q31_t) (x >> 24) != ((q31_t) x >> 23)) ?
+      ((0x7F ^ ((q7_t) (x >> 31)))) : (q7_t) x;
+  }
+
+  /**
+   * @brief Clips Q31 to Q15 values.
+   */
+  __STATIC_FORCEINLINE q15_t clip_q31_to_q15(
+  q31_t x)
+  {
+    return ((q31_t) (x >> 16) != ((q31_t) x >> 15)) ?
+      ((0x7FFF ^ ((q15_t) (x >> 31)))) : (q15_t) x;
+  }
+
+  /**
+   * @brief Multiplies 32 X 64 and returns 32 bit result in 2.30 format.
+   */
+  __STATIC_FORCEINLINE q63_t mult32x64(
+  q63_t x,
+  q31_t y)
+  {
+    return ((((q63_t) (x & 0x00000000FFFFFFFF) * y) >> 32) +
+            (((q63_t) (x >> 32)                * y)      )  );
+  }
+
+/* SMMLAR */
+#define multAcc_32x32_keep32_R(a, x, y) \
+    a = (q31_t) (((((q63_t) a) << 32) + ((q63_t) x * y) + 0x80000000LL ) >> 32)
+
+/* SMMLSR */
+#define multSub_32x32_keep32_R(a, x, y) \
+    a = (q31_t) (((((q63_t) a) << 32) - ((q63_t) x * y) + 0x80000000LL ) >> 32)
+
+/* SMMULR */
+#define mult_32x32_keep32_R(a, x, y) \
+    a = (q31_t) (((q63_t) x * y + 0x80000000LL ) >> 32)
+
+/* SMMLA */
+#define multAcc_32x32_keep32(a, x, y) \
+    a += (q31_t) (((q63_t) x * y) >> 32)
+
+/* SMMLS */
+#define multSub_32x32_keep32(a, x, y) \
+    a -= (q31_t) (((q63_t) x * y) >> 32)
+
+/* SMMUL */
+#define mult_32x32_keep32(a, x, y) \
+    a = (q31_t) (((q63_t) x * y ) >> 32)
+
+#ifndef ARM_MATH_DSP
+  /**
+   * @brief definition to pack two 16 bit values.
+   */
+  #define __PKHBT(ARG1, ARG2, ARG3) ( (((int32_t)(ARG1) <<    0) & (int32_t)0x0000FFFF) | \
+                                      (((int32_t)(ARG2) << ARG3) & (int32_t)0xFFFF0000)  )
+  #define __PKHTB(ARG1, ARG2, ARG3) ( (((int32_t)(ARG1) <<    0) & (int32_t)0xFFFF0000) | \
+                                      (((int32_t)(ARG2) >> ARG3) & (int32_t)0x0000FFFF)  )
+#endif
+
+   /**
+   * @brief definition to pack four 8 bit values.
+   */
+#ifndef ARM_MATH_BIG_ENDIAN
+  #define __PACKq7(v0,v1,v2,v3) ( (((int32_t)(v0) <<  0) & (int32_t)0x000000FF) | \
+                                  (((int32_t)(v1) <<  8) & (int32_t)0x0000FF00) | \
+                                  (((int32_t)(v2) << 16) & (int32_t)0x00FF0000) | \
+                                  (((int32_t)(v3) << 24) & (int32_t)0xFF000000)  )
+#else
+  #define __PACKq7(v0,v1,v2,v3) ( (((int32_t)(v3) <<  0) & (int32_t)0x000000FF) | \
+                                  (((int32_t)(v2) <<  8) & (int32_t)0x0000FF00) | \
+                                  (((int32_t)(v1) << 16) & (int32_t)0x00FF0000) | \
+                                  (((int32_t)(v0) << 24) & (int32_t)0xFF000000)  )
+#endif
+
+
+ 
+
+/*
+ * @brief C custom defined intrinsic functions
+ */
+#if !defined (ARM_MATH_DSP)
+
+
+  /*
+   * @brief C custom defined QADD8
+   */
+  __STATIC_FORCEINLINE uint32_t __QADD8(
+  uint32_t x,
+  uint32_t y)
+  {
+    q31_t r, s, t, u;
+
+    r = __SSAT(((((q31_t)x << 24) >> 24) + (((q31_t)y << 24) >> 24)), 8) & (int32_t)0x000000FF;
+    s = __SSAT(((((q31_t)x << 16) >> 24) + (((q31_t)y << 16) >> 24)), 8) & (int32_t)0x000000FF;
+    t = __SSAT(((((q31_t)x <<  8) >> 24) + (((q31_t)y <<  8) >> 24)), 8) & (int32_t)0x000000FF;
+    u = __SSAT(((((q31_t)x      ) >> 24) + (((q31_t)y      ) >> 24)), 8) & (int32_t)0x000000FF;
+
+    return ((uint32_t)((u << 24) | (t << 16) | (s <<  8) | (r      )));
+  }
+
+
+  /*
+   * @brief C custom defined QSUB8
+   */
+  __STATIC_FORCEINLINE uint32_t __QSUB8(
+  uint32_t x,
+  uint32_t y)
+  {
+    q31_t r, s, t, u;
+
+    r = __SSAT(((((q31_t)x << 24) >> 24) - (((q31_t)y << 24) >> 24)), 8) & (int32_t)0x000000FF;
+    s = __SSAT(((((q31_t)x << 16) >> 24) - (((q31_t)y << 16) >> 24)), 8) & (int32_t)0x000000FF;
+    t = __SSAT(((((q31_t)x <<  8) >> 24) - (((q31_t)y <<  8) >> 24)), 8) & (int32_t)0x000000FF;
+    u = __SSAT(((((q31_t)x      ) >> 24) - (((q31_t)y      ) >> 24)), 8) & (int32_t)0x000000FF;
+
+    return ((uint32_t)((u << 24) | (t << 16) | (s <<  8) | (r      )));
+  }
+
+
+  /*
+   * @brief C custom defined QADD16
+   */
+  __STATIC_FORCEINLINE uint32_t __QADD16(
+  uint32_t x,
+  uint32_t y)
+  {
+/*  q31_t r,     s;  without initialisation 'arm_offset_q15 test' fails  but 'intrinsic' tests pass! for armCC */
+    q31_t r = 0, s = 0;
+
+    r = __SSAT(((((q31_t)x << 16) >> 16) + (((q31_t)y << 16) >> 16)), 16) & (int32_t)0x0000FFFF;
+    s = __SSAT(((((q31_t)x      ) >> 16) + (((q31_t)y      ) >> 16)), 16) & (int32_t)0x0000FFFF;
+
+    return ((uint32_t)((s << 16) | (r      )));
+  }
+
+
+  /*
+   * @brief C custom defined SHADD16
+   */
+  __STATIC_FORCEINLINE uint32_t __SHADD16(
+  uint32_t x,
+  uint32_t y)
+  {
+    q31_t r, s;
+
+    r = (((((q31_t)x << 16) >> 16) + (((q31_t)y << 16) >> 16)) >> 1) & (int32_t)0x0000FFFF;
+    s = (((((q31_t)x      ) >> 16) + (((q31_t)y      ) >> 16)) >> 1) & (int32_t)0x0000FFFF;
+
+    return ((uint32_t)((s << 16) | (r      )));
+  }
+
+
+  /*
+   * @brief C custom defined QSUB16
+   */
+  __STATIC_FORCEINLINE uint32_t __QSUB16(
+  uint32_t x,
+  uint32_t y)
+  {
+    q31_t r, s;
+
+    r = __SSAT(((((q31_t)x << 16) >> 16) - (((q31_t)y << 16) >> 16)), 16) & (int32_t)0x0000FFFF;
+    s = __SSAT(((((q31_t)x      ) >> 16) - (((q31_t)y      ) >> 16)), 16) & (int32_t)0x0000FFFF;
+
+    return ((uint32_t)((s << 16) | (r      )));
+  }
+
+
+  /*
+   * @brief C custom defined SHSUB16
+   */
+  __STATIC_FORCEINLINE uint32_t __SHSUB16(
+  uint32_t x,
+  uint32_t y)
+  {
+    q31_t r, s;
+
+    r = (((((q31_t)x << 16) >> 16) - (((q31_t)y << 16) >> 16)) >> 1) & (int32_t)0x0000FFFF;
+    s = (((((q31_t)x      ) >> 16) - (((q31_t)y      ) >> 16)) >> 1) & (int32_t)0x0000FFFF;
+
+    return ((uint32_t)((s << 16) | (r      )));
+  }
+
+
+  /*
+   * @brief C custom defined QASX
+   */
+  __STATIC_FORCEINLINE uint32_t __QASX(
+  uint32_t x,
+  uint32_t y)
+  {
+    q31_t r, s;
+
+    r = __SSAT(((((q31_t)x << 16) >> 16) - (((q31_t)y      ) >> 16)), 16) & (int32_t)0x0000FFFF;
+    s = __SSAT(((((q31_t)x      ) >> 16) + (((q31_t)y << 16) >> 16)), 16) & (int32_t)0x0000FFFF;
+
+    return ((uint32_t)((s << 16) | (r      )));
+  }
+
+
+  /*
+   * @brief C custom defined SHASX
+   */
+  __STATIC_FORCEINLINE uint32_t __SHASX(
+  uint32_t x,
+  uint32_t y)
+  {
+    q31_t r, s;
+
+    r = (((((q31_t)x << 16) >> 16) - (((q31_t)y      ) >> 16)) >> 1) & (int32_t)0x0000FFFF;
+    s = (((((q31_t)x      ) >> 16) + (((q31_t)y << 16) >> 16)) >> 1) & (int32_t)0x0000FFFF;
+
+    return ((uint32_t)((s << 16) | (r      )));
+  }
+
+
+  /*
+   * @brief C custom defined QSAX
+   */
+  __STATIC_FORCEINLINE uint32_t __QSAX(
+  uint32_t x,
+  uint32_t y)
+  {
+    q31_t r, s;
+
+    r = __SSAT(((((q31_t)x << 16) >> 16) + (((q31_t)y      ) >> 16)), 16) & (int32_t)0x0000FFFF;
+    s = __SSAT(((((q31_t)x      ) >> 16) - (((q31_t)y << 16) >> 16)), 16) & (int32_t)0x0000FFFF;
+
+    return ((uint32_t)((s << 16) | (r      )));
+  }
+
+
+  /*
+   * @brief C custom defined SHSAX
+   */
+  __STATIC_FORCEINLINE uint32_t __SHSAX(
+  uint32_t x,
+  uint32_t y)
+  {
+    q31_t r, s;
+
+    r = (((((q31_t)x << 16) >> 16) + (((q31_t)y      ) >> 16)) >> 1) & (int32_t)0x0000FFFF;
+    s = (((((q31_t)x      ) >> 16) - (((q31_t)y << 16) >> 16)) >> 1) & (int32_t)0x0000FFFF;
+
+    return ((uint32_t)((s << 16) | (r      )));
+  }
+
+
+  /*
+   * @brief C custom defined SMUSDX
+   */
+  __STATIC_FORCEINLINE uint32_t __SMUSDX(
+  uint32_t x,
+  uint32_t y)
+  {
+    return ((uint32_t)(((((q31_t)x << 16) >> 16) * (((q31_t)y      ) >> 16)) -
+                       ((((q31_t)x      ) >> 16) * (((q31_t)y << 16) >> 16))   ));
+  }
+
+  /*
+   * @brief C custom defined SMUADX
+   */
+  __STATIC_FORCEINLINE uint32_t __SMUADX(
+  uint32_t x,
+  uint32_t y)
+  {
+    return ((uint32_t)(((((q31_t)x << 16) >> 16) * (((q31_t)y      ) >> 16)) +
+                       ((((q31_t)x      ) >> 16) * (((q31_t)y << 16) >> 16))   ));
+  }
+
+
+  /*
+   * @brief C custom defined QADD
+   */
+  __STATIC_FORCEINLINE int32_t __QADD(
+  int32_t x,
+  int32_t y)
+  {
+    return ((int32_t)(clip_q63_to_q31((q63_t)x + (q31_t)y)));
+  }
+
+
+  /*
+   * @brief C custom defined QSUB
+   */
+  __STATIC_FORCEINLINE int32_t __QSUB(
+  int32_t x,
+  int32_t y)
+  {
+    return ((int32_t)(clip_q63_to_q31((q63_t)x - (q31_t)y)));
+  }
+
+
+  /*
+   * @brief C custom defined SMLAD
+   */
+  __STATIC_FORCEINLINE uint32_t __SMLAD(
+  uint32_t x,
+  uint32_t y,
+  uint32_t sum)
+  {
+    return ((uint32_t)(((((q31_t)x << 16) >> 16) * (((q31_t)y << 16) >> 16)) +
+                       ((((q31_t)x      ) >> 16) * (((q31_t)y      ) >> 16)) +
+                       ( ((q31_t)sum    )                                  )   ));
+  }
+
+
+  /*
+   * @brief C custom defined SMLADX
+   */
+  __STATIC_FORCEINLINE uint32_t __SMLADX(
+  uint32_t x,
+  uint32_t y,
+  uint32_t sum)
+  {
+    return ((uint32_t)(((((q31_t)x << 16) >> 16) * (((q31_t)y      ) >> 16)) +
+                       ((((q31_t)x      ) >> 16) * (((q31_t)y << 16) >> 16)) +
+                       ( ((q31_t)sum    )                                  )   ));
+  }
+
+
+  /*
+   * @brief C custom defined SMLSDX
+   */
+  __STATIC_FORCEINLINE uint32_t __SMLSDX(
+  uint32_t x,
+  uint32_t y,
+  uint32_t sum)
+  {
+    return ((uint32_t)(((((q31_t)x << 16) >> 16) * (((q31_t)y      ) >> 16)) -
+                       ((((q31_t)x      ) >> 16) * (((q31_t)y << 16) >> 16)) +
+                       ( ((q31_t)sum    )                                  )   ));
+  }
+
+
+  /*
+   * @brief C custom defined SMLALD
+   */
+  __STATIC_FORCEINLINE uint64_t __SMLALD(
+  uint32_t x,
+  uint32_t y,
+  uint64_t sum)
+  {
+/*  return (sum + ((q15_t) (x >> 16) * (q15_t) (y >> 16)) + ((q15_t) x * (q15_t) y)); */
+    return ((uint64_t)(((((q31_t)x << 16) >> 16) * (((q31_t)y << 16) >> 16)) +
+                       ((((q31_t)x      ) >> 16) * (((q31_t)y      ) >> 16)) +
+                       ( ((q63_t)sum    )                                  )   ));
+  }
+
+
+  /*
+   * @brief C custom defined SMLALDX
+   */
+  __STATIC_FORCEINLINE uint64_t __SMLALDX(
+  uint32_t x,
+  uint32_t y,
+  uint64_t sum)
+  {
+/*  return (sum + ((q15_t) (x >> 16) * (q15_t) y)) + ((q15_t) x * (q15_t) (y >> 16)); */
+    return ((uint64_t)(((((q31_t)x << 16) >> 16) * (((q31_t)y      ) >> 16)) +
+                       ((((q31_t)x      ) >> 16) * (((q31_t)y << 16) >> 16)) +
+                       ( ((q63_t)sum    )                                  )   ));
+  }
+
+
+  /*
+   * @brief C custom defined SMUAD
+   */
+  __STATIC_FORCEINLINE uint32_t __SMUAD(
+  uint32_t x,
+  uint32_t y)
+  {
+    return ((uint32_t)(((((q31_t)x << 16) >> 16) * (((q31_t)y << 16) >> 16)) +
+                       ((((q31_t)x      ) >> 16) * (((q31_t)y      ) >> 16))   ));
+  }
+
+
+  /*
+   * @brief C custom defined SMUSD
+   */
+  __STATIC_FORCEINLINE uint32_t __SMUSD(
+  uint32_t x,
+  uint32_t y)
+  {
+    return ((uint32_t)(((((q31_t)x << 16) >> 16) * (((q31_t)y << 16) >> 16)) -
+                       ((((q31_t)x      ) >> 16) * (((q31_t)y      ) >> 16))   ));
+  }
+
+
+  /*
+   * @brief C custom defined SXTB16
+   */
+  __STATIC_FORCEINLINE uint32_t __SXTB16(
+  uint32_t x)
+  {
+    return ((uint32_t)(((((q31_t)x << 24) >> 24) & (q31_t)0x0000FFFF) |
+                       ((((q31_t)x <<  8) >>  8) & (q31_t)0xFFFF0000)  ));
+  }
+
+  /*
+   * @brief C custom defined SMMLA
+   */
+  __STATIC_FORCEINLINE int32_t __SMMLA(
+  int32_t x,
+  int32_t y,
+  int32_t sum)
+  {
+    return (sum + (int32_t) (((int64_t) x * y) >> 32));
+  }
+
+#endif /* !defined (ARM_MATH_DSP) */
+
+
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _TRANSFORM_FUNCTIONS_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/quaternion_math_functions.h

@@ -0,0 +1,159 @@
+/******************************************************************************
+ * @file     quaternion_math_functions.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ *
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2021 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _QUATERNION_MATH_FUNCTIONS_H_
+#define _QUATERNION_MATH_FUNCTIONS_H_
+
+#include "arm_math_types.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * @defgroup groupQuaternionMath Quaternion Math Functions
+ * Functions to operates on quaternions and convert between a
+ * rotation and quaternion representation.
+ */
+
+
+/**
+  @brief         Floating-point quaternion Norm.
+  @param[in]     pInputQuaternions       points to the input vector of quaternions
+  @param[out]    pNorms                  points to the output vector of norms
+  @param[in]     nbQuaternions           number of quaternions in each vector
+  @return        none
+ */
+
+
+
+void arm_quaternion_norm_f32(const float32_t *pInputQuaternions, 
+    float32_t *pNorms,
+    uint32_t nbQuaternions);
+
+
+/**
+  @brief         Floating-point quaternion inverse.
+  @param[in]     pInputQuaternions            points to the input vector of quaternions
+  @param[out]    pInverseQuaternions          points to the output vector of inverse quaternions
+  @param[in]     nbQuaternions                number of quaternions in each vector
+  @return        none
+ */
+
+void arm_quaternion_inverse_f32(const float32_t *pInputQuaternions, 
+    float32_t *pInverseQuaternions, 
+    uint32_t nbQuaternions);
+
+/**
+  @brief         Floating-point quaternion conjugates.
+  @param[in]     pInputQuaternions            points to the input vector of quaternions
+  @param[out]    pConjugateQuaternions        points to the output vector of conjugate quaternions
+  @param[in]     nbQuaternions                number of quaternions in each vector
+  @return        none
+ */
+void arm_quaternion_conjugate_f32(const float32_t *inputQuaternions, 
+    float32_t *pConjugateQuaternions, 
+    uint32_t nbQuaternions);
+
+/**
+  @brief         Floating-point normalization of quaternions.
+  @param[in]     pInputQuaternions            points to the input vector of quaternions
+  @param[out]    pNormalizedQuaternions       points to the output vector of normalized quaternions
+  @param[in]     nbQuaternions                number of quaternions in each vector
+  @return        none
+ */
+void arm_quaternion_normalize_f32(const float32_t *inputQuaternions, 
+    float32_t *pNormalizedQuaternions, 
+    uint32_t nbQuaternions);
+
+
+/**
+  @brief         Floating-point product of two quaternions.
+  @param[in]     qa       First quaternion
+  @param[in]     qb       Second quaternion
+  @param[out]    r        Product of two quaternions
+  @return        none
+ */
+void arm_quaternion_product_single_f32(const float32_t *qa, 
+    const float32_t *qb, 
+    float32_t *r);
+
+/**
+  @brief         Floating-point elementwise product two quaternions.
+  @param[in]     qa                  First array of quaternions
+  @param[in]     qb                  Second array of quaternions
+  @param[out]    r                   Elementwise product of quaternions
+  @param[in]     nbQuaternions       Number of quaternions in the array
+  @return        none
+ */
+void arm_quaternion_product_f32(const float32_t *qa, 
+    const float32_t *qb, 
+    float32_t *r,
+    uint32_t nbQuaternions);
+
+/**
+ * @brief Conversion of quaternion to equivalent rotation matrix.
+ * @param[in]       pInputQuaternions points to an array of normalized quaternions
+ * @param[out]      pOutputRotations points to an array of 3x3 rotations (in row order)
+ * @param[in]       nbQuaternions in the array
+ * @return none.
+ *
+ * <b>Format of rotation matrix</b>
+ * \par
+ * The quaternion a + ib + jc + kd is converted into rotation matrix:
+ *   a^2 + b^2 - c^2 - d^2                 2bc - 2ad                 2bd + 2ac
+ *               2bc + 2ad     a^2 - b^2 + c^2 - d^2                 2cd - 2ab
+ *               2bd - 2ac                 2cd + 2ab     a^2 - b^2 - c^2 + d^2
+ *
+ * Rotation matrix is saved in row order : R00 R01 R02 R10 R11 R12 R20 R21 R22
+ */
+void arm_quaternion2rotation_f32(const float32_t *pInputQuaternions, 
+    float32_t *pOutputRotations, 
+    uint32_t nbQuaternions);
+
+/**
+ * @brief Conversion of a rotation matrix to equivalent quaternion.
+ * @param[in]       pInputRotations points to an array 3x3 rotation matrix (in row order)
+ * @param[out]      pOutputQuaternions points to an array of quaternions
+ * @param[in]       nbQuaternions in the array
+ * @return none.
+*/
+void arm_rotation2quaternion_f32(const float32_t *pInputRotations, 
+    float32_t *pOutputQuaternions,  
+    uint32_t nbQuaternions);
+
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _QUATERNION_MATH_FUNCTIONS_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/statistics_functions.h

@@ -0,0 +1,977 @@
+/******************************************************************************
+ * @file     statistics_functions.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _STATISTICS_FUNCTIONS_H_
+#define _STATISTICS_FUNCTIONS_H_
+
+#include "arm_math_types.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+
+#include "dsp/basic_math_functions.h"
+#include "dsp/fast_math_functions.h"
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+
+/**
+ * @defgroup groupStats Statistics Functions
+ */
+
+/**
+ * @brief Computation of the LogSumExp
+ *
+ * In probabilistic computations, the dynamic of the probability values can be very
+ * wide because they come from gaussian functions.
+ * To avoid underflow and overflow issues, the values are represented by their log.
+ * In this representation, multiplying the original exp values is easy : their logs are added.
+ * But adding the original exp values is requiring some special handling and it is the
+ * goal of the LogSumExp function.
+ *
+ * If the values are x1...xn, the function is computing:
+ *
+ * ln(exp(x1) + ... + exp(xn)) and the computation is done in such a way that
+ * rounding issues are minimised.
+ *
+ * The max xm of the values is extracted and the function is computing:
+ * xm + ln(exp(x1 - xm) + ... + exp(xn - xm))
+ *
+ * @param[in]  *in         Pointer to an array of input values.
+ * @param[in]  blockSize   Number of samples in the input array.
+ * @return LogSumExp
+ *
+ */
+
+
+float32_t arm_logsumexp_f32(const float32_t *in, uint32_t blockSize);
+
+/**
+ * @brief Dot product with log arithmetic
+ *
+ * Vectors are containing the log of the samples
+ *
+ * @param[in]       pSrcA points to the first input vector
+ * @param[in]       pSrcB points to the second input vector
+ * @param[in]       blockSize number of samples in each vector
+ * @param[in]       pTmpBuffer temporary buffer of length blockSize
+ * @return The log of the dot product .
+ *
+ */
+
+
+float32_t arm_logsumexp_dot_prod_f32(const float32_t * pSrcA,
+  const float32_t * pSrcB,
+  uint32_t blockSize,
+  float32_t *pTmpBuffer);
+
+/**
+ * @brief Entropy
+ *
+ * @param[in]  pSrcA        Array of input values.
+ * @param[in]  blockSize    Number of samples in the input array.
+ * @return     Entropy      -Sum(p ln p)
+ *
+ */
+
+
+float32_t arm_entropy_f32(const float32_t * pSrcA,uint32_t blockSize);
+
+
+/**
+ * @brief Entropy
+ *
+ * @param[in]  pSrcA        Array of input values.
+ * @param[in]  blockSize    Number of samples in the input array.
+ * @return     Entropy      -Sum(p ln p)
+ *
+ */
+
+
+float64_t arm_entropy_f64(const float64_t * pSrcA, uint32_t blockSize);
+
+
+/**
+ * @brief Kullback-Leibler
+ *
+ * @param[in]  pSrcA         Pointer to an array of input values for probability distribution A.
+ * @param[in]  pSrcB         Pointer to an array of input values for probability distribution B.
+ * @param[in]  blockSize     Number of samples in the input array.
+ * @return Kullback-Leibler  Divergence D(A || B)
+ *
+ */
+float32_t arm_kullback_leibler_f32(const float32_t * pSrcA
+  ,const float32_t * pSrcB
+  ,uint32_t blockSize);
+
+
+/**
+ * @brief Kullback-Leibler
+ *
+ * @param[in]  pSrcA         Pointer to an array of input values for probability distribution A.
+ * @param[in]  pSrcB         Pointer to an array of input values for probability distribution B.
+ * @param[in]  blockSize     Number of samples in the input array.
+ * @return Kullback-Leibler  Divergence D(A || B)
+ *
+ */
+float64_t arm_kullback_leibler_f64(const float64_t * pSrcA, 
+                const float64_t * pSrcB, 
+                uint32_t blockSize);
+
+
+ /**
+   * @brief  Sum of the squares of the elements of a Q31 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_power_q31(
+  const q31_t * pSrc,
+        uint32_t blockSize,
+        q63_t * pResult);
+
+
+  /**
+   * @brief  Sum of the squares of the elements of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_power_f32(
+  const float32_t * pSrc,
+        uint32_t blockSize,
+        float32_t * pResult);
+
+
+  /**
+   * @brief  Sum of the squares of the elements of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_power_f64(
+  const float64_t * pSrc,
+        uint32_t blockSize,
+        float64_t * pResult);
+
+
+  /**
+   * @brief  Sum of the squares of the elements of a Q15 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_power_q15(
+  const q15_t * pSrc,
+        uint32_t blockSize,
+        q63_t * pResult);
+
+
+  /**
+   * @brief  Sum of the squares of the elements of a Q7 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_power_q7(
+  const q7_t * pSrc,
+        uint32_t blockSize,
+        q31_t * pResult);
+
+
+  /**
+   * @brief  Mean value of a Q7 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_mean_q7(
+  const q7_t * pSrc,
+        uint32_t blockSize,
+        q7_t * pResult);
+
+
+  /**
+   * @brief  Mean value of a Q15 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_mean_q15(
+  const q15_t * pSrc,
+        uint32_t blockSize,
+        q15_t * pResult);
+
+
+  /**
+   * @brief  Mean value of a Q31 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_mean_q31(
+  const q31_t * pSrc,
+        uint32_t blockSize,
+        q31_t * pResult);
+
+
+  /**
+   * @brief  Mean value of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_mean_f32(
+  const float32_t * pSrc,
+        uint32_t blockSize,
+        float32_t * pResult);
+
+
+  /**
+   * @brief  Mean value of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_mean_f64(
+  const float64_t * pSrc,
+        uint32_t blockSize,
+        float64_t * pResult);
+
+
+  /**
+   * @brief  Variance of the elements of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_var_f32(
+  const float32_t * pSrc,
+        uint32_t blockSize,
+        float32_t * pResult);
+
+
+  /**
+   * @brief  Variance of the elements of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_var_f64(
+  const float64_t * pSrc,
+        uint32_t blockSize,
+        float64_t * pResult);
+
+
+  /**
+   * @brief  Variance of the elements of a Q31 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_var_q31(
+  const q31_t * pSrc,
+        uint32_t blockSize,
+        q31_t * pResult);
+
+
+  /**
+   * @brief  Variance of the elements of a Q15 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_var_q15(
+  const q15_t * pSrc,
+        uint32_t blockSize,
+        q15_t * pResult);
+
+
+  /**
+   * @brief  Root Mean Square of the elements of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_rms_f32(
+  const float32_t * pSrc,
+        uint32_t blockSize,
+        float32_t * pResult);
+
+
+  /**
+   * @brief  Root Mean Square of the elements of a Q31 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_rms_q31(
+  const q31_t * pSrc,
+        uint32_t blockSize,
+        q31_t * pResult);
+
+
+  /**
+   * @brief  Root Mean Square of the elements of a Q15 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_rms_q15(
+  const q15_t * pSrc,
+        uint32_t blockSize,
+        q15_t * pResult);
+
+
+  /**
+   * @brief  Standard deviation of the elements of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_std_f32(
+  const float32_t * pSrc,
+        uint32_t blockSize,
+        float32_t * pResult);
+
+
+  /**
+   * @brief  Standard deviation of the elements of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_std_f64(
+  const float64_t * pSrc,
+        uint32_t blockSize,
+        float64_t * pResult);
+
+
+  /**
+   * @brief  Standard deviation of the elements of a Q31 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_std_q31(
+  const q31_t * pSrc,
+        uint32_t blockSize,
+        q31_t * pResult);
+
+
+  /**
+   * @brief  Standard deviation of the elements of a Q15 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_std_q15(
+  const q15_t * pSrc,
+        uint32_t blockSize,
+        q15_t * pResult);
+
+
+  
+  /**
+   * @brief  Minimum value of a Q7 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] result     is output pointer
+   * @param[in]  index      is the array index of the minimum value in the input buffer.
+   */
+  void arm_min_q7(
+  const q7_t * pSrc,
+        uint32_t blockSize,
+        q7_t * result,
+        uint32_t * index);
+
+  /**
+   * @brief  Minimum value of absolute values of a Q7 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] result     is output pointer
+   * @param[in]  index      is the array index of the minimum value in the input buffer.
+   */
+  void arm_absmin_q7(
+  const q7_t * pSrc,
+        uint32_t blockSize,
+        q7_t * result,
+        uint32_t * index);
+
+    /**
+   * @brief  Minimum value of absolute values of a Q7 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] result     is output pointer
+   */
+  void arm_absmin_no_idx_q7(
+  const q7_t * pSrc,
+        uint32_t blockSize,
+        q7_t * result);
+
+
+  /**
+   * @brief  Minimum value of a Q15 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output pointer
+   * @param[in]  pIndex     is the array index of the minimum value in the input buffer.
+   */
+  void arm_min_q15(
+  const q15_t * pSrc,
+        uint32_t blockSize,
+        q15_t * pResult,
+        uint32_t * pIndex);
+
+/**
+   * @brief  Minimum value of absolute values of a Q15 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output pointer
+   * @param[in]  pIndex     is the array index of the minimum value in the input buffer.
+   */
+  void arm_absmin_q15(
+  const q15_t * pSrc,
+        uint32_t blockSize,
+        q15_t * pResult,
+        uint32_t * pIndex);
+
+  /**
+   * @brief  Minimum value of absolute values of a Q15 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output pointer
+   */
+  void arm_absmin_no_idx_q15(
+  const q15_t * pSrc,
+        uint32_t blockSize,
+        q15_t * pResult);
+
+
+  /**
+   * @brief  Minimum value of a Q31 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output pointer
+   * @param[out] pIndex     is the array index of the minimum value in the input buffer.
+   */
+  void arm_min_q31(
+  const q31_t * pSrc,
+        uint32_t blockSize,
+        q31_t * pResult,
+        uint32_t * pIndex);
+
+  /**
+   * @brief  Minimum value of absolute values of a Q31 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output pointer
+   * @param[out] pIndex     is the array index of the minimum value in the input buffer.
+   */
+  void arm_absmin_q31(
+  const q31_t * pSrc,
+        uint32_t blockSize,
+        q31_t * pResult,
+        uint32_t * pIndex);
+
+ /**
+   * @brief  Minimum value of absolute values of a Q31 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output pointer
+   */
+  void arm_absmin_no_idx_q31(
+  const q31_t * pSrc,
+        uint32_t blockSize,
+        q31_t * pResult);
+
+
+  /**
+   * @brief  Minimum value of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output pointer
+   * @param[out] pIndex     is the array index of the minimum value in the input buffer.
+   */
+  void arm_min_f32(
+  const float32_t * pSrc,
+        uint32_t blockSize,
+        float32_t * pResult,
+        uint32_t * pIndex);
+
+  /**
+   * @brief  Minimum value of absolute values of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output pointer
+   * @param[out] pIndex     is the array index of the minimum value in the input buffer.
+   */
+  void arm_absmin_f32(
+  const float32_t * pSrc,
+        uint32_t blockSize,
+        float32_t * pResult,
+        uint32_t * pIndex);
+
+  /**
+   * @brief  Minimum value of absolute values of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output pointer
+   */
+  void arm_absmin_no_idx_f32(
+  const float32_t * pSrc,
+        uint32_t blockSize,
+        float32_t * pResult);
+
+
+  /**
+   * @brief  Minimum value of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output pointer
+   * @param[out] pIndex     is the array index of the minimum value in the input buffer.
+   */
+  void arm_min_f64(
+  const float64_t * pSrc,
+        uint32_t blockSize,
+        float64_t * pResult,
+        uint32_t * pIndex);
+
+  /**
+   * @brief  Minimum value of absolute values of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output pointer
+   * @param[out] pIndex     is the array index of the minimum value in the input buffer.
+   */
+  void arm_absmin_f64(
+  const float64_t * pSrc,
+        uint32_t blockSize,
+        float64_t * pResult,
+        uint32_t * pIndex);
+
+  /**
+   * @brief  Minimum value of absolute values of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output pointer
+   */
+  void arm_absmin_no_idx_f64(
+  const float64_t * pSrc,
+        uint32_t blockSize,
+        float64_t * pResult);
+
+
+/**
+ * @brief Maximum value of a Q7 vector.
+ * @param[in]  pSrc       points to the input buffer
+ * @param[in]  blockSize  length of the input vector
+ * @param[out] pResult    maximum value returned here
+ * @param[out] pIndex     index of maximum value returned here
+ */
+  void arm_max_q7(
+  const q7_t * pSrc,
+        uint32_t blockSize,
+        q7_t * pResult,
+        uint32_t * pIndex);
+
+/**
+ * @brief Maximum value of absolute values of a Q7 vector.
+ * @param[in]  pSrc       points to the input buffer
+ * @param[in]  blockSize  length of the input vector
+ * @param[out] pResult    maximum value returned here
+ * @param[out] pIndex     index of maximum value returned here
+ */
+  void arm_absmax_q7(
+  const q7_t * pSrc,
+        uint32_t blockSize,
+        q7_t * pResult,
+        uint32_t * pIndex);
+
+/**
+ * @brief Maximum value of absolute values of a Q7 vector.
+ * @param[in]  pSrc       points to the input buffer
+ * @param[in]  blockSize  length of the input vector
+ * @param[out] pResult    maximum value returned here
+ */
+  void arm_absmax_no_idx_q7(
+  const q7_t * pSrc,
+        uint32_t blockSize,
+        q7_t * pResult);
+
+
+/**
+ * @brief Maximum value of a Q15 vector.
+ * @param[in]  pSrc       points to the input buffer
+ * @param[in]  blockSize  length of the input vector
+ * @param[out] pResult    maximum value returned here
+ * @param[out] pIndex     index of maximum value returned here
+ */
+  void arm_max_q15(
+  const q15_t * pSrc,
+        uint32_t blockSize,
+        q15_t * pResult,
+        uint32_t * pIndex);
+
+/**
+ * @brief Maximum value of absolute values of a Q15 vector.
+ * @param[in]  pSrc       points to the input buffer
+ * @param[in]  blockSize  length of the input vector
+ * @param[out] pResult    maximum value returned here
+ * @param[out] pIndex     index of maximum value returned here
+ */
+  void arm_absmax_q15(
+  const q15_t * pSrc,
+        uint32_t blockSize,
+        q15_t * pResult,
+        uint32_t * pIndex);
+
+  /**
+ * @brief Maximum value of absolute values of a Q15 vector.
+ * @param[in]  pSrc       points to the input buffer
+ * @param[in]  blockSize  length of the input vector
+ * @param[out] pResult    maximum value returned here
+ */
+  void arm_absmax_no_idx_q15(
+  const q15_t * pSrc,
+        uint32_t blockSize,
+        q15_t * pResult);
+
+/**
+ * @brief Maximum value of a Q31 vector.
+ * @param[in]  pSrc       points to the input buffer
+ * @param[in]  blockSize  length of the input vector
+ * @param[out] pResult    maximum value returned here
+ * @param[out] pIndex     index of maximum value returned here
+ */
+  void arm_max_q31(
+  const q31_t * pSrc,
+        uint32_t blockSize,
+        q31_t * pResult,
+        uint32_t * pIndex);
+
+/**
+ * @brief Maximum value of absolute values of a Q31 vector.
+ * @param[in]  pSrc       points to the input buffer
+ * @param[in]  blockSize  length of the input vector
+ * @param[out] pResult    maximum value returned here
+ * @param[out] pIndex     index of maximum value returned here
+ */
+  void arm_absmax_q31(
+  const q31_t * pSrc,
+        uint32_t blockSize,
+        q31_t * pResult,
+        uint32_t * pIndex);
+
+ /**
+ * @brief Maximum value of absolute values of a Q31 vector.
+ * @param[in]  pSrc       points to the input buffer
+ * @param[in]  blockSize  length of the input vector
+ * @param[out] pResult    maximum value returned here
+ */
+  void arm_absmax_no_idx_q31(
+  const q31_t * pSrc,
+        uint32_t blockSize,
+        q31_t * pResult);
+
+/**
+ * @brief Maximum value of a floating-point vector.
+ * @param[in]  pSrc       points to the input buffer
+ * @param[in]  blockSize  length of the input vector
+ * @param[out] pResult    maximum value returned here
+ * @param[out] pIndex     index of maximum value returned here
+ */
+  void arm_max_f32(
+  const float32_t * pSrc,
+        uint32_t blockSize,
+        float32_t * pResult,
+        uint32_t * pIndex);
+
+/**
+ * @brief Maximum value of absolute values of a floating-point vector.
+ * @param[in]  pSrc       points to the input buffer
+ * @param[in]  blockSize  length of the input vector
+ * @param[out] pResult    maximum value returned here
+ * @param[out] pIndex     index of maximum value returned here
+ */
+  void arm_absmax_f32(
+  const float32_t * pSrc,
+        uint32_t blockSize,
+        float32_t * pResult,
+        uint32_t * pIndex);
+
+ /**
+ * @brief Maximum value of absolute values of a floating-point vector.
+ * @param[in]  pSrc       points to the input buffer
+ * @param[in]  blockSize  length of the input vector
+ * @param[out] pResult    maximum value returned here
+ */
+  void arm_absmax_no_idx_f32(
+  const float32_t * pSrc,
+        uint32_t blockSize,
+        float32_t * pResult);
+
+/**
+ * @brief Maximum value of a floating-point vector.
+ * @param[in]  pSrc       points to the input buffer
+ * @param[in]  blockSize  length of the input vector
+ * @param[out] pResult    maximum value returned here
+ * @param[out] pIndex     index of maximum value returned here
+ */
+  void arm_max_f64(
+  const float64_t * pSrc,
+        uint32_t blockSize,
+        float64_t * pResult,
+        uint32_t * pIndex);
+
+/**
+ * @brief Maximum value of absolute values of a floating-point vector.
+ * @param[in]  pSrc       points to the input buffer
+ * @param[in]  blockSize  length of the input vector
+ * @param[out] pResult    maximum value returned here
+ * @param[out] pIndex     index of maximum value returned here
+ */
+  void arm_absmax_f64(
+  const float64_t * pSrc,
+        uint32_t blockSize,
+        float64_t * pResult,
+        uint32_t * pIndex);
+
+/**
+ * @brief Maximum value of absolute values of a floating-point vector.
+ * @param[in]  pSrc       points to the input buffer
+ * @param[in]  blockSize  length of the input vector
+ * @param[out] pResult    maximum value returned here
+ */
+  void arm_absmax_no_idx_f64(
+  const float64_t * pSrc,
+        uint32_t blockSize,
+        float64_t * pResult);
+
+  /**
+    @brief         Maximum value of a floating-point vector.
+    @param[in]     pSrc       points to the input vector
+    @param[in]     blockSize  number of samples in input vector
+    @param[out]    pResult    maximum value returned here
+    @return        none
+   */
+  void arm_max_no_idx_f32(
+      const float32_t *pSrc,
+      uint32_t   blockSize,
+      float32_t *pResult);
+
+  /**
+    @brief         Minimum value of a floating-point vector.
+    @param[in]     pSrc       points to the input vector
+    @param[in]     blockSize  number of samples in input vector
+    @param[out]    pResult    minimum value returned here
+    @return        none
+   */
+  void arm_min_no_idx_f32(
+      const float32_t *pSrc,
+      uint32_t   blockSize,
+      float32_t *pResult);
+
+  /**
+    @brief         Maximum value of a floating-point vector.
+    @param[in]     pSrc       points to the input vector
+    @param[in]     blockSize  number of samples in input vector
+    @param[out]    pResult    maximum value returned here
+    @return        none
+   */
+  void arm_max_no_idx_f64(
+      const float64_t *pSrc,
+      uint32_t   blockSize,
+      float64_t *pResult);
+
+  /**
+    @brief         Maximum value of a q31 vector.
+    @param[in]     pSrc       points to the input vector
+    @param[in]     blockSize  number of samples in input vector
+    @param[out]    pResult    maximum value returned here
+    @return        none
+   */
+  void arm_max_no_idx_q31(
+      const q31_t *pSrc,
+      uint32_t   blockSize,
+      q31_t *pResult);
+
+  /**
+    @brief         Maximum value of a q15 vector.
+    @param[in]     pSrc       points to the input vector
+    @param[in]     blockSize  number of samples in input vector
+    @param[out]    pResult    maximum value returned here
+    @return        none
+   */
+  void arm_max_no_idx_q15(
+      const q15_t *pSrc,
+      uint32_t   blockSize,
+      q15_t *pResult);
+
+  /**
+    @brief         Maximum value of a q7 vector.
+    @param[in]     pSrc       points to the input vector
+    @param[in]     blockSize  number of samples in input vector
+    @param[out]    pResult    maximum value returned here
+    @return        none
+   */
+  void arm_max_no_idx_q7(
+      const q7_t *pSrc,
+      uint32_t   blockSize,
+      q7_t *pResult);
+
+  /**
+    @brief         Minimum value of a floating-point vector.
+    @param[in]     pSrc       points to the input vector
+    @param[in]     blockSize  number of samples in input vector
+    @param[out]    pResult    minimum value returned here
+    @return        none
+   */
+  void arm_min_no_idx_f64(
+      const float64_t *pSrc,
+      uint32_t   blockSize,
+      float64_t *pResult);
+
+/**
+    @brief         Minimum value of a q31 vector.
+    @param[in]     pSrc       points to the input vector
+    @param[in]     blockSize  number of samples in input vector
+    @param[out]    pResult    minimum value returned here
+    @return        none
+   */
+  void arm_min_no_idx_q31(
+      const q31_t *pSrc,
+      uint32_t   blockSize,
+      q31_t *pResult);
+
+  /**
+    @brief         Minimum value of a q15 vector.
+    @param[in]     pSrc       points to the input vector
+    @param[in]     blockSize  number of samples in input vector
+    @param[out]    pResult    minimum value returned here
+    @return        none
+   */
+  void arm_min_no_idx_q15(
+      const q15_t *pSrc,
+      uint32_t   blockSize,
+      q15_t *pResult);
+
+  /**
+    @brief         Minimum value of a q7 vector.
+    @param[in]     pSrc       points to the input vector
+    @param[in]     blockSize  number of samples in input vector
+    @param[out]    pResult    minimum value returned here
+    @return        none
+   */
+  void arm_min_no_idx_q7(
+      const q7_t *pSrc,
+      uint32_t   blockSize,
+      q7_t *pResult);
+
+/**
+  @brief         Mean square error between two Q7 vectors.
+  @param[in]     pSrcA       points to the first input vector
+  @param[in]     pSrcB       points to the second input vector
+  @param[in]     blockSize  number of samples in input vector
+  @param[out]    pResult    mean square error
+  @return        none 
+*/
+  
+void arm_mse_q7(
+  const q7_t * pSrcA,
+  const q7_t * pSrcB,
+        uint32_t blockSize,
+        q7_t * pResult);
+
+/**
+  @brief         Mean square error between two Q15 vectors.
+  @param[in]     pSrcA       points to the first input vector
+  @param[in]     pSrcB       points to the second input vector
+  @param[in]     blockSize  number of samples in input vector
+  @param[out]    pResult    mean square error
+  @return        none 
+*/
+  
+void arm_mse_q15(
+  const q15_t * pSrcA,
+  const q15_t * pSrcB,
+        uint32_t blockSize,
+        q15_t * pResult);
+
+/**
+  @brief         Mean square error between two Q31 vectors.
+  @param[in]     pSrcA       points to the first input vector
+  @param[in]     pSrcB       points to the second input vector
+  @param[in]     blockSize  number of samples in input vector
+  @param[out]    pResult    mean square error
+  @return        none 
+*/
+  
+void arm_mse_q31(
+  const q31_t * pSrcA,
+  const q31_t * pSrcB,
+        uint32_t blockSize,
+        q31_t * pResult);
+
+/**
+  @brief         Mean square error between two single precision float vectors.
+  @param[in]     pSrcA       points to the first input vector
+  @param[in]     pSrcB       points to the second input vector
+  @param[in]     blockSize  number of samples in input vector
+  @param[out]    pResult    mean square error
+  @return        none 
+*/
+  
+void arm_mse_f32(
+  const float32_t * pSrcA,
+  const float32_t * pSrcB,
+        uint32_t blockSize,
+        float32_t * pResult);
+
+/**
+  @brief         Mean square error between two double precision float vectors.
+  @param[in]     pSrcA       points to the first input vector
+  @param[in]     pSrcB       points to the second input vector
+  @param[in]     blockSize  number of samples in input vector
+  @param[out]    pResult    mean square error
+  @return        none 
+*/
+  
+void arm_mse_f64(
+  const float64_t * pSrcA,
+  const float64_t * pSrcB,
+        uint32_t blockSize,
+        float64_t * pResult);
+
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _STATISTICS_FUNCTIONS_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/statistics_functions_f16.h

@@ -0,0 +1,266 @@
+/******************************************************************************
+ * @file     statistics_functions_f16.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _STATISTICS_FUNCTIONS_F16_H_
+#define _STATISTICS_FUNCTIONS_F16_H_
+
+#include "arm_math_types_f16.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+
+#include "dsp/basic_math_functions_f16.h"
+#include "dsp/fast_math_functions_f16.h"
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+#if defined(ARM_FLOAT16_SUPPORTED)
+
+ /**
+   * @brief  Sum of the squares of the elements of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_power_f16(
+  const float16_t * pSrc,
+        uint32_t blockSize,
+        float16_t * pResult);
+
+ /**
+   * @brief  Mean value of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_mean_f16(
+  const float16_t * pSrc,
+        uint32_t blockSize,
+        float16_t * pResult);
+
+  /**
+   * @brief  Variance of the elements of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_var_f16(
+  const float16_t * pSrc,
+        uint32_t blockSize,
+        float16_t * pResult);
+
+ /**
+   * @brief  Root Mean Square of the elements of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_rms_f16(
+  const float16_t * pSrc,
+        uint32_t blockSize,
+        float16_t * pResult);
+
+ /**
+   * @brief  Standard deviation of the elements of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output value.
+   */
+  void arm_std_f16(
+  const float16_t * pSrc,
+        uint32_t blockSize,
+        float16_t * pResult);
+
+ /**
+   * @brief  Minimum value of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output pointer
+   * @param[out] pIndex     is the array index of the minimum value in the input buffer.
+   */
+  void arm_min_f16(
+  const float16_t * pSrc,
+        uint32_t blockSize,
+        float16_t * pResult,
+        uint32_t * pIndex);
+
+ /**
+   * @brief  Minimum value of absolute values of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output pointer
+   * @param[out] pIndex     is the array index of the minimum value in the input buffer.
+   */
+  void arm_absmin_f16(
+  const float16_t * pSrc,
+        uint32_t blockSize,
+        float16_t * pResult,
+        uint32_t * pIndex);
+
+/**
+ * @brief Maximum value of a floating-point vector.
+ * @param[in]  pSrc       points to the input buffer
+ * @param[in]  blockSize  length of the input vector
+ * @param[out] pResult    maximum value returned here
+ * @param[out] pIndex     index of maximum value returned here
+ */
+  void arm_max_f16(
+  const float16_t * pSrc,
+        uint32_t blockSize,
+        float16_t * pResult,
+        uint32_t * pIndex);
+
+/**
+ * @brief Maximum value of absolute values of a floating-point vector.
+ * @param[in]  pSrc       points to the input buffer
+ * @param[in]  blockSize  length of the input vector
+ * @param[out] pResult    maximum value returned here
+ * @param[out] pIndex     index of maximum value returned here
+ */
+  void arm_absmax_f16(
+  const float16_t * pSrc,
+        uint32_t blockSize,
+        float16_t * pResult,
+        uint32_t * pIndex);
+
+    /**
+   * @brief  Minimum value of absolute values of a floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[in]  blockSize  is the number of samples to process
+   * @param[out] pResult    is output pointer
+   */
+  void arm_absmin_no_idx_f16(
+  const float16_t * pSrc,
+        uint32_t blockSize,
+        float16_t * pResult);
+
+/**
+ * @brief Maximum value of a floating-point vector.
+ * @param[in]  pSrc       points to the input buffer
+ * @param[in]  blockSize  length of the input vector
+ * @param[out] pResult    maximum value returned here
+ */
+  void arm_absmax_no_idx_f16(
+  const float16_t * pSrc,
+        uint32_t blockSize,
+        float16_t * pResult);
+
+
+/**
+ * @brief Entropy
+ *
+ * @param[in]  pSrcA        Array of input values.
+ * @param[in]  blockSize    Number of samples in the input array.
+ * @return     Entropy      -Sum(p ln p)
+ *
+ */
+
+
+float16_t arm_entropy_f16(const float16_t * pSrcA,uint32_t blockSize);
+
+float16_t arm_logsumexp_f16(const float16_t *in, uint32_t blockSize);
+
+/**
+ * @brief Dot product with log arithmetic
+ *
+ * Vectors are containing the log of the samples
+ *
+ * @param[in]       pSrcA points to the first input vector
+ * @param[in]       pSrcB points to the second input vector
+ * @param[in]       blockSize number of samples in each vector
+ * @param[in]       pTmpBuffer temporary buffer of length blockSize
+ * @return The log of the dot product .
+ *
+ */
+
+
+float16_t arm_logsumexp_dot_prod_f16(const float16_t * pSrcA,
+  const float16_t * pSrcB,
+  uint32_t blockSize,
+  float16_t *pTmpBuffer);
+
+/**
+ * @brief Kullback-Leibler
+ *
+ * @param[in]  pSrcA         Pointer to an array of input values for probability distribution A.
+ * @param[in]  pSrcB         Pointer to an array of input values for probability distribution B.
+ * @param[in]  blockSize     Number of samples in the input array.
+ * @return Kullback-Leibler  Divergence D(A || B)
+ *
+ */
+float16_t arm_kullback_leibler_f16(const float16_t * pSrcA
+  ,const float16_t * pSrcB
+  ,uint32_t blockSize);
+
+/**
+    @brief         Maximum value of a floating-point vector.
+    @param[in]     pSrc       points to the input vector
+    @param[in]     blockSize  number of samples in input vector
+    @param[out]    pResult    maximum value returned here
+    @return        none
+   */
+  void arm_max_no_idx_f16(
+      const float16_t *pSrc,
+      uint32_t   blockSize,
+      float16_t *pResult);
+
+/**
+    @brief         Minimum value of a floating-point vector.
+    @param[in]     pSrc       points to the input vector
+    @param[in]     blockSize  number of samples in input vector
+    @param[out]    pResult    minimum value returned here
+    @return        none
+   */
+  void arm_min_no_idx_f16(
+      const float16_t *pSrc,
+      uint32_t   blockSize,
+      float16_t *pResult);
+
+/**
+  @brief         Mean square error between two half precision float vectors.
+  @param[in]     pSrcA       points to the first input vector
+  @param[in]     pSrcB       points to the second input vector
+  @param[in]     blockSize  number of samples in input vector
+  @param[out]    pResult    mean square error
+  @return        none 
+*/
+  
+void arm_mse_f16(
+  const float16_t * pSrcA,
+  const float16_t * pSrcB,
+        uint32_t blockSize,
+        float16_t * pResult);
+
+#endif /*defined(ARM_FLOAT16_SUPPORTED)*/
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _STATISTICS_FUNCTIONS_F16_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/support_functions.h

@@ -0,0 +1,453 @@
+/******************************************************************************
+ * @file     support_functions.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _SUPPORT_FUNCTIONS_H_
+#define _SUPPORT_FUNCTIONS_H_
+
+#include "arm_math_types.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * @defgroup groupSupport Support Functions
+ */
+
+
+/**
+   * @brief Converts the elements of the floating-point vector to Q31 vector.
+   * @param[in]  pSrc       points to the floating-point input vector
+   * @param[out] pDst       points to the Q31 output vector
+   * @param[in]  blockSize  length of the input vector
+   */
+  void arm_float_to_q31(
+  const float32_t * pSrc,
+        q31_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief Converts the elements of the floating-point vector to Q15 vector.
+   * @param[in]  pSrc       points to the floating-point input vector
+   * @param[out] pDst       points to the Q15 output vector
+   * @param[in]  blockSize  length of the input vector
+   */
+  void arm_float_to_q15(
+  const float32_t * pSrc,
+        q15_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief Converts the elements of the floating-point vector to Q7 vector.
+   * @param[in]  pSrc       points to the floating-point input vector
+   * @param[out] pDst       points to the Q7 output vector
+   * @param[in]  blockSize  length of the input vector
+   */
+  void arm_float_to_q7(
+  const float32_t * pSrc,
+        q7_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Converts the elements of the Q31 vector to floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[out] pDst       is output pointer
+   * @param[in]  blockSize  is the number of samples to process
+   */
+  void arm_q31_to_float(
+  const q31_t * pSrc,
+        float32_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Converts the elements of the Q31 vector to Q15 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[out] pDst       is output pointer
+   * @param[in]  blockSize  is the number of samples to process
+   */
+  void arm_q31_to_q15(
+  const q31_t * pSrc,
+        q15_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Converts the elements of the Q31 vector to Q7 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[out] pDst       is output pointer
+   * @param[in]  blockSize  is the number of samples to process
+   */
+  void arm_q31_to_q7(
+  const q31_t * pSrc,
+        q7_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Converts the elements of the Q15 vector to floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[out] pDst       is output pointer
+   * @param[in]  blockSize  is the number of samples to process
+   */
+  void arm_q15_to_float(
+  const q15_t * pSrc,
+        float32_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Converts the elements of the Q15 vector to Q31 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[out] pDst       is output pointer
+   * @param[in]  blockSize  is the number of samples to process
+   */
+  void arm_q15_to_q31(
+  const q15_t * pSrc,
+        q31_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Converts the elements of the Q15 vector to Q7 vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[out] pDst       is output pointer
+   * @param[in]  blockSize  is the number of samples to process
+   */
+  void arm_q15_to_q7(
+  const q15_t * pSrc,
+        q7_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Converts the elements of the Q7 vector to floating-point vector.
+   * @param[in]  pSrc       is input pointer
+   * @param[out] pDst       is output pointer
+   * @param[in]  blockSize  is the number of samples to process
+   */
+  void arm_q7_to_float(
+  const q7_t * pSrc,
+        float32_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Converts the elements of the Q7 vector to Q31 vector.
+   * @param[in]  pSrc       input pointer
+   * @param[out] pDst       output pointer
+   * @param[in]  blockSize  number of samples to process
+   */
+  void arm_q7_to_q31(
+  const q7_t * pSrc,
+        q31_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Converts the elements of the Q7 vector to Q15 vector.
+   * @param[in]  pSrc       input pointer
+   * @param[out] pDst       output pointer
+   * @param[in]  blockSize  number of samples to process
+   */
+  void arm_q7_to_q15(
+  const q7_t * pSrc,
+        q15_t * pDst,
+        uint32_t blockSize);
+
+
+
+
+  
+  /**
+   * @brief Struct for specifying sorting algorithm
+   */
+  typedef enum
+  {
+    ARM_SORT_BITONIC   = 0,
+             /**< Bitonic sort   */
+    ARM_SORT_BUBBLE    = 1,
+             /**< Bubble sort    */
+    ARM_SORT_HEAP      = 2,
+             /**< Heap sort      */
+    ARM_SORT_INSERTION = 3,
+             /**< Insertion sort */
+    ARM_SORT_QUICK     = 4,
+             /**< Quick sort     */
+    ARM_SORT_SELECTION = 5
+             /**< Selection sort */
+  } arm_sort_alg;
+
+  /**
+   * @brief Struct for specifying sorting algorithm
+   */
+  typedef enum
+  {
+    ARM_SORT_DESCENDING = 0,
+             /**< Descending order (9 to 0) */
+    ARM_SORT_ASCENDING = 1
+             /**< Ascending order (0 to 9) */
+  } arm_sort_dir;
+
+  /**
+   * @brief Instance structure for the sorting algorithms.
+   */
+  typedef struct            
+  {
+    arm_sort_alg alg;        /**< Sorting algorithm selected */
+    arm_sort_dir dir;        /**< Sorting order (direction)  */
+  } arm_sort_instance_f32;  
+
+  /**
+   * @param[in]  S          points to an instance of the sorting structure.
+   * @param[in]  pSrc       points to the block of input data.
+   * @param[out] pDst       points to the block of output data.
+   * @param[in]  blockSize  number of samples to process.
+   */
+  void arm_sort_f32(
+    const arm_sort_instance_f32 * S, 
+          float32_t * pSrc, 
+          float32_t * pDst, 
+          uint32_t blockSize);
+
+  /**
+   * @param[in,out]  S            points to an instance of the sorting structure.
+   * @param[in]      alg          Selected algorithm.
+   * @param[in]      dir          Sorting order.
+   */
+  void arm_sort_init_f32(
+    arm_sort_instance_f32 * S, 
+    arm_sort_alg alg, 
+    arm_sort_dir dir); 
+
+  /**
+   * @brief Instance structure for the sorting algorithms.
+   */
+  typedef struct            
+  {
+    arm_sort_dir dir;        /**< Sorting order (direction)  */
+    float32_t * buffer;      /**< Working buffer */
+  } arm_merge_sort_instance_f32;  
+
+  /**
+   * @param[in]      S          points to an instance of the sorting structure.
+   * @param[in,out]  pSrc       points to the block of input data.
+   * @param[out]     pDst       points to the block of output data
+   * @param[in]      blockSize  number of samples to process.
+   */
+  void arm_merge_sort_f32(
+    const arm_merge_sort_instance_f32 * S,
+          float32_t *pSrc,
+          float32_t *pDst,
+          uint32_t blockSize);
+
+  /**
+   * @param[in,out]  S            points to an instance of the sorting structure.
+   * @param[in]      dir          Sorting order.
+   * @param[in]      buffer       Working buffer.
+   */
+  void arm_merge_sort_init_f32(
+    arm_merge_sort_instance_f32 * S,
+    arm_sort_dir dir,
+    float32_t * buffer);
+
+ 
+ 
+  /**
+   * @brief  Copies the elements of a floating-point vector.
+   * @param[in]  pSrc       input pointer
+   * @param[out] pDst       output pointer
+   * @param[in]  blockSize  number of samples to process
+   */
+  void arm_copy_f32(
+  const float32_t * pSrc,
+        float32_t * pDst,
+        uint32_t blockSize);
+
+ 
+ 
+  /**
+   * @brief  Copies the elements of a floating-point vector.
+   * @param[in]  pSrc       input pointer
+   * @param[out] pDst       output pointer
+   * @param[in]  blockSize  number of samples to process
+   */
+  void arm_copy_f64(
+  const float64_t * pSrc,
+        float64_t * pDst,
+        uint32_t blockSize);
+
+
+
+  /**
+   * @brief  Copies the elements of a Q7 vector.
+   * @param[in]  pSrc       input pointer
+   * @param[out] pDst       output pointer
+   * @param[in]  blockSize  number of samples to process
+   */
+  void arm_copy_q7(
+  const q7_t * pSrc,
+        q7_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Copies the elements of a Q15 vector.
+   * @param[in]  pSrc       input pointer
+   * @param[out] pDst       output pointer
+   * @param[in]  blockSize  number of samples to process
+   */
+  void arm_copy_q15(
+  const q15_t * pSrc,
+        q15_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Copies the elements of a Q31 vector.
+   * @param[in]  pSrc       input pointer
+   * @param[out] pDst       output pointer
+   * @param[in]  blockSize  number of samples to process
+   */
+  void arm_copy_q31(
+  const q31_t * pSrc,
+        q31_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Fills a constant value into a floating-point vector.
+   * @param[in]  value      input value to be filled
+   * @param[out] pDst       output pointer
+   * @param[in]  blockSize  number of samples to process
+   */
+  void arm_fill_f32(
+        float32_t value,
+        float32_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Fills a constant value into a floating-point vector.
+   * @param[in]  value      input value to be filled
+   * @param[out] pDst       output pointer
+   * @param[in]  blockSize  number of samples to process
+   */
+  void arm_fill_f64(
+        float64_t value,
+        float64_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Fills a constant value into a Q7 vector.
+   * @param[in]  value      input value to be filled
+   * @param[out] pDst       output pointer
+   * @param[in]  blockSize  number of samples to process
+   */
+  void arm_fill_q7(
+        q7_t value,
+        q7_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Fills a constant value into a Q15 vector.
+   * @param[in]  value      input value to be filled
+   * @param[out] pDst       output pointer
+   * @param[in]  blockSize  number of samples to process
+   */
+  void arm_fill_q15(
+        q15_t value,
+        q15_t * pDst,
+        uint32_t blockSize);
+
+
+  /**
+   * @brief  Fills a constant value into a Q31 vector.
+   * @param[in]  value      input value to be filled
+   * @param[out] pDst       output pointer
+   * @param[in]  blockSize  number of samples to process
+   */
+  void arm_fill_q31(
+        q31_t value,
+        q31_t * pDst,
+        uint32_t blockSize);
+
+
+
+
+
+
+
+/**
+ * @brief Weighted sum
+ *
+ *
+ * @param[in]    *in           Array of input values.
+ * @param[in]    *weigths      Weights
+ * @param[in]    blockSize     Number of samples in the input array.
+ * @return Weighted sum
+ *
+ */
+float32_t arm_weighted_sum_f32(const float32_t *in
+  , const float32_t *weigths
+  , uint32_t blockSize);
+
+
+/**
+ * @brief Barycenter
+ *
+ *
+ * @param[in]    in         List of vectors
+ * @param[in]    weights    Weights of the vectors
+ * @param[out]   out        Barycenter
+ * @param[in]    nbVectors  Number of vectors
+ * @param[in]    vecDim     Dimension of space (vector dimension)
+ * @return       None
+ *
+ */
+void arm_barycenter_f32(const float32_t *in
+  , const float32_t *weights
+  , float32_t *out
+  , uint32_t nbVectors
+  , uint32_t vecDim);
+
+
+
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _SUPPORT_FUNCTIONS_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/support_functions_f16.h

@@ -0,0 +1,187 @@
+/******************************************************************************
+ * @file     support_functions_f16.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _SUPPORT_FUNCTIONS_F16_H_
+#define _SUPPORT_FUNCTIONS_F16_H_
+
+#include "arm_math_types_f16.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+#if defined(ARM_FLOAT16_SUPPORTED)
+
+  /**
+   * @brief  Copies the elements of a floating-point vector.
+   * @param[in]  pSrc       input pointer
+   * @param[out] pDst       output pointer
+   * @param[in]  blockSize  number of samples to process
+   */
+void arm_copy_f16(const float16_t * pSrc, float16_t * pDst, uint32_t blockSize);
+
+  /**
+   * @brief  Fills a constant value into a floating-point vector.
+   * @param[in]  value      input value to be filled
+   * @param[out] pDst       output pointer
+   * @param[in]  blockSize  number of samples to process
+   */
+void arm_fill_f16(float16_t value, float16_t * pDst, uint32_t blockSize);
+
+/**
+   * @brief Converts the elements of the floating-point vector to Q31 vector.
+   * @param[in]  pSrc       points to the f16 input vector
+   * @param[out] pDst       points to the q15 output vector
+   * @param[in]  blockSize  length of the input vector
+   */
+void arm_f16_to_q15(const float16_t * pSrc, q15_t * pDst, uint32_t blockSize);
+
+/**
+   * @brief Converts the elements of the floating-point vector to Q31 vector.
+   * @param[in]  pSrc       points to the q15 input vector
+   * @param[out] pDst       points to the f16 output vector
+   * @param[in]  blockSize  length of the input vector
+   */
+void arm_q15_to_f16(const q15_t * pSrc, float16_t * pDst, uint32_t blockSize);
+
+
+/**
+   * @brief Converts the elements of the floating-point vector to Q31 vector.
+   * @param[in]  pSrc       points to the f32 input vector
+   * @param[out] pDst       points to the f16 output vector
+   * @param[in]  blockSize  length of the input vector
+   */
+void arm_float_to_f16(const float32_t * pSrc, float16_t * pDst, uint32_t blockSize);
+
+/**
+   * @brief Converts the elements of the floating-point vector to Q31 vector.
+   * @param[in]  pSrc       points to the f16 input vector
+   * @param[out] pDst       points to the f32 output vector
+   * @param[in]  blockSize  length of the input vector
+   */
+void arm_f16_to_float(const float16_t * pSrc, float32_t * pDst, uint32_t blockSize);
+
+/**
+ * @brief Weighted sum
+ *
+ *
+ * @param[in]    *in           Array of input values.
+ * @param[in]    *weigths      Weights
+ * @param[in]    blockSize     Number of samples in the input array.
+ * @return Weighted sum
+ *
+ */
+float16_t arm_weighted_sum_f16(const float16_t *in
+  , const float16_t *weigths
+  , uint32_t blockSize);
+
+/**
+ * @brief Barycenter
+ *
+ *
+ * @param[in]    in         List of vectors
+ * @param[in]    weights    Weights of the vectors
+ * @param[out]   out        Barycenter
+ * @param[in]    nbVectors  Number of vectors
+ * @param[in]    vecDim     Dimension of space (vector dimension)
+ * @return       None
+ *
+ */
+void arm_barycenter_f16(const float16_t *in
+  , const float16_t *weights
+  , float16_t *out
+  , uint32_t nbVectors
+  , uint32_t vecDim);
+
+
+/**
+  @ingroup groupSupport
+ */
+
+/**
+ * @defgroup typecast Typecasting
+ */
+
+/**
+  @addtogroup typecast
+  @{
+ */
+
+/**
+   * @brief  Interpret a f16 as an s16 value
+   * @param[in] x  input value.
+   * @return  return value.
+   * 
+   * @par    Description
+   *            It is a typecast. No conversion of the float to int is done.
+   *            The memcpy will be optimized out by the compiler.
+   *            memcpy is used to prevent type punning issues.
+   *            With gcc, -fno-builtins MUST not be used or the
+   *            memcpy will not be optimized out.
+   */
+__STATIC_INLINE int16_t arm_typecast_s16_f16(float16_t x)
+{
+   int16_t res;
+   res=*(int16_t*)memcpy((char*)&res,(char*)&x,sizeof(float16_t));
+   return(res);
+}
+
+/**
+   * @brief  Interpret an s16 as an f16 value
+   * @param[in] x  input value.
+   * @return  return value.
+   * 
+   * @par    Description
+   *            It is a typecast. No conversion of the int to float is done.
+   *            The memcpy will be optimized out by the compiler.
+   *            memcpy is used to prevent type punning issues.
+   *            With gcc, -fno-builtins MUST not be used or the
+   *            memcpy will not be optimized out.
+   */
+__STATIC_INLINE float16_t arm_typecast_f16_s16(int16_t x)
+{
+   float16_t res;
+   res=*(float16_t*)memcpy((char*)&res,(char*)&x,sizeof(int16_t));
+   return(res);
+}
+
+
+/**
+  @} end of typecast group
+ */
+
+
+#endif /*defined(ARM_FLOAT16_SUPPORTED)*/
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _SUPPORT_FUNCTIONS_F16_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/svm_defines.h

@@ -0,0 +1,46 @@
+/******************************************************************************
+ * @file     svm_defines.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ *
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _SVM_DEFINES_H_
+#define _SVM_DEFINES_H_
+
+/**
+ * @brief Struct for specifying SVM Kernel
+ */
+typedef enum
+{
+    ARM_ML_KERNEL_LINEAR = 0,
+             /**< Linear kernel */
+    ARM_ML_KERNEL_POLYNOMIAL = 1,
+             /**< Polynomial kernel */
+    ARM_ML_KERNEL_RBF = 2,
+             /**< Radial Basis Function kernel */
+    ARM_ML_KERNEL_SIGMOID = 3
+             /**< Sigmoid kernel */
+} arm_ml_kernel_type;
+
+#endif

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/svm_functions.h

@@ -0,0 +1,299 @@
+/******************************************************************************
+ * @file     svm_functions.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _SVM_FUNCTIONS_H_
+#define _SVM_FUNCTIONS_H_
+
+#include "arm_math_types.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+#include "dsp/svm_defines.h"
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+#define STEP(x) (x) <= 0 ? 0 : 1
+
+/**
+ * @defgroup groupSVM SVM Functions
+ * This set of functions is implementing SVM classification on 2 classes.
+ * The training must be done from scikit-learn. The parameters can be easily
+ * generated from the scikit-learn object. Some examples are given in
+ * DSP/Testing/PatternGeneration/SVM.py
+ *
+ * If more than 2 classes are needed, the functions in this folder 
+ * will have to be used, as building blocks, to do multi-class classification.
+ *
+ * No multi-class classification is provided in this SVM folder.
+ * 
+ */
+
+/**
+ * @brief Integer exponentiation
+ * @param[in]    x           value
+ * @param[in]    nb          integer exponent >= 1
+ * @return x^nb
+ *
+ */
+__STATIC_INLINE float32_t arm_exponent_f32(float32_t x, int32_t nb)
+{
+    float32_t r = x;
+    nb --;
+    while(nb > 0)
+    {
+        r = r * x;
+        nb--;
+    }
+    return(r);
+}
+
+  
+
+
+
+/**
+ * @brief Instance structure for linear SVM prediction function.
+ */
+typedef struct
+{
+  uint32_t        nbOfSupportVectors;     /**< Number of support vectors */
+  uint32_t        vectorDimension;        /**< Dimension of vector space */
+  float32_t       intercept;              /**< Intercept */
+  const float32_t *dualCoefficients;      /**< Dual coefficients */
+  const float32_t *supportVectors;        /**< Support vectors */
+  const int32_t   *classes;               /**< The two SVM classes */
+} arm_svm_linear_instance_f32;
+
+
+/**
+ * @brief Instance structure for polynomial SVM prediction function.
+ */
+typedef struct
+{
+  uint32_t        nbOfSupportVectors;     /**< Number of support vectors */
+  uint32_t        vectorDimension;        /**< Dimension of vector space */
+  float32_t       intercept;              /**< Intercept */
+  const float32_t *dualCoefficients;      /**< Dual coefficients */
+  const float32_t *supportVectors;        /**< Support vectors */
+  const int32_t   *classes;               /**< The two SVM classes */
+  int32_t         degree;                 /**< Polynomial degree */
+  float32_t       coef0;                  /**< Polynomial constant */
+  float32_t       gamma;                  /**< Gamma factor */
+} arm_svm_polynomial_instance_f32;
+
+/**
+ * @brief Instance structure for rbf SVM prediction function.
+ */
+typedef struct
+{
+  uint32_t        nbOfSupportVectors;     /**< Number of support vectors */
+  uint32_t        vectorDimension;        /**< Dimension of vector space */
+  float32_t       intercept;              /**< Intercept */
+  const float32_t *dualCoefficients;      /**< Dual coefficients */
+  const float32_t *supportVectors;        /**< Support vectors */
+  const int32_t   *classes;               /**< The two SVM classes */
+  float32_t       gamma;                  /**< Gamma factor */
+} arm_svm_rbf_instance_f32;
+
+/**
+ * @brief Instance structure for sigmoid SVM prediction function.
+ */
+typedef struct
+{
+  uint32_t        nbOfSupportVectors;     /**< Number of support vectors */
+  uint32_t        vectorDimension;        /**< Dimension of vector space */
+  float32_t       intercept;              /**< Intercept */
+  const float32_t *dualCoefficients;      /**< Dual coefficients */
+  const float32_t *supportVectors;        /**< Support vectors */
+  const int32_t   *classes;               /**< The two SVM classes */
+  float32_t       coef0;                  /**< Independent constant */
+  float32_t       gamma;                  /**< Gamma factor */
+} arm_svm_sigmoid_instance_f32;
+
+/**
+ * @brief        SVM linear instance init function
+ * @param[in]    S                      Parameters for SVM functions
+ * @param[in]    nbOfSupportVectors     Number of support vectors
+ * @param[in]    vectorDimension        Dimension of vector space
+ * @param[in]    intercept              Intercept
+ * @param[in]    dualCoefficients       Array of dual coefficients
+ * @param[in]    supportVectors         Array of support vectors
+ * @param[in]    classes                Array of 2 classes ID
+ * @return none.
+ *
+ */
+
+
+void arm_svm_linear_init_f32(arm_svm_linear_instance_f32 *S, 
+  uint32_t nbOfSupportVectors,
+  uint32_t vectorDimension,
+  float32_t intercept,
+  const float32_t *dualCoefficients,
+  const float32_t *supportVectors,
+  const int32_t  *classes);
+
+/**
+ * @brief SVM linear prediction
+ * @param[in]    S          Pointer to an instance of the linear SVM structure.
+ * @param[in]    in         Pointer to input vector
+ * @param[out]   pResult    Decision value
+ * @return none.
+ *
+ */
+  
+void arm_svm_linear_predict_f32(const arm_svm_linear_instance_f32 *S, 
+   const float32_t * in, 
+   int32_t * pResult);
+
+
+/**
+ * @brief        SVM polynomial instance init function
+ * @param[in]    S                      points to an instance of the polynomial SVM structure.
+ * @param[in]    nbOfSupportVectors     Number of support vectors
+ * @param[in]    vectorDimension        Dimension of vector space
+ * @param[in]    intercept              Intercept
+ * @param[in]    dualCoefficients       Array of dual coefficients
+ * @param[in]    supportVectors         Array of support vectors
+ * @param[in]    classes                Array of 2 classes ID
+ * @param[in]    degree                 Polynomial degree
+ * @param[in]    coef0                  coeff0 (scikit-learn terminology)
+ * @param[in]    gamma                  gamma (scikit-learn terminology)
+ * @return none.
+ *
+ */
+
+
+void arm_svm_polynomial_init_f32(arm_svm_polynomial_instance_f32 *S, 
+  uint32_t nbOfSupportVectors,
+  uint32_t vectorDimension,
+  float32_t intercept,
+  const float32_t *dualCoefficients,
+  const float32_t *supportVectors,
+  const int32_t   *classes,
+  int32_t      degree,
+  float32_t coef0,
+  float32_t gamma
+  );
+
+/**
+ * @brief SVM polynomial prediction
+ * @param[in]    S          Pointer to an instance of the polynomial SVM structure.
+ * @param[in]    in         Pointer to input vector
+ * @param[out]   pResult    Decision value
+ * @return none.
+ *
+ */
+void arm_svm_polynomial_predict_f32(const arm_svm_polynomial_instance_f32 *S, 
+   const float32_t * in, 
+   int32_t * pResult);
+
+
+/**
+ * @brief        SVM radial basis function instance init function
+ * @param[in]    S                      points to an instance of the polynomial SVM structure.
+ * @param[in]    nbOfSupportVectors     Number of support vectors
+ * @param[in]    vectorDimension        Dimension of vector space
+ * @param[in]    intercept              Intercept
+ * @param[in]    dualCoefficients       Array of dual coefficients
+ * @param[in]    supportVectors         Array of support vectors
+ * @param[in]    classes                Array of 2 classes ID
+ * @param[in]    gamma                  gamma (scikit-learn terminology)
+ * @return none.
+ *
+ */
+
+void arm_svm_rbf_init_f32(arm_svm_rbf_instance_f32 *S, 
+  uint32_t nbOfSupportVectors,
+  uint32_t vectorDimension,
+  float32_t intercept,
+  const float32_t *dualCoefficients,
+  const float32_t *supportVectors,
+  const int32_t   *classes,
+  float32_t gamma
+  );
+
+/**
+ * @brief SVM rbf prediction
+ * @param[in]    S         Pointer to an instance of the rbf SVM structure.
+ * @param[in]    in        Pointer to input vector
+ * @param[out]   pResult   decision value
+ * @return none.
+ *
+ */
+void arm_svm_rbf_predict_f32(const arm_svm_rbf_instance_f32 *S, 
+   const float32_t * in, 
+   int32_t * pResult);
+
+/**
+ * @brief        SVM sigmoid instance init function
+ * @param[in]    S                      points to an instance of the rbf SVM structure.
+ * @param[in]    nbOfSupportVectors     Number of support vectors
+ * @param[in]    vectorDimension        Dimension of vector space
+ * @param[in]    intercept              Intercept
+ * @param[in]    dualCoefficients       Array of dual coefficients
+ * @param[in]    supportVectors         Array of support vectors
+ * @param[in]    classes                Array of 2 classes ID
+ * @param[in]    coef0                  coeff0 (scikit-learn terminology)
+ * @param[in]    gamma                  gamma (scikit-learn terminology)
+ * @return none.
+ *
+ */
+
+void arm_svm_sigmoid_init_f32(arm_svm_sigmoid_instance_f32 *S, 
+  uint32_t nbOfSupportVectors,
+  uint32_t vectorDimension,
+  float32_t intercept,
+  const float32_t *dualCoefficients,
+  const float32_t *supportVectors,
+  const int32_t   *classes,
+  float32_t coef0,
+  float32_t gamma
+  );
+
+/**
+ * @brief SVM sigmoid prediction
+ * @param[in]    S        Pointer to an instance of the rbf SVM structure.
+ * @param[in]    in       Pointer to input vector
+ * @param[out]   pResult  Decision value
+ * @return none.
+ *
+ */
+void arm_svm_sigmoid_predict_f32(const arm_svm_sigmoid_instance_f32 *S, 
+   const float32_t * in, 
+   int32_t * pResult);
+
+
+
+
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _SVM_FUNCTIONS_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/svm_functions_f16.h

@@ -0,0 +1,281 @@
+/******************************************************************************
+ * @file     svm_functions_f16.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _SVM_FUNCTIONS_F16_H_
+#define _SVM_FUNCTIONS_F16_H_
+
+#include "arm_math_types_f16.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+#include "dsp/svm_defines.h"
+
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+#if defined(ARM_FLOAT16_SUPPORTED)
+
+#define STEP(x) (x) <= 0 ? 0 : 1
+
+/**
+ * @defgroup groupSVM SVM Functions
+ * This set of functions is implementing SVM classification on 2 classes.
+ * The training must be done from scikit-learn. The parameters can be easily
+ * generated from the scikit-learn object. Some examples are given in
+ * DSP/Testing/PatternGeneration/SVM.py
+ *
+ * If more than 2 classes are needed, the functions in this folder 
+ * will have to be used, as building blocks, to do multi-class classification.
+ *
+ * No multi-class classification is provided in this SVM folder.
+ * 
+ */
+
+
+
+/**
+ * @brief Instance structure for linear SVM prediction function.
+ */
+typedef struct
+{
+  uint32_t        nbOfSupportVectors;     /**< Number of support vectors */
+  uint32_t        vectorDimension;        /**< Dimension of vector space */
+  float16_t       intercept;              /**< Intercept */
+  const float16_t *dualCoefficients;      /**< Dual coefficients */
+  const float16_t *supportVectors;        /**< Support vectors */
+  const int32_t   *classes;               /**< The two SVM classes */
+} arm_svm_linear_instance_f16;
+
+
+/**
+ * @brief Instance structure for polynomial SVM prediction function.
+ */
+typedef struct
+{
+  uint32_t        nbOfSupportVectors;     /**< Number of support vectors */
+  uint32_t        vectorDimension;        /**< Dimension of vector space */
+  float16_t       intercept;              /**< Intercept */
+  const float16_t *dualCoefficients;      /**< Dual coefficients */
+  const float16_t *supportVectors;        /**< Support vectors */
+  const int32_t   *classes;               /**< The two SVM classes */
+  int32_t         degree;                 /**< Polynomial degree */
+  float16_t       coef0;                  /**< Polynomial constant */
+  float16_t       gamma;                  /**< Gamma factor */
+} arm_svm_polynomial_instance_f16;
+
+/**
+ * @brief Instance structure for rbf SVM prediction function.
+ */
+typedef struct
+{
+  uint32_t        nbOfSupportVectors;     /**< Number of support vectors */
+  uint32_t        vectorDimension;        /**< Dimension of vector space */
+  float16_t       intercept;              /**< Intercept */
+  const float16_t *dualCoefficients;      /**< Dual coefficients */
+  const float16_t *supportVectors;        /**< Support vectors */
+  const int32_t   *classes;               /**< The two SVM classes */
+  float16_t       gamma;                  /**< Gamma factor */
+} arm_svm_rbf_instance_f16;
+
+/**
+ * @brief Instance structure for sigmoid SVM prediction function.
+ */
+typedef struct
+{
+  uint32_t        nbOfSupportVectors;     /**< Number of support vectors */
+  uint32_t        vectorDimension;        /**< Dimension of vector space */
+  float16_t       intercept;              /**< Intercept */
+  const float16_t *dualCoefficients;      /**< Dual coefficients */
+  const float16_t *supportVectors;        /**< Support vectors */
+  const int32_t   *classes;               /**< The two SVM classes */
+  float16_t       coef0;                  /**< Independent constant */
+  float16_t       gamma;                  /**< Gamma factor */
+} arm_svm_sigmoid_instance_f16;
+
+/**
+ * @brief        SVM linear instance init function
+ * @param[in]    S                      Parameters for SVM functions
+ * @param[in]    nbOfSupportVectors     Number of support vectors
+ * @param[in]    vectorDimension        Dimension of vector space
+ * @param[in]    intercept              Intercept
+ * @param[in]    dualCoefficients       Array of dual coefficients
+ * @param[in]    supportVectors         Array of support vectors
+ * @param[in]    classes                Array of 2 classes ID
+ * @return none.
+ *
+ */
+
+
+void arm_svm_linear_init_f16(arm_svm_linear_instance_f16 *S, 
+  uint32_t nbOfSupportVectors,
+  uint32_t vectorDimension,
+  float16_t intercept,
+  const float16_t *dualCoefficients,
+  const float16_t *supportVectors,
+  const int32_t  *classes);
+
+/**
+ * @brief SVM linear prediction
+ * @param[in]    S          Pointer to an instance of the linear SVM structure.
+ * @param[in]    in         Pointer to input vector
+ * @param[out]   pResult    Decision value
+ * @return none.
+ *
+ */
+  
+void arm_svm_linear_predict_f16(const arm_svm_linear_instance_f16 *S, 
+   const float16_t * in, 
+   int32_t * pResult);
+
+
+/**
+ * @brief        SVM polynomial instance init function
+ * @param[in]    S                      points to an instance of the polynomial SVM structure.
+ * @param[in]    nbOfSupportVectors     Number of support vectors
+ * @param[in]    vectorDimension        Dimension of vector space
+ * @param[in]    intercept              Intercept
+ * @param[in]    dualCoefficients       Array of dual coefficients
+ * @param[in]    supportVectors         Array of support vectors
+ * @param[in]    classes                Array of 2 classes ID
+ * @param[in]    degree                 Polynomial degree
+ * @param[in]    coef0                  coeff0 (scikit-learn terminology)
+ * @param[in]    gamma                  gamma (scikit-learn terminology)
+ * @return none.
+ *
+ */
+
+
+void arm_svm_polynomial_init_f16(arm_svm_polynomial_instance_f16 *S, 
+  uint32_t nbOfSupportVectors,
+  uint32_t vectorDimension,
+  float16_t intercept,
+  const float16_t *dualCoefficients,
+  const float16_t *supportVectors,
+  const int32_t   *classes,
+  int32_t      degree,
+  float16_t coef0,
+  float16_t gamma
+  );
+
+/**
+ * @brief SVM polynomial prediction
+ * @param[in]    S          Pointer to an instance of the polynomial SVM structure.
+ * @param[in]    in         Pointer to input vector
+ * @param[out]   pResult    Decision value
+ * @return none.
+ *
+ */
+void arm_svm_polynomial_predict_f16(const arm_svm_polynomial_instance_f16 *S, 
+   const float16_t * in, 
+   int32_t * pResult);
+
+
+/**
+ * @brief        SVM radial basis function instance init function
+ * @param[in]    S                      points to an instance of the polynomial SVM structure.
+ * @param[in]    nbOfSupportVectors     Number of support vectors
+ * @param[in]    vectorDimension        Dimension of vector space
+ * @param[in]    intercept              Intercept
+ * @param[in]    dualCoefficients       Array of dual coefficients
+ * @param[in]    supportVectors         Array of support vectors
+ * @param[in]    classes                Array of 2 classes ID
+ * @param[in]    gamma                  gamma (scikit-learn terminology)
+ * @return none.
+ *
+ */
+
+void arm_svm_rbf_init_f16(arm_svm_rbf_instance_f16 *S, 
+  uint32_t nbOfSupportVectors,
+  uint32_t vectorDimension,
+  float16_t intercept,
+  const float16_t *dualCoefficients,
+  const float16_t *supportVectors,
+  const int32_t   *classes,
+  float16_t gamma
+  );
+
+/**
+ * @brief SVM rbf prediction
+ * @param[in]    S         Pointer to an instance of the rbf SVM structure.
+ * @param[in]    in        Pointer to input vector
+ * @param[out]   pResult   decision value
+ * @return none.
+ *
+ */
+void arm_svm_rbf_predict_f16(const arm_svm_rbf_instance_f16 *S, 
+   const float16_t * in, 
+   int32_t * pResult);
+
+/**
+ * @brief        SVM sigmoid instance init function
+ * @param[in]    S                      points to an instance of the rbf SVM structure.
+ * @param[in]    nbOfSupportVectors     Number of support vectors
+ * @param[in]    vectorDimension        Dimension of vector space
+ * @param[in]    intercept              Intercept
+ * @param[in]    dualCoefficients       Array of dual coefficients
+ * @param[in]    supportVectors         Array of support vectors
+ * @param[in]    classes                Array of 2 classes ID
+ * @param[in]    coef0                  coeff0 (scikit-learn terminology)
+ * @param[in]    gamma                  gamma (scikit-learn terminology)
+ * @return none.
+ *
+ */
+
+void arm_svm_sigmoid_init_f16(arm_svm_sigmoid_instance_f16 *S, 
+  uint32_t nbOfSupportVectors,
+  uint32_t vectorDimension,
+  float16_t intercept,
+  const float16_t *dualCoefficients,
+  const float16_t *supportVectors,
+  const int32_t   *classes,
+  float16_t coef0,
+  float16_t gamma
+  );
+
+/**
+ * @brief SVM sigmoid prediction
+ * @param[in]    S        Pointer to an instance of the rbf SVM structure.
+ * @param[in]    in       Pointer to input vector
+ * @param[out]   pResult  Decision value
+ * @return none.
+ *
+ */
+void arm_svm_sigmoid_predict_f16(const arm_svm_sigmoid_instance_f16 *S, 
+   const float16_t * in, 
+   int32_t * pResult);
+
+
+
+#endif /*defined(ARM_FLOAT16_SUPPORTED)*/
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _SVM_FUNCTIONS_F16_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/transform_functions.h

@@ -0,0 +1,735 @@
+/******************************************************************************
+ * @file     transform_functions.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _TRANSFORM_FUNCTIONS_H_
+#define _TRANSFORM_FUNCTIONS_H_
+
+#include "arm_math_types.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+
+#include "dsp/basic_math_functions.h"
+#include "dsp/complex_math_functions.h"
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+
+/**
+ * @defgroup groupTransforms Transform Functions
+ */
+
+
+  /**
+   * @brief Instance structure for the Q15 CFFT/CIFFT function.
+   */
+  typedef struct
+  {
+          uint16_t fftLen;                 /**< length of the FFT. */
+          uint8_t ifftFlag;                /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
+          uint8_t bitReverseFlag;          /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
+    const q15_t *pTwiddle;                 /**< points to the Sin twiddle factor table. */
+    const uint16_t *pBitRevTable;          /**< points to the bit reversal table. */
+          uint16_t twidCoefModifier;       /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+          uint16_t bitRevFactor;           /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
+  } arm_cfft_radix2_instance_q15;
+
+/* Deprecated */
+  arm_status arm_cfft_radix2_init_q15(
+        arm_cfft_radix2_instance_q15 * S,
+        uint16_t fftLen,
+        uint8_t ifftFlag,
+        uint8_t bitReverseFlag);
+
+/* Deprecated */
+  void arm_cfft_radix2_q15(
+  const arm_cfft_radix2_instance_q15 * S,
+        q15_t * pSrc);
+
+
+  /**
+   * @brief Instance structure for the Q15 CFFT/CIFFT function.
+   */
+  typedef struct
+  {
+          uint16_t fftLen;                 /**< length of the FFT. */
+          uint8_t ifftFlag;                /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
+          uint8_t bitReverseFlag;          /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
+    const q15_t *pTwiddle;                 /**< points to the twiddle factor table. */
+    const uint16_t *pBitRevTable;          /**< points to the bit reversal table. */
+          uint16_t twidCoefModifier;       /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+          uint16_t bitRevFactor;           /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
+  } arm_cfft_radix4_instance_q15;
+
+/* Deprecated */
+  arm_status arm_cfft_radix4_init_q15(
+        arm_cfft_radix4_instance_q15 * S,
+        uint16_t fftLen,
+        uint8_t ifftFlag,
+        uint8_t bitReverseFlag);
+
+/* Deprecated */
+  void arm_cfft_radix4_q15(
+  const arm_cfft_radix4_instance_q15 * S,
+        q15_t * pSrc);
+
+  /**
+   * @brief Instance structure for the Radix-2 Q31 CFFT/CIFFT function.
+   */
+  typedef struct
+  {
+          uint16_t fftLen;                 /**< length of the FFT. */
+          uint8_t ifftFlag;                /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
+          uint8_t bitReverseFlag;          /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
+    const q31_t *pTwiddle;                 /**< points to the Twiddle factor table. */
+    const uint16_t *pBitRevTable;          /**< points to the bit reversal table. */
+          uint16_t twidCoefModifier;       /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+          uint16_t bitRevFactor;           /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
+  } arm_cfft_radix2_instance_q31;
+
+/* Deprecated */
+  arm_status arm_cfft_radix2_init_q31(
+        arm_cfft_radix2_instance_q31 * S,
+        uint16_t fftLen,
+        uint8_t ifftFlag,
+        uint8_t bitReverseFlag);
+
+/* Deprecated */
+  void arm_cfft_radix2_q31(
+  const arm_cfft_radix2_instance_q31 * S,
+        q31_t * pSrc);
+
+  /**
+   * @brief Instance structure for the Q31 CFFT/CIFFT function.
+   */
+  typedef struct
+  {
+          uint16_t fftLen;                 /**< length of the FFT. */
+          uint8_t ifftFlag;                /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
+          uint8_t bitReverseFlag;          /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
+    const q31_t *pTwiddle;                 /**< points to the twiddle factor table. */
+    const uint16_t *pBitRevTable;          /**< points to the bit reversal table. */
+          uint16_t twidCoefModifier;       /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+          uint16_t bitRevFactor;           /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
+  } arm_cfft_radix4_instance_q31;
+
+/* Deprecated */
+  void arm_cfft_radix4_q31(
+  const arm_cfft_radix4_instance_q31 * S,
+        q31_t * pSrc);
+
+/* Deprecated */
+  arm_status arm_cfft_radix4_init_q31(
+        arm_cfft_radix4_instance_q31 * S,
+        uint16_t fftLen,
+        uint8_t ifftFlag,
+        uint8_t bitReverseFlag);
+
+  /**
+   * @brief Instance structure for the floating-point CFFT/CIFFT function.
+   */
+  typedef struct
+  {
+          uint16_t fftLen;                   /**< length of the FFT. */
+          uint8_t ifftFlag;                  /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
+          uint8_t bitReverseFlag;            /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
+    const float32_t *pTwiddle;               /**< points to the Twiddle factor table. */
+    const uint16_t *pBitRevTable;            /**< points to the bit reversal table. */
+          uint16_t twidCoefModifier;         /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+          uint16_t bitRevFactor;             /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
+          float32_t onebyfftLen;             /**< value of 1/fftLen. */
+  } arm_cfft_radix2_instance_f32;
+
+
+/* Deprecated */
+  arm_status arm_cfft_radix2_init_f32(
+        arm_cfft_radix2_instance_f32 * S,
+        uint16_t fftLen,
+        uint8_t ifftFlag,
+        uint8_t bitReverseFlag);
+
+/* Deprecated */
+  void arm_cfft_radix2_f32(
+  const arm_cfft_radix2_instance_f32 * S,
+        float32_t * pSrc);
+
+  /**
+   * @brief Instance structure for the floating-point CFFT/CIFFT function.
+   */
+  typedef struct
+  {
+          uint16_t fftLen;                   /**< length of the FFT. */
+          uint8_t ifftFlag;                  /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
+          uint8_t bitReverseFlag;            /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
+    const float32_t *pTwiddle;               /**< points to the Twiddle factor table. */
+    const uint16_t *pBitRevTable;            /**< points to the bit reversal table. */
+          uint16_t twidCoefModifier;         /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+          uint16_t bitRevFactor;             /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
+          float32_t onebyfftLen;             /**< value of 1/fftLen. */
+  } arm_cfft_radix4_instance_f32;
+
+
+
+/* Deprecated */
+  arm_status arm_cfft_radix4_init_f32(
+        arm_cfft_radix4_instance_f32 * S,
+        uint16_t fftLen,
+        uint8_t ifftFlag,
+        uint8_t bitReverseFlag);
+
+/* Deprecated */
+  void arm_cfft_radix4_f32(
+  const arm_cfft_radix4_instance_f32 * S,
+        float32_t * pSrc);
+
+  /**
+   * @brief Instance structure for the fixed-point CFFT/CIFFT function.
+   */
+  typedef struct
+  {
+          uint16_t fftLen;                   /**< length of the FFT. */
+    const q15_t *pTwiddle;             /**< points to the Twiddle factor table. */
+    const uint16_t *pBitRevTable;      /**< points to the bit reversal table. */
+          uint16_t bitRevLength;             /**< bit reversal table length. */
+#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE)
+   const uint32_t *rearranged_twiddle_tab_stride1_arr;        /**< Per stage reordered twiddle pointer (offset 1) */                                                       \
+   const uint32_t *rearranged_twiddle_tab_stride2_arr;        /**< Per stage reordered twiddle pointer (offset 2) */                                                       \
+   const uint32_t *rearranged_twiddle_tab_stride3_arr;        /**< Per stage reordered twiddle pointer (offset 3) */                                                       \
+   const q15_t *rearranged_twiddle_stride1; /**< reordered twiddle offset 1 storage */                                                                   \
+   const q15_t *rearranged_twiddle_stride2; /**< reordered twiddle offset 2 storage */                                                                   \
+   const q15_t *rearranged_twiddle_stride3;
+#endif
+  } arm_cfft_instance_q15;
+
+arm_status arm_cfft_init_q15(
+  arm_cfft_instance_q15 * S,
+  uint16_t fftLen);
+
+void arm_cfft_q15(
+    const arm_cfft_instance_q15 * S,
+          q15_t * p1,
+          uint8_t ifftFlag,
+          uint8_t bitReverseFlag);
+
+  /**
+   * @brief Instance structure for the fixed-point CFFT/CIFFT function.
+   */
+  typedef struct
+  {
+          uint16_t fftLen;                   /**< length of the FFT. */
+    const q31_t *pTwiddle;             /**< points to the Twiddle factor table. */
+    const uint16_t *pBitRevTable;      /**< points to the bit reversal table. */
+          uint16_t bitRevLength;             /**< bit reversal table length. */
+#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE)
+   const uint32_t *rearranged_twiddle_tab_stride1_arr;        /**< Per stage reordered twiddle pointer (offset 1) */                                                       \
+   const uint32_t *rearranged_twiddle_tab_stride2_arr;        /**< Per stage reordered twiddle pointer (offset 2) */                                                       \
+   const uint32_t *rearranged_twiddle_tab_stride3_arr;        /**< Per stage reordered twiddle pointer (offset 3) */                                                       \
+   const q31_t *rearranged_twiddle_stride1; /**< reordered twiddle offset 1 storage */                                                                   \
+   const q31_t *rearranged_twiddle_stride2; /**< reordered twiddle offset 2 storage */                                                                   \
+   const q31_t *rearranged_twiddle_stride3;
+#endif
+  } arm_cfft_instance_q31;
+
+arm_status arm_cfft_init_q31(
+  arm_cfft_instance_q31 * S,
+  uint16_t fftLen);
+
+void arm_cfft_q31(
+    const arm_cfft_instance_q31 * S,
+          q31_t * p1,
+          uint8_t ifftFlag,
+          uint8_t bitReverseFlag);
+
+  /**
+   * @brief Instance structure for the floating-point CFFT/CIFFT function.
+   */
+  typedef struct
+  {
+          uint16_t fftLen;                   /**< length of the FFT. */
+    const float32_t *pTwiddle;         /**< points to the Twiddle factor table. */
+    const uint16_t *pBitRevTable;      /**< points to the bit reversal table. */
+          uint16_t bitRevLength;             /**< bit reversal table length. */
+#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE)
+   const uint32_t *rearranged_twiddle_tab_stride1_arr;        /**< Per stage reordered twiddle pointer (offset 1) */                                                       \
+   const uint32_t *rearranged_twiddle_tab_stride2_arr;        /**< Per stage reordered twiddle pointer (offset 2) */                                                       \
+   const uint32_t *rearranged_twiddle_tab_stride3_arr;        /**< Per stage reordered twiddle pointer (offset 3) */                                                       \
+   const float32_t *rearranged_twiddle_stride1; /**< reordered twiddle offset 1 storage */                                                                   \
+   const float32_t *rearranged_twiddle_stride2; /**< reordered twiddle offset 2 storage */                                                                   \
+   const float32_t *rearranged_twiddle_stride3;
+#endif
+  } arm_cfft_instance_f32;
+
+
+
+  arm_status arm_cfft_init_f32(
+  arm_cfft_instance_f32 * S,
+  uint16_t fftLen);
+
+  void arm_cfft_f32(
+  const arm_cfft_instance_f32 * S,
+        float32_t * p1,
+        uint8_t ifftFlag,
+        uint8_t bitReverseFlag);
+
+
+  /**
+   * @brief Instance structure for the Double Precision Floating-point CFFT/CIFFT function.
+   */
+  typedef struct
+  {
+          uint16_t fftLen;                   /**< length of the FFT. */
+    const float64_t *pTwiddle;         /**< points to the Twiddle factor table. */
+    const uint16_t *pBitRevTable;      /**< points to the bit reversal table. */
+          uint16_t bitRevLength;             /**< bit reversal table length. */
+  } arm_cfft_instance_f64;
+
+  arm_status arm_cfft_init_f64(
+  arm_cfft_instance_f64 * S,
+  uint16_t fftLen);
+  
+  void arm_cfft_f64(
+  const arm_cfft_instance_f64 * S,
+        float64_t * p1,
+        uint8_t ifftFlag,
+        uint8_t bitReverseFlag);
+
+  /**
+   * @brief Instance structure for the Q15 RFFT/RIFFT function.
+   */
+  typedef struct
+  {
+          uint32_t fftLenReal;                      /**< length of the real FFT. */
+          uint8_t ifftFlagR;                        /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */
+          uint8_t bitReverseFlagR;                  /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */
+          uint32_t twidCoefRModifier;               /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+    const q15_t *pTwiddleAReal;                     /**< points to the real twiddle factor table. */
+    const q15_t *pTwiddleBReal;                     /**< points to the imag twiddle factor table. */
+#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE)
+    arm_cfft_instance_q15 cfftInst;
+#else
+    const arm_cfft_instance_q15 *pCfft;       /**< points to the complex FFT instance. */
+#endif
+  } arm_rfft_instance_q15;
+
+  arm_status arm_rfft_init_q15(
+        arm_rfft_instance_q15 * S,
+        uint32_t fftLenReal,
+        uint32_t ifftFlagR,
+        uint32_t bitReverseFlag);
+
+  void arm_rfft_q15(
+  const arm_rfft_instance_q15 * S,
+        q15_t * pSrc,
+        q15_t * pDst);
+
+  /**
+   * @brief Instance structure for the Q31 RFFT/RIFFT function.
+   */
+  typedef struct
+  {
+          uint32_t fftLenReal;                        /**< length of the real FFT. */
+          uint8_t ifftFlagR;                          /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */
+          uint8_t bitReverseFlagR;                    /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */
+          uint32_t twidCoefRModifier;                 /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+    const q31_t *pTwiddleAReal;                       /**< points to the real twiddle factor table. */
+    const q31_t *pTwiddleBReal;                       /**< points to the imag twiddle factor table. */
+#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE)
+    arm_cfft_instance_q31 cfftInst;
+#else
+    const arm_cfft_instance_q31 *pCfft;         /**< points to the complex FFT instance. */
+#endif
+  } arm_rfft_instance_q31;
+
+  arm_status arm_rfft_init_q31(
+        arm_rfft_instance_q31 * S,
+        uint32_t fftLenReal,
+        uint32_t ifftFlagR,
+        uint32_t bitReverseFlag);
+
+  void arm_rfft_q31(
+  const arm_rfft_instance_q31 * S,
+        q31_t * pSrc,
+        q31_t * pDst);
+
+  /**
+   * @brief Instance structure for the floating-point RFFT/RIFFT function.
+   */
+  typedef struct
+  {
+          uint32_t fftLenReal;                        /**< length of the real FFT. */
+          uint16_t fftLenBy2;                         /**< length of the complex FFT. */
+          uint8_t ifftFlagR;                          /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */
+          uint8_t bitReverseFlagR;                    /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */
+          uint32_t twidCoefRModifier;                     /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+    const float32_t *pTwiddleAReal;                   /**< points to the real twiddle factor table. */
+    const float32_t *pTwiddleBReal;                   /**< points to the imag twiddle factor table. */
+          arm_cfft_radix4_instance_f32 *pCfft;        /**< points to the complex FFT instance. */
+  } arm_rfft_instance_f32;
+
+  arm_status arm_rfft_init_f32(
+        arm_rfft_instance_f32 * S,
+        arm_cfft_radix4_instance_f32 * S_CFFT,
+        uint32_t fftLenReal,
+        uint32_t ifftFlagR,
+        uint32_t bitReverseFlag);
+
+  void arm_rfft_f32(
+  const arm_rfft_instance_f32 * S,
+        float32_t * pSrc,
+        float32_t * pDst);
+
+  /**
+   * @brief Instance structure for the Double Precision Floating-point RFFT/RIFFT function.
+   */
+typedef struct
+  {
+          arm_cfft_instance_f64 Sint;      /**< Internal CFFT structure. */
+          uint16_t fftLenRFFT;             /**< length of the real sequence */
+    const float64_t * pTwiddleRFFT;        /**< Twiddle factors real stage  */
+  } arm_rfft_fast_instance_f64 ;
+
+arm_status arm_rfft_fast_init_f64 (
+         arm_rfft_fast_instance_f64 * S,
+         uint16_t fftLen);
+
+
+void arm_rfft_fast_f64(
+    arm_rfft_fast_instance_f64 * S,
+    float64_t * p, float64_t * pOut,
+    uint8_t ifftFlag);
+
+
+  /**
+   * @brief Instance structure for the floating-point RFFT/RIFFT function.
+   */
+typedef struct
+  {
+          arm_cfft_instance_f32 Sint;      /**< Internal CFFT structure. */
+          uint16_t fftLenRFFT;             /**< length of the real sequence */
+    const float32_t * pTwiddleRFFT;        /**< Twiddle factors real stage  */
+  } arm_rfft_fast_instance_f32 ;
+
+arm_status arm_rfft_fast_init_f32 (
+         arm_rfft_fast_instance_f32 * S,
+         uint16_t fftLen);
+
+
+  void arm_rfft_fast_f32(
+        const arm_rfft_fast_instance_f32 * S,
+        float32_t * p, float32_t * pOut,
+        uint8_t ifftFlag);
+
+  /**
+   * @brief Instance structure for the floating-point DCT4/IDCT4 function.
+   */
+  typedef struct
+  {
+          uint16_t N;                          /**< length of the DCT4. */
+          uint16_t Nby2;                       /**< half of the length of the DCT4. */
+          float32_t normalize;                 /**< normalizing factor. */
+    const float32_t *pTwiddle;                 /**< points to the twiddle factor table. */
+    const float32_t *pCosFactor;               /**< points to the cosFactor table. */
+          arm_rfft_instance_f32 *pRfft;        /**< points to the real FFT instance. */
+          arm_cfft_radix4_instance_f32 *pCfft; /**< points to the complex FFT instance. */
+  } arm_dct4_instance_f32;
+
+
+  /**
+   * @brief  Initialization function for the floating-point DCT4/IDCT4.
+   * @param[in,out] S          points to an instance of floating-point DCT4/IDCT4 structure.
+   * @param[in]     S_RFFT     points to an instance of floating-point RFFT/RIFFT structure.
+   * @param[in]     S_CFFT     points to an instance of floating-point CFFT/CIFFT structure.
+   * @param[in]     N          length of the DCT4.
+   * @param[in]     Nby2       half of the length of the DCT4.
+   * @param[in]     normalize  normalizing factor.
+   * @return      arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if <code>fftLenReal</code> is not a supported transform length.
+   */
+  arm_status arm_dct4_init_f32(
+        arm_dct4_instance_f32 * S,
+        arm_rfft_instance_f32 * S_RFFT,
+        arm_cfft_radix4_instance_f32 * S_CFFT,
+        uint16_t N,
+        uint16_t Nby2,
+        float32_t normalize);
+
+
+  /**
+   * @brief Processing function for the floating-point DCT4/IDCT4.
+   * @param[in]     S              points to an instance of the floating-point DCT4/IDCT4 structure.
+   * @param[in]     pState         points to state buffer.
+   * @param[in,out] pInlineBuffer  points to the in-place input and output buffer.
+   */
+  void arm_dct4_f32(
+  const arm_dct4_instance_f32 * S,
+        float32_t * pState,
+        float32_t * pInlineBuffer);
+
+
+  /**
+   * @brief Instance structure for the Q31 DCT4/IDCT4 function.
+   */
+  typedef struct
+  {
+          uint16_t N;                          /**< length of the DCT4. */
+          uint16_t Nby2;                       /**< half of the length of the DCT4. */
+          q31_t normalize;                     /**< normalizing factor. */
+    const q31_t *pTwiddle;                     /**< points to the twiddle factor table. */
+    const q31_t *pCosFactor;                   /**< points to the cosFactor table. */
+          arm_rfft_instance_q31 *pRfft;        /**< points to the real FFT instance. */
+          arm_cfft_radix4_instance_q31 *pCfft; /**< points to the complex FFT instance. */
+  } arm_dct4_instance_q31;
+
+
+  /**
+   * @brief  Initialization function for the Q31 DCT4/IDCT4.
+   * @param[in,out] S          points to an instance of Q31 DCT4/IDCT4 structure.
+   * @param[in]     S_RFFT     points to an instance of Q31 RFFT/RIFFT structure
+   * @param[in]     S_CFFT     points to an instance of Q31 CFFT/CIFFT structure
+   * @param[in]     N          length of the DCT4.
+   * @param[in]     Nby2       half of the length of the DCT4.
+   * @param[in]     normalize  normalizing factor.
+   * @return      arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if <code>N</code> is not a supported transform length.
+   */
+  arm_status arm_dct4_init_q31(
+        arm_dct4_instance_q31 * S,
+        arm_rfft_instance_q31 * S_RFFT,
+        arm_cfft_radix4_instance_q31 * S_CFFT,
+        uint16_t N,
+        uint16_t Nby2,
+        q31_t normalize);
+
+
+  /**
+   * @brief Processing function for the Q31 DCT4/IDCT4.
+   * @param[in]     S              points to an instance of the Q31 DCT4 structure.
+   * @param[in]     pState         points to state buffer.
+   * @param[in,out] pInlineBuffer  points to the in-place input and output buffer.
+   */
+  void arm_dct4_q31(
+  const arm_dct4_instance_q31 * S,
+        q31_t * pState,
+        q31_t * pInlineBuffer);
+
+
+  /**
+   * @brief Instance structure for the Q15 DCT4/IDCT4 function.
+   */
+  typedef struct
+  {
+          uint16_t N;                          /**< length of the DCT4. */
+          uint16_t Nby2;                       /**< half of the length of the DCT4. */
+          q15_t normalize;                     /**< normalizing factor. */
+    const q15_t *pTwiddle;                     /**< points to the twiddle factor table. */
+    const q15_t *pCosFactor;                   /**< points to the cosFactor table. */
+          arm_rfft_instance_q15 *pRfft;        /**< points to the real FFT instance. */
+          arm_cfft_radix4_instance_q15 *pCfft; /**< points to the complex FFT instance. */
+  } arm_dct4_instance_q15;
+
+
+  /**
+   * @brief  Initialization function for the Q15 DCT4/IDCT4.
+   * @param[in,out] S          points to an instance of Q15 DCT4/IDCT4 structure.
+   * @param[in]     S_RFFT     points to an instance of Q15 RFFT/RIFFT structure.
+   * @param[in]     S_CFFT     points to an instance of Q15 CFFT/CIFFT structure.
+   * @param[in]     N          length of the DCT4.
+   * @param[in]     Nby2       half of the length of the DCT4.
+   * @param[in]     normalize  normalizing factor.
+   * @return      arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if <code>N</code> is not a supported transform length.
+   */
+  arm_status arm_dct4_init_q15(
+        arm_dct4_instance_q15 * S,
+        arm_rfft_instance_q15 * S_RFFT,
+        arm_cfft_radix4_instance_q15 * S_CFFT,
+        uint16_t N,
+        uint16_t Nby2,
+        q15_t normalize);
+
+
+  /**
+   * @brief Processing function for the Q15 DCT4/IDCT4.
+   * @param[in]     S              points to an instance of the Q15 DCT4 structure.
+   * @param[in]     pState         points to state buffer.
+   * @param[in,out] pInlineBuffer  points to the in-place input and output buffer.
+   */
+  void arm_dct4_q15(
+  const arm_dct4_instance_q15 * S,
+        q15_t * pState,
+        q15_t * pInlineBuffer);
+
+  /**
+   * @brief Instance structure for the Floating-point MFCC function.
+   */
+typedef struct
+  {
+     const float32_t *dctCoefs; /**< Internal DCT coefficients */
+     const float32_t *filterCoefs; /**< Internal Mel filter coefficients */ 
+     const float32_t *windowCoefs; /**< Windowing coefficients */ 
+     const uint32_t *filterPos; /**< Internal Mel filter positions in spectrum */ 
+     const uint32_t *filterLengths; /**< Internal Mel filter  lengths */ 
+     uint32_t fftLen; /**< FFT length */
+     uint32_t nbMelFilters; /**< Number of Mel filters */
+     uint32_t nbDctOutputs; /**< Number of DCT outputs */
+#if defined(ARM_MFCC_CFFT_BASED)
+     /* Implementation of the MFCC is using a CFFT */
+     arm_cfft_instance_f32 cfft; /**< Internal CFFT instance */
+#else
+     /* Implementation of the MFCC is using a RFFT (default) */
+     arm_rfft_fast_instance_f32 rfft;
+#endif
+  } arm_mfcc_instance_f32 ;
+
+arm_status arm_mfcc_init_f32(
+  arm_mfcc_instance_f32 * S,
+  uint32_t fftLen,
+  uint32_t nbMelFilters,
+  uint32_t nbDctOutputs,
+  const float32_t *dctCoefs,
+  const uint32_t *filterPos,
+  const uint32_t *filterLengths,
+  const float32_t *filterCoefs,
+  const float32_t *windowCoefs
+  );
+
+
+/**
+  @brief         MFCC F32
+  @param[in]    S       points to the mfcc instance structure
+  @param[in]     pSrc points to the input samples
+  @param[out]     pDst  points to the output MFCC values
+  @param[inout]     pTmp  points to a temporary buffer of complex
+  @return        none
+ */
+  void arm_mfcc_f32(
+  const arm_mfcc_instance_f32 * S,
+  float32_t *pSrc,
+  float32_t *pDst,
+  float32_t *pTmp
+  );
+
+typedef struct
+  {
+     const q31_t *dctCoefs; /**< Internal DCT coefficients */
+     const q31_t *filterCoefs; /**< Internal Mel filter coefficients */ 
+     const q31_t *windowCoefs; /**< Windowing coefficients */ 
+     const uint32_t *filterPos; /**< Internal Mel filter positions in spectrum */ 
+     const uint32_t *filterLengths; /**< Internal Mel filter  lengths */ 
+     uint32_t fftLen; /**< FFT length */
+     uint32_t nbMelFilters; /**< Number of Mel filters */
+     uint32_t nbDctOutputs; /**< Number of DCT outputs */
+#if defined(ARM_MFCC_CFFT_BASED)
+     /* Implementation of the MFCC is using a CFFT */
+     arm_cfft_instance_q31 cfft; /**< Internal CFFT instance */
+#else
+     /* Implementation of the MFCC is using a RFFT (default) */
+     arm_rfft_instance_q31 rfft;
+#endif
+  } arm_mfcc_instance_q31 ;
+
+arm_status arm_mfcc_init_q31(
+  arm_mfcc_instance_q31 * S,
+  uint32_t fftLen,
+  uint32_t nbMelFilters,
+  uint32_t nbDctOutputs,
+  const q31_t *dctCoefs,
+  const uint32_t *filterPos,
+  const uint32_t *filterLengths,
+  const q31_t *filterCoefs,
+  const q31_t *windowCoefs
+  );
+
+
+/**
+  @brief         MFCC Q31
+  @param[in]    S       points to the mfcc instance structure
+  @param[in]     pSrc points to the input samples
+  @param[out]     pDst  points to the output MFCC values
+  @param[inout]     pTmp  points to a temporary buffer of complex
+  @return        none
+ */
+  arm_status arm_mfcc_q31(
+  const arm_mfcc_instance_q31 * S,
+  q31_t *pSrc,
+  q31_t *pDst,
+  q31_t *pTmp
+  );
+
+typedef struct
+  {
+     const q15_t *dctCoefs; /**< Internal DCT coefficients */
+     const q15_t *filterCoefs; /**< Internal Mel filter coefficients */ 
+     const q15_t *windowCoefs; /**< Windowing coefficients */ 
+     const uint32_t *filterPos; /**< Internal Mel filter positions in spectrum */ 
+     const uint32_t *filterLengths; /**< Internal Mel filter  lengths */ 
+     uint32_t fftLen; /**< FFT length */
+     uint32_t nbMelFilters; /**< Number of Mel filters */
+     uint32_t nbDctOutputs; /**< Number of DCT outputs */
+#if defined(ARM_MFCC_CFFT_BASED)
+     /* Implementation of the MFCC is using a CFFT */
+     arm_cfft_instance_q15 cfft; /**< Internal CFFT instance */
+#else
+     /* Implementation of the MFCC is using a RFFT (default) */
+     arm_rfft_instance_q15 rfft;
+#endif
+  } arm_mfcc_instance_q15 ;
+
+arm_status arm_mfcc_init_q15(
+  arm_mfcc_instance_q15 * S,
+  uint32_t fftLen,
+  uint32_t nbMelFilters,
+  uint32_t nbDctOutputs,
+  const q15_t *dctCoefs,
+  const uint32_t *filterPos,
+  const uint32_t *filterLengths,
+  const q15_t *filterCoefs,
+  const q15_t *windowCoefs
+  );
+
+
+/**
+  @brief         MFCC Q15
+  @param[in]    S       points to the mfcc instance structure
+  @param[in]     pSrc points to the input samples
+  @param[out]     pDst  points to the output MFCC values in q8.7 format
+  @param[inout]     pTmp  points to a temporary buffer of complex
+  @return        error status
+ */
+  arm_status arm_mfcc_q15(
+  const arm_mfcc_instance_q15 * S,
+  q15_t *pSrc,
+  q15_t *pDst,
+  q31_t *pTmp
+  );
+
+
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _TRANSFORM_FUNCTIONS_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/transform_functions_f16.h

@@ -0,0 +1,208 @@
+/******************************************************************************
+ * @file     transform_functions_f16.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.10.0
+ * @date     08 July 2021
+ * Target Processor: Cortex-M and Cortex-A cores
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+ 
+#ifndef _TRANSFORM_FUNCTIONS_F16_H_
+#define _TRANSFORM_FUNCTIONS_F16_H_
+
+#include "arm_math_types_f16.h"
+#include "arm_math_memory.h"
+
+#include "dsp/none.h"
+#include "dsp/utils.h"
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+
+
+#if defined(ARM_FLOAT16_SUPPORTED)
+
+
+  /**
+   * @brief Instance structure for the floating-point CFFT/CIFFT function.
+   */
+  typedef struct
+  {
+          uint16_t fftLen;                   /**< length of the FFT. */
+          uint8_t ifftFlag;                  /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
+          uint8_t bitReverseFlag;            /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
+    const float16_t *pTwiddle;               /**< points to the Twiddle factor table. */
+    const uint16_t *pBitRevTable;            /**< points to the bit reversal table. */
+          uint16_t twidCoefModifier;         /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+          uint16_t bitRevFactor;             /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
+          float16_t onebyfftLen;             /**< value of 1/fftLen. */
+  } arm_cfft_radix2_instance_f16;
+
+  /**
+   * @brief Instance structure for the floating-point CFFT/CIFFT function.
+   */
+  typedef struct
+  {
+          uint16_t fftLen;                   /**< length of the FFT. */
+          uint8_t ifftFlag;                  /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
+          uint8_t bitReverseFlag;            /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
+    const float16_t *pTwiddle;               /**< points to the Twiddle factor table. */
+    const uint16_t *pBitRevTable;            /**< points to the bit reversal table. */
+          uint16_t twidCoefModifier;         /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+          uint16_t bitRevFactor;             /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
+          float16_t onebyfftLen;             /**< value of 1/fftLen. */
+  } arm_cfft_radix4_instance_f16;
+
+  /**
+   * @brief Instance structure for the floating-point CFFT/CIFFT function.
+   */
+  typedef struct
+  {
+          uint16_t fftLen;                   /**< length of the FFT. */
+    const float16_t *pTwiddle;         /**< points to the Twiddle factor table. */
+    const uint16_t *pBitRevTable;      /**< points to the bit reversal table. */
+          uint16_t bitRevLength;             /**< bit reversal table length. */
+#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE)
+   const uint32_t *rearranged_twiddle_tab_stride1_arr;        /**< Per stage reordered twiddle pointer (offset 1) */                                                       \
+   const uint32_t *rearranged_twiddle_tab_stride2_arr;        /**< Per stage reordered twiddle pointer (offset 2) */                                                       \
+   const uint32_t *rearranged_twiddle_tab_stride3_arr;        /**< Per stage reordered twiddle pointer (offset 3) */                                                       \
+   const float16_t *rearranged_twiddle_stride1; /**< reordered twiddle offset 1 storage */                                                                   \
+   const float16_t *rearranged_twiddle_stride2; /**< reordered twiddle offset 2 storage */                                                                   \
+   const float16_t *rearranged_twiddle_stride3;
+#endif
+  } arm_cfft_instance_f16;
+
+
+  arm_status arm_cfft_init_f16(
+  arm_cfft_instance_f16 * S,
+  uint16_t fftLen);
+
+  void arm_cfft_f16(
+  const arm_cfft_instance_f16 * S,
+        float16_t * p1,
+        uint8_t ifftFlag,
+        uint8_t bitReverseFlag);
+
+  /**
+   * @brief Instance structure for the floating-point RFFT/RIFFT function.
+   */
+typedef struct
+  {
+          arm_cfft_instance_f16 Sint;      /**< Internal CFFT structure. */
+          uint16_t fftLenRFFT;             /**< length of the real sequence */
+    const float16_t * pTwiddleRFFT;        /**< Twiddle factors real stage  */
+  } arm_rfft_fast_instance_f16 ;
+
+arm_status arm_rfft_fast_init_f16 (
+         arm_rfft_fast_instance_f16 * S,
+         uint16_t fftLen);
+
+
+  void arm_rfft_fast_f16(
+        const arm_rfft_fast_instance_f16 * S,
+        float16_t * p, float16_t * pOut,
+        uint8_t ifftFlag);
+
+/* Deprecated */
+  arm_status arm_cfft_radix4_init_f16(
+        arm_cfft_radix4_instance_f16 * S,
+        uint16_t fftLen,
+        uint8_t ifftFlag,
+        uint8_t bitReverseFlag);
+
+/* Deprecated */
+  void arm_cfft_radix4_f16(
+  const arm_cfft_radix4_instance_f16 * S,
+        float16_t * pSrc);
+
+
+/* Deprecated */
+  arm_status arm_cfft_radix2_init_f16(
+        arm_cfft_radix2_instance_f16 * S,
+        uint16_t fftLen,
+        uint8_t ifftFlag,
+        uint8_t bitReverseFlag);
+
+/* Deprecated */
+  void arm_cfft_radix2_f16(
+  const arm_cfft_radix2_instance_f16 * S,
+        float16_t * pSrc);
+
+  /**
+   * @brief Instance structure for the Floating-point MFCC function.
+   */
+typedef struct
+  {
+     const float16_t *dctCoefs; /**< Internal DCT coefficients */
+     const float16_t *filterCoefs; /**< Internal Mel filter coefficients */ 
+     const float16_t *windowCoefs; /**< Windowing coefficients */ 
+     const uint32_t *filterPos; /**< Internal Mel filter positions in spectrum */ 
+     const uint32_t *filterLengths; /**< Internal Mel filter  lengths */ 
+     uint32_t fftLen; /**< FFT length */
+     uint32_t nbMelFilters; /**< Number of Mel filters */
+     uint32_t nbDctOutputs; /**< Number of DCT outputs */
+#if defined(ARM_MFCC_CFFT_BASED)
+     /* Implementation of the MFCC is using a CFFT */
+     arm_cfft_instance_f16 cfft; /**< Internal CFFT instance */
+#else
+     /* Implementation of the MFCC is using a RFFT (default) */
+     arm_rfft_fast_instance_f16 rfft;
+#endif
+  } arm_mfcc_instance_f16 ;
+
+arm_status arm_mfcc_init_f16(
+  arm_mfcc_instance_f16 * S,
+  uint32_t fftLen,
+  uint32_t nbMelFilters,
+  uint32_t nbDctOutputs,
+  const float16_t *dctCoefs,
+  const uint32_t *filterPos,
+  const uint32_t *filterLengths,
+  const float16_t *filterCoefs,
+  const float16_t *windowCoefs
+  );
+
+
+/**
+  @brief         MFCC F16
+  @param[in]    S       points to the mfcc instance structure
+  @param[in]     pSrc points to the input samples
+  @param[out]     pDst  points to the output MFCC values
+  @param[inout]     pTmp  points to a temporary buffer of complex
+  @return        none
+ */
+  void arm_mfcc_f16(
+  const arm_mfcc_instance_f16 * S,
+  float16_t *pSrc,
+  float16_t *pDst,
+  float16_t *pTmp
+  );
+
+  
+#endif /* defined(ARM_FLOAT16_SUPPORTED)*/
+
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /* ifndef _TRANSFORM_FUNCTIONS_F16_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/dsp/utils.h

@@ -0,0 +1,240 @@
+/******************************************************************************
+ * @file     arm_math_utils.h
+ * @brief    Public header file for CMSIS DSP Library
+ * @version  V1.9.0
+ * @date     20. July 2020
+ ******************************************************************************/
+/*
+ * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * 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
+ *
+ * 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.
+ */
+
+#ifndef _ARM_MATH_UTILS_H_
+
+#define _ARM_MATH_UTILS_H_
+
+#include "arm_math_types.h"
+
+#ifdef   __cplusplus
+extern "C"
+{
+#endif
+
+  /**
+   * @brief Macros required for reciprocal calculation in Normalized LMS
+   */
+
+#define INDEX_MASK         0x0000003F
+
+
+#define SQ(x) ((x) * (x))
+
+#define ROUND_UP(N, S) ((((N) + (S) - 1) / (S)) * (S))
+
+
+  /**
+   * @brief Function to Calculates 1/in (reciprocal) value of Q31 Data type.
+   */
+  __STATIC_FORCEINLINE uint32_t arm_recip_q31(
+        q31_t in,
+        q31_t * dst,
+  const q31_t * pRecipTable)
+  {
+    q31_t out;
+    uint32_t tempVal;
+    uint32_t index, i;
+    uint32_t signBits;
+
+    if (in > 0)
+    {
+      signBits = ((uint32_t) (__CLZ( in) - 1));
+    }
+    else
+    {
+      signBits = ((uint32_t) (__CLZ(-in) - 1));
+    }
+
+    /* Convert input sample to 1.31 format */
+    in = (in << signBits);
+
+    /* calculation of index for initial approximated Val */
+    index = (uint32_t)(in >> 24);
+    index = (index & INDEX_MASK);
+
+    /* 1.31 with exp 1 */
+    out = pRecipTable[index];
+
+    /* calculation of reciprocal value */
+    /* running approximation for two iterations */
+    for (i = 0U; i < 2U; i++)
+    {
+      tempVal = (uint32_t) (((q63_t) in * out) >> 31);
+      tempVal = 0x7FFFFFFFu - tempVal;
+      /*      1.31 with exp 1 */
+      /* out = (q31_t) (((q63_t) out * tempVal) >> 30); */
+      out = clip_q63_to_q31(((q63_t) out * tempVal) >> 30);
+    }
+
+    /* write output */
+    *dst = out;
+
+    /* return num of signbits of out = 1/in value */
+    return (signBits + 1U);
+  }
+
+
+  /**
+   * @brief Function to Calculates 1/in (reciprocal) value of Q15 Data type.
+   */
+  __STATIC_FORCEINLINE uint32_t arm_recip_q15(
+        q15_t in,
+        q15_t * dst,
+  const q15_t * pRecipTable)
+  {
+    q15_t out = 0;
+    uint32_t tempVal = 0;
+    uint32_t index = 0, i = 0;
+    uint32_t signBits = 0;
+
+    if (in > 0)
+    {
+      signBits = ((uint32_t)(__CLZ( in) - 17));
+    }
+    else
+    {
+      signBits = ((uint32_t)(__CLZ(-in) - 17));
+    }
+
+    /* Convert input sample to 1.15 format */
+    in = (in << signBits);
+
+    /* calculation of index for initial approximated Val */
+    index = (uint32_t)(in >>  8);
+    index = (index & INDEX_MASK);
+
+    /*      1.15 with exp 1  */
+    out = pRecipTable[index];
+
+    /* calculation of reciprocal value */
+    /* running approximation for two iterations */
+    for (i = 0U; i < 2U; i++)
+    {
+      tempVal = (uint32_t) (((q31_t) in * out) >> 15);
+      tempVal = 0x7FFFu - tempVal;
+      /*      1.15 with exp 1 */
+      out = (q15_t) (((q31_t) out * tempVal) >> 14);
+      /* out = clip_q31_to_q15(((q31_t) out * tempVal) >> 14); */
+    }
+
+    /* write output */
+    *dst = out;
+
+    /* return num of signbits of out = 1/in value */
+    return (signBits + 1);
+  }
+
+
+/**
+ * @brief  64-bit to 32-bit unsigned normalization
+ * @param[in]  in           is input unsigned long long value
+ * @param[out] normalized   is the 32-bit normalized value
+ * @param[out] norm         is norm scale
+ */
+__STATIC_INLINE  void arm_norm_64_to_32u(uint64_t in, int32_t * normalized, int32_t *norm)
+{
+    int32_t     n1;
+    int32_t     hi = (int32_t) (in >> 32);
+    int32_t     lo = (int32_t) ((in << 32) >> 32);
+
+    n1 = __CLZ(hi) - 32;
+    if (!n1)
+    {
+        /*
+         * input fits in 32-bit
+         */
+        n1 = __CLZ(lo);
+        if (!n1)
+        {
+            /*
+             * MSB set, need to scale down by 1
+             */
+            *norm = -1;
+            *normalized = (((uint32_t) lo) >> 1);
+        } else
+        {
+            if (n1 == 32)
+            {
+                /*
+                 * input is zero
+                 */
+                *norm = 0;
+                *normalized = 0;
+            } else
+            {
+                /*
+                 * 32-bit normalization
+                 */
+                *norm = n1 - 1;
+                *normalized = lo << *norm;
+            }
+        }
+    } else
+    {
+        /*
+         * input fits in 64-bit
+         */
+        n1 = 1 - n1;
+        *norm = -n1;
+        /*
+         * 64 bit normalization
+         */
+        *normalized = (((uint32_t) lo) >> n1) | (hi << (32 - n1));
+    }
+}
+
+__STATIC_INLINE q31_t arm_div_q63_to_q31(q63_t num, q31_t den)
+{
+    q31_t   result;
+    uint64_t   absNum;
+    int32_t   normalized;
+    int32_t   norm;
+
+    /*
+     * if sum fits in 32bits
+     * avoid costly 64-bit division
+     */
+    absNum = num > 0 ? num : -num;
+    arm_norm_64_to_32u(absNum, &normalized, &norm);
+    if (norm > 0)
+        /*
+         * 32-bit division
+         */
+        result = (q31_t) num / den;
+    else
+        /*
+         * 64-bit division
+         */
+        result = (q31_t) (num / den);
+
+    return result;
+}
+
+
+#ifdef   __cplusplus
+}
+#endif
+
+#endif /*ifndef _ARM_MATH_UTILS_H_ */

APP_Framework/Applications/robot_app/motor_control_algo/foc_basic/include/foc.h

@@ -0,0 +1,104 @@
+#pragma once
+
+#define rad60 deg2rad(60)
+#define SQRT3 1.73205080756887729353
+#define deg2rad(a) (PI * (a) / 180)
+#define rad2deg(a) (180 * (a) / PI)
+#define max(a, b) ((a) > (b) ? (a) : (b))
+#define min(a, b) ((a) < (b) ? (a) : (b))
+
+
+
+
+
+/********************************************
+         * 0 不进行控制
+         * 1 速度环               速度
+         * 2 力矩环               转矩
+         * 
+         * 4 位置环               VF
+         * 5 MIT控制              DQ
+         ********************************************/
+typedef enum
+{
+    control_type_null,                  // 0不进行控制
+    control_type_speed,                 // 1速度控制
+    control_type_torque,                // 2力矩控制 
+    control_type_speed_torque,          // 3速度-力矩控制
+    control_type_position,              // 4位置控制 
+    control_type_mit_control,           // 5MIT控制
+    control_type_position_speed_torque, // 位置-速度-力矩控制
+} motor_control_type;
+
+typedef enum
+{
+    motor_idle,
+    motor_running,
+    motor_fault,
+    motor_stall,
+    motor_break
+} motor_status_e;
+
+typedef struct
+{
+    motor_control_type type;
+    float position;        // 目标角度，单位度
+    float speed;           // 目标速度，单位rad/s
+    float torque_norm_d;   // 目标d轴强度，0~1
+    float torque_norm_q;   // 目标q轴强度，0~1
+    float max_speed;       // 串级控制时的最大速度，单位rad/s
+    float max_torque_norm; // 串级控制时的最大q轴力矩，0~1
+    float pid_value;       // PID参数值
+    float mit_kp;          //MIT控制kp值
+    float mit_kd;          //MIT控制kd值
+    float mit_torque;      //MIT控制目标力矩值
+    int control_mode;
+    float pwm_u;          //u项占空比
+    int position_reached_flag;
+    int count;
+} motor_control_context_t;
+
+extern motor_control_context_t motor_control_context;
+extern motor_status_e motor_status;
+
+
+void foc_start();
+void foc_stop();
+void foc_break();
+void foc_loop();
+float cycle_diff(float diff, float cycle);
+void foc_forward(float d, float q, float rotor_rad);
+float cycle_diff(float diff, float cycle);
+float low_pass_filter(float input, float last_output, float alpha);
+void TIM_Handler();
+
+void lib_position_control(float rad);
+void lib_speed_control(float speed);
+void lib_torque_control(float torque_norm_d, float torque_norm_q);
+void lib_speed_torque_control(float speed_rad);
+void lib_position_speed_torque_control(float position);
+void lib_mit_control(float pos_des, float vel_des,
+                     float kp, float kd, float tau_ff);
+
+void set_motor_pid(
+    float position_p, float position_i, float position_d,
+    float speed_p, float speed_i, float speed_d,
+    float torque_d_p, float torque_d_i, float torque_d_d,
+    float torque_q_p, float torque_q_i, float torque_q_d);
+
+
+
+
+typedef struct {
+    float kp;
+    float ki;
+    float kd;
+    float integral;
+    float prev_error;
+    float output_limit;
+} PID_t;
+
+// PID 初始化
+void PID_Init(PID_t *pid, float kp, float ki, float kd, float limit);
+// PID 计算
+float PID_Update(PID_t *pid, float error);

APP_Framework/Framework/knowing/Kconfig

@@ -11,4 +11,5 @@ if SUPPORT_KNOWING_FRAMEWORK
         source "$APP_DIR/Framework/knowing/cmsis_5/Kconfig"
         source "$APP_DIR/Framework/knowing/kpu/Kconfig"
         source "$APP_DIR/Framework/knowing/nnom/Kconfig"
+        source "$APP_DIR/Framework/knowing/micropython/Kconfig"
 endif

APP_Framework/Framework/knowing/Makefile

@@ -1,4 +1,4 @@
-SRC_DIR := cmsis_5 filter image_processing  kpu nnom ota tensorflow-lite
+SRC_DIR := cmsis_5 filter image_processing  kpu nnom ota tensorflow-lite micropython
 
 
 include $(KERNEL_ROOT)/compiler.mk

APP_Framework/Framework/knowing/micropython/Kconfig

@@ -0,0 +1,39 @@
+menuconfig USING_MICROPYTHON
+    bool "MicroPython Support"
+    default n
+    help
+        Enable MicroPython interpreter support for XiUOS
+
+if USING_MICROPYTHON
+
+    config USING_MICROPYTHON_EXTMOD
+        bool "External Modules Support"
+        default y
+        help
+            Enable MicroPython external modules (machine, platform, etc.)
+
+    config USING_MICROPYTHON_SHARED_READLINE
+        bool "Readline Support"
+        default y
+        help
+            Enable readline library for REPL interaction
+
+    config USING_MICROPYTHON_SHARED_RUNTIME
+        bool "Runtime Components"
+        default y
+        help
+            Enable shared runtime components
+
+    config MICROPYTHON_GC_HEAP_SIZE
+        int "GC Heap Size (bytes)"
+        default 16384
+        help
+            Garbage collector heap size in bytes
+
+    config MICROPYTHON_STACK_SIZE
+        int "Python Stack Size (bytes)"
+        default 8192
+        help
+            Python interpreter stack size in bytes
+
+endif

APP_Framework/Framework/knowing/micropython/Makefile

@@ -0,0 +1,131 @@
+ifeq ($(CONFIG_USING_MICROPYTHON),y)
+
+# Calculate absolute path to the micropython directory
+MICROPYTHON_DIR := $(abspath $(dir $(lastword $(MAKEFILE_LIST))))
+
+# Set up MicroPython build directory
+BUILD ?= $(MICROPYTHON_DIR)/build
+HEADER_BUILD ?= $(MICROPYTHON_DIR)/genhdr
+
+# Choose build method: "full" (with QSTR) or "simple" (minimal headers)
+MICROPYTHON_BUILD_MODE ?= simple
+
+# XiUOS specific configuration
+CFLAGS += -I$(MICROPYTHON_DIR) -I$(BUILD) -I$(MICROPYTHON_DIR)
+CFLAGS += -I$(MICROPYTHON_DIR)/py -I$(MICROPYTHON_DIR)/py_port -I$(MICROPYTHON_DIR)/extmod -I$(MICROPYTHON_DIR)/shared -I$(MICROPYTHON_DIR)/shared/readline -I$(MICROPYTHON_DIR)/shared/runtime
+# Only include FSTRINGS since other flags are defined in mpconfigport.h
+CFLAGS += -DMICROPY_PY_FSTRINGS=1
+
+# Add path to search for source files
+VPATH += $(MICROPYTHON_DIR)
+
+ifeq ($(MICROPYTHON_BUILD_MODE),full)
+# # Full build with QSTR generation
+# .PHONY: generate-qstr-headers
+# generate-qstr-headers:
+# 	@$(ECHO) "Generating MicroPython QSTR headers (full mode)..."
+# 	@chmod +x $(MICROPYTHON_DIR)/generate_qstr_headers.sh
+# 	@$(MICROPYTHON_DIR)/generate_qstr_headers.sh
+
+# MicroPython source files for full build
+SRC_C = \
+	$(MICROPYTHON_DIR)/py_port/py_main.c \
+	$(MICROPYTHON_DIR)/py_port/mphalport.c \
+	$(MICROPYTHON_DIR)/shared/readline/readline.c \
+	$(MICROPYTHON_DIR)/shared/runtime/gchelper_generic.c \
+	$(MICROPYTHON_DIR)/shared/runtime/pyexec.c \
+	$(MICROPYTHON_DIR)/shared/runtime/stdout_helpers.c
+
+# Get MicroPython core object files
+PY_CORE_SOURCES = $(wildcard $(MICROPYTHON_DIR)/py/*.c)
+PY_CORE_O_BASENAME = $(notdir $(PY_CORE_SOURCES))
+PY_CORE_O = $(addprefix $(BUILD)/py/, $(PY_CORE_O_BASENAME:.c=.o))
+
+# All objects including core and port files
+OBJ = $(PY_CORE_O) $(addprefix $(BUILD)/, $(SRC_C:$(MICROPYTHON_DIR)/%.c=%.o))
+
+# Ensure qstr headers are generated before any compilation happens
+QSTR_HEADERS := $(HEADER_BUILD)/qstrdefs.generated.h $(HEADER_BUILD)/mpversion.h $(HEADER_BUILD)/moduledefs.h $(HEADER_BUILD)/root_pointers.h
+
+# Make XiUOS compiler target depend on qstr headers
+COMPILER: generate-qstr-headers mpconfigport.h
+
+# Set build directories for object files
+PY_BUILD = $(BUILD)/py
+
+# Rules to build MicroPython objects (full mode)
+$(PY_BUILD)/%.o: $(MICROPYTHON_DIR)/py/%.c $(QSTR_HEADERS)
+	@mkdir -p $(dir $@)
+	@echo "CC $<"
+	@$(CROSS_COMPILE)gcc $(CFLAGS) -c $< -o $@
+
+$(BUILD)/%.o: $(MICROPYTHON_DIR)/%.c $(QSTR_HEADERS)
+	@mkdir -p $(dir $@)
+	@echo "CC $<"
+	@$(CROSS_COMPILE)gcc $(CFLAGS) -c $< -o $@
+
+else
+# Simple build with minimal headers - completely avoid MicroPython's mkrules.mk conflicts
+# Use our conflict-free build system designed specifically for XiUOS
+
+# Use XiUOS-simple build system that has no rule conflicts
+include $(MICROPYTHON_DIR)/xiuos_simple_build.mk
+
+# Generate headers using XiUOS-compatible method
+.PHONY: generate-xiuos-headers
+generate-xiuos-headers:
+	@$(MAKE) -C $(MICROPYTHON_DIR) -f xiuos_simple_build.mk headers BUILD=$(BUILD) CROSS_COMPILE=$(CROSS_COMPILE)
+
+# Create a symlink or copy for mpconfigport.h to satisfy expectations
+mpconfigport.h:
+	@if [ ! -f mpconfigport.h ]; then \
+		ln -sf $(MICROPYTHON_DIR)/py_port/mpconfigport.h .; \
+	fi
+
+# Set up XiUOS integration using our conflict-free approach
+# The xiuos_simple_build.mk sets SRC_FILES for XiUOS
+COMPILER: generate-xiuos-headers mpconfigport.h
+
+endif
+
+# Display build mode information
+.PHONY: show-build-info
+show-build-info:
+	@$(ECHO) "MicroPython Build Configuration:"
+	@$(ECHO) "  Mode: $(MICROPYTHON_BUILD_MODE)"
+	@$(ECHO) "  Directory: $(MICROPYTHON_DIR)"
+	@$(ECHO) "  Build: $(BUILD)"
+	@$(ECHO) "  Headers: $(HEADER_BUILD)"
+
+else
+# If CONFIG_USING_MICROPYTHON is not set, provide dummy targets
+.PHONY: show-build-info
+show-build-info:
+	@$(ECHO) "MicroPython not enabled in configuration"
+	@$(ECHO) "Set CONFIG_USING_MICROPYTHON=y to enable"
+
+endif
+
+# Always available targets
+.PHONY: help
+help:
+	@echo "MicroPython XiUOS Integration Makefile"
+	@echo ""
+	@echo "Available targets:"
+	@echo "  show-build-info  - Show build configuration"
+	@echo "  help            - Show this help"
+	@echo ""
+	@echo "Build modes:"
+	@echo "  simple          - Use minimal headers (default)"
+	@echo "  full            - Use complete QSTR generation"
+	@echo ""
+	@echo "Example usage:"
+	@echo "  make MICROPYTHON_BUILD_MODE=simple show-build-info"
+	@echo "  make MICROPYTHON_BUILD_MODE=full"
+
+# Include XiUOS compiler definitions  
+# Only include if KERNEL_ROOT is defined (when building within XiUOS)
+ifdef KERNEL_ROOT
+all: COMPILER
+include $(KERNEL_ROOT)/compiler.mk
+endif

APP_Framework/Framework/knowing/micropython/README.md

@@ -0,0 +1,54 @@
+# MicroPython for XiUOS
+
+This directory contains a MicroPython port for the XiUOS operating system.
+
+## Directory Structure
+
+```
+micropython/
+├── py/              # MicroPython core implementation
+├── py_port/         # Port-specific code and configuration
+├── extmod/          # External modules
+├── shared/          # Shared components
+│   ├── readline/    # Readline library for REPL
+│   └── runtime/     # Runtime components
+├── Kconfig          # Kernel configuration options
+├── Makefile         # Make build rules
+├── SConscript       # SCons build script
+└── README.md        # This file
+```
+
+## Configuration
+
+Enable MicroPython support through the XiUOS configuration menu:
+
+```
+make menuconfig
+```
+
+Navigate to:
+```
+-> Framework
+  -> Support knowing framework
+    -> [*] MicroPython Support
+```
+
+### Configuration Options
+
+- **MicroPython External Modules**: Enable external modules support
+- **MicroPython Shared Readline**: Enable readline for REPL
+- **MicroPython Shared Runtime**: Enable shared runtime components
+- **GC Heap Size**: Set garbage collector heap size (default: 16KB)
+- **Stack Size**: Set Python stack size (default: 8KB)
+
+## Building
+
+After configuring, build XiUOS as usual:
+
+```bash
+make
+```
+
+## Usage
+
+In lettershell, print `python` command to start interactive shell.

APP_Framework/Framework/knowing/micropython/SConscript

@@ -0,0 +1,40 @@
+import os
+from building import *
+
+cwd = GetCurrentDir()
+src = []
+CPPPATH = []
+CPPDEFINES = []
+
+if GetDepend(['USING_MICROPYTHON']):
+    # Include paths
+    CPPPATH = [
+        cwd,
+        os.path.join(cwd, 'py'),
+        os.path.join(cwd, 'py_port'),
+        os.path.join(cwd, 'extmod'),
+        os.path.join(cwd, 'shared'),
+        os.path.join(cwd, 'shared', 'readline'),
+        os.path.join(cwd, 'shared', 'runtime')
+    ]
+    
+    # Source files from all directories
+    src += Glob('py/*.c')
+    src += Glob('py_port/*.c')
+    
+    # Conditional modules
+    if GetDepend(['USING_MICROPYTHON_EXTMOD']):
+        src += Glob('extmod/*.c')
+    
+    if GetDepend(['USING_MICROPYTHON_SHARED_READLINE']):
+        src += Glob('shared/readline/*.c')
+    
+    if GetDepend(['USING_MICROPYTHON_SHARED_RUNTIME']):
+        src += Glob('shared/runtime/*.c')
+    
+    # Core definitions
+    CPPDEFINES = ['MICROPY_ENABLE_GC=1', 'MICROPY_PY_IO=1', 'MICROPY_PY_FSTRINGS=1']
+
+group = DefineGroup('micropython', src, depend = ['USING_MICROPYTHON'], CPPPATH = CPPPATH, LOCAL_CPPDEFINES=CPPDEFINES)
+
+Return('group')

APP_Framework/Framework/knowing/micropython/extmod/modplatform.h

@@ -0,0 +1,129 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2013-2021 Ibrahim Abdelkader <iabdalkader@openmv.io>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifndef MICROPY_INCLUDED_MODPLATFORM_H
+#define MICROPY_INCLUDED_MODPLATFORM_H
+
+#include "py/misc.h"  // For MP_STRINGIFY.
+#include "py/mpconfig.h"
+
+// Preprocessor directives identifying the platform.
+// The platform module itself is guarded by MICROPY_PY_PLATFORM, see the
+// .c file, but these are made available because they're generally usable.
+// TODO: Add more architectures, compilers and libraries.
+// See: https://sourceforge.net/p/predef/wiki/Home/
+
+#if defined(__ARM_ARCH)
+#if defined(__ARM_ARCH_ISA_A64)
+#define MICROPY_PLATFORM_ARCH   "aarch64"
+#else
+#define MICROPY_PLATFORM_ARCH   "arm"
+#endif
+#elif defined(__x86_64__) || defined(_M_X64)
+#define MICROPY_PLATFORM_ARCH   "x86_64"
+#elif defined(__i386__) || defined(_M_IX86)
+#define MICROPY_PLATFORM_ARCH   "x86"
+#elif defined(__xtensa__)
+#define MICROPY_PLATFORM_ARCH   "xtensa"
+#elif defined(__riscv)
+#if __riscv_xlen == 64
+#define MICROPY_PLATFORM_ARCH   "riscv64"
+#else
+#define MICROPY_PLATFORM_ARCH   "riscv"
+#endif
+#else
+#define MICROPY_PLATFORM_ARCH   ""
+#endif
+
+#if defined(__clang__)
+#define MICROPY_PLATFORM_COMPILER \
+    "Clang " \
+    MP_STRINGIFY(__clang_major__) "." \
+    MP_STRINGIFY(__clang_minor__) "." \
+    MP_STRINGIFY(__clang_patchlevel__)
+#elif defined(__GNUC__)
+#define MICROPY_PLATFORM_COMPILER \
+    "GCC " \
+    MP_STRINGIFY(__GNUC__) "." \
+    MP_STRINGIFY(__GNUC_MINOR__) "." \
+    MP_STRINGIFY(__GNUC_PATCHLEVEL__)
+#elif defined(__ARMCC_VERSION)
+#define MICROPY_PLATFORM_COMPILER \
+    "ARMCC " \
+    MP_STRINGIFY((__ARMCC_VERSION / 1000000)) "." \
+    MP_STRINGIFY((__ARMCC_VERSION / 10000 % 100)) "." \
+    MP_STRINGIFY((__ARMCC_VERSION % 10000))
+#elif defined(_MSC_VER)
+#if defined(_WIN64)
+#define MICROPY_PLATFORM_COMPILER_BITS  "64 bit"
+#elif defined(_M_IX86)
+#define MICROPY_PLATFORM_COMPILER_BITS  "32 bit"
+#else
+#define MICROPY_PLATFORM_COMPILER_BITS  ""
+#endif
+#define MICROPY_PLATFORM_COMPILER \
+    "MSC v." MP_STRINGIFY(_MSC_VER) " " MICROPY_PLATFORM_COMPILER_BITS
+#else
+#define MICROPY_PLATFORM_COMPILER       ""
+#endif
+
+#if defined(__GLIBC__)
+#define MICROPY_PLATFORM_LIBC_LIB       "glibc"
+#define MICROPY_PLATFORM_LIBC_VER \
+    MP_STRINGIFY(__GLIBC__) "." \
+    MP_STRINGIFY(__GLIBC_MINOR__)
+#elif defined(__NEWLIB__)
+#define MICROPY_PLATFORM_LIBC_LIB       "newlib"
+#define MICROPY_PLATFORM_LIBC_VER       _NEWLIB_VERSION
+#elif defined(_PICOLIBC__)
+#define MICROPY_PLATFORM_LIBC_LIB       "picolibc"
+#define MICROPY_PLATFORM_LIBC_VER       _PICOLIBC_VERSION
+#elif defined(__ANDROID__)
+#define MICROPY_PLATFORM_LIBC_LIB       "bionic"
+#define MICROPY_PLATFORM_LIBC_VER       MP_STRINGIFY(__ANDROID_API__)
+#else
+#define MICROPY_PLATFORM_LIBC_LIB       ""
+#define MICROPY_PLATFORM_LIBC_VER       ""
+#endif
+
+#if defined(__ANDROID__)
+#define MICROPY_PLATFORM_SYSTEM         "Android"
+#elif defined(__linux)
+#define MICROPY_PLATFORM_SYSTEM         "Linux"
+#elif defined(__unix__)
+#define MICROPY_PLATFORM_SYSTEM         "Unix"
+#elif defined(__CYGWIN__)
+#define MICROPY_PLATFORM_SYSTEM         "Cygwin"
+#elif defined(_WIN32)
+#define MICROPY_PLATFORM_SYSTEM         "Windows"
+#else
+#define MICROPY_PLATFORM_SYSTEM         "MicroPython"
+#endif
+
+#ifndef MICROPY_PLATFORM_VERSION
+#define MICROPY_PLATFORM_VERSION ""
+#endif
+
+#endif // MICROPY_INCLUDED_MODPLATFORM_H

APP_Framework/Framework/knowing/micropython/extmod/virtpin.h

@@ -0,0 +1,47 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2016 Paul Sokolovsky
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifndef MICROPY_INCLUDED_EXTMOD_VIRTPIN_H
+#define MICROPY_INCLUDED_EXTMOD_VIRTPIN_H
+
+#include "py/obj.h"
+
+#define MP_PIN_READ   (1)
+#define MP_PIN_WRITE  (2)
+#define MP_PIN_INPUT  (3)
+#define MP_PIN_OUTPUT (4)
+
+// Pin protocol
+typedef struct _mp_pin_p_t {
+    mp_uint_t (*ioctl)(mp_obj_t obj, mp_uint_t request, uintptr_t arg, int *errcode);
+} mp_pin_p_t;
+
+int mp_virtual_pin_read(mp_obj_t pin);
+void mp_virtual_pin_write(mp_obj_t pin, int value);
+
+// If a port exposes a Pin object, it's constructor should be like this
+mp_obj_t mp_pin_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args);
+
+#endif // MICROPY_INCLUDED_EXTMOD_VIRTPIN_H

APP_Framework/Framework/knowing/micropython/genhdr/moduledefs.h

@@ -0,0 +1,45 @@
+// Automatically generated by makemoduledefs.py.
+
+extern const struct _mp_obj_module_t mp_module_array;
+#undef MODULE_DEF_ARRAY
+#define MODULE_DEF_ARRAY { MP_ROM_QSTR(MP_QSTR_array), MP_ROM_PTR(&mp_module_array) },
+
+extern const struct _mp_obj_module_t mp_module___main__;
+#undef MODULE_DEF___MAIN__
+#define MODULE_DEF___MAIN__ { MP_ROM_QSTR(MP_QSTR___main__), MP_ROM_PTR(&mp_module___main__) },
+
+extern const struct _mp_obj_module_t mp_module_builtins;
+#undef MODULE_DEF_BUILTINS
+#define MODULE_DEF_BUILTINS { MP_ROM_QSTR(MP_QSTR_builtins), MP_ROM_PTR(&mp_module_builtins) },
+
+extern const struct _mp_obj_module_t mp_module_gc;
+#undef MODULE_DEF_GC
+#define MODULE_DEF_GC { MP_ROM_QSTR(MP_QSTR_gc), MP_ROM_PTR(&mp_module_gc) },
+
+extern const struct _mp_obj_module_t mp_module_micropython;
+#undef MODULE_DEF_MICROPYTHON
+#define MODULE_DEF_MICROPYTHON { MP_ROM_QSTR(MP_QSTR_micropython), MP_ROM_PTR(&mp_module_micropython) },
+
+extern const struct _mp_obj_module_t mp_module_sys;
+#undef MODULE_DEF_SYS
+#define MODULE_DEF_SYS { MP_ROM_QSTR(MP_QSTR_sys), MP_ROM_PTR(&mp_module_sys) },
+
+
+#define MICROPY_REGISTERED_MODULES \
+    MODULE_DEF_BUILTINS \
+    MODULE_DEF_GC \
+    MODULE_DEF_MICROPYTHON \
+    MODULE_DEF_SYS \
+    MODULE_DEF___MAIN__ \
+// MICROPY_REGISTERED_MODULES
+
+#define MICROPY_HAVE_REGISTERED_EXTENSIBLE_MODULES  1
+
+#define MICROPY_REGISTERED_EXTENSIBLE_MODULES \
+    MODULE_DEF_ARRAY \
+// MICROPY_REGISTERED_EXTENSIBLE_MODULES
+
+extern void mp_module_sys_attr(mp_obj_t self_in, qstr attr, mp_obj_t *dest);
+#define MICROPY_MODULE_DELEGATIONS \
+    { MP_ROM_PTR(&mp_module_sys), mp_module_sys_attr }, \
+// MICROPY_MODULE_DELEGATIONS

APP_Framework/Framework/knowing/micropython/genhdr/mpversion.h

@@ -0,0 +1,4 @@
+// This file was generated by py/makeversionhdr.py
+#define MICROPY_GIT_TAG "v1.28.0-preview.4.gef567dc928"
+#define MICROPY_GIT_HASH "ef567dc92"
+#define MICROPY_BUILD_DATE "2025-12-25"

APP_Framework/Framework/knowing/micropython/genhdr/qstrdefs.collected.h

@@ -0,0 +1,783 @@
+Q(ArithmeticError)
+
+Q(ArithmeticError)
+
+Q(AssertionError)
+
+Q(AssertionError)
+
+Q(AssertionError)
+
+Q(AttributeError)
+
+Q(AttributeError)
+
+Q(BaseException)
+
+Q(BaseException)
+
+Q(EOFError)
+
+Q(EOFError)
+
+Q(Ellipsis)
+
+Q(Ellipsis)
+
+Q(Exception)
+
+Q(Exception)
+
+Q(GeneratorExit)
+
+Q(GeneratorExit)
+
+Q(ImportError)
+
+Q(ImportError)
+
+Q(IndentationError)
+
+Q(IndentationError)
+
+Q(IndexError)
+
+Q(IndexError)
+
+Q(KeyError)
+
+Q(KeyError)
+
+Q(KeyboardInterrupt)
+
+Q(KeyboardInterrupt)
+
+Q(LookupError)
+
+Q(LookupError)
+
+Q(MemoryError)
+
+Q(MemoryError)
+
+Q(NameError)
+
+Q(NameError)
+
+Q(NoneType)
+
+Q(NotImplementedError)
+
+Q(NotImplementedError)
+
+Q(OSError)
+
+Q(OSError)
+
+Q(OverflowError)
+
+Q(OverflowError)
+
+Q(RuntimeError)
+
+Q(RuntimeError)
+
+Q(StopIteration)
+
+Q(StopIteration)
+
+Q(SyntaxError)
+
+Q(SyntaxError)
+
+Q(SystemExit)
+
+Q(SystemExit)
+
+Q(TypeError)
+
+Q(TypeError)
+
+Q(ValueError)
+
+Q(ValueError)
+
+Q(ZeroDivisionError)
+
+Q(ZeroDivisionError)
+
+Q(_0x0a_)
+
+Q(__add__)
+
+Q(__bases__)
+
+Q(__bool__)
+
+Q(__build_class__)
+
+Q(__call__)
+
+Q(__class__)
+
+Q(__class__)
+
+Q(__class__)
+
+Q(__class__)
+
+Q(__class__)
+
+Q(__class__)
+
+Q(__complex__)
+
+Q(__contains__)
+
+Q(__delitem__)
+
+Q(__delitem__)
+
+Q(__dict__)
+
+Q(__dict__)
+
+Q(__dict__)
+
+Q(__enter__)
+
+Q(__eq__)
+
+Q(__eq__)
+
+Q(__exit__)
+
+Q(__file__)
+
+Q(__file__)
+
+Q(__file__)
+
+Q(__float__)
+
+Q(__ge__)
+
+Q(__getattr__)
+
+Q(__getattr__)
+
+Q(__getattr__)
+
+Q(__getattr__)
+
+Q(__getitem__)
+
+Q(__getitem__)
+
+Q(__getitem__)
+
+Q(__getitem__)
+
+Q(__gt__)
+
+Q(__hash__)
+
+Q(__iadd__)
+
+Q(__import__)
+
+Q(__init__)
+
+Q(__init__)
+
+Q(__init__)
+
+Q(__int__)
+
+Q(__isub__)
+
+Q(__iter__)
+
+Q(__le__)
+
+Q(__len__)
+
+Q(__lt__)
+
+Q(__main__)
+
+Q(__main__)
+
+Q(__module__)
+
+Q(__name__)
+
+Q(__name__)
+
+Q(__name__)
+
+Q(__name__)
+
+Q(__name__)
+
+Q(__name__)
+
+Q(__name__)
+
+Q(__name__)
+
+Q(__name__)
+
+Q(__name__)
+
+Q(__name__)
+
+Q(__name__)
+
+Q(__ne__)
+
+Q(__new__)
+
+Q(__new__)
+
+Q(__new__)
+
+Q(__next__)
+
+Q(__next__)
+
+Q(__next__)
+
+Q(__next__)
+
+Q(__path__)
+
+Q(__path__)
+
+Q(__path__)
+
+Q(__path__)
+
+Q(__qualname__)
+
+Q(__repl_print__)
+
+Q(__repl_print__)
+
+Q(__repr__)
+
+Q(__repr__)
+
+Q(__reversed__)
+
+Q(__setitem__)
+
+Q(__setitem__)
+
+Q(__str__)
+
+Q(__sub__)
+
+Q(__traceback__)
+
+Q(_brace_open__colon__hash_b_brace_close_)
+
+Q(_lt_dictcomp_gt_)
+
+Q(_lt_dictcomp_gt_)
+
+Q(_lt_dictcomp_gt_)
+
+Q(_lt_genexpr_gt_)
+
+Q(_lt_genexpr_gt_)
+
+Q(_lt_genexpr_gt_)
+
+Q(_lt_lambda_gt_)
+
+Q(_lt_lambda_gt_)
+
+Q(_lt_lambda_gt_)
+
+Q(_lt_listcomp_gt_)
+
+Q(_lt_listcomp_gt_)
+
+Q(_lt_listcomp_gt_)
+
+Q(_lt_module_gt_)
+
+Q(_lt_module_gt_)
+
+Q(_lt_module_gt_)
+
+Q(_lt_setcomp_gt_)
+
+Q(_lt_setcomp_gt_)
+
+Q(_lt_setcomp_gt_)
+
+Q(_lt_stdin_gt_)
+
+Q(_lt_stdin_gt_)
+
+Q(_lt_string_gt_)
+
+Q(_percent__hash_o)
+
+Q(_percent__hash_x)
+
+Q(_space_)
+
+Q(_star_)
+
+Q(_star_)
+
+Q(abs)
+
+Q(all)
+
+Q(any)
+
+Q(append)
+
+Q(append)
+
+Q(args)
+
+Q(argv)
+
+Q(array)
+
+Q(array)
+
+Q(array)
+
+Q(array)
+
+Q(bin)
+
+Q(bool)
+
+Q(bool)
+
+Q(bound_method)
+
+Q(builtins)
+
+Q(builtins)
+
+Q(bytearray)
+
+Q(bytearray)
+
+Q(bytecode)
+
+Q(byteorder)
+
+Q(bytes)
+
+Q(bytes)
+
+Q(callable)
+
+Q(chr)
+
+Q(classmethod)
+
+Q(classmethod)
+
+Q(clear)
+
+Q(clear)
+
+Q(close)
+
+Q(close)
+
+Q(closure)
+
+Q(collect)
+
+Q(complex)
+
+Q(complex)
+
+Q(const)
+
+Q(const)
+
+Q(copy)
+
+Q(copy)
+
+Q(count)
+
+Q(count)
+
+Q(count)
+
+Q(decode)
+
+Q(default)
+
+Q(delattr)
+
+Q(deleter)
+
+Q(dict)
+
+Q(dict)
+
+Q(dict_view)
+
+Q(dir)
+
+Q(disable)
+
+Q(divmod)
+
+Q(doc)
+
+Q(enable)
+
+Q(encode)
+
+Q(end)
+
+Q(endswith)
+
+Q(enumerate)
+
+Q(enumerate)
+
+Q(errno)
+
+Q(eval)
+
+Q(exec)
+
+Q(exit)
+
+Q(extend)
+
+Q(extend)
+
+Q(filter)
+
+Q(filter)
+
+Q(find)
+
+Q(float)
+
+Q(float)
+
+Q(format)
+
+Q(from_bytes)
+
+Q(fromkeys)
+
+Q(function)
+
+Q(function)
+
+Q(function)
+
+Q(function)
+
+Q(function)
+
+Q(function)
+
+Q(function)
+
+Q(function)
+
+Q(gc)
+
+Q(gc)
+
+Q(generator)
+
+Q(generator)
+
+Q(get)
+
+Q(getattr)
+
+Q(getter)
+
+Q(globals)
+
+Q(hasattr)
+
+Q(hash)
+
+Q(heap_lock)
+
+Q(heap_unlock)
+
+Q(hex)
+
+Q(id)
+
+Q(imag)
+
+Q(implementation)
+
+Q(index)
+
+Q(index)
+
+Q(index)
+
+Q(insert)
+
+Q(int)
+
+Q(int)
+
+Q(isalpha)
+
+Q(isdigit)
+
+Q(isenabled)
+
+Q(isinstance)
+
+Q(islower)
+
+Q(isspace)
+
+Q(issubclass)
+
+Q(isupper)
+
+Q(items)
+
+Q(iter)
+
+Q(iterable)
+
+Q(iterator)
+
+Q(iterator)
+
+Q(iterator)
+
+Q(iterator)
+
+Q(iterator)
+
+Q(join)
+
+Q(key)
+
+Q(key)
+
+Q(keys)
+
+Q(keys)
+
+Q(len)
+
+Q(list)
+
+Q(list)
+
+Q(little)
+
+Q(little)
+
+Q(little)
+
+Q(locals)
+
+Q(lower)
+
+Q(lstrip)
+
+Q(map)
+
+Q(map)
+
+Q(max)
+
+Q(mem_alloc)
+
+Q(mem_free)
+
+Q(micropython)
+
+Q(micropython)
+
+Q(micropython)
+
+Q(micropython)
+
+Q(min)
+
+Q(module)
+
+Q(modules)
+
+Q(next)
+
+Q(object)
+
+Q(object)
+
+Q(oct)
+
+Q(opt_level)
+
+Q(ord)
+
+Q(path)
+
+Q(pend_throw)
+
+Q(pop)
+
+Q(pop)
+
+Q(popitem)
+
+Q(pow)
+
+Q(preview)
+
+Q(print)
+
+Q(print_exception)
+
+Q(property)
+
+Q(property)
+
+Q(range)
+
+Q(range)
+
+Q(range)
+
+Q(real)
+
+Q(remove)
+
+Q(replace)
+
+Q(repr)
+
+Q(reverse)
+
+Q(reverse)
+
+Q(reversed)
+
+Q(reversed)
+
+Q(rfind)
+
+Q(rindex)
+
+Q(round)
+
+Q(rsplit)
+
+Q(rstrip)
+
+Q(send)
+
+Q(send)
+
+Q(sep)
+
+Q(setattr)
+
+Q(setdefault)
+
+Q(setter)
+
+Q(slice)
+
+Q(slice)
+
+Q(sort)
+
+Q(sorted)
+
+Q(split)
+
+Q(start)
+
+Q(start)
+
+Q(startswith)
+
+Q(staticmethod)
+
+Q(staticmethod)
+
+Q(step)
+
+Q(stop)
+
+Q(str)
+
+Q(str)
+
+Q(strip)
+
+Q(sum)
+
+Q(super)
+
+Q(super)
+
+Q(super)
+
+Q(sys)
+
+Q(sys)
+
+Q(threshold)
+
+Q(throw)
+
+Q(throw)
+
+Q(to_bytes)
+
+Q(tuple)
+
+Q(tuple)
+
+Q(type)
+
+Q(type)
+
+Q(update)
+
+Q(upper)
+
+Q(usys)
+
+Q(utf_hyphen_8)
+
+Q(utf_hyphen_8)
+
+Q(value)
+
+Q(values)
+
+Q(version)
+
+Q(version_info)
+
+Q(zip)
+
+Q(zip)

APP_Framework/Framework/knowing/micropython/genhdr/qstrdefs.generated.h

@@ -0,0 +1,259 @@
+// This file was automatically generated by makeqstrdata.py
+
+QDEF0(MP_QSTRnull, 0, 0, "")
+QDEF0(MP_QSTR_, 5, 0, "")
+QDEF0(MP_QSTR___dir__, 122, 7, "__dir__")
+QDEF0(MP_QSTR__0x0a_, 175, 1, "\x0a")
+QDEF0(MP_QSTR__space_, 133, 1, " ")
+QDEF0(MP_QSTR__star_, 143, 1, "*")
+QDEF0(MP_QSTR__slash_, 138, 1, "/")
+QDEF0(MP_QSTR__lt_module_gt_, 189, 8, "<module>")
+QDEF0(MP_QSTR__, 250, 1, "_")
+QDEF0(MP_QSTR___call__, 167, 8, "__call__")
+QDEF0(MP_QSTR___class__, 43, 9, "__class__")
+QDEF0(MP_QSTR___delitem__, 253, 11, "__delitem__")
+QDEF0(MP_QSTR___enter__, 109, 9, "__enter__")
+QDEF0(MP_QSTR___exit__, 69, 8, "__exit__")
+QDEF0(MP_QSTR___getattr__, 64, 11, "__getattr__")
+QDEF0(MP_QSTR___getitem__, 38, 11, "__getitem__")
+QDEF0(MP_QSTR___hash__, 247, 8, "__hash__")
+QDEF0(MP_QSTR___init__, 95, 8, "__init__")
+QDEF0(MP_QSTR___int__, 22, 7, "__int__")
+QDEF0(MP_QSTR___iter__, 207, 8, "__iter__")
+QDEF0(MP_QSTR___len__, 226, 7, "__len__")
+QDEF0(MP_QSTR___main__, 142, 8, "__main__")
+QDEF0(MP_QSTR___module__, 255, 10, "__module__")
+QDEF0(MP_QSTR___name__, 226, 8, "__name__")
+QDEF0(MP_QSTR___new__, 121, 7, "__new__")
+QDEF0(MP_QSTR___next__, 2, 8, "__next__")
+QDEF0(MP_QSTR___qualname__, 107, 12, "__qualname__")
+QDEF0(MP_QSTR___repr__, 16, 8, "__repr__")
+QDEF0(MP_QSTR___setitem__, 50, 11, "__setitem__")
+QDEF0(MP_QSTR___str__, 208, 7, "__str__")
+QDEF0(MP_QSTR_ArithmeticError, 45, 15, "ArithmeticError")
+QDEF0(MP_QSTR_AssertionError, 151, 14, "AssertionError")
+QDEF0(MP_QSTR_AttributeError, 33, 14, "AttributeError")
+QDEF0(MP_QSTR_BaseException, 7, 13, "BaseException")
+QDEF0(MP_QSTR_EOFError, 145, 8, "EOFError")
+QDEF0(MP_QSTR_Ellipsis, 240, 8, "Ellipsis")
+QDEF0(MP_QSTR_Exception, 242, 9, "Exception")
+QDEF0(MP_QSTR_GeneratorExit, 22, 13, "GeneratorExit")
+QDEF0(MP_QSTR_ImportError, 32, 11, "ImportError")
+QDEF0(MP_QSTR_IndentationError, 92, 16, "IndentationError")
+QDEF0(MP_QSTR_IndexError, 131, 10, "IndexError")
+QDEF0(MP_QSTR_KeyError, 234, 8, "KeyError")
+QDEF0(MP_QSTR_KeyboardInterrupt, 175, 17, "KeyboardInterrupt")
+QDEF0(MP_QSTR_LookupError, 255, 11, "LookupError")
+QDEF0(MP_QSTR_MemoryError, 220, 11, "MemoryError")
+QDEF0(MP_QSTR_NameError, 186, 9, "NameError")
+QDEF0(MP_QSTR_NoneType, 23, 8, "NoneType")
+QDEF0(MP_QSTR_NotImplementedError, 198, 19, "NotImplementedError")
+QDEF0(MP_QSTR_OSError, 161, 7, "OSError")
+QDEF0(MP_QSTR_OverflowError, 129, 13, "OverflowError")
+QDEF0(MP_QSTR_RuntimeError, 97, 12, "RuntimeError")
+QDEF0(MP_QSTR_StopIteration, 234, 13, "StopIteration")
+QDEF0(MP_QSTR_SyntaxError, 148, 11, "SyntaxError")
+QDEF0(MP_QSTR_SystemExit, 32, 10, "SystemExit")
+QDEF0(MP_QSTR_TypeError, 37, 9, "TypeError")
+QDEF0(MP_QSTR_ValueError, 150, 10, "ValueError")
+QDEF0(MP_QSTR_ZeroDivisionError, 182, 17, "ZeroDivisionError")
+QDEF0(MP_QSTR_abs, 149, 3, "abs")
+QDEF0(MP_QSTR_all, 68, 3, "all")
+QDEF0(MP_QSTR_any, 19, 3, "any")
+QDEF0(MP_QSTR_append, 107, 6, "append")
+QDEF0(MP_QSTR_args, 194, 4, "args")
+QDEF0(MP_QSTR_bool, 235, 4, "bool")
+QDEF0(MP_QSTR_builtins, 247, 8, "builtins")
+QDEF0(MP_QSTR_bytearray, 118, 9, "bytearray")
+QDEF0(MP_QSTR_bytecode, 34, 8, "bytecode")
+QDEF0(MP_QSTR_bytes, 92, 5, "bytes")
+QDEF0(MP_QSTR_callable, 13, 8, "callable")
+QDEF0(MP_QSTR_chr, 220, 3, "chr")
+QDEF0(MP_QSTR_classmethod, 180, 11, "classmethod")
+QDEF0(MP_QSTR_clear, 124, 5, "clear")
+QDEF0(MP_QSTR_close, 51, 5, "close")
+QDEF0(MP_QSTR_const, 192, 5, "const")
+QDEF0(MP_QSTR_copy, 224, 4, "copy")
+QDEF0(MP_QSTR_count, 166, 5, "count")
+QDEF0(MP_QSTR_dict, 63, 4, "dict")
+QDEF0(MP_QSTR_dir, 250, 3, "dir")
+QDEF0(MP_QSTR_divmod, 184, 6, "divmod")
+QDEF0(MP_QSTR_end, 10, 3, "end")
+QDEF0(MP_QSTR_endswith, 27, 8, "endswith")
+QDEF0(MP_QSTR_eval, 155, 4, "eval")
+QDEF0(MP_QSTR_exec, 30, 4, "exec")
+QDEF0(MP_QSTR_extend, 99, 6, "extend")
+QDEF0(MP_QSTR_find, 1, 4, "find")
+QDEF0(MP_QSTR_format, 38, 6, "format")
+QDEF0(MP_QSTR_from_bytes, 53, 10, "from_bytes")
+QDEF0(MP_QSTR_get, 51, 3, "get")
+QDEF0(MP_QSTR_getattr, 192, 7, "getattr")
+QDEF0(MP_QSTR_globals, 157, 7, "globals")
+QDEF0(MP_QSTR_hasattr, 140, 7, "hasattr")
+QDEF0(MP_QSTR_hash, 183, 4, "hash")
+QDEF0(MP_QSTR_id, 40, 2, "id")
+QDEF0(MP_QSTR_index, 123, 5, "index")
+QDEF0(MP_QSTR_insert, 18, 6, "insert")
+QDEF0(MP_QSTR_int, 22, 3, "int")
+QDEF0(MP_QSTR_isalpha, 235, 7, "isalpha")
+QDEF0(MP_QSTR_isdigit, 168, 7, "isdigit")
+QDEF0(MP_QSTR_isinstance, 182, 10, "isinstance")
+QDEF0(MP_QSTR_islower, 252, 7, "islower")
+QDEF0(MP_QSTR_isspace, 91, 7, "isspace")
+QDEF0(MP_QSTR_issubclass, 181, 10, "issubclass")
+QDEF0(MP_QSTR_isupper, 221, 7, "isupper")
+QDEF0(MP_QSTR_items, 227, 5, "items")
+QDEF0(MP_QSTR_iter, 143, 4, "iter")
+QDEF0(MP_QSTR_join, 167, 4, "join")
+QDEF0(MP_QSTR_key, 50, 3, "key")
+QDEF0(MP_QSTR_keys, 1, 4, "keys")
+QDEF0(MP_QSTR_len, 98, 3, "len")
+QDEF0(MP_QSTR_list, 39, 4, "list")
+QDEF0(MP_QSTR_little, 137, 6, "little")
+QDEF0(MP_QSTR_locals, 59, 6, "locals")
+QDEF0(MP_QSTR_lower, 198, 5, "lower")
+QDEF0(MP_QSTR_lstrip, 229, 6, "lstrip")
+QDEF0(MP_QSTR_main, 206, 4, "main")
+QDEF0(MP_QSTR_map, 185, 3, "map")
+QDEF0(MP_QSTR_micropython, 11, 11, "micropython")
+QDEF0(MP_QSTR_next, 66, 4, "next")
+QDEF0(MP_QSTR_object, 144, 6, "object")
+QDEF0(MP_QSTR_open, 209, 4, "open")
+QDEF0(MP_QSTR_ord, 28, 3, "ord")
+QDEF0(MP_QSTR_pop, 42, 3, "pop")
+QDEF0(MP_QSTR_popitem, 191, 7, "popitem")
+QDEF0(MP_QSTR_pow, 45, 3, "pow")
+QDEF0(MP_QSTR_print, 84, 5, "print")
+QDEF0(MP_QSTR_range, 26, 5, "range")
+QDEF0(MP_QSTR_read, 183, 4, "read")
+QDEF0(MP_QSTR_readinto, 75, 8, "readinto")
+QDEF0(MP_QSTR_readline, 249, 8, "readline")
+QDEF0(MP_QSTR_remove, 99, 6, "remove")
+QDEF0(MP_QSTR_replace, 73, 7, "replace")
+QDEF0(MP_QSTR_repr, 208, 4, "repr")
+QDEF0(MP_QSTR_reverse, 37, 7, "reverse")
+QDEF0(MP_QSTR_rfind, 210, 5, "rfind")
+QDEF0(MP_QSTR_rindex, 233, 6, "rindex")
+QDEF0(MP_QSTR_round, 231, 5, "round")
+QDEF0(MP_QSTR_rsplit, 165, 6, "rsplit")
+QDEF0(MP_QSTR_rstrip, 59, 6, "rstrip")
+QDEF0(MP_QSTR_self, 121, 4, "self")
+QDEF0(MP_QSTR_send, 185, 4, "send")
+QDEF0(MP_QSTR_sep, 35, 3, "sep")
+QDEF0(MP_QSTR_set, 39, 3, "set")
+QDEF0(MP_QSTR_setattr, 212, 7, "setattr")
+QDEF0(MP_QSTR_setdefault, 108, 10, "setdefault")
+QDEF0(MP_QSTR_sort, 191, 4, "sort")
+QDEF0(MP_QSTR_sorted, 94, 6, "sorted")
+QDEF0(MP_QSTR_split, 183, 5, "split")
+QDEF0(MP_QSTR_start, 133, 5, "start")
+QDEF0(MP_QSTR_startswith, 116, 10, "startswith")
+QDEF0(MP_QSTR_staticmethod, 98, 12, "staticmethod")
+QDEF0(MP_QSTR_step, 87, 4, "step")
+QDEF0(MP_QSTR_stop, 157, 4, "stop")
+QDEF0(MP_QSTR_str, 80, 3, "str")
+QDEF0(MP_QSTR_strip, 41, 5, "strip")
+QDEF0(MP_QSTR_sum, 46, 3, "sum")
+QDEF0(MP_QSTR_super, 196, 5, "super")
+QDEF0(MP_QSTR_throw, 179, 5, "throw")
+QDEF0(MP_QSTR_to_bytes, 216, 8, "to_bytes")
+QDEF0(MP_QSTR_tuple, 253, 5, "tuple")
+QDEF0(MP_QSTR_type, 157, 4, "type")
+QDEF0(MP_QSTR_update, 180, 6, "update")
+QDEF0(MP_QSTR_upper, 39, 5, "upper")
+QDEF0(MP_QSTR_utf_hyphen_8, 183, 5, "utf-8")
+QDEF0(MP_QSTR_value, 78, 5, "value")
+QDEF0(MP_QSTR_values, 125, 6, "values")
+QDEF0(MP_QSTR_write, 152, 5, "write")
+QDEF0(MP_QSTR_zip, 230, 3, "zip")
+QDEF1(MP_QSTR__percent__hash_o, 108, 3, "%#o")
+QDEF1(MP_QSTR__percent__hash_x, 123, 3, "%#x")
+QDEF0(MP_QSTR__lt_dictcomp_gt_, 204, 10, "<dictcomp>")
+QDEF0(MP_QSTR__lt_genexpr_gt_, 52, 9, "<genexpr>")
+QDEF0(MP_QSTR__lt_lambda_gt_, 128, 8, "<lambda>")
+QDEF0(MP_QSTR__lt_listcomp_gt_, 212, 10, "<listcomp>")
+QDEF0(MP_QSTR__lt_setcomp_gt_, 84, 9, "<setcomp>")
+QDEF1(MP_QSTR__lt_stdin_gt_, 227, 7, "<stdin>")
+QDEF1(MP_QSTR__lt_string_gt_, 82, 8, "<string>")
+QDEF0(MP_QSTR___add__, 196, 7, "__add__")
+QDEF1(MP_QSTR___bases__, 3, 9, "__bases__")
+QDEF0(MP_QSTR___bool__, 43, 8, "__bool__")
+QDEF1(MP_QSTR___build_class__, 66, 15, "__build_class__")
+QDEF0(MP_QSTR___complex__, 197, 11, "__complex__")
+QDEF0(MP_QSTR___contains__, 198, 12, "__contains__")
+QDEF1(MP_QSTR___dict__, 127, 8, "__dict__")
+QDEF0(MP_QSTR___eq__, 113, 6, "__eq__")
+QDEF1(MP_QSTR___file__, 3, 8, "__file__")
+QDEF0(MP_QSTR___float__, 53, 9, "__float__")
+QDEF0(MP_QSTR___ge__, 167, 6, "__ge__")
+QDEF0(MP_QSTR___gt__, 182, 6, "__gt__")
+QDEF0(MP_QSTR___iadd__, 109, 8, "__iadd__")
+QDEF1(MP_QSTR___import__, 56, 10, "__import__")
+QDEF0(MP_QSTR___isub__, 8, 8, "__isub__")
+QDEF0(MP_QSTR___le__, 204, 6, "__le__")
+QDEF0(MP_QSTR___lt__, 93, 6, "__lt__")
+QDEF0(MP_QSTR___ne__, 14, 6, "__ne__")
+QDEF1(MP_QSTR___path__, 200, 8, "__path__")
+QDEF1(MP_QSTR___repl_print__, 1, 14, "__repl_print__")
+QDEF1(MP_QSTR___reversed__, 97, 12, "__reversed__")
+QDEF0(MP_QSTR___sub__, 33, 7, "__sub__")
+QDEF1(MP_QSTR___traceback__, 79, 13, "__traceback__")
+QDEF1(MP_QSTR_argv, 199, 4, "argv")
+QDEF1(MP_QSTR_array, 124, 5, "array")
+QDEF1(MP_QSTR_bin, 224, 3, "bin")
+QDEF1(MP_QSTR_bound_method, 151, 12, "bound_method")
+QDEF1(MP_QSTR_byteorder, 97, 9, "byteorder")
+QDEF1(MP_QSTR_closure, 116, 7, "closure")
+QDEF1(MP_QSTR_collect, 155, 7, "collect")
+QDEF1(MP_QSTR_complex, 197, 7, "complex")
+QDEF1(MP_QSTR_decode, 169, 6, "decode")
+QDEF1(MP_QSTR_default, 206, 7, "default")
+QDEF1(MP_QSTR_delattr, 219, 7, "delattr")
+QDEF1(MP_QSTR_deleter, 110, 7, "deleter")
+QDEF1(MP_QSTR_dict_view, 45, 9, "dict_view")
+QDEF1(MP_QSTR_disable, 145, 7, "disable")
+QDEF1(MP_QSTR_doc, 45, 3, "doc")
+QDEF1(MP_QSTR_enable, 4, 6, "enable")
+QDEF1(MP_QSTR_encode, 67, 6, "encode")
+QDEF1(MP_QSTR_enumerate, 113, 9, "enumerate")
+QDEF1(MP_QSTR_errno, 193, 5, "errno")
+QDEF1(MP_QSTR_exit, 133, 4, "exit")
+QDEF1(MP_QSTR_filter, 37, 6, "filter")
+QDEF1(MP_QSTR_float, 53, 5, "float")
+QDEF1(MP_QSTR_fromkeys, 55, 8, "fromkeys")
+QDEF1(MP_QSTR_function, 39, 8, "function")
+QDEF1(MP_QSTR_gc, 97, 2, "gc")
+QDEF1(MP_QSTR_generator, 150, 9, "generator")
+QDEF1(MP_QSTR_getter, 144, 6, "getter")
+QDEF1(MP_QSTR_heap_lock, 173, 9, "heap_lock")
+QDEF1(MP_QSTR_heap_unlock, 86, 11, "heap_unlock")
+QDEF1(MP_QSTR_hex, 112, 3, "hex")
+QDEF1(MP_QSTR_imag, 71, 4, "imag")
+QDEF1(MP_QSTR_implementation, 23, 14, "implementation")
+QDEF1(MP_QSTR_isenabled, 154, 9, "isenabled")
+QDEF1(MP_QSTR_iterable, 37, 8, "iterable")
+QDEF1(MP_QSTR_iterator, 71, 8, "iterator")
+QDEF1(MP_QSTR_max, 177, 3, "max")
+QDEF1(MP_QSTR_maximum_space_recursion_space_depth_space_exceeded, 115, 32, "maximum recursion depth exceeded")
+QDEF1(MP_QSTR_mem_alloc, 82, 9, "mem_alloc")
+QDEF1(MP_QSTR_mem_free, 203, 8, "mem_free")
+QDEF1(MP_QSTR_min, 175, 3, "min")
+QDEF1(MP_QSTR_module, 191, 6, "module")
+QDEF1(MP_QSTR_modules, 236, 7, "modules")
+QDEF1(MP_QSTR_oct, 253, 3, "oct")
+QDEF1(MP_QSTR_opt_level, 135, 9, "opt_level")
+QDEF1(MP_QSTR_path, 136, 4, "path")
+QDEF1(MP_QSTR_pend_throw, 243, 10, "pend_throw")
+QDEF1(MP_QSTR_preview, 239, 7, "preview")
+QDEF1(MP_QSTR_print_exception, 28, 15, "print_exception")
+QDEF1(MP_QSTR_property, 194, 8, "property")
+QDEF1(MP_QSTR_real, 191, 4, "real")
+QDEF1(MP_QSTR_reversed, 161, 8, "reversed")
+QDEF1(MP_QSTR_setter, 4, 6, "setter")
+QDEF1(MP_QSTR_slice, 181, 5, "slice")
+QDEF1(MP_QSTR_sys, 188, 3, "sys")
+QDEF1(MP_QSTR_threshold, 242, 9, "threshold")
+QDEF1(MP_QSTR_usys, 201, 4, "usys")
+QDEF1(MP_QSTR_version, 191, 7, "version")
+QDEF1(MP_QSTR_version_info, 110, 12, "version_info")
+QDEF1(MP_QSTR__brace_open__colon__hash_b_brace_close_, 88, 5, "{:#b}")

APP_Framework/Framework/knowing/micropython/genhdr/root_pointers.h

@@ -0,0 +1,5 @@
+// Automatically generated by make_root_pointers.py.
+
+const char *readline_hist[(8)];
+mp_obj_list_t mp_sys_argv_obj;
+mp_obj_t sys_mutable[MP_SYS_MUTABLE_NUM];

APP_Framework/Framework/knowing/micropython/py/argcheck.c

@@ -0,0 +1,148 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2013, 2014 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <stdlib.h>
+#include <assert.h>
+
+#include "py/runtime.h"
+
+void mp_arg_check_num_sig(size_t n_args, size_t n_kw, uint32_t sig) {
+    // TODO maybe take the function name as an argument so we can print nicer error messages
+
+    // The reverse of MP_OBJ_FUN_MAKE_SIG
+    bool takes_kw = sig & 1;
+    size_t n_args_min = sig >> 17;
+    size_t n_args_max = (sig >> 1) & 0xffff;
+
+    if (n_kw && !takes_kw) {
+        #if MICROPY_ERROR_REPORTING <= MICROPY_ERROR_REPORTING_TERSE
+        mp_arg_error_terse_mismatch();
+        #else
+        mp_raise_TypeError(MP_ERROR_TEXT("function doesn't take keyword arguments"));
+        #endif
+    }
+
+    if (n_args_min == n_args_max) {
+        if (n_args != n_args_min) {
+            #if MICROPY_ERROR_REPORTING <= MICROPY_ERROR_REPORTING_TERSE
+            mp_arg_error_terse_mismatch();
+            #else
+            mp_raise_msg_varg(&mp_type_TypeError,
+                MP_ERROR_TEXT("function takes %d positional arguments but %d were given"),
+                n_args_min, n_args);
+            #endif
+        }
+    } else {
+        if (n_args < n_args_min) {
+            #if MICROPY_ERROR_REPORTING <= MICROPY_ERROR_REPORTING_TERSE
+            mp_arg_error_terse_mismatch();
+            #else
+            mp_raise_msg_varg(&mp_type_TypeError,
+                MP_ERROR_TEXT("function missing %d required positional arguments"),
+                n_args_min - n_args);
+            #endif
+        } else if (n_args > n_args_max) {
+            #if MICROPY_ERROR_REPORTING <= MICROPY_ERROR_REPORTING_TERSE
+            mp_arg_error_terse_mismatch();
+            #else
+            mp_raise_msg_varg(&mp_type_TypeError,
+                MP_ERROR_TEXT("function expected at most %d arguments, got %d"),
+                n_args_max, n_args);
+            #endif
+        }
+    }
+}
+
+void mp_arg_parse_all(size_t n_pos, const mp_obj_t *pos, mp_map_t *kws, size_t n_allowed, const mp_arg_t *allowed, mp_arg_val_t *out_vals) {
+    size_t pos_found = 0, kws_found = 0;
+    for (size_t i = 0; i < n_allowed; i++) {
+        mp_obj_t given_arg;
+        if (i < n_pos) {
+            if (allowed[i].flags & MP_ARG_KW_ONLY) {
+                goto extra_positional;
+            }
+            pos_found++;
+            given_arg = pos[i];
+        } else {
+            mp_map_elem_t *kw = mp_map_lookup(kws, MP_OBJ_NEW_QSTR(allowed[i].qst), MP_MAP_LOOKUP);
+            if (kw == NULL) {
+                if (allowed[i].flags & MP_ARG_REQUIRED) {
+                    #if MICROPY_ERROR_REPORTING <= MICROPY_ERROR_REPORTING_TERSE
+                    mp_arg_error_terse_mismatch();
+                    #else
+                    mp_raise_msg_varg(&mp_type_TypeError, MP_ERROR_TEXT("'%q' argument required"), allowed[i].qst);
+                    #endif
+                }
+                out_vals[i] = allowed[i].defval;
+                continue;
+            } else {
+                kws_found++;
+                given_arg = kw->value;
+            }
+        }
+        if ((allowed[i].flags & MP_ARG_KIND_MASK) == MP_ARG_BOOL) {
+            out_vals[i].u_bool = mp_obj_is_true(given_arg);
+        } else if ((allowed[i].flags & MP_ARG_KIND_MASK) == MP_ARG_INT) {
+            out_vals[i].u_int = mp_obj_get_int(given_arg);
+        } else {
+            assert((allowed[i].flags & MP_ARG_KIND_MASK) == MP_ARG_OBJ);
+            out_vals[i].u_obj = given_arg;
+        }
+    }
+    if (pos_found < n_pos) {
+    extra_positional:
+        #if MICROPY_ERROR_REPORTING <= MICROPY_ERROR_REPORTING_TERSE
+        mp_arg_error_terse_mismatch();
+        #else
+        // TODO better error message
+        mp_raise_TypeError(MP_ERROR_TEXT("extra positional arguments given"));
+        #endif
+    }
+    if (kws_found < kws->used) {
+        #if MICROPY_ERROR_REPORTING <= MICROPY_ERROR_REPORTING_TERSE
+        mp_arg_error_terse_mismatch();
+        #else
+        // TODO better error message
+        mp_raise_TypeError(MP_ERROR_TEXT("extra keyword arguments given"));
+        #endif
+    }
+}
+
+void mp_arg_parse_all_kw_array(size_t n_pos, size_t n_kw, const mp_obj_t *args, size_t n_allowed, const mp_arg_t *allowed, mp_arg_val_t *out_vals) {
+    mp_map_t kw_args;
+    mp_map_init_fixed_table(&kw_args, n_kw, args + n_pos);
+    mp_arg_parse_all(n_pos, args, &kw_args, n_allowed, allowed, out_vals);
+}
+
+MP_NORETURN void mp_arg_error_terse_mismatch(void) {
+    mp_raise_TypeError(MP_ERROR_TEXT("argument num/types mismatch"));
+}
+
+#if MICROPY_CPYTHON_COMPAT
+MP_NORETURN void mp_arg_error_unimpl_kw(void) {
+    mp_raise_NotImplementedError(MP_ERROR_TEXT("keyword argument(s) not implemented - use normal args instead"));
+}
+#endif

APP_Framework/Framework/knowing/micropython/py/asmarm.c

@@ -0,0 +1,469 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2014 Fabian Vogt
+ * Copyright (c) 2013, 2014 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <stdio.h>
+#include <assert.h>
+#include <string.h>
+
+#include "py/mpconfig.h"
+
+// wrapper around everything in this file
+#if MICROPY_EMIT_ARM
+
+#include "py/asmarm.h"
+
+#define REG_TEMP ASM_ARM_REG_R8
+
+// Insert word into instruction flow
+static void emit(asm_arm_t *as, uint op) {
+    uint8_t *c = mp_asm_base_get_cur_to_write_bytes(&as->base, 4);
+    if (c != NULL) {
+        *(uint32_t *)c = op;
+    }
+}
+
+// Insert word into instruction flow, add "ALWAYS" condition code
+static void emit_al(asm_arm_t *as, uint op) {
+    emit(as, op | ASM_ARM_CC_AL);
+}
+
+// Basic instructions without condition code
+static uint asm_arm_op_push(uint reglist) {
+    // stmfd sp!, {reglist}
+    return 0x92d0000 | (reglist & 0xFFFF);
+}
+
+static uint asm_arm_op_pop(uint reglist) {
+    // ldmfd sp!, {reglist}
+    return 0x8bd0000 | (reglist & 0xFFFF);
+}
+
+static uint asm_arm_op_mov_reg(uint rd, uint rn) {
+    // mov rd, rn
+    return 0x1a00000 | (rd << 12) | rn;
+}
+
+static uint asm_arm_op_mov_imm(uint rd, uint imm) {
+    // mov rd, #imm
+    return 0x3a00000 | (rd << 12) | imm;
+}
+
+static uint asm_arm_op_mvn_imm(uint rd, uint imm) {
+    // mvn rd, #imm
+    return 0x3e00000 | (rd << 12) | imm;
+}
+
+static uint asm_arm_op_mvn_reg(uint rd, uint rm) {
+    // mvn rd, rm
+    return 0x1e00000 | (rd << 12) | rm;
+}
+
+static uint asm_arm_op_add_imm(uint rd, uint rn, uint imm) {
+    // add rd, rn, #imm
+    return 0x2800000 | (rn << 16) | (rd << 12) | (imm & 0xFF);
+}
+
+static uint asm_arm_op_add_reg(uint rd, uint rn, uint rm) {
+    // add rd, rn, rm
+    return 0x0800000 | (rn << 16) | (rd << 12) | rm;
+}
+
+static uint asm_arm_op_sub_imm(uint rd, uint rn, uint imm) {
+    // sub rd, rn, #imm
+    return 0x2400000 | (rn << 16) | (rd << 12) | (imm & 0xFF);
+}
+
+static uint asm_arm_op_sub_reg(uint rd, uint rn, uint rm) {
+    // sub rd, rn, rm
+    return 0x0400000 | (rn << 16) | (rd << 12) | rm;
+}
+
+static uint asm_arm_op_rsb_imm(uint rd, uint rn, uint imm) {
+    // rsb rd, rn, #imm
+    return 0x2600000 | (rn << 16) | (rd << 12) | (imm & 0xFF);
+}
+
+static uint asm_arm_op_mul_reg(uint rd, uint rm, uint rs) {
+    // mul rd, rm, rs
+    assert(rd != rm);
+    return 0x0000090 | (rd << 16) | (rs << 8) | rm;
+}
+
+static uint asm_arm_op_and_reg(uint rd, uint rn, uint rm) {
+    // and rd, rn, rm
+    return 0x0000000 | (rn << 16) | (rd << 12) | rm;
+}
+
+static uint asm_arm_op_eor_reg(uint rd, uint rn, uint rm) {
+    // eor rd, rn, rm
+    return 0x0200000 | (rn << 16) | (rd << 12) | rm;
+}
+
+static uint asm_arm_op_orr_reg(uint rd, uint rn, uint rm) {
+    // orr rd, rn, rm
+    return 0x1800000 | (rn << 16) | (rd << 12) | rm;
+}
+
+void asm_arm_bkpt(asm_arm_t *as) {
+    // bkpt #0
+    emit_al(as, 0x1200070);
+}
+
+// locals:
+//  - stored on the stack in ascending order
+//  - numbered 0 through num_locals-1
+//  - SP points to first local
+//
+//  | SP
+//  v
+//  l0  l1  l2  ...  l(n-1)
+//  ^                ^
+//  | low address    | high address in RAM
+
+void asm_arm_entry(asm_arm_t *as, int num_locals) {
+    assert(num_locals >= 0);
+
+    as->stack_adjust = 0;
+    as->push_reglist = 1 << ASM_ARM_REG_R1
+        | 1 << ASM_ARM_REG_R2
+        | 1 << ASM_ARM_REG_R3
+        | 1 << ASM_ARM_REG_R4
+        | 1 << ASM_ARM_REG_R5
+        | 1 << ASM_ARM_REG_R6
+        | 1 << ASM_ARM_REG_R7
+        | 1 << ASM_ARM_REG_R8;
+
+    // Only adjust the stack if there are more locals than usable registers
+    if (num_locals > 3) {
+        as->stack_adjust = num_locals * 4;
+        // Align stack to 8 bytes
+        if (num_locals & 1) {
+            as->stack_adjust += 4;
+        }
+    }
+
+    emit_al(as, asm_arm_op_push(as->push_reglist | 1 << ASM_ARM_REG_LR));
+    if (as->stack_adjust > 0) {
+        if (as->stack_adjust < 0x100) {
+            emit_al(as, asm_arm_op_sub_imm(ASM_ARM_REG_SP, ASM_ARM_REG_SP, as->stack_adjust));
+        } else {
+            asm_arm_mov_reg_i32_optimised(as, REG_TEMP, as->stack_adjust);
+            emit_al(as, asm_arm_op_sub_reg(ASM_ARM_REG_SP, ASM_ARM_REG_SP, REG_TEMP));
+        }
+    }
+}
+
+void asm_arm_exit(asm_arm_t *as) {
+    if (as->stack_adjust > 0) {
+        if (as->stack_adjust < 0x100) {
+            emit_al(as, asm_arm_op_add_imm(ASM_ARM_REG_SP, ASM_ARM_REG_SP, as->stack_adjust));
+        } else {
+            asm_arm_mov_reg_i32_optimised(as, REG_TEMP, as->stack_adjust);
+            emit_al(as, asm_arm_op_add_reg(ASM_ARM_REG_SP, ASM_ARM_REG_SP, REG_TEMP));
+        }
+    }
+
+    emit_al(as, asm_arm_op_pop(as->push_reglist | (1 << ASM_ARM_REG_PC)));
+}
+
+void asm_arm_push(asm_arm_t *as, uint reglist) {
+    emit_al(as, asm_arm_op_push(reglist));
+}
+
+void asm_arm_pop(asm_arm_t *as, uint reglist) {
+    emit_al(as, asm_arm_op_pop(reglist));
+}
+
+void asm_arm_mov_reg_reg(asm_arm_t *as, uint reg_dest, uint reg_src) {
+    emit_al(as, asm_arm_op_mov_reg(reg_dest, reg_src));
+}
+
+size_t asm_arm_mov_reg_i32(asm_arm_t *as, uint rd, int imm) {
+    // Insert immediate into code and jump over it
+    emit_al(as, 0x59f0000 | (rd << 12)); // ldr rd, [pc]
+    emit_al(as, 0xa000000); // b pc
+    size_t loc = mp_asm_base_get_code_pos(&as->base);
+    emit(as, imm);
+    return loc;
+}
+
+void asm_arm_mov_reg_i32_optimised(asm_arm_t *as, uint rd, int imm) {
+    // TODO: There are more variants of immediate values
+    if ((imm & 0xFF) == imm) {
+        emit_al(as, asm_arm_op_mov_imm(rd, imm));
+    } else if (imm < 0 && imm >= -256) {
+        // mvn is "move not", not "move negative"
+        emit_al(as, asm_arm_op_mvn_imm(rd, ~imm));
+    } else {
+        asm_arm_mov_reg_i32(as, rd, imm);
+    }
+}
+
+void asm_arm_mov_local_reg(asm_arm_t *as, int local_num, uint rd) {
+    // str rd, [sp, #local_num*4]
+    emit_al(as, 0x58d0000 | (rd << 12) | (local_num << 2));
+}
+
+void asm_arm_mov_reg_local(asm_arm_t *as, uint rd, int local_num) {
+    // ldr rd, [sp, #local_num*4]
+    emit_al(as, 0x59d0000 | (rd << 12) | (local_num << 2));
+}
+
+void asm_arm_cmp_reg_i8(asm_arm_t *as, uint rd, int imm) {
+    // cmp rd, #imm
+    emit_al(as, 0x3500000 | (rd << 16) | (imm & 0xFF));
+}
+
+void asm_arm_cmp_reg_reg(asm_arm_t *as, uint rd, uint rn) {
+    // cmp rd, rn
+    emit_al(as, 0x1500000 | (rd << 16) | rn);
+}
+
+void asm_arm_setcc_reg(asm_arm_t *as, uint rd, uint cond) {
+    emit(as, asm_arm_op_mov_imm(rd, 1) | cond); // movCOND rd, #1
+    emit(as, asm_arm_op_mov_imm(rd, 0) | (cond ^ (1 << 28))); // mov!COND rd, #0
+}
+
+void asm_arm_mvn_reg_reg(asm_arm_t *as, uint rd, uint rm) {
+    // mvn rd, rm
+    // computes: rd := ~rm
+    emit_al(as, asm_arm_op_mvn_reg(rd, rm));
+}
+
+void asm_arm_add_reg_reg_reg(asm_arm_t *as, uint rd, uint rn, uint rm) {
+    // add rd, rn, rm
+    emit_al(as, asm_arm_op_add_reg(rd, rn, rm));
+}
+
+void asm_arm_rsb_reg_reg_imm(asm_arm_t *as, uint rd, uint rn, uint imm) {
+    // rsb rd, rn, #imm
+    // computes: rd := #imm - rn
+    emit_al(as, asm_arm_op_rsb_imm(rd, rn, imm));
+}
+
+void asm_arm_sub_reg_reg_reg(asm_arm_t *as, uint rd, uint rn, uint rm) {
+    // sub rd, rn, rm
+    emit_al(as, asm_arm_op_sub_reg(rd, rn, rm));
+}
+
+void asm_arm_mul_reg_reg_reg(asm_arm_t *as, uint rd, uint rs, uint rm) {
+    // rs and rm are swapped because of restriction rd!=rm
+    // mul rd, rm, rs
+    emit_al(as, asm_arm_op_mul_reg(rd, rm, rs));
+}
+
+void asm_arm_and_reg_reg_reg(asm_arm_t *as, uint rd, uint rn, uint rm) {
+    // and rd, rn, rm
+    emit_al(as, asm_arm_op_and_reg(rd, rn, rm));
+}
+
+void asm_arm_eor_reg_reg_reg(asm_arm_t *as, uint rd, uint rn, uint rm) {
+    // eor rd, rn, rm
+    emit_al(as, asm_arm_op_eor_reg(rd, rn, rm));
+}
+
+void asm_arm_orr_reg_reg_reg(asm_arm_t *as, uint rd, uint rn, uint rm) {
+    // orr rd, rn, rm
+    emit_al(as, asm_arm_op_orr_reg(rd, rn, rm));
+}
+
+void asm_arm_mov_reg_local_addr(asm_arm_t *as, uint rd, int local_num) {
+    if (local_num >= 0x40) {
+        // mov temp, #local_num*4
+        // add rd, sp, temp
+        asm_arm_mov_reg_i32_optimised(as, REG_TEMP, local_num << 2);
+        emit_al(as, asm_arm_op_add_reg(rd, ASM_ARM_REG_SP, REG_TEMP));
+    } else {
+        // add rd, sp, #local_num*4
+        emit_al(as, asm_arm_op_add_imm(rd, ASM_ARM_REG_SP, local_num << 2));
+    }
+}
+
+void asm_arm_mov_reg_pcrel(asm_arm_t *as, uint reg_dest, uint label) {
+    assert(label < as->base.max_num_labels);
+    mp_uint_t dest = as->base.label_offsets[label];
+    mp_int_t rel = dest - as->base.code_offset;
+    rel -= 12 + 8; // adjust for load of rel, and then PC+8 prefetch of add_reg_reg_reg
+
+    // To load rel int reg_dest, insert immediate into code and jump over it
+    emit_al(as, 0x59f0000 | (reg_dest << 12)); // ldr rd, [pc]
+    emit_al(as, 0xa000000); // b pc
+    emit(as, rel);
+
+    // Do reg_dest += PC
+    asm_arm_add_reg_reg_reg(as, reg_dest, reg_dest, ASM_ARM_REG_PC);
+}
+
+void asm_arm_lsl_reg_reg(asm_arm_t *as, uint rd, uint rs) {
+    // mov rd, rd, lsl rs
+    emit_al(as, 0x1a00010 | (rd << 12) | (rs << 8) | rd);
+}
+
+void asm_arm_lsr_reg_reg(asm_arm_t *as, uint rd, uint rs) {
+    // mov rd, rd, lsr rs
+    emit_al(as, 0x1a00030 | (rd << 12) | (rs << 8) | rd);
+}
+
+void asm_arm_asr_reg_reg(asm_arm_t *as, uint rd, uint rs) {
+    // mov rd, rd, asr rs
+    emit_al(as, 0x1a00050 | (rd << 12) | (rs << 8) | rd);
+}
+
+void asm_arm_ldr_reg_reg_offset(asm_arm_t *as, uint rd, uint rn, uint byte_offset) {
+    if (byte_offset < 0x1000) {
+        // ldr rd, [rn, #off]
+        emit_al(as, 0x5900000 | (rn << 16) | (rd << 12) | byte_offset);
+    } else {
+        // mov temp, #off
+        // ldr rd, [rn, temp]
+        asm_arm_mov_reg_i32_optimised(as, REG_TEMP, byte_offset);
+        emit_al(as, 0x7900000 | (rn << 16) | (rd << 12) | REG_TEMP);
+    }
+}
+
+void asm_arm_ldrh_reg_reg_reg(asm_arm_t *as, uint rd, uint rm, uint rn) {
+    // ldrh doesn't support scaled register index
+    emit_al(as, 0x1a00080 | (REG_TEMP << 12) | rn); // mov temp, rn, lsl #1
+    emit_al(as, 0x19000b0 | (rm << 16) | (rd << 12) | REG_TEMP); // ldrh rd, [rm, temp];
+}
+
+void asm_arm_ldrh_reg_reg_offset(asm_arm_t *as, uint rd, uint rn, uint byte_offset) {
+    if (byte_offset < 0x100) {
+        // ldrh rd, [rn, #off]
+        emit_al(as, 0x1d000b0 | (rn << 16) | (rd << 12) | ((byte_offset & 0xf0) << 4) | (byte_offset & 0xf));
+    } else {
+        // mov temp, #off
+        // ldrh rd, [rn, temp]
+        asm_arm_mov_reg_i32_optimised(as, REG_TEMP, byte_offset);
+        emit_al(as, 0x19000b0 | (rn << 16) | (rd << 12) | REG_TEMP);
+    }
+}
+
+void asm_arm_ldrb_reg_reg_reg(asm_arm_t *as, uint rd, uint rm, uint rn) {
+    // ldrb rd, [rm, rn]
+    emit_al(as, 0x7d00000 | (rm << 16) | (rd << 12) | rn);
+}
+
+void asm_arm_ldrb_reg_reg_offset(asm_arm_t *as, uint rd, uint rn, uint byte_offset) {
+    if (byte_offset < 0x1000) {
+        // ldrb rd, [rn, #off]
+        emit_al(as, 0x5d00000 | (rn << 16) | (rd << 12) | byte_offset);
+    } else {
+        // mov temp, #off
+        // ldrb rd, [rn, temp]
+        asm_arm_mov_reg_i32_optimised(as, REG_TEMP, byte_offset);
+        emit_al(as, 0x7d00000 | (rn << 16) | (rd << 12) | REG_TEMP);
+    }
+}
+
+void asm_arm_ldr_reg_reg_reg(asm_arm_t *as, uint rd, uint rm, uint rn) {
+    // ldr rd, [rm, rn, lsl #2]
+    emit_al(as, 0x7900100 | (rm << 16) | (rd << 12) | rn);
+}
+
+void asm_arm_str_reg_reg_offset(asm_arm_t *as, uint rd, uint rm, uint byte_offset) {
+    if (byte_offset < 0x1000) {
+        // str rd, [rm, #off]
+        emit_al(as, 0x5800000 | (rm << 16) | (rd << 12) | byte_offset);
+    } else {
+        // mov temp, #off
+        // str rd, [rm, temp]
+        asm_arm_mov_reg_i32_optimised(as, REG_TEMP, byte_offset);
+        emit_al(as, 0x7800000 | (rm << 16) | (rd << 12) | REG_TEMP);
+    }
+}
+
+void asm_arm_strh_reg_reg_offset(asm_arm_t *as, uint rd, uint rn, uint byte_offset) {
+    if (byte_offset < 0x100) {
+        // strh rd, [rn, #off]
+        emit_al(as, 0x1c000b0 | (rn << 16) | (rd << 12) | ((byte_offset & 0xf0) << 4) | (byte_offset & 0xf));
+    } else {
+        // mov temp, #off
+        // strh rd, [rn, temp]
+        asm_arm_mov_reg_i32_optimised(as, REG_TEMP, byte_offset);
+        emit_al(as, 0x18000b0 | (rn << 16) | (rd << 12) | REG_TEMP);
+    }
+}
+
+void asm_arm_strb_reg_reg_offset(asm_arm_t *as, uint rd, uint rm, uint byte_offset) {
+    if (byte_offset < 0x1000) {
+        // strb rd, [rm, #off]
+        emit_al(as, 0x5c00000 | (rm << 16) | (rd << 12) | byte_offset);
+    } else {
+        // mov temp, #off
+        // strb rd, [rm, temp]
+        asm_arm_mov_reg_i32_optimised(as, REG_TEMP, byte_offset);
+        emit_al(as, 0x7c00000 | (rm << 16) | (rd << 12) | REG_TEMP);
+    }
+}
+
+void asm_arm_str_reg_reg_reg(asm_arm_t *as, uint rd, uint rm, uint rn) {
+    // str rd, [rm, rn, lsl #2]
+    emit_al(as, 0x7800100 | (rm << 16) | (rd << 12) | rn);
+}
+
+void asm_arm_strh_reg_reg_reg(asm_arm_t *as, uint rd, uint rm, uint rn) {
+    // strh doesn't support scaled register index
+    emit_al(as, 0x1a00080 | (REG_TEMP << 12) | rn); // mov temp, rn, lsl #1
+    emit_al(as, 0x18000b0 | (rm << 16) | (rd << 12) | REG_TEMP); // strh rd, [rm, temp]
+}
+
+void asm_arm_strb_reg_reg_reg(asm_arm_t *as, uint rd, uint rm, uint rn) {
+    // strb rd, [rm, rn]
+    emit_al(as, 0x7c00000 | (rm << 16) | (rd << 12) | rn);
+}
+
+void asm_arm_bcc_label(asm_arm_t *as, int cond, uint label) {
+    assert(label < as->base.max_num_labels);
+    mp_uint_t dest = as->base.label_offsets[label];
+    mp_int_t rel = dest - as->base.code_offset;
+    rel -= 8; // account for instruction prefetch, PC is 8 bytes ahead of this instruction
+    rel >>= 2; // in ARM mode the branch target is 32-bit aligned, so the 2 LSB are omitted
+
+    if (MP_FIT_SIGNED(24, rel)) {
+        emit(as, cond | 0xa000000 | (rel & 0xffffff));
+    } else {
+        printf("asm_arm_bcc: branch does not fit in 24 bits\n");
+    }
+}
+
+void asm_arm_b_label(asm_arm_t *as, uint label) {
+    asm_arm_bcc_label(as, ASM_ARM_CC_AL, label);
+}
+
+void asm_arm_bl_ind(asm_arm_t *as, uint fun_id, uint reg_temp) {
+    // The table offset should fit into the ldr instruction
+    assert(fun_id < (0x1000 / 4));
+    emit_al(as, asm_arm_op_mov_reg(ASM_ARM_REG_LR, ASM_ARM_REG_PC)); // mov lr, pc
+    emit_al(as, 0x597f000 | (fun_id << 2)); // ldr pc, [r7, #fun_id*4]
+}
+
+void asm_arm_bx_reg(asm_arm_t *as, uint reg_src) {
+    emit_al(as, 0x012fff10 | reg_src);
+}
+
+#endif // MICROPY_EMIT_ARM

APP_Framework/Framework/knowing/micropython/py/asmarm.h

@@ -0,0 +1,235 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2014 Fabian Vogt
+ * Copyright (c) 2013, 2014 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifndef MICROPY_INCLUDED_PY_ASMARM_H
+#define MICROPY_INCLUDED_PY_ASMARM_H
+
+#include "py/misc.h"
+#include "py/asmbase.h"
+
+#define ASM_ARM_REG_R0  (0)
+#define ASM_ARM_REG_R1  (1)
+#define ASM_ARM_REG_R2  (2)
+#define ASM_ARM_REG_R3  (3)
+#define ASM_ARM_REG_R4  (4)
+#define ASM_ARM_REG_R5  (5)
+#define ASM_ARM_REG_R6  (6)
+#define ASM_ARM_REG_R7  (7)
+#define ASM_ARM_REG_R8  (8)
+#define ASM_ARM_REG_R9  (9)
+#define ASM_ARM_REG_R10 (10)
+#define ASM_ARM_REG_R11 (11)
+#define ASM_ARM_REG_R12 (12)
+#define ASM_ARM_REG_R13 (13)
+#define ASM_ARM_REG_R14 (14)
+#define ASM_ARM_REG_R15 (15)
+#define ASM_ARM_REG_SP  (ASM_ARM_REG_R13)
+#define ASM_ARM_REG_LR  (ASM_ARM_REG_R14)
+#define ASM_ARM_REG_PC  (ASM_ARM_REG_R15)
+
+#define ASM_ARM_CC_EQ (0x0 << 28)
+#define ASM_ARM_CC_NE (0x1 << 28)
+#define ASM_ARM_CC_CS (0x2 << 28)
+#define ASM_ARM_CC_CC (0x3 << 28)
+#define ASM_ARM_CC_MI (0x4 << 28)
+#define ASM_ARM_CC_PL (0x5 << 28)
+#define ASM_ARM_CC_VS (0x6 << 28)
+#define ASM_ARM_CC_VC (0x7 << 28)
+#define ASM_ARM_CC_HI (0x8 << 28)
+#define ASM_ARM_CC_LS (0x9 << 28)
+#define ASM_ARM_CC_GE (0xa << 28)
+#define ASM_ARM_CC_LT (0xb << 28)
+#define ASM_ARM_CC_GT (0xc << 28)
+#define ASM_ARM_CC_LE (0xd << 28)
+#define ASM_ARM_CC_AL (0xe << 28)
+
+typedef struct _asm_arm_t {
+    mp_asm_base_t base;
+    uint push_reglist;
+    uint stack_adjust;
+} asm_arm_t;
+
+static inline void asm_arm_end_pass(asm_arm_t *as) {
+    (void)as;
+}
+
+void asm_arm_entry(asm_arm_t *as, int num_locals);
+void asm_arm_exit(asm_arm_t *as);
+
+void asm_arm_bkpt(asm_arm_t *as);
+
+// mov
+void asm_arm_mov_reg_reg(asm_arm_t *as, uint reg_dest, uint reg_src);
+size_t asm_arm_mov_reg_i32(asm_arm_t *as, uint rd, int imm);
+void asm_arm_mov_reg_i32_optimised(asm_arm_t *as, uint rd, int imm);
+void asm_arm_mov_local_reg(asm_arm_t *as, int local_num, uint rd);
+void asm_arm_mov_reg_local(asm_arm_t *as, uint rd, int local_num);
+void asm_arm_setcc_reg(asm_arm_t *as, uint rd, uint cond);
+
+// compare
+void asm_arm_cmp_reg_i8(asm_arm_t *as, uint rd, int imm);
+void asm_arm_cmp_reg_reg(asm_arm_t *as, uint rd, uint rn);
+
+// arithmetic
+void asm_arm_mvn_reg_reg(asm_arm_t *as, uint rd, uint rm);
+void asm_arm_add_reg_reg_reg(asm_arm_t *as, uint rd, uint rn, uint rm);
+void asm_arm_sub_reg_reg_reg(asm_arm_t *as, uint rd, uint rn, uint rm);
+void asm_arm_rsb_reg_reg_imm(asm_arm_t *as, uint rd, uint rn, uint imm);
+void asm_arm_mul_reg_reg_reg(asm_arm_t *as, uint rd, uint rn, uint rm);
+void asm_arm_and_reg_reg_reg(asm_arm_t *as, uint rd, uint rn, uint rm);
+void asm_arm_eor_reg_reg_reg(asm_arm_t *as, uint rd, uint rn, uint rm);
+void asm_arm_orr_reg_reg_reg(asm_arm_t *as, uint rd, uint rn, uint rm);
+void asm_arm_mov_reg_local_addr(asm_arm_t *as, uint rd, int local_num);
+void asm_arm_mov_reg_pcrel(asm_arm_t *as, uint reg_dest, uint label);
+void asm_arm_lsl_reg_reg(asm_arm_t *as, uint rd, uint rs);
+void asm_arm_lsr_reg_reg(asm_arm_t *as, uint rd, uint rs);
+void asm_arm_asr_reg_reg(asm_arm_t *as, uint rd, uint rs);
+
+// memory
+void asm_arm_ldr_reg_reg_offset(asm_arm_t *as, uint rd, uint rn, uint byte_offset);
+void asm_arm_ldrh_reg_reg_offset(asm_arm_t *as, uint rd, uint rn, uint byte_offset);
+void asm_arm_ldrb_reg_reg_offset(asm_arm_t *as, uint rd, uint rn, uint byte_offset);
+void asm_arm_str_reg_reg_offset(asm_arm_t *as, uint rd, uint rm, uint byte_offset);
+void asm_arm_strh_reg_reg_offset(asm_arm_t *as, uint rd, uint rm, uint byte_offset);
+void asm_arm_strb_reg_reg_offset(asm_arm_t *as, uint rd, uint rm, uint byte_offset);
+
+// load from array
+void asm_arm_ldr_reg_reg_reg(asm_arm_t *as, uint rd, uint rm, uint rn);
+void asm_arm_ldrh_reg_reg_reg(asm_arm_t *as, uint rd, uint rm, uint rn);
+void asm_arm_ldrb_reg_reg_reg(asm_arm_t *as, uint rd, uint rm, uint rn);
+
+// store to array
+void asm_arm_str_reg_reg_reg(asm_arm_t *as, uint rd, uint rm, uint rn);
+void asm_arm_strh_reg_reg_reg(asm_arm_t *as, uint rd, uint rm, uint rn);
+void asm_arm_strb_reg_reg_reg(asm_arm_t *as, uint rd, uint rm, uint rn);
+
+// stack
+void asm_arm_push(asm_arm_t *as, uint reglist);
+void asm_arm_pop(asm_arm_t *as, uint reglist);
+
+// control flow
+void asm_arm_bcc_label(asm_arm_t *as, int cond, uint label);
+void asm_arm_b_label(asm_arm_t *as, uint label);
+void asm_arm_bl_ind(asm_arm_t *as, uint fun_id, uint reg_temp);
+void asm_arm_bx_reg(asm_arm_t *as, uint reg_src);
+
+// Holds a pointer to mp_fun_table
+#define ASM_ARM_REG_FUN_TABLE ASM_ARM_REG_R7
+
+#if GENERIC_ASM_API
+
+// The following macros provide a (mostly) arch-independent API to
+// generate native code, and are used by the native emitter.
+
+#define ASM_WORD_SIZE (4)
+
+#define REG_RET ASM_ARM_REG_R0
+#define REG_ARG_1 ASM_ARM_REG_R0
+#define REG_ARG_2 ASM_ARM_REG_R1
+#define REG_ARG_3 ASM_ARM_REG_R2
+#define REG_ARG_4 ASM_ARM_REG_R3
+
+#define REG_TEMP0 ASM_ARM_REG_R0
+#define REG_TEMP1 ASM_ARM_REG_R1
+#define REG_TEMP2 ASM_ARM_REG_R2
+
+#define REG_LOCAL_1 ASM_ARM_REG_R4
+#define REG_LOCAL_2 ASM_ARM_REG_R5
+#define REG_LOCAL_3 ASM_ARM_REG_R6
+#define REG_LOCAL_NUM (3)
+
+// Holds a pointer to mp_fun_table
+#define REG_FUN_TABLE ASM_ARM_REG_FUN_TABLE
+
+#define ASM_T                           asm_arm_t
+#define ASM_END_PASS                    asm_arm_end_pass
+#define ASM_ENTRY(as, num_locals, name) asm_arm_entry((as), (num_locals))
+#define ASM_EXIT                        asm_arm_exit
+
+#define ASM_JUMP                        asm_arm_b_label
+#define ASM_JUMP_IF_REG_ZERO(as, reg, label, bool_test) \
+    do { \
+        asm_arm_cmp_reg_i8(as, reg, 0); \
+        asm_arm_bcc_label(as, ASM_ARM_CC_EQ, label); \
+    } while (0)
+#define ASM_JUMP_IF_REG_NONZERO(as, reg, label, bool_test) \
+    do { \
+        asm_arm_cmp_reg_i8(as, reg, 0); \
+        asm_arm_bcc_label(as, ASM_ARM_CC_NE, label); \
+    } while (0)
+#define ASM_JUMP_IF_REG_EQ(as, reg1, reg2, label) \
+    do { \
+        asm_arm_cmp_reg_reg(as, reg1, reg2); \
+        asm_arm_bcc_label(as, ASM_ARM_CC_EQ, label); \
+    } while (0)
+#define ASM_JUMP_REG(as, reg) asm_arm_bx_reg((as), (reg))
+#define ASM_CALL_IND(as, idx) asm_arm_bl_ind(as, idx, ASM_ARM_REG_R3)
+
+#define ASM_MOV_LOCAL_REG(as, local_num, reg_src) asm_arm_mov_local_reg((as), (local_num), (reg_src))
+#define ASM_MOV_REG_IMM(as, reg_dest, imm) asm_arm_mov_reg_i32_optimised((as), (reg_dest), (imm))
+#define ASM_MOV_REG_LOCAL(as, reg_dest, local_num) asm_arm_mov_reg_local((as), (reg_dest), (local_num))
+#define ASM_MOV_REG_REG(as, reg_dest, reg_src) asm_arm_mov_reg_reg((as), (reg_dest), (reg_src))
+#define ASM_MOV_REG_LOCAL_ADDR(as, reg_dest, local_num) asm_arm_mov_reg_local_addr((as), (reg_dest), (local_num))
+#define ASM_MOV_REG_PCREL(as, reg_dest, label) asm_arm_mov_reg_pcrel((as), (reg_dest), (label))
+
+#define ASM_NOT_REG(as, reg_dest) asm_arm_mvn_reg_reg((as), (reg_dest), (reg_dest))
+#define ASM_NEG_REG(as, reg_dest) asm_arm_rsb_reg_reg_imm((as), (reg_dest), (reg_dest), 0)
+#define ASM_LSL_REG_REG(as, reg_dest, reg_shift) asm_arm_lsl_reg_reg((as), (reg_dest), (reg_shift))
+#define ASM_LSR_REG_REG(as, reg_dest, reg_shift) asm_arm_lsr_reg_reg((as), (reg_dest), (reg_shift))
+#define ASM_ASR_REG_REG(as, reg_dest, reg_shift) asm_arm_asr_reg_reg((as), (reg_dest), (reg_shift))
+#define ASM_OR_REG_REG(as, reg_dest, reg_src) asm_arm_orr_reg_reg_reg((as), (reg_dest), (reg_dest), (reg_src))
+#define ASM_XOR_REG_REG(as, reg_dest, reg_src) asm_arm_eor_reg_reg_reg((as), (reg_dest), (reg_dest), (reg_src))
+#define ASM_AND_REG_REG(as, reg_dest, reg_src) asm_arm_and_reg_reg_reg((as), (reg_dest), (reg_dest), (reg_src))
+#define ASM_ADD_REG_REG(as, reg_dest, reg_src) asm_arm_add_reg_reg_reg((as), (reg_dest), (reg_dest), (reg_src))
+#define ASM_SUB_REG_REG(as, reg_dest, reg_src) asm_arm_sub_reg_reg_reg((as), (reg_dest), (reg_dest), (reg_src))
+#define ASM_MUL_REG_REG(as, reg_dest, reg_src) asm_arm_mul_reg_reg_reg((as), (reg_dest), (reg_dest), (reg_src))
+
+#define ASM_LOAD_REG_REG_OFFSET(as, reg_dest, reg_base, word_offset) ASM_LOAD32_REG_REG_OFFSET((as), (reg_dest), (reg_base), (word_offset))
+#define ASM_LOAD8_REG_REG(as, reg_dest, reg_base) ASM_LOAD8_REG_REG_OFFSET((as), (reg_dest), (reg_base), 0)
+#define ASM_LOAD8_REG_REG_OFFSET(as, reg_dest, reg_base, byte_offset) asm_arm_ldrb_reg_reg_offset((as), (reg_dest), (reg_base), (byte_offset))
+#define ASM_LOAD16_REG_REG(as, reg_dest, reg_base) ASM_LOAD16_REG_REG_OFFSET((as), (reg_dest), (reg_base), 0)
+#define ASM_LOAD16_REG_REG_OFFSET(as, reg_dest, reg_base, halfword_offset) asm_arm_ldrh_reg_reg_offset((as), (reg_dest), (reg_base), 2 * (halfword_offset))
+#define ASM_LOAD32_REG_REG(as, reg_dest, reg_base) ASM_LOAD32_REG_REG_OFFSET((as), (reg_dest), (reg_base), 0)
+#define ASM_LOAD32_REG_REG_OFFSET(as, reg_dest, reg_base, word_offset) asm_arm_ldr_reg_reg_offset((as), (reg_dest), (reg_base), 4 * (word_offset))
+
+#define ASM_STORE_REG_REG_OFFSET(as, reg_value, reg_base, word_offset) ASM_STORE32_REG_REG_OFFSET((as), (reg_value), (reg_base), (word_offset))
+#define ASM_STORE8_REG_REG(as, reg_value, reg_base) ASM_STORE8_REG_REG_OFFSET((as), (reg_value), (reg_base), 0)
+#define ASM_STORE8_REG_REG_OFFSET(as, reg_value, reg_base, byte_offset) asm_arm_strb_reg_reg_offset((as), (reg_value), (reg_base), (byte_offset))
+#define ASM_STORE16_REG_REG(as, reg_value, reg_base) ASM_STORE16_REG_REG_OFFSET((as), (reg_value), (reg_base), 0)
+#define ASM_STORE16_REG_REG_OFFSET(as, reg_value, reg_base, halfword_offset) asm_arm_strh_reg_reg_offset((as), (reg_value), (reg_base), 2 * (halfword_offset))
+#define ASM_STORE32_REG_REG(as, reg_value, reg_base) ASM_STORE32_REG_REG_OFFSET((as), (reg_value), (reg_base), 0)
+#define ASM_STORE32_REG_REG_OFFSET(as, reg_value, reg_base, word_offset) asm_arm_str_reg_reg_offset((as), (reg_value), (reg_base), 4 * (word_offset))
+
+#define ASM_LOAD8_REG_REG_REG(as, reg_dest, reg_base, reg_index) asm_arm_ldrb_reg_reg_reg((as), (reg_dest), (reg_base), (reg_index))
+#define ASM_LOAD16_REG_REG_REG(as, reg_dest, reg_base, reg_index) asm_arm_ldrh_reg_reg_reg((as), (reg_dest), (reg_base), (reg_index))
+#define ASM_LOAD32_REG_REG_REG(as, reg_dest, reg_base, reg_index) asm_arm_ldr_reg_reg_reg((as), (reg_dest), (reg_base), (reg_index))
+#define ASM_STORE8_REG_REG_REG(as, reg_val, reg_base, reg_index) asm_arm_strb_reg_reg_reg((as), (reg_val), (reg_base), (reg_index))
+#define ASM_STORE16_REG_REG_REG(as, reg_val, reg_base, reg_index) asm_arm_strh_reg_reg_reg((as), (reg_val), (reg_base), (reg_index))
+#define ASM_STORE32_REG_REG_REG(as, reg_val, reg_base, reg_index) asm_arm_str_reg_reg_reg((as), (reg_val), (reg_base), (reg_index))
+
+#endif // GENERIC_ASM_API
+
+#endif // MICROPY_INCLUDED_PY_ASMARM_H

APP_Framework/Framework/knowing/micropython/py/asmbase.c

@@ -0,0 +1,119 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2016 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include "py/obj.h"
+#include "py/misc.h"
+#include "py/asmbase.h"
+#include "py/persistentcode.h"
+
+#if MICROPY_EMIT_MACHINE_CODE
+
+void mp_asm_base_init(mp_asm_base_t *as, size_t max_num_labels) {
+    as->max_num_labels = max_num_labels;
+    as->label_offsets = m_new(size_t, max_num_labels);
+}
+
+void mp_asm_base_deinit(mp_asm_base_t *as, bool free_code) {
+    if (free_code) {
+        MP_PLAT_FREE_EXEC(as->code_base, as->code_size);
+    }
+    m_del(size_t, as->label_offsets, as->max_num_labels);
+}
+
+void mp_asm_base_start_pass(mp_asm_base_t *as, int pass) {
+    if (pass < MP_ASM_PASS_EMIT) {
+        // Reset labels so we can detect backwards jumps (and verify unique assignment)
+        memset(as->label_offsets, -1, as->max_num_labels * sizeof(size_t));
+    } else {
+        // allocating executable RAM is platform specific
+        MP_PLAT_ALLOC_EXEC(as->code_offset, (void **)&as->code_base, &as->code_size);
+        assert(as->code_size == 0 || as->code_base != NULL);
+    }
+    as->pass = pass;
+    as->suppress = false;
+    as->code_offset = 0;
+}
+
+// all functions must go through this one to emit bytes
+// if as->pass < MP_ASM_PASS_EMIT, then this function just counts the number
+// of bytes needed and returns NULL, and callers should not store any data
+// It also returns NULL if generated code should be suppressed at this point.
+uint8_t *mp_asm_base_get_cur_to_write_bytes(void *as_in, size_t num_bytes_to_write) {
+    mp_asm_base_t *as = as_in;
+    uint8_t *c = NULL;
+    if (as->suppress) {
+        return c;
+    }
+    if (as->pass == MP_ASM_PASS_EMIT) {
+        assert(as->code_offset + num_bytes_to_write <= as->code_size);
+        c = as->code_base + as->code_offset;
+    }
+    as->code_offset += num_bytes_to_write;
+    return c;
+}
+
+void mp_asm_base_label_assign(mp_asm_base_t *as, size_t label) {
+    assert(label < as->max_num_labels);
+
+    // Assigning a label ends any dead-code region, and all following machine
+    // code should be emitted (until another mp_asm_base_suppress_code() call).
+    as->suppress = false;
+
+    if (as->pass < MP_ASM_PASS_EMIT) {
+        // assign label offset
+        assert(as->label_offsets[label] == (size_t)-1);
+        as->label_offsets[label] = as->code_offset;
+    } else {
+        // ensure label offset has not changed from PASS_COMPUTE to PASS_EMIT
+        assert(as->label_offsets[label] == as->code_offset);
+        #if MICROPY_DYNAMIC_COMPILER && MICROPY_EMIT_NATIVE_DEBUG
+        if (mp_dynamic_compiler.native_arch == MP_NATIVE_ARCH_DEBUG) {
+            mp_printf(MICROPY_EMIT_NATIVE_DEBUG_PRINTER, "label(label_%u)\n", (unsigned int)label);
+        }
+        #endif
+    }
+}
+
+// align must be a multiple of 2
+void mp_asm_base_align(mp_asm_base_t *as, unsigned int align) {
+    as->code_offset = (as->code_offset + align - 1) & (~(size_t)(align - 1));
+}
+
+// this function assumes a little endian machine
+void mp_asm_base_data(mp_asm_base_t *as, unsigned int bytesize, uintptr_t val) {
+    uint8_t *c = mp_asm_base_get_cur_to_write_bytes(as, bytesize);
+    if (c != NULL) {
+        for (unsigned int i = 0; i < bytesize; i++) {
+            *c++ = val;
+            val >>= 8;
+        }
+    }
+}
+
+#endif // MICROPY_EMIT_MACHINE_CODE

APP_Framework/Framework/knowing/micropython/py/asmbase.h

@@ -0,0 +1,79 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2016 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifndef MICROPY_INCLUDED_PY_ASMBASE_H
+#define MICROPY_INCLUDED_PY_ASMBASE_H
+
+#include <stdint.h>
+#include <stddef.h>
+#include <stdbool.h>
+
+#define MP_ASM_PASS_COMPUTE (1)
+#define MP_ASM_PASS_EMIT    (2)
+
+typedef struct _mp_asm_base_t {
+    uint8_t pass;
+
+    // Set to true using mp_asm_base_suppress_code() if the code generator
+    // should suppress emitted code due to it being dead code.
+    bool suppress;
+
+    size_t code_offset;
+    size_t code_size;
+    uint8_t *code_base;
+
+    size_t max_num_labels;
+    size_t *label_offsets;
+} mp_asm_base_t;
+
+void mp_asm_base_init(mp_asm_base_t *as, size_t max_num_labels);
+void mp_asm_base_deinit(mp_asm_base_t *as, bool free_code);
+void mp_asm_base_start_pass(mp_asm_base_t *as, int pass);
+uint8_t *mp_asm_base_get_cur_to_write_bytes(void *as, size_t num_bytes_to_write);
+void mp_asm_base_label_assign(mp_asm_base_t *as, size_t label);
+void mp_asm_base_align(mp_asm_base_t *as, unsigned int align);
+void mp_asm_base_data(mp_asm_base_t *as, unsigned int bytesize, uintptr_t val);
+
+static inline void mp_asm_base_suppress_code(mp_asm_base_t *as) {
+    as->suppress = true;
+}
+
+static inline size_t mp_asm_base_get_code_pos(mp_asm_base_t *as) {
+    return as->code_offset;
+}
+
+static inline size_t mp_asm_base_get_code_size(mp_asm_base_t *as) {
+    return as->code_size;
+}
+
+static inline void *mp_asm_base_get_code(mp_asm_base_t *as) {
+    #if defined(MP_PLAT_COMMIT_EXEC)
+    return MP_PLAT_COMMIT_EXEC(as->code_base, as->code_size, NULL);
+    #else
+    return as->code_base;
+    #endif
+}
+
+#endif // MICROPY_INCLUDED_PY_ASMBASE_H

APP_Framework/Framework/knowing/micropython/py/asmrv32.c

@@ -0,0 +1,614 @@
+/*
+ * This file is part of the MicroPython project, https://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2024 Alessandro Gatti
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "py/emit.h"
+#include "py/misc.h"
+#include "py/mpconfig.h"
+
+// wrapper around everything in this file
+#if MICROPY_EMIT_RV32
+
+#include "py/asmrv32.h"
+#include "py/mpstate.h"
+#include "py/persistentcode.h"
+
+#if MICROPY_DEBUG_VERBOSE
+#define DEBUG_PRINT (1)
+#define DEBUG_printf DEBUG_printf
+#else
+#define DEBUG_printf(...) (void)0
+#endif
+
+#define INTERNAL_TEMPORARY ASM_RV32_REG_S0
+
+#define FIT_UNSIGNED(value, bits) (((value) & ~((1U << (bits)) - 1)) == 0)
+#define FIT_SIGNED(value, bits) \
+    ((((value) & ~((1U << ((bits) - 1)) - 1)) == 0) ||                                      \
+    (((value) & ~((1U << ((bits) - 1)) - 1)) == ~((1U << ((bits) - 1)) - 1)))
+
+///////////////////////////////////////////////////////////////////////////////
+
+void asm_rv32_emit_word_opcode(asm_rv32_t *state, mp_uint_t word) {
+    uint8_t *cursor = mp_asm_base_get_cur_to_write_bytes(&state->base, sizeof(uint32_t));
+    if (cursor == NULL) {
+        return;
+    }
+
+    #if MP_ENDIANNESS_LITTLE
+    cursor[0] = word & 0xFF;
+    cursor[1] = (word >> 8) & 0xFF;
+    cursor[2] = (word >> 16) & 0xFF;
+    cursor[3] = (word >> 24) & 0xFF;
+    #else
+    cursor[0] = (word >> 24) & 0xFF;
+    cursor[1] = (word >> 16) & 0xFF;
+    cursor[2] = (word >> 8) & 0xFF;
+    cursor[3] = word & 0xFF;
+    #endif
+}
+
+void asm_rv32_emit_halfword_opcode(asm_rv32_t *state, mp_uint_t word) {
+    uint8_t *cursor = mp_asm_base_get_cur_to_write_bytes(&state->base, sizeof(uint16_t));
+    if (cursor == NULL) {
+        return;
+    }
+
+    #if MP_ENDIANNESS_LITTLE
+    cursor[0] = word & 0xFF;
+    cursor[1] = (word >> 8) & 0xFF;
+    #else
+    cursor[0] = (word >> 8) & 0xFF;
+    cursor[1] = word & 0xFF;
+    #endif
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+static void split_immediate(mp_int_t immediate, mp_uint_t *upper, mp_uint_t *lower) {
+    assert(upper != NULL && "Upper pointer is NULL.");
+    assert(lower != NULL && "Lower pointer is NULL.");
+
+    mp_uint_t unsigned_immediate = *((mp_uint_t *)&immediate);
+    *upper = unsigned_immediate & 0xFFFFF000;
+    *lower = unsigned_immediate & 0x00000FFF;
+
+    // Turn the lower half from unsigned to signed.
+    if ((*lower & 0x800) != 0) {
+        *upper += 0x1000;
+    }
+}
+
+static void load_upper_immediate(asm_rv32_t *state, mp_uint_t rd, mp_uint_t immediate) {
+    // if immediate fits in 17 bits and is ≠ 0:
+    //   c.lui rd, HI(immediate)
+    // else:
+    //   lui   rd, HI(immediate)
+    if (FIT_SIGNED(immediate, 17) && ((immediate >> 12) != 0)) {
+        asm_rv32_opcode_clui(state, rd, immediate);
+    } else {
+        asm_rv32_opcode_lui(state, rd, immediate);
+    }
+}
+
+static void load_lower_immediate(asm_rv32_t *state, mp_uint_t rd, mp_uint_t immediate) {
+    // WARNING: This must be executed on a register that has either been
+    //          previously cleared or was the target of a LUI/C.LUI or
+    //          AUIPC opcode.
+
+    if (immediate == 0) {
+        return;
+    }
+
+    // if LO(immediate) fits in 6 bits:
+    //   c.addi rd, LO(immediate)
+    // else:
+    //   addi   rd, rd, LO(immediate)
+    if (FIT_SIGNED(immediate, 6)) {
+        asm_rv32_opcode_caddi(state, rd, immediate);
+    } else {
+        asm_rv32_opcode_addi(state, rd, rd, immediate);
+    }
+}
+
+static void load_full_immediate(asm_rv32_t *state, mp_uint_t rd, mp_int_t immediate) {
+    mp_uint_t upper = 0;
+    mp_uint_t lower = 0;
+    split_immediate(immediate, &upper, &lower);
+
+    // if immediate fits in 17 bits:
+    //   c.lui  rd, HI(immediate)
+    // else:
+    //   lui    rd, HI(immediate)
+    // if LO(immediate) fits in 6 bits && LO(immediate) != 0:
+    //   c.addi rd, LO(immediate)
+    // else:
+    //   addi   rd, rd, LO(immediate)
+    load_upper_immediate(state, rd, upper);
+    load_lower_immediate(state, rd, lower);
+}
+
+void asm_rv32_emit_optimised_load_immediate(asm_rv32_t *state, mp_uint_t rd, mp_int_t immediate) {
+    if (FIT_SIGNED(immediate, 6)) {
+        // c.li rd, immediate
+        asm_rv32_opcode_cli(state, rd, immediate);
+        return;
+    }
+
+    if (FIT_SIGNED(immediate, 12)) {
+        // addi rd, zero, immediate
+        asm_rv32_opcode_addi(state, rd, ASM_RV32_REG_ZERO, immediate);
+        return;
+    }
+
+    load_full_immediate(state, rd, immediate);
+}
+
+// RV32 does not have dedicated push/pop opcodes, so series of loads and
+// stores are generated in their place.
+
+static void emit_registers_store(asm_rv32_t *state, mp_uint_t registers_mask) {
+    mp_uint_t offset = 0;
+    for (mp_uint_t register_index = 0; register_index < RV32_AVAILABLE_REGISTERS_COUNT; register_index++) {
+        if (registers_mask & (1U << register_index)) {
+            assert(FIT_UNSIGNED(offset >> 2, 6) && "Registers save stack offset out of range.");
+            // c.swsp register, offset
+            asm_rv32_opcode_cswsp(state, register_index, offset);
+            offset += sizeof(uint32_t);
+        }
+    }
+}
+
+static void emit_registers_load(asm_rv32_t *state, mp_uint_t registers_mask) {
+    mp_uint_t offset = 0;
+    for (mp_uint_t register_index = 0; register_index < RV32_AVAILABLE_REGISTERS_COUNT; register_index++) {
+        if (registers_mask & (1U << register_index)) {
+            assert(FIT_UNSIGNED(offset >> 2, 6) && "Registers load stack offset out of range.");
+            // c.lwsp register, offset
+            asm_rv32_opcode_clwsp(state, register_index, offset);
+            offset += sizeof(uint32_t);
+        }
+    }
+}
+
+static void adjust_stack(asm_rv32_t *state, mp_int_t stack_size) {
+    if (stack_size == 0) {
+        return;
+    }
+
+    if (FIT_SIGNED(stack_size, 6)) {
+        // c.addi sp, stack_size
+        asm_rv32_opcode_caddi(state, ASM_RV32_REG_SP, stack_size);
+        return;
+    }
+
+    if (FIT_SIGNED(stack_size, 12)) {
+        // addi sp, sp, stack_size
+        asm_rv32_opcode_addi(state, ASM_RV32_REG_SP, ASM_RV32_REG_SP, stack_size);
+        return;
+    }
+
+    // li    temporary, stack_size
+    // c.add sp, temporary
+    load_full_immediate(state, REG_TEMP0, stack_size);
+    asm_rv32_opcode_cadd(state, ASM_RV32_REG_SP, REG_TEMP0);
+}
+
+// Generate a generic function entry prologue code sequence, setting up the
+// stack to hold all the tainted registers and an arbitrary amount of space
+// for locals.
+static void emit_function_prologue(asm_rv32_t *state, mp_uint_t registers) {
+    mp_uint_t registers_count = mp_popcount(registers);
+    state->stack_size = (registers_count + state->locals_count) * sizeof(uint32_t);
+    mp_uint_t old_saved_registers_mask = state->saved_registers_mask;
+    // Move stack pointer up.
+    adjust_stack(state, -state->stack_size);
+    // Store registers at the top of the saved stack area.
+    emit_registers_store(state, registers);
+    state->locals_stack_offset = registers_count * sizeof(uint32_t);
+    state->saved_registers_mask = old_saved_registers_mask;
+}
+
+// Restore registers and reset the stack pointer to its initial value.
+static void emit_function_epilogue(asm_rv32_t *state, mp_uint_t registers) {
+    mp_uint_t old_saved_registers_mask = state->saved_registers_mask;
+    // Restore registers from the top of the stack area.
+    emit_registers_load(state, registers);
+    // Move stack pointer down.
+    adjust_stack(state, state->stack_size);
+    state->saved_registers_mask = old_saved_registers_mask;
+}
+
+static bool calculate_displacement_for_label(asm_rv32_t *state, mp_uint_t label, ptrdiff_t *displacement) {
+    assert(displacement != NULL && "Displacement pointer is NULL");
+
+    mp_uint_t label_offset = state->base.label_offsets[label];
+    *displacement = (ptrdiff_t)(label_offset - state->base.code_offset);
+    return (label_offset != (mp_uint_t)-1) && (*displacement < 0);
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void asm_rv32_entry(asm_rv32_t *state, mp_uint_t locals) {
+    state->saved_registers_mask |= (1U << REG_FUN_TABLE) | (1U << REG_LOCAL_1) | \
+        (1U << REG_LOCAL_2) | (1U << REG_LOCAL_3);
+    state->locals_count = locals;
+    emit_function_prologue(state, state->saved_registers_mask);
+}
+
+void asm_rv32_exit(asm_rv32_t *state) {
+    emit_function_epilogue(state, state->saved_registers_mask);
+    // c.jr ra
+    asm_rv32_opcode_cjr(state, ASM_RV32_REG_RA);
+}
+
+void asm_rv32_end_pass(asm_rv32_t *state) {
+    (void)state;
+}
+
+void asm_rv32_emit_call_ind(asm_rv32_t *state, mp_uint_t index) {
+    mp_uint_t offset = index * ASM_WORD_SIZE;
+    state->saved_registers_mask |= (1U << ASM_RV32_REG_RA);
+
+    if (RV32_IS_IN_C_REGISTER_WINDOW(REG_FUN_TABLE) && RV32_IS_IN_C_REGISTER_WINDOW(INTERNAL_TEMPORARY) && FIT_UNSIGNED(offset, 6)) {
+        state->saved_registers_mask |= (1U << INTERNAL_TEMPORARY);
+        // c.lw   temporary, offset(fun_table)
+        // c.jalr temporary
+        asm_rv32_opcode_clw(state, RV32_MAP_IN_C_REGISTER_WINDOW(INTERNAL_TEMPORARY), RV32_MAP_IN_C_REGISTER_WINDOW(REG_FUN_TABLE), offset);
+        asm_rv32_opcode_cjalr(state, INTERNAL_TEMPORARY);
+        return;
+    }
+
+    if (FIT_UNSIGNED(offset, 11)) {
+        // lw     temporary, offset(fun_table)
+        // c.jalr temporary
+        asm_rv32_opcode_lw(state, REG_TEMP2, REG_FUN_TABLE, offset);
+        asm_rv32_opcode_cjalr(state, REG_TEMP2);
+        return;
+    }
+
+    mp_uint_t upper = 0;
+    mp_uint_t lower = 0;
+    split_immediate(offset, &upper, &lower);
+
+    // lui    temporary, HI(index) ; Or c.lui if possible
+    // c.add  temporary, fun_table
+    // lw     temporary, LO(index)(temporary)
+    // c.jalr temporary
+    load_upper_immediate(state, REG_TEMP2, upper);
+    asm_rv32_opcode_cadd(state, REG_TEMP2, REG_FUN_TABLE);
+    asm_rv32_opcode_lw(state, REG_TEMP2, REG_TEMP2, lower);
+    asm_rv32_opcode_cjalr(state, REG_TEMP2);
+}
+
+void asm_rv32_emit_jump_if_reg_eq(asm_rv32_t *state, mp_uint_t rs1, mp_uint_t rs2, mp_uint_t label) {
+    ptrdiff_t displacement = 0;
+    bool can_emit_short_jump = calculate_displacement_for_label(state, label, &displacement);
+
+    if (can_emit_short_jump && FIT_SIGNED(displacement, 13)) {
+        // beq rs1, rs2, displacement
+        asm_rv32_opcode_beq(state, rs1, rs2, displacement);
+        return;
+    }
+
+    // Compensate for the initial BNE opcode.
+    displacement -= ASM_WORD_SIZE;
+
+    mp_uint_t upper = 0;
+    mp_uint_t lower = 0;
+    split_immediate(displacement, &upper, &lower);
+
+    // bne   rs1, rs2, 12                      ; PC + 0
+    // auipc temporary, HI(displacement)       ; PC + 4
+    // jalr  zero, temporary, LO(displacement) ; PC + 8
+    // ...                                     ; PC + 12
+    asm_rv32_opcode_bne(state, rs1, rs2, 12);
+    asm_rv32_opcode_auipc(state, REG_TEMP2, upper);
+    asm_rv32_opcode_jalr(state, ASM_RV32_REG_ZERO, REG_TEMP2, lower);
+}
+
+void asm_rv32_emit_jump_if_reg_nonzero(asm_rv32_t *state, mp_uint_t rs, mp_uint_t label) {
+    ptrdiff_t displacement = 0;
+    bool can_emit_short_jump = calculate_displacement_for_label(state, label, &displacement);
+
+    if (can_emit_short_jump && FIT_SIGNED(displacement, 8) && RV32_IS_IN_C_REGISTER_WINDOW(rs)) {
+        // c.bnez rs', displacement
+        asm_rv32_opcode_cbnez(state, RV32_MAP_IN_C_REGISTER_WINDOW(rs), displacement);
+        return;
+    }
+
+    if (can_emit_short_jump && FIT_SIGNED(displacement, 13)) {
+        // bne rs, zero, displacement
+        asm_rv32_opcode_bne(state, rs, ASM_RV32_REG_ZERO, displacement);
+        return;
+    }
+
+    // if rs1 in C window and displacement is negative:
+    //    c.beqz rs', 10                           ; PC + 0
+    //    auipc  temporary, HI(displacement)       ; PC + 2
+    //    jalr   zero, temporary, LO(displacement) ; PC + 6
+    //    ...                                      ; PC + 10
+    // else:
+    //    beq    rs, zero, 12                      ; PC + 0
+    //    auipc  temporary, HI(displacement)       ; PC + 4
+    //    jalr   zero, temporary, LO(displacement) ; PC + 8
+    //    ...                                      ; PC + 12
+
+    if (can_emit_short_jump && RV32_IS_IN_C_REGISTER_WINDOW(rs)) {
+        asm_rv32_opcode_cbeqz(state, RV32_MAP_IN_C_REGISTER_WINDOW(rs), 10);
+        // Compensate for the C.BEQZ opcode.
+        displacement -= ASM_HALFWORD_SIZE;
+    } else {
+        asm_rv32_opcode_beq(state, rs, ASM_RV32_REG_ZERO, 12);
+        // Compensate for the BEQ opcode.
+        displacement -= ASM_WORD_SIZE;
+    }
+
+    mp_uint_t upper = 0;
+    mp_uint_t lower = 0;
+    split_immediate(displacement, &upper, &lower);
+    asm_rv32_opcode_auipc(state, REG_TEMP2, upper);
+    asm_rv32_opcode_jalr(state, ASM_RV32_REG_ZERO, REG_TEMP2, lower);
+}
+
+void asm_rv32_emit_mov_local_reg(asm_rv32_t *state, mp_uint_t local, mp_uint_t rs) {
+    mp_uint_t offset = state->locals_stack_offset + (local * ASM_WORD_SIZE);
+
+    if (FIT_UNSIGNED(offset >> 2, 6)) {
+        // c.swsp rs, offset
+        asm_rv32_opcode_cswsp(state, rs, offset);
+        return;
+    }
+
+    if (FIT_UNSIGNED(offset, 11)) {
+        // sw rs, offset(sp)
+        asm_rv32_opcode_sw(state, rs, ASM_RV32_REG_SP, offset);
+        return;
+    }
+
+    mp_uint_t upper = 0;
+    mp_uint_t lower = 0;
+    split_immediate(offset, &upper, &lower);
+
+    // lui   temporary, HI(offset) ; Or c.lui if possible
+    // c.add temporary, sp
+    // sw    rs, LO(offset)(temporary)
+    load_upper_immediate(state, REG_TEMP2, upper);
+    asm_rv32_opcode_cadd(state, REG_TEMP2, ASM_RV32_REG_SP);
+    asm_rv32_opcode_sw(state, rs, REG_TEMP2, lower);
+}
+
+void asm_rv32_emit_mov_reg_local(asm_rv32_t *state, mp_uint_t rd, mp_uint_t local) {
+    mp_uint_t offset = state->locals_stack_offset + (local * ASM_WORD_SIZE);
+
+    if (FIT_UNSIGNED(offset >> 2, 6)) {
+        // c.lwsp rd, offset
+        asm_rv32_opcode_clwsp(state, rd, offset);
+        return;
+    }
+
+    if (FIT_UNSIGNED(offset, 11)) {
+        // lw rd, offset(sp)
+        asm_rv32_opcode_lw(state, rd, ASM_RV32_REG_SP, offset);
+        return;
+    }
+
+    mp_uint_t upper = 0;
+    mp_uint_t lower = 0;
+    split_immediate(offset, &upper, &lower);
+
+    // lui   rd, HI(offset) ; Or c.lui if possible
+    // c.add rd, sp
+    // lw    rd, LO(offset)(rd)
+    load_upper_immediate(state, rd, upper);
+    asm_rv32_opcode_cadd(state, rd, ASM_RV32_REG_SP);
+    asm_rv32_opcode_lw(state, rd, rd, lower);
+}
+
+void asm_rv32_emit_mov_reg_local_addr(asm_rv32_t *state, mp_uint_t rd, mp_uint_t local) {
+    mp_uint_t offset = state->locals_stack_offset + (local * ASM_WORD_SIZE);
+
+    if (FIT_UNSIGNED(offset, 10) && offset != 0 && RV32_IS_IN_C_REGISTER_WINDOW(rd)) {
+        // c.addi4spn rd', offset
+        asm_rv32_opcode_caddi4spn(state, RV32_MAP_IN_C_REGISTER_WINDOW(rd), offset);
+        return;
+    }
+
+    if (FIT_UNSIGNED(offset, 11)) {
+        // addi rd, sp, offset
+        asm_rv32_opcode_addi(state, rd, ASM_RV32_REG_SP, offset);
+        return;
+    }
+
+    // li    rd, offset
+    // c.add rd, sp
+    load_full_immediate(state, rd, offset);
+    asm_rv32_opcode_cadd(state, rd, ASM_RV32_REG_SP);
+}
+
+static const uint8_t RV32_LOAD_OPCODE_TABLE[3] = {
+    0x04, 0x05, 0x02
+};
+
+void asm_rv32_emit_load_reg_reg_offset(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, int32_t offset, mp_uint_t operation_size) {
+    assert(operation_size <= 2 && "Operation size value out of range.");
+
+    int32_t scaled_offset = offset << operation_size;
+
+    if (scaled_offset >= 0 && operation_size == 2 && RV32_IS_IN_C_REGISTER_WINDOW(rd) && RV32_IS_IN_C_REGISTER_WINDOW(rs) && MP_FIT_UNSIGNED(6, scaled_offset)) {
+        // c.lw rd', offset(rs')
+        asm_rv32_opcode_clw(state, RV32_MAP_IN_C_REGISTER_WINDOW(rd), RV32_MAP_IN_C_REGISTER_WINDOW(rs), scaled_offset);
+        return;
+    }
+
+    if (MP_FIT_SIGNED(12, scaled_offset)) {
+        // lbu|lhu|lw rd, offset(rs)
+        asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_I(0x03, RV32_LOAD_OPCODE_TABLE[operation_size], rd, rs, scaled_offset));
+        return;
+    }
+
+    mp_uint_t upper = 0;
+    mp_uint_t lower = 0;
+    split_immediate(scaled_offset, &upper, &lower);
+
+    // lui        rd, HI(offset) ; Or c.lui if possible
+    // c.add      rd, rs
+    // lbu|lhu|lw rd, LO(offset)(rd)
+    load_upper_immediate(state, rd, upper);
+    asm_rv32_opcode_cadd(state, rd, rs);
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_I(0x03, RV32_LOAD_OPCODE_TABLE[operation_size], rd, rd, lower));
+}
+
+void asm_rv32_emit_jump(asm_rv32_t *state, mp_uint_t label) {
+    ptrdiff_t displacement = 0;
+    bool can_emit_short_jump = calculate_displacement_for_label(state, label, &displacement);
+
+    if (can_emit_short_jump && FIT_SIGNED(displacement, 12)) {
+        // c.j displacement
+        asm_rv32_opcode_cj(state, displacement);
+        return;
+    }
+
+    mp_uint_t upper = 0;
+    mp_uint_t lower = 0;
+    split_immediate(displacement, &upper, &lower);
+
+    // auipc temporary, HI(displacement)
+    // jalr  zero, temporary, LO(displacement)
+    asm_rv32_opcode_auipc(state, REG_TEMP2, upper);
+    asm_rv32_opcode_jalr(state, ASM_RV32_REG_ZERO, REG_TEMP2, lower);
+}
+
+void asm_rv32_emit_store_reg_reg_offset(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, int32_t offset, mp_uint_t operation_size) {
+    assert(operation_size <= 2 && "Operation size value out of range.");
+
+    int32_t scaled_offset = offset << operation_size;
+
+    if (scaled_offset >= 0 && operation_size == 2 && RV32_IS_IN_C_REGISTER_WINDOW(rd) && RV32_IS_IN_C_REGISTER_WINDOW(rs) && MP_FIT_UNSIGNED(6, scaled_offset)) {
+        // c.sw rd', offset(rs')
+        asm_rv32_opcode_csw(state, RV32_MAP_IN_C_REGISTER_WINDOW(rd), RV32_MAP_IN_C_REGISTER_WINDOW(rs), scaled_offset);
+        return;
+    }
+
+    if (MP_FIT_SIGNED(12, scaled_offset)) {
+        // sb|sh|sw rd, offset(rs)
+        asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_S(0x23, operation_size, rs, rd, scaled_offset));
+        return;
+    }
+
+    mp_uint_t upper = 0;
+    mp_uint_t lower = 0;
+    split_immediate(scaled_offset, &upper, &lower);
+
+    // lui      temporary, HI(offset) ; Or c.lui if possible
+    // c.add    temporary, rs
+    // sb|sh|sw rd, LO(offset)(temporary)
+    load_upper_immediate(state, REG_TEMP2, upper);
+    asm_rv32_opcode_cadd(state, REG_TEMP2, rs);
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_S(0x23, operation_size, REG_TEMP2, rd, lower));
+}
+
+void asm_rv32_emit_mov_reg_pcrel(asm_rv32_t *state, mp_uint_t rd, mp_uint_t label) {
+    ptrdiff_t displacement = (ptrdiff_t)(state->base.label_offsets[label] - state->base.code_offset);
+    mp_uint_t upper = 0;
+    mp_uint_t lower = 0;
+    split_immediate(displacement, &upper, &lower);
+
+    // auipc rd, HI(relative)
+    // addi  rd, rd, LO(relative)
+    asm_rv32_opcode_auipc(state, rd, upper);
+    asm_rv32_opcode_addi(state, rd, rd, lower);
+}
+
+void asm_rv32_emit_optimised_xor(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs) {
+    if (rs == rd) {
+        // c.li rd, 0
+        asm_rv32_opcode_cli(state, rd, 0);
+        return;
+    }
+
+    // xor rd, rd, rs
+    asm_rv32_opcode_xor(state, rd, rd, rs);
+}
+
+static bool asm_rv32_allow_zba_opcodes(void) {
+    return asm_rv32_allowed_extensions() & RV32_EXT_ZBA;
+}
+
+static void asm_rv32_fix_up_scaled_reg_reg_reg(asm_rv32_t *state, mp_uint_t rs1, mp_uint_t rs2, mp_uint_t operation_size) {
+    assert(operation_size <= 2 && "Operation size value out of range.");
+
+    if (operation_size > 0 && asm_rv32_allow_zba_opcodes()) {
+        // sh{1,2}add rs1, rs2, rs1
+        asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 1 << operation_size, 0x10, rs1, rs2, rs1));
+    } else {
+        if (operation_size > 0) {
+            asm_rv32_opcode_cslli(state, rs2, operation_size);
+        }
+        asm_rv32_opcode_cadd(state, rs1, rs2);
+    }
+}
+
+void asm_rv32_emit_load_reg_reg_reg(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2, mp_uint_t operation_size) {
+    asm_rv32_fix_up_scaled_reg_reg_reg(state, rs1, rs2, operation_size);
+    asm_rv32_emit_load_reg_reg_offset(state, rd, rs1, 0, operation_size);
+}
+
+void asm_rv32_emit_store_reg_reg_reg(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2, mp_uint_t operation_size) {
+    asm_rv32_fix_up_scaled_reg_reg_reg(state, rs1, rs2, operation_size);
+    asm_rv32_emit_store_reg_reg_offset(state, rd, rs1, 0, operation_size);
+}
+
+void asm_rv32_meta_comparison_eq(asm_rv32_t *state, mp_uint_t rs1, mp_uint_t rs2, mp_uint_t rd) {
+    // sub   rd, rs1, rs2
+    // sltiu rd, rd, 1
+    asm_rv32_opcode_sub(state, rd, rs1, rs2);
+    asm_rv32_opcode_sltiu(state, rd, rd, 1);
+}
+
+void asm_rv32_meta_comparison_ne(asm_rv32_t *state, mp_uint_t rs1, mp_uint_t rs2, mp_uint_t rd) {
+    // sub  rd, rs1, rs2
+    // sltu rd, zero, rd
+    asm_rv32_opcode_sub(state, rd, rs1, rs2);
+    asm_rv32_opcode_sltu(state, rd, ASM_RV32_REG_ZERO, rd);
+}
+
+void asm_rv32_meta_comparison_lt(asm_rv32_t *state, mp_uint_t rs1, mp_uint_t rs2, mp_uint_t rd, bool unsigned_comparison) {
+    // slt|sltu rd, rs1, rs2
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, (0x02 | (unsigned_comparison ? 1 : 0)), 0x00, rd, rs1, rs2));
+}
+
+void asm_rv32_meta_comparison_le(asm_rv32_t *state, mp_uint_t rs1, mp_uint_t rs2, mp_uint_t rd, bool unsigned_comparison) {
+    // slt[u] rd, rs2, rs1
+    // xori   rd, rd, 1
+    asm_rv32_meta_comparison_lt(state, rs2, rs1, rd, unsigned_comparison);
+    asm_rv32_opcode_xori(state, rd, rd, 1);
+}
+
+#endif // MICROPY_EMIT_RV32

APP_Framework/Framework/knowing/micropython/py/asmrv32.h

@@ -0,0 +1,817 @@
+/*
+ * This file is part of the MicroPython project, https://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2024 Alessandro Gatti
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#ifndef MICROPY_INCLUDED_PY_ASMRV32_H
+#define MICROPY_INCLUDED_PY_ASMRV32_H
+
+#include <assert.h>
+
+#include "py/asmbase.h"
+#include "py/emit.h"
+#include "py/misc.h"
+#include "py/persistentcode.h"
+
+#define ASM_RV32_REG_X0 (0)   // Zero
+#define ASM_RV32_REG_X1 (1)   // RA
+#define ASM_RV32_REG_X2 (2)   // SP
+#define ASM_RV32_REG_X3 (3)   // GP
+#define ASM_RV32_REG_X4 (4)   // TP
+#define ASM_RV32_REG_X5 (5)   // T0
+#define ASM_RV32_REG_X6 (6)   // T1
+#define ASM_RV32_REG_X7 (7)   // T2
+#define ASM_RV32_REG_X8 (8)   // S0
+#define ASM_RV32_REG_X9 (9)   // S1
+#define ASM_RV32_REG_X10 (10) // A0
+#define ASM_RV32_REG_X11 (11) // A1
+#define ASM_RV32_REG_X12 (12) // A2
+#define ASM_RV32_REG_X13 (13) // A3
+#define ASM_RV32_REG_X14 (14) // A4
+#define ASM_RV32_REG_X15 (15) // A5
+#define ASM_RV32_REG_X16 (16) // A6
+#define ASM_RV32_REG_X17 (17) // A7
+#define ASM_RV32_REG_X18 (18) // S2
+#define ASM_RV32_REG_X19 (19) // S3
+#define ASM_RV32_REG_X20 (20) // S4
+#define ASM_RV32_REG_X21 (21) // S5
+#define ASM_RV32_REG_X22 (22) // S6
+#define ASM_RV32_REG_X23 (23) // S7
+#define ASM_RV32_REG_X24 (24) // S8
+#define ASM_RV32_REG_X25 (25) // S9
+#define ASM_RV32_REG_X26 (26) // S10
+#define ASM_RV32_REG_X27 (27) // S11
+#define ASM_RV32_REG_X28 (28) // T3
+#define ASM_RV32_REG_X29 (29) // T4
+#define ASM_RV32_REG_X30 (30) // T5
+#define ASM_RV32_REG_X31 (31) // T6
+
+// Alternate register names.
+
+#define ASM_RV32_REG_ZERO (ASM_RV32_REG_X0)
+#define ASM_RV32_REG_RA (ASM_RV32_REG_X1)
+#define ASM_RV32_REG_SP (ASM_RV32_REG_X2)
+#define ASM_RV32_REG_GP (ASM_RV32_REG_X3)
+#define ASM_RV32_REG_TP (ASM_RV32_REG_X4)
+#define ASM_RV32_REG_A0 (ASM_RV32_REG_X10)
+#define ASM_RV32_REG_A1 (ASM_RV32_REG_X11)
+#define ASM_RV32_REG_A2 (ASM_RV32_REG_X12)
+#define ASM_RV32_REG_A3 (ASM_RV32_REG_X13)
+#define ASM_RV32_REG_A4 (ASM_RV32_REG_X14)
+#define ASM_RV32_REG_A5 (ASM_RV32_REG_X15)
+#define ASM_RV32_REG_A6 (ASM_RV32_REG_X16)
+#define ASM_RV32_REG_A7 (ASM_RV32_REG_X17)
+#define ASM_RV32_REG_T0 (ASM_RV32_REG_X5)
+#define ASM_RV32_REG_T1 (ASM_RV32_REG_X6)
+#define ASM_RV32_REG_T2 (ASM_RV32_REG_X7)
+#define ASM_RV32_REG_T3 (ASM_RV32_REG_X28)
+#define ASM_RV32_REG_T4 (ASM_RV32_REG_X29)
+#define ASM_RV32_REG_T5 (ASM_RV32_REG_X30)
+#define ASM_RV32_REG_T6 (ASM_RV32_REG_X31)
+#define ASM_RV32_REG_FP (ASM_RV32_REG_X8)
+#define ASM_RV32_REG_S0 (ASM_RV32_REG_X8)
+#define ASM_RV32_REG_S1 (ASM_RV32_REG_X9)
+#define ASM_RV32_REG_S2 (ASM_RV32_REG_X18)
+#define ASM_RV32_REG_S3 (ASM_RV32_REG_X19)
+#define ASM_RV32_REG_S4 (ASM_RV32_REG_X20)
+#define ASM_RV32_REG_S5 (ASM_RV32_REG_X21)
+#define ASM_RV32_REG_S6 (ASM_RV32_REG_X22)
+#define ASM_RV32_REG_S7 (ASM_RV32_REG_X23)
+#define ASM_RV32_REG_S8 (ASM_RV32_REG_X24)
+#define ASM_RV32_REG_S9 (ASM_RV32_REG_X25)
+#define ASM_RV32_REG_S10 (ASM_RV32_REG_X26)
+#define ASM_RV32_REG_S11 (ASM_RV32_REG_X27)
+
+#define RV32_AVAILABLE_REGISTERS_COUNT 32
+#define RV32_MAP_IN_C_REGISTER_WINDOW(register_number) \
+    ((register_number) - ASM_RV32_REG_X8)
+#define RV32_IS_IN_C_REGISTER_WINDOW(register_number) \
+    (((register_number) >= ASM_RV32_REG_X8) && ((register_number) <= ASM_RV32_REG_X15))
+
+typedef struct _asm_rv32_t {
+    // Opaque emitter state.
+    mp_asm_base_t base;
+    // Which registers are tainted and need saving/restoring.
+    mp_uint_t saved_registers_mask;
+    // How many locals must be stored on the stack.
+    mp_uint_t locals_count;
+    // The computed function stack size.
+    mp_uint_t stack_size;
+    // The stack offset where stack-based locals start to be stored.
+    mp_uint_t locals_stack_offset;
+} asm_rv32_t;
+
+enum {
+    RV32_EXT_NONE = 0,
+    RV32_EXT_ZBA = 1 << 0,
+
+    RV32_EXT_ALL = RV32_EXT_ZBA
+};
+
+typedef struct _asm_rv32_backend_options_t {
+    // This is a bitmask holding a combination of RV32_EXT_* entries.
+    uint8_t allowed_extensions;
+} asm_rv32_backend_options_t;
+
+void asm_rv32_entry(asm_rv32_t *state, mp_uint_t locals);
+void asm_rv32_exit(asm_rv32_t *state);
+void asm_rv32_end_pass(asm_rv32_t *state);
+
+////////////////////////////////////////////////////////////////////////////////
+
+#define RV32_ENCODE_TYPE_B(op, ft3, rs1, rs2, imm) \
+    ((op & 0x7F) | ((ft3 & 0x07) << 12) | ((imm & 0x800) >> 4) | \
+    ((imm & 0x1E) << 7) | ((rs1 & 0x1F) << 15) | ((rs2 & 0x1F) << 20) | \
+    ((imm & 0x7E0) << 20) | ((imm & 0x1000) << 19))
+
+#define RV32_ENCODE_TYPE_CSRI(op, ft3, rd, csr, imm) \
+    ((op & 0x7F) | ((rd & 0x1F) << 7) | ((ft3 & 0x07) << 12) | \
+    ((csr & 0xFFF) << 20) | ((imm & 0x1F) << 15))
+
+#define RV32_ENCODE_TYPE_I(op, ft3, rd, rs, imm) \
+    ((op & 0x7F) | ((rd & 0x1F) << 7) | ((ft3 & 0x07) << 12) | \
+    ((rs & 0x1F) << 15) | ((imm & 0xFFF) << 20))
+
+#define RV32_ENCODE_TYPE_J(op, rd, imm) \
+    ((op & 0x7F) | ((rd & 0x1F) << 7) | (imm & 0xFF000) | \
+    ((imm & 0x800) << 9) | ((imm & 0x7FE) << 20) | ((imm & 0x100000) << 11))
+
+#define RV32_ENCODE_TYPE_R(op, ft3, ft7, rd, rs1, rs2) \
+    ((op & 0x7F) | ((rd & 0x1F) << 7) | ((ft3 & 0x07) << 12) | \
+    ((rs1 & 0x1F) << 15) | ((rs2 & 0x1F) << 20) | ((ft7 & 0x7F) << 25))
+
+#define RV32_ENCODE_TYPE_S(op, ft3, rs1, rs2, imm) \
+    ((op & 0x7F) | ((imm & 0x1F) << 7) | ((ft3 & 0x07) << 12) | \
+    ((rs1 & 0x1F) << 15) | ((rs2 & 0x1F) << 20) | ((imm & 0xFE0) << 20))
+
+#define RV32_ENCODE_TYPE_CA(op, ft6, ft2, rd, rs) \
+    ((op & 0x03) | ((ft6 & 0x3F) << 10) | ((ft2 & 0x03) << 5) | \
+    ((rd & 0x03) << 7) | ((rs & 0x03) << 2))
+
+#define RV32_ENCODE_TYPE_U(op, rd, imm) \
+    ((op & 0x7F) | ((rd & 0x1F) << 7) | (imm & 0xFFFFF000))
+
+#define RV32_ENCODE_TYPE_CB(op, ft3, rs, imm) \
+    ((op & 0x03) | ((ft3 & 0x07) << 13) | ((rs & 0x07) << 7) | \
+    (((imm) & 0xE0) << 5) | (((imm) & 0x1F) << 2))
+
+#define RV32_ENCODE_TYPE_CI(op, ft3, rd, imm) \
+    ((op & 0x03) | ((ft3 & 0x07) << 13) | ((rd & 0x1F) << 7) | \
+    (((imm) & 0x20) << 7) | (((imm) & 0x1F) << 2))
+
+#define RV32_ENCODE_TYPE_CIW(op, ft3, rd, imm) \
+    ((op & 0x03) | ((ft3 & 0x07) << 13) | ((rd & 0x07) << 2) | \
+    ((imm & 0x3C0) << 1) | ((imm & 0x30) << 7) | \
+    ((imm & 0x08) << 2) | ((imm & 0x04) << 4))
+
+#define RV32_ENCODE_TYPE_CJ(op, ft3, imm) \
+    ((op & 0x03) | ((ft3 & 0x07) << 13) | ((imm & 0x0E) << 2) | \
+    ((imm & 0x300) << 1) | ((imm & 0x800) << 1) | ((imm & 0x400) >> 2) | \
+    ((imm & 0x80) >> 1) | ((imm & 0x40) << 1) | ((imm & 0x20) >> 3) | \
+    ((imm & 0x10) << 7))
+
+#define RV32_ENCODE_TYPE_CL(op, ft3, rd, rs, imm) \
+    ((op & 0x03) | ((ft3 & 0x07) << 13) | ((rd & 0x07) << 2) | \
+    ((rs & 0x07) << 7) | ((imm & 0x40) >> 1) | ((imm & 0x38) << 7) | \
+    ((imm & 0x04) << 4))
+
+#define RV32_ENCODE_TYPE_CR(op, ft4, rs1, rs2) \
+    ((op & 0x03) | ((rs2 & 0x1F) << 2) | ((rs1 & 0x1F) << 7) | ((ft4 & 0x0F) << 12))
+
+#define RV32_ENCODE_TYPE_CS(op, ft3, rd, rs, imm) \
+    ((op & 0x03) | ((ft3 & 0x07) << 13) | ((rd & 0x07) << 2) | \
+    ((rs & 0x07) << 7) | ((imm & 0x40) >> 1) | ((imm & 0x38) << 7) | \
+    ((imm & 0x04) << 4))
+
+#define RV32_ENCODE_TYPE_CSS(op, ft3, rs, imm) \
+    ((op & 0x03) | ((ft3 & 0x07) << 13) | ((rs & 0x1F) << 2) | ((imm) & 0x3F) << 7)
+
+void asm_rv32_emit_word_opcode(asm_rv32_t *state, mp_uint_t opcode);
+void asm_rv32_emit_halfword_opcode(asm_rv32_t *state, mp_uint_t opcode);
+
+// ADD RD, RS1, RS2
+static inline void asm_rv32_opcode_add(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2) {
+    // R: 0000000 ..... ..... 000 ..... 0110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 0x00, 0x00, rd, rs1, rs2));
+}
+
+// ADDI RD, RS, IMMEDIATE
+static inline void asm_rv32_opcode_addi(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, mp_int_t immediate) {
+    // I: ............ ..... 000 ..... 0010011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_I(0x13, 0x00, rd, rs, immediate));
+}
+
+// AND RD, RS1, RS2
+static inline void asm_rv32_opcode_and(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2) {
+    // R: 0000000 ..... ..... 111 ..... 0110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 0x07, 0x00, rd, rs1, rs2));
+}
+
+// ANDI RD, RS, IMMEDIATE
+static inline void asm_rv32_opcode_andi(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, mp_int_t immediate) {
+    // I: ............ ..... 111 ..... 0010011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_I(0x13, 0x07, rd, rs, immediate));
+}
+
+// AUIPC RD, offset
+static inline void asm_rv32_opcode_auipc(asm_rv32_t *state, mp_uint_t rd, mp_int_t offset) {
+    // U: .................... ..... 0010111
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_U(0x17, rd, offset));
+}
+
+// BEQ RS1, RS2, OFFSET
+static inline void asm_rv32_opcode_beq(asm_rv32_t *state, mp_uint_t rs1, mp_uint_t rs2, mp_int_t offset) {
+    // B: . ...... ..... ..... 000 .... . 1100011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_B(0x63, 0x00, rs1, rs2, offset));
+}
+
+// BGE RS1, RS2, OFFSET
+static inline void asm_rv32_opcode_bge(asm_rv32_t *state, mp_uint_t rs1, mp_uint_t rs2, mp_int_t offset) {
+    // B: . ...... ..... ..... 101 .... . 1100011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_B(0x63, 0x05, rs1, rs2, offset));
+}
+
+// BGEU RS1, RS2, OFFSET
+static inline void asm_rv32_opcode_bgeu(asm_rv32_t *state, mp_uint_t rs1, mp_uint_t rs2, mp_int_t offset) {
+    // B: . ...... ..... ..... 111 .... . 1100011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_B(0x63, 0x07, rs1, rs2, offset));
+}
+
+// BLT RS1, RS2, OFFSET
+static inline void asm_rv32_opcode_blt(asm_rv32_t *state, mp_uint_t rs1, mp_uint_t rs2, mp_int_t offset) {
+    // B: . ...... ..... ..... 100 .... . 1100011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_B(0x63, 0x04, rs1, rs2, offset));
+}
+
+// BLTU RS1, RS2, OFFSET
+static inline void asm_rv32_opcode_bltu(asm_rv32_t *state, mp_uint_t rs1, mp_uint_t rs2, mp_int_t offset) {
+    // B: . ...... ..... ..... 110 .... . 1100011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_B(0x63, 0x06, rs1, rs2, offset));
+}
+
+// BNE RS1, RS2, OFFSET
+static inline void asm_rv32_opcode_bne(asm_rv32_t *state, mp_uint_t rs1, mp_uint_t rs2, mp_int_t offset) {
+    // B: . ...... ..... ..... 001 .... . 1100011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_B(0x63, 0x01, rs1, rs2, offset));
+}
+
+// C.ADD RD, RS
+static inline void asm_rv32_opcode_cadd(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs) {
+    // CR: 1001 ..... ..... 10
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CR(0x02, 0x09, rd, rs));
+}
+
+// C.ADDI RD, IMMEDIATE
+static inline void asm_rv32_opcode_caddi(asm_rv32_t *state, mp_uint_t rd, mp_int_t immediate) {
+    // CI: 000 . ..... ..... 01
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CI(0x01, 0x00, rd, immediate));
+}
+
+// C.ADDI4SPN RD', IMMEDIATE
+static inline void asm_rv32_opcode_caddi4spn(asm_rv32_t *state, mp_uint_t rd, mp_uint_t immediate) {
+    // CIW: 000 ........ ... 00
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CIW(0x00, 0x00, rd, immediate));
+}
+
+// C.AND RD', RS'
+static inline void asm_rv32_opcode_cand(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs) {
+    // CA: 100011 ... 11 ... 01
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CA(0x01, 0x23, 0x03, rd, rs));
+}
+
+// C.ANDI RD', IMMEDIATE
+static inline void asm_rv32_opcode_candi(asm_rv32_t *state, mp_uint_t rd, mp_int_t immediate) {
+    // CB: 100 . 10 ... ..... 01
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CB(0x01, 0x04, rd,
+        (((immediate & 0x20) << 2) | (immediate & 0x1F) | 0x40)));
+}
+
+// C.BEQZ RS', IMMEDIATE
+static inline void asm_rv32_opcode_cbeqz(asm_rv32_t *state, mp_uint_t rs, mp_int_t offset) {
+    // CB: 110 ... ... ..... 01
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CB(0x01, 0x06, rs,
+        (((offset & 0x100) >> 1) | ((offset & 0xC0) >> 3) | ((offset & 0x20) >> 5) |
+            ((offset & 0x18) << 2) | (offset & 0x06))));
+}
+
+// C.BNEZ RS', IMMEDIATE
+static inline void asm_rv32_opcode_cbnez(asm_rv32_t *state, mp_uint_t rs, mp_int_t offset) {
+    // CB: 111 ... ... ..... 01
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CB(0x01, 0x07, rs,
+        (((offset & 0x100) >> 1) | ((offset & 0xC0) >> 3) | ((offset & 0x20) >> 5) |
+            ((offset & 0x18) << 2) | (offset & 0x06))));
+}
+
+// C.EBREAK
+static inline void asm_rv32_opcode_cebreak(asm_rv32_t *state) {
+    // CA: 100 1 00000 00000 10
+    asm_rv32_emit_halfword_opcode(state, 0x9002);
+}
+
+// C.J OFFSET
+static inline void asm_rv32_opcode_cj(asm_rv32_t *state, mp_int_t offset) {
+    // CJ: 101 ........... 01
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CJ(0x01, 0x05, offset));
+}
+
+// C.JAL OFFSET
+static inline void asm_rv32_opcode_cjal(asm_rv32_t *state, mp_int_t offset) {
+    // CJ: 001 ........... 01
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CJ(0x01, 0x01, offset));
+}
+
+// C.JALR RS
+static inline void asm_rv32_opcode_cjalr(asm_rv32_t *state, mp_uint_t rs) {
+    // CR: 1001 ..... 00000 10
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CR(0x02, 0x09, rs, 0));
+}
+
+// C.JR RS
+static inline void asm_rv32_opcode_cjr(asm_rv32_t *state, mp_uint_t rs) {
+    // CR: 1000 ..... 00000 10
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CR(0x02, 0x08, rs, 0));
+}
+
+// C.LI RD, IMMEDIATE
+static inline void asm_rv32_opcode_cli(asm_rv32_t *state, mp_uint_t rd, mp_int_t immediate) {
+    // CI: 010 . ..... ..... 01
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CI(0x01, 0x02, rd, immediate));
+}
+
+// C.LUI RD, IMMEDIATE
+static inline void asm_rv32_opcode_clui(asm_rv32_t *state, mp_uint_t rd, mp_int_t immediate) {
+    // CI: 011 . ..... ..... 01
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CI(0x01, 0x03, rd, immediate >> 12));
+}
+
+// C.LW RD', OFFSET(RS')
+static inline void asm_rv32_opcode_clw(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, mp_int_t offset) {
+    // CL: 010 ... ... .. ... 00
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CL(0x00, 0x02, rd, rs, offset));
+}
+
+// C.LWSP RD, OFFSET
+static inline void asm_rv32_opcode_clwsp(asm_rv32_t *state, mp_uint_t rd, mp_uint_t offset) {
+    // CI: 010 . ..... ..... 10
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CI(0x02, 0x02, rd, ((offset & 0xC0) >> 6) | (offset & 0x3C)));
+}
+
+// C.MV RD, RS
+static inline void asm_rv32_opcode_cmv(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs) {
+    // CR: 1000 ..... ..... 10
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CR(0x02, 0x08, rd, rs));
+}
+
+// C.NOP
+static inline void asm_rv32_opcode_cnop(asm_rv32_t *state) {
+    // CI: 000 . 00000 ..... 01
+    asm_rv32_emit_halfword_opcode(state, 0x0001);
+}
+
+// C.OR RD', RS'
+static inline void asm_rv32_opcode_cor(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs) {
+    // CA: 100011 ... 10 ... 01
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CA(0x01, 0x23, 0x02, rd, rs));
+}
+
+// C.SLLI RD, IMMEDIATE
+static inline void asm_rv32_opcode_cslli(asm_rv32_t *state, mp_uint_t rd, mp_int_t immediate) {
+    // CI: 000 . ..... ..... 10
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CI(0x02, 0x00, rd, immediate));
+}
+
+// C.SRAI RD, IMMEDIATE
+static inline void asm_rv32_opcode_csrai(asm_rv32_t *state, mp_uint_t rd, mp_int_t immediate) {
+    // CB: 100 . 01 ... ..... 01
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CB(0x01, 0x04, rd,
+        (((immediate & 0x20) << 2) | (immediate & 0x1F) | 0x20)));
+}
+
+// C.SRLI RD, IMMEDIATE
+static inline void asm_rv32_opcode_csrli(asm_rv32_t *state, mp_uint_t rd, mp_int_t immediate) {
+    // CB: 100 . 00 ... ..... 01
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CB(0x01, 0x04, rd,
+        (((immediate & 0x20) << 2) | (immediate & 0x1F))));
+}
+
+// C.SUB RD', RS'
+static inline void asm_rv32_opcode_csub(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs) {
+    // CA: 100011 ... 00 ... 01
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CA(0x01, 0x23, 0x00, rd, rs));
+}
+
+// C.SW RS1', OFFSET(RS2')
+static inline void asm_rv32_opcode_csw(asm_rv32_t *state, mp_uint_t rs1, mp_uint_t rs2, mp_int_t offset) {
+    // CS: 110 ... ... .. ... 00
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CL(0x00, 0x06, rs1, rs2, offset));
+}
+
+// C.SWSP RS, OFFSET
+static inline void asm_rv32_opcode_cswsp(asm_rv32_t *state, mp_uint_t rs, mp_uint_t offset) {
+    // CSS: 010 ...... ..... 10
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CSS(0x02, 0x06, rs, ((offset & 0xC0) >> 6) | (offset & 0x3C)));
+}
+
+// C.XOR RD', RS'
+static inline void asm_rv32_opcode_cxor(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs) {
+    // CA: 100011 ... 01 ... 01
+    asm_rv32_emit_halfword_opcode(state, RV32_ENCODE_TYPE_CA(0x01, 0x23, 0x01, rd, rs));
+}
+
+// CSRRC RD, RS, IMMEDIATE
+static inline void asm_rv32_opcode_csrrc(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, mp_int_t immediate) {
+    // I: ............ ..... 011 ..... 1110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_I(0x73, 0x03, rd, rs, immediate));
+}
+
+// CSRRS RD, RS, IMMEDIATE
+static inline void asm_rv32_opcode_csrrs(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, mp_int_t immediate) {
+    // I: ............ ..... 010 ..... 1110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_I(0x73, 0x02, rd, rs, immediate));
+}
+
+// CSRRW RD, RS, IMMEDIATE
+static inline void asm_rv32_opcode_csrrw(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, mp_int_t immediate) {
+    // I: ............ ..... 001 ..... 1110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_I(0x73, 0x01, rd, rs, immediate));
+}
+
+// CSRRCI RD, CSR, IMMEDIATE
+static inline void asm_rv32_opcode_csrrci(asm_rv32_t *state, mp_uint_t rd, mp_uint_t csr, mp_int_t immediate) {
+    // CSRI: ............ ..... 111 ..... 1110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_CSRI(0x73, 0x07, rd, csr, immediate));
+}
+
+// CSRRSI RD, CSR, IMMEDIATE
+static inline void asm_rv32_opcode_csrrsi(asm_rv32_t *state, mp_uint_t rd, mp_uint_t csr, mp_int_t immediate) {
+    // CSRI: ............ ..... 110 ..... 1110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_CSRI(0x73, 0x06, rd, csr, immediate));
+}
+
+// CSRRWI RD, CSR, IMMEDIATE
+static inline void asm_rv32_opcode_csrrwi(asm_rv32_t *state, mp_uint_t rd, mp_uint_t csr, mp_int_t immediate) {
+    // CSRI: ............ ..... 101 ..... 1110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_CSRI(0x73, 0x05, rd, csr, immediate));
+}
+
+// DIV RD, RS1, RS2
+static inline void asm_rv32_opcode_div(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2) {
+    // R: 0000001 ..... ..... 100 ..... 0110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 0x04, 0x01, rd, rs1, rs2));
+}
+
+// DIVU RD, RS1, RS2
+static inline void asm_rv32_opcode_divu(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2) {
+    // R: 0000001 ..... ..... 101 ..... 0110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 0x05, 0x01, rd, rs1, rs2));
+}
+
+// EBREAK
+static inline void asm_rv32_opcode_ebreak(asm_rv32_t *state) {
+    // I: 000000000001 00000 000 00000 1110011
+    asm_rv32_emit_word_opcode(state, 0x100073);
+}
+
+// ECALL
+static inline void asm_rv32_opcode_ecall(asm_rv32_t *state) {
+    // I: 000000000000 00000 000 00000 1110011
+    asm_rv32_emit_word_opcode(state, 0x73);
+}
+
+// JAL RD, OFFSET
+static inline void asm_rv32_opcode_jal(asm_rv32_t *state, mp_uint_t rd, mp_int_t offset) {
+    // J: ......................... 1101111
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_J(0x6F, rd, offset));
+}
+
+// JALR RD, RS, OFFSET
+static inline void asm_rv32_opcode_jalr(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, mp_int_t offset) {
+    // I: ............ ..... 000 ..... 1100111
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_I(0x67, 0x00, rd, rs, offset));
+}
+
+// LB RD, OFFSET(RS)
+static inline void asm_rv32_opcode_lb(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, mp_int_t offset) {
+    // I: ............ ..... 000 ..... 0000011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_I(0x03, 0x00, rd, rs, offset));
+}
+
+// LBU RD, OFFSET(RS)
+static inline void asm_rv32_opcode_lbu(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, mp_int_t offset) {
+    // I: ............ ..... 100 ..... 0000011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_I(0x03, 0x04, rd, rs, offset));
+}
+
+// LH RD, OFFSET(RS)
+static inline void asm_rv32_opcode_lh(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, mp_int_t offset) {
+    // I: ............ ..... 001 ..... 0000011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_I(0x03, 0x01, rd, rs, offset));
+}
+
+// LHU RD, OFFSET(RS)
+static inline void asm_rv32_opcode_lhu(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, mp_int_t offset) {
+    // I: ............ ..... 101 ..... 0000011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_I(0x03, 0x05, rd, rs, offset));
+}
+
+// LUI RD, IMMEDIATE
+static inline void asm_rv32_opcode_lui(asm_rv32_t *state, mp_uint_t rd, mp_int_t immediate) {
+    // U: .................... ..... 0110111
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_U(0x37, rd, immediate));
+}
+
+// LW RD, OFFSET(RS)
+static inline void asm_rv32_opcode_lw(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, mp_int_t offset) {
+    // I: ............ ..... 010 ..... 0000011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_I(0x03, 0x02, rd, rs, offset));
+}
+
+// MUL RD, RS1, RS2
+static inline void asm_rv32_opcode_mul(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2) {
+    // R: 0000001 ..... ..... 000 ..... 0110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 0x00, 0x01, rd, rs1, rs2));
+}
+
+// MULH RD, RS1, RS2
+static inline void asm_rv32_opcode_mulh(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2) {
+    // R: 0000001 ..... ..... 001 ..... 0110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 0x01, 0x01, rd, rs1, rs2));
+}
+
+// MULHSU RD, RS1, RS2
+static inline void asm_rv32_opcode_mulhsu(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2) {
+    // R: 0000001 ..... ..... 010 ..... 0110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 0x02, 0x01, rd, rs1, rs2));
+}
+
+// MULHU RD, RS1, RS2
+static inline void asm_rv32_opcode_mulhu(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2) {
+    // R: 0000001 ..... ..... 011 ..... 0110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 0x03, 0x01, rd, rs1, rs2));
+}
+
+// OR RD, RS1, RS2
+static inline void asm_rv32_opcode_or(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2) {
+    // R: 0000000 ..... ..... 110 ..... 0110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 0x06, 0x00, rd, rs1, rs2));
+}
+
+// ORI RD, RS, IMMEDIATE
+static inline void asm_rv32_opcode_ori(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, mp_int_t immediate) {
+    // I: ............ ..... 110 ..... 0010011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_I(0x13, 0x06, rd, rs, immediate));
+}
+
+// REM RD, RS1, RS2
+static inline void asm_rv32_opcode_rem(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2) {
+    // R: 0000001 ..... ..... 110 ..... 0110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 0x06, 0x01, rd, rs1, rs2));
+}
+
+// REMU RD, RS1, RS2
+static inline void asm_rv32_opcode_remu(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2) {
+    // R: 0000001 ..... ..... 111 ..... 0110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 0x07, 0x01, rd, rs1, rs2));
+}
+
+// SH1ADD RD, RS1, RS2
+static inline void asm_rv32_opcode_sh1add(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2) {
+    // R: 0010000 ..... ..... 010 ..... 0110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 0x02, 0x10, rd, rs1, rs2));
+}
+
+// SH2ADD RD, RS1, RS2
+static inline void asm_rv32_opcode_sh2add(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2) {
+    // R: 0010000 ..... ..... 100 ..... 0110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 0x04, 0x10, rd, rs1, rs2));
+}
+
+// SH3ADD RD, RS1, RS2
+static inline void asm_rv32_opcode_sh3add(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2) {
+    // R: 0010000 ..... ..... 110 ..... 0110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 0x06, 0x10, rd, rs1, rs2));
+}
+
+// SLL RD, RS1, RS2
+static inline void asm_rv32_opcode_sll(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2) {
+    // R: 0000000 ..... ..... 001 ..... 0110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 0x01, 0x00, rd, rs1, rs2));
+}
+
+// SLLI RD, RS, IMMEDIATE
+static inline void asm_rv32_opcode_slli(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, mp_int_t immediate) {
+    // I: 0000000..... ..... 001 ..... 0010011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_I(0x13, 0x01, rd, rs, immediate & 0x1F));
+}
+
+// SLT RD, RS1, RS2
+static inline void asm_rv32_opcode_slt(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2) {
+    // R: 0000000 ..... ..... 010 ..... 0110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 0x02, 0x00, rd, rs1, rs2));
+}
+
+// SLTI RD, RS, IMMEDIATE
+static inline void asm_rv32_opcode_slti(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, mp_int_t immediate) {
+    // I: ............ ..... 010 ..... 0010011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_I(0x13, 0x02, rd, rs, immediate));
+}
+
+// SLTIU RD, RS, IMMEDIATE
+static inline void asm_rv32_opcode_sltiu(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, mp_int_t immediate) {
+    // I: ............ ..... 011 ..... 0010011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_I(0x13, 0x03, rd, rs, immediate));
+}
+
+// SLTU RD, RS1, RS2
+static inline void asm_rv32_opcode_sltu(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2) {
+    // R: 0000000 ..... ..... 011 ..... 0110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 0x03, 0x00, rd, rs1, rs2));
+}
+
+// SRA RD, RS1, RS2
+static inline void asm_rv32_opcode_sra(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2) {
+    // R: 0100000 ..... ..... 101 ..... 0110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 0x05, 0x20, rd, rs1, rs2));
+}
+
+// SRAI RD, RS, IMMEDIATE
+static inline void asm_rv32_opcode_srai(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, mp_int_t immediate) {
+    // I: 0100000..... ..... 101 ..... 0010011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_I(0x13, 0x05, rd, rs, ((immediate & 0x1F) | 0x400)));
+}
+
+// SRL RD, RS1, RS2
+static inline void asm_rv32_opcode_srl(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2) {
+    // R: 0000000 ..... ..... 101 ..... 0110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 0x05, 0x00, rd, rs1, rs2));
+}
+
+// SRLI RD, RS, IMMEDIATE
+static inline void asm_rv32_opcode_srli(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, mp_int_t immediate) {
+    // I: 0000000..... ..... 101 ..... 0010011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_I(0x13, 0x05, rd, rs, immediate & 0x1F));
+}
+
+// SUB RD, RS1, RS2
+static inline void asm_rv32_opcode_sub(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2) {
+    // R: 0100000 ..... ..... 000 ..... 0110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 0x00, 0x20, rd, rs1, rs2));
+}
+
+// SB RS2, OFFSET(RS1)
+static inline void asm_rv32_opcode_sb(asm_rv32_t *state, mp_uint_t rs2, mp_uint_t rs1, mp_int_t offset) {
+    // S: ....... ..... ..... 000 ..... 0100011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_S(0x23, 0x00, rs1, rs2, offset));
+}
+
+// SH RS2, OFFSET(RS1)
+static inline void asm_rv32_opcode_sh(asm_rv32_t *state, mp_uint_t rs2, mp_uint_t rs1, mp_int_t offset) {
+    // S: ....... ..... ..... 001 ..... 0100011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_S(0x23, 0x01, rs1, rs2, offset));
+}
+
+// SW RS2, OFFSET(RS1)
+static inline void asm_rv32_opcode_sw(asm_rv32_t *state, mp_uint_t rs2, mp_uint_t rs1, mp_int_t offset) {
+    // S: ....... ..... ..... 010 ..... 0100011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_S(0x23, 0x02, rs1, rs2, offset));
+}
+
+// XOR RD, RS1, RS2
+static inline void asm_rv32_opcode_xor(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2) {
+    // R: 0000000 ..... ..... 100 ..... 0110011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_R(0x33, 0x04, 0x00, rd, rs1, rs2));
+}
+
+// XORI RD, RS, IMMEDIATE
+static inline void asm_rv32_opcode_xori(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, mp_int_t immediate) {
+    // I: ............ ..... 100 ..... 0010011
+    asm_rv32_emit_word_opcode(state, RV32_ENCODE_TYPE_I(0x13, 0x04, rd, rs, immediate));
+}
+
+#define MICROPY_RV32_EXTENSIONS \
+    (MICROPY_EMIT_RV32_ZBA ? RV32_EXT_ZBA : 0)
+
+static inline uint8_t asm_rv32_allowed_extensions(void) {
+    uint8_t extensions = MICROPY_RV32_EXTENSIONS;
+    #if MICROPY_DYNAMIC_COMPILER
+    if (mp_dynamic_compiler.backend_options != NULL) {
+        extensions |= ((asm_rv32_backend_options_t *)mp_dynamic_compiler.backend_options)->allowed_extensions;
+    }
+    #endif
+    return extensions;
+}
+
+#define ASM_WORD_SIZE (4)
+#define ASM_HALFWORD_SIZE (2)
+
+#define REG_RET ASM_RV32_REG_A0
+#define REG_ARG_1 ASM_RV32_REG_A0
+#define REG_ARG_2 ASM_RV32_REG_A1
+#define REG_ARG_3 ASM_RV32_REG_A2
+#define REG_ARG_4 ASM_RV32_REG_A3
+#define REG_TEMP0 ASM_RV32_REG_T1
+#define REG_TEMP1 ASM_RV32_REG_T2
+#define REG_TEMP2 ASM_RV32_REG_T3
+#define REG_FUN_TABLE ASM_RV32_REG_S1
+#define REG_LOCAL_1 ASM_RV32_REG_S3
+#define REG_LOCAL_2 ASM_RV32_REG_S4
+#define REG_LOCAL_3 ASM_RV32_REG_S5
+#define REG_ZERO ASM_RV32_REG_ZERO
+
+void asm_rv32_meta_comparison_eq(asm_rv32_t *state, mp_uint_t rs1, mp_uint_t rs2, mp_uint_t rd);
+void asm_rv32_meta_comparison_ne(asm_rv32_t *state, mp_uint_t rs1, mp_uint_t rs2, mp_uint_t rd);
+void asm_rv32_meta_comparison_lt(asm_rv32_t *state, mp_uint_t rs1, mp_uint_t rs2, mp_uint_t rd, bool unsigned_comparison);
+void asm_rv32_meta_comparison_le(asm_rv32_t *state, mp_uint_t rs1, mp_uint_t rs2, mp_uint_t rd, bool unsigned_comparison);
+
+void asm_rv32_emit_optimised_load_immediate(asm_rv32_t *state, mp_uint_t rd, mp_int_t immediate);
+void asm_rv32_emit_load_reg_reg_reg(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2, mp_uint_t operation_size);
+void asm_rv32_emit_store_reg_reg_reg(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs1, mp_uint_t rs2, mp_uint_t operation_size);
+
+#ifdef GENERIC_ASM_API
+
+void asm_rv32_emit_call_ind(asm_rv32_t *state, mp_uint_t index);
+void asm_rv32_emit_jump(asm_rv32_t *state, mp_uint_t label);
+void asm_rv32_emit_jump_if_reg_eq(asm_rv32_t *state, mp_uint_t rs1, mp_uint_t rs2, mp_uint_t label);
+void asm_rv32_emit_jump_if_reg_nonzero(asm_rv32_t *state, mp_uint_t rs, mp_uint_t label);
+void asm_rv32_emit_load_reg_reg_offset(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs, int32_t offset, mp_uint_t operation_size);
+void asm_rv32_emit_mov_local_reg(asm_rv32_t *state, mp_uint_t local, mp_uint_t rs);
+void asm_rv32_emit_mov_reg_local_addr(asm_rv32_t *state, mp_uint_t rd, mp_uint_t local);
+void asm_rv32_emit_mov_reg_local(asm_rv32_t *state, mp_uint_t rd, mp_uint_t local);
+void asm_rv32_emit_mov_reg_pcrel(asm_rv32_t *state, mp_uint_t rd, mp_uint_t label);
+void asm_rv32_emit_optimised_xor(asm_rv32_t *state, mp_uint_t rd, mp_uint_t rs);
+void asm_rv32_emit_store_reg_reg_offset(asm_rv32_t *state, mp_uint_t source, mp_uint_t base, int32_t offset, mp_uint_t operation_size);
+
+#define ASM_T asm_rv32_t
+#define ASM_ENTRY(state, labels, name) asm_rv32_entry(state, labels)
+#define ASM_EXIT(state) asm_rv32_exit(state)
+#define ASM_END_PASS(state) asm_rv32_end_pass(state)
+
+#define ASM_ADD_REG_REG(state, rd, rs) asm_rv32_opcode_cadd(state, rd, rs)
+#define ASM_AND_REG_REG(state, rd, rs) asm_rv32_opcode_and(state, rd, rs, rd)
+#define ASM_ASR_REG_REG(state, rd, rs) asm_rv32_opcode_sra(state, rd, rd, rs)
+#define ASM_CALL_IND(state, index) asm_rv32_emit_call_ind(state, index)
+#define ASM_JUMP(state, label) asm_rv32_emit_jump(state, label)
+#define ASM_JUMP_IF_REG_EQ(state, rs1, rs2, label) asm_rv32_emit_jump_if_reg_eq(state, rs1, rs2, label)
+#define ASM_JUMP_IF_REG_NONZERO(state, rs, label, bool_test) asm_rv32_emit_jump_if_reg_nonzero(state, rs, label)
+#define ASM_JUMP_IF_REG_ZERO(state, rs, label, bool_test) asm_rv32_emit_jump_if_reg_eq(state, rs, ASM_RV32_REG_ZERO, label)
+#define ASM_JUMP_REG(state, rs) asm_rv32_opcode_cjr(state, rs)
+#define ASM_LOAD_REG_REG_OFFSET(state, rd, rs, offset) ASM_LOAD32_REG_REG_OFFSET(state, rd, rs, offset)
+#define ASM_LOAD8_REG_REG(state, rd, rs) ASM_LOAD8_REG_REG_OFFSET(state, rd, rs, 0)
+#define ASM_LOAD16_REG_REG(state, rd, rs) ASM_LOAD16_REG_REG_OFFSET(state, rd, rs, 0)
+#define ASM_LOAD32_REG_REG(state, rd, rs) ASM_LOAD32_REG_REG_OFFSET(state, rd, rs, 0)
+#define ASM_LOAD8_REG_REG_OFFSET(state, rd, rs, offset) asm_rv32_emit_load_reg_reg_offset(state, rd, rs, offset, 0)
+#define ASM_LOAD16_REG_REG_OFFSET(state, rd, rs, offset) asm_rv32_emit_load_reg_reg_offset(state, rd, rs, offset, 1)
+#define ASM_LOAD32_REG_REG_OFFSET(state, rd, rs, offset) asm_rv32_emit_load_reg_reg_offset(state, rd, rs, offset, 2)
+#define ASM_LSL_REG_REG(state, rd, rs) asm_rv32_opcode_sll(state, rd, rd, rs)
+#define ASM_LSR_REG_REG(state, rd, rs) asm_rv32_opcode_srl(state, rd, rd, rs)
+#define ASM_MOV_LOCAL_REG(state, local, rs) asm_rv32_emit_mov_local_reg(state, local, rs)
+#define ASM_MOV_REG_IMM(state, rd, imm) asm_rv32_emit_optimised_load_immediate(state, rd, imm)
+#define ASM_MOV_REG_LOCAL_ADDR(state, rd, local) asm_rv32_emit_mov_reg_local_addr(state, rd, local)
+#define ASM_MOV_REG_LOCAL(state, rd, local) asm_rv32_emit_mov_reg_local(state, rd, local)
+#define ASM_MOV_REG_PCREL(state, rd, label) asm_rv32_emit_mov_reg_pcrel(state, rd, label)
+#define ASM_MOV_REG_REG(state, rd, rs) asm_rv32_opcode_cmv(state, rd, rs)
+#define ASM_MUL_REG_REG(state, rd, rs) asm_rv32_opcode_mul(state, rd, rd, rs)
+#define ASM_NEG_REG(state, rd) asm_rv32_opcode_sub(state, rd, ASM_RV32_REG_ZERO, rd)
+#define ASM_NOT_REG(state, rd) asm_rv32_opcode_xori(state, rd, rd, -1)
+#define ASM_OR_REG_REG(state, rd, rs) asm_rv32_opcode_or(state, rd, rd, rs)
+#define ASM_STORE_REG_REG_OFFSET(state, rd, rs, offset) ASM_STORE32_REG_REG_OFFSET(state, rd, rs, offset)
+#define ASM_STORE8_REG_REG(state, rs1, rs2) ASM_STORE8_REG_REG_OFFSET(state, rs1, rs2, 0)
+#define ASM_STORE16_REG_REG(state, rs1, rs2) ASM_STORE16_REG_REG_OFFSET(state, rs1, rs2, 0)
+#define ASM_STORE32_REG_REG(state, rs1, rs2) ASM_STORE32_REG_REG_OFFSET(state, rs1, rs2, 0)
+#define ASM_STORE8_REG_REG_OFFSET(state, rd, rs, offset) asm_rv32_emit_store_reg_reg_offset(state, rd, rs, offset, 0)
+#define ASM_STORE16_REG_REG_OFFSET(state, rd, rs, offset) asm_rv32_emit_store_reg_reg_offset(state, rd, rs, offset, 1)
+#define ASM_STORE32_REG_REG_OFFSET(state, rd, rs, offset) asm_rv32_emit_store_reg_reg_offset(state, rd, rs, offset, 2)
+#define ASM_SUB_REG_REG(state, rd, rs) asm_rv32_opcode_sub(state, rd, rd, rs)
+#define ASM_XOR_REG_REG(state, rd, rs) asm_rv32_emit_optimised_xor(state, rd, rs)
+#define ASM_CLR_REG(state, rd)
+#define ASM_LOAD8_REG_REG_REG(state, rd, rs1, rs2) asm_rv32_emit_load_reg_reg_reg(state, rd, rs1, rs2, 0)
+#define ASM_LOAD16_REG_REG_REG(state, rd, rs1, rs2) asm_rv32_emit_load_reg_reg_reg(state, rd, rs1, rs2, 1)
+#define ASM_LOAD32_REG_REG_REG(state, rd, rs1, rs2) asm_rv32_emit_load_reg_reg_reg(state, rd, rs1, rs2, 2)
+#define ASM_STORE8_REG_REG_REG(state, rd, rs1, rs2) asm_rv32_emit_store_reg_reg_reg(state, rd, rs1, rs2, 0)
+#define ASM_STORE16_REG_REG_REG(state, rd, rs1, rs2) asm_rv32_emit_store_reg_reg_reg(state, rd, rs1, rs2, 1)
+#define ASM_STORE32_REG_REG_REG(state, rd, rs1, rs2) asm_rv32_emit_store_reg_reg_reg(state, rd, rs1, rs2, 2)
+
+#endif
+
+#endif // MICROPY_INCLUDED_PY_ASMRV32_H

APP_Framework/Framework/knowing/micropython/py/asmthumb.c

@@ -0,0 +1,612 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2013, 2014 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <stdio.h>
+#include <assert.h>
+#include <string.h>
+
+#include "py/mpconfig.h"
+
+// wrapper around everything in this file
+#if MICROPY_EMIT_THUMB || MICROPY_EMIT_INLINE_THUMB
+
+#include "py/mpstate.h"
+#include "py/asmthumb.h"
+#include "py/misc.h"
+
+#define UNSIGNED_FIT7(x) ((uint32_t)(x) < 128)
+#define UNSIGNED_FIT8(x) (((x) & 0xffffff00) == 0)
+#define UNSIGNED_FIT16(x) (((x) & 0xffff0000) == 0)
+#define SIGNED_FIT8(x) (((x) & 0xffffff80) == 0) || (((x) & 0xffffff80) == 0xffffff80)
+#define SIGNED_FIT9(x) (((x) & 0xffffff00) == 0) || (((x) & 0xffffff00) == 0xffffff00)
+#define SIGNED_FIT12(x) (((x) & 0xfffff800) == 0) || (((x) & 0xfffff800) == 0xfffff800)
+#define SIGNED_FIT23(x) (((x) & 0xffc00000) == 0) || (((x) & 0xffc00000) == 0xffc00000)
+
+// Note: these actually take an imm12 but the high-bit is not encoded here
+#define OP_ADD_W_RRI_HI(reg_src) (0xf200 | (reg_src))
+#define OP_ADD_W_RRI_LO(reg_dest, imm11) ((imm11 << 4 & 0x7000) | reg_dest << 8 | (imm11 & 0xff))
+#define OP_SUB_W_RRI_HI(reg_src) (0xf2a0 | (reg_src))
+#define OP_SUB_W_RRI_LO(reg_dest, imm11) ((imm11 << 4 & 0x7000) | reg_dest << 8 | (imm11 & 0xff))
+
+static inline byte *asm_thumb_get_cur_to_write_bytes(asm_thumb_t *as, int n) {
+    return mp_asm_base_get_cur_to_write_bytes(&as->base, n);
+}
+
+/*
+static void asm_thumb_write_byte_1(asm_thumb_t *as, byte b1) {
+    byte *c = asm_thumb_get_cur_to_write_bytes(as, 1);
+    c[0] = b1;
+}
+*/
+
+/*
+#define IMM32_L0(x) ((x) & 0xff)
+#define IMM32_L1(x) (((x) >> 8) & 0xff)
+#define IMM32_L2(x) (((x) >> 16) & 0xff)
+#define IMM32_L3(x) (((x) >> 24) & 0xff)
+
+static void asm_thumb_write_word32(asm_thumb_t *as, int w32) {
+    byte *c = asm_thumb_get_cur_to_write_bytes(as, 4);
+    c[0] = IMM32_L0(w32);
+    c[1] = IMM32_L1(w32);
+    c[2] = IMM32_L2(w32);
+    c[3] = IMM32_L3(w32);
+}
+*/
+
+// rlolist is a bit map indicating desired lo-registers
+#define OP_PUSH_RLIST(rlolist)      (0xb400 | (rlolist))
+#define OP_PUSH_RLIST_LR(rlolist)   (0xb400 | 0x0100 | (rlolist))
+#define OP_POP_RLIST(rlolist)       (0xbc00 | (rlolist))
+#define OP_POP_RLIST_PC(rlolist)    (0xbc00 | 0x0100 | (rlolist))
+
+// The number of words must fit in 7 unsigned bits
+#define OP_ADD_SP(num_words) (0xb000 | (num_words))
+#define OP_SUB_SP(num_words) (0xb080 | (num_words))
+
+// locals:
+//  - stored on the stack in ascending order
+//  - numbered 0 through num_locals-1
+//  - SP points to first local
+//
+//  | SP
+//  v
+//  l0  l1  l2  ...  l(n-1)
+//  ^                ^
+//  | low address    | high address in RAM
+
+void asm_thumb_entry(asm_thumb_t *as, int num_locals) {
+    assert(num_locals >= 0);
+
+    // If this Thumb machine code is run from ARM state then add a prelude
+    // to switch to Thumb state for the duration of the function.
+    #if MICROPY_DYNAMIC_COMPILER || MICROPY_EMIT_ARM || (defined(__arm__) && !defined(__thumb2__) && !defined(__thumb__))
+    #if MICROPY_DYNAMIC_COMPILER
+    if (mp_dynamic_compiler.native_arch == MP_NATIVE_ARCH_ARMV6)
+    #endif
+    {
+        asm_thumb_op32(as, 0x4010, 0xe92d); // push {r4, lr}
+        asm_thumb_op32(as, 0xe009, 0xe28f); // add lr, pc, 8 + 1
+        asm_thumb_op32(as, 0xff3e, 0xe12f); // blx lr
+        asm_thumb_op32(as, 0x4010, 0xe8bd); // pop {r4, lr}
+        asm_thumb_op32(as, 0xff1e, 0xe12f); // bx lr
+    }
+    #endif
+
+    // work out what to push and how many extra spaces to reserve on stack
+    // so that we have enough for all locals and it's aligned an 8-byte boundary
+    // we push extra regs (r1, r2, r3) to help do the stack adjustment
+    // we probably should just always subtract from sp, since this would be more efficient
+    // for push rlist, lowest numbered register at the lowest address
+    uint reglist;
+    uint stack_adjust;
+    // don't pop r0 because it's used for return value
+    switch (num_locals) {
+        case 0:
+            reglist = 0xf2;
+            stack_adjust = 0;
+            break;
+
+        case 1:
+            reglist = 0xf2;
+            stack_adjust = 0;
+            break;
+
+        case 2:
+            reglist = 0xfe;
+            stack_adjust = 0;
+            break;
+
+        case 3:
+            reglist = 0xfe;
+            stack_adjust = 0;
+            break;
+
+        default:
+            reglist = 0xfe;
+            stack_adjust = ((num_locals - 3) + 1) & (~1);
+            break;
+    }
+    asm_thumb_op16(as, OP_PUSH_RLIST_LR(reglist));
+    if (stack_adjust > 0) {
+        if (asm_thumb_allow_armv7m(as)) {
+            if (UNSIGNED_FIT7(stack_adjust)) {
+                asm_thumb_op16(as, OP_SUB_SP(stack_adjust));
+            } else {
+                asm_thumb_op32(as, OP_SUB_W_RRI_HI(ASM_THUMB_REG_SP), OP_SUB_W_RRI_LO(ASM_THUMB_REG_SP, stack_adjust * 4));
+            }
+        } else {
+            int adj = stack_adjust;
+            // we don't expect the stack_adjust to be massive
+            while (!UNSIGNED_FIT7(adj)) {
+                asm_thumb_op16(as, OP_SUB_SP(127));
+                adj -= 127;
+            }
+            asm_thumb_op16(as, OP_SUB_SP(adj));
+        }
+    }
+    as->push_reglist = reglist;
+    as->stack_adjust = stack_adjust;
+}
+
+void asm_thumb_exit(asm_thumb_t *as) {
+    if (as->stack_adjust > 0) {
+        if (asm_thumb_allow_armv7m(as)) {
+            if (UNSIGNED_FIT7(as->stack_adjust)) {
+                asm_thumb_op16(as, OP_ADD_SP(as->stack_adjust));
+            } else {
+                asm_thumb_op32(as, OP_ADD_W_RRI_HI(ASM_THUMB_REG_SP), OP_ADD_W_RRI_LO(ASM_THUMB_REG_SP, as->stack_adjust * 4));
+            }
+        } else {
+            int adj = as->stack_adjust;
+            // we don't expect the stack_adjust to be massive
+            while (!UNSIGNED_FIT7(adj)) {
+                asm_thumb_op16(as, OP_ADD_SP(127));
+                adj -= 127;
+            }
+            asm_thumb_op16(as, OP_ADD_SP(adj));
+        }
+    }
+    asm_thumb_op16(as, OP_POP_RLIST_PC(as->push_reglist));
+}
+
+static mp_uint_t get_label_dest(asm_thumb_t *as, uint label) {
+    assert(label < as->base.max_num_labels);
+    return as->base.label_offsets[label];
+}
+
+void asm_thumb_op16(asm_thumb_t *as, uint op) {
+    byte *c = asm_thumb_get_cur_to_write_bytes(as, 2);
+    if (c != NULL) {
+        // little endian
+        c[0] = op;
+        c[1] = op >> 8;
+    }
+}
+
+void asm_thumb_op32(asm_thumb_t *as, uint op1, uint op2) {
+    byte *c = asm_thumb_get_cur_to_write_bytes(as, 4);
+    if (c != NULL) {
+        // little endian, op1 then op2
+        c[0] = op1;
+        c[1] = op1 >> 8;
+        c[2] = op2;
+        c[3] = op2 >> 8;
+    }
+}
+
+#define OP_FORMAT_4(op, rlo_dest, rlo_src) ((op) | ((rlo_src) << 3) | (rlo_dest))
+
+void asm_thumb_format_4(asm_thumb_t *as, uint op, uint rlo_dest, uint rlo_src) {
+    assert(rlo_dest < ASM_THUMB_REG_R8);
+    assert(rlo_src < ASM_THUMB_REG_R8);
+    asm_thumb_op16(as, OP_FORMAT_4(op, rlo_dest, rlo_src));
+}
+
+void asm_thumb_mov_reg_reg(asm_thumb_t *as, uint reg_dest, uint reg_src) {
+    uint op_lo;
+    if (reg_src < 8) {
+        op_lo = reg_src << 3;
+    } else {
+        op_lo = 0x40 | ((reg_src - 8) << 3);
+    }
+    if (reg_dest < 8) {
+        op_lo |= reg_dest;
+    } else {
+        op_lo |= 0x80 | (reg_dest - 8);
+    }
+    // mov reg_dest, reg_src
+    asm_thumb_op16(as, 0x4600 | op_lo);
+}
+
+// if loading lo half with movw, the i16 value will be zero extended into the r32 register!
+void asm_thumb_mov_reg_i16(asm_thumb_t *as, uint mov_op, uint reg_dest, int i16_src) {
+    assert(reg_dest < ASM_THUMB_REG_R15);
+    // mov[wt] reg_dest, #i16_src
+    asm_thumb_op32(as, mov_op | ((i16_src >> 1) & 0x0400) | ((i16_src >> 12) & 0xf), ((i16_src << 4) & 0x7000) | (reg_dest << 8) | (i16_src & 0xff));
+}
+
+static void asm_thumb_mov_rlo_i16(asm_thumb_t *as, uint rlo_dest, int i16_src) {
+    asm_thumb_mov_rlo_i8(as, rlo_dest, (i16_src >> 8) & 0xff);
+    asm_thumb_lsl_rlo_rlo_i5(as, rlo_dest, rlo_dest, 8);
+    asm_thumb_add_rlo_i8(as, rlo_dest, i16_src & 0xff);
+}
+
+#define OP_B_N(byte_offset) (0xe000 | (((byte_offset) >> 1) & 0x07ff))
+
+bool asm_thumb_b_n_label(asm_thumb_t *as, uint label) {
+    mp_uint_t dest = get_label_dest(as, label);
+    mp_int_t rel = dest - as->base.code_offset;
+    rel -= 4; // account for instruction prefetch, PC is 4 bytes ahead of this instruction
+    asm_thumb_op16(as, OP_B_N(rel));
+    return as->base.pass != MP_ASM_PASS_EMIT || SIGNED_FIT12(rel);
+}
+
+#define OP_BCC_N(cond, byte_offset) (0xd000 | ((cond) << 8) | (((byte_offset) >> 1) & 0x00ff))
+
+#define OP_BCC_W_HI(cond, byte_offset) (0xf000 | ((cond) << 6) | (((byte_offset) >> 10) & 0x0400) | (((byte_offset) >> 12) & 0x003f))
+#define OP_BCC_W_LO(byte_offset) (0x8000 | (((byte_offset) >> 5) & 0x2000) | (((byte_offset) >> 8) & 0x0800) | (((byte_offset) >> 1) & 0x07ff))
+
+bool asm_thumb_bcc_nw_label(asm_thumb_t *as, int cond, uint label, bool wide) {
+    mp_uint_t dest = get_label_dest(as, label);
+    mp_int_t rel = dest - as->base.code_offset;
+    rel -= 4; // account for instruction prefetch, PC is 4 bytes ahead of this instruction
+    if (!wide) {
+        asm_thumb_op16(as, OP_BCC_N(cond, rel));
+        return as->base.pass != MP_ASM_PASS_EMIT || SIGNED_FIT9(rel);
+    } else if (asm_thumb_allow_armv7m(as)) {
+        asm_thumb_op32(as, OP_BCC_W_HI(cond, rel), OP_BCC_W_LO(rel));
+        return true;
+    } else {
+        // this method should not be called for ARMV6M
+        return false;
+    }
+}
+
+#define OP_BL_HI(byte_offset) (0xf000 | (((byte_offset) >> 12) & 0x07ff))
+#define OP_BL_LO(byte_offset) (0xf800 | (((byte_offset) >> 1) & 0x07ff))
+
+bool asm_thumb_bl_label(asm_thumb_t *as, uint label) {
+    mp_uint_t dest = get_label_dest(as, label);
+    mp_int_t rel = dest - as->base.code_offset;
+    rel -= 4; // account for instruction prefetch, PC is 4 bytes ahead of this instruction
+    asm_thumb_op32(as, OP_BL_HI(rel), OP_BL_LO(rel));
+    return as->base.pass != MP_ASM_PASS_EMIT || SIGNED_FIT23(rel);
+}
+
+size_t asm_thumb_mov_reg_i32(asm_thumb_t *as, uint reg_dest, mp_uint_t i32) {
+    // movw, movt does it in 8 bytes
+    // ldr [pc, #], dw does it in 6 bytes, but we might not reach to end of code for dw
+
+    size_t loc = mp_asm_base_get_code_pos(&as->base);
+
+    if (asm_thumb_allow_armv7m(as)) {
+        asm_thumb_mov_reg_i16(as, ASM_THUMB_OP_MOVW, reg_dest, i32);
+        asm_thumb_mov_reg_i16(as, ASM_THUMB_OP_MOVT, reg_dest, i32 >> 16);
+    } else {
+        // should only be called with lo reg for ARMV6M
+        assert(reg_dest < ASM_THUMB_REG_R8);
+
+        // sanity check that generated code is aligned
+        assert(!as->base.code_base || !(3u & (uintptr_t)as->base.code_base));
+
+        // basically:
+        //        (nop)
+        //        ldr reg_dest, _data
+        //        b 1f
+        // _data: .word i32
+        //  1:
+        if (as->base.code_offset & 2u) {
+            asm_thumb_op16(as, ASM_THUMB_OP_NOP);
+        }
+        asm_thumb_ldr_rlo_pcrel_i8(as, reg_dest, 0);
+        asm_thumb_op16(as, OP_B_N(2));
+        asm_thumb_op16(as, i32 & 0xffff);
+        asm_thumb_op16(as, i32 >> 16);
+    }
+
+    return loc;
+}
+
+void asm_thumb_mov_reg_i32_optimised(asm_thumb_t *as, uint reg_dest, int i32) {
+    if (reg_dest < 8 && UNSIGNED_FIT8(i32)) {
+        asm_thumb_mov_rlo_i8(as, reg_dest, i32);
+    } else if (asm_thumb_allow_armv7m(as)) {
+        if (UNSIGNED_FIT16(i32)) {
+            asm_thumb_mov_reg_i16(as, ASM_THUMB_OP_MOVW, reg_dest, i32);
+        } else {
+            asm_thumb_mov_reg_i32(as, reg_dest, i32);
+        }
+    } else {
+        uint rlo_dest = reg_dest;
+        assert(rlo_dest < ASM_THUMB_REG_R8); // should never be called for ARMV6M
+
+        bool negate = i32 < 0 && ((i32 + i32) & 0xffffffffu); // don't negate 0x80000000
+        if (negate) {
+            i32 = -i32;
+        }
+
+        uint clz = mp_clz(i32);
+        uint ctz = i32 ? mp_ctz(i32) : 0;
+        assert(clz + ctz <= 32);
+        if (clz + ctz >= 24) {
+            asm_thumb_mov_rlo_i8(as, rlo_dest, (i32 >> ctz) & 0xff);
+            asm_thumb_lsl_rlo_rlo_i5(as, rlo_dest, rlo_dest, ctz);
+        } else if (UNSIGNED_FIT16(i32)) {
+            asm_thumb_mov_rlo_i16(as, rlo_dest, i32);
+        } else {
+            if (negate) {
+                // no point in negating if we're storing in 32 bit anyway
+                negate = false;
+                i32 = -i32;
+            }
+            asm_thumb_mov_reg_i32(as, rlo_dest, i32);
+        }
+        if (negate) {
+            asm_thumb_neg_rlo_rlo(as, rlo_dest, rlo_dest);
+        }
+    }
+}
+
+#define OP_STR_TO_SP_OFFSET(rlo_dest, word_offset) (0x9000 | ((rlo_dest) << 8) | ((word_offset) & 0x00ff))
+#define OP_LDR_FROM_SP_OFFSET(rlo_dest, word_offset) (0x9800 | ((rlo_dest) << 8) | ((word_offset) & 0x00ff))
+
+static void asm_thumb_mov_local_check(asm_thumb_t *as, int word_offset) {
+    if (as->base.pass >= MP_ASM_PASS_EMIT) {
+        assert(word_offset >= 0);
+        if (!UNSIGNED_FIT8(word_offset)) {
+            mp_raise_NotImplementedError(MP_ERROR_TEXT("too many locals for native method"));
+        }
+    }
+}
+
+void asm_thumb_mov_local_reg(asm_thumb_t *as, int local_num, uint rlo_src) {
+    assert(rlo_src < ASM_THUMB_REG_R8);
+    int word_offset = local_num;
+    asm_thumb_mov_local_check(as, word_offset);
+    asm_thumb_op16(as, OP_STR_TO_SP_OFFSET(rlo_src, word_offset));
+}
+
+void asm_thumb_mov_reg_local(asm_thumb_t *as, uint rlo_dest, int local_num) {
+    assert(rlo_dest < ASM_THUMB_REG_R8);
+    int word_offset = local_num;
+    asm_thumb_mov_local_check(as, word_offset);
+    asm_thumb_op16(as, OP_LDR_FROM_SP_OFFSET(rlo_dest, word_offset));
+}
+
+#define OP_ADD_REG_SP_OFFSET(rlo_dest, word_offset) (0xa800 | ((rlo_dest) << 8) | ((word_offset) & 0x00ff))
+
+void asm_thumb_mov_reg_local_addr(asm_thumb_t *as, uint rlo_dest, int local_num) {
+    assert(rlo_dest < ASM_THUMB_REG_R8);
+    int word_offset = local_num;
+    assert(as->base.pass < MP_ASM_PASS_EMIT || word_offset >= 0);
+    asm_thumb_op16(as, OP_ADD_REG_SP_OFFSET(rlo_dest, word_offset));
+}
+
+void asm_thumb_mov_reg_pcrel(asm_thumb_t *as, uint rlo_dest, uint label) {
+    mp_uint_t dest = get_label_dest(as, label);
+    mp_int_t rel = dest - as->base.code_offset;
+    rel |= 1; // to stay in Thumb state when jumping to this address
+    if (asm_thumb_allow_armv7m(as)) {
+        rel -= 6 + 4; // adjust for mov_reg_i16, sxth_rlo_rlo and then PC+4 prefetch of add_reg_reg
+        asm_thumb_mov_reg_i16(as, ASM_THUMB_OP_MOVW, rlo_dest, rel); // 4 bytes
+        asm_thumb_sxth_rlo_rlo(as, rlo_dest, rlo_dest); // 2 bytes
+    } else {
+        rel -= 8 + 4; // adjust for four instructions and then PC+4 prefetch of add_reg_reg
+        // 6 bytes
+        asm_thumb_mov_rlo_i16(as, rlo_dest, rel);
+        // 2 bytes - not always needed, but we want to keep the size the same
+        asm_thumb_sxth_rlo_rlo(as, rlo_dest, rlo_dest);
+    }
+    asm_thumb_add_reg_reg(as, rlo_dest, ASM_THUMB_REG_R15); // 2 bytes
+}
+
+// emits code for: reg_dest = reg_base + offset << offset_shift
+static void asm_thumb_add_reg_reg_offset(asm_thumb_t *as, uint reg_dest, uint reg_base, uint offset, uint offset_shift) {
+    if (reg_dest < ASM_THUMB_REG_R8 && reg_base < ASM_THUMB_REG_R8) {
+        if (offset << offset_shift < 256) {
+            if (reg_dest != reg_base) {
+                asm_thumb_mov_reg_reg(as, reg_dest, reg_base);
+            }
+            asm_thumb_add_rlo_i8(as, reg_dest, offset << offset_shift);
+        } else if (UNSIGNED_FIT8(offset) && reg_dest != reg_base) {
+            asm_thumb_mov_rlo_i8(as, reg_dest, offset);
+            asm_thumb_lsl_rlo_rlo_i5(as, reg_dest, reg_dest, offset_shift);
+            asm_thumb_add_rlo_rlo_rlo(as, reg_dest, reg_dest, reg_base);
+        } else if (reg_dest != reg_base) {
+            asm_thumb_mov_reg_i32_optimised(as, reg_dest, offset << offset_shift);
+            asm_thumb_add_rlo_rlo_rlo(as, reg_dest, reg_dest, reg_base);
+        } else {
+            uint reg_other = reg_dest ^ 7;
+            asm_thumb_op16(as, OP_PUSH_RLIST((1 << reg_other)));
+            asm_thumb_mov_reg_i32_optimised(as, reg_other, offset << offset_shift);
+            asm_thumb_add_rlo_rlo_rlo(as, reg_dest, reg_dest, reg_other);
+            asm_thumb_op16(as, OP_POP_RLIST((1 << reg_other)));
+        }
+    } else {
+        assert(0); // should never be called for ARMV6M
+    }
+}
+
+#define OP_LDR_STR_W_HI(operation_size, reg) ((0xf880 | (operation_size) << 5) | (reg))
+#define OP_LDR_STR_W_LO(reg, imm12) (((reg) << 12) | (imm12))
+
+#define OP_LDR 0x01
+#define OP_STR 0x00
+
+#define OP_LDR_W 0x10
+#define OP_STR_W 0x00
+
+static const uint8_t OP_LDR_STR_TABLE[3] = {
+    0x0E, 0x10, 0x0C
+};
+
+void asm_thumb_load_reg_reg_offset(asm_thumb_t *as, uint reg_dest, uint reg_base, uint offset, uint operation_size) {
+    assert(operation_size <= 2 && "Operation size out of range.");
+
+    if (MP_FIT_UNSIGNED(5, offset) && (reg_dest < ASM_THUMB_REG_R8) && (reg_base < ASM_THUMB_REG_R8)) {
+        // Can use T1 encoding
+        asm_thumb_op16(as, ((OP_LDR_STR_TABLE[operation_size] | OP_LDR) << 11) | (offset << 6) | (reg_base << 3) | reg_dest);
+    } else if (asm_thumb_allow_armv7m(as) && MP_FIT_UNSIGNED(12, offset << operation_size)) {
+        // Can use T3 encoding
+        asm_thumb_op32(as, (OP_LDR_STR_W_HI(operation_size, reg_base) | OP_LDR_W), OP_LDR_STR_W_LO(reg_dest, (offset << operation_size)));
+    } else {
+        // Must use the generic sequence
+        asm_thumb_add_reg_reg_offset(as, reg_dest, reg_base, offset - 31, operation_size);
+        asm_thumb_op16(as, ((OP_LDR_STR_TABLE[operation_size] | OP_LDR) << 11) | (31 << 6) | (reg_dest << 3) | (reg_dest));
+    }
+}
+
+void asm_thumb_store_reg_reg_offset(asm_thumb_t *as, uint reg_src, uint reg_base, uint offset, uint operation_size) {
+    assert(operation_size <= 2 && "Operation size out of range.");
+
+    if (MP_FIT_UNSIGNED(5, offset) && (reg_src < ASM_THUMB_REG_R8) && (reg_base < ASM_THUMB_REG_R8)) {
+        // Can use T1 encoding
+        asm_thumb_op16(as, ((OP_LDR_STR_TABLE[operation_size] | OP_STR) << 11) | (offset << 6) | (reg_base << 3) | reg_src);
+    } else if (asm_thumb_allow_armv7m(as) && MP_FIT_UNSIGNED(12, offset << operation_size)) {
+        // Can use T3 encoding
+        asm_thumb_op32(as, (OP_LDR_STR_W_HI(operation_size, reg_base) | OP_STR_W), OP_LDR_STR_W_LO(reg_src, (offset << operation_size)));
+    } else {
+        // Must use the generic sequence
+        asm_thumb_op16(as, OP_PUSH_RLIST(1 << reg_base));
+        asm_thumb_add_reg_reg_offset(as, reg_base, reg_base, offset - 31, operation_size);
+        asm_thumb_op16(as, ((OP_LDR_STR_TABLE[operation_size] | OP_STR) << 11) | (31 << 6) | (reg_base << 3) | reg_src);
+        asm_thumb_op16(as, OP_POP_RLIST(1 << reg_base));
+    }
+}
+
+// this could be wrong, because it should have a range of +/- 16MiB...
+#define OP_BW_HI(byte_offset) (0xf000 | (((byte_offset) >> 12) & 0x07ff))
+#define OP_BW_LO(byte_offset) (0xb800 | (((byte_offset) >> 1) & 0x07ff))
+
+// In Thumb1 mode, this may clobber r1.
+void asm_thumb_b_label(asm_thumb_t *as, uint label) {
+    mp_uint_t dest = get_label_dest(as, label);
+    mp_int_t rel = dest - as->base.code_offset;
+    rel -= 4; // account for instruction prefetch, PC is 4 bytes ahead of this instruction
+
+    if (dest != (mp_uint_t)-1 && rel <= -4) {
+        // is a backwards jump, so we know the size of the jump on the first pass
+        // calculate rel assuming 12 bit relative jump
+        if (SIGNED_FIT12(rel)) {
+            asm_thumb_op16(as, OP_B_N(rel));
+            return;
+        }
+    }
+
+    // is a large backwards jump, or a forwards jump (that must be assumed large)
+
+    if (asm_thumb_allow_armv7m(as)) {
+        asm_thumb_op32(as, OP_BW_HI(rel), OP_BW_LO(rel));
+    } else {
+        // this code path has to be the same instruction size irrespective of the value of rel
+        bool need_align = as->base.code_offset & 2u;
+        if (SIGNED_FIT12(rel)) {
+            asm_thumb_op16(as, OP_B_N(rel));
+            asm_thumb_op16(as, ASM_THUMB_OP_NOP);
+            asm_thumb_op16(as, ASM_THUMB_OP_NOP);
+            asm_thumb_op16(as, ASM_THUMB_OP_NOP);
+            if (need_align) {
+                asm_thumb_op16(as, ASM_THUMB_OP_NOP);
+            }
+        } else {
+            // do a large jump using:
+            //     (nop)
+            //     ldr r1, [pc, _data]
+            //     add pc, r1
+            // _data: .word rel
+            //
+            // note: can't use r0 as a temporary because native code can have the return value
+            // in that register and use a large jump to get to the exit point of the function
+
+            rel -= 2; // account for the "ldr r1, [pc, _data]"
+            if (need_align) {
+                asm_thumb_op16(as, ASM_THUMB_OP_NOP);
+                rel -= 2; // account for this nop
+            }
+            asm_thumb_ldr_rlo_pcrel_i8(as, ASM_THUMB_REG_R1, 0);
+            asm_thumb_add_reg_reg(as, ASM_THUMB_REG_R15, ASM_THUMB_REG_R1);
+            asm_thumb_op16(as, rel & 0xffff);
+            asm_thumb_op16(as, rel >> 16);
+        }
+    }
+}
+
+// In Thumb1 mode, this may clobber r1.
+void asm_thumb_bcc_label(asm_thumb_t *as, int cond, uint label) {
+    mp_uint_t dest = get_label_dest(as, label);
+    mp_int_t rel = dest - as->base.code_offset;
+    rel -= 4; // account for instruction prefetch, PC is 4 bytes ahead of this instruction
+
+    if (dest != (mp_uint_t)-1 && rel <= -4) {
+        // is a backwards jump, so we know the size of the jump on the first pass
+        // calculate rel assuming 9 bit relative jump
+        if (SIGNED_FIT9(rel)) {
+            asm_thumb_op16(as, OP_BCC_N(cond, rel));
+            return;
+        }
+    }
+
+    // is a large backwards jump, or a forwards jump (that must be assumed large)
+
+    if (asm_thumb_allow_armv7m(as)) {
+        asm_thumb_op32(as, OP_BCC_W_HI(cond, rel), OP_BCC_W_LO(rel));
+    } else {
+        // reverse the sense of the branch to jump over a longer branch
+        size_t code_offset_start = as->base.code_offset;
+        byte *c = asm_thumb_get_cur_to_write_bytes(as, 2);
+        asm_thumb_b_label(as, label);
+        size_t bytes_to_skip = as->base.code_offset - code_offset_start;
+        uint16_t op = OP_BCC_N(cond ^ 1, bytes_to_skip - 4);
+        if (c != NULL) {
+            c[0] = op;
+            c[1] = op >> 8;
+        }
+    }
+}
+
+void asm_thumb_bcc_rel9(asm_thumb_t *as, int cond, int rel) {
+    rel -= 4; // account for instruction prefetch, PC is 4 bytes ahead of this instruction
+    assert(SIGNED_FIT9(rel));
+    asm_thumb_op16(as, OP_BCC_N(cond, rel));
+}
+
+void asm_thumb_b_rel12(asm_thumb_t *as, int rel) {
+    rel -= 4; // account for instruction prefetch, PC is 4 bytes ahead of this instruction
+    assert(SIGNED_FIT12(rel));
+    asm_thumb_op16(as, OP_B_N(rel));
+}
+
+#define OP_BLX(reg) (0x4780 | ((reg) << 3))
+#define OP_SVC(arg) (0xdf00 | (arg))
+
+void asm_thumb_bl_ind(asm_thumb_t *as, uint fun_id, uint reg_temp) {
+    // Load ptr to function from table, indexed by fun_id, then call it
+    asm_thumb_load_reg_reg_offset(as, reg_temp, ASM_THUMB_REG_FUN_TABLE, fun_id, 2);
+    asm_thumb_op16(as, OP_BLX(reg_temp));
+}
+
+#endif // MICROPY_EMIT_THUMB || MICROPY_EMIT_INLINE_THUMB

APP_Framework/Framework/knowing/micropython/py/asmthumb.h

@@ -0,0 +1,490 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2013, 2014 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifndef MICROPY_INCLUDED_PY_ASMTHUMB_H
+#define MICROPY_INCLUDED_PY_ASMTHUMB_H
+
+#include <assert.h>
+#include "py/misc.h"
+#include "py/asmbase.h"
+#include "py/persistentcode.h"
+
+#define ASM_THUMB_REG_R0  (0)
+#define ASM_THUMB_REG_R1  (1)
+#define ASM_THUMB_REG_R2  (2)
+#define ASM_THUMB_REG_R3  (3)
+#define ASM_THUMB_REG_R4  (4)
+#define ASM_THUMB_REG_R5  (5)
+#define ASM_THUMB_REG_R6  (6)
+#define ASM_THUMB_REG_R7  (7)
+#define ASM_THUMB_REG_R8  (8)
+#define ASM_THUMB_REG_R9  (9)
+#define ASM_THUMB_REG_R10 (10)
+#define ASM_THUMB_REG_R11 (11)
+#define ASM_THUMB_REG_R12 (12)
+#define ASM_THUMB_REG_R13 (13)
+#define ASM_THUMB_REG_R14 (14)
+#define ASM_THUMB_REG_R15 (15)
+#define ASM_THUMB_REG_SP  (ASM_THUMB_REG_R13)
+#define ASM_THUMB_REG_LR  (REG_R14)
+
+#define ASM_THUMB_CC_EQ (0x0)
+#define ASM_THUMB_CC_NE (0x1)
+#define ASM_THUMB_CC_CS (0x2)
+#define ASM_THUMB_CC_CC (0x3)
+#define ASM_THUMB_CC_MI (0x4)
+#define ASM_THUMB_CC_PL (0x5)
+#define ASM_THUMB_CC_VS (0x6)
+#define ASM_THUMB_CC_VC (0x7)
+#define ASM_THUMB_CC_HI (0x8)
+#define ASM_THUMB_CC_LS (0x9)
+#define ASM_THUMB_CC_GE (0xa)
+#define ASM_THUMB_CC_LT (0xb)
+#define ASM_THUMB_CC_GT (0xc)
+#define ASM_THUMB_CC_LE (0xd)
+
+typedef struct _asm_thumb_t {
+    mp_asm_base_t base;
+    uint32_t push_reglist;
+    uint32_t stack_adjust;
+} asm_thumb_t;
+
+#if MICROPY_DYNAMIC_COMPILER
+
+static inline bool asm_thumb_allow_armv7m(asm_thumb_t *as) {
+    return MP_NATIVE_ARCH_ARMV7M <= mp_dynamic_compiler.native_arch
+           && mp_dynamic_compiler.native_arch <= MP_NATIVE_ARCH_ARMV7EMDP;
+}
+
+#else
+
+static inline bool asm_thumb_allow_armv7m(asm_thumb_t *as) {
+    return MICROPY_EMIT_THUMB_ARMV7M;
+}
+
+#endif
+
+static inline void asm_thumb_end_pass(asm_thumb_t *as) {
+    (void)as;
+}
+
+void asm_thumb_entry(asm_thumb_t *as, int num_locals);
+void asm_thumb_exit(asm_thumb_t *as);
+
+// argument order follows ARM, in general dest is first
+// note there is a difference between movw and mov.w, and many others!
+
+#define ASM_THUMB_OP_IT (0xbf00)
+#define ASM_THUMB_OP_ITE_EQ (0xbf0c)
+#define ASM_THUMB_OP_ITE_NE (0xbf14)
+#define ASM_THUMB_OP_ITE_CS (0xbf2c)
+#define ASM_THUMB_OP_ITE_CC (0xbf34)
+#define ASM_THUMB_OP_ITE_MI (0xbf4c)
+#define ASM_THUMB_OP_ITE_PL (0xbf54)
+#define ASM_THUMB_OP_ITE_VS (0xbf6c)
+#define ASM_THUMB_OP_ITE_VC (0xbf74)
+#define ASM_THUMB_OP_ITE_HI (0xbf8c)
+#define ASM_THUMB_OP_ITE_LS (0xbf94)
+#define ASM_THUMB_OP_ITE_GE (0xbfac)
+#define ASM_THUMB_OP_ITE_LT (0xbfb4)
+#define ASM_THUMB_OP_ITE_GT (0xbfcc)
+#define ASM_THUMB_OP_ITE_LE (0xbfd4)
+
+#define ASM_THUMB_OP_NOP        (0xbf00)
+#define ASM_THUMB_OP_WFI        (0xbf30)
+#define ASM_THUMB_OP_CPSID_I    (0xb672) // cpsid i, disable irq
+#define ASM_THUMB_OP_CPSIE_I    (0xb662) // cpsie i, enable irq
+
+void asm_thumb_op16(asm_thumb_t *as, uint op);
+void asm_thumb_op32(asm_thumb_t *as, uint op1, uint op2);
+
+static inline void asm_thumb_it_cc(asm_thumb_t *as, uint cc, uint mask) {
+    asm_thumb_op16(as, ASM_THUMB_OP_IT | (cc << 4) | mask);
+}
+
+// FORMAT 1: move shifted register
+
+#define ASM_THUMB_FORMAT_1_LSL (0x0000)
+#define ASM_THUMB_FORMAT_1_LSR (0x0800)
+#define ASM_THUMB_FORMAT_1_ASR (0x1000)
+
+#define ASM_THUMB_FORMAT_1_ENCODE(op, rlo_dest, rlo_src, offset) \
+    ((op) | ((offset) << 6) | ((rlo_src) << 3) | (rlo_dest))
+
+static inline void asm_thumb_format_1(asm_thumb_t *as, uint op, uint rlo_dest, uint rlo_src, uint offset) {
+    assert(rlo_dest < ASM_THUMB_REG_R8);
+    assert(rlo_src < ASM_THUMB_REG_R8);
+    asm_thumb_op16(as, ASM_THUMB_FORMAT_1_ENCODE(op, rlo_dest, rlo_src, offset));
+}
+
+// FORMAT 2: add/subtract
+
+#define ASM_THUMB_FORMAT_2_ADD (0x1800)
+#define ASM_THUMB_FORMAT_2_SUB (0x1a00)
+#define ASM_THUMB_FORMAT_2_REG_OPERAND (0x0000)
+#define ASM_THUMB_FORMAT_2_IMM_OPERAND (0x0400)
+
+#define ASM_THUMB_FORMAT_2_ENCODE(op, rlo_dest, rlo_src, src_b) \
+    ((op) | ((src_b) << 6) | ((rlo_src) << 3) | (rlo_dest))
+
+static inline void asm_thumb_format_2(asm_thumb_t *as, uint op, uint rlo_dest, uint rlo_src, int src_b) {
+    assert(rlo_dest < ASM_THUMB_REG_R8);
+    assert(rlo_src < ASM_THUMB_REG_R8);
+    asm_thumb_op16(as, ASM_THUMB_FORMAT_2_ENCODE(op, rlo_dest, rlo_src, src_b));
+}
+
+static inline void asm_thumb_add_rlo_rlo_rlo(asm_thumb_t *as, uint rlo_dest, uint rlo_src_a, uint rlo_src_b) {
+    asm_thumb_format_2(as, ASM_THUMB_FORMAT_2_ADD | ASM_THUMB_FORMAT_2_REG_OPERAND, rlo_dest, rlo_src_a, rlo_src_b);
+}
+static inline void asm_thumb_add_rlo_rlo_i3(asm_thumb_t *as, uint rlo_dest, uint rlo_src_a, int i3_src) {
+    asm_thumb_format_2(as, ASM_THUMB_FORMAT_2_ADD | ASM_THUMB_FORMAT_2_IMM_OPERAND, rlo_dest, rlo_src_a, i3_src);
+}
+static inline void asm_thumb_sub_rlo_rlo_rlo(asm_thumb_t *as, uint rlo_dest, uint rlo_src_a, uint rlo_src_b) {
+    asm_thumb_format_2(as, ASM_THUMB_FORMAT_2_SUB | ASM_THUMB_FORMAT_2_REG_OPERAND, rlo_dest, rlo_src_a, rlo_src_b);
+}
+static inline void asm_thumb_sub_rlo_rlo_i3(asm_thumb_t *as, uint rlo_dest, uint rlo_src_a, int i3_src) {
+    asm_thumb_format_2(as, ASM_THUMB_FORMAT_2_SUB | ASM_THUMB_FORMAT_2_IMM_OPERAND, rlo_dest, rlo_src_a, i3_src);
+}
+
+// FORMAT 3: move/compare/add/subtract immediate
+// These instructions all do zero extension of the i8 value
+
+#define ASM_THUMB_FORMAT_3_MOV (0x2000)
+#define ASM_THUMB_FORMAT_3_CMP (0x2800)
+#define ASM_THUMB_FORMAT_3_ADD (0x3000)
+#define ASM_THUMB_FORMAT_3_SUB (0x3800)
+#define ASM_THUMB_FORMAT_3_LDR (0x4800)
+
+#define ASM_THUMB_FORMAT_3_ENCODE(op, rlo, i8) ((op) | ((rlo) << 8) | (i8))
+
+static inline void asm_thumb_format_3(asm_thumb_t *as, uint op, uint rlo, int i8) {
+    assert(rlo < ASM_THUMB_REG_R8);
+    asm_thumb_op16(as, ASM_THUMB_FORMAT_3_ENCODE(op, rlo, i8));
+}
+
+static inline void asm_thumb_mov_rlo_i8(asm_thumb_t *as, uint rlo, int i8) {
+    asm_thumb_format_3(as, ASM_THUMB_FORMAT_3_MOV, rlo, i8);
+}
+static inline void asm_thumb_cmp_rlo_i8(asm_thumb_t *as, uint rlo, int i8) {
+    asm_thumb_format_3(as, ASM_THUMB_FORMAT_3_CMP, rlo, i8);
+}
+static inline void asm_thumb_add_rlo_i8(asm_thumb_t *as, uint rlo, int i8) {
+    asm_thumb_format_3(as, ASM_THUMB_FORMAT_3_ADD, rlo, i8);
+}
+static inline void asm_thumb_sub_rlo_i8(asm_thumb_t *as, uint rlo, int i8) {
+    asm_thumb_format_3(as, ASM_THUMB_FORMAT_3_SUB, rlo, i8);
+}
+static inline void asm_thumb_ldr_rlo_pcrel_i8(asm_thumb_t *as, uint rlo, uint i8) {
+    asm_thumb_format_3(as, ASM_THUMB_FORMAT_3_LDR, rlo, i8);
+}
+
+// FORMAT 4: ALU operations
+
+#define ASM_THUMB_FORMAT_4_AND (0x4000)
+#define ASM_THUMB_FORMAT_4_EOR (0x4040)
+#define ASM_THUMB_FORMAT_4_LSL (0x4080)
+#define ASM_THUMB_FORMAT_4_LSR (0x40c0)
+#define ASM_THUMB_FORMAT_4_ASR (0x4100)
+#define ASM_THUMB_FORMAT_4_ADC (0x4140)
+#define ASM_THUMB_FORMAT_4_SBC (0x4180)
+#define ASM_THUMB_FORMAT_4_ROR (0x41c0)
+#define ASM_THUMB_FORMAT_4_TST (0x4200)
+#define ASM_THUMB_FORMAT_4_NEG (0x4240)
+#define ASM_THUMB_FORMAT_4_CMP (0x4280)
+#define ASM_THUMB_FORMAT_4_CMN (0x42c0)
+#define ASM_THUMB_FORMAT_4_ORR (0x4300)
+#define ASM_THUMB_FORMAT_4_MUL (0x4340)
+#define ASM_THUMB_FORMAT_4_BIC (0x4380)
+#define ASM_THUMB_FORMAT_4_MVN (0x43c0)
+
+void asm_thumb_format_4(asm_thumb_t *as, uint op, uint rlo_dest, uint rlo_src);
+
+static inline void asm_thumb_cmp_rlo_rlo(asm_thumb_t *as, uint rlo_dest, uint rlo_src) {
+    asm_thumb_format_4(as, ASM_THUMB_FORMAT_4_CMP, rlo_dest, rlo_src);
+}
+static inline void asm_thumb_mvn_rlo_rlo(asm_thumb_t *as, uint rlo_dest, uint rlo_src) {
+    asm_thumb_format_4(as, ASM_THUMB_FORMAT_4_MVN, rlo_dest, rlo_src);
+}
+static inline void asm_thumb_neg_rlo_rlo(asm_thumb_t *as, uint rlo_dest, uint rlo_src) {
+    asm_thumb_format_4(as, ASM_THUMB_FORMAT_4_NEG, rlo_dest, rlo_src);
+}
+
+// FORMAT 5: hi register operations (add, cmp, mov, bx)
+// For add/cmp/mov, at least one of the args must be a high register
+
+#define ASM_THUMB_FORMAT_5_ADD (0x4400)
+#define ASM_THUMB_FORMAT_5_BX (0x4700)
+
+#define ASM_THUMB_FORMAT_5_ENCODE(op, r_dest, r_src) \
+    ((op) | ((r_dest) << 4 & 0x0080) | ((r_src) << 3) | ((r_dest) & 0x0007))
+
+static inline void asm_thumb_format_5(asm_thumb_t *as, uint op, uint r_dest, uint r_src) {
+    asm_thumb_op16(as, ASM_THUMB_FORMAT_5_ENCODE(op, r_dest, r_src));
+}
+
+static inline void asm_thumb_add_reg_reg(asm_thumb_t *as, uint r_dest, uint r_src) {
+    asm_thumb_format_5(as, ASM_THUMB_FORMAT_5_ADD, r_dest, r_src);
+}
+static inline void asm_thumb_bx_reg(asm_thumb_t *as, uint r_src) {
+    asm_thumb_format_5(as, ASM_THUMB_FORMAT_5_BX, 0, r_src);
+}
+
+// FORMAT 7: load/store with register offset
+// FORMAT 8: load/store sign-extended byte/halfword
+
+#define ASM_THUMB_FORMAT_7_LDR (0x5800)
+#define ASM_THUMB_FORMAT_7_STR (0x5000)
+#define ASM_THUMB_FORMAT_7_WORD_TRANSFER (0x0000)
+#define ASM_THUMB_FORMAT_7_BYTE_TRANSFER (0x0400)
+#define ASM_THUMB_FORMAT_8_LDRH (0x5A00)
+#define ASM_THUMB_FORMAT_8_STRH (0x5200)
+
+#define ASM_THUMB_FORMAT_7_8_ENCODE(op, rlo_dest, rlo_base, rlo_index) \
+    ((op) | ((rlo_index) << 6) | ((rlo_base) << 3) | ((rlo_dest)))
+
+static inline void asm_thumb_format_7_8(asm_thumb_t *as, uint op, uint rlo_dest, uint rlo_base, uint rlo_index) {
+    assert(rlo_dest < ASM_THUMB_REG_R8);
+    assert(rlo_base < ASM_THUMB_REG_R8);
+    assert(rlo_index < ASM_THUMB_REG_R8);
+    asm_thumb_op16(as, ASM_THUMB_FORMAT_7_8_ENCODE(op, rlo_dest, rlo_base, rlo_index));
+}
+
+static inline void asm_thumb_ldrb_rlo_rlo_rlo(asm_thumb_t *as, uint rlo_dest, uint rlo_base, uint rlo_index) {
+    asm_thumb_format_7_8(as, ASM_THUMB_FORMAT_7_LDR | ASM_THUMB_FORMAT_7_BYTE_TRANSFER, rlo_dest, rlo_base, rlo_index);
+}
+
+static inline void asm_thumb_ldrh_rlo_rlo_rlo(asm_thumb_t *as, uint rlo_dest, uint rlo_base, uint rlo_index) {
+    asm_thumb_format_7_8(as, ASM_THUMB_FORMAT_8_LDRH, rlo_dest, rlo_base, rlo_index);
+}
+
+static inline void asm_thumb_ldr_rlo_rlo_rlo(asm_thumb_t *as, uint rlo_dest, uint rlo_base, uint rlo_index) {
+    asm_thumb_format_7_8(as, ASM_THUMB_FORMAT_7_LDR | ASM_THUMB_FORMAT_7_WORD_TRANSFER, rlo_dest, rlo_base, rlo_index);
+}
+
+static inline void asm_thumb_strb_rlo_rlo_rlo(asm_thumb_t *as, uint rlo_src, uint rlo_base, uint rlo_index) {
+    asm_thumb_format_7_8(as, ASM_THUMB_FORMAT_7_STR | ASM_THUMB_FORMAT_7_BYTE_TRANSFER, rlo_src, rlo_base, rlo_index);
+}
+
+static inline void asm_thumb_strh_rlo_rlo_rlo(asm_thumb_t *as, uint rlo_dest, uint rlo_base, uint rlo_index) {
+    asm_thumb_format_7_8(as, ASM_THUMB_FORMAT_8_STRH, rlo_dest, rlo_base, rlo_index);
+}
+
+static inline void asm_thumb_str_rlo_rlo_rlo(asm_thumb_t *as, uint rlo_src, uint rlo_base, uint rlo_index) {
+    asm_thumb_format_7_8(as, ASM_THUMB_FORMAT_7_STR | ASM_THUMB_FORMAT_7_WORD_TRANSFER, rlo_src, rlo_base, rlo_index);
+}
+
+// FORMAT 9: load/store with immediate offset
+// For word transfers the offset must be aligned, and >>2
+
+// FORMAT 10: load/store halfword
+// The offset must be aligned, and >>1
+// The load is zero extended into the register
+
+#define ASM_THUMB_FORMAT_9_STR (0x6000)
+#define ASM_THUMB_FORMAT_9_LDR (0x6800)
+#define ASM_THUMB_FORMAT_9_WORD_TRANSFER (0x0000)
+#define ASM_THUMB_FORMAT_9_BYTE_TRANSFER (0x1000)
+
+#define ASM_THUMB_FORMAT_10_STRH (0x8000)
+#define ASM_THUMB_FORMAT_10_LDRH (0x8800)
+
+#define ASM_THUMB_FORMAT_9_10_ENCODE(op, rlo_dest, rlo_base, offset) \
+    ((op) | (((offset) << 6) & 0x07c0) | ((rlo_base) << 3) | (rlo_dest))
+
+static inline void asm_thumb_format_9_10(asm_thumb_t *as, uint op, uint rlo_dest, uint rlo_base, uint offset) {
+    asm_thumb_op16(as, ASM_THUMB_FORMAT_9_10_ENCODE(op, rlo_dest, rlo_base, offset));
+}
+
+static inline void asm_thumb_lsl_rlo_rlo_i5(asm_thumb_t *as, uint rlo_dest, uint rlo_src, uint shift) {
+    asm_thumb_format_1(as, ASM_THUMB_FORMAT_1_LSL, rlo_dest, rlo_src, shift);
+}
+static inline void asm_thumb_asr_rlo_rlo_i5(asm_thumb_t *as, uint rlo_dest, uint rlo_src, uint shift) {
+    asm_thumb_format_1(as, ASM_THUMB_FORMAT_1_ASR, rlo_dest, rlo_src, shift);
+}
+
+// FORMAT 11: sign/zero extend
+
+#define ASM_THUMB_FORMAT_11_ENCODE(op, rlo_dest, rlo_src) \
+    ((op) | ((rlo_src) << 3) | (rlo_dest))
+
+#define ASM_THUMB_FORMAT_11_SXTH (0xb200)
+#define ASM_THUMB_FORMAT_11_SXTB (0xb240)
+#define ASM_THUMB_FORMAT_11_UXTH (0xb280)
+#define ASM_THUMB_FORMAT_11_UXTB (0xb2c0)
+
+static inline void asm_thumb_format_11(asm_thumb_t *as, uint op, uint rlo_dest, uint rlo_src) {
+    assert(rlo_dest < ASM_THUMB_REG_R8);
+    assert(rlo_src < ASM_THUMB_REG_R8);
+    asm_thumb_op16(as, ASM_THUMB_FORMAT_11_ENCODE(op, rlo_dest, rlo_src));
+}
+
+static inline void asm_thumb_sxth_rlo_rlo(asm_thumb_t *as, uint rlo_dest, uint rlo_src) {
+    asm_thumb_format_11(as, ASM_THUMB_FORMAT_11_SXTH, rlo_dest, rlo_src);
+}
+
+// TODO convert these to above format style
+
+#define ASM_THUMB_OP_MOVW (0xf240)
+#define ASM_THUMB_OP_MOVT (0xf2c0)
+
+void asm_thumb_mov_reg_reg(asm_thumb_t *as, uint reg_dest, uint reg_src);
+void asm_thumb_mov_reg_i16(asm_thumb_t *as, uint mov_op, uint reg_dest, int i16_src);
+
+// these return true if the destination is in range, false otherwise
+bool asm_thumb_b_n_label(asm_thumb_t *as, uint label);
+bool asm_thumb_bcc_nw_label(asm_thumb_t *as, int cond, uint label, bool wide);
+bool asm_thumb_bl_label(asm_thumb_t *as, uint label);
+
+size_t asm_thumb_mov_reg_i32(asm_thumb_t *as, uint reg_dest, mp_uint_t i32_src); // convenience
+void asm_thumb_mov_reg_i32_optimised(asm_thumb_t *as, uint reg_dest, int i32_src); // convenience
+void asm_thumb_mov_local_reg(asm_thumb_t *as, int local_num_dest, uint rlo_src); // convenience
+void asm_thumb_mov_reg_local(asm_thumb_t *as, uint rlo_dest, int local_num); // convenience
+void asm_thumb_mov_reg_local_addr(asm_thumb_t *as, uint rlo_dest, int local_num); // convenience
+void asm_thumb_mov_reg_pcrel(asm_thumb_t *as, uint rlo_dest, uint label);
+
+// Generate optimised load dest, [src, #offset] sequence
+void asm_thumb_load_reg_reg_offset(asm_thumb_t *as, uint reg_dest, uint reg_base, uint offset, uint operation_size);
+// Generate optimised store src, [dest, #offset] sequence
+void asm_thumb_store_reg_reg_offset(asm_thumb_t *as, uint reg_src, uint reg_base, uint offset, uint operation_size);
+
+void asm_thumb_b_label(asm_thumb_t *as, uint label); // convenience: picks narrow or wide branch
+void asm_thumb_bcc_label(asm_thumb_t *as, int cc, uint label); // convenience: picks narrow or wide branch
+void asm_thumb_bl_ind(asm_thumb_t *as, uint fun_id, uint reg_temp); // convenience
+void asm_thumb_bcc_rel9(asm_thumb_t *as, int cc, int rel);
+void asm_thumb_b_rel12(asm_thumb_t *as, int rel);
+
+// Holds a pointer to mp_fun_table
+#define ASM_THUMB_REG_FUN_TABLE ASM_THUMB_REG_R7
+
+#if GENERIC_ASM_API
+
+// The following macros provide a (mostly) arch-independent API to
+// generate native code, and are used by the native emitter.
+
+#define ASM_WORD_SIZE (4)
+
+#define REG_RET ASM_THUMB_REG_R0
+#define REG_ARG_1 ASM_THUMB_REG_R0
+#define REG_ARG_2 ASM_THUMB_REG_R1
+#define REG_ARG_3 ASM_THUMB_REG_R2
+#define REG_ARG_4 ASM_THUMB_REG_R3
+// rest of args go on stack
+
+#define REG_TEMP0 ASM_THUMB_REG_R0
+#define REG_TEMP1 ASM_THUMB_REG_R1
+#define REG_TEMP2 ASM_THUMB_REG_R2
+
+#define REG_LOCAL_1 ASM_THUMB_REG_R4
+#define REG_LOCAL_2 ASM_THUMB_REG_R5
+#define REG_LOCAL_3 ASM_THUMB_REG_R6
+#define REG_LOCAL_NUM (3)
+
+#define REG_FUN_TABLE ASM_THUMB_REG_FUN_TABLE
+
+#define ASM_T                           asm_thumb_t
+#define ASM_END_PASS                    asm_thumb_end_pass
+#define ASM_ENTRY(as, num_locals, name) asm_thumb_entry((as), (num_locals))
+#define ASM_EXIT                        asm_thumb_exit
+
+#define ASM_JUMP                        asm_thumb_b_label
+#define ASM_JUMP_IF_REG_ZERO(as, reg, label, bool_test) \
+    do { \
+        asm_thumb_cmp_rlo_i8(as, reg, 0); \
+        asm_thumb_bcc_label(as, ASM_THUMB_CC_EQ, label); \
+    } while (0)
+#define ASM_JUMP_IF_REG_NONZERO(as, reg, label, bool_test) \
+    do { \
+        asm_thumb_cmp_rlo_i8(as, reg, 0); \
+        asm_thumb_bcc_label(as, ASM_THUMB_CC_NE, label); \
+    } while (0)
+#define ASM_JUMP_IF_REG_EQ(as, reg1, reg2, label) \
+    do { \
+        asm_thumb_cmp_rlo_rlo(as, reg1, reg2); \
+        asm_thumb_bcc_label(as, ASM_THUMB_CC_EQ, label); \
+    } while (0)
+#define ASM_JUMP_REG(as, reg) asm_thumb_bx_reg((as), (reg))
+#define ASM_CALL_IND(as, idx) asm_thumb_bl_ind(as, idx, ASM_THUMB_REG_R3)
+
+#define ASM_MOV_LOCAL_REG(as, local_num, reg) asm_thumb_mov_local_reg((as), (local_num), (reg))
+#define ASM_MOV_REG_IMM(as, reg_dest, imm) asm_thumb_mov_reg_i32_optimised((as), (reg_dest), (imm))
+#define ASM_MOV_REG_LOCAL(as, reg_dest, local_num) asm_thumb_mov_reg_local((as), (reg_dest), (local_num))
+#define ASM_MOV_REG_REG(as, reg_dest, reg_src) asm_thumb_mov_reg_reg((as), (reg_dest), (reg_src))
+#define ASM_MOV_REG_LOCAL_ADDR(as, reg_dest, local_num) asm_thumb_mov_reg_local_addr((as), (reg_dest), (local_num))
+#define ASM_MOV_REG_PCREL(as, rlo_dest, label) asm_thumb_mov_reg_pcrel((as), (rlo_dest), (label))
+
+#define ASM_NOT_REG(as, reg_dest) asm_thumb_mvn_rlo_rlo((as), (reg_dest), (reg_dest))
+#define ASM_NEG_REG(as, reg_dest) asm_thumb_neg_rlo_rlo((as), (reg_dest), (reg_dest))
+#define ASM_LSL_REG_REG(as, reg_dest, reg_shift) asm_thumb_format_4((as), ASM_THUMB_FORMAT_4_LSL, (reg_dest), (reg_shift))
+#define ASM_LSR_REG_REG(as, reg_dest, reg_shift) asm_thumb_format_4((as), ASM_THUMB_FORMAT_4_LSR, (reg_dest), (reg_shift))
+#define ASM_ASR_REG_REG(as, reg_dest, reg_shift) asm_thumb_format_4((as), ASM_THUMB_FORMAT_4_ASR, (reg_dest), (reg_shift))
+#define ASM_OR_REG_REG(as, reg_dest, reg_src) asm_thumb_format_4((as), ASM_THUMB_FORMAT_4_ORR, (reg_dest), (reg_src))
+#define ASM_XOR_REG_REG(as, reg_dest, reg_src) asm_thumb_format_4((as), ASM_THUMB_FORMAT_4_EOR, (reg_dest), (reg_src))
+#define ASM_AND_REG_REG(as, reg_dest, reg_src) asm_thumb_format_4((as), ASM_THUMB_FORMAT_4_AND, (reg_dest), (reg_src))
+#define ASM_ADD_REG_REG(as, reg_dest, reg_src) asm_thumb_add_rlo_rlo_rlo((as), (reg_dest), (reg_dest), (reg_src))
+#define ASM_SUB_REG_REG(as, reg_dest, reg_src) asm_thumb_sub_rlo_rlo_rlo((as), (reg_dest), (reg_dest), (reg_src))
+#define ASM_MUL_REG_REG(as, reg_dest, reg_src) asm_thumb_format_4((as), ASM_THUMB_FORMAT_4_MUL, (reg_dest), (reg_src))
+
+#define ASM_LOAD_REG_REG_OFFSET(as, reg_dest, reg_base, word_offset) ASM_LOAD32_REG_REG_OFFSET((as), (reg_dest), (reg_base), (word_offset))
+#define ASM_LOAD8_REG_REG(as, reg_dest, reg_base) ASM_LOAD8_REG_REG_OFFSET((as), (reg_dest), (reg_base), 0)
+#define ASM_LOAD8_REG_REG_OFFSET(as, reg_dest, reg_base, byte_offset) asm_thumb_load_reg_reg_offset((as), (reg_dest), (reg_base), (byte_offset), 0)
+#define ASM_LOAD16_REG_REG(as, reg_dest, reg_base) ASM_LOAD16_REG_REG_OFFSET((as), (reg_dest), (reg_base), 0)
+#define ASM_LOAD16_REG_REG_OFFSET(as, reg_dest, reg_base, halfword_offset) asm_thumb_load_reg_reg_offset((as), (reg_dest), (reg_base), (halfword_offset), 1)
+#define ASM_LOAD32_REG_REG(as, reg_dest, reg_base) ASM_LOAD32_REG_REG_OFFSET((as), (reg_dest), (reg_base), 0)
+#define ASM_LOAD32_REG_REG_OFFSET(as, reg_dest, reg_base, word_offset) asm_thumb_load_reg_reg_offset((as), (reg_dest), (reg_base), (word_offset), 2)
+
+#define ASM_STORE_REG_REG_OFFSET(as, reg_src, reg_base, word_offset) ASM_STORE32_REG_REG_OFFSET((as), (reg_src), (reg_base), (word_offset))
+#define ASM_STORE8_REG_REG(as, reg_src, reg_base) ASM_STORE8_REG_REG_OFFSET((as), (reg_src), (reg_base), 0)
+#define ASM_STORE8_REG_REG_OFFSET(as, reg_src, reg_base, byte_offset) asm_thumb_store_reg_reg_offset((as), (reg_src), (reg_base), (byte_offset), 0)
+#define ASM_STORE16_REG_REG(as, reg_src, reg_base) ASM_STORE16_REG_REG_OFFSET((as), (reg_src), (reg_base), 0)
+#define ASM_STORE16_REG_REG_OFFSET(as, reg_src, reg_base, halfword_offset) asm_thumb_store_reg_reg_offset((as), (reg_src), (reg_base), (halfword_offset), 1)
+#define ASM_STORE32_REG_REG(as, reg_src, reg_base) ASM_STORE32_REG_REG_OFFSET((as), (reg_src), (reg_base), 0)
+#define ASM_STORE32_REG_REG_OFFSET(as, reg_src, reg_base, word_offset) asm_thumb_store_reg_reg_offset((as), (reg_src), (reg_base), (word_offset), 2)
+
+#define ASM_LOAD8_REG_REG_REG(as, reg_dest, reg_base, reg_index) asm_thumb_ldrb_rlo_rlo_rlo((as), (reg_dest), (reg_base), (reg_index))
+#define ASM_LOAD16_REG_REG_REG(as, reg_dest, reg_base, reg_index) \
+    do { \
+        asm_thumb_lsl_rlo_rlo_i5((as), (reg_index), (reg_index), 1); \
+        asm_thumb_ldrh_rlo_rlo_rlo((as), (reg_dest), (reg_base), (reg_index)); \
+    } while (0)
+#define ASM_LOAD32_REG_REG_REG(as, reg_dest, reg_base, reg_index) \
+    do { \
+        asm_thumb_lsl_rlo_rlo_i5((as), (reg_index), (reg_index), 2); \
+        asm_thumb_ldr_rlo_rlo_rlo((as), (reg_dest), (reg_base), (reg_index)); \
+    } while (0)
+#define ASM_STORE8_REG_REG_REG(as, reg_val, reg_base, reg_index) asm_thumb_strb_rlo_rlo_rlo((as), (reg_val), (reg_base), (reg_index))
+#define ASM_STORE16_REG_REG_REG(as, reg_val, reg_base, reg_index) \
+    do { \
+        asm_thumb_lsl_rlo_rlo_i5((as), (reg_index), (reg_index), 1); \
+        asm_thumb_strh_rlo_rlo_rlo((as), (reg_val), (reg_base), (reg_index)); \
+    } while (0)
+#define ASM_STORE32_REG_REG_REG(as, reg_val, reg_base, reg_index) \
+    do { \
+        asm_thumb_lsl_rlo_rlo_i5((as), (reg_index), (reg_index), 2); \
+        asm_thumb_str_rlo_rlo_rlo((as), (reg_val), (reg_base), (reg_index)); \
+    } while (0)
+
+#endif // GENERIC_ASM_API
+
+#endif // MICROPY_INCLUDED_PY_ASMTHUMB_H

APP_Framework/Framework/knowing/micropython/py/asmx64.c

@@ -0,0 +1,642 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2013, 2014 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <stdint.h>
+#include <stdio.h>
+#include <assert.h>
+#include <string.h>
+
+#include "py/mpconfig.h"
+
+// wrapper around everything in this file
+#if MICROPY_EMIT_X64
+
+#include "py/asmx64.h"
+
+/* all offsets are measured in multiples of 8 bytes */
+#define WORD_SIZE                (8)
+
+#define OPCODE_NOP               (0x90)
+#define OPCODE_PUSH_R64          (0x50) /* +rq */
+#define OPCODE_PUSH_I64          (0x68)
+#define OPCODE_PUSH_M64          (0xff) /* /6 */
+#define OPCODE_POP_R64           (0x58) /* +rq */
+#define OPCODE_RET               (0xc3)
+#define OPCODE_MOV_I8_TO_R8      (0xb0) /* +rb */
+#define OPCODE_MOV_I64_TO_R64    (0xb8) /* +rq */
+#define OPCODE_MOV_I32_TO_RM32   (0xc7)
+#define OPCODE_MOV_R8_TO_RM8     (0x88) /* /r */
+#define OPCODE_MOV_R64_TO_RM64   (0x89) /* /r */
+#define OPCODE_MOV_RM64_TO_R64   (0x8b) /* /r */
+#define OPCODE_MOVZX_RM8_TO_R64  (0xb6) /* 0x0f 0xb6/r */
+#define OPCODE_MOVZX_RM16_TO_R64 (0xb7) /* 0x0f 0xb7/r */
+#define OPCODE_LEA_MEM_TO_R64    (0x8d) /* /r */
+#define OPCODE_NOT_RM64          (0xf7) /* /2 */
+#define OPCODE_NEG_RM64          (0xf7) /* /3 */
+#define OPCODE_AND_R64_TO_RM64   (0x21) /* /r */
+#define OPCODE_OR_R64_TO_RM64    (0x09) /* /r */
+#define OPCODE_XOR_R64_TO_RM64   (0x31) /* /r */
+#define OPCODE_ADD_R64_TO_RM64   (0x01) /* /r */
+#define OPCODE_ADD_I32_TO_RM32   (0x81) /* /0 */
+#define OPCODE_ADD_I8_TO_RM32    (0x83) /* /0 */
+#define OPCODE_SUB_R64_FROM_RM64 (0x29)
+#define OPCODE_SUB_I32_FROM_RM64 (0x81) /* /5 */
+#define OPCODE_SUB_I8_FROM_RM64  (0x83) /* /5 */
+// #define OPCODE_SHL_RM32_BY_I8    (0xc1) /* /4 */
+// #define OPCODE_SHR_RM32_BY_I8    (0xc1) /* /5 */
+// #define OPCODE_SAR_RM32_BY_I8    (0xc1) /* /7 */
+#define OPCODE_SHL_RM64_CL       (0xd3) /* /4 */
+#define OPCODE_SHR_RM64_CL       (0xd3) /* /5 */
+#define OPCODE_SAR_RM64_CL       (0xd3) /* /7 */
+// #define OPCODE_CMP_I32_WITH_RM32 (0x81) /* /7 */
+// #define OPCODE_CMP_I8_WITH_RM32  (0x83) /* /7 */
+#define OPCODE_CMP_R64_WITH_RM64 (0x39) /* /r */
+// #define OPCODE_CMP_RM32_WITH_R32 (0x3b)
+#define OPCODE_TEST_R8_WITH_RM8  (0x84) /* /r */
+#define OPCODE_TEST_R64_WITH_RM64 (0x85) /* /r */
+#define OPCODE_JMP_REL8          (0xeb)
+#define OPCODE_JMP_REL32         (0xe9)
+#define OPCODE_JMP_RM64          (0xff) /* /4 */
+#define OPCODE_JCC_REL8          (0x70) /* | jcc type */
+#define OPCODE_JCC_REL32_A       (0x0f)
+#define OPCODE_JCC_REL32_B       (0x80) /* | jcc type */
+#define OPCODE_SETCC_RM8_A       (0x0f)
+#define OPCODE_SETCC_RM8_B       (0x90) /* | jcc type, /0 */
+#define OPCODE_CALL_REL32        (0xe8)
+#define OPCODE_CALL_RM32         (0xff) /* /2 */
+#define OPCODE_LEAVE             (0xc9)
+
+#define MODRM_R64(x)    (((x) & 0x7) << 3)
+#define MODRM_RM_DISP0  (0x00)
+#define MODRM_RM_DISP8  (0x40)
+#define MODRM_RM_DISP32 (0x80)
+#define MODRM_RM_REG    (0xc0)
+#define MODRM_RM_R64(x) ((x) & 0x7)
+
+#define OP_SIZE_PREFIX (0x66)
+
+#define REX_PREFIX  (0x40)
+#define REX_W       (0x08)  // width
+#define REX_R       (0x04)  // register
+#define REX_X       (0x02)  // index
+#define REX_B       (0x01)  // base
+#define REX_W_FROM_R64(r64) ((r64) >> 0 & 0x08)
+#define REX_R_FROM_R64(r64) ((r64) >> 1 & 0x04)
+#define REX_X_FROM_R64(r64) ((r64) >> 2 & 0x02)
+#define REX_B_FROM_R64(r64) ((r64) >> 3 & 0x01)
+
+#define IMM32_L0(x) ((x) & 0xff)
+#define IMM32_L1(x) (((x) >> 8) & 0xff)
+#define IMM32_L2(x) (((x) >> 16) & 0xff)
+#define IMM32_L3(x) (((x) >> 24) & 0xff)
+#define IMM64_L4(x) (((x) >> 32) & 0xff)
+#define IMM64_L5(x) (((x) >> 40) & 0xff)
+#define IMM64_L6(x) (((x) >> 48) & 0xff)
+#define IMM64_L7(x) (((x) >> 56) & 0xff)
+
+#define UNSIGNED_FIT8(x) (((x) & 0xffffffffffffff00) == 0)
+#define UNSIGNED_FIT32(x) (((x) & 0xffffffff00000000) == 0)
+#define SIGNED_FIT8(x) (((x) & 0xffffff80) == 0) || (((x) & 0xffffff80) == 0xffffff80)
+
+static inline byte *asm_x64_get_cur_to_write_bytes(asm_x64_t *as, int n) {
+    return mp_asm_base_get_cur_to_write_bytes(&as->base, n);
+}
+
+static void asm_x64_write_byte_1(asm_x64_t *as, byte b1) {
+    byte *c = asm_x64_get_cur_to_write_bytes(as, 1);
+    if (c != NULL) {
+        c[0] = b1;
+    }
+}
+
+static void asm_x64_write_byte_2(asm_x64_t *as, byte b1, byte b2) {
+    byte *c = asm_x64_get_cur_to_write_bytes(as, 2);
+    if (c != NULL) {
+        c[0] = b1;
+        c[1] = b2;
+    }
+}
+
+static void asm_x64_write_byte_3(asm_x64_t *as, byte b1, byte b2, byte b3) {
+    byte *c = asm_x64_get_cur_to_write_bytes(as, 3);
+    if (c != NULL) {
+        c[0] = b1;
+        c[1] = b2;
+        c[2] = b3;
+    }
+}
+
+static void asm_x64_write_word32(asm_x64_t *as, int w32) {
+    byte *c = asm_x64_get_cur_to_write_bytes(as, 4);
+    if (c != NULL) {
+        c[0] = IMM32_L0(w32);
+        c[1] = IMM32_L1(w32);
+        c[2] = IMM32_L2(w32);
+        c[3] = IMM32_L3(w32);
+    }
+}
+
+static void asm_x64_write_word64(asm_x64_t *as, int64_t w64) {
+    byte *c = asm_x64_get_cur_to_write_bytes(as, 8);
+    if (c != NULL) {
+        c[0] = IMM32_L0(w64);
+        c[1] = IMM32_L1(w64);
+        c[2] = IMM32_L2(w64);
+        c[3] = IMM32_L3(w64);
+        c[4] = IMM64_L4(w64);
+        c[5] = IMM64_L5(w64);
+        c[6] = IMM64_L6(w64);
+        c[7] = IMM64_L7(w64);
+    }
+}
+
+/* unused
+static void asm_x64_write_word32_to(asm_x64_t *as, int offset, int w32) {
+    byte* c;
+    assert(offset + 4 <= as->code_size);
+    c = as->code_base + offset;
+    c[0] = IMM32_L0(w32);
+    c[1] = IMM32_L1(w32);
+    c[2] = IMM32_L2(w32);
+    c[3] = IMM32_L3(w32);
+}
+*/
+
+static void asm_x64_write_r64_disp(asm_x64_t *as, int r64, int disp_r64, int disp_offset) {
+    uint8_t rm_disp;
+    if (disp_offset == 0 && (disp_r64 & 7) != ASM_X64_REG_RBP) {
+        rm_disp = MODRM_RM_DISP0;
+    } else if (SIGNED_FIT8(disp_offset)) {
+        rm_disp = MODRM_RM_DISP8;
+    } else {
+        rm_disp = MODRM_RM_DISP32;
+    }
+    asm_x64_write_byte_1(as, MODRM_R64(r64) | rm_disp | MODRM_RM_R64(disp_r64));
+    if ((disp_r64 & 7) == ASM_X64_REG_RSP) {
+        // Special case for rsp and r12, they need a SIB byte
+        asm_x64_write_byte_1(as, 0x24);
+    }
+    if (rm_disp == MODRM_RM_DISP8) {
+        asm_x64_write_byte_1(as, IMM32_L0(disp_offset));
+    } else if (rm_disp == MODRM_RM_DISP32) {
+        asm_x64_write_word32(as, disp_offset);
+    }
+}
+
+static void asm_x64_generic_r64_r64(asm_x64_t *as, int dest_r64, int src_r64, int op) {
+    asm_x64_write_byte_3(as, REX_PREFIX | REX_W | REX_R_FROM_R64(src_r64) | REX_B_FROM_R64(dest_r64), op, MODRM_R64(src_r64) | MODRM_RM_REG | MODRM_RM_R64(dest_r64));
+}
+
+void asm_x64_nop(asm_x64_t *as) {
+    asm_x64_write_byte_1(as, OPCODE_NOP);
+}
+
+void asm_x64_push_r64(asm_x64_t *as, int src_r64) {
+    if (src_r64 < 8) {
+        asm_x64_write_byte_1(as, OPCODE_PUSH_R64 | src_r64);
+    } else {
+        asm_x64_write_byte_2(as, REX_PREFIX | REX_B, OPCODE_PUSH_R64 | (src_r64 & 7));
+    }
+}
+
+/*
+void asm_x64_push_i32(asm_x64_t *as, int src_i32) {
+    asm_x64_write_byte_1(as, OPCODE_PUSH_I64);
+    asm_x64_write_word32(as, src_i32); // will be sign extended to 64 bits
+}
+*/
+
+/*
+void asm_x64_push_disp(asm_x64_t *as, int src_r64, int src_offset) {
+    assert(src_r64 < 8);
+    asm_x64_write_byte_1(as, OPCODE_PUSH_M64);
+    asm_x64_write_r64_disp(as, 6, src_r64, src_offset);
+}
+*/
+
+void asm_x64_pop_r64(asm_x64_t *as, int dest_r64) {
+    if (dest_r64 < 8) {
+        asm_x64_write_byte_1(as, OPCODE_POP_R64 | dest_r64);
+    } else {
+        asm_x64_write_byte_2(as, REX_PREFIX | REX_B, OPCODE_POP_R64 | (dest_r64 & 7));
+    }
+}
+
+static void asm_x64_ret(asm_x64_t *as) {
+    asm_x64_write_byte_1(as, OPCODE_RET);
+}
+
+void asm_x64_mov_r64_r64(asm_x64_t *as, int dest_r64, int src_r64) {
+    asm_x64_generic_r64_r64(as, dest_r64, src_r64, OPCODE_MOV_R64_TO_RM64);
+}
+
+void asm_x64_mov_r8_to_mem8(asm_x64_t *as, int src_r64, int dest_r64, int dest_disp) {
+    if (src_r64 < 8 && dest_r64 < 8) {
+        asm_x64_write_byte_1(as, OPCODE_MOV_R8_TO_RM8);
+    } else {
+        asm_x64_write_byte_2(as, REX_PREFIX | REX_R_FROM_R64(src_r64) | REX_B_FROM_R64(dest_r64), OPCODE_MOV_R8_TO_RM8);
+    }
+    asm_x64_write_r64_disp(as, src_r64, dest_r64, dest_disp);
+}
+
+void asm_x64_mov_r16_to_mem16(asm_x64_t *as, int src_r64, int dest_r64, int dest_disp) {
+    if (src_r64 < 8 && dest_r64 < 8) {
+        asm_x64_write_byte_2(as, OP_SIZE_PREFIX, OPCODE_MOV_R64_TO_RM64);
+    } else {
+        asm_x64_write_byte_3(as, OP_SIZE_PREFIX, REX_PREFIX | REX_R_FROM_R64(src_r64) | REX_B_FROM_R64(dest_r64), OPCODE_MOV_R64_TO_RM64);
+    }
+    asm_x64_write_r64_disp(as, src_r64, dest_r64, dest_disp);
+}
+
+void asm_x64_mov_r32_to_mem32(asm_x64_t *as, int src_r64, int dest_r64, int dest_disp) {
+    if (src_r64 < 8 && dest_r64 < 8) {
+        asm_x64_write_byte_1(as, OPCODE_MOV_R64_TO_RM64);
+    } else {
+        asm_x64_write_byte_2(as, REX_PREFIX | REX_R_FROM_R64(src_r64) | REX_B_FROM_R64(dest_r64), OPCODE_MOV_R64_TO_RM64);
+    }
+    asm_x64_write_r64_disp(as, src_r64, dest_r64, dest_disp);
+}
+
+void asm_x64_mov_r64_to_mem64(asm_x64_t *as, int src_r64, int dest_r64, int dest_disp) {
+    // use REX prefix for 64 bit operation
+    asm_x64_write_byte_2(as, REX_PREFIX | REX_W | REX_R_FROM_R64(src_r64) | REX_B_FROM_R64(dest_r64), OPCODE_MOV_R64_TO_RM64);
+    asm_x64_write_r64_disp(as, src_r64, dest_r64, dest_disp);
+}
+
+void asm_x64_mov_mem8_to_r64zx(asm_x64_t *as, int src_r64, int src_disp, int dest_r64) {
+    if (src_r64 < 8 && dest_r64 < 8) {
+        asm_x64_write_byte_2(as, 0x0f, OPCODE_MOVZX_RM8_TO_R64);
+    } else {
+        asm_x64_write_byte_3(as, REX_PREFIX | REX_R_FROM_R64(dest_r64) | REX_B_FROM_R64(src_r64), 0x0f, OPCODE_MOVZX_RM8_TO_R64);
+    }
+    asm_x64_write_r64_disp(as, dest_r64, src_r64, src_disp);
+}
+
+void asm_x64_mov_mem16_to_r64zx(asm_x64_t *as, int src_r64, int src_disp, int dest_r64) {
+    if (src_r64 < 8 && dest_r64 < 8) {
+        asm_x64_write_byte_2(as, 0x0f, OPCODE_MOVZX_RM16_TO_R64);
+    } else {
+        asm_x64_write_byte_3(as, REX_PREFIX | REX_R_FROM_R64(dest_r64) | REX_B_FROM_R64(src_r64), 0x0f, OPCODE_MOVZX_RM16_TO_R64);
+    }
+    asm_x64_write_r64_disp(as, dest_r64, src_r64, src_disp);
+}
+
+void asm_x64_mov_mem32_to_r64zx(asm_x64_t *as, int src_r64, int src_disp, int dest_r64) {
+    if (src_r64 < 8 && dest_r64 < 8) {
+        asm_x64_write_byte_1(as, OPCODE_MOV_RM64_TO_R64);
+    } else {
+        asm_x64_write_byte_2(as, REX_PREFIX | REX_R_FROM_R64(dest_r64) | REX_B_FROM_R64(src_r64), OPCODE_MOV_RM64_TO_R64);
+    }
+    asm_x64_write_r64_disp(as, dest_r64, src_r64, src_disp);
+}
+
+void asm_x64_mov_mem64_to_r64(asm_x64_t *as, int src_r64, int src_disp, int dest_r64) {
+    // use REX prefix for 64 bit operation
+    asm_x64_write_byte_2(as, REX_PREFIX | REX_W | REX_R_FROM_R64(dest_r64) | REX_B_FROM_R64(src_r64), OPCODE_MOV_RM64_TO_R64);
+    asm_x64_write_r64_disp(as, dest_r64, src_r64, src_disp);
+}
+
+static void asm_x64_lea_disp_to_r64(asm_x64_t *as, int src_r64, int src_disp, int dest_r64) {
+    // use REX prefix for 64 bit operation
+    asm_x64_write_byte_2(as, REX_PREFIX | REX_W | REX_R_FROM_R64(dest_r64) | REX_B_FROM_R64(src_r64), OPCODE_LEA_MEM_TO_R64);
+    asm_x64_write_r64_disp(as, dest_r64, src_r64, src_disp);
+}
+
+/*
+void asm_x64_mov_i8_to_r8(asm_x64_t *as, int src_i8, int dest_r64) {
+    assert(dest_r64 < 8);
+    asm_x64_write_byte_2(as, OPCODE_MOV_I8_TO_R8 | dest_r64, src_i8);
+}
+*/
+
+size_t asm_x64_mov_i32_to_r64(asm_x64_t *as, int src_i32, int dest_r64) {
+    // cpu defaults to i32 to r64, with zero extension
+    if (dest_r64 < 8) {
+        asm_x64_write_byte_1(as, OPCODE_MOV_I64_TO_R64 | dest_r64);
+    } else {
+        asm_x64_write_byte_2(as, REX_PREFIX | REX_B, OPCODE_MOV_I64_TO_R64 | (dest_r64 & 7));
+    }
+    size_t loc = mp_asm_base_get_code_pos(&as->base);
+    asm_x64_write_word32(as, src_i32);
+    return loc;
+}
+
+void asm_x64_mov_i64_to_r64(asm_x64_t *as, int64_t src_i64, int dest_r64) {
+    // cpu defaults to i32 to r64
+    // to mov i64 to r64 need to use REX prefix
+    asm_x64_write_byte_2(as,
+        REX_PREFIX | REX_W | (dest_r64 < 8 ? 0 : REX_B),
+        OPCODE_MOV_I64_TO_R64 | (dest_r64 & 7));
+    asm_x64_write_word64(as, src_i64);
+}
+
+void asm_x64_mov_i64_to_r64_optimised(asm_x64_t *as, int64_t src_i64, int dest_r64) {
+    // TODO use movzx, movsx if possible
+    if (UNSIGNED_FIT32(src_i64)) {
+        // 5 bytes
+        asm_x64_mov_i32_to_r64(as, src_i64 & 0xffffffff, dest_r64);
+    } else {
+        // 10 bytes
+        asm_x64_mov_i64_to_r64(as, src_i64, dest_r64);
+    }
+}
+
+void asm_x64_not_r64(asm_x64_t *as, int dest_r64) {
+    asm_x64_generic_r64_r64(as, dest_r64, 2, OPCODE_NOT_RM64);
+}
+
+void asm_x64_neg_r64(asm_x64_t *as, int dest_r64) {
+    asm_x64_generic_r64_r64(as, dest_r64, 3, OPCODE_NEG_RM64);
+}
+
+void asm_x64_and_r64_r64(asm_x64_t *as, int dest_r64, int src_r64) {
+    asm_x64_generic_r64_r64(as, dest_r64, src_r64, OPCODE_AND_R64_TO_RM64);
+}
+
+void asm_x64_or_r64_r64(asm_x64_t *as, int dest_r64, int src_r64) {
+    asm_x64_generic_r64_r64(as, dest_r64, src_r64, OPCODE_OR_R64_TO_RM64);
+}
+
+void asm_x64_xor_r64_r64(asm_x64_t *as, int dest_r64, int src_r64) {
+    asm_x64_generic_r64_r64(as, dest_r64, src_r64, OPCODE_XOR_R64_TO_RM64);
+}
+
+void asm_x64_shl_r64_cl(asm_x64_t *as, int dest_r64) {
+    asm_x64_generic_r64_r64(as, dest_r64, 4, OPCODE_SHL_RM64_CL);
+}
+
+void asm_x64_shr_r64_cl(asm_x64_t *as, int dest_r64) {
+    asm_x64_generic_r64_r64(as, dest_r64, 5, OPCODE_SHR_RM64_CL);
+}
+
+void asm_x64_sar_r64_cl(asm_x64_t *as, int dest_r64) {
+    asm_x64_generic_r64_r64(as, dest_r64, 7, OPCODE_SAR_RM64_CL);
+}
+
+void asm_x64_add_r64_r64(asm_x64_t *as, int dest_r64, int src_r64) {
+    asm_x64_generic_r64_r64(as, dest_r64, src_r64, OPCODE_ADD_R64_TO_RM64);
+}
+
+void asm_x64_sub_r64_r64(asm_x64_t *as, int dest_r64, int src_r64) {
+    asm_x64_generic_r64_r64(as, dest_r64, src_r64, OPCODE_SUB_R64_FROM_RM64);
+}
+
+void asm_x64_mul_r64_r64(asm_x64_t *as, int dest_r64, int src_r64) {
+    // imul reg64, reg/mem64 -- 0x0f 0xaf /r
+    asm_x64_write_byte_1(as, REX_PREFIX | REX_W | REX_R_FROM_R64(dest_r64) | REX_B_FROM_R64(src_r64));
+    asm_x64_write_byte_3(as, 0x0f, 0xaf, MODRM_R64(dest_r64) | MODRM_RM_REG | MODRM_RM_R64(src_r64));
+}
+
+/*
+void asm_x64_sub_i32_from_r32(asm_x64_t *as, int src_i32, int dest_r32) {
+    if (SIGNED_FIT8(src_i32)) {
+        // defaults to 32 bit operation
+        asm_x64_write_byte_2(as, OPCODE_SUB_I8_FROM_RM64, MODRM_R64(5) | MODRM_RM_REG | MODRM_RM_R64(dest_r32));
+        asm_x64_write_byte_1(as, src_i32 & 0xff);
+    } else {
+        // defaults to 32 bit operation
+        asm_x64_write_byte_2(as, OPCODE_SUB_I32_FROM_RM64, MODRM_R64(5) | MODRM_RM_REG | MODRM_RM_R64(dest_r32));
+        asm_x64_write_word32(as, src_i32);
+    }
+}
+*/
+
+static void asm_x64_sub_r64_i32(asm_x64_t *as, int dest_r64, int src_i32) {
+    assert(dest_r64 < 8);
+    if (SIGNED_FIT8(src_i32)) {
+        // use REX prefix for 64 bit operation
+        asm_x64_write_byte_3(as, REX_PREFIX | REX_W, OPCODE_SUB_I8_FROM_RM64, MODRM_R64(5) | MODRM_RM_REG | MODRM_RM_R64(dest_r64));
+        asm_x64_write_byte_1(as, src_i32 & 0xff);
+    } else {
+        // use REX prefix for 64 bit operation
+        asm_x64_write_byte_3(as, REX_PREFIX | REX_W, OPCODE_SUB_I32_FROM_RM64, MODRM_R64(5) | MODRM_RM_REG | MODRM_RM_R64(dest_r64));
+        asm_x64_write_word32(as, src_i32);
+    }
+}
+
+/*
+void asm_x64_shl_r32_by_imm(asm_x64_t *as, int r32, int imm) {
+    asm_x64_write_byte_2(as, OPCODE_SHL_RM32_BY_I8, MODRM_R64(4) | MODRM_RM_REG | MODRM_RM_R64(r32));
+    asm_x64_write_byte_1(as, imm);
+}
+
+void asm_x64_shr_r32_by_imm(asm_x64_t *as, int r32, int imm) {
+    asm_x64_write_byte_2(as, OPCODE_SHR_RM32_BY_I8, MODRM_R64(5) | MODRM_RM_REG | MODRM_RM_R64(r32));
+    asm_x64_write_byte_1(as, imm);
+}
+
+void asm_x64_sar_r32_by_imm(asm_x64_t *as, int r32, int imm) {
+    asm_x64_write_byte_2(as, OPCODE_SAR_RM32_BY_I8, MODRM_R64(7) | MODRM_RM_REG | MODRM_RM_R64(r32));
+    asm_x64_write_byte_1(as, imm);
+}
+*/
+
+void asm_x64_cmp_r64_with_r64(asm_x64_t *as, int src_r64_a, int src_r64_b) {
+    asm_x64_generic_r64_r64(as, src_r64_b, src_r64_a, OPCODE_CMP_R64_WITH_RM64);
+}
+
+/*
+void asm_x64_cmp_i32_with_r32(asm_x64_t *as, int src_i32, int src_r32) {
+    if (SIGNED_FIT8(src_i32)) {
+        asm_x64_write_byte_2(as, OPCODE_CMP_I8_WITH_RM32, MODRM_R64(7) | MODRM_RM_REG | MODRM_RM_R64(src_r32));
+        asm_x64_write_byte_1(as, src_i32 & 0xff);
+    } else {
+        asm_x64_write_byte_2(as, OPCODE_CMP_I32_WITH_RM32, MODRM_R64(7) | MODRM_RM_REG | MODRM_RM_R64(src_r32));
+        asm_x64_write_word32(as, src_i32);
+    }
+}
+*/
+
+void asm_x64_test_r8_with_r8(asm_x64_t *as, int src_r64_a, int src_r64_b) {
+    assert(src_r64_a < 8);
+    assert(src_r64_b < 8);
+    asm_x64_write_byte_2(as, OPCODE_TEST_R8_WITH_RM8, MODRM_R64(src_r64_a) | MODRM_RM_REG | MODRM_RM_R64(src_r64_b));
+}
+
+void asm_x64_test_r64_with_r64(asm_x64_t *as, int src_r64_a, int src_r64_b) {
+    asm_x64_generic_r64_r64(as, src_r64_b, src_r64_a, OPCODE_TEST_R64_WITH_RM64);
+}
+
+void asm_x64_setcc_r8(asm_x64_t *as, int jcc_type, int dest_r8) {
+    assert(dest_r8 < 8);
+    asm_x64_write_byte_3(as, OPCODE_SETCC_RM8_A, OPCODE_SETCC_RM8_B | jcc_type, MODRM_R64(0) | MODRM_RM_REG | MODRM_RM_R64(dest_r8));
+}
+
+void asm_x64_jmp_reg(asm_x64_t *as, int src_r64) {
+    assert(src_r64 < 8);
+    asm_x64_write_byte_2(as, OPCODE_JMP_RM64, MODRM_R64(4) | MODRM_RM_REG | MODRM_RM_R64(src_r64));
+}
+
+static mp_uint_t get_label_dest(asm_x64_t *as, mp_uint_t label) {
+    assert(label < as->base.max_num_labels);
+    return as->base.label_offsets[label];
+}
+
+void asm_x64_jmp_label(asm_x64_t *as, mp_uint_t label) {
+    mp_uint_t dest = get_label_dest(as, label);
+    mp_int_t rel = dest - as->base.code_offset;
+    if (dest != (mp_uint_t)-1 && rel < 0) {
+        // is a backwards jump, so we know the size of the jump on the first pass
+        // calculate rel assuming 8 bit relative jump
+        rel -= 2;
+        if (SIGNED_FIT8(rel)) {
+            asm_x64_write_byte_2(as, OPCODE_JMP_REL8, rel & 0xff);
+        } else {
+            rel += 2;
+            goto large_jump;
+        }
+    } else {
+        // is a forwards jump, so need to assume it's large
+    large_jump:
+        rel -= 5;
+        asm_x64_write_byte_1(as, OPCODE_JMP_REL32);
+        asm_x64_write_word32(as, rel);
+    }
+}
+
+void asm_x64_jcc_label(asm_x64_t *as, int jcc_type, mp_uint_t label) {
+    mp_uint_t dest = get_label_dest(as, label);
+    mp_int_t rel = dest - as->base.code_offset;
+    if (dest != (mp_uint_t)-1 && rel < 0) {
+        // is a backwards jump, so we know the size of the jump on the first pass
+        // calculate rel assuming 8 bit relative jump
+        rel -= 2;
+        if (SIGNED_FIT8(rel)) {
+            asm_x64_write_byte_2(as, OPCODE_JCC_REL8 | jcc_type, rel & 0xff);
+        } else {
+            rel += 2;
+            goto large_jump;
+        }
+    } else {
+        // is a forwards jump, so need to assume it's large
+    large_jump:
+        rel -= 6;
+        asm_x64_write_byte_2(as, OPCODE_JCC_REL32_A, OPCODE_JCC_REL32_B | jcc_type);
+        asm_x64_write_word32(as, rel);
+    }
+}
+
+void asm_x64_entry(asm_x64_t *as, int num_locals) {
+    assert(num_locals >= 0);
+    asm_x64_push_r64(as, ASM_X64_REG_RBP);
+    asm_x64_push_r64(as, ASM_X64_REG_RBX);
+    asm_x64_push_r64(as, ASM_X64_REG_R12);
+    asm_x64_push_r64(as, ASM_X64_REG_R13);
+    num_locals |= 1; // make it odd so stack is aligned on 16 byte boundary
+    asm_x64_sub_r64_i32(as, ASM_X64_REG_RSP, num_locals * WORD_SIZE);
+    as->num_locals = num_locals;
+}
+
+void asm_x64_exit(asm_x64_t *as) {
+    asm_x64_sub_r64_i32(as, ASM_X64_REG_RSP, -as->num_locals * WORD_SIZE);
+    asm_x64_pop_r64(as, ASM_X64_REG_R13);
+    asm_x64_pop_r64(as, ASM_X64_REG_R12);
+    asm_x64_pop_r64(as, ASM_X64_REG_RBX);
+    asm_x64_pop_r64(as, ASM_X64_REG_RBP);
+    asm_x64_ret(as);
+}
+
+// locals:
+//  - stored on the stack in ascending order
+//  - numbered 0 through as->num_locals-1
+//  - RSP points to the first local
+//
+//  | RSP
+//  v
+//  l0  l1  l2  ...  l(n-1)
+//  ^                ^
+//  | low address    | high address in RAM
+//
+static int asm_x64_local_offset_from_rsp(asm_x64_t *as, int local_num) {
+    (void)as;
+    // Stack is full descending, RSP points to local0
+    return local_num * WORD_SIZE;
+}
+
+void asm_x64_mov_local_to_r64(asm_x64_t *as, int src_local_num, int dest_r64) {
+    asm_x64_mov_mem64_to_r64(as, ASM_X64_REG_RSP, asm_x64_local_offset_from_rsp(as, src_local_num), dest_r64);
+}
+
+void asm_x64_mov_r64_to_local(asm_x64_t *as, int src_r64, int dest_local_num) {
+    asm_x64_mov_r64_to_mem64(as, src_r64, ASM_X64_REG_RSP, asm_x64_local_offset_from_rsp(as, dest_local_num));
+}
+
+void asm_x64_mov_local_addr_to_r64(asm_x64_t *as, int local_num, int dest_r64) {
+    int offset = asm_x64_local_offset_from_rsp(as, local_num);
+    if (offset == 0) {
+        asm_x64_mov_r64_r64(as, dest_r64, ASM_X64_REG_RSP);
+    } else {
+        asm_x64_lea_disp_to_r64(as, ASM_X64_REG_RSP, offset, dest_r64);
+    }
+}
+
+void asm_x64_mov_reg_pcrel(asm_x64_t *as, int dest_r64, mp_uint_t label) {
+    mp_uint_t dest = get_label_dest(as, label);
+    mp_int_t rel = dest - (as->base.code_offset + 7);
+    asm_x64_write_byte_3(as, REX_PREFIX | REX_W | REX_R_FROM_R64(dest_r64), OPCODE_LEA_MEM_TO_R64, MODRM_R64(dest_r64) | MODRM_RM_R64(5));
+    asm_x64_write_word32(as, rel);
+}
+
+/*
+void asm_x64_push_local(asm_x64_t *as, int local_num) {
+    asm_x64_push_disp(as, ASM_X64_REG_RSP, asm_x64_local_offset_from_rsp(as, local_num));
+}
+
+void asm_x64_push_local_addr(asm_x64_t *as, int local_num, int temp_r64) {
+    asm_x64_mov_r64_r64(as, temp_r64, ASM_X64_REG_RSP);
+    asm_x64_add_i32_to_r32(as, asm_x64_local_offset_from_rsp(as, local_num), temp_r64);
+    asm_x64_push_r64(as, temp_r64);
+}
+*/
+
+/*
+   can't use these because code might be relocated when resized
+
+void asm_x64_call(asm_x64_t *as, void* func) {
+    asm_x64_sub_i32_from_r32(as, 8, ASM_X64_REG_RSP);
+    asm_x64_write_byte_1(as, OPCODE_CALL_REL32);
+    asm_x64_write_word32(as, func - (void*)(as->code_cur + 4));
+    asm_x64_mov_r64_r64(as, ASM_X64_REG_RSP, ASM_X64_REG_RBP);
+}
+
+void asm_x64_call_i1(asm_x64_t *as, void* func, int i1) {
+    asm_x64_sub_i32_from_r32(as, 8, ASM_X64_REG_RSP);
+    asm_x64_sub_i32_from_r32(as, 12, ASM_X64_REG_RSP);
+    asm_x64_push_i32(as, i1);
+    asm_x64_write_byte_1(as, OPCODE_CALL_REL32);
+    asm_x64_write_word32(as, func - (void*)(as->code_cur + 4));
+    asm_x64_add_i32_to_r32(as, 16, ASM_X64_REG_RSP);
+    asm_x64_mov_r64_r64(as, ASM_X64_REG_RSP, ASM_X64_REG_RBP);
+}
+*/
+
+void asm_x64_call_ind(asm_x64_t *as, size_t fun_id, int temp_r64) {
+    assert(temp_r64 < 8);
+    asm_x64_mov_mem64_to_r64(as, ASM_X64_REG_FUN_TABLE, fun_id * WORD_SIZE, temp_r64);
+    asm_x64_write_byte_2(as, OPCODE_CALL_RM32, MODRM_R64(2) | MODRM_RM_REG | MODRM_RM_R64(temp_r64));
+}
+
+#endif // MICROPY_EMIT_X64

APP_Framework/Framework/knowing/micropython/py/asmx64.h

@@ -0,0 +1,226 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2013, 2014 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifndef MICROPY_INCLUDED_PY_ASMX64_H
+#define MICROPY_INCLUDED_PY_ASMX64_H
+
+#include "py/mpconfig.h"
+#include "py/misc.h"
+#include "py/asmbase.h"
+
+// AMD64 calling convention is:
+//  - args pass in: RDI, RSI, RDX, RCX, R08, R09
+//  - return value in RAX
+//  - stack must be aligned on a 16-byte boundary before all calls
+//  - RAX, RCX, RDX, RSI, RDI, R08, R09, R10, R11 are caller-save
+//  - RBX, RBP, R12, R13, R14, R15 are callee-save
+
+// In the functions below, argument order follows x86 docs and generally
+// the destination is the first argument.
+// NOTE: this is a change from the old convention used in this file and
+// some functions still use the old (reverse) convention.
+
+#define ASM_X64_REG_RAX (0)
+#define ASM_X64_REG_RCX (1)
+#define ASM_X64_REG_RDX (2)
+#define ASM_X64_REG_RBX (3)
+#define ASM_X64_REG_RSP (4)
+#define ASM_X64_REG_RBP (5)
+#define ASM_X64_REG_RSI (6)
+#define ASM_X64_REG_RDI (7)
+#define ASM_X64_REG_R08 (8)
+#define ASM_X64_REG_R09 (9)
+#define ASM_X64_REG_R10 (10)
+#define ASM_X64_REG_R11 (11)
+#define ASM_X64_REG_R12 (12)
+#define ASM_X64_REG_R13 (13)
+#define ASM_X64_REG_R14 (14)
+#define ASM_X64_REG_R15 (15)
+
+// condition codes, used for jcc and setcc (despite their j-name!)
+#define ASM_X64_CC_JB  (0x2) // below, unsigned
+#define ASM_X64_CC_JAE (0x3) // above or equal, unsigned
+#define ASM_X64_CC_JZ  (0x4)
+#define ASM_X64_CC_JE  (0x4)
+#define ASM_X64_CC_JNZ (0x5)
+#define ASM_X64_CC_JNE (0x5)
+#define ASM_X64_CC_JBE (0x6) // below or equal, unsigned
+#define ASM_X64_CC_JA  (0x7) // above, unsigned
+#define ASM_X64_CC_JL  (0xc) // less, signed
+#define ASM_X64_CC_JGE (0xd) // greater or equal, signed
+#define ASM_X64_CC_JLE (0xe) // less or equal, signed
+#define ASM_X64_CC_JG  (0xf) // greater, signed
+
+typedef struct _asm_x64_t {
+    mp_asm_base_t base;
+    int num_locals;
+} asm_x64_t;
+
+static inline void asm_x64_end_pass(asm_x64_t *as) {
+    (void)as;
+}
+
+void asm_x64_nop(asm_x64_t *as);
+void asm_x64_push_r64(asm_x64_t *as, int src_r64);
+void asm_x64_pop_r64(asm_x64_t *as, int dest_r64);
+void asm_x64_mov_r64_r64(asm_x64_t *as, int dest_r64, int src_r64);
+size_t asm_x64_mov_i32_to_r64(asm_x64_t *as, int src_i32, int dest_r64);
+void asm_x64_mov_i64_to_r64(asm_x64_t *as, int64_t src_i64, int dest_r64);
+void asm_x64_mov_i64_to_r64_optimised(asm_x64_t *as, int64_t src_i64, int dest_r64);
+void asm_x64_mov_r8_to_mem8(asm_x64_t *as, int src_r64, int dest_r64, int dest_disp);
+void asm_x64_mov_r16_to_mem16(asm_x64_t *as, int src_r64, int dest_r64, int dest_disp);
+void asm_x64_mov_r32_to_mem32(asm_x64_t *as, int src_r64, int dest_r64, int dest_disp);
+void asm_x64_mov_r64_to_mem64(asm_x64_t *as, int src_r64, int dest_r64, int dest_disp);
+void asm_x64_mov_mem8_to_r64zx(asm_x64_t *as, int src_r64, int src_disp, int dest_r64);
+void asm_x64_mov_mem16_to_r64zx(asm_x64_t *as, int src_r64, int src_disp, int dest_r64);
+void asm_x64_mov_mem32_to_r64zx(asm_x64_t *as, int src_r64, int src_disp, int dest_r64);
+void asm_x64_mov_mem64_to_r64(asm_x64_t *as, int src_r64, int src_disp, int dest_r64);
+void asm_x64_not_r64(asm_x64_t *as, int dest_r64);
+void asm_x64_neg_r64(asm_x64_t *as, int dest_r64);
+void asm_x64_and_r64_r64(asm_x64_t *as, int dest_r64, int src_r64);
+void asm_x64_or_r64_r64(asm_x64_t *as, int dest_r64, int src_r64);
+void asm_x64_xor_r64_r64(asm_x64_t *as, int dest_r64, int src_r64);
+void asm_x64_shl_r64_cl(asm_x64_t *as, int dest_r64);
+void asm_x64_shr_r64_cl(asm_x64_t *as, int dest_r64);
+void asm_x64_sar_r64_cl(asm_x64_t *as, int dest_r64);
+void asm_x64_add_r64_r64(asm_x64_t *as, int dest_r64, int src_r64);
+void asm_x64_sub_r64_r64(asm_x64_t *as, int dest_r64, int src_r64);
+void asm_x64_mul_r64_r64(asm_x64_t *as, int dest_r64, int src_r64);
+void asm_x64_cmp_r64_with_r64(asm_x64_t *as, int src_r64_a, int src_r64_b);
+void asm_x64_test_r8_with_r8(asm_x64_t *as, int src_r64_a, int src_r64_b);
+void asm_x64_test_r64_with_r64(asm_x64_t *as, int src_r64_a, int src_r64_b);
+void asm_x64_setcc_r8(asm_x64_t *as, int jcc_type, int dest_r8);
+void asm_x64_jmp_reg(asm_x64_t *as, int src_r64);
+void asm_x64_jmp_label(asm_x64_t *as, mp_uint_t label);
+void asm_x64_jcc_label(asm_x64_t *as, int jcc_type, mp_uint_t label);
+void asm_x64_entry(asm_x64_t *as, int num_locals);
+void asm_x64_exit(asm_x64_t *as);
+void asm_x64_mov_local_to_r64(asm_x64_t *as, int src_local_num, int dest_r64);
+void asm_x64_mov_r64_to_local(asm_x64_t *as, int src_r64, int dest_local_num);
+void asm_x64_mov_local_addr_to_r64(asm_x64_t *as, int local_num, int dest_r64);
+void asm_x64_mov_reg_pcrel(asm_x64_t *as, int dest_r64, mp_uint_t label);
+void asm_x64_call_ind(asm_x64_t *as, size_t fun_id, int temp_r32);
+
+// Holds a pointer to mp_fun_table
+#define ASM_X64_REG_FUN_TABLE ASM_X64_REG_RBP
+
+#if GENERIC_ASM_API
+
+// The following macros provide a (mostly) arch-independent API to
+// generate native code, and are used by the native emitter.
+
+#define ASM_WORD_SIZE (8)
+
+#define REG_RET ASM_X64_REG_RAX
+#define REG_ARG_1 ASM_X64_REG_RDI
+#define REG_ARG_2 ASM_X64_REG_RSI
+#define REG_ARG_3 ASM_X64_REG_RDX
+#define REG_ARG_4 ASM_X64_REG_RCX
+#define REG_ARG_5 ASM_X64_REG_R08
+
+// caller-save
+#define REG_TEMP0 ASM_X64_REG_RAX
+#define REG_TEMP1 ASM_X64_REG_RDI
+#define REG_TEMP2 ASM_X64_REG_RSI
+
+// callee-save
+#define REG_LOCAL_1 ASM_X64_REG_RBX
+#define REG_LOCAL_2 ASM_X64_REG_R12
+#define REG_LOCAL_3 ASM_X64_REG_R13
+#define REG_LOCAL_NUM (3)
+
+// Holds a pointer to mp_fun_table
+#define REG_FUN_TABLE ASM_X64_REG_FUN_TABLE
+
+#define ASM_T                           asm_x64_t
+#define ASM_END_PASS                    asm_x64_end_pass
+#define ASM_ENTRY(as, num_locals, name) asm_x64_entry((as), (num_locals))
+#define ASM_EXIT                        asm_x64_exit
+
+#define ASM_JUMP                        asm_x64_jmp_label
+#define ASM_JUMP_IF_REG_ZERO(as, reg, label, bool_test) \
+    do { \
+        if (bool_test) { \
+            asm_x64_test_r8_with_r8((as), (reg), (reg)); \
+        } else { \
+            asm_x64_test_r64_with_r64((as), (reg), (reg)); \
+        } \
+        asm_x64_jcc_label(as, ASM_X64_CC_JZ, label); \
+    } while (0)
+#define ASM_JUMP_IF_REG_NONZERO(as, reg, label, bool_test) \
+    do { \
+        if (bool_test) { \
+            asm_x64_test_r8_with_r8((as), (reg), (reg)); \
+        } else { \
+            asm_x64_test_r64_with_r64((as), (reg), (reg)); \
+        } \
+        asm_x64_jcc_label(as, ASM_X64_CC_JNZ, label); \
+    } while (0)
+#define ASM_JUMP_IF_REG_EQ(as, reg1, reg2, label) \
+    do { \
+        asm_x64_cmp_r64_with_r64(as, reg1, reg2); \
+        asm_x64_jcc_label(as, ASM_X64_CC_JE, label); \
+    } while (0)
+#define ASM_JUMP_REG(as, reg) asm_x64_jmp_reg((as), (reg))
+#define ASM_CALL_IND(as, idx) asm_x64_call_ind(as, idx, ASM_X64_REG_RAX)
+
+#define ASM_MOV_LOCAL_REG(as, local_num, reg_src) asm_x64_mov_r64_to_local((as), (reg_src), (local_num))
+#define ASM_MOV_REG_IMM(as, reg_dest, imm) asm_x64_mov_i64_to_r64_optimised((as), (imm), (reg_dest))
+#define ASM_MOV_REG_LOCAL(as, reg_dest, local_num) asm_x64_mov_local_to_r64((as), (local_num), (reg_dest))
+#define ASM_MOV_REG_REG(as, reg_dest, reg_src) asm_x64_mov_r64_r64((as), (reg_dest), (reg_src))
+#define ASM_MOV_REG_LOCAL_ADDR(as, reg_dest, local_num) asm_x64_mov_local_addr_to_r64((as), (local_num), (reg_dest))
+#define ASM_MOV_REG_PCREL(as, reg_dest, label) asm_x64_mov_reg_pcrel((as), (reg_dest), (label))
+
+#define ASM_NOT_REG(as, reg) asm_x64_not_r64((as), (reg))
+#define ASM_NEG_REG(as, reg) asm_x64_neg_r64((as), (reg))
+#define ASM_LSL_REG(as, reg) asm_x64_shl_r64_cl((as), (reg))
+#define ASM_LSR_REG(as, reg) asm_x64_shr_r64_cl((as), (reg))
+#define ASM_ASR_REG(as, reg) asm_x64_sar_r64_cl((as), (reg))
+#define ASM_OR_REG_REG(as, reg_dest, reg_src) asm_x64_or_r64_r64((as), (reg_dest), (reg_src))
+#define ASM_XOR_REG_REG(as, reg_dest, reg_src) asm_x64_xor_r64_r64((as), (reg_dest), (reg_src))
+#define ASM_AND_REG_REG(as, reg_dest, reg_src) asm_x64_and_r64_r64((as), (reg_dest), (reg_src))
+#define ASM_ADD_REG_REG(as, reg_dest, reg_src) asm_x64_add_r64_r64((as), (reg_dest), (reg_src))
+#define ASM_SUB_REG_REG(as, reg_dest, reg_src) asm_x64_sub_r64_r64((as), (reg_dest), (reg_src))
+#define ASM_MUL_REG_REG(as, reg_dest, reg_src) asm_x64_mul_r64_r64((as), (reg_dest), (reg_src))
+
+#define ASM_LOAD_REG_REG_OFFSET(as, reg_dest, reg_base, qword_offset) asm_x64_mov_mem64_to_r64((as), (reg_base), 8 * (qword_offset), (reg_dest))
+#define ASM_LOAD8_REG_REG(as, reg_dest, reg_base) ASM_LOAD8_REG_REG_OFFSET((as), (reg_dest), (reg_base), 0)
+#define ASM_LOAD8_REG_REG_OFFSET(as, reg_dest, reg_base, byte_offset) asm_x64_mov_mem8_to_r64zx((as), (reg_base), (byte_offset), (reg_dest))
+#define ASM_LOAD16_REG_REG(as, reg_dest, reg_base) ASM_LOAD16_REG_REG_OFFSET((as), (reg_dest), (reg_base), 0)
+#define ASM_LOAD16_REG_REG_OFFSET(as, reg_dest, reg_base, word_offset) asm_x64_mov_mem16_to_r64zx((as), (reg_base), 2 * (word_offset), (reg_dest))
+#define ASM_LOAD32_REG_REG(as, reg_dest, reg_base) ASM_LOAD32_REG_REG_OFFSET((as), (reg_dest), (reg_base), 0)
+#define ASM_LOAD32_REG_REG_OFFSET(as, reg_dest, reg_base, dword_offset) asm_x64_mov_mem32_to_r64zx((as), (reg_base), 4 * (dword_offset), (reg_dest))
+
+#define ASM_STORE_REG_REG_OFFSET(as, reg_src, reg_base, qword_offset) asm_x64_mov_r64_to_mem64((as), (reg_src), (reg_base), 8 * (qword_offset))
+#define ASM_STORE8_REG_REG(as, reg_src, reg_base) ASM_STORE8_REG_REG_OFFSET((as), (reg_src), (reg_base), 0)
+#define ASM_STORE8_REG_REG_OFFSET(as, reg_src, reg_base, byte_offset) asm_x64_mov_r8_to_mem8((as), (reg_src), (reg_base), (byte_offset))
+#define ASM_STORE16_REG_REG(as, reg_src, reg_base) ASM_STORE16_REG_REG_OFFSET((as), (reg_src), (reg_base), 0)
+#define ASM_STORE16_REG_REG_OFFSET(as, reg_src, reg_base, word_offset) asm_x64_mov_r16_to_mem16((as), (reg_src), (reg_base), 2 * (word_offset))
+#define ASM_STORE32_REG_REG(as, reg_src, reg_base) ASM_STORE32_REG_REG_OFFSET((as), (reg_src), (reg_base), 0)
+#define ASM_STORE32_REG_REG_OFFSET(as, reg_src, reg_base, dword_offset) asm_x64_mov_r32_to_mem32((as), (reg_src), (reg_base), 4 * (dword_offset))
+
+#endif // GENERIC_ASM_API
+
+#endif // MICROPY_INCLUDED_PY_ASMX64_H

APP_Framework/Framework/knowing/micropython/py/asmx86.c

@@ -0,0 +1,545 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2014 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <stdint.h>
+#include <stdio.h>
+#include <assert.h>
+#include <string.h>
+
+#include "py/mpconfig.h"
+
+// wrapper around everything in this file
+#if MICROPY_EMIT_X86
+
+#include "py/asmx86.h"
+
+/* all offsets are measured in multiples of 4 bytes */
+#define WORD_SIZE                (4)
+
+#define OPCODE_NOP               (0x90)
+#define OPCODE_PUSH_R32          (0x50)
+// #define OPCODE_PUSH_I32          (0x68)
+// #define OPCODE_PUSH_M32          (0xff) /* /6 */
+#define OPCODE_POP_R32           (0x58)
+#define OPCODE_RET               (0xc3)
+// #define OPCODE_MOV_I8_TO_R8      (0xb0) /* +rb */
+#define OPCODE_MOV_I32_TO_R32    (0xb8)
+// #define OPCODE_MOV_I32_TO_RM32   (0xc7)
+#define OPCODE_MOV_R8_TO_RM8     (0x88) /* /r */
+#define OPCODE_MOV_R32_TO_RM32   (0x89) /* /r */
+#define OPCODE_MOV_RM32_TO_R32   (0x8b) /* /r */
+#define OPCODE_MOVZX_RM8_TO_R32  (0xb6) /* 0x0f 0xb6/r */
+#define OPCODE_MOVZX_RM16_TO_R32 (0xb7) /* 0x0f 0xb7/r */
+#define OPCODE_LEA_MEM_TO_R32    (0x8d) /* /r */
+#define OPCODE_NOT_RM32          (0xf7) /* /2 */
+#define OPCODE_NEG_RM32          (0xf7) /* /3 */
+#define OPCODE_AND_R32_TO_RM32   (0x21) /* /r */
+#define OPCODE_OR_R32_TO_RM32    (0x09) /* /r */
+#define OPCODE_XOR_R32_TO_RM32   (0x31) /* /r */
+#define OPCODE_ADD_R32_TO_RM32   (0x01)
+#define OPCODE_ADD_I32_TO_RM32   (0x81) /* /0 */
+#define OPCODE_ADD_I8_TO_RM32    (0x83) /* /0 */
+#define OPCODE_SUB_R32_FROM_RM32 (0x29)
+#define OPCODE_SUB_I32_FROM_RM32 (0x81) /* /5 */
+#define OPCODE_SUB_I8_FROM_RM32  (0x83) /* /5 */
+// #define OPCODE_SHL_RM32_BY_I8    (0xc1) /* /4 */
+// #define OPCODE_SHR_RM32_BY_I8    (0xc1) /* /5 */
+// #define OPCODE_SAR_RM32_BY_I8    (0xc1) /* /7 */
+#define OPCODE_SHL_RM32_CL       (0xd3) /* /4 */
+#define OPCODE_SHR_RM32_CL       (0xd3) /* /5 */
+#define OPCODE_SAR_RM32_CL       (0xd3) /* /7 */
+// #define OPCODE_CMP_I32_WITH_RM32 (0x81) /* /7 */
+// #define OPCODE_CMP_I8_WITH_RM32  (0x83) /* /7 */
+#define OPCODE_CMP_R32_WITH_RM32 (0x39)
+// #define OPCODE_CMP_RM32_WITH_R32 (0x3b)
+#define OPCODE_TEST_R8_WITH_RM8  (0x84) /* /r */
+#define OPCODE_TEST_R32_WITH_RM32 (0x85) /* /r */
+#define OPCODE_JMP_REL8          (0xeb)
+#define OPCODE_JMP_REL32         (0xe9)
+#define OPCODE_JMP_RM32          (0xff) /* /4 */
+#define OPCODE_JCC_REL8          (0x70) /* | jcc type */
+#define OPCODE_JCC_REL32_A       (0x0f)
+#define OPCODE_JCC_REL32_B       (0x80) /* | jcc type */
+#define OPCODE_SETCC_RM8_A       (0x0f)
+#define OPCODE_SETCC_RM8_B       (0x90) /* | jcc type, /0 */
+#define OPCODE_CALL_REL32        (0xe8)
+#define OPCODE_CALL_RM32         (0xff) /* /2 */
+#define OPCODE_LEAVE             (0xc9)
+
+#define MODRM_R32(x)    ((x) << 3)
+#define MODRM_RM_DISP0  (0x00)
+#define MODRM_RM_DISP8  (0x40)
+#define MODRM_RM_DISP32 (0x80)
+#define MODRM_RM_REG    (0xc0)
+#define MODRM_RM_R32(x) (x)
+
+#define OP_SIZE_PREFIX (0x66)
+
+#define IMM32_L0(x) ((x) & 0xff)
+#define IMM32_L1(x) (((x) >> 8) & 0xff)
+#define IMM32_L2(x) (((x) >> 16) & 0xff)
+#define IMM32_L3(x) (((x) >> 24) & 0xff)
+
+#define SIGNED_FIT8(x) (((x) & 0xffffff80) == 0) || (((x) & 0xffffff80) == 0xffffff80)
+
+static void asm_x86_write_byte_1(asm_x86_t *as, byte b1) {
+    byte *c = mp_asm_base_get_cur_to_write_bytes(&as->base, 1);
+    if (c != NULL) {
+        c[0] = b1;
+    }
+}
+
+static void asm_x86_write_byte_2(asm_x86_t *as, byte b1, byte b2) {
+    byte *c = mp_asm_base_get_cur_to_write_bytes(&as->base, 2);
+    if (c != NULL) {
+        c[0] = b1;
+        c[1] = b2;
+    }
+}
+
+static void asm_x86_write_byte_3(asm_x86_t *as, byte b1, byte b2, byte b3) {
+    byte *c = mp_asm_base_get_cur_to_write_bytes(&as->base, 3);
+    if (c != NULL) {
+        c[0] = b1;
+        c[1] = b2;
+        c[2] = b3;
+    }
+}
+
+static void asm_x86_write_word32(asm_x86_t *as, int w32) {
+    byte *c = mp_asm_base_get_cur_to_write_bytes(&as->base, 4);
+    if (c != NULL) {
+        c[0] = IMM32_L0(w32);
+        c[1] = IMM32_L1(w32);
+        c[2] = IMM32_L2(w32);
+        c[3] = IMM32_L3(w32);
+    }
+}
+
+static void asm_x86_write_r32_disp(asm_x86_t *as, int r32, int disp_r32, int disp_offset) {
+    uint8_t rm_disp;
+    if (disp_offset == 0 && disp_r32 != ASM_X86_REG_EBP) {
+        rm_disp = MODRM_RM_DISP0;
+    } else if (SIGNED_FIT8(disp_offset)) {
+        rm_disp = MODRM_RM_DISP8;
+    } else {
+        rm_disp = MODRM_RM_DISP32;
+    }
+    asm_x86_write_byte_1(as, MODRM_R32(r32) | rm_disp | MODRM_RM_R32(disp_r32));
+    if (disp_r32 == ASM_X86_REG_ESP) {
+        // Special case for esp, it needs a SIB byte
+        asm_x86_write_byte_1(as, 0x24);
+    }
+    if (rm_disp == MODRM_RM_DISP8) {
+        asm_x86_write_byte_1(as, IMM32_L0(disp_offset));
+    } else if (rm_disp == MODRM_RM_DISP32) {
+        asm_x86_write_word32(as, disp_offset);
+    }
+}
+
+static void asm_x86_generic_r32_r32(asm_x86_t *as, int dest_r32, int src_r32, int op) {
+    asm_x86_write_byte_2(as, op, MODRM_R32(src_r32) | MODRM_RM_REG | MODRM_RM_R32(dest_r32));
+}
+
+#if 0
+static void asm_x86_nop(asm_x86_t *as) {
+    asm_x86_write_byte_1(as, OPCODE_NOP);
+}
+#endif
+
+static void asm_x86_push_r32(asm_x86_t *as, int src_r32) {
+    asm_x86_write_byte_1(as, OPCODE_PUSH_R32 | src_r32);
+}
+
+#if 0
+void asm_x86_push_i32(asm_x86_t *as, int src_i32) {
+    asm_x86_write_byte_1(as, OPCODE_PUSH_I32);
+    asm_x86_write_word32(as, src_i32);
+}
+
+void asm_x86_push_disp(asm_x86_t *as, int src_r32, int src_offset) {
+    asm_x86_write_byte_1(as, OPCODE_PUSH_M32);
+    asm_x86_write_r32_disp(as, 6, src_r32, src_offset);
+}
+#endif
+
+static void asm_x86_pop_r32(asm_x86_t *as, int dest_r32) {
+    asm_x86_write_byte_1(as, OPCODE_POP_R32 | dest_r32);
+}
+
+static void asm_x86_ret(asm_x86_t *as) {
+    asm_x86_write_byte_1(as, OPCODE_RET);
+}
+
+void asm_x86_mov_r32_r32(asm_x86_t *as, int dest_r32, int src_r32) {
+    asm_x86_generic_r32_r32(as, dest_r32, src_r32, OPCODE_MOV_R32_TO_RM32);
+}
+
+void asm_x86_mov_r8_to_mem8(asm_x86_t *as, int src_r32, int dest_r32, int dest_disp) {
+    asm_x86_write_byte_1(as, OPCODE_MOV_R8_TO_RM8);
+    asm_x86_write_r32_disp(as, src_r32, dest_r32, dest_disp);
+}
+
+void asm_x86_mov_r16_to_mem16(asm_x86_t *as, int src_r32, int dest_r32, int dest_disp) {
+    asm_x86_write_byte_2(as, OP_SIZE_PREFIX, OPCODE_MOV_R32_TO_RM32);
+    asm_x86_write_r32_disp(as, src_r32, dest_r32, dest_disp);
+}
+
+void asm_x86_mov_r32_to_mem32(asm_x86_t *as, int src_r32, int dest_r32, int dest_disp) {
+    asm_x86_write_byte_1(as, OPCODE_MOV_R32_TO_RM32);
+    asm_x86_write_r32_disp(as, src_r32, dest_r32, dest_disp);
+}
+
+void asm_x86_mov_mem8_to_r32zx(asm_x86_t *as, int src_r32, int src_disp, int dest_r32) {
+    asm_x86_write_byte_2(as, 0x0f, OPCODE_MOVZX_RM8_TO_R32);
+    asm_x86_write_r32_disp(as, dest_r32, src_r32, src_disp);
+}
+
+void asm_x86_mov_mem16_to_r32zx(asm_x86_t *as, int src_r32, int src_disp, int dest_r32) {
+    asm_x86_write_byte_2(as, 0x0f, OPCODE_MOVZX_RM16_TO_R32);
+    asm_x86_write_r32_disp(as, dest_r32, src_r32, src_disp);
+}
+
+void asm_x86_mov_mem32_to_r32(asm_x86_t *as, int src_r32, int src_disp, int dest_r32) {
+    asm_x86_write_byte_1(as, OPCODE_MOV_RM32_TO_R32);
+    asm_x86_write_r32_disp(as, dest_r32, src_r32, src_disp);
+}
+
+static void asm_x86_lea_disp_to_r32(asm_x86_t *as, int src_r32, int src_disp, int dest_r32) {
+    asm_x86_write_byte_1(as, OPCODE_LEA_MEM_TO_R32);
+    asm_x86_write_r32_disp(as, dest_r32, src_r32, src_disp);
+}
+
+#if 0
+void asm_x86_mov_i8_to_r8(asm_x86_t *as, int src_i8, int dest_r32) {
+    asm_x86_write_byte_2(as, OPCODE_MOV_I8_TO_R8 | dest_r32, src_i8);
+}
+#endif
+
+size_t asm_x86_mov_i32_to_r32(asm_x86_t *as, int32_t src_i32, int dest_r32) {
+    asm_x86_write_byte_1(as, OPCODE_MOV_I32_TO_R32 | dest_r32);
+    size_t loc = mp_asm_base_get_code_pos(&as->base);
+    asm_x86_write_word32(as, src_i32);
+    return loc;
+}
+
+void asm_x86_not_r32(asm_x86_t *as, int dest_r32) {
+    asm_x86_generic_r32_r32(as, dest_r32, 2, OPCODE_NOT_RM32);
+}
+
+void asm_x86_neg_r32(asm_x86_t *as, int dest_r32) {
+    asm_x86_generic_r32_r32(as, dest_r32, 3, OPCODE_NEG_RM32);
+}
+
+void asm_x86_and_r32_r32(asm_x86_t *as, int dest_r32, int src_r32) {
+    asm_x86_generic_r32_r32(as, dest_r32, src_r32, OPCODE_AND_R32_TO_RM32);
+}
+
+void asm_x86_or_r32_r32(asm_x86_t *as, int dest_r32, int src_r32) {
+    asm_x86_generic_r32_r32(as, dest_r32, src_r32, OPCODE_OR_R32_TO_RM32);
+}
+
+void asm_x86_xor_r32_r32(asm_x86_t *as, int dest_r32, int src_r32) {
+    asm_x86_generic_r32_r32(as, dest_r32, src_r32, OPCODE_XOR_R32_TO_RM32);
+}
+
+void asm_x86_shl_r32_cl(asm_x86_t *as, int dest_r32) {
+    asm_x86_generic_r32_r32(as, dest_r32, 4, OPCODE_SHL_RM32_CL);
+}
+
+void asm_x86_shr_r32_cl(asm_x86_t *as, int dest_r32) {
+    asm_x86_generic_r32_r32(as, dest_r32, 5, OPCODE_SHR_RM32_CL);
+}
+
+void asm_x86_sar_r32_cl(asm_x86_t *as, int dest_r32) {
+    asm_x86_generic_r32_r32(as, dest_r32, 7, OPCODE_SAR_RM32_CL);
+}
+
+void asm_x86_add_r32_r32(asm_x86_t *as, int dest_r32, int src_r32) {
+    asm_x86_generic_r32_r32(as, dest_r32, src_r32, OPCODE_ADD_R32_TO_RM32);
+}
+
+static void asm_x86_add_i32_to_r32(asm_x86_t *as, int src_i32, int dest_r32) {
+    if (SIGNED_FIT8(src_i32)) {
+        asm_x86_write_byte_2(as, OPCODE_ADD_I8_TO_RM32, MODRM_R32(0) | MODRM_RM_REG | MODRM_RM_R32(dest_r32));
+        asm_x86_write_byte_1(as, src_i32 & 0xff);
+    } else {
+        asm_x86_write_byte_2(as, OPCODE_ADD_I32_TO_RM32, MODRM_R32(0) | MODRM_RM_REG | MODRM_RM_R32(dest_r32));
+        asm_x86_write_word32(as, src_i32);
+    }
+}
+
+void asm_x86_sub_r32_r32(asm_x86_t *as, int dest_r32, int src_r32) {
+    asm_x86_generic_r32_r32(as, dest_r32, src_r32, OPCODE_SUB_R32_FROM_RM32);
+}
+
+static void asm_x86_sub_r32_i32(asm_x86_t *as, int dest_r32, int src_i32) {
+    if (SIGNED_FIT8(src_i32)) {
+        // defaults to 32 bit operation
+        asm_x86_write_byte_2(as, OPCODE_SUB_I8_FROM_RM32, MODRM_R32(5) | MODRM_RM_REG | MODRM_RM_R32(dest_r32));
+        asm_x86_write_byte_1(as, src_i32 & 0xff);
+    } else {
+        // defaults to 32 bit operation
+        asm_x86_write_byte_2(as, OPCODE_SUB_I32_FROM_RM32, MODRM_R32(5) | MODRM_RM_REG | MODRM_RM_R32(dest_r32));
+        asm_x86_write_word32(as, src_i32);
+    }
+}
+
+void asm_x86_mul_r32_r32(asm_x86_t *as, int dest_r32, int src_r32) {
+    // imul reg32, reg/mem32 -- 0x0f 0xaf /r
+    asm_x86_write_byte_3(as, 0x0f, 0xaf, MODRM_R32(dest_r32) | MODRM_RM_REG | MODRM_RM_R32(src_r32));
+}
+
+#if 0
+/* shifts not tested */
+void asm_x86_shl_r32_by_imm(asm_x86_t *as, int r32, int imm) {
+    asm_x86_write_byte_2(as, OPCODE_SHL_RM32_BY_I8, MODRM_R32(4) | MODRM_RM_REG | MODRM_RM_R32(r32));
+    asm_x86_write_byte_1(as, imm);
+}
+
+void asm_x86_shr_r32_by_imm(asm_x86_t *as, int r32, int imm) {
+    asm_x86_write_byte_2(as, OPCODE_SHR_RM32_BY_I8, MODRM_R32(5) | MODRM_RM_REG | MODRM_RM_R32(r32));
+    asm_x86_write_byte_1(as, imm);
+}
+
+void asm_x86_sar_r32_by_imm(asm_x86_t *as, int r32, int imm) {
+    asm_x86_write_byte_2(as, OPCODE_SAR_RM32_BY_I8, MODRM_R32(7) | MODRM_RM_REG | MODRM_RM_R32(r32));
+    asm_x86_write_byte_1(as, imm);
+}
+#endif
+
+void asm_x86_cmp_r32_with_r32(asm_x86_t *as, int src_r32_a, int src_r32_b) {
+    asm_x86_generic_r32_r32(as, src_r32_b, src_r32_a, OPCODE_CMP_R32_WITH_RM32);
+}
+
+#if 0
+void asm_x86_cmp_i32_with_r32(asm_x86_t *as, int src_i32, int src_r32) {
+    if (SIGNED_FIT8(src_i32)) {
+        asm_x86_write_byte_2(as, OPCODE_CMP_I8_WITH_RM32, MODRM_R32(7) | MODRM_RM_REG | MODRM_RM_R32(src_r32));
+        asm_x86_write_byte_1(as, src_i32 & 0xff);
+    } else {
+        asm_x86_write_byte_2(as, OPCODE_CMP_I32_WITH_RM32, MODRM_R32(7) | MODRM_RM_REG | MODRM_RM_R32(src_r32));
+        asm_x86_write_word32(as, src_i32);
+    }
+}
+#endif
+
+void asm_x86_test_r8_with_r8(asm_x86_t *as, int src_r32_a, int src_r32_b) {
+    asm_x86_write_byte_2(as, OPCODE_TEST_R8_WITH_RM8, MODRM_R32(src_r32_a) | MODRM_RM_REG | MODRM_RM_R32(src_r32_b));
+}
+
+void asm_x86_test_r32_with_r32(asm_x86_t *as, int src_r32_a, int src_r32_b) {
+    asm_x86_generic_r32_r32(as, src_r32_b, src_r32_a, OPCODE_TEST_R32_WITH_RM32);
+}
+
+void asm_x86_setcc_r8(asm_x86_t *as, mp_uint_t jcc_type, int dest_r8) {
+    asm_x86_write_byte_3(as, OPCODE_SETCC_RM8_A, OPCODE_SETCC_RM8_B | jcc_type, MODRM_R32(0) | MODRM_RM_REG | MODRM_RM_R32(dest_r8));
+}
+
+void asm_x86_jmp_reg(asm_x86_t *as, int src_r32) {
+    asm_x86_write_byte_2(as, OPCODE_JMP_RM32, MODRM_R32(4) | MODRM_RM_REG | MODRM_RM_R32(src_r32));
+}
+
+static mp_uint_t get_label_dest(asm_x86_t *as, mp_uint_t label) {
+    assert(label < as->base.max_num_labels);
+    return as->base.label_offsets[label];
+}
+
+void asm_x86_jmp_label(asm_x86_t *as, mp_uint_t label) {
+    mp_uint_t dest = get_label_dest(as, label);
+    mp_int_t rel = dest - as->base.code_offset;
+    if (dest != (mp_uint_t)-1 && rel < 0) {
+        // is a backwards jump, so we know the size of the jump on the first pass
+        // calculate rel assuming 8 bit relative jump
+        rel -= 2;
+        if (SIGNED_FIT8(rel)) {
+            asm_x86_write_byte_2(as, OPCODE_JMP_REL8, rel & 0xff);
+        } else {
+            rel += 2;
+            goto large_jump;
+        }
+    } else {
+        // is a forwards jump, so need to assume it's large
+    large_jump:
+        rel -= 5;
+        asm_x86_write_byte_1(as, OPCODE_JMP_REL32);
+        asm_x86_write_word32(as, rel);
+    }
+}
+
+void asm_x86_jcc_label(asm_x86_t *as, mp_uint_t jcc_type, mp_uint_t label) {
+    mp_uint_t dest = get_label_dest(as, label);
+    mp_int_t rel = dest - as->base.code_offset;
+    if (dest != (mp_uint_t)-1 && rel < 0) {
+        // is a backwards jump, so we know the size of the jump on the first pass
+        // calculate rel assuming 8 bit relative jump
+        rel -= 2;
+        if (SIGNED_FIT8(rel)) {
+            asm_x86_write_byte_2(as, OPCODE_JCC_REL8 | jcc_type, rel & 0xff);
+        } else {
+            rel += 2;
+            goto large_jump;
+        }
+    } else {
+        // is a forwards jump, so need to assume it's large
+    large_jump:
+        rel -= 6;
+        asm_x86_write_byte_2(as, OPCODE_JCC_REL32_A, OPCODE_JCC_REL32_B | jcc_type);
+        asm_x86_write_word32(as, rel);
+    }
+}
+
+void asm_x86_entry(asm_x86_t *as, int num_locals) {
+    assert(num_locals >= 0);
+    asm_x86_push_r32(as, ASM_X86_REG_EBP);
+    asm_x86_push_r32(as, ASM_X86_REG_EBX);
+    asm_x86_push_r32(as, ASM_X86_REG_ESI);
+    asm_x86_push_r32(as, ASM_X86_REG_EDI);
+    num_locals |= 3; // make it odd so stack is aligned on 16 byte boundary
+    asm_x86_sub_r32_i32(as, ASM_X86_REG_ESP, num_locals * WORD_SIZE);
+    as->num_locals = num_locals;
+}
+
+void asm_x86_exit(asm_x86_t *as) {
+    asm_x86_sub_r32_i32(as, ASM_X86_REG_ESP, -as->num_locals * WORD_SIZE);
+    asm_x86_pop_r32(as, ASM_X86_REG_EDI);
+    asm_x86_pop_r32(as, ASM_X86_REG_ESI);
+    asm_x86_pop_r32(as, ASM_X86_REG_EBX);
+    asm_x86_pop_r32(as, ASM_X86_REG_EBP);
+    asm_x86_ret(as);
+}
+
+static int asm_x86_arg_offset_from_esp(asm_x86_t *as, size_t arg_num) {
+    // Above esp are: locals, 4 saved registers, return eip, arguments
+    return (as->num_locals + 4 + 1 + arg_num) * WORD_SIZE;
+}
+
+#if 0
+void asm_x86_push_arg(asm_x86_t *as, int src_arg_num) {
+    asm_x86_push_disp(as, ASM_X86_REG_ESP, asm_x86_arg_offset_from_esp(as, src_arg_num));
+}
+#endif
+
+void asm_x86_mov_arg_to_r32(asm_x86_t *as, int src_arg_num, int dest_r32) {
+    asm_x86_mov_mem32_to_r32(as, ASM_X86_REG_ESP, asm_x86_arg_offset_from_esp(as, src_arg_num), dest_r32);
+}
+
+#if 0
+void asm_x86_mov_r32_to_arg(asm_x86_t *as, int src_r32, int dest_arg_num) {
+    asm_x86_mov_r32_to_mem32(as, src_r32, ASM_X86_REG_ESP, asm_x86_arg_offset_from_esp(as, dest_arg_num));
+}
+#endif
+
+// locals:
+//  - stored on the stack in ascending order
+//  - numbered 0 through as->num_locals-1
+//  - ESP points to the first local
+//
+//  | ESP
+//  v
+//  l0  l1  l2  ...  l(n-1)
+//  ^                ^
+//  | low address    | high address in RAM
+//
+static int asm_x86_local_offset_from_esp(asm_x86_t *as, int local_num) {
+    (void)as;
+    // Stack is full descending, ESP points to local0
+    return local_num * WORD_SIZE;
+}
+
+void asm_x86_mov_local_to_r32(asm_x86_t *as, int src_local_num, int dest_r32) {
+    asm_x86_mov_mem32_to_r32(as, ASM_X86_REG_ESP, asm_x86_local_offset_from_esp(as, src_local_num), dest_r32);
+}
+
+void asm_x86_mov_r32_to_local(asm_x86_t *as, int src_r32, int dest_local_num) {
+    asm_x86_mov_r32_to_mem32(as, src_r32, ASM_X86_REG_ESP, asm_x86_local_offset_from_esp(as, dest_local_num));
+}
+
+void asm_x86_mov_local_addr_to_r32(asm_x86_t *as, int local_num, int dest_r32) {
+    int offset = asm_x86_local_offset_from_esp(as, local_num);
+    if (offset == 0) {
+        asm_x86_mov_r32_r32(as, dest_r32, ASM_X86_REG_ESP);
+    } else {
+        asm_x86_lea_disp_to_r32(as, ASM_X86_REG_ESP, offset, dest_r32);
+    }
+}
+
+void asm_x86_mov_reg_pcrel(asm_x86_t *as, int dest_r32, mp_uint_t label) {
+    asm_x86_write_byte_1(as, OPCODE_CALL_REL32);
+    asm_x86_write_word32(as, 0);
+    mp_uint_t dest = get_label_dest(as, label);
+    mp_int_t rel = dest - as->base.code_offset;
+    asm_x86_pop_r32(as, dest_r32);
+    // PC rel is usually a forward reference, so need to assume it's large
+    asm_x86_write_byte_2(as, OPCODE_ADD_I32_TO_RM32, MODRM_R32(0) | MODRM_RM_REG | MODRM_RM_R32(dest_r32));
+    asm_x86_write_word32(as, rel);
+}
+
+#if 0
+void asm_x86_push_local(asm_x86_t *as, int local_num) {
+    asm_x86_push_disp(as, ASM_X86_REG_ESP, asm_x86_local_offset_from_esp(as, local_num));
+}
+
+void asm_x86_push_local_addr(asm_x86_t *as, int local_num, int temp_r32) {
+    asm_x86_mov_r32_r32(as, temp_r32, ASM_X86_REG_ESP);
+    asm_x86_add_i32_to_r32(as, asm_x86_local_offset_from_esp(as, local_num), temp_r32);
+    asm_x86_push_r32(as, temp_r32);
+}
+#endif
+
+void asm_x86_call_ind(asm_x86_t *as, size_t fun_id, mp_uint_t n_args, int temp_r32) {
+    assert(n_args <= 4);
+
+    // Align stack on 16-byte boundary during the call
+    unsigned int align = ((n_args + 3) & ~3) - n_args;
+    if (align) {
+        asm_x86_sub_r32_i32(as, ASM_X86_REG_ESP, align * WORD_SIZE);
+    }
+
+    if (n_args > 3) {
+        asm_x86_push_r32(as, ASM_X86_REG_ARG_4);
+    }
+    if (n_args > 2) {
+        asm_x86_push_r32(as, ASM_X86_REG_ARG_3);
+    }
+    if (n_args > 1) {
+        asm_x86_push_r32(as, ASM_X86_REG_ARG_2);
+    }
+    if (n_args > 0) {
+        asm_x86_push_r32(as, ASM_X86_REG_ARG_1);
+    }
+
+    // Load the pointer to the function and make the call
+    asm_x86_mov_mem32_to_r32(as, ASM_X86_REG_FUN_TABLE, fun_id * WORD_SIZE, temp_r32);
+    asm_x86_write_byte_2(as, OPCODE_CALL_RM32, MODRM_R32(2) | MODRM_RM_REG | MODRM_RM_R32(temp_r32));
+
+    // the caller must clean up the stack
+    if (n_args > 0) {
+        asm_x86_add_i32_to_r32(as, (n_args + align) * WORD_SIZE, ASM_X86_REG_ESP);
+    }
+}
+
+#endif // MICROPY_EMIT_X86

APP_Framework/Framework/knowing/micropython/py/asmx86.h

@@ -0,0 +1,221 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2014 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifndef MICROPY_INCLUDED_PY_ASMX86_H
+#define MICROPY_INCLUDED_PY_ASMX86_H
+
+#include "py/mpconfig.h"
+#include "py/misc.h"
+#include "py/asmbase.h"
+
+// x86 cdecl calling convention is:
+//  - args passed on the stack in reverse order
+//  - return value in EAX
+//  - caller cleans up the stack after a call
+//  - stack must be aligned to 16-byte boundary before all calls
+//  - EAX, ECX, EDX are caller-save
+//  - EBX, ESI, EDI, EBP, ESP, EIP are callee-save
+
+// In the functions below, argument order follows x86 docs and generally
+// the destination is the first argument.
+// NOTE: this is a change from the old convention used in this file and
+// some functions still use the old (reverse) convention.
+
+#define ASM_X86_REG_EAX (0)
+#define ASM_X86_REG_ECX (1)
+#define ASM_X86_REG_EDX (2)
+#define ASM_X86_REG_EBX (3)
+#define ASM_X86_REG_ESP (4)
+#define ASM_X86_REG_EBP (5)
+#define ASM_X86_REG_ESI (6)
+#define ASM_X86_REG_EDI (7)
+
+// x86 passes values on the stack, but the emitter is register based, so we need
+// to define registers that can temporarily hold the function arguments.  They
+// need to be defined here so that asm_x86_call_ind can push them onto the stack
+// before the call.
+#define ASM_X86_REG_ARG_1 ASM_X86_REG_EAX
+#define ASM_X86_REG_ARG_2 ASM_X86_REG_ECX
+#define ASM_X86_REG_ARG_3 ASM_X86_REG_EDX
+#define ASM_X86_REG_ARG_4 ASM_X86_REG_EBX
+
+// condition codes, used for jcc and setcc (despite their j-name!)
+#define ASM_X86_CC_JB  (0x2) // below, unsigned
+#define ASM_X86_CC_JAE (0x3) // above or equal, unsigned
+#define ASM_X86_CC_JZ  (0x4)
+#define ASM_X86_CC_JE  (0x4)
+#define ASM_X86_CC_JNZ (0x5)
+#define ASM_X86_CC_JNE (0x5)
+#define ASM_X86_CC_JBE (0x6) // below or equal, unsigned
+#define ASM_X86_CC_JA  (0x7) // above, unsigned
+#define ASM_X86_CC_JL  (0xc) // less, signed
+#define ASM_X86_CC_JGE (0xd) // greater or equal, signed
+#define ASM_X86_CC_JLE (0xe) // less or equal, signed
+#define ASM_X86_CC_JG  (0xf) // greater, signed
+
+typedef struct _asm_x86_t {
+    mp_asm_base_t base;
+    int num_locals;
+} asm_x86_t;
+
+static inline void asm_x86_end_pass(asm_x86_t *as) {
+    (void)as;
+}
+
+void asm_x86_mov_r32_r32(asm_x86_t *as, int dest_r32, int src_r32);
+size_t asm_x86_mov_i32_to_r32(asm_x86_t *as, int32_t src_i32, int dest_r32);
+void asm_x86_mov_r8_to_mem8(asm_x86_t *as, int src_r32, int dest_r32, int dest_disp);
+void asm_x86_mov_r16_to_mem16(asm_x86_t *as, int src_r32, int dest_r32, int dest_disp);
+void asm_x86_mov_r32_to_mem32(asm_x86_t *as, int src_r32, int dest_r32, int dest_disp);
+void asm_x86_mov_mem8_to_r32zx(asm_x86_t *as, int src_r32, int src_disp, int dest_r32);
+void asm_x86_mov_mem16_to_r32zx(asm_x86_t *as, int src_r32, int src_disp, int dest_r32);
+void asm_x86_mov_mem32_to_r32(asm_x86_t *as, int src_r32, int src_disp, int dest_r32);
+void asm_x86_not_r32(asm_x86_t *as, int dest_r32);
+void asm_x86_neg_r32(asm_x86_t *as, int dest_r32);
+void asm_x86_and_r32_r32(asm_x86_t *as, int dest_r32, int src_r32);
+void asm_x86_or_r32_r32(asm_x86_t *as, int dest_r32, int src_r32);
+void asm_x86_xor_r32_r32(asm_x86_t *as, int dest_r32, int src_r32);
+void asm_x86_shl_r32_cl(asm_x86_t *as, int dest_r32);
+void asm_x86_shr_r32_cl(asm_x86_t *as, int dest_r32);
+void asm_x86_sar_r32_cl(asm_x86_t *as, int dest_r32);
+void asm_x86_add_r32_r32(asm_x86_t *as, int dest_r32, int src_r32);
+void asm_x86_sub_r32_r32(asm_x86_t *as, int dest_r32, int src_r32);
+void asm_x86_mul_r32_r32(asm_x86_t *as, int dest_r32, int src_r32);
+void asm_x86_cmp_r32_with_r32(asm_x86_t *as, int src_r32_a, int src_r32_b);
+void asm_x86_test_r8_with_r8(asm_x86_t *as, int src_r32_a, int src_r32_b);
+void asm_x86_test_r32_with_r32(asm_x86_t *as, int src_r32_a, int src_r32_b);
+void asm_x86_setcc_r8(asm_x86_t *as, mp_uint_t jcc_type, int dest_r8);
+void asm_x86_jmp_reg(asm_x86_t *as, int src_r86);
+void asm_x86_jmp_label(asm_x86_t *as, mp_uint_t label);
+void asm_x86_jcc_label(asm_x86_t *as, mp_uint_t jcc_type, mp_uint_t label);
+void asm_x86_entry(asm_x86_t *as, int num_locals);
+void asm_x86_exit(asm_x86_t *as);
+void asm_x86_mov_arg_to_r32(asm_x86_t *as, int src_arg_num, int dest_r32);
+void asm_x86_mov_local_to_r32(asm_x86_t *as, int src_local_num, int dest_r32);
+void asm_x86_mov_r32_to_local(asm_x86_t *as, int src_r32, int dest_local_num);
+void asm_x86_mov_local_addr_to_r32(asm_x86_t *as, int local_num, int dest_r32);
+void asm_x86_mov_reg_pcrel(asm_x86_t *as, int dest_r64, mp_uint_t label);
+void asm_x86_call_ind(asm_x86_t *as, size_t fun_id, mp_uint_t n_args, int temp_r32);
+
+// Holds a pointer to mp_fun_table
+#define ASM_X86_REG_FUN_TABLE ASM_X86_REG_EBP
+
+#if GENERIC_ASM_API
+
+// The following macros provide a (mostly) arch-independent API to
+// generate native code, and are used by the native emitter.
+
+#define ASM_WORD_SIZE (4)
+
+#define REG_RET ASM_X86_REG_EAX
+#define REG_ARG_1 ASM_X86_REG_ARG_1
+#define REG_ARG_2 ASM_X86_REG_ARG_2
+#define REG_ARG_3 ASM_X86_REG_ARG_3
+#define REG_ARG_4 ASM_X86_REG_ARG_4
+
+// caller-save, so can be used as temporaries
+#define REG_TEMP0 ASM_X86_REG_EAX
+#define REG_TEMP1 ASM_X86_REG_ECX
+#define REG_TEMP2 ASM_X86_REG_EDX
+
+// callee-save, so can be used as locals
+#define REG_LOCAL_1 ASM_X86_REG_EBX
+#define REG_LOCAL_2 ASM_X86_REG_ESI
+#define REG_LOCAL_3 ASM_X86_REG_EDI
+#define REG_LOCAL_NUM (3)
+
+// Holds a pointer to mp_fun_table
+#define REG_FUN_TABLE ASM_X86_REG_FUN_TABLE
+
+#define ASM_T                           asm_x86_t
+#define ASM_END_PASS                    asm_x86_end_pass
+#define ASM_ENTRY(as, num_locals, name) asm_x86_entry((as), (num_locals))
+#define ASM_EXIT                        asm_x86_exit
+
+#define ASM_JUMP                        asm_x86_jmp_label
+#define ASM_JUMP_IF_REG_ZERO(as, reg, label, bool_test) \
+    do { \
+        if (bool_test) { \
+            asm_x86_test_r8_with_r8(as, reg, reg); \
+        } else { \
+            asm_x86_test_r32_with_r32(as, reg, reg); \
+        } \
+        asm_x86_jcc_label(as, ASM_X86_CC_JZ, label); \
+    } while (0)
+#define ASM_JUMP_IF_REG_NONZERO(as, reg, label, bool_test) \
+    do { \
+        if (bool_test) { \
+            asm_x86_test_r8_with_r8(as, reg, reg); \
+        } else { \
+            asm_x86_test_r32_with_r32(as, reg, reg); \
+        } \
+        asm_x86_jcc_label(as, ASM_X86_CC_JNZ, label); \
+    } while (0)
+#define ASM_JUMP_IF_REG_EQ(as, reg1, reg2, label) \
+    do { \
+        asm_x86_cmp_r32_with_r32(as, reg1, reg2); \
+        asm_x86_jcc_label(as, ASM_X86_CC_JE, label); \
+    } while (0)
+#define ASM_JUMP_REG(as, reg) asm_x86_jmp_reg((as), (reg))
+#define ASM_CALL_IND(as, idx) asm_x86_call_ind(as, idx, mp_f_n_args[idx], ASM_X86_REG_EAX)
+
+#define ASM_MOV_LOCAL_REG(as, local_num, reg_src) asm_x86_mov_r32_to_local((as), (reg_src), (local_num))
+#define ASM_MOV_REG_IMM(as, reg_dest, imm) asm_x86_mov_i32_to_r32((as), (imm), (reg_dest))
+#define ASM_MOV_REG_LOCAL(as, reg_dest, local_num) asm_x86_mov_local_to_r32((as), (local_num), (reg_dest))
+#define ASM_MOV_REG_REG(as, reg_dest, reg_src) asm_x86_mov_r32_r32((as), (reg_dest), (reg_src))
+#define ASM_MOV_REG_LOCAL_ADDR(as, reg_dest, local_num) asm_x86_mov_local_addr_to_r32((as), (local_num), (reg_dest))
+#define ASM_MOV_REG_PCREL(as, reg_dest, label) asm_x86_mov_reg_pcrel((as), (reg_dest), (label))
+
+#define ASM_NOT_REG(as, reg) asm_x86_not_r32((as), (reg))
+#define ASM_NEG_REG(as, reg) asm_x86_neg_r32((as), (reg))
+#define ASM_LSL_REG(as, reg) asm_x86_shl_r32_cl((as), (reg))
+#define ASM_LSR_REG(as, reg) asm_x86_shr_r32_cl((as), (reg))
+#define ASM_ASR_REG(as, reg) asm_x86_sar_r32_cl((as), (reg))
+#define ASM_OR_REG_REG(as, reg_dest, reg_src) asm_x86_or_r32_r32((as), (reg_dest), (reg_src))
+#define ASM_XOR_REG_REG(as, reg_dest, reg_src) asm_x86_xor_r32_r32((as), (reg_dest), (reg_src))
+#define ASM_AND_REG_REG(as, reg_dest, reg_src) asm_x86_and_r32_r32((as), (reg_dest), (reg_src))
+#define ASM_ADD_REG_REG(as, reg_dest, reg_src) asm_x86_add_r32_r32((as), (reg_dest), (reg_src))
+#define ASM_SUB_REG_REG(as, reg_dest, reg_src) asm_x86_sub_r32_r32((as), (reg_dest), (reg_src))
+#define ASM_MUL_REG_REG(as, reg_dest, reg_src) asm_x86_mul_r32_r32((as), (reg_dest), (reg_src))
+
+#define ASM_LOAD_REG_REG_OFFSET(as, reg_dest, reg_base, dword_offset) ASM_LOAD32_REG_REG_OFFSET((as), (reg_dest), (reg_base), (dword_offset))
+#define ASM_LOAD8_REG_REG(as, reg_dest, reg_base) ASM_LOAD8_REG_REG_OFFSET((as), (reg_dest), (reg_base), 0)
+#define ASM_LOAD8_REG_REG_OFFSET(as, reg_dest, reg_base, byte_offset) asm_x86_mov_mem8_to_r32zx((as), (reg_base), (byte_offset), (reg_dest))
+#define ASM_LOAD16_REG_REG(as, reg_dest, reg_base) ASM_LOAD16_REG_REG_OFFSET((as), (reg_dest), (reg_base), 0)
+#define ASM_LOAD16_REG_REG_OFFSET(as, reg_dest, reg_base, word_offset) asm_x86_mov_mem16_to_r32zx((as), (reg_base), 2 * (word_offset), (reg_dest))
+#define ASM_LOAD32_REG_REG(as, reg_dest, reg_base) ASM_LOAD32_REG_REG_OFFSET((as), (reg_dest), (reg_base), 0)
+#define ASM_LOAD32_REG_REG_OFFSET(as, reg_dest, reg_base, dword_offset) asm_x86_mov_mem32_to_r32((as), (reg_base), 4 * (dword_offset), (reg_dest))
+
+#define ASM_STORE_REG_REG_OFFSET(as, reg_src, reg_base, dword_offset) ASM_STORE32_REG_REG_OFFSET((as), (reg_src), (reg_base), (dword_offset))
+#define ASM_STORE8_REG_REG(as, reg_src, reg_base) ASM_STORE8_REG_REG_OFFSET((as), (reg_src), (reg_base), 0)
+#define ASM_STORE8_REG_REG_OFFSET(as, reg_src, reg_base, byte_offset) asm_x86_mov_r8_to_mem8((as), (reg_src), (reg_base), (byte_offset))
+#define ASM_STORE16_REG_REG(as, reg_src, reg_base) ASM_STORE16_REG_REG_OFFSET((as), (reg_src), (reg_base), 0)
+#define ASM_STORE16_REG_REG_OFFSET(as, reg_src, reg_base, word_offset) asm_x86_mov_r16_to_mem16((as), (reg_src), (reg_base), 2 * (word_offset))
+#define ASM_STORE32_REG_REG(as, reg_src, reg_base) ASM_STORE32_REG_REG_OFFSET((as), (reg_src), (reg_base), 0)
+#define ASM_STORE32_REG_REG_OFFSET(as, reg_src, reg_base, dword_offset) asm_x86_mov_r32_to_mem32((as), (reg_src), (reg_base), 4 * (dword_offset))
+
+#endif // GENERIC_ASM_API
+
+#endif // MICROPY_INCLUDED_PY_ASMX86_H

APP_Framework/Framework/knowing/micropython/py/asmxtensa.c

@@ -0,0 +1,399 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2016 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <stdio.h>
+#include <assert.h>
+
+#include "py/runtime.h"
+
+// wrapper around everything in this file
+#if MICROPY_EMIT_XTENSA || MICROPY_EMIT_INLINE_XTENSA || MICROPY_EMIT_XTENSAWIN
+
+#include "py/asmxtensa.h"
+
+#if N_XTENSAWIN
+#define REG_TEMP ASM_XTENSA_REG_TEMPORARY_WIN
+#else
+#define REG_TEMP ASM_XTENSA_REG_TEMPORARY
+#endif
+
+#define WORD_SIZE (4)
+#define SIGNED_FIT6(x) ((((x) & 0xffffffe0) == 0) || (((x) & 0xffffffe0) == 0xffffffe0))
+#define SIGNED_FIT8(x) ((((x) & 0xffffff80) == 0) || (((x) & 0xffffff80) == 0xffffff80))
+#define SIGNED_FIT12(x) ((((x) & 0xfffff800) == 0) || (((x) & 0xfffff800) == 0xfffff800))
+#define SIGNED_FIT18(x) ((((x) & 0xfffe0000) == 0) || (((x) & 0xfffe0000) == 0xfffe0000))
+
+#define ET_OUT_OF_RANGE MP_ERROR_TEXT("ERROR: xtensa %q out of range")
+#define ET_NOT_ALIGNED MP_ERROR_TEXT("ERROR: %q %q not word-aligned")
+
+void asm_xtensa_end_pass(asm_xtensa_t *as) {
+    as->num_const = as->cur_const;
+    as->cur_const = 0;
+
+    #if 0
+    // make a hex dump of the machine code
+    if (as->base.pass == MP_ASM_PASS_EMIT) {
+        uint8_t *d = as->base.code_base;
+        printf("XTENSA ASM:");
+        for (size_t i = 0; i < ((as->base.code_size + 15) & ~15); ++i) {
+            if (i % 16 == 0) {
+                printf("\n%p:", &d[i]);
+            }
+            if (i % 2 == 0) {
+                printf(" ");
+            }
+            printf("%02x", d[i]);
+        }
+        printf("\n");
+    }
+    #endif
+}
+
+void asm_xtensa_entry(asm_xtensa_t *as, int num_locals) {
+    if (as->num_const > 0) {
+        // jump over the constants
+        asm_xtensa_op_j(as, as->num_const * WORD_SIZE + 4 - 4);
+        mp_asm_base_get_cur_to_write_bytes(&as->base, 1); // padding/alignment byte
+        as->const_table = (uint32_t *)mp_asm_base_get_cur_to_write_bytes(&as->base, as->num_const * 4);
+    }
+
+    // adjust the stack-pointer to store a0, a12, a13, a14, a15 and locals, 16-byte aligned
+    as->stack_adjust = (((ASM_XTENSA_NUM_REGS_SAVED + num_locals) * WORD_SIZE) + 15) & ~15;
+    if (SIGNED_FIT8(-as->stack_adjust)) {
+        asm_xtensa_op_addi(as, ASM_XTENSA_REG_A1, ASM_XTENSA_REG_A1, -as->stack_adjust);
+    } else {
+        asm_xtensa_op_movi(as, ASM_XTENSA_REG_A9, as->stack_adjust);
+        asm_xtensa_op_sub(as, ASM_XTENSA_REG_A1, ASM_XTENSA_REG_A1, ASM_XTENSA_REG_A9);
+    }
+
+    // save return value (a0) and callee-save registers (a12, a13, a14, a15)
+    asm_xtensa_op_s32i_n(as, ASM_XTENSA_REG_A0, ASM_XTENSA_REG_A1, 0);
+    for (int i = 1; i < ASM_XTENSA_NUM_REGS_SAVED; ++i) {
+        asm_xtensa_op_s32i_n(as, ASM_XTENSA_REG_A11 + i, ASM_XTENSA_REG_A1, i);
+    }
+}
+
+void asm_xtensa_exit(asm_xtensa_t *as) {
+    // restore registers
+    for (int i = ASM_XTENSA_NUM_REGS_SAVED - 1; i >= 1; --i) {
+        asm_xtensa_op_l32i_n(as, ASM_XTENSA_REG_A11 + i, ASM_XTENSA_REG_A1, i);
+    }
+    asm_xtensa_op_l32i_n(as, ASM_XTENSA_REG_A0, ASM_XTENSA_REG_A1, 0);
+
+    // restore stack-pointer and return
+    if (SIGNED_FIT8(as->stack_adjust)) {
+        asm_xtensa_op_addi(as, ASM_XTENSA_REG_A1, ASM_XTENSA_REG_A1, as->stack_adjust);
+    } else {
+        asm_xtensa_op_movi(as, ASM_XTENSA_REG_A9, as->stack_adjust);
+        asm_xtensa_op_add_n(as, ASM_XTENSA_REG_A1, ASM_XTENSA_REG_A1, ASM_XTENSA_REG_A9);
+    }
+
+    asm_xtensa_op_ret_n(as);
+}
+
+void asm_xtensa_entry_win(asm_xtensa_t *as, int num_locals) {
+    // jump over the constants
+    asm_xtensa_op_j(as, as->num_const * WORD_SIZE + 4 - 4);
+    mp_asm_base_get_cur_to_write_bytes(&as->base, 1); // padding/alignment byte
+    as->const_table = (uint32_t *)mp_asm_base_get_cur_to_write_bytes(&as->base, as->num_const * 4);
+
+    as->stack_adjust = 32 + ((((ASM_XTENSA_NUM_REGS_SAVED_WIN + num_locals) * WORD_SIZE) + 15) & ~15);
+    asm_xtensa_op_entry(as, ASM_XTENSA_REG_A1, as->stack_adjust);
+    asm_xtensa_op_s32i_n(as, ASM_XTENSA_REG_A0, ASM_XTENSA_REG_A1, 0);
+}
+
+void asm_xtensa_exit_win(asm_xtensa_t *as) {
+    asm_xtensa_op_l32i_n(as, ASM_XTENSA_REG_A0, ASM_XTENSA_REG_A1, 0);
+    asm_xtensa_op_retw_n(as);
+}
+
+static uint32_t get_label_dest(asm_xtensa_t *as, uint label) {
+    assert(label < as->base.max_num_labels);
+    return as->base.label_offsets[label];
+}
+
+void asm_xtensa_op16(asm_xtensa_t *as, uint16_t op) {
+    uint8_t *c = mp_asm_base_get_cur_to_write_bytes(&as->base, 2);
+    if (c != NULL) {
+        c[0] = op;
+        c[1] = op >> 8;
+    }
+}
+
+void asm_xtensa_op24(asm_xtensa_t *as, uint32_t op) {
+    uint8_t *c = mp_asm_base_get_cur_to_write_bytes(&as->base, 3);
+    if (c != NULL) {
+        c[0] = op;
+        c[1] = op >> 8;
+        c[2] = op >> 16;
+    }
+}
+
+void asm_xtensa_j_label(asm_xtensa_t *as, uint label) {
+    uint32_t dest = get_label_dest(as, label);
+    int32_t rel = dest - as->base.code_offset - 4;
+    // we assume rel, as a signed int, fits in 18-bits
+    asm_xtensa_op_j(as, rel);
+}
+
+static bool calculate_branch_displacement(asm_xtensa_t *as, uint label, ptrdiff_t *displacement) {
+    assert(displacement != NULL && "Displacement pointer is NULL");
+
+    uint32_t label_offset = get_label_dest(as, label);
+    *displacement = (ptrdiff_t)(label_offset - as->base.code_offset - 4);
+    return (label_offset != (uint32_t)-1) && (*displacement < 0);
+}
+
+void asm_xtensa_bccz_reg_label(asm_xtensa_t *as, uint cond, uint reg, uint label) {
+    ptrdiff_t rel = 0;
+    bool can_emit_short_jump = calculate_branch_displacement(as, label, &rel);
+
+    if (can_emit_short_jump && SIGNED_FIT12(rel)) {
+        // Backwards BCCZ opcodes with an offset that fits in 12 bits can
+        // be emitted without any change.
+        asm_xtensa_op_bccz(as, cond, reg, rel);
+        return;
+    }
+
+    // Range is effectively extended to 18 bits, as a more complex jump code
+    // sequence is emitted.
+    if (as->base.pass == MP_ASM_PASS_EMIT && !SIGNED_FIT18(rel - 6)) {
+        mp_raise_msg_varg(&mp_type_RuntimeError, ET_OUT_OF_RANGE, MP_QSTR_bccz);
+    }
+
+    // ~BCCZ skip ; +0  <- Condition is flipped here (EQ -> NE, etc.)
+    //  J    addr ; +3
+    // skip:      ; +6
+    asm_xtensa_op_bccz(as, cond ^ 1, reg, 6 - 4);
+    asm_xtensa_op_j(as, rel - 3);
+}
+
+void asm_xtensa_bcc_reg_reg_label(asm_xtensa_t *as, uint cond, uint reg1, uint reg2, uint label) {
+    ptrdiff_t rel = 0;
+    bool can_emit_short_jump = calculate_branch_displacement(as, label, &rel);
+
+    if (can_emit_short_jump && SIGNED_FIT8(rel)) {
+        // Backwards BCC opcodes with an offset that fits in 8 bits can
+        // be emitted without any change.
+        asm_xtensa_op_bcc(as, cond, reg1, reg2, rel);
+        return;
+    }
+
+    // Range is effectively extended to 18 bits, as a more complex jump code
+    // sequence is emitted.
+    if (as->base.pass == MP_ASM_PASS_EMIT && !SIGNED_FIT18(rel - 6)) {
+        mp_raise_msg_varg(&mp_type_RuntimeError, ET_OUT_OF_RANGE, MP_QSTR_bcc);
+    }
+
+    // ~BCC skip ; +0  <- Condition is flipped here (EQ -> NE, etc.)
+    //  J   addr ; +3
+    // skip:     ; +6
+    asm_xtensa_op_bcc(as, cond ^ 8, reg1, reg2, 6 - 4);
+    asm_xtensa_op_j(as, rel - 3);
+}
+
+// convenience function; reg_dest must be different from reg_src[12]
+void asm_xtensa_setcc_reg_reg_reg(asm_xtensa_t *as, uint cond, uint reg_dest, uint reg_src1, uint reg_src2) {
+    asm_xtensa_op_movi_n(as, reg_dest, 1);
+    asm_xtensa_op_bcc(as, cond, reg_src1, reg_src2, 1);
+    asm_xtensa_op_movi_n(as, reg_dest, 0);
+}
+
+size_t asm_xtensa_mov_reg_i32(asm_xtensa_t *as, uint reg_dest, uint32_t i32) {
+    // load the constant
+    uint32_t const_table_offset = (uint8_t *)as->const_table - as->base.code_base;
+    size_t loc = const_table_offset + as->cur_const * WORD_SIZE;
+    asm_xtensa_op_l32r(as, reg_dest, as->base.code_offset, loc);
+    // store the constant in the table
+    if (as->const_table != NULL) {
+        as->const_table[as->cur_const] = i32;
+    } else {
+        assert((as->base.pass != MP_ASM_PASS_EMIT) && "Constants table was not built.");
+    }
+    ++as->cur_const;
+    return loc;
+}
+
+void asm_xtensa_mov_reg_i32_optimised(asm_xtensa_t *as, uint reg_dest, uint32_t i32) {
+    if (-32 <= (int)i32 && (int)i32 <= 95) {
+        asm_xtensa_op_movi_n(as, reg_dest, i32);
+    } else if (SIGNED_FIT12(i32)) {
+        asm_xtensa_op_movi(as, reg_dest, i32);
+    } else {
+        asm_xtensa_mov_reg_i32(as, reg_dest, i32);
+    }
+}
+
+void asm_xtensa_mov_local_reg(asm_xtensa_t *as, int local_num, uint reg_src) {
+    asm_xtensa_op_s32i(as, reg_src, ASM_XTENSA_REG_A1, local_num);
+}
+
+void asm_xtensa_mov_reg_local(asm_xtensa_t *as, uint reg_dest, int local_num) {
+    asm_xtensa_op_l32i(as, reg_dest, ASM_XTENSA_REG_A1, local_num);
+}
+
+void asm_xtensa_mov_reg_local_addr(asm_xtensa_t *as, uint reg_dest, int local_num) {
+    uint off = local_num * WORD_SIZE;
+    if (SIGNED_FIT8(off)) {
+        asm_xtensa_op_addi(as, reg_dest, ASM_XTENSA_REG_A1, off);
+    } else {
+        asm_xtensa_op_movi(as, reg_dest, off);
+        asm_xtensa_op_add_n(as, reg_dest, reg_dest, ASM_XTENSA_REG_A1);
+    }
+}
+
+void asm_xtensa_mov_reg_pcrel(asm_xtensa_t *as, uint reg_dest, uint label) {
+    // Get relative offset from PC
+    uint32_t dest = get_label_dest(as, label);
+    int32_t rel = dest - as->base.code_offset;
+    rel -= 3 + 3; // account for 3 bytes of movi instruction, 3 bytes call0 adjustment
+    asm_xtensa_op_movi(as, reg_dest, rel); // imm has 12-bit range
+
+    // Use call0 to get PC+3 into a0
+    // call0 destination must be aligned on 4 bytes:
+    //  - code_offset&3=0: off=0, pad=1
+    //  - code_offset&3=1: off=0, pad=0
+    //  - code_offset&3=2: off=1, pad=3
+    //  - code_offset&3=3: off=1, pad=2
+    uint32_t off = as->base.code_offset >> 1 & 1;
+    uint32_t pad = (5 - as->base.code_offset) & 3;
+    asm_xtensa_op_call0(as, off);
+    mp_asm_base_get_cur_to_write_bytes(&as->base, pad);
+
+    // Add PC to relative offset
+    asm_xtensa_op_add_n(as, reg_dest, reg_dest, ASM_XTENSA_REG_A0);
+}
+
+void asm_xtensa_l32i_optimised(asm_xtensa_t *as, uint reg_dest, uint reg_base, uint word_offset) {
+    if (word_offset < 16) {
+        asm_xtensa_op_l32i_n(as, reg_dest, reg_base, word_offset);
+    } else if (word_offset < 256) {
+        asm_xtensa_op_l32i(as, reg_dest, reg_base, word_offset);
+    } else {
+        asm_xtensa_mov_reg_i32_optimised(as, reg_dest, word_offset * 4);
+        asm_xtensa_op_add_n(as, reg_dest, reg_base, reg_dest);
+        asm_xtensa_op_l32i_n(as, reg_dest, reg_dest, 0);
+    }
+}
+
+void asm_xtensa_load_reg_reg_offset(asm_xtensa_t *as, uint reg_dest, uint reg_base, uint offset, uint operation_size) {
+    assert(operation_size <= 2 && "Operation size value out of range.");
+
+    if (operation_size == 2 && MP_FIT_UNSIGNED(4, offset)) {
+        asm_xtensa_op_l32i_n(as, reg_dest, reg_base, offset);
+        return;
+    }
+
+    if (MP_FIT_UNSIGNED(8, offset)) {
+        asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRI8(2, operation_size, reg_base, reg_dest, offset));
+        return;
+    }
+
+    asm_xtensa_mov_reg_i32_optimised(as, reg_dest, offset << operation_size);
+    asm_xtensa_op_add_n(as, reg_dest, reg_base, reg_dest);
+    if (operation_size == 2) {
+        asm_xtensa_op_l32i_n(as, reg_dest, reg_dest, 0);
+    } else {
+        asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRI8(2, operation_size, reg_dest, reg_dest, 0));
+    }
+}
+
+void asm_xtensa_store_reg_reg_offset(asm_xtensa_t *as, uint reg_src, uint reg_base, uint offset, uint operation_size) {
+    assert(operation_size <= 2 && "Operation size value out of range.");
+
+    if (operation_size == 2 && MP_FIT_UNSIGNED(4, offset)) {
+        asm_xtensa_op_s32i_n(as, reg_src, reg_base, offset);
+        return;
+    }
+
+    if (MP_FIT_UNSIGNED(8, offset)) {
+        asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRI8(2, 0x04 | operation_size, reg_base, reg_src, offset));
+        return;
+    }
+
+    asm_xtensa_mov_reg_i32_optimised(as, REG_TEMP, offset << operation_size);
+    asm_xtensa_op_add_n(as, REG_TEMP, reg_base, REG_TEMP);
+    if (operation_size == 2) {
+        asm_xtensa_op_s32i_n(as, reg_src, REG_TEMP, 0);
+    } else {
+        asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRI8(2, 0x04 | operation_size, REG_TEMP, reg_src, 0));
+    }
+}
+
+void asm_xtensa_call_ind(asm_xtensa_t *as, uint idx) {
+    asm_xtensa_l32i_optimised(as, ASM_XTENSA_REG_A0, ASM_XTENSA_REG_FUN_TABLE, idx);
+    asm_xtensa_op_callx0(as, ASM_XTENSA_REG_A0);
+}
+
+void asm_xtensa_call_ind_win(asm_xtensa_t *as, uint idx) {
+    asm_xtensa_l32i_optimised(as, ASM_XTENSA_REG_A8, ASM_XTENSA_REG_FUN_TABLE_WIN, idx);
+    asm_xtensa_op_callx8(as, ASM_XTENSA_REG_A8);
+}
+
+void asm_xtensa_bit_branch(asm_xtensa_t *as, mp_uint_t reg, mp_uint_t bit, mp_uint_t label, mp_uint_t condition) {
+    uint32_t dest = get_label_dest(as, label);
+    int32_t rel = dest - as->base.code_offset - 4;
+    if (as->base.pass == MP_ASM_PASS_EMIT && !SIGNED_FIT8(rel)) {
+        mp_raise_msg_varg(&mp_type_RuntimeError, ET_OUT_OF_RANGE, MP_QSTR_bit_branch);
+    }
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRI8(7, condition | ((bit >> 4) & 0x01), reg, bit & 0x0F, rel & 0xFF));
+}
+
+void asm_xtensa_call0(asm_xtensa_t *as, mp_uint_t label) {
+    uint32_t dest = get_label_dest(as, label);
+    int32_t rel = dest - as->base.code_offset - 3;
+    if (as->base.pass == MP_ASM_PASS_EMIT) {
+        if ((dest & 0x03) != 0) {
+            mp_raise_msg_varg(&mp_type_RuntimeError, ET_NOT_ALIGNED, MP_QSTR_call0, MP_QSTR_target);
+        }
+        if ((rel & 0x03) != 0) {
+            mp_raise_msg_varg(&mp_type_RuntimeError, ET_NOT_ALIGNED, MP_QSTR_call0, MP_QSTR_location);
+        }
+        if (!SIGNED_FIT18(rel)) {
+            mp_raise_msg_varg(&mp_type_RuntimeError, ET_OUT_OF_RANGE, MP_QSTR_call0);
+        }
+    }
+    asm_xtensa_op_call0(as, rel);
+}
+
+void asm_xtensa_l32r(asm_xtensa_t *as, mp_uint_t reg, mp_uint_t label) {
+    uint32_t dest = get_label_dest(as, label);
+    int32_t rel = dest - as->base.code_offset;
+    if (as->base.pass == MP_ASM_PASS_EMIT) {
+        if ((dest & 0x03) != 0) {
+            mp_raise_msg_varg(&mp_type_RuntimeError, ET_NOT_ALIGNED, MP_QSTR_l32r, MP_QSTR_target);
+        }
+        if ((rel & 0x03) != 0) {
+            mp_raise_msg_varg(&mp_type_RuntimeError, ET_NOT_ALIGNED, MP_QSTR_l32r, MP_QSTR_location);
+        }
+        if (!SIGNED_FIT18(rel) || (rel >= 0)) {
+            mp_raise_msg_varg(&mp_type_RuntimeError, ET_OUT_OF_RANGE, MP_QSTR_l32r);
+        }
+    }
+    asm_xtensa_op_l32r(as, reg, as->base.code_offset, dest);
+}
+
+#endif // MICROPY_EMIT_XTENSA || MICROPY_EMIT_INLINE_XTENSA || MICROPY_EMIT_XTENSAWIN

APP_Framework/Framework/knowing/micropython/py/asmxtensa.h

@@ -0,0 +1,469 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2016 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifndef MICROPY_INCLUDED_PY_ASMXTENSA_H
+#define MICROPY_INCLUDED_PY_ASMXTENSA_H
+
+#include "py/misc.h"
+#include "py/asmbase.h"
+
+// calling conventions:
+// up to 6 args in a2-a7
+// return value in a2
+// PC stored in a0
+// stack pointer is a1, stack full descending, is aligned to 16 bytes
+// callee save: a1, a12, a13, a14, a15
+// caller save: a3
+
+// With windowed registers, size 8:
+// - a0: return PC
+// - a1: stack pointer, full descending, aligned to 16 bytes
+// - a2-a7: incoming args, and essentially callee save
+// - a2: return value
+// - a8-a15: caller save temporaries
+// - a10-a15: input args to called function
+// - a10: return value of called function
+// note: a0-a7 are saved automatically via window shift of called function
+
+#define ASM_XTENSA_REG_A0  (0)
+#define ASM_XTENSA_REG_A1  (1)
+#define ASM_XTENSA_REG_A2  (2)
+#define ASM_XTENSA_REG_A3  (3)
+#define ASM_XTENSA_REG_A4  (4)
+#define ASM_XTENSA_REG_A5  (5)
+#define ASM_XTENSA_REG_A6  (6)
+#define ASM_XTENSA_REG_A7  (7)
+#define ASM_XTENSA_REG_A8  (8)
+#define ASM_XTENSA_REG_A9  (9)
+#define ASM_XTENSA_REG_A10 (10)
+#define ASM_XTENSA_REG_A11 (11)
+#define ASM_XTENSA_REG_A12 (12)
+#define ASM_XTENSA_REG_A13 (13)
+#define ASM_XTENSA_REG_A14 (14)
+#define ASM_XTENSA_REG_A15 (15)
+
+// for bccz and bcci
+#define ASM_XTENSA_CCZ_EQ (0)
+#define ASM_XTENSA_CCZ_NE (1)
+#define ASM_XTENSA_CCZ_LT (2)
+#define ASM_XTENSA_CCZ_GE (3)
+
+// for bcc and setcc
+#define ASM_XTENSA_CC_NONE  (0)
+#define ASM_XTENSA_CC_EQ    (1)
+#define ASM_XTENSA_CC_LT    (2)
+#define ASM_XTENSA_CC_LTU   (3)
+#define ASM_XTENSA_CC_ALL   (4)
+#define ASM_XTENSA_CC_BC    (5)
+#define ASM_XTENSA_CC_ANY   (8)
+#define ASM_XTENSA_CC_NE    (9)
+#define ASM_XTENSA_CC_GE    (10)
+#define ASM_XTENSA_CC_GEU   (11)
+#define ASM_XTENSA_CC_NALL  (12)
+#define ASM_XTENSA_CC_BS    (13)
+
+// macros for encoding instructions (little endian versions)
+#define ASM_XTENSA_ENCODE_RRR(op0, op1, op2, r, s, t) \
+    ((((uint32_t)op2) << 20) | (((uint32_t)op1) << 16) | ((r) << 12) | ((s) << 8) | ((t) << 4) | (op0))
+#define ASM_XTENSA_ENCODE_RRI4(op0, op1, r, s, t, imm4) \
+    (((imm4) << 20) | ((op1) << 16) | ((r) << 12) | ((s) << 8) | ((t) << 4) | (op0))
+#define ASM_XTENSA_ENCODE_RRI8(op0, r, s, t, imm8) \
+    ((((uint32_t)imm8) << 16) | ((r) << 12) | ((s) << 8) | ((t) << 4) | (op0))
+#define ASM_XTENSA_ENCODE_RI16(op0, t, imm16) \
+    (((imm16) << 8) | ((t) << 4) | (op0))
+#define ASM_XTENSA_ENCODE_RSR(op0, op1, op2, rs, t) \
+    (((op2) << 20) | ((op1) << 16) | ((rs) << 8) | ((t) << 4) | (op0))
+#define ASM_XTENSA_ENCODE_CALL(op0, n, offset) \
+    (((offset) << 6) | ((n) << 4) | (op0))
+#define ASM_XTENSA_ENCODE_CALLX(op0, op1, op2, r, s, m, n) \
+    ((((uint32_t)op2) << 20) | (((uint32_t)op1) << 16) | ((r) << 12) | ((s) << 8) | ((m) << 6) | ((n) << 4) | (op0))
+#define ASM_XTENSA_ENCODE_BRI8(op0, r, s, m, n, imm8) \
+    (((imm8) << 16) | ((r) << 12) | ((s) << 8) | ((m) << 6) | ((n) << 4) | (op0))
+#define ASM_XTENSA_ENCODE_BRI12(op0, s, m, n, imm12) \
+    (((imm12) << 12) | ((s) << 8) | ((m) << 6) | ((n) << 4) | (op0))
+#define ASM_XTENSA_ENCODE_RRRN(op0, r, s, t) \
+    (((r) << 12) | ((s) << 8) | ((t) << 4) | (op0))
+#define ASM_XTENSA_ENCODE_RI7(op0, s, imm7) \
+    ((((imm7) & 0xf) << 12) | ((s) << 8) | ((imm7) & 0x70) | (op0))
+
+// Number of registers saved on the stack upon entry to function
+#define ASM_XTENSA_NUM_REGS_SAVED (5)
+#define ASM_XTENSA_NUM_REGS_SAVED_WIN (1)
+
+typedef struct _asm_xtensa_t {
+    mp_asm_base_t base;
+    uint32_t cur_const;
+    uint32_t num_const;
+    uint32_t *const_table;
+    uint32_t stack_adjust;
+} asm_xtensa_t;
+
+void asm_xtensa_end_pass(asm_xtensa_t *as);
+
+void asm_xtensa_entry(asm_xtensa_t *as, int num_locals);
+void asm_xtensa_exit(asm_xtensa_t *as);
+
+void asm_xtensa_entry_win(asm_xtensa_t *as, int num_locals);
+void asm_xtensa_exit_win(asm_xtensa_t *as);
+
+void asm_xtensa_op16(asm_xtensa_t *as, uint16_t op);
+void asm_xtensa_op24(asm_xtensa_t *as, uint32_t op);
+
+// raw instructions
+
+static inline void asm_xtensa_op_entry(asm_xtensa_t *as, uint reg_src, int32_t num_bytes) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_BRI12(6, reg_src, 0, 3, (num_bytes / 8) & 0xfff));
+}
+
+static inline void asm_xtensa_op_add_n(asm_xtensa_t *as, uint reg_dest, uint reg_src_a, uint reg_src_b) {
+    asm_xtensa_op16(as, ASM_XTENSA_ENCODE_RRRN(10, reg_dest, reg_src_a, reg_src_b));
+}
+
+static inline void asm_xtensa_op_addi(asm_xtensa_t *as, uint reg_dest, uint reg_src, int imm8) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRI8(2, 12, reg_src, reg_dest, imm8 & 0xff));
+}
+
+static inline void asm_xtensa_op_addx2(asm_xtensa_t *as, uint reg_dest, uint reg_src_a, uint reg_src_b) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRR(0, 0, 9, reg_dest, reg_src_a, reg_src_b));
+}
+
+static inline void asm_xtensa_op_addx4(asm_xtensa_t *as, uint reg_dest, uint reg_src_a, uint reg_src_b) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRR(0, 0, 10, reg_dest, reg_src_a, reg_src_b));
+}
+
+static inline void asm_xtensa_op_and(asm_xtensa_t *as, uint reg_dest, uint reg_src_a, uint reg_src_b) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRR(0, 0, 1, reg_dest, reg_src_a, reg_src_b));
+}
+
+static inline void asm_xtensa_op_bcc(asm_xtensa_t *as, uint cond, uint reg_src1, uint reg_src2, int32_t rel8) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRI8(7, cond, reg_src1, reg_src2, rel8 & 0xff));
+}
+
+static inline void asm_xtensa_op_bccz(asm_xtensa_t *as, uint cond, uint reg_src, int32_t rel12) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_BRI12(6, reg_src, cond, 1, rel12 & 0xfff));
+}
+
+static inline void asm_xtensa_op_call0(asm_xtensa_t *as, int32_t rel18) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_CALL(5, 0, rel18 & 0x3ffff));
+}
+
+static inline void asm_xtensa_op_callx0(asm_xtensa_t *as, uint reg) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_CALLX(0, 0, 0, 0, reg, 3, 0));
+}
+
+static inline void asm_xtensa_op_callx8(asm_xtensa_t *as, uint reg) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_CALLX(0, 0, 0, 0, reg, 3, 2));
+}
+
+static inline void asm_xtensa_op_j(asm_xtensa_t *as, int32_t rel18) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_CALL(6, 0, rel18 & 0x3ffff));
+}
+
+static inline void asm_xtensa_op_jx(asm_xtensa_t *as, uint reg) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_CALLX(0, 0, 0, 0, reg, 2, 2));
+}
+
+static inline void asm_xtensa_op_l8ui(asm_xtensa_t *as, uint reg_dest, uint reg_base, uint byte_offset) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRI8(2, 0, reg_base, reg_dest, byte_offset & 0xff));
+}
+
+static inline void asm_xtensa_op_l16ui(asm_xtensa_t *as, uint reg_dest, uint reg_base, uint half_word_offset) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRI8(2, 1, reg_base, reg_dest, half_word_offset & 0xff));
+}
+
+static inline void asm_xtensa_op_l32i(asm_xtensa_t *as, uint reg_dest, uint reg_base, uint word_offset) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRI8(2, 2, reg_base, reg_dest, word_offset & 0xff));
+}
+
+static inline void asm_xtensa_op_l32i_n(asm_xtensa_t *as, uint reg_dest, uint reg_base, uint word_offset) {
+    asm_xtensa_op16(as, ASM_XTENSA_ENCODE_RRRN(8, word_offset & 0xf, reg_base, reg_dest));
+}
+
+static inline void asm_xtensa_op_l32r(asm_xtensa_t *as, uint reg_dest, uint32_t op_off, uint32_t dest_off) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RI16(1, reg_dest, ((dest_off - ((op_off + 3) & ~3)) >> 2) & 0xffff));
+}
+
+static inline void asm_xtensa_op_mov_n(asm_xtensa_t *as, uint reg_dest, uint reg_src) {
+    asm_xtensa_op16(as, ASM_XTENSA_ENCODE_RRRN(13, 0, reg_src, reg_dest));
+}
+
+static inline void asm_xtensa_op_movi(asm_xtensa_t *as, uint reg_dest, int32_t imm12) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRI8(2, 10, (imm12 >> 8) & 0xf, reg_dest, imm12 & 0xff));
+}
+
+// Argument must be in the range (-32 .. 95) inclusive.
+static inline void asm_xtensa_op_movi_n(asm_xtensa_t *as, uint reg_dest, int imm7) {
+    asm_xtensa_op16(as, ASM_XTENSA_ENCODE_RI7(12, reg_dest, imm7));
+}
+
+static inline void asm_xtensa_op_mull(asm_xtensa_t *as, uint reg_dest, uint reg_src_a, uint reg_src_b) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRR(0, 2, 8, reg_dest, reg_src_a, reg_src_b));
+}
+
+static inline void asm_xtensa_op_neg(asm_xtensa_t *as, uint reg_dest, uint reg_src) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRR(0, 0, 6, reg_dest, 0, reg_src));
+}
+
+static inline void asm_xtensa_op_or(asm_xtensa_t *as, uint reg_dest, uint reg_src_a, uint reg_src_b) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRR(0, 0, 2, reg_dest, reg_src_a, reg_src_b));
+}
+
+static inline void asm_xtensa_op_ret_n(asm_xtensa_t *as) {
+    asm_xtensa_op16(as, ASM_XTENSA_ENCODE_RRRN(13, 15, 0, 0));
+}
+
+static inline void asm_xtensa_op_retw_n(asm_xtensa_t *as) {
+    asm_xtensa_op16(as, ASM_XTENSA_ENCODE_RRRN(13, 15, 0, 1));
+}
+
+static inline void asm_xtensa_op_s8i(asm_xtensa_t *as, uint reg_src, uint reg_base, uint byte_offset) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRI8(2, 4, reg_base, reg_src, byte_offset & 0xff));
+}
+
+static inline void asm_xtensa_op_s16i(asm_xtensa_t *as, uint reg_src, uint reg_base, uint half_word_offset) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRI8(2, 5, reg_base, reg_src, half_word_offset & 0xff));
+}
+
+static inline void asm_xtensa_op_s32i(asm_xtensa_t *as, uint reg_src, uint reg_base, uint word_offset) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRI8(2, 6, reg_base, reg_src, word_offset & 0xff));
+}
+
+static inline void asm_xtensa_op_s32i_n(asm_xtensa_t *as, uint reg_src, uint reg_base, uint word_offset) {
+    asm_xtensa_op16(as, ASM_XTENSA_ENCODE_RRRN(9, word_offset & 0xf, reg_base, reg_src));
+}
+
+static inline void asm_xtensa_op_sll(asm_xtensa_t *as, uint reg_dest, uint reg_src) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRR(0, 1, 10, reg_dest, reg_src, 0));
+}
+
+static inline void asm_xtensa_op_srl(asm_xtensa_t *as, uint reg_dest, uint reg_src) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRR(0, 1, 9, reg_dest, 0, reg_src));
+}
+
+static inline void asm_xtensa_op_sra(asm_xtensa_t *as, uint reg_dest, uint reg_src) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRR(0, 1, 11, reg_dest, 0, reg_src));
+}
+
+static inline void asm_xtensa_op_ssl(asm_xtensa_t *as, uint reg_src) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRR(0, 0, 4, 1, reg_src, 0));
+}
+
+static inline void asm_xtensa_op_ssr(asm_xtensa_t *as, uint reg_src) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRR(0, 0, 4, 0, reg_src, 0));
+}
+
+static inline void asm_xtensa_op_sub(asm_xtensa_t *as, uint reg_dest, uint reg_src_a, uint reg_src_b) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRR(0, 0, 12, reg_dest, reg_src_a, reg_src_b));
+}
+
+static inline void asm_xtensa_op_xor(asm_xtensa_t *as, uint reg_dest, uint reg_src_a, uint reg_src_b) {
+    asm_xtensa_op24(as, ASM_XTENSA_ENCODE_RRR(0, 0, 3, reg_dest, reg_src_a, reg_src_b));
+}
+
+// convenience functions
+void asm_xtensa_j_label(asm_xtensa_t *as, uint label);
+void asm_xtensa_bccz_reg_label(asm_xtensa_t *as, uint cond, uint reg, uint label);
+void asm_xtensa_bcc_reg_reg_label(asm_xtensa_t *as, uint cond, uint reg1, uint reg2, uint label);
+void asm_xtensa_setcc_reg_reg_reg(asm_xtensa_t *as, uint cond, uint reg_dest, uint reg_src1, uint reg_src2);
+size_t asm_xtensa_mov_reg_i32(asm_xtensa_t *as, uint reg_dest, uint32_t i32);
+void asm_xtensa_mov_reg_i32_optimised(asm_xtensa_t *as, uint reg_dest, uint32_t i32);
+void asm_xtensa_mov_local_reg(asm_xtensa_t *as, int local_num, uint reg_src);
+void asm_xtensa_mov_reg_local(asm_xtensa_t *as, uint reg_dest, int local_num);
+void asm_xtensa_mov_reg_local_addr(asm_xtensa_t *as, uint reg_dest, int local_num);
+void asm_xtensa_mov_reg_pcrel(asm_xtensa_t *as, uint reg_dest, uint label);
+void asm_xtensa_load_reg_reg_offset(asm_xtensa_t *as, uint reg_dest, uint reg_base, uint offset, uint operation_size);
+void asm_xtensa_store_reg_reg_offset(asm_xtensa_t *as, uint reg_src, uint reg_base, uint offset, uint operation_size);
+void asm_xtensa_call_ind(asm_xtensa_t *as, uint idx);
+void asm_xtensa_call_ind_win(asm_xtensa_t *as, uint idx);
+void asm_xtensa_bit_branch(asm_xtensa_t *as, mp_uint_t reg, mp_uint_t bit, mp_uint_t label, mp_uint_t condition);
+void asm_xtensa_immediate_branch(asm_xtensa_t *as, mp_uint_t reg, mp_uint_t immediate, mp_uint_t label, mp_uint_t cond);
+void asm_xtensa_call0(asm_xtensa_t *as, mp_uint_t label);
+void asm_xtensa_l32r(asm_xtensa_t *as, mp_uint_t reg, mp_uint_t label);
+
+// Holds a pointer to mp_fun_table
+#define ASM_XTENSA_REG_FUN_TABLE ASM_XTENSA_REG_A15
+#define ASM_XTENSA_REG_FUN_TABLE_WIN ASM_XTENSA_REG_A7
+
+// Internal temporary register (currently aliased to REG_ARG_5 for xtensa,
+// and to REG_TEMP2 for xtensawin).
+#define ASM_XTENSA_REG_TEMPORARY ASM_XTENSA_REG_A6
+#define ASM_XTENSA_REG_TEMPORARY_WIN ASM_XTENSA_REG_A12
+
+#if GENERIC_ASM_API
+
+// The following macros provide a (mostly) arch-independent API to
+// generate native code, and are used by the native emitter.
+
+#define ASM_WORD_SIZE (4)
+
+#if !GENERIC_ASM_API_WIN
+// Configuration for non-windowed calls
+
+#define REG_RET ASM_XTENSA_REG_A2
+#define REG_ARG_1 ASM_XTENSA_REG_A2
+#define REG_ARG_2 ASM_XTENSA_REG_A3
+#define REG_ARG_3 ASM_XTENSA_REG_A4
+#define REG_ARG_4 ASM_XTENSA_REG_A5
+#define REG_ARG_5 ASM_XTENSA_REG_A6
+
+#define REG_TEMP0 ASM_XTENSA_REG_A2
+#define REG_TEMP1 ASM_XTENSA_REG_A3
+#define REG_TEMP2 ASM_XTENSA_REG_A4
+
+#define REG_LOCAL_1 ASM_XTENSA_REG_A12
+#define REG_LOCAL_2 ASM_XTENSA_REG_A13
+#define REG_LOCAL_3 ASM_XTENSA_REG_A14
+#define REG_LOCAL_NUM (3)
+
+#define ASM_NUM_REGS_SAVED ASM_XTENSA_NUM_REGS_SAVED
+#define REG_FUN_TABLE ASM_XTENSA_REG_FUN_TABLE
+
+#define ASM_ENTRY(as, nlocal, name)   asm_xtensa_entry((as), (nlocal))
+#define ASM_EXIT(as)                  asm_xtensa_exit((as))
+#define ASM_CALL_IND(as, idx)         asm_xtensa_call_ind((as), (idx))
+
+#else
+// Configuration for windowed calls with window size 8
+
+#define REG_PARENT_RET ASM_XTENSA_REG_A2
+#define REG_PARENT_ARG_1 ASM_XTENSA_REG_A2
+#define REG_PARENT_ARG_2 ASM_XTENSA_REG_A3
+#define REG_PARENT_ARG_3 ASM_XTENSA_REG_A4
+#define REG_PARENT_ARG_4 ASM_XTENSA_REG_A5
+#define REG_RET ASM_XTENSA_REG_A10
+#define REG_ARG_1 ASM_XTENSA_REG_A10
+#define REG_ARG_2 ASM_XTENSA_REG_A11
+#define REG_ARG_3 ASM_XTENSA_REG_A12
+#define REG_ARG_4 ASM_XTENSA_REG_A13
+
+#define REG_TEMP0 ASM_XTENSA_REG_A10
+#define REG_TEMP1 ASM_XTENSA_REG_A11
+#define REG_TEMP2 ASM_XTENSA_REG_A12
+
+#define REG_LOCAL_1 ASM_XTENSA_REG_A4
+#define REG_LOCAL_2 ASM_XTENSA_REG_A5
+#define REG_LOCAL_3 ASM_XTENSA_REG_A6
+#define REG_LOCAL_NUM (3)
+
+#define ASM_NUM_REGS_SAVED ASM_XTENSA_NUM_REGS_SAVED_WIN
+#define REG_FUN_TABLE ASM_XTENSA_REG_FUN_TABLE_WIN
+
+#define ASM_ENTRY(as, nlocal, name)   asm_xtensa_entry_win((as), (nlocal))
+#define ASM_EXIT(as)                  asm_xtensa_exit_win((as))
+#define ASM_CALL_IND(as, idx)         asm_xtensa_call_ind_win((as), (idx))
+
+#endif
+
+#define ASM_T               asm_xtensa_t
+#define ASM_END_PASS        asm_xtensa_end_pass
+
+#define ASM_JUMP            asm_xtensa_j_label
+#define ASM_JUMP_IF_REG_ZERO(as, reg, label, bool_test) \
+    asm_xtensa_bccz_reg_label(as, ASM_XTENSA_CCZ_EQ, reg, label)
+#define ASM_JUMP_IF_REG_NONZERO(as, reg, label, bool_test) \
+    asm_xtensa_bccz_reg_label(as, ASM_XTENSA_CCZ_NE, reg, label)
+#define ASM_JUMP_IF_REG_EQ(as, reg1, reg2, label) \
+    asm_xtensa_bcc_reg_reg_label(as, ASM_XTENSA_CC_EQ, reg1, reg2, label)
+#define ASM_JUMP_REG(as, reg) asm_xtensa_op_jx((as), (reg))
+
+#define ASM_MOV_LOCAL_REG(as, local_num, reg_src) asm_xtensa_mov_local_reg((as), ASM_NUM_REGS_SAVED + (local_num), (reg_src))
+#define ASM_MOV_REG_IMM(as, reg_dest, imm) asm_xtensa_mov_reg_i32_optimised((as), (reg_dest), (imm))
+#define ASM_MOV_REG_LOCAL(as, reg_dest, local_num) asm_xtensa_mov_reg_local((as), (reg_dest), ASM_NUM_REGS_SAVED + (local_num))
+#define ASM_MOV_REG_REG(as, reg_dest, reg_src) asm_xtensa_op_mov_n((as), (reg_dest), (reg_src))
+#define ASM_MOV_REG_LOCAL_ADDR(as, reg_dest, local_num) asm_xtensa_mov_reg_local_addr((as), (reg_dest), ASM_NUM_REGS_SAVED + (local_num))
+#define ASM_MOV_REG_PCREL(as, reg_dest, label) asm_xtensa_mov_reg_pcrel((as), (reg_dest), (label))
+
+#define ASM_NEG_REG(as, reg_dest) asm_xtensa_op_neg((as), (reg_dest), (reg_dest))
+#define ASM_LSL_REG_REG(as, reg_dest, reg_shift) \
+    do { \
+        asm_xtensa_op_ssl((as), (reg_shift)); \
+        asm_xtensa_op_sll((as), (reg_dest), (reg_dest)); \
+    } while (0)
+#define ASM_LSR_REG_REG(as, reg_dest, reg_shift) \
+    do { \
+        asm_xtensa_op_ssr((as), (reg_shift)); \
+        asm_xtensa_op_srl((as), (reg_dest), (reg_dest)); \
+    } while (0)
+#define ASM_ASR_REG_REG(as, reg_dest, reg_shift) \
+    do { \
+        asm_xtensa_op_ssr((as), (reg_shift)); \
+        asm_xtensa_op_sra((as), (reg_dest), (reg_dest)); \
+    } while (0)
+#define ASM_OR_REG_REG(as, reg_dest, reg_src) asm_xtensa_op_or((as), (reg_dest), (reg_dest), (reg_src))
+#define ASM_XOR_REG_REG(as, reg_dest, reg_src) asm_xtensa_op_xor((as), (reg_dest), (reg_dest), (reg_src))
+#define ASM_AND_REG_REG(as, reg_dest, reg_src) asm_xtensa_op_and((as), (reg_dest), (reg_dest), (reg_src))
+#define ASM_ADD_REG_REG(as, reg_dest, reg_src) asm_xtensa_op_add_n((as), (reg_dest), (reg_dest), (reg_src))
+#define ASM_SUB_REG_REG(as, reg_dest, reg_src) asm_xtensa_op_sub((as), (reg_dest), (reg_dest), (reg_src))
+#define ASM_MUL_REG_REG(as, reg_dest, reg_src) asm_xtensa_op_mull((as), (reg_dest), (reg_dest), (reg_src))
+
+#define ASM_LOAD_REG_REG_OFFSET(as, reg_dest, reg_base, word_offset) ASM_LOAD32_REG_REG_OFFSET((as), (reg_dest), (reg_base), (word_offset))
+#define ASM_LOAD8_REG_REG(as, reg_dest, reg_base) ASM_LOAD8_REG_REG_OFFSET((as), (reg_dest), (reg_base), 0)
+#define ASM_LOAD8_REG_REG_OFFSET(as, reg_dest, reg_base, byte_offset) asm_xtensa_load_reg_reg_offset((as), (reg_dest), (reg_base), (byte_offset), 0)
+#define ASM_LOAD8_REG_REG_REG(as, reg_dest, reg_base, reg_index) \
+    do { \
+        asm_xtensa_op_add_n((as), (reg_base), (reg_index), (reg_base)); \
+        asm_xtensa_op_l8ui((as), (reg_dest), (reg_base), 0); \
+    } while (0)
+#define ASM_LOAD16_REG_REG(as, reg_dest, reg_base) ASM_LOAD16_REG_REG_OFFSET((as), (reg_dest), (reg_base), 0)
+#define ASM_LOAD16_REG_REG_OFFSET(as, reg_dest, reg_base, halfword_offset) asm_xtensa_load_reg_reg_offset((as), (reg_dest), (reg_base), (halfword_offset), 1)
+#define ASM_LOAD16_REG_REG_REG(as, reg_dest, reg_base, reg_index) \
+    do { \
+        asm_xtensa_op_addx2((as), (reg_base), (reg_index), (reg_base)); \
+        asm_xtensa_op_l16ui((as), (reg_dest), (reg_base), 0); \
+    } while (0)
+#define ASM_LOAD32_REG_REG(as, reg_dest, reg_base) ASM_LOAD32_REG_REG_OFFSET((as), (reg_dest), (reg_base), 0)
+#define ASM_LOAD32_REG_REG_OFFSET(as, reg_dest, reg_base, word_offset) asm_xtensa_load_reg_reg_offset((as), (reg_dest), (reg_base), (word_offset), 2)
+#define ASM_LOAD32_REG_REG_REG(as, reg_dest, reg_base, reg_index) \
+    do { \
+        asm_xtensa_op_addx4((as), (reg_base), (reg_index), (reg_base)); \
+        asm_xtensa_op_l32i_n((as), (reg_dest), (reg_base), 0); \
+    } while (0)
+
+#define ASM_STORE_REG_REG_OFFSET(as, reg_dest, reg_base, word_offset) ASM_STORE32_REG_REG_OFFSET((as), (reg_dest), (reg_base), (word_offset))
+#define ASM_STORE8_REG_REG(as, reg_src, reg_base) ASM_STORE8_REG_REG_OFFSET((as), (reg_src), (reg_base), 0)
+#define ASM_STORE8_REG_REG_OFFSET(as, reg_src, reg_base, byte_offset) asm_xtensa_store_reg_reg_offset((as), (reg_src), (reg_base), (byte_offset), 0)
+#define ASM_STORE8_REG_REG_REG(as, reg_val, reg_base, reg_index) \
+    do { \
+        asm_xtensa_op_add_n((as), (reg_base), (reg_index), (reg_base)); \
+        asm_xtensa_op_s8i((as), (reg_val), (reg_base), 0); \
+    } while (0)
+#define ASM_STORE16_REG_REG(as, reg_src, reg_base) ASM_STORE16_REG_REG_OFFSET((as), (reg_src), (reg_base), 0)
+#define ASM_STORE16_REG_REG_OFFSET(as, reg_src, reg_base, halfword_offset) asm_xtensa_store_reg_reg_offset((as), (reg_src), (reg_base), (halfword_offset), 1)
+#define ASM_STORE16_REG_REG_REG(as, reg_val, reg_base, reg_index) \
+    do { \
+        asm_xtensa_op_addx2((as), (reg_base), (reg_index), (reg_base)); \
+        asm_xtensa_op_s16i((as), (reg_val), (reg_base), 0); \
+    } while (0)
+#define ASM_STORE32_REG_REG(as, reg_src, reg_base) ASM_STORE32_REG_REG_OFFSET((as), (reg_src), (reg_base), 0)
+#define ASM_STORE32_REG_REG_OFFSET(as, reg_dest, reg_base, word_offset) asm_xtensa_store_reg_reg_offset((as), (reg_dest), (reg_base), (word_offset), 2)
+#define ASM_STORE32_REG_REG_REG(as, reg_val, reg_base, reg_index) \
+    do { \
+        asm_xtensa_op_addx4((as), (reg_base), (reg_index), (reg_base)); \
+        asm_xtensa_op_s32i_n((as), (reg_val), (reg_base), 0); \
+    } while (0)
+
+#endif // GENERIC_ASM_API
+
+#endif // MICROPY_INCLUDED_PY_ASMXTENSA_H

APP_Framework/Framework/knowing/micropython/py/bc.c

@@ -0,0 +1,345 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2014 Damien P. George
+ * Copyright (c) 2014 Paul Sokolovsky
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <stdbool.h>
+#include <string.h>
+#include <assert.h>
+
+#include "py/bc0.h"
+#include "py/bc.h"
+#include "py/objfun.h"
+
+#if MICROPY_DEBUG_VERBOSE // print debugging info
+#define DEBUG_PRINT (1)
+#else // don't print debugging info
+#define DEBUG_PRINT (0)
+#define DEBUG_printf(...) (void)0
+#endif
+
+void mp_encode_uint(void *env, mp_encode_uint_allocator_t allocator, mp_uint_t val) {
+    // We store each 7 bits in a separate byte, and that's how many bytes needed
+    byte buf[MP_ENCODE_UINT_MAX_BYTES];
+    byte *p = buf + sizeof(buf);
+    // We encode in little-ending order, but store in big-endian, to help decoding
+    do {
+        *--p = val & 0x7f;
+        val >>= 7;
+    } while (val != 0);
+    byte *c = allocator(env, buf + sizeof(buf) - p);
+    if (c != NULL) {
+        while (p != buf + sizeof(buf) - 1) {
+            *c++ = *p++ | 0x80;
+        }
+        *c = *p;
+    }
+}
+
+mp_uint_t mp_decode_uint(const byte **ptr) {
+    mp_uint_t unum = 0;
+    byte val;
+    const byte *p = *ptr;
+    do {
+        val = *p++;
+        unum = (unum << 7) | (val & 0x7f);
+    } while ((val & 0x80) != 0);
+    *ptr = p;
+    return unum;
+}
+
+// This function is used to help reduce stack usage at the caller, for the case when
+// the caller doesn't need to increase the ptr argument.  If ptr is a local variable
+// and the caller uses mp_decode_uint(&ptr) instead of this function, then the compiler
+// must allocate a slot on the stack for ptr, and this slot cannot be reused for
+// anything else in the function because the pointer may have been stored in a global
+// and reused later in the function.
+mp_uint_t mp_decode_uint_value(const byte *ptr) {
+    return mp_decode_uint(&ptr);
+}
+
+// This function is used to help reduce stack usage at the caller, for the case when
+// the caller doesn't need the actual value and just wants to skip over it.
+const byte *mp_decode_uint_skip(const byte *ptr) {
+    while ((*ptr++) & 0x80) {
+    }
+    return ptr;
+}
+
+static MP_NORETURN void fun_pos_args_mismatch(mp_obj_fun_bc_t *f, size_t expected, size_t given) {
+    #if MICROPY_ERROR_REPORTING <= MICROPY_ERROR_REPORTING_TERSE
+    // generic message, used also for other argument issues
+    (void)f;
+    (void)expected;
+    (void)given;
+    mp_arg_error_terse_mismatch();
+    #elif MICROPY_ERROR_REPORTING == MICROPY_ERROR_REPORTING_NORMAL
+    (void)f;
+    mp_raise_msg_varg(&mp_type_TypeError,
+        MP_ERROR_TEXT("function takes %d positional arguments but %d were given"), expected, given);
+    #elif MICROPY_ERROR_REPORTING == MICROPY_ERROR_REPORTING_DETAILED
+    mp_raise_msg_varg(&mp_type_TypeError,
+        MP_ERROR_TEXT("%q() takes %d positional arguments but %d were given"),
+        mp_obj_fun_get_name(MP_OBJ_FROM_PTR(f)), expected, given);
+    #endif
+}
+
+#if DEBUG_PRINT
+static void dump_args(const mp_obj_t *a, size_t sz) {
+    DEBUG_printf("%p: ", a);
+    for (size_t i = 0; i < sz; i++) {
+        DEBUG_printf("%p ", a[i]);
+    }
+    DEBUG_printf("\n");
+}
+#else
+#define dump_args(...) (void)0
+#endif
+
+// On entry code_state should be allocated somewhere (stack/heap) and
+// contain the following valid entries:
+//    - code_state->fun_bc should contain a pointer to the function object
+//    - code_state->ip should contain a pointer to the beginning of the prelude
+//    - code_state->sp should be: &code_state->state[0] - 1
+//    - code_state->n_state should be the number of objects in the local state
+static void mp_setup_code_state_helper(mp_code_state_t *code_state, size_t n_args, size_t n_kw, const mp_obj_t *args) {
+    // This function is pretty complicated.  It's main aim is to be efficient in speed and RAM
+    // usage for the common case of positional only args.
+
+    // get the function object that we want to set up (could be bytecode or native code)
+    mp_obj_fun_bc_t *self = code_state->fun_bc;
+
+    // Get cached n_state (rather than decode it again)
+    size_t n_state = code_state->n_state;
+
+    // Decode prelude
+    size_t n_state_unused, n_exc_stack_unused, scope_flags, n_pos_args, n_kwonly_args, n_def_pos_args;
+    MP_BC_PRELUDE_SIG_DECODE_INTO(code_state->ip, n_state_unused, n_exc_stack_unused, scope_flags, n_pos_args, n_kwonly_args, n_def_pos_args);
+    MP_BC_PRELUDE_SIZE_DECODE(code_state->ip);
+    (void)n_state_unused;
+    (void)n_exc_stack_unused;
+
+    mp_obj_t *code_state_state = code_state->sp + 1;
+    code_state->exc_sp_idx = 0;
+
+    // zero out the local stack to begin with
+    memset(code_state_state, 0, n_state * sizeof(*code_state->state));
+
+    const mp_obj_t *kwargs = args + n_args;
+
+    // var_pos_kw_args points to the stack where the var-args tuple, and var-kw dict, should go (if they are needed)
+    mp_obj_t *var_pos_kw_args = &code_state_state[n_state - 1 - n_pos_args - n_kwonly_args];
+
+    // check positional arguments
+
+    if (n_args > n_pos_args) {
+        // given more than enough arguments
+        if ((scope_flags & MP_SCOPE_FLAG_VARARGS) == 0) {
+            fun_pos_args_mismatch(self, n_pos_args, n_args);
+        }
+        // put extra arguments in varargs tuple
+        *var_pos_kw_args-- = mp_obj_new_tuple(n_args - n_pos_args, args + n_pos_args);
+        n_args = n_pos_args;
+    } else {
+        if ((scope_flags & MP_SCOPE_FLAG_VARARGS) != 0) {
+            DEBUG_printf("passing empty tuple as *args\n");
+            *var_pos_kw_args-- = mp_const_empty_tuple;
+        }
+        // Apply processing and check below only if we don't have kwargs,
+        // otherwise, kw handling code below has own extensive checks.
+        if (n_kw == 0 && (scope_flags & MP_SCOPE_FLAG_DEFKWARGS) == 0) {
+            if (n_args >= (size_t)(n_pos_args - n_def_pos_args)) {
+                // given enough arguments, but may need to use some default arguments
+                for (size_t i = n_args; i < n_pos_args; i++) {
+                    code_state_state[n_state - 1 - i] = self->extra_args[i - (n_pos_args - n_def_pos_args)];
+                }
+            } else {
+                fun_pos_args_mismatch(self, n_pos_args - n_def_pos_args, n_args);
+            }
+        }
+    }
+
+    // copy positional args into state
+    for (size_t i = 0; i < n_args; i++) {
+        code_state_state[n_state - 1 - i] = args[i];
+    }
+
+    // check keyword arguments
+
+    if (n_kw != 0 || (scope_flags & MP_SCOPE_FLAG_DEFKWARGS) != 0) {
+        DEBUG_printf("Initial args: ");
+        dump_args(code_state_state + n_state - n_pos_args - n_kwonly_args, n_pos_args + n_kwonly_args);
+
+        mp_obj_t dict = MP_OBJ_NULL;
+        if ((scope_flags & MP_SCOPE_FLAG_VARKEYWORDS) != 0) {
+            dict = mp_obj_new_dict(n_kw); // TODO: better go conservative with 0?
+            *var_pos_kw_args = dict;
+        }
+
+        for (size_t i = 0; i < n_kw; i++) {
+            // the keys in kwargs are expected to be qstr objects
+            mp_obj_t wanted_arg_name = kwargs[2 * i];
+
+            // get pointer to arg_names array
+            const uint8_t *arg_names = code_state->ip;
+            arg_names = mp_decode_uint_skip(arg_names);
+
+            for (size_t j = 0; j < n_pos_args + n_kwonly_args; j++) {
+                qstr arg_qstr = mp_decode_uint(&arg_names);
+                #if MICROPY_EMIT_BYTECODE_USES_QSTR_TABLE
+                arg_qstr = self->context->constants.qstr_table[arg_qstr];
+                #endif
+                if (wanted_arg_name == MP_OBJ_NEW_QSTR(arg_qstr)) {
+                    if (code_state_state[n_state - 1 - j] != MP_OBJ_NULL) {
+                    error_multiple:
+                        mp_raise_msg_varg(&mp_type_TypeError,
+                            MP_ERROR_TEXT("function got multiple values for argument '%q'"), MP_OBJ_QSTR_VALUE(wanted_arg_name));
+                    }
+                    code_state_state[n_state - 1 - j] = kwargs[2 * i + 1];
+                    goto continue2;
+                }
+            }
+            // Didn't find name match with positional args
+            if ((scope_flags & MP_SCOPE_FLAG_VARKEYWORDS) == 0) {
+                #if MICROPY_ERROR_REPORTING <= MICROPY_ERROR_REPORTING_TERSE
+                mp_raise_TypeError(MP_ERROR_TEXT("unexpected keyword argument"));
+                #else
+                mp_raise_msg_varg(&mp_type_TypeError,
+                    MP_ERROR_TEXT("unexpected keyword argument '%q'"), MP_OBJ_QSTR_VALUE(wanted_arg_name));
+                #endif
+            }
+            mp_map_elem_t *elem = mp_map_lookup(mp_obj_dict_get_map(dict), wanted_arg_name, MP_MAP_LOOKUP_ADD_IF_NOT_FOUND);
+            if (elem->value == MP_OBJ_NULL) {
+                elem->value = kwargs[2 * i + 1];
+            } else {
+                goto error_multiple;
+            }
+        continue2:;
+        }
+
+        DEBUG_printf("Args with kws flattened: ");
+        dump_args(code_state_state + n_state - n_pos_args - n_kwonly_args, n_pos_args + n_kwonly_args);
+
+        // fill in defaults for positional args
+        mp_obj_t *d = &code_state_state[n_state - n_pos_args];
+        mp_obj_t *s = &self->extra_args[n_def_pos_args - 1];
+        for (size_t i = n_def_pos_args; i > 0; i--, d++, s--) {
+            if (*d == MP_OBJ_NULL) {
+                *d = *s;
+            }
+        }
+
+        DEBUG_printf("Args after filling default positional: ");
+        dump_args(code_state_state + n_state - n_pos_args - n_kwonly_args, n_pos_args + n_kwonly_args);
+
+        // Check that all mandatory positional args are specified
+        while (d < &code_state_state[n_state]) {
+            if (*d++ == MP_OBJ_NULL) {
+                mp_raise_msg_varg(&mp_type_TypeError,
+                    MP_ERROR_TEXT("function missing required positional argument #%d"), &code_state_state[n_state] - d);
+            }
+        }
+
+        // Check that all mandatory keyword args are specified
+        // Fill in default kw args if we have them
+        const uint8_t *arg_names = mp_decode_uint_skip(code_state->ip);
+        for (size_t i = 0; i < n_pos_args; i++) {
+            arg_names = mp_decode_uint_skip(arg_names);
+        }
+        for (size_t i = 0; i < n_kwonly_args; i++) {
+            qstr arg_qstr = mp_decode_uint(&arg_names);
+            #if MICROPY_EMIT_BYTECODE_USES_QSTR_TABLE
+            arg_qstr = self->context->constants.qstr_table[arg_qstr];
+            #endif
+            if (code_state_state[n_state - 1 - n_pos_args - i] == MP_OBJ_NULL) {
+                mp_map_elem_t *elem = NULL;
+                if ((scope_flags & MP_SCOPE_FLAG_DEFKWARGS) != 0) {
+                    elem = mp_map_lookup(&((mp_obj_dict_t *)MP_OBJ_TO_PTR(self->extra_args[n_def_pos_args]))->map, MP_OBJ_NEW_QSTR(arg_qstr), MP_MAP_LOOKUP);
+                }
+                if (elem != NULL) {
+                    code_state_state[n_state - 1 - n_pos_args - i] = elem->value;
+                } else {
+                    mp_raise_msg_varg(&mp_type_TypeError,
+                        MP_ERROR_TEXT("function missing required keyword argument '%q'"), arg_qstr);
+                }
+            }
+        }
+
+    } else {
+        // no keyword arguments given
+        if (n_kwonly_args != 0) {
+            mp_raise_TypeError(MP_ERROR_TEXT("function missing keyword-only argument"));
+        }
+        if ((scope_flags & MP_SCOPE_FLAG_VARKEYWORDS) != 0) {
+            *var_pos_kw_args = mp_obj_new_dict(0);
+        }
+    }
+
+    // jump over code info (source file, argument names and line-number mapping)
+    const uint8_t *ip = code_state->ip + n_info;
+
+    // bytecode prelude: initialise closed over variables
+    for (; n_cell; --n_cell) {
+        size_t local_num = *ip++;
+        code_state_state[n_state - 1 - local_num] =
+            mp_obj_new_cell(code_state_state[n_state - 1 - local_num]);
+    }
+
+    // now that we skipped over the prelude, set the ip for the VM
+    code_state->ip = ip;
+
+    DEBUG_printf("Calling: n_pos_args=%d, n_kwonly_args=%d\n", n_pos_args, n_kwonly_args);
+    dump_args(code_state_state + n_state - n_pos_args - n_kwonly_args, n_pos_args + n_kwonly_args);
+    dump_args(code_state_state, n_state);
+}
+
+// On entry code_state should be allocated somewhere (stack/heap) and
+// contain the following valid entries:
+//    - code_state->fun_bc should contain a pointer to the function object
+//    - code_state->n_state should be the number of objects in the local state
+void mp_setup_code_state(mp_code_state_t *code_state, size_t n_args, size_t n_kw, const mp_obj_t *args) {
+    code_state->ip = code_state->fun_bc->bytecode;
+    code_state->sp = &code_state->state[0] - 1;
+    #if MICROPY_STACKLESS
+    code_state->prev = NULL;
+    #endif
+    #if MICROPY_PY_SYS_SETTRACE
+    code_state->prev_state = NULL;
+    code_state->frame = NULL;
+    #endif
+    mp_setup_code_state_helper(code_state, n_args, n_kw, args);
+}
+
+#if MICROPY_EMIT_NATIVE
+// On entry code_state should be allocated somewhere (stack/heap) and
+// contain the following valid entries:
+//    - code_state->fun_bc should contain a pointer to the function object
+//    - code_state->n_state should be the number of objects in the local state
+void mp_setup_code_state_native(mp_code_state_native_t *code_state, size_t n_args, size_t n_kw, const mp_obj_t *args) {
+    code_state->ip = mp_obj_fun_native_get_prelude_ptr(code_state->fun_bc);
+    code_state->sp = &code_state->state[0] - 1;
+    mp_setup_code_state_helper((mp_code_state_t *)code_state, n_args, n_kw, args);
+}
+#endif

APP_Framework/Framework/knowing/micropython/py/bc.h

@@ -0,0 +1,349 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2013, 2014 Damien P. George
+ * Copyright (c) 2014 Paul Sokolovsky
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifndef MICROPY_INCLUDED_PY_BC_H
+#define MICROPY_INCLUDED_PY_BC_H
+
+#include "py/runtime.h"
+
+// bytecode layout:
+//
+//  func signature  : var uint
+//      contains six values interleaved bit-wise as: xSSSSEAA [xFSSKAED repeated]
+//          x = extension           another byte follows
+//          S = n_state - 1         number of entries in Python value stack
+//          E = n_exc_stack         number of entries in exception stack
+//          F = scope_flags         four bits of flags, MP_SCOPE_FLAG_xxx
+//          A = n_pos_args          number of arguments this function takes
+//          K = n_kwonly_args       number of keyword-only arguments this function takes
+//          D = n_def_pos_args      number of default positional arguments
+//
+//  prelude size    : var uint
+//      contains two values interleaved bit-wise as: xIIIIIIC repeated
+//          x = extension           another byte follows
+//          I = n_info              number of bytes in source info section (always > 0)
+//          C = n_cells             number of bytes/cells in closure section
+//
+//  source info section:
+//      simple_name : var qstr      always exists
+//      argname0    : var qstr
+//      ...         : var qstr
+//      argnameN    : var qstr      N = num_pos_args + num_kwonly_args - 1
+//      <line number info>
+//
+//  closure section:
+//      local_num0  : byte
+//      ...         : byte
+//      local_numN  : byte          N = n_cells-1
+//
+//  <bytecode>
+//
+//
+// constant table layout:
+//
+//  const0          : obj
+//  constN          : obj
+
+#define MP_ENCODE_UINT_MAX_BYTES ((MP_BYTES_PER_OBJ_WORD * 8 + 6) / 7)
+
+#define MP_BC_PRELUDE_SIG_ENCODE(S, E, scope, out_byte, out_env) \
+    do {                                                            \
+        /*// Get values to store in prelude */                      \
+        size_t F = scope->scope_flags & MP_SCOPE_FLAG_ALL_SIG;      \
+        size_t A = scope->num_pos_args;                             \
+        size_t K = scope->num_kwonly_args;                          \
+        size_t D = scope->num_def_pos_args;                         \
+                                                                \
+        /* Adjust S to shrink range, to compress better */          \
+        S -= 1;                                                     \
+                                                                \
+        /* Encode prelude */                                        \
+        /* xSSSSEAA */                                              \
+        uint8_t z = (S & 0xf) << 3 | (E & 1) << 2 | (A & 3);        \
+        S >>= 4;                                                    \
+        E >>= 1;                                                    \
+        A >>= 2;                                                    \
+        while (S | E | F | A | K | D) {                             \
+            out_byte(out_env, 0x80 | z);                            \
+            /* xFSSKAED */                                          \
+            z = (F & 1) << 6 | (S & 3) << 4 | (K & 1) << 3          \
+                | (A & 1) << 2 | (E & 1) << 1 | (D & 1);            \
+            S >>= 2;                                                \
+            E >>= 1;                                                \
+            F >>= 1;                                                \
+            A >>= 1;                                                \
+            K >>= 1;                                                \
+            D >>= 1;                                                \
+        }                                                           \
+        out_byte(out_env, z);                                       \
+    } while (0)
+
+#define MP_BC_PRELUDE_SIG_DECODE_INTO(ip, S, E, F, A, K, D)     \
+    do {                                                            \
+        uint8_t z = *(ip)++;                                        \
+        /* xSSSSEAA */                                              \
+        S = (z >> 3) & 0xf;                                         \
+        E = (z >> 2) & 0x1;                                         \
+        F = 0;                                                      \
+        A = z & 0x3;                                                \
+        K = 0;                                                      \
+        D = 0;                                                      \
+        for (unsigned n = 0; z & 0x80; ++n) {                       \
+            z = *(ip)++;                                            \
+            /* xFSSKAED */                                          \
+            S |= (z & 0x30) << (2 * n);                             \
+            E |= (z & 0x02) << n;                                   \
+            F |= ((z & 0x40) >> 6) << n;                            \
+            A |= (z & 0x4) << n;                                    \
+            K |= ((z & 0x08) >> 3) << n;                            \
+            D |= (z & 0x1) << n;                                    \
+        }                                                           \
+        S += 1;                                                     \
+    } while (0)
+
+#define MP_BC_PRELUDE_SIG_DECODE(ip) \
+    size_t n_state, n_exc_stack, scope_flags, n_pos_args, n_kwonly_args, n_def_pos_args; \
+    MP_BC_PRELUDE_SIG_DECODE_INTO(ip, n_state, n_exc_stack, scope_flags, n_pos_args, n_kwonly_args, n_def_pos_args); \
+    (void)n_state; (void)n_exc_stack; (void)scope_flags; \
+    (void)n_pos_args; (void)n_kwonly_args; (void)n_def_pos_args
+
+#define MP_BC_PRELUDE_SIZE_ENCODE(I, C, out_byte, out_env)      \
+    do {                                                            \
+        /* Encode bit-wise as: xIIIIIIC */                          \
+        uint8_t z = 0;                                              \
+        do {                                                        \
+            z = (I & 0x3f) << 1 | (C & 1);                          \
+            C >>= 1;                                                \
+            I >>= 6;                                                \
+            if (C | I) {                                            \
+                z |= 0x80;                                          \
+            }                                                       \
+            out_byte(out_env, z);                                   \
+        } while (C | I);                                            \
+    } while (0)
+
+#define MP_BC_PRELUDE_SIZE_DECODE_INTO(ip, I, C)                \
+    do {                                                            \
+        uint8_t z;                                                  \
+        C = 0;                                                      \
+        I = 0;                                                      \
+        for (unsigned n = 0;; ++n) {                                \
+            z = *(ip)++;                                            \
+            /* xIIIIIIC */                                          \
+            C |= (z & 1) << n;                                      \
+            I |= ((z & 0x7e) >> 1) << (6 * n);                      \
+            if (!(z & 0x80)) {                                      \
+                break;                                              \
+            }                                                       \
+        }                                                           \
+    } while (0)
+
+#define MP_BC_PRELUDE_SIZE_DECODE(ip) \
+    size_t n_info, n_cell; \
+    MP_BC_PRELUDE_SIZE_DECODE_INTO(ip, n_info, n_cell); \
+    (void)n_info; (void)n_cell
+
+// Sentinel value for mp_code_state_t.exc_sp_idx
+#define MP_CODE_STATE_EXC_SP_IDX_SENTINEL ((uint16_t)-1)
+
+// To convert mp_code_state_t.exc_sp_idx to/from a pointer to mp_exc_stack_t
+#define MP_CODE_STATE_EXC_SP_IDX_FROM_PTR(exc_stack, exc_sp) ((exc_sp) + 1 - (exc_stack))
+#define MP_CODE_STATE_EXC_SP_IDX_TO_PTR(exc_stack, exc_sp_idx) ((exc_stack) + (exc_sp_idx) - 1)
+
+typedef struct _mp_bytecode_prelude_t {
+    uint n_state;
+    uint n_exc_stack;
+    uint scope_flags;
+    uint n_pos_args;
+    uint n_kwonly_args;
+    uint n_def_pos_args;
+    qstr qstr_block_name_idx;
+    const byte *line_info;
+    const byte *line_info_top;
+    const byte *opcodes;
+} mp_bytecode_prelude_t;
+
+// Exception stack entry
+typedef struct _mp_exc_stack_t {
+    const byte *handler;
+    // bit 0 is currently unused
+    // bit 1 is whether the opcode was SETUP_WITH or SETUP_FINALLY
+    mp_obj_t *val_sp;
+    // Saved exception
+    mp_obj_base_t *prev_exc;
+} mp_exc_stack_t;
+
+// Constants associated with a module, to interface bytecode with runtime.
+typedef struct _mp_module_constants_t {
+    #if MICROPY_EMIT_BYTECODE_USES_QSTR_TABLE
+    qstr_short_t *qstr_table;
+    #else
+    qstr source_file;
+    #endif
+    mp_obj_t *obj_table;
+} mp_module_constants_t;
+
+// State associated with a module.
+typedef struct _mp_module_context_t {
+    mp_obj_module_t module;
+    mp_module_constants_t constants;
+} mp_module_context_t;
+
+// Outer level struct defining a compiled module.
+typedef struct _mp_compiled_module_t {
+    mp_module_context_t *context;
+    const struct _mp_raw_code_t *rc;
+    #if MICROPY_PERSISTENT_CODE_SAVE
+    bool has_native;
+    size_t n_qstr;
+    size_t n_obj;
+    size_t arch_flags;
+    #endif
+} mp_compiled_module_t;
+
+// Outer level struct defining a frozen module.
+typedef struct _mp_frozen_module_t {
+    const mp_module_constants_t constants;
+    const void *proto_fun;
+} mp_frozen_module_t;
+
+// State for an executing function.
+typedef struct _mp_code_state_t {
+    // The fun_bc entry points to the underlying function object that is being executed.
+    // It is needed to access the start of bytecode and the const_table.
+    // It is also needed to prevent the GC from reclaiming the bytecode during execution,
+    // because the ip pointer below will always point to the interior of the bytecode.
+    struct _mp_obj_fun_bc_t *fun_bc;
+    const byte *ip;
+    mp_obj_t *sp;
+    uint16_t n_state;
+    uint16_t exc_sp_idx;
+    mp_obj_dict_t *old_globals;
+    #if MICROPY_STACKLESS
+    struct _mp_code_state_t *prev;
+    #endif
+    #if MICROPY_PY_SYS_SETTRACE
+    struct _mp_code_state_t *prev_state;
+    struct _mp_obj_frame_t *frame;
+    #endif
+    // Variable-length
+    mp_obj_t state[0];
+    // Variable-length, never accessed by name, only as (void*)(state + n_state)
+    // mp_exc_stack_t exc_state[0];
+} mp_code_state_t;
+
+// State for an executing native function (based on mp_code_state_t).
+typedef struct _mp_code_state_native_t {
+    struct _mp_obj_fun_bc_t *fun_bc;
+    const byte *ip;
+    mp_obj_t *sp;
+    uint16_t n_state;
+    uint16_t exc_sp_idx;
+    mp_obj_dict_t *old_globals;
+    mp_obj_t state[0];
+} mp_code_state_native_t;
+
+// Allocator may return NULL, in which case data is not stored (can be used to compute size).
+typedef uint8_t *(*mp_encode_uint_allocator_t)(void *env, size_t nbytes);
+
+void mp_encode_uint(void *env, mp_encode_uint_allocator_t allocator, mp_uint_t val);
+mp_uint_t mp_decode_uint(const byte **ptr);
+mp_uint_t mp_decode_uint_value(const byte *ptr);
+const byte *mp_decode_uint_skip(const byte *ptr);
+
+mp_vm_return_kind_t mp_execute_bytecode(mp_code_state_t *code_state,
+#ifndef __cplusplus
+    volatile
+#endif
+    mp_obj_t inject_exc);
+mp_code_state_t *mp_obj_fun_bc_prepare_codestate(mp_obj_t func, size_t n_args, size_t n_kw, const mp_obj_t *args);
+void mp_setup_code_state(mp_code_state_t *code_state, size_t n_args, size_t n_kw, const mp_obj_t *args);
+void mp_setup_code_state_native(mp_code_state_native_t *code_state, size_t n_args, size_t n_kw, const mp_obj_t *args);
+void mp_bytecode_print(const mp_print_t *print, const struct _mp_raw_code_t *rc, size_t fun_data_len, const mp_module_constants_t *cm);
+void mp_bytecode_print2(const mp_print_t *print, const byte *ip, size_t len, struct _mp_raw_code_t *const *child_table, const mp_module_constants_t *cm);
+const byte *mp_bytecode_print_str(const mp_print_t *print, const byte *ip_start, const byte *ip, struct _mp_raw_code_t *const *child_table, const mp_module_constants_t *cm);
+#define mp_bytecode_print_inst(print, code, x_table) mp_bytecode_print2(print, code, 1, x_table)
+
+// Helper macros to access pointer with least significant bits holding flags
+#define MP_TAGPTR_PTR(x) ((void *)((uintptr_t)(x) & ~((uintptr_t)3)))
+#define MP_TAGPTR_TAG0(x) ((uintptr_t)(x) & 1)
+#define MP_TAGPTR_TAG1(x) ((uintptr_t)(x) & 2)
+#define MP_TAGPTR_MAKE(ptr, tag) ((void *)((uintptr_t)(ptr) | (tag)))
+
+static inline void mp_module_context_alloc_tables(mp_module_context_t *context, size_t n_qstr, size_t n_obj) {
+    #if MICROPY_EMIT_BYTECODE_USES_QSTR_TABLE
+    size_t nq = (n_qstr * sizeof(qstr_short_t) + sizeof(mp_uint_t) - 1) / sizeof(mp_uint_t);
+    size_t no = n_obj;
+    mp_uint_t *mem = m_new(mp_uint_t, nq + no);
+    context->constants.qstr_table = (qstr_short_t *)mem;
+    context->constants.obj_table = (mp_obj_t *)(mem + nq);
+    #else
+    if (n_obj == 0) {
+        context->constants.obj_table = NULL;
+    } else {
+        context->constants.obj_table = m_new(mp_obj_t, n_obj);
+    }
+    #endif
+}
+
+typedef struct _mp_code_lineinfo_t {
+    size_t bc_increment;
+    size_t line_increment;
+} mp_code_lineinfo_t;
+
+static inline mp_code_lineinfo_t mp_bytecode_decode_lineinfo(const byte **line_info) {
+    mp_code_lineinfo_t result;
+    size_t c = (*line_info)[0];
+    if ((c & 0x80) == 0) {
+        // 0b0LLBBBBB encoding
+        result.bc_increment = c & 0x1f;
+        result.line_increment = c >> 5;
+        *line_info += 1;
+    } else {
+        // 0b1LLLBBBB 0bLLLLLLLL encoding (l's LSB in second byte)
+        result.bc_increment = c & 0xf;
+        result.line_increment = ((c << 4) & 0x700) | (*line_info)[1];
+        *line_info += 2;
+    }
+    return result;
+}
+
+static inline size_t mp_bytecode_get_source_line(const byte *line_info, const byte *line_info_top, size_t bc_offset) {
+    size_t source_line = 1;
+    while (line_info < line_info_top) {
+        mp_code_lineinfo_t decoded = mp_bytecode_decode_lineinfo(&line_info);
+        if (bc_offset >= decoded.bc_increment) {
+            bc_offset -= decoded.bc_increment;
+            source_line += decoded.line_increment;
+        } else {
+            // found source line corresponding to bytecode offset
+            break;
+        }
+    }
+    return source_line;
+}
+
+#endif // MICROPY_INCLUDED_PY_BC_H

APP_Framework/Framework/knowing/micropython/py/bc0.h

@@ -0,0 +1,162 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2013, 2014 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifndef MICROPY_INCLUDED_PY_BC0_H
+#define MICROPY_INCLUDED_PY_BC0_H
+
+// MicroPython bytecode opcodes, grouped based on the format of the opcode
+
+// All opcodes are encoded as a byte with an optional argument.  Arguments are
+// variable-length encoded so they can be as small as possible.  The possible
+// encodings for arguments are (ip[0] is the opcode):
+//
+//  - unsigned relative bytecode offset:
+//      - if ip[1] high bit is clear then: arg = ip[1]
+//      - if ip[1] high bit is set then:   arg = ip[1] & 0x7f | ip[2] << 7
+//
+//  - signed relative bytecode offset:
+//      - if ip[1] high bit is clear then: arg = ip[1] - 0x40
+//      - if ip[1] high bit is set then:   arg = (ip[1] & 0x7f | ip[2] << 7) - 0x4000
+
+#define MP_BC_MASK_FORMAT                   (0xf0)
+#define MP_BC_MASK_EXTRA_BYTE               (0x9e)
+
+#define MP_BC_FORMAT_BYTE                   (0)
+#define MP_BC_FORMAT_QSTR                   (1)
+#define MP_BC_FORMAT_VAR_UINT               (2)
+#define MP_BC_FORMAT_OFFSET                 (3)
+
+// Nibbles in magic number are: BB BB BB BB BB BO VV QU
+#define MP_BC_FORMAT(op) ((0x000003a4 >> (2 * ((op) >> 4))) & 3)
+
+// Load, Store, Delete, Import, Make, Build, Unpack, Call, Jump, Exception, For, sTack, Return, Yield, Op
+#define MP_BC_BASE_RESERVED                 (0x00) // ----------------
+#define MP_BC_BASE_QSTR_O                   (0x10) // LLLLLLSSSDDII---
+#define MP_BC_BASE_VINT_E                   (0x20) // MMLLLLSSDDBBBBBB
+#define MP_BC_BASE_VINT_O                   (0x30) // UUMMCCCC--------
+#define MP_BC_BASE_JUMP_E                   (0x40) // J-JJJJJEEEEF----
+#define MP_BC_BASE_BYTE_O                   (0x50) // LLLLSSDTTTTTEEFF
+#define MP_BC_BASE_BYTE_E                   (0x60) // --BREEEYYI------
+#define MP_BC_LOAD_CONST_SMALL_INT_MULTI    (0x70) // LLLLLLLLLLLLLLLL
+//                                          (0x80) // LLLLLLLLLLLLLLLL
+//                                          (0x90) // LLLLLLLLLLLLLLLL
+//                                          (0xa0) // LLLLLLLLLLLLLLLL
+#define MP_BC_LOAD_FAST_MULTI               (0xb0) // LLLLLLLLLLLLLLLL
+#define MP_BC_STORE_FAST_MULTI              (0xc0) // SSSSSSSSSSSSSSSS
+#define MP_BC_UNARY_OP_MULTI                (0xd0) // OOOOOOO
+#define MP_BC_BINARY_OP_MULTI               (0xd7) //        OOOOOOOOO
+//                                          (0xe0) // OOOOOOOOOOOOOOOO
+//                                          (0xf0) // OOOOOOOOOO------
+
+#define MP_BC_LOAD_CONST_SMALL_INT_MULTI_NUM (64)
+#define MP_BC_LOAD_CONST_SMALL_INT_MULTI_EXCESS (16)
+#define MP_BC_LOAD_FAST_MULTI_NUM           (16)
+#define MP_BC_STORE_FAST_MULTI_NUM          (16)
+#define MP_BC_UNARY_OP_MULTI_NUM            (MP_UNARY_OP_NUM_BYTECODE)
+#define MP_BC_BINARY_OP_MULTI_NUM           (MP_BINARY_OP_NUM_BYTECODE)
+
+#define MP_BC_LOAD_CONST_FALSE              (MP_BC_BASE_BYTE_O + 0x00)
+#define MP_BC_LOAD_CONST_NONE               (MP_BC_BASE_BYTE_O + 0x01)
+#define MP_BC_LOAD_CONST_TRUE               (MP_BC_BASE_BYTE_O + 0x02)
+#define MP_BC_LOAD_CONST_SMALL_INT          (MP_BC_BASE_VINT_E + 0x02) // signed var-int
+#define MP_BC_LOAD_CONST_STRING             (MP_BC_BASE_QSTR_O + 0x00) // qstr
+#define MP_BC_LOAD_CONST_OBJ                (MP_BC_BASE_VINT_E + 0x03) // ptr
+#define MP_BC_LOAD_NULL                     (MP_BC_BASE_BYTE_O + 0x03)
+
+#define MP_BC_LOAD_FAST_N                   (MP_BC_BASE_VINT_E + 0x04) // uint
+#define MP_BC_LOAD_DEREF                    (MP_BC_BASE_VINT_E + 0x05) // uint
+#define MP_BC_LOAD_NAME                     (MP_BC_BASE_QSTR_O + 0x01) // qstr
+#define MP_BC_LOAD_GLOBAL                   (MP_BC_BASE_QSTR_O + 0x02) // qstr
+#define MP_BC_LOAD_ATTR                     (MP_BC_BASE_QSTR_O + 0x03) // qstr
+#define MP_BC_LOAD_METHOD                   (MP_BC_BASE_QSTR_O + 0x04) // qstr
+#define MP_BC_LOAD_SUPER_METHOD             (MP_BC_BASE_QSTR_O + 0x05) // qstr
+#define MP_BC_LOAD_BUILD_CLASS              (MP_BC_BASE_BYTE_O + 0x04)
+#define MP_BC_LOAD_SUBSCR                   (MP_BC_BASE_BYTE_O + 0x05)
+
+#define MP_BC_STORE_FAST_N                  (MP_BC_BASE_VINT_E + 0x06) // uint
+#define MP_BC_STORE_DEREF                   (MP_BC_BASE_VINT_E + 0x07) // uint
+#define MP_BC_STORE_NAME                    (MP_BC_BASE_QSTR_O + 0x06) // qstr
+#define MP_BC_STORE_GLOBAL                  (MP_BC_BASE_QSTR_O + 0x07) // qstr
+#define MP_BC_STORE_ATTR                    (MP_BC_BASE_QSTR_O + 0x08) // qstr
+#define MP_BC_STORE_SUBSCR                  (MP_BC_BASE_BYTE_O + 0x06)
+
+#define MP_BC_DELETE_FAST                   (MP_BC_BASE_VINT_E + 0x08) // uint
+#define MP_BC_DELETE_DEREF                  (MP_BC_BASE_VINT_E + 0x09) // uint
+#define MP_BC_DELETE_NAME                   (MP_BC_BASE_QSTR_O + 0x09) // qstr
+#define MP_BC_DELETE_GLOBAL                 (MP_BC_BASE_QSTR_O + 0x0a) // qstr
+
+#define MP_BC_DUP_TOP                       (MP_BC_BASE_BYTE_O + 0x07)
+#define MP_BC_DUP_TOP_TWO                   (MP_BC_BASE_BYTE_O + 0x08)
+#define MP_BC_POP_TOP                       (MP_BC_BASE_BYTE_O + 0x09)
+#define MP_BC_ROT_TWO                       (MP_BC_BASE_BYTE_O + 0x0a)
+#define MP_BC_ROT_THREE                     (MP_BC_BASE_BYTE_O + 0x0b)
+
+#define MP_BC_UNWIND_JUMP                   (MP_BC_BASE_JUMP_E + 0x00) // signed relative bytecode offset; then a byte
+#define MP_BC_JUMP                          (MP_BC_BASE_JUMP_E + 0x02) // signed relative bytecode offset
+#define MP_BC_POP_JUMP_IF_TRUE              (MP_BC_BASE_JUMP_E + 0x03) // signed relative bytecode offset
+#define MP_BC_POP_JUMP_IF_FALSE             (MP_BC_BASE_JUMP_E + 0x04) // signed relative bytecode offset
+#define MP_BC_JUMP_IF_TRUE_OR_POP           (MP_BC_BASE_JUMP_E + 0x05) // unsigned relative bytecode offset
+#define MP_BC_JUMP_IF_FALSE_OR_POP          (MP_BC_BASE_JUMP_E + 0x06) // unsigned relative bytecode offset
+#define MP_BC_SETUP_WITH                    (MP_BC_BASE_JUMP_E + 0x07) // unsigned relative bytecode offset
+#define MP_BC_SETUP_EXCEPT                  (MP_BC_BASE_JUMP_E + 0x08) // unsigned relative bytecode offset
+#define MP_BC_SETUP_FINALLY                 (MP_BC_BASE_JUMP_E + 0x09) // unsigned relative bytecode offset
+#define MP_BC_POP_EXCEPT_JUMP               (MP_BC_BASE_JUMP_E + 0x0a) // unsigned relative bytecode offset
+#define MP_BC_FOR_ITER                      (MP_BC_BASE_JUMP_E + 0x0b) // unsigned relative bytecode offset
+#define MP_BC_WITH_CLEANUP                  (MP_BC_BASE_BYTE_O + 0x0c)
+#define MP_BC_END_FINALLY                   (MP_BC_BASE_BYTE_O + 0x0d)
+#define MP_BC_GET_ITER                      (MP_BC_BASE_BYTE_O + 0x0e)
+#define MP_BC_GET_ITER_STACK                (MP_BC_BASE_BYTE_O + 0x0f)
+
+#define MP_BC_BUILD_TUPLE                   (MP_BC_BASE_VINT_E + 0x0a) // uint
+#define MP_BC_BUILD_LIST                    (MP_BC_BASE_VINT_E + 0x0b) // uint
+#define MP_BC_BUILD_MAP                     (MP_BC_BASE_VINT_E + 0x0c) // uint
+#define MP_BC_STORE_MAP                     (MP_BC_BASE_BYTE_E + 0x02)
+#define MP_BC_BUILD_SET                     (MP_BC_BASE_VINT_E + 0x0d) // uint
+#define MP_BC_BUILD_SLICE                   (MP_BC_BASE_VINT_E + 0x0e) // uint
+#define MP_BC_STORE_COMP                    (MP_BC_BASE_VINT_E + 0x0f) // uint
+#define MP_BC_UNPACK_SEQUENCE               (MP_BC_BASE_VINT_O + 0x00) // uint
+#define MP_BC_UNPACK_EX                     (MP_BC_BASE_VINT_O + 0x01) // uint
+
+#define MP_BC_RETURN_VALUE                  (MP_BC_BASE_BYTE_E + 0x03)
+#define MP_BC_RAISE_LAST                    (MP_BC_BASE_BYTE_E + 0x04)
+#define MP_BC_RAISE_OBJ                     (MP_BC_BASE_BYTE_E + 0x05)
+#define MP_BC_RAISE_FROM                    (MP_BC_BASE_BYTE_E + 0x06)
+#define MP_BC_YIELD_VALUE                   (MP_BC_BASE_BYTE_E + 0x07)
+#define MP_BC_YIELD_FROM                    (MP_BC_BASE_BYTE_E + 0x08)
+
+#define MP_BC_MAKE_FUNCTION                 (MP_BC_BASE_VINT_O + 0x02) // uint
+#define MP_BC_MAKE_FUNCTION_DEFARGS         (MP_BC_BASE_VINT_O + 0x03) // uint
+#define MP_BC_MAKE_CLOSURE                  (MP_BC_BASE_VINT_E + 0x00) // uint; extra byte
+#define MP_BC_MAKE_CLOSURE_DEFARGS          (MP_BC_BASE_VINT_E + 0x01) // uint; extra byte
+#define MP_BC_CALL_FUNCTION                 (MP_BC_BASE_VINT_O + 0x04) // uint
+#define MP_BC_CALL_FUNCTION_VAR_KW          (MP_BC_BASE_VINT_O + 0x05) // uint
+#define MP_BC_CALL_METHOD                   (MP_BC_BASE_VINT_O + 0x06) // uint
+#define MP_BC_CALL_METHOD_VAR_KW            (MP_BC_BASE_VINT_O + 0x07) // uint
+
+#define MP_BC_IMPORT_NAME                   (MP_BC_BASE_QSTR_O + 0x0b) // qstr
+#define MP_BC_IMPORT_FROM                   (MP_BC_BASE_QSTR_O + 0x0c) // qstr
+#define MP_BC_IMPORT_STAR                   (MP_BC_BASE_BYTE_E + 0x09)
+
+#endif // MICROPY_INCLUDED_PY_BC0_H

APP_Framework/Framework/knowing/micropython/py/binary.c

@@ -0,0 +1,554 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2014-2017 Paul Sokolovsky
+ * Copyright (c) 2014-2019 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <stddef.h>
+#include <string.h>
+#include <assert.h>
+
+#include "py/binary.h"
+#include "py/smallint.h"
+#include "py/objint.h"
+#include "py/runtime.h"
+
+// Helpers to work with binary-encoded data
+
+#ifndef alignof
+#define alignof(type) offsetof(struct { char c; type t; }, t)
+#endif
+
+size_t mp_binary_get_size(char struct_type, char val_type, size_t *palign) {
+    size_t size = 0;
+    int align = 1;
+    switch (struct_type) {
+        case '<':
+        case '>':
+            switch (val_type) {
+                case 'b':
+                case 'B':
+                    size = 1;
+                    break;
+                case 'h':
+                case 'H':
+                    size = 2;
+                    break;
+                case 'i':
+                case 'I':
+                    size = 4;
+                    break;
+                case 'l':
+                case 'L':
+                    size = 4;
+                    break;
+                case 'q':
+                case 'Q':
+                    size = 8;
+                    break;
+                #if MICROPY_PY_STRUCT_UNSAFE_TYPECODES
+                case 'P':
+                case 'O':
+                case 'S':
+                    size = sizeof(void *);
+                    break;
+                #endif
+                case 'e':
+                    size = 2;
+                    break;
+                case 'f':
+                    size = 4;
+                    break;
+                case 'd':
+                    size = 8;
+                    break;
+            }
+            break;
+        case '@': {
+            // TODO:
+            // The simplest heuristic for alignment is to align by value
+            // size, but that doesn't work for "bigger than int" types,
+            // for example, long long may very well have long alignment
+            // So, we introduce separate alignment handling, but having
+            // formal support for that is different from actually supporting
+            // particular (or any) ABI.
+            switch (val_type) {
+                case BYTEARRAY_TYPECODE:
+                case 'b':
+                case 'B':
+                    align = size = 1;
+                    break;
+                case 'h':
+                case 'H':
+                    align = alignof(short);
+                    size = sizeof(short);
+                    break;
+                case 'i':
+                case 'I':
+                    align = alignof(int);
+                    size = sizeof(int);
+                    break;
+                case 'l':
+                case 'L':
+                    align = alignof(long);
+                    size = sizeof(long);
+                    break;
+                case 'q':
+                case 'Q':
+                    align = alignof(long long);
+                    size = sizeof(long long);
+                    break;
+                #if MICROPY_PY_STRUCT_UNSAFE_TYPECODES
+                case 'P':
+                case 'O':
+                case 'S':
+                    align = alignof(void *);
+                    size = sizeof(void *);
+                    break;
+                #endif
+                case 'e':
+                    align = 2;
+                    size = 2;
+                    break;
+                case 'f':
+                    align = alignof(float);
+                    size = sizeof(float);
+                    break;
+                case 'd':
+                    align = alignof(double);
+                    size = sizeof(double);
+                    break;
+            }
+        }
+    }
+
+    if (size == 0) {
+        mp_raise_ValueError(MP_ERROR_TEXT("bad typecode"));
+    }
+
+    if (palign != NULL) {
+        *palign = align;
+    }
+    return size;
+}
+
+#if MICROPY_PY_BUILTINS_FLOAT && MICROPY_FLOAT_USE_NATIVE_FLT16
+
+static inline float mp_decode_half_float(uint16_t hf) {
+    union {
+        uint16_t i;
+        _Float16 f;
+    } fpu = { .i = hf };
+    return fpu.f;
+}
+
+static inline uint16_t mp_encode_half_float(float x) {
+    union {
+        uint16_t i;
+        _Float16 f;
+    } fp_sp = { .f = (_Float16)x };
+    return fp_sp.i;
+}
+
+#elif MICROPY_PY_BUILTINS_FLOAT
+
+static float mp_decode_half_float(uint16_t hf) {
+    union {
+        uint32_t i;
+        float f;
+    } fpu;
+
+    uint16_t m = hf & 0x3ff;
+    int e = (hf >> 10) & 0x1f;
+    if (e == 0x1f) {
+        // Half-float is infinity.
+        e = 0xff;
+    } else if (e) {
+        // Half-float is normal.
+        e += 127 - 15;
+    } else if (m) {
+        // Half-float is subnormal, make it normal.
+        e = 127 - 15;
+        while (!(m & 0x400)) {
+            m <<= 1;
+            --e;
+        }
+        m -= 0x400;
+        ++e;
+    }
+
+    fpu.i = ((hf & 0x8000u) << 16) | (e << 23) | (m << 13);
+    return fpu.f;
+}
+
+static uint16_t mp_encode_half_float(float x) {
+    union {
+        uint32_t i;
+        float f;
+    } fpu = { .f = x };
+
+    uint16_t m = (fpu.i >> 13) & 0x3ff;
+    if (fpu.i & (1 << 12)) {
+        // Round up.
+        ++m;
+    }
+    int e = (fpu.i >> 23) & 0xff;
+
+    if (e == 0xff) {
+        // Infinity.
+        e = 0x1f;
+    } else if (e != 0) {
+        e -= 127 - 15;
+        if (e < 0) {
+            // Underflow: denormalized, or zero.
+            if (e >= -11) {
+                m = (m | 0x400) >> -e;
+                if (m & 1) {
+                    m = (m >> 1) + 1;
+                } else {
+                    m >>= 1;
+                }
+            } else {
+                m = 0;
+            }
+            e = 0;
+        } else if (e > 0x3f) {
+            // Overflow: infinity.
+            e = 0x1f;
+            m = 0;
+        }
+    }
+
+    uint16_t bits = ((fpu.i >> 16) & 0x8000) | (e << 10) | m;
+    return bits;
+}
+
+#endif
+
+mp_obj_t mp_binary_get_val_array(char typecode, void *p, size_t index) {
+    mp_int_t val = 0;
+    switch (typecode) {
+        case 'b':
+            val = ((signed char *)p)[index];
+            break;
+        case BYTEARRAY_TYPECODE:
+        case 'B':
+            val = ((unsigned char *)p)[index];
+            break;
+        case 'h':
+            val = ((short *)p)[index];
+            break;
+        case 'H':
+            val = ((unsigned short *)p)[index];
+            break;
+        case 'i':
+            return mp_obj_new_int(((int *)p)[index]);
+        case 'I':
+            return mp_obj_new_int_from_uint(((unsigned int *)p)[index]);
+        case 'l':
+            return mp_obj_new_int(((long *)p)[index]);
+        case 'L':
+            return mp_obj_new_int_from_uint(((unsigned long *)p)[index]);
+        #if MICROPY_LONGINT_IMPL != MICROPY_LONGINT_IMPL_NONE
+        case 'q':
+            return mp_obj_new_int_from_ll(((long long *)p)[index]);
+        case 'Q':
+            return mp_obj_new_int_from_ull(((unsigned long long *)p)[index]);
+        #endif
+        #if MICROPY_PY_BUILTINS_FLOAT
+        case 'f':
+            return mp_obj_new_float_from_f(((float *)p)[index]);
+        case 'd':
+            return mp_obj_new_float_from_d(((double *)p)[index]);
+        #endif
+            // Extension to CPython: array of objects
+        #if MICROPY_PY_STRUCT_UNSAFE_TYPECODES
+        case 'O':
+            return ((mp_obj_t *)p)[index];
+        // Extension to CPython: array of pointers
+        case 'P':
+            return mp_obj_new_int((mp_int_t)(uintptr_t)((void **)p)[index]);
+        #endif
+    }
+    return MP_OBJ_NEW_SMALL_INT(val);
+}
+
+// The long long type is guaranteed to hold at least 64 bits, and size is at
+// most 8 (for q and Q), so we will always be able to parse the given data
+// and fit it into a long long.
+long long mp_binary_get_int(size_t size, bool is_signed, bool big_endian, const byte *src) {
+    int delta;
+    if (!big_endian) {
+        delta = -1;
+        src += size - 1;
+    } else {
+        delta = 1;
+    }
+
+    unsigned long long val = 0;
+    if (is_signed && *src & 0x80) {
+        val = -1;
+    }
+    for (uint i = 0; i < size; i++) {
+        val <<= 8;
+        val |= *src;
+        src += delta;
+    }
+
+    return val;
+}
+
+#define is_signed(typecode) (typecode > 'Z')
+mp_obj_t mp_binary_get_val(char struct_type, char val_type, byte *p_base, byte **ptr) {
+    byte *p = *ptr;
+    size_t align;
+
+    size_t size = mp_binary_get_size(struct_type, val_type, &align);
+    if (struct_type == '@') {
+        // Align p relative to p_base
+        p = p_base + (uintptr_t)MP_ALIGN(p - p_base, align);
+        #if MP_ENDIANNESS_LITTLE
+        struct_type = '<';
+        #else
+        struct_type = '>';
+        #endif
+    }
+    *ptr = p + size;
+
+    long long val = mp_binary_get_int(size, is_signed(val_type), (struct_type == '>'), p);
+
+    if (MICROPY_PY_STRUCT_UNSAFE_TYPECODES && val_type == 'O') {
+        return (mp_obj_t)(mp_uint_t)val;
+    } else if (MICROPY_PY_STRUCT_UNSAFE_TYPECODES && val_type == 'S') {
+        const char *s_val = (const char *)(uintptr_t)(mp_uint_t)val;
+        return mp_obj_new_str_from_cstr(s_val);
+    #if MICROPY_PY_BUILTINS_FLOAT
+    } else if (val_type == 'e') {
+        return mp_obj_new_float_from_f(mp_decode_half_float(val));
+    } else if (val_type == 'f') {
+        union {
+            uint32_t i;
+            float f;
+        } fpu = {val};
+        return mp_obj_new_float_from_f(fpu.f);
+    } else if (val_type == 'd') {
+        union {
+            uint64_t i;
+            double f;
+        } fpu = {val};
+        return mp_obj_new_float_from_d(fpu.f);
+    #endif
+    } else if (is_signed(val_type)) {
+        if ((long long)MP_SMALL_INT_MIN <= val && val <= (long long)MP_SMALL_INT_MAX) {
+            return mp_obj_new_int((mp_int_t)val);
+        } else {
+            return mp_obj_new_int_from_ll(val);
+        }
+    } else {
+        if ((unsigned long long)val <= (unsigned long long)MP_SMALL_INT_MAX) {
+            return mp_obj_new_int_from_uint((mp_uint_t)val);
+        } else {
+            return mp_obj_new_int_from_ull(val);
+        }
+    }
+}
+
+void mp_binary_set_int(size_t val_sz, bool big_endian, byte *dest, mp_uint_t val) {
+    if (MP_ENDIANNESS_LITTLE && !big_endian) {
+        memcpy(dest, &val, val_sz);
+    } else if (MP_ENDIANNESS_BIG && big_endian) {
+        // only copy the least-significant val_sz bytes
+        memcpy(dest, (byte *)&val + sizeof(mp_uint_t) - val_sz, val_sz);
+    } else {
+        const byte *src;
+        if (MP_ENDIANNESS_LITTLE) {
+            src = (const byte *)&val + val_sz;
+        } else {
+            src = (const byte *)&val + sizeof(mp_uint_t);
+        }
+        while (val_sz--) {
+            *dest++ = *--src;
+        }
+    }
+}
+
+void mp_binary_set_val(char struct_type, char val_type, mp_obj_t val_in, byte *p_base, byte **ptr) {
+    byte *p = *ptr;
+    size_t align;
+
+    size_t size = mp_binary_get_size(struct_type, val_type, &align);
+    if (struct_type == '@') {
+        // Align p relative to p_base
+        p = p_base + (uintptr_t)MP_ALIGN(p - p_base, align);
+        if (MP_ENDIANNESS_LITTLE) {
+            struct_type = '<';
+        } else {
+            struct_type = '>';
+        }
+    }
+    *ptr = p + size;
+
+    mp_uint_t val;
+    switch (val_type) {
+        #if MICROPY_PY_STRUCT_UNSAFE_TYPECODES
+        case 'O':
+            val = (mp_uint_t)val_in;
+            break;
+        #endif
+        #if MICROPY_PY_BUILTINS_FLOAT
+        case 'e':
+            val = mp_encode_half_float(mp_obj_get_float_to_f(val_in));
+            break;
+        case 'f': {
+            union {
+                uint32_t i;
+                float f;
+            } fp_sp;
+            fp_sp.f = mp_obj_get_float_to_f(val_in);
+            val = fp_sp.i;
+            break;
+        }
+        case 'd': {
+            union {
+                uint64_t i64;
+                uint32_t i32[2];
+                double f;
+            } fp_dp;
+            fp_dp.f = mp_obj_get_float_to_d(val_in);
+            if (MP_BYTES_PER_OBJ_WORD == 8) {
+                val = fp_dp.i64;
+            } else {
+                int be = struct_type == '>';
+                mp_binary_set_int(sizeof(uint32_t), be, p, fp_dp.i32[MP_ENDIANNESS_BIG ^ be]);
+                p += sizeof(uint32_t);
+                val = fp_dp.i32[MP_ENDIANNESS_LITTLE ^ be];
+            }
+            break;
+        }
+        #endif
+        default:
+            #if MICROPY_LONGINT_IMPL != MICROPY_LONGINT_IMPL_NONE
+            if (mp_obj_is_exact_type(val_in, &mp_type_int)) {
+                mp_obj_int_to_bytes_impl(val_in, struct_type == '>', size, p);
+                return;
+            }
+            #endif
+
+            val = mp_obj_get_int(val_in);
+            // zero/sign extend if needed
+            if (MP_BYTES_PER_OBJ_WORD < 8 && size > sizeof(val)) {
+                int c = (mp_int_t)val < 0 ? 0xff : 0x00;
+                memset(p, c, size);
+                if (struct_type == '>') {
+                    p += size - sizeof(val);
+                }
+            }
+            break;
+    }
+
+    mp_binary_set_int(MIN((size_t)size, sizeof(val)), struct_type == '>', p, val);
+}
+
+void mp_binary_set_val_array(char typecode, void *p, size_t index, mp_obj_t val_in) {
+    switch (typecode) {
+        #if MICROPY_PY_BUILTINS_FLOAT
+        case 'f':
+            ((float *)p)[index] = mp_obj_get_float_to_f(val_in);
+            break;
+        case 'd':
+            ((double *)p)[index] = mp_obj_get_float_to_d(val_in);
+            break;
+        #endif
+        #if MICROPY_PY_STRUCT_UNSAFE_TYPECODES
+        // Extension to CPython: array of objects
+        case 'O':
+            ((mp_obj_t *)p)[index] = val_in;
+            break;
+        #endif
+        default:
+            #if MICROPY_LONGINT_IMPL != MICROPY_LONGINT_IMPL_NONE
+            if (mp_obj_is_exact_type(val_in, &mp_type_int)) {
+                size_t size = mp_binary_get_size('@', typecode, NULL);
+                mp_obj_int_to_bytes_impl(val_in, MP_ENDIANNESS_BIG,
+                    size, (uint8_t *)p + index * size);
+                return;
+            }
+            #endif
+            mp_binary_set_val_array_from_int(typecode, p, index, mp_obj_get_int(val_in));
+    }
+}
+
+void mp_binary_set_val_array_from_int(char typecode, void *p, size_t index, mp_int_t val) {
+    switch (typecode) {
+        case 'b':
+            ((signed char *)p)[index] = val;
+            break;
+        case BYTEARRAY_TYPECODE:
+        case 'B':
+            ((unsigned char *)p)[index] = val;
+            break;
+        case 'h':
+            ((short *)p)[index] = val;
+            break;
+        case 'H':
+            ((unsigned short *)p)[index] = val;
+            break;
+        case 'i':
+            ((int *)p)[index] = val;
+            break;
+        case 'I':
+            ((unsigned int *)p)[index] = val;
+            break;
+        case 'l':
+            ((long *)p)[index] = val;
+            break;
+        case 'L':
+            ((unsigned long *)p)[index] = val;
+            break;
+        #if MICROPY_LONGINT_IMPL != MICROPY_LONGINT_IMPL_NONE
+        case 'q':
+            ((long long *)p)[index] = val;
+            break;
+        case 'Q':
+            ((unsigned long long *)p)[index] = val;
+            break;
+        #endif
+        #if MICROPY_PY_BUILTINS_FLOAT
+        case 'f':
+            ((float *)p)[index] = (float)val;
+            break;
+        case 'd':
+            ((double *)p)[index] = (double)val;
+            break;
+        #endif
+            // Extension to CPython: array of pointers
+        #if MICROPY_PY_STRUCT_UNSAFE_TYPECODES
+        case 'P':
+            ((void **)p)[index] = (void *)(uintptr_t)val;
+            break;
+        #endif
+    }
+}