/* support routines for doing gpios in userspace - oliford*/

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
#include <signal.h>
#include <fcntl.h>
#include <ctype.h>
#include <termios.h>
#include <sys/types.h>
#include <sys/mman.h>
  
#define FATAL do { fprintf(stderr, "Error at line %d, file %s (%d) [%s]\n", \
  __LINE__, __FILE__, errno, strerror(errno)); exit(1); } while(0)
 
#define MAP_SIZE 4096UL
#define MAP_MASK (MAP_SIZE - 1)

#define	GPIOBASE	0x40E00000  /* where we request the map */
#define	UARTBASE	0x40100000  /* where we request the map */
#define	MFPBASE		0x40E10000  /* where we request the map */

#define GPLR0		0x40E00000  /* GPIO Pin-Level Register GPIO<31:0> */
#define GPLR1		0x40E00004  /* GPIO Pin-Level Register GPIO<63:32> */
#define GPLR2		0x40E00008  /* GPIO Pin-Level Register GPIO<80:64> */

#define GPDR0		0x40E0000C  /* GPIO Pin Direction Register GPIO<31:0> */
#define GPDR1		0x40E00010  /* GPIO Pin Direction Register GPIO<63:32> */
#define GPDR2		0x40E00014  /* GPIO Pin Direction Register GPIO<80:64> */

#define GPSR0		0x40E00018  /* GPIO Pin Output Set Register GPIO<31:0> */
#define GPSR1		0x40E0001C  /* GPIO Pin Output Set Register GPIO<63:32> */
#define GPSR2		0x40E00020  /* GPIO Pin Output Set Register GPIO<80:64> */

#define GPCR0		0x40E00024  /* GPIO Pin Output Clear Register GPIO<31:0> */
#define GPCR1		0x40E00028  /* GPIO Pin Output Clear Register GPIO <63:32> */
#define GPCR2		0x40E0002C /* GPIO Pin Output Clear Register GPIO <80:64> */

#define GPLR3		0x40E00100  /* GPIO Pin-Level Register GPIO<127:96> */
#define GPDR3		0x40E0010C  /* GPIO Pin Direction Register GPIO<127:96> */
#define GPSR3		0x40E00118  /* GPIO Pin Output Set Register GPIO<127:96> */
#define GPCR3		0x40E00124  /* GPIO Pin Output Clear Register GPIO<127:96> */

#define NREGS	4


//relative addresses
off_t gpioLevelReg[] = { GPLR0 - GPIOBASE, GPLR1 - GPIOBASE, GPLR2 - GPIOBASE, GPLR3 - GPIOBASE };
off_t gpioDirReg[]   = { GPDR0 - GPIOBASE, GPDR1 - GPIOBASE, GPDR2 - GPIOBASE, GPDR3 - GPIOBASE };
off_t gpioSetReg[]   = { GPSR0 - GPIOBASE, GPSR1 - GPIOBASE, GPSR2 - GPIOBASE, GPSR3 - GPIOBASE };
off_t gpioClearReg[] = { GPCR0 - GPIOBASE, GPCR1 - GPIOBASE, GPCR2 - GPIOBASE, GPCR3 - GPIOBASE };

void *gpio_map_base, *gpio_virt_base; 
void *uart_map_base, *uart_virt_base;
void *mfp_map_base, *mfp_virt_base;
int fd;
int dbg;
	
//get mapping for gpio and list basic info,
void gpioInit(int verbose){
	unsigned long level,dir;
	dbg = verbose;
	off_t gpioTarget = GPIOBASE;
	off_t uartTarget = UARTBASE;
	off_t mfpTarget = MFPBASE;
	int i;
	unsigned long *ptr;

	if((fd = open("/dev/mem", O_RDWR | O_SYNC)) == -1) FATAL;
	if(dbg){
		printf("gpioInit: /dev/mem opened.\n"); 
		fflush(stdout);
	}
    
	/* Map one page */
	gpio_map_base = mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, gpioTarget & ~MAP_MASK);
	if(gpio_map_base == (void *) -1) FATAL;
	if(dbg){
		printf("gpioInit: GPIO Memory mapped at address %p.\n", gpio_map_base); 
		fflush(stdout);
	}
    
	uart_map_base = mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, uartTarget & ~MAP_MASK);
	if(uart_map_base == (void *) -1) FATAL;
	if(dbg){
		printf("gpioInit: UART Memory mapped at address %p.\n", uart_map_base); 
		fflush(stdout);
	}

	mfp_map_base = mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, mfpTarget & ~MAP_MASK);
	if(mfp_map_base == (void *) -1) FATAL;
	if(dbg){
		printf("gpioInit: MFP Memory mapped at address %p.\n", mfp_map_base); 
		fflush(stdout);
	}
    
	gpio_virt_base = gpio_map_base + (gpioTarget & MAP_MASK);
	uart_virt_base = uart_map_base + (uartTarget & MAP_MASK);
	mfp_virt_base = mfp_map_base + (mfpTarget & MAP_MASK);

	if(dbg){
		printf("gpioInit: Init, gpio virtual base at %p (phys=0x%X)\n", gpio_virt_base, gpioTarget); 
		printf("gpioInit: Init, uart virtual base at %p (phys=0x%X)\n", uart_virt_base, uartTarget); 
		printf("gpioInit: Init, mfp  virtual base at %p (phys=0x%X)\n", mfp_virt_base, mfpTarget); 
		fflush(stdout);
	}
	

	for(i=0;i<NREGS;i++){
		ptr = gpio_virt_base + gpioLevelReg[i]; 
		level = *ptr;
		ptr = gpio_virt_base + gpioDirReg[i]; 
		dir = *ptr;
		if(dbg)
			printf("gpioInit: GPIO register %i: Level=0x%08X Dir=0x%08X\n",i, level, dir); 
	}
	if(dbg)
		fflush(stdout);
	
	
	return;
}

