/* -*- Mode: C++; tab-width: 2; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
/***************************************************************************
 *            i2s.c
 *
 *  Thu Aug 21 18:09:54 CEST 2014
 *  Copyright 2014 Bent Bisballe Nyeng
 *  deva@aasimon.org
 ****************************************************************************/

/*
 *  This file is part of Pedal2Metal.
 *
 *  Pedal2Metal is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  Pedal2Metal 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 General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Pedal2Metal; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA.
 */
#include "i2s.h"

#include <cli.h>

void i2s_set_power(int power)
{
	// 1. Enable I²S in PCONP register. Set PCI2S to 1 (see table 46, pg. 64)
	// Remark: On reset, the I²S interface is disabled (PCI2S = 0).
  uint32_t *pconp = (uint32_t *)0x400FC0C4;
	if(power) *pconp |= 0b1 << 27; // Set bit
  else *pconp &= ~(0b1 << 27); // Clear bit
}

void i2s_set_clksel(i2s_clksel_t sel)
{
	// 2. Clock: In PCLKSEL1 select PCLK_I2S. (see table 41, pg. 57)
	uint32_t *pclksel1 = (uint32_t*)0x400FC1AC;
	*pclksel1 &= ~(0b11 << 22); // Clear bits
	*pclksel1 |= ( (sel & 0b11) << 22); // Set bits
} 

void i2s_set_pinsel()
{
	//3. Pins: Select I²S pins and their modes in PINSEL0 to PINSEL4 and PINMODE0
	// to PINMODE4 (see Section 8.5).

	// See table 79, pg. 108
	uint32_t *pinsel0 = (uint32_t*)0x4002C000; 

	// Reset ports
  *pinsel0 &=
		~(
			// TX:
			(0b11 << 8)  | // P0.4
			(0b11 << 10) | // P0.5
			(0b11 << 12) | // P0.6
			// RX:
			(0b11 << 14) | // P0.7
			(0b11 << 16) | // P0.8
			(0b11 << 18) | // P0.9
      0);

	// Set ports 
  *pinsel0 |=
		(
		 // TX:
		 (0b01 << 8)  | // P0.4: I2SRX_CLK
		 (0b01 << 10) | // P0.5: I2SRX_WS
		 (0b01 << 12) | // P0.6: I2SRX_SDA
		 // RX:
		 (0b01 << 14) | // P0.7: I2STX_CLK
     (0b01 << 16) | // P0.8: I2STX_WS
		 (0b01 << 18) | // P0.9: I2STX_SDA
     0);

	// See table 85, pg. 111
  uint32_t *pinsel9 = (uint32_t *)0x4002C024;

	// Reset
  *pinsel9 &=
    ~(0b11 << 26) // P4.28
    ;

	// Set port
  *pinsel9 |=
    (0b01 << 26) // P4.28: TX_MCLK
    ;
}

void i2s_set_dao_register(i2s_wordwidth_t ww, i2s_channels_t ch, int hp)
{
#if 0
  (void)ww;
  (void)ch;
  (void)hp;

  uint32_t DAOValue;

  // Stop the I²S to start. Audio output is master, audio input is the slave.
  // 16 bit data, set STOP and RESET bits to reset the channels
  DAOValue = LPC_I2S->I2SDAO;

  // Switch to master mode, TX channel, no mute
  DAOValue &= ~((0x01 << 5)|(0x01 << 15));
  LPC_I2S->I2SDAO = (0x01 << 4) | (0x01 << 3) | DAOValue;	// Master
#else

	// See table 405, pg. 476
	uint32_t *i2sdao = (uint32_t*)0x400A8000;
	*i2sdao =
		(ww  << 0) | // Set bit word width
		(ch  << 2) | // stereo
		(0b1 << 3) | // stop bit
		(0b1 << 4) | // reset
		(0b0 << 5) | // ws_sel in master mode (0: master - 1: slave)
		((hp & 0xff) << 6) | // ws_halfperiod
		(0b0 << 15) // no mute
		;
#endif
}

void i2s_set_dai_register(i2s_wordwidth_t ww, i2s_channels_t ch, int hp)
{
	// See table 406, pg. 477
	uint32_t *i2sdai = (uint32_t*)0x400A8004;
	*i2sdai =
		(ww  << 0) | // Set bit word width
		(ch  << 2) | // stereo
		(0b1 << 3) | // stop bit
		(0b1 << 4) | // reset
		(0b0 << 5) | // ws_sel in master mode (0: master - 1: slave)
		((hp & 0xff) << 6) | // ws_halfperiod
		(0b0 << 15) // no mute
		;		
}

