diff --git a/arch/arm/src/stm32/Kconfig b/arch/arm/src/stm32/Kconfig index 647bbee4c1..ba6bdb4b79 100644 --- a/arch/arm/src/stm32/Kconfig +++ b/arch/arm/src/stm32/Kconfig @@ -1530,6 +1530,7 @@ config STM32_STM32F40XX select STM32_HAVE_TIM4 select STM32_HAVE_SPI2 select STM32_HAVE_SPI3 + select STM32_HAVE_I2S3 select STM32_HAVE_I2C2 select STM32_HAVE_I2C3 @@ -1542,6 +1543,9 @@ config STM32_STM32F401 select STM32_HAVE_TIM9 select STM32_HAVE_TIM10 select STM32_HAVE_TIM11 + select STM32_HAVE_SPI2 + select STM32_HAVE_SPI3 + select STM32_HAVE_I2S3 config STM32_STM32F410 bool @@ -1563,6 +1567,9 @@ config STM32_STM32F411 select STM32_HAVE_TIM9 select STM32_HAVE_TIM10 select STM32_HAVE_TIM11 + select STM32_HAVE_SPI2 + select STM32_HAVE_SPI3 + select STM32_HAVE_I2S3 select STM32_HAVE_SPI4 select STM32_HAVE_SPI5 @@ -1655,6 +1662,9 @@ config STM32_STM32F427 select STM32_HAVE_DAC2 select STM32_HAVE_RNG select STM32_HAVE_ETHMAC + select STM32_HAVE_SPI2 + select STM32_HAVE_SPI3 + select STM32_HAVE_I2S3 select STM32_HAVE_SPI4 select STM32_HAVE_SPI5 select STM32_HAVE_SPI6 @@ -1691,6 +1701,9 @@ config STM32_STM32F429 select STM32_HAVE_DAC2 select STM32_HAVE_RNG select STM32_HAVE_ETHMAC + select STM32_HAVE_SPI2 + select STM32_HAVE_SPI3 + select STM32_HAVE_I2S3 select STM32_HAVE_SPI4 select STM32_HAVE_SPI5 select STM32_HAVE_SPI6 @@ -2016,6 +2029,10 @@ config STM32_HAVE_SPI3 bool default n +config STM32_HAVE_I2S3 + bool + default n + config STM32_HAVE_SPI4 bool default n @@ -2353,6 +2370,13 @@ config STM32_SPI3 select SPI select STM32_SPI +config STM32_I2S3 + bool "I2S3" + default n + depends on STM32_HAVE_I2S3 + select I2S + select STM32_I2S + config STM32_SPI4 bool "SPI4" default n @@ -2610,6 +2634,11 @@ config STM32_SPI3_REMAP default n depends on STM32_STM32F10XX && STM32_SPI3 && !STM32_VALUELINE +config STM32_I2S3_REMAP + bool "I2S3 Alternate Pin Mapping" + default n + depends on STM32_STM32F10XX && STM32_I2S3 && !STM32_VALUELINE + choice prompt "TIM1 Alternate Pin Mappings" depends on STM32_STM32F10XX && STM32_TIM1 @@ -6270,6 +6299,66 @@ config STM32_SPI_DMA endmenu +menu "I2S Configuration" + depends on STM32_I2S3 + +config STM32_I2S_MCK + bool "I2S_MCK" + default n + ---help--- + TBD. + +config STM32_I2S_MAXINFLIGHT + int "I2S queue size" + default 16 + ---help--- + This is the total number of transfers, both RX and TX, that can be + enqueue before the caller is required to wait. This setting + determines the number certain queue data structures that will be + pre-allocated. + +comment "I2S3 Configuration" + +config STM32_I2S3_DATALEN + int "Data width (bits)" + default 16 + ---help--- + Data width in bits. This is a default value and may be change + via the I2S interface + +#if STM32_I2S +config STM32_I2S3_RX + bool "Enable I2C receiver" + default n + ---help--- + Enable I2S receipt logic + +config STM32_I2S3_TX + bool "Enable I2C transmitter" + default n + ---help--- + Enable I2S transmission logic + +config STM32_I2S_DMADEBUG + bool "I2S DMA transfer debug" + depends on DEBUG_DMA + default n + ---help--- + Enable special debug instrumentation analyze I2S DMA data transfers. + This logic is as non-invasive as possible: It samples DMA + registers at key points in the data transfer and then dumps all of + the registers at the end of the transfer. + +config STM32_I2S_REGDEBUG + bool "SSC Register level debug" + depends on DEBUG + default n + ---help--- + Output detailed register-level SSC device debug information. + Very invasive! Requires also DEBUG. + +endmenu # I2S Configuration + menu "I2C Configuration" depends on STM32_I2C diff --git a/arch/arm/src/stm32/Make.defs b/arch/arm/src/stm32/Make.defs index 3429ba88a4..07e31f7329 100644 --- a/arch/arm/src/stm32/Make.defs +++ b/arch/arm/src/stm32/Make.defs @@ -101,7 +101,7 @@ CHIP_ASRCS = CHIP_CSRCS = stm32_allocateheap.c stm32_start.c stm32_rcc.c stm32_lse.c CHIP_CSRCS += stm32_lsi.c stm32_gpio.c stm32_exti_gpio.c stm32_flash.c CHIP_CSRCS += stm32_irq.c stm32_dma.c stm32_lowputc.c -CHIP_CSRCS += stm32_serial.c stm32_spi.c stm32_sdio.c stm32_tim.c +CHIP_CSRCS += stm32_serial.c stm32_spi.c stm32_i2s.c stm32_sdio.c stm32_tim.c CHIP_CSRCS += stm32_waste.c stm32_ccm.c stm32_uid.c stm32_capture.c ifeq ($(CONFIG_TIMER),y) diff --git a/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h b/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h index d44c664edb..1f7b5944ea 100644 --- a/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h +++ b/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h @@ -615,7 +615,7 @@ #endif #define GPIO_SPI3_MISO_1 (GPIO_ALT|GPIO_AF6|GPIO_SPEED_50MHz|GPIO_PORTB|GPIO_PIN4) -#define GPIO_SPI3_MISO_2 (GPIO_ALT|GPIO_AF6|GPIO_SPEED_50MHz|GPIO_PORTC|GPIO_PIN11) +#define GPIO_SPI3_MISO_2 (GPIO_ALT|GPIO_AF6|GPIO_SPEED_50MHz|GPIO_PORTC|GPIO_PIN7) #define GPIO_SPI3_MOSI_1 (GPIO_ALT|GPIO_AF6|GPIO_SPEED_50MHz|GPIO_PORTB|GPIO_PIN5) #define GPIO_SPI3_MOSI_2 (GPIO_ALT|GPIO_AF6|GPIO_SPEED_50MHz|GPIO_PORTC|GPIO_PIN12) #define GPIO_SPI3_NSS_1 (GPIO_ALT|GPIO_AF6|GPIO_SPEED_50MHz|GPIO_PORTA|GPIO_PIN15) diff --git a/arch/arm/src/stm32/stm32_i2s.c b/arch/arm/src/stm32/stm32_i2s.c new file mode 100644 index 0000000000..0f246f5763 --- /dev/null +++ b/arch/arm/src/stm32/stm32_i2s.c @@ -0,0 +1,2658 @@ +/**************************************************************************** + * arm/arm/src/stm32/stm32_i2s.c + * + * Copyright (C) 2017 Gregory Nutt. All rights reserved. + * Author: Taras Drozdovskiy + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * The external functions, stm32_spi1/2/3select and stm32_spi1/2/3status + * must be provided by board-specific logic. They are implementations of + * the select and status methods of the SPI interface defined by struct + * spi_ops_s (see include/nuttx/spi/spi.h). All other methods (including + * up_spiinitialize()) are provided by common STM32 logic. To use this + * common SPI logic on your board: + * + * 1. Provide logic in stm32_boardinitialize() to configure I2S chip select + * pins. + * 2. Provide stm32_i2s2/3select() and stm32_i2s2/3status() functions in your + * board-specific logic. These functions will perform chip selection and + * status operations using GPIOs in the way your board is configured. + * 3. Add a calls to up_spiinitialize() in your low level application + * initialization logic + * 4. The handle returned by stm32_i2sdev_initialize() may then be used to + * bind the I2S driver to higher level logic + * + ****************************************************c***********************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include "up_internal.h" +#include "up_arch.h" + +#if defined(CONFIG_STM32_I2S2) || defined(CONFIG_STM32_I2S3) + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +#ifndef CONFIG_SCHED_WORKQUEUE +# error Work queue support is required (CONFIG_SCHED_WORKQUEUE) +#endif + +#ifndef CONFIG_AUDIO +# error CONFIG_AUDIO required by this driver +#endif + +#ifndef CONFIG_STM32_I2S_MAXINFLIGHT +# define CONFIG_STM32_I2S_MAXINFLIGHT 16 +#endif + +/* Assume no RX/TX support until we learn better */ + +#undef I2S_HAVE_RX +#undef I2S_HAVE_TX + +/* Check for I2S RX support */ + +# if defined(CONFIG_STM32_I2S3_RX) +# define I2S_HAVE_RX 1 + +# ifdef CONFIG_STM32_I2S_MCK +# define I2S_HAVE_MCK 1 +# endif + +# endif + +/* Check for I2S3 TX support */ + +# if defined(CONFIG_STM32_I2S3_TX) +# define I2S_HAVE_TX 1 + +# ifdef CONFIG_STM32_I2S_MCK +# define I2S_HAVE_MCK 1 +# endif + +# endif + +/* Configuration ********************************************************************/ +/* I2S interrupts */ + +#ifdef CONFIG_STM32_SPI_INTERRUPTS +# error "Interrupt driven I2S not yet supported" +#endif + +/* Can't have both interrupt driven SPI and SPI DMA */ + +#if defined(CONFIG_STM32_SPI_INTERRUPTS) && defined(CONFIG_STM32_SPI_DMA) +# error "Cannot enable both interrupt mode and DMA mode for SPI" +#endif + +/* SPI DMA priority */ + +#ifdef CONFIG_STM32_SPI_DMA + +# if defined(CONFIG_SPI_DMAPRIO) +# define SPI_DMA_PRIO CONFIG_SPI_DMAPRIO +# elif defined(CONFIG_STM32_STM32F10XX) || defined(CONFIG_STM32_STM32L15XX) +# define SPI_DMA_PRIO DMA_CCR_PRIMED +# elif defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) +# define SPI_DMA_PRIO DMA_SCR_PRIMED +# else +# error "Unknown STM32 DMA" +# endif + +# if defined(CONFIG_STM32_STM32F10XX) || defined(CONFIG_STM32_STM32L15XX) +# if (SPI_DMA_PRIO & ~DMA_CCR_PL_MASK) != 0 +# error "Illegal value for CONFIG_SPI_DMAPRIO" +# endif +# elif defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) +# if (SPI_DMA_PRIO & ~DMA_SCR_PL_MASK) != 0 +# error "Illegal value for CONFIG_SPI_DMAPRIO" +# endif +# else +# error "Unknown STM32 DMA" +# endif + +#endif + +/* DMA channel configuration */ + +#if defined(CONFIG_STM32_STM32F10XX) || defined(CONFIG_STM32_STM32F30XX) || \ + defined(CONFIG_STM32_STM32L15XX) +# define SPI_RXDMA16_CONFIG (SPI_DMA_PRIO|DMA_CCR_MSIZE_16BITS|DMA_CCR_PSIZE_16BITS|DMA_CCR_MINC ) +# define SPI_RXDMA8_CONFIG (SPI_DMA_PRIO|DMA_CCR_MSIZE_8BITS |DMA_CCR_PSIZE_8BITS |DMA_CCR_MINC ) +# define SPI_RXDMA16NULL_CONFIG (SPI_DMA_PRIO|DMA_CCR_MSIZE_8BITS |DMA_CCR_PSIZE_16BITS ) +# define SPI_RXDMA8NULL_CONFIG (SPI_DMA_PRIO|DMA_CCR_MSIZE_8BITS |DMA_CCR_PSIZE_8BITS ) +# define SPI_TXDMA16_CONFIG (SPI_DMA_PRIO|DMA_CCR_MSIZE_16BITS|DMA_CCR_PSIZE_16BITS|DMA_CCR_MINC|DMA_CCR_DIR) +# define SPI_TXDMA8_CONFIG (SPI_DMA_PRIO|DMA_CCR_MSIZE_8BITS |DMA_CCR_PSIZE_8BITS |DMA_CCR_MINC|DMA_CCR_DIR) +# define SPI_TXDMA16NULL_CONFIG (SPI_DMA_PRIO|DMA_CCR_MSIZE_8BITS |DMA_CCR_PSIZE_16BITS |DMA_CCR_DIR) +# define SPI_TXDMA8NULL_CONFIG (SPI_DMA_PRIO|DMA_CCR_MSIZE_8BITS |DMA_CCR_PSIZE_8BITS |DMA_CCR_DIR) +#elif defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) +# define SPI_RXDMA16_CONFIG (SPI_DMA_PRIO|DMA_SCR_MSIZE_16BITS|DMA_SCR_PSIZE_16BITS|DMA_SCR_MINC|DMA_SCR_DIR_P2M) +# define SPI_RXDMA8_CONFIG (SPI_DMA_PRIO|DMA_SCR_MSIZE_8BITS |DMA_SCR_PSIZE_8BITS |DMA_SCR_MINC|DMA_SCR_DIR_P2M) +# define SPI_RXDMA16NULL_CONFIG (SPI_DMA_PRIO|DMA_SCR_MSIZE_8BITS |DMA_SCR_PSIZE_16BITS |DMA_SCR_DIR_P2M) +# define SPI_RXDMA8NULL_CONFIG (SPI_DMA_PRIO|DMA_SCR_MSIZE_8BITS |DMA_SCR_PSIZE_8BITS |DMA_SCR_DIR_P2M) +# define SPI_TXDMA16_CONFIG (SPI_DMA_PRIO|DMA_SCR_MSIZE_16BITS|DMA_SCR_PSIZE_16BITS|DMA_SCR_MINC|DMA_SCR_DIR_M2P) +# define SPI_TXDMA8_CONFIG (SPI_DMA_PRIO|DMA_SCR_MSIZE_8BITS |DMA_SCR_PSIZE_8BITS |DMA_SCR_MINC|DMA_SCR_DIR_M2P) +# define SPI_TXDMA16NULL_CONFIG (SPI_DMA_PRIO|DMA_SCR_MSIZE_8BITS |DMA_SCR_PSIZE_16BITS |DMA_SCR_DIR_M2P) +# define SPI_TXDMA8NULL_CONFIG (SPI_DMA_PRIO|DMA_SCR_MSIZE_8BITS |DMA_SCR_PSIZE_8BITS |DMA_SCR_DIR_M2P) +#else +# error "Unknown STM32 DMA" +#endif + +/* Debug *******************************************************************/ +/* Check if SSC debug is enabled (non-standard.. no support in + * include/debug.h + */ + +#ifndef CONFIG_DEBUG_I2S_INFO +# undef CONFIG_STM32_I2S_DMADEBUG +# undef CONFIG_STM32_I2S_REGDEBUG +# undef CONFIG_STM32_I2S_QDEBUG +# undef CONFIG_STM32_I2S_DUMPBUFFERS +#endif + +/* The I2S can handle most any bit width from 8 to 32. However, the DMA + * logic here is constrained to byte, half-word, and word sizes. + */ + +#ifndef CONFIG_STM32_I2S3_DATALEN +# define CONFIG_STM32_I2S3_DATALEN 16 +#endif + +#if CONFIG_STM32_I2S3_DATALEN == 8 +# define STM32_I2S3_DATAMASK 0 +#elif CONFIG_STM32_I2S3_DATALEN == 16 +# define STM32_I2S3_DATAMASK 1 +#elif CONFIG_STM32_I2S3_DATALEN < 8 || CONFIG_STM32_I2S3_DATALEN > 16 +# error Invalid value for CONFIG_STM32_I2S3_DATALEN +#else +# error Valid but supported value for CONFIG_STM32_I2S3_DATALEN +#endif + +/* Check if we need to build RX and/or TX support */ + +#if defined(I2S_HAVE_RX) || defined(I2S_HAVE_TX) + +#ifndef CONFIG_DEBUG_DMA +# undef CONFIG_STM32_I2S_DMADEBUG +#endif + +#define DMA_INITIAL 0 +#define DMA_AFTER_SETUP 1 +#define DMA_AFTER_START 2 +#define DMA_CALLBACK 3 +#define DMA_TIMEOUT 3 +#define DMA_END_TRANSFER 4 +#define DMA_NSAMPLES 5 + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/* I2S buffer container */ + +struct stm32_buffer_s +{ + struct stm32_buffer_s *flink; /* Supports a singly linked list */ + i2s_callback_t callback; /* Function to call when the transfer completes */ + uint32_t timeout; /* The timeout value to use with DMA transfers */ + void *arg; /* The argument to be returned with the callback */ + struct ap_buffer_s *apb; /* The audio buffer */ + int result; /* The result of the transfer */ +}; + +/* This structure describes the state of one receiver or transmitter transport */ + +struct stm32_transport_s +{ + DMA_HANDLE dma; /* I2S DMA handle */ + WDOG_ID dog; /* Watchdog that handles DMA timeouts */ + sq_queue_t pend; /* A queue of pending transfers */ + sq_queue_t act; /* A queue of active transfers */ + sq_queue_t done; /* A queue of completed transfers */ + struct work_s work; /* Supports worker thread operations */ + +#ifdef CONFIG_STM32_I2S_DMADEBUG + struct stm32_dmaregs_s dmaregs[DMA_NSAMPLES]; +#endif +}; + +/* The state of the one I2S peripheral */ + +struct stm32_i2s_s +{ + struct i2s_dev_s dev; /* Externally visible I2S interface */ + uintptr_t base; /* I2S controller register base address */ + sem_t exclsem; /* Assures mutually exclusive acess to I2S */ + uint8_t datalen; /* Data width (8 or 16) */ +#ifdef CONFIG_DEBUG_FEATURES + uint8_t align; /* Log2 of data width (0 or 1) */ +#endif + uint8_t rxenab:1; /* True: RX transfers enabled */ + uint8_t txenab:1; /* True: TX transfers enabled */ + uint8_t i2sno:6; /* I2S controller number (0 or 1) */ +#ifdef I2S_HAVE_MCK + uint32_t samplerate; /* Data sample rate (determines only MCK divider) */ +#endif + uint32_t rxccr; /* DMA control register for RX transfers */ + uint32_t txccr; /* DMA control register for TX transfers */ +#ifdef I2S_HAVE_RX + struct stm32_transport_s rx; /* RX transport state */ +#endif +#ifdef I2S_HAVE_TX + struct stm32_transport_s tx; /* TX transport state */ +#endif + + /* Pre-allocated pool of buffer containers */ + + sem_t bufsem; /* Buffer wait semaphore */ + struct stm32_buffer_s *freelist; /* A list a free buffer containers */ + struct stm32_buffer_s containers[CONFIG_STM32_I2S_MAXINFLIGHT]; + + /* Debug stuff */ + +#ifdef CONFIG_STM32_I2S_REGDEBUG + bool wr; /* Last was a write */ + uint32_t regaddr; /* Last address */ + uint16_t regval; /* Last value */ + int count; /* Number of times */ +#endif /* CONFIG_STM32_I2S_REGDEBUG */ +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/* Register helpers */ + +#ifdef CONFIG_STM32_I2S_REGDEBUG +static bool i2s_checkreg(struct stm32_i2s_s *priv, bool wr, uint16_t regval, + uint32_t regaddr); +#else +# define i2s_checkreg(priv,wr,regval,regaddr) (false) +#endif + +static inline uint16_t i2s_getreg(struct stm32_i2s_s *priv, uint8_t offset); +static inline void i2s_putreg(struct stm32_i2s_s *priv, uint8_t offset, + uint16_t regval); + +#if defined(CONFIG_DEBUG_I2S_INFO) +static void i2s_dump_regs(struct stm32_i2s_s *priv, const char *msg); +#else +# define i2s_dump_regs(s,m) +#endif + +#ifdef CONFIG_STM32_I2S_DUMPBUFFERS +# define i2s_init_buffer(b,s) memset(b, 0x55, s); +# define i2s_dump_buffer(m,b,s) lib_dumpbuffer(m,b,s) +#else +# define i2s_init_buffer(b,s) +# define i2s_dump_buffer(m,b,s) +#endif + +/* Semaphore helpers */ + +static void i2s_exclsem_take(struct stm32_i2s_s *priv); +#define i2s_exclsem_give(priv) sem_post(&priv->exclsem) + +static void i2s_bufsem_take(struct stm32_i2s_s *priv); +#define i2s_bufsem_give(priv) sem_post(&priv->bufsem) + +/* Buffer container helpers */ + +static struct stm32_buffer_s * + i2s_buf_allocate(struct stm32_i2s_s *priv); +static void i2s_buf_free(struct stm32_i2s_s *priv, + struct stm32_buffer_s *bfcontainer); +static void i2s_buf_initialize(struct stm32_i2s_s *priv); + +/* DMA support */ + +#ifdef CONFIG_STM32_I2S_DMADEBUG +static void i2s_dma_sampleinit(struct stm32_i2s_s *priv, + struct stm32_transport_s *xpt); +#endif + +#if defined(CONFIG_STM32_I2S_DMADEBUG) && defined(I2S_HAVE_RX) +# define i2s_rxdma_sample(s,i) stm32_dmasample((s)->rx.dma, &(s)->rx.dmaregs[i]) +# define i2s_rxdma_sampleinit(s) i2s_dma_sampleinit(s, &(s)->rx) +static void i2s_rxdma_sampledone(struct stm32_i2s_s *priv, int result); + +#else +# define i2s_rxdma_sample(s,i) +# define i2s_rxdma_sampleinit(s) +# define i2s_rxdma_sampledone(s,r) + +#endif + +#if defined(CONFIG_STM32_I2S_DMADEBUG) && defined(I2S_HAVE_TX) +# define i2s_txdma_sample(s,i) stm32_dmasample((s)->tx.dma, &(s)->tx.dmaregs[i]) +# define i2s_txdma_sampleinit(s) i2s_dma_sampleinit(s, &(s)->tx) +static void i2s_txdma_sampledone(struct stm32_i2s_s *priv, int result); + +#else +# define i2s_txdma_sample(s,i) +# define i2s_txdma_sampleinit(s) +# define i2s_txdma_sampledone(s,r) + +#endif + +#ifdef I2S_HAVE_RX +static void i2s_rxdma_timeout(int argc, uint32_t arg); +static int i2s_rxdma_setup(struct stm32_i2s_s *priv); +static void i2s_rx_worker(void *arg); +static void i2s_rx_schedule(struct stm32_i2s_s *priv, int result); +static void i2s_rxdma_callback(DMA_HANDLE handle, uint8_t result, void *arg); +#endif +#ifdef I2S_HAVE_TX +static void i2s_txdma_timeout(int argc, uint32_t arg); +static int i2s_txdma_setup(struct stm32_i2s_s *priv); +static void i2s_tx_worker(void *arg); +static void i2s_tx_schedule(struct stm32_i2s_s *priv, int result); +static void i2s_txdma_callback(DMA_HANDLE handle, uint8_t result, void *arg); +#endif + +/* I2S methods (and close friends) */ + +static int i2s_checkwidth(struct stm32_i2s_s *priv, int bits); + +static uint32_t stm32_i2s_rxsamplerate(struct i2s_dev_s *dev, uint32_t rate); +static uint32_t stm32_i2s_rxdatawidth(struct i2s_dev_s *dev, int bits); +static int stm32_i2s_receive(struct i2s_dev_s *dev, struct ap_buffer_s *apb, + i2s_callback_t callback, void *arg, uint32_t timeout); +static uint32_t stm32_i2s_txsamplerate(struct i2s_dev_s *dev, uint32_t rate); +static uint32_t stm32_i2s_txdatawidth(struct i2s_dev_s *dev, int bits); +static int stm32_i2s_send(struct i2s_dev_s *dev, struct ap_buffer_s *apb, + i2s_callback_t callback, void *arg, + uint32_t timeout); + +/* Initialization */ + +static uint32_t i2s_mckdivider(struct stm32_i2s_s *priv); +static int i2s_dma_flags(struct stm32_i2s_s *priv); +static int i2s_dma_allocate(struct stm32_i2s_s *priv); +static void i2s_dma_free(struct stm32_i2s_s *priv); +#ifdef CONFIG_STM32_I2S2 +static void i2s2_configure(struct stm32_i2s_s *priv); +#endif +#ifdef CONFIG_STM32_I2S3 +static void i2s3_configure(struct stm32_i2s_s *priv); +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/* I2S device operations */ + +static const struct i2s_ops_s g_i2sops = +{ + /* Receiver methods */ + + .i2s_rxsamplerate = stm32_i2s_rxsamplerate, + .i2s_rxdatawidth = stm32_i2s_rxdatawidth, + .i2s_receive = stm32_i2s_receive, + + /* Transmitter methods */ + + .i2s_txsamplerate = stm32_i2s_txsamplerate, + .i2s_txdatawidth = stm32_i2s_txdatawidth, + .i2s_send = stm32_i2s_send, +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: i2s_checkreg + * + * Description: + * Check if the current register access is a duplicate of the preceding. + * + * Input Parameters: + * regval - The value to be written + * regaddr - The address of the register to write to + * + * Returned Value: + * true: This is the first register access of this type. + * flase: This is the same as the preceding register access. + * + ****************************************************************************/ + +#ifdef CONFIG_STM32_I2S_REGDEBUG +static bool i2s_checkreg(struct stm32_i2s_s *priv, bool wr, uint16_t regval, + uint32_t regaddr) +{ + if (wr == priv->wr && /* Same kind of access? */ + regval == priv->regval && /* Same value? */ + regaddr == priv->regaddr) /* Same address? */ + { + /* Yes, then just keep a count of the number of times we did this. */ + + priv->count++; + return false; + } + else + { + /* Did we do the previous operation more than once? */ + + if (priv->count > 0) + { + /* Yes... show how many times we did it */ + + i2sinfo("...[Repeats %d times]...\n", priv->count); + } + + /* Save information about the new access */ + + priv->wr = wr; + priv->regval = regval; + priv->regaddr = regaddr; + priv->count = 0; + } + + /* Return true if this is the first time that we have done this operation */ + + return true; +} +#endif + +/**************************************************************************** + * Name: i2s_getreg + * + * Description: + * Get the contents of the I2S register at offset + * + * Input Parameters: + * priv - private I2S device structure + * offset - offset to the register of interest + * + * Returned Value: + * The contents of the 16-bit register + * + ****************************************************************************/ + +static inline uint16_t i2s_getreg(FAR struct stm32_i2s_s *priv, + uint8_t offset) +{ + uint32_t regaddr = priv->base + offset; + uint16_t regval = getreg16(regaddr); + +#ifdef CONFIG_STM32_I2S_REGDEBUG + if (i2s_checkreg(priv, false, regval, regaddr)) + { + i2sinfo("%08x->%04x\n", regaddr, regval); + } +#endif + + return regval; +} + +/**************************************************************************** + * Name: spi_putreg + * + * Description: + * Write a 16-bit value to the SPI register at offset + * + * Input Parameters: + * priv - private SPI device structure + * offset - offset to the register of interest + * value - the 16-bit value to be written + * + * Returned Value: + * The contents of the 16-bit register + * + ****************************************************************************/ + +static inline void i2s_putreg(FAR struct stm32_i2s_s *priv, uint8_t offset, + uint16_t regval) +{ + uint32_t regaddr = priv->base + offset; + +#ifdef CONFIG_STM32_I2S_REGDEBUG + if (i2s_checkreg(priv, true, regval, regaddr)) + { + i2sinfo("%08x<-%04x\n", regaddr, regval); + } +#endif + + putreg16(regval, regaddr); +} + +/**************************************************************************** + * Name: i2s_dump_regs + * + * Description: + * Dump the contents of all I2S registers + * + * Input Parameters: + * priv - The I2S controller to dump + * msg - Message to print before the register data + * + * Returned Value: + * None + * + ****************************************************************************/ + +#if defined(CONFIG_DEBUG_I2S) +static void i2s_dump_regs(struct stm32_i2s_s *priv, const char *msg) +{ + i2sinfo("I2S%d: %s\n", priv->i2sno, msg); + i2sinfo(" CR1:%04x CR2:%04x SR:%04x DR:%04x\n", + i2s_getreg(priv, STM32_SPI_CR1_OFFSET), + i2s_getreg(priv, STM32_SPI_CR2_OFFSET), + i2s_getreg(priv, STM32_SPI_SR_OFFSET), + i2s_getreg(priv, STM32_SPI_DR_OFFSET)); + i2sinfo(" I2SCFGR:%04x I2SPR:%04x\n", + i2s_getreg(priv, STM32_SPI_I2SCFGR_OFFSET), + i2s_getreg(priv, STM32_SPI_I2SPR_OFFSET)); +} +#endif + +/**************************************************************************** + * Name: i2s_exclsem_take + * + * Description: + * Take the exclusive access semaphore handling any exceptional conditions + * + * Input Parameters: + * priv - A reference to the i2s peripheral state + * + * Returned Value: + * None + * + ****************************************************************************/ + +static void i2s_exclsem_take(struct stm32_i2s_s *priv) +{ + int ret; + + /* Wait until we successfully get the semaphore. EINTR is the only + * expected 'failure' (meaning that the wait for the semaphore was + * interrupted by a signal. + */ + + do + { + ret = sem_wait(&priv->exclsem); + DEBUGASSERT(ret == 0 || errno == EINTR); + } + while (ret < 0); +} + +/**************************************************************************** + * Name: i2s_bufsem_take + * + * Description: + * Take the buffer semaphore handling any exceptional conditions + * + * Input Parameters: + * priv - A reference to the i2s peripheral state + * + * Returned Value: + * None + * + ****************************************************************************/ + +static void i2s_bufsem_take(struct stm32_i2s_s *priv) +{ + int ret; + + /* Wait until we successfully get the semaphore. EINTR is the only + * expected 'failure' (meaning that the wait for the semaphore was + * interrupted by a signal. + */ + + do + { + ret = sem_wait(&priv->bufsem); + DEBUGASSERT(ret == 0 || errno == EINTR); + } + while (ret < 0); +} + +/**************************************************************************** + * Name: i2s_buf_allocate + * + * Description: + * Allocate a buffer container by removing the one at the head of the + * free list + * + * Input Parameters: + * priv - I2S state instance + * + * Returned Value: + * A non-NULL pointer to the allocate buffer container on success; NULL if + * there are no available buffer containers. + * + * Assumptions: + * The caller does NOT have exclusive access to the I2S state structure. + * That would result in a deadlock! + * + ****************************************************************************/ + +static struct stm32_buffer_s *i2s_buf_allocate(struct stm32_i2s_s *priv) +{ + struct stm32_buffer_s *bfcontainer; + irqstate_t flags; + + /* Set aside a buffer container. By doing this, we guarantee that we will + * have at least one free buffer container. + */ + + i2s_bufsem_take(priv); + + /* Get the buffer from the head of the free list */ + + flags = enter_critical_section(); + bfcontainer = priv->freelist; + ASSERT(bfcontainer); + + /* Unlink the buffer from the freelist */ + + priv->freelist = bfcontainer->flink; + leave_critical_section(flags); + return bfcontainer; +} + +/**************************************************************************** + * Name: i2s_buf_free + * + * Description: + * Free buffer container by adding it to the head of the free list + * + * Input Parameters: + * priv - I2S state instance + * bfcontainer - The buffer container to be freed + * + * Returned Value: + * None + * + * Assumptions: + * The caller has exclusive access to the I2S state structure + * + ****************************************************************************/ + +static void i2s_buf_free(struct stm32_i2s_s *priv, struct stm32_buffer_s *bfcontainer) +{ + irqstate_t flags; + + /* Put the buffer container back on the free list */ + + flags = enter_critical_section(); + bfcontainer->flink = priv->freelist; + priv->freelist = bfcontainer; + leave_critical_section(flags); + + /* Wake up any threads waiting for a buffer container */ + + i2s_bufsem_give(priv); +} + +/**************************************************************************** + * Name: i2s_buf_initialize + * + * Description: + * Initialize the buffer container allocator by adding all of the + * pre-allocated buffer containers to the free list + * + * Input Parameters: + * priv - I2S state instance + * + * Returned Value: + * None + * + * Assumptions: + * Called early in I2S initialization so that there are no issues with + * concurrency. + * + ****************************************************************************/ + +static void i2s_buf_initialize(struct stm32_i2s_s *priv) +{ + int i; + + priv->freelist = NULL; + sem_init(&priv->bufsem, 0, CONFIG_STM32_I2S_MAXINFLIGHT); + + for (i = 0; i < CONFIG_STM32_I2S_MAXINFLIGHT; i++) + { + i2s_buf_free(priv, &priv->containers[i]); + } +} + +/**************************************************************************** + * Name: i2s_dma_sampleinit + * + * Description: + * Initialize sampling of DMA registers (if CONFIG_STM32_I2S_DMADEBUG) + * + * Input Parameters: + * priv - I2S state instance + * + * Returned Value: + * None + * + ****************************************************************************/ + +#if defined(CONFIG_STM32_I2S_DMADEBUG) +static void i2s_dma_sampleinit(struct stm32_i2s_s *priv, + struct stm32_transport_s *xpt) +{ + /* Put contents of register samples into a known state */ + + memset(xpt->dmaregs, 0xff, DMA_NSAMPLES * sizeof(struct stm32_dmaregs_s)); + + /* Then get the initial samples */ + + stm32_dmasample(xpt->dma, &xpt->dmaregs[DMA_INITIAL]); +} +#endif + +/**************************************************************************** + * Name: i2s_rxdma_sampledone + * + * Description: + * Dump sampled RX DMA registers + * + * Input Parameters: + * priv - I2S state instance + * + * Returned Value: + * None + * + ****************************************************************************/ + +#if defined(CONFIG_STM32_I2S_DMADEBUG) && defined(I2S_HAVE_RX) +static void i2s_rxdma_sampledone(struct stm32_i2s_s *priv, int result) +{ + i2sinfo("result: %d\n", result); + + /* Sample the final registers */ + + stm32_dmasample(priv->rx.dma, &priv->rx.dmaregs[DMA_END_TRANSFER]); + + /* Then dump the sampled DMA registers */ + /* Initial register values */ + + stm32_dmadump(priv->rx.dma, &priv->rx.dmaregs[DMA_INITIAL], + "RX: Initial Registers"); + + /* Register values after DMA setup */ + + stm32_dmadump(priv->rx.dma, &priv->rx.dmaregs[DMA_AFTER_SETUP], + "RX: After DMA Setup"); + + /* Register values after DMA start */ + + stm32_dmadump(priv->rx.dma, &priv->rx.dmaregs[DMA_AFTER_START], + "RX: After DMA Start"); + + /* Register values at the time of the TX and RX DMA callbacks + * -OR- DMA timeout. + * + * If the DMA timedout, then there will not be any RX DMA + * callback samples. There is probably no TX DMA callback + * samples either, but we don't know for sure. + */ + + if (result == -ETIMEDOUT || result == -EINTR) + { + stm32_dmadump(priv->rx.dma, &priv->rx.dmaregs[DMA_TIMEOUT], + "RX: At DMA timeout"); + } + else + { + stm32_dmadump(priv->rx.dma, &priv->rx.dmaregs[DMA_CALLBACK], + "RX: At DMA callback"); + } + + stm32_dmadump(priv->rx.dma, &priv->rx.dmaregs[DMA_END_TRANSFER], + "RX: At End-of-Transfer"); + + i2s_dump_regs(priv, "RX: At End-of-Transfer"); +} +#endif + +/**************************************************************************** + * Name: i2s_txdma_sampledone + * + * Description: + * Dump sampled DMA registers + * + * Input Parameters: + * priv - I2S state instance + * + * Returned Value: + * None + * + ****************************************************************************/ + +#if defined(CONFIG_STM32_I2S_DMADEBUG) && defined(I2S_HAVE_TX) +static void i2s_txdma_sampledone(struct stm32_i2s_s *priv, int result) +{ + i2sinfo("result: %d\n", result); + + /* Sample the final registers */ + + stm32_dmasample(priv->tx.dma, &priv->tx.dmaregs[DMA_END_TRANSFER]); + + /* Then dump the sampled DMA registers */ + /* Initial register values */ + + stm32_dmadump(priv->tx.dma, &priv->tx.dmaregs[DMA_INITIAL], + "TX: Initial Registers"); + + /* Register values after DMA setup */ + + stm32_dmadump(priv->tx.dma, &priv->tx.dmaregs[DMA_AFTER_SETUP], + "TX: After DMA Setup"); + + /* Register values after DMA start */ + + stm32_dmadump(priv->tx.dma, &priv->tx.dmaregs[DMA_AFTER_START], + "TX: After DMA Start"); + + /* Register values at the time of the TX and RX DMA callbacks + * -OR- DMA timeout. + */ + + if (result == -ETIMEDOUT || result == -EINTR) + { + stm32_dmadump(priv->tx.dma, &priv->tx.dmaregs[DMA_TIMEOUT], + "TX: At DMA timeout"); + } + else + { + stm32_dmadump(priv->tx.dma, &priv->tx.dmaregs[DMA_CALLBACK], + "TX: At DMA callback"); + } + + stm32_dmadump(priv->tx.dma, &priv->tx.dmaregs[DMA_END_TRANSFER], + "TX: At End-of-Transfer"); + + i2s_dump_regs(priv, "TX: At End-of-Transfer"); +} +#endif + +/**************************************************************************** + * Name: i2s_rxdma_timeout + * + * Description: + * The RX watchdog timeout without completion of the RX DMA. + * + * Input Parameters: + * argc - The number of arguments (should be 1) + * arg - The argument (state structure reference cast to uint32_t) + * + * Returned Value: + * None + * + * Assumptions: + * Always called from the interrupt level with interrupts disabled. + * + ****************************************************************************/ + +#ifdef I2S_HAVE_RX +static void i2s_rxdma_timeout(int argc, uint32_t arg) +{ + struct stm32_i2s_s *priv = (struct stm32_i2s_s *)arg; + DEBUGASSERT(priv != NULL); + + /* Sample DMA registers at the time of the timeout */ + + i2s_rxdma_sample(priv, DMA_TIMEOUT); + + /* Cancel the DMA */ + + stm32_dmastop(priv->rx.dma); + + /* Then schedule completion of the transfer to occur on the worker thread. + * NOTE: stm32_dmastop() will call the DMA complete callback with an error + * of -EINTR. So the following is just insurance and should have no + * effect if the worker is already schedule. + */ + + i2s_rx_schedule(priv, -ETIMEDOUT); +} +#endif + +/**************************************************************************** + * Name: i2s_rxdma_setup + * + * Description: + * Setup and initiate the next RX DMA transfer + * + * Input Parameters: + * priv - I2S state instance + * + * Returned Value: + * OK on success; a negated errno value on failure + * + * Assumptions: + * Interrupts are disabled + * + ****************************************************************************/ + +#ifdef I2S_HAVE_RX +static int i2s_rxdma_setup(struct stm32_i2s_s *priv) +{ + struct stm32_buffer_s *bfcontainer; + struct ap_buffer_s *apb; + uintptr_t samp; + uint32_t timeout; + bool notimeout; + int ret; + + /* If there is already an active transmission in progress, then bail + * returning success. + */ + + if (!sq_empty(&priv->rx.act)) + { + return OK; + } + + /* If there are no pending transfer, then bail returning success */ + + if (sq_empty(&priv->rx.pend)) + { + return OK; + } + + /* Initialize DMA register sampling */ + + i2s_rxdma_sampleinit(priv); + + /* Loop, adding each pending DMA */ + + timeout = 0; + notimeout = false; + + do + { + /* Remove the pending RX transfer at the head of the RX pending queue. */ + + bfcontainer = (struct stm32_buffer_s *)sq_remfirst(&priv->rx.pend); + DEBUGASSERT(bfcontainer && bfcontainer->apb); + + apb = bfcontainer->apb; + DEBUGASSERT(((uintptr_t)apb->samp % priv->align) == 0); + + /* No data received yet */ + + apb->nbytes = 0; + apb->curbyte = 0; + samp = (uintptr_t)&apb->samp[apb->curbyte]; + + /* Configure the RX DMA */ + + stm32_dmasetup(priv->rx.dma, priv->base + STM32_SPI_DR_OFFSET, + (uint32_t)samp, apb->nmaxbytes, priv->rxccr); + + /* Increment the DMA timeout */ + + if (bfcontainer->timeout > 0) + { + timeout += bfcontainer->timeout; + } + else + { + notimeout = true; + } + + /* Add the container to the list of active DMAs */ + + sq_addlast((sq_entry_t *)bfcontainer, &priv->rx.act); + } +#if 1 /* REVISIT: Chained RX transfers */ + while (0); +#else + while (!sq_empty(&priv->rx.pend)); +#endif + + /* Sample DMA registers */ + + i2s_rxdma_sample(priv, DMA_AFTER_SETUP); + + /* Start the DMA, saving the container as the current active transfer */ + + stm32_dmastart(priv->rx.dma, i2s_rxdma_callback, priv, false); + + i2s_rxdma_sample(priv, DMA_AFTER_START); + + /* Enable the receiver */ + + i2s_putreg(priv, STM32_SPI_CR2_OFFSET, + i2s_getreg(priv, STM32_SPI_CR2_OFFSET) | SPI_CR2_RXDMAEN); + + /* Start a watchdog to catch DMA timeouts */ + + if (!notimeout) + { + ret = wd_start(priv->rx.dog, timeout, (wdentry_t)i2s_rxdma_timeout, + 1, (uint32_t)priv); + + /* Check if we have successfully started the watchdog timer. Note + * that we do nothing in the case of failure to start the timer. We + * are already committed to the DMA anyway. Let's just hope that the + * DMA does not hang. + */ + + if (ret < 0) + { + i2serr("ERROR: wd_start failed: %d\n", errno); + } + } + + return OK; +} +#endif + +/**************************************************************************** + * Name: i2s_rx_worker + * + * Description: + * RX transfer done worker + * + * Input Parameters: + * arg - the I2S device instance cast to void* + * + * Returned Value: + * None + * + ****************************************************************************/ + +#ifdef I2S_HAVE_RX +static void i2s_rx_worker(void *arg) +{ + struct stm32_i2s_s *priv = (struct stm32_i2s_s *)arg; + struct stm32_buffer_s *bfcontainer; + struct ap_buffer_s *apb; + irqstate_t flags; + + DEBUGASSERT(priv); + + /* When the transfer was started, the active buffer containers were removed + * from the rx.pend queue and saved in the rx.act queue. We get here when the + * DMA is finished... either successfully, with a DMA error, or with a DMA + * timeout. + * + * In any case, the buffer containers in rx.act will be moved to the end + * of the rx.done queue and rx.act queue will be emptied before this worker + * is started. + * + * REVISIT: Normal DMA callback processing should restart the DMA + * immediately to avoid audio artifacts at the boundaries between DMA + * transfers. Unfortunately, the DMA callback occurs at the interrupt + * level and we cannot call dma_rxsetup() from the interrupt level. + * So we have to start the next DMA here. + */ + + i2sinfo("rx.act.head=%p rx.done.head=%p\n", + priv->rx.act.head, priv->rx.done.head); + + /* Check if the DMA is IDLE */ + + if (sq_empty(&priv->rx.act)) + { +#ifdef CONFIG_STM32_I2S_DMADEBUG + bfcontainer = (struct stm32_buffer_s *)sq_peek(&priv->rx.done); + if (bfcontainer) + { + /* Dump the DMA registers */ + + i2s_rxdma_sampledone(priv, bfcontainer->result); + } +#endif + + /* Then start the next DMA. This must be done with interrupts + * disabled. + */ + + flags = enter_critical_section(); + (void)i2s_rxdma_setup(priv); + leave_critical_section(flags); + } + + /* Process each buffer in the rx.done queue */ + + while (sq_peek(&priv->rx.done) != NULL) + { + /* Remove the buffer container from the rx.done queue. NOTE that + * interrupts must be enabled to do this because the rx.done queue is + * also modified from the interrupt level. + */ + + flags = enter_critical_section(); + bfcontainer = (struct stm32_buffer_s *)sq_remfirst(&priv->rx.done); + leave_critical_section(flags); + + DEBUGASSERT(bfcontainer && bfcontainer->apb && bfcontainer->callback); + apb = bfcontainer->apb; + + /* If the DMA was successful, then update the number of valid bytes in + * the audio buffer. + */ + + if (bfcontainer->result == OK) + { + apb->nbytes = apb->nmaxbytes; + } + + i2s_dump_buffer("Received", apb->samp, apb->nbytes); + + /* Perform the RX transfer done callback */ + + bfcontainer->callback(&priv->dev, apb, bfcontainer->arg, + bfcontainer->result); + + /* Release our reference on the audio buffer. This may very likely + * cause the audio buffer to be freed. + */ + + apb_free(apb); + + /* And release the buffer container */ + + i2s_buf_free(priv, bfcontainer); + } +} +#endif + +/**************************************************************************** + * Name: i2s_rx_schedule + * + * Description: + * An RX DMA completion or timeout has occurred. Schedule processing on + * the working thread. + * + * Input Parameters: + * handle - The DMA handler + * arg - A pointer to the chip select struction + * result - The result of the DMA transfer + * + * Returned Value: + * None + * + * Assumptions: + * Interrupts are disabled + * + ****************************************************************************/ + +#ifdef I2S_HAVE_RX +static void i2s_rx_schedule(struct stm32_i2s_s *priv, int result) +{ + struct stm32_buffer_s *bfcontainer; + int ret; + + /* Upon entry, the transfer(s) that just completed are the ones in the + * priv->rx.act queue. NOTE: In certain conditions, this function may + * be called an additional time, hence, we can't assert this to be true. + * For example, in the case of a timeout, this function will be called by + * both indirectly via the stm32_dmastop() logic and directly via the + * i2s_rxdma_timeout() logic. + */ + + /* Move all entries from the rx.act queue to the rx.done queue */ + + while (!sq_empty(&priv->rx.act)) + { + /* Remove the next buffer container from the rx.act list */ + + bfcontainer = (struct stm32_buffer_s *)sq_remfirst(&priv->rx.act); + + /* Report the result of the transfer */ + + bfcontainer->result = result; + + /* Add the completed buffer container to the tail of the rx.done queue */ + + sq_addlast((sq_entry_t *)bfcontainer, &priv->rx.done); + } + + /* If the worker has completed running, then reschedule the working thread. + * REVISIT: There may be a race condition here. So we do nothing is the + * worker is not available. + */ + + if (work_available(&priv->rx.work)) + { + /* Schedule the TX DMA done processing to occur on the worker thread. */ + + ret = work_queue(HPWORK, &priv->rx.work, i2s_rx_worker, priv, 0); + if (ret != 0) + { + i2serr("ERROR: Failed to queue RX work: %d\n", ret); + } + } +} +#endif + +/**************************************************************************** + * Name: i2s_rxdma_callback + * + * Description: + * This callback function is invoked at the completion of the I2S RX DMA. + * + * Input Parameters: + * handle - The DMA handler + * arg - A pointer to the chip select struction + * result - The result of the DMA transfer + * + * Returned Value: + * None + * + ****************************************************************************/ + +#ifdef I2S_HAVE_RX +static void i2s_rxdma_callback(DMA_HANDLE handle, uint8_t result, void *arg) +{ + struct stm32_i2s_s *priv = (struct stm32_i2s_s *)arg; + DEBUGASSERT(priv != NULL); + + /* Cancel the watchdog timeout */ + + (void)wd_cancel(priv->rx.dog); + + /* Sample DMA registers at the time of the DMA completion */ + + i2s_rxdma_sample(priv, DMA_CALLBACK); + + /* REVISIT: We would like to the next DMA started here so that we do not + * get audio glitches at the boundaries between DMA transfers. + * Unfortunately, we cannot call stm32_dmasetup() from an interrupt handler! + */ + + /* Then schedule completion of the transfer to occur on the worker thread */ + + i2s_rx_schedule(priv, result); +} +#endif + +/**************************************************************************** + * Name: i2s_txdma_timeout + * + * Description: + * The RX watchdog timeout without completion of the RX DMA. + * + * Input Parameters: + * argc - The number of arguments (should be 1) + * arg - The argument (state structure reference cast to uint32_t) + * + * Returned Value: + * None + * + * Assumptions: + * Always called from the interrupt level with interrupts disabled. + * + ****************************************************************************/ + +#ifdef I2S_HAVE_TX +static void i2s_txdma_timeout(int argc, uint32_t arg) +{ + struct stm32_i2s_s *priv = (struct stm32_i2s_s *)arg; + DEBUGASSERT(priv != NULL); + + /* Sample DMA registers at the time of the timeout */ + + i2s_txdma_sample(priv, DMA_TIMEOUT); + + /* Cancel the DMA */ + + stm32_dmastop(priv->tx.dma); + + /* Then schedule completion of the transfer to occur on the worker thread. + * NOTE: stm32_dmastop() will call the DMA complete callback with an error + * of -EINTR. So the following is just insurance and should have no + * effect if the worker is already schedule. + */ + + i2s_tx_schedule(priv, -ETIMEDOUT); +} +#endif + +/**************************************************************************** + * Name: i2s_txdma_setup + * + * Description: + * Setup and initiate the next TX DMA transfer + * + * Input Parameters: + * priv - I2S state instance + * + * Returned Value: + * OK on success; a negated errno value on failure + * + * Assumptions: + * Interrupts are disabled + * + ****************************************************************************/ + +#ifdef I2S_HAVE_TX +static int i2s_txdma_setup(struct stm32_i2s_s *priv) +{ + struct stm32_buffer_s *bfcontainer; + struct ap_buffer_s *apb; + uintptr_t samp; + uint32_t timeout; + apb_samp_t nbytes; + bool notimeout; + int ret; + + /* If there is already an active transmission in progress, then bail + * returning success. + */ + + if (!sq_empty(&priv->tx.act)) + { + return OK; + } + + /* If there are no pending transfer, then bail returning success */ + + if (sq_empty(&priv->tx.pend)) + { + return OK; + } + + /* Initialize DMA register sampling */ + + i2s_txdma_sampleinit(priv); + + /* Loop, adding each pending DMA */ + + timeout = 0; + notimeout = false; + + do + { + /* Remove the pending TX transfer at the head of the TX pending queue. */ + + bfcontainer = (struct stm32_buffer_s *)sq_remfirst(&priv->tx.pend); + DEBUGASSERT(bfcontainer && bfcontainer->apb); + + apb = bfcontainer->apb; + + /* Get the transfer information, accounting for any data offset */ + + samp = (uintptr_t)&apb->samp[apb->curbyte]; + nbytes = apb->nbytes - apb->curbyte; + DEBUGASSERT((samp & priv->align) == 0 && (nbytes & priv->align) == 0); + + /* Configure DMA stream */ + + stm32_dmasetup(priv->tx.dma, priv->base + STM32_SPI_DR_OFFSET, + (uint32_t)samp, nbytes/2, priv->txccr); + + /* Increment the DMA timeout */ + if (bfcontainer->timeout > 0) + { + timeout += bfcontainer->timeout; + } + else + { + notimeout = true; + } + + /* Add the container to the list of active DMAs */ + + sq_addlast((sq_entry_t *)bfcontainer, &priv->tx.act); + } +#if 1 /* REVISIT: Chained TX transfers */ + while (0); +#else + while (!sq_empty(&priv->tx.pend)); +#endif + + /* Sample DMA registers */ + + i2s_txdma_sample(priv, DMA_AFTER_SETUP); + + /* Start the DMA, saving the container as the current active transfer */ + + stm32_dmastart(priv->tx.dma, i2s_txdma_callback, priv, true); + + i2s_txdma_sample(priv, DMA_AFTER_START); + + /* Enable the transmitter */ + + i2s_putreg(priv, STM32_SPI_CR2_OFFSET, i2s_getreg(priv, STM32_SPI_CR2_OFFSET) | SPI_CR2_TXDMAEN); + + /* Start a watchdog to catch DMA timeouts */ + + if (!notimeout) + { + ret = wd_start(priv->tx.dog, timeout, (wdentry_t)i2s_txdma_timeout, + 1, (uint32_t)priv); + + /* Check if we have successfully started the watchdog timer. Note + * that we do nothing in the case of failure to start the timer. We + * are already committed to the DMA anyway. Let's just hope that the + * DMA does not hang. + */ + + if (ret < 0) + { + i2serr("ERROR: wd_start failed: %d\n", errno); + } + } + + return OK; +} +#endif + +/**************************************************************************** + * Name: i2s_tx_worker + * + * Description: + * TX transfer done worker + * + * Input Parameters: + * arg - the I2S device instance cast to void* + * + * Returned Value: + * None + * + ****************************************************************************/ + +#ifdef I2S_HAVE_TX +static void i2s_tx_worker(void *arg) +{ + struct stm32_i2s_s *priv = (struct stm32_i2s_s *)arg; + struct stm32_buffer_s *bfcontainer; + irqstate_t flags; + + DEBUGASSERT(priv); + + /* When the transfer was started, the active buffer containers were removed + * from the tx.pend queue and saved in the tx.act queue. We get here when the + * DMA is finished... either successfully, with a DMA error, or with a DMA + * timeout. + * + * In any case, the buffer containers in tx.act will be moved to the end + * of the tx.done queue and tx.act will be emptied before this worker is + * started. + * + * REVISIT: Normal DMA callback processing should restart the DMA + * immediately to avoid audio artifacts at the boundaries between DMA + * transfers. Unfortunately, the DMA callback occurs at the interrupt + * level and we cannot call dma_txsetup() from the interrupt level. + * So we have to start the next DMA here. + */ + + i2sinfo("tx.act.head=%p tx.done.head=%p\n", + priv->tx.act.head, priv->tx.done.head); + + /* Check if the DMA is IDLE */ + + if (sq_empty(&priv->tx.act)) + { +#ifdef CONFIG_STM32_I2S_DMADEBUG + bfcontainer = (struct stm32_buffer_s *)sq_peek(&priv->tx.done); + if (bfcontainer) + { + /* Dump the DMA registers */ + + i2s_txdma_sampledone(priv, bfcontainer->result); + } +#endif + + /* Then start the next DMA. This must be done with interrupts + * disabled. + */ + + flags = enter_critical_section(); + (void)i2s_txdma_setup(priv); + leave_critical_section(flags); + } + + /* Process each buffer in the tx.done queue */ + + while (sq_peek(&priv->tx.done) != NULL) + { + /* Remove the buffer container from the tx.done queue. NOTE that + * interupts must be enabled to do this because the tx.done queue is + * also modified from the interrupt level. + */ + + flags = enter_critical_section(); + bfcontainer = (struct stm32_buffer_s *)sq_remfirst(&priv->tx.done); + leave_critical_section(flags); + + /* Perform the TX transfer done callback */ + + DEBUGASSERT(bfcontainer && bfcontainer->callback); + bfcontainer->callback(&priv->dev, bfcontainer->apb, + bfcontainer->arg, bfcontainer->result); + + /* Release our reference on the audio buffer. This may very likely + * cause the audio buffer to be freed. + */ + + apb_free(bfcontainer->apb); + + /* And release the buffer container */ + + i2s_buf_free(priv, bfcontainer); + } +} +#endif + +/**************************************************************************** + * Name: i2s_tx_schedule + * + * Description: + * An TX DMA completion or timeout has occurred. Schedule processing on + * the working thread. + * + * Input Parameters: + * handle - The DMA handler + * arg - A pointer to the chip select struction + * result - The result of the DMA transfer + * + * Returned Value: + * None + * + * Assumptions: + * - Interrupts are disabled + * - The TX timeout has been canceled. + * + ****************************************************************************/ + +#ifdef I2S_HAVE_TX +static void i2s_tx_schedule(struct stm32_i2s_s *priv, int result) +{ + struct stm32_buffer_s *bfcontainer; + int ret; + + /* Upon entry, the transfer(s) that just completed are the ones in the + * priv->tx.act queue. NOTE: In certain conditions, this function may + * be called an additional time, hence, we can't assert this to be true. + * For example, in the case of a timeout, this function will be called by + * both indirectly via the stm32_dmastop() logic and directly via the + * i2s_txdma_timeout() logic. + */ + + /* Move all entries from the tx.act queue to the tx.done queue */ + + while (!sq_empty(&priv->tx.act)) + { + /* Remove the next buffer container from the tx.act list */ + + bfcontainer = (struct stm32_buffer_s *)sq_remfirst(&priv->tx.act); + + /* Report the result of the transfer */ + + bfcontainer->result = result; + + /* Add the completed buffer container to the tail of the tx.done queue */ + + sq_addlast((sq_entry_t *)bfcontainer, &priv->tx.done); + } + + /* If the worker has completed running, then reschedule the working thread. + * REVISIT: There may be a race condition here. So we do nothing is the + * worker is not available. + */ + + if (work_available(&priv->tx.work)) + { + /* Schedule the TX DMA done processing to occur on the worker thread. */ + + ret = work_queue(HPWORK, &priv->tx.work, i2s_tx_worker, priv, 0); + if (ret != 0) + { + i2serr("ERROR: Failed to queue TX work: %d\n", ret); + } + } +} +#endif + +/**************************************************************************** + * Name: i2s_txdma_callback + * + * Description: + * This callback function is invoked at the completion of the I2S TX DMA. + * + * Input Parameters: + * handle - The DMA handler + * arg - A pointer to the chip select struction + * result - The result of the DMA transfer + * + * Returned Value: + * None + * + ****************************************************************************/ + +#ifdef I2S_HAVE_TX +static void i2s_txdma_callback(DMA_HANDLE handle, uint8_t result, void *arg) +{ + struct stm32_i2s_s *priv = (struct stm32_i2s_s *)arg; + DEBUGASSERT(priv != NULL); + + /* Cancel the watchdog timeout */ + + (void)wd_cancel(priv->tx.dog); + + /* Sample DMA registers at the time of the DMA completion */ + + i2s_txdma_sample(priv, DMA_CALLBACK); + + /* REVISIT: We would like to the next DMA started here so that we do not + * get audio glitches at the boundaries between DMA transfers. + * Unfortunately, we cannot call stm32_dmasetup() from an interrupt handler! + */ + + /* Then schedule completion of the transfer to occur on the worker thread */ + + i2s_tx_schedule(priv, result); +} +#endif + +/**************************************************************************** + * Name: i2s_checkwidth + * + * Description: + * Check for a valid bit width. The I2S is capable of handling most any + * bit width from 8 to 16, but the DMA logic in this driver is constrained + * to 8- and 16-bit data widths + * + * Input Parameters: + * dev - Device-specific state data + * rate - The I2S sample rate in samples (not bits) per second + * + * Returned Value: + * Returns the resulting bitrate + * + ****************************************************************************/ + +static int i2s_checkwidth(struct stm32_i2s_s *priv, int bits) +{ + /* The I2S can handle most any bit width from 8 to 32. However, the DMA + * logic here is constrained to byte, half-word, and word sizes. + */ + + switch (bits) + { + case 8: +#ifdef CONFIG_DEBUG + priv->align = 0; +#endif + break; + + case 16: +#ifdef CONFIG_DEBUG + priv->align = 1; +#endif + break; + + default: + i2serr("ERROR: Unsupported or invalid data width: %d\n", bits); + return (bits < 8 || bits > 16) ? -EINVAL : -ENOSYS; + } + + /* Save the new data width */ + + priv->datalen = bits; + return OK; +} + +/**************************************************************************** + * Name: stm32_i2s_rxsamplerate + * + * Description: + * Set the I2S RX sample rate. NOTE: This will have no effect if (1) the + * driver does not support an I2C receiver or if (2) the sample rate is + * driven by the I2C frame clock. This may also have unexpected side- + * effects of the RX sample is coupled with the TX sample rate. + * + * Input Parameters: + * dev - Device-specific state data + * rate - The I2S sample rate in samples (not bits) per second + * + * Returned Value: + * Returns the resulting bitrate + * + ****************************************************************************/ + +static uint32_t stm32_i2s_rxsamplerate(struct i2s_dev_s *dev, uint32_t rate) +{ +#if defined(I2S_HAVE_RX) && defined(I2S_HAVE_MCK) + struct stm32_i2s_s *priv = (struct stm32_i2s_s *)dev; + DEBUGASSERT(priv && priv->samplerate > 0 && rate > 0); + + /* Check if the receiver is driven by the MCK */ + + if (priv->samplerate != rate) + { + /* Save the new sample rate and update the MCK divider */ + + priv->samplerate = rate; + return i2s_mckdivider(priv); + } +#endif + + return 0; +} + +/**************************************************************************** + * Name: stm32_i2s_rxdatawidth + * + * Description: + * Set the I2S RX data width. The RX bitrate is determined by + * sample_rate * data_width. + * + * Input Parameters: + * dev - Device-specific state data + * width - The I2S data with in bits. + * + * Returned Value: + * Returns the resulting bitrate + * + ****************************************************************************/ + +static uint32_t stm32_i2s_rxdatawidth(struct i2s_dev_s *dev, int bits) +{ +#ifdef I2S_HAVE_RX + struct stm32_i2s_s *priv = (struct stm32_i2s_s *)dev; + int ret; + + DEBUGASSERT(priv && bits > 1); + + /* Check if this is a bit width that we are configured to handle */ + + ret = i2s_checkwidth(priv, bits); + if (ret < 0) + { + i2serr("ERROR: i2s_checkwidth failed: %d\n", ret); + return 0; + } + + /* Update the DMA flags */ + + ret = i2s_dma_flags(priv); + if (ret < 0) + { + i2serr("ERROR: i2s_dma_flags failed: %d\n", ret); + return 0; + } + +#endif + + return 0; +} + +/**************************************************************************** + * Name: stm32_i2s_receive + * + * Description: + * Receive a block of data from I2S. + * + * Input Parameters: + * dev - Device-specific state data + * apb - A pointer to the audio buffer in which to recieve data + * callback - A user provided callback function that will be called at + * the completion of the transfer. The callback will be + * performed in the context of the worker thread. + * arg - An opaque argument that will be provided to the callback + * when the transfer complete + * timeout - The timeout value to use. The transfer will be canceled + * and an ETIMEDOUT error will be reported if this timeout + * elapsed without completion of the DMA transfer. Units + * are system clock ticks. Zero means no timeout. + * + * Returned Value: + * OK on success; a negated errno value on failure. NOTE: This function + * only enqueues the transfer and returns immediately. Success here only + * means that the transfer was enqueued correctly. + * + * When the transfer is complete, a 'result' value will be provided as + * an argument to the callback function that will indicate if the transfer + * failed. + * + ****************************************************************************/ + +static int stm32_i2s_receive(struct i2s_dev_s *dev, struct ap_buffer_s *apb, + i2s_callback_t callback, void *arg, uint32_t timeout) +{ + struct stm32_i2s_s *priv = (struct stm32_i2s_s *)dev; +#ifdef I2S_HAVE_RX + struct stm32_buffer_s *bfcontainer; + irqstate_t flags; + int ret; +#endif + + DEBUGASSERT(priv && apb && ((uintptr_t)apb->samp & priv->align) == 0); + i2sinfo("apb=%p nmaxbytes=%d arg=%p timeout=%d\n", + apb, apb->nmaxbytes, arg, timeout); + + i2s_init_buffer(apb->samp, apb->nmaxbytes); + +#ifdef I2S_HAVE_RX + /* Allocate a buffer container in advance */ + + bfcontainer = i2s_buf_allocate(priv); + DEBUGASSERT(bfcontainer); + + /* Get exclusive access to the I2S driver data */ + + i2s_exclsem_take(priv); + + /* Has the RX channel been enabled? */ + + if (!priv->rxenab) + { + i2serr("ERROR: I2S%d has no receiver\n", priv->i2sno); + ret = -EAGAIN; + goto errout_with_exclsem; + } + + /* Add a reference to the audio buffer */ + + apb_reference(apb); + + /* Initialize the buffer container structure */ + + bfcontainer->callback = (void *)callback; + bfcontainer->timeout = timeout; + bfcontainer->arg = arg; + bfcontainer->apb = apb; + bfcontainer->result = -EBUSY; + + /* Add the buffer container to the end of the RX pending queue */ + + flags = enter_critical_section(); + sq_addlast((sq_entry_t *)bfcontainer, &priv->rx.pend); + + /* Then start the next transfer. If there is already a transfer in progess, + * then this will do nothing. + */ + + ret = i2s_rxdma_setup(priv); + DEBUGASSERT(ret == OK); + leave_critical_section(flags); + i2s_exclsem_give(priv); + return OK; + +errout_with_exclsem: + i2s_exclsem_give(priv); + i2s_buf_free(priv, bfcontainer); + return ret; + +#else + i2serr("ERROR: I2S%d has no receiver\n", priv->i2sno); + UNUSED(priv); + return -ENOSYS; +#endif +} + +static int roundf(float num) +{ + if(((int)(num + 0.5f)) > num) + { + return num + 1; + } + + return num; +} + +/**************************************************************************** + * Name: stm32_i2s_txsamplerate + * + * Description: + * Set the I2S TX sample rate. NOTE: This will have no effect if (1) the + * driver does not support an I2C transmitter or if (2) the sample rate is + * driven by the I2C frame clock. This may also have unexpected side- + * effects of the TX sample is coupled with the RX sample rate. + * + * Input Parameters: + * dev - Device-specific state data + * rate - The I2S sample rate in samples (not bits) per second + * + * Returned Value: + * Returns the resulting bitrate + * + ****************************************************************************/ + +static uint32_t stm32_i2s_txsamplerate(struct i2s_dev_s *dev, uint32_t rate) +{ +#if defined(I2S_HAVE_TX) && defined(I2S_HAVE_MCK) + struct stm32_i2s_s *priv = (struct stm32_i2s_s *)dev; + + DEBUGASSERT(priv && priv->samplerate > 0 && rate > 0); + + /* Check if the receiver is driven by the MCK/2 */ + + if (priv->samplerate != rate) + { + /* Save the new sample rate and update the MCK/2 divider */ + + priv->samplerate = rate; + return i2s_mckdivider(priv); + } +#endif + + return 0; +} + +/**************************************************************************** + * Name: stm32_i2s_txdatawidth + * + * Description: + * Set the I2S TX data width. The TX bitrate is determined by + * sample_rate * data_width. + * + * Input Parameters: + * dev - Device-specific state data + * width - The I2S data with in bits. + * + * Returned Value: + * Returns the resulting bitrate + * + ****************************************************************************/ + +static uint32_t stm32_i2s_txdatawidth(struct i2s_dev_s *dev, int bits) +{ +#ifdef I2S_HAVE_TX + struct stm32_i2s_s *priv = (struct stm32_i2s_s *)dev; + int ret; + + i2sinfo("Data width bits of tx = %d\n",bits); + DEBUGASSERT(priv && bits > 1); + + /* Check if this is a bit width that we are configured to handle */ + + ret = i2s_checkwidth(priv, bits); + if (ret < 0) + { + i2serr("ERROR: i2s_checkwidth failed: %d\n", ret); + return 0; + } + + /* Upate the DMA flags */ + + ret = i2s_dma_flags(priv); + if (ret < 0) + { + i2serr("ERROR: i2s_dma_flags failed: %d\n", ret); + return 0; + } +#endif + + return 0; +} + +/**************************************************************************** + * Name: stm32_i2s_send + * + * Description: + * Send a block of data on I2S. + * + * Input Parameters: + * dev - Device-specific state data + * apb - A pointer to the audio buffer from which to send data + * callback - A user provided callback function that will be called at + * the completion of the transfer. The callback will be + * performed in the context of the worker thread. + * arg - An opaque argument that will be provided to the callback + * when the transfer complete + * timeout - The timeout value to use. The transfer will be canceled + * and an ETIMEDOUT error will be reported if this timeout + * elapsed without completion of the DMA transfer. Units + * are system clock ticks. Zero means no timeout. + * + * Returned Value: + * OK on success; a negated errno value on failure. NOTE: This function + * only enqueues the transfer and returns immediately. Success here only + * means that the transfer was enqueued correctly. + * + * When the transfer is complete, a 'result' value will be provided as + * an argument to the callback function that will indicate if the transfer + * failed. + * + ****************************************************************************/ + +static int stm32_i2s_send(struct i2s_dev_s *dev, struct ap_buffer_s *apb, + i2s_callback_t callback, void *arg, uint32_t timeout) +{ + struct stm32_i2s_s *priv = (struct stm32_i2s_s *)dev; +#ifdef I2S_HAVE_TX + struct stm32_buffer_s *bfcontainer; + irqstate_t flags; + int ret; +#endif + + /* Make sure that we have valid pointers that that the data has uint32_t + * alignment. + */ + + DEBUGASSERT(priv && apb); + i2sinfo("apb=%p nbytes=%d arg=%p timeout=%d\n", + apb, apb->nbytes - apb->curbyte, arg, timeout); + + i2s_dump_buffer("Sending", &apb->samp[apb->curbyte], + apb->nbytes - apb->curbyte); + DEBUGASSERT(((uintptr_t)&apb->samp[apb->curbyte] & priv->align) == 0); + +#ifdef I2S_HAVE_TX + /* Allocate a buffer container in advance */ + + bfcontainer = i2s_buf_allocate(priv); + DEBUGASSERT(bfcontainer); + + /* Get exclusive access to the I2S driver data */ + + i2s_exclsem_take(priv); + + /* Has the TX channel been enabled? */ + + if (!priv->txenab) + { + i2serr("ERROR: I2S%d has no transmitter\n", priv->i2sno); + ret = -EAGAIN; + goto errout_with_exclsem; + } + + /* Add a reference to the audio buffer */ + + apb_reference(apb); + + /* Initialize the buffer container structure */ + + bfcontainer->callback = (void *)callback; + bfcontainer->timeout = timeout; + bfcontainer->arg = arg; + bfcontainer->apb = apb; + bfcontainer->result = -EBUSY; + + /* Add the buffer container to the end of the TX pending queue */ + + flags = enter_critical_section(); + sq_addlast((sq_entry_t *)bfcontainer, &priv->tx.pend); + + /* Then start the next transfer. If there is already a transfer in progess, + * then this will do nothing. + */ + + ret = i2s_txdma_setup(priv); + DEBUGASSERT(ret == OK); + leave_critical_section(flags); + i2s_exclsem_give(priv); + return OK; + +errout_with_exclsem: + i2s_exclsem_give(priv); + i2s_buf_free(priv, bfcontainer); + return ret; + +#else + i2serr("ERROR: I2S%d has no transmitter\n", priv->i2sno); + UNUSED(priv); + return -ENOSYS; +#endif +} + +/**************************************************************************** + * Name: i2s_mckdivider + * + * Description: + * Setup the MCK divider based on the currently selected data width and + * the sample rate + * + * Input Parameter: + * priv - I2C device structure (only the sample rate and data length is + * needed at this point). + * + * Returned Value: + * The current bitrate + * + ****************************************************************************/ + +static uint32_t i2s_mckdivider(struct stm32_i2s_s *priv) +{ +#ifdef I2S_HAVE_MCK + uint32_t bitrate; + uint32_t regval; + + uint16_t pllr = 5, plln = 256, div = 12, odd = 1; + + DEBUGASSERT(priv && priv->samplerate > 0 && priv->datalen > 0); + + /* A zero sample rate means to disable the MCK/2 clock */ + + if (priv->samplerate == 0) + { + bitrate = 0; + regval = 0; + } + else + { + int R, n, Od; + int Napprox; + int diff; + int diff_min = 500000000; + + for (Od = 0; Od <= 1; ++Od) + { + for (R = 2; R <= 7; ++R) + { + for (n = 2; n <= 256; ++n) + { + Napprox = roundf(priv->samplerate / 1000000.0f * (8 * 32 * R * (2 * n + Od))); + if ((Napprox > 432) || (Napprox < 50)) + { + continue; + } + + diff = abs(priv->samplerate - 1000000 * Napprox / (8 * 32 * R * (2 * n + Od))); + if (diff_min > diff) + { + diff_min = diff; + plln = Napprox; + pllr = R; + div = n; + odd = Od; + } + } + } + } + + /* Calculate the new bitrate in Hz */ + + bitrate = priv->samplerate * priv->datalen; + } + + /* Configure MCK divider */ + + /* Disable I2S */ + + i2s_putreg(priv, STM32_SPI_I2SCFGR_OFFSET, 0); + + /* I2S clock configuration */ + + putreg32((getreg32(STM32_RCC_CR) & (~RCC_CR_PLLI2SON)), STM32_RCC_CR); + + /* PLLI2S clock used as I2S clock source */ + + putreg32(((getreg32(STM32_RCC_CFGR)) & (~RCC_CFGR_I2SSRC)), STM32_RCC_CFGR); + regval = (pllr << 28) | (plln << 6); + putreg32(regval, STM32_RCC_PLLI2SCFGR); + + /* Enable PLLI2S and wait until it is ready */ + + putreg32((getreg32(STM32_RCC_CR) | RCC_CR_PLLI2SON), STM32_RCC_CR); + while (!(getreg32(STM32_RCC_CR) & RCC_CR_PLLI2SRDY)); + + i2s_putreg(priv, STM32_SPI_I2SPR_OFFSET, + div | (odd << 8) | SPI_I2SPR_MCKOE); + i2s_putreg(priv, STM32_SPI_I2SCFGR_OFFSET, + SPI_I2SCFGR_I2SMOD | SPI_I2SCFGR_I2SCFG_MTX | SPI_I2SCFGR_I2SE); + + putreg32(((getreg32(STM32_DMA1_HIFCR)) | 0x80000000 /* DMA_HIFCR_CTCIF7 */), + STM32_DMA1_HIFCR); + + return bitrate; +#else + return 0; +#endif +} + +/**************************************************************************** + * Name: i2s_dma_flags + * + * Description: + * Determine DMA FLAGS based on PID and data width + * + * Input Parameters: + * priv - Partially initialized I2C device structure. + * + * Returned Value: + * OK on success; a negated errno value on failure + * + ****************************************************************************/ + +static int i2s_dma_flags(struct stm32_i2s_s *priv) +{ + switch (priv->datalen) + { + case 8: + /* Reconfigure the RX DMA (and TX DMA if applicable) */ + priv->rxccr = SPI_RXDMA8_CONFIG; + priv->txccr = SPI_TXDMA8_CONFIG; + break; + + case 16: + priv->rxccr = SPI_RXDMA16_CONFIG; + priv->txccr = SPI_TXDMA16_CONFIG; + break; + + default: + i2serr("ERROR: Unsupported data width: %d\n", priv->datalen); + return -ENOSYS; + } + + return OK; +} + +/**************************************************************************** + * Name: i2s_dma_allocate + * + * Description: + * Allocate I2S DMA channels + * + * Input Parameters: + * priv - Partially initialized I2S device structure. This function + * will complete the DMA specific portions of the initialization + * + * Returned Value: + * OK on success; A negated errno value on failure. + * + ****************************************************************************/ + +static int i2s_dma_allocate(struct stm32_i2s_s *priv) +{ + int ret; + + /* Get the DMA flags for this channel */ + + ret = i2s_dma_flags(priv); + if (ret < 0) + { + i2serr("ERROR: i2s_dma_flags failed: %d\n", ret); + return ret; + } + + /* Allocate DMA channels. These allocations exploit that fact that + * I2S2 is managed by DMA1 and I2S3 is managed by DMA2. Hence, + * the I2S number (i2sno) is the same as the DMA number. + */ + +#ifdef I2S_HAVE_RX + if (priv->rxenab) + { + /* Allocate an RX DMA channel */ + + priv->rx.dma = stm32_dmachannel(DMAMAP_SPI3_RX_2); + if (!priv->rx.dma) + { + i2serr("ERROR: Failed to allocate the RX DMA channel\n"); + goto errout; + } + + /* Create a watchdog time to catch RX DMA timeouts */ + + priv->rx.dog = wd_create(); + if (!priv->rx.dog) + { + i2serr("ERROR: Failed to create the RX DMA watchdog\n"); + goto errout; + } + } +#endif + +#ifdef I2S_HAVE_TX + if (priv->txenab) + { + /* Allocate a TX DMA channel */ + + priv->tx.dma = stm32_dmachannel(DMAMAP_SPI3_TX_2); + if (!priv->tx.dma) + { + i2serr("ERROR: Failed to allocate the TX DMA channel\n"); + goto errout; + } + + /* Create a watchdog time to catch TX DMA timeouts */ + + priv->tx.dog = wd_create(); + if (!priv->tx.dog) + { + i2serr("ERROR: Failed to create the TX DMA watchdog\n"); + goto errout; + } + } +#endif + + /* Success exit */ + + return OK; + + /* Error exit */ + +errout: + i2s_dma_free(priv); + return -ENOMEM; +} + +/**************************************************************************** + * Name: i2s_dma_free + * + * Description: + * Release DMA-related resources allocated by i2s_dma_allocate() + * + * Input Parameters: + * priv - Partially initialized I2C device structure. + * + * Returned Value: + * None + * + ****************************************************************************/ + +static void i2s_dma_free(struct stm32_i2s_s *priv) +{ +#ifdef I2S_HAVE_TX + if (priv->tx.dog) + { + wd_delete(priv->tx.dog); + } + + if (priv->tx.dma) + { + stm32_dmafree(priv->tx.dma); + } +#endif + +#ifdef I2S_HAVE_RX + if (priv->rx.dog) + { + wd_delete(priv->rx.dog); + } + + if (priv->rx.dma) + { + stm32_dmafree(priv->rx.dma); + } +#endif +} + +/**************************************************************************** + * Name: i2s2_configure + * + * Description: + * Configure I2S2 + * + * Input Parameters: + * priv - Partially initialized I2C device structure. These functions + * will complete the I2S specific portions of the initialization + * + * Returned Value: + * None + * + ****************************************************************************/ + +#ifdef CONFIG_STM32_I2S2 +static void i2s2_configure(struct stm32_i2s_s *priv) +{ + /* Configure multiplexed pins as connected on the board. Chip + * select pins must be selected by board-specific logic. + */ + + priv->base = STM32_I2S2_BASE; + +#ifdef CONFIG_STM32_I2S2_RX + priv->rxenab = true; + + if ((i2s_getreg(priv, STM32_SPI_CR1_OFFSET) & SPI_CR1_SPE) == 0) + { + /* Configure I2S2 pins: MCK, SD, CK, WS */ + + stm32_configgpio(GPIO_I2S2_MCK); + stm32_configgpio(GPIO_I2S2_SD); + stm32_configgpio(GPIO_I2S2_CK); + stm32_configgpio(GPIO_I2S2_WS); + } +#endif /* CONFIG_STM32_I2S2_RX */ + +#ifdef CONFIG_STM32_I2S2_TX + priv->txenab = true; + + /* Only configure if the port is not already configured */ + + if ((i2s_getreg(priv, STM32_SPI_CR1_OFFSET) & SPI_CR1_SPE) == 0) + { + /* Configure I2S2 pins: MCK, SD, CK, WS */ + + stm32_configgpio(GPIO_I2S2_MCK); + stm32_configgpio(GPIO_I2S2_SD); + stm32_configgpio(GPIO_I2S2_CK); + stm32_configgpio(GPIO_I2S2_WS); + } +#endif /* CONFIG_STM32_I2S2_TX */ + + /* Configure driver state specific to this I2S peripheral */ + + priv->datalen = CONFIG_STM32_I2S2_DATALEN; +#ifdef CONFIG_DEBUG + priv->align = STM32_I2S2_DATAMASK; +#endif +} +#endif /* CONFIG_STM32_I2S2 */ + +/**************************************************************************** + * Name: i2s3_configure + * + * Description: + * Configure I2S3 + * + * Input Parameters: + * priv - Partially initialized I2C device structure. These functions + * will complete the I2S specific portions of the initialization + * + * Returned Value: + * None + * + ****************************************************************************/ + +#ifdef CONFIG_STM32_I2S3 +static void i2s3_configure(struct stm32_i2s_s *priv) +{ + /* Configure multiplexed pins as connected on the board. Chip + * select pins must be selected by board-specific logic. + */ + + priv->base = STM32_I2S3_BASE; + +#ifdef CONFIG_STM32_I2S3_RX + priv->rxenab = true; + + if ((i2s_getreg(priv, STM32_SPI_CR1_OFFSET) & SPI_CR1_SPE) == 0) + { + /* Configure I2S3 pins: MCK, SD, CK, WS */ + + stm32_configgpio(GPIO_I2S3_MCK); + stm32_configgpio(GPIO_I2S3_SD); + stm32_configgpio(GPIO_I2S3_CK); + stm32_configgpio(GPIO_I2S3_WS); + } +#endif /* CONFIG_STM32_I2S3_RX */ + +#ifdef CONFIG_STM32_I2S3_TX + priv->txenab = true; + + /* Only configure if the port is not already configured */ + + if ((i2s_getreg(priv, STM32_SPI_CR1_OFFSET) & SPI_CR1_SPE) == 0) + { + /* Configure I2S3 pins: MCK, SD, CK, WS */ + + stm32_configgpio(GPIO_I2S3_MCK); + stm32_configgpio(GPIO_I2S3_SD); + stm32_configgpio(GPIO_I2S3_CK); + stm32_configgpio(GPIO_I2S3_WS); + } +#endif /* CONFIG_STM32_I2S3_TX */ + + /* Configure driver state specific to this I2S peripheral */ + + priv->datalen = CONFIG_STM32_I2S3_DATALEN; +#ifdef CONFIG_DEBUG + priv->align = STM32_I2S3_DATAMASK; +#endif +} +#endif /* CONFIG_STM32_I2S3 */ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_i2sdev_initialize + * + * Description: + * Initialize the selected i2S port + * + * Input Parameter: + * Port number (for hardware that has mutiple I2S interfaces) + * + * Returned Value: + * Valid I2S device structure reference on succcess; a NULL on failure + * + ************************************************************************************/ + +FAR struct i2s_dev_s *stm32_i2sdev_initialize(int port) +{ + FAR struct stm32_i2s_s *priv = NULL; + irqstate_t flags; + int ret; + + /* The support STM32 parts have only a single I2S port */ + + i2sinfo("port: %d\n", port); + + /* Allocate a new state structure for this chip select. NOTE that there + * is no protection if the same chip select is used in two different + * chip select structures. + */ + + priv = (struct stm32_i2s_s *)zalloc(sizeof(struct stm32_i2s_s)); + if (!priv) + { + i2serr("ERROR: Failed to allocate a chip select structure\n"); + return NULL; + } + + /* Set up the initial state for this chip select structure. Other fields + * were zeroed by zalloc(). + */ + + /* Initialize the common parts for the I2S device structure */ + + sem_init(&priv->exclsem, 0, 1); + priv->dev.ops = &g_i2sops; + priv->i2sno = port; + + /* Initialize buffering */ + + i2s_buf_initialize(priv); + + flags = enter_critical_section(); + +#ifdef CONFIG_STM32_I2S2 + if (port == 2) + { + /* Select I2S2 */ + + i2s2_configure(priv); + } + else +#endif +#ifdef CONFIG_STM32_I2S3 + if (port == 3) + { + /* Select I2S3 */ + + i2s3_configure(priv); + } + else +#endif + { + i2serr("ERROR: Unsupported I2S port: %d\n", port); + return NULL; + } + + /* Allocate DMA channels */ + + ret = i2s_dma_allocate(priv); + if (ret < 0) + { + goto errout_with_alloc; + } + + leave_critical_section(flags); + i2s_dump_regs(priv, "After initialization"); + + /* Success exit */ + + return &priv->dev; + + /* Failure exits */ + +errout_with_alloc: + sem_destroy(&priv->exclsem); + kmm_free(priv); + return NULL; +} +#endif /* I2S_HAVE_RX || I2S_HAVE_TX */ + +#endif /* CONFIG_STM32_I2S2 || CONFIG_STM32_I2S3 */ diff --git a/arch/arm/src/stm32/stm32_i2s.h b/arch/arm/src/stm32/stm32_i2s.h new file mode 100644 index 0000000000..5e6d51b817 --- /dev/null +++ b/arch/arm/src/stm32/stm32_i2s.h @@ -0,0 +1,90 @@ +/************************************************************************************ + * arch/arm/src/stm32/stm32_i2s.h + * + * Copyright (C) 2017 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ + +#ifndef __ARCH_ARM_SRC_STM32_I2S_H +#define __ARCH_ARM_SRC_STM32_I2S_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#include + +#include "chip.h" +#include "chip/stm32_i2s.h" + +#ifndef __ASSEMBLY__ + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" +{ +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_i2sdev_initialize + * + * Description: + * Initialize the selected I2S port + * + * Input Parameter: + * Port number (for hardware that has mutiple I2S interfaces) + * + * Returned Value: + * Valid I2S device structure reference on succcess; a NULL on failure + * + ************************************************************************************/ + +FAR struct i2s_dev_s *stm32_i2sdev_initialize(int port); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_ARM_SRC_STM32_I2S_H */ diff --git a/configs/stm32f4discovery/include/board.h b/configs/stm32f4discovery/include/board.h index 805d5f6ce1..91077e674f 100644 --- a/configs/stm32f4discovery/include/board.h +++ b/configs/stm32f4discovery/include/board.h @@ -300,9 +300,9 @@ /* SPI - There is a MEMS device on SPI1 using these pins: */ -#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 -#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 -#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 /* SPI2 - Test MAX31855 on SPI2 PB10 = SCK, PB14 = MISO */ @@ -310,10 +310,38 @@ #define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 #define GPIO_SPI2_SCK GPIO_SPI2_SCK_1 +/* SPI3 - Onboard devices use SPI3 */ + +#define GPIO_SPI3_MISO GPIO_SPI3_MISO_2 +#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_2 +#define GPIO_SPI3_SCK GPIO_SPI3_SCK_2 +#define GPIO_SPI3_NSS GPIO_SPI3_NSS_2 + +/* I2S3 - Onboard devices use I2S3 */ + +#define GPIO_I2S3_SD GPIO_I2S3_SD_2 +#define GPIO_I2S3_CK GPIO_I2S3_CK_2 +#define GPIO_I2S3_WS GPIO_I2S3_WS_1 + +#define DMACHAN_SPI3_RX DMAMAP_SPI3_RX_2 +#define DMACHAN_SPI3_TX DMAMAP_SPI3_TX_2 + /* I2C config to use with Nunchuk PB7 (SDA) and PB8 (SCL) */ +#if 0 #define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 #define GPIO_I2C1_SDA GPIO_I2C1_SDA_1 +#endif + +/* I2C. Only I2C1 is available on the stm32f4discovery. I2C1_SCL and I2C1_SDA are + * available on the following pins: + * + * - PB6 is I2C1_SCL + * - PB9 is I2C1_SDA + */ + +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_1 +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* Timer Inputs/Outputs (see the README.txt file for options) */ diff --git a/configs/stm32f4discovery/src/Makefile b/configs/stm32f4discovery/src/Makefile index d51d09ce1d..d7154251b3 100644 --- a/configs/stm32f4discovery/src/Makefile +++ b/configs/stm32f4discovery/src/Makefile @@ -44,6 +44,10 @@ else CSRCS += stm32_userleds.c endif +ifeq ($(CONFIG_AUDIO_CS43L22),y) +CSRCS += stm32_cs43l22.c +endif + ifeq ($(CONFIG_ARCH_BUTTONS),y) CSRCS += stm32_buttons.c endif diff --git a/configs/stm32f4discovery/src/stm32_bringup.c b/configs/stm32f4discovery/src/stm32_bringup.c index e360c8cc0c..042c3386fc 100644 --- a/configs/stm32f4discovery/src/stm32_bringup.c +++ b/configs/stm32f4discovery/src/stm32_bringup.c @@ -216,6 +216,16 @@ int stm32_bringup(void) } #endif +#ifdef HAVE_CS43L22 + /* Configure CS43L22 audio */ + + ret = stm32_cs43l22_initialize(1); + if (ret != OK) + { + serr("Failed to initialize CS43L22 audio: %d\n", ret); + } +#endif + #ifdef HAVE_ELF /* Initialize the ELF binary loader */ diff --git a/configs/stm32f4discovery/src/stm32_cs43l22.c b/configs/stm32f4discovery/src/stm32_cs43l22.c new file mode 100644 index 0000000000..156458ead8 --- /dev/null +++ b/configs/stm32f4discovery/src/stm32_cs43l22.c @@ -0,0 +1,389 @@ +/************************************************************************************ + * configs/stm32f4discovery/src/stm32_cs43l22.c + * + * Copyright (C) 2017 Gregory Nutt. All rights reserved. + * Author: Taras Drozdovskiy + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include "stm32.h" +#include "stm32f4discovery.h" + +#ifdef HAVE_CS43L22 + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +struct stm32_mwinfo_s +{ + /* Standard CS43L22 interface */ + + struct cs43l22_lower_s lower; + + /* Extensions for the stm32f4discovery board */ + + cs43l22_handler_t handler; + FAR void *arg; +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/* IRQ/PIO access callbacks. These operations all hidden behind + * callbacks to isolate the CS43L22 driver from differences in PIO + * interrupt handling by varying boards and MCUs. If possible, + * interrupts should be configured on both rising and falling edges + * so that contact and loss-of-contact events can be detected. + * + * attach - Attach the CS43L22 interrupt handler to the PIO interrupt + * enable - Enable or disable the PIO interrupt + */ + +static int cs43l22_attach(FAR const struct cs43l22_lower_s *lower, + cs43l22_handler_t isr, FAR void *arg); +static bool cs43l22_enable(FAR const struct cs43l22_lower_s *lower, + bool enable); +static void cs43l22_hw_reset(FAR const struct cs43l22_lower_s *lower); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/* A reference to a structure of this type must be passed to the CS43L22 + * driver. This structure provides information about the configuration + * of the CS43L22 and provides some board-specific hooks. + * + * Memory for this structure is provided by the caller. It is not copied + * by the driver and is presumed to persist while the driver is active. + */ + +#define CONFIG_STM32_CS43L22_I2CFREQUENCY 100000 +#define BOARD_MAINCK_FREQUENCY 8000000 + +static struct stm32_mwinfo_s g_cs43l22info = +{ + .lower = + { + .address = CS43L22_I2C_ADDRESS, + .frequency = CONFIG_STM32_CS43L22_I2CFREQUENCY, +#ifdef CONFIG_STM32_CS43L22_SRCSCK + .mclk = BOARD_SLOWCLK_FREQUENCY, +#else + .mclk = BOARD_MAINCK_FREQUENCY, +#endif + .attach = cs43l22_attach, + .enable = cs43l22_enable, + .reset = cs43l22_hw_reset, + }, +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * IRQ/PIO access callbacks. These operations all hidden behind + * callbacks to isolate the CS43L22 driver from differences in PIO + * interrupt handling by varying boards and MCUs. If possible, + * interrupts should be configured on both rising and falling edges + * so that contact and loss-of-contact events can be detected. + * + * attach - Attach the CS43L22 interrupt handler to the PIO interrupt + * enable - Enable or disable the PIO interrupt + * clear - Acknowledge/clear any pending PIO interrupt + * + ****************************************************************************/ + +static int cs43l22_attach(FAR const struct cs43l22_lower_s *lower, + cs43l22_handler_t isr, FAR void *arg) +{ + if (isr) + { + /* Just save the address of the handler and its argument for now. The + * new handler will called via cs43l22_interrupt() when the interrupt occurs. + */ + + audinfo("Attaching %p\n", isr); + g_cs43l22info.handler = isr; + g_cs43l22info.arg = arg; + } + else + { + audinfo("Detaching %p\n", g_cs43l22info.handler); + (void)cs43l22_enable(lower, false); + g_cs43l22info.handler = NULL; + g_cs43l22info.arg = NULL; + } + + return OK; +} + +static bool cs43l22_enable(FAR const struct cs43l22_lower_s *lower, bool enable) +{ + static bool enabled; + irqstate_t flags; + bool ret; + + /* Has the interrupt state changed */ + + flags = enter_critical_section(); + if (enable != enabled) + { + /* Enable or disable interrupts */ + + if (enable && g_cs43l22info.handler) + { + audinfo("Enabling\n"); + /* TODO: stm32_pioirqenable(IRQ_INT_CS43L22); */ + enabled = true; + } + else + { + + audinfo("Disabling\n"); + /* TODO: stm32_pioirqdisable(IRQ_INT_CS43L22); */ + enabled = false; + } + } + + ret = enabled; + leave_critical_section(flags); + return ret; +} + +#if 0 +static int cs43l22_interrupt(int irq, FAR void *context) +{ + Just forward the interrupt to the CS43L22 driver + + audinfo("handler %p\n", g_cs43l22info.handler); + if (g_cs43l22info.handler) + { + return g_cs43l22info.handler(&g_cs43l22info.lower, g_cs43l22info.arg); + } + + We got an interrupt with no handler. This should not + happen. + + TODO: stm32_pioirqdisable(IRQ_INT_CS43L22); + return OK; +} +#endif + +static void cs43l22_hw_reset(FAR const struct cs43l22_lower_s *lower) +{ + int i; + + /* Reset the codec */ + + stm32_gpiowrite(GPIO_CS43L22_RESET, false); + for (i = 0; i < 0x4fff; i++) + { + __asm__ volatile("nop"); + } + + stm32_gpiowrite(GPIO_CS43L22_RESET, true); +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_cs43l22_initialize + * + * Description: + * This function is called by platform-specific, setup logic to configure + * and register the CS43L22 device. This function will register the driver + * as /dev/audio/pcm[x] where x is determined by the minor device number. + * + * Input Parameters: + * minor - The input device minor number + * + * Returned Value: + * Zero is returned on success. Otherwise, a negated errno value is + * returned to indicate the nature of the failure. + * + ****************************************************************************/ + +int stm32_cs43l22_initialize(int minor) +{ + FAR struct audio_lowerhalf_s *cs43l22; + FAR struct audio_lowerhalf_s *pcm; + FAR struct i2c_master_s *i2c; + FAR struct i2s_dev_s *i2s; + static bool initialized = false; + char devname[12]; + int ret; + + audinfo("minor %d\n", minor); + DEBUGASSERT(minor >= 0 && minor <= 25); + + /* Have we already initialized? Since we never uninitialize we must prevent + * multiple initializations. This is necessary, for example, when the + * touchscreen example is used as a built-in application in NSH and can be + * called numerous time. It will attempt to initialize each time. + */ + + if (!initialized) + { + stm32_configgpio(GPIO_CS43L22_RESET); + + /* Configure the CS43L22 interrupt pin */ + + /* TODO: (void)stm32_configgpio(PIO_INT_CS43L22); */ + + /* Get an instance of the I2C interface for the CS43L22 chip select */ + + i2c = stm32_i2cbus_initialize(CS43L22_I2C_BUS); + if (!i2c) + { + auderr("ERROR: Failed to initialize TWI%d\n", CS43L22_I2C_BUS); + ret = -ENODEV; + goto errout; + } + /* Get an instance of the I2S interface for the CS43L22 data channel */ + + i2s = stm32_i2sdev_initialize(CS43L22_I2S_BUS); + if (!i2s) + { + auderr("ERROR: Failed to initialize I2S%d\n", CS43L22_I2S_BUS); + ret = -ENODEV; + goto errout_with_i2c; + } + + /* Configure the DAC master clock. This clock is provided by PCK2 (PB10) + * that is connected to the CS43L22 MCLK. + */ + + /* Configure CS43L22 interrupts */ + +#if 0 /* TODO: */ + stm32_pioirq(PIO_INT_CS43L22); + ret = irq_attach(IRQ_INT_CS43L22, cs43l22_interrupt); + if (ret < 0) + { + auderr("ERROR: Failed to attach CS43L22 interrupt: %d\n", ret); + goto errout_with_i2s; + } +#endif + + /* Now we can use these I2C and I2S interfaces to initialize the + * CS43L22 which will return an audio interface. + */ + + cs43l22 = cs43l22_initialize(i2c, i2s, &g_cs43l22info.lower); + if (!cs43l22) + { + auderr("ERROR: Failed to initialize the CS43L22\n"); + ret = -ENODEV; + goto errout_with_irq; + } + /* No we can embed the CS43L22/I2C/I2S conglomerate into a PCM decoder + * instance so that we will have a PCM front end for the the CS43L22 + * driver. + */ + + pcm = pcm_decode_initialize(cs43l22); + if (!pcm) + { + auderr("ERROR: Failed create the PCM decoder\n"); + ret = -ENODEV; + goto errout_with_cs43l22; + } + /* Create a device name */ + + snprintf(devname, 12, "pcm%d", minor); + + /* Finally, we can register the PCM/CS43L22/I2C/I2S audio device. + * + * Is anyone young enough to remember Rube Goldberg? + */ + + ret = audio_register(devname, pcm); + if (ret < 0) + { + auderr("ERROR: Failed to register /dev/%s device: %d\n", + devname, ret); + goto errout_with_pcm; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; + + /* Error exits. Unfortunately there is no mechanism in place now to + * recover resources from most errors on initialization failures. + */ + +errout_with_pcm: +errout_with_cs43l22: +errout_with_irq: + +#if 0 + irq_detach(IRQ_INT_CS43L22); +errout_with_i2s: +#endif + +errout_with_i2c: +errout: + return ret; +} + +#endif /* HAVE_CS43L22 */ diff --git a/configs/stm32f4discovery/src/stm32f4discovery.h b/configs/stm32f4discovery/src/stm32f4discovery.h index 75892830ad..ebb618095e 100644 --- a/configs/stm32f4discovery/src/stm32f4discovery.h +++ b/configs/stm32f4discovery/src/stm32f4discovery.h @@ -77,6 +77,7 @@ #define HAVE_USBHOST 1 #define HAVE_USBMONITOR 1 #define HAVE_SDIO 1 +#define HAVE_CS43L22 1 #define HAVE_RTC_DRIVER 1 #define HAVE_ELF 1 #define HAVE_NETMONITOR 1 @@ -148,6 +149,26 @@ # endif #endif +/* The CS43L22 depends on the CS43L22 driver, I2C1, and I2S3 support */ + +#if !defiend(CONFIG_AUDIO_CS43L22) || !defined(CONFIG_STM32_I2C1) || \ + !defined(CONFIG_STM32_I2S3) +# undef HAVE_CS43L22 +#endif + +#ifdef HAVE_CS43L22 + /* The CS43L22 communicates on I2C1, I2C address 0x1a for control + * operations + */ + +# define CS43L22_I2C_BUS 1 +# define CS43L22_I2C_ADDRESS (0x94 >> 1) + + /* The CS43L22 transfers data on I2S3 */ + +# define CS43L22_I2S_BUS 3 +#endif + /* Check if we can support the RTC driver */ #if !defined(CONFIG_RTC) || !defined(CONFIG_RTC_DRIVER) @@ -216,6 +237,8 @@ #define GPIO_ZEROCROSS (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN0) +#define GPIO_CS43L22_RESET (GPIO_OUTPUT|GPIO_SPEED_50MHz|GPIO_PORTD|GPIO_PIN4) + /* PWM * * The STM32F4 Discovery has no real on-board PWM devices, but the board can be @@ -369,6 +392,17 @@ void weak_function stm32_spidev_initialize(void); + /**************************************************************************** + * Name: stm32_i2sdev_initialize + * + * Description: + * Called to configure I2S chip select GPIO pins for the stm32f4discovery + * board. + * + ****************************************************************************/ + + FAR struct i2s_dev_s *stm32_i2sdev_initialize(int port); + /**************************************************************************** * Name: stm32_bh1750initialize * @@ -608,6 +642,27 @@ int stm32_zerocross_initialize(void); int stm32_max6675initialize(FAR const char *devpath); #endif +/**************************************************************************** + * Name: stm32_cs43l22_initialize + * + * Description: + * This function is called by platform-specific, setup logic to configure + * and register the CS43L22 device. This function will register the driver + * as /dev/cs43l22[x] where x is determined by the minor device number. + * + * Input Parameters: + * minor - The input device minor number + * + * Returned Value: + * Zero is returned on success. Otherwise, a negated errno value is + * returned to indicate the nature of the failure. + * + ****************************************************************************/ + +#ifdef HAVE_CS43L22 +int stm32_cs43l22_initialize(int minor); +#endif /* HAVE_CS43L22 */ + /**************************************************************************** * Name: stm32_pca9635_initialize * diff --git a/drivers/audio/Kconfig b/drivers/audio/Kconfig index ade8bc9375..3de9d1f60a 100644 --- a/drivers/audio/Kconfig +++ b/drivers/audio/Kconfig @@ -95,6 +95,67 @@ endif # AUDIO_DRIVER_SPECIFIC_BUFFERS endif # VS1053 +config AUDIO_CS43L22 + bool "CS43L22 audio chip" + default n + depends on AUDIO + ---help--- + Select to enable support for the CS43L22 Audio codec by Cirrus Logic. + This chip is a lower level audio chip.. basically + an exotic D-to-A. It includes no built-in support for audio CODECS + The CS43L22 provides: + + - Low power consumption + - High SNR + - Stereo digital microphone input + - Digital Dynamic Range Controller (compressor / limiter) + - Digital sidetone mixing + - Ground-referenced headphone driver + - Ground-referenced line outputs + + NOTE: This driver also depends on both I2C and I2S support although + that dependency is not explicit here. + +if AUDIO_CS43L22 + +config CS43L22_INITVOLUME + int "CS43L22 initial volume setting" + default 250 + +config CS43L22_INFLIGHT + int "CS43L22 maximum in-flight audio buffers" + default 2 + +config CS43L22_MSG_PRIO + int "CS43L22 message priority" + default 1 + +config CS43L22_BUFFER_SIZE + int "CS43L22 preferred buffer size" + default 8192 + +config CS43L22_NUM_BUFFERS + int "CS43L22 preferred number of buffers" + default 4 + +config CS43L22_WORKER_STACKSIZE + int "CS43L22 worker thread stack size" + default 768 + +config CS43L22_REGDUMP + bool "CS43L22 register dump" + default n + ---help--- + Enable logic to dump the contents of all CS43L22 registers. + +config CS43L22_CLKDEBUG + bool "CS43L22 clock analysis" + default n + ---help--- + Enable logic to analyze CS43L22 clock configuation. + +endif # AUDIO_CS43L22 + config AUDIO_WM8904 bool "WM8904 audio chip" default n diff --git a/drivers/audio/Make.defs b/drivers/audio/Make.defs index 5932714f6d..96db396860 100644 --- a/drivers/audio/Make.defs +++ b/drivers/audio/Make.defs @@ -43,6 +43,17 @@ ifeq ($(CONFIG_VS1053),y) CSRCS += vs1053.c endif +ifeq ($(CONFIG_AUDIO_CS43L22),y) +CSRCS += cs43l22.c +ifeq ($(CONFIG_CS43L22_REGDUMP),y) +CSRCS += cs43l22_debug.c +else +ifeq ($(CONFIG_CS43L22_CLKDEBUG),y) +CSRCS += cs43l22_debug.c +endif +endif +endif + ifeq ($(CONFIG_AUDIO_WM8904),y) CSRCS += wm8904.c ifeq ($(CONFIG_WM8904_REGDUMP),y) diff --git a/drivers/audio/cs43l22.c b/drivers/audio/cs43l22.c new file mode 100644 index 0000000000..239144c6da --- /dev/null +++ b/drivers/audio/cs43l22.c @@ -0,0 +1,1959 @@ +/**************************************************************************** + * drivers/audio/cs43l22.c + * Audio device driver for Cirrus logic CS43L22 Audio codec. + * + * Copyright (C) 2017 Gregory Nutt. All rights reserved. + * Author: Taras Drozdovskiy + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cs43l22.h" + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +#if !defined(CONFIG_CS43L22_REGDUMP) && !defined(CONFIG_CS43L22_CLKDEBUG) +static +#endif +uint8_t cs43l22_readreg(FAR struct cs43l22_dev_s *priv, uint8_t regaddr); +static void cs43l22_writereg(FAR struct cs43l22_dev_s *priv, uint8_t regaddr, + uint8_t regval); +static void cs43l22_takesem(sem_t * sem); +#define cs43l22_givesem(s) sem_post(s) + +#ifndef CONFIG_AUDIO_EXCLUDE_VOLUME +static inline uint16_t cs43l22_scalevolume(uint16_t volume, b16_t scale); +static void cs43l22_setvolume(FAR struct cs43l22_dev_s *priv, uint16_t volume, + bool mute); +#endif +#ifndef CONFIG_AUDIO_EXCLUDE_TONE +static void cs43l22_setbass(FAR struct cs43l22_dev_s *priv, uint8_t bass); +static void cs43l22_settreble(FAR struct cs43l22_dev_s *priv, uint8_t treble); +#endif + +static void cs43l22_setdatawidth(FAR struct cs43l22_dev_s *priv); +static void cs43l22_setbitrate(FAR struct cs43l22_dev_s *priv); + +/* Audio lower half methods (and close friends) */ + +static int cs43l22_getcaps(FAR struct audio_lowerhalf_s *dev, int type, + FAR struct audio_caps_s *caps); +#ifdef CONFIG_AUDIO_MULTI_SESSION +static int cs43l22_configure(FAR struct audio_lowerhalf_s *dev, + FAR void *session, + FAR const struct audio_caps_s *caps); +#else +static int cs43l22_configure(FAR struct audio_lowerhalf_s *dev, + FAR const struct audio_caps_s *caps); +#endif +static int cs43l22_shutdown(FAR struct audio_lowerhalf_s *dev); +static void cs43l22_senddone(FAR struct i2s_dev_s *i2s, + FAR struct ap_buffer_s *apb, FAR void *arg, + int result); +static void cs43l22_returnbuffers(FAR struct cs43l22_dev_s *priv); +static int cs43l22_sendbuffer(FAR struct cs43l22_dev_s *priv); + +#ifdef CONFIG_AUDIO_MULTI_SESSION +static int cs43l22_start(FAR struct audio_lowerhalf_s *dev, FAR void *session); +#else +static int cs43l22_start(FAR struct audio_lowerhalf_s *dev); +#endif +#ifndef CONFIG_AUDIO_EXCLUDE_STOP +#ifdef CONFIG_AUDIO_MULTI_SESSION +static int cs43l22_stop(FAR struct audio_lowerhalf_s *dev, FAR void *session); +#else +static int cs43l22_stop(FAR struct audio_lowerhalf_s *dev); +#endif +#endif +#ifndef CONFIG_AUDIO_EXCLUDE_PAUSE_RESUME +#ifdef CONFIG_AUDIO_MULTI_SESSION +static int cs43l22_pause(FAR struct audio_lowerhalf_s *dev, FAR void *session); +static int cs43l22_resume(FAR struct audio_lowerhalf_s *dev, FAR void *session); +#else +static int cs43l22_pause(FAR struct audio_lowerhalf_s *dev); +static int cs43l22_resume(FAR struct audio_lowerhalf_s *dev); +#endif +#endif +static int cs43l22_enqueuebuffer(FAR struct audio_lowerhalf_s *dev, + FAR struct ap_buffer_s *apb); +static int cs43l22_cancelbuffer(FAR struct audio_lowerhalf_s *dev, + FAR struct ap_buffer_s *apb); +static int cs43l22_ioctl(FAR struct audio_lowerhalf_s *dev, int cmd, + unsigned long arg); +#ifdef CONFIG_AUDIO_MULTI_SESSION +static int cs43l22_reserve(FAR struct audio_lowerhalf_s *dev, + FAR void **session); +#else +static int cs43l22_reserve(FAR struct audio_lowerhalf_s *dev); +#endif +#ifdef CONFIG_AUDIO_MULTI_SESSION +static int cs43l22_release(FAR struct audio_lowerhalf_s *dev, + FAR void *session); +#else +static int cs43l22_release(FAR struct audio_lowerhalf_s *dev); +#endif + +/* Interrupt handling an worker thread */ + +#ifdef CS43L22_USE_FFLOCK_INT +static void cs43l22_interrupt_work(FAR void *arg); +static int cs43l22_interrupt(FAR const struct cs43l22_lower_s *lower, + FAR void *arg); +#endif + +static void *cs43l22_workerthread(pthread_addr_t pvarg); + +/* Initialization */ + +static void cs43l22_audio_output(FAR struct cs43l22_dev_s *priv); +#if 0 /* Not used */ +static void cs43l22_audio_input(FAR struct cs43l22_dev_s *priv); +#endif +#ifdef CS43L22_USE_FFLOCK_INT +static void cs43l22_configure_ints(FAR struct cs43l22_dev_s *priv); +#else +# define cs43l22_configure_ints(p) +#endif +static void cs43l22_reset(FAR struct cs43l22_dev_s *priv); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static const struct audio_ops_s g_audioops = +{ + cs43l22_getcaps, /* getcaps */ + cs43l22_configure, /* configure */ + cs43l22_shutdown, /* shutdown */ + cs43l22_start, /* start */ +#ifndef CONFIG_AUDIO_EXCLUDE_STOP + cs43l22_stop, /* stop */ +#endif +#ifndef CONFIG_AUDIO_EXCLUDE_PAUSE_RESUME + cs43l22_pause, /* pause */ + cs43l22_resume, /* resume */ +#endif + NULL, /* allocbuffer */ + NULL, /* freebuffer */ + cs43l22_enqueuebuffer, /* enqueue_buffer */ + cs43l22_cancelbuffer, /* cancel_buffer */ + cs43l22_ioctl, /* ioctl */ + NULL, /* read */ + NULL, /* write */ + cs43l22_reserve, /* reserve */ + cs43l22_release /* release */ +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cs43l22_readreg + * + * Description + * Read the specified 16-bit register from the CS43L22 device. + * + ****************************************************************************/ + +#if !defined(CONFIG_CS43L22_REGDUMP) && !defined(CONFIG_CS43L22_CLKDEBUG) +static +#endif +uint8_t cs43l22_readreg(FAR struct cs43l22_dev_s *priv, uint8_t regaddr) +{ + int retries; + + /* Try up to three times to read the register */ + + for (retries = 1; retries <= 3; retries++) + { + struct i2c_msg_s msg[2]; + uint8_t data; + int ret; + + /* Set up to write the address */ + + msg[0].frequency = priv->lower->frequency; + msg[0].addr = priv->lower->address; + msg[0].flags = 0; + msg[0].buffer = ®addr; + msg[0].length = 1; + + /* Followed by the read data */ + + msg[1].frequency = priv->lower->frequency; + msg[1].addr = priv->lower->address; + msg[1].flags = I2C_M_READ; + msg[1].buffer = &data; + msg[1].length = 12; + + /* Read the register data. The returned value is the number messages + * completed. + */ + + ret = I2C_TRANSFER(priv->i2c, msg, 2); + if (ret < 0) + { +#ifdef CONFIG_I2C_RESET + /* Perhaps the I2C bus is locked up? Try to shake the bus free */ + + audwarn("WARNING: I2C_TRANSFER failed: %d ... Resetting\n", ret); + + ret = I2C_RESET(priv->i2c); + if (ret < 0) + { + auderr("ERROR: I2C_RESET failed: %d\n", ret); + break; + } +#else + auderr("ERROR: I2C_TRANSFER failed: %d\n", ret); +#endif + } + else + { + + /* The I2C transfer was successful... break out of the loop and + * return the value read. + */ + + audinfo("Read: %02x -> %02x\n", regaddr, data); + return data; + } + + audinfo("retries=%d regaddr=%02x\n", retries, regaddr); + } + + /* No error indication is returned on a failure... just return zero */ + + return 0; +} + +/************************************************************************************ + * Name: cs43l22_writereg + * + * Description: + * Write the specified 16-bit register to the CS43L22 device. + * + ************************************************************************************/ + +static void +cs43l22_writereg(FAR struct cs43l22_dev_s *priv, uint8_t regaddr, + uint8_t regval) +{ + struct i2c_config_s config; + int retries; + + /* Setup up the I2C configuration */ + + config.frequency = priv->lower->frequency; + config.address = priv->lower->address; + config.addrlen = 7; + + /* Try up to three times to read the register */ + + for (retries = 1; retries <= 3; retries++) + { + uint8_t data[2]; + int ret; + + /* Set up the data to write */ + + data[0] = regaddr; + data[1] = regval; + + /* Read the register data. The returned value is the number messages + * completed. + */ + + ret = i2c_write(priv->i2c, &config, data, 2); + if (ret < 0) + { +#ifdef CONFIG_I2C_RESET + /* Perhaps the I2C bus is locked up? Try to shake the bus free */ + + audwarn("WARNING: i2c_write failed: %d ... Resetting\n", ret); + + ret = I2C_RESET(priv->i2c); + if (ret < 0) + { + auderr("ERROR: I2C_RESET failed: %d\n", ret); + break; + } +#else + auderr("ERROR: I2C_TRANSFER failed: %d\n", ret); +#endif + } + else + { + /* The I2C transfer was successful... break out of the loop and + * return the value read. + */ + + audinfo("Write: %02x <- %02x\n", regaddr, regval); + return; + } + + audinfo("retries=%d regaddr=%02x\n", retries, regaddr); + } +} + +/************************************************************************************ + * Name: cs43l22_takesem + * + * Description: + * Take a semaphore count, handling the nasty EINTR return if we are interrupted + * by a signal. + * + ************************************************************************************/ + +static void cs43l22_takesem(sem_t * sem) +{ + int ret; + + do + { + ret = sem_wait(sem); + DEBUGASSERT(ret == 0 || errno == EINTR); + } + while (ret < 0); +} + +/************************************************************************************ + * Name: cs43l22_scalevolume + * + * Description: + * Set the right and left volume values in the CS43L22 device based on the current + * volume and balance settings. + * + ************************************************************************************/ + +#ifndef CONFIG_AUDIO_EXCLUDE_VOLUME +static inline uint16_t cs43l22_scalevolume(uint16_t volume, b16_t scale) +{ + return b16toi((b16_t) volume * scale); +} +#endif + +/************************************************************************************ + * Name: cs43l22_setvolume + * + * Description: + * Set the right and left volume values in the CS43L22 device based on the current + * volume and balance settings. + * + ************************************************************************************/ + +#ifndef CONFIG_AUDIO_EXCLUDE_VOLUME +static void +cs43l22_setvolume(FAR struct cs43l22_dev_s *priv, uint16_t volume, bool mute) +{ + uint32_t leftlevel; + uint32_t rightlevel; + uint8_t regval; + + audinfo("volume=%u mute=%u\n", volume, mute); + +#ifndef CONFIG_AUDIO_EXCLUDE_BALANCE + /* Calculate the left channel volume level {0..1000} */ + + if (priv->balance <= 500) + { + leftlevel = volume; + } + else if (priv->balance == 1000) + { + leftlevel = 0; + } + else + { + leftlevel = ((((1000 - priv->balance) * 100) / 500) * volume) / 100; + } + +/* Calculate the right channel volume level {0..1000} */ + + if (priv->balance >= 500) + { + rightlevel = volume; + } + else if (priv->balance == 0) + { + rightlevel = 0; + } + else + { + rightlevel = (((priv->balance * 100) / 500) * volume) / 100; + } + +# else + leftlevel = priv->volume; + rightlevel = priv->volume; +# endif + + /* Set the volume */ + + regval = (rightlevel + 0x19) & 0xff; + cs43l22_writereg(priv, CS43L22_MS_VOL_CTRL_A, regval); + regval = ((leftlevel + 0x19) & 0xff); + cs43l22_writereg(priv, CS43L22_MS_VOL_CTRL_B, regval); + +#if 0 + regval = (rightlevel + 0x01) & 0xff; + cs43l22_writereg(priv, CS43L22_HP_VOL_CTRL_A, regval); + regval = (leftlevel + 0x01) & 0xff; + cs43l22_writereg(priv, CS43L22_HP_VOL_CTRL_B, regval); +#endif + + regval = cs43l22_readreg(priv, CS43L22_PLAYBACK_CTRL2); + + if (mute) + { + regval |= (CS43L22_HPAMUTE | CS43L22_HPBMUTE); + } + else + { + regval &= ~(CS43L22_HPAMUTE | CS43L22_HPBMUTE); + } + + cs43l22_writereg(priv, CS43L22_PLAYBACK_CTRL2, regval); + + /* Remember the volume level and mute settings */ + + priv->volume = volume; + priv->mute = mute; +} +#endif /* CONFIG_AUDIO_EXCLUDE_VOLUME */ + +/************************************************************************************ + * Name: cs43l22_setbass + * + * Description: + * Set the bass level. + * + * The level and range are in whole percentage levels (0-100). + * + ************************************************************************************/ + +#ifndef CONFIG_AUDIO_EXCLUDE_TONE +static void cs43l22_setbass(FAR struct cs43l22_dev_s *priv, uint8_t bass) +{ + audinfo("bass=%u\n", bass); +#warning Missing logic +} +#endif /* CONFIG_AUDIO_EXCLUDE_TONE */ + +/************************************************************************************ + * Name: cs43l22_settreble + * + * Description: + * Set the treble level . + * + * The level and range are in whole percentage levels (0-100). + * + ************************************************************************************/ + +#ifndef CONFIG_AUDIO_EXCLUDE_TONE +static void cs43l22_settreble(FAR struct cs43l22_dev_s *priv, uint8_t treble) +{ + audinfo("treble=%u\n", treble); +#warning Missing logic +} +#endif /* CONFIG_AUDIO_EXCLUDE_TONE */ + +/**************************************************************************** + * Name: cs43l22_setdatawidth + * + * Description: + * Set the 8- or 16-bit data modes + * + ****************************************************************************/ + +static void cs43l22_setdatawidth(FAR struct cs43l22_dev_s *priv) +{ + if (priv->bpsamp == 16) + { + /* Reset default default setting */ + priv->i2s->ops->i2s_txdatawidth(priv->i2s, 16); + } + else + { + /* This should select 8-bit with no companding */ + priv->i2s->ops->i2s_txdatawidth(priv->i2s, 8); + } +} + +/**************************************************************************** + * Name: cs43l22_setbitrate + * + ****************************************************************************/ + +static void cs43l22_setbitrate(FAR struct cs43l22_dev_s *priv) +{ + DEBUGASSERT(priv && priv->lower); + + priv->i2s->ops->i2s_txsamplerate(priv->i2s, priv->samprate); + + audinfo("sample rate=%u nchannels=%u bpsamp=%u\n", + priv->samprate, priv->nchannels, priv->bpsamp); +} + +/**************************************************************************** + * Name: cs43l22_getcaps + * + * Description: + * Get the audio device capabilities + * + ****************************************************************************/ + +static int cs43l22_getcaps(FAR struct audio_lowerhalf_s *dev, int type, + FAR struct audio_caps_s *caps) +{ + /* Validate the structure */ + + DEBUGASSERT(caps && caps->ac_len >= sizeof(struct audio_caps_s)); + audinfo("type=%d ac_type=%d\n", type, caps->ac_type); + + /* Fill in the caller's structure based on requested info */ + + caps->ac_format.hw = 0; + caps->ac_controls.w = 0; + + switch (caps->ac_type) + { + /* Caller is querying for the types of units we support */ + + case AUDIO_TYPE_QUERY: + + /* Provide our overall capabilities. The interfacing software + * must then call us back for specific info for each capability. + */ + + caps->ac_channels = 2; /* Stereo output */ + + switch (caps->ac_subtype) + { + case AUDIO_TYPE_QUERY: + /* We don't decode any formats! Only something above us in + * the audio stream can perform decoding on our behalf. + */ + + /* The types of audio units we implement */ + + caps->ac_controls.b[0] = AUDIO_TYPE_OUTPUT | AUDIO_TYPE_FEATURE | + AUDIO_TYPE_PROCESSING; + break; + + case AUDIO_FMT_MIDI: + /* We only support Format 0 */ + + caps->ac_controls.b[0] = AUDIO_SUBFMT_END; + break; + + default: + caps->ac_controls.b[0] = AUDIO_SUBFMT_END; + break; + } + + break; + + /* Provide capabilities of our OUTPUT unit */ + + case AUDIO_TYPE_OUTPUT: + + caps->ac_channels = 2; + + switch (caps->ac_subtype) + { + case AUDIO_TYPE_QUERY: + + /* Report the Sample rates we support */ + + caps->ac_controls.b[0] = AUDIO_SAMP_RATE_8K | AUDIO_SAMP_RATE_11K | + AUDIO_SAMP_RATE_16K | AUDIO_SAMP_RATE_22K | + AUDIO_SAMP_RATE_32K | AUDIO_SAMP_RATE_44K | + AUDIO_SAMP_RATE_48K; + break; + + case AUDIO_FMT_MP3: + case AUDIO_FMT_WMA: + case AUDIO_FMT_PCM: + break; + + default: + break; + } + + break; + + /* Provide capabilities of our FEATURE units */ + + case AUDIO_TYPE_FEATURE: + + /* If the sub-type is UNDEF, then report the Feature Units we support */ + + if (caps->ac_subtype == AUDIO_FU_UNDEF) + { + /* Fill in the ac_controls section with the Feature Units we have */ + + caps->ac_controls.b[0] = AUDIO_FU_VOLUME | AUDIO_FU_BASS | AUDIO_FU_TREBLE; + caps->ac_controls.b[1] = AUDIO_FU_BALANCE >> 8; + } + else + { + /* TODO: Do we need to provide specific info for the Feature Units, + * such as volume setting ranges, etc.? + */ + } + + break; + + /* Provide capabilities of our PROCESSING unit */ + + case AUDIO_TYPE_PROCESSING: + + switch (caps->ac_subtype) + { + case AUDIO_PU_UNDEF: + /* Provide the type of Processing Units we support */ + + caps->ac_controls.b[0] = AUDIO_PU_STEREO_EXTENDER; + break; + + case AUDIO_PU_STEREO_EXTENDER: + /* Provide capabilities of our Stereo Extender */ + + caps->ac_controls.b[0] = AUDIO_STEXT_ENABLE | AUDIO_STEXT_WIDTH; + break; + + default: + /* Other types of processing uint we don't support */ + + break; + } + break; + + /* All others we don't support */ + + default: + + /* Zero out the fields to indicate no support */ + + caps->ac_subtype = 0; + caps->ac_channels = 0; + + break; + } + + /* Return the length of the audio_caps_s struct for validation of + * proper Audio device type. + */ + + return caps->ac_len; +} + +/**************************************************************************** + * Name: cs43l22_configure + * + * Description: + * Configure the audio device for the specified mode of operation. + * + ****************************************************************************/ + +#ifdef CONFIG_AUDIO_MULTI_SESSION +static int +cs43l22_configure(FAR struct audio_lowerhalf_s *dev, + FAR void *session, FAR const struct audio_caps_s *caps) +#else +static int +cs43l22_configure(FAR struct audio_lowerhalf_s *dev, + FAR const struct audio_caps_s *caps) +#endif +{ +#if !defined(CONFIG_AUDIO_EXCLUDE_VOLUME) || !defined(CONFIG_AUDIO_EXCLUDE_TONE) + FAR struct cs43l22_dev_s *priv = (FAR struct cs43l22_dev_s *)dev; +#endif + int ret = OK; + + DEBUGASSERT(priv && caps); + audinfo("ac_type: %d\n", caps->ac_type); + + /* Process the configure operation */ + + switch (caps->ac_type) + { + case AUDIO_TYPE_FEATURE: + audinfo(" AUDIO_TYPE_FEATURE\n"); + + /* Process based on Feature Unit */ + + switch (caps->ac_format.hw) + { +#ifndef CONFIG_AUDIO_EXCLUDE_VOLUME + case AUDIO_FU_VOLUME: + { + /* Set the volume */ + + uint16_t volume = caps->ac_controls.hw[0]; + audinfo(" Volume: %d\n", volume); + + if (volume >= 0 && volume <= 1000) + { + /* Scale the volume setting to the range {76..255} */ + + cs43l22_setvolume(priv, (179 * volume / 1000) + 76, priv->mute); + } + else + { + ret = -EDOM; + } + } + break; +#endif /* CONFIG_AUDIO_EXCLUDE_VOLUME */ + +#ifndef CONFIG_AUDIO_EXCLUDE_BALANCE + case AUDIO_FU_BALANCE: + { + /* Set the Balance */ + + uint16_t balance = caps->ac_controls.hw[0]; + audinfo(" Balance: %d\n", balance); + if (balance >= 0 && balance <= 1000) + { + /* Scale the volume setting to the range {76..255} */ + + cs43l22_setvolume(priv, (179 * priv->volume / 1000) + 76, + priv->mute); + } + else + { + ret = -EDOM; + } + } + break; +#endif /* CONFIG_AUDIO_EXCLUDE_VOLUME */ + +#ifndef CONFIG_AUDIO_EXCLUDE_TONE + case AUDIO_FU_BASS: + { + /* Set the bass. The percentage level (0-100) is in the + * ac_controls.b[0] parameter. + */ + + uint8_t bass = caps->ac_controls.b[0]; + audinfo(" Bass: %d\n", bass); + + if (bass <= 100) + { + cs43l22_setbass(priv, bass); + } + else + { + ret = -EDOM; + } + } + break; + + case AUDIO_FU_TREBLE: + { + /* Set the treble. The percentage level (0-100) is in the + * ac_controls.b[0] parameter. + */ + + uint8_t treble = caps->ac_controls.b[0]; + audinfo(" Treble: %d\n", treble); + + if (treble <= 100) + { + cs43l22_settreble(priv, treble); + } + else + { + ret = -EDOM; + } + } + break; +#endif /* CONFIG_AUDIO_EXCLUDE_TONE */ + + default: + auderr(" ERROR: Unrecognized feature unit\n"); + ret = -ENOTTY; + break; + } + break; + + case AUDIO_TYPE_OUTPUT: + { + audinfo(" AUDIO_TYPE_OUTPUT:\n"); + audinfo(" Number of channels: %u\n", caps->ac_channels); + audinfo(" Sample rate: %u\n", caps->ac_controls.hw[0]); + audinfo(" Sample width: %u\n", caps->ac_controls.b[2]); + + /* Verify that all of the requested values are supported */ + + ret = -ERANGE; + if (caps->ac_channels != 1 && caps->ac_channels != 2) + { + auderr("ERROR: Unsupported number of channels: %d\n", + caps->ac_channels); + break; + } + + if (caps->ac_controls.b[2] != 8 && caps->ac_controls.b[2] != 16) + { + auderr("ERROR: Unsupported bits per sample: %d\n", + caps->ac_controls.b[2]); + break; + } + + /* Save the current stream configuration */ + + priv->samprate = caps->ac_controls.hw[0]; + priv->nchannels = caps->ac_channels; + priv->bpsamp = caps->ac_controls.b[2]; + + /* Reconfigure the FLL to support the resulting number or channels, + * bits per sample, and bitrate. + */ + + cs43l22_setdatawidth(priv); + cs43l22_setbitrate(priv); + cs43l22_clock_analysis(&priv->dev, "AUDIO_TYPE_OUTPUT"); + ret = OK; + } + break; + + case AUDIO_TYPE_PROCESSING: + break; + } + + return ret; +} + +/**************************************************************************** + * Name: cs43l22_shutdown + * + * Description: + * Shutdown the CS43L22 chip and put it in the lowest power state possible. + * + ****************************************************************************/ + +static int cs43l22_shutdown(FAR struct audio_lowerhalf_s *dev) +{ + FAR struct cs43l22_dev_s *priv = (FAR struct cs43l22_dev_s *)dev; + + DEBUGASSERT(priv); + + /* First disable interrupts */ + + CS43L22_DISABLE(priv->lower); + + /* Now issue a software reset. This puts all CS43L22 registers back in + * their default state. + */ + + cs43l22_reset(priv); + return OK; +} + +/**************************************************************************** + * Name: cs43l22_senddone + * + * Description: + * This is the I2S callback function that is invoked when the transfer + * completes. + * + ****************************************************************************/ + +static void +cs43l22_senddone(FAR struct i2s_dev_s *i2s, + FAR struct ap_buffer_s *apb, FAR void *arg, int result) +{ + FAR struct cs43l22_dev_s *priv = (FAR struct cs43l22_dev_s *)arg; + struct audio_msg_s msg; + irqstate_t flags; + int ret; + + DEBUGASSERT(i2s && priv && priv->running && apb); + audinfo("apb=%p inflight=%d result=%d\n", apb, priv->inflight, result); + + /* We do not place any restriction on the context in which this function + * is called. It may be called from an interrupt handler. Therefore, the + * doneq and in-flight values might be accessed from the interrupt level. + * Not the best design. But we will use interrupt controls to protect + * against that possibility. + */ + + flags = enter_critical_section(); + + /* Add the completed buffer to the end of our doneq. We do not yet + * decrement the reference count. + */ + + dq_addlast((FAR dq_entry_t *)apb, &priv->doneq); + + /* And decrement the number of buffers in-flight */ + + DEBUGASSERT(priv->inflight > 0); + priv->inflight--; + + /* Save the result of the transfer */ + /* REVISIT: This can be overwritten */ + + priv->result = result; + leave_critical_section(flags); + + /* Now send a message to the worker thread, informing it that there are + * buffers in the done queue that need to be cleaned up. + */ + + msg.msgId = AUDIO_MSG_COMPLETE; + ret = mq_send(priv->mq, (FAR const char *)&msg, sizeof(msg), + CONFIG_CS43L22_MSG_PRIO); + if (ret < 0) + { + auderr("ERROR: mq_send failed: %d\n", errno); + } +} + +/**************************************************************************** + * Name: cs43l22_returnbuffers + * + * Description: + * This function is called after the complete of one or more data + * transfers. This function will empty the done queue and release our + * reference to each buffer. + * + ****************************************************************************/ + +static void cs43l22_returnbuffers(FAR struct cs43l22_dev_s *priv) +{ + FAR struct ap_buffer_s *apb; + irqstate_t flags; + + /* The doneq and in-flight values might be accessed from the interrupt + * level in some implementations. Not the best design. But we will + * use interrupt controls to protect against that possibility. + */ + + flags = enter_critical_section(); + while (dq_peek(&priv->doneq) != NULL) + { + /* Take the next buffer from the queue of completed transfers */ + + apb = (FAR struct ap_buffer_s *)dq_remfirst(&priv->doneq); + leave_critical_section(flags); + + audinfo("Returning: apb=%p curbyte=%d nbytes=%d flags=%04x\n", + apb, apb->curbyte, apb->nbytes, apb->flags); + + /* Are we returning the final buffer in the stream? */ + + if ((apb->flags & AUDIO_APB_FINAL) != 0) + { + /* Both the pending and the done queues should be empty and there + * should be no buffers in-flight. + */ + + DEBUGASSERT(dq_empty(&priv->doneq) && dq_empty(&priv->pendq) && + priv->inflight == 0); + + /* Set the terminating flag. This will, eventually, cause the + * worker thread to exit (if it is not already terminating). + */ + + audinfo("Terminating\n"); + priv->terminating = true; + } + + /* Release our reference to the audio buffer */ + + apb_free(apb); + + /* Send the buffer back up to the previous level. */ + +#ifdef CONFIG_AUDIO_MULTI_SESSION + priv->dev.upper(priv->dev.priv, AUDIO_CALLBACK_DEQUEUE, apb, OK, NULL); +#else + priv->dev.upper(priv->dev.priv, AUDIO_CALLBACK_DEQUEUE, apb, OK); +#endif + flags = enter_critical_section(); + } + + leave_critical_section(flags); +} + +/**************************************************************************** + * Name: cs43l22_sendbuffer + * + * Description: + * Start the transfer an audio buffer to the CS43L22 via I2S. This + * will not wait for the transfer to complete but will return immediately. + * the wmd8904_senddone called will be invoked when the transfer + * completes, stimulating the worker thread to call this function again. + * + ****************************************************************************/ + +static int cs43l22_sendbuffer(FAR struct cs43l22_dev_s *priv) +{ + FAR struct ap_buffer_s *apb; + irqstate_t flags; + uint32_t timeout; + int shift; + int ret = OK; + + /* Loop while there are audio buffers to be sent and we have few than + * CONFIG_CS43L22_INFLIGHT then "in-flight" + * + * The 'inflight' value might be modified from the interrupt level in some + * implementations. We will use interrupt controls to protect against + * that possibility. + * + * The 'pendq', on the other hand, is protected via a semaphore. Let's + * hold the semaphore while we are busy here and disable the interrupts + * only while accessing 'inflight'. + */ + + cs43l22_takesem(&priv->pendsem); + while (priv->inflight < CONFIG_CS43L22_INFLIGHT && + dq_peek(&priv->pendq) != NULL && !priv->paused) + { + /* Take next buffer from the queue of pending transfers */ + + apb = (FAR struct ap_buffer_s *)dq_remfirst(&priv->pendq); + audinfo("Sending apb=%p, size=%d inflight=%d\n", + apb, apb->nbytes, priv->inflight); + + /* Increment the number of buffers in-flight before sending in order + * to avoid a possible race condition. + */ + + flags = enter_critical_section(); + priv->inflight++; + leave_critical_section(flags); + + /* Send the entire audio buffer via I2S. What is a reasonable timeout + * to use? This would depend on the bit rate and size of the buffer. + * + * Samples in the buffer (samples): + * = buffer_size * 8 / bpsamp samples + * Sample rate (samples/second): + * = samplerate * nchannels + * Expected transfer time (seconds): + * = (buffer_size * 8) / bpsamp / samplerate / nchannels + * + * We will set the timeout about twice that. + * + * NOTES: + * - The multiplier of 8 becomes 16000 for 2x and units of + * milliseconds. + * - 16000 is a approximately 16384 (1 << 14), bpsamp is either + * (1 << 3) or (1 << 4), and nchannels is either (1 << 0) or + * (1 << 1). So this can be simplifies to (milliseconds): + * + * = (buffer_size << shift) / samplerate + */ + + shift = (priv->bpsamp == 8) ? 14 - 3 : 14 - 4; + shift -= (priv->nchannels > 1) ? 1 : 0; + + timeout = MSEC2TICK(((uint32_t)(apb->nbytes - apb->curbyte) << shift) / + (uint32_t)priv->samprate); + + ret = I2S_SEND(priv->i2s, apb, cs43l22_senddone, priv, timeout); + if (ret < 0) + { + auderr("ERROR: I2S_SEND failed: %d\n", ret); + break; + } + } + + cs43l22_givesem(&priv->pendsem); + return ret; +} + +/**************************************************************************** + * Name: cs43l22_start + * + * Description: + * Start the configured operation (audio streaming, volume enabled, etc.). + * + ****************************************************************************/ + +#ifdef CONFIG_AUDIO_MULTI_SESSION +static int cs43l22_start(FAR struct audio_lowerhalf_s *dev, FAR void *session) +#else +static int cs43l22_start(FAR struct audio_lowerhalf_s *dev) +#endif +{ + FAR struct cs43l22_dev_s *priv = (FAR struct cs43l22_dev_s *)dev; + struct sched_param sparam; + struct mq_attr attr; + pthread_attr_t tattr; + FAR void *value; + int ret; + + audinfo("Entry\n"); + + /* Exit reduced power modes of operation */ + /* REVISIT */ + + /* Create a message queue for the worker thread */ + + snprintf(priv->mqname, sizeof(priv->mqname), "/tmp/%X", priv); + + attr.mq_maxmsg = 16; + attr.mq_msgsize = sizeof(struct audio_msg_s); + attr.mq_curmsgs = 0; + attr.mq_flags = 0; + + priv->mq = mq_open(priv->mqname, O_RDWR | O_CREAT, 0644, &attr); + if (priv->mq == NULL) + { + /* Error creating message queue! */ + + auderr("ERROR: Couldn't allocate message queue\n"); + return -ENOMEM; + } + + /* Join any old worker thread we had created to prevent a memory leak */ + + if (priv->threadid != 0) + { + audinfo("Joining old thread\n"); + pthread_join(priv->threadid, &value); + } + + /* Start our thread for sending data to the device */ + + pthread_attr_init(&tattr); + sparam.sched_priority = sched_get_priority_max(SCHED_FIFO) - 3; + (void)pthread_attr_setschedparam(&tattr, &sparam); + (void)pthread_attr_setstacksize(&tattr, CONFIG_CS43L22_WORKER_STACKSIZE); + + audinfo("Starting worker thread\n"); + ret = pthread_create(&priv->threadid, &tattr, cs43l22_workerthread, + (pthread_addr_t)priv); + if (ret != OK) + { + auderr("ERROR: pthread_create failed: %d\n", ret); + } + else + { + pthread_setname_np(priv->threadid, "cs43l22"); + audinfo("Created worker thread\n"); + } + + return ret; +} + +/**************************************************************************** + * Name: cs43l22_stop + * + * Description: Stop the configured operation (audio streaming, volume + * disabled, etc.). + * + ****************************************************************************/ + +#ifndef CONFIG_AUDIO_EXCLUDE_STOP +# ifdef CONFIG_AUDIO_MULTI_SESSION +static int cs43l22_stop(FAR struct audio_lowerhalf_s *dev, FAR void *session) +# else +static int cs43l22_stop(FAR struct audio_lowerhalf_s *dev) +# endif +{ + FAR struct cs43l22_dev_s *priv = (FAR struct cs43l22_dev_s *)dev; + struct audio_msg_s term_msg; + FAR void *value; + + /* Send a message to stop all audio streaming */ + + term_msg.msgId = AUDIO_MSG_STOP; + term_msg.u.data = 0; + mq_send(priv->mq, (FAR const char *)&term_msg, sizeof(term_msg), + CONFIG_CS43L22_MSG_PRIO); + + /* Join the worker thread */ + + pthread_join(priv->threadid, &value); + priv->threadid = 0; + + /* Enter into a reduced power usage mode */ + /* REVISIT: */ + + return OK; +} +#endif + +/**************************************************************************** + * Name: cs43l22_pause + * + * Description: Pauses the playback. + * + ****************************************************************************/ + +#ifndef CONFIG_AUDIO_EXCLUDE_PAUSE_RESUME +# ifdef CONFIG_AUDIO_MULTI_SESSION +static int cs43l22_pause(FAR struct audio_lowerhalf_s *dev, FAR void *session) +# else +static int cs43l22_pause(FAR struct audio_lowerhalf_s *dev) +# endif +{ + FAR struct cs43l22_dev_s *priv = (FAR struct cs43l22_dev_s *)dev; + + if (priv->running && !priv->paused) + { + /* Disable interrupts to prevent us from suppling any more data */ + + priv->paused = true; + cs43l22_setvolume(priv, priv->volume, true); + CS43L22_DISABLE(priv->lower); + } + + return OK; +} +#endif /* CONFIG_AUDIO_EXCLUDE_PAUSE_RESUME */ + +/**************************************************************************** + * Name: cs43l22_resume + * + * Description: Resumes the playback. + * + ****************************************************************************/ + +#ifndef CONFIG_AUDIO_EXCLUDE_PAUSE_RESUME +# ifdef CONFIG_AUDIO_MULTI_SESSION +static int cs43l22_resume(FAR struct audio_lowerhalf_s *dev, FAR void *session) +# else +static int cs43l22_resume(FAR struct audio_lowerhalf_s *dev) +# endif +{ + FAR struct cs43l22_dev_s *priv = (FAR struct cs43l22_dev_s *)dev; + + if (priv->running && priv->paused) + { + priv->paused = false; + cs43l22_setvolume(priv, priv->volume, false); + + /* Enable interrupts to allow sampling data */ + + cs43l22_sendbuffer(priv); +#ifdef CS43L22_USE_FFLOCK_INT + CS43L22_ENABLE(priv->lower); +#endif + } + + return OK; +} +#endif /* CONFIG_AUDIO_EXCLUDE_PAUSE_RESUME */ + +/**************************************************************************** + * Name: cs43l22_enqueuebuffer + * + * Description: Enqueue an Audio Pipeline Buffer for playback/ processing. + * + ****************************************************************************/ + +static int cs43l22_enqueuebuffer(FAR struct audio_lowerhalf_s *dev, + FAR struct ap_buffer_s *apb) +{ + FAR struct cs43l22_dev_s *priv = (FAR struct cs43l22_dev_s *)dev; + struct audio_msg_s term_msg; + int ret; + + audinfo("Enqueueing: apb=%p curbyte=%d nbytes=%d flags=%04x\n", + apb, apb->curbyte, apb->nbytes, apb->flags); + + /* Take a reference on the new audio buffer */ + + apb_reference(apb); + + /* Add the new buffer to the tail of pending audio buffers */ + + cs43l22_takesem(&priv->pendsem); + apb->flags |= AUDIO_APB_OUTPUT_ENQUEUED; + dq_addlast(&apb->dq_entry, &priv->pendq); + cs43l22_givesem(&priv->pendsem); + + /* Send a message to the worker thread indicating that a new buffer has been + * enqueued. If mq is NULL, then the playing has not yet started. In that + * case we are just "priming the pump" and we don't need to send any message. + */ + + ret = OK; + if (priv->mq != NULL) + { + term_msg.msgId = AUDIO_MSG_ENQUEUE; + term_msg.u.data = 0; + + ret = mq_send(priv->mq, (FAR const char *)&term_msg, sizeof(term_msg), + CONFIG_CS43L22_MSG_PRIO); + if (ret < 0) + { + int errcode = errno; + DEBUGASSERT(errcode > 0); + + auderr("ERROR: mq_send failed: %d\n", errcode); + UNUSED(errcode); + } + } + + return ret; +} + +/**************************************************************************** + * Name: cs43l22_cancelbuffer + * + * Description: Called when an enqueued buffer is being cancelled. + * + ****************************************************************************/ + +static int cs43l22_cancelbuffer(FAR struct audio_lowerhalf_s *dev, + FAR struct ap_buffer_s *apb) +{ + audinfo("apb=%p\n", apb); + return OK; +} + +/**************************************************************************** + * Name: cs43l22_ioctl + * + * Description: Perform a device ioctl + * + ****************************************************************************/ + +static int cs43l22_ioctl(FAR struct audio_lowerhalf_s *dev, int cmd, + unsigned long arg) +{ +#ifdef CONFIG_AUDIO_DRIVER_SPECIFIC_BUFFERS + FAR struct ap_buffer_info_s *bufinfo; +#endif + + /* Deal with ioctls passed from the upper-half driver */ + + switch (cmd) + { + /* Check for AUDIOIOC_HWRESET ioctl. This ioctl is passed straight + * through from the upper-half audio driver. + */ + + case AUDIOIOC_HWRESET: + { + /* REVISIT: Should we completely re-initialize the chip? We + * can't just issue a software reset; that would puts all WM8904 + * registers back in their default state. + */ + + audinfo("AUDIOIOC_HWRESET:\n"); + } + break; + + /* Report our preferred buffer size and quantity */ + +#ifdef CONFIG_AUDIO_DRIVER_SPECIFIC_BUFFERS + case AUDIOIOC_GETBUFFERINFO: + { + audinfo("AUDIOIOC_GETBUFFERINFO:\n"); + bufinfo = (FAR struct ap_buffer_info_s *)arg; + bufinfo->buffer_size = CONFIG_CS43L22_BUFFER_SIZE; + bufinfo->nbuffers = CONFIG_CS43L22_NUM_BUFFERS; + } + break; +#endif + + default: + audinfo("Ignored\n"); + break; + } + + return OK; +} + +/**************************************************************************** + * Name: cs43l22_reserve + * + * Description: Reserves a session (the only one we have). + * + ****************************************************************************/ + +#ifdef CONFIG_AUDIO_MULTI_SESSION +static int +cs43l22_reserve(FAR struct audio_lowerhalf_s *dev, FAR void **session) +#else +static int cs43l22_reserve(FAR struct audio_lowerhalf_s *dev) +#endif +{ + FAR struct cs43l22_dev_s *priv = (FAR struct cs43l22_dev_s *)dev; + int ret = OK; + + /* Borrow the APBQ semaphore for thread sync */ + + cs43l22_takesem(&priv->pendsem); + if (priv->reserved) + { + ret = -EBUSY; + } + else + { + /* Initialize the session context */ + +#ifdef CONFIG_AUDIO_MULTI_SESSION + *session = NULL; +#endif + priv->inflight = 0; + priv->running = false; + priv->paused = false; +#ifndef CONFIG_AUDIO_EXCLUDE_STOP + priv->terminating = false; +#endif + priv->reserved = true; + } + + cs43l22_givesem(&priv->pendsem); + + return ret; +} + +/**************************************************************************** + * Name: cs43l22_release + * + * Description: Releases the session (the only one we have). + * + ****************************************************************************/ + +#ifdef CONFIG_AUDIO_MULTI_SESSION +static int cs43l22_release(FAR struct audio_lowerhalf_s *dev, FAR void *session) +#else +static int cs43l22_release(FAR struct audio_lowerhalf_s *dev) +#endif +{ + FAR struct cs43l22_dev_s *priv = (FAR struct cs43l22_dev_s *)dev; + void *value; + + /* Join any old worker thread we had created to prevent a memory leak */ + + if (priv->threadid != 0) + { + pthread_join(priv->threadid, &value); + priv->threadid = 0; + } + + /* Borrow the APBQ semaphore for thread sync */ + + cs43l22_takesem(&priv->pendsem); + + /* Really we should free any queued buffers here */ + + priv->reserved = false; + cs43l22_givesem(&priv->pendsem); + + return OK; +} + +/**************************************************************************** + * Name: cs43l22_interrupt_work + * + * Description: + * CS43L22 interrupt actions cannot be performed in the interrupt handler + * because I2C access is not possible in that context. Instead, all I2C + * operations are deferred to the work queue. + * + * Assumptions: + * CS43L22 interrupts were disabled in the interrupt handler. + * + ****************************************************************************/ + +#ifdef CS43L22_USE_FFLOCK_INT +static void cs43l22_interrupt_work(FAR void *arg) +{ + /* TODO */ +#warning Missing logic +} +#endif + +/**************************************************************************** + * Name: cs43l22_interrupt + * + * Description: + * This is the ISR that services the GPIO1/IRQ pin from the CS43L22. It + * signals CS43L22 events such FLL lock. + * + ****************************************************************************/ + +#ifdef CS43L22_USE_FFLOCK_INT +static int +cs43l22_interrupt(FAR const struct cs43l22_lower_s *lower, FAR void *arg) +{ + /* TODO */ +#warning Missing logic +} +#endif + +/**************************************************************************** + * Name: cs43l22_workerthread + * + * This is the thread that feeds data to the chip and keeps the audio + * stream going. + * + ****************************************************************************/ + +static void *cs43l22_workerthread(pthread_addr_t pvarg) +{ + FAR struct cs43l22_dev_s *priv = (struct cs43l22_dev_s *)pvarg; + struct audio_msg_s msg; + FAR struct ap_buffer_s *apb; + int msglen; + int prio; + + audinfo("Entry\n"); + +#ifndef CONFIG_AUDIO_EXCLUDE_STOP + priv->terminating = false; +#endif + +/* Mark ourself as running and make sure that CS43L22 interrupts are + * enabled. + */ + + priv->running = true; +#ifdef CS43L22_USE_FFLOCK_INT + CS43L22_ENABLE(priv->lower); +#endif + cs43l22_setvolume(priv, priv->volume, false); + + /* Loop as long as we are supposed to be running and as long as we have + * buffers in-flight. + */ + + while (priv->running || priv->inflight > 0) + { + /* Check if we have been asked to terminate. We have to check if we + * still have buffers in-flight. If we do, then we can't stop until + * birds come back to roost. + */ + + if (priv->terminating && priv->inflight <= 0) + { + /* We are IDLE. Break out of the loop and exit. */ + + break; + } + else + { + /* Check if we can send more audio buffers to the CS43L22 */ + + cs43l22_sendbuffer(priv); + } + + /* Wait for messages from our message queue */ + + msglen = mq_receive(priv->mq, (FAR char *)&msg, sizeof(msg), &prio); + + /* Handle the case when we return with no message */ + + if (msglen < sizeof(struct audio_msg_s)) + { + auderr("ERROR: Message too small: %d\n", msglen); + continue; + } + + /* Process the message */ + + switch (msg.msgId) + { + /* The ISR has requested more data. We will catch this case at + * the top of the loop. + */ + + case AUDIO_MSG_DATA_REQUEST: + audinfo("AUDIO_MSG_DATA_REQUEST\n"); + break; + + /* Stop the playback */ + +#ifndef CONFIG_AUDIO_EXCLUDE_STOP + case AUDIO_MSG_STOP: + /* Indicate that we are terminating */ + + audinfo("AUDIO_MSG_STOP: Terminating\n"); + priv->terminating = true; + break; +#endif + + /* We have a new buffer to send. We will catch this case at + * the top of the loop. + */ + + case AUDIO_MSG_ENQUEUE: + audinfo("AUDIO_MSG_ENQUEUE\n"); + break; + + /* We will wake up from the I2S callback with this message */ + + case AUDIO_MSG_COMPLETE: + audinfo("AUDIO_MSG_COMPLETE\n"); + cs43l22_returnbuffers(priv); + break; + + default: + auderr("ERROR: Ignoring message ID %d\n", msg.msgId); + break; + } + } + + /* Reset the CS43L22 hardware */ + + cs43l22_reset(priv); + + /* Return any pending buffers in our pending queue */ + + cs43l22_takesem(&priv->pendsem); + while ((apb = (FAR struct ap_buffer_s *)dq_remfirst(&priv->pendq)) != NULL) + { + /* Release our reference to the buffer */ + + apb_free(apb); + + /* Send the buffer back up to the previous level. */ + +#ifdef CONFIG_AUDIO_MULTI_SESSION + priv->dev.upper(priv->dev.priv, AUDIO_CALLBACK_DEQUEUE, apb, OK, NULL); +#else + priv->dev.upper(priv->dev.priv, AUDIO_CALLBACK_DEQUEUE, apb, OK); +#endif + } + + cs43l22_givesem(&priv->pendsem); + + /* Return any pending buffers in our done queue */ + + cs43l22_returnbuffers(priv); + + /* Close the message queue */ + + mq_close(priv->mq); + mq_unlink(priv->mqname); + priv->mq = NULL; + + /* Send an AUDIO_MSG_COMPLETE message to the client */ + +#ifdef CONFIG_AUDIO_MULTI_SESSION + priv->dev.upper(priv->dev.priv, AUDIO_CALLBACK_COMPLETE, NULL, OK, NULL); +#else + priv->dev.upper(priv->dev.priv, AUDIO_CALLBACK_COMPLETE, NULL, OK); +#endif + + audinfo("Exit\n"); + return NULL; +} + +/**************************************************************************** + * Name: cs43l22_audio_output + * + * Description: + * Initialize and configure the CS43L22 device as an audio output device. + * + * Input Parameters: + * priv - A reference to the driver state structure + * + * Returned Value: + * None. No failures are detected. + * + ****************************************************************************/ + +static void cs43l22_audio_output(FAR struct cs43l22_dev_s *priv) +{ + uint8_t regval; + + /* Keep codec powered off */ + + regval = CS43L22_POWER_DOWN; + cs43l22_writereg(priv, CS43L22_POWER_CTRL1, regval); + + /* SPK always off and HP always on */ + + regval = CS43L22_PDN_HPB_ON | CS43L22_PDN_HPA_ON | CS43L22_PDN_SPKB_OFF | CS43L22_PDN_SPKA_OFF; + cs43l22_writereg(priv, CS43L22_POWER_CTRL2, regval); + + /* Clock configuration: Auto detection */ + + regval = CS43L22_AUTO_DETECT_ENABLE | CS43L22_CLKDIV2_ENABLE; + cs43l22_writereg(priv, CS43L22_CLOCK_CTRL, regval); + + /* Set slave mode and Philips audio standard */ + + regval = CS43L22_DAC_IF_I2S; + cs43l22_writereg(priv, CS43L22_INTERFACE_CTRL1, regval); + + /* Power on the codec */ + + regval = CS43L22_POWER_UP; + cs43l22_writereg(priv, CS43L22_POWER_CTRL1, regval); + + /* Disable the analog soft ramp */ + + regval = 0x00; + cs43l22_writereg(priv, CS43L22_ANLG_ZC_SR_SEL, regval); + + /* Disable the digital soft ramp */ + + regval = CS43L22_DEEMPHASIS_ENABLE; + cs43l22_writereg(priv, CS43L22_MISCLLNS_CTRL, regval); + + /* Disable the limiter attack level */ + + regval = 0x00; + cs43l22_writereg(priv, CS43L22_LIM_CTRL1, regval); + + /* Adjust bass and treble levels */ + + regval = CS43L22_BASS_GAIN(0x0f); + cs43l22_writereg(priv, CS43L22_TONE_CTRL, regval); + + /* Adjust PCM volume level */ + + regval = 0x0a; + cs43l22_writereg(priv, CS43L22_PCM_VOL_A, regval); + cs43l22_writereg(priv, CS43L22_PCM_VOL_B, regval); + + cs43l22_setdatawidth(priv); + + cs43l22_setvolume(priv, CONFIG_CS43L22_INITVOLUME, false); +} + +/**************************************************************************** + * Name: cs43l22_audio_input + * + * Description: + * Initialize and configure the CS43L22 device as an audio output device + * (Right input only). cs43l22_audio_output() must be called first, this + * function then modifies the configuration to support audio input. + * + * Input Parameters: + * priv - A reference to the driver state structure + * + * Returned Value: + * None. No failures are detected. + * + ****************************************************************************/ + +#if 0 /* Not used */ +static void cs43l22_audio_input(FAR struct cs43l22_dev_s *priv) +{ + /* TODO */ +#warning Missing logic +} +#endif + +/**************************************************************************** + * Name: cs43l22_configure_ints + * + * Description: + * Configure the GPIO/IRQ interrupt + * + * Input Parameters: + * priv - A reference to the driver state structure + * + * Returned Value: + * None + * + ****************************************************************************/ + +#ifdef CS43L22_USE_FFLOCK_INT +static void cs43l22_configure_ints(FAR struct cs43l22_dev_s *priv) +{ + /* TODO */ +#warning Missing logic +} +#endif + +/**************************************************************************** + * Name: cs43l22_reset + * + * Description: + * Reset and re-initialize the CS43L22 + * + * Input Parameters: + * priv - A reference to the driver state structure + * + * Returned Value: + * None + * + ****************************************************************************/ + +static void cs43l22_reset(FAR struct cs43l22_dev_s *priv) +{ + /* Put audio output back to its initial configuration */ + + /* Put audio output back to its initial configuration */ + + priv->samprate = CS43L22_DEFAULT_SAMPRATE; + priv->nchannels = CS43L22_DEFAULT_NCHANNELS; + priv->bpsamp = CS43L22_DEFAULT_BPSAMP; +#if !defined(CONFIG_AUDIO_EXCLUDE_VOLUME) && !defined(CONFIG_AUDIO_EXCLUDE_BALANCE) + priv->balance = 500; // b16HALF; /* Center balance */ +#endif + + /* Software reset. This puts all CS43L22 registers back in their + * default state. + */ + + /* cs43l22_writereg(priv, CS43L22_SWRST, 0); */ + + /* Configure the CS43L22 hardware as an audio input device */ + + cs43l22_audio_output(priv); + + /* Configure interrupts */ + + /* cs43l22_configure_ints(priv); */ + + /* Configure the FLL and the LRCLK */ + + cs43l22_setbitrate(priv); + + /* Dump some information and return the device instance */ + + cs43l22_dump_registers(&priv->dev, "After configuration"); + cs43l22_clock_analysis(&priv->dev, "After configuration"); +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cs43l22_initialize + * + * Description: + * Initialize the CS43L22 device. + * + * Input Parameters: + * i2c - An I2C driver instance + * i2s - An I2S driver instance + * lower - Persistent board configuration data + * + * Returned Value: + * A new lower half audio interface for the CS43L22 device is returned on + * success; NULL is returned on failure. + * + ****************************************************************************/ + +FAR struct audio_lowerhalf_s *cs43l22_initialize(FAR struct i2c_master_s *i2c, + FAR struct i2s_dev_s *i2s, + FAR const struct + cs43l22_lower_s *lower) +{ + FAR struct cs43l22_dev_s *priv; + uint16_t regval; + + /* Sanity check */ + + DEBUGASSERT(i2c && i2s && lower); + + /* Allocate a CS43L22 device structure */ + + priv = (FAR struct cs43l22_dev_s *)kmm_zalloc(sizeof(struct cs43l22_dev_s)); + if (priv) + { + /* Initialize the CS43L22 device structure. Since we used kmm_zalloc, + * only the non-zero elements of the structure need to be initialized. + */ + + priv->dev.ops = &g_audioops; + priv->lower = lower; + priv->i2c = i2c; + priv->i2s = i2s; + + sem_init(&priv->pendsem, 0, 1); + dq_init(&priv->pendq); + dq_init(&priv->doneq); + + /* Initialize I2C */ + + audinfo("address=%02x frequency=%d\n", lower->address, lower->frequency); + + /* Software reset. This puts all CS43L22 registers back in their default + * state. */ + + CS43L22_HW_RESET(priv->lower); + + cs43l22_dump_registers(&priv->dev, "After reset"); + + /* Verify that CS43L22 is present and available on this I2C */ + + regval = cs43l22_readreg(priv, CS43L22_ID_REV); + if ((regval & 0xff) != CS43L22_DEV_ID_REV) + { + auderr("ERROR: CS43L22 not found: ID=%02x\n", regval); + goto errout_with_dev; + } + + /* Reset and reconfigure the CS43L22 hardware */ + + cs43l22_reset(priv); + return &priv->dev; + } + + return NULL; + +errout_with_dev: + sem_destroy(&priv->pendsem); + kmm_free(priv); + return NULL; +} diff --git a/drivers/audio/cs43l22.h b/drivers/audio/cs43l22.h new file mode 100644 index 0000000000..f2ec078e10 --- /dev/null +++ b/drivers/audio/cs43l22.h @@ -0,0 +1,388 @@ +/**************************************************************************** + * drivers/audio/cs43l22.h + * + * Copyright (C) 2017 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Reference: + * "CS43L22 Ultra Low Power CODEC for Portable Audio Applications, Pre- + * Production", September 2012, Rev 3.3, Wolfson Microelectronics + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef __DRIVERS_AUDIO_CS43L22_H +#define __DRIVERS_AUDIO_CS43L22_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include + +#include +#include + +#define getreg32(a) (*(volatile uint32_t *)(a)) +#define putreg32(v,a) (*(volatile uint32_t *)(a) = (v)) +#define getreg16(a) (*(volatile uint16_t *)(a)) +#define putreg16(v,a) (*(volatile uint16_t *)(a) = (v)) + +#ifdef CONFIG_AUDIO + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ +/* So far, I have not been able to get FLL lock interrupts. Worse, I have + * been able to get the FLL to claim that it is locked at all even when + * polling. What am I doing wrong? + * + * Hmmm.. seems unnecessary anyway + */ + +#undef CS43L22_USE_FFLOCK_INT +#undef CS43L22_USE_FFLOCK_POLL + +/* Registers Addresses ******************************************************/ + +#define CS43L22_ID_REV 0x01 /* Chip I.D. and Revision */ +#define CS43L22_POWER_CTRL1 0x02 /* Power Control 1 */ +#define CS43L22_POWER_CTRL2 0x04 /* Power Control 2 */ +#define CS43L22_CLOCK_CTRL 0x05 /* Clocking Control */ +#define CS43L22_INTERFACE_CTRL1 0x06 /* Interface Control 1 */ +#define CS43L22_INTERFACE_CTRL2 0x07 /* Interface Control 2 */ +#define CS43L22_PASS_SEL_A 0x08 /* Passthrough x Select: PassA */ +#define CS43L22_PASS_SEL_B 0x09 /* Passthrough x Select: PassB */ +#define CS43L22_ANLG_ZC_SR_SEL 0x0A /* Analog ZC and SR Settings */ +#define CS43L22_PASS_GANG_CTRL 0x0C /* Passthrough Gang Control */ +#define CS43L22_PLAYBACK_CTRL1 0x0D /* Playback Control 1 */ +#define CS43L22_MISCLLNS_CTRL 0x0E /* Miscellaneous Controls */ +#define CS43L22_PLAYBACK_CTRL2 0x0F /* Playback Control 2 */ +#define CS43L22_PASS_VOL_A 0x14 /* Passthrough x Volume: PASSAVOL */ +#define CS43L22_PASS_VOL_B 0x15 /* Passthrough x Volume: PASSBVOL */ +#define CS43L22_PCM_VOL_A 0x1A /* PCMx Volume: PCMA */ +#define CS43L22_PCM_VOL_B 0x1B /* PCMx Volume: PCMB */ +#define CS43L22_BP_FREQ_ON_TIME 0x1C /* Beep Frequency & On Time */ +#define CS43L22_BP_VOL_OFF_TIME 0x1D /* Beep Volume & Off Time */ +#define CS43L22_BP_TONE_CFG 0x1E /* Beep & Tone Configuration */ +#define CS43L22_TONE_CTRL 0x1F /* Tone Control */ +#define CS43L22_MS_VOL_CTRL_A 0x20 /* Master Volume Control: MSTA */ +#define CS43L22_MS_VOL_CTRL_B 0x21 /* Master Volume Control: MSTB */ +#define CS43L22_HP_VOL_CTRL_A 0x22 /* Headphone Volume Control: HPA */ +#define CS43L22_HP_VOL_CTRL_B 0x23 /* Headphone Volume Control: HPB */ +#define CS43L22_SPK_VOL_CTRL_A 0x24 /* Speaker Volume Control: SPKA */ +#define CS43L22_SPK_VOL_CTRL_B 0x25 /* Speaker Volume Control: SPKB */ +#define CS43L22_PCM_CH_SWAP 0x26 /* PCM Channel Swap */ +#define CS43L22_LIM_CTRL1 0x27 /* Limiter Control 1, Min/Max Thresholds */ +#define CS43L22_LIM_CTRL2 0x28 /* Limiter Control 2, Release Rate */ +#define CS43L22_LIM_ATTACK_RATE 0x29 /* Limiter Attack Rate */ +#define CS43L22_STATUS 0x2E /* Status */ +#define CS43L22_BAT_COMP 0x2F /* Battery Compensation */ +#define CS43L22_VP_BAT_LEVEL 0x30 /* VP Battery Level */ +#define CS43L22_SPK_STATUS 0x31 /* Speaker Status */ +#define CS43L22_TEMP_MON_CTRL 0x32 /* Temperature Monitor Control */ +#define CS43L22_THERMAL_FOLDBACK 0x33 /* Thermal Foldback */ +#define CS43L22_CHRG_PUMP_FREQ 0x34 /* Charge Pump Frequency */ + +#define CS43L22_HPBMUTE (1 << 7) +#define CS43L22_HPAMUTE (1 << 6) +#define CS43L22_SPKBMUTE (1 << 5) +#define CS43L22_SPKAMUTE (1 << 4) + +/* Register Default Values **************************************************/ +/* Registers have some undocumented bits set on power up. These probably + * should be retained on writes (?). + */ + +#define CS43L22_ID_REV_DEFAULT 0xe3 /* Chip I.D. and Revision */ +#define CS43L22_POWER_CTRL1_DEFAULT 0x01 /* Power Control 1 */ +#define CS43L22_POWER_CTRL2_DEFAULT 0x05 /* Power Control 2 */ +#define CS43L22_CLOCK_CTRL_DEFAULT 0xa0 /* Clocking Control */ +#define CS43L22_INTERFACE_CTRL1_DEFAULT 0x00 /* Interface Control 1 */ +#define CS43L22_INTERFACE_CTRL2_DEFAULT 0x00 /* Interface Control 2 */ +#define CS43L22_PASS_SEL_A_DEFAULT 0x81 /* Passthrough x Select: PassA */ +#define CS43L22_PASS_SEL_B_DEFAULT 0x81 /* Passthrough x Select: PassB */ +#define CS43L22_ANLG_ZC_SR_SEL_DEFAULT 0xa5 /* Analog ZC and SR Settings */ +#define CS43L22_PASS_GANG_CTRL_DEFAULT 0x00 /* Passthrough Gang Control */ +#define CS43L22_PLAYBACK_CTRL1_DEFAULT 0x60 /* Playback Control 1 */ +#define CS43L22_MISCLLNS_CTRL_DEFAULT 0x02 /* Miscellaneous Controls */ +#define CS43L22_PLAYBACK_CTRL2_DEFAULT 0x00 /* Playback Control 2 */ +#define CS43L22_PASS_VOL_A_DEFAULT 0x00 /* Passthrough x Volume: PASSAVOL */ +#define CS43L22_PASS_VOL_B_DEFAULT 0x00 /* Passthrough x Volume: PASSBVOL */ +#define CS43L22_PCM_VOL_A_DEFAULT 0x00 /* PCMx Volume: PCMA */ +#define CS43L22_PCM_VOL_B_DEFAULT 0x00 /* PCMx Volume: PCMB */ +#define CS43L22_BP_FREQ_ON_TIME_DEFAULT 0x00 /* Beep Frequency & On Time */ +#define CS43L22_BP_VOL_OFF_TIME_DEFAULT 0x00 /* Beep Volume & Off Time */ +#define CS43L22_BP_TONE_CFG_DEFAULT 0x00 /* Beep & Tone Configuration */ +#define CS43L22_TONE_CTRL_DEFAULT 0x88 /* Tone Control */ +#define CS43L22_MS_VOL_CTRL_A_DEFAULT 0x00 /* Master Volume Control: MSTA */ +#define CS43L22_MS_VOL_CTRL_B_DEFAULT 0x00 /* Master Volume Control: MSTB */ +#define CS43L22_HP_VOL_CTRL_A_DEFAULT 0x00 /* Headphone Volume Control: HPA */ +#define CS43L22_HP_VOL_CTRL_B_DEFAULT 0x00 /* Headphone Volume Control: HPB */ +#define CS43L22_SPK_VOL_CTRL_A_DEFAULT 0x00 /* Speaker Volume Control: SPKA */ +#define CS43L22_SPK_VOL_CTRL_B_DEFAULT 0x00 /* Speaker Volume Control: SPKB */ +#define CS43L22_PCM_CH_SWAP_DEFAULT 0x00 /* PCM Channel Swap */ +#define CS43L22_LIM_CTRL1_DEFAULT 0x00 /* Limiter Control 1, Min/Max Thresholds */ +#define CS43L22_LIM_CTRL2_DEFAULT 0x7f /* Limiter Control 2, Release Rate */ +#define CS43L22_LIM_ATTACK_RATE_DEFAULT 0xc0 /* Limiter Attack Rate */ +#define CS43L22_STATUS_DEFAULT 0x00 /* Status */ +#define CS43L22_BAT_COMP_DEFAULT 0x00 /* Battery Compensation */ +#define CS43L22_VP_BAT_LEVEL_DEFAULT 0x00 /* VP Battery Level */ +#define CS43L22_SPK_STATUS_DEFAULT 0x00 /* Speaker Status */ +#define CS43L22_TEMP_MON_CTRL_DEFAULT 0x3b /* Temperature Monitor Control */ +#define CS43L22_THERMAL_FOLDBACK_DEFAULT 0x00 /* Thermal Foldback */ +#define CS43L22_CHRG_PUMP_FREQ_DEFAULT 0x5f /* Charge Pump Frequency */ + +/* Register Bit Definitions *************************************************/ + +/* 0x01 Chip I.D. and Revision (Read Only) */ +#define CS43L22_DEV_ID_REV (0xe3) +#define CS43L22_ID_SHIFT (3) +#define CS43L22_ID_MASK (0x1f << CS43L22_ID_SHIFT) +#define CS43L22_REV_SHIFT (0) +#define CS43L22_REV_MASK (0x07 << CS43L22_REV_SHIFT) + +/* 0x02 Power Control 1 */ +#define CS43L22_POWER_DOWN (0x01) /* Powered Down */ +#define CS43L22_POWER_UP (0x9e) /* Powered Up */ + +/* 0x04 Power Control 2 */ +#define CS43L22_PDN_HPB_SHIFT (6) /* Bits 6-7: Headphone channel B Control */ +#define CS43L22_PDN_HPB_ON_HW_PIN_LO (0 << CS43L22_PDN_HPB_SHIFT) /* PDN_HPx[1:0] 00 Headphone channel is ON when the SPK/HP_SW pin, 6, is LO + Headphone channel is OFF when the SPK/HP_SW pin, 6, is HI */ +#define CS43L22_PDN_HPB_ON_HW_PIN_HI (1 << CS43L22_PDN_HPB_SHIFT) /* PDN_HPx[1:0] 01 Headphone channel is ON when the SPK/HP_SW pin, 6, is HI + Headphone channel is OFF when the SPK/HP_SW pin, 6, is LO */ +#define CS43L22_PDN_HPB_ON (2 << CS43L22_PDN_HPB_SHIFT) /* PDN_HPx[1:0] 10 Headphone channel is always ON */ +#define CS43L22_PDN_HPB_OFF (3 << CS43L22_PDN_HPB_SHIFT) /* PDN_HPx[1:0] 11 Headphone channel is always OFF */ + +#define CS43L22_PDN_HPA_SHIFT (4) /* Bits 4-5: Headphone channel A Control */ +#define CS43L22_PDN_HPA_ON_HW_PIN_LO (0 << CS43L22_PDN_HPA_SHIFT) /* PDN_HPx[1:0] 00 Headphone channel is ON when the SPK/HP_SW pin, 6, is LO + Headphone channel is OFF when the SPK/HP_SW pin, 6, is HI */ +#define CS43L22_PDN_HPA_ON_HW_PIN_HI (1 << CS43L22_PDN_HPA_SHIFT) /* PDN_HPx[1:0] 01 Headphone channel is ON when the SPK/HP_SW pin, 6, is HI + Headphone channel is OFF when the SPK/HP_SW pin, 6, is LO */ +#define CS43L22_PDN_HPA_ON (2 << CS43L22_PDN_HPA_SHIFT) /* PDN_HPx[1:0] 10 Headphone channel is always ON */ +#define CS43L22_PDN_HPA_OFF (3 << CS43L22_PDN_HPA_SHIFT) /* PDN_HPx[1:0] 11 Headphone channel is always OFF */ + +#define CS43L22_PDN_SPKB_SHIFT (2) /* Bits 2-3: Speaker channel B Control */ +#define CS43L22_PDN_SPKB_ON_HW_PIN_LO (0 << CS43L22_PDN_SPKB_SHIFT) /* PDN_HPx[1:0] 00 Speaker channel is ON when the SPK/HP_SW pin, 6, is LO + Speaker channel is OFF when the SPK/HP_SW pin, 6, is HI */ +#define CS43L22_PDN_SPKB_ON_HW_PIN_HI (1 << CS43L22_PDN_SPKB_SHIFT) /* PDN_HPx[1:0] 01 Speaker channel is ON when the SPK/HP_SW pin, 6, is HI + Speaker channel is OFF when the SPK/HP_SW pin, 6, is LO */ +#define CS43L22_PDN_SPKB_ON (2 << CS43L22_PDN_SPKB_SHIFT) /* PDN_HPx[1:0] 10 Speaker channel is always ON */ +#define CS43L22_PDN_SPKB_OFF (3 << CS43L22_PDN_SPKB_SHIFT) /* PDN_HPx[1:0] 11 Speaker channel is always OFF */ + +#define CS43L22_PDN_SPKA_SHIFT (0) /* Bits 0-1: Speaker channel A Control */ +#define CS43L22_PDN_SPKA_ON_HW_PIN_LO (0 << CS43L22_PDN_SPKA_SHIFT) /* PDN_HPx[1:0] 00 Speaker channel is ON when the SPK/HP_SW pin, 6, is LO + Speaker channel is OFF when the SPK/HP_SW pin, 6, is HI */ +#define CS43L22_PDN_SPKA_ON_HW_PIN_HI (1 << CS43L22_PDN_SPKA_SHIFT) /* PDN_HPx[1:0] 01 Speaker channel is ON when the SPK/HP_SW pin, 6, is HI + Speaker channel is OFF when the SPK/HP_SW pin, 6, is LO */ +#define CS43L22_PDN_SPKA_ON (2 << CS43L22_PDN_SPKA_SHIFT) /* PDN_HPx[1:0] 10 Speaker channel is always ON */ +#define CS43L22_PDN_SPKA_OFF (3 << CS43L22_PDN_SPKA_SHIFT) /* PDN_HPx[1:0] 11 Speaker channel is always OFF */ + +/* 0x05 Clocking Control */ +#define CS43L22_AUTO_DETECT_ENABLE (1 << 7) /* Auto-detection of speed mode enable */ + +#define CS43L22_SPEED_SHIFT (5) /* Bits 5-6: Speed mode */ +#define CS43L22_SPEED_DOUBLE (0 << CS43L22_SPEED_SHIFT) /* Slave: Double-Speed Mode (DSM - 50 kHz -100 kHz Fs) Master: MCLK=512 SCLK=64*/ +#define CS43L22_SPEED_SINGLE (1 << CS43L22_SPEED_SHIFT) /* Slave: Single-Speed Mode (SSM - 4 kHz -50 kHz Fs) Master: MCLK=256 SCLK=64*/ +#define CS43L22_SPEED_HALF (2 << CS43L22_SPEED_SHIFT) /* Slave: Half-Speed Mode (HSM - 12.5kHz -25 kHz Fs) Master: MCLK=128 SCLK=64*/ +#define CS43L22_SPEED_QUARTER (3 << CS43L22_SPEED_SHIFT) /* Slave: Quarter-Speed Mode (QSM - 4 kHz -12.5 kHz Fs)Master: MCLK=128 SCLK=64*/ + +#define CS43L22_32k_GROUP_ENABLE (1 << 4) /* Bit 4: Specifies whether or not the input/output sample rate is 8 kHz, 16 kHz or 32 kHz */ + +#define CS43L22_VIDEOCLK_ENABLE (1 << 3) /* Bit 3: Specifies whether or not the external MCLK frequency is 27 MHz */ + +#define CS43L22_MCLK_LRCK_RATIO_SHIFT (1) /* Bits 1-2: Internal MCLK/LRCK Ratio */ +#define CS43L22_RATIO_128_64 (0 << CS43L22_MCLK_LRCK_RATIO_SHIFT) /* RATIO[1:0] Internal MCLK Cycles per LRCK=128, SCLK/LRCK=64 Ratio in Master Mode */ +#define CS43L22_RATIO_125_62 (1 << CS43L22_MCLK_LRCK_RATIO_SHIFT) /* RATIO[1:0] Internal MCLK Cycles per LRCK=125, SCLK/LRCK=62 Ratio in Master Mode */ +#define CS43L22_RATIO_132_66 (2 << CS43L22_MCLK_LRCK_RATIO_SHIFT) /* RATIO[1:0] Internal MCLK Cycles per LRCK=132, SCLK/LRCK=66 Ratio in Master Mode */ +#define CS43L22_RATIO_136_68 (3 << CS43L22_MCLK_LRCK_RATIO_SHIFT) /* RATIO[1:0] Internal MCLK Cycles per LRCK=136, SCLK/LRCK=68 Ratio in Master Mode */ + +#define CS43L22_CLKDIV2_ENABLE (1 << 0) /* Bit 0: Divided by 2 */ + +/* 0x06 Interface Control 1 */ +#define CS43L22_MODE_MASTER (1 << 7) /* Configures the serial port I/O clocking */ + +#define CS43L22_SCLK_POLARITY_INVERT (1 << 6) /* Configures the polarity of the SCLK signal */ + +#define CS43L22_DSP_MODE_ENABLE (1 << 4) /* Configures a data-packed interface format for the DAC */ + +#define CS43L22_DAC_IF_FORMAT_SHIFT (2) /* Bits 2-3: Configures the digital interface format for data on SDIN */ +#define CS43L22_DAC_IF_LEFT_JUSTIFIED (0 << CS43L22_DAC_IF_FORMAT_SHIFT) /* DACDIF[1:0] Left Justified, up to 24-bit data */ +#define CS43L22_DAC_IF_I2S (1 << CS43L22_DAC_IF_FORMAT_SHIFT) /* DACDIF[1:0] I2S, up to 24-bit data */ +#define CS43L22_DAC_IF_RIGHT_JUSTIFIED (2 << CS43L22_DAC_IF_FORMAT_SHIFT) /* DACDIF[1:0] Right Justified */ +#define CS43L22_DAC_IF_RESERVED (3 << CS43L22_DAC_IF_FORMAT_SHIFT) /* DACDIF[1:0] Reserved */ + +#define CS43L22_AUDIO_WORD_LENGHT_SHIFT (0) /* Bits 0-1: Configures the audio sample word length used for the data into SDIN */ +#define CS43L22_AWL_DSP_32_RJ_24 (0 << CS43L22_AUDIO_WORD_LENGHT_SHIFT)/* AWL[1:0] DSP Mode: 32-bit data, Right Justified: 24-bit data */ +#define CS43L22_AWL_DSP_24_RJ_20 (1 << CS43L22_AUDIO_WORD_LENGHT_SHIFT)/* AWL[1:0] DSP Mode: 24-bit data, Right Justified: 20-bit data */ +#define CS43L22_AWL_DSP_20_RJ_18 (2 << CS43L22_AUDIO_WORD_LENGHT_SHIFT)/* AWL[1:0] DSP Mode: 20-bit data, Right Justified: 18-bit data */ +#define CS43L22_AWL_DSP_16_RJ_16 (3 << CS43L22_AUDIO_WORD_LENGHT_SHIFT)/* AWL[1:0] DSP Mode: 16 bit data, Right Justified: 16-bit data */ + +/* 0x0E Miscellaneous Controls */ +#define CS43L22_FREEZE (1 << 3) /* Configures a hold on all register settings */ +#define CS43L22_DEEMPHASIS_ENABLE (1 << 2) /* Configures a 15μs/50μs digital de-emphasis filter response on the headphone/line and speaker outputs */ + +/* 0x1F Tone Control */ +#define CS43L22_TREB_GAIN_SHIFT (4) /* Sets the gain of the treble shelving filter */ +#define CS43L22_TREB_GAIN(a) ((a) << CS43L22_TREB_GAIN_SHIFT) + /* TREB[3:0] Gain Setting:*/ + /* 0000 +12.0 dB */ + /* ··· ··· */ + /* 0111 +1.5 dB */ + /* 1000 0 dB */ + /* 1001 -1.5 dB */ + /* 1111 -10.5 dB */ + /* Step Size: 1.5 dB */ + +#define CS43L22_BASS_GAIN_SHIFT (0) /* Sets the gain of the bass shelving filter */ +#define CS43L22_BASS_GAIN(a) ((a) << CS43L22_BASS_GAIN_SHIFT) + /* BASS[3:0] Gain Setting:*/ + /* 0000 +12.0 dB */ + /* ··· ··· */ + /* 0111 +1.5 dB */ + /* 1000 0 dB */ + /* 1001 -1.5 dB */ + /* 1111 -10.5 dB */ + /* Step Size: 1.5 dB */ + +/* FLL Configuration *********************************************************/ +/* Default FLL configuration */ + +#define CS43L22_DEFAULT_SAMPRATE 11025 /* Initial sample rate */ +#define CS43L22_DEFAULT_NCHANNELS 1 /* Initial number of channels */ +#define CS43L22_DEFAULT_BPSAMP 16 /* Initial bits per sample */ + +#define CS43L22_NFLLRATIO 5 /* Number of FLL_RATIO values */ + +#define CS43L22_MINOUTDIV 4 /* Minimum FLL_OUTDIV divider */ +#define CS43L22_MAXOUTDIV 64 /* Maximum FLL_OUTDIV divider */ + +#define CS43L22_BCLK_MAXDIV 20 /* Maximum BCLK divider */ + +#define CS43L22_FRAMELEN8 14 /* Bits per frame for 8-bit data */ +#define CS43L22_FRAMELEN16 32 /* Bits per frame for 16-bit data */ + +/* Commonly defined and redefined macros */ + +#ifndef MIN +# define MIN(a,b) (((a) < (b)) ? (a) : (b)) +#endif + +#ifndef MAX +# define MAX(a,b) (((a) > (b)) ? (a) : (b)) +#endif + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +struct cs43l22_dev_s +{ + /* We are an audio lower half driver (We are also the upper "half" of + * the CS43L22 driver with respect to the board lower half driver). + * + * Terminology: Our "lower" half audio instances will be called dev for the + * publicly visible version and "priv" for the version that only this driver + * knows. From the point of view of this driver, it is the board lower + * "half" that is referred to as "lower". + */ + + struct audio_lowerhalf_s dev; /* CS43L22 audio lower half (this device) */ + + /* Our specific driver data goes here */ + + const FAR struct cs43l22_lower_s *lower; /* Pointer to the board lower functions */ + FAR struct i2c_master_s *i2c; /* I2C driver to use */ + FAR struct i2s_dev_s *i2s; /* I2S driver to use */ + struct dq_queue_s pendq; /* Queue of pending buffers to be sent */ + struct dq_queue_s doneq; /* Queue of sent buffers to be returned */ + mqd_t mq; /* Message queue for receiving messages */ + char mqname[16]; /* Our message queue name */ + pthread_t threadid; /* ID of our thread */ + uint32_t bitrate; /* Actual programmed bit rate */ + sem_t pendsem; /* Protect pendq */ +#ifdef CS43L22_USE_FFLOCK_INT + struct work_s work; /* Interrupt work */ +#endif + uint16_t samprate; /* Configured samprate (samples/sec) */ +#ifndef CONFIG_AUDIO_EXCLUDE_VOLUME +#ifndef CONFIG_AUDIO_EXCLUDE_BALANCE + uint16_t balance; /* Current balance level (b16) */ +#endif /* CONFIG_AUDIO_EXCLUDE_BALANCE */ + uint8_t volume; /* Current volume level {0..63} */ +#endif /* CONFIG_AUDIO_EXCLUDE_VOLUME */ + uint8_t nchannels; /* Number of channels (1 or 2) */ + uint8_t bpsamp; /* Bits per sample (8 or 16) */ + volatile uint8_t inflight; /* Number of audio buffers in-flight */ +#ifdef CS43L22_USE_FFLOCK_INT + volatile bool locked; /* FLL is locked */ +#endif + bool running; /* True: Worker thread is running */ + bool paused; /* True: Playing is paused */ + bool mute; /* True: Output is muted */ +#ifndef CONFIG_AUDIO_EXCLUDE_STOP + bool terminating; /* True: Stop requested */ +#endif + bool reserved; /* True: Device is reserved */ + volatile int result; /* The result of the last transfer */ +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +#ifdef CONFIG_CS43L22_CLKDEBUG +extern const uint8_t g_sysclk_scaleb1[CS43L22_BCLK_MAXDIV+1]; +extern const uint8_t g_fllratio[CS43L22_NFLLRATIO]; +#endif + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Name: cs43l22_readreg + * + * Description + * Read the specified 8-bit register from the CS43L22 device. + * + ****************************************************************************/ + +#if defined(CONFIG_CS43L22_REGDUMP) || defined(CONFIG_CS43L22_CLKDEBUG) +struct cs43l22_dev_s; +uint8_t cs43l22_readreg(FAR struct cs43l22_dev_s *priv, uint8_t regaddr); +#endif + +#endif /* CONFIG_AUDIO */ +#endif /* __DRIVERS_AUDIO_CS43L22_H */ diff --git a/drivers/audio/cs43l22_debug.c b/drivers/audio/cs43l22_debug.c new file mode 100644 index 0000000000..2d39fe988a --- /dev/null +++ b/drivers/audio/cs43l22_debug.c @@ -0,0 +1,184 @@ +/**************************************************************************** + * drivers/audio/cs43l22_debug.c + * Audio device driver for Cirrus Logic CS43L22 Audio codec. + * + * Copyright (C) 2017 Gregory Nutt. All rights reserved. + * Author: Taras Drozdovsky + * + * References: + * - "CS43L22 Ultra Low Power CODEC for Portable Audio Applications, Pre- + * Production", September 2012, Rev b1, Cirrus Logic + * - The framework for this driver is based on Ken Pettit's VS1053 driver. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include "cs43l22.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +#ifdef CONFIG_CS43L22_REGDUMP +struct cs43l22_regdump_s +{ + FAR const char *regname; + uint8_t regaddr; +}; +#endif + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +#ifdef CONFIG_CS43L22_REGDUMP +static const struct cs43l22_regdump_s g_cs43l22_debug[] = +{ + {"CHIP_ID_REV", CS43L22_ID_REV }, + {"POWER_CTRL1", CS43L22_POWER_CTRL1 }, + {"POWER_CTRL2", CS43L22_POWER_CTRL2 }, + {"CLOCK_CTRL", CS43L22_CLOCK_CTRL }, + {"INTERFACE_CTRL1", CS43L22_INTERFACE_CTRL1 }, + {"INTERFACE_CTRL2", CS43L22_INTERFACE_CTRL2 }, + {"PASS_SEL_A", CS43L22_PASS_SEL_A }, + {"PASS_SEL_B", CS43L22_PASS_SEL_B }, + {"ANLG_ZC_SR_SEL", CS43L22_ANLG_ZC_SR_SEL }, + {"PASS_GANG_CTRL", CS43L22_PASS_GANG_CTRL }, + {"PLAYBACK_CTRL1", CS43L22_PLAYBACK_CTRL1 }, + {"MISCLLNS_CTRL", CS43L22_MISCLLNS_CTRL }, + {"PLAYBACK_CTRL2", CS43L22_PLAYBACK_CTRL2 }, + {"PASS_VOL_A", CS43L22_PASS_VOL_A }, + {"PASS_VOL_B", CS43L22_PASS_VOL_B }, + {"PCM_VOL_A", CS43L22_PCM_VOL_A }, + {"PCM_VOL_B", CS43L22_PCM_VOL_B }, + {"BP_FREQ_ON_T", CS43L22_BP_FREQ_ON_TIME }, + {"BP_VOL_OFF_T", CS43L22_BP_VOL_OFF_TIME }, + {"BP_TONE_CFG", CS43L22_BP_TONE_CFG }, + {"TONE_CTRL", CS43L22_TONE_CTRL }, + {"MS_VOL_CTRL_A", CS43L22_MS_VOL_CTRL_A }, + {"MS_VOL_CTRL_B", CS43L22_MS_VOL_CTRL_B }, + {"HP_VOL_CTRL_A", CS43L22_HP_VOL_CTRL_A }, + {"HP_VOL_CTRL_B", CS43L22_HP_VOL_CTRL_B }, + {"SPK_VOL_CTRL_A", CS43L22_SPK_VOL_CTRL_A }, + {"SPK_VOL_CTRL_B", CS43L22_SPK_VOL_CTRL_B }, + {"PCM_CH_SWAP", CS43L22_PCM_CH_SWAP }, + {"LIM_CTRL1", CS43L22_LIM_CTRL1 }, + {"LIM_CTRL2", CS43L22_LIM_CTRL2 }, + {"LIM_ATTACK_RATE", CS43L22_LIM_ATTACK_RATE }, + {"STATUS", CS43L22_STATUS }, + {"BAT_COMP", CS43L22_BAT_COMP }, + {"VP_BAT_LEVEL", CS43L22_VP_BAT_LEVEL }, + {"SPK_STATUS", CS43L22_SPK_STATUS }, + {"TEMP_MON_CTRL", CS43L22_TEMP_MON_CTRL }, + {"THERMAL_FOLDBACK",CS43L22_THERMAL_FOLDBACK}, + {"CHRG_PUMP_FREQ", CS43L22_CHRG_PUMP_FREQ } +}; + +# define CS43L22_NREGISTERS (sizeof(g_cs43l22_debug)/sizeof(struct cs43l22_regdump_s)) +#endif /* CONFIG_CS43L22_REGDUMP */ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cs43l22_dump_registers + * + * Description: + * Dump the contents of all CS43L22 registers to the syslog device + * + * Input Parameters: + * dev - The device instance returned by cs43l22_initialize + * + * Returned Value: + * None. + * + ****************************************************************************/ + +#ifdef CONFIG_CS43L22_REGDUMP +void cs43l22_dump_registers(FAR struct audio_lowerhalf_s *dev, + FAR const char *msg) +{ + int i; + + syslog(LOG_INFO, "CS43L22 Registers: %s\n", msg); + for (i = 0; i < CS43L22_NREGISTERS; i++) + { + syslog(LOG_INFO, "%16s[%02x]: %02x\n", + g_cs43l22_debug[i].regname, g_cs43l22_debug[i].regaddr, + cs43l22_readreg((FAR struct cs43l22_dev_s *)dev, + g_cs43l22_debug[i].regaddr)); + } +} +#endif /* CONFIG_CS43L22_REGDUMP */ + +/**************************************************************************** + * Name: cs43l22_clock_analysis + * + * Description: + * Analyze the settings in the clock chain and dump to syslog. + * + * Input Parameters: + * dev - The device instance returned by cs43l22_initialize + * + * Returned Value: + * None. + * + ****************************************************************************/ + +#ifdef CONFIG_CS43L22_CLKDEBUG +void cs43l22_clock_analysis(FAR struct audio_lowerhalf_s *dev, + FAR const char *msg) +{ + #warning Missing logic + /* TODO */ +} +#endif /* CONFIG_CS43L22_CLKDEBUG */ diff --git a/include/nuttx/audio/cs43l22.h b/include/nuttx/audio/cs43l22.h new file mode 100644 index 0000000000..9065478c2d --- /dev/null +++ b/include/nuttx/audio/cs43l22.h @@ -0,0 +1,280 @@ +/**************************************************************************** + * include/nuttx/audio/cs43l22.h + * + * Copyright (C) 2017 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Reference: + * "CS43L22 Ultra Low Power CODEC for Portable Audio Applications, Pre- + * Production", September 2012, Rev 3.3, Wolfson Microelectronics + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef __INCLUDE_NUTTX_AUDIO_CS43L22_H +#define __INCLUDE_NUTTX_AUDIO_CS43L22_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include + +#ifdef CONFIG_AUDIO_CS43L22 + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ +/* Configuration ************************************************************ + * + * CONFIG_AUDIO_CS43L22 - Enables CS43L22 support + * CONFIG_CS43L22_INITVOLUME - The initial volume level in the range {0..1000} + * CONFIG_CS43L22_INFLIGHT - Maximum number of buffers that the CS43L22 driver + * will send to the I2S driver before any have completed. + * CONFIG_CS43L22_MSG_PRIO - Priority of messages sent to the CS43L22 worker + * thread. + * CONFIG_CS43L22_BUFFER_SIZE - Preferred buffer size + * CONFIG_CS43L22_NUM_BUFFERS - Preferred number of buffers + * CONFIG_CS43L22_WORKER_STACKSIZE - Stack size to use when creating the the + * CS43L22 worker thread. + * CONFIG_CS43L22_REGDUMP - Enable logic to dump all CS43L22 registers to + * the SYSLOG device. + */ + +/* Pre-requisites */ + +#ifndef CONFIG_AUDIO +# error CONFIG_AUDIO is required for audio subsystem support +#endif + +#ifndef CONFIG_I2S +# error CONFIG_I2S is required by the CS43L22 driver +#endif + +#ifndef CONFIG_I2C +# error CONFIG_I2C is required by the CS43L22 driver +#endif + +#ifndef CONFIG_SCHED_WORKQUEUE +# error CONFIG_SCHED_WORKQUEUE is required by the CS43L22 driver +#endif + +/* Default configuration values */ + +#ifndef CONFIG_CS43L22_INITVOLUME +# define CONFIG_CS43L22_INITVOLUME 400 +#endif + +#ifndef CONFIG_CS43L22_INFLIGHT +# define CONFIG_CS43L22_INFLIGHT 2 +#endif + +#if CONFIG_CS43L22_INFLIGHT > 255 +# error CONFIG_CS43L22_INFLIGHT must fit in a uint8_t +#endif + +#ifndef CONFIG_CS43L22_MSG_PRIO +# define CONFIG_CS43L22_MSG_PRIO 1 +#endif + +#ifndef CONFIG_CS43L22_BUFFER_SIZE +# define CONFIG_CS43L22_BUFFER_SIZE 8192 +#endif + +#ifndef CONFIG_CS43L22_NUM_BUFFERS +# define CONFIG_CS43L22_NUM_BUFFERS 4 +#endif + +#ifndef CONFIG_CS43L22_WORKER_STACKSIZE +# define CONFIG_CS43L22_WORKER_STACKSIZE 768 +#endif + +/* Helper macros ************************************************************/ + +#define CS43L22_ATTACH(s,isr,arg) ((s)->attach(s,isr,arg)) +#define CS43L22_DETACH(s) ((s)->attach(s,NULL,NULL)) +#define CS43L22_ENABLE(s) ((s)->enable(s,true)) +#define CS43L22_DISABLE(s) ((s)->enable(s,false)) +#define CS43L22_RESTORE(s,e) ((s)->enable(s,e)) +#define CS43L22_HW_RESET(s) ((s)->reset(s)) + +/**************************************************************************** + * Public Types + ****************************************************************************/ +/* This is the type of the CS43L22 interrupt handler. The lower level code + * will intercept the interrupt and provide the upper level with the private + * data that was provided when the interrupt was attached. + */ + +struct cs43l22_lower_s; /* Forward reference. Defined below */ + +typedef CODE int (*cs43l22_handler_t)(FAR const struct cs43l22_lower_s *lower, + FAR void *arg); + +/* A reference to a structure of this type must be passed to the CS43L22 + * driver. This structure provides information about the configuration + * of the CS43L22 and provides some board-specific hooks. + * + * Memory for this structure is provided by the caller. It is not copied + * by the driver and is presumed to persist while the driver is active. + */ + +struct cs43l22_lower_s +{ + /* I2C characterization */ + + uint32_t frequency; /* Initial I2C frequency */ + uint8_t address; /* 7-bit I2C address (only bits 0-6 used) */ + + /* Clocking is provided via MCLK. The CS43L22 driver will need to know + * the frequency of MCLK in order to generate the correct bitrates. + */ + + uint32_t mclk; /* CS43L22 Master clock frequency */ + + /* IRQ/GPIO access callbacks. These operations all hidden behind + * callbacks to isolate the CS43L22 driver from differences in GPIO + * interrupt handling by varying boards and MCUs. If possible, + * interrupts should be configured on both rising and falling edges + * so that contact and loss-of-contact events can be detected. + * + * attach - Attach or detach the CS43L22 interrupt handler to the GPIO + * interrupt + * enable - Enable or disable the GPIO interrupt. Returns the + * previous interrupt state. + * reset - HW reset of the CS43L22 chip + */ + + CODE int (*attach)(FAR const struct cs43l22_lower_s *lower, + cs43l22_handler_t isr, FAR void *arg); + CODE bool (*enable)(FAR const struct cs43l22_lower_s *lower, bool enable); + CODE void (*reset)(FAR const struct cs43l22_lower_s *lower); +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" +{ +#else +#define EXTERN extern +#endif + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Name: cs43l22_initialize + * + * Description: + * Initialize the CS43L22 device. + * + * Input Parameters: + * i2c - An I2C driver instance + * i2s - An I2S driver instance + * lower - Persistent board configuration data + * + * Returned Value: + * A new lower half audio interface for the CS43L22 device is returned on + * success; NULL is returned on failure. + * + ****************************************************************************/ + +struct i2c_master_s; /* Forward reference. Defined in include/nuttx/i2c/i2c_master.h */ +struct i2s_dev_s; /* Forward reference. Defined in include/nuttx/audio/i2s.h */ +struct audio_lowerhalf_s; /* Forward reference. Defined in nuttx/audio/audio.h */ + +FAR struct audio_lowerhalf_s * + cs43l22_initialize(FAR struct i2c_master_s *i2c, FAR struct i2s_dev_s *i2s, + FAR const struct cs43l22_lower_s *lower); + +/**************************************************************************** + * Name: cs43l22_dump_registers + * + * Description: + * Dump the contents of all CS43L22 registers to the syslog device + * + * Input Parameters: + * dev - The device instance returned by cs43l22_initialize + * + * Returned Value: + * None. + * + ****************************************************************************/ + +#ifdef CONFIG_CS43L22_REGDUMP +void cs43l22_dump_registers(FAR struct audio_lowerhalf_s *dev, + FAR const char *msg); +#else + /* This eliminates the need for any conditional compilation in the + * including file. + */ + +# define cs43l22_dump_registers(d,m) +#endif + +/**************************************************************************** + * Name: cs43l22_clock_analysis + * + * Description: + * Analyze the settings in the clock chain and dump to syslog. + * + * Input Parameters: + * dev - The device instance returned by cs43l22_initialize + * + * Returned Value: + * None. + * + ****************************************************************************/ + +#ifdef CONFIG_CS43L22_CLKDEBUG +void cs43l22_clock_analysis(FAR struct audio_lowerhalf_s *dev, + FAR const char *msg); +#else + /* This eliminates the need for any conditional compilation in the + * including file. + */ + +# define cs43l22_clock_analysis(d,m) +#endif + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* CONFIG_AUDIO_CS43L22 */ +#endif /* __INCLUDE_NUTTX_AUDIO_CS43L22_H */