Higher bitdepth software volume scaling

Operates between 0 and -74 dB (mute) without issue

Change-Id: I497e002bd8db43833a09ebbc29212fbb6cc8ebfd

5 files changed, 118 insertions, 32 deletions

diff --git a/firmware/drivers/audio/eros_qn_codec.c b/firmware/drivers/audio/eros_qn_codec.c
index fdf21d2f9d..17b0acf13e 100644
--- a/firmware/drivers/audio/eros_qn_codec.c
+++ b/firmware/drivers/audio/eros_qn_codec.c
@@ -70,10 +70,14 @@ void audiohw_set_volume(int vol_l, int vol_r)
     }
 #endif
 
-    l = l <= PCM5102A_VOLUME_MIN ? PCM_MUTE_LEVEL : l;
-    r = r <= PCM5102A_VOLUME_MIN ? PCM_MUTE_LEVEL : r;
-
-    pcm_set_master_volume(l, r);
+    if (l <= PCM5102A_VOLUME_MIN || r <= PCM5102A_VOLUME_MIN)
+    {
+        pcm_set_master_volume(PCM_MUTE_LEVEL, PCM_MUTE_LEVEL);
+    }
+    else
+    {
+        pcm_set_master_volume(l/20, r/20);
+    }
 }
 
 void audiohw_mute_hp(int mute)
diff --git a/firmware/export/config/erosqnative.h b/firmware/export/config/erosqnative.h
index 1b37b6042a..b2b6f3f89e 100644
--- a/firmware/export/config/erosqnative.h
+++ b/firmware/export/config/erosqnative.h
@@ -64,6 +64,9 @@
 #define HAVE_SW_TONE_CONTROLS
 #define HAVE_SW_VOLUME_CONTROL
 
+/* use high-bitdepth volume scaling */
+#define PCM_NATIVE_BITDEPTH 24
+
 /* Button defines */
 #define CONFIG_KEYPAD   EROSQ_PAD
 #define HAVE_SCROLLWHEEL
diff --git a/firmware/export/eros_qn_codec.h b/firmware/export/eros_qn_codec.h
index 9c900186a8..ae04c66799 100644
--- a/firmware/export/eros_qn_codec.h
+++ b/firmware/export/eros_qn_codec.h
@@ -29,7 +29,7 @@
 /* a small DC offset appears to prevent play/pause clicking */
 #define PCM_DC_OFFSET_VALUE -1
 
-AUDIOHW_SETTING(VOLUME, "dB", 0, 1, PCM5102A_VOLUME_MIN/10, PCM5102A_VOLUME_MAX/10, 0)
+AUDIOHW_SETTING(VOLUME, "dB", 0, 2, PCM5102A_VOLUME_MIN/10, PCM5102A_VOLUME_MAX/10, 0)
 
 /* this just calls audiohw_set_volume() with the last (locally) known volume,
  * used for switching to/from fixed line out volume. */
diff --git a/firmware/pcm_sw_volume.c b/firmware/pcm_sw_volume.c
index 3593c684af..646f2a680a 100644
--- a/firmware/pcm_sw_volume.c
+++ b/firmware/pcm_sw_volume.c
@@ -26,6 +26,16 @@
 #include "fixedpoint.h"
 #include "pcm_sw_volume.h"
 
+/*
+ * NOTE: With the addition of 32-bit software scaling to this
+ * file, sometimes the "size" variable gets a little confusing.
+ *
+ * The source buffer (as of right now) is always 16-bit, and the
+ * destination buffer can potentially be 32-bit. I've tried to
+ * make it consistent: when passed in a function call, try to use
+ * the source buffer (16-bit) size.
+ */
+
 /* volume factors set by pcm_set_master_volume */
 static uint32_t vol_factor_l = 0, vol_factor_r = 0;
 
@@ -39,6 +49,30 @@ static uint32_t pcm_new_factor_l = 0, pcm_new_factor_r = 0;
 static uint32_t pcm_factor_l = 0, pcm_factor_r = 0;
 static typeof (memcpy) *pcm_scaling_fn = NULL;
 
