[linux-2.6-block.git] / drivers / i2c / busses / i2c-bfin-twi.c

/*
 * drivers/i2c/busses/i2c-bfin-twi.c
 *
 * Description: Driver for Blackfin Two Wire Interface
 *
 * Author:      sonicz  <sonic.zhang@analog.com>
 *
 * Copyright (c) 2005-2007 Analog Devices, Inc.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/init.h>
#include <linux/i2c.h>
#include <linux/mm.h>
#include <linux/timer.h>
#include <linux/spinlock.h>
#include <linux/completion.h>
#include <linux/interrupt.h>
#include <linux/platform_device.h>

#include <asm/blackfin.h>
#include <asm/irq.h>

#define POLL_TIMEOUT       (2 * HZ)

/* SMBus mode*/
#define TWI_I2C_MODE_STANDARD		1
#define TWI_I2C_MODE_STANDARDSUB	2
#define TWI_I2C_MODE_COMBINED		3
#define TWI_I2C_MODE_REPEAT		4

struct bfin_twi_iface {
	int			irq;
	spinlock_t		lock;
	char			read_write;
	u8			command;
	u8			*transPtr;
	int			readNum;
	int			writeNum;
	int			cur_mode;
	int			manual_stop;
	int			result;
	int			timeout_count;
	struct timer_list	timeout_timer;
	struct i2c_adapter	adap;
	struct completion	complete;
	struct i2c_msg 		*pmsg;
	int			msg_num;
	int			cur_msg;
};

static struct bfin_twi_iface twi_iface;

static void bfin_twi_handle_interrupt(struct bfin_twi_iface *iface)
{
	unsigned short twi_int_status = bfin_read_TWI_INT_STAT();
	unsigned short mast_stat = bfin_read_TWI_MASTER_STAT();

	if (twi_int_status & XMTSERV) {
		/* Transmit next data */
		if (iface->writeNum > 0) {
			bfin_write_TWI_XMT_DATA8(*(iface->transPtr++));
			iface->writeNum--;
		}
		/* start receive immediately after complete sending in
		 * combine mode.
		 */
		else if (iface->cur_mode == TWI_I2C_MODE_COMBINED)
			bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL()
				| MDIR | RSTART);
		else if (iface->manual_stop)
			bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL()
				| STOP);
		else if (iface->cur_mode == TWI_I2C_MODE_REPEAT &&
				iface->cur_msg+1 < iface->msg_num)
			bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL()
				| RSTART);
		SSYNC();
		/* Clear status */
		bfin_write_TWI_INT_STAT(XMTSERV);
		SSYNC();
	}
	if (twi_int_status & RCVSERV) {
		if (iface->readNum > 0) {
			/* Receive next data */
			*(iface->transPtr) = bfin_read_TWI_RCV_DATA8();
			if (iface->cur_mode == TWI_I2C_MODE_COMBINED) {
				/* Change combine mode into sub mode after
				 * read first data.
				 */
				iface->cur_mode = TWI_I2C_MODE_STANDARDSUB;
				/* Get read number from first byte in block
				 * combine mode.
				 */
				if (iface->readNum == 1 && iface->manual_stop)
					iface->readNum = *iface->transPtr + 1;
			}
			iface->transPtr++;
			iface->readNum--;
		} else if (iface->manual_stop) {
			bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL()
				| STOP);
			SSYNC();
		} else if (iface->cur_mode == TWI_I2C_MODE_REPEAT &&
				iface->cur_msg+1 < iface->msg_num) {
			bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL()
				| RSTART);
			SSYNC();
		}
		/* Clear interrupt source */
		bfin_write_TWI_INT_STAT(RCVSERV);
		SSYNC();
	}
	if (twi_int_status & MERR) {
		bfin_write_TWI_INT_STAT(MERR);
		bfin_write_TWI_INT_MASK(0);
		bfin_write_TWI_MASTER_STAT(0x3e);
		bfin_write_TWI_MASTER_CTL(0);
		SSYNC();
		iface->result = -EIO;
		/* if both err and complete int stats are set, return proper
		 * results.
		 */
		if (twi_int_status & MCOMP) {
			bfin_write_TWI_INT_STAT(MCOMP);
			bfin_write_TWI_INT_MASK(0);
			bfin_write_TWI_MASTER_CTL(0);
			SSYNC();
			/* If it is a quick transfer, only address bug no data,
			 * not an err, return 1.
			 */
			if (iface->writeNum == 0 && (mast_stat & BUFRDERR))
				iface->result = 1;
			/* If address not acknowledged return -1,
			 * else return 0.
			 */
			else if (!(mast_stat & ANAK))
				iface->result = 0;
		}
		complete(&iface->complete);
		return;
	}
	if (twi_int_status & MCOMP) {
		bfin_write_TWI_INT_STAT(MCOMP);
		SSYNC();
		if (iface->cur_mode == TWI_I2C_MODE_COMBINED) {
			if (iface->readNum == 0) {
				/* set the read number to 1 and ask for manual
				 * stop in block combine mode
				 */
				iface->readNum = 1;
				iface->manual_stop = 1;
				bfin_write_TWI_MASTER_CTL(
					bfin_read_TWI_MASTER_CTL()
					| (0xff << 6));
			} else {
				/* set the readd number in other
				 * combine mode.
				 */
				bfin_write_TWI_MASTER_CTL(
					(bfin_read_TWI_MASTER_CTL() &
					(~(0xff << 6))) |
					( iface->readNum << 6));
			}
			/* remove restart bit and enable master receive */
			bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() &
				~RSTART);
			bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() |
				MEN | MDIR);
			SSYNC();
		} else if (iface->cur_mode == TWI_I2C_MODE_REPEAT &&
				iface->cur_msg+1 < iface->msg_num) {
			iface->cur_msg++;
			iface->transPtr = iface->pmsg[iface->cur_msg].buf;
			iface->writeNum = iface->readNum =
				iface->pmsg[iface->cur_msg].len;
			/* Set Transmit device address */
			bfin_write_TWI_MASTER_ADDR(
				iface->pmsg[iface->cur_msg].addr);
			if (iface->pmsg[iface->cur_msg].flags & I2C_M_RD)
				iface->read_write = I2C_SMBUS_READ;
			else {
				iface->read_write = I2C_SMBUS_WRITE;
				/* Transmit first data */
				if (iface->writeNum > 0) {
					bfin_write_TWI_XMT_DATA8(
						*(iface->transPtr++));
					iface->writeNum--;
					SSYNC();
				}
			}

			if (iface->pmsg[iface->cur_msg].len <= 255)
				bfin_write_TWI_MASTER_CTL(
				iface->pmsg[iface->cur_msg].len << 6);
			else {
				bfin_write_TWI_MASTER_CTL(0xff << 6);
				iface->manual_stop = 1;
			}
			/* remove restart bit and enable master receive */
			bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() &
				~RSTART);
			bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() |
				MEN | ((iface->read_write == I2C_SMBUS_READ) ?
				MDIR : 0));
			SSYNC();
		} else {
			iface->result = 1;
			bfin_write_TWI_INT_MASK(0);
			bfin_write_TWI_MASTER_CTL(0);
			SSYNC();
			complete(&iface->complete);
		}
	}
}

