Diff: drivers/I2S.cpp

Revision:

9:c4c2240e06d6

Parent:

4:21603d68bcf7

Child:

10:1a612c2e4a85

--- a/drivers/I2S.cpp	Thu Dec 22 08:59:42 2016 +0100
+++ b/drivers/I2S.cpp	Thu Dec 22 11:10:10 2016 +0100
@@ -7,434 +7,300 @@
 namespace mbed {
 
 /* betzw - WAS
-#if TRANSACTION_QUEUE_SIZE_I2S
-CircularBuffer<Transaction<I2S>, TRANSACTION_QUEUE_SIZE_I2S> I2S::_transaction_buffer;
-#endif
+   #if TRANSACTION_QUEUE_SIZE_I2S
+   CircularBuffer<Transaction<I2S>, TRANSACTION_QUEUE_SIZE_I2S> I2S::_transaction_buffer;
+   #endif
 */
 
-I2S* I2S::_owner = NULL;
-SingletonPtr<PlatformMutex> I2S::_mutex; // intentional class level lock!
+    I2S* I2S::_owner = NULL;
+    SingletonPtr<PlatformMutex> I2S::_mutex; // intentional class level lock!
 
-rtos::Thread I2S::I2sBhHandler::_i2s_bh_daemon;
-events::EventQueue I2S::I2sBhHandler::_i2s_bh_queue;
+    rtos::Thread I2S::I2sBhHandler::_i2s_bh_daemon;
+    events::EventQueue I2S::I2sBhHandler::_i2s_bh_queue;
 
-void I2S::lock() {
+    void I2S::lock() {
 #ifdef NDEBUG
-    _mutex->lock(); // intentional class level lock!
+	_mutex->lock(); // intentional class level lock!
 #else
-    osStatus ret = _mutex->lock(); // intentional class level lock!
-    MBED_ASSERT(ret == osOK);
+	osStatus ret = _mutex->lock(); // intentional class level lock!
+	MBED_ASSERT(ret == osOK);
 #endif
-}
+    }
 
-void I2S::unlock() {
+    void I2S::unlock() {
 #ifdef NDEBUG
-    _mutex->unlock(); // intentional class level lock!
+	_mutex->unlock(); // intentional class level lock!
 #else
-    osStatus ret = _mutex->unlock(); // intentional class level lock!
-    MBED_ASSERT(ret == osOK);
+	osStatus ret = _mutex->unlock(); // intentional class level lock!
+	MBED_ASSERT(ret == osOK);
 #endif
-}
+    }
 
-I2S::I2S(PinName dpin, PinName clk, PinName wsel, PinName fdpin, PinName mck) :
+    I2S::I2S(PinName dpin, PinName clk, PinName wsel, PinName fdpin, PinName mck) :
     _i2s(),
-    _irq_tx(this), _irq_rx(this),
-    _priority(MEDIUM),
-    _dbits(16),
-    _fbits(16),
-    _polarity(0),
-    _protocol(PHILIPS),
-    _mode(MASTER_TX),
-    _circular(false),
-    _hz(44100) {
-    lock();
-    /* Init instance */
-    i2s_init(&_i2s, dpin, clk, wsel, fdpin, mck, _mode);
-    acquire();
+	_irq_tx(this), _irq_rx(this),
+	_priority(MEDIUM),
+	_dbits(16),
+	_fbits(16),
+	_polarity(0),
+	_protocol(PHILIPS),
+	_mode(MASTER_TX),
+	_circular(false),
+	_hz(44100) {
+	lock();
+	/* Init instance */
+	i2s_init(&_i2s, dpin, clk, wsel, fdpin, mck, _mode);
+	acquire();
 #if TRANSACTION_QUEUE_SIZE_I2S
-    /* Init bottom half daemon */
-    I2sBhHandler::init();
+	/* Init bottom half daemon */
+	I2sBhHandler::init();
 #endif
-    unlock();
-}
+	unlock();
+    }
 
-int I2S::format(int dbits, int fbits, int polarity) {
-    lock();
-    if (i2s_active(&_i2s)) {
-    	unlock();
-        return -1;
+    int I2S::format(int dbits, int fbits, int polarity) {
+	lock();
+	if (i2s_active(&_i2s)) {
+	    unlock();
+	    return -1;
+	}
+	_dbits = dbits;
+	_fbits = fbits;
+	_polarity = polarity;
+	I2S::_owner = NULL; // Not that elegant, but works. rmeyer
+	acquire();
+	unlock();
+	return 0;
     }
-    _dbits = dbits;
-    _fbits = fbits;
-    _polarity = polarity;
-    I2S::_owner = NULL; // Not that elegant, but works. rmeyer
-    acquire();
-    unlock();
-    return 0;
-}
 
-int I2S::audio_frequency(unsigned int hz) {
-    lock();
-    if (i2s_active(&_i2s)) {
-    	unlock();
-        return -1;
+    int I2S::audio_frequency(unsigned int hz) {
+	lock();
+	if (i2s_active(&_i2s)) {
+	    unlock();
+	    return -1;
+	}
+	_hz = hz;
+	I2S::_owner = NULL; // Not that elegant, but works. rmeyer
+	acquire();
+	unlock();
+	return 0;
     }
-    _hz = hz;
-    I2S::_owner = NULL; // Not that elegant, but works. rmeyer
-    acquire();
-    unlock();
-    return 0;
-}
 
-int I2S::set_protocol(i2s_bitorder_t protocol) {
-    lock();
-    if (i2s_active(&_i2s)) {
-    	unlock();
-        return -1;
+    int I2S::set_protocol(i2s_bitorder_t protocol) {
+	lock();
+	if (i2s_active(&_i2s)) {
+	    unlock();
+	    return -1;
+	}
+	_protocol = protocol;
+	I2S::_owner = NULL; // Not that elegant, but works. rmeyer
+	acquire();
+	unlock();
+	return 0;
     }
-    _protocol = protocol;
-    I2S::_owner = NULL; // Not that elegant, but works. rmeyer
-    acquire();
-    unlock();
-    return 0;
-}
 
-int I2S::set_mode(i2s_mode_t mode, bool circular) {
-    lock();
-    if (i2s_active(&_i2s)) {
-    	unlock();
-        return -1;
+    int I2S::set_mode(i2s_mode_t mode, bool circular) {
+	lock();
+	if (i2s_active(&_i2s)) {
+	    unlock();
+	    return -1;
+	}
+	_mode = mode;
+	_circular = circular;
+	I2S::_owner = NULL; // Not that elegant, but works. rmeyer
+	acquire();
+	unlock();
+	return 0;
     }
-    _mode = mode;
-    _circular = circular;
-    I2S::_owner = NULL; // Not that elegant, but works. rmeyer
-    acquire();
-    unlock();
-    return 0;
-}
+
+    int I2S::harmonize(I2S &dev_i2s_1, I2S &dev_i2s_2) {
+	dev_i2s_1.lock();
+	if (i2s_active(&dev_i2s_1._i2s)) {
+	    dev_i2s_1.unlock();
+	    return -1;
+	}
+
+	dev_i2s_2.lock();
+	if (i2s_active(&dev_i2s_2._i2s)) {
+	    dev_i2s_1.unlock();
+	    return -1;
+	}
+
+	uint32_t hz1 = dev_i2s_1._hz;
+	uint32_t hz2 = dev_i2s_2._hz;
+	int8_t ret = i2s_harmonize(&dev_i2s_1._i2s, &hz1, &dev_i2s_2._i2s, &hz2);
 
-void I2S::abort_transfer()
-{
-    lock();
-    i2s_abort_asynch(&_i2s);
+	if(ret == 0) {
+	    dev_i2s_1.audio_frequency(hz1);
+	    dev_i2s_2.audio_frequency(hz2);
+	}
+
+	dev_i2s_2.unlock();
+	dev_i2s_1.unlock();
+
+	return ret;
+    }
+
+    void I2S::abort_transfer()
+    {
+	lock();
+	i2s_abort_asynch(&_i2s);
 #if TRANSACTION_QUEUE_SIZE_I2S
-    dequeue_transaction();
+	dequeue_transaction();
 #endif
-    unlock();
-}
+	unlock();
+    }
 
 
-void I2S::clear_transfer_buffer()
-{
+    void I2S::clear_transfer_buffer()
+    {
 #if TRANSACTION_QUEUE_SIZE_I2S
-    lock();
-    _transaction_buffer.reset();
-    unlock();
+	lock();
+	_transaction_buffer.reset();
+	unlock();
 #endif
-}
+    }
 
-void I2S::abort_all_transfers()
-{
-    lock();
-    clear_transfer_buffer();
-    abort_transfer();
-    unlock();
-}
+    void I2S::abort_all_transfers()
+    {
+	lock();
+	clear_transfer_buffer();
+	abort_transfer();
+	unlock();
+    }
 
-int I2S::get_transfer_status()
-{
-    lock();
-    if (i2s_active(&_i2s)) {
-    	unlock();
-        return -1;
+    int I2S::get_transfer_status()
+    {
+	lock();
+	if (i2s_active(&_i2s)) {
+	    unlock();
+	    return -1;
+	}
+	unlock();
+	return  0;
     }
-    unlock();
-    return  0;
-}
 
-unsigned int I2S::get_module()
-{
-    return i2s_get_module(&_i2s);
-}
+    unsigned int I2S::get_module()
+    {
+	return i2s_get_module(&_i2s);
+    }
 
-int I2S::set_dma_priority(i2s_dma_prio_t prio)
-{
-    lock();
-    if (i2s_active(&_i2s)) {
-    	unlock();
-        return -1;
+    int I2S::set_dma_priority(i2s_dma_prio_t prio)
+    {
+	lock();
+	if (i2s_active(&_i2s)) {
+	    unlock();
+	    return -1;
+	}
+	_priority = prio;
+	unlock();
+	return  0;
     }
-    _priority = prio;
-    unlock();
-    return  0;
-}
 
-int I2S::queue_transfer(const void *tx_buffer, int tx_length, void *rx_buffer, int rx_length, const event_callback_t& callback, int event)
-{ // betzw: MUST be called with lock held!
+    int I2S::queue_transfer(const void *tx_buffer, int tx_length, void *rx_buffer, int rx_length, const event_callback_t& callback, int event)
+    { // betzw: MUST be called with lock held!
 #if TRANSACTION_QUEUE_SIZE_I2S
-    transaction_t t;
+	transaction_t t;
 
-    t.tx_buffer = const_cast<void *>(tx_buffer);
-    t.tx_length = tx_length;
-    t.rx_buffer = rx_buffer;
-    t.rx_length = rx_length;
-    t.event = event;
-    t.callback = callback;
-    t.width = 16;
-    Transaction<I2S> transaction(this, t);
-    core_util_critical_section_enter();
-    if (_transaction_buffer.full()) {
-        core_util_critical_section_enter();
-        return -1; // the buffer is full
-    } else {
-        _transaction_buffer.push(transaction);
-        core_util_critical_section_exit();
-        // betzw - seems to be redundant - WAS: dequeue_transaction();
-        return 0;
+	t.tx_buffer = const_cast<void *>(tx_buffer);
+	t.tx_length = tx_length;
+	t.rx_buffer = rx_buffer;
+	t.rx_length = rx_length;
+	t.event = event;
+	t.callback = callback;
+	t.width = 16;
+	Transaction<I2S> transaction(this, t);
+	core_util_critical_section_enter();
+	if (_transaction_buffer.full()) {
+	    core_util_critical_section_enter();
+	    return -1; // the buffer is full
+	} else {
+	    _transaction_buffer.push(transaction);
+	    core_util_critical_section_exit();
+	    // betzw - seems to be redundant - WAS: dequeue_transaction();
+	    return 0;
+	}
+#else
+	return -1;
+#endif
     }
-#else
-    return -1;
-#endif
-}
 
 
-// ignore the fact there are multiple physical i2s's, and always update if it wasn't us last
-void I2S::acquire() { // betzw: MUST be called with lock held!
-    if (_owner != this) {
-    	i2s_format(&_i2s, _dbits, _fbits, _polarity);
-        i2s_audio_frequency(&_i2s, _hz);
-        i2s_set_protocol(&_i2s, _protocol);
-        i2s_set_mode(&_i2s, _mode);
-        _owner = this;
+// ignore the fact that there are multiple physical i2s's, and always update if it wasn't us last
+    void I2S::acquire() { // betzw: MUST be called with lock held!
+	if (_owner != this) {
+	    i2s_format(&_i2s, _dbits, _fbits, _polarity);
+	    i2s_audio_frequency(&_i2s, _hz);
+	    i2s_set_protocol(&_i2s, _protocol);
+	    i2s_set_mode(&_i2s, _mode);
+	    _owner = this;
+	}
     }
-}
 
-void I2S::start_transfer(const void *tx_buffer, int tx_length, void *rx_buffer, int rx_length, 
-			 const event_callback_t& callback, int event)
-{ // betzw: MUST be called with lock held!
+    void I2S::start_transfer(const void *tx_buffer, int tx_length, void *rx_buffer, int rx_length, 
+			     const event_callback_t& callback, int event)
+    { // betzw: MUST be called with lock held!
 	acquire();
-    _callback = callback;
-    _irq_tx.callback(&I2S::irq_handler_asynch_tx);
-    _irq_rx.callback(&I2S::irq_handler_asynch_rx);
-    i2s_transfer(&_i2s,
-		 const_cast<void *>(tx_buffer), tx_length, rx_buffer, rx_length,
-		 _circular, _priority,
-		 _irq_tx.entry(), _irq_rx.entry(),
-		 event);
-}
+	_callback = callback;
+	_irq_tx.callback(&I2S::irq_handler_asynch_tx);
+	_irq_rx.callback(&I2S::irq_handler_asynch_rx);
+	i2s_transfer(&_i2s,
+		     const_cast<void *>(tx_buffer), tx_length, rx_buffer, rx_length,
+		     _circular, _priority,
+		     _irq_tx.entry(), _irq_rx.entry(),
+		     event);
+    }
 
 #if TRANSACTION_QUEUE_SIZE_I2S
 
-void I2S::start_transaction(transaction_t *data)
-{ // betzw: MUST be called with lock held!
-    start_transfer(data->tx_buffer, data->tx_length, data->rx_buffer, data->rx_length, data->callback, data->event);
-}
+    void I2S::start_transaction(transaction_t *data)
+    { // betzw: MUST be called with lock held!
+	start_transfer(data->tx_buffer, data->tx_length, data->rx_buffer, data->rx_length, data->callback, data->event);
+    }
 
-void I2S::dequeue_transaction()
-{
-    lock();
-    if (!i2s_active(&_i2s)) {
-    	Transaction<I2S> t;
-    	if (_transaction_buffer.pop(t)) {
+    void I2S::dequeue_transaction()
+    {
+	lock();
+	if (!i2s_active(&_i2s)) {
+	    Transaction<I2S> t;
+	    if (_transaction_buffer.pop(t)) {
     		I2S* obj = t.get_object();
     		transaction_t* data = t.get_transaction();
     		MBED_ASSERT(obj == this); // betzw: what if 'obj' is NOT equal to 'this'?
     		obj->start_transaction(data);
-    	}
+	    }
+	}
+	unlock();
     }
-    unlock();
-}
 
 #endif
 
-void I2S::irq_handler_asynch_rx(void)
-{
-    int event = i2s_irq_handler_asynch(&_i2s, I2S_RX_EVENT);
-    if (_callback && (event & I2S_EVENT_ALL)) {
-    	I2sBhHandler::i2s_defer_function(_callback, event & I2S_EVENT_ALL);
-    }
+    void I2S::irq_handler_asynch_rx(void)
+    {
+	int event = i2s_irq_handler_asynch(&_i2s, I2S_RX_EVENT);
+	if (_callback && (event & I2S_EVENT_ALL)) {
+	    I2sBhHandler::i2s_defer_function(_callback, event & I2S_EVENT_ALL);
+	}
 #if TRANSACTION_QUEUE_SIZE_I2S
-    if (event & I2S_EVENT_INTERNAL_TRANSFER_COMPLETE) {
-    	I2sBhHandler::i2s_defer_function(Callback<void()>(this, &I2S::dequeue_transaction));
-    }
-#endif
-}
-
-void I2S::irq_handler_asynch_tx(void)
-{
-    int event = i2s_irq_handler_asynch(&_i2s, I2S_TX_EVENT);
-    if (_callback && (event & I2S_EVENT_ALL)) {
-    	I2sBhHandler::i2s_defer_function(_callback, event & I2S_EVENT_ALL);
-    }
-#if TRANSACTION_QUEUE_SIZE_I2S
-    if (event & I2S_EVENT_INTERNAL_TRANSFER_COMPLETE) {
-    	I2sBhHandler::i2s_defer_function(Callback<void()>(this, &I2S::dequeue_transaction));
-    }
+	if (event & I2S_EVENT_INTERNAL_TRANSFER_COMPLETE) {
+	    I2sBhHandler::i2s_defer_function(Callback<void()>(this, &I2S::dequeue_transaction));
+	}
 #endif
-}
-
-float I2S::compute_real_frequency(I2S *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->_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.
- */
-float I2S::compute_magic_factor(I2S *dev_i2s, float f)
-{
-    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->_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;
-}
-
-float I2S::compute_desired_frequency(I2S *dev_i2s, float mf)
-{
-    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->_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;
-}
-
-int I2S::harmonize(I2S *dev_i2s_1, I2S *dev_i2s_2)
-{
-    if (dev_i2s_1->_hz == dev_i2s_2->_hz)
-        return 0;
-
-    /* Compute the real frequencies. */
-    float f1 = compute_real_frequency(dev_i2s_1);
-    float f2 = 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)
+    void I2S::irq_handler_asynch_tx(void)
     {
-        if (f1 < f2)
-        {
-            float mf = compute_magic_factor(dev_i2s_1, f2 / 2);
-            r = mf - (uint32_t) mf;
-            if (r > 0)
-            {
-                if (f2 > dev_i2s_2->_hz)
-                    f1 = compute_desired_frequency(dev_i2s_1, ((uint32_t) mf) + 1);
-                else
-                    f1 = compute_desired_frequency(dev_i2s_1, ((uint32_t) mf) - 1);
-                f2 = 2 * f1;
-            }
-            else
-                f1 = f2 / 2;
-        }
-        else
-        {
-            float mf = compute_magic_factor(dev_i2s_2, f1 / 2);
-            r = mf - (uint32_t) mf;
-            if (r > 0)
-            {
-                if (f1 > dev_i2s_1->_hz)
-                    f2 = compute_desired_frequency(dev_i2s_2, ((uint32_t) mf) + 1);
-                else
-                    f2 = compute_desired_frequency(dev_i2s_2, ((uint32_t) mf) - 1);
-                f1 = 2 * f2;
-            }
-            else
-                f2 = f1 / 2;
-        }
+	int event = i2s_irq_handler_asynch(&_i2s, I2S_TX_EVENT);
+	if (_callback && (event & I2S_EVENT_ALL)) {
+	    I2sBhHandler::i2s_defer_function(_callback, event & I2S_EVENT_ALL);
+	}
+#if TRANSACTION_QUEUE_SIZE_I2S
+	if (event & I2S_EVENT_INTERNAL_TRANSFER_COMPLETE) {
+	    I2sBhHandler::i2s_defer_function(Callback<void()>(this, &I2S::dequeue_transaction));
+	}
+#endif
     }
 
-    //printf("DESIRED: %f %f\r\n", f1, f2);
-
-    /* Set the desired frequencies. */
-    dev_i2s_1->audio_frequency(f1);
-    dev_i2s_2->audio_frequency(f2);
-
-    return 0;
-}
-
 } // namespace mbed
 
 #endif