+/* default to 16-bit volume scaling unless specified */
+#if !defined(PCM_NATIVE_BITDEPTH)
+# define PCM_NATIVE_BITDEPTH 16
+#endif
+
+/* take care of some defines for 32-bit software vol */
+#if (PCM_NATIVE_BITDEPTH > 16) /* >16-bit */
+
+# define HAVE_SWVOL_32
+# define PCM_VOL_SAMPLE_SIZE     (2 * sizeof (int32_t))
+# define PCM_DBL_BUF_SIZE_T int32_t
+
+# if !defined(PCM_DC_OFFSET_VALUE)
+/* PCM_DC_OFFSET_VALUE is only needed due to hardware quirk on Eros Q */
+#  define PCM_DC_OFFSET_VALUE 0
+# endif
+
+#else /* 16-BIT */
+
+# define PCM_VOL_SAMPLE_SIZE     PCM_SAMPLE_SIZE
+# define PCM_DBL_BUF_SIZE_T int16_t
+
+#endif /* 16-BIT */
+
 /***
  ** Volume scaling routines
  ** If unbuffered, called externally by pcm driver
@@ -54,54 +88,53 @@ static typeof (memcpy) *pcm_scaling_fn = NULL;
 /* Scale sample by PCM factor */
 static inline int32_t pcm_scale_sample(PCM_F_T f, int32_t s)
 {
-#if defined(PCM_DC_OFFSET_VALUE)
-    return (f * s + PCM_DC_OFFSET_VALUE) >> PCM_SW_VOLUME_FRACBITS;
+#if defined(HAVE_SWVOL_32)
+    return (f * s + PCM_DC_OFFSET_VALUE) >> (32 - PCM_NATIVE_BITDEPTH);
 #else
     return (f * s) >> PCM_SW_VOLUME_FRACBITS;
 #endif
 }
 
-/* Both UNITY, use direct copy */
-/* static void * memcpy(void *dst, const void *src, size_t size); */
-
 /* Either cut (both <= UNITY), no clipping needed */
-static void * pcm_scale_buffer_cut(void *dst, const void *src, size_t size)
+static void * pcm_scale_buffer_cut(void *dst, const void *src, size_t src_size)
 {
-    int16_t *d = dst;
+    PCM_DBL_BUF_SIZE_T *d = dst;
     const int16_t *s = src;
     uint32_t factor_l = pcm_factor_l, factor_r = pcm_factor_r;
 
-    while (size)
+    while (src_size)
     {
         *d++ = pcm_scale_sample(factor_l, *s++);
         *d++ = pcm_scale_sample(factor_r, *s++);
-        size -= PCM_SAMPLE_SIZE;
+        src_size -= PCM_SAMPLE_SIZE;
     }
 
     return dst;
 }
 
+#if !defined(HAVE_SWVOL_32) /* NOTE: 32-bit scaling is hardcoded to the cut function! */
 /* Either boost (any > UNITY) requires clipping */
-static void * pcm_scale_buffer_boost(void *dst, const void *src, size_t size)
+static void * pcm_scale_buffer_boost(void *dst, const void *src, size_t src_size)
 {
     int16_t *d = dst;
     const int16_t *s = src;
     uint32_t factor_l = pcm_factor_l, factor_r = pcm_factor_r;
 
-    while (size)
+    while (src_size)
     {
         *d++ = clip_sample_16(pcm_scale_sample(factor_l, *s++));
         *d++ = clip_sample_16(pcm_scale_sample(factor_r, *s++));
-        size -= PCM_SAMPLE_SIZE;
+        src_size -= PCM_SAMPLE_SIZE;
     }
 
     return dst;
 }
+#endif
 
 /* Transition the volume change smoothly across a frame */
