Refactor: Separate XUA lite and app code

examples/xua_lite_example/xua_lite/rate_control/fifo_impl.h

@@ -0,0 +1,206 @@
+#ifndef __FIFO__
+#define __FIFO__
+#include <string.h> //memcpy
+#include "fifo_types.h"
+
+//Asynch FIFO implementaion
+//Note these are in the include file to allow the compiler to inline for performance
+
+///////////////////////////////////////
+//Shared memory FIFO (sample by sample)
+//Can be any size
+///////////////////////////////////////
+
+
+static inline unsigned fifo_get_fill(volatile mem_fifo_t * unsafe fifo) {
+    unsafe{
+        unsigned fifo_fill = 0;
+        if (fifo->write_idx >= fifo->read_idx){
+            fifo_fill = fifo->write_idx - fifo->read_idx;
+        }
+        else{
+            fifo_fill = (fifo->size + fifo->write_idx) - fifo->read_idx;
+        }
+        return fifo_fill;
+    }
+}
+
+static inline unsigned fifo_get_fill_short(volatile mem_fifo_short_t * unsafe fifo) {
+    unsafe{
+        unsigned fifo_fill = 0;
+        if (fifo->write_idx >= fifo->read_idx){
+            fifo_fill = fifo->write_idx - fifo->read_idx;
+        }
+        else{
+            fifo_fill = (fifo->size + fifo->write_idx) - fifo->read_idx;
+        }
+        return fifo_fill;
+    }
+}
+
+static inline void fifo_init_short(volatile mem_fifo_short_t * unsafe fifo) {
+    unsafe{
+        fifo->write_idx = 0;
+        fifo->read_idx = (fifo->size * 2) / 4;
+        memset(fifo->data_base_ptr , 0, fifo->size * sizeof(short));
+    }
+}
+
+#pragma unsafe arrays
+static inline fifo_ret_t fifo_block_push(volatile mem_fifo_t * unsafe fifo, int data[], unsigned n) {
+    unsafe{
+        //check there is a block of space large enough
+        unsigned space_remaining = fifo->size - fifo_get_fill(fifo) - 1;
+        if (n > space_remaining) {
+            return FIFO_FULL;
+        }
+        for (int i = 0; i < n; i++){
+            unsigned next_idx = fifo->write_idx + 1;
+            if (next_idx == fifo->size) next_idx = 0; //Check for wrap
+            fifo->data_base_ptr[fifo->write_idx] = data[i];
+            fifo->write_idx = next_idx;
+        }
+        return FIFO_SUCCESS;
+    }
+}
+
+#pragma unsafe arrays
+static inline fifo_ret_t fifo_block_push_short(volatile mem_fifo_short_t * unsafe fifo, short data[], unsigned n) {
+    unsafe{
+        //check there is a block of space large enough
+        unsigned space_remaining = fifo->size - fifo_get_fill_short(fifo) - 1;
+        if (n > space_remaining) {
+            return FIFO_FULL;
+        }
+        for (int i = 0; i < n; i++){
+            unsigned next_idx = fifo->write_idx + 1;
+            if (next_idx == fifo->size) next_idx = 0; //Check for wrap
+            fifo->data_base_ptr[fifo->write_idx] = data[i];
+            fifo->write_idx = next_idx;
+        }
+        return FIFO_SUCCESS;
+    }
+}
+
+#pragma unsafe arrays
+static inline fifo_ret_t fifo_block_push_short_fast(volatile mem_fifo_short_t * unsafe fifo, short data[], unsigned n) {
+    unsafe{
+        //check there is a block of space large enough
+        unsigned space_remaining = fifo->size - fifo_get_fill_short(fifo) - 1;
+        if (n > space_remaining) {
+            return FIFO_FULL;
+        }
+        //We will write either one or two blocks depending on wrap
+        unsigned first_block_size = 0;
+        unsigned second_block_size = 0;
+
+        //See if we need to wrap during block writes
+        unsigned space_left_at_top = fifo->size - fifo->write_idx;
+        //printf("space_left_at_top %d\n", space_left_at_top);
+        //Yes, we do need to wrap
+        if (n > space_left_at_top){
+            first_block_size = space_left_at_top;
+            second_block_size = n - space_left_at_top;
+            memcpy(&fifo->data_base_ptr[fifo->write_idx], &data[0], first_block_size * sizeof(short));
+            memcpy(&fifo->data_base_ptr[0], &data[first_block_size], second_block_size * sizeof(short));
+            fifo->write_idx = second_block_size;
+        }
+        //No wrap, do all in one go
+        else{
+            first_block_size = n;
+            second_block_size = 0;
+            memcpy(&fifo->data_base_ptr[fifo->write_idx], &data[0], first_block_size * sizeof(short));
+            fifo->write_idx += first_block_size;
+        }
+
+        return FIFO_SUCCESS;
+    }
+}
+
+#pragma unsafe arrays
+static inline fifo_ret_t fifo_block_pop(volatile mem_fifo_t * unsafe fifo, int data[], unsigned n) {
+    unsafe{
+        //Check we have a block big enough to send
+        if (n > fifo_get_fill(fifo)){
+            return FIFO_EMPTY;
+        }
+        for (int i = 0; i < n; i++){
+            data[i] = fifo->data_base_ptr[fifo->read_idx];
+            fifo->read_idx++;
+            if (fifo->read_idx == fifo->size) fifo->read_idx = 0; //Check for wrap
+        }
+        return FIFO_SUCCESS;
+    }
+}
+
+#pragma unsafe arrays
+static inline fifo_ret_t fifo_block_pop_short(volatile mem_fifo_short_t * unsafe fifo, short data[], unsigned n) {
+    unsafe{
+        //Check we have a block big enough to send
+        if (n > fifo_get_fill_short(fifo)){
+            return FIFO_EMPTY;
+        }
+        for (int i = 0; i < n; i++){
+            data[i] = fifo->data_base_ptr[fifo->read_idx];
+            fifo->read_idx++;
+            if (fifo->read_idx == fifo->size) fifo->read_idx = 0; //Check for wrap
+        }
+        return FIFO_SUCCESS;
+    }
+}
+
+#pragma unsafe arrays
+static inline fifo_ret_t fifo_block_pop_short_fast(volatile mem_fifo_short_t * unsafe fifo, short data[], unsigned n) {
+    unsafe{
+        //Check we have a block big enough to send
+        if (n > fifo_get_fill_short(fifo)){
+            return FIFO_EMPTY;
+        }
+        //We will read either one or two blocks depending on wrap
+        unsigned first_block_size = 0;
+        unsigned second_block_size = 0;
+
+        //See if we need to wrap during block read
+        unsigned num_read_at_top = fifo->size - fifo->read_idx;
+        // printf("num_read_at_top %d\n", num_read_at_top);
+        //Yes, we do need to wrap
+        if (n > num_read_at_top){
+            first_block_size = num_read_at_top;
+            second_block_size = n - num_read_at_top;
+            memcpy(&data[0], &fifo->data_base_ptr[fifo->read_idx], first_block_size * sizeof(short));
+            memcpy( &data[first_block_size], &fifo->data_base_ptr[0], second_block_size * sizeof(short));
+            fifo->read_idx = second_block_size;
+            // printf("wrap\n");
+        }
+        //No wrap, do all in one go
+        else{
+            first_block_size = n;
+            second_block_size = 0;
+            memcpy(&data[0], &fifo->data_base_ptr[fifo->read_idx], first_block_size * sizeof(short));
+            fifo->read_idx += first_block_size;
+            // printf("no wrap\n");
+
+        }
+
+        return FIFO_SUCCESS;
+    }
+}
+
+//Version of above that returns fill level relative to half full
+static inline int fifo_get_fill_relative_half(volatile mem_fifo_t * unsafe fifo){
+    unsafe{
+        int fifo_fill = (int)fifo_get_fill(fifo);
+        fifo_fill -= (fifo->size / 2);
+        return fifo_fill;
+    }
+}
+
+//Version of above that returns fill level relative to half full
+static inline int fifo_get_fill_relative_half_short(volatile mem_fifo_short_t * unsafe fifo){
+    unsafe{
+        int fifo_fill = (int)fifo_get_fill_short(fifo);
+        fifo_fill -= (fifo->size / 2);
+        return fifo_fill;
+    }
+}
+#endif