void i2s_set_tx_rate(uint8_t x_div, uint8_t y_div)
{
	// See table 413, pg. 480
	uint32_t *i2stxrate = (uint32_t*)0x400A8020;
	*i2stxrate =
    (y_div << 0) |
    (x_div << 8)
    ;
}

void i2s_set_rx_rate(uint8_t x_div, uint8_t y_div)
{
	// See table 414, pg. 481
	uint32_t *i2srxrate = (uint32_t*)0x400A8024;
	*i2srxrate =
    (y_div << 0) |
    (x_div << 8)
    ;
}

void i2s_set_tx_clock_bitrate(uint8_t bitrate)
{
  // See table 415, pg. 481
	uint32_t *i2stxbitrate = (uint32_t*)0x400A8028;
	*i2stxbitrate = (bitrate & 0b111111);
}

void i2s_set_rx_clock_bitrate(uint8_t bitrate)
{
  // See table 416, pg. 481
	uint32_t *i2srxbitrate = (uint32_t*)0x400A802C;
	*i2srxbitrate = (bitrate & 0b111111);
}

void i2s_set_tx_mode_control(i2s_tx_clksel_t c, int _4pin, int mcena)
{
	// See table 417, pg 482
	uint32_t *i2stxmode = (uint32_t*)0x400A8030;
	*i2stxmode = c |
		((_4pin & 0b1) << 2) |
		((mcena & 0b1) << 3)
    ;
}

void i2s_set_rx_mode_control(i2s_rx_clksel_t c, int _4pin, int mcena)
{
	// See table 418, pg 483
	uint32_t *i2srxmode = (uint32_t*)0x400A8034;
	*i2srxmode = c |
		((_4pin & 0b1) << 2) |
		((mcena & 0b1) << 3)
    ;
}

void i2s_tx_reset_fifo()
{
	// See table 405, pg. 476
	uint32_t *i2sdao = (uint32_t*)0x400A8000;
	*i2sdao |=
		(0b1 << 4) // reset
		;
}

void i2s_tx_stop()
{
	// See table 405, pg. 476
	uint32_t *i2sdao = (uint32_t*)0x400A8000;
	*i2sdao |=
		(0b1 << 3) // stop bit
		;
}

void i2s_tx_start()
{
	// See table 405, pg. 476
	uint32_t *i2sdao = (uint32_t*)0x400A8000;
	*i2sdao &=
		~((0b1 << 3) | // unset stop bit (start)
      (0b1 << 4) | // unset reset bit 
      (0b1 << 15)) // unset mute bit (unmute)
		;
}

void i2s_rx_reset_fifo()
{
	// See table 406, pg. 477
	uint32_t *i2sdai = (uint32_t*)0x400A8004;
	*i2sdai |=
		(0b1 << 4) // set reset bit
		;		
}

void i2s_rx_stop()
{
	// See table 406, pg. 477
	uint32_t *i2sdai = (uint32_t*)0x400A8004;
	*i2sdai |=
		(0b1 << 3) // stop bit
		;		
}

void i2s_rx_start()
{
	// See table 406, pg. 477
	uint32_t *i2sdai = (uint32_t*)0x400A8004;
	*i2sdai &=
		~((0b1 << 3) | // unset stop bit
      (0b1 << 4))  // unset reset bit
		;		
}

/**
 * This bit reflects the presence of Receive Interrupt or Transmit Interrupt.
 * This is determined by comparing the current FIFO levels to the rx_depth_irq
 * and tx_depth_irq fields in the I2SIRQ register.
 */
int i2s_get_state_irq()
{
	// See table 409, pg.478
	uint32_t *i2sstate = (uint32_t*)0x400A8010;
  return (*i2sstate >> 0) & 0b1; // bit 0
}

/**
 * This bit reflects the presence of Receive or Transmit DMA Request 1. This is
 * determined by comparing the current FIFO levels to the rx_depth_dma1 and
 * tx_depth_dma1 fields in the I2SDMA1 register.
 */
int i2s_get_state_dmareq1()
{
	// See table 409, pg.478
	uint32_t *i2sstate = (uint32_t*)0x400A8010;
  return (*i2sstate >> 1) & 0b1; // bit 1
}

