Diff: targets/TARGET_STM/stm_i2s_api.c

Revision:

9:c4c2240e06d6

Parent:

8:561d7ee70ef6

Child:

10:1a612c2e4a85

--- a/targets/TARGET_STM/stm_i2s_api.c	Thu Dec 22 08:59:42 2016 +0100
+++ b/targets/TARGET_STM/stm_i2s_api.c	Thu Dec 22 11:10:10 2016 +0100
@@ -779,9 +779,184 @@
     __HAL_I2S_ENABLE(i2s_handle);
 }
 
-I2S_HandleTypeDef *i2s_get_handle(i2s_t *obj)
+/*** Code for harmonizing frequencies ***/
+static inline I2S_HandleTypeDef *i2s_get_handle(i2s_t *obj)
+{
+    return (I2S_HandleTypeDef *) &I2sHandle[obj->i2s.module];
+}
+
+/** Compute the real frequency of a given I2S objects.
+ *
+ *  @param dev_i2s reference to the I2S object.
+ *  @return the computed real frequency.
+ */
+static float i2s_compute_real_frequency(i2s_t *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);
+
+    /* 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.
+ */
+static float i2s_compute_magic_factor(i2s_t *dev_i2s, float f)
 {
-	return (I2S_HandleTypeDef *) &I2sHandle[obj->i2s.module];
+    uint32_t i2sclk = 0U;
+    I2S_HandleTypeDef *hi2s;
+
+    /* Get the I2S handle. */
+    hi2s = i2s_get_handle(dev_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;
+}
+
+/** Compute the desired frequency of a given I2S objects, given the magic
+ *  factor.
+ *
+ *  @param dev_i2s reference to the I2S object.
+ *  @param mf the two-div-plus-odd factor.
+ *  @return the computed desired frequency.
+ */
+static float i2s_compute_desired_frequency(i2s_t *dev_i2s, float mf)
+{
+    uint32_t i2sclk = 0U;
+    I2S_HandleTypeDef *hi2s;
+
+    /* Get the I2S handle. */
+    hi2s = i2s_get_handle(dev_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;
+}
+
+int8_t i2s_harmonize(i2s_t *dev_i2s_1, uint32_t *freq_i2s_1, i2s_t *dev_i2s_2, uint32_t *freq_i2s_2)
+{
+    if (*freq_i2s_1 == *freq_i2s_2)
+        return 0;
+
+    /* Compute the real frequencies. */
+    float f1 = i2s_compute_real_frequency(dev_i2s_1);
+    float f2 = i2s_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 = i2s_compute_magic_factor(dev_i2s_1, f2 / 2);
+            r = mf - (uint32_t) mf;
+            if (r > 0)
+            {
+                if (f2 > *freq_i2s_2)
+                    f1 = i2s_compute_desired_frequency(dev_i2s_1, ((uint32_t) mf) + 1);
+                else
+                    f1 = i2s_compute_desired_frequency(dev_i2s_1, ((uint32_t) mf) - 1);
+                f2 = 2 * f1;
+            }
+            else
+                f1 = f2 / 2;
+        }
+        else
+        {
+            float mf = i2s_compute_magic_factor(dev_i2s_2, f1 / 2);
+            r = mf - (uint32_t) mf;
+            if (r > 0)
+            {
+                if (f1 > *freq_i2s_1)
+                    f2 = i2s_compute_desired_frequency(dev_i2s_2, ((uint32_t) mf) + 1);
+                else
+                    f2 = i2s_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);
+
+    /* Return the desired frequencies. */
+    *freq_i2s_1 = (uint32_t)f1;
+    *freq_i2s_2 = (uint32_t)f2;
+
+    return 0;
 }
 
 #endif