examples/xua_lite_example/xua_lite/rate_control/fifo_types.h

@@ -0,0 +1,37 @@
+#ifndef __ASRC_FIFO_TYPES__
+#define __ASRC_FIFO_TYPES__
+#include <stdint.h>
+
+//Shared FIFO return types
+typedef enum fifo_ret_t {
+  FIFO_SUCCESS = 0,
+  FIFO_FULL,
+  FIFO_EMPTY
+} fifo_ret_t;
+
+/////////////////////////////////////////////////////////////////////////
+//Shared memory FIFO (sample by sample or block)
+//Can be any size
+//
+//Note that the actual storage for the FIFO is declared externally
+//and a reference to the base address of the storage is passed in along
+//with the size of the storage. This way, multiple instances may be
+//different sizes.
+//
+/////////////////////////////////////////////////////////////////////////
+
+typedef struct mem_fifo_t {
+    const unsigned size;              //Size in INTs
+    int * const unsafe data_base_ptr; //Base of the data array - declared externally so we can have differnt sized FIFOs
+    unsigned write_idx;
+    unsigned read_idx;
+} mem_fifo_t;
+
+typedef struct mem_fifo_short_t {
+    const unsigned size;              //Size in SHORTs
+    short * const unsafe data_base_ptr; //Base of the data array - declared externally so we can have differnt sized FIFOs
+    unsigned write_idx;
+    unsigned read_idx;
+} mem_fifo_short_t;
+
+#endif

