From 12640972a921944b954d90b47c951ab5cf60a6c8 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Fri, 15 Dec 2017 10:07:19 +0000
Subject: [PATCH] Preparation for using instruction cache on F7

---
 src/main/common/filter.c                 | 20 +++++++++++---------
 src/main/fc/fc_core.c                    | 10 +++++-----
 src/main/fc/fc_init.c                    |  8 ++++++++
 src/main/flight/mixer.c                  |  8 ++++----
 src/main/flight/pid.c                    |  2 +-
 src/main/scheduler/scheduler.c           |  6 +++---
 src/main/sensors/boardalignment.c        |  4 +++-
 src/main/sensors/gyro.c                  | 10 +++++-----
 src/main/sensors/gyroanalyse.c           |  6 +++---
 src/main/target/common_fc_pre.h          |  6 ++++++
 src/main/target/common_osd_slave.h       |  2 ++
 src/main/target/link/stm32_flash_f722.ld |  6 ++++++
 src/test/unit/platform.h                 |  2 ++
 13 files changed, 59 insertions(+), 31 deletions(-)

diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index aaa95aff2e..3eff1a9858 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -20,6 +20,8 @@
 #include <string.h>
 #include <math.h>
 
+#include "platform.h"
+
 #include "common/filter.h"
 #include "common/maths.h"
 #include "common/utils.h"
@@ -31,7 +33,7 @@
 
 // NULL filter
 
-float nullFilterApply(void *filter, float input)
+FAST_CODE float nullFilterApply(void *filter, float input)
 {
     UNUSED(filter);
     return input;
@@ -47,13 +49,13 @@ void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT)
     filter->k = filter->dT / (filter->RC + filter->dT);
 }
 
-float pt1FilterApply(pt1Filter_t *filter, float input)
+FAST_CODE float pt1FilterApply(pt1Filter_t *filter, float input)
 {
     filter->state = filter->state + filter->k * (input - filter->state);
     return filter->state;
 }
 
-float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
+FAST_CODE float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
 {
     // Pre calculate and store RC
     if (!filter->RC) {
@@ -76,7 +78,7 @@ void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold)
     filter->threshold = threshold;
 }
 
-float slewFilterApply(slewFilter_t *filter, float input)
+FAST_CODE float slewFilterApply(slewFilter_t *filter, float input)
 {
     if (filter->state >= filter->threshold) {
         if (input >= filter->state - filter->slewLimit) {
@@ -153,7 +155,7 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh
     filter->y1 = filter->y2 = 0;
 }
 
-void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
+FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
 {
     // backup state
     float x1 = filter->x1;
@@ -171,7 +173,7 @@ void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refre
 }
 
 /* Computes a biquadFilter_t filter on a sample (slightly less precise than df2 but works in dynamic mode) */
-float biquadFilterApplyDF1(biquadFilter_t *filter, float input)
+FAST_CODE float biquadFilterApplyDF1(biquadFilter_t *filter, float input)
 {
     /* compute result */
     const float result = filter->b0 * input + filter->b1 * filter->x1 + filter->b2 * filter->x2 - filter->a1 * filter->y1 - filter->a2 * filter->y2;
@@ -188,7 +190,7 @@ float biquadFilterApplyDF1(biquadFilter_t *filter, float input)
 }
 
 /* Computes a biquadFilter_t filter in direct form 2 on a sample (higher precision but can't handle changes in coefficients */
-float biquadFilterApply(biquadFilter_t *filter, float input)
+FAST_CODE float biquadFilterApply(biquadFilter_t *filter, float input)
 {
     const float result = filter->b0 * input + filter->x1;
     filter->x1 = filter->b1 * input - filter->a1 * result + filter->x2;
@@ -244,7 +246,7 @@ void firFilterUpdateAverage(firFilter_t *filter, float input)
     }
 }
 
-float firFilterApply(const firFilter_t *filter)
+FAST_CODE float firFilterApply(const firFilter_t *filter)
 {
     float ret = 0.0f;
     int ii = 0;
@@ -258,7 +260,7 @@ float firFilterApply(const firFilter_t *filter)
     return ret;
 }
 
-float firFilterUpdateAndApply(firFilter_t *filter, float input)
+FAST_CODE float firFilterUpdateAndApply(firFilter_t *filter, float input)
 {
     firFilterUpdate(filter, input);
     return firFilterApply(filter);
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 0c5e7f3b5b..48eeb52530 100644
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -600,7 +600,7 @@ void processRx(timeUs_t currentTimeUs)
 #endif
 }
 
-static void subTaskPidController(timeUs_t currentTimeUs)
+static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs)
 {
     uint32_t startTime = 0;
     if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
@@ -609,7 +609,7 @@ static void subTaskPidController(timeUs_t currentTimeUs)
     DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
 }
 
