diff --git a/firmware/pcm.c b/firmware/pcm.c
index 94b0d6e..9e2b900 100644
--- a/firmware/pcm.c
+++ b/firmware/pcm.c
@@ -258,6 +258,21 @@ bool pcm_is_initialized(void)
return pcm_is_ready;
}

+static inline int32_t FRACMUL(int32_t x, int32_t y)
+{
+ return (int32_t) (((int64_t)x * y) >> 31);
+}
+
+static inline void pcm_apply_gain(const void *addr, size_t s)
+{
+ int16_t *pcm = (int16_t *)addr; /* cast away constness, yay */
+ int32_t gain = 0x7fffffff>>5;
+ for (unsigned i = 0; i < s/2; i++)
+ {
+ pcm[i] = FRACMUL(pcm[i], gain);
+ }
+}
+
/* Common code to pcm_play_data and pcm_play_pause */
static void pcm_play_data_start(const void *addr, size_t size)
{
@@ -281,6 +296,7 @@ static void pcm_play_data_start(const void *addr, size_t size)
{
logf(" pcm_play_dma_start");
pcm_apply_settings();
+ pcm_apply_gain(addr, size);
pcm_play_dma_start(addr, size);
pcm_playing = true;
pcm_paused = false;
@@ -327,9 +343,11 @@ bool pcm_play_dma_complete_callback(enum pcm_dma_status status,
/* Call registered callback to obtain next buffer */
get_more(addr, size);
ALIGN_AUDIOBUF(*addr, *size);
-
if (*addr && *size)
+ {
+ pcm_apply_gain(*addr, *size);
return true;
+ }
}

/* Error, callback missing or no more DMA to do */