Diff: targets/TARGET_STM/stm_i2s_api.c

Revision:

14:0060a9850c5f

Parent:

13:fa1b24df9025

Child:

15:13ee1f813328

--- a/targets/TARGET_STM/stm_i2s_api.c	Fri Dec 23 12:27:30 2016 +0100
+++ b/targets/TARGET_STM/stm_i2s_api.c	Fri Jan 20 11:06:07 2017 +0100
@@ -785,13 +785,61 @@
     return (I2S_HandleTypeDef *) &I2sHandle[obj->i2s.module];
 }
 
-/** Compute the real frequency of a given I2S objects.
+/** Computes the two-div-plus-odd factor of a given I2S objects
+ *  on a desired frequency.
  *
  *  @param dev_i2s reference to the I2S object.
- *  @return the computed real frequency.
+ *  @param frequency the desired frequency.
+ *  @return the computed two-div-plus-odd factor.
  */
-static uint32_t i2s_compute_real_frequency(i2s_t *dev_i2s)
+static float i2s_compute_magic_factor(i2s_t *dev_i2s, float frequency)
 {
+    float i2sclk;
+    I2S_HandleTypeDef *hi2s;
+
+    /* Get the I2S handle. */
+    hi2s = i2s_get_handle(dev_i2s);
+
+    /* Get I2S source Clock frequency. */
+    i2sclk = (float)I2S_GetInputClock(hi2s);
+    /* Compute the I2S frequencies. */
+    float format_factor = (float)(hi2s->Init.DataFormat == I2S_DATAFORMAT_16B ? 16 : 32);
+    float mclk_factor = (float)(hi2s->Init.MCLKOutput == I2S_MCLKOUTPUT_ENABLE ? (format_factor == 16 ? 8 : 4) : 1);
+    float mf = i2sclk / ((float)2.0 * format_factor * frequency * mclk_factor);
+
+    return mf;
+}
+
+/** Computing the frequency of an I2S peripheral given a certain
+ *  two-div-plus-odd factor.
+ *
+ *  @param obj reference to an i2s_t structure.
+ *  @param magic_factor the given two-div-plus-odd factor.
+ *  @return the computed frequency.
+ */
+static uint32_t i2s_compute_frequency(i2s_t *obj, int32_t magic_factor)
+{
+    float i2sclk;
+    I2S_HandleTypeDef *hi2s;
+
+    /* Getting the I2S handle. */
+    hi2s = i2s_get_handle(obj);
+
+    /* Getting the I2S source clock frequency. */
+    i2sclk = (float)I2S_GetInputClock(hi2s);
+
+    /* Computing the frequencies. */
+    float format_factor = (float)(hi2s->Init.DataFormat == I2S_DATAFORMAT_16B ? 16 : 32);
+    float mclk_factor = (float)(hi2s->Init.MCLKOutput == I2S_MCLKOUTPUT_ENABLE ? (format_factor == 16 ? 8 : 4) : 1);
+    float f = i2sclk / ((float)2.0 * format_factor * (float)magic_factor * mclk_factor);
+
+    //printf("f = %d / (2 * %d * %f * %d) = %f\r\n", i2sclk, format_factor, magic_factor, mclk_factor, f);
+
+    /* Returning the computed frequency. */
+    return (uint32_t)f;
+}
+
+static uint32_t i2s_compute_closest_frequency(i2s_t *dev_i2s, uint32_t target_freq) {
     uint32_t i2sclk = 0U, i2sdiv = 2U, i2sodd = 0U, packetlength = 1U, tmp = 0U;
     I2S_HandleTypeDef *hi2s;
 
@@ -812,12 +860,12 @@
     if (hi2s->Init.MCLKOutput == I2S_MCLKOUTPUT_ENABLE)
     {
         /* MCLK output is enabled. */
-        tmp = (uint32_t)(((((i2sclk / 256U) * 10U) / hi2s->Init.AudioFreq)) + 5U);
+        tmp = (uint32_t)(((((i2sclk / 256U) * 10U) / target_freq)) + 5U);
     }
     else
     {
         /* MCLK output is disabled. */
-        tmp = (uint32_t)(((((i2sclk / (32U * packetlength)) * 10U) / hi2s->Init.AudioFreq)) + 5U);
+        tmp = (uint32_t)(((((i2sclk / (32U * packetlength)) * 10U) / target_freq)) + 5U);
     }
 
     /* Remove the flatting point. */
@@ -845,32 +893,68 @@
     return f;
 }
 
