i2c-bfin-twi.c 18.3 KB
Newer Older
1
/*
2
 * Blackfin On-Chip Two Wire Interface Driver
3
 *
4
 * Copyright 2005-2007 Analog Devices Inc.
5
 *
6
 * Enter bugs at http://blackfin.uclinux.org/
7
 *
8
 * Licensed under the GPL-2 or later.
9 10 11 12 13 14 15 16 17 18 19 20 21 22
 */

#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>
23
#include <asm/portmux.h>
24 25 26 27 28
#include <asm/irq.h>

#define POLL_TIMEOUT       (2 * HZ)

/* SMBus mode*/
29 30 31 32
#define TWI_I2C_MODE_STANDARD		1
#define TWI_I2C_MODE_STANDARDSUB	2
#define TWI_I2C_MODE_COMBINED		3
#define TWI_I2C_MODE_REPEAT		4
33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48

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;
49 50 51
	struct i2c_msg 		*pmsg;
	int			msg_num;
	int			cur_msg;
52
	void __iomem		*regs_base;
53 54
};

55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77

#define DEFINE_TWI_REG(reg, off) \
static inline u16 read_##reg(struct bfin_twi_iface *iface) \
	{ return bfin_read16(iface->regs_base + (off)); } \
static inline void write_##reg(struct bfin_twi_iface *iface, u16 v) \
	{ bfin_write16(iface->regs_base + (off), v); }

DEFINE_TWI_REG(CLKDIV, 0x00)
DEFINE_TWI_REG(CONTROL, 0x04)
DEFINE_TWI_REG(SLAVE_CTL, 0x08)
DEFINE_TWI_REG(SLAVE_STAT, 0x0C)
DEFINE_TWI_REG(SLAVE_ADDR, 0x10)
DEFINE_TWI_REG(MASTER_CTL, 0x14)
DEFINE_TWI_REG(MASTER_STAT, 0x18)
DEFINE_TWI_REG(MASTER_ADDR, 0x1C)
DEFINE_TWI_REG(INT_STAT, 0x20)
DEFINE_TWI_REG(INT_MASK, 0x24)
DEFINE_TWI_REG(FIFO_CTL, 0x28)
DEFINE_TWI_REG(FIFO_STAT, 0x2C)
DEFINE_TWI_REG(XMT_DATA8, 0x80)
DEFINE_TWI_REG(XMT_DATA16, 0x84)
DEFINE_TWI_REG(RCV_DATA8, 0x88)
DEFINE_TWI_REG(RCV_DATA16, 0x8C)
78

79 80 81 82 83
static const u16 pin_req[2][3] = {
	{P_TWI0_SCL, P_TWI0_SDA, 0},
	{P_TWI1_SCL, P_TWI1_SDA, 0},
};

