#include<unistd.h>
#include<sys/types.h>
#include<sys/mman.h>
#include<sys/time.h>
#include<stdio.h>
#include<fcntl.h>
#include<string.h>
#include <stdlib.h>

#define GPIOBASE        0x80840000
#define SYSCONBASE	0x80930000
#define MODELBASE       0x22000000
#define DEVICE_CFG	(0x80 / sizeof(unsigned int))
#define SFTWR_LCK       (0xC0 / sizeof(unsigned int))
#define TS7300  0x03
#define PBDR    (0x04 / sizeof(unsigned int))
#define PBDDR   (0x14 / sizeof(unsigned int))
#define PBMASK  0xFB
#define PFDR    (0x30 / sizeof(unsigned int))
#define PFDDR   (0x34 / sizeof(unsigned int))
#define PFMASK  0x02

// Use 0 for busy-wait
#define POLL_FREQ	30

// Use 0 for busy-wait
#define DEBOUNCE_POLL_FREQ	256

#define DEBOUNCE_STABLE_MS	15//10
#define REPEAT_FREQ	30
#define REPEAT_DELAY_MS	500

// struct timeval utility macros
#define TV_ADD_MS(x, y)	{ \
	(x).tv_usec += (y) * 1000; \
	(x).tv_sec += (x).tv_usec / 1000000; \
	(x).tv_usec = (x).tv_usec % 1000000; \
}

#define TV_GTE(x, y) ((x).tv_sec > (y).tv_sec || ((x).tv_sec == (y).tv_sec && \
	(x).tv_usec >= (y).tv_usec)) 

#define TV_ELAPSED_MS(x, y)	((((x).tv_sec - (y).tv_sec) * 1000000 + \
	((x).tv_usec - (y).tv_usec)) / 1000)

unsigned int get_keys(void);
unsigned int debounce(unsigned int);
void keypad_event(unsigned int, unsigned int);
void keypad_init(void);

static unsigned int lastkey = -1;
static struct timeval repeat_time;

static unsigned char keys[] = {
  '1', '2', '3', 'A',
  '4', '5', '6', 'B',
  '7', '8', '9', 'C',
  '*', '0', '#', 'D',
};

volatile unsigned int *model_ptr;
unsigned int model;
volatile unsigned int *gpio, *syscon;
volatile unsigned int *pfdr;
volatile unsigned int *pfddr;
volatile unsigned int *pbdr;
volatile unsigned int *pbddr;

int main(int argc, char **argv) {
	unsigned int k = 0, pressed = 0; 

	keypad_init();

	setvbuf(stdout, NULL, _IONBF, 0);

	for(;;) {
		struct timeval now;
		unsigned int changed;

		k = get_keys();
		changed = pressed ^ k;
		if (changed) {
			k = debounce(changed);
			changed &= pressed ^ k;
		}
		if (changed) {
			keypad_event(changed & k, changed & pressed);
			pressed &= ~(changed & pressed);
			pressed |= changed & k;
		}
		
		// Do key repeat as necessary
		if (lastkey != -1) {
			gettimeofday(&now, NULL);
			if (TV_GTE(now, repeat_time)) {
				printf("%c", lastkey);
				TV_ADD_MS(repeat_time, REPEAT_FREQ / 1000);
			}
		}

		if (POLL_FREQ) usleep(1000000 / POLL_FREQ);
	}
}

void keypad_init(void)
{

	int fd = open("/dev/mem", O_RDWR|O_SYNC);

	if (fd == -1) {
		perror("/dev/mem");
		exit(2);
	}

        gpio = (unsigned int *)mmap(0, getpagesize(),
	  PROT_READ|PROT_WRITE, MAP_SHARED, fd, GPIOBASE);
	model_ptr = (unsigned int *)mmap(0, getpagesize(),
	  PROT_READ|PROT_WRITE, MAP_SHARED, fd, MODELBASE);
	syscon = (unsigned int *)mmap(0, getpagesize(),
	  PROT_READ|PROT_WRITE, MAP_SHARED, fd, SYSCONBASE);
	model = *model_ptr & 0x03;

	//Disable DMA on GPIO
	*(syscon + SFTWR_LCK) = 0xAA;
	*(syscon + DEVICE_CFG)  = 0x08140d00;
	//*(syscon + DEVICE_CFG)  &= ~0x60000000;

	pbdr = &gpio[PBDR];
	pbddr = &gpio[PBDDR];

	if(model == TS7300) {
		pfdr = &gpio[PFDR];
		pfddr = &gpio[PFDDR];

		*pfddr = *pfddr | PFMASK; 
	}
	
	*pbddr = 0xf | PBMASK; 

}

void keypad_event(unsigned int on, unsigned int off) {
	unsigned int i;

	lastkey = -1;
	for(i = 0; i < sizeof(keys) / sizeof(keys[0]); i++) {
		if (on & (1 << i)) {
			printf("%c", keys[i]);
			lastkey = keys[i];
		}
	}
	if (REPEAT_DELAY_MS && lastkey != -1) {
		gettimeofday(&repeat_time, NULL);
		TV_ADD_MS(repeat_time, REPEAT_DELAY_MS);
	}
}

unsigned int get_keys(void) {
	unsigned int pos, on = 0;

	if(model == TS7300)
	{

		for(pos = 0; pos < 4; pos++) {
			if(pos == 2) //DIO_2 is on PortF
			{
				*pbddr &= ~PBMASK;
				*pfddr |= PFMASK;
				*pfdr &= ~PFMASK;
				
			} else
			{
				*pfddr &= ~PFMASK; 
				*pbddr &= ~PBMASK;
				*pbddr |= (1 << pos);

				*pbdr |= PBMASK;
				*pbdr &= ~(1 << pos);
			}
			on |= (~(*pbdr >> 4) & 0xf) << (4 * pos);
		}

	} else {				

		for(pos = 0; pos < 4; pos++) {
			*pbddr = (1 << pos);
			*pbdr = ~(1 << pos);
			on |= (~(*pbdr >> 4) & 0xf) << (4 * pos);
		}
	}
	return on;
}

// debounce will return when masked keys have been stable for DEBOUNCE_STABLE_MS
unsigned int debounce(unsigned int mask) {
	struct timeval start, now;
	unsigned int last_val, val;
	
	last_val = get_keys(); 
	gettimeofday(&start, NULL);
	do {
		val = get_keys();
		gettimeofday(&now, NULL);
		if ((val ^ last_val) & mask) 
		  memcpy(&start, &now, sizeof(struct timeval));
		last_val = val;
		if (DEBOUNCE_POLL_FREQ) usleep(1000000 / DEBOUNCE_POLL_FREQ);
	} while (TV_ELAPSED_MS(now, start) <= DEBOUNCE_STABLE_MS);
	
	return val;
}