diff --git a/libraries/CurieI2S/examples/I2SDMA_RXCallBack/I2SDMA_RXCallBack.ino b/libraries/CurieI2S/examples/I2SDMA_RXCallBack/I2SDMA_RXCallBack.ino new file mode 100644 index 00000000..97ab8163 --- /dev/null +++ b/libraries/CurieI2S/examples/I2SDMA_RXCallBack/I2SDMA_RXCallBack.ino @@ -0,0 +1,99 @@ +/* + * Copyright (c) 2016 Intel Corporation. All rights reserved. + * See the bottom of this file for the license terms. + */ + +/** + * A simple sketch to test the rx channel of the i2s interface. + * A callback function is used to fill up a buffer whenever data is received + * + * To test this sketch you will need a second Arduino/Genuino 101 board with the I2SDMA_TxCallback sketch uploaded + * + * Connection: + * GND -> GND + * I2S_RSCK(pin 8) -> I2S_TSCK(pin 2) + * I2S_RWS(pin 3) -> I2S_TWS(pin 4) + * I2S_RXD(pin 5) -> I2S_TXD(pin 7) + * +**/ +#include + +#define BUFF_SIZE 128 +#define OFFSET 2 +uint32_t dataBuff[BUFF_SIZE+OFFSET]; // extra 2 buffer is for the padding zero + +uint8_t start_flag = 0; +uint8_t done_flag = 0; +uint32_t loop_count = 0; + +void setup() +{ + Serial.begin(115200); + while(!Serial); + Serial.println("CurieI2SDMA Rx Callback"); + + CurieI2SDMA.iniRX(); + /* + * CurieI2SDMA.beginTX(sample_rate, resolution, master,mode) + * mode 1 : PHILIPS_MODE + * 2 : RIGHT_JST_MODE + * 3 : LEFT_JST_MODE + * 4 : DSP_MODE + */ + CurieI2SDMA.beginRX(44100, 32,0,1); + +} + +void loop() +{ + int status = CurieI2SDMA.transRX(dataBuff,sizeof(dataBuff)); + if(status) + return; + + if(start_flag) + { + if((dataBuff[OFFSET]>>16) != loop_count+1) + Serial.println("+++ loop_count jump +++"); + } + else + { + start_flag = 1; + } + loop_count = (dataBuff[OFFSET] >> 16); + + done_flag = 1; + for(uint32_t i = 0 ;i < BUFF_SIZE;++i) + { + //Serial.println(dataBuff[i+OFFSET],HEX); + if ((dataBuff[i+OFFSET] & 0XFFFF0000) == (loop_count <<16) + && (dataBuff[i+OFFSET] & 0XFFFF) == (i+1)) + ; + else + { + done_flag = 0; + Serial.println(dataBuff[i+OFFSET],HEX); + Serial.println("ERROR"); + break; + } + } + + if(done_flag) + Serial.println("RX done"); + delay(100); +} + +/* + Copyright (c) 2016 Intel Corporation. All rights reserved. + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110- + 1301 USA +*/ \ No newline at end of file diff --git a/libraries/CurieI2S/examples/I2SDMA_TXCallBack/I2SDMA_TXCallBack.ino b/libraries/CurieI2S/examples/I2SDMA_TXCallBack/I2SDMA_TXCallBack.ino new file mode 100644 index 00000000..fe27ab2e --- /dev/null +++ b/libraries/CurieI2S/examples/I2SDMA_TXCallBack/I2SDMA_TXCallBack.ino @@ -0,0 +1,69 @@ +/* + * Copyright (c) 2016 Intel Corporation. All rights reserved. + * See the bottom of this file for the license terms. + */ + +//I2S_TX -> Pin 7 +//I2S_TSK -> Pin 4 +//I2S_TSCK -> pin 2 + +#include + +#define BUFF_SIZE 128 +boolean blinkState = true; // state of the LED +uint32_t dataBuff[BUFF_SIZE]; +uint32_t loop_count = 0; +void setup() +{ + Serial.begin(115200); + while(!Serial); + Serial.println("CurieI2SDMA Tx Callback"); + + CurieI2SDMA.iniTX(); + /* + * CurieI2SDMA.beginTX(sample_rate, resolution, master,mode) + * mode 1 : PHILIPS_MODE + * 2 : RIGHT_JST_MODE + * 3 : LEFT_JST_MODE + * 4 : DSP_MODE + */ + CurieI2SDMA.beginTX(44100, 32,1, 1); + digitalWrite(13, blinkState); +} + +void loop() +{ + for(uint32_t i = 0; i + +static void txi2s_done(void* x); +static void rxi2s_done(void* x); +static void txi2s_err(void* x); +static void rxi2s_err(void* x); + +volatile uint8_t txdone_flag = 0; +volatile uint8_t txerror_flag = 0; + +volatile uint8_t rxdone_flag = 0; +volatile uint8_t rxerror_flag = 0; + + +static void txi2s_done(void* x) +{ + txdone_flag = 1; + + return; +} + +static void rxi2s_done(void* x) +{ + rxdone_flag = 1; + + return; +} + +static void txi2s_err(void* x) +{ + txerror_flag = 1; + + return; +} + +static void rxi2s_err(void* x) +{ + rxerror_flag = 1; + + return; +} + +struct soc_i2s_cfg txcfg ; + +struct soc_i2s_cfg rxcfg ; + +Curie_I2SDMA CurieI2SDMA; + +Curie_I2SDMA::Curie_I2SDMA() +{ +} + +int Curie_I2SDMA::iniTX() +{ + muxTX(1); + soc_i2s_init(); + soc_dma_init(); + return I2S_DMA_OK; +} + +int Curie_I2SDMA::iniRX() +{ + muxRX(1); + soc_i2s_init(); + soc_dma_init(); + return I2S_DMA_OK; +} + +void Curie_I2SDMA::muxTX(bool enable) +{ + int mux_mode = GPIO_MUX_MODE; + if(enable) + { + mux_mode = I2S_MUX_MODE; + } + + /* Set SoC pin mux configuration */ + SET_PIN_MODE(g_APinDescription[I2S_TXD].ulSocPin, mux_mode); + SET_PIN_MODE(g_APinDescription[I2S_TWS].ulSocPin, mux_mode); + SET_PIN_MODE(g_APinDescription[I2S_TSCK].ulSocPin, mux_mode); + g_APinDescription[I2S_TXD].ulPinMode = mux_mode; + g_APinDescription[I2S_TWS].ulPinMode = mux_mode; + g_APinDescription[I2S_TSCK].ulPinMode = mux_mode; +} + +void Curie_I2SDMA::muxRX(bool enable) +{ + int mux_mode = GPIO_MUX_MODE; + if(enable) + { + mux_mode = I2S_MUX_MODE; + } + + /* Set SoC pin mux configuration */ + SET_PIN_MODE(49, mux_mode); //I2S_RXD + SET_PIN_MODE(51, mux_mode); //I2S_RWS + SET_PIN_MODE(50, mux_mode); //I2S_RSCK + g_APinDescription[I2S_RXD].ulPinMode = mux_mode; + g_APinDescription[I2S_RWS].ulPinMode = mux_mode; + g_APinDescription[I2S_RSCK].ulPinMode = mux_mode; +} + +int Curie_I2SDMA::beginTX(uint16_t sample_rate,uint8_t resolution,uint8_t master,uint8_t mode) +{ + switch(mode) + { + case 1: + mode = I2S_MODE_PHILLIPS ; + break; + case 2: + mode = I2S_MODE_RJ; + break; + case 3: + mode = I2S_MODE_LJ; + break; + case 4: + mode = I2S_MODE_DSP; + break; + default: + break; + } + + txcfg.sample_rate = sample_rate; + txcfg.resolution = resolution; + txcfg.mode = mode; + txcfg.master = master; + txcfg.cb_done = txi2s_done; + txcfg.cb_err = txi2s_err; + txdone_flag = 0; + txerror_flag = 0; + soc_i2s_config(I2S_CHANNEL_TX, &txcfg); + + return I2S_DMA_OK; +} + +int Curie_I2SDMA::beginRX(uint16_t sample_rate,uint8_t resolution,uint8_t master,uint8_t mode) +{ + switch(mode) + { + case 1: + mode = I2S_MODE_PHILLIPS ; + break; + case 2: + mode = I2S_MODE_RJ; + break; + case 3: + mode = I2S_MODE_LJ; + break; + case 4: + mode = I2S_MODE_DSP; + break; + default: + break; + } + + rxcfg.sample_rate = sample_rate; + rxcfg.resolution = resolution; + rxcfg.mode = mode; + rxcfg.master = master; + + rxcfg.cb_done = rxi2s_done; + rxcfg.cb_err = rxi2s_err; + + rxdone_flag = 0; + rxerror_flag = 0; + + soc_i2s_config(I2S_CHANNEL_RX, &rxcfg); + + return I2S_DMA_OK; +} + +int Curie_I2SDMA::transTX(uint32_t* buf_TX,uint32_t len) +{ + soc_i2s_stream(buf_TX, len,0); + + while (1) + { + // check the DMA and I2S status + if(txdone_flag && !txerror_flag) + { + txdone_flag = 0; + txerror_flag = 0; + return I2S_DMA_OK; + } + if(txerror_flag) + { + return I2S_DMA_FAIL; + } + } +} + +int Curie_I2SDMA::transRX(uint32_t* buf_RX,uint32_t len) +{ + soc_i2s_listen(buf_RX, len ,0); + + while (1) + { + // check the DMA and I2S status + if(rxdone_flag && !rxerror_flag) + { + rxdone_flag = 0; + rxerror_flag = 0; + return I2S_DMA_OK; + } + if(rxerror_flag) + { + return I2S_DMA_FAIL; + } + } + return I2S_DMA_OK; +} + +void Curie_I2SDMA::stopTX() +{ + soc_i2s_stop_stream(); + muxTX(0); +} + +void Curie_I2SDMA::stopRX() +{ + soc_i2s_stop_listen(); + muxRX(0); +} + +int Curie_I2SDMA::mergeData(uint32_t* buf_left,uint32_t* buf_right,uint32_t* buf_TX) +{ + int length = (int)(sizeof(buf_left) / sizeof(buf_left[0])); + for(int i = 0; i < length;++i) + { + buf_TX[2*i] = buf_left[i]; + buf_TX[2*i+1] = buf_right[i]; + } + + return I2S_DMA_OK; +} + +int Curie_I2SDMA::separateData(uint32_t* buf_left,uint32_t* buf_right,uint32_t* buf_RX) +{ + int length1 = (int)( sizeof(buf_RX) / sizeof(buf_RX[0])/2 ); + int length2 = (int)( sizeof(buf_left) / sizeof(buf_left[0]) ); + int length = length1 < length2 ? length1 : length2; + + for(int i = 0; i < length;++i) + { + buf_left[i] = buf_RX[2*i]; + buf_right[i] = buf_RX[2*i+1]; + } + return I2S_DMA_OK; +} diff --git a/libraries/CurieI2S/src/CurieI2SDMA.h b/libraries/CurieI2S/src/CurieI2SDMA.h new file mode 100644 index 00000000..a1707123 --- /dev/null +++ b/libraries/CurieI2S/src/CurieI2SDMA.h @@ -0,0 +1,81 @@ +//*************************************************************** +// +// Copyright (c) 2016 Intel Corporation. All rights reserved. +// +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA +// +//*************************************************************** + +//CurieI2SDMA.h + +#ifndef _CURI2SDMA_H_ +#define _CURI2SDMA_H_ + +#include + +//I2S Arduino Pins +#define I2S_TXD 7 +#define I2S_TWS 4 +#define I2S_TSCK 2 +#define I2S_RXD 5 +#define I2S_RWS 3 +#define I2S_RSCK 8 + +#define I2S_DMA_OK 0 +#define I2S_DMA_FAIL 1 +class Curie_I2SDMA +{ + private: + // mux/demux the i2s rx pins into i2s mode + void muxRX(bool enable); + + // mux/demux the i2s tx pins into i2s mode + void muxTX(bool enable); + + public: + Curie_I2SDMA(); + + // + int beginTX(uint16_t sample_rate,uint8_t resolution,uint8_t master,uint8_t mode); + // + int beginRX(uint16_t sample_rate,uint8_t resolution,uint8_t master,uint8_t mode); + + // initializes i2s interface + int iniTX(); + //void transTX(); + int iniRX(); + + // starts transmission of data to the tx channel + int transTX(uint32_t* buf_TX,uint32_t len); + + // starts listening to the rx channel + int transRX(uint32_t* buf_RX,uint32_t len); + + // merge data of left and right channel into one buffer + int mergeData(uint32_t* buf_left,uint32_t* buf_right,uint32_t* buf_TX); + + // seperate the data to left and right channl + int separateData(uint32_t* buf_left,uint32_t* buf_right,uint32_t* buf_RX); + + // + void stopTX(); + // + void stopRX(); + +}; + +extern Curie_I2SDMA CurieI2SDMA; + +#endif diff --git a/system/libarc32_arduino101/common/scss_registers.h b/system/libarc32_arduino101/common/scss_registers.h index ddf393f4..600459a4 100644 --- a/system/libarc32_arduino101/common/scss_registers.h +++ b/system/libarc32_arduino101/common/scss_registers.h @@ -120,6 +120,8 @@ #define QRK_CLKGATE_CTRL_PWM_ENABLE (1 << 12) #define PERIPH_CLK_GATE_CTRL (SCSS_REGISTER_BASE + 0x018) +#define MLAYER_AHB_CTL (SCSS_REGISTER_BASE + 0x034) +#define SS_PERIPH_CLK_GATE_CTL (SCSS_REGISTER_BASE + 0x028) /* PWM */ #define QRK_PWM_BASE_ADDR 0xB0000800 @@ -216,6 +218,9 @@ #define QRK_RTC_ENABLE (1 << 2) #define QRK_RTC_WRAP_ENABLE (1 << 3) +/* DMA */ +#define SOC_DMA_BASE (0xB0700000) + /* MPR */ #define QRK_MPR_BASE_ADDR 0xB0400000 #define QRK_MPR_REGS_LEN 0x04 @@ -244,6 +249,9 @@ #define SOC_MST_SPI1_REGISTER_BASE (0xB0001400) #define SOC_SLV_SPI_REGISTER_BASE (0xB0001800) +/* I2S */ +#define SOC_I2S_BASE (0xB0003800) + /* Mailbox Interrupt*/ #define IO_REG_MAILBOX_INT_MASK (SCSS_REGISTER_BASE + 0x4A0) @@ -392,11 +400,21 @@ #define SOC_SPIS0_INTERRUPT 0x4 #define SOC_UART0_INTERRUPT 0x5 #define SOC_UART1_INTERRUPT 0x6 +#define SOC_I2S_INTERRUPT 0x7 #define SOC_GPIO_INTERRUPT 0x8 #define SOC_PWM_INTERRUPT 0x9 #define SOC_RTC_INTERRUPT 0xb #define SOC_WDT_INTERRUPT 0xc +#define SOC_DMA_CHANNEL0_INTERRUPT 0xd +#define SOC_DMA_CHANNEL1_INTERRUPT 0xe +#define SOC_DMA_CHANNEL2_INTERRUPT 0xf +#define SOC_DMA_CHANNEL3_INTERRUPT 0x10 +#define SOC_DMA_CHANNEL4_INTERRUPT 0x11 +#define SOC_DMA_CHANNEL5_INTERRUPT 0x12 +#define SOC_DMA_CHANNEL6_INTERRUPT 0x13 +#define SOC_DMA_CHANNEL7_INTERRUPT 0x14 #define SOC_MBOX_INTERRUPT 0x15 +#define SOC_DMA_ERR_INTERRUPT 0x18 #define SOC_MPR_INTERRUPT 0x19 #define SOC_GPIO_AON_INTERRUPT 0x1F @@ -411,9 +429,19 @@ #define SOC_SPIS0_INTERRUPT (40) #define SOC_UART0_INTERRUPT (41) #define SOC_UART1_INTERRUPT (42) +#define SOC_I2S_INTERRUPT (43) #define SOC_GPIO_INTERRUPT (44) #define SOC_PWM_INTERRUPT (45) +#define SOC_DMA_CHANNEL0_INTERRUPT (49) +#define SOC_DMA_CHANNEL1_INTERRUPT (50) +#define SOC_DMA_CHANNEL2_INTERRUPT (51) +#define SOC_DMA_CHANNEL3_INTERRUPT (52) +#define SOC_DMA_CHANNEL4_INTERRUPT (53) +#define SOC_DMA_CHANNEL5_INTERRUPT (54) +#define SOC_DMA_CHANNEL6_INTERRUPT (55) +#define SOC_DMA_CHANNEL7_INTERRUPT (56) #define SOC_MBOX_INTERRUPT (57) +#define SOC_DMA_ERR_INTERRUPT (60) #define SOC_GPIO_AON_INTERRUPT (67) #define DRV_REG(_driver_) (MMIO_REG_VAL_FROM_BASE(SCSS_REGISTER_BASE, _driver_)) @@ -424,4 +452,8 @@ #endif +/* Clock gate mask */ +#define I2C0_CLK_GATE_MASK 0x00080004 +#define I2C1_CLK_GATE_MASK 0x00100008 +#define I2S_CLK_GATE_MASK 0x00200200 #endif /* SCSS_REGISTERS_H_ */ diff --git a/system/libarc32_arduino101/drivers/clk_system.c b/system/libarc32_arduino101/drivers/clk_system.c new file mode 100644 index 00000000..a3b57c79 --- /dev/null +++ b/system/libarc32_arduino101/drivers/clk_system.c @@ -0,0 +1,41 @@ +/* + * Copyright (c) 2015, Intel Corporation. All rights reserved. + * + * 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 of the copyright holder 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 HOLDER 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. + */ + +#include "clk_system.h" +#include "scss_registers.h" + +void set_clock_gate(struct clk_gate_info_s* clk_gate_info, uint32_t value) +{ + uint32_t tmp ; + tmp = MMIO_REG_VAL(clk_gate_info->clk_gate_register); + tmp &= ~(clk_gate_info->bits_mask); + tmp |= ((clk_gate_info->bits_mask) & value); + MMIO_REG_VAL(clk_gate_info->clk_gate_register) = tmp; +} diff --git a/system/libarc32_arduino101/drivers/clk_system.h b/system/libarc32_arduino101/drivers/clk_system.h new file mode 100644 index 00000000..99b8e748 --- /dev/null +++ b/system/libarc32_arduino101/drivers/clk_system.h @@ -0,0 +1,64 @@ +/* + * Copyright (c) 2015, Intel Corporation. All rights reserved. + * + * 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 of the copyright holder 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 HOLDER 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 CLK_SYSTEM_H_ +#define CLK_SYSTEM_H_ + +#include + +#define CLK_GATE_OFF (0) +#define CLK_GATE_ON (~CLK_GATE_OFF) + +/** + * @defgroup clk_gate Clock gating driver + * Clk Gate driver API. + * @ingroup common_drivers + * @{ + */ + +/** + * Clock gate data which contain register and bits implicated. + */ +struct clk_gate_info_s { + uint32_t clk_gate_register; /*!< register changed for clock gate */ + uint32_t bits_mask; /*!< mask used for clock gate */ +}; + +/** +* Configure clock gate to specified device +* +* @param clk_gate_info : pointer to a clock gate data structure +* @param value : state of clock gate desired +*/ +void set_clock_gate(struct clk_gate_info_s* clk_gate_info, uint32_t value); + +/** @} */ + +#endif /* CLK_SYSTEM_H_ */ diff --git a/system/libarc32_arduino101/drivers/i2s_priv.h b/system/libarc32_arduino101/drivers/i2s_priv.h new file mode 100644 index 00000000..6b90af1a --- /dev/null +++ b/system/libarc32_arduino101/drivers/i2s_priv.h @@ -0,0 +1,302 @@ +/* + * Copyright (c) 2016, Intel Corporation. All rights reserved. + * + * 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 of the copyright holder 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 HOLDER 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 I2S_PRIV_H_ +#define I2S_PRIV_H_ + +#include "soc_i2s.h" + +#define I2S_TFIFO_SIZE 4 +#define I2S_RFIFO_SIZE 4 + +#define I2S_RFIFO_THR 1 +#define I2S_TFIFO_THR 1 + +// I2S Registers (from base register) and fields +#define SOC_I2S_CTRL (0x00) +#define SOC_I2S_CTRL_EN_0 (0) +#define SOC_I2S_CTRL_EN_1 (1) +#define SOC_I2S_CTRL_TR_CFG_0 (8) +#define SOC_I2S_CTRL_TR_CFG_1 (9) +#define SOC_I2S_CTRL_LOOP_BACK_0_1 (16) +#define SOC_I2S_CTRL_SFR_RST (20) +#define SOC_I2S_CTRL_T_MS (21) +#define SOC_I2S_CTRL_R_MS (22) +#define SOC_I2S_CTRL_TFIFO_RST (23) +#define SOC_I2S_CTRL_RFIFO_RST (24) +#define SOC_I2S_CTRL_TSYNC_RST (25) +#define SOC_I2S_CTRL_RSYNC_RST (26) +#define SOC_I2S_CTRL_TSYNC_LOOP_BACK (27) +#define SOC_I2S_CTRL_RSYNC_LOOP_BACK (28) + +#define SOC_I2S_STAT (0x04) +#define SOC_I2S_STAT_TDATA_UNDERR (0) +#define SOC_I2S_STAT_UNDERR_CODE (1) +#define SOC_I2S_STAT_RDATA_OVRERR (4) +#define SOC_I2S_STAT_OVRERR_CODE (5) +#define SOC_I2S_STAT_TFIFO_EMPTY (8) +#define SOC_I2S_STAT_TFIFO_AEMPTY (9) +#define SOC_I2S_STAT_TFIFO_FULL (10) +#define SOC_I2S_STAT_TFIFO_AFULL (11) +#define SOC_I2S_STAT_RFIFO_EMPTY (12) +#define SOC_I2S_STAT_RFIFO_AEMPTY (13) +#define SOC_I2S_STAT_RFIFO_FULL (14) +#define SOC_I2S_STAT_RFIFO_AFULL (15) + +#define SOC_I2S_SRR (0x08) +#define SOC_I2S_SRR_TSAMPLE_RATE (0) +#define SOC_I2S_SRR_TRESOLUTION (11) +#define SOC_I2S_SRR_RSAMPLE_RATE (16) +#define SOC_I2S_SRR_RRESOLUTION (27) + +#define SOC_I2S_CID_CTRL (0x0C) +#define SOC_I2S_CID_CTRL_STROBE_0 (0) +#define SOC_I2S_CID_CTRL_STROBE_1 (1) +#define SOC_I2S_CID_CTRL_STROBE_TS (8) +#define SOC_I2S_CID_CTRL_STROBE_RS (9) +#define SOC_I2S_CID_CTRL_INTREQ_MASK (15) +#define SOC_I2S_CID_CTRL_MASK_0 (16) +#define SOC_I2S_CID_CTRL_MASK_1 (17) +#define SOC_I2S_CID_CTRL_TFIFO_EMPTY_MASK (24) +#define SOC_I2S_CID_CTRL_TFIFO_AEMPTY_MASK (25) +#define SOC_I2S_CID_CTRL_TFIFO_FULL_MASK (26) +#define SOC_I2S_CID_CTRL_TFIFO_AFULL_MASK (27) +#define SOC_I2S_CID_CTRL_RFIFO_EMPTY_MASK (28) +#define SOC_I2S_CID_CTRL_RFIFO_AEMPTY_MASK (29) +#define SOC_I2S_CID_CTRL_RFIFO_FULL_MASK (30) +#define SOC_I2S_CID_CTRL_RFIFO_AFULL_MASK (31) + +#define SOC_I2S_TFIFO_STAT (0x10) + +#define SOC_I2S_RFIFO_STAT (0x14) + +#define SOC_I2S_TFIFO_CTRL (0x18) +#define SOC_I2S_TFIFO_CTRL_TAEMPTY_THRS (0) +#define SOC_I2S_TFIFO_CTRL_TAFULL_THRS (16) + +#define SOC_I2S_RFIFO_CTRL (0x1C) +#define SOC_I2S_RFIFO_CTRL_RAEMPTY_THRS (0) +#define SOC_I2S_RFIFO_CTRL_RAFULL_THRS (16) + +#define SOC_I2S_DEV_CONF (0x20) +#define SOC_I2S_DEV_CONF_TRAN_SCK_POLAR (0) +#define SOC_I2S_DEV_CONF_TRAN_WS_POLAR (1) +#define SOC_I2S_DEV_CONF_TRAN_ABP_ALIGN_LR (2) +#define SOC_I2S_DEV_CONF_TRAN_I2S_ALIGN_LR (3) +#define SOC_I2S_DEV_CONF_TRAN_DATA_WS_DEL (4) +#define SOC_I2S_DEV_CONF_TRAN_WS_DSP_MODE (5) +#define SOC_I2S_DEV_CONF_REC_SCK_POLAR (6) +#define SOC_I2S_DEV_CONF_REC_WS_POLAR (7) +#define SOC_I2S_DEV_CONF_REC_ABP_ALIGN_LR (8) +#define SOC_I2S_DEV_CONF_REC_I2S_ALIGN_LR (9) +#define SOC_I2S_DEV_CONF_REC_DATA_WS_DEL (10) +#define SOC_I2S_DEV_CONF_REC_WS_DSP_MODE (11) + +#define SOC_I2S_DATA_REG (0x50) + +struct i2s_channel_regs { + // CRTL Register + uint8_t ctrl; + uint8_t ctrl_en; + uint8_t ctrl_tr_cfg; + uint8_t ctrl_ms; + uint8_t ctrl_fifo_rst; + uint8_t ctrl_sync_rst; + uint8_t ctrl_sync_loopback; + + // STAT Register + uint8_t stat; + uint8_t stat_err; + uint8_t stat_code; + uint8_t stat_fifo_empty; + uint8_t stat_fifo_aempty; + uint8_t stat_fifo_full; + uint8_t stat_fifo_afull; + uint32_t stat_mask; + + // SRR Register + uint8_t srr; + uint8_t srr_sample_rate; + uint8_t srr_resolution; + uint32_t srr_mask; + + // CID CTRL Register + uint8_t cid_ctrl; + uint8_t cid_ctrl_strobe; + uint8_t cid_ctrl_strobe_sync; + uint8_t cid_ctrl_mask; + uint8_t cid_ctrl_fifo_empty_mask; + uint8_t cid_ctrl_fifo_aempty_mask; + uint8_t cid_ctrl_fifo_full_mask; + uint8_t cid_ctrl_fifo_afull_mask; + + // FIFO STAT Register + uint8_t fifo_stat; + + // FIFO CTRL Register + uint8_t fifo_ctrl; + uint8_t fifo_ctrl_aempty_thr; + uint8_t fifo_ctrl_afull_thr; + + // DEV CONF Register + uint8_t dev_conf; + uint8_t dev_conf_sck_polar; + uint8_t dev_conf_ws_polar; + uint8_t dev_conf_abp_align_lr; + uint8_t dev_conf_i2s_align_lr; + uint8_t dev_conf_data_ws_del; + uint8_t dev_conf_ws_dsp_mode; + uint32_t dev_conf_mask; + + // DATA Register + uint8_t data_reg; +}; + +const struct i2s_channel_regs i2s_reg_map[I2S_NUM_CHANNELS] = { + // TX Channel + { + // CRTL Register + .ctrl = SOC_I2S_CTRL, + .ctrl_en = SOC_I2S_CTRL_EN_0, + .ctrl_tr_cfg = SOC_I2S_CTRL_TR_CFG_0, + .ctrl_ms = SOC_I2S_CTRL_T_MS, + .ctrl_fifo_rst = SOC_I2S_CTRL_TFIFO_RST, + .ctrl_sync_rst = SOC_I2S_CTRL_TSYNC_RST, + .ctrl_sync_loopback = SOC_I2S_CTRL_TSYNC_LOOP_BACK, + + // STAT Register + .stat = SOC_I2S_STAT, + .stat_err = SOC_I2S_STAT_TDATA_UNDERR, + .stat_code = SOC_I2S_STAT_UNDERR_CODE, + .stat_fifo_empty = SOC_I2S_STAT_TFIFO_EMPTY, + .stat_fifo_aempty = SOC_I2S_STAT_TFIFO_AEMPTY, + .stat_fifo_full = SOC_I2S_STAT_TFIFO_FULL, + .stat_fifo_afull = SOC_I2S_STAT_TFIFO_AFULL, + .stat_mask = 0x00000F0F, + + // SRR Register + .srr = SOC_I2S_SRR, + .srr_sample_rate = SOC_I2S_SRR_TSAMPLE_RATE, + .srr_resolution = SOC_I2S_SRR_TRESOLUTION, + .srr_mask = 0x0000FFFF, + + // CID CTRL Register + .cid_ctrl = SOC_I2S_CID_CTRL, + .cid_ctrl_strobe = SOC_I2S_CID_CTRL_STROBE_0, + .cid_ctrl_strobe_sync = SOC_I2S_CID_CTRL_STROBE_TS, + .cid_ctrl_mask = SOC_I2S_CID_CTRL_MASK_0, + .cid_ctrl_fifo_empty_mask = SOC_I2S_CID_CTRL_TFIFO_EMPTY_MASK, + .cid_ctrl_fifo_aempty_mask = SOC_I2S_CID_CTRL_TFIFO_AEMPTY_MASK, + .cid_ctrl_fifo_full_mask = SOC_I2S_CID_CTRL_TFIFO_FULL_MASK, + .cid_ctrl_fifo_afull_mask = SOC_I2S_CID_CTRL_TFIFO_AFULL_MASK, + + // FIFO STAT Register + .fifo_stat = SOC_I2S_TFIFO_STAT, + + // FIFO CTRL Register + .fifo_ctrl = SOC_I2S_TFIFO_CTRL, + .fifo_ctrl_aempty_thr = SOC_I2S_TFIFO_CTRL_TAEMPTY_THRS, + .fifo_ctrl_afull_thr = SOC_I2S_TFIFO_CTRL_TAFULL_THRS, + + // DEV CONF Register + .dev_conf = SOC_I2S_DEV_CONF, + .dev_conf_sck_polar = SOC_I2S_DEV_CONF_TRAN_SCK_POLAR, + .dev_conf_ws_polar = SOC_I2S_DEV_CONF_TRAN_WS_POLAR, + .dev_conf_abp_align_lr = SOC_I2S_DEV_CONF_TRAN_ABP_ALIGN_LR, + .dev_conf_i2s_align_lr = SOC_I2S_DEV_CONF_TRAN_I2S_ALIGN_LR, + .dev_conf_data_ws_del = SOC_I2S_DEV_CONF_TRAN_DATA_WS_DEL, + .dev_conf_ws_dsp_mode = SOC_I2S_DEV_CONF_TRAN_WS_DSP_MODE, + .dev_conf_mask = 0x0000003F, + + // DATA Register + .data_reg = SOC_I2S_DATA_REG + }, + + // RX Channel + { + // CRTL Register + .ctrl = SOC_I2S_CTRL, + .ctrl_en = SOC_I2S_CTRL_EN_1, + .ctrl_tr_cfg = SOC_I2S_CTRL_TR_CFG_1, + .ctrl_ms = SOC_I2S_CTRL_R_MS, + .ctrl_fifo_rst = SOC_I2S_CTRL_RFIFO_RST, + .ctrl_sync_rst = SOC_I2S_CTRL_RSYNC_RST, + .ctrl_sync_loopback = SOC_I2S_CTRL_RSYNC_LOOP_BACK, + + // STAT Register + .stat = SOC_I2S_STAT, + .stat_err = SOC_I2S_STAT_RDATA_OVRERR, + .stat_code = SOC_I2S_STAT_OVRERR_CODE, + .stat_fifo_empty = SOC_I2S_STAT_RFIFO_EMPTY, + .stat_fifo_aempty = SOC_I2S_STAT_RFIFO_AEMPTY, + .stat_fifo_full = SOC_I2S_STAT_RFIFO_FULL, + .stat_fifo_afull = SOC_I2S_STAT_RFIFO_AFULL, + .stat_mask = 0x0000F0F0, + + // SRR Register + .srr = SOC_I2S_SRR, + .srr_sample_rate = SOC_I2S_SRR_RSAMPLE_RATE, + .srr_resolution = SOC_I2S_SRR_RRESOLUTION, + .srr_mask = 0xFFFF0000, + + // CID CTRL Register + .cid_ctrl = SOC_I2S_CID_CTRL, + .cid_ctrl_strobe = SOC_I2S_CID_CTRL_STROBE_1, + .cid_ctrl_strobe_sync = SOC_I2S_CID_CTRL_STROBE_RS, + .cid_ctrl_mask = SOC_I2S_CID_CTRL_MASK_1, + .cid_ctrl_fifo_empty_mask = SOC_I2S_CID_CTRL_RFIFO_EMPTY_MASK, + .cid_ctrl_fifo_aempty_mask = SOC_I2S_CID_CTRL_RFIFO_AEMPTY_MASK, + .cid_ctrl_fifo_full_mask = SOC_I2S_CID_CTRL_RFIFO_FULL_MASK, + .cid_ctrl_fifo_afull_mask = SOC_I2S_CID_CTRL_RFIFO_AFULL_MASK, + + // FIFO STAT Register + .fifo_stat = SOC_I2S_RFIFO_STAT, + + // FIFO CTRL Register + .fifo_ctrl = SOC_I2S_RFIFO_CTRL, + .fifo_ctrl_aempty_thr = SOC_I2S_RFIFO_CTRL_RAEMPTY_THRS, + .fifo_ctrl_afull_thr = SOC_I2S_RFIFO_CTRL_RAFULL_THRS, + + // DEV CONF Register + .dev_conf = SOC_I2S_DEV_CONF, + .dev_conf_sck_polar = SOC_I2S_DEV_CONF_REC_SCK_POLAR, + .dev_conf_ws_polar = SOC_I2S_DEV_CONF_REC_WS_POLAR, + .dev_conf_abp_align_lr = SOC_I2S_DEV_CONF_REC_ABP_ALIGN_LR, + .dev_conf_i2s_align_lr = SOC_I2S_DEV_CONF_REC_I2S_ALIGN_LR, + .dev_conf_data_ws_del = SOC_I2S_DEV_CONF_REC_DATA_WS_DEL, + .dev_conf_ws_dsp_mode = SOC_I2S_DEV_CONF_REC_WS_DSP_MODE, + .dev_conf_mask = 0x00000FC0, + + // DATA Register + .data_reg = SOC_I2S_DATA_REG + } +}; + +#endif /* I2S_PRIV_H_ */ diff --git a/system/libarc32_arduino101/drivers/soc_dma.c b/system/libarc32_arduino101/drivers/soc_dma.c new file mode 100644 index 00000000..f0b0120c --- /dev/null +++ b/system/libarc32_arduino101/drivers/soc_dma.c @@ -0,0 +1,793 @@ +/* + * Copyright (c) 2016, Intel Corporation. All rights reserved. + * + * 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 of the copyright holder 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 HOLDER 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. + */ + +#include "scss_registers.h" +#include "portable.h" +#include "os/os.h" +#include "data_type.h" +#include "clk_system.h" + +#include "soc_dma.h" +#include "soc_dma_priv.h" + +// Some useful helper macros for setting and clearing bits and values (NR: dont read the value first) +// x - address or variable +// f - bit field to set or clear +#define SETB(x, f) ((x) |= (1 << (f))) +#define CLRB(x, f) ((x) &= ~(1 << (f))) + +#define SETB_NR(x, f) ((x) = (1 << (f))) +#define CLRB_NR(x, f) ((x) = ~(1 << (f))) + +// x - address or variable +// f - starting bit for value +// l - length of value +// v - value to set +#define SETV(x, f, l, v) ((x) &= ~(((1 << (l)) - 1) << (f))); \ + ((x) |= ((((1 << (l)) - 1) & (v)) << (f))) + +#define SETV_NR(x, f, l, v) ((x) = ((((1 << (l)) - 1) & (v)) << (f))) + +// Function Prototypes +static void dma_disable(struct soc_dma_channel *channel); +static void dma_enable(struct soc_dma_channel *channel); +static struct soc_dma_xfer_item *dma_find_list_end(struct soc_dma_xfer_item *head); +static struct dma_lli *dma_find_ll_end(struct dma_lli *head); +static DRIVER_API_RC dma_create_ll(struct soc_dma_channel *channel, struct soc_dma_cfg *cfg); +static void dma_interrupt_handler(void *num); +DRIVER_API_RC soc_dma_config(struct soc_dma_channel *channel, struct soc_dma_cfg *cfg); +DRIVER_API_RC soc_dma_deconfig(struct soc_dma_channel *channel); +DRIVER_API_RC soc_dma_acquire(struct soc_dma_channel *channel); +DRIVER_API_RC soc_dma_release(struct soc_dma_channel *channel); +DRIVER_API_RC soc_dma_start_transfer(struct soc_dma_channel *channel); +DRIVER_API_RC soc_dma_stop_transfer(struct soc_dma_channel *channel); +DRIVER_API_RC soc_dma_alloc_list_item(struct soc_dma_xfer_item **ret, struct soc_dma_xfer_item *base); +DRIVER_API_RC soc_dma_free_list(struct soc_dma_cfg *cfg); +DRIVER_API_RC dma_init(); + +DECLARE_INTERRUPT_HANDLER static void dma_ch0_interrupt_handler() +{ + dma_interrupt_handler((void *)0); +} + +DECLARE_INTERRUPT_HANDLER static void dma_ch1_interrupt_handler() +{ + dma_interrupt_handler((void *)1); +} + +struct soc_dma_info g_dma_info = { + .int_mask = { + INT_DMA_CHANNEL_0_MASK, + INT_DMA_CHANNEL_1_MASK, + INT_DMA_CHANNEL_2_MASK, + INT_DMA_CHANNEL_3_MASK, + INT_DMA_CHANNEL_4_MASK, + INT_DMA_CHANNEL_5_MASK, + INT_DMA_CHANNEL_6_MASK, + INT_DMA_CHANNEL_7_MASK + }, + .int_vector = { + SOC_DMA_CHANNEL0_INTERRUPT, + SOC_DMA_CHANNEL1_INTERRUPT, + SOC_DMA_CHANNEL2_INTERRUPT, + SOC_DMA_CHANNEL3_INTERRUPT, + SOC_DMA_CHANNEL4_INTERRUPT, + SOC_DMA_CHANNEL5_INTERRUPT, + SOC_DMA_CHANNEL6_INTERRUPT, + SOC_DMA_CHANNEL7_INTERRUPT + }, + .int_handler = { + dma_ch0_interrupt_handler, + dma_ch1_interrupt_handler, + NULL, + NULL, + NULL, + NULL, + NULL, + NULL + }, + .err_mask = INT_DMA_ERROR_MASK, + .err_vector = SOC_DMA_ERR_INTERRUPT +}; + +static struct soc_dma_info* dma_info = &g_dma_info; + +/* Internal Functions */ +static void dma_disable(struct soc_dma_channel *channel) +{ + uint32_t reg; + + // Turn off the given channel + reg = MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_CH_EN_REG); + SETB(reg, SOC_DMA_CH_EN_REG_CH_EN_WE + (channel->id)); + CLRB(reg, SOC_DMA_CH_EN_REG_CH_EN + (channel->id)); + MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_CH_EN_REG) = reg; + + // Clear interrupts and disable them + SETB_NR(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_CLEAR_TFR), SOC_DMA_CLEAR_CLEAR + (channel->id)); + SETB_NR(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_CLEAR_BLOCK), SOC_DMA_CLEAR_CLEAR + (channel->id)); + SETB_NR(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_CLEAR_ERR), SOC_DMA_CLEAR_CLEAR + (channel->id)); + + reg = MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_MASK_TFR); + SETB(reg, SOC_DMA_MASK_INT_MASK_WE + (channel->id)); + CLRB(reg, SOC_DMA_MASK_INT_MASK + (channel->id)); + MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_MASK_TFR) = reg; + + reg = MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_MASK_BLOCK); + SETB(reg, SOC_DMA_MASK_INT_MASK_WE + (channel->id)); + CLRB(reg, SOC_DMA_MASK_INT_MASK + (channel->id)); + MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_MASK_BLOCK) = reg; + + reg = MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_MASK_ERR); + SETB(reg, SOC_DMA_MASK_INT_MASK_WE + (channel->id)); + CLRB(reg, SOC_DMA_MASK_INT_MASK + (channel->id)); + MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_MASK_ERR) = reg; + + CLRB(dma_regs[channel->id].CTL_L, SOC_DMA_CTL_L_INT_EN); + + // Deactivate channel + channel->active = 0; + CLRB(dma_info->active, channel->id); + + return; +} + +static void dma_enable(struct soc_dma_channel *channel) +{ + uint32_t reg; + + // Clear interrupts and enable them + SETB_NR(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_CLEAR_TFR), SOC_DMA_CLEAR_CLEAR + (channel->id)); + SETB_NR(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_CLEAR_BLOCK), SOC_DMA_CLEAR_CLEAR + (channel->id)); + SETB_NR(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_CLEAR_ERR), SOC_DMA_CLEAR_CLEAR + (channel->id)); + + reg = MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_MASK_TFR); + SETB(reg, SOC_DMA_MASK_INT_MASK_WE + (channel->id)); + SETB(reg, SOC_DMA_MASK_INT_MASK + (channel->id)); + MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_MASK_TFR) = reg; + + reg = MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_MASK_BLOCK); + SETB(reg, SOC_DMA_MASK_INT_MASK_WE + (channel->id)); + SETB(reg, SOC_DMA_MASK_INT_MASK + (channel->id)); + MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_MASK_BLOCK) = reg; + + reg = MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_MASK_ERR); + SETB(reg, SOC_DMA_MASK_INT_MASK_WE + (channel->id)); + SETB(reg, SOC_DMA_MASK_INT_MASK + (channel->id)); + MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_MASK_ERR) = reg; + + SETB(dma_regs[channel->id].CTL_L, SOC_DMA_CTL_L_INT_EN); + + // Active channel + channel->active = 1; + SETB(dma_info->active, channel->id); + + // Turn on the given channel + reg = MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_CH_EN_REG); + SETB(reg, SOC_DMA_CH_EN_REG_CH_EN_WE + channel->id); + SETB(reg, SOC_DMA_CH_EN_REG_CH_EN + channel->id); + MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_CH_EN_REG) = reg; + + return; +} + +static struct soc_dma_xfer_item *dma_find_list_end(struct soc_dma_xfer_item *head) +{ + struct soc_dma_xfer_item *t; + struct soc_dma_xfer_item *h; + + // Get rid of easy cases + if ((head == NULL) || (head->next == NULL) || + (head->next->next == NULL)) + { + return NULL; + } + + // Tortoise and hare check for cyclic ll + t = head->next; + h = head->next->next; + while (t != h) + { + h = h->next; + if (h == NULL) + { + return NULL; + } + h = h->next; + if (h == NULL) + { + return NULL; + } + t = t->next; + } + + // Find the meeting place since a cycle was detected + t = head; + while (t != h) + { + t = t->next; + h = h->next; + } + + return t; +} + +static struct dma_lli *dma_find_ll_end(struct dma_lli *head) +{ + struct dma_lli *t; + struct dma_lli *h; + + // Get rid of easy cases + if ((head == NULL) || (((struct dma_lli *)(head->llp)) == NULL) || + (((struct dma_lli *)(((struct dma_lli *)(head->llp))->llp)) == NULL)) + { + return NULL; + } + + // Tortoise and hare check for cyclic ll + t = (struct dma_lli *)(head->llp); + h = (struct dma_lli *)(((struct dma_lli *)(head->llp))->llp); + while (t != h) + { + h = (struct dma_lli *)(h->llp); + if (h == NULL) + { + return NULL; + } + h = (struct dma_lli *)(h->llp); + if (h == NULL) + { + return NULL; + } + t = (struct dma_lli *)(t->llp); + } + + // Find the meeting place since a cycle was detected + t = head; + while (t != h) + { + t = (struct dma_lli *)(t->llp); + h = (struct dma_lli *)(h->llp); + } + + return t; +} + +static DRIVER_API_RC dma_create_ll(struct soc_dma_channel *channel, struct soc_dma_cfg *cfg) +{ + OS_ERR_TYPE err; + struct dma_lli *lli; + struct dma_lli *last_lli; + struct soc_dma_xfer_item *xfer; + struct soc_dma_xfer_item *last_xfer; + uint8_t list_done; + uint16_t size_left; + uint32_t curr_src; + uint8_t src_delta_amount; + uint32_t curr_dest; + uint8_t dest_delta_amount; + uint32_t reg; + + // Save the LL + channel->ll = balloc(sizeof(struct dma_lli), &err); + lli = (struct dma_lli *)(channel->ll); + last_lli = NULL; + + if (channel->ll == NULL) + { + return DRV_RC_FAIL; + } + + xfer = &(cfg->xfer); + last_xfer = dma_find_list_end(xfer); + + list_done = 0; + + // Set register to default CTL_L register value + reg = 0; + SETB(reg, SOC_DMA_CTL_L_LLP_SRC_EN); + SETB(reg, SOC_DMA_CTL_L_LLP_DST_EN); + SETV(reg, SOC_DMA_CTL_L_TT_FC, SOC_DMA_CTL_L_TT_FC_LEN, cfg->type); + SETV(reg, SOC_DMA_CTL_L_SRC_TR_WIDTH, SOC_DMA_CTL_L_SRC_TR_WIDTH_LEN, SOC_DMA_WIDTH_32); + SETV(reg, SOC_DMA_CTL_L_DST_TR_WIDTH, SOC_DMA_CTL_L_DST_TR_WIDTH_LEN, SOC_DMA_WIDTH_32); + SETB(reg, SOC_DMA_CTL_L_INT_EN); + + while (!list_done) + { + size_left = xfer->size; + curr_src = (uint32_t)(xfer->src.addr); + curr_dest = (uint32_t)(xfer->dest.addr); + + if (xfer == last_xfer) + { + last_lli = lli; + } + + while (size_left) + { + lli->sar = curr_src; + lli->dar = curr_dest; + + // Set the control register for this block + SETV(lli->ctl_u, SOC_DMA_CTL_U_BLOCK_TS, SOC_DMA_CTL_U_BLOCK_TS_LEN, + (size_left > SOC_DMA_BLOCK_SIZE_MAX) ? SOC_DMA_BLOCK_SIZE_MAX : size_left); + + lli->ctl_l = reg; + + if (cfg->src_step_count) + { + SETB(lli->ctl_l, SOC_DMA_CTL_L_SRC_GATHER_EN); + + // Scatter-Gather adds too much complexity for breaking up big blocks + if (size_left > SOC_DMA_BLOCK_SIZE_MAX) + { + soc_dma_deconfig(channel); + return DRV_RC_INVALID_CONFIG; + } + } + + if (cfg->dest_step_count) + { + SETB(lli->ctl_l, SOC_DMA_CTL_L_DST_SCATTER_EN); + + // Scatter-Gather adds too much complexity for breaking up big blocks + if (size_left > SOC_DMA_BLOCK_SIZE_MAX) + { + soc_dma_deconfig(channel); + return DRV_RC_INVALID_CONFIG; + } + } + + SETV(lli->ctl_l, SOC_DMA_CTL_L_SINC, SOC_DMA_CTL_L_SINC_LEN, xfer->src.delta); + SETV(lli->ctl_l, SOC_DMA_CTL_L_DINC, SOC_DMA_CTL_L_DINC_LEN, xfer->dest.delta); + + SETV(lli->ctl_l, SOC_DMA_CTL_L_SRC_TR_WIDTH, SOC_DMA_CTL_L_SRC_TR_WIDTH_LEN, xfer->src.width); + SETV(lli->ctl_l, SOC_DMA_CTL_L_DST_TR_WIDTH, SOC_DMA_CTL_L_DST_TR_WIDTH_LEN, xfer->dest.width); + + if (size_left > SOC_DMA_BLOCK_SIZE_MAX) + { + lli->end_group = 0; + lli->llp = (uint32_t)balloc(sizeof(struct dma_lli), &err); + + if (lli->llp == 0) + { + soc_dma_deconfig(channel); + return DRV_RC_FAIL; + } + + // Calculate new addresses for next block + size_left -= SOC_DMA_BLOCK_SIZE_MAX; + + src_delta_amount = ((xfer->src.width == SOC_DMA_WIDTH_8) ? 1 : + ((xfer->src.width == SOC_DMA_WIDTH_16) ? 2 : 4)); + dest_delta_amount = ((xfer->dest.width == SOC_DMA_WIDTH_8) ? 1 : + ((xfer->dest.width == SOC_DMA_WIDTH_16) ? 2 : 4)); + + switch (xfer->src.delta) + { + case SOC_DMA_DELTA_INCR: + curr_src += (src_delta_amount * SOC_DMA_BLOCK_SIZE_MAX); + break; + case SOC_DMA_DELTA_DECR: + curr_src -= (src_delta_amount * SOC_DMA_BLOCK_SIZE_MAX); + break; + } + + switch (xfer->dest.delta) + { + case SOC_DMA_DELTA_INCR: + curr_dest += (dest_delta_amount * SOC_DMA_BLOCK_SIZE_MAX); + break; + case SOC_DMA_DELTA_DECR: + curr_dest -= (dest_delta_amount * SOC_DMA_BLOCK_SIZE_MAX); + break; + } + + lli = (struct dma_lli *)lli->llp; + } + else + { + // Terminate this group of blocks for this xfer item + lli->end_group = 1; + size_left = 0; + } + } + + // Are we on the last block? If so, complete the list and leave, else setup the next loop + if ((xfer->next) == last_xfer) + { + lli->llp = (uint32_t)last_lli; + list_done = 1; + + if (lli->llp == 0) + { + SETB(lli->ctl_u, SOC_DMA_CTL_U_DONE_BIT); + CLRB(lli->ctl_l, SOC_DMA_CTL_L_LLP_SRC_EN); + CLRB(lli->ctl_l, SOC_DMA_CTL_L_LLP_DST_EN); + } + } + else + { + lli->llp = (uint32_t)balloc(sizeof(struct dma_lli), &err); + + if (lli->llp == 0) + { + soc_dma_deconfig(channel); + return DRV_RC_FAIL; + } + + xfer = xfer->next; + lli = (struct dma_lli *)lli->llp; + } + } + + return DRV_RC_OK; +} + +/* ISR */ +static void dma_interrupt_handler(void *num) +{ + uint32_t id = (uint32_t)num; + struct dma_lli *curr = ((struct dma_lli *)(dma_info->channel[id]->curr)); + + // Figure out whether this was a block or done interrupt + if (MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_STATUS_TFR) & (1 << (SOC_DMA_STATUS_STATUS + id))) + { + if ((dma_info->channel[id]) && (dma_info->channel[id]->cfg.cb_done)) + { + dma_info->channel[id]->cfg.cb_done(dma_info->channel[id]->cfg.cb_done_arg); + } + + // The user's callback might have already released the channel + if (dma_info->channel[id]) + { + dma_disable(dma_info->channel[id]); + } + } + else if (MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_STATUS_BLOCK) & (1 << (SOC_DMA_STATUS_STATUS + id))) + { + if ((dma_info->channel[id]) && (dma_info->channel[id]->cfg.cb_block) && (curr->end_group)) + { + dma_info->channel[id]->cfg.cb_block(dma_info->channel[id]->cfg.cb_block_arg); + } + + if (dma_info->channel[id]) + { + dma_info->channel[id]->curr = ((void *)(curr->llp)); + SETB_NR(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_CLEAR_BLOCK), SOC_DMA_CLEAR_CLEAR + id); + } + } + + return; +} + +DECLARE_INTERRUPT_HANDLER static void dma_interrupt_err_handler() +{ + uint32_t reg = MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_STATUS_ERR); + + // Loop through channels and see which have errors; calling callback and disabling + for (int i = 0; i < SOC_DMA_NUM_CHANNELS; i++) + { + if (reg & (1 << (SOC_DMA_STATUS_STATUS + i))) + { + if ((dma_info->channel[i]) && (dma_info->channel[i]->cfg.cb_err)) + { + dma_info->channel[i]->cfg.cb_err(dma_info->channel[i]->cfg.cb_err_arg); + } + + if (dma_info->channel[i]) + { + dma_disable(dma_info->channel[i]); + } + } + } + + return; +} + +/* External API */ +DRIVER_API_RC soc_dma_config(struct soc_dma_channel *channel, struct soc_dma_cfg *cfg) +{ + DRIVER_API_RC ret; + uint32_t id = channel->id; + + // Check channel to be sure its alright to configure + if (channel == NULL) + { + return DRV_RC_FAIL; + } + else if (channel->active) + { + return DRV_RC_CONTROLLER_IN_USE; + } + + // Setup the config register + CLRB(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, dma_regs[id].CFG_L), SOC_DMA_CFG_L_RELOAD_DST); + CLRB(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, dma_regs[id].CFG_L), SOC_DMA_CFG_L_RELOAD_SRC); + + CLRB(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, dma_regs[id].CFG_L), SOC_DMA_CFG_L_SRC_HS_POL); + CLRB(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, dma_regs[id].CFG_L), SOC_DMA_CFG_L_DST_HS_POL); + + CLRB(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, dma_regs[id].CFG_L), SOC_DMA_CFG_L_HS_SEL_SRC); + CLRB(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, dma_regs[id].CFG_L), SOC_DMA_CFG_L_HS_SEL_DST); + + SETV(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, dma_regs[id].CFG_U), + SOC_DMA_CFG_U_DEST_PER, SOC_DMA_CFG_U_DEST_PER_LEN, cfg->dest_interface); + SETV(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, dma_regs[id].CFG_U), + SOC_DMA_CFG_U_SRC_PER, SOC_DMA_CFG_U_SRC_PER_LEN, cfg->src_interface); + + CLRB(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, dma_regs[id].CFG_U), SOC_DMA_CFG_U_SS_UPD_EN); + CLRB(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, dma_regs[id].CFG_U), SOC_DMA_CFG_U_DS_UPD_EN); + + SETB(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, dma_regs[id].CFG_U), SOC_DMA_CFG_U_FIFO_MODE); + CLRB(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, dma_regs[id].CFG_U), SOC_DMA_CFG_U_FCMODE); + + // Create new LL for DMA transfer + ret = dma_create_ll(channel, cfg); + + if (ret != DRV_RC_OK) + { + return ret; + } + + // Setup scatter-gather registers + SETV(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, dma_regs[id].SGR), + SOC_DMA_SGR_SGC, SOC_DMA_SGR_SGC_LEN, cfg->src_step_count); + SETV(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, dma_regs[id].SGR), + SOC_DMA_SGR_SGI, SOC_DMA_SGR_SGI_LEN, cfg->src_step_interval); + + SETV(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, dma_regs[id].DSR), + SOC_DMA_DSR_DSC, SOC_DMA_DSR_DSC_LEN, cfg->dest_step_count); + SETV(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, dma_regs[id].DSR), + SOC_DMA_DSR_DSI, SOC_DMA_DSR_DSI_LEN, cfg->dest_step_interval); + + // Setup the channel structs cfg stuff + channel->cfgd = 1; + channel->cfg = *cfg; + + return DRV_RC_OK; +} + +DRIVER_API_RC soc_dma_deconfig(struct soc_dma_channel *channel) +{ + struct dma_lli *lli; + struct dma_lli *lli_next; + struct dma_lli *last_lli; + + // Check channel to be sure its alright to deconfigure + if (channel == NULL) + { + return DRV_RC_FAIL; + } + else if (channel->active) + { + return DRV_RC_CONTROLLER_IN_USE; + } + + // Free the link list + lli = (struct dma_lli *)(channel->ll); + last_lli = dma_find_ll_end(lli); + + while (lli) + { + if (((struct dma_lli *)(lli->llp)) == last_lli) + { + bfree(lli); + lli = NULL; + } + else + { + lli_next = ((struct dma_lli *)(lli->llp)); + bfree(lli); + lli = lli_next; + } + } + + channel->ll = NULL; + + channel->cfgd = 0; + + return DRV_RC_OK; +} + +DRIVER_API_RC soc_dma_acquire(struct soc_dma_channel *channel) +{ + uint32_t save; + + // Get a channel from the semaphore if one is free + save = interrupt_lock(); + + // Find the first free channel and set it up in the given struct + for (int i = 0; i < SOC_DMA_NUM_CHANNELS; i++) + { + if (dma_info->channel[i] == NULL) + { + dma_info->channel[i] = channel; + channel->id = i; + channel->active = 0; + soc_dma_deconfig(channel); + interrupt_unlock(save); + return DRV_RC_OK; + } + } + + // Somehow, we got an item from the semaphore but found no free channels... + interrupt_unlock(save); + + return DRV_RC_FAIL; +} + +DRIVER_API_RC soc_dma_release(struct soc_dma_channel *channel) +{ + + if (channel->active) + { + return DRV_RC_CONTROLLER_IN_USE; + } + + // Deconfig the channel if still configured + if (channel->cfgd) + { + soc_dma_deconfig(channel); + } + + // Clear the channel + dma_info->channel[channel->id] = NULL; + + return DRV_RC_OK; +} + +DRIVER_API_RC soc_dma_start_transfer(struct soc_dma_channel *channel) +{ + uint32_t save; + + if (!channel->cfgd) + { + return DRV_RC_FAIL; + } + + // Setup the control register for first "transfer" + SETB(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, dma_regs[channel->id].CTL_L), SOC_DMA_CTL_L_LLP_SRC_EN); + SETB(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, dma_regs[channel->id].CTL_L), SOC_DMA_CTL_L_LLP_DST_EN); + + SETB(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, dma_regs[channel->id].CTL_L), SOC_DMA_CTL_L_INT_EN); + + SETV(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, dma_regs[channel->id].CTL_L), + SOC_DMA_CTL_L_TT_FC, SOC_DMA_CTL_L_TT_FC_LEN, channel->cfg.type); + + // Setup LLP register to point to first block + SETV(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, dma_regs[channel->id].LLP), + SOC_DMA_LLP_LOC, SOC_DMA_LLP_LOC_LEN, ((uint32_t)(channel->ll)) >> SOC_DMA_LLP_LOC); + + // Set the current LLI pointer to point at the head of the list + channel->curr = channel->ll; + + save = interrupt_lock(); + dma_enable(channel); + interrupt_unlock(save); + + return DRV_RC_OK; +} + +DRIVER_API_RC soc_dma_stop_transfer(struct soc_dma_channel *channel) +{ + uint32_t save; + + save = interrupt_lock(); + dma_disable(channel); + interrupt_unlock(save); + + return DRV_RC_OK; +} + +DRIVER_API_RC soc_dma_alloc_list_item(struct soc_dma_xfer_item **ret, struct soc_dma_xfer_item *base) +{ + OS_ERR_TYPE err; + + *ret = balloc(sizeof(struct soc_dma_xfer_item), &err); + + if (*ret == NULL) + { + *ret = NULL; + return DRV_RC_FAIL; + } + + // Copy and connect to the base if one is given + if (base) + { + **ret = *base; + base->next = (*ret); + } + + return DRV_RC_OK; +} + +DRIVER_API_RC soc_dma_free_list(struct soc_dma_cfg *cfg) +{ + struct soc_dma_xfer_item *p; + struct soc_dma_xfer_item *np; + struct soc_dma_xfer_item *lp; + + p = cfg->xfer.next; + lp = dma_find_list_end(&(cfg->xfer)); + + while (p) + { + if (p->next == lp) + { + bfree(p); + p = NULL; + } + else + { + np = p->next; + bfree(p); + p = np; + } + } + + cfg->xfer.next = NULL; + + return DRV_RC_OK; +} + +/* Driver API */ +DRIVER_API_RC soc_dma_init() +{ + struct soc_dma_channel ch; + + dma_info->active = 0; + + // Enable global clock + SETB(MMIO_REG_VAL(MLAYER_AHB_CTL), 6); + + // Setup ISRs (and enable) + for (int i = 0; i < SOC_DMA_NUM_CHANNELS; i++) + { + SET_INTERRUPT_HANDLER(dma_info->int_vector[i], dma_info->int_handler[i]); + SOC_UNMASK_INTERRUPTS(dma_info->int_mask[i]); + } + SET_INTERRUPT_HANDLER(dma_info->err_vector, &dma_interrupt_err_handler); + SOC_UNMASK_INTERRUPTS(dma_info->err_mask); + + // Setup global DMA registers settings + SETB(MMIO_REG_VAL_FROM_BASE(SOC_DMA_BASE, SOC_DMA_DMA_CFG_REG), SOC_DMA_DMA_CFG_REG_DMA_EN); + + // Disable all channels for now and leave the channel array empty + for (int i = 0; i < SOC_DMA_NUM_CHANNELS; i++) + { + ch.id = i; + dma_disable(&ch); + dma_info->channel[i] = NULL; + } + + return DRV_RC_OK; +} + diff --git a/system/libarc32_arduino101/drivers/soc_dma.h b/system/libarc32_arduino101/drivers/soc_dma.h new file mode 100644 index 00000000..e81d071b --- /dev/null +++ b/system/libarc32_arduino101/drivers/soc_dma.h @@ -0,0 +1,239 @@ +/* + * Copyright (c) 2016, Intel Corporation. All rights reserved. + * + * 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 of the copyright holder 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 HOLDER 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 SOC_DMA_H_ +#define SOC_DMA_H_ + +#include "data_type.h" +#include "os/os.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define SOC_DMA_NUM_CHANNELS 8 + +// Valid values for type +#define SOC_DMA_TYPE_MEM2MEM 0 +#define SOC_DMA_TYPE_MEM2PER 1 +#define SOC_DMA_TYPE_PER2MEM 2 +#define SOC_DMA_TYPE_PER2PER 3 + +// Valid values for delta +#define SOC_DMA_DELTA_INCR 0 +#define SOC_DMA_DELTA_DECR 1 +#define SOC_DMA_DELTA_NONE 2 + +// Valid values for width +#define SOC_DMA_WIDTH_8 0 +#define SOC_DMA_WIDTH_16 1 +#define SOC_DMA_WIDTH_32 2 + +// Valid values for dest_interface or src_interface +#define SOC_DMA_INTERFACE_UART0_TX 0 +#define SOC_DMA_INTERFACE_UART0_RX 1 +#define SOC_DMA_INTERFACE_UART1_TX 2 +#define SOC_DMA_INTERFACE_UART1_RX 3 +#define SOC_DMA_INTERFACE_SPIM0_TX 4 +#define SOC_DMA_INTERFACE_SPIM0_RX 5 +#define SOC_DMA_INTERFACE_SPIM1_TX 6 +#define SOC_DMA_INTERFACE_SPIM1_RX 7 +#define SOC_DMA_INTERFACE_SPIS_TX 8 +#define SOC_DMA_INTERFACE_SPIS_RX 9 +#define SOC_DMA_INTERFACE_I2S_TX 10 +#define SOC_DMA_INTERFACE_I2S_RX 11 +#define SOC_DMA_INTERFACE_I2C0_TX 12 +#define SOC_DMA_INTERFACE_I2C0_RX 13 +#define SOC_DMA_INTERFACE_I2C1_TX 14 +#define SOC_DMA_INTERFACE_I2C1_RX 15 + +// Part of the list item that is the same from destination and source +struct soc_dma_xfer_part { + uint8_t delta; + uint8_t width; + void *addr; +}; + +// DMa transfer list item +struct soc_dma_xfer_item { + struct soc_dma_xfer_part src; + struct soc_dma_xfer_part dest; + struct soc_dma_xfer_item *next; + uint16_t size; +}; + +// DMA configuration object +struct soc_dma_cfg { + uint8_t type; + + uint8_t dest_interface; + uint8_t src_interface; + uint8_t dest_step_count; + uint8_t src_step_count; + uint32_t dest_step_interval; + uint32_t src_step_interval; + + struct soc_dma_xfer_item xfer; + + void (*cb_done)(void *); + void (*cb_block)(void *); + void (*cb_err)(void *); + void *cb_done_arg; + void *cb_block_arg; + void *cb_err_arg; +}; + +// DMA channel object +struct soc_dma_channel { + uint8_t id; + uint8_t active; + uint8_t cfgd; + struct soc_dma_cfg cfg; + void *ll; + void *curr; +}; + +typedef void (*isr_func)(void); + +// Internal struct for use by the controller (and soc_config) +struct soc_dma_info { + uint32_t int_mask[SOC_DMA_NUM_CHANNELS]; + uint32_t int_vector[SOC_DMA_NUM_CHANNELS]; + isr_func int_handler[SOC_DMA_NUM_CHANNELS]; + uint32_t err_mask; + uint32_t err_vector; + + struct soc_dma_channel* channel[SOC_DMA_NUM_CHANNELS]; + + uint32_t active; +}; + + +/** + * Function to configure a DMA channel (and set it up for transfer) + * + * @param channel : pointer to channel object + * @param cfg : pointer to configuration by which to configure the channel + * + * @return + * - DRV_RC_OK on success + * - DRV_RC_INVALID_CONFIG - if any configuration parameters are not valid + * - DRV_RC_CONTROLLER_IN_USE, - if controller is in use + * - DRV_RC_FAIL otherwise + */ +DRIVER_API_RC soc_dma_config(struct soc_dma_channel *channel, struct soc_dma_cfg *cfg); + +/** + * Function to deconfigure and disable a DMA channel + * + * @param channel : pointer to channel object + * + * @return + * - DRV_RC_OK on success + * - DRV_RC_CONTROLLER_IN_USE, - if controller is in use + * - DRV_RC_FAIL otherwise + */ +DRIVER_API_RC soc_dma_deconfig(struct soc_dma_channel *channel); + +/** + * Function to acquire a free DMA channel, must be called before any other DMA functions are used + * + * @param channel : pointer to channel object + * + * @return + * - DRV_RC_OK on success + * - DRV_RC_FAIL otherwise + */ +DRIVER_API_RC soc_dma_acquire(struct soc_dma_channel *channel); + +/** + * Function to release a held DMA channel so it can be used by others + * + * @param channel : pointer to channel object + * + * @return + * - DRV_RC_OK on success + * - DRV_RC_CONTROLLER_IN_USE, - if controller is in use + * - DRV_RC_FAIL otherwise + */ +DRIVER_API_RC soc_dma_release(struct soc_dma_channel *channel); + +/** + * Function to start a DMA transfer, the channel must be configured before this function is called + * + * @param channel : pointer to channel object + * + * @return + * - DRV_RC_OK on success + * - DRV_RC_FAIL otherwise + */ +DRIVER_API_RC soc_dma_start_transfer(struct soc_dma_channel *channel); + +/** + * Function to stop an ongoing DMA channel, does nothing if no transfer is in progress + * + * @param channel : pointer to channel object + * + * @return + * - DRV_RC_OK on success + */ +DRIVER_API_RC soc_dma_stop_transfer(struct soc_dma_channel *channel); + +/** + * Function to create a new node for a DMA xfer list. If base is provided, the allocated item will + * inherit all the fields from base and the new item will be linked as the item after base + * + * @param ret : pointer to pointer which will be assigned to the new list item + * @param base : pointer to list item to be used as a base + * + * @return + * - DRV_RC_OK on success + * - DRV_RC_FAIL otherwise + */ +DRIVER_API_RC soc_dma_alloc_list_item(struct soc_dma_xfer_item **ret, struct soc_dma_xfer_item *base); + +/** + * Function to free a DMA xfer list (given in the configuration object), should only be used if the + * list was allocated with soc_dma_alloc_list_item + * + * @param cfg : pointer to configuration object + * + * @return + * - DRV_RC_OK on success + */ +DRIVER_API_RC soc_dma_free_list(struct soc_dma_cfg *cfg); + +DRIVER_API_RC soc_dma_init(); + +#ifdef __cplusplus +} +#endif + +#endif /* SOC_DMA_H_ */ diff --git a/system/libarc32_arduino101/drivers/soc_dma_priv.h b/system/libarc32_arduino101/drivers/soc_dma_priv.h new file mode 100644 index 00000000..5497e3a0 --- /dev/null +++ b/system/libarc32_arduino101/drivers/soc_dma_priv.h @@ -0,0 +1,339 @@ +/* + * Copyright (c) 2016, Intel Corporation. All rights reserved. + * + * 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 of the copyright holder 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 HOLDER 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 SOC_DMA_PRIV_H_ +#define SOC_DMA_PRIV_H_ + +#include "soc_dma.h" + +// Timeout on acquiring the DMA semaphore +#define SOC_DMA_SEMAPHORE_DELAY 1000 + +// Largest size a single DMA transfer can be (limited by BLOCK_TS bits in CTL_H) +#define SOC_DMA_BLOCK_SIZE_MAX 4095 + +// DMA Register map and fields +#define SOC_DMA_SAR_0 (0x000) +#define SOC_DMA_SAR_1 (0x058) +#define SOC_DMA_SAR_2 (0x0B0) +#define SOC_DMA_SAR_3 (0x108) +#define SOC_DMA_SAR_4 (0x160) +#define SOC_DMA_SAR_5 (0x1B8) +#define SOC_DMA_SAR_6 (0x210) +#define SOC_DMA_SAR_7 (0x268) +#define SOC_DMA_SAR_SAR (0) +#define SOC_DMA_SAR_SAR_LEN (32) + +#define SOC_DMA_DAR_0 (0x008) +#define SOC_DMA_DAR_1 (0x060) +#define SOC_DMA_DAR_2 (0x0B8) +#define SOC_DMA_DAR_3 (0x110) +#define SOC_DMA_DAR_4 (0x168) +#define SOC_DMA_DAR_5 (0x1C0) +#define SOC_DMA_DAR_6 (0x218) +#define SOC_DMA_DAR_7 (0x270) +#define SOC_DMA_DAR_DAR (0) +#define SOC_DMA_DAR_DAR_LEN (32) + +#define SOC_DMA_LLP_0 (0x010) +#define SOC_DMA_LLP_1 (0x068) +#define SOC_DMA_LLP_2 (0x0C0) +#define SOC_DMA_LLP_3 (0x118) +#define SOC_DMA_LLP_4 (0x170) +#define SOC_DMA_LLP_5 (0x1C8) +#define SOC_DMA_LLP_6 (0x220) +#define SOC_DMA_LLP_7 (0x278) +#define SOC_DMA_LLP_LOC (2) +#define SOC_DMA_LLP_LOC_LEN (30) + +#define SOC_DMA_CTL_L_0 (0x018) +#define SOC_DMA_CTL_L_1 (0x070) +#define SOC_DMA_CTL_L_2 (0x0C8) +#define SOC_DMA_CTL_L_3 (0x120) +#define SOC_DMA_CTL_L_4 (0x178) +#define SOC_DMA_CTL_L_5 (0x1D0) +#define SOC_DMA_CTL_L_6 (0x228) +#define SOC_DMA_CTL_L_7 (0x280) +#define SOC_DMA_CTL_L_LLP_SRC_EN (28) +#define SOC_DMA_CTL_L_LLP_DST_EN (27) +#define SOC_DMA_CTL_L_TT_FC (20) +#define SOC_DMA_CTL_L_TT_FC_LEN (3) +#define SOC_DMA_CTL_L_DST_SCATTER_EN (18) +#define SOC_DMA_CTL_L_SRC_GATHER_EN (17) +#define SOC_DMA_CTL_L_SRC_MSIZE (14) +#define SOC_DMA_CTL_L_SRC_MSIZE_LEN (3) +#define SOC_DMA_CTL_L_DEST_MSIZE (11) +#define SOC_DMA_CTL_L_DEST_MSIZE_LEN (3) +#define SOC_DMA_CTL_L_SINC (9) +#define SOC_DMA_CTL_L_SINC_LEN (2) +#define SOC_DMA_CTL_L_DINC (7) +#define SOC_DMA_CTL_L_DINC_LEN (2) +#define SOC_DMA_CTL_L_SRC_TR_WIDTH (4) +#define SOC_DMA_CTL_L_SRC_TR_WIDTH_LEN (3) +#define SOC_DMA_CTL_L_DST_TR_WIDTH (1) +#define SOC_DMA_CTL_L_DST_TR_WIDTH_LEN (3) +#define SOC_DMA_CTL_L_INT_EN (0) + +#define SOC_DMA_CTL_U_0 (0x01C) +#define SOC_DMA_CTL_U_1 (0x074) +#define SOC_DMA_CTL_U_2 (0x0CC) +#define SOC_DMA_CTL_U_3 (0x124) +#define SOC_DMA_CTL_U_4 (0x17C) +#define SOC_DMA_CTL_U_5 (0x1D4) +#define SOC_DMA_CTL_U_6 (0x22C) +#define SOC_DMA_CTL_U_7 (0x284) +#define SOC_DMA_CTL_U_DONE_BIT (12) +#define SOC_DMA_CTL_U_BLOCK_TS (0) +#define SOC_DMA_CTL_U_BLOCK_TS_LEN (12) + +#define SOC_DMA_CFG_L_0 (0x040) +#define SOC_DMA_CFG_L_1 (0x098) +#define SOC_DMA_CFG_L_2 (0x0F0) +#define SOC_DMA_CFG_L_3 (0x148) +#define SOC_DMA_CFG_L_4 (0x1A0) +#define SOC_DMA_CFG_L_5 (0x1F8) +#define SOC_DMA_CFG_L_6 (0x250) +#define SOC_DMA_CFG_L_7 (0x2A8) +#define SOC_DMA_CFG_L_RELOAD_DST (31) +#define SOC_DMA_CFG_L_RELOAD_SRC (30) +#define SOC_DMA_CFG_L_SRC_HS_POL (19) +#define SOC_DMA_CFG_L_DST_HS_POL (18) +#define SOC_DMA_CFG_L_HS_SEL_SRC (11) +#define SOC_DMA_CFG_L_HS_SEL_DST (10) +#define SOC_DMA_CFG_L_FIFO_EMPTY (9) +#define SOC_DMA_CFG_L_CH_SUSP (8) +#define SOC_DMA_CFG_L_CH_PRIOR (5) +#define SOC_DMA_CFG_L_CH_PRIOR_LEN (3) + +#define SOC_DMA_CFG_U_0 (0x044) +#define SOC_DMA_CFG_U_1 (0x09C) +#define SOC_DMA_CFG_U_2 (0x0F4) +#define SOC_DMA_CFG_U_3 (0x14C) +#define SOC_DMA_CFG_U_4 (0x1A4) +#define SOC_DMA_CFG_U_5 (0x1FC) +#define SOC_DMA_CFG_U_6 (0x254) +#define SOC_DMA_CFG_U_7 (0x2AC) +#define SOC_DMA_CFG_U_DEST_PER (11) +#define SOC_DMA_CFG_U_DEST_PER_LEN (4) +#define SOC_DMA_CFG_U_SRC_PER (7) +#define SOC_DMA_CFG_U_SRC_PER_LEN (4) +#define SOC_DMA_CFG_U_SS_UPD_EN (6) +#define SOC_DMA_CFG_U_DS_UPD_EN (5) +#define SOC_DMA_CFG_U_PROTCTL (2) +#define SOC_DMA_CFG_U_PROTCTL_LEN (3) +#define SOC_DMA_CFG_U_FIFO_MODE (1) +#define SOC_DMA_CFG_U_FCMODE (0) + +#define SOC_DMA_SGR_0 (0x048) +#define SOC_DMA_SGR_1 (0x0A0) +#define SOC_DMA_SGR_2 (0x0F8) +#define SOC_DMA_SGR_3 (0x150) +#define SOC_DMA_SGR_4 (0x1A8) +#define SOC_DMA_SGR_5 (0x200) +#define SOC_DMA_SGR_6 (0x258) +#define SOC_DMA_SGR_7 (0x2B0) +#define SOC_DMA_SGR_SGC (20) +#define SOC_DMA_SGR_SGC_LEN (5) +#define SOC_DMA_SGR_SGI (0) +#define SOC_DMA_SGR_SGI_LEN (20) + +#define SOC_DMA_DSR_0 (0x050) +#define SOC_DMA_DSR_1 (0x0A8) +#define SOC_DMA_DSR_2 (0x100) +#define SOC_DMA_DSR_3 (0x158) +#define SOC_DMA_DSR_4 (0x1B0) +#define SOC_DMA_DSR_5 (0x208) +#define SOC_DMA_DSR_6 (0x260) +#define SOC_DMA_DSR_7 (0x2B8) +#define SOC_DMA_DSR_DSC (20) +#define SOC_DMA_DSR_DSC_LEN (5) +#define SOC_DMA_DSR_DSI (0) +#define SOC_DMA_DSR_DSI_LEN (20) + +#define SOC_DMA_RAW_TFR (0x2C0) +#define SOC_DMA_RAW_BLOCK (0x2C8) +#define SOC_DMA_RAW_SRC_TRAN (0x2D0) +#define SOC_DMA_RAW_DST_TRAN (0x2D8) +#define SOC_DMA_RAW_ERR (0x2E0) +#define SOC_DMA_RAW_RAW (0) + +#define SOC_DMA_STATUS_TFR (0x2E8) +#define SOC_DMA_STATUS_BLOCK (0x2F0) +#define SOC_DMA_STATUS_SRC_TRAN (0x2F8) +#define SOC_DMA_STATUS_DST_TRAN (0x300) +#define SOC_DMA_STATUS_ERR (0x308) +#define SOC_DMA_STATUS_STATUS (0) + +#define SOC_DMA_MASK_TFR (0x310) +#define SOC_DMA_MASK_BLOCK (0x318) +#define SOC_DMA_MASK_SRC_TRAN (0x320) +#define SOC_DMA_MASK_DST_TRAN (0x328) +#define SOC_DMA_MASK_ERR (0x330) +#define SOC_DMA_MASK_INT_MASK_WE (8) +#define SOC_DMA_MASK_INT_MASK (0) + +#define SOC_DMA_CLEAR_TFR (0x338) +#define SOC_DMA_CLEAR_BLOCK (0x340) +#define SOC_DMA_CLEAR_SRC_TRAN (0x348) +#define SOC_DMA_CLEAR_DST_TRAN (0x350) +#define SOC_DMA_CLEAR_ERR (0x358) +#define SOC_DMA_CLEAR_CLEAR (0) + +#define SOC_DMA_DMA_CFG_REG (0x398) +#define SOC_DMA_DMA_CFG_REG_DMA_EN (0) + +#define SOC_DMA_CH_EN_REG (0x3A0) +#define SOC_DMA_CH_EN_REG_CH_EN_WE (8) +#define SOC_DMA_CH_EN_REG_CH_EN (0) + +// DMA Register map struct, used to refer to a channel's specific registers by base name and id number +struct dma_reg_map { + uint16_t SAR; + uint16_t DAR; + + uint16_t LLP; + + uint16_t CTL_L; + + uint16_t CTL_U; + + uint16_t CFG_L; + + uint16_t CFG_U; + + uint16_t SGR; + + uint16_t DSR; +}; + +struct dma_reg_map dma_regs[SOC_DMA_NUM_CHANNELS] = { + { + .SAR = SOC_DMA_SAR_0, + .DAR = SOC_DMA_DAR_0, + .LLP = SOC_DMA_LLP_0, + .CTL_L = SOC_DMA_CTL_L_0, + .CTL_U = SOC_DMA_CTL_U_0, + .CFG_L = SOC_DMA_CFG_L_0, + .CFG_U = SOC_DMA_CFG_U_0, + .SGR = SOC_DMA_SGR_0, + .DSR = SOC_DMA_DSR_0 + }, + { + .SAR = SOC_DMA_SAR_1, + .DAR = SOC_DMA_DAR_1, + .LLP = SOC_DMA_LLP_1, + .CTL_L = SOC_DMA_CTL_L_1, + .CTL_U = SOC_DMA_CTL_U_1, + .CFG_L = SOC_DMA_CFG_L_1, + .CFG_U = SOC_DMA_CFG_U_1, + .SGR = SOC_DMA_SGR_1, + .DSR = SOC_DMA_DSR_1 + }, + { + .SAR = SOC_DMA_SAR_2, + .DAR = SOC_DMA_DAR_2, + .LLP = SOC_DMA_LLP_2, + .CTL_L = SOC_DMA_CTL_L_2, + .CTL_U = SOC_DMA_CTL_U_2, + .CFG_L = SOC_DMA_CFG_L_2, + .CFG_U = SOC_DMA_CFG_U_2, + .SGR = SOC_DMA_SGR_2, + .DSR = SOC_DMA_DSR_2 + }, + { + .SAR = SOC_DMA_SAR_3, + .DAR = SOC_DMA_DAR_3, + .LLP = SOC_DMA_LLP_3, + .CTL_L = SOC_DMA_CTL_L_3, + .CTL_U = SOC_DMA_CTL_U_3, + .CFG_L = SOC_DMA_CFG_L_3, + .CFG_U = SOC_DMA_CFG_U_3, + .SGR = SOC_DMA_SGR_3, + .DSR = SOC_DMA_DSR_3 + }, + { + .SAR = SOC_DMA_SAR_4, + .DAR = SOC_DMA_DAR_4, + .LLP = SOC_DMA_LLP_4, + .CTL_L = SOC_DMA_CTL_L_4, + .CTL_U = SOC_DMA_CTL_U_4, + .CFG_L = SOC_DMA_CFG_L_4, + .CFG_U = SOC_DMA_CFG_U_4, + .SGR = SOC_DMA_SGR_4, + .DSR = SOC_DMA_DSR_4 + }, + { + .SAR = SOC_DMA_SAR_5, + .DAR = SOC_DMA_DAR_5, + .LLP = SOC_DMA_LLP_5, + .CTL_L = SOC_DMA_CTL_L_5, + .CTL_U = SOC_DMA_CTL_U_5, + .CFG_L = SOC_DMA_CFG_L_5, + .CFG_U = SOC_DMA_CFG_U_5, + .SGR = SOC_DMA_SGR_5, + .DSR = SOC_DMA_DSR_5 + }, + { + .SAR = SOC_DMA_SAR_6, + .DAR = SOC_DMA_DAR_6, + .LLP = SOC_DMA_LLP_6, + .CTL_L = SOC_DMA_CTL_L_6, + .CTL_U = SOC_DMA_CTL_U_6, + .CFG_L = SOC_DMA_CFG_L_6, + .CFG_U = SOC_DMA_CFG_U_6, + .SGR = SOC_DMA_SGR_6, + .DSR = SOC_DMA_DSR_6 + }, + { + .SAR = SOC_DMA_SAR_7, + .DAR = SOC_DMA_DAR_7, + .LLP = SOC_DMA_LLP_7, + .CTL_L = SOC_DMA_CTL_L_7, + .CTL_U = SOC_DMA_CTL_U_7, + .CFG_L = SOC_DMA_CFG_L_7, + .CFG_U = SOC_DMA_CFG_U_7, + .SGR = SOC_DMA_SGR_7, + .DSR = SOC_DMA_DSR_7 + } +}; + +// DMA Link List Item as specified by the DW DMAC doc; end_group is special field used by controller +struct dma_lli { + uint32_t sar; + uint32_t dar; + uint32_t llp; + uint32_t ctl_l; + uint32_t ctl_u; + uint32_t dstat; + uint32_t sstat; + uint8_t end_group; +}; + +#endif /* SOC_DMA_PRIV_H_ */ diff --git a/system/libarc32_arduino101/drivers/soc_i2s.c b/system/libarc32_arduino101/drivers/soc_i2s.c new file mode 100644 index 00000000..70b51429 --- /dev/null +++ b/system/libarc32_arduino101/drivers/soc_i2s.c @@ -0,0 +1,672 @@ +/* + * Copyright (c) 2016, Intel Corporation. All rights reserved. + * + * 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 of the copyright holder 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 HOLDER 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. + */ + +#include "scss_registers.h" +#include "portable.h" +#include "os/os.h" +#include "i2s_priv.h" +#include "data_type.h" +#include "clk_system.h" +#include "soc_i2s.h" + +/* Info struct */ +struct soc_i2s_info g_i2s_info = { + .reg_base = SOC_I2S_BASE, + .int_vector = SOC_I2S_INTERRUPT, + .int_mask = INT_I2S_MASK, + .clk_speed = 32000000, + .clk_gate_info = &(struct clk_gate_info_s) + { + .clk_gate_register = PERIPH_CLK_GATE_CTRL, + .bits_mask = I2S_CLK_GATE_MASK, + } +}; + +struct soc_i2s_info* i2s_info = &g_i2s_info; + +/* Function Prototypes */ +static void i2s_enable(uint8_t channel); +static void i2s_disable(uint8_t channel); +static void i2s_isr(void); +DRIVER_API_RC soc_i2s_config(uint8_t channel, struct soc_i2s_cfg *cfg); +DRIVER_API_RC soc_i2s_deconfig(uint8_t channel); +DRIVER_API_RC soc_i2s_read(uint32_t *buf, uint32_t len); +DRIVER_API_RC soc_i2s_listen(uint32_t *buf, uint32_t len, uint8_t num_bufs); +DRIVER_API_RC soc_i2s_stop_listen(void); +DRIVER_API_RC soc_i2s_write(uint32_t *buf, uint32_t len); +DRIVER_API_RC soc_i2s_stream(uint32_t *buf, uint32_t len, uint32_t num_bufs); +DRIVER_API_RC soc_i2s_stop_stream(void); +DRIVER_API_RC soc_i2s_init(); + +/* Internal functions */ +static void i2s_enable(uint8_t channel) +{ + uint32_t reg; + + // Enable local clock and interrupts + reg = MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, i2s_reg_map[channel].cid_ctrl); + reg &= ~(1 << (i2s_reg_map[channel].cid_ctrl_strobe)); + reg &= ~(1 << (i2s_reg_map[channel].cid_ctrl_strobe_sync)); + reg |= (1 << (i2s_reg_map[channel].cid_ctrl_mask)); + MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, i2s_reg_map[channel].cid_ctrl) = reg; + + // Clear all interrupts + reg = MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, i2s_reg_map[channel].stat); + reg &= ~(i2s_reg_map[channel].stat_mask); + reg |= (1 << SOC_I2S_STAT_TDATA_UNDERR); + reg |= (1 << SOC_I2S_STAT_RDATA_OVRERR); + MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, i2s_reg_map[channel].stat) = reg; + + // Set enabled flag for channel + i2s_info->en[channel] = 1; + + return; +} + +static void i2s_disable(uint8_t channel) +{ + uint32_t reg; + uint32_t num_active; + int i; + + // Release DMA resources + if (i2s_info->en[channel]) + { + soc_dma_stop_transfer(&(i2s_info->dma_ch[channel])); + soc_dma_release(&(i2s_info->dma_ch[channel])); + soc_dma_free_list(&(i2s_info->dma_cfg[channel])); + } + // Clear enabled flag for channel + i2s_info->en[channel] = 0; + + // Let the processor do whatever power down it wants + num_active = 0; + for (i = 0; i < I2S_NUM_CHANNELS; i++) + { + if (i2s_info->en[i]) + { + num_active++; + } + } + + // Disable channel and hold parts in reset + reg = MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, i2s_reg_map[channel].ctrl); + reg &= ~(1 << (i2s_reg_map[channel].ctrl_fifo_rst)); + reg &= ~(1 << (i2s_reg_map[channel].ctrl_sync_rst)); + MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, i2s_reg_map[channel].ctrl) = reg; + + // Clear all interrupts + reg = MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, i2s_reg_map[channel].stat); + reg &= ~(i2s_reg_map[channel].stat_mask); + reg &= ~(1 << i2s_reg_map[channel].stat_err); + MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, i2s_reg_map[channel].stat) = reg; + + // Disable local clock and interrupts + reg = MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, i2s_reg_map[channel].cid_ctrl); + reg |= (1 << (i2s_reg_map[channel].cid_ctrl_strobe)); + reg |= (1 << (i2s_reg_map[channel].cid_ctrl_strobe_sync)); + reg &= ~(1 << (i2s_reg_map[channel].cid_ctrl_mask)); + MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, i2s_reg_map[channel].cid_ctrl) = reg; + + return; + +} + +/* ISR */ +static void i2s_isr(void) +{ + uint32_t stat; + + // Determine interrupt source + stat = MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, SOC_I2S_STAT); + + // Check for errors + if (stat & (1 << SOC_I2S_STAT_TDATA_UNDERR)) + { + if (i2s_info->cfg[I2S_CHANNEL_TX].cb_err) + { + i2s_info->cfg[I2S_CHANNEL_TX].cb_err_arg=(void *)stat; + i2s_info->cfg[I2S_CHANNEL_TX].cb_err(i2s_info->cfg[I2S_CHANNEL_TX].cb_err_arg); + } + i2s_disable(I2S_CHANNEL_TX); + } + if (stat & (1 << SOC_I2S_STAT_RDATA_OVRERR)) + { + if (i2s_info->cfg[I2S_CHANNEL_RX].cb_err) + { + i2s_info->cfg[I2S_CHANNEL_RX].cb_err_arg=(void *)stat; + i2s_info->cfg[I2S_CHANNEL_RX].cb_err(i2s_info->cfg[I2S_CHANNEL_RX].cb_err_arg); + } + i2s_disable(I2S_CHANNEL_RX); + } + + return; +} + +DECLARE_INTERRUPT_HANDLER static void i2s_interrupt_handler() +{ + i2s_isr(); +} + +/* DMA Callbacks */ +static void i2s_dma_cb_err(void *num) +{ + uint8_t channel = (uint32_t)num; + + if (i2s_info->cfg[channel].cb_err) + { + i2s_info->cfg[channel].cb_err(i2s_info->cfg[channel].cb_err_arg); + } + i2s_disable(channel); + + return; +} + +static void i2s_dma_cb_done(void *num) +{ + uint8_t channel = (uint32_t)num; + uint32_t reg; + + if(channel == I2S_CHANNEL_TX) + { + if((0x00200000 & MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, SOC_I2S_CTRL)) + && !(0x18000000 & MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, SOC_I2S_CTRL))) + { + for(int i = 0; i < 4; ++i) + MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, SOC_I2S_DATA_REG) = 0x0; + } + + do + { + reg = MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, i2s_reg_map[channel].fifo_stat); + } while(reg & 0x000000FF); + } + + if (i2s_info->cfg[channel].cb_done) + { + i2s_info->cfg[channel].cb_done(i2s_info->cfg[channel].cb_done_arg); + } + + i2s_disable(channel); + + return; +} + +static void i2s_dma_cb_block(void *num) +{ + uint8_t channel = (uint32_t)num; + + if (i2s_info->cfg[channel].cb_done) + { + i2s_info->cfg[channel].cb_done(i2s_info->cfg[channel].cb_done_arg); + } + + return; +} + +/* External API */ +DRIVER_API_RC soc_i2s_config(uint8_t channel, struct soc_i2s_cfg *cfg) +{ + uint32_t reg; + uint16_t sample_rate; + + // Check channel no in use + if (channel >= I2S_NUM_CHANNELS) + { + return DRV_RC_FAIL; + } + else if (i2s_info->en[channel]) + { + return DRV_RC_CONTROLLER_IN_USE; + } + + // Set master/slave + reg = MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, i2s_reg_map[channel].ctrl); + reg &= ~(1 << (i2s_reg_map[channel].ctrl_ms)); + reg |= (cfg->master & 0x1) << i2s_reg_map[channel].ctrl_ms; + MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, i2s_reg_map[channel].ctrl) = reg; + + // Calculate sample_rate divider (note, acts as if resolution is always 32) + sample_rate = i2s_info->clk_speed / (cfg->sample_rate * cfg->resolution * 2); + + // Setup resolution and sampling rate + reg = MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, i2s_reg_map[channel].srr); + reg &= ~(i2s_reg_map[channel].srr_mask); + reg |= (sample_rate & 0x7FF) << i2s_reg_map[channel].srr_sample_rate; + reg |= ((cfg->resolution - 1) & 0x1F) << i2s_reg_map[channel].srr_resolution; + MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, i2s_reg_map[channel].srr) = reg; + + // Setup mode + reg = MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, i2s_reg_map[channel].dev_conf); + reg &= ~(i2s_reg_map[channel].dev_conf_mask); + // Use sck_polar as shift amount as its the LSb of the DEV_CONF settings + reg |= ((cfg->mode & 0x3F) << i2s_reg_map[channel].dev_conf_sck_polar); + MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, i2s_reg_map[channel].dev_conf) = reg; + + // Complete configuration (and set flag) + i2s_info->cfg[channel] = *cfg; + i2s_info->cfgd[channel] = 1; + + return DRV_RC_OK; +} + +DRIVER_API_RC soc_i2s_deconfig(uint8_t channel) +{ + // Check channel no in use + if (channel >= I2S_NUM_CHANNELS) + { + return DRV_RC_FAIL; + } + else if (i2s_info->en[channel]) + { + return DRV_RC_CONTROLLER_IN_USE; + } + + i2s_info->cfgd[channel] = 0; + + return DRV_RC_OK; +} + +DRIVER_API_RC soc_i2s_read(uint32_t *buf, uint32_t len) +{ + // Calling listen with 0 buffers is the same as a onetime read of the whole buffer + return soc_i2s_listen(buf, len, 0); +} + +DRIVER_API_RC soc_i2s_listen(uint32_t *buf, uint32_t len, uint8_t num_bufs) +{ + DRIVER_API_RC ret; + uint8_t channel = I2S_CHANNEL_RX; + uint32_t reg; + uint32_t len_per_buf; + int i; + struct soc_dma_xfer_item *dma_list; + + // Check channel no in use and configured + if (channel >= I2S_NUM_CHANNELS) + { + return DRV_RC_FAIL; + } + else if (i2s_info->en[channel] || !(i2s_info->cfgd[channel])) + { + return DRV_RC_FAIL; + } + + // Get a DMA channel + ret = soc_dma_acquire(&(i2s_info->dma_ch[channel])); + + if (ret != DRV_RC_OK) + { + return DRV_RC_FAIL; + } + + // Enable the channel + i2s_enable(channel); + + // Determine the length of a single buffer + if (num_bufs == 0) + { + len_per_buf = len; + } + else + { + len_per_buf = len / num_bufs; + } + + // Prep some configuration + i2s_info->dma_cfg[channel].type = SOC_DMA_TYPE_PER2MEM; + i2s_info->dma_cfg[channel].src_interface = SOC_DMA_INTERFACE_I2S_RX; + i2s_info->dma_cfg[channel].dest_step_count = 0; + i2s_info->dma_cfg[channel].src_step_count = 0; + + i2s_info->dma_cfg[channel].xfer.dest.delta = SOC_DMA_DELTA_INCR; + i2s_info->dma_cfg[channel].xfer.dest.width = SOC_DMA_WIDTH_32; + i2s_info->dma_cfg[channel].xfer.src.delta = SOC_DMA_DELTA_NONE; + i2s_info->dma_cfg[channel].xfer.src.width = SOC_DMA_WIDTH_32; + i2s_info->dma_cfg[channel].xfer.src.addr = (void *)(SOC_I2S_BASE + SOC_I2S_DATA_REG); + + if (num_bufs == 0) + { + i2s_info->dma_cfg[channel].cb_done = i2s_dma_cb_done; + i2s_info->dma_cfg[channel].cb_done_arg = (void *)((uint32_t)channel); + } + else + { + i2s_info->dma_cfg[channel].cb_block = i2s_dma_cb_block; + i2s_info->dma_cfg[channel].cb_block_arg = (void *)((uint32_t)channel); + } + + i2s_info->dma_cfg[channel].cb_err = i2s_dma_cb_err; + i2s_info->dma_cfg[channel].cb_err_arg = (void *)((uint32_t)channel); + + // Setup the linked list + for (i = 0; i < ((num_bufs == 0) ? 1 : num_bufs); i++) + { + if (i == 0) + { + dma_list = &(i2s_info->dma_cfg[channel].xfer); + } + else + { + ret = soc_dma_alloc_list_item(&dma_list, dma_list); + + if (ret != DRV_RC_OK) + { + goto fail; + } + } + + dma_list->dest.addr = (void *)(&(buf[i * (len_per_buf / sizeof(uint32_t))])); + dma_list->size = len_per_buf / sizeof(uint32_t); + } + + // Create a circular list if we are doing circular buffering + if (num_bufs != 0) + { + dma_list->next = &(i2s_info->dma_cfg[channel].xfer); + } + + // Setup and start the DMA engine + ret = soc_dma_config(&(i2s_info->dma_ch[channel]), &(i2s_info->dma_cfg[channel])); + + if (ret != DRV_RC_OK) + { + goto fail; + } + + ret = soc_dma_start_transfer(&(i2s_info->dma_ch[channel])); + + if (ret != DRV_RC_OK) + { + goto fail; + } + + // Enable the channel and let it go! + reg = MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, i2s_reg_map[channel].ctrl); + reg |= (1 << (i2s_reg_map[channel].ctrl_en)); + reg |= (1 << (i2s_reg_map[channel].ctrl_sync_rst)); + MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, i2s_reg_map[channel].ctrl) = reg; + + return DRV_RC_OK; + +fail: + i2s_disable(channel); + soc_dma_release(&(i2s_info->dma_ch[channel])); + return DRV_RC_FAIL; +} + +DRIVER_API_RC soc_i2s_stop_listen(void) +{ + uint8_t channel = I2S_CHANNEL_RX; + uint32_t save; + + if (channel >= I2S_NUM_CHANNELS) + { + return DRV_RC_FAIL; + } + else if (!(i2s_info->en[channel])) + { + return DRV_RC_FAIL; + } + + save = interrupt_lock(); + i2s_disable(channel); + interrupt_unlock(save); + + return DRV_RC_OK; +} + +DRIVER_API_RC soc_i2s_write(uint32_t *buf, uint32_t len) +{ + // Calling stream with 0 buffers is the same as a onetime write of the whole buffer + return soc_i2s_stream(buf, len, 0); +} + +DRIVER_API_RC soc_i2s_stream(uint32_t *buf, uint32_t len, uint32_t num_bufs) +{ + DRIVER_API_RC ret; + uint8_t channel = I2S_CHANNEL_TX; + uint32_t reg; + uint32_t len_per_buf; + int i; + struct soc_dma_xfer_item *dma_list; + + // Check channel no in use and configured + if (channel >= I2S_NUM_CHANNELS) + { + return DRV_RC_FAIL; + } + else if (i2s_info->en[channel] || !(i2s_info->cfgd[channel])) + { + return DRV_RC_FAIL; + } + + // Get a DMA channel + ret = soc_dma_acquire(&(i2s_info->dma_ch[channel])); + + if (ret != DRV_RC_OK) + { + return DRV_RC_FAIL; + } + + // Enable the channel + i2s_enable(channel); + + // Determine the length of a single buffer + if (num_bufs == 0) + { + len_per_buf = len; + } + else + { + len_per_buf = len / num_bufs; + } + + // Prep some configuration + i2s_info->dma_cfg[channel].type = SOC_DMA_TYPE_MEM2PER; + i2s_info->dma_cfg[channel].dest_interface = SOC_DMA_INTERFACE_I2S_TX; + i2s_info->dma_cfg[channel].dest_step_count = 0; + i2s_info->dma_cfg[channel].src_step_count = 0; + + i2s_info->dma_cfg[channel].xfer.dest.delta = SOC_DMA_DELTA_NONE; + i2s_info->dma_cfg[channel].xfer.dest.width = SOC_DMA_WIDTH_32; + i2s_info->dma_cfg[channel].xfer.dest.addr = (void *)(SOC_I2S_BASE + SOC_I2S_DATA_REG); + i2s_info->dma_cfg[channel].xfer.src.delta = SOC_DMA_DELTA_INCR; + i2s_info->dma_cfg[channel].xfer.src.width = SOC_DMA_WIDTH_32; + + if (num_bufs == 0) + { + i2s_info->dma_cfg[channel].cb_done = i2s_dma_cb_done; + i2s_info->dma_cfg[channel].cb_done_arg = (void *)((uint32_t)channel); + } + else + { + i2s_info->dma_cfg[channel].cb_block = i2s_dma_cb_block; + i2s_info->dma_cfg[channel].cb_block_arg = (void *)((uint32_t)channel); + } + + i2s_info->dma_cfg[channel].cb_err = i2s_dma_cb_err; + i2s_info->dma_cfg[channel].cb_err_arg = (void *)((uint32_t)channel); + + // Setup the linked list + for (i = 0; i < ((num_bufs == 0) ? 1 : num_bufs); i++) + { + if (i == 0) + { + dma_list = &(i2s_info->dma_cfg[channel].xfer); + } + else + { + ret = soc_dma_alloc_list_item(&dma_list, dma_list); + + if (ret != DRV_RC_OK) + { + goto fail; + } + } + + dma_list->src.addr = (void *)(&(buf[i * (len_per_buf / sizeof(uint32_t))])); + dma_list->size = len_per_buf / sizeof(uint32_t); + } + + // Create a circular list if we are doing circular buffering + if (num_bufs != 0) + { + dma_list->next = &(i2s_info->dma_cfg[channel].xfer); + } + + // Setup and start the DMA engine + ret = soc_dma_config(&(i2s_info->dma_ch[channel]), &(i2s_info->dma_cfg[channel])); + + if (ret != DRV_RC_OK) + { + goto fail; + } + + ret = soc_dma_start_transfer(&(i2s_info->dma_ch[channel])); + + if (ret != DRV_RC_OK) + { + goto fail; + } + + // Enable the channel and let it go! + reg = MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, i2s_reg_map[channel].ctrl); + reg |= (1 << (i2s_reg_map[channel].ctrl_en)); + reg |= (1 << (i2s_reg_map[channel].ctrl_sync_rst)); + MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, i2s_reg_map[channel].ctrl) = reg; + + return DRV_RC_OK; + +fail: + i2s_disable(channel); + soc_dma_release(&(i2s_info->dma_ch[channel])); + return DRV_RC_FAIL; +} + + +DRIVER_API_RC soc_i2s_stop_stream(void) +{ + uint8_t channel = I2S_CHANNEL_TX; + uint32_t save; + + if (channel >= I2S_NUM_CHANNELS) + { + return DRV_RC_FAIL; + } + else if (!(i2s_info->en[channel])) + { + return DRV_RC_FAIL; + } + + save = interrupt_lock(); + i2s_disable(channel); + interrupt_unlock(save); + + return DRV_RC_OK; +} + +/* Driver API */ +DRIVER_API_RC soc_i2s_init() +{ + int i; + uint32_t reg; + + // Prep info struct + for (i = 0; i < I2S_NUM_CHANNELS; i++) + { + i2s_info->en[i] = 0; + i2s_info->cfgd[i] = 0; + i2s_info->cfg[i].cb_done = NULL; + i2s_info->cfg[i].cb_err = NULL; + } + + // Enable global clock, use local clock gating per channel instead + set_clock_gate(i2s_info->clk_gate_info, CLK_GATE_ON); + + // Setup ISR (and enable) + SET_INTERRUPT_HANDLER(i2s_info->int_vector, i2s_interrupt_handler); + SOC_UNMASK_INTERRUPTS(i2s_info->int_mask); + + // Set up control register + reg = MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, SOC_I2S_CTRL); + reg |= (1 << SOC_I2S_CTRL_TR_CFG_0); + reg &= ~(1 << SOC_I2S_CTRL_TSYNC_LOOP_BACK); + reg &= ~(1 << SOC_I2S_CTRL_RSYNC_LOOP_BACK); + MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, SOC_I2S_CTRL) = reg; + + // Set the watermark levels + MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, SOC_I2S_TFIFO_CTRL) &= 0xFFFCFFFF; + MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, SOC_I2S_TFIFO_CTRL) |= (I2S_TFIFO_THR << SOC_I2S_TFIFO_CTRL_TAFULL_THRS); + + MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, SOC_I2S_RFIFO_CTRL) &= 0xFFFCFFFF; + MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, SOC_I2S_RFIFO_CTRL) |= (I2S_RFIFO_THR << SOC_I2S_RFIFO_CTRL_RAFULL_THRS); + + // Enable global interrupt mask + reg = MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, SOC_I2S_CID_CTRL); + reg |= (1 << SOC_I2S_CID_CTRL_INTREQ_MASK); + MMIO_REG_VAL_FROM_BASE(SOC_I2S_BASE, SOC_I2S_CID_CTRL) = reg; + + // Initially, have all channels disabled + for (i = 0; i < I2S_NUM_CHANNELS; i++) + { + i2s_disable(i); + } + + return DRV_RC_OK; +} + +DRIVER_API_RC soc_i2s_reset(uint8_t channel) +{ + i2s_info->en[channel] = 0; + i2s_info->cfgd[channel] = 0; + i2s_info->cfg[channel].cb_done = NULL; + i2s_info->cfg[channel].cb_err = NULL; + + i2s_info->cfg[channel].cb_done_arg = NULL; + i2s_info->cfg[channel].cb_err_arg = NULL; + + + i2s_info->dma_ch[channel].active = 0; + i2s_info->dma_ch[channel].id = 0; + i2s_info->dma_ch[channel].ll = NULL; + i2s_info->dma_ch[channel].curr = NULL; + + i2s_info->dma_cfg[channel].cb_done_arg = NULL; + i2s_info->dma_cfg[channel].cb_done = NULL; + i2s_info->dma_cfg[channel].cb_block_arg= NULL; + i2s_info->dma_cfg[channel].cb_block = NULL; + i2s_info->dma_cfg[channel].cb_err_arg = NULL; + i2s_info->dma_cfg[channel].cb_err = NULL; + + soc_i2s_config(channel, &(i2s_info->cfg[channel])); + + return DRV_RC_OK; +} diff --git a/system/libarc32_arduino101/drivers/soc_i2s.h b/system/libarc32_arduino101/drivers/soc_i2s.h new file mode 100644 index 00000000..5a14f5f9 --- /dev/null +++ b/system/libarc32_arduino101/drivers/soc_i2s.h @@ -0,0 +1,214 @@ +/* + * Copyright (c) 2016, Intel Corporation. All rights reserved. + * + * 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 of the copyright holder 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 HOLDER 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 SOC_I2s_H_ +#define SOC_I2s_H_ + +#include "data_type.h" +#include "soc_dma.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @defgroup common_drivers Common Drivers + * Definition of the drivers APIs accessible from any processor. + * @ingroup drivers + */ + +/** + * @defgroup soc_i2s Quark SE SOC I2S + * Quark SE SOC I2S (Audio) drivers API. + * @ingroup common_drivers + * @{ + */ + +// I2S channels +#define I2S_CHANNEL_TX 0 +#define I2S_CHANNEL_RX 1 +#define I2S_NUM_CHANNELS 2 + +// I2S modes +#define I2S_MODE_SCK_POL (0x01) +#define I2S_MODE_WS_POL (0x02) +#define I2S_MODE_LR_ALIGN (0x08) +#define I2S_MODE_SAMPLE_DEL (0x10) +#define I2S_MODE_WS_DSP (0x20) + +#define I2S_MODE_PHILLIPS (I2S_MODE_SCK_POL | I2S_MODE_LR_ALIGN) +#define I2S_MODE_RJ (I2S_MODE_SCK_POL | I2S_MODE_WS_POL | \ + I2S_MODE_SAMPLE_DEL) +#define I2S_MODE_LJ (I2S_MODE_SCK_POL | I2S_MODE_WS_POL | \ + I2S_MODE_LR_ALIGN | I2S_MODE_SAMPLE_DEL) +#define I2S_MODE_DSP (I2S_MODE_SCK_POL | I2S_MODE_LR_ALIGN | \ + I2S_MODE_WS_DSP) + +// I2S configuration object +struct soc_i2s_cfg { + uint16_t sample_rate; // in Hz + uint8_t resolution; + uint8_t mode; + uint8_t master; + + void (*cb_done)(void *); + void *cb_done_arg; + + void (*cb_err)(void *); + void *cb_err_arg; +}; + +// Internal struct for use by the controller (and soc_config) +struct soc_i2s_info { + uint32_t reg_base; + uint32_t int_vector; + uint32_t int_mask; + + uint32_t clk_speed; + + struct soc_i2s_cfg cfg[I2S_NUM_CHANNELS]; + uint8_t en[I2S_NUM_CHANNELS]; + uint8_t cfgd[I2S_NUM_CHANNELS]; + + struct soc_dma_cfg dma_cfg[I2S_NUM_CHANNELS]; + struct soc_dma_channel dma_ch[I2S_NUM_CHANNELS]; + + struct clk_gate_info_s *clk_gate_info; +}; + +extern struct driver soc_i2s_driver; + +/** + * Function to configure specified I2S channel + * + * Configuration parameters must be valid or an error is returned - see return values below. + * + * @param channel : I2S channel number + * @param cfg : pointer to configuration structure + * + * @return + * - DRV_RC_OK on success + * - DRV_RC_INVALID_CONFIG - if any configuration parameters are not valid + * - DRV_RC_CONTROLLER_IN_USE, - if controller is in use + * - DRV_RC_FAIL otherwise + */ +DRIVER_API_RC soc_i2s_config(uint8_t channel, struct soc_i2s_cfg *cfg); + +/** + * Function to deconfigure specified I2S channel + * + * @param channel : I2S channel number + * + * @return + * - DRV_RC_OK on success + * - DRV_RC_CONTROLLER_IN_USE, - if controller is in use + * - DRV_RC_FAIL otherwise + */ +DRIVER_API_RC soc_i2s_deconfig(uint8_t channel); + +/** + * Function to transmit a block of audio data + * + * @param buf : pointer to data to write + * @param len : length of data to write (in bytes) + * + * @return + * - DRV_RC_OK on success + * - DRV_RC_FAIL otherwise + */ +DRIVER_API_RC soc_i2s_write(uint32_t *buf, uint32_t len); + +/** + * Function to continuously transmit blocks of audio data + * + * @param buf : pointer to buffer to read data from + * @param len : length of buffer (in bytes) + * @param num_bufs : number of parts into which to split the buffer; calling the callback + * after each is sent + * + * @return + * - DRV_RC_OK on success + * - DRV_RC_FAIL otherwise + */ +DRIVER_API_RC soc_i2s_stream(uint32_t *buf, uint32_t len, uint32_t num_bufs); + +/** + * Function to stop a continuous audio data write + * + * @return + * - DRV_RC_OK on success + * - DRV_RC_FAIL otherwise + */ +DRIVER_API_RC soc_i2s_stop_stream(void); + +/** + * Function to receive a block of audio data + * + * @param buf : pointer to buffer to fill with data + * @param len : length of buffer (in bytes) + * + * @return + * - DRV_RC_OK on success + * - DRV_RC_FAIL otherwise + */ +DRIVER_API_RC soc_i2s_read(uint32_t *buf, uint32_t len); + +/** + * Function to continuously receive blocks of audio data + * + * @param buf : pointer to buffer to fill with data + * @param len : length of buffer (in bytes) + * @param num_bufs : number of parts into which to split the buffer; calling the callback + * after each is filled + * + * @return + * - DRV_RC_OK on success + * - DRV_RC_FAIL otherwise + */ +DRIVER_API_RC soc_i2s_listen(uint32_t *buf, uint32_t len, uint8_t num_bufs); + +/** + * Function to stop a continuous audio data read + * + * @return + * - DRV_RC_OK on success + * - DRV_RC_FAIL otherwise + */ +DRIVER_API_RC soc_i2s_stop_listen(void); + +DRIVER_API_RC soc_i2s_init(); + +/** @} */ + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/variants/arduino_101/libarc32drv_arduino101.a b/variants/arduino_101/libarc32drv_arduino101.a index c7e60d8a..705a4986 100644 Binary files a/variants/arduino_101/libarc32drv_arduino101.a and b/variants/arduino_101/libarc32drv_arduino101.a differ