///////////////////////////////////////
/// 640x480 version!
/// test VGA with hardware video input copy to VGA
// compile with
// gcc pio_test_speed.c -o pio_speed 
///////////////////////////////////////
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/ipc.h> 
#include <sys/shm.h> 
#include <sys/mman.h>
#include <sys/time.h> 
#include <math.h> 

// === main bus; PIO
#define FPGA_AXI_BASE 	0xC0000000
#define FPGA_AXI_SPAN   0x00001000
// main axi bus base
void *h2p_virtual_base;
volatile unsigned int * axi_pio_ptr = NULL ;
volatile unsigned int * axi_pio_read_ptr = NULL ;

// === lw bus; PIO
#define FPGA_LW_BASE 	0xff200000
#define FPGA_LW_SPAN	0x00001000
// the light weight bus base
void *h2p_lw_virtual_base;
//  HPS_to_FPGA FIFO status address = 0
volatile unsigned int * lw_pio_ptr = NULL ;
volatile unsigned int * lw_pio_read_ptr = NULL ;
// read offset is 0x10 for both busses
// remember that eaxh axi master bus needs unique address
#define FPGA_PIO_READ	0x10
#define FPGA_PIO_WRITE	0x00

// === direct gpio to FPGA
#define FPGA_GP_BASE 0xFF706000
#define FPGA_GP_SPAN 0x1000
#define FPGA_GP_GPO  0x10
#define FPGA_GP_GPI  0x14
#define FPGA_STAT 	 0x00
// gpio base
void *h2f_gp_virtual_base;
// HPS_to_FPGA gpio
volatile unsigned int * gp_gpo_ptr = NULL ;
volatile unsigned int * gp_gpi_ptr = NULL ;
volatile unsigned int * fpga_stat_ptr = NULL ;


// /dev/mem file id
int fd;	
	
int main(void)
{

	// Declare volatile pointers to I/O registers (volatile 	
	// means that IO load and store instructions will be used 	
	// to access these pointer locations,  
  
	// === get FPGA addresses ==================
    // Open /dev/mem
	if( ( fd = open( "/dev/mem", ( O_RDWR | O_SYNC ) ) ) == -1 ) 	{
		printf( "ERROR: could not open \"/dev/mem\"...\n" );
		return( 1 );
	}
    
	// ===========================================
	// get virtual address for
	// gpio base
	h2f_gp_virtual_base = mmap( NULL, FPGA_GP_SPAN, ( PROT_READ | PROT_WRITE ), MAP_SHARED, fd, FPGA_GP_BASE); 	
	if( h2f_gp_virtual_base == MAP_FAILED ) {
		printf( "ERROR: mmap3() failed...\n" );
		close( fd );
		return(1);
	}
    // Get the addresses that map to the gpio port 
	gp_gpo_ptr =(unsigned int *)(h2f_gp_virtual_base + FPGA_GP_GPO);
	gp_gpi_ptr =(unsigned int *)(h2f_gp_virtual_base + FPGA_GP_GPI);
	fpga_stat_ptr = (unsigned int *)(h2f_gp_virtual_base + FPGA_STAT);
	//============================================
	
	//============================================
    // get virtual addr that maps to physical
	// for light weight AXI bus
	h2p_lw_virtual_base = mmap( NULL, FPGA_LW_SPAN, ( PROT_READ | PROT_WRITE ), MAP_SHARED, fd, FPGA_LW_BASE );	
	if( h2p_lw_virtual_base == MAP_FAILED ) {
		printf( "ERROR: mmap1() failed...\n" );
		close( fd );
		return(1);
	}
	// Get the addresses that map to the two parallel ports on the light-weight bus
	lw_pio_ptr = (unsigned int *)(h2p_lw_virtual_base);
	lw_pio_read_ptr = (unsigned int *)(h2p_lw_virtual_base + FPGA_PIO_READ);
	
	//============================================
	
	// ===========================================
	// get virtual address for
	// AXI bus addr 
	h2p_virtual_base = mmap( NULL, FPGA_AXI_SPAN, ( PROT_READ | PROT_WRITE ), MAP_SHARED, fd, FPGA_AXI_BASE); 	
	if( h2p_virtual_base == MAP_FAILED ) {
		printf( "ERROR: mmap3() failed...\n" );
		close( fd );
		return(1);
	}
    // Get the addresses that map to the two parallel ports on the AXI bus
	axi_pio_ptr =(unsigned int *)(h2p_virtual_base);
	axi_pio_read_ptr =(unsigned int *)(h2p_virtual_base + FPGA_PIO_READ);
	//============================================
	
	// measure time
	struct timeval t1, t2;
	double elapsedTime;
	
	while(1) 
	{
		int num, pio_read;
		int junk; 
		
		// input a number
		junk = scanf("%d", &num);
		// send to PIOs
		*(lw_pio_ptr)  = num ;
		*(axi_pio_ptr) = num ;
		*(gp_gpo_ptr)  = num ;
		
		int i;
		// ==================
		// start timer
		gettimeofday(&t1, NULL);
		for(i=0; i<1000000; i++){
			*(lw_pio_ptr)  = *(lw_pio_read_ptr) ;
		}
		// stop timer
		gettimeofday(&t2, NULL);
		elapsedTime = (t2.tv_sec - t1.tv_sec)*1000000 + (t2.tv_usec - t1.tv_usec) ;    
		printf("T_lw = %f uSec  \n\r", elapsedTime);
		
		// ==================
		// start timer
		gettimeofday(&t1, NULL);
		for(i=0; i<1000000; i++){
			*(axi_pio_ptr)  = *(axi_pio_read_ptr) ;
		}
		// stop timer
		gettimeofday(&t2, NULL);
		elapsedTime =  (t2.tv_sec - t1.tv_sec)*1000000 + (t2.tv_usec - t1.tv_usec) ;    
		printf("T_axi = %f uSec  \n\r", elapsedTime);
		
		// ==================
		// start timer
		gettimeofday(&t1, NULL);
		for(i=0; i<1000000; i++){
			*(gp_gpo_ptr)  = *(gp_gpi_ptr) ;
		}
		// stop timer
		gettimeofday(&t2, NULL);
		elapsedTime = (t2.tv_sec - t1.tv_sec)*1000000 + (t2.tv_usec - t1.tv_usec) ;    
		printf("T_gp = %f uSec  \n\r", elapsedTime);
		
		// ==================
		
		// send to PIOs
		//*(lw_pio_ptr)  = num ;
		//*(axi_pio_ptr) = num ;
		//*(gp_gpo_ptr)  = num ;
		
		// receive back and print
		// low 3 bits of stat is 4 if in FPGA user mode
		printf("pio in=%d %d %d \n\r",
		*(lw_pio_read_ptr), *(axi_pio_read_ptr), *(gp_gpi_ptr) ) ;
		
	} // end while(1)
} // end main

/// /// ///////////////////////////////////// 
/// end /////////////////////////////////////