#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/mman.h>
#include "fpga.h"
#include "pin_mux.h"
#include "upp_reg.h"
#include <string.h>

#define DEBUG_UPP 1

/* definitions for upp tx and rx buffers */
#define UPP_BUF_SIZE (4096*10)
void *rx_buf, *tx_buf;


static int upp_mmap_size, psc1_mmap_size, pinmux_size;
static uintptr_t upp_base, psc1_base, pinmux_base;

/* enbale the upp for digital loop back */
int upp_enable(void) 
{
	int mux_val;
	int reg_stat;


	reg_stat = 	( *(uint32_t *)(upp_base+UPPCR) ) = 0;
	printf("loopback register: %#010x\n",reg_stat); 


	return 1;
	/*	( *(uint32_t *)(upp_base+UPDBL) )= 0x01;
	reg_stat = ( *(uint32_t *)(upp_base+UPDBL) );
	printf("loopback register: %#010x\n",reg_stat);  */

	/*	mux_val = ( *(uint32_t *)(upp_base+UPDBL));
		printf("DBL: %#010x\n",mux_val);  */


}

int upp_reset(void)
{
	int reg_stat;

	reg_stat = ( *(uint32_t *)(upp_base+UPPCR) );
	printf("upp Control before reset: %#010x\n",reg_stat); 	


	/* lets clear out EN first */
	( *(uint32_t *)(upp_base+UPPCR) ) = 0;
	/* check the dma active bit */
	while (( *(uint32_t *)(upp_base+UPPCR) ) & 0x00 ){
	  usleep(5);
	}
	reg_stat = UPPRESET;
	( *(uint32_t *)(upp_base+UPPCR) ) = reg_stat;
	usleep(100000);

	( *(uint32_t *)(upp_base+UPPCR) ) = 0;
	reg_stat = ( *(uint32_t *)(upp_base+UPPCR) );
	printf("upp Control UPPCR: %#010x\n",reg_stat); 	

	return 1;
}

int upp_config(void)
{
  int reg_stat;
  
  reg_stat = 0x02020007;
  ( *(uint32_t *)(upp_base+UPCTL) ) = reg_stat;
  reg_stat = ( *(uint32_t *)(upp_base+UPCTL) );
  printf("UPCTL: %#010x\n",reg_stat); 	

  reg_stat = 0x180020;
  ( *(uint32_t *)(upp_base+UPICR) ) = reg_stat;
  reg_stat = ( *(uint32_t *)(upp_base+UPICR) );
  printf("UPICR: %#010x\n",reg_stat); 	
 
  /*  reg_stat = 0xDEAD; */
  ( *(uint32_t *)(upp_base+UPIVR) ) = 0x00;
  reg_stat = ( *(uint32_t *)(upp_base+UPIVR) );
  printf("UPIVR: %#010x\n",reg_stat); 	

  reg_stat = 0x010100;
  ( *(uint32_t *)(upp_base+UPTCR) ) = reg_stat;
  reg_stat = ( *(uint32_t *)(upp_base+UPTCR) );
  printf("UPICR: %#010x\n",reg_stat); 	


  reg_stat = 0x01000;
  ( *(uint32_t *)(upp_base+UPDBL) ) = reg_stat;
  reg_stat = ( *(uint32_t *)(upp_base+UPDBL) );
#if DEBUG_UPP
  printf("UPDBL: %#010x\n",reg_stat); 	
#endif


  ( *(uint32_t *)(upp_base+UPIEC) ) = 0xffffffff;
  reg_stat = ( *(uint32_t *)(upp_base+UPIEC) );
#if DEBUG_UPP
  printf("UPIEC: %#010x\n",reg_stat); 	
#endif


  ( *(uint32_t *)(upp_base+UPIES) ) = 0x0f0f;
  reg_stat = ( *(uint32_t *)(upp_base+UPIES) );
#if DEBUG_UPP
  printf("UPIES: %#010x\n",reg_stat); 	
#endif

  reg_stat = ( *(uint32_t *)(upp_base+UPISR) );
#if DEBUG_UPP
  printf("UPISR: %#010x\n",reg_stat); 	
#endif

  printf("Wait for interrupt\n\n");
  /*  while ( !(*(uint32_t *)(upp_base+UPIER))&& !(*(uint32_t *)(upp_base+UPISR)) )
      usleep(2); */

  /*  printf("Interrupt done\n");
  printf("Rx buf\n");
  printf("0x%08x ", *(int *)(rx_buf));
  printf("0x%08x ", *(int *)(rx_buf + 4));
  printf("0x%08x ", *(int *)(rx_buf + 8));
  printf("0x%08x ", *(int *)(rx_buf + 12));

  printf("Tx buf\n");
  printf("0x%08x ", *(int *)(tx_buf));
  printf("0x%08x ", *(int *)(tx_buf + 4));
  printf("0x%08x ", *(int *)(tx_buf + 8));
  printf("0x%08x ", *(int *)(tx_buf + 12)); */

  return 1;
  
}
 

