Diff: targets/TARGET_STM/stm_i2s_api.c

Revision:

23:6ebde375d2d1

Parent:

22:e04af8667cad

Child:

24:b78825180506

--- a/targets/TARGET_STM/stm_i2s_api.c	Tue Jan 31 11:30:06 2017 +0100
+++ b/targets/TARGET_STM/stm_i2s_api.c	Tue Feb 07 11:16:14 2017 +0100
@@ -864,6 +864,23 @@
     return (hi2s->Init.MCLKOutput == I2S_MCLKOUTPUT_ENABLE);
 }
 
+static inline uint32_t i2s_get_matching_freq(i2s_t *dev_i2s, float freq) {
+    uint32_t freq_int_ceil = (uint32_t)ceilf(freq);
+    uint32_t freq_int_floor = (uint32_t)freq;
+
+    float real_freq = i2s_compute_closest_frequency(dev_i2s, freq_int_ceil);
+    if(real_freq == freq) {
+	return freq_int_ceil;
+    } else {
+	real_freq = i2s_compute_closest_frequency(dev_i2s, freq_int_floor);
+	if(real_freq == freq) {
+	    return freq_int_floor;
+	} else {
+	    return 0;
+	}
+    }
+}
+
 /** Computing the harmonized frequencies of two I2S peripherals.
  *
  *  @param[in]     i2s_t_l reference to the i2s_t structure with the lower frequency.
@@ -890,48 +907,29 @@
     bool low_freq_mclk = i2s_has_mclk(i2s_t_l);
     bool high_freq_mclk = i2s_has_mclk(i2s_t_h);
 
-    uint32_t new_low_freq_int  = *ptr_low_freq;
-    uint32_t new_high_freq_int = *ptr_high_freq;
+    uint32_t new_low_freq_int = i2s_get_matching_freq(i2s_t_l, real_low_freq);
+    uint32_t new_high_freq_int = i2s_get_matching_freq(i2s_t_h, real_high_freq);
+    MBED_ASSERT((new_low_freq_int != 0) && (new_high_freq_int != 0));
 
     /* Computing the harmonized frequencies so that they are multiple one of the
        other by a certain factor. */
     if(low_freq_mclk && !high_freq_mclk) { // start from low frequency
 	float new_high_freq = real_low_freq * multiplier;
-	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;
-
-	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; // should never happen (assuming MCLK always enabled)!
-	    }
+	new_high_freq_int = i2s_get_matching_freq(i2s_t_h, new_high_freq);
+	if(new_high_freq_int == 0) {
+	    return -1; // should never happen!
 	}
     } else { // start from high frequency
 	float new_low_freq = real_high_freq / multiplier;
-	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;
-
-	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; /* might happen for both devices with MCLK disabled & 
-			      higher frequency 16-bit wide and lower frequency 32-bit wide
-			      and odd multiplier!
-			   */
-	    }
+	new_low_freq_int = i2s_get_matching_freq(i2s_t_l, new_low_freq);
+	if(new_low_freq_int == 0) {
+	    return -1; /* might happen for both devices with MCLK disabled & 
+			  higher frequency 16-bit wide and lower frequency 32-bit wide
+			  and odd multiplier!
+		       */
 	}
     }
-
+    
     *ptr_low_freq = new_low_freq_int;
     *ptr_high_freq = new_high_freq_int;