/***********************************************************************
 *
 * (C) Copyright 2002
 * DENX Software Engineering
 * Wolfgang Denk, wd@denx.de
 * All rights reserved.
 *
 ***********************************************************************/

#include <linux/module.h>
#include <linux/version.h>
#include <linux/init.h>
#include <linux/poll.h>
#include <asm/irq.h>
#include <asm/types.h>
#include <asm/io.h>
#include <asm/hardware.h>
#include <linux/proc_fs.h>
#include <linux/fs.h>

#define	DRIVER_VERSION		"$Revision: 1.12 $"

/*#define	DEBUG*/

#ifdef	DEBUG
# define DBG(fmt,args...)	printk(fmt ,##args)
#else
# define DBG(fmt,args...)
#endif


/* The following definition defines whether the ADC operates
   in polling or interrupt-driven mode, default is
   interrupt-driven. */
#undef ADC_POLL_WAIT

/* for the latest CPLD version */
#define NEW_CPLD

/*
 * Prototypes
 */

static int trabio_open (struct inode *inode, struct file *file);
static ssize_t trabio_read (struct file *file,
					char *buf, size_t count, loff_t *ppos);
static ssize_t trabio_write (struct file *file,
					const char *buf, size_t count, loff_t *ppos);
static unsigned int trabio_poll (struct file *file, poll_table *wait);


#define	DEV_MAJOR	242

#define DIP_SWITCH_MINOR  0
#define DIGITAL_IN_MINOR  1
#define KEY_IN_MINOR      2
#define KEY_OUT_MINOR     3
#define DIGITAL_OUT_MINOR 4
#define JTAG_CPLD_MINOR   5

#define MINOR_MAX	JTAG_CPLD_MINOR




#define DEVICE_NAME "trabio"
#define DRIVER_NAME "TRAB Digital I/O driver"

static struct file_operations trabio_fops = {
	owner:		THIS_MODULE,
	open:		trabio_open,
	read:		trabio_read,
	write:		trabio_write,
	poll:		trabio_poll,
};

#define CPLD_BASE_PHYS		0x04000000
#define CPLD_SIZE           0x30000
static unsigned long cpld_base;

/**********************************************************/
/* private DIP_SWITCH definitions and declarations        */
/**********************************************************/
static struct semaphore dip_switch_mutex;
static DECLARE_WAIT_QUEUE_HEAD(adc_dav_wait);
static inline int dip_switch_init(void);
static inline void dip_switch_cleanup(void);
static int dip_switch_read(char *);
#ifndef ADC_POLL_WAIT
static void adc_dav_interrupt(int nr, void *devid, struct pt_regs *regs);
#endif

#ifdef CONFIG_PROC_FS
static int dip_switch_read_proc(char *page, char **start,
						off_t off, int count,
						int *eof, void *data);
#endif
/**********************************************************/
/* private KEY_OUT definitions and declarations           */
/**********************************************************/
static unsigned char kbd_led_status;
struct semaphore key_out_mutex;

static inline int key_out_init(void);
static int key_out_read(char *);
static int key_out_write(char *);
static void led_on_off(int num, int on);
static inline void key_out_cleanup(void);

#ifdef CONFIG_PROC_FS
static int key_out_read_proc(char *page, char **start,
						off_t off, int count,
						int *eof, void *data);
static int key_out_write_proc(struct file *file,
						  const char *buffer,
						  unsigned long count,
						  void *data);
#endif
/**********************************************************/
/* private DIGITAL_OUT definitions and declarations        */
/**********************************************************/
static unsigned char motor_status;
static unsigned char led_status;
static struct semaphore digital_out_mutex;

static inline int digital_out_init(void);
static int digital_out_read(char *);
static int digital_out_write(char *);
static inline void digital_out_cleanup(void);
static void led_motor_on_off(int led, int motor);
#ifdef CONFIG_PROC_FS
static int digital_out_read_proc(char *page, char **start,
						off_t off, int count,
						int *eof, void *data);
static int digital_out_write_proc(struct file *file,
					  const char *buffer,
					  unsigned long count,
					  void *data);
#endif
/**********************************************************/
/* private KEY_IN definitions and declarations            */
/**********************************************************/
static DECLARE_WAIT_QUEUE_HEAD (key_in_wait);
static char key_in_value, key_in_ready;

#define rCPLD_TASTER  *((volatile unsigned long *) (cpld_base + 0x20000))

#define CPLD_TASTER_SHIFT	16
#define CPLD_TASTER_0		(1 << 16)
#define CPLD_TASTER_1		(1 << 17)
#define CPLD_TASTER_2		(1 << 18)
#define CPLD_TASTER_3		(1 << 19)
#define CPLD_ALL_TASTER		0x000f0000

