Diff: targets/TARGET_STM/stm_i2s_api.c

Revision:

20:54b1a9b620c5

Parent:

19:ef6ef1795e30

Child:

21:7cf6b1538d29

--- a/targets/TARGET_STM/stm_i2s_api.c	Fri Jan 27 08:35:23 2017 +0100
+++ b/targets/TARGET_STM/stm_i2s_api.c	Mon Jan 30 10:17:44 2017 +0100
@@ -785,61 +785,7 @@
     return (I2S_HandleTypeDef *) &I2sHandle[obj->i2s.module];
 }
 
-/** Computes the two-div-plus-odd factor of a given I2S objects
- *  on a desired frequency.
- *
- *  @param dev_i2s reference to the I2S object.
- *  @param frequency the desired frequency.
- *  @return the computed two-div-plus-odd factor.
- */
-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)roundf(f);
-}
-
-static uint32_t i2s_compute_closest_frequency(i2s_t *dev_i2s, uint32_t target_freq) {
+static float 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;
 
@@ -890,7 +836,7 @@
     uint32_t mclk_factor = (hi2s->Init.MCLKOutput == I2S_MCLKOUTPUT_ENABLE ? (format_factor == 16 ? 8 : 4) : 1);
     float f = ((float)i2sclk / (float)(2 * format_factor * ((2 * i2sdiv) + i2sodd) * mclk_factor));
 
-    return (uint32_t)roundf(f);
+    return f;
 }
 
 /** Compute the real frequency of a given I2S objects.
@@ -898,7 +844,7 @@
  *  @param dev_i2s reference to the I2S object.
  *  @return the computed real frequency.
  */
-static inline uint32_t i2s_compute_real_frequency(i2s_t *dev_i2s)
+static inline float i2s_compute_real_frequency(i2s_t *dev_i2s)
 {
     I2S_HandleTypeDef *hi2s;
 
@@ -918,10 +864,11 @@
  *  @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)
+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,
+		float real_low_freq, float 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 division = real_high_freq / real_low_freq;
     float rest = (division - (uint32_t)division);
     MBED_ASSERT(rest >= 0);
     if (rest == 0) {
@@ -931,25 +878,42 @@
     /* 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)magic_factor;
+    float new_low_freq = real_low_freq;
+    uint32_t new_low_freq_int_ceil = (uint32_t)ceilf(new_low_freq);
+    uint32_t new_low_freq_int_floor = (uint32_t)new_low_freq;
 
-    if(real_high_freq > (*ptr_high_freq)) {
-    	if(magic_factor_floor <= 1) {
-	    return -1;
-        }
+    uint32_t new_low_freq_int;
+    float real_new_low_freq = i2s_compute_closest_frequency(i2s_t_l, new_low_freq_int_floor);
+    if(real_new_low_freq == new_low_freq) {
+    	new_low_freq_int = new_low_freq_int_floor;
+    } else {
+    	real_new_low_freq = i2s_compute_closest_frequency(i2s_t_l, new_low_freq_int_ceil);
+    	if(real_new_low_freq == new_low_freq) {
+    		new_low_freq_int = new_low_freq_int_ceil;
+    	} else {
+    		return -1; // should never happen!
+    	}
     }
 
-    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; // potentially this high frequency might not be achievable!
-    uint32_t real_new_high_freq = i2s_compute_closest_frequency(i2s_t_h, new_high_freq);
+    float new_high_freq = multiplier * new_low_freq;
+    uint32_t new_high_freq_int_ceil = (uint32_t)ceilf(new_high_freq);
+    uint32_t new_high_freq_int_floor = (uint32_t)new_high_freq;
 
-    if(new_high_freq != real_new_high_freq) {
-    	return -1;
+    uint32_t new_high_freq_int;
+    float real_new_high_freq = i2s_compute_closest_frequency(i2s_t_h, new_high_freq_int_ceil);
+    if(real_new_high_freq == new_high_freq) {
+    	new_high_freq_int = new_high_freq_int_ceil;
+    } else {
+    	real_new_high_freq = i2s_compute_closest_frequency(i2s_t_h, new_high_freq_int_floor);
+    	if(real_new_high_freq == new_high_freq) {
+        	new_high_freq_int = new_high_freq_int_floor;
+    	} else {
+    		return -1;
+    	}
     }
 
-    *ptr_low_freq = new_low_freq;
-    *ptr_high_freq = new_high_freq;
+    *ptr_low_freq = new_low_freq_int;
+    *ptr_high_freq = new_high_freq_int;
 
     return 0;
 }
@@ -962,8 +926,8 @@
     }
 
     /* 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);
+    float real_f1 = i2s_compute_real_frequency(dev_i2s_1);
+    float real_f2 = i2s_compute_real_frequency(dev_i2s_2);
 
     /* Returning if the two real frequencies are already equal. */
     if(real_f1 == real_f2) {