13 files changed, 146 insertions, 286 deletions

diff --git a/apps/lang/english.lang b/apps/lang/english.lang
index 1e7e5154bc..118a01ebb5 100644
--- a/apps/lang/english.lang
+++ b/apps/lang/english.lang
@@ -11478,3 +11478,32 @@
     *: "Say file type"
   </voice>
 </phrase>
+<phrase>
+  id: LANG_BASS_CUTOFF
+  desc: Bass setting cut-off frequency
+  user:
+  <source>
+    *: "Bass Cutoff"
+  </source>
+  <dest>
+    *: "Bass Cutoff"
+  </dest>
+  <voice>
+    *: "Bass Cutoff"
+  </voice>
+</phrase>
+<phrase>
+  id: LANG_TREBLE_CUTOFF
+  desc: Treble setting cut-off frequency
+  user:
+  <source>
+    *: "Treble Cutoff"
+  </source>
+  <dest>
+    *: "Treble Cutoff"
+  </dest>
+  <voice>
+    *: "Treble Cutoff"
+  </voice>
+</phrase>
+
diff --git a/apps/menus/eq_menu.c b/apps/menus/eq_menu.c
index 4dd8c06c65..2c2e596496 100644
--- a/apps/menus/eq_menu.c
+++ b/apps/menus/eq_menu.c
@@ -46,11 +46,6 @@
 #include "keyboard.h"
 #include "gui/scrollbar.h"
 #include "eq_menu.h"