/**
 * This bit reflects the presence of Receive or Transmit DMA Request 2. This is
 * determined by comparing the current FIFO levels to the rx_depth_dma2 and
 * tx_depth_dma2 fields in the I2SDMA2 register.
 */
int i2s_get_state_dmareq2()
{
	// See table 409, pg.478
	uint32_t *i2sstate = (uint32_t*)0x400A8010;
  return (*i2sstate >> 2) & 0b1; // bit 2
}

/**
 * Reflects the current level of the Receive FIFO.
 */
int i2s_get_state_rx_level()
{
	// See table 409, pg.478
	uint32_t *i2sstate = (uint32_t*)0x400A8010;
  return (*i2sstate >> 8) & 0b1111; // bit 8-11
}

/**
 * Reflects the current level of the Transmit FIFO.
 */
int i2s_get_state_tx_level()
{
	// See table 409, pg.478
	uint32_t *i2sstate = (uint32_t*)0x400A8010;
  return (*i2sstate >> 16) & 0b1111; // bit 16-19
}

int i2s_init(int pclkdiv, int bitrate, int x, int y,
             int bitwidth, int channels)
{
	i2s_set_power(1);

	i2s_set_pinsel();

  i2s_clksel_t clk = I2S_CCLK;
  switch(pclkdiv) {
  case 1: clk = I2S_CCLK; break;
  case 2: clk = I2S_CCLK_2; break;
  case 4: clk = I2S_CCLK_4; break;
  case 8: clk = I2S_CCLK_8; break;
  default:
    return 1;
  }
  i2s_set_clksel(clk);

  i2s_wordwidth_t ww = I2S_WW_16_BIT;
  switch(bitwidth) {
  case 8: ww = I2S_WW_8_BIT; break;
  case 16: ww = I2S_WW_16_BIT; break;
  case 32: ww = I2S_WW_32_BIT; break;
  default:
    return 1;
  }

  i2s_channels_t ch = I2S_CH_MONO;
  switch(channels) {
  case 1: ch = I2S_CH_MONO; break;
  case 2: ch = I2S_CH_STEREO; break;
  default:
    return 1;
  }

	i2s_set_dao_register(ww, ch, bitwidth);

  if((x & 0xff) != x || (y & 0xff) != y) return 1;
  if(y < x) return 1;
	i2s_set_tx_rate(x, y);

  if((bitrate & 0b111111) != bitrate) return 1;
	i2s_set_tx_clock_bitrate(bitrate);

	i2s_set_tx_mode_control(CLK_TX_SRC, 0, 1);

	//i2s_tx_reset();
	i2s_tx_stop();
  /*
  int i;
  i = i2s_get_state_irq();
  i = i2s_get_state_dmareq1();
  i = i2s_get_state_dmareq2();
  i = i2s_get_state_rx_level();
  i = i2s_get_state_tx_level();
  (void)i;
  */
  return 0;
}

typedef struct {
	int pclkdiv_idx;
	unsigned int bitrate;
	unsigned int x;
	unsigned int y;
	int err;
} result_t;

static const int pclkdiv[] = { 1, 2, 4, 8 };

int _fabs(int a) { return a<0?a*-1:a; }