84 85
static void bfin_twi_handle_interrupt(struct bfin_twi_iface *iface)
{
86 87
	unsigned short twi_int_status = read_INT_STAT(iface);
	unsigned short mast_stat = read_MASTER_STAT(iface);
88 89 90 91

	if (twi_int_status & XMTSERV) {
		/* Transmit next data */
		if (iface->writeNum > 0) {
92
			write_XMT_DATA8(iface, *(iface->transPtr++));
93 94 95 96 97
			iface->writeNum--;
		}
		/* start receive immediately after complete sending in
		 * combine mode.
		 */
98
		else if (iface->cur_mode == TWI_I2C_MODE_COMBINED)
99 100
			write_MASTER_CTL(iface,
				read_MASTER_CTL(iface) | MDIR | RSTART);
101
		else if (iface->manual_stop)
102 103
			write_MASTER_CTL(iface,
				read_MASTER_CTL(iface) | STOP);
104 105
		else if (iface->cur_mode == TWI_I2C_MODE_REPEAT &&
				iface->cur_msg+1 < iface->msg_num)
106 107
			write_MASTER_CTL(iface,
				read_MASTER_CTL(iface) | RSTART);
108 109
		SSYNC();
		/* Clear status */
110
		write_INT_STAT(iface, XMTSERV);
111 112 113 114 115
		SSYNC();
	}
	if (twi_int_status & RCVSERV) {
		if (iface->readNum > 0) {
			/* Receive next data */
116
			*(iface->transPtr) = read_RCV_DATA8(iface);
117 118 119 120 121 122 123 124 125 126 127 128 129 130
			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) {
131 132
			write_MASTER_CTL(iface,
				read_MASTER_CTL(iface) | STOP);
133
			SSYNC();
134 135
		} else if (iface->cur_mode == TWI_I2C_MODE_REPEAT &&
				iface->cur_msg+1 < iface->msg_num) {
136 137
			write_MASTER_CTL(iface,
				read_MASTER_CTL(iface) | RSTART);
138
			SSYNC();
139 140
		}
		/* Clear interrupt source */
141
		write_INT_STAT(iface, RCVSERV);
142 143 144
		SSYNC();
	}
	if (twi_int_status & MERR) {
145 146 147 148
		write_INT_STAT(iface, MERR);
		write_INT_MASK(iface, 0);
		write_MASTER_STAT(iface, 0x3e);
		write_MASTER_CTL(iface, 0);
149
		SSYNC();
150
		iface->result = -EIO;
151 152 153 154
		/* if both err and complete int stats are set, return proper
		 * results.
		 */
		if (twi_int_status & MCOMP) {
155 156 157
			write_INT_STAT(iface, MCOMP);
			write_INT_MASK(iface, 0);
			write_MASTER_CTL(iface, 0);
158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173
			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) {
174
		write_INT_STAT(iface, MCOMP);
175 176 177 178 179 180 181 182
		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;
183 184
				write_MASTER_CTL(iface,
					read_MASTER_CTL(iface) | (0xff << 6));
185 186 187 188
			} else {
				/* set the readd number in other
				 * combine mode.
				 */
189 190
				write_MASTER_CTL(iface,
					(read_MASTER_CTL(iface) &
191
					(~(0xff << 6))) |
192
					(iface->readNum << 6));
193 194
			}
			/* remove restart bit and enable master receive */
195 196 197 198
			write_MASTER_CTL(iface,
				read_MASTER_CTL(iface) & ~RSTART);
			write_MASTER_CTL(iface,
				read_MASTER_CTL(iface) | MEN | MDIR);
199
			SSYNC();
200 201 202 203 204 205 206
		} 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 */
207
			write_MASTER_ADDR(iface,
208 209 210 211 212 213 214
				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) {
215
					write_XMT_DATA8(iface,
216 217 218 219 220 221 222
						*(iface->transPtr++));
					iface->writeNum--;
					SSYNC();
				}
			}

			if (iface->pmsg[iface->cur_msg].len <= 255)
223
				write_MASTER_CTL(iface,
224 225
				iface->pmsg[iface->cur_msg].len << 6);
			else {
226
				write_MASTER_CTL(iface, 0xff << 6);
227 228 229
				iface->manual_stop = 1;
			}
			/* remove restart bit and enable master receive */
230 231 232
			write_MASTER_CTL(iface,
				read_MASTER_CTL(iface) & ~RSTART);
			write_MASTER_CTL(iface, read_MASTER_CTL(iface) |
233 234 235
				MEN | ((iface->read_write == I2C_SMBUS_READ) ?
				MDIR : 0));
			SSYNC();
236 237
		} else {
			iface->result = 1;
238 239
			write_INT_MASK(iface, 0);
			write_MASTER_CTL(iface, 0);
240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288
			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;

289
	if (!(read_CONTROL(iface) & TWI_ENA))
290 291
		return -ENXIO;

292
	while (read_MASTER_STAT(iface) & BUSBUSY)
293 294
		yield();

295 296 297
	iface->pmsg = msgs;
	iface->msg_num = num;
	iface->cur_msg = 0;
298

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

305 306 307 308 309 310
	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;
311
	init_completion(&(iface->complete));
312
	/* Set Transmit device address */
313
	write_MASTER_ADDR(iface, pmsg->addr);
314 315 316 317

	/* FIFO Initiation. Data in FIFO should be
	 *  discarded before start a new operation.
	 */
318
	write_FIFO_CTL(iface, 0x3);
319
	SSYNC();
320
	write_FIFO_CTL(iface, 0);
321 322 323 324 325 326 327 328
	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) {
329
			write_XMT_DATA8(iface, *(iface->transPtr++));
330 331
			iface->writeNum--;
			SSYNC();
332
		}
333
	}
334

335
	/* clear int stat */
336
	write_INT_STAT(iface, MERR | MCOMP | XMTSERV | RCVSERV);
337

338
	/* Interrupt mask . Enable XMT, RCV interrupt */
339
	write_INT_MASK(iface, MCOMP | MERR | RCVSERV | XMTSERV);
340
	SSYNC();
341

342
	if (pmsg->len <= 255)
343
		write_MASTER_CTL(iface, pmsg->len << 6);
344
	else {
345
		write_MASTER_CTL(iface, 0xff << 6);
346 347
		iface->manual_stop = 1;
	}
