1 files changed, 20 insertions, 2 deletions

diff --git a/firmware/drivers/wm8758.c b/firmware/drivers/wm8758.c
index aec6f3b598..7c9ac77395 100644
--- a/firmware/drivers/wm8758.c
+++ b/firmware/drivers/wm8758.c
@@ -86,8 +86,7 @@ void wm8758_write(int reg, int data)
  * Note, I'm using the WM8750 datasheet as its apparently close.
  */
 int wmcodec_init(void) {
-    /* reset I2C */
-    i2c_init();
+    int old_irq_level = set_irq_level(HIGHEST_IRQ_LEVEL);
 
     /* normal outputs for CDI and I2S pin groups */
     outl(inl(0x70000020) & ~0x300, 0x70000020);
@@ -109,6 +108,7 @@ int wmcodec_init(void) {
     /* external dev clock to 24MHz */
     outl(inl(0x70000018) & ~0xc, 0x70000018);
 
+    set_irq_level(old_irq_level);
     return 0;
 }
 
@@ -117,6 +117,8 @@ void wmcodec_enable_output(bool enable)
 {
     if (enable) 
     {
+        int old_irq_level = set_irq_level(HIGHEST_IRQ_LEVEL);
+
         /* reset the I2S controller into known state */
         i2s_reset();
 
@@ -140,6 +142,7 @@ void wmcodec_enable_output(bool enable)
         wm8758_write(LOUTMIX,0x1); /* Enable mixer */
         wm8758_write(ROUTMIX,0x1); /* Enable mixer */
         wmcodec_mute(0);
+        set_irq_level(old_irq_level);
     } else {
         wmcodec_mute(1);
     }
@@ -147,6 +150,7 @@ void wmcodec_enable_output(bool enable)
 
 int wmcodec_set_master_vol(int vol_l, int vol_r)
 {
+    int old_irq_level = set_irq_level(HIGHEST_IRQ_LEVEL);
     /* OUT1 */
     wm8758_write(LOUT1VOL, vol_l);
     wm8758_write(ROUT1VOL, 0x100 | vol_r);
@@ -154,6 +158,8 @@ int wmcodec_set_master_vol(int vol_l, int vol_r)
     /* OUT2 */
     wm8758_write(LOUT2VOL, vol_l);
     wm8758_write(ROUT2VOL, 0x100 | vol_r);
+    
+    set_irq_level(old_irq_level);
 
     return 0;
 }
@@ -198,6 +204,8 @@ void wmcodec_set_treble(int value)
 
 int wmcodec_mute(int mute)
 {
+    int old_irq_level = set_irq_level(HIGHEST_IRQ_LEVEL);
+
     if (mute)
     {
         /* Set DACMU = 1 to soft-mute the audio DACs. */
@@ -207,12 +215,16 @@ int wmcodec_mute(int mute)
         wm8758_write(DACCTRL, 0x0);
     }
 
+    set_irq_level(old_irq_level);
+
     return 0;
 }
 
 /* Nice shutdown of WM8758 codec */
 void wmcodec_close(void)
 {
+    int old_irq_level = set_irq_level(HIGHEST_IRQ_LEVEL);
+
     wmcodec_mute(1);
 
     wm8758_write(PWRMGMT3, 0x0);
@@ -220,6 +232,8 @@ void wmcodec_close(void)
     wm8758_write(PWRMGMT1, 0x0);
 
     wm8758_write(PWRMGMT2, 0x40);
+
+    set_irq_level(old_irq_level);
 }
 
 /* Change the order of the noise shaper, 5th order is recommended above 32kHz */
@@ -231,6 +245,8 @@ void wmcodec_set_nsorder(int order)
 /* Note: Disable output before calling this function */
 void wmcodec_set_sample_rate(int sampling_control)
 {
+    int old_irq_level = set_irq_level(HIGHEST_IRQ_LEVEL);
+
     /**** We force 44.1KHz for now. ****/
     (void)sampling_control;
 
@@ -248,6 +264,8 @@ void wmcodec_set_sample_rate(int sampling_control)
 
     /* set srate */
     wm8758_write(SRATECTRL, (0 << 1));
+
+    set_irq_level(old_irq_level);
 }
 
 void wmcodec_enable_recording(bool source_mic)