Reverse stereo and preamp.

firmware/code/configuration_manager.c

@@ -47,12 +47,13 @@ static const default_configuration default_config = {
     .set_configuration = { SET_CONFIGURATION, sizeof(default_config) },
     .filters = {
         .filter = { FILTER_CONFIGURATION, sizeof(default_config.filters) },
-        .f1 = { PEAKING,    38,   -19,  0.9 },
+        .f1 = { PEAKING,    {0},    38,   -19,  0.9 },
-        .f2 = { LOWSHELF,   2900,   2,  0.7 },
+        .f2 = { LOWSHELF,   {0},    2900,   2,  0.7 },
-        .f3 = { PEAKING,    430,    3,  3.5 },
+        .f3 = { PEAKING,    {0},    430,    3,  3.5 },
-        .f4 = { HIGHSHELF,  8400,   2,  0.7 },
+        .f4 = { HIGHSHELF,  {0},    8400,   2,  0.7 },
-        .f5 = { PEAKING,    4800,   3,    5 }
+        .f5 = { PEAKING,    {0},    4800,   3,    5 }
-    }
+    },
+    .preprocessing = { .header = { PREPROCESSING_CONFIGURATION, sizeof(default_config.preprocessing) }, -0.1f, false, {0} }
 };
 
 // Grab the last 4k page of flash for our configuration strutures.