-static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
+static FAST_CODE void subTaskMainSubprocesses(timeUs_t currentTimeUs)
 {
     uint32_t startTime = 0;
     if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
@@ -683,7 +683,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
     DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime);
 }
 
-static void subTaskMotorUpdate(timeUs_t currentTimeUs)
+static FAST_CODE void subTaskMotorUpdate(timeUs_t currentTimeUs)
 {
     uint32_t startTime = 0;
     if (debugMode == DEBUG_CYCLETIME) {
@@ -711,7 +711,7 @@ static void subTaskMotorUpdate(timeUs_t currentTimeUs)
     DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime);
 }
 
-uint8_t setPidUpdateCountDown(void)
+FAST_CODE uint8_t setPidUpdateCountDown(void)
 {
     if (gyroConfig()->gyro_soft_lpf_hz) {
         return pidConfig()->pid_process_denom - 1;
@@ -721,7 +721,7 @@ uint8_t setPidUpdateCountDown(void)
 }
 
 // Function for loop trigger
-void taskMainPidLoop(timeUs_t currentTimeUs)
+FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
 {
     static uint8_t pidUpdateCountdown = 0;
 
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 3faee7e2d2..1849f8791d 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -236,6 +236,14 @@ void spiPreInit(void)
 
 void init(void)
 {
+#ifdef USE_ITCM_RAM
+    /* Load functions into ITCM RAM */
+    extern unsigned char tcm_code_start;
+    extern unsigned char tcm_code_end;
+    extern unsigned char tcm_code;
+    memcpy(&tcm_code_start, &tcm_code, (int)(&tcm_code_end - &tcm_code_start));
+#endif
+
 #ifdef USE_HAL_DRIVER
     HAL_Init();
 #endif
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index e85d3d53e7..34de6bd473 100644
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -482,7 +482,7 @@ void mixerResetDisarmedMotors(void)
     }
 }
 
-void writeMotors(void)
+FAST_CODE void writeMotors(void)
 {
     if (pwmAreMotorsEnabled()) {
         for (int i = 0; i < motorCount; i++) {
@@ -520,7 +520,7 @@ static float motorRangeMax;
 static float motorOutputRange;
 static int8_t motorOutputMixSign;
 
-static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
+static FAST_CODE void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
 {
     static uint16_t rcThrottlePrevious = 0;   // Store the last throttle direction for deadband transitions
     static timeUs_t reversalTimeUs = 0; // time when motors last reversed in 3D mode
@@ -637,7 +637,7 @@ static void applyFlipOverAfterCrashModeToMotors(void)
     }
 }
 
-static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS])
+static FAST_CODE void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS])
 {
     // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
     // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
@@ -668,7 +668,7 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS])
     }
 }
 
-void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation)
+FAST_CODE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation)
 {
     if (isFlipOverAfterCrashMode()) {
         applyFlipOverAfterCrashModeToMotors();
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 26761a6be5..a1150730e3 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -405,7 +405,7 @@ static float accelerationLimit(int axis, float currentPidSetpoint)
 
 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
 // Based on 2DOF reference design (matlab)
-void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs)
+FAST_CODE void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs)
 {
     static float previousRateError[2];
     const float tpaFactor = getThrottlePIDAttenuation();
diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c
index 89aba89529..c3d0a54ae0 100644
--- a/src/main/scheduler/scheduler.c
+++ b/src/main/scheduler/scheduler.c
@@ -106,7 +106,7 @@ bool queueRemove(cfTask_t *task)
 /*
  * Returns first item queue or NULL if queue empty
  */
-cfTask_t *queueFirst(void)
+FAST_CODE cfTask_t *queueFirst(void)
 {
     taskQueuePos = 0;
     return taskQueueArray[0]; // guaranteed to be NULL if queue is empty
@@ -115,7 +115,7 @@ cfTask_t *queueFirst(void)
 /*
  * Returns next item in queue or NULL if at end of queue
  */
-cfTask_t *queueNext(void)
+FAST_CODE cfTask_t *queueNext(void)
 {
     return taskQueueArray[++taskQueuePos]; // guaranteed to be NULL at end of queue
 }
@@ -225,7 +225,7 @@ void schedulerInit(void)
     queueAdd(&cfTasks[TASK_SYSTEM]);
 }
 
-void scheduler(void)
+FAST_CODE void scheduler(void)
 {
     // Cache currentTime
     const timeUs_t currentTimeUs = micros();
diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c
index f62fa33f71..b30e192079 100644
--- a/src/main/sensors/boardalignment.c
+++ b/src/main/sensors/boardalignment.c
@@ -20,6 +20,8 @@
 #include <math.h>
 #include <string.h>
 
+#include "platform.h"
+
 #include "common/maths.h"
 #include "common/axis.h"
 
@@ -68,7 +70,7 @@ static void alignBoard(int32_t *vec)
     vec[Z] = lrintf(boardRotation[0][Z] * x + boardRotation[1][Z] * y + boardRotation[2][Z] * z);
 }
 
-void alignSensors(int32_t *dest, uint8_t rotation)
+FAST_CODE void alignSensors(int32_t *dest, uint8_t rotation)
 {
     const int32_t x = dest[X];
     const int32_t y = dest[Y];
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 596edf143a..c759adb558 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -518,12 +518,12 @@ void gyroInitFilters(void)
     gyroInitSensorFilters(&gyroSensor1);
 }
 
-bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor)
+FAST_CODE bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor)
 {
     return gyroSensor->calibration.calibratingG == 0;
 }
 
