drivers/i2c/busses/i2c-fsi.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * FSI-attached I2C master algorithm
   4  *
   5  * Copyright 2018 IBM Corporation
   6  *
   7  * This program is free software; you can redistribute it and/or
   8  * modify it under the terms of the GNU General Public License
   9  * as published by the Free Software Foundation; either version
  10  * 2 of the License, or (at your option) any later version.
  11  */
  12
  13 #include <linux/bitfield.h>
  14 #include <linux/bitops.h>
  15 #include <linux/delay.h>
  16 #include <linux/device.h>
  17 #include <linux/errno.h>
  18 #include <linux/fsi.h>
  19 #include <linux/i2c.h>
  20 #include <linux/jiffies.h>
  21 #include <linux/kernel.h>
  22 #include <linux/list.h>
  23 #include <linux/module.h>
  24 #include <linux/mutex.h>
  25 #include <linux/of.h>
  26 #include <linux/slab.h>
  27
  28 #define FSI_ENGID_I2C           0x7
  29
  30 #define I2C_DEFAULT_CLK_DIV     6
  31
  32 /* i2c registers */
  33 #define I2C_FSI_FIFO            0x00
  34 #define I2C_FSI_CMD             0x04
  35 #define I2C_FSI_MODE            0x08
  36 #define I2C_FSI_WATER_MARK      0x0C
  37 #define I2C_FSI_INT_MASK        0x10
  38 #define I2C_FSI_INT_COND        0x14
  39 #define I2C_FSI_OR_INT_MASK     0x14
  40 #define I2C_FSI_INTS            0x18
  41 #define I2C_FSI_AND_INT_MASK    0x18
  42 #define I2C_FSI_STAT            0x1C
  43 #define I2C_FSI_RESET_I2C       0x1C
  44 #define I2C_FSI_ESTAT           0x20
  45 #define I2C_FSI_RESET_ERR       0x20
  46 #define I2C_FSI_RESID_LEN       0x24
  47 #define I2C_FSI_SET_SCL         0x24
  48 #define I2C_FSI_PORT_BUSY       0x28
  49 #define I2C_FSI_RESET_SCL       0x2C
  50 #define I2C_FSI_SET_SDA         0x30
  51 #define I2C_FSI_RESET_SDA       0x34
  52
  53 /* cmd register */
  54 #define I2C_CMD_WITH_START      BIT(31)
  55 #define I2C_CMD_WITH_ADDR       BIT(30)
  56 #define I2C_CMD_RD_CONT         BIT(29)
  57 #define I2C_CMD_WITH_STOP       BIT(28)
  58 #define I2C_CMD_FORCELAUNCH     BIT(27)
  59 #define I2C_CMD_ADDR            GENMASK(23, 17)
  60 #define I2C_CMD_READ            BIT(16)
  61 #define I2C_CMD_LEN             GENMASK(15, 0)
  62
  63 /* mode register */
  64 #define I2C_MODE_CLKDIV         GENMASK(31, 16)
  65 #define I2C_MODE_PORT           GENMASK(15, 10)
  66 #define I2C_MODE_ENHANCED       BIT(3)
  67 #define I2C_MODE_DIAG           BIT(2)
  68 #define I2C_MODE_PACE_ALLOW     BIT(1)
  69 #define I2C_MODE_WRAP           BIT(0)
  70
  71 /* watermark register */
  72 #define I2C_WATERMARK_HI        GENMASK(15, 12)
  73 #define I2C_WATERMARK_LO        GENMASK(7, 4)
  74
  75 #define I2C_FIFO_HI_LVL         4
  76 #define I2C_FIFO_LO_LVL         4
  77
  78 /* interrupt register */
  79 #define I2C_INT_INV_CMD         BIT(15)
  80 #define I2C_INT_PARITY          BIT(14)
  81 #define I2C_INT_BE_OVERRUN      BIT(13)
  82 #define I2C_INT_BE_ACCESS       BIT(12)
  83 #define I2C_INT_LOST_ARB        BIT(11)
  84 #define I2C_INT_NACK            BIT(10)
  85 #define I2C_INT_DAT_REQ         BIT(9)
  86 #define I2C_INT_CMD_COMP        BIT(8)
  87 #define I2C_INT_STOP_ERR        BIT(7)
  88 #define I2C_INT_BUSY            BIT(6)
  89 #define I2C_INT_IDLE            BIT(5)
  90
  91 /* status register */
  92 #define I2C_STAT_INV_CMD        BIT(31)
  93 #define I2C_STAT_PARITY         BIT(30)
  94 #define I2C_STAT_BE_OVERRUN     BIT(29)
  95 #define I2C_STAT_BE_ACCESS      BIT(28)
  96 #define I2C_STAT_LOST_ARB       BIT(27)
  97 #define I2C_STAT_NACK           BIT(26)
  98 #define I2C_STAT_DAT_REQ        BIT(25)
  99 #define I2C_STAT_CMD_COMP       BIT(24)
 100 #define I2C_STAT_STOP_ERR       BIT(23)
 101 #define I2C_STAT_MAX_PORT       GENMASK(22, 16)
 102 #define I2C_STAT_ANY_INT        BIT(15)
 103 #define I2C_STAT_SCL_IN         BIT(11)
 104 #define I2C_STAT_SDA_IN         BIT(10)
 105 #define I2C_STAT_PORT_BUSY      BIT(9)
 106 #define I2C_STAT_SELF_BUSY      BIT(8)
 107 #define I2C_STAT_FIFO_COUNT     GENMASK(7, 0)
 108
 109 #define I2C_STAT_ERR            (I2C_STAT_INV_CMD |                     \
 110                                  I2C_STAT_PARITY |                      \
 111                                  I2C_STAT_BE_OVERRUN |                  \
 112                                  I2C_STAT_BE_ACCESS |                   \
 113                                  I2C_STAT_LOST_ARB |                    \
 114                                  I2C_STAT_NACK |                        \
 115                                  I2C_STAT_STOP_ERR)
 116 #define I2C_STAT_ANY_RESP       (I2C_STAT_ERR |                         \
 117                                  I2C_STAT_DAT_REQ |                     \
 118                                  I2C_STAT_CMD_COMP)
 119
 120 /* extended status register */
 121 #define I2C_ESTAT_FIFO_SZ       GENMASK(31, 24)
 122 #define I2C_ESTAT_SCL_IN_SY     BIT(15)
 123 #define I2C_ESTAT_SDA_IN_SY     BIT(14)
 124 #define I2C_ESTAT_S_SCL         BIT(13)
 125 #define I2C_ESTAT_S_SDA         BIT(12)
 126 #define I2C_ESTAT_M_SCL         BIT(11)
 127 #define I2C_ESTAT_M_SDA         BIT(10)
 128 #define I2C_ESTAT_HI_WATER      BIT(9)
 129 #define I2C_ESTAT_LO_WATER      BIT(8)
 130 #define I2C_ESTAT_PORT_BUSY     BIT(7)
 131 #define I2C_ESTAT_SELF_BUSY     BIT(6)
 132 #define I2C_ESTAT_VERSION       GENMASK(4, 0)
 133
 134 /* port busy register */
 135 #define I2C_PORT_BUSY_RESET     BIT(31)
 136
 137 /* wait for command complete or data request */
 138 #define I2C_CMD_SLEEP_MAX_US    500
 139 #define I2C_CMD_SLEEP_MIN_US    50
 140
 141 /* wait after reset; choose time from legacy driver */
 142 #define I2C_RESET_SLEEP_MAX_US  2000
 143 #define I2C_RESET_SLEEP_MIN_US  1000
 144
 145 /* choose timeout length from legacy driver; it's well tested */
 146 #define I2C_ABORT_TIMEOUT       msecs_to_jiffies(100)
 147
 148 struct fsi_i2c_master {
 149         struct fsi_device       *fsi;
 150         u8                      fifo_size;
 151         struct list_head        ports;
 152         struct mutex            lock;
 153 };
 154
 155 struct fsi_i2c_port {
 156         struct list_head        list;
 157         struct i2c_adapter      adapter;
 158         struct fsi_i2c_master   *master;
 159         u16                     port;
 160         u16                     xfrd;
 161 };
 162
 163 static int fsi_i2c_read_reg(struct fsi_device *fsi, unsigned int reg,
 164                             u32 *data)
 165 {
 166         int rc;
 167         __be32 data_be;
 168
 169         rc = fsi_device_read(fsi, reg, &data_be, sizeof(data_be));
 170         if (rc)
 171                 return rc;
 172
 173         *data = be32_to_cpu(data_be);
 174
 175         return 0;
 176 }
 177
 178 static int fsi_i2c_write_reg(struct fsi_device *fsi, unsigned int reg,
 179                              u32 *data)
 180 {
 181         __be32 data_be = cpu_to_be32p(data);
 182
 183         return fsi_device_write(fsi, reg, &data_be, sizeof(data_be));
 184 }
 185
 186 static int fsi_i2c_dev_init(struct fsi_i2c_master *i2c)
 187 {
 188         int rc;
 189         u32 mode = I2C_MODE_ENHANCED, extended_status, watermark;
 190         u32 interrupt = 0;
 191
 192         /* since we use polling, disable interrupts */
 193         rc = fsi_i2c_write_reg(i2c->fsi, I2C_FSI_INT_MASK, &interrupt);
 194         if (rc)
 195                 return rc;
 196
 197         mode |= FIELD_PREP(I2C_MODE_CLKDIV, I2C_DEFAULT_CLK_DIV);
 198         rc = fsi_i2c_write_reg(i2c->fsi, I2C_FSI_MODE, &mode);
 199         if (rc)
 200                 return rc;
 201
 202         rc = fsi_i2c_read_reg(i2c->fsi, I2C_FSI_ESTAT, &extended_status);
 203         if (rc)
 204                 return rc;
 205
 206         i2c->fifo_size = FIELD_GET(I2C_ESTAT_FIFO_SZ, extended_status);
 207         watermark = FIELD_PREP(I2C_WATERMARK_HI,
 208                                i2c->fifo_size - I2C_FIFO_HI_LVL);
 209         watermark |= FIELD_PREP(I2C_WATERMARK_LO, I2C_FIFO_LO_LVL);
 210
 211         return fsi_i2c_write_reg(i2c->fsi, I2C_FSI_WATER_MARK, &watermark);
 212 }
 213
 214 static int fsi_i2c_set_port(struct fsi_i2c_port *port)
 215 {
 216         int rc;
 217         struct fsi_device *fsi = port->master->fsi;
 218         u32 mode, dummy = 0;
 219
 220         rc = fsi_i2c_read_reg(fsi, I2C_FSI_MODE, &mode);
 221         if (rc)
 222                 return rc;
 223
 224         if (FIELD_GET(I2C_MODE_PORT, mode) == port->port)
 225                 return 0;
 226
 227         mode = (mode & ~I2C_MODE_PORT) | FIELD_PREP(I2C_MODE_PORT, port->port);
 228         rc = fsi_i2c_write_reg(fsi, I2C_FSI_MODE, &mode);
 229         if (rc)
 230                 return rc;
 231
 232         /* reset engine when port is changed */
 233         return fsi_i2c_write_reg(fsi, I2C_FSI_RESET_ERR, &dummy);
 234 }
 235
 236 static int fsi_i2c_start(struct fsi_i2c_port *port, struct i2c_msg *msg,
 237                          bool stop)
 238 {
 239         struct fsi_i2c_master *i2c = port->master;
 240         u32 cmd = I2C_CMD_WITH_START | I2C_CMD_WITH_ADDR;
 241
 242         port->xfrd = 0;
 243
 244         if (msg->flags & I2C_M_RD)
 245                 cmd |= I2C_CMD_READ;
 246
 247         if (stop || msg->flags & I2C_M_STOP)
 248                 cmd |= I2C_CMD_WITH_STOP;
 249
 250         cmd |= FIELD_PREP(I2C_CMD_ADDR, msg->addr);
 251         cmd |= FIELD_PREP(I2C_CMD_LEN, msg->len);
 252
 253         return fsi_i2c_write_reg(i2c->fsi, I2C_FSI_CMD, &cmd);
 254 }
 255
 256 static int fsi_i2c_get_op_bytes(int op_bytes)
 257 {
 258         /* fsi is limited to max 4 byte aligned ops */
 259         if (op_bytes > 4)
 260                 return 4;
 261         else if (op_bytes == 3)
 262                 return 2;
 263         return op_bytes;
 264 }
 265
 266 static int fsi_i2c_write_fifo(struct fsi_i2c_port *port, struct i2c_msg *msg,
 267                               u8 fifo_count)
 268 {
 269         int write;
 270         int rc;
 271         struct fsi_i2c_master *i2c = port->master;
 272         int bytes_to_write = i2c->fifo_size - fifo_count;
 273         int bytes_remaining = msg->len - port->xfrd;
 274
 275         bytes_to_write = min(bytes_to_write, bytes_remaining);
 276
 277         while (bytes_to_write) {
 278                 write = fsi_i2c_get_op_bytes(bytes_to_write);
 279
 280                 rc = fsi_device_write(i2c->fsi, I2C_FSI_FIFO,
 281                                       &msg->buf[port->xfrd], write);
 282                 if (rc)
 283                         return rc;
 284
 285                 port->xfrd += write;
 286                 bytes_to_write -= write;
 287         }
 288
 289         return 0;
 290 }
 291
 292 static int fsi_i2c_read_fifo(struct fsi_i2c_port *port, struct i2c_msg *msg,
 293                              u8 fifo_count)
 294 {
 295         int read;
 296         int rc;
 297         struct fsi_i2c_master *i2c = port->master;
 298         int bytes_to_read;
 299         int xfr_remaining = msg->len - port->xfrd;
 300         u32 dummy;
 301
 302         bytes_to_read = min_t(int, fifo_count, xfr_remaining);
 303
 304         while (bytes_to_read) {
 305                 read = fsi_i2c_get_op_bytes(bytes_to_read);
 306
 307                 if (xfr_remaining) {
 308                         rc = fsi_device_read(i2c->fsi, I2C_FSI_FIFO,
 309                                              &msg->buf[port->xfrd], read);
 310                         if (rc)
 311                                 return rc;
 312
 313                         port->xfrd += read;
 314                         xfr_remaining -= read;
 315                 } else {
 316                         /* no more buffer but data in fifo, need to clear it */
 317                         rc = fsi_device_read(i2c->fsi, I2C_FSI_FIFO, &dummy,
 318                                              read);
 319                         if (rc)
 320                                 return rc;
 321                 }
 322
 323                 bytes_to_read -= read;
 324         }
 325
 326         return 0;
 327 }
 328
 329 static int fsi_i2c_get_scl(struct i2c_adapter *adap)
 330 {
 331         u32 stat = 0;
 332         struct fsi_i2c_port *port = adap->algo_data;
 333         struct fsi_i2c_master *i2c = port->master;
 334
 335         fsi_i2c_read_reg(i2c->fsi, I2C_FSI_STAT, &stat);
 336
 337         return !!(stat & I2C_STAT_SCL_IN);
 338 }
 339
 340 static void fsi_i2c_set_scl(struct i2c_adapter *adap, int val)
 341 {
 342         u32 dummy = 0;
 343         struct fsi_i2c_port *port = adap->algo_data;
 344         struct fsi_i2c_master *i2c = port->master;
 345
 346         if (val)
 347                 fsi_i2c_write_reg(i2c->fsi, I2C_FSI_SET_SCL, &dummy);
 348         else
 349                 fsi_i2c_write_reg(i2c->fsi, I2C_FSI_RESET_SCL, &dummy);
 350 }
 351
 352 static int fsi_i2c_get_sda(struct i2c_adapter *adap)
 353 {
 354         u32 stat = 0;
 355         struct fsi_i2c_port *port = adap->algo_data;
 356         struct fsi_i2c_master *i2c = port->master;
 357
 358         fsi_i2c_read_reg(i2c->fsi, I2C_FSI_STAT, &stat);
 359
 360         return !!(stat & I2C_STAT_SDA_IN);
 361 }
 362
 363 static void fsi_i2c_set_sda(struct i2c_adapter *adap, int val)
 364 {
 365         u32 dummy = 0;
 366         struct fsi_i2c_port *port = adap->algo_data;
 367         struct fsi_i2c_master *i2c = port->master;
 368
 369         if (val)
 370                 fsi_i2c_write_reg(i2c->fsi, I2C_FSI_SET_SDA, &dummy);
 371         else
 372                 fsi_i2c_write_reg(i2c->fsi, I2C_FSI_RESET_SDA, &dummy);
 373 }
 374
 375 static void fsi_i2c_prepare_recovery(struct i2c_adapter *adap)
 376 {
 377         int rc;
 378         u32 mode;
 379         struct fsi_i2c_port *port = adap->algo_data;
 380         struct fsi_i2c_master *i2c = port->master;
 381
 382         rc = fsi_i2c_read_reg(i2c->fsi, I2C_FSI_MODE, &mode);
 383         if (rc)
 384                 return;
 385
 386         mode |= I2C_MODE_DIAG;
 387         fsi_i2c_write_reg(i2c->fsi, I2C_FSI_MODE, &mode);
 388 }
 389
 390 static void fsi_i2c_unprepare_recovery(struct i2c_adapter *adap)
 391 {
 392         int rc;
 393         u32 mode;
 394         struct fsi_i2c_port *port = adap->algo_data;
 395         struct fsi_i2c_master *i2c = port->master;
 396
 397         rc = fsi_i2c_read_reg(i2c->fsi, I2C_FSI_MODE, &mode);
 398         if (rc)
 399                 return;
 400
 401         mode &= ~I2C_MODE_DIAG;
 402         fsi_i2c_write_reg(i2c->fsi, I2C_FSI_MODE, &mode);
 403 }
 404
 405 static int fsi_i2c_reset_bus(struct fsi_i2c_master *i2c,
 406                              struct fsi_i2c_port *port)
 407 {
 408         int rc;
 409         u32 stat, dummy = 0;
 410
 411         /* force bus reset, ignore errors */
 412         i2c_recover_bus(&port->adapter);
 413
 414         /* reset errors */
 415         rc = fsi_i2c_write_reg(i2c->fsi, I2C_FSI_RESET_ERR, &dummy);
 416         if (rc)
 417                 return rc;
 418
 419         /* wait for command complete */
 420         usleep_range(I2C_RESET_SLEEP_MIN_US, I2C_RESET_SLEEP_MAX_US);
 421
 422         rc = fsi_i2c_read_reg(i2c->fsi, I2C_FSI_STAT, &stat);
 423         if (rc)
 424                 return rc;
 425
 426         if (stat & I2C_STAT_CMD_COMP)
 427                 return 0;
 428
 429         /* failed to get command complete; reset engine again */
 430         rc = fsi_i2c_write_reg(i2c->fsi, I2C_FSI_RESET_I2C, &dummy);
 431         if (rc)
 432                 return rc;
 433
 434         /* re-init engine again */
 435         return fsi_i2c_dev_init(i2c);
 436 }
 437
 438 static int fsi_i2c_reset_engine(struct fsi_i2c_master *i2c, u16 port)
 439 {
 440         int rc;
 441         u32 mode, dummy = 0;
 442
 443         /* reset engine */
 444         rc = fsi_i2c_write_reg(i2c->fsi, I2C_FSI_RESET_I2C, &dummy);
 445         if (rc)
 446                 return rc;
 447
 448         /* re-init engine */
 449         rc = fsi_i2c_dev_init(i2c);
 450         if (rc)
 451                 return rc;
 452
 453         rc = fsi_i2c_read_reg(i2c->fsi, I2C_FSI_MODE, &mode);
 454         if (rc)
 455                 return rc;
 456
 457         /* set port; default after reset is 0 */
 458         if (port) {
 459                 mode &= ~I2C_MODE_PORT;
 460                 mode |= FIELD_PREP(I2C_MODE_PORT, port);
 461                 rc = fsi_i2c_write_reg(i2c->fsi, I2C_FSI_MODE, &mode);
 462                 if (rc)
 463                         return rc;
 464         }
 465
 466         /* reset busy register; hw workaround */
 467         dummy = I2C_PORT_BUSY_RESET;
 468         rc = fsi_i2c_write_reg(i2c->fsi, I2C_FSI_PORT_BUSY, &dummy);
 469         if (rc)
 470                 return rc;
 471
 472         return 0;
 473 }
 474
 475 static int fsi_i2c_abort(struct fsi_i2c_port *port, u32 status)
 476 {
 477         int rc;
 478         unsigned long start;
 479         u32 cmd = I2C_CMD_WITH_STOP;
 480         u32 stat;
 481         struct fsi_i2c_master *i2c = port->master;
 482         struct fsi_device *fsi = i2c->fsi;
 483
 484         rc = fsi_i2c_reset_engine(i2c, port->port);
 485         if (rc)
 486                 return rc;
 487
 488         rc = fsi_i2c_read_reg(fsi, I2C_FSI_STAT, &stat);
 489         if (rc)
 490                 return rc;
 491
 492         /* if sda is low, peform full bus reset */
 493         if (!(stat & I2C_STAT_SDA_IN)) {
 494                 rc = fsi_i2c_reset_bus(i2c, port);
 495                 if (rc)
 496                         return rc;
 497         }
 498
 499         /* skip final stop command for these errors */
 500         if (status & (I2C_STAT_PARITY | I2C_STAT_LOST_ARB | I2C_STAT_STOP_ERR))
 501                 return 0;
 502
 503         /* write stop command */
 504         rc = fsi_i2c_write_reg(fsi, I2C_FSI_CMD, &cmd);
 505         if (rc)
 506                 return rc;
 507
 508         /* wait until we see command complete in the master */
 509         start = jiffies;
 510
 511         do {
 512                 rc = fsi_i2c_read_reg(fsi, I2C_FSI_STAT, &status);
 513                 if (rc)
 514                         return rc;
 515
 516                 if (status & I2C_STAT_CMD_COMP)
 517                         return 0;
 518
 519                 usleep_range(I2C_CMD_SLEEP_MIN_US, I2C_CMD_SLEEP_MAX_US);
 520         } while (time_after(start + I2C_ABORT_TIMEOUT, jiffies));
 521
 522         return -ETIMEDOUT;
 523 }
 524
 525 static int fsi_i2c_handle_status(struct fsi_i2c_port *port,
 526                                  struct i2c_msg *msg, u32 status)
 527 {
 528         int rc;
 529         u8 fifo_count;
 530
 531         if (status & I2C_STAT_ERR) {
 532                 rc = fsi_i2c_abort(port, status);
 533                 if (rc)
 534                         return rc;
 535
 536                 if (status & I2C_STAT_INV_CMD)
 537                         return -EINVAL;
 538
 539                 if (status & (I2C_STAT_PARITY | I2C_STAT_BE_OVERRUN |
 540                     I2C_STAT_BE_ACCESS))
 541                         return -EPROTO;
 542
 543                 if (status & I2C_STAT_NACK)
 544                         return -ENXIO;
 545
 546                 if (status & I2C_STAT_LOST_ARB)
 547                         return -EAGAIN;
 548
 549                 if (status & I2C_STAT_STOP_ERR)
 550                         return -EBADMSG;
 551
 552                 return -EIO;
 553         }
 554
 555         if (status & I2C_STAT_DAT_REQ) {
 556                 fifo_count = FIELD_GET(I2C_STAT_FIFO_COUNT, status);
 557
 558                 if (msg->flags & I2C_M_RD)
 559                         return fsi_i2c_read_fifo(port, msg, fifo_count);
 560
 561                 return fsi_i2c_write_fifo(port, msg, fifo_count);
 562         }
 563
 564         if (status & I2C_STAT_CMD_COMP) {
 565                 if (port->xfrd < msg->len)
 566                         return -ENODATA;
 567
 568                 return msg->len;
 569         }
 570
 571         return 0;
 572 }
 573
 574 static int fsi_i2c_wait(struct fsi_i2c_port *port, struct i2c_msg *msg,
 575                         unsigned long timeout)
 576 {
 577         u32 status = 0;
 578         int rc;
 579         unsigned long start = jiffies;
 580
 581         do {
 582                 rc = fsi_i2c_read_reg(port->master->fsi, I2C_FSI_STAT,
 583                                       &status);
 584                 if (rc)
 585                         return rc;
 586
 587                 if (status & I2C_STAT_ANY_RESP) {
 588                         rc = fsi_i2c_handle_status(port, msg, status);
 589                         if (rc < 0)
 590                                 return rc;
 591
 592                         /* cmd complete and all data xfrd */
 593                         if (rc == msg->len)
 594                                 return 0;
 595
 596                         /* need to xfr more data, but maybe don't need wait */
 597                         continue;
 598                 }
 599
 600                 usleep_range(I2C_CMD_SLEEP_MIN_US, I2C_CMD_SLEEP_MAX_US);
 601         } while (time_after(start + timeout, jiffies));
 602
 603         return -ETIMEDOUT;
 604 }
 605
 606 static int fsi_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs,
 607                         int num)
 608 {
 609         int i, rc;
 610         unsigned long start_time;
 611         struct fsi_i2c_port *port = adap->algo_data;
 612         struct fsi_i2c_master *master = port->master;
 613         struct i2c_msg *msg;
 614
 615         mutex_lock(&master->lock);
 616
 617         rc = fsi_i2c_set_port(port);
 618         if (rc)
 619                 goto unlock;
 620
 621         for (i = 0; i < num; i++) {
 622                 msg = msgs + i;
 623                 start_time = jiffies;
 624
 625                 rc = fsi_i2c_start(port, msg, i == num - 1);
 626                 if (rc)
 627                         goto unlock;
 628
 629                 rc = fsi_i2c_wait(port, msg,
 630                                   adap->timeout - (jiffies - start_time));
 631                 if (rc)
 632                         goto unlock;
 633         }
 634
 635 unlock:
 636         mutex_unlock(&master->lock);
 637         return rc ? : num;
 638 }
 639
 640 static u32 fsi_i2c_functionality(struct i2c_adapter *adap)
 641 {
 642         return I2C_FUNC_I2C | I2C_FUNC_PROTOCOL_MANGLING |
 643                 I2C_FUNC_SMBUS_EMUL | I2C_FUNC_SMBUS_BLOCK_DATA;
 644 }
 645
 646 static struct i2c_bus_recovery_info fsi_i2c_bus_recovery_info = {
 647         .recover_bus = i2c_generic_scl_recovery,
 648         .get_scl = fsi_i2c_get_scl,
 649         .set_scl = fsi_i2c_set_scl,
 650         .get_sda = fsi_i2c_get_sda,
 651         .set_sda = fsi_i2c_set_sda,
 652         .prepare_recovery = fsi_i2c_prepare_recovery,
 653         .unprepare_recovery = fsi_i2c_unprepare_recovery,
 654 };
 655
 656 static const struct i2c_algorithm fsi_i2c_algorithm = {
 657         .master_xfer = fsi_i2c_xfer,
 658         .functionality = fsi_i2c_functionality,
 659 };
 660
 661 static int fsi_i2c_probe(struct device *dev)
 662 {
 663         struct fsi_i2c_master *i2c;
 664         struct fsi_i2c_port *port;
 665         struct device_node *np;
 666         int rc;
 667         u32 port_no;
 668
 669         i2c = devm_kzalloc(dev, sizeof(*i2c), GFP_KERNEL);
 670         if (!i2c)
 671                 return -ENOMEM;
 672
 673         mutex_init(&i2c->lock);
 674         i2c->fsi = to_fsi_dev(dev);
 675         INIT_LIST_HEAD(&i2c->ports);
 676
 677         rc = fsi_i2c_dev_init(i2c);
 678         if (rc)
 679                 return rc;
 680
 681         /* Add adapter for each i2c port of the master. */
 682         for_each_available_child_of_node(dev->of_node, np) {
 683                 rc = of_property_read_u32(np, "reg", &port_no);
 684                 if (rc || port_no > USHRT_MAX)
 685                         continue;
 686
 687                 port = kzalloc(sizeof(*port), GFP_KERNEL);
 688                 if (!port)
 689                         break;
 690
 691                 port->master = i2c;
 692                 port->port = port_no;
 693
 694                 port->adapter.owner = THIS_MODULE;
 695                 port->adapter.dev.of_node = np;
 696                 port->adapter.dev.parent = dev;
 697                 port->adapter.algo = &fsi_i2c_algorithm;
 698                 port->adapter.bus_recovery_info = &fsi_i2c_bus_recovery_info;
 699                 port->adapter.algo_data = port;
 700
 701                 snprintf(port->adapter.name, sizeof(port->adapter.name),
 702                          "i2c_bus-%u", port_no);
 703
 704                 rc = i2c_add_adapter(&port->adapter);
 705                 if (rc < 0) {
 706                         dev_err(dev, "Failed to register adapter: %d\n", rc);
 707                         kfree(port);
 708                         continue;
 709                 }
 710
 711                 list_add(&port->list, &i2c->ports);
 712         }
 713
 714         dev_set_drvdata(dev, i2c);
 715
 716         return 0;
 717 }
 718
 719 static int fsi_i2c_remove(struct device *dev)
 720 {
 721         struct fsi_i2c_master *i2c = dev_get_drvdata(dev);
 722         struct fsi_i2c_port *port, *tmp;
 723
 724         list_for_each_entry_safe(port, tmp, &i2c->ports, list) {
 725                 list_del(&port->list);
 726                 i2c_del_adapter(&port->adapter);
 727                 kfree(port);
 728         }
 729
 730         return 0;
 731 }
 732
 733 static const struct fsi_device_id fsi_i2c_ids[] = {
 734         { FSI_ENGID_I2C, FSI_VERSION_ANY },
 735         { }
 736 };
 737
 738 static struct fsi_driver fsi_i2c_driver = {
 739         .id_table = fsi_i2c_ids,
 740         .drv = {
 741                 .name = "i2c-fsi",
 742                 .bus = &fsi_bus_type,
 743                 .probe = fsi_i2c_probe,
 744                 .remove = fsi_i2c_remove,
 745         },
 746 };
 747
 748 module_fsi_driver(fsi_i2c_driver);
 749
 750 MODULE_AUTHOR("Eddie James <eajames@us.ibm.com>");
 751 MODULE_DESCRIPTION("FSI attached I2C master");
 752 MODULE_LICENSE("GPL");