static inline int key_in_init(void);
static inline void key_in_cleanup(void);
static int key_in_read(char *);
#ifdef CONFIG_PROC_FS
static int key_in_read_proc(char *page, char **start,
					off_t off, int count,
					int *eof, void *data);
#endif
/**********************************************************/
/* private DIGITAL_IN definitions and declarations        */
/**********************************************************/
static DECLARE_WAIT_QUEUE_HEAD (digital_in_wait);
static char digital_in_value, digital_in_ready;

#define rCPLD_FUELLSTAND   *((volatile unsigned long *) (cpld_base + 0x8000))

#define CPLD_FUELLSTAND_SHIFT	16
#define CPLD_FUELLSTAND			(1 << 16)
#define CPLD_SW_KLAPPENM		(1 << 17)
#define CPLD_ALL_FUELLSTAND		0x00030000

#define START3                          (1 << 16)
#define UPDATE3                         (1 << 17)
#define INVERT3                         (1 << 18)
#define RELOAD3                         (1 << 19)

static inline int digital_in_init(void);
static void inline digital_in_cleanup(void);
static int digital_in_read(char *);
#ifdef CONFIG_PROC_FS
static int digital_in_read_proc(char *page, char **start,
							off_t off, int count,
							int *eof, void *data);
#endif
/**********************************************************/
/* private JTAG_CPLD definitions and declarations	  */
/**********************************************************/
static struct semaphore jtag_cpld_mutex;

static inline int jtag_cpld_init(void);
static int jtag_cpld_read(char *);
static int jtag_cpld_write(char *);
static inline void jtag_cpld_cleanup(void);
#ifdef CONFIG_PROC_FS
static int jtag_cpld_read_proc (char *page, char **start,
				off_t off, int count,
				int *eof, void *data); 
static int jtag_cpld_write_proc(struct file *file,
				const char *buffer,
				unsigned long count,
				void *data); 
#endif
/*
 * Initialize the driver - Register the character device
 */
static int __init trabio_init (void)
{
	int i;

	/* This stuff is needed to get interrupts on stop-position
	 * contact events.
	 * (Copied from the LCD initialization routine.)
	 */
	if (rLCDCON1 == 0)
	{
		rPCCON = (rPCCON & 0xFFFFFF00)| 0x000000AA;
		rPDCON = (rPDCON & 0xFFFFFF03)| 0x000000A8;
#if 0
		rPDCON = (rPDCON & 0xFFFFFF00)| 0x000000AA;
#endif
		rLCDCON2 = 0x000DC000;
		rLCDCON3 = 0x0051000A;
		rLCDCON4 = 0x00000001;
		rLCDCON5 = 0x00000440;
		rLCDCON1 = 0x00000B75;
	}

	cpld_base = (unsigned long) ioremap(CPLD_BASE_PHYS, CPLD_SIZE);
	if (cpld_base == 0) {
		printk("Unable to remap CPLD  physaddr (%#x), len %d\n",
			   CPLD_BASE_PHYS, CPLD_SIZE);
		goto exit;
	}

	if((i = dip_switch_init())) {
		printk("Error initializing dip_switch driver\n");
		goto dip_switch_exit;
	}

	if((i = key_out_init())) {
		printk("Error initializing key_out driver\n");
		goto key_out_exit;
	}

	if((i = digital_out_init())) {
		printk("Error initializing digital_out driver\n");
		goto digital_out_exit;
	}

	if((i = key_in_init())) {
		printk("Error initializing key_in driver\n");
		goto key_in_exit;
	}

	if((i = digital_in_init())) {
		printk("Error initializing digital_in driver\n");
		goto digital_in_exit;
	}

	if((i = jtag_cpld_init())) {
		printk("Error initializing jtag_cpld driver\n");
		goto jtag_cpld_exit;
	}

	/*
	 * Register the character device
	 */
	if ((i = register_chrdev(DEV_MAJOR, DEVICE_NAME, &trabio_fops)) < 0) {
		printk("Unable to get major %d for TRAB driver: rc=%d\n",
			DEV_MAJOR, i);
		goto reg_err;
	}
	printk (KERN_INFO DRIVER_NAME " " DRIVER_VERSION "\n");
	return 0;

 reg_err:
 	jtag_cpld_cleanup();
jtag_cpld_exit:
	digital_in_cleanup();
 digital_in_exit:
	key_in_cleanup();
 key_in_exit:
	digital_out_cleanup();
 digital_out_exit:
	key_out_cleanup();
 key_out_exit:
	dip_switch_cleanup();
 dip_switch_exit:
	iounmap((void *)cpld_base);
 exit:
	return (-EINVAL);
}

/*
 * general open routine
 */
static int trabio_open (struct inode *inode, struct file *file)
{
	int minor  = MINOR(inode->i_rdev);

	DBG ("trabio_open(%p,%p): minor %d\n", inode, file, minor);

	if (minor > MINOR_MAX)
		return -ENXIO;

	return 0;
}