/* Interrupt handler */
static irqreturn_t bfin_twi_interrupt_entry(int irq, void *dev_id)
{
	struct bfin_twi_iface *iface = dev_id;
	unsigned long flags;

	spin_lock_irqsave(&iface->lock, flags);
	del_timer(&iface->timeout_timer);
	bfin_twi_handle_interrupt(iface);
	spin_unlock_irqrestore(&iface->lock, flags);
	return IRQ_HANDLED;
}

static void bfin_twi_timeout(unsigned long data)
{
	struct bfin_twi_iface *iface = (struct bfin_twi_iface *)data;
	unsigned long flags;

	spin_lock_irqsave(&iface->lock, flags);
	bfin_twi_handle_interrupt(iface);
	if (iface->result == 0) {
		iface->timeout_count--;
		if (iface->timeout_count > 0) {
			iface->timeout_timer.expires = jiffies + POLL_TIMEOUT;
			add_timer(&iface->timeout_timer);
		} else {
			iface->result = -1;
			complete(&iface->complete);
		}
	}
	spin_unlock_irqrestore(&iface->lock, flags);
}

/*
 * Generic i2c master transfer entrypoint
 */
static int bfin_twi_master_xfer(struct i2c_adapter *adap,
				struct i2c_msg *msgs, int num)
{
	struct bfin_twi_iface *iface = adap->algo_data;
	struct i2c_msg *pmsg;
	int rc = 0;

	if (!(bfin_read_TWI_CONTROL() & TWI_ENA))
		return -ENXIO;

	while (bfin_read_TWI_MASTER_STAT() & BUSBUSY) {
		yield();
	}

	iface->pmsg = msgs;
	iface->msg_num = num;
	iface->cur_msg = 0;

	pmsg = &msgs[0];
	if (pmsg->flags & I2C_M_TEN) {
		dev_err(&adap->dev, "10 bits addr not supported!\n");
		return -EINVAL;
	}

	iface->cur_mode = TWI_I2C_MODE_REPEAT;
	iface->manual_stop = 0;
	iface->transPtr = pmsg->buf;
	iface->writeNum = iface->readNum = pmsg->len;
	iface->result = 0;
	iface->timeout_count = 10;
	/* Set Transmit device address */
	bfin_write_TWI_MASTER_ADDR(pmsg->addr);

	/* FIFO Initiation. Data in FIFO should be
	 *  discarded before start a new operation.
	 */
	bfin_write_TWI_FIFO_CTL(0x3);
	SSYNC();
	bfin_write_TWI_FIFO_CTL(0);
	SSYNC();

	if (pmsg->flags & I2C_M_RD)
		iface->read_write = I2C_SMBUS_READ;
	else {
		iface->read_write = I2C_SMBUS_WRITE;
		/* Transmit first data */
		if (iface->writeNum > 0) {
			bfin_write_TWI_XMT_DATA8(*(iface->transPtr++));
			iface->writeNum--;
			SSYNC();
		}
	}

	/* clear int stat */
	bfin_write_TWI_INT_STAT(MERR | MCOMP | XMTSERV | RCVSERV);

	/* Interrupt mask . Enable XMT, RCV interrupt */
	bfin_write_TWI_INT_MASK(MCOMP | MERR | RCVSERV | XMTSERV);
	SSYNC();

	if (pmsg->len <= 255)
		bfin_write_TWI_MASTER_CTL(pmsg->len << 6);
	else {
		bfin_write_TWI_MASTER_CTL(0xff << 6);
		iface->manual_stop = 1;
	}

	iface->timeout_timer.expires = jiffies + POLL_TIMEOUT;
	add_timer(&iface->timeout_timer);

	/* Master enable */
	bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() | MEN |
		((iface->read_write == I2C_SMBUS_READ) ? MDIR : 0) |
		((CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ > 100) ? FAST : 0));
	SSYNC();

	wait_for_completion(&iface->complete);

	rc = iface->result;

	if (rc == 1)
		return num;
	else
		return rc;
}

/*
 * SMBus type transfer entrypoint
 */