-#ifdef HAVE_WM8758
-#include "wm8758.h"
-#endif
-
-
 
 /*
  * Utility functions
@@ -667,151 +662,3 @@ MAKE_MENU(equalizer_menu, ID2P(LANG_EQUALIZER), soundmenu_callback, Icon_EQ,
         &eq_enable, &eq_graphical, &eq_precut, &gain_menu, 
         &advanced_eq_menu_, &eq_save, &eq_browse);
 
-
-#ifdef HAVE_WM8758
-
-void eq_hw_gain_format(char* buffer, size_t buffer_size, int value,
-                                    const char* unit)
-{
-    snprintf(buffer, buffer_size, "%d %s", value, unit);
-}
-
-static int band0_callback(int action, const struct menu_item_ex *this_item)
-{
-    (void)this_item;
-    if (action == ACTION_EXIT_MENUITEM)
-    {
-#ifndef SIMULATOR
-        audiohw_set_equalizer_band(0, global_settings.eq_hw_band0_cutoff, 0,
-                                global_settings.eq_hw_band0_gain);
-#endif
-    }
-    return action;
-}
-static int band1_callback(int action, const struct menu_item_ex *this_item)
-{
-    (void)this_item;
-    if (action == ACTION_EXIT_MENUITEM)
-    {
-#ifndef SIMULATOR
-        audiohw_set_equalizer_band(1, global_settings.eq_hw_band1_center,
-                                global_settings.eq_hw_band1_bandwidth,
-                                global_settings.eq_hw_band1_gain);
-#endif
-    }
-    return action;
-}
-static int band2_callback(int action, const struct menu_item_ex *this_item)
-{
-    (void)this_item;
-    if (action == ACTION_EXIT_MENUITEM)
-    {
-#ifndef SIMULATOR
-        audiohw_set_equalizer_band(2, global_settings.eq_hw_band2_center,
-                                global_settings.eq_hw_band2_bandwidth,
-                                global_settings.eq_hw_band2_gain);
-#endif
-    }
-    return action;
-}
-static int band3_callback(int action, const struct menu_item_ex *this_item)
-{
-    (void)this_item;
-    if (action == ACTION_EXIT_MENUITEM)
-    {
-#ifndef SIMULATOR
-        audiohw_set_equalizer_band(3, global_settings.eq_hw_band3_center,
-                                global_settings.eq_hw_band3_bandwidth,
-                                global_settings.eq_hw_band3_gain);
-#endif
-    }
-    return action;
-}
-static int band4_callback(int action, const struct menu_item_ex *this_item)
-{
-    (void)this_item;
-    if (action == ACTION_EXIT_MENUITEM)
-    {
-#ifndef SIMULATOR
-        audiohw_set_equalizer_band(4, global_settings.eq_hw_band4_cutoff, 0,
-                                global_settings.eq_hw_band4_gain);
-#endif
-    }
-    return action;
-}
-void eq_hw_enable(bool enable)
-{
-#ifdef SIMULATOR
-    (void) enable;
-#else
-    if (enable) {
-        audiohw_set_equalizer_band(0, global_settings.eq_hw_band0_cutoff,
-                                   0, global_settings.eq_hw_band0_gain);
-        audiohw_set_equalizer_band(1, global_settings.eq_hw_band1_center,
-                                   global_settings.eq_hw_band1_bandwidth,
-                                   global_settings.eq_hw_band1_gain);
-        audiohw_set_equalizer_band(2, global_settings.eq_hw_band2_center,
-                                   global_settings.eq_hw_band2_bandwidth,
-                                   global_settings.eq_hw_band2_gain);
-        audiohw_set_equalizer_band(3, global_settings.eq_hw_band3_center,
-                                   global_settings.eq_hw_band3_bandwidth,
-                                   global_settings.eq_hw_band3_gain);
-        audiohw_set_equalizer_band(4, global_settings.eq_hw_band4_cutoff,
-                                   0, global_settings.eq_hw_band4_gain);
-    } else {
-        audiohw_set_equalizer_band(0, global_settings.eq_hw_band0_cutoff, 0, 0);
-        audiohw_set_equalizer_band(1, global_settings.eq_hw_band1_center,
-                                   global_settings.eq_hw_band1_bandwidth, 0);
-        audiohw_set_equalizer_band(2, global_settings.eq_hw_band2_center,
-                                   global_settings.eq_hw_band2_bandwidth, 0);
-        audiohw_set_equalizer_band(3, global_settings.eq_hw_band3_center,
-                                   global_settings.eq_hw_band3_bandwidth, 0);
-        audiohw_set_equalizer_band(4, global_settings.eq_hw_band4_cutoff, 0, 0);
-    }
-#endif
-}
-static int hweq_enable_callback(int action, const struct menu_item_ex *this_item)
-{
-    (void)this_item;
-    if (action == ACTION_EXIT_MENUITEM)
-    {
-        eq_hw_enable(global_settings.eq_hw_enabled);
-    }
-    return action;
-}
-MENUITEM_SETTING(hw_eq_enable, &global_settings.eq_hw_enabled, hweq_enable_callback);
-
-MENUITEM_SETTING(hw_eq_cutoff_0, &global_settings.eq_hw_band0_cutoff, band0_callback);
-MENUITEM_SETTING(hw_eq_gain_0, &global_settings.eq_hw_band0_gain, band0_callback);
-MAKE_MENU(hw_eq_band0, ID2P(LANG_EQUALIZER_BAND_LOW_SHELF), NULL, Icon_NOICON, 
-    &hw_eq_cutoff_0, &hw_eq_gain_0);
-
-MENUITEM_SETTING(hw_eq_cutoff_1, &global_settings.eq_hw_band1_center, band1_callback);
-MENUITEM_SETTING(hw_eq_bandwidth_1, &global_settings.eq_hw_band1_bandwidth, band1_callback);
-MENUITEM_SETTING(hw_eq_gain_1, &global_settings.eq_hw_band1_gain, band1_callback);
-MAKE_MENU(hw_eq_band1, "Peak Filter 1", NULL, Icon_NOICON, 
-    &hw_eq_cutoff_1, &hw_eq_bandwidth_1, &hw_eq_gain_1);
-
-MENUITEM_SETTING(hw_eq_cutoff_2, &global_settings.eq_hw_band2_center, band2_callback);
-MENUITEM_SETTING(hw_eq_bandwidth_2, &global_settings.eq_hw_band2_bandwidth, band2_callback);
-MENUITEM_SETTING(hw_eq_gain_2, &global_settings.eq_hw_band2_gain, band2_callback);
-MAKE_MENU(hw_eq_band2, "Peak Filter 2", NULL, Icon_NOICON, 
-    &hw_eq_cutoff_2, &hw_eq_bandwidth_2, &hw_eq_gain_2);
-
-MENUITEM_SETTING(hw_eq_cutoff_3, &global_settings.eq_hw_band3_center, band3_callback);
-MENUITEM_SETTING(hw_eq_bandwidth_3, &global_settings.eq_hw_band3_bandwidth, band3_callback);
-MENUITEM_SETTING(hw_eq_gain_3, &global_settings.eq_hw_band3_gain, band3_callback);
-MAKE_MENU(hw_eq_band3, "Peak Filter 3", NULL, Icon_NOICON, 
-    &hw_eq_cutoff_3, &hw_eq_bandwidth_3, &hw_eq_gain_3);
-
-MENUITEM_SETTING(hw_eq_cutoff_4, &global_settings.eq_hw_band4_cutoff, band4_callback);
-MENUITEM_SETTING(hw_eq_gain_4, &global_settings.eq_hw_band4_gain, band4_callback);
-MAKE_MENU(hw_eq_band4, ID2P(LANG_EQUALIZER_BAND_HIGH_SHELF), NULL, Icon_NOICON, 
-    &hw_eq_cutoff_4, &hw_eq_gain_4);
-
-MAKE_MENU(hw_eq_menu, ID2P(LANG_EQUALIZER_HARDWARE), NULL, Icon_EQ,
-    &hw_eq_enable, &hw_eq_band0, &hw_eq_band1, 
-    &hw_eq_band2, &hw_eq_band3, &hw_eq_band4);
-
-
-#endif
diff --git a/apps/menus/eq_menu.h b/apps/menus/eq_menu.h
index b101ff36ed..ebeedff787 100644
--- a/apps/menus/eq_menu.h
+++ b/apps/menus/eq_menu.h
@@ -44,16 +44,5 @@ bool eq_menu_graphical(void);
 void eq_gain_format(char* buffer, size_t buffer_size, int value, const char* unit);
 void eq_q_format(char* buffer, size_t buffer_size, int value, const char* unit);
 void eq_precut_format(char* buffer, size_t buffer_size, int value, const char* unit);
-#ifdef HAVE_WM8758
-void eq_hw_gain_format(char* buffer, size_t buffer_size, int value,
-                                    const char* unit);
-/* WM8758 equalizer supports -12 to +12 dB gain in 1 dB increments. */
-#define EQ_HW_GAIN_STEP 1
-#define EQ_HW_GAIN_MIN -12
-#define EQ_HW_GAIN_MAX 12
-
-bool eq_hw_menu(void);
-void eq_hw_enable(bool enable);
-#endif
 
 #endif