/* General read entry */
static ssize_t trabio_read (struct file *file,
			       char *buf, size_t count, loff_t *ppos)
{
	int minor = MINOR(file->f_dentry->d_inode->i_rdev);
	int rc = 0;
	char c;

	if (count < 1)
		return -ENOMEM;

	switch(minor) {
	case DIP_SWITCH_MINOR:
		rc = dip_switch_read(&c);
		break;
	case DIGITAL_IN_MINOR:
		rc = digital_in_read(&c);
		break;
	case KEY_IN_MINOR:
		rc = key_in_read(&c);
		break;
	case KEY_OUT_MINOR:
		rc = key_out_read(&c);
		break;
	case DIGITAL_OUT_MINOR:
		rc = digital_out_read(&c);
		break;
	case JTAG_CPLD_MINOR:
		rc = jtag_cpld_read(&c);
		break;
	default:
		rc = -EINVAL;
		break;
	}

	if (rc < 0)
		return rc;

	if (put_user(c, buf))
		return -EFAULT;
	else
		return 1;
}

/*
 * general write entry
 */
static ssize_t trabio_write (struct file *file,
			       const char *buf, size_t count, loff_t *ppos)
{
	int minor = MINOR(file->f_dentry->d_inode->i_rdev);
	char c;
	int rc = 0;

	if (count < 1)
		return -ENOMEM;

	if (get_user(c, buf))
		return -EFAULT;

	switch(minor) {
	case DIP_SWITCH_MINOR:
	case DIGITAL_IN_MINOR:
	case KEY_IN_MINOR:
	default:
		rc = -ENOSYS;
		break;
	case KEY_OUT_MINOR:
		rc = key_out_write(&c);
		break;
	case DIGITAL_OUT_MINOR:
		rc = digital_out_write(&c);
		break;
	case JTAG_CPLD_MINOR:
		rc = jtag_cpld_write(&c);
		break;
	}

	return (rc);
}

static unsigned int trabio_poll (struct file *file, poll_table *wait)
{
	int minor = MINOR(file->f_dentry->d_inode->i_rdev);
	unsigned int rc;

	switch(minor) {
	case DIP_SWITCH_MINOR:
		rc = POLLIN | POLLRDNORM;
		break;
	case KEY_OUT_MINOR:
	case DIGITAL_OUT_MINOR:
		rc = POLLIN | POLLRDNORM | POLLWRNORM | POLLOUT;
		break;
	case DIGITAL_IN_MINOR:
		poll_wait(file, &digital_in_wait, wait);
		rc = 0;
		if (digital_in_ready)
			rc = POLLIN | POLLRDNORM;
		break;
	case KEY_IN_MINOR:
		poll_wait(file, &key_in_wait, wait);
		rc = 0;
		if (key_in_ready)
			rc = POLLIN | POLLRDNORM;
		break;
	default:
		rc = POLLNVAL;
		break;
	}

	return rc;
}


/******************************
 **** Module Declarations *****
 **************************** */

module_init (trabio_init);

#ifdef MODULE
/*
 * Cleanup - unregister the driver
 */
void trabio_cleanup (void)
{
	int ret;

	/*
	 * Unregister the device
	 */
	ret = unregister_chrdev (DEV_MAJOR, DEVICE_NAME);

	/*
	 * If there's an error, report it
	 */
	if (ret < 0) {
		printk ("unregister_chrdev: error %d\n", ret);
	}

	dip_switch_cleanup();
	key_out_cleanup();
	digital_out_cleanup();
	key_in_cleanup();
	digital_in_cleanup();

	iounmap((void *)cpld_base);
}

module_exit (trabio_cleanup);

#endif	/* MODULE */



/*********************************************************
   DIP_SWITCH functions
*********************************************************/

static inline int dip_switch_init(void)
{
	int rc = 0;
#ifdef CONFIG_PROC_FS
	struct proc_dir_entry *entry;

	if ((entry =
		 create_proc_read_entry("driver/dip_switch",
					S_IFREG|S_IRUGO,
					NULL,
					dip_switch_read_proc,
					NULL)) == NULL) {
		printk("Unable to create /proc/driver/dip_switch entry\n");
		return -EINVAL;
	}
#endif

    init_MUTEX(&dip_switch_mutex);

#ifndef ADC_POLL_WAIT
	if (request_irq(IRQ_ADC, adc_dav_interrupt, 0, "adc_dav", NULL))
	{
		printk("Can't request irq %d\n", IRQ_ADC);
		rc = -EBUSY;
		goto exit;
	}
#endif

	/* Now set up hardware */
	/* Enable prescaler, but stay in standby mode */

	DBG("%s: adccon %#x\n", __FUNCTION__, rADCCON);

	rADCCON &= ~(0xff << 6); /* clear prescaler bits */
	rADCCON |= ((49 << 6) | ADC_PRSCEN); /* prescaler value 49 */

	DBG("%s: init completed: adccon %#x\n", __FUNCTION__, rADCCON);

	return rc;

#ifndef ADC_POLL_WAIT
 exit:
#endif
#ifdef CONFIG_PROC_FS
	remove_proc_entry("driver/dip_switch", NULL);
#endif
	return rc;

}