/* lets map the pinmux registers */
int upp_pinmux(void) 
{
	int mux_val;
	/*unlock the sysconfig registers */
	mux_val=( *(uint32_t *)(pinmux_base+KICK0R));
	printf("Original KICK0: %#010x\n",mux_val); 

	mux_val=( *(uint32_t *)(pinmux_base+KICK1R));
	printf("Original KICK1: %#010x\n",mux_val); 
	( *(uint32_t *)(pinmux_base+KICK0R)) = KICK0UL;
	( *(uint32_t *)(pinmux_base+KICK1R)) = KICK1UL;
	mux_val=( *(uint32_t *)(pinmux_base+KICK0R));
	printf("Changed KICK0: %#010x\n",mux_val); 

	mux_val=( *(uint32_t *)(pinmux_base+KICK1R));
	printf("Changed KICK1: %#010x\n",mux_val); 


	mux_val = ( *(uint32_t *)(pinmux_base+PINMUX13));
	printf("Original PINMUX13: %#010x\n",mux_val); 
	mux_val |= REG13;
	(*(uint32_t *)(pinmux_base+PINMUX13))=0x00;
	//	(*(uint32_t *)(pinmux_base+PINMUX13))=mux_val;
	mux_val = ( *(uint32_t *)(pinmux_base+PINMUX13));
	printf("PINMUX13: %#010x\n",mux_val); 
	(*(uint32_t *)(pinmux_base+PINMUX14))=REG14;
	(*(uint32_t *)(pinmux_base+PINMUX15))=REG15;
	mux_val = ( *(uint32_t *)(pinmux_base+PINMUX15));
	printf("PINMUX15: %#010x\n",mux_val); 
	printf("We want to set 15 to: %#010x\n",REG15); 

	(*(uint32_t *)(pinmux_base+PINMUX16))=REG16;
	(*(uint32_t *)(pinmux_base+PINMUX17))=REG17;
	(*(uint32_t *)(pinmux_base+PINMUX18))=REG18;
	mux_val = ( *(uint32_t *)(pinmux_base+PINMUX15));
	printf("PINMUX15: %#010x\n",mux_val); 

	return 1;

}

static int upp_buff_init(void)
{
    int ret;
    int ps = getpagesize();

    if ( (ret = posix_memalign( &rx_buf, ps, UPP_BUF_SIZE) ) ) {
        perror("Memalign failed");
        return -1;
    }

    if ( (ret = posix_memalign( &tx_buf, ps, UPP_BUF_SIZE) ) ) {
        perror("Memalign failed");
        return -1;
    }

    memset( (void *)tx_buf, 0xab, UPP_BUF_SIZE );
    memset( (void *)rx_buf, 0xDE, UPP_BUF_SIZE );

#if DEBUG_UPP
    printf("Tx buf addr = 0x%08x (0x%08x)\n", (void *)tx_buf, *(int *)tx_buf);
    printf("Rx buf addr = 0x%08x (0x%08x)\n", (void *)rx_buf, *(int *)rx_buf);
#endif

    return 0;
}

