drivers/i2c/busses/i2c-bfin-twi.c

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