348

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

352
	/* Master enable */
353
	write_MASTER_CTL(iface, read_MASTER_CTL(iface) | MEN |
354 355 356 357 358 359 360
		((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;
361

362 363 364 365
	if (rc == 1)
		return num;
	else
		return rc;
366 367 368 369 370 371 372 373 374 375 376 377 378
}

/*
 * 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;

379
	if (!(read_CONTROL(iface) & TWI_ENA))
380 381
		return -ENXIO;

382
	while (read_MASTER_STAT(iface) & BUSBUSY)
383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450
		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;
451
	init_completion(&(iface->complete));
452 453 454 455

	/* FIFO Initiation. Data in FIFO should be discarded before
	 * start a new operation.
	 */
456
	write_FIFO_CTL(iface, 0x3);
457
	SSYNC();
458
	write_FIFO_CTL(iface, 0);
459 460

	/* clear int stat */
461
	write_INT_STAT(iface, MERR | MCOMP | XMTSERV | RCVSERV);
462 463

	/* Set Transmit device address */
464
	write_MASTER_ADDR(iface, addr);
465 466 467 468 469 470 471
	SSYNC();

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

	switch (iface->cur_mode) {
	case TWI_I2C_MODE_STANDARDSUB:
472 473
		write_XMT_DATA8(iface, iface->command);
		write_INT_MASK(iface, MCOMP | MERR |
474 475 476 477 478
			((iface->read_write == I2C_SMBUS_READ) ?
			RCVSERV : XMTSERV));
		SSYNC();

		if (iface->writeNum + 1 <= 255)
479
			write_MASTER_CTL(iface, (iface->writeNum + 1) << 6);
480
		else {
481
			write_MASTER_CTL(iface, 0xff << 6);
482 483 484
			iface->manual_stop = 1;
		}
		/* Master enable */
485
		write_MASTER_CTL(iface, read_MASTER_CTL(iface) | MEN |
486 487 488
			((CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ>100) ? FAST : 0));
		break;
	case TWI_I2C_MODE_COMBINED:
489 490
		write_XMT_DATA8(iface, iface->command);
		write_INT_MASK(iface, MCOMP | MERR | RCVSERV | XMTSERV);
491 492 493
		SSYNC();

		if (iface->writeNum > 0)
494
			write_MASTER_CTL(iface, (iface->writeNum + 1) << 6);
495
		else
496
			write_MASTER_CTL(iface, 0x1 << 6);
497
		/* Master enable */
498
		write_MASTER_CTL(iface, read_MASTER_CTL(iface) | MEN |
499 500 501
			((CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ>100) ? FAST : 0));
		break;
	default:
502
		write_MASTER_CTL(iface, 0);
503 504 505 506 507 508
		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) {
509 510
					write_XMT_DATA8(iface,
						*(iface->transPtr++));
511
					if (iface->writeNum <= 255)
512 513
						write_MASTER_CTL(iface,
							iface->writeNum << 6);
514
					else {
515 516
						write_MASTER_CTL(iface,
							0xff << 6);
517 518 519 520
						iface->manual_stop = 1;
					}
					iface->writeNum--;
				} else {
521 522
					write_XMT_DATA8(iface, iface->command);
					write_MASTER_CTL(iface, 1 << 6);
523 524 525
				}
			} else {
				if (iface->readNum > 0 && iface->readNum <= 255)
526 527
					write_MASTER_CTL(iface,
						iface->readNum << 6);
528
				else if (iface->readNum > 255) {
529
					write_MASTER_CTL(iface, 0xff << 6);
530 531 532 533 534 535 536
					iface->manual_stop = 1;
				} else {
					del_timer(&iface->timeout_timer);
					break;
				}
			}
		}
537
		write_INT_MASK(iface, MCOMP | MERR |
538 539 540 541 542
			((iface->read_write == I2C_SMBUS_READ) ?
			RCVSERV : XMTSERV));
		SSYNC();

		/* Master enable */