int bfin_twi_smbus_xfer(struct i2c_adapter *adap, u16 addr,
			unsigned short flags, char read_write,
			u8 command, int size, union i2c_smbus_data *data)
{
	struct bfin_twi_iface *iface = adap->algo_data;
	int rc = 0;

	if (!(bfin_read_TWI_CONTROL() & TWI_ENA))
		return -ENXIO;

	while (bfin_read_TWI_MASTER_STAT() & BUSBUSY) {
		yield();
	}

	iface->writeNum = 0;
	iface->readNum = 0;

	/* Prepare datas & select mode */
	switch (size) {
	case I2C_SMBUS_QUICK:
		iface->transPtr = NULL;
		iface->cur_mode = TWI_I2C_MODE_STANDARD;
		break;
	case I2C_SMBUS_BYTE:
		if (data == NULL)
			iface->transPtr = NULL;
		else {
			if (read_write == I2C_SMBUS_READ)
				iface->readNum = 1;
			else
				iface->writeNum = 1;
			iface->transPtr = &data->byte;
		}
		iface->cur_mode = TWI_I2C_MODE_STANDARD;
		break;
	case I2C_SMBUS_BYTE_DATA:
		if (read_write == I2C_SMBUS_READ) {
			iface->readNum = 1;
			iface->cur_mode = TWI_I2C_MODE_COMBINED;
		} else {
			iface->writeNum = 1;
			iface->cur_mode = TWI_I2C_MODE_STANDARDSUB;
		}
		iface->transPtr = &data->byte;
		break;
	case I2C_SMBUS_WORD_DATA:
		if (read_write == I2C_SMBUS_READ) {
			iface->readNum = 2;
			iface->cur_mode = TWI_I2C_MODE_COMBINED;
		} else {
			iface->writeNum = 2;
			iface->cur_mode = TWI_I2C_MODE_STANDARDSUB;
		}
		iface->transPtr = (u8 *)&data->word;
		break;
	case I2C_SMBUS_PROC_CALL:
		iface->writeNum = 2;
		iface->readNum = 2;
		iface->cur_mode = TWI_I2C_MODE_COMBINED;
		iface->transPtr = (u8 *)&data->word;
		break;
	case I2C_SMBUS_BLOCK_DATA:
		if (read_write == I2C_SMBUS_READ) {
			iface->readNum = 0;
			iface->cur_mode = TWI_I2C_MODE_COMBINED;
		} else {
			iface->writeNum = data->block[0] + 1;
			iface->cur_mode = TWI_I2C_MODE_STANDARDSUB;
		}
		iface->transPtr = data->block;
		break;
	default:
		return -1;
	}

	iface->result = 0;
	iface->manual_stop = 0;
	iface->read_write = read_write;
	iface->command = command;
	iface->timeout_count = 10;

	/* FIFO Initiation. Data in FIFO should be discarded before
	 * start a new operation.
	 */
	bfin_write_TWI_FIFO_CTL(0x3);
	SSYNC();
	bfin_write_TWI_FIFO_CTL(0);

	/* clear int stat */
	bfin_write_TWI_INT_STAT(MERR|MCOMP|XMTSERV|RCVSERV);

	/* Set Transmit device address */
	bfin_write_TWI_MASTER_ADDR(addr);
	SSYNC();

	iface->timeout_timer.expires = jiffies + POLL_TIMEOUT;
	add_timer(&iface->timeout_timer);

	switch (iface->cur_mode) {
	case TWI_I2C_MODE_STANDARDSUB:
		bfin_write_TWI_XMT_DATA8(iface->command);
		bfin_write_TWI_INT_MASK(MCOMP | MERR |
			((iface->read_write == I2C_SMBUS_READ) ?
			RCVSERV : XMTSERV));
		SSYNC();

		if (iface->writeNum + 1 <= 255)
			bfin_write_TWI_MASTER_CTL((iface->writeNum + 1) << 6);
		else {
			bfin_write_TWI_MASTER_CTL(0xff << 6);
			iface->manual_stop = 1;
		}
		/* Master enable */
		bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() | MEN |
			((CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ>100) ? FAST : 0));
		break;
	case TWI_I2C_MODE_COMBINED:
		bfin_write_TWI_XMT_DATA8(iface->command);
		bfin_write_TWI_INT_MASK(MCOMP | MERR | RCVSERV | XMTSERV);
		SSYNC();

		if (iface->writeNum > 0)
			bfin_write_TWI_MASTER_CTL((iface->writeNum + 1) << 6);
		else
			bfin_write_TWI_MASTER_CTL(0x1 << 6);
		/* Master enable */
		bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() | MEN |
			((CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ>100) ? FAST : 0));
		break;
	default:
		bfin_write_TWI_MASTER_CTL(0);
		if (size != I2C_SMBUS_QUICK) {
			/* Don't access xmit data register when this is a
			 * read operation.
			 */
			if (iface->read_write != I2C_SMBUS_READ) {
				if (iface->writeNum > 0) {
					bfin_write_TWI_XMT_DATA8(*(iface->transPtr++));
					if (iface->writeNum <= 255)
						bfin_write_TWI_MASTER_CTL(iface->writeNum << 6);
					else {
						bfin_write_TWI_MASTER_CTL(0xff << 6);
						iface->manual_stop = 1;
					}
					iface->writeNum--;
				} else {
					bfin_write_TWI_XMT_DATA8(iface->command);
					bfin_write_TWI_MASTER_CTL(1 << 6);
				}
			} else {
				if (iface->readNum > 0 && iface->readNum <= 255)
					bfin_write_TWI_MASTER_CTL(iface->readNum << 6);
				else if (iface->readNum > 255) {
					bfin_write_TWI_MASTER_CTL(0xff << 6);
					iface->manual_stop = 1;
				} else {
					del_timer(&iface->timeout_timer);
					break;
				}
			}
		}
		bfin_write_TWI_INT_MASK(MCOMP | MERR |
			((iface->read_write == I2C_SMBUS_READ) ?
			RCVSERV : XMTSERV));
		SSYNC();

		/* Master enable */
		bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() | MEN |
			((iface->read_write == I2C_SMBUS_READ) ? MDIR : 0) |
			((CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ > 100) ? FAST : 0));
		break;
	}
	SSYNC();

	wait_for_completion(&iface->complete);

	rc = (iface->result >= 0) ? 0 : -1;

	return rc;
}

/*
 * Return what the adapter supports
 */
static u32 bfin_twi_functionality(struct i2c_adapter *adap)
{
	return I2C_FUNC_SMBUS_QUICK | I2C_FUNC_SMBUS_BYTE |
	       I2C_FUNC_SMBUS_BYTE_DATA | I2C_FUNC_SMBUS_WORD_DATA |
	       I2C_FUNC_SMBUS_BLOCK_DATA | I2C_FUNC_SMBUS_PROC_CALL |
	       I2C_FUNC_I2C;
}


static struct i2c_algorithm bfin_twi_algorithm = {
	.master_xfer   = bfin_twi_master_xfer,
	.smbus_xfer    = bfin_twi_smbus_xfer,
	.functionality = bfin_twi_functionality,
};


static int i2c_bfin_twi_suspend(struct platform_device *dev, pm_message_t state)
{
/*	struct bfin_twi_iface *iface = platform_get_drvdata(dev);*/

	/* Disable TWI */
	bfin_write_TWI_CONTROL(bfin_read_TWI_CONTROL() & ~TWI_ENA);
	SSYNC();

	return 0;
}