#ifndef ADC_POLL_WAIT
static void adc_dav_interrupt(int nr, void *devid, struct pt_regs *regs)
{
	DBG ("ADC int\n");
	wake_up_interruptible(&adc_dav_wait);
}
#endif

static inline void dip_switch_cleanup(void)
{
#ifndef ADC_POLL_WAIT
	free_irq(IRQ_ADC, NULL);
#endif
#ifdef CONFIG_PROC_FS
	remove_proc_entry("driver/dip_switch", NULL);
#endif
}


static int dip_switch_read(char *p)
{
	int i;

	down(&dip_switch_mutex);

	/***********************************************************
	 DIP switch connection (according to wa4-cpu.sp.100.pdf, page 3):
	   SW1 - AIN7
	   SW2 - AIN6
	   SW3 - AIN5
	   SW4 - AIN4

	   "On" DIP switch position short-circuits the voltage from
	   the input channel (i.e. '0' conversion result means "on").

	*************************************************************/

	*p = 0;

	rADCCON &= ~ADC_STDBM; /* select normal mode */

	for (i = 7; i > 3; i--) {
#ifdef ADC_POLL_WAIT
		int j;
		j = 100000; /* value for wait loop */
#endif
		rADCCON &= ~(0x7 << 3); /* clear the channel bits */

		rADCCON |= ((i << 3) | ADC_ENABLE_START); /* select channel */

		DBG("%s: reading ch %d (ctrl reg %#x)\n", __FUNCTION__, i, rADCCON);

#ifdef ADC_POLL_WAIT
		while (j--) {
			if ((rADCCON & ADC_ENABLE_START) == 0)
				break;

		}
		if (j == 0) {
			DBG("%s: ADC timeout\n", __FUNCTION__);
			rADCCON |= ADC_STDBM; /* select standby mode */
			up(&dip_switch_mutex);
			return -EINVAL;
		}
#else
		if (wait_event_interruptible(adc_dav_wait,
									 (rADCCON & ADC_ENABLE_START) == 0)) {
			rADCCON |= ADC_STDBM; /* select standby mode */
			return -ERESTARTSYS;
		}
#endif

		DBG("%s: ch %d: %#x\n", __FUNCTION__, i, rADCDAT);

		/*
		 * Input voltage (switch open) is 1.8 V.
		 * (Vin_High/VRef)*adc_res = (1,8V/2,5V)*1023) = 736
		 * Set trigger at halve that value.
		 */
		if ((rADCDAT & 0x3ff) < 368)
			*p |= (1 << (i-4));
	}
	rADCCON |= ADC_STDBM; /* select standby mode */

	DBG("%s: return %#x, adccon %#x\n", __FUNCTION__, *p, rADCCON);
	up(&dip_switch_mutex);

	return 1;
}


#ifdef CONFIG_PROC_FS
static int dip_switch_read_proc(char *page, char **start,
				off_t off, int count,
				int *eof, void *data)
{
	char *out = page;
	int len;
	int ret;
	char ch;
	char *fmt = "%d %d %d %d\n";

	if (off >= 8)
		return 0;

	if ((ret = dip_switch_read(&ch)) < 0)
		return ret;

	out += sprintf(out, fmt,
				   (ch >> 3) & 1,
				   (ch >> 2) & 1,
				   (ch >> 1) & 1,
				   (ch >> 0) & 1);

	len = out - page;
	len -= off;

	if (len < count) {
		*eof = 1;
		if (len < 0) {
			len = 0;
		}
	} else {
		len = count;
	}

	if (len) {
		*start = page + off;
	}

	return len;
}
#endif

/*********************************************************
   DIGITAL_IN functions
*********************************************************/
static int digital_in_read(char *p)
{
#ifndef NEW_CPLD
	*p = digital_in_value;
#else
	*p = (((rCPLD_FUELLSTAND & CPLD_ALL_FUELLSTAND) >> CPLD_FUELLSTAND_SHIFT)
		& 0x3);
#endif
	digital_in_ready = 0;
	return 1;
}

#ifdef CONFIG_PROC_FS
static int digital_in_read_proc(char *page, char **start,
			 	off_t off, int count,
			 	int *eof, void *data)
{
	char *out = page;
	int len;
	char ch;
	char *fmt = "%d %d\n";

	if (off >= 4)
		return 0;

#ifndef NEW_CPLD
	ch = digital_in_value;
#else
	ch = (((rCPLD_FUELLSTAND & CPLD_FUELLSTAND) >> CPLD_FUELLSTAND_SHIFT)
		  & 0x1) | (digital_in_value & 0x2);
#endif

	out += sprintf(out, fmt,
				   (ch >> 0) & 1,
				   (ch >> 1) & 1);

	len = out - page;
	len -= off;

	if (len < count) {
		*eof = 1;
		if (len < 0) {
			len = 0;
		}
	} else {
		len = count;
	}

	if (len) {
		*start = page + off;
	}

	return len;
}
#endif

