Preparation for using instruction cache on F7

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);

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;
 

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

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();

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();

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();

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];

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);
 }

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;

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

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

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

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