static int i2c_bfin_twi_resume(struct platform_device *dev)
{
/*	struct bfin_twi_iface *iface = platform_get_drvdata(dev);*/

	/* Enable TWI */
	bfin_write_TWI_CONTROL(bfin_read_TWI_CONTROL() | TWI_ENA);
	SSYNC();

	return 0;
}

static int i2c_bfin_twi_probe(struct platform_device *dev)
{
	struct bfin_twi_iface *iface = &twi_iface;
	struct i2c_adapter *p_adap;
	int rc;

	spin_lock_init(&(iface->lock));
	init_completion(&(iface->complete));
	iface->irq = IRQ_TWI;

	init_timer(&(iface->timeout_timer));
	iface->timeout_timer.function = bfin_twi_timeout;
	iface->timeout_timer.data = (unsigned long)iface;

	p_adap = &iface->adap;
	p_adap->id = I2C_HW_BLACKFIN;
	p_adap->nr = dev->id;
	strlcpy(p_adap->name, dev->name, sizeof(p_adap->name));
	p_adap->algo = &bfin_twi_algorithm;
	p_adap->algo_data = iface;
	p_adap->class = I2C_CLASS_ALL;
	p_adap->dev.parent = &dev->dev;

	rc = request_irq(iface->irq, bfin_twi_interrupt_entry,
		IRQF_DISABLED, dev->name, iface);
	if (rc) {
		dev_err(&(p_adap->dev), "i2c-bfin-twi: can't get IRQ %d !\n",
			iface->irq);
		return -ENODEV;
	}

	/* Set TWI internal clock as 10MHz */
	bfin_write_TWI_CONTROL(((get_sclk() / 1024 / 1024 + 5) / 10) & 0x7F);

	/* Set Twi interface clock as specified */
	bfin_write_TWI_CLKDIV((( 5*1024 / CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ )
			<< 8) | (( 5*1024 / CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ )
			& 0xFF));

	/* Enable TWI */
	bfin_write_TWI_CONTROL(bfin_read_TWI_CONTROL() | TWI_ENA);
	SSYNC();

	rc = i2c_add_numbered_adapter(p_adap);
	if (rc < 0)
		free_irq(iface->irq, iface);
	else
		platform_set_drvdata(dev, iface);

	return rc;
}

static int i2c_bfin_twi_remove(struct platform_device *pdev)
{
	struct bfin_twi_iface *iface = platform_get_drvdata(pdev);

	platform_set_drvdata(pdev, NULL);

	i2c_del_adapter(&(iface->adap));
	free_irq(iface->irq, iface);

	return 0;
}

static struct platform_driver i2c_bfin_twi_driver = {
	.probe		= i2c_bfin_twi_probe,
	.remove		= i2c_bfin_twi_remove,
	.suspend	= i2c_bfin_twi_suspend,
	.resume		= i2c_bfin_twi_resume,
	.driver		= {
		.name	= "i2c-bfin-twi",
		.owner	= THIS_MODULE,
	},
};

static int __init i2c_bfin_twi_init(void)
{
	pr_info("I2C: Blackfin I2C TWI driver\n");

	return platform_driver_register(&i2c_bfin_twi_driver);
}

static void __exit i2c_bfin_twi_exit(void)
{
	platform_driver_unregister(&i2c_bfin_twi_driver);
}

MODULE_AUTHOR("Sonic Zhang <sonic.zhang@analog.com>");
MODULE_DESCRIPTION("I2C-Bus adapter routines for Blackfin TWI");
MODULE_LICENSE("GPL");

module_init(i2c_bfin_twi_init);
module_exit(i2c_bfin_twi_exit);
Commit	Line	Data
d24ecfcc BW	1	/*
	2	* drivers/i2c/busses/i2c-bfin-twi.c
	3	*
	4	* Description: Driver for Blackfin Two Wire Interface
	5	*
	6	* Author: sonicz <sonic.zhang@analog.com>
	7	*
	8	* Copyright (c) 2005-2007 Analog Devices, Inc.
	9	*
	10	* This program is free software; you can redistribute it and/or modify
	11	* it under the terms of the GNU General Public License as published by
	12	* the Free Software Foundation; either version 2 of the License, or
	13	* (at your option) any later version.
	14	*
	15	* This program is distributed in the hope that it will be useful,
	16	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	17	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	18	* GNU General Public License for more details.
	19	*
	20	* You should have received a copy of the GNU General Public License
	21	* along with this program; if not, write to the Free Software
	22	* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
	23	*/
	24
	25	#include <linux/module.h>
	26	#include <linux/kernel.h>
	27	#include <linux/init.h>
	28	#include <linux/i2c.h>
	29	#include <linux/mm.h>
	30	#include <linux/timer.h>
	31	#include <linux/spinlock.h>
	32	#include <linux/completion.h>
	33	#include <linux/interrupt.h>
	34	#include <linux/platform_device.h>
	35
	36	#include <asm/blackfin.h>
	37	#include <asm/irq.h>
	38
	39	#define POLL_TIMEOUT (2 * HZ)
	40
	41	/* SMBus mode*/
4dd39bb1 SZ	42	#define TWI_I2C_MODE_STANDARD 1
	43	#define TWI_I2C_MODE_STANDARDSUB 2
	44	#define TWI_I2C_MODE_COMBINED 3
	45	#define TWI_I2C_MODE_REPEAT 4
