From 0fe725909e118982e25e3810386316ed3923380c Mon Sep 17 00:00:00 2001
From: Kristijan Gjoshev <crypter@mail.com>
Date: Sun, 21 Jan 2018 00:58:42 +0100
Subject: [PATCH] Update I2S base frequency and frequency dividers to real ones
 (#4031)

* Fix I2S base frequency
* Update frequency dividers for I2S
Reference: https://github.com/espressif/ESP8266_MP3_DECODER/blob/master/mp3/driver/i2s_freertos.c
* fixed i2s_set_rate, added i2s_get_real_rate() and i2s_set_dividers
* Minimise float calculations
---
 cores/esp8266/core_esp8266_i2s.c | 34 +++++++++++++++++++++++++++++---
 cores/esp8266/esp8266_peri.h     |  2 +-
 cores/esp8266/i2s.h              |  4 +++-
 3 files changed, 35 insertions(+), 5 deletions(-)

diff --git a/cores/esp8266/core_esp8266_i2s.c b/cores/esp8266/core_esp8266_i2s.c
index 119e8e78a..942a0e73b 100644
--- a/cores/esp8266/core_esp8266_i2s.c
+++ b/cores/esp8266/core_esp8266_i2s.c
@@ -212,15 +212,43 @@ static uint32_t _i2s_sample_rate;
 void ICACHE_FLASH_ATTR i2s_set_rate(uint32_t rate){ //Rate in HZ
   if(rate == _i2s_sample_rate) return;
   _i2s_sample_rate = rate;
-  uint32_t i2s_clock_div = (I2SBASEFREQ/(_i2s_sample_rate*32)) & I2SCDM;
-  uint8_t i2s_bck_div = (I2SBASEFREQ/(_i2s_sample_rate*i2s_clock_div*2)) & I2SBDM;
+
+  uint32_t scaled_base_freq = I2SBASEFREQ/32;
+  float delta_best = scaled_base_freq;
+
+  uint8_t sbd_div_best=1;
+  uint8_t scd_div_best=1;
+  for (uint8_t i=1; i<64; i++){
+    for (uint8_t j=i; j<64; j++){
+      float new_delta = fabs(((float)scaled_base_freq/i/j) - rate);
+      if (new_delta < delta_best){
+      	delta_best = new_delta;
+        sbd_div_best = i;
+        scd_div_best = j;
+      }
+    }
+  }
+
   //os_printf("Rate %u Div %u Bck %u Frq %u\n", _i2s_sample_rate, i2s_clock_div, i2s_bck_div, I2SBASEFREQ/(i2s_clock_div*i2s_bck_div*2));
 
   //!trans master, !bits mod, rece slave mod, rece msb shift, right first, msb right
   I2SC &= ~(I2STSM | (I2SBMM << I2SBM) | (I2SBDM << I2SBD) | (I2SCDM << I2SCD));
-  I2SC |= I2SRF | I2SMR | I2SRSM | I2SRMS | ((i2s_bck_div-1) << I2SBD) | ((i2s_clock_div-1) << I2SCD);
+  I2SC |= I2SRF | I2SMR | I2SRSM | I2SRMS | ((sbd_div_best) << I2SBD) | ((scd_div_best) << I2SCD);
 }
 
+void ICACHE_FLASH_ATTR i2s_set_dividers(uint8_t div1, uint8_t div2){
+  div1 &= I2SBDM;
+  div2 &= I2SCDM;
+
+  I2SC &= ~(I2STSM | (I2SBMM << I2SBM) | (I2SBDM << I2SBD) | (I2SCDM << I2SCD));
+  I2SC |= I2SRF | I2SMR | I2SRSM | I2SRMS | (div1 << I2SBD) | (div2 << I2SCD);
+}
+
+float ICACHE_FLASH_ATTR i2s_get_real_rate(){
+  return (float)I2SBASEFREQ/32/((I2SC>>I2SBD) & I2SBDM)/((I2SC >> I2SCD) & I2SCDM);
+}
+
+
 void ICACHE_FLASH_ATTR i2s_begin(){
   _i2s_sample_rate = 0;
   i2s_slc_begin();
diff --git a/cores/esp8266/esp8266_peri.h b/cores/esp8266/esp8266_peri.h
index 3610cb693..01f2a417c 100644
--- a/cores/esp8266/esp8266_peri.h
+++ b/cores/esp8266/esp8266_peri.h
@@ -754,7 +754,7 @@ extern uint8_t esp8266_gpioToFn[16];
 #define i2c_bbpll_en_audio_clock_out_msb  7
 #define i2c_bbpll_en_audio_clock_out_lsb  7
 #define I2S_CLK_ENABLE()                  i2c_writeReg_Mask_def(i2c_bbpll, i2c_bbpll_en_audio_clock_out, 1)
-#define I2SBASEFREQ                       (12000000L)
+#define I2SBASEFREQ                       (160000000L)
 
 #define I2STXF  ESP8266_REG(0xe00) //I2STXFIFO (32bit)
 #define I2SRXF  ESP8266_REG(0xe04) //I2SRXFIFO (32bit)
diff --git a/cores/esp8266/i2s.h b/cores/esp8266/i2s.h
index fd8e0ab0e..64e306d65 100644
--- a/cores/esp8266/i2s.h
+++ b/cores/esp8266/i2s.h
@@ -43,6 +43,8 @@ extern "C" {
 void i2s_begin();
 void i2s_end();
 void i2s_set_rate(uint32_t rate);//Sample Rate in Hz (ex 44100, 48000)
+void i2s_set_dividers(uint8_t div1, uint8_t div2);//Direct control over output rate
+float i2s_get_real_rate();//The actual Sample Rate on output
 bool i2s_write_sample(uint32_t sample);//32bit sample with channels being upper and lower 16 bits (blocking when DMA is full)
 bool i2s_write_sample_nb(uint32_t sample);//same as above but does not block when DMA is full and returns false instead
 bool i2s_write_lr(int16_t left, int16_t right);//combines both channels and calls i2s_write_sample with the result
@@ -54,4 +56,4 @@ int16_t i2s_available();// returns the number of samples than can be written bef
 }
 #endif
 
-#endif
\ No newline at end of file
+#endif