static void digital_in_interrupt(int nr, void *devid, struct pt_regs *regs)
{
	int val = rCPLD_FUELLSTAND & CPLD_ALL_FUELLSTAND;

	DBG ("D:0x%08x\n", val );

#ifndef NEW_CPLD /* For new CPLD */
	if (val)
	{
		digital_in_value ^= (val >> CPLD_FUELLSTAND_SHIFT);
#else
		digital_in_value = (val >> CPLD_FUELLSTAND_SHIFT);
#endif
		digital_in_ready = 1;

		wake_up_interruptible(&digital_in_wait);

#ifndef NEW_CPLD /* No need to ack interrupts for new CPLD */
		rCPLD_FUELLSTAND = val;
	}
#endif
}

static inline int digital_in_init(void)
{
	int ret;
#ifdef CONFIG_PROC_FS
	struct proc_dir_entry *entry;

	if ((entry =
		 create_proc_read_entry("driver/digital_in",
					S_IFREG|S_IRUGO,
					NULL,
					digital_in_read_proc,
					NULL)) == NULL) {
		printk("Unable to create /proc/driver/digital_in entry\n");
		ret = -EINVAL;
		goto exit;
	}
#endif
	/* Enable 500 Hz timer for fill level sensor to operate properly */
	/* Configure TOUT3 as functional pin, disable pull-up */
	rPDCON &= ~0x30000;
	rPDCON |= 0x20000;
	rPDUP |= (1 << 8);

	/* Configure the prescaler */
	rTCFG0 &= ~0xff00;
	rTCFG0 |= 0x0f00;

	/* Select MUX input (divider) for timer3 (1/16) */
	rTCFG1 &= ~0xf000;
	rTCFG1 |= 0x3000;

	/* Enable autoreload and set the counter and compare
	   registers to values for the 500 Hz clock
	   (for a given  prescaler (15) and divider (16)):
	   counter = (66000000 / 500) >> 9;
	*/
	rTCNTB3 = 0x101;
	rTCMPB3 = 0x101 / 2;

	/* Start timer */
	rTCON = (rTCON | UPDATE3 | RELOAD3) & ~INVERT3;
	rTCON = (rTCON | START3) & ~UPDATE3;


	/* Configure EINT2 as rising edge triggered. */
	rEXTINT &= ~(7 << 8);
	rEXTINT |=  (4 << 8);

	/* Configure PE2 pin as EINT2. */
	rPECON &= ~(3 << 4);
	rPECON |=  (2 << 4);

	digital_in_value = digital_in_ready = 0;

#ifndef NEW_CPLD  /* No need for new CPLD */
	/* Clear interrupt */
	rCPLD_FUELLSTAND = CPLD_ALL_FUELLSTAND;
#else
	digital_in_value = ((rCPLD_FUELLSTAND & CPLD_ALL_FUELLSTAND)
						>> CPLD_FUELLSTAND_SHIFT);
#endif
	if (request_irq(IRQ_EINT2, digital_in_interrupt, 0, "digital_in", NULL))
	{
		printk("Can't request irq %d\n", IRQ_EINT2);
		ret = -EBUSY;
		goto proc_exit;
	}

	return 0;

 proc_exit:
#ifdef CONFIG_PROC_FS
	remove_proc_entry("driver/digital_in", NULL);

 exit:
#endif
	return ret;
}

static inline void digital_in_cleanup(void)
{
	free_irq(IRQ_EINT2, NULL);

	/* Stop timer3 */
	rTCON &= ~(START3 | RELOAD3);

#ifdef CONFIG_PROC_FS
	remove_proc_entry("driver/digital_in", NULL);
#endif
}

/*********************************************************
   KEY_IN functions
*********************************************************/
static int key_in_read(char *p)
{
	*p = key_in_value;
	key_in_ready = 0;
	return 1;
}

#ifdef CONFIG_PROC_FS
static int key_in_read_proc(char *page, char **start,
				 off_t off, int count,
				 int *eof, void *data)
{
	char *out = page;
	int len;
	char ch;
	char *fmt = "%d %d %d %d\n";

	if (off >= 8)
		return 0;

	ch = key_in_value;

	out += sprintf(out, fmt,
				   (ch >> 0) & 1,
				   (ch >> 1) & 1,
				   (ch >> 2) & 1,
				   (ch >> 3) & 1);

	len = out - page;
	len -= off;

	if (len < count) {
		*eof = 1;
		if (len < 0) {
			len = 0;
		}
	} else {
		len = count;
	}

	if (len) {
		*start = page + off;
	}

	return len;
}
#endif