d24ecfcc BW	46
d24ecfcc BW	47	struct bfin_twi_iface {
d24ecfcc BW	48	int irq;
	49	spinlock_t lock;
	50	char read_write;
	51	u8 command;
	52	u8 *transPtr;
	53	int readNum;
	54	int writeNum;
	55	int cur_mode;
	56	int manual_stop;
	57	int result;
	58	int timeout_count;
	59	struct timer_list timeout_timer;
	60	struct i2c_adapter adap;
	61	struct completion complete;
4dd39bb1 SZ	62	struct i2c_msg *pmsg;
	63	int msg_num;
	64	int cur_msg;
d24ecfcc BW	65	};
	66
	67	static struct bfin_twi_iface twi_iface;
	68
	69	static void bfin_twi_handle_interrupt(struct bfin_twi_iface *iface)
	70	{
	71	unsigned short twi_int_status = bfin_read_TWI_INT_STAT();
	72	unsigned short mast_stat = bfin_read_TWI_MASTER_STAT();
	73
	74	if (twi_int_status & XMTSERV) {
	75	/* Transmit next data */
	76	if (iface->writeNum > 0) {
	77	bfin_write_TWI_XMT_DATA8(*(iface->transPtr++));
	78	iface->writeNum--;
	79	}
	80	/* start receive immediately after complete sending in
	81	* combine mode.
	82	*/
4dd39bb1	83	else if (iface->cur_mode == TWI_I2C_MODE_COMBINED)
d24ecfcc BW	84	bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL()
d24ecfcc BW	85	\| MDIR \| RSTART);
4dd39bb1	86	else if (iface->manual_stop)
d24ecfcc BW	87	bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL()
d24ecfcc BW	88	\| STOP);
4dd39bb1 SZ	89	else if (iface->cur_mode == TWI_I2C_MODE_REPEAT &&
	90	iface->cur_msg+1 < iface->msg_num)
	91	bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL()
	92	\| RSTART);
d24ecfcc BW	93	SSYNC();
	94	/* Clear status */
	95	bfin_write_TWI_INT_STAT(XMTSERV);
	96	SSYNC();
	97	}
	98	if (twi_int_status & RCVSERV) {
	99	if (iface->readNum > 0) {
	100	/* Receive next data */
	101	*(iface->transPtr) = bfin_read_TWI_RCV_DATA8();
	102	if (iface->cur_mode == TWI_I2C_MODE_COMBINED) {
	103	/* Change combine mode into sub mode after
	104	* read first data.
	105	*/
	106	iface->cur_mode = TWI_I2C_MODE_STANDARDSUB;
	107	/* Get read number from first byte in block
	108	* combine mode.
	109	*/
	110	if (iface->readNum == 1 && iface->manual_stop)
	111	iface->readNum = *iface->transPtr + 1;
	112	}
	113	iface->transPtr++;
	114	iface->readNum--;
	115	} else if (iface->manual_stop) {
	116	bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL()
	117	\| STOP);
	118	SSYNC();
4dd39bb1 SZ	119	} else if (iface->cur_mode == TWI_I2C_MODE_REPEAT &&
	120	iface->cur_msg+1 < iface->msg_num) {
	121	bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL()
	122	\| RSTART);
	123	SSYNC();
d24ecfcc BW	124	}
	125	/* Clear interrupt source */
	126	bfin_write_TWI_INT_STAT(RCVSERV);
	127	SSYNC();
	128	}
	129	if (twi_int_status & MERR) {
	130	bfin_write_TWI_INT_STAT(MERR);
	131	bfin_write_TWI_INT_MASK(0);
	132	bfin_write_TWI_MASTER_STAT(0x3e);
	133	bfin_write_TWI_MASTER_CTL(0);
	134	SSYNC();
4dd39bb1	135	iface->result = -EIO;
d24ecfcc BW	136	/* if both err and complete int stats are set, return proper
	137	* results.
	138	*/
	139	if (twi_int_status & MCOMP) {
	140	bfin_write_TWI_INT_STAT(MCOMP);
	141	bfin_write_TWI_INT_MASK(0);
	142	bfin_write_TWI_MASTER_CTL(0);
	143	SSYNC();
	144	/* If it is a quick transfer, only address bug no data,
	145	* not an err, return 1.
	146	*/
	147	if (iface->writeNum == 0 && (mast_stat & BUFRDERR))
	148	iface->result = 1;
	149	/* If address not acknowledged return -1,
	150	* else return 0.
	151	*/
	152	else if (!(mast_stat & ANAK))
	153	iface->result = 0;
	154	}
	155	complete(&iface->complete);
	156	return;
	157	}
	158	if (twi_int_status & MCOMP) {
	159	bfin_write_TWI_INT_STAT(MCOMP);
	160	SSYNC();
	161	if (iface->cur_mode == TWI_I2C_MODE_COMBINED) {
	162	if (iface->readNum == 0) {
	163	/* set the read number to 1 and ask for manual
	164	* stop in block combine mode
	165	*/
	166	iface->readNum = 1;
	167	iface->manual_stop = 1;
	168	bfin_write_TWI_MASTER_CTL(
	169	bfin_read_TWI_MASTER_CTL()
	170	\| (0xff << 6));
	171	} else {
	172	/* set the readd number in other
	173	* combine mode.
	174	*/
	175	bfin_write_TWI_MASTER_CTL(
	176	(bfin_read_TWI_MASTER_CTL() &
	177	(~(0xff << 6))) \|
	178	( iface->readNum << 6));
	179	}
	180	/* remove restart bit and enable master receive */
	181	bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() &
	182	~RSTART);
	183	bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() \|
	184	MEN \| MDIR);
	185	SSYNC();
4dd39bb1 SZ	186	} else if (iface->cur_mode == TWI_I2C_MODE_REPEAT &&
	187	iface->cur_msg+1 < iface->msg_num) {
	188	iface->cur_msg++;
	189	iface->transPtr = iface->pmsg[iface->cur_msg].buf;
	190	iface->writeNum = iface->readNum =
	191	iface->pmsg[iface->cur_msg].len;
	192	/* Set Transmit device address */
	193	bfin_write_TWI_MASTER_ADDR(
	194	iface->pmsg[iface->cur_msg].addr);
	195	if (iface->pmsg[iface->cur_msg].flags & I2C_M_RD)
	196	iface->read_write = I2C_SMBUS_READ;
	197	else {
	198	iface->read_write = I2C_SMBUS_WRITE;
	199	/* Transmit first data */
	200	if (iface->writeNum > 0) {
	201	bfin_write_TWI_XMT_DATA8(
	202	*(iface->transPtr++));
	203	iface->writeNum--;
	204	SSYNC();
	205	}
	206	}
	207
	208	if (iface->pmsg[iface->cur_msg].len <= 255)
	209	bfin_write_TWI_MASTER_CTL(
	210	iface->pmsg[iface->cur_msg].len << 6);
	211	else {
	212	bfin_write_TWI_MASTER_CTL(0xff << 6);
	213	iface->manual_stop = 1;
	214	}
	215	/* remove restart bit and enable master receive */
	216	bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() &
	217	~RSTART);
	218	bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() \|
	219	MEN \| ((iface->read_write == I2C_SMBUS_READ) ?
	220	MDIR : 0));
	221	SSYNC();
