#include <stdio.h>
#include <stdlib.h>
#include <sys/mman.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <errno.h>
#include <stdint.h>

#define MEMTEST_MAGIC		'M'
#define MEMTEST_DMA_SIZE	_IO(MEMTEST_MAGIC, 0)
#define MEMTEST_DMA_PADDR	_IO(MEMTEST_MAGIC, 1)
#define MEMTEST_DMA_VADDR	_IO(MEMTEST_MAGIC, 2)

#define DEVDIAG			"/dev/memtest"
#define DEVMEM			"/dev/mem"
uint32_t get_size(void);
unsigned long get_paddr(void);
unsigned int * mmap_memtest(unsigned long paddr, uint32_t size);
unsigned int * mmap_mem(unsigned long paddr, uint32_t size);

uint32_t get_size(void)
{
	int diagfd, rv;
	uint32_t size;

	diagfd = open( DEVDIAG, O_RDWR | O_SYNC | O_DSYNC | O_RSYNC );
	if (diagfd < 0)
	{
		perror("Error : fail to open" DEVDIAG);
    		return 0;
  	}

	rv = ioctl( diagfd, MEMTEST_DMA_SIZE, &size);
    	if (rv < 0)
	{
	      	perror("Fail to perform ioctl");
      		return 0;
    	}
	close(diagfd);

	return size;
}

unsigned long get_paddr(void)
{
	int diagfd, rv;
	unsigned long paddr;

	diagfd = open( DEVDIAG, O_RDWR | O_SYNC | O_DSYNC | O_RSYNC );
	if (diagfd < 0)
	{
		perror("Error : fail to open" DEVDIAG);
    		return 0;
  	}
	rv = ioctl( diagfd, MEMTEST_DMA_PADDR, &paddr);
    	if ( rv < 0 ) {
	      	perror("Fail to perform ioctl");
      		return 0;
    	}
	close(diagfd);

	return paddr;
}

unsigned int * mmap_memtest(unsigned long paddr, uint32_t size)
{
	int diagfd;
	unsigned int page_size = getpagesize();
	unsigned int page_mask = ~(page_size - 1);
	unsigned int *vaddr = NULL;

	/** test mmap */
	if ( paddr & ~page_mask ) {
		printf("Error : not algined %#lxx & %08x\n", paddr, ~page_mask );
		return NULL;
	}

	diagfd = open( DEVDIAG, O_RDWR | O_SYNC | O_DSYNC | O_RSYNC );
	if (diagfd < 0)
	{
		printf("Error : fail to open "DEVDIAG);
    		return NULL;
  	}


	vaddr =  (unsigned int*) mmap(NULL, size, PROT_READ, MAP_SHARED, diagfd, paddr);
	close(diagfd);

	return vaddr;
}

unsigned int * mmap_mem(unsigned long paddr, uint32_t size)
{
	int memfd;
	unsigned int page_size = getpagesize();
	unsigned int page_mask = ~(page_size - 1);
	unsigned int *vaddr = NULL;

	/** test mmap */
	if ( paddr & ~page_mask ) {
		printf("Error : not algined %#lxx & %08x\n", paddr, ~page_mask );
		return NULL;
	}

	memfd = open(DEVMEM, O_RDWR |  O_SYNC | O_DSYNC | O_RSYNC);
	if (memfd < 0)
	{
		perror("Error : fail to open "DEVMEM);
		return NULL;
	}

	vaddr =  (unsigned int*) mmap(NULL, size, PROT_READ, MAP_SHARED, memfd, paddr);
	close(memfd);

	return vaddr;
}

int main(int argc, char **argv)
{
	uint32_t size = get_size();
	unsigned long paddr = get_paddr();
	unsigned int *mem = mmap_mem(paddr, size);
	unsigned int *map = mmap_memtest(paddr, size);

  	printf("paddr = %#lx\n", paddr);
	printf("  mem = %p\n", mem);
	printf("  map = %p\n", map);

	if (map)
	{
		printf("map[0]= %x\n", map[0]);
		printf("map[1]= %x\n", map[1]);
	}

	if (mem)
	{
		printf("mem[0]= %x\n", mem[0]);
		printf("mem[1]= %x\n", mem[1]);
	}

	  return 0;
}