543
		write_MASTER_CTL(iface, read_MASTER_CTL(iface) | MEN |
544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577
			((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)
{
578
	struct bfin_twi_iface *iface = platform_get_drvdata(dev);
579 580

	/* Disable TWI */
581
	write_CONTROL(iface, read_CONTROL(iface) & ~TWI_ENA);
582 583 584 585 586 587 588
	SSYNC();

	return 0;
}

static int i2c_bfin_twi_resume(struct platform_device *dev)
{
589
	struct bfin_twi_iface *iface = platform_get_drvdata(dev);
590 591

	/* Enable TWI */
592
	write_CONTROL(iface, read_CONTROL(iface) | TWI_ENA);
593 594 595 596 597
	SSYNC();

	return 0;
}

598
static int i2c_bfin_twi_probe(struct platform_device *pdev)
599
{
600
	struct bfin_twi_iface *iface;
601
	struct i2c_adapter *p_adap;
602
	struct resource *res;
603 604
	int rc;

605 606 607 608 609 610 611
	iface = kzalloc(sizeof(struct bfin_twi_iface), GFP_KERNEL);
	if (!iface) {
		dev_err(&pdev->dev, "Cannot allocate memory\n");
		rc = -ENOMEM;
		goto out_error_nomem;
	}

612
	spin_lock_init(&(iface->lock));
613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634

	/* Find and map our resources */
	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
	if (res == NULL) {
		dev_err(&pdev->dev, "Cannot get IORESOURCE_MEM\n");
		rc = -ENOENT;
		goto out_error_get_res;
	}

	iface->regs_base = ioremap(res->start, res->end - res->start + 1);
	if (iface->regs_base == NULL) {
		dev_err(&pdev->dev, "Cannot map IO\n");
		rc = -ENXIO;
		goto out_error_ioremap;
	}

	iface->irq = platform_get_irq(pdev, 0);
	if (iface->irq < 0) {
		dev_err(&pdev->dev, "No IRQ specified\n");
		rc = -ENOENT;
		goto out_error_no_irq;
	}
635 636 637 638 639 640 641

	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;
642 643
	p_adap->nr = pdev->id;
	strlcpy(p_adap->name, pdev->name, sizeof(p_adap->name));
644 645 646
	p_adap->algo = &bfin_twi_algorithm;
	p_adap->algo_data = iface;
	p_adap->class = I2C_CLASS_ALL;
647
	p_adap->dev.parent = &pdev->dev;
648

649 650 651 652 653 654
	rc = peripheral_request_list(pin_req[pdev->id], "i2c-bfin-twi");
	if (rc) {
		dev_err(&pdev->dev, "Can't setup pin mux!\n");
		goto out_error_pin_mux;
	}

655
	rc = request_irq(iface->irq, bfin_twi_interrupt_entry,
656
		IRQF_DISABLED, pdev->name, iface);
657
	if (rc) {
658 659 660
		dev_err(&pdev->dev, "Can't get IRQ %d !\n", iface->irq);
		rc = -ENODEV;
		goto out_error_req_irq;
661 662 663
	}

	/* Set TWI internal clock as 10MHz */
664
	write_CONTROL(iface, ((get_sclk() / 1024 / 1024 + 5) / 10) & 0x7F);
665 666

	/* Set Twi interface clock as specified */
667 668
	write_CLKDIV(iface, ((5*1024 / CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ)
			<< 8) | ((5*1024 / CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ)
669 670 671
			& 0xFF));

	/* Enable TWI */
672
	write_CONTROL(iface, read_CONTROL(iface) | TWI_ENA);
673 674
	SSYNC();

675
	rc = i2c_add_numbered_adapter(p_adap);
676 677 678 679 680 681
	if (rc < 0) {
		dev_err(&pdev->dev, "Can't add i2c adapter!\n");
		goto out_error_add_adapter;
	}

	platform_set_drvdata(pdev, iface);
682

683 684
	dev_info(&pdev->dev, "Blackfin BF5xx on-chip I2C TWI Contoller, "
		"regs_base@%p\n", iface->regs_base);
685 686 687 688 689 690 691

	return 0;

out_error_add_adapter:
	free_irq(iface->irq, iface);
out_error_req_irq:
out_error_no_irq:
692 693
	peripheral_free_list(pin_req[pdev->id]);
out_error_pin_mux:
694 695 696 697 698
	iounmap(iface->regs_base);
out_error_ioremap:
out_error_get_res:
	kfree(iface);
out_error_nomem:
699 700 701 702 703 704 705 706 707 708 709
	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);
710
	peripheral_free_list(pin_req[pdev->id]);
711 712
	iounmap(iface->regs_base);
	kfree(iface);
713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739

	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)
{
	return platform_driver_register(&i2c_bfin_twi_driver);
}

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

module_init(i2c_bfin_twi_init);
module_exit(i2c_bfin_twi_exit);
740 741 742 743

MODULE_AUTHOR("Bryan Wu, Sonic Zhang");
MODULE_DESCRIPTION("Blackfin BF5xx on-chip I2C TWI Contoller Driver");
MODULE_LICENSE("GPL");
744
MODULE_ALIAS("platform:i2c-bfin-twi");