static void key_in_interrupt(int nr, void *devid, struct pt_regs *regs)
{
	int val = rCPLD_TASTER & CPLD_ALL_TASTER;

	DBG ("K:0x%08x\n", val);

#ifndef NEW_CPLD /* For new CPLD */
	if (val)
	{
		key_in_value ^= (val >> CPLD_TASTER_SHIFT);
#else
		key_in_value = (val >> CPLD_TASTER_SHIFT);
#endif
		key_in_ready = 1;

		wake_up_interruptible(&key_in_wait);

#ifndef NEW_CPLD /* No need to ack interrupts for new CPLD */
		rCPLD_TASTER = val;
	}
#endif
}

static inline int key_in_init(void)
{
	int ret;
#ifdef CONFIG_PROC_FS
	struct proc_dir_entry *entry;

	if ((entry =
		 create_proc_read_entry("driver/key_in",
					S_IFREG|S_IRUGO,
					NULL,
					key_in_read_proc,
					NULL)) == NULL) {
		printk("Unable to create /proc/driver/key_in entry\n");
		ret = -EINVAL;
		goto exit;
	}
#endif


	/* Configure EINT6 as rising edge triggered. */
	rEXTINT &= ~(7 << 24);
	rEXTINT |=  (4 << 24);

	/* Configure PE6 pin as EINT6. */
	rPECON &= ~(3 << 12);
	rPECON |=  (2 << 12);

	key_in_value = key_in_ready = 0;

#ifndef NEW_CPLD /* No need for new CPDL */
	/* Clear interrupt */
	rCPLD_TASTER = CPLD_ALL_TASTER;
#else
	key_in_value = ((rCPLD_TASTER & CPLD_ALL_TASTER) >> CPLD_TASTER_SHIFT);
#endif
	if (request_irq(IRQ_EINT6, key_in_interrupt, 0, "key_in", NULL))
	{
		printk("Can't request irq %d\n", IRQ_EINT6);
		ret = -EBUSY;
		goto proc_exit;
	}

	return 0;

 proc_exit:
#ifdef CONFIG_PROC_FS
	remove_proc_entry("driver/key_in", NULL);

 exit:
#endif
	return ret;
}

static inline void key_in_cleanup(void)
{
	free_irq(IRQ_EINT6, NULL);

#ifdef CONFIG_PROC_FS
	remove_proc_entry("driver/key_in", NULL);
#endif
}

/*********************************************************
   KEY_OUT functions
*********************************************************/
static inline int key_out_init(void)
{
	int i;
#ifdef CONFIG_PROC_FS
	struct proc_dir_entry *entry;

	if ((entry = create_proc_entry("driver/key_out",
					S_IFREG|S_IRUGO|S_IWUGO,
					NULL)) == NULL) {
		printk("Unable to create /proc/driver/key_out entry\n");
		return -EINVAL;
	}
	entry->read_proc = key_out_read_proc;
	entry->write_proc = key_out_write_proc;
	entry->data = NULL;
#endif
    init_MUTEX(&key_out_mutex);

	/* Configure ports, turn off leds */
	/* LED1 on GPC14 */
	/* LED2 on GPC15 */
	rPCCON &= ~(0x30000000 | 0xc0000000); /* clear pc14, pc15 */
	rPCCON |=  (0x10000000 | 0x40000000); /* output pins */
	rPCUP |= (0x4000 | 0x8000);           /* disable pullups */

	/* LED3 on GPD0 */
	/* LED4 on GPD4 */
	rPDCON &= ~(0x3 | 0x300);        /* clear pd0, pd4 */
	rPDCON |=  (0x1 | 0x100);        /* output pins */
	rPDUP |= (0x1 | 0x10);           /* disable pullups */

	kbd_led_status = 0;

	for (i = 0; i < 4; i++)
		led_on_off(i, 0);
	return 0;
}


static int key_out_read(char *p)
{
	down(&key_out_mutex);

	*p = kbd_led_status;

	up(&key_out_mutex);
	return 1;
}



static int key_out_write(char *p)
{
	int rc;
	int i;

	down(&key_out_mutex);

	kbd_led_status = (*p & 0xf);

	for(i = 0; i < 4; i++) {
		DBG("%s: led %d: %s\n", __FUNCTION__, i,
			  (kbd_led_status & (1 << i)) ? "ON" : "OFF");
		led_on_off(i, (kbd_led_status & (1 << i)));
	}
	rc = 1;
	up(&key_out_mutex);
	return rc;
}

static inline void key_out_cleanup(void)
{
	int i;

#ifdef CONFIG_PROC_FS
	remove_proc_entry("driver/key_out", NULL);
#endif
	/* Turn off all leds */
	for (i = 0; i < 4; i++)
		led_on_off(i, 0);
}


