From dad27f76baa6d1ed8fe2a7bb1b6dc1926bc16fb7 Mon Sep 17 00:00:00 2001
From: Joel Bodenmann <joel@unormal.org>
Date: Sat, 28 Jun 2014 06:33:47 +0200
Subject: [PATCH] fixing volume control for vs1053

---
 drivers/gaudio/vs1053/gaudio_play_lld.c | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/drivers/gaudio/vs1053/gaudio_play_lld.c b/drivers/gaudio/vs1053/gaudio_play_lld.c
index b9c762e6..8e7fb0a5 100644
--- a/drivers/gaudio/vs1053/gaudio_play_lld.c
+++ b/drivers/gaudio/vs1053/gaudio_play_lld.c
@@ -278,11 +278,19 @@ bool_t gaudio_play_lld_init(uint16_t channel, uint32_t frequency, ArrayDataForma
 }
 
 bool_t gaudio_play_lld_set_volume(uint8_t vol) {
+	uint16_t tmp;
+
 	// Volume is 0xFE -> 0x00. Adjust vol to match
 	vol = ~vol;
-	if (vol == 0xFF) vol = 0xFE;
+	if (vol > 0xFE)
+		vol = 0xFE;
+
+	tmp  = 0;
+	tmp |= (( vol << VOL_LEFT_SHIFT ) & VOL_LEFT_MASK );
+	tmp |= (( vol << VOL_RIGHT_SHIFT ) & VOL_RIGHT_MASK );
+
+	cmd_write(SCI_VOL, tmp);
 
-	cmd_write(SCI_VOL, ((uint16_t)vol) << 8 | vol);
 	return TRUE;
 }