@@ -96,6 +97,8 @@ bool validate_filter_configuration(filter_configuration_tlv *filters)
         case BANDPASSPEAK:
         case NOTCH:
         case ALLPASS: {
+            //filter2 *args = (filter2 *)ptr;
+            //printf("Found Filter %d: %0.2f %0.2f\n", args->type, args->f0, args->Q);
             if (remaining < sizeof(filter2)) {
                 printf("Error! Not enough data left for filter2 (%d)..\n", remaining);
                 return false;
@@ -105,6 +108,8 @@ bool validate_filter_configuration(filter_configuration_tlv *filters)
         case PEAKING:
         case LOWSHELF:
         case HIGHSHELF: {
+            //filter3 *args = (filter3 *)ptr;
+            //printf("Found Filter %d: %0.2f %0.2f %0.2f\n", args->type, args->f0, args->db_gain, args->Q);
             if (remaining < sizeof(filter3)) {
                 printf("Error! Not enough data left for filter3 (%d)..\n", remaining);
                 return false;
@@ -155,15 +160,25 @@ void apply_filter_configuration(filter_configuration_tlv *filters) {
 }
 
 bool validate_configuration(tlv_header *config) {
-    if (config->type != SET_CONFIGURATION) {
+    uint8_t *ptr = NULL; 
-        printf("Unexpcected Config type: %d\n", config->type);
+    switch (config->type)
-        return false;
+    {
+        case SET_CONFIGURATION:
+            ptr = (uint8_t *) config->value;
+            break;
+        case FLASH_HEADER: {
+            ptr = (uint8_t *) ((flash_header_tlv*) config)->tlvs;
+            break;
+        }
+        default:
+            printf("Unexpected Config type: %d\n", config->type);
+            return false;
     }
-    uint8_t *ptr = (uint8_t *)config->value;
     const uint8_t *end = (uint8_t *)config + config->length;
     while (ptr < end) {
         tlv_header* tlv = (tlv_header*) ptr;
         if (tlv->length < 4) {
+            printf("Bad length... %d\n", tlv->length);
             return false;
         }
         switch (tlv->type) {
@@ -172,6 +187,14 @@ bool validate_configuration(tlv_header *config) {
                     return false;
                 }
                 break;
+            case PREPROCESSING_CONFIGURATION: {
+                preprocessing_configuration_tlv* preprocessing_config = (preprocessing_configuration_tlv*) tlv;
+                //printf("Preproc %0.2f %d\n", preprocessing_config->preamp, preprocessing_config->reverse_stereo);
+                if (tlv->length != sizeof(preprocessing_configuration_tlv)) {
+                    printf("Preprocessing size missmatch: %u != %zu\n", tlv->length, sizeof(preprocessing_configuration_tlv));
+                    return false;
+                }
+                break;}
             default:
                 // Unknown TLVs are not invalid, just ignored.
                 break;
@@ -193,17 +216,23 @@ bool apply_configuration(tlv_header *config) {
             break;
         }
         default:
-            printf("Unexpcected Config type: %d\n", config->type);
+            printf("Unexpected Config type: %d\n", config->type);
             return false;
     }
 
     const uint8_t *end = (uint8_t *)config + config->length;
-    while (ptr < end) {
+    while ((ptr + 4) < end) {
         tlv_header* tlv = (tlv_header*) ptr;
         switch (tlv->type) {
             case FILTER_CONFIGURATION:
                 apply_filter_configuration((filter_configuration_tlv*) tlv);
                 break;
+            case PREPROCESSING_CONFIGURATION: {
+                preprocessing_configuration_tlv* preprocessing_config = (preprocessing_configuration_tlv*) tlv;
+                preprocessing.preamp = fix16_from_dbl(1.0 + preprocessing_config->preamp);
+                preprocessing.reverse_stereo = preprocessing_config->reverse_stereo;
+                break;
+            }
             default:
                 break;
         }
@@ -248,7 +277,8 @@ bool process_cmd(tlv_header* cmd) {
     switch (cmd->type) {
         case SET_CONFIGURATION:
             if (validate_configuration(cmd)) {
-                inactive_working_configuration = inactive_working_configuration ? 0 : 1;
+                inactive_working_configuration = (inactive_working_configuration ? 0 : 1);
+                ((tlv_header*) working_configuration[inactive_working_configuration])->length = 0;
                 reload_config = true;
                 return true;
             }
@@ -277,7 +307,7 @@ void config_out_packet(struct usb_endpoint *ep) {
 
     const uint16_t transfer_length = ((tlv_header*) working_configuration[inactive_working_configuration])->length;
     //printf("config_length %d %d\n", transfer_length, write_offset);
-    if (write_offset >= transfer_length) {
+    if (transfer_length && write_offset >= transfer_length) {
         // Command complete, fill the result buffer
         tlv_header* result = ((tlv_header*) result_buffer);
         write_offset = 0;

firmware/code/configuration_manager.h

@@ -32,8 +32,8 @@ struct usb_endpoint;
 
 #define INIT_FILTER3(T) { \
     filter3 *args = (filter3 *)ptr; \
-    bqf_##T##_config(SAMPLING_FREQ, args->f0, args->dBgain, args->Q, &bqf_filters_left[filter_stages]); \
+    bqf_##T##_config(SAMPLING_FREQ, args->f0, args->db_gain, args->Q, &bqf_filters_left[filter_stages]); \
-    bqf_##T##_config(SAMPLING_FREQ, args->f0, args->dBgain, args->Q, &bqf_filters_right[filter_stages]); \
+    bqf_##T##_config(SAMPLING_FREQ, args->f0, args->db_gain, args->Q, &bqf_filters_right[filter_stages]); \
     ptr += sizeof(filter3); \
     break; \
     }

firmware/code/configuration_types.h

@@ -49,15 +49,17 @@ typedef struct __attribute__((__packed__)) _tlv_header {
 } tlv_header;
 
 typedef struct __attribute__((__packed__)) _filter2 {
-    uint32_t type;
+    uint8_t type;
+    uint8_t reserved[3];
     double f0;
     double Q;
 } filter2;
 
 typedef struct __attribute__((__packed__)) _filter3 {
-    uint32_t type;
+    uint8_t type;
+    uint8_t reserved[3];
     double f0;
-    double dBgain;
+    double db_gain;
     double Q;
 } filter3;
 
@@ -80,6 +82,13 @@ typedef struct __attribute__((__packed__)) _flash_header_tlv {
     const uint8_t tlvs[];
 } flash_header_tlv;
 
+typedef struct __attribute__((__packed__)) _preprocessing_configuration_tlv {
+    tlv_header header;
+    double preamp;
+    uint8_t reverse_stereo;
+    uint8_t reserved[3];
+} preprocessing_configuration_tlv;
+
 typedef struct __attribute__((__packed__)) _filter_configuration_tlv {
     tlv_header header;
     const uint8_t filters[];
@@ -101,6 +110,7 @@ typedef struct __attribute__((__packed__)) _default_configuration {
         filter3 f4;
         filter3 f5;
     } filters;
+    preprocessing_configuration_tlv preprocessing;
 } default_configuration;
 
 #endif // __CONFIGURATION_TYPES_H__

firmware/code/run.c

@@ -64,6 +64,11 @@ static struct {
     .freq = 48000,
 };
 
+preprocessing_config preprocessing = {
+    .preamp = fix16_one,
+    .reverse_stereo = false
+};
+
 static char spi_serial_number[17] = "";
 
 enum vendor_cmds {
@@ -103,8 +108,16 @@ static void _as_audio_packet(struct usb_endpoint *ep) {
     int32_t *out = (int32_t *) userbuf;
     int samples = usb_buffer->data_len / 2;
 
-    for (int i = 0; i < samples; i++)
+    if (preprocessing.reverse_stereo) {
-        out[i] = in[i];
+        for (int i = 0; i < samples; i+=2) {
+            out[i] = in[i+1];
+            out[i+1] = in[i];
+        }
+    }
+    else {
+        for (int i = 0; i < samples; i++)
+            out[i] = in[i];
+    }
 
     multicore_fifo_push_blocking(CORE0_READY);
     multicore_fifo_push_blocking(samples);
@@ -112,7 +125,7 @@ static void _as_audio_packet(struct usb_endpoint *ep) {
     for (int j = 0; j < filter_stages; j++) {
         // Left channel filter
         for (int i = 0; i < samples; i += 2) {
-            fix16_t x_f16 = fix16_from_int((int16_t) out[i]);
+            fix16_t x_f16 = fix16_mul(fix16_from_int((int16_t) out[i]), preprocessing.preamp);
 
             x_f16 = bqf_transform(x_f16, &bqf_filters_left[j],
                 &bqf_filters_mem_left[j]);
@@ -168,7 +181,7 @@ void core1_entry() {
 
         for (int j = 0; j < filter_stages; j++) {
             for (int i = 1; i < limit; i += 2) {
-                fix16_t x_f16 = fix16_from_int((int16_t) out[i]);
+                fix16_t x_f16 = fix16_mul(fix16_from_int((int16_t) out[i]), preprocessing.preamp);
 
                 x_f16 = bqf_transform(x_f16, &bqf_filters_right[j],
                     &bqf_filters_mem_right[j]);
@@ -198,6 +211,11 @@ void setup() {
     sleep_ms(100);
     stdio_init_all();
 
+    for (int i=0; i<MAX_FILTER_STAGES; i++) {
+        bqf_memreset(&bqf_filters_mem_left[i]);
+        bqf_memreset(&bqf_filters_mem_right[i]);
+    }
+
     pico_get_unique_board_id_string(spi_serial_number, 17);
     descriptor_strings[2] = spi_serial_number;
     userbuf = malloc(sizeof(uint8_t) * RINGBUF_LEN_IN_BYTES);

firmware/code/run.h

@@ -28,6 +28,7 @@
 
 #include "ringbuf.h"
 #include "i2s.h"
+#include "fix16.h"
 
 /*****************************************************************************
  * USB-related definitions begin here.
@@ -74,6 +75,13 @@ typedef struct _audio_device_config {
     struct usb_endpoint_descriptor ep4;
 } audio_device_config;
 
+typedef struct _preprocessing_config {
+    fix16_t preamp;
+    int reverse_stereo;
+} preprocessing_config;
+
+extern preprocessing_config preprocessing;
+
 static char *descriptor_strings[] = {
     "Ploopy Corporation",
     "Ploopy Headphones",