#include "bme.h"
#include "include/framercpu.h"
#include "include/xdsl_fmc.h"
#include "bme_app.h"
#include "mem.h"
#include "hostType.h"


typedef enum {FRAMER_CPU_CMD_Q = 0, RXCPU_CMD_Q, DeFRAMER_CPU_CMD_Q} CMD_Q_TYPES;
typedef enum {FRAMER_CPU_MSG_Q = 0, RXCPU_MSG_Q, DeFRAMER_CPU_MSG_Q} MSG_Q_TYPES;

typedef struct {
	int cmdQ[MAX_NUM_CMD][SZ_RX_CPU_ENTRY];
	int	wr;
	int	rd;
	int	q_len;
	int elem_size;
} Cir_Q;

static Cir_Q 	frameCmdQ[NUM_DSL_CHANNELS];
static Cir_Q	rxCpuCmdQ[NUM_DSL_CHANNELS];
static Cir_Q 	deframerCpuCmdQ[NUM_DSL_CHANNELS];
static Cir_Q 	*q_array[3] = {frameCmdQ, rxCpuCmdQ,  deframerCpuCmdQ};
static int		elem_size[3] = {SZ_FRAMER_CPU_ENTRY, SZ_RX_CPU_ENTRY, SZ_DEFRAMER_CPU_ENTRY};

static const int *cmd_addr_array[3][NUM_DSL_CHANNELS][2] = {
	{	// ch 0
  		{(int *) (FCPU_BMECTRL_MSGS), (int *) (FCPU_BMECTRL_MSGS + (SZ_FRAMER_CPU_ENTRY_IN_BYTES * 1))},
  		// ch 1
	  	{(int *) (FCPU_BMECTRL_MSGS + (SZ_FRAMER_CPU_ENTRY_IN_BYTES * 2)), (int *) (FCPU_BMECTRL_MSGS + (SZ_FRAMER_CPU_ENTRY_IN_BYTES * 3))},
  		// ch 2
  		{(int *) (FCPU_BMECTRL_MSGS + (SZ_FRAMER_CPU_ENTRY_IN_BYTES * 4)), (int *) (FCPU_BMECTRL_MSGS + (SZ_FRAMER_CPU_ENTRY_IN_BYTES * 5))},
  		// ch 3
  		{(int *) (FCPU_BMECTRL_MSGS + (SZ_FRAMER_CPU_ENTRY_IN_BYTES * 6)), (int *) (FCPU_BMECTRL_MSGS + (SZ_FRAMER_CPU_ENTRY_IN_BYTES * 7))},
	},
	{
		{ (int *) (RXCPU_BMECTRL_MSGS), (int *) (RXCPU_BMECTRL_MSGS + (SZ_RX_CPU_ENTRY_IN_BYTES * 1)) },
  		{ (int *) (RXCPU_BMECTRL_MSGS + (SZ_RX_CPU_ENTRY_IN_BYTES * 2)), (int *) (RXCPU_BMECTRL_MSGS + (SZ_RX_CPU_ENTRY_IN_BYTES * 3))},
  		{ (int *) (RXCPU_BMECTRL_MSGS + (SZ_RX_CPU_ENTRY_IN_BYTES * 4)), (int *) (RXCPU_BMECTRL_MSGS + (SZ_RX_CPU_ENTRY_IN_BYTES * 5))},
  		{ (int *) (RXCPU_BMECTRL_MSGS + (SZ_RX_CPU_ENTRY_IN_BYTES * 6)), (int *) (RXCPU_BMECTRL_MSGS + (SZ_RX_CPU_ENTRY_IN_BYTES * 7))},
	},
	{
		{ (int *) (DEFCPU_BMECTRL_MSGS), (int *) (DEFCPU_BMECTRL_MSGS + (SZ_DEFRAMER_CPU_ENTRY_IN_BYTES * 1))},
  		{ (int *) (DEFCPU_BMECTRL_MSGS + (SZ_DEFRAMER_CPU_ENTRY_IN_BYTES * 2)), (int *) (DEFCPU_BMECTRL_MSGS + (SZ_DEFRAMER_CPU_ENTRY_IN_BYTES * 3))},
  		{ (int *) (DEFCPU_BMECTRL_MSGS + (SZ_DEFRAMER_CPU_ENTRY_IN_BYTES * 4)), (int *) (DEFCPU_BMECTRL_MSGS + (SZ_DEFRAMER_CPU_ENTRY_IN_BYTES * 5))},
  		{ (int *) (DEFCPU_BMECTRL_MSGS + (SZ_DEFRAMER_CPU_ENTRY_IN_BYTES * 6)), (int *) (DEFCPU_BMECTRL_MSGS + (SZ_DEFRAMER_CPU_ENTRY_IN_BYTES * 7))},
  }
};

void init_cmd_q(Cir_Q *cir_q, CMD_Q_TYPES q_type)
{
	int		i;
	
	for(i=0;i<NUM_DSL_CHANNELS;i++) {
		cir_q[i].wr = 0;
		cir_q[i].rd = 0;
		cir_q[i].q_len = MAX_NUM_CMD;
		cir_q[i].elem_size = elem_size[q_type];
	}	
}

void initCpuCmdQ(void)
{
	int i, j, k;

	for(k=0;k<3;k++)  {
		for (i = 0; i<NUM_DSL_CHANNELS;i++) {
	 		for(j=0;j<elem_size[k];j++) {	
				writePIO_SingleRegChannel((int *)cmd_addr_array[k][i][0]+j, 0, 0xffffffff);
				writePIO_SingleRegChannel((int *)cmd_addr_array[k][i][1]+j, 0, 0xffffffff);
			}
		}
	}
}