d24ecfcc BW	222	} else {
	223	iface->result = 1;
	224	bfin_write_TWI_INT_MASK(0);
	225	bfin_write_TWI_MASTER_CTL(0);
	226	SSYNC();
	227	complete(&iface->complete);
	228	}
	229	}
	230	}
	231
	232	/* Interrupt handler */
	233	static irqreturn_t bfin_twi_interrupt_entry(int irq, void *dev_id)
	234	{
	235	struct bfin_twi_iface *iface = dev_id;
	236	unsigned long flags;
	237
	238	spin_lock_irqsave(&iface->lock, flags);
	239	del_timer(&iface->timeout_timer);
	240	bfin_twi_handle_interrupt(iface);
	241	spin_unlock_irqrestore(&iface->lock, flags);
	242	return IRQ_HANDLED;
	243	}
	244
	245	static void bfin_twi_timeout(unsigned long data)
	246	{
	247	struct bfin_twi_iface iface = (struct bfin_twi_iface )data;
	248	unsigned long flags;
	249
	250	spin_lock_irqsave(&iface->lock, flags);
	251	bfin_twi_handle_interrupt(iface);
	252	if (iface->result == 0) {
	253	iface->timeout_count--;
	254	if (iface->timeout_count > 0) {
	255	iface->timeout_timer.expires = jiffies + POLL_TIMEOUT;
	256	add_timer(&iface->timeout_timer);
	257	} else {
	258	iface->result = -1;
	259	complete(&iface->complete);
	260	}
	261	}
	262	spin_unlock_irqrestore(&iface->lock, flags);
	263	}
	264
	265	/*
	266	* Generic i2c master transfer entrypoint
	267	*/
	268	static int bfin_twi_master_xfer(struct i2c_adapter *adap,
	269	struct i2c_msg *msgs, int num)
	270	{
	271	struct bfin_twi_iface *iface = adap->algo_data;
	272	struct i2c_msg *pmsg;
d24ecfcc BW	273	int rc = 0;
	274
	275	if (!(bfin_read_TWI_CONTROL() & TWI_ENA))
	276	return -ENXIO;
	277
d24ecfcc	278	while (bfin_read_TWI_MASTER_STAT() & BUSBUSY) {
d24ecfcc	279	yield();
d24ecfcc BW	280	}
d24ecfcc BW	281
4dd39bb1 SZ	282	iface->pmsg = msgs;
	283	iface->msg_num = num;
	284	iface->cur_msg = 0;
d24ecfcc	285
4dd39bb1 SZ	286	pmsg = &msgs[0];
	287	if (pmsg->flags & I2C_M_TEN) {
	288	dev_err(&adap->dev, "10 bits addr not supported!\n");
	289	return -EINVAL;
	290	}
d24ecfcc	291
4dd39bb1 SZ	292	iface->cur_mode = TWI_I2C_MODE_REPEAT;
	293	iface->manual_stop = 0;
	294	iface->transPtr = pmsg->buf;
	295	iface->writeNum = iface->readNum = pmsg->len;
	296	iface->result = 0;
	297	iface->timeout_count = 10;
	298	/* Set Transmit device address */
	299	bfin_write_TWI_MASTER_ADDR(pmsg->addr);
	300
	301	/* FIFO Initiation. Data in FIFO should be
	302	* discarded before start a new operation.
	303	*/
	304	bfin_write_TWI_FIFO_CTL(0x3);
	305	SSYNC();
	306	bfin_write_TWI_FIFO_CTL(0);
	307	SSYNC();
	308
	309	if (pmsg->flags & I2C_M_RD)
	310	iface->read_write = I2C_SMBUS_READ;
	311	else {
	312	iface->read_write = I2C_SMBUS_WRITE;
	313	/* Transmit first data */
	314	if (iface->writeNum > 0) {
	315	bfin_write_TWI_XMT_DATA8(*(iface->transPtr++));
	316	iface->writeNum--;
	317	SSYNC();
d24ecfcc	318	}
4dd39bb1	319	}
d24ecfcc	320
4dd39bb1 SZ	321	/* clear int stat */
4dd39bb1 SZ	322	bfin_write_TWI_INT_STAT(MERR \| MCOMP \| XMTSERV \| RCVSERV);
d24ecfcc	323
4dd39bb1 SZ	324	/* Interrupt mask . Enable XMT, RCV interrupt */
	325	bfin_write_TWI_INT_MASK(MCOMP \| MERR \| RCVSERV \| XMTSERV);
	326	SSYNC();
d24ecfcc	327
4dd39bb1 SZ	328	if (pmsg->len <= 255)
	329	bfin_write_TWI_MASTER_CTL(pmsg->len << 6);
	330	else {
	331	bfin_write_TWI_MASTER_CTL(0xff << 6);
	332	iface->manual_stop = 1;
	333	}
d24ecfcc	334
4dd39bb1 SZ	335	iface->timeout_timer.expires = jiffies + POLL_TIMEOUT;
4dd39bb1 SZ	336	add_timer(&iface->timeout_timer);
d24ecfcc	337
4dd39bb1 SZ	338	/* Master enable */
	339	bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() \| MEN \|
	340	((iface->read_write == I2C_SMBUS_READ) ? MDIR : 0) \|
	341	((CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ > 100) ? FAST : 0));
	342	SSYNC();
	343
	344	wait_for_completion(&iface->complete);
	345
	346	rc = iface->result;
d24ecfcc	347
4dd39bb1 SZ	348	if (rc == 1)
	349	return num;
	350	else
	351	return rc;