static int set_dma(void)
{

  int reg_stat;

  /*--------------------------------------------------------------------------
   *  UPID0 - Channel A (xmit)
   *--------------------------------------------------------------------------*/
  
  *(uint32_t *)(upp_base + UPID0) = *(uint32_t *)(tx_buf);
  reg_stat = *(uint32_t *)(upp_base + UPID0);
#if DEBUG_UPP
  printf("UPID0 = 0x%08x\n", reg_stat);
#endif
  /*--------------------------------------------------------------------------
   * UPID1 - Channel A (xmit)
   *--------------------------------------------------------------------------*/
     *(uint32_t *)(upp_base + UPID1) = (0x4 << 16 | 0x400);
    reg_stat = *(uint32_t *)(upp_base + UPID1);
#if DEBUG_UPP
    printf("UPID1 = 0x%08x\n", reg_stat);
#endif
    /*--------------------------------------------------------------------------
     * UPID2 - Channel A (xmit)
     *--------------------------------------------------------------------------*/
    *(uint32_t *)(upp_base + UPID2) = 0x00000400;
    reg_stat = *(uint32_t *)(upp_base + UPID2);
#if DEBUG_UPP
    printf("UPID2 = 0x%08x\n", reg_stat);
#endif
    /*--------------------------------------------------------------------------
     * UPQD0 - Channel B (Rcv)
     *--------------------------------------------------------------------------*/
    *(uint32_t *)(upp_base + UPQD0) = *(uint32_t *)(tx_buf);
    reg_stat = *(uint32_t *)(upp_base + UPQD0);
#if DEBUG_UPP 
    printf("UPQD0 = 0x%08x\n", reg_stat);
#endif

    /*--------------------------------------------------------------------------
     * UPQD1 - Channel B (Rcv)
     *--------------------------------------------------------------------------*/
     *(uint32_t *)(upp_base + UPQD1) = (0x4 << 16 | 0x400);
    reg_stat = *(uint32_t *)(upp_base + UPQD1);
#if DEBUG_UPP 
    printf("UPQD1 = 0x%08x\n", reg_stat);
#endif

    /*--------------------------------------------------------------------------
     * UPQD2 - Channel B (Rcv)
     *-------------------------------------------------------------------------*/
    *(uint32_t *)(upp_base + UPQD2) = 0x00000400;
    reg_stat = *(uint32_t *)(upp_base + UPQD2);
#if DEBUG_UPP
    printf("UPQD2 = 0x%08x\n", reg_stat);
#endif

}



/* lets map the pinmux registers */
static int upp_mmap_init(void)
{
  int mem_fd;
  int reg_stat;

  mem_fd = open("/dev/mem", O_RDWR);
  if ( mem_fd < 0){
    printf("failed to open /dev/mem\n");
    return mem_fd;
  }
  /* map the pin mux */
  pinmux_size = getpagesize();
  pinmux_base = (uintptr_t)mmap(0,pinmux_size, PROT_READ|PROT_WRITE,
			     MAP_SHARED, mem_fd, SYSBASE);
  if ( pinmux_base == (uintptr_t)MAP_FAILED){
    printf("\n\nupp_base mmap fail!\n\n");
    return -1;
} 

  /* map the uPP */
  upp_mmap_size = getpagesize();
  upp_base = (uintptr_t)mmap(0,upp_mmap_size, PROT_READ|PROT_WRITE,
			     MAP_SHARED, mem_fd, UPPBASE);
  if ( upp_base == (uintptr_t)MAP_FAILED){
    printf("\n\nupp_base mmap fail!\n\n");
    return -1;
  }

  /* map the Power control module */
  psc1_mmap_size = getpagesize();
  psc1_base = (uintptr_t)mmap(0,psc1_mmap_size, PROT_READ|PROT_WRITE,
			     MAP_SHARED, mem_fd, PSC1);
  if ( psc1_base == (uintptr_t)MAP_FAILED){
    printf("\n\nupp_base mmap fail!\n\n");
    return -1;
  }

  close(mem_fd);
}


/* lets unmap the pinmux registers */
static int upp_mmap_deinit(void){

  if (munmap( (void *)pinmux_base, pinmux_size) == -1){
    printf("\n\n munmap fail\n");
    return -1;
  }



  if (munmap( (void *)upp_base, upp_mmap_size) == -1){
    printf("\n\n munmap fail\n");
    return -1;
  }

  if (munmap( (void *)psc1_base, psc1_mmap_size) == -1){
    printf("\n\n munmap fail\n");
    return -1;
  }

}