examples/xua_lite_example/xua_lite/rate_control/rate_controller.h

@@ -0,0 +1,21 @@
+#include <stdint.h>
+#include <limits.h>
+
+typedef int32_t xua_lite_fixed_point_t;
+
+typedef struct pid_state_t{
+  xua_lite_fixed_point_t fifo_level_filtered_old;
+  xua_lite_fixed_point_t fifo_level_accum;
+} pid_state_t;
+
+
+//USB Adaptive mode helper
+xua_lite_fixed_point_t do_rate_control(int fill_level, pid_state_t *pid_state);
+
+//PDM modulator for clock control
+void do_clock_nudge_pdm(xua_lite_fixed_point_t controller_out, int *clock_nudge);
+
+
+//USB Asynch mode helper
+void do_feedback_calculation(unsigned &sof_count, const unsigned mclk_hz, unsigned mclk_port_counter,unsigned &mclk_port_counter_old
+                                ,long long &feedback_value, unsigned &mod_from_last_time, unsigned fb_clocks[1]);

examples/xua_lite_example/xua_lite/rate_control/rate_controller.xc

@@ -0,0 +1,201 @@
+#include <xs1.h>
+#include "xua_buffer_lite.h"
+#include "rate_controller.h"
+#define DEBUG_UNIT XUA_RATE_CONTROL
+#define DEBUG_PRINT_ENABLE_XUA_RATE_CONTROL 1
+#include "debug_print.h"
+
+#define XUA_LIGHT_FIXED_POINT_Q_BITS        10 //Including sign bit. 10b gets us to +511.999999 to -512.000000
+#define XUA_LIGHT_FIXED_POINT_TOTAL_BITS    (sizeof(xua_lite_fixed_point_t) * 8)
+#define XUA_LIGHT_FIXED_POINT_FRAC_BITS     (XUA_LIGHT_FIXED_POINT_TOTAL_BITS - XUA_LIGHT_FIXED_POINT_Q_BITS)
+#define XUA_LIGHT_FIXED_POINT_ONE           (1 << XUA_LIGHT_FIXED_POINT_FRAC_BITS)
+#define XUA_LIGHT_FIXED_POINT_MINUS_ONE     (-XUA_LIGHT_FIXED_POINT_ONE)
+
+#define FIFO_LEVEL_EMA_COEFF                0.939 //Proportion of signal from y[-1].
+                                                  //0.939 gives ~10Hz 3db cutoff low pass filter for filter rate of 1kHz
+                                                  //dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency/40465
+#define FIFO_LEVEL_A_COEFF                  ((int32_t)(INT_MAX * FIFO_LEVEL_EMA_COEFF)) //Scale to signed 1.31 format
+#define FIFO_LEVEL_B_COEFF                  (INT_MAX - FIFO_LEVEL_A_COEFF)
+
+#define RANDOMISATION_PERCENT               20 //How much radnom noise to inject in percent of existing signal amplitude
+#define RANDOMISATION_COEFF_A               ((INT_MAX / 100) * RANDOMISATION_PERCENT)
+
+#define PID_CALC_OVERHEAD_BITS              2 //Allow large P,I or D constants, up to 2^(this number)
+
+
+#define PID_CONTROL_P_TERM                  10.0
+#define PID_CONTROL_I_TERM                  150.0
+#define PID_CONTROL_D_TERM                  1.0
+
+#define PID_RATE_MULTIPLIER                 SOF_FREQ_HZ
+
+#define PID_CONTROL_P_TERM_COEFF            ((xua_lite_fixed_point_t)((XUA_LIGHT_FIXED_POINT_ONE >> PID_CALC_OVERHEAD_BITS) * (float)PID_CONTROL_P_TERM)) //scale to fixed point
+#define PID_CONTROL_I_TERM_COEFF            ((xua_lite_fixed_point_t)((XUA_LIGHT_FIXED_POINT_ONE >> PID_CALC_OVERHEAD_BITS) * (float)PID_CONTROL_I_TERM / PID_RATE_MULTIPLIER)) //scale to fixed point
+#define PID_CONTROL_D_TERM_COEFF            ((xua_lite_fixed_point_t)((XUA_LIGHT_FIXED_POINT_ONE >> PID_CALC_OVERHEAD_BITS) * (float)PID_CONTROL_D_TERM * PID_RATE_MULTIPLIER)) //scale to fixed point
+
+
+static inline xua_lite_fixed_point_t do_fifo_depth_lowpass_filter(xua_lite_fixed_point_t old, int fifo_depth){
+  //we grow from 32b to 64b for intermediate
+  int64_t intermediate = ((int64_t)(fifo_depth << XUA_LIGHT_FIXED_POINT_FRAC_BITS) * (int64_t)FIFO_LEVEL_B_COEFF) + ((int64_t)old * (int64_t)FIFO_LEVEL_A_COEFF);
+  xua_lite_fixed_point_t new_fifo_depth = (xua_lite_fixed_point_t)( intermediate >> (64 - XUA_LIGHT_FIXED_POINT_TOTAL_BITS - 1)); //-1 because signed int
+  return new_fifo_depth;
+}
+
+static inline int32_t get_random_number(void)
+{
+  static const unsigned random_poly = 0xEDB88320;
+  static unsigned random = 0x12345678;
+  crc32(random, -1, random_poly);
+  return (int32_t) random;
+}
+
+static inline xua_lite_fixed_point_t add_noise(xua_lite_fixed_point_t input){
+  //Note the input number cannot be bigger than 2 ^ (FIXED_POINT_Q_BITS - 1) * (1 + PERCENT) else we could oveflow
+  //Eg. if Q bits = 10 then biggest input value is 255.9999
+  int32_t random = get_random_number();
+  int32_t input_fraction = ((int64_t)input * (int64_t)RANDOMISATION_COEFF_A) >> (XUA_LIGHT_FIXED_POINT_TOTAL_BITS - 1);
+  int64_t output_64 = ((int64_t)input << (XUA_LIGHT_FIXED_POINT_TOTAL_BITS - 1)) + ((int64_t)input_fraction * (int64_t)random);
+  return (xua_lite_fixed_point_t)( output_64 >> (64 - XUA_LIGHT_FIXED_POINT_TOTAL_BITS - 1));
+}
+
+//Convert the control input into a pdm output (dither) with optional noise
+void do_clock_nudge_pdm(xua_lite_fixed_point_t controller_out, int *clock_nudge){
+
+  //Randomise - add a proportion of rectangular probability distribution noise to spread the spectrum
+  controller_out = add_noise(controller_out);
+
+  //Convert to pulse density modulation (sigma-delta)
+  static xua_lite_fixed_point_t nudge_accumulator = 0;
+  nudge_accumulator += controller_out; //Note no overflow check as if we reach XUA_LIGHT_FIXED_POINT_Q_BITS
+                                       //something is very wrong
+  //printf("co: %d  ratio: %f \n", controller_out, (float)controller_out/XUA_LIGHT_FIXED_POINT_ONE);
+  if (nudge_accumulator >= XUA_LIGHT_FIXED_POINT_ONE){
+    *clock_nudge = 1;
+    nudge_accumulator -= XUA_LIGHT_FIXED_POINT_ONE;
+  }
+  else if (nudge_accumulator <= XUA_LIGHT_FIXED_POINT_MINUS_ONE){
+    nudge_accumulator -= XUA_LIGHT_FIXED_POINT_MINUS_ONE;
+    *clock_nudge = -1;
+  }
+  else{
+    *clock_nudge = 0;
+  }
+}
+
+
+//Do PI control and modulation for adaptive USB audio
+xua_lite_fixed_point_t do_rate_control(int fill_level, pid_state_t *pid_state){
+
+  //We always check the FIFO level after USB has produced a block, and total FIFO size is 2x max, so half full is at 3/4
+  const int half_full = ((MAX_OUT_SAMPLES_PER_SOF_PERIOD * 2) * 3) / 4;
+  int fill_level_wrt_half = fill_level - half_full; //Will be +ve for more than half full and negative for less than half full
+
+  //Low pass filter fill level and get error w.r.t. to set point which is depth = 0 (relative to half full)
+  xua_lite_fixed_point_t fifo_level_filtered = do_fifo_depth_lowpass_filter(pid_state->fifo_level_filtered_old , fill_level_wrt_half);
+  //printf("old fill_level: %f fill_level: %f\n", (float)pid_state->fifo_level_filtered_old/(1<<XUA_LIGHT_FIXED_POINT_FRAC_BITS) , (float)fifo_level_filtered/(1<<XUA_LIGHT_FIXED_POINT_FRAC_BITS) );
+
+  //Calculate the value for the integral term which is the accumulated fill level error
+  xua_lite_fixed_point_t i_term_pre_clip = pid_state->fifo_level_accum + fifo_level_filtered;
+
+  //clip the I term (which can wind up) to maximum fixed point representation. Check to see if overflow (which will change sign)
+  if (fifo_level_filtered >= 0){ //If it was positive before, ensure it still is else clip to positive
+    if (i_term_pre_clip >= pid_state->fifo_level_accum){
+      //debug_printf("grow %d %d\n", (int32_t)i_term_pre_clip,  (int32_t)pid_state->fifo_level_accum);
+      pid_state->fifo_level_accum = i_term_pre_clip;
+    }
+    else{
+      pid_state->fifo_level_accum = INT_MAX;
+      //debug_printf("clip+ %d\n", pid_state->fifo_level_accum);
+    }
+  }
+  else{                           //Value was negative so ensure it still is else clip negative
+    if (i_term_pre_clip <= pid_state->fifo_level_accum){
+      pid_state->fifo_level_accum = i_term_pre_clip;
+    }
+    else{
+      pid_state->fifo_level_accum = INT_MIN;
+      //debug_printf("clip- %d %d\n", pid_state->fifo_level_accum, fifo_level_filtered);
+    }
+  }
+
+  //Calculate D term. No clipping required because it can never be larger than the D term,
+  //which is already scaled to fit within the fixed point representation
+  xua_lite_fixed_point_t fifo_level_delta = fifo_level_filtered - pid_state->fifo_level_filtered_old;
+
+  //Save to struct for next iteration
+  pid_state->fifo_level_filtered_old = fifo_level_filtered;
+
+  //Do PID calculation. Note there is an implicit cast back to xua_lite_fixed_point_t before assignment
+  xua_lite_fixed_point_t p_term = (((int64_t) fifo_level_filtered * (int64_t)PID_CONTROL_P_TERM_COEFF)) >> XUA_LIGHT_FIXED_POINT_FRAC_BITS;
+  xua_lite_fixed_point_t i_term = (((int64_t) pid_state->fifo_level_accum * (int64_t)PID_CONTROL_I_TERM_COEFF)) >> XUA_LIGHT_FIXED_POINT_FRAC_BITS;
+  xua_lite_fixed_point_t d_term = (((int64_t) fifo_level_delta * (int64_t)PID_CONTROL_D_TERM_COEFF)) >> XUA_LIGHT_FIXED_POINT_FRAC_BITS;
+
+  //debug_printf("p: %d i: %d f: %d\n", p_term >> XUA_LIGHT_FIXED_POINT_Q_BITS, i_term >> XUA_LIGHT_FIXED_POINT_Q_BITS, fifo_level_filtered >> (XUA_LIGHT_FIXED_POINT_FRAC_BITS - 10));
+  //printf("p: %f i: %f d: %f filtered: %f integrated: %f\n", (float)p_term / (1<<(XUA_LIGHT_FIXED_POINT_FRAC_BITS-PID_CALC_OVERHEAD_BITS)), (float)i_term / (1<<(XUA_LIGHT_FIXED_POINT_FRAC_BITS-PID_CALC_OVERHEAD_BITS)), (float)d_term / (1<<(XUA_LIGHT_FIXED_POINT_FRAC_BITS-PID_CALC_OVERHEAD_BITS)), (float)fifo_level_filtered/(1<<XUA_LIGHT_FIXED_POINT_FRAC_BITS), (float)pid_state->fifo_level_accum/(1<<XUA_LIGHT_FIXED_POINT_FRAC_BITS));
+
+  //Sum and scale to +- 1.0 (important it does not exceed these values for following step)
+  xua_lite_fixed_point_t controller_out = (p_term + i_term + d_term) >> (XUA_LIGHT_FIXED_POINT_Q_BITS - 1 - PID_CALC_OVERHEAD_BITS);
+
+
+  //debug_printf("filtered: %d raw: %d\n", fifo_level_filtered >> 22, fill_level_wrt_half);
+
+  //static unsigned counter; counter++; if (counter>100){counter = 0; debug_printf("pid: %d\n",i_term >> (XUA_LIGHT_FIXED_POINT_FRAC_BITS - 10));}
+  debug_printf("co: %d\n", controller_out >> XUA_LIGHT_FIXED_POINT_FRAC_BITS);
+  return controller_out;
+}
+
+
+//Calculate feedback for asynchronous USB audio
+void do_feedback_calculation(unsigned &sof_count
+                                ,const unsigned mclk_hz
+                                ,unsigned mclk_port_counter
+                                ,unsigned &mclk_port_counter_old
+                                ,long long &feedback_value
+                                ,unsigned &mod_from_last_time
+                                ,unsigned fb_clocks[1]){
+  // Assuming 48kHz from a 24.576 master clock (0.0407uS period)
+  // MCLK ticks per SOF = 125uS / 0.0407 = 3072 MCLK ticks per SOF.
+  // expected Feedback is 48000/8000 = 6 samples. so 0x60000 in 16:16 format.
+  // Average over 128 SOFs - 128 x 3072 = 0x60000.
+
+  unsigned long long feedbackMul = 64ULL;
+  if(AUDIO_CLASS == 1) feedbackMul = 8ULL;  // TODO Use 4 instead of 8 to avoid windows LSB issues?
+
+  // Number of MCLK ticks in this SOF period (E.g = 125 * 24.576 = 3072)
+  int mclk_ticks_this_sof_period = (int) ((short)(mclk_port_counter - mclk_port_counter_old));
+  unsigned long long full_result = mclk_ticks_this_sof_period * feedbackMul * DEFAULT_FREQ;
+  feedback_value += full_result;
+
+  // Store MCLK for next time around...
+  mclk_port_counter_old = mclk_port_counter;
+
+  // Reset counts based on SOF counting.  Expect 16ms (128 HS SOFs/16 FS SOFS) per feedback poll
+  // We always count 128 SOFs, so 16ms @ HS, 128ms @ FS
+  if(sof_count == 128){
+    //debug_printf("fb\n");
+    sof_count = 0;
+
+    feedback_value += mod_from_last_time;
+    unsigned clocks = feedback_value / mclk_hz;
+    mod_from_last_time = feedback_value % mclk_hz;
+    feedback_value = 0;
+
+    //Scale for working out number of samps to take from device for input
+    if(AUDIO_CLASS == 2){
+        clocks <<= 3;
+    }
+    else{
+        clocks <<= 6;
+    }
+    asm volatile("stw %0, dp[g_speed]"::"r"(clocks));   // g_speed = clocks
+
+    //Write to feedback EP buffer
+    if (AUDIO_CLASS == 2){
+        fb_clocks[0] = clocks;
+    }
+    else{
+        fb_clocks[0] = clocks >> 2;
+    }
+  }
+}
+