-bool isGyroCalibrationComplete(void)
+FAST_CODE bool isGyroCalibrationComplete(void)
 {
     return isGyroSensorCalibrationComplete(&gyroSensor1);
 }
@@ -604,7 +604,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t
 }
 
 #if defined(USE_GYRO_SLEW_LIMITER)
-int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis)
+FAST_CODE int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis)
 {
     int32_t newRawGyro = (int32_t)gyroSensor->gyroDev.gyroADCRaw[axis];
     if (abs(newRawGyro - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) {
@@ -616,7 +616,7 @@ int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis)
 }
 #endif
 
-static void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
+static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
 {
     if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) {
         return;
@@ -701,7 +701,7 @@ static void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
     }
 }
 
-void gyroUpdate(timeUs_t currentTimeUs)
+FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
 {
     gyroUpdateSensor(&gyroSensor1, currentTimeUs);
 }
diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c
index 1c3a7c0e30..5b4f1a0615 100644
--- a/src/main/sensors/gyroanalyse.c
+++ b/src/main/sensors/gyroanalyse.c
@@ -136,7 +136,7 @@ const gyroFftData_t *gyroFftData(int axis)
     return &fftResult[axis];
 }
 
-bool isDynamicFilterActive(void)
+FAST_CODE bool isDynamicFilterActive(void)
 {
     return feature(FEATURE_DYNAMIC_FILTER);
 }
@@ -144,7 +144,7 @@ bool isDynamicFilterActive(void)
 /*
  * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
  */
-void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn)
+FAST_CODE void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn)
 {
     if (!isDynamicFilterActive()) {
         return;
@@ -197,7 +197,7 @@ typedef enum {
 /*
  * Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds
  */
-void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn)
+FAST_CODE void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn)
 {
     static int axis = 0;
     static int step = 0;
diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h
index 60327c1042..078acc3db5 100644
--- a/src/main/target/common_fc_pre.h
+++ b/src/main/target/common_fc_pre.h
@@ -76,6 +76,12 @@
 #define DEFAULT_AUX_CHANNEL_COUNT       6
 #endif
 
+#ifdef USE_ITCM_RAM
+#define FAST_CODE __attribute__((section(".tcm_code")))
+#else
+#define FAST_CODE
+#endif
+
 #define USE_CLI
 #define USE_PPM
 #define USE_PWM
diff --git a/src/main/target/common_osd_slave.h b/src/main/target/common_osd_slave.h
index fa4be9f2a8..732b13fe6b 100644
--- a/src/main/target/common_osd_slave.h
+++ b/src/main/target/common_osd_slave.h
@@ -56,5 +56,7 @@
 #define SCHEDULER_DELAY_LIMIT           100
 #endif
 
+#define FAST_CODE
+
 //CLI needs FC dependencies removed before we can compile it, disabling for now
 //#define USE_CLI
diff --git a/src/main/target/link/stm32_flash_f722.ld b/src/main/target/link/stm32_flash_f722.ld
index f3b0e5b67c..ce22546f52 100644
--- a/src/main/target/link/stm32_flash_f722.ld
+++ b/src/main/target/link/stm32_flash_f722.ld
@@ -22,6 +22,12 @@ ENTRY(Reset_Handler)
 /* Specify the memory areas */
 MEMORY
 {
+    ITCM_ISR (rx)     : ORIGIN = 0x00000000, LENGTH = 1K
+    ITCM_RAM (rx)     : ORIGIN = 0x00000400, LENGTH = 15K
+
+    ITCMFL (rx)       : ORIGIN = 0x00200000, LENGTH = 16K 
+    ITCMFL1 (rx)      : ORIGIN = 0x00208000, LENGTH = 480K 
+
     FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 16K
     FLASH_CONFIG (r)  : ORIGIN = 0x08004000, LENGTH = 16K
     FLASH1 (rx)       : ORIGIN = 0x08008000, LENGTH = 480K
diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h
index 6c643621f6..243c4d185c 100644
--- a/src/test/unit/platform.h
+++ b/src/test/unit/platform.h
@@ -25,6 +25,8 @@
 #define U_ID_1 1
 #define U_ID_2 2
 
+#define FAST_CODE
+
 #define MAX_PROFILE_COUNT 3
 #define USE_MAG
 #define USE_BARO