d24ecfcc BW	352	}
	353
	354	/*
	355	* SMBus type transfer entrypoint
	356	*/
	357
	358	int bfin_twi_smbus_xfer(struct i2c_adapter *adap, u16 addr,
	359	unsigned short flags, char read_write,
	360	u8 command, int size, union i2c_smbus_data *data)
	361	{
	362	struct bfin_twi_iface *iface = adap->algo_data;
	363	int rc = 0;
	364
	365	if (!(bfin_read_TWI_CONTROL() & TWI_ENA))
	366	return -ENXIO;
	367
d24ecfcc	368	while (bfin_read_TWI_MASTER_STAT() & BUSBUSY) {
d24ecfcc	369	yield();
d24ecfcc BW	370	}
	371
	372	iface->writeNum = 0;
	373	iface->readNum = 0;
	374
	375	/* Prepare datas & select mode */
	376	switch (size) {
	377	case I2C_SMBUS_QUICK:
	378	iface->transPtr = NULL;
	379	iface->cur_mode = TWI_I2C_MODE_STANDARD;
	380	break;
	381	case I2C_SMBUS_BYTE:
	382	if (data == NULL)
	383	iface->transPtr = NULL;
	384	else {
	385	if (read_write == I2C_SMBUS_READ)
	386	iface->readNum = 1;
	387	else
	388	iface->writeNum = 1;
	389	iface->transPtr = &data->byte;
	390	}
	391	iface->cur_mode = TWI_I2C_MODE_STANDARD;
	392	break;
	393	case I2C_SMBUS_BYTE_DATA:
	394	if (read_write == I2C_SMBUS_READ) {
	395	iface->readNum = 1;
	396	iface->cur_mode = TWI_I2C_MODE_COMBINED;
	397	} else {
	398	iface->writeNum = 1;
	399	iface->cur_mode = TWI_I2C_MODE_STANDARDSUB;
	400	}
	401	iface->transPtr = &data->byte;
	402	break;
	403	case I2C_SMBUS_WORD_DATA:
	404	if (read_write == I2C_SMBUS_READ) {
	405	iface->readNum = 2;
	406	iface->cur_mode = TWI_I2C_MODE_COMBINED;
	407	} else {
	408	iface->writeNum = 2;
	409	iface->cur_mode = TWI_I2C_MODE_STANDARDSUB;
	410	}
	411	iface->transPtr = (u8 *)&data->word;
	412	break;
	413	case I2C_SMBUS_PROC_CALL:
	414	iface->writeNum = 2;
	415	iface->readNum = 2;
	416	iface->cur_mode = TWI_I2C_MODE_COMBINED;
	417	iface->transPtr = (u8 *)&data->word;
	418	break;
	419	case I2C_SMBUS_BLOCK_DATA:
	420	if (read_write == I2C_SMBUS_READ) {
	421	iface->readNum = 0;
	422	iface->cur_mode = TWI_I2C_MODE_COMBINED;
	423	} else {
	424	iface->writeNum = data->block[0] + 1;
	425	iface->cur_mode = TWI_I2C_MODE_STANDARDSUB;
	426	}
	427	iface->transPtr = data->block;
	428	break;
	429	default:
	430	return -1;
	431	}
	432
	433	iface->result = 0;
434	iface->manual_stop = 0;
435	iface->read_write = read_write;
436	iface->command = command;
437	iface->timeout_count = 10;
438
439	/* FIFO Initiation. Data in FIFO should be discarded before
440	* start a new operation.
441	*/
442	bfin_write_TWI_FIFO_CTL(0x3);
443	SSYNC();
444	bfin_write_TWI_FIFO_CTL(0);
445
446	/* clear int stat */
447	bfin_write_TWI_INT_STAT(MERR\|MCOMP\|XMTSERV\|RCVSERV);
448
449	/* Set Transmit device address */
450	bfin_write_TWI_MASTER_ADDR(addr);
451	SSYNC();
452
453	iface->timeout_timer.expires = jiffies + POLL_TIMEOUT;
454	add_timer(&iface->timeout_timer);
455
456	switch (iface->cur_mode) {
457	case TWI_I2C_MODE_STANDARDSUB:
458	bfin_write_TWI_XMT_DATA8(iface->command);
459	bfin_write_TWI_INT_MASK(MCOMP \| MERR \|
460	((iface->read_write == I2C_SMBUS_READ) ?
461	RCVSERV : XMTSERV));
462	SSYNC();
463
464	if (iface->writeNum + 1 <= 255)
465	bfin_write_TWI_MASTER_CTL((iface->writeNum + 1) << 6);
466	else {
467	bfin_write_TWI_MASTER_CTL(0xff << 6);
468	iface->manual_stop = 1;
469	}
470	/* Master enable */
471	bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() \| MEN \|
472	((CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ>100) ? FAST : 0));
473	break;
474	case TWI_I2C_MODE_COMBINED:
475	bfin_write_TWI_XMT_DATA8(iface->command);
476	bfin_write_TWI_INT_MASK(MCOMP \| MERR \| RCVSERV \| XMTSERV);
477	SSYNC();
478
479	if (iface->writeNum > 0)
480	bfin_write_TWI_MASTER_CTL((iface->writeNum + 1) << 6);
481	else
482	bfin_write_TWI_MASTER_CTL(0x1 << 6);
483	/* Master enable */
484	bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() \| MEN \|
485	((CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ>100) ? FAST : 0));
486	break;
487	default:
488	bfin_write_TWI_MASTER_CTL(0);
489	if (size != I2C_SMBUS_QUICK) {
490	/* Don't access xmit data register when this is a
491	* read operation.
492	*/
493	if (iface->read_write != I2C_SMBUS_READ) {
494	if (iface->writeNum > 0) {
495	bfin_write_TWI_XMT_DATA8(*(iface->transPtr++));
496	if (iface->writeNum <= 255)
497	bfin_write_TWI_MASTER_CTL(iface->writeNum << 6);
498	else {
499	bfin_write_TWI_MASTER_CTL(0xff << 6);
500	iface->manual_stop = 1;
501	}
502	iface->writeNum--;
503	} else {
504	bfin_write_TWI_XMT_DATA8(iface->command);
505	bfin_write_TWI_MASTER_CTL(1 << 6);
506	}
507	} else {
508	if (iface->readNum > 0 && iface->readNum <= 255)
509	bfin_write_TWI_MASTER_CTL(iface->readNum << 6);
510	else if (iface->readNum > 255) {
511	bfin_write_TWI_MASTER_CTL(0xff << 6);
512	iface->manual_stop = 1;
513	} else {
514	del_timer(&iface->timeout_timer);
515	break;
516	}
517	}
518	}
519	bfin_write_TWI_INT_MASK(MCOMP \| MERR \|
520	((iface->read_write == I2C_SMBUS_READ) ?
521	RCVSERV : XMTSERV));
522	SSYNC();
523
524	/* Master enable */
525	bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() \| MEN \|
526	((iface->read_write == I2C_SMBUS_READ) ? MDIR : 0) \|
527	((CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ > 100) ? FAST : 0));
528	break;
529	}
530	SSYNC();
531
532	wait_for_completion(&iface->complete);
533
534	rc = (iface->result >= 0) ? 0 : -1;
535
d24ecfcc BW	536	return rc;
	537	}
	538
	539	/*
	540	* Return what the adapter supports
	541	*/
	542	static u32 bfin_twi_functionality(struct i2c_adapter *adap)
	543	{
	544	return I2C_FUNC_SMBUS_QUICK \| I2C_FUNC_SMBUS_BYTE \|
	545	I2C_FUNC_SMBUS_BYTE_DATA \| I2C_FUNC_SMBUS_WORD_DATA \|
	546	I2C_FUNC_SMBUS_BLOCK_DATA \| I2C_FUNC_SMBUS_PROC_CALL \|
	547	I2C_FUNC_I2C;
	548	}
	549
	550
	551	static struct i2c_algorithm bfin_twi_algorithm = {
	552	.master_xfer = bfin_twi_master_xfer,
	553	.smbus_xfer = bfin_twi_smbus_xfer,
	554	.functionality = bfin_twi_functionality,
	555	};
	556
	557
	558	static int i2c_bfin_twi_suspend(struct platform_device *dev, pm_message_t state)
	559	{
	560	/* struct bfin_twi_iface iface = platform_get_drvdata(dev);/
	561
	562	/* Disable TWI */
	563	bfin_write_TWI_CONTROL(bfin_read_TWI_CONTROL() & ~TWI_ENA);
	564	SSYNC();
	565
	566	return 0;
	567	}
	568
	569	static int i2c_bfin_twi_resume(struct platform_device *dev)
	570	{
	571	/* struct bfin_twi_iface iface = platform_get_drvdata(dev);/
	572
	573	/* Enable TWI */
	574	bfin_write_TWI_CONTROL(bfin_read_TWI_CONTROL() \| TWI_ENA);
	575	SSYNC();
	576
	577	return 0;
	578	}
	579
	580	static int i2c_bfin_twi_probe(struct platform_device *dev)
	581	{
	582	struct bfin_twi_iface *iface = &twi_iface;
	583	struct i2c_adapter *p_adap;
	584	int rc;
	585
d24ecfcc BW	586	spin_lock_init(&(iface->lock));
	587	init_completion(&(iface->complete));
	588	iface->irq = IRQ_TWI;
	589
	590	init_timer(&(iface->timeout_timer));
	591	iface->timeout_timer.function = bfin_twi_timeout;
	592	iface->timeout_timer.data = (unsigned long)iface;
	593
	594	p_adap = &iface->adap;
	595	p_adap->id = I2C_HW_BLACKFIN;
991dee59	596	p_adap->nr = dev->id;
d24ecfcc BW	597	strlcpy(p_adap->name, dev->name, sizeof(p_adap->name));
	598	p_adap->algo = &bfin_twi_algorithm;
	599	p_adap->algo_data = iface;
	600	p_adap->class = I2C_CLASS_ALL;
	601	p_adap->dev.parent = &dev->dev;
	602
	603	rc = request_irq(iface->irq, bfin_twi_interrupt_entry,
	604	IRQF_DISABLED, dev->name, iface);
	605	if (rc) {
	606	dev_err(&(p_adap->dev), "i2c-bfin-twi: can't get IRQ %d !\n",
	607	iface->irq);
	608	return -ENODEV;
	609	}
	610
	611	/* Set TWI internal clock as 10MHz */
	612	bfin_write_TWI_CONTROL(((get_sclk() / 1024 / 1024 + 5) / 10) & 0x7F);
	613
	614	/* Set Twi interface clock as specified */
	615	bfin_write_TWI_CLKDIV((( 5*1024 / CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ )
	616	<< 8) \| (( 5*1024 / CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ )
	617	& 0xFF));
	618
	619	/* Enable TWI */
	620	bfin_write_TWI_CONTROL(bfin_read_TWI_CONTROL() \| TWI_ENA);
	621	SSYNC();
	622
991dee59	623	rc = i2c_add_numbered_adapter(p_adap);
d24ecfcc BW	624	if (rc < 0)
	625	free_irq(iface->irq, iface);
	626	else
	627	platform_set_drvdata(dev, iface);
	628
	629	return rc;
	630	}
	631
	632	static int i2c_bfin_twi_remove(struct platform_device *pdev)
	633	{
	634	struct bfin_twi_iface *iface = platform_get_drvdata(pdev);
	635
	636	platform_set_drvdata(pdev, NULL);
	637
	638	i2c_del_adapter(&(iface->adap));
	639	free_irq(iface->irq, iface);
	640
	641	return 0;
	642	}
	643
	644	static struct platform_driver i2c_bfin_twi_driver = {
	645	.probe = i2c_bfin_twi_probe,
	646	.remove = i2c_bfin_twi_remove,
	647	.suspend = i2c_bfin_twi_suspend,
	648	.resume = i2c_bfin_twi_resume,
	649	.driver = {
	650	.name = "i2c-bfin-twi",
	651	.owner = THIS_MODULE,
	652	},
	653	};
	654
	655	static int __init i2c_bfin_twi_init(void)
	656	{
	657	pr_info("I2C: Blackfin I2C TWI driver\n");
	658
	659	return platform_driver_register(&i2c_bfin_twi_driver);
	660	}
	661
	662	static void __exit i2c_bfin_twi_exit(void)
	663	{
	664	platform_driver_unregister(&i2c_bfin_twi_driver);
	665	}
	666
	667	MODULE_AUTHOR("Sonic Zhang <sonic.zhang@analog.com>");
	668	MODULE_DESCRIPTION("I2C-Bus adapter routines for Blackfin TWI");
	669	MODULE_LICENSE("GPL");
	670
	671	module_init(i2c_bfin_twi_init);
	672	module_exit(i2c_bfin_twi_exit);