-static void * pcm_scale_buffer_trans(void *dst, const void *src, size_t size)
+static void * pcm_scale_buffer_trans(void *dst, const void *src, size_t src_size)
 {
-    int16_t *d = dst;
+    PCM_DBL_BUF_SIZE_T *d = dst;
     const int16_t *s = src;
     uint32_t factor_l = pcm_factor_l, factor_r = pcm_factor_r;
 
@@ -114,13 +147,19 @@ static void * pcm_scale_buffer_trans(void *dst, const void *src, size_t size)
     int32_t diff_l = (int32_t)new_factor_l - (int32_t)factor_l;
     int32_t diff_r = (int32_t)new_factor_r - (int32_t)factor_r;
 
-    for (size_t done = 0; done < size; done += PCM_SAMPLE_SIZE)
+    for (size_t done = 0; done < src_size; done += PCM_SAMPLE_SIZE)
     {
-        int32_t sweep = (1 << 14) - fp14_cos(180*done / size); /* 0.0..2.0 */
+        int32_t sweep = (1 << 14) - fp14_cos(180*done / src_size); /* 0.0..2.0 */
         uint32_t f_l = fp_mul(sweep, diff_l, 15) + factor_l;
         uint32_t f_r = fp_mul(sweep, diff_r, 15) + factor_r;
+#if defined(HAVE_SWVOL_32)
+        /* do not clip to 16 bits */
+        *d++ = pcm_scale_sample(f_l, *s++);
+        *d++ = pcm_scale_sample(f_r, *s++);
+#else
         *d++ = clip_sample_16(pcm_scale_sample(f_l, *s++));
         *d++ = clip_sample_16(pcm_scale_sample(f_r, *s++));
+#endif
     }
 
     /* Select steady-state operation */
@@ -133,9 +172,9 @@ static void * pcm_scale_buffer_trans(void *dst, const void *src, size_t size)
 #ifndef PCM_SW_VOLUME_UNBUFFERED
 static inline
 #endif
-void pcm_sw_volume_copy_buffer(void *dst, const void *src, size_t size)
+void pcm_sw_volume_copy_buffer(void *dst, const void *src, size_t src_size)
 {
-    pcm_scaling_fn(dst, src, size);
+    pcm_scaling_fn(dst, src, src_size);
 }
 
 /* Assign the new scaling function for normal steady-state operation */
@@ -147,6 +186,13 @@ void pcm_sync_pcm_factors(void)
     pcm_factor_l = new_factor_l;
     pcm_factor_r = new_factor_r;
 
+/* NOTE: 32-bit scaling is limited to 0 db <--> -74 db, we will hardcode to cut.
+ *       MEMCPY CANNOT BE USED, because we do need to at minimum multiply each
+ *       sample up to 32-bit size. */
+#if defined(HAVE_SWVOL_32)
+    pcm_scaling_fn = pcm_scale_buffer_cut;
+#else
+
     if (new_factor_l == PCM_FACTOR_UNITY &&
         new_factor_r == PCM_FACTOR_UNITY)
     {
@@ -161,6 +207,7 @@ void pcm_sync_pcm_factors(void)
     {
         pcm_scaling_fn = pcm_scale_buffer_boost;
     }
+#endif
 }
 
 #ifndef PCM_SW_VOLUME_UNBUFFERED
@@ -168,10 +215,10 @@ void pcm_sync_pcm_factors(void)
 static const void * volatile src_buf_addr = NULL;
 static size_t volatile src_buf_rem = 0;
 
-#define PCM_PLAY_DBL_BUF_SIZE (PCM_PLAY_DBL_BUF_SAMPLE*PCM_SAMPLE_SIZE)
+#define PCM_PLAY_DBL_BUF_SIZE (PCM_PLAY_DBL_BUF_SAMPLE*PCM_VOL_SAMPLE_SIZE)
 
 /* double buffer and frame length control */
-static int16_t pcm_dbl_buf[2][PCM_PLAY_DBL_BUF_SAMPLES*2]
+static PCM_DBL_BUF_SIZE_T pcm_dbl_buf[2][PCM_PLAY_DBL_BUF_SAMPLES*2]
         PCM_DBL_BUF_BSS MEM_ALIGN_ATTR;
 static size_t pcm_dbl_buf_size[2];
 static int pcm_dbl_buf_num = 0;
@@ -209,11 +256,12 @@ bool pcm_play_dma_complete_callback(enum pcm_dma_status status,
    in one chunk */
 static void update_frame_params(size_t size)
 {
-    int count    = size / PCM_SAMPLE_SIZE;
+    /* multiply by 2 for 32 bit, optimize away to 1 for 16 bit */
+    int count    = (size * (sizeof(PCM_DBL_BUF_SIZE_T)/sizeof(int16_t))) / PCM_VOL_SAMPLE_SIZE;
     frame_count  = (count + PCM_PLAY_DBL_BUF_SAMPLES - 1) /
                    PCM_PLAY_DBL_BUF_SAMPLES;
     int perframe = count / frame_count;
-    frame_size   = perframe * PCM_SAMPLE_SIZE;
+    frame_size   = perframe * PCM_VOL_SAMPLE_SIZE;
     frame_frac   = count - perframe * frame_count;
     frame_err    = 0;
 }
@@ -225,7 +273,8 @@ pcm_play_dma_status_callback_int(enum pcm_dma_status status)
     if (status != PCM_DMAST_STARTED)
         return status;
 
-    size_t size = pcm_dbl_buf_size[pcm_dbl_buf_num];
+    /* divide by 2 for 32 bit, optimize away to 1 for 16 bit */
+    size_t size = pcm_dbl_buf_size[pcm_dbl_buf_num] / (sizeof(PCM_DBL_BUF_SIZE_T)/sizeof(int16_t));
     const void *addr = src_buf_addr + size;
 
     size = src_buf_rem - size;
@@ -241,7 +290,8 @@ pcm_play_dma_status_callback_int(enum pcm_dma_status status)
 
     if (size != 0)
     {
-        size = frame_size;
+        /* multiply by 2 for 32 bit, optimize away to 1 for 16 bit */
+        size = frame_size / (sizeof(PCM_DBL_BUF_SIZE_T)/sizeof(int16_t));
 
         if ((frame_err += frame_frac) >= frame_count)
         {
@@ -251,7 +301,8 @@ pcm_play_dma_status_callback_int(enum pcm_dma_status status)
     }
 
     pcm_dbl_buf_num ^= 1;
-    pcm_dbl_buf_size[pcm_dbl_buf_num] = size;
+    /* multiply by 2 for 32 bit, optimize away to 1 for 16 bit */
+    pcm_dbl_buf_size[pcm_dbl_buf_num] = size * (sizeof(PCM_DBL_BUF_SIZE_T)/sizeof(int16_t));
     pcm_sw_volume_copy_buffer(pcm_dbl_buf[pcm_dbl_buf_num], addr, size);
 
     return PCM_DMAST_OK;
@@ -269,6 +320,7 @@ static void start_pcm(bool reframe)
     if (reframe)
         update_frame_params(src_buf_rem);
 
+
     pcm_play_dma_status_callback(PCM_DMAST_STARTED);
     pcm_play_dma_status_callback(PCM_DMAST_STARTED);
 
@@ -278,7 +330,8 @@ static void start_pcm(bool reframe)
 void pcm_play_dma_start_int(const void *addr, size_t size)
 {
     src_buf_addr = addr;
-    src_buf_rem = size;
+    /* divide by 2 for 32 bit, optimize away to 1 for 16 bit */
+    src_buf_rem = size / (sizeof(PCM_DBL_BUF_SIZE_T)/sizeof(int16_t));
     start_pcm(true);
 }
 
@@ -299,13 +352,32 @@ static uint32_t pcm_centibels_to_factor(int volume)
 {
     if (volume == PCM_MUTE_LEVEL)
         return 0; /* mute */
-
+#if defined(HAVE_SWVOL_32)
+    /*
+     * 32-bit software volume taken from pcm-alsa.c
+     */
+    volume += 48; /* -42dB .. 0dB => 5dB .. 48dB */
+    /* NOTE if vol_dB = 5 then vol_shift = 1 but r = 1 so we do vol_shift - 1 >= 0
+     * otherwise vol_dB >= 0 implies vol_shift >= 2 so vol_shift - 2 >= 0 */
+    int vol_shift = volume / 3;
+    int r = volume % 3;
+    int32_t dig_vol_mult;
+    if(r == 0)
+        dig_vol_mult = 1 << vol_shift;
+    else if(r == 1)
+        dig_vol_mult = 1 << vol_shift | 1 << (vol_shift - 2);
+    else
+        dig_vol_mult = 1 << vol_shift | 1 << (vol_shift - 1);
+    return dig_vol_mult;
+#else /* standard software volume */
     /* Centibels -> fixedpoint */
     return (uint32_t)fp_factor(fp_div(volume, 10, PCM_SW_VOLUME_FRACBITS),
                                PCM_SW_VOLUME_FRACBITS);
+#endif /* HAVE_SWVOL_32 */
 }
 
 
+
 /** Public functions **/
 
 /* Produce final pcm scale factor */
diff --git a/firmware/target/mips/ingenic_x1000/pcm-x1000.c b/firmware/target/mips/ingenic_x1000/pcm-x1000.c
index a3da3411f2..ce2fbb17a9 100644
--- a/firmware/target/mips/ingenic_x1000/pcm-x1000.c
+++ b/firmware/target/mips/ingenic_x1000/pcm-x1000.c
@@ -66,10 +66,17 @@ void pcm_play_dma_init(void)
     /* Let the target initialize its hardware and setup the AIC */
     audiohw_init();
 
+#if (PCM_NATIVE_BITDEPTH > 16)
+    /* Program audio format (stereo, 24 bit samples) */
+    jz_writef(AIC_CCR, PACK16(0), CHANNEL_V(STEREO),
+              OSS_V(24BIT), ISS_V(24BIT), M2S(0));
+    jz_writef(AIC_I2SCR, SWLH(0));
+#else
     /* Program audio format (stereo, packed 16 bit samples) */
     jz_writef(AIC_CCR, PACK16(1), CHANNEL_V(STEREO),
               OSS_V(16BIT), ISS_V(16BIT), M2S(0));
     jz_writef(AIC_I2SCR, SWLH(0));
+#endif
 
     /* Set DMA settings */
     jz_writef(AIC_CFG, TFTH(16), RFTH(16));