-#if 0 // betzw
-/** Computes the two-div-plus-odd factor of a given I2S objects
- *  on a desired frequency.
+/** Compute the real frequency of a given I2S objects.
  *
  *  @param dev_i2s reference to the I2S object.
- *  @frequency the desired frequency.
- *  @return the computed two-div-plus-odd factor.
+ *  @return the computed real frequency.
  */
-static float i2s_compute_magic_factor(i2s_t *dev_i2s, float f)
+static inline uint32_t i2s_compute_real_frequency(i2s_t *dev_i2s)
 {
-    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;
+    return i2s_compute_closest_frequency(dev_i2s, hi2s->Init.AudioFreq);
 }
 
+/** Computing the harmonized frequencies of two I2S peripherals.
+ *
+ *  @param[in]     i2s_t_l reference to the i2s_t structure with the lower frequency.
+ *  @param[in]     i2s_t_h reference to the i2s_t structure with the higher frequency.
+ *  @param[in|out] ptr_low_freq  pointer to lower frequency.
+ *  @param[in|out] ptr_high_freq  pointer to higher frequency.
+ *  @param[in]     real_low_freq real higher frequency.
+ *  @param[in]     real_high_freq real lower frequency.
+ *  @return "0" if the frequencies have been harmonized, "-1" otherwise.
+ */
+static int8_t i2s_compute_harmonized_frequencies(i2s_t *i2s_t_l, i2s_t *i2s_t_h, uint32_t *ptr_low_freq, uint32_t *ptr_high_freq, uint32_t real_low_freq, uint32_t real_high_freq)
+{
+    /* Returning if the two real frequencies are already multiple one of the other. */
+    float division = (float)real_high_freq / (float)real_low_freq;
+    float rest = (division - floorf(division));
+    MBED_ASSERT(rest >= 0);
+    if (rest == 0) {
+        return 0;
+    }
+
+    /* Computing the harmonized frequencies so that they are multiple one of the
+       other by a certain factor. */
+    uint32_t multiplier = ((rest >= 0.5) ? ((uint32_t)division + 1) : (uint32_t)division);
+    float magic_factor = i2s_compute_magic_factor(i2s_t_l, (float)real_high_freq / (float)multiplier);
+    uint32_t magic_factor_floor = (uint32_t)floorf(magic_factor);
+
+    if(real_high_freq > (*ptr_high_freq)) {
+    	if(magic_factor_floor <= 1) {
+	    return -1;
+        }
+    }
+
+    uint32_t new_low_freq = i2s_compute_frequency(i2s_t_l, magic_factor_floor + ((real_high_freq > (*ptr_high_freq)) ? 1 : -1));
+    uint32_t new_high_freq = multiplier * new_low_freq; // betzw: potentially this high frequency might not be achievable!
+    uint32_t real_new_high_freq = i2s_compute_closest_frequency(i2s_t_h, new_high_freq);
+
+    if(new_high_freq != real_new_high_freq) {
+    	return -1;
+    }
+
+    *ptr_low_freq = new_low_freq;
+    *ptr_high_freq = new_high_freq;
+
+    return 0;
+}
+
+#if 0 // betzw
 /** Compute the desired frequency of a given I2S objects, given the magic
  *  factor.
  *
@@ -896,7 +980,6 @@
 
     return f;
 }
-#endif // 0
 
 static uint32_t i2s_compute_closest_frequency(i2s_t *dev_i2s, uint32_t target_freq, uint8_t *mclk_enabled) {
     uint32_t i2sclk = 0U, i2sdiv = 2U, i2sodd = 0U, packetlength = 1U, tmp = 0U;
@@ -1016,38 +1099,32 @@
     /* rest mclk enabled */
     hi2s->Init.MCLKOutput = I2S_MCLKOUTPUT_DISABLE;
 }
