Diff: drivers/I2S.cpp

Revision:

4:21603d68bcf7

Parent:

0:752e74bf5ef1

Child:

9:c4c2240e06d6

--- a/drivers/I2S.cpp	Wed Dec 14 14:23:12 2016 +0100
+++ b/drivers/I2S.cpp	Wed Dec 21 20:24:16 2016 +0100
@@ -273,6 +273,168 @@
 #endif
 }
 
+float I2S::compute_real_frequency(I2S *dev_i2s)
+{
+    uint32_t i2sclk = 0U, i2sdiv = 2U, i2sodd = 0U, packetlength = 1U, tmp = 0U;
+    I2S_HandleTypeDef *hi2s;
+
+    /* Get the I2S handle. */
+    hi2s = i2s_get_handle(&(dev_i2s->_i2s));
+
+    /* Check the frame length (For the Prescaler computing). */
+    if (hi2s->Init.DataFormat != I2S_DATAFORMAT_16B)
+    {
+        /* Packet length is 32 bits */
+        packetlength = 2U;
+    }
+
+    /* Get I2S source Clock frequency. */
+    i2sclk = I2S_GetInputClock(hi2s);
+
+    /* Compute the Real divider depending on the MCLK output state, with a floating point. */
+    if (hi2s->Init.MCLKOutput == I2S_MCLKOUTPUT_ENABLE)
+    {
+        /* MCLK output is enabled. */
+        tmp = (uint32_t)(((((i2sclk / 256U) * 10U) / hi2s->Init.AudioFreq)) + 5U);
+    }
+    else
+    {
+        /* MCLK output is disabled. */
+        tmp = (uint32_t)(((((i2sclk / (32U * packetlength)) * 10U) / hi2s->Init.AudioFreq)) + 5U);
+    }
+
+    /* Remove the flatting point. */
+    tmp = tmp / 10U;
+
+    /* Check the parity of the divider. */
+    i2sodd = (uint32_t)(tmp & (uint32_t)1U);
+
+    /* Compute the i2sdiv prescaler. */
+    i2sdiv = (uint32_t)((tmp - i2sodd) / 2U);
+
+    /* Test if the divider is 1 or 0 or greater than 0xFF. */
+    if ((i2sdiv < 2U) || (i2sdiv > 0xFFU))
+    {
+        /* Set the default values. */
+        i2sdiv = 2U;
+        i2sodd = 0U;
+    }
+
+    /* Compute the I2S frequencies. */
+    uint32_t format_factor = (hi2s->Init.DataFormat == I2S_DATAFORMAT_16B ? 16 : 32);
+    uint32_t mclk_factor = (hi2s->Init.MCLKOutput == I2S_MCLKOUTPUT_ENABLE ? (format_factor == 16 ? 8 : 4) : 1);
+    float f = i2sclk / (2 * format_factor * ((2 * i2sdiv) + i2sodd) * mclk_factor);
+
+    return f;
+}
+
+/** Computes the two-div-plus-odd factor of a given I2S objects
+ *  on a desired frequency.
+ *
+ *  @param dev_i2s reference to the I2S object.
+ *  @frequency the desired frequency.
+ *  @return the computed two-div-plus-odd factor.
+ */
+float I2S::compute_magic_factor(I2S *dev_i2s, float f)
+{
+    uint32_t i2sclk = 0U, i2sdiv = 2U, i2sodd = 0U, packetlength = 1U, tmp = 0U;
+    I2S_HandleTypeDef *hi2s;
+
+    /* Get the I2S handle. */
+    hi2s = i2s_get_handle(&(dev_i2s->_i2s));
+
+    /* Get I2S source Clock frequency. */
+    i2sclk = I2S_GetInputClock(hi2s);
+    /* Compute the I2S frequencies. */
+    uint32_t format_factor = (hi2s->Init.DataFormat == I2S_DATAFORMAT_16B ? 16 : 32);
+    uint32_t mclk_factor = (hi2s->Init.MCLKOutput == I2S_MCLKOUTPUT_ENABLE ? (format_factor == 16 ? 8 : 4) : 1);
+    float mf = i2sclk / (2 * format_factor * f * mclk_factor);
+
+    return mf;
+}
+
+float I2S::compute_desired_frequency(I2S *dev_i2s, float mf)
+{
+    uint32_t i2sclk = 0U, i2sdiv = 2U, i2sodd = 0U, packetlength = 1U, tmp = 0U;
+    I2S_HandleTypeDef *hi2s;
+
+    /* Get the I2S handle. */
+    hi2s = i2s_get_handle(&(dev_i2s->_i2s));
+
+    /* Get I2S source Clock frequency. */
+    i2sclk = I2S_GetInputClock(hi2s);
+
+    /* Compute the I2S frequencies. */
+    uint32_t format_factor = (hi2s->Init.DataFormat == I2S_DATAFORMAT_16B ? 16 : 32);
+    uint32_t mclk_factor = (hi2s->Init.MCLKOutput == I2S_MCLKOUTPUT_ENABLE ? (format_factor == 16 ? 8 : 4) : 1);
+    float f = i2sclk / (2 * format_factor * mf * mclk_factor);
+
+    return f;
+}
+
+int I2S::harmonize(I2S *dev_i2s_1, I2S *dev_i2s_2)
+{
+    if (dev_i2s_1->_hz == dev_i2s_2->_hz)
+        return 0;
+
+    /* Compute the real frequencies. */
+    float f1 = compute_real_frequency(dev_i2s_1);
+    float f2 = compute_real_frequency(dev_i2s_2);
+
+    //printf("REAL: %f %f\r\n", f1, f2);
+
+    /* Compute the desired frequencies so that they are multiple one of the
+       other. */
+    float q;
+    if (f1 < f2)
+        q = f2 / f1;
+    else
+        q = f1 / f2;
+    float r = q - (uint32_t) q;
+
+    if (r > 0)
+    {
+        if (f1 < f2)
+        {
+            float mf = compute_magic_factor(dev_i2s_1, f2 / 2);
+            r = mf - (uint32_t) mf;
+            if (r > 0)
+            {
+                if (f2 > dev_i2s_2->_hz)
+                    f1 = compute_desired_frequency(dev_i2s_1, ((uint32_t) mf) + 1);
+                else
+                    f1 = compute_desired_frequency(dev_i2s_1, ((uint32_t) mf) - 1);
+                f2 = 2 * f1;
+            }
+            else
+                f1 = f2 / 2;
+        }
+        else
+        {
+            float mf = compute_magic_factor(dev_i2s_2, f1 / 2);
+            r = mf - (uint32_t) mf;
+            if (r > 0)
+            {
+                if (f1 > dev_i2s_1->_hz)
+                    f2 = compute_desired_frequency(dev_i2s_2, ((uint32_t) mf) + 1);
+                else
+                    f2 = compute_desired_frequency(dev_i2s_2, ((uint32_t) mf) - 1);
+                f1 = 2 * f2;
+            }
+            else
+                f2 = f1 / 2;
+        }
+    }
+
+    //printf("DESIRED: %f %f\r\n", f1, f2);
+
+    /* Set the desired frequencies. */
+    dev_i2s_1->audio_frequency(f1);
+    dev_i2s_2->audio_frequency(f2);
+
+    return 0;
+}
+
 } // namespace mbed
 
 #endif