void initCircBuffers()
{
	int i;
	
	for(i=0;i<3;i++) 
		init_cmd_q(q_array[i], (CMD_Q_TYPES)i);		
}

int is_space_avail(Cir_Q *cir_q)
{
	int	wr;
	
	wr = (cir_q->wr+1)&Q_WRAP_MASK;
	if(wr == cir_q->rd)
		return(0);
	else
		return(1);
}	

int is_not_empty(Cir_Q *cir_q)
{
	if(cir_q->rd == cir_q->wr)
		return(0);
	else
		return(1);			
}
				
int writeCirQ(Cir_Q *cir_q, int *data)
{
	int		i, wr;
	
	if(is_space_avail(cir_q)) {
		wr = cir_q->wr;
		for(i=0;i<cir_q->elem_size;i++)
			cir_q->cmdQ[wr][i]=data[i];
		wr=(wr+1)&Q_WRAP_MASK;
		cir_q->wr = wr;
		return(1);
	} 
		
	return(0);		
}

int	readCirQ(Cir_Q *cir_q, int *data)
{
	int	i;
	
	if(is_not_empty(cir_q)) {
		for(i=0;i<cir_q->elem_size;i++)
			data[i] = cir_q->cmdQ[cir_q->rd][i];
		cir_q->rd = (cir_q->rd+1) & Q_WRAP_MASK;
		return(cir_q->elem_size);
	}
	
	return(0);
}

int dir_writeCirQ(CMD_Q_TYPES q_type, int channel, int *data, int size)
{
	int flag, i, index, msg_q_index=0;
	
	flag = 1;
	while(flag) {
		for(index = 0; index<2;index++) {
			if(readPIO_short((u16 *)(cmd_addr_array[q_type][channel][index]))== 0xffff) {  // check for first 16 bit of the 1st 32bit word
				msg_q_index = index;
				flag = 0;
				break;
			}
		}
	}
	
	for(i= size-1; i>0; i--) {
		writePIO_SingleRegChannel((int *)cmd_addr_array[q_type][channel][msg_q_index]+i, 0, data[i]);
	}

	writePIO_short((u16 *)(cmd_addr_array[q_type][channel][msg_q_index])+1, (unsigned short)(data[0] & 0xffff));
	writePIO_short((u16 *)(cmd_addr_array[q_type][channel][msg_q_index]), (unsigned short)(data[0]>>16));		
	return(1);		
}

int writeFramerCPU_CQ (int channel, int  *data)
{
	if(write_q_mode == DIRECT_WRITE_CMD_Q)
		return(dir_writeCirQ(FRAMER_CPU_CMD_Q, channel, data, elem_size[FRAMER_CPU_CMD_Q]));
	else	
		return(writeCirQ(q_array[FRAMER_CPU_CMD_Q]+channel, data));
}

int writeRxCPU_CQ(int channel, int  *data)
{
	if(write_q_mode == DIRECT_WRITE_CMD_Q)
		return(dir_writeCirQ(RXCPU_CMD_Q, channel, data, elem_size[RXCPU_CMD_Q]));
	else	
		return(writeCirQ(q_array[RXCPU_CMD_Q]+channel, data));
}


int	writeDeFramerCPU_CQ(int channel, int *data)
{
	if(write_q_mode == DIRECT_WRITE_CMD_Q)
		return(dir_writeCirQ(DeFRAMER_CPU_CMD_Q, channel, data, elem_size[DeFRAMER_CPU_CMD_Q]));
	else	
 		return(writeCirQ(q_array[DeFRAMER_CPU_CMD_Q]+channel, data));
}

int hw_q_empty(CMD_Q_TYPES q_type, int channel)
{
	int	index;
	
   	for(index = 0; index<2;index++) {
		if(readPIO_short((u16 *)(cmd_addr_array[q_type][channel][index]))== 0xffff) {  // check for first 16 bit of the 1st 32bit word
			return(index);			
		}
	}	
	
	return(-1);	
}

void writeFramerQ(CMD_Q_TYPES q_type, int channel, int msg_q_index, int *data, int size)
{
	int i;

  	for(i= size-1; i>0; i--)
		writePIO_SingleRegChannel((int *)cmd_addr_array[q_type][channel][msg_q_index]+i, 0, data[i]);
		
	writePIO_short((u16 *)(cmd_addr_array[q_type][channel][msg_q_index])+1, (unsigned short)(data[0] & 0xffff));
	writePIO_short((u16 *)(cmd_addr_array[q_type][channel][msg_q_index]), (unsigned short)(data[0]>>16));		
}

int flush_xframer_cmd_q(int channel)
{
	int cmd_size;
	int i, msg_slot;
	int	cmd[SZ_RX_CPU_ENTRY];
	int status = 0;
	
	if(write_q_mode == DIRECT_WRITE_CMD_Q)
		return(0);
		
	for(i=0;i<3;i++) {
		while((msg_slot = hw_q_empty((CMD_Q_TYPES)i, channel))>=0 && (cmd_size = readCirQ(q_array[(CMD_Q_TYPES)i]+channel, cmd))>0) {
			writeFramerQ((CMD_Q_TYPES)i, channel, msg_slot, cmd, cmd_size);
			status = 1;
		}
	}	
	
	return(status);
}