void gpioCleanup(){
	if(munmap(gpio_map_base, MAP_SIZE) == -1) FATAL;
	if(munmap(uart_map_base, MAP_SIZE) == -1) FATAL;	
	if(munmap(mfp_map_base, MAP_SIZE) == -1) FATAL;	
	close(fd);
}

unsigned long getUARTReg(int reg){ 
	volatile unsigned long *ptr;

	ptr = uart_virt_base + (off_t)(4 * reg);
	//printf("Getting UART register %i from %p\n",reg,ptr);

	return *ptr;
}

void setUARTReg(int reg, unsigned long val){ 
	volatile unsigned long *ptr;

	ptr = uart_virt_base + (off_t)(4 * reg);
	//printf("Setting UART register %i at %p\n",reg,ptr);

	*ptr = val;	
}

void mfpSet( unsigned long offset,  unsigned long val){	
	volatile unsigned long *ptr;

	ptr = mfp_virt_base + (off_t)offset;
	printf("Setting MFP register at %p to 0x%08x\n",ptr,val);

	*ptr = val;	
}

unsigned long mfpGet(unsigned long offset){
	volatile unsigned long *ptr;

	ptr = mfp_virt_base + (off_t)offset;	
	printf("getting the MFP register at %p (its 0x%08x)\n",ptr,*ptr);

	return *ptr;
}


void gpioDumpState(int gpio){
	int reg = gpio / 32;
	int bit = gpio % 32;
	unsigned long mask = 1 << bit;
	volatile unsigned long *ptrLevel;
	volatile unsigned long *ptrDir;
	unsigned long levels=0;
	unsigned long dirs=0;
	
	if(dbg){
		printf("gpioDumpState: Reading reg %i bit %i\n",reg,bit);
		fflush(stdout);
	}

	ptrLevel = gpio_virt_base + gpioLevelReg[reg];
	ptrDir = gpio_virt_base + gpioDirReg[reg];

	levels =  *ptrLevel;
	dirs = *ptrDir;
	
	if(dbg){
		printf("gpioDumpState: State of GPIO %i: Level: 0x%08X (from 0x%08X at %p), Direction: 0x%08X (from 0x%08X at %p)\n",
				(levels&mask),levels,ptrLevel,(dirs&mask),dirs,ptrDir);
		fflush(stdout);
	}
}

int gpioGet(int gpio){
	int reg = gpio / 32;
	int bit = gpio % 32;
	unsigned long mask = 1 << bit;
	volatile unsigned long *ptr;
	unsigned long val;

	//printf("gpioGet: Reading reg %i bit %i\n",reg,bit);

	ptr = gpio_virt_base + gpioLevelReg[reg];
	
	//printf("gpioGet: Getting GPIO %i (by reading %p)\n",gpio,ptr);
	return (*ptr & mask) == 0 ? 0 : 1; //explicitly return 0 or 1
}

void gpioSet(int gpio, int high){
	int reg = gpio / 32;
	int bit = gpio % 32;
	unsigned long mask = 1 << bit;
	volatile unsigned long *ptr;
	unsigned long val;

	//printf("gpioSet: Writing reg %i bit %i\n",reg,bit);

	if(high)
		ptr = gpio_virt_base + gpioSetReg[reg];
	else
		ptr = gpio_virt_base + gpioClearReg[reg];

	//printf("gpioSet: Setting GPIO %i %s (by writing %08X to %p)\n",gpio,(high?"high":"low"),mask,ptr);
	*ptr = mask;
}

int gpioGetDir(int gpio){
	int reg = gpio / 32;
	int bit = gpio % 32;
	unsigned long mask = 1 << bit;
	volatile unsigned long *ptr;
	unsigned long val;

	//printf("gpioGet: Reading reg %i bit %i\n",reg,bit);

	ptr = gpio_virt_base + gpioDirReg[reg];
	
	//printf("gpioGet: Getting GPIO %i (by reading %p)\n",gpio,ptr);
	return (*ptr & mask) == 0 ? 0 : 1; //explicitly return 0 or 1
}

void gpioSetDir(int gpio, int output){
	int reg = gpio / 32;
	int bit = gpio % 32;
	unsigned long mask = 1 << bit;
	volatile unsigned long *ptr;
	unsigned long val;

	ptr = gpio_virt_base + gpioDirReg[reg];
		
	printf("setting dir of gpio %i to %i: ptr=%p, ",gpio,output,ptr);
	printf("before = %08x, ",*ptr);

	if(output){
		*ptr |= mask;
	}else{
		*ptr &= ~mask;
	}

	printf("after = %08x\n",*ptr);
	
}