int main(void)
{
	int fd;
	char *mem;
	unsigned short *reg;
	int i;
	int id;
	int reg_stat;
	unsigned short val;




	/* map the pinmux registers */
	upp_mmap_init();
	
	reg_stat = ( *(uint32_t *)(psc1_base)); 
	printf("Peripheral Revision is: %#010x\n",reg_stat);

	reg_stat = ( *(uint32_t *)(psc1_base+PSTAT)); 
	printf("PSTAT is: %#010x\n",reg_stat);

	reg_stat = 0x03;
	( *(uint32_t *)(psc1_base+UPPCONTROL))= reg_stat;
	printf("uPP PSC status: %#010x\n",reg_stat); 

	reg_stat = ( *(uint32_t *)(psc1_base+PSCMD)); 
	reg_stat &= 0xfffffffc;
	reg_stat |= 0x03;
	( *(uint32_t *)(psc1_base+PSCMD))= reg_stat;
	
	while ( *(uint32_t *)(psc1_base + PSTAT) & 0x01)
	  usleep(2);

	reg_stat = ( *(uint32_t *)(psc1_base+UPPSTATUS));
	printf("uPP PSC status: %#010x\n",reg_stat); 
	
	
	reg_stat = ( *(uint32_t *)(upp_base) );
	printf("uPP Peripheral ID register: %#010x\n",reg_stat); 



	/* mux the upp */
	upp_pinmux();

	/* now enbale upp loopback */
	/*upp_enable();*/
	upp_reset();

	/* configure the upp for loopback */
	upp_config();

	/* setup the upp xmit and rcv buffers */
	upp_buff_init();



	/* enable the device */
	*(uint32_t *)(upp_base + UPPCR) = (0x08);
	reg_stat = ( *(uint32_t *)(upp_base+UPPCR) );
	printf("upp Control now: %#010x\n",reg_stat); 	

	reg_stat = ( *(uint32_t *)(psc1_base+UPIS0)); 
	printf("Chan I status0: %#010x\n",reg_stat);
	reg_stat = ( *(uint32_t *)(psc1_base+UPIS1)); 
	printf("Chan I status1: %#010x\n",reg_stat);
	reg_stat = ( *(uint32_t *)(psc1_base+UPIS2)); 
	printf("Chan I status2: %#010x\n",reg_stat);
	reg_stat = ( *(uint32_t *)(psc1_base+UPQS0)); 
	printf("Chan Q status0: %#010x\n",reg_stat);
	reg_stat = ( *(uint32_t *)(psc1_base+UPQS1)); 
	printf("Chan Q status1: %#010x\n",reg_stat);
	reg_stat = ( *(uint32_t *)(psc1_base+UPQS2)); 
	printf("Chan Q status2: %#010x\n",reg_stat);

 	/* set the dma transfers */
	set_dma();

	reg_stat = ( *(uint32_t *)(psc1_base+UPIES)); 
	printf("UPIES: %#010x\n",reg_stat);

	/*	for (i=0;i<2048;i++){
	  *(tx_buf+i)=i;
	  }*/

	reg_stat = ( *(uint32_t *)(psc1_base+UPIES)); 
	printf("UPIES: %#010x\n",reg_stat);


	reg_stat = ( *(uint32_t *)(psc1_base+UPIS0)); 
	printf("Chan I status0: %#010x\n",reg_stat);
	reg_stat = ( *(uint32_t *)(psc1_base+UPIS1)); 
	printf("Chan I status1: %#010x\n",reg_stat);
	reg_stat = ( *(uint32_t *)(psc1_base+UPIS2)); 
	printf("Chan I status2: %#010x\n",reg_stat);
	reg_stat = ( *(uint32_t *)(psc1_base+UPQS0)); 
	printf("Chan Q status0: %#010x\n",reg_stat);
	reg_stat = ( *(uint32_t *)(psc1_base+UPQS1)); 
	printf("Chan Q status1: %#010x\n",reg_stat);
	reg_stat = ( *(uint32_t *)(psc1_base+UPQS2)); 
	printf("Chan Q status2: %#010x\n",reg_stat);

	reg_stat = ( *(uint32_t *)(psc1_base+UPIES)); 
	printf("UPIES: %#010x\n",reg_stat);




	/* unmap the pinmux registers */
	upp_mmap_deinit();

	return EXIT_SUCCESS;
}