int i2s_clkcalc_init(int fs, int bw, int ch)
{
  // Hack: These value work!
  if(fs == 48000 && bw == 16 && ch == 2) {
    i2s_init(8, 3, 57, 59, bw, ch);
    return 0;
  }
#if 1
	result_t best;
	best.err = 999999;

	int target = fs * bw * ch * 100;

	int cclk = 100000000;

	result_t res;
	res.pclkdiv_idx = 3;

	res.bitrate = 0;
	res.x = 1;
	res.y = 1;

  unsigned int largest = 0;

	for(;res.pclkdiv_idx >= 0; res.pclkdiv_idx--) {

    // [100000000/8; 100000000]
		uint32_t clk = (cclk / pclkdiv[res.pclkdiv_idx]);
    //cli_printf("clk: %d\n", clk);

		for(res.y = 1; res.y < 256; res.y++) {

      // [100000000/8/255; 100000000]
			uint32_t clk_y = clk / res.y;
      //cli_printf("clk_y: %d\n", clk_y);
      
			// Note y must be greater than or equal to x.
			for(res.x = 1; res.x <= res.y; res.x++) {

        // [100000000/8/255*1; 100000000*255]
				uint32_t clk_y_x_2 = clk_y * res.x;
        if(clk_y_x_2 > largest) largest = clk_y_x_2;

        // [100000000/8/255*1/2; 100000000*255/2]
				clk_y_x_2 /= 2;

				for(res.bitrate = 0; res.bitrate < 65; res.bitrate++) {

          // [100000000/8/255*1/2/65; 100000000*255/2/1]
					int val = clk_y_x_2 / (res.bitrate + 1);

					res.err = _fabs(val - target);
					if(res.err < best.err) {
            cli_printf("val: %d - target: %d\n", val, target);
            best = res;
          }

					if(res.err == 0) {
						goto found;
					}
				}
			}
		}
	}

	goto not_found;

 found:
  /*
	printf("Found:\n");
	printf(" pclkdiv: %d\n", pclkdiv[res.pclkdiv_idx]);
	printf(" bitrate: %d\n", res.bitrate);
	printf(" x: %d\n", res.x);
	printf(" y: %d\n", res.y);
	return 0;
  */
 not_found:
  /*
	printf("Not found - best was:\n");
	printf(" pclkdiv: %d\n", pclkdiv[best.pclkdiv_idx]);
	printf(" bitrate: %d\n", best.bitrate);
	printf(" x: %d\n", best.x);
	printf(" y: %d\n", best.y);
	printf(" err: %f\n", best.err);
	return 1;
  */
  cli_printf(" pclkdiv: %d (should be 8)\n", pclkdiv[best.pclkdiv_idx]);
  cli_printf(" bitrate: %d (should be 3)\n", best.bitrate);
	cli_printf(" x:       %d (should be 57)\n", best.x);
	cli_printf(" y:       %d (should be 59)\n", best.y);
	cli_printf(" err:     %d bits/ms\n", best.err);
	cli_printf(" largest: %d\n", largest);

  i2s_init(pclkdiv[best.pclkdiv_idx], best.bitrate,
                     best.x, best.y, bw, ch);

#else
  uint32_t pClk;
  uint32_t x, y;
  uint32_t divider;
  uint16_t dif;
  uint16_t x_divide = 0, y_divide = 0;
  uint32_t N;
  uint16_t err, min_err = 0xffff;
  
  // TODO: Read PLL/chip clock somewhere...
  pClk = 100000000/8;//Chip_Clock_GetRate(CLK_APB1_I2S);

  // divider is a fixed point number with 16 fractional bits
  //  divider = ((fs * 2 * bw * 2) / pClk) << 16;
  divider = (((uint32_t)(fs) * 2 * (bw) * 2) << 16) / pClk;

  // find N that make x/y <= 1 -> divider <= 2^16
  for(N = 64; N > 0; N--) {
    if((divider * N) < (1 << 16)) {
      break;
    }
  }
  if(N == 0) {
    return 1;
  }
  divider *= N;
  for(y = 255; y > 0; y--) {
    x = y * divider;
    if (x & (0xff000000)) {
      continue;
    }
    dif = x & 0xffff;
    if(dif > 0x8000) {
      err = 0x10000 - dif;
    } else {
      err = dif;
    }
    if(err == 0) {
      y_divide = y;
      break;
    } else if(err < min_err) {
      min_err = err;
      y_divide = y;
    }
  }

  x_divide = (y_divide * fs * 2 * bw * N * 2) / pClk;
  if(x_divide >= 256) {
    x_divide = 0xff;
  }
  if(x_divide == 0) {
    x_divide = 1;
  }

  divider = 1;//8;// This should be the value

  cli_printf(" pclkdiv: %d (should be 8)\n", divider);
  cli_printf(" bitrate: %d (should be 3)\n", N - 1);
	cli_printf(" x:       %d (should be 57)\n", x_divide);
	cli_printf(" y:       %d (should be 59)\n", y_divide);

  int res = i2s_init(divider, N - 1, x_divide, y_divide, bw, ch);
  if(res) return res;
#endif

  return 0;
}

#if 1
#include <LPC17xx.h>
//#include "type.h"
#include "dma.h"

/* treat I²S TX and RX as a constant address, make the code and buffer 
easier for both DMA and non-DMA test */
volatile uint8_t *I2STXBuffer = (uint8_t *)(DMA_SRC); 
volatile uint8_t *I2SRXBuffer = (uint8_t *)(DMA_DST);
volatile uint32_t I2SReadLength = 0;
volatile uint32_t I2SWriteLength = 0;
volatile uint32_t I2SRXDone = 0, I2STXDone = 0;