static void led_on_off(int n, int on)
{
	switch (n) {
	case 0: /* led1, PC14 */
		on ? (rPCDAT |= 0x4000) : (rPCDAT &= ~0x4000);
		break;
	case 1: /* led2, PC15 */
		on ? (rPCDAT |= 0x8000) : (rPCDAT &= ~0x8000);
		break;
	case 2: /* led3, PD0 */
		on ? (rPDDAT |= 0x1) : (rPDDAT &= ~0x1);
		break;
	case 3: /* led4, PD4 */
		on ? (rPDDAT |= 0x10) : (rPDDAT &= ~0x10);
		break;
	}
}

#ifdef CONFIG_PROC_FS
static int key_out_read_proc(char *page, char **start,
				 off_t off, int count,
				 int *eof, void *data)
{
	char *out = page;
	int len;
	int ret;
	char ch;

	/* the line format is "%d %d %d %d\n",
	   numeration from left to right */
	char *fmt = "%d %d %d %d\n";

	if (off >= 8)
		return 0;

	if ((ret = key_out_read(&ch)) < 0)
		return ret;

	out += sprintf(out, fmt,
				   (ch >> 0) & 1,
				   (ch >> 1) & 1,
				   (ch >> 2) & 1,
				   (ch >> 3) & 1);

	len = out - page;
	len -= off;

	if (len < count) {
		*eof = 1;
		if (len < 0) {
			len = 0;
		}
	} else {
		len = count;
	}

	if (len) {
		*start = page + off;
	}

	return len;
}


static int key_out_write_proc(struct file *file, const char *buffer,
			unsigned long count, void *data)
{
	unsigned char buf[10];
	char c;
	int i, j;

	/* the line format is "%d %d %d %d\n",
	 numeration from left to right */
	if (count != 8)
		return -EINVAL;

	if (copy_from_user(buf, buffer, count)) {
		return -EFAULT;
	}

	if (buf[1] != ' ' ||
		buf[3] != ' ' ||
		buf[5] != ' ' ||
		buf[7] != '\n') {
		return -EINVAL;
	}

	c = 0;
	for (i = 0, j = 0; j < 7; i++, j += 2) {
		if (buf[j] - '0') {
			c |= (1 << i);
		}
	}

	key_out_write(&c);
	return count;
}
#endif

/*********************************************************
   DIGITAL_OUT functions
*********************************************************/
static inline int digital_out_init(void)
{
#ifdef CONFIG_PROC_FS
	struct proc_dir_entry *entry;

	if ((entry = create_proc_entry("driver/digital_out",
					S_IFREG|S_IRUGO|S_IWUGO,
					NULL)) == NULL) {
		printk("Unable to create /proc/driver/digital_out entry\n");
		return -EINVAL;
	}
	entry->read_proc = digital_out_read_proc;
	entry->write_proc = digital_out_write_proc;
	entry->data = NULL;
#endif
	init_MUTEX(&digital_out_mutex);

	/* Configure ports */
	/* Status led on GPA12 (no pullup reg for port A) */
	rPACON &= ~(1 << 12); /* output pins */

	/* Motor control on GPG0 */
	rPGCON &= ~0x3;        /* clear PG0 */
	rPGCON |=  0x1;        /* output pins */
	rPGUP |= 0x1;       /* disable pullups */

	led_motor_on_off(led_status, motor_status);
	led_status = 0;
	motor_status = 0;

	return 0;
}

static int digital_out_read(char *p)
{
	int rc = 1;
	down(&digital_out_mutex);
	/* bit definition: xxml */
	*p = led_status | (motor_status << 1);
	up(&digital_out_mutex);
	return rc;
}



static int digital_out_write(char *p)
{
	int rc;

	down(&key_out_mutex);

	led_status = (*p & 0x1);
	motor_status = ((*p >> 1) & 0x1);
	led_motor_on_off(led_status, motor_status);
	rc = 1;
	up(&key_out_mutex);
	return rc;
}

static inline void digital_out_cleanup(void)
{

#ifdef CONFIG_PROC_FS
	remove_proc_entry("driver/digital_out", NULL);
#endif
	led_motor_on_off(0, 0);
}


static void led_motor_on_off(int led, int motor)
{
	/* led is on when pin state is 0 */
	if (led)
		rPADAT &= ~(1 << 12);
	else
		rPADAT |= (1 << 12);

	/* LM2941 on/off logic is also inverted */
	if (motor)
		rPGDAT &= ~0x1;
	else
		rPGDAT |= 0x1;
}