-
-static inline uint32_t i2s_align_freq(uint32_t real_freq) {
-	uint32_t ret = (uint32_t)real_freq;
-
-	/* align to used digital audio frequencies */
-	if((ret == 11025) || (ret == 44056)) return ret;
-
-	/* align to 10 */
-	ret /= 10;
-	ret *= 10;
-
-	if((ret == 22050) || (ret == 47250)) return ret;
-
-	/* align to 100 */
-	ret /= 100;
-	ret *= 100;
-
-	return ret;
-}
+#endif // 0
 
 int8_t i2s_harmonize(i2s_t *dev_i2s_1, uint32_t *freq_i2s_1, i2s_t *dev_i2s_2, uint32_t *freq_i2s_2)
 {
-    /* Compute the real frequencies. */
-    uint32_t f1 = i2s_compute_real_frequency(dev_i2s_1);
-    uint32_t f2 = i2s_compute_real_frequency(dev_i2s_2);
+    /* Returning if the two set frequencies are not multiple one of the other. */
+    if((*freq_i2s_1 < *freq_i2s_2) ? ((*freq_i2s_2 % *freq_i2s_1) != 0) : ((*freq_i2s_1 % *freq_i2s_2) != 0)) {
+	return -1;
+    }
 
-    if (f1 == f2) {
-        *freq_i2s_1 = i2s_align_freq(f1);
-        *freq_i2s_2 = i2s_align_freq(f2);
+    /* Compute the real frequencies. */
+    uint32_t real_f1 = i2s_compute_real_frequency(dev_i2s_1);
+    uint32_t real_f2 = i2s_compute_real_frequency(dev_i2s_2);
+
+    /* Returning if the two real frequencies are already equal. */
+    if(real_f1 == real_f2) {
         return 0;
     }
 
+    /* Computing the harmonized frequencies. */
+    if(real_f1 < real_f2) {
+    	return i2s_compute_harmonized_frequencies(dev_i2s_1, dev_i2s_2, freq_i2s_1, freq_i2s_2, real_f1, real_f2);
+    } else {
+    	return i2s_compute_harmonized_frequencies(dev_i2s_2, dev_i2s_1, freq_i2s_2, freq_i2s_1, real_f2, real_f1);
+    }
+
+#if 0 // betzw
     //printf("REAL: %f %f\r\n", f1, f2);
 
     /* Compute the desired frequencies so that they are multiple one of the
@@ -1078,13 +1155,13 @@
 
     if(closest_freq == target_freq) {
     	if(target_dev == dev_i2s_1) {
-    		/* Return the desired frequencies. */
-    		*freq_i2s_1 = i2s_align_freq(closest_freq);
-    		*freq_i2s_2 = i2s_align_freq(f2);
+	    /* Return the desired frequencies. */
+	    *freq_i2s_1 = i2s_align_freq(closest_freq);
+	    *freq_i2s_2 = i2s_align_freq(f2);
     	} else {
-    		/* Return the desired frequencies. */
-    		*freq_i2s_1 = i2s_align_freq(f1);
-    		*freq_i2s_2 = i2s_align_freq(closest_freq);
+	    /* Return the desired frequencies. */
+	    *freq_i2s_1 = i2s_align_freq(f1);
+	    *freq_i2s_2 = i2s_align_freq(closest_freq);
     	}
     	return 0;
     }
@@ -1109,13 +1186,13 @@
 
     if(closest_freq == target_freq) {
     	if(target_dev == dev_i2s_1) {
-    		/* Return the desired frequencies. */
-    		*freq_i2s_1 = i2s_align_freq(closest_freq);
-    		*freq_i2s_2 = i2s_align_freq(f2);
+	    /* Return the desired frequencies. */
+	    *freq_i2s_1 = i2s_align_freq(closest_freq);
+	    *freq_i2s_2 = i2s_align_freq(f2);
     	} else {
-    		/* Return the desired frequencies. */
-    		*freq_i2s_1 = i2s_align_freq(f1);
-    		*freq_i2s_2 = i2s_align_freq(closest_freq);
+	    /* Return the desired frequencies. */
+	    *freq_i2s_1 = i2s_align_freq(f1);
+	    *freq_i2s_2 = i2s_align_freq(closest_freq);
     	}
     	return 0;
     }
@@ -1127,61 +1204,6 @@
 
     /* return failure */
     return -1;
-
-#if 0 // betzw - QUESTION: seems to resolve only the case of multiples of 2!?!
-    float q;
-    if (f1 < f2)
-        q = f2 / f1;
-    else
-        q = f1 / f2;
-    float r = q - (uint32_t) q; /* betzw - TODO: call libc functions to get a guarantee 
-				   for actually desired behavior */
-						 
-    if (r > 0)
-    {
-        if (f1 < f2)
-        {
-            float mf = i2s_compute_magic_factor(dev_i2s_1, f2 / 2);
-            r = mf - (uint32_t) mf; /* betzw - TODO: call libc functions to get a guarantee 
-				       for actually desired behavior */
-            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; /* betzw - TODO: call libc functions to get a guarantee 
-				       for actually desired behavior */
-            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;
-        }
-    } else { // betzw - QUESTION for Davide!
-	return -1;
-    }
-
-    //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 // 0
 }