/*****************************************************************************
** Function name:		I2S_IRQHandler
**
** Descriptions:		I²S interrupt handler, only RX interrupt is enabled
**						for simplicity.
**
** parameters:			None
** Returned value:		None
** 
*****************************************************************************/
#if 0
void I2S_IRQHandler (void)  
{
  uint32_t RxCount = 0;

  if ( LPC_I2S->I2SSTATE & 0x01 )
  {
	RxCount = (LPC_I2S->I2SSTATE >> 8) & 0xFF;
	if ( (RxCount != RXFIFO_EMPTY) && !I2SRXDone )
	{
	  while ( RxCount > 0 )
	  {
		if ( I2SReadLength == BUFSIZE )
		{
		  LPC_I2S->I2SDAI |= ((0x01 << 3) | (0x01 << 4));
		  LPC_I2S->I2SIRQ &= ~(0x01 << 0);	// Disable RX	
		  I2SRXDone = 1;
		  break;
		}
		else
		{
		  I2SRXBuffer[I2SReadLength++] = LPC_I2S->I2SRXFIFO;
		}
		RxCount--;
	  }
	}
  }
  return;
}
#endif

/*****************************************************************************
** Function name:		I2SStart
**
** Descriptions:		Start I²S DAI and DAO
**
** parameters:			None
** Returned value:		None
** 
*****************************************************************************/
void I2SStart( void )
{
  //  uint32_t DAIValue;
  uint32_t DAOValue;
  
  /* Audio output is the master, audio input is the slave, */
  /* 16 bit data, stereo, reset, master mode, not mute. */
  DAOValue = LPC_I2S->I2SDAO;
  //  DAIValue = LPC_I2S->I2SDAI;
  LPC_I2S->I2SDAO = DAOValue & (~((0x01 << 4)|(0x01 <<3)));
  /* 16 bit data, stereo, reset, slave mode, not mute. */
  //  LPC_I2S->I2SDAI = DAIValue & (~((0x01 << 4)|(0x01 <<3)));
  return;
}

/*****************************************************************************
** Function name:		I2SStop
**
** Descriptions:		Stop I²S DAI and DAO
**
** parameters:			None
** Returned value:		None
** 
*****************************************************************************/
void I2SStop( void )
{
  //  uint32_t DAIValue;
  uint32_t DAOValue;

  /* Stop the I²S to start. Audio output is master, audio input is the slave. */
  /* 16 bit data, set STOP and RESET bits to reset the channels */
  DAOValue = LPC_I2S->I2SDAO;
  /* Switch to master mode, TX channel, no mute */
  DAOValue &= ~((0x01 << 5)|(0x01 << 15));
  //  DAIValue = LPC_I2S->I2SDAI;
  // DAIValue &= ~(0x01 << 15);
  LPC_I2S->I2SDAO = (0x01 << 4) | (0x01 << 3) | DAOValue;	/* Master */
  //  LPC_I2S->I2SDAI = (0x01 << 4) | (0x01 << 3) | DAIValue;	/* Slave */
  return;
}

/*****************************************************************************
** Function name:		I2SInit
**
** Descriptions:		Initialize I²S controller
**
** parameters:			None
** Returned value:		true or false, return false if the I²S
**						interrupt handler was not installed correctly
** 
*****************************************************************************/
void I2SInit( void ) 
{

  /*enable I²S in the PCONP register. I²S is disabled on reset*/
  LPC_SC->PCONP |= (1 << 27);
 
  /*connect the I²S sigals to port pins(P0.4-P0.9)*/
  LPC_PINCON->PINSEL0 &= ~0x000FFF00;
  LPC_PINCON->PINSEL0 |= 0x00055500;

  /* Please note, in order to generate accurate TX/RX clock rate for I²S, 
  PCLK and CCLK needs to be carefully reconsidered. For this test 
  program, the TX is looped back to RX without external I²S device, 
  clock rate is not critical in this matter. */
  LPC_I2S->I2STXRATE = //0x241;
		(100 << 0) | // Y
		(49 << 8)   // X
		;

	//  LPC_I2S->I2SRXRATE = 0x241;

  I2SStop();
  return;
}
#endif