diff --git a/apps/menus/sound_menu.c b/apps/menus/sound_menu.c
index 4d6a3e2095..127b01d342 100644
--- a/apps/menus/sound_menu.c
+++ b/apps/menus/sound_menu.c
@@ -58,7 +58,13 @@ int soundmenu_callback(int action, const struct menu_item_ex *this_item)
 
 MENUITEM_SETTING(volume, &global_settings.volume, soundmenu_callback);
 MENUITEM_SETTING(bass, &global_settings.bass, soundmenu_callback);
+#ifdef HAVE_WM8758
+MENUITEM_SETTING(bass_cutoff, &global_settings.bass_cutoff, soundmenu_callback);
+#endif
 MENUITEM_SETTING(treble, &global_settings.treble, soundmenu_callback);
+#ifdef HAVE_WM8758
+MENUITEM_SETTING(treble_cutoff, &global_settings.treble_cutoff, soundmenu_callback);
+#endif
 MENUITEM_SETTING(balance, &global_settings.balance, soundmenu_callback);
 MENUITEM_SETTING(channel_config, &global_settings.channel_config, soundmenu_callback);
 MENUITEM_SETTING(stereo_width, &global_settings.stereo_width, soundmenu_callback);
@@ -97,14 +103,18 @@ MENUITEM_SETTING(stereo_width, &global_settings.stereo_width, soundmenu_callback
 
 MAKE_MENU(sound_settings, ID2P(LANG_SOUND_SETTINGS), NULL, Icon_Audio,
           &volume,
-          &bass,&treble,
+          &bass,
+#ifdef HAVE_WM8758
+          &bass_cutoff,
+#endif
+          &treble,
+#ifdef HAVE_WM8758
+          &treble_cutoff,
+#endif
           &balance,&channel_config,&stereo_width
 #if CONFIG_CODEC == SWCODEC
          ,&crossfeed_menu, &equalizer_menu, &dithering_enabled
 #endif
-#ifdef HAVE_WM8758
-         ,&hw_eq_menu
-#endif
 #if (CONFIG_CODEC == MAS3587F) || (CONFIG_CODEC == MAS3539F)
          ,&loudness,&avc,&superbass,&mdb_enable,&mdb_strength
          ,&mdb_harmonics,&mdb_center,&mdb_shape
diff --git a/apps/playback.c b/apps/playback.c
index a880e4ef13..5e42b7629d 100644
--- a/apps/playback.c
+++ b/apps/playback.c
@@ -83,10 +83,6 @@
 #include "talk.h"
 #endif
 
-#ifdef HAVE_WM8758
-#include "menus/eq_menu.h"
-#endif
-
 #define PLAYBACK_VOICE
 
 /* default point to start buffer refill */
@@ -2601,9 +2597,6 @@ void audio_init(void)
     audio_is_initialized = true;
 
     sound_settings_apply();
-#ifdef HAVE_WM8758
-    eq_hw_enable(global_settings.eq_hw_enabled);
-#endif
 #ifndef HAVE_FLASH_STORAGE
     audio_set_buffer_margin(global_settings.buffer_margin);
 #endif
diff --git a/apps/settings.c b/apps/settings.c
index 0b82ad41ac..6467a0e4d6 100644
--- a/apps/settings.c
+++ b/apps/settings.c
@@ -648,6 +648,11 @@ void sound_settings_apply(void)
     sound_set(SOUND_SUPERBASS, global_settings.superbass);
 #endif
 
+#ifdef HAVE_WM8758
+    sound_set(SOUND_BASS_CUTOFF, global_settings.bass_cutoff);
+    sound_set(SOUND_TREBLE_CUTOFF, global_settings.treble_cutoff);
+#endif
+
 #ifdef HAVE_USB_POWER
 #if CONFIG_CHARGING
     usb_charging_enable(global_settings.usb_charging);
diff --git a/apps/settings.h b/apps/settings.h
index e5a2b1790b..369c7fdb4b 100644
--- a/apps/settings.h
+++ b/apps/settings.h
@@ -306,6 +306,11 @@ struct user_settings
     bool superbass; /* true/false */
 #endif
 
+#ifdef HAVE_WM8758
+    int bass_cutoff;
+    int treble_cutoff;
+#endif
+
 #if CONFIG_CODEC == SWCODEC
     int crossfade;     /* Enable crossfade (0=off,1=shuffle,2=trackskip,3=shuff&trackskip,4=always) */
     int crossfade_fade_in_delay;      /* Fade in delay (0-15s)             */
@@ -616,6 +621,7 @@ struct user_settings
     bool dithering_enabled;
 #endif
 
+
 #if LCD_DEPTH > 1
     unsigned char backdrop_file[MAX_FILENAME+1];  /* backdrop bitmap file */
 #endif
@@ -656,27 +662,6 @@ struct user_settings
 #endif
 #endif
 
-#ifdef HAVE_WM8758
-    bool eq_hw_enabled;            /* Enable hardware equalizer */
-    
-    int eq_hw_band0_cutoff;
-    int eq_hw_band0_gain;  
-
-    int eq_hw_band1_center;
-    int eq_hw_band1_bandwidth;
-    int eq_hw_band1_gain;
-
-    int eq_hw_band2_center;
-    int eq_hw_band2_bandwidth;
-    int eq_hw_band2_gain;
-
-    int eq_hw_band3_center;
-    int eq_hw_band3_bandwidth;
-    int eq_hw_band3_gain;
-
-    int eq_hw_band4_cutoff;
-    int eq_hw_band4_gain;
-#endif
     bool hold_lr_for_scroll_in_list; /* hold L/R scrolls the list left/right */
     int show_path_in_browser; /* 0=off, 1=current directory, 2=full path */
 
diff --git a/apps/settings_list.c b/apps/settings_list.c
index 6aa816f6fe..ea1e2f9bcb 100644
--- a/apps/settings_list.c
+++ b/apps/settings_list.c
@@ -1001,6 +1001,12 @@ const struct settings_list settings[] = {
     OFFON_SETTING(0, dithering_enabled, LANG_DITHERING,
         false, "dithering enabled", dsp_dither_enable),
 #endif
+#ifdef HAVE_WM8758
+    SOUND_SETTING(F_NO_WRAP, bass_cutoff, LANG_BASS_CUTOFF, "bass cutoff",
+                  SOUND_BASS_CUTOFF),
+    SOUND_SETTING(F_NO_WRAP, treble_cutoff, LANG_TREBLE_CUTOFF, "treble cutoff",
+                  SOUND_TREBLE_CUTOFF),
+#endif
 #ifdef HAVE_DIRCACHE
     OFFON_SETTING(0,dircache,LANG_DIRCACHE_ENABLE,false,"dircache",NULL),
     SYSTEM_SETTING(NVRAM(4),dircache_size,0),
@@ -1059,63 +1065,6 @@ const struct settings_list settings[] = {
 #endif
 #endif /* HAVE_BACKLIGHT */
 
-#ifdef HAVE_WM8758
-    OFFON_SETTING(0,eq_hw_enabled,LANG_EQUALIZER_HARDWARE_ENABLED,false,
-        "eq hardware enabled",NULL),
-
-    STRINGCHOICE_SETTING(0, eq_hw_band0_cutoff, LANG_EQUALIZER_BAND_CUTOFF, 1,
-        "eq hardware band 0 cutoff", "80Hz,105Hz,135Hz,175Hz", NULL, 4,
-        TALK_ID(80, UNIT_HERTZ), TALK_ID(105, UNIT_HERTZ), 
-        TALK_ID(135, UNIT_HERTZ), TALK_ID(175, UNIT_HERTZ)),
-    INT_SETTING(0, eq_hw_band0_gain, LANG_GAIN, 0, 
-        "eq hardware band 0 gain", UNIT_DB, EQ_HW_GAIN_MIN, 
-        EQ_HW_GAIN_MAX, EQ_HW_GAIN_STEP, eq_hw_gain_format, NULL, NULL),
-
-    STRINGCHOICE_SETTING(0, eq_hw_band1_center, LANG_EQUALIZER_BAND_CENTER, 1,
-        "eq hardware band 1 center", "230Hz,300Hz,385Hz,500Hz", NULL, 4,
-        TALK_ID(230, UNIT_HERTZ), TALK_ID(300, UNIT_HERTZ), 
-        TALK_ID(385, UNIT_HERTZ), TALK_ID(500, UNIT_HERTZ)),
-    CHOICE_SETTING(0, eq_hw_band1_bandwidth, LANG_EQUALIZER_BANDWIDTH, 0, 
-        "eq hardware band 1 bandwidth", "narrow,wide", NULL, 2,
-        ID2P(LANG_EQUALIZER_HARDWARE_BANDWIDTH_NARROW), 
-        ID2P(LANG_EQUALIZER_HARDWARE_BANDWIDTH_WIDE)),
-    INT_SETTING(0, eq_hw_band1_gain, LANG_GAIN, 0, 
-        "eq hardware band 1 gain", UNIT_DB, EQ_HW_GAIN_MIN, 
-        EQ_HW_GAIN_MAX, EQ_HW_GAIN_STEP, eq_hw_gain_format, NULL, NULL),
-
-    STRINGCHOICE_SETTING(0, eq_hw_band2_center, LANG_EQUALIZER_BAND_CENTER, 1,
-        "eq hardware band 2 center", "650Hz,850Hz,1.1kHz,1.4kHz", NULL, 4,
-        TALK_ID(650, UNIT_HERTZ), TALK_ID(850, UNIT_HERTZ), 
-        TALK_ID(1100, UNIT_HERTZ), TALK_ID(1400, UNIT_HERTZ)),
-    CHOICE_SETTING(0, eq_hw_band2_bandwidth, LANG_EQUALIZER_BANDWIDTH, 0, 
-        "eq hardware band 2 bandwidth", "narrow,wide", NULL, 2,
-        ID2P(LANG_EQUALIZER_HARDWARE_BANDWIDTH_NARROW), 
-        ID2P(LANG_EQUALIZER_HARDWARE_BANDWIDTH_WIDE)),
-    INT_SETTING(0, eq_hw_band2_gain, LANG_GAIN, 0, 
-        "eq hardware band 2 gain", UNIT_DB, EQ_HW_GAIN_MIN, 
-        EQ_HW_GAIN_MAX, EQ_HW_GAIN_STEP, eq_hw_gain_format, NULL, NULL),
-
-    STRINGCHOICE_SETTING(0, eq_hw_band3_center, LANG_EQUALIZER_BAND_CENTER, 1,
-        "eq hardware band 3 center", "1.8kHz,2.4kHz,3.2kHz,4.1kHz", NULL, 4,
-        TALK_ID(1800, UNIT_HERTZ), TALK_ID(2400, UNIT_HERTZ), 
-        TALK_ID(3200, UNIT_HERTZ), TALK_ID(4100, UNIT_HERTZ)),
-    CHOICE_SETTING(0, eq_hw_band3_bandwidth, LANG_EQUALIZER_BANDWIDTH, 0, 
-        "eq hardware band 3 bandwidth", "narrow,wide", NULL, 2,
-        ID2P(LANG_EQUALIZER_HARDWARE_BANDWIDTH_NARROW), 
-        ID2P(LANG_EQUALIZER_HARDWARE_BANDWIDTH_WIDE)),
-    INT_SETTING(0, eq_hw_band3_gain, LANG_GAIN, 0, 
-        "eq hardware band 3 gain", UNIT_DB, EQ_HW_GAIN_MIN, 
-        EQ_HW_GAIN_MAX, EQ_HW_GAIN_STEP, eq_hw_gain_format, NULL, NULL),
-
-    STRINGCHOICE_SETTING(0, eq_hw_band4_cutoff, LANG_EQUALIZER_BAND_CUTOFF, 1,
-        "eq hardware band 4 cutoff", "5.3kHz,6.9kHz,9kHz,11.7kHz", NULL, 4,
-        TALK_ID(5300, UNIT_HERTZ), TALK_ID(6900, UNIT_HERTZ), 
-        TALK_ID(9000, UNIT_HERTZ), TALK_ID(11700, UNIT_HERTZ)),
-    INT_SETTING(0, eq_hw_band4_gain, LANG_GAIN, 0, 
-        "eq hardware band 4 gain", UNIT_DB, EQ_HW_GAIN_MIN, 
-        EQ_HW_GAIN_MAX, EQ_HW_GAIN_STEP, eq_hw_gain_format, NULL, NULL),
-#endif
-
     OFFON_SETTING(0,hold_lr_for_scroll_in_list,-1,true,
         "hold_lr_for_scroll_in_list",NULL),
     CHOICE_SETTING(0, show_path_in_browser, LANG_SHOW_PATH, SHOW_PATH_OFF,
diff --git a/firmware/drivers/audio/wm8758.c b/firmware/drivers/audio/wm8758.c
index 1cfac7c7cb..a9f10fbe67 100644
--- a/firmware/drivers/audio/wm8758.c
+++ b/firmware/drivers/audio/wm8758.c
@@ -33,16 +33,22 @@
 
 const struct sound_settings_info audiohw_settings[] = {
     [SOUND_VOLUME]        = {"dB", 0,  1, -58,   6, -25},
-    [SOUND_BASS]          = {"dB", 0,  1,  -6,   9,   0},
-    [SOUND_TREBLE]        = {"dB", 0,  1,  -6,   9,   0},
+    [SOUND_BASS]          = {"dB", 0,  1, -12,  12,   0},
+    [SOUND_TREBLE]        = {"dB", 0,  1, -12,  12,   0},
     [SOUND_BALANCE]       = {"%",  0,  1,-100, 100,   0},
     [SOUND_CHANNELS]      = {"",   0,  1,   0,   5,   0},
     [SOUND_STEREO_WIDTH]  = {"%",  0,  5,   0, 250, 100},
     [SOUND_LEFT_GAIN]     = {"dB", 1,  1,-128,  96,   0},
     [SOUND_RIGHT_GAIN]    = {"dB", 1,  1,-128,  96,   0},
     [SOUND_MIC_GAIN]      = {"dB", 1,  1,-128, 108,  16},
+    [SOUND_BASS_CUTOFF]   = {"",   0,  1,   1,   4,   1},
+    [SOUND_TREBLE_CUTOFF] = {"",   0,  1,   1,   4,   1},
 };
 
+/* shadow registers */
+unsigned int eq1_reg;
+unsigned int eq5_reg;
+
 /* convert tenth of dB volume (-57..6) to master volume register value */
 int tenthdb2master(int db)
 {
@@ -138,15 +144,28 @@ int audiohw_set_mixer_vol(int channel1, int channel2)
     return 0;
 }
 
-/* We are using Linear bass control */
 void audiohw_set_bass(int value)
 {
-    (void)value;
+    eq1_reg = (eq1_reg & ~EQ_GAIN_MASK) | EQ_GAIN_VALUE(value);
+    wmcodec_write(EQ1, 0x100 | eq1_reg);
+}
+
+void audiohw_set_bass_cutoff(int value)
+{
+    eq1_reg = (eq1_reg & ~EQ_CUTOFF_MASK) | EQ_CUTOFF_VALUE(value);
+    wmcodec_write(EQ1, 0x100 | eq1_reg);
 }
 
 void audiohw_set_treble(int value)
 {
-    (void)value;
+    eq5_reg = (eq5_reg & ~EQ_GAIN_MASK) | EQ_GAIN_VALUE(value);
+    wmcodec_write(EQ5, eq5_reg); 
+}
+
+void audiohw_set_treble_cutoff(int value)
+{
+    eq5_reg = (eq5_reg & ~EQ_CUTOFF_MASK) | EQ_CUTOFF_VALUE(value);
+    wmcodec_write(EQ5, eq5_reg); 
 }
 
 void audiohw_mute(bool mute)
@@ -271,27 +290,3 @@ void audiohw_set_monitor(bool enable) {
     (void)enable;
 }
 
-void audiohw_set_equalizer_band(int band, int freq, int bw, int gain)
-{
-    unsigned int eq = 0;
-
-    /* Band 1..3 are peak filters */
-    if (band >= 1 && band <= 3) {
-        eq |= (bw << 8);
-    }
-
-    eq |= (freq << 5);
-    eq |= 12 - gain;
-
-    if (band == 0) {
-        wmcodec_write(EQ1, eq | 0x100); /* Always apply EQ to the DAC path */
-    } else if (band == 1) {
-        wmcodec_write(EQ2, eq);
-    } else if (band == 2) {
-        wmcodec_write(EQ3, eq);
-    } else if (band == 3) {
-        wmcodec_write(EQ4, eq);
-    } else if (band == 4) {
-        wmcodec_write(EQ5, eq);
-    }
-}
diff --git a/firmware/export/audiohw.h b/firmware/export/audiohw.h
index c5093a151a..5dc7550478 100644
--- a/firmware/export/audiohw.h
+++ b/firmware/export/audiohw.h
@@ -67,6 +67,10 @@ enum {
     SOUND_RIGHT_GAIN,
     SOUND_MIC_GAIN,
 #endif
+#ifdef HAVE_WM8758
+    SOUND_BASS_CUTOFF,
+    SOUND_TREBLE_CUTOFF,
+#endif
 };
 
 enum Channel {
diff --git a/firmware/export/sound.h b/firmware/export/sound.h
index 929b83b9e6..5933f4cc75 100644
--- a/firmware/export/sound.h
+++ b/firmware/export/sound.h
@@ -41,6 +41,10 @@ void sound_set_bass(int value);
 void sound_set_treble(int value);
 void sound_set_channels(int value);
 void sound_set_stereo_width(int value);
+#ifdef HAVE_WM8758
+void sound_set_bass_cutoff(int value);
+void sound_set_treble_cutoff(int value);
+#endif
 #if (CONFIG_CODEC == MAS3587F) || (CONFIG_CODEC == MAS3539F)
 void sound_set_loudness(int value);
 void sound_set_avc(int value);
diff --git a/firmware/export/wm8758.h b/firmware/export/wm8758.h
index 6d3220a5fc..df60851550 100644
--- a/firmware/export/wm8758.h
+++ b/firmware/export/wm8758.h
@@ -32,12 +32,12 @@ extern int audiohw_set_master_vol(int vol_l, int vol_r);
 extern int audiohw_set_lineout_vol(int vol_l, int vol_r);
 extern int audiohw_set_mixer_vol(int channel1, int channel2);
 extern void audiohw_set_bass(int value);
+extern void audiohw_set_bass_cutoff(int value);
 extern void audiohw_set_treble(int value);
+extern void audiohw_set_treble_cutoff(int value);
 extern void audiohw_set_nsorder(int order);
 extern void audiohw_set_sample_rate(int sampling_control);
 
-extern void audiohw_set_equalizer_band(int band, int freq, int bw, int gain);
-
 #define RESET      0x00
 #define PWRMGMT1   0x01
 #define PWRMGMT2   0x02
@@ -70,6 +70,10 @@ extern void audiohw_set_equalizer_band(int band, int freq, int bw, int gain);
 #define EQ3        0x14
 #define EQ4        0x15
 #define EQ5        0x16
+#define EQ_GAIN_MASK       0x001f
+#define EQ_CUTOFF_MASK     0x0060
+#define EQ_GAIN_VALUE(x)   (((-x) + 12) & 0x1f)
+#define EQ_CUTOFF_VALUE(x) ((((x) - 1) & 0x03) << 5)
 
 /* Register settings for the supported samplerates: */
 #define WM8758_8000HZ      0x4d
diff --git a/firmware/sound.c b/firmware/sound.c
index 9cb4c28f32..0e1e7c5576 100644
--- a/firmware/sound.c
+++ b/firmware/sound.c
@@ -70,6 +70,10 @@ const struct sound_settings_info audiohw_settings[] = {
     [SOUND_RIGHT_GAIN]    = {"dB", 1,  1,-128,  96,   0},
     [SOUND_MIC_GAIN]      = {"dB", 1,  1,-128, 108,  16},
 #endif
+#if defined(HAVE_WM8758)
+    [SOUND_BASS_CUTOFF]   = {"",   0,  1,   1,   4,   1},
+    [SOUND_TREBLE_CUTOFF] = {"",   0,  1,   1,   4,   1},
+#endif
 };
 #endif
 
@@ -132,6 +136,16 @@ sound_set_type* sound_get_fn(int setting)
             result = sound_set_stereo_width;
             break;
 
+#ifdef HAVE_WM8758
+        case SOUND_BASS_CUTOFF:
+            result = sound_set_bass_cutoff;
+            break;
+
+        case SOUND_TREBLE_CUTOFF:
+            result = sound_set_treble_cutoff;
+            break;
+#endif
+
 #if (CONFIG_CODEC == MAS3587F) || (CONFIG_CODEC == MAS3539F)
         case SOUND_LOUDNESS:
             result = sound_set_loudness;
@@ -219,7 +233,8 @@ static void set_prescaled_volume(void)
 /* The WM codecs listed don't have suitable prescaler functionality, so we let
  * the prescaler stay at 0 for these unless SW tone controls are in use */
 #if defined(HAVE_SW_TONE_CONTROLS) || !(defined(HAVE_WM8975) \
-    || defined(HAVE_WM8731) || defined(HAVE_WM8721) || defined(HAVE_WM8751))
+    || defined(HAVE_WM8731) || defined(HAVE_WM8721) || defined(HAVE_WM8751) \
+    || defined(HAVE_WM8758))
 
     prescale = MAX(current_bass, current_treble);
     if (prescale < 0)
@@ -237,7 +252,7 @@ static void set_prescaled_volume(void)
     dsp_callback(DSP_CALLBACK_SET_PRESCALE, prescale);
 #elif CONFIG_CODEC == MAS3507D
     mas_writereg(MAS_REG_KPRESCALE, prescale_table[prescale/10]);
-#elif defined(HAVE_UDA1380) || defined(HAVE_WM8758)
+#elif defined(HAVE_UDA1380)
     audiohw_set_mixer_vol(tenthdb2mixer(-prescale), tenthdb2mixer(-prescale));
 #endif
 
@@ -492,6 +507,24 @@ void sound_set_stereo_width(int value)
 #endif
 }
 
+#ifdef HAVE_WM8758
+void sound_set_bass_cutoff(int value)
+{
+    if(!audio_is_initialized)
+        return;
+
+    audiohw_set_bass_cutoff(value);
+}
+
+void sound_set_treble_cutoff(int value)
+{
+    if(!audio_is_initialized)
+        return;
+
+    audiohw_set_treble_cutoff(value);
+}
+#endif
+
 #if (CONFIG_CODEC == MAS3587F) || (CONFIG_CODEC == MAS3539F)
 void sound_set_loudness(int value)
 {
@@ -652,6 +685,19 @@ void sound_set_superbass(int value)
     (void)value;
 }
 #endif /* (CONFIG_CODEC == MAS3587F) || (CONFIG_CODEC == MAS3539F) */
+
+#ifdef HAVE_WM8758
+void sound_set_bass_cutoff(int value)
+{
+    (void) value;
+}
+
+void sound_set_treble_cutoff(int value)
+{
+    (void) value;
+}
+#endif /* HAVE_WM8758 */
+
 #endif /* SIMULATOR */
 
 void sound_set(int setting, int value)