#ifdef CONFIG_PROC_FS
static int digital_out_read_proc(char *page, char **start,
				 off_t off, int count,
				 int *eof, void *data)
{
	char *out = page;
	int len;
	int ret;
	char ch;
	char *fmt = "%d %d\n"; /* "L M\n" */

	if (off >= 4)
		return 0;

	if ((ret = digital_out_read(&ch)) < 0)
		return ret;

	out += sprintf(out, fmt,
				   (ch >> 0) & 1,
				   (ch >> 1) & 1);

	len = out - page;
	len -= off;

	if (len < count) {
		*eof = 1;
		if (len < 0) {
			len = 0;
		}
	} else {
		len = count;
	}

	if (len) {
		*start = page + off;
	}

	return len;
}


static int digital_out_write_proc(struct file *file, const char *buffer,
				  unsigned long count, void *data)
{
	unsigned char buf[10];
	char c;

	/* the line format is "L M\n" */
	if (count != 4)
		return -EINVAL;

	if (copy_from_user(buf, buffer, count)) {
		return -EFAULT;
	}

	if (buf[1] != ' ' ||
		buf[3] != '\n') {
		return -EINVAL;
	}

	c = 0;
	if (buf[0] - '0') { /* led */
		c = 1;
	}

	if (buf[2] - '0') { /* motor */
		c |= 2;
	}

	digital_out_write(&c);
	return count;
}
#endif

/*********************************************************
   JTAG_CPLD functions
*********************************************************/
static inline int jtag_cpld_init(void)
{
#ifdef CONFIG_PROC_FS
	struct proc_dir_entry *entry;

	if ((entry = create_proc_entry("driver/jtag_cpld",
					S_IFREG|S_IRUGO|S_IWUGO,
					NULL)) == NULL) {
		printk("Unable to create /proc/driver/jtag_cpld entry\n");
		return -EINVAL;
	}
	entry->read_proc = jtag_cpld_read_proc;
	entry->write_proc = jtag_cpld_write_proc;
	entry->data = NULL;
#endif
	init_MUTEX(&jtag_cpld_mutex);
	/* Configure ports for cpld-jtag-lines*/
	rPGDAT |= 0x0000001A;
	rPGCON &= 0xFFFFFC03;
	rPGCON |= 0x00000144;
	rPGUP |=  0x0000001E;       /* enable pullups */

	return 0;
}

static int jtag_cpld_read(char *p)
{
	down(&jtag_cpld_mutex);
	*p = ((rPGDAT & 0x0000001E)>>1); /* Read TCK, TMS, TDO and TDI Line */
/*	*p = ((rPGDAT & 0x00000004)>>1);	** Read TDO-line */
	up(&jtag_cpld_mutex);
	return 1;
}


static int jtag_cpld_write(char *p)
{
	down(&jtag_cpld_mutex);
	rPGDAT = (rPGDAT & 0xFFFFFFE5)|((*p << 1)&0x0000001A); /* Set TDO, TMS, TCK - lines */
	up(&jtag_cpld_mutex);
	return 1;
}

static inline void jtag_cpld_cleanup(void)
{

#ifdef CONFIG_PROC_FS
	remove_proc_entry("driver/jtag_cpld", NULL);
#endif
	rPGDAT |= 0x0000001C;
}


#ifdef CONFIG_PROC_FS
static int jtag_cpld_read_proc(char *page, char **start,
				 off_t off, int count,
				 int *eof, void *data)
{
	char *out = page;
	int len;
	int ret;
	char ch;
	char *fmt = "%d %d %d %d\n"; /* "TCK_PLD TMS_PLD TDO_PLD TDI_PLD\n" */

	if (off >= 4)
		return 0;

	if ((ret = jtag_cpld_read(&ch)) < 0)
		return ret;

	out += sprintf(out, fmt, ((ch>>3)&0x01), ((ch>>2)&0x01), ((ch>>1)&0x01), (ch&0x01));

	len = out - page;
	len -= off;

	if (len < count) {
		*eof = 1;
		if (len < 0) {
			len = 0;
		}
	} else {
		len = count;
	}

	if (len) {
		*start = page + off;
	}

	return len;
}


static int jtag_cpld_write_proc(struct file *file, const char *buffer,
				  unsigned long count, void *data)
{
	unsigned char buf[10];
	char c;

	/* the line format is "TCK_PLD TMS_PLD TDO_PLD TDI_PLD\n" */
	if (count != 8)
		return -EINVAL;

	if (copy_from_user(buf, buffer, count)) {
		return -EFAULT;
	}

	if (buf[1] != ' ' ||
	    buf[3] != ' ' ||
	    buf[5] != ' ' ||
		buf[7] != '\n') {
		return -EINVAL;
	}

	c = 0;
	if (buf[0] - '0') { /* TCK */
		c |= 8;
	}

	if (buf[2] - '0') { /* TMS */
		c |= 4;
	}

	if (buf[4] - '0') { /* TDO */
		c |= 2;
	}

	if (buf[6] - '0') { /* TDI */
		c |= 1;
	}

	jtag_cpld_write(&c);
	return count;
}
#endif
