Diff: targets/TARGET_STM/stm_i2s_api.c

Revision:

13:fa1b24df9025

Parent:

12:7309748f058a

Child:

14:0060a9850c5f

diff -r 7309748f058a -r fa1b24df9025 targets/TARGET_STM/stm_i2s_api.c
--- a/targets/TARGET_STM/stm_i2s_api.c	Thu Dec 22 15:00:34 2016 +0100
+++ b/targets/TARGET_STM/stm_i2s_api.c	Fri Dec 23 12:27:30 2016 +0100
@@ -790,7 +790,7 @@
  *  @param dev_i2s reference to the I2S object.
  *  @return the computed real frequency.
  */
-static float i2s_compute_real_frequency(i2s_t *dev_i2s)
+static uint32_t i2s_compute_real_frequency(i2s_t *dev_i2s)
 {
     uint32_t i2sclk = 0U, i2sdiv = 2U, i2sodd = 0U, packetlength = 1U, tmp = 0U;
     I2S_HandleTypeDef *hi2s;
@@ -840,11 +840,12 @@
     /* 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);
+    uint32_t f = (uint32_t)((float)i2sclk / (float)(2 * format_factor * ((2 * i2sdiv) + i2sodd) * mclk_factor));
 
     return f;
 }
 
+#if 0 // betzw
 /** Computes the two-div-plus-odd factor of a given I2S objects
  *  on a desired frequency.
  *
@@ -895,17 +896,155 @@
 
     return f;
 }
+#endif // 0
 
-// betzw - QUESTION: seems to resolve only the case of multiples of 2!?!
+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;
+    uint32_t f1, f2;
+    I2S_HandleTypeDef *hi2s;
+
+    /* init mclk_enabled */
+    *mclk_enabled = 0;
+
+    /* 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);
+
+    if(hi2s->Init.MCLKOutput == I2S_MCLKOUTPUT_DISABLE) { // betzw: one more degree of freedom, i.e. mclk can be enabled
+        /* MCLK output is disabled. */
+        tmp = (uint32_t)(((((i2sclk / (32U * packetlength)) * 10U) / target_freq)) + 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 frequency */
+        uint32_t format_factor = (hi2s->Init.DataFormat == I2S_DATAFORMAT_16B ? 16 : 32);
+        uint32_t mclk_factor = 1;
+        f1 = (uint32_t)((float)i2sclk / (float)(2 * format_factor * ((2 * i2sdiv) + i2sodd) * mclk_factor));
+
+        if(f1 == target_freq) return f1;
+
+        /* set mclk_enabled */
+        *mclk_enabled = 1;
+    }
+
+    /* Set mclk enabled */
+    hi2s->Init.MCLKOutput = I2S_MCLKOUTPUT_ENABLE;
+
+    /* MCLK output is enabled. */
+    tmp = (uint32_t)(((((i2sclk / 256U) * 10U) / target_freq)) + 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 frequency */
+    uint32_t format_factor = (hi2s->Init.DataFormat == I2S_DATAFORMAT_16B ? 16 : 32);
+    uint32_t mclk_factor = (format_factor == 16 ? 8 : 4);
+
+    f2 = (uint32_t)((float)i2sclk / (float)(2 * format_factor * ((2 * i2sdiv) + i2sodd) * mclk_factor));
+    if(f2 == target_freq) return f2;
+
+    /* return closest result */
+    uint32_t diff1, diff2;
+
+    if(*mclk_enabled != 0) {
+    	if(target_freq > f1) diff1 = target_freq - f1;
+    	else diff1 = f1 - target_freq;
+    } else { // no 'f1' available
+    	return f2;
+    }
+
+    if(target_freq > f2) diff2 = target_freq - f2;
+    else diff2 = f2 - target_freq;
+
+    if(diff1 < diff2) {
+    	/* reset mclk */
+        hi2s->Init.MCLKOutput = I2S_MCLKOUTPUT_DISABLE;
+
+        /* reset mclk_enabled */
+        *mclk_enabled = 0;
+
+        return f1;
+    }
+
+    return f2;
+}
+
+static inline void i2s_reset_mclk(i2s_t *dev_i2s) {
+    I2S_HandleTypeDef *hi2s;
+
+    /* Get the I2S handle. */
+    hi2s = i2s_get_handle(dev_i2s);
+
+    /* 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;
+}
+
 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. */
-    float f1 = i2s_compute_real_frequency(dev_i2s_1);
-    float f2 = i2s_compute_real_frequency(dev_i2s_2);
+    uint32_t f1 = i2s_compute_real_frequency(dev_i2s_1);
+    uint32_t f2 = i2s_compute_real_frequency(dev_i2s_2);
 
     if (f1 == f2) {
-        *freq_i2s_1 = (uint32_t)f1;
-        *freq_i2s_2 = (uint32_t)f2;
+        *freq_i2s_1 = i2s_align_freq(f1);
+        *freq_i2s_2 = i2s_align_freq(f2);
         return 0;
     }
 
@@ -913,6 +1052,83 @@
 
     /* Compute the desired frequencies so that they are multiple one of the
        other. */
+
+    /* figure out multiply value and i2s to adapt and it's target freq */
+    uint32_t multiplier;
+    i2s_t *target_dev;
+    uint32_t target_freq;
+
+    if(f1 > f2) {
+    	multiplier = (uint32_t)((float)f1 / (float)f2);
+    	target_freq = f2 * multiplier;
+    	target_dev = dev_i2s_1;
+    } else {
+    	multiplier = (uint32_t)((float)f2 / (float)f1);
+    	target_freq = f1 * multiplier;
+    	target_dev = dev_i2s_2;
+    }
+
+    if(multiplier == 1) { // nothing that can be reasonably done
+    	return -1;
+    }
+
+    /* compute closest freq */
+    uint8_t mclk_enabled = 0;
+    uint32_t closest_freq = i2s_compute_closest_frequency(target_dev, target_freq, &mclk_enabled);
+
+    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);
+    	} else {
+    		/* Return the desired frequencies. */
+    		*freq_i2s_1 = i2s_align_freq(f1);
+    		*freq_i2s_2 = i2s_align_freq(closest_freq);
+    	}
+    	return 0;
+    }
+
+    /* reset mclk */
+    if(mclk_enabled != 0) {
+    	i2s_reset_mclk(target_dev);
+    }
+
+    /* try other way round */
+    if(target_dev == dev_i2s_1) {
+    	target_dev = dev_i2s_2;
+    	target_freq = (uint32_t)((float)f1 / (float)multiplier);
+    } else {
+    	target_dev = dev_i2s_1;
+    	target_freq = (uint32_t) ((float)f2 / (float)multiplier);
+    }
+
+    /* compute closest freq */
+    mclk_enabled = 0;
+    closest_freq = i2s_compute_closest_frequency(target_dev, target_freq, &mclk_enabled);
+
+    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);
+    	} else {
+    		/* Return the desired frequencies. */
+    		*freq_i2s_1 = i2s_align_freq(f1);
+    		*freq_i2s_2 = i2s_align_freq(closest_freq);
+    	}
+    	return 0;
+    }
+
+    /* reset mclk */
+    if(mclk_enabled != 0) {
+    	i2s_reset_mclk(target_dev);
+    }
+
+    /* 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;
@@ -966,6 +1182,7 @@
     *freq_i2s_2 = (uint32_t)f2;
 
     return 0;
+#endif // 0
 }
 
 #endif