drivers/i2c/i2c-stub.c

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003     i2c-stub.c - I2C/SMBus chip emulator
0004
0005     Copyright (c) 2004 Mark M. Hoffman <mhoffman@lightlink.com>
0006     Copyright (C) 2007-2014 Jean Delvare <jdelvare@suse.de>
0007
0008 */
0009
0010 #define pr_fmt(fmt) "i2c-stub: " fmt
0011
0012 #include <linux/errno.h>
0013 #include <linux/i2c.h>
0014 #include <linux/init.h>
0015 #include <linux/kernel.h>
0016 #include <linux/list.h>
0017 #include <linux/module.h>
0018 #include <linux/slab.h>
0019
0020 #define MAX_CHIPS 10
0021
0022 /*
0023  * Support for I2C_FUNC_SMBUS_BLOCK_DATA is disabled by default and must
0024  * be enabled explicitly by setting the I2C_FUNC_SMBUS_BLOCK_DATA bits
0025  * in the 'functionality' module parameter.
0026  */
0027 #define STUB_FUNC_DEFAULT \
0028         (I2C_FUNC_SMBUS_QUICK | I2C_FUNC_SMBUS_BYTE | \
0029          I2C_FUNC_SMBUS_BYTE_DATA | I2C_FUNC_SMBUS_WORD_DATA | \
0030          I2C_FUNC_SMBUS_I2C_BLOCK)
0031
0032 #define STUB_FUNC_ALL \
0033         (STUB_FUNC_DEFAULT | I2C_FUNC_SMBUS_BLOCK_DATA)
0034
0035 static unsigned short chip_addr[MAX_CHIPS];
0036 module_param_array(chip_addr, ushort, NULL, S_IRUGO);
0037 MODULE_PARM_DESC(chip_addr,
0038          "Chip addresses (up to 10, between 0x03 and 0x77)");
0039
0040 static unsigned long functionality = STUB_FUNC_DEFAULT;
0041 module_param(functionality, ulong, S_IRUGO | S_IWUSR);
0042 MODULE_PARM_DESC(functionality, "Override functionality bitfield");
0043
0044 /* Some chips have banked register ranges */
0045
0046 static u8 bank_reg[MAX_CHIPS];
0047 module_param_array(bank_reg, byte, NULL, S_IRUGO);
0048 MODULE_PARM_DESC(bank_reg, "Bank register");
0049
0050 static u8 bank_mask[MAX_CHIPS];
0051 module_param_array(bank_mask, byte, NULL, S_IRUGO);
0052 MODULE_PARM_DESC(bank_mask, "Bank value mask");
0053
0054 static u8 bank_start[MAX_CHIPS];
0055 module_param_array(bank_start, byte, NULL, S_IRUGO);
0056 MODULE_PARM_DESC(bank_start, "First banked register");
0057
0058 static u8 bank_end[MAX_CHIPS];
0059 module_param_array(bank_end, byte, NULL, S_IRUGO);
0060 MODULE_PARM_DESC(bank_end, "Last banked register");
0061
0062 struct smbus_block_data {
0063     struct list_head node;
0064     u8 command;
0065     u8 len;
0066     u8 block[I2C_SMBUS_BLOCK_MAX];
0067 };
0068
0069 struct stub_chip {
0070     u8 pointer;
0071     u16 words[256];     /* Byte operations use the LSB as per SMBus
0072                    specification */
0073     struct list_head smbus_blocks;
0074
0075     /* For chips with banks, extra registers are allocated dynamically */
0076     u8 bank_reg;
0077     u8 bank_shift;
0078     u8 bank_mask;
0079     u8 bank_sel;        /* Currently selected bank */
0080     u8 bank_start;
0081     u8 bank_end;
0082     u16 bank_size;
0083     u16 *bank_words;    /* Room for bank_mask * bank_size registers */
0084 };
0085
0086 static struct stub_chip *stub_chips;
0087 static int stub_chips_nr;
0088
0089 static struct smbus_block_data *stub_find_block(struct device *dev,
0090                         struct stub_chip *chip,
0091                         u8 command, bool create)
0092 {
0093     struct smbus_block_data *b, *rb = NULL;
0094
0095     list_for_each_entry(b, &chip->smbus_blocks, node) {
0096         if (b->command == command) {
0097             rb = b;
0098             break;
0099         }
0100     }
0101     if (rb == NULL && create) {
0102         rb = devm_kzalloc(dev, sizeof(*rb), GFP_KERNEL);
0103         if (rb == NULL)
0104             return rb;
0105         rb->command = command;
0106         list_add(&rb->node, &chip->smbus_blocks);
0107     }
0108     return rb;
0109 }
0110
0111 static u16 *stub_get_wordp(struct stub_chip *chip, u8 offset)
0112 {
0113     if (chip->bank_sel &&
0114         offset >= chip->bank_start && offset <= chip->bank_end)
0115         return chip->bank_words +
0116                (chip->bank_sel - 1) * chip->bank_size +
0117                offset - chip->bank_start;
0118     else
0119         return chip->words + offset;
0120 }
0121
0122 /* Return negative errno on error. */
0123 static s32 stub_xfer(struct i2c_adapter *adap, u16 addr, unsigned short flags,
0124     char read_write, u8 command, int size, union i2c_smbus_data *data)
0125 {
0126     s32 ret;
0127     int i, len;
0128     struct stub_chip *chip = NULL;
0129     struct smbus_block_data *b;
0130     u16 *wordp;
0131
0132     /* Search for the right chip */
0133     for (i = 0; i < stub_chips_nr; i++) {
0134         if (addr == chip_addr[i]) {
0135             chip = stub_chips + i;
0136             break;
0137         }
0138     }
0139     if (!chip)
0140         return -ENODEV;
0141
0142     switch (size) {
0143
0144     case I2C_SMBUS_QUICK:
0145         dev_dbg(&adap->dev, "smbus quick - addr 0x%02x\n", addr);
0146         ret = 0;
0147         break;
0148
0149     case I2C_SMBUS_BYTE:
0150         if (read_write == I2C_SMBUS_WRITE) {
0151             chip->pointer = command;
0152             dev_dbg(&adap->dev,
0153                 "smbus byte - addr 0x%02x, wrote 0x%02x.\n",
0154                 addr, command);
0155         } else {
0156             wordp = stub_get_wordp(chip, chip->pointer++);
0157             data->byte = *wordp & 0xff;
0158             dev_dbg(&adap->dev,
0159                 "smbus byte - addr 0x%02x, read  0x%02x.\n",
0160                 addr, data->byte);
0161         }
0162
0163         ret = 0;
0164         break;
0165
0166     case I2C_SMBUS_BYTE_DATA:
0167         wordp = stub_get_wordp(chip, command);
0168         if (read_write == I2C_SMBUS_WRITE) {
0169             *wordp &= 0xff00;
0170             *wordp |= data->byte;
0171             dev_dbg(&adap->dev,
0172                 "smbus byte data - addr 0x%02x, wrote 0x%02x at 0x%02x.\n",
0173                 addr, data->byte, command);
0174
0175             /* Set the bank as needed */
0176             if (chip->bank_words && command == chip->bank_reg) {
0177                 chip->bank_sel =
0178                     (data->byte >> chip->bank_shift)
0179                     & chip->bank_mask;
0180                 dev_dbg(&adap->dev,
0181                     "switching to bank %u.\n",
0182                     chip->bank_sel);
0183             }
0184         } else {
0185             data->byte = *wordp & 0xff;
0186             dev_dbg(&adap->dev,
0187                 "smbus byte data - addr 0x%02x, read  0x%02x at 0x%02x.\n",
0188                 addr, data->byte, command);
0189         }
0190         chip->pointer = command + 1;
0191
0192         ret = 0;
0193         break;
0194
0195     case I2C_SMBUS_WORD_DATA:
0196         wordp = stub_get_wordp(chip, command);
0197         if (read_write == I2C_SMBUS_WRITE) {
0198             *wordp = data->word;
0199             dev_dbg(&adap->dev,
0200                 "smbus word data - addr 0x%02x, wrote 0x%04x at 0x%02x.\n",
0201                 addr, data->word, command);
0202         } else {
0203             data->word = *wordp;
0204             dev_dbg(&adap->dev,
0205                 "smbus word data - addr 0x%02x, read  0x%04x at 0x%02x.\n",
0206                 addr, data->word, command);
0207         }
0208
0209         ret = 0;
0210         break;
0211
0212     case I2C_SMBUS_I2C_BLOCK_DATA:
0213         /*
0214          * We ignore banks here, because banked chips don't use I2C
0215          * block transfers
0216          */
0217         if (data->block[0] > 256 - command) /* Avoid overrun */
0218             data->block[0] = 256 - command;
0219         len = data->block[0];
0220         if (read_write == I2C_SMBUS_WRITE) {
0221             for (i = 0; i < len; i++) {
0222                 chip->words[command + i] &= 0xff00;
0223                 chip->words[command + i] |= data->block[1 + i];
0224             }
0225             dev_dbg(&adap->dev,
0226                 "i2c block data - addr 0x%02x, wrote %d bytes at 0x%02x.\n",
0227                 addr, len, command);
0228         } else {
0229             for (i = 0; i < len; i++) {
0230                 data->block[1 + i] =
0231                     chip->words[command + i] & 0xff;
0232             }
0233             dev_dbg(&adap->dev,
0234                 "i2c block data - addr 0x%02x, read  %d bytes at 0x%02x.\n",
0235                 addr, len, command);
0236         }
0237
0238         ret = 0;
0239         break;
0240
0241     case I2C_SMBUS_BLOCK_DATA:
0242         /*
0243          * We ignore banks here, because chips typically don't use both
0244          * banks and SMBus block transfers
0245          */
0246         b = stub_find_block(&adap->dev, chip, command, false);
0247         if (read_write == I2C_SMBUS_WRITE) {
0248             len = data->block[0];
0249             if (len == 0 || len > I2C_SMBUS_BLOCK_MAX) {
0250                 ret = -EINVAL;
0251                 break;
0252             }
0253             if (b == NULL) {
0254                 b = stub_find_block(&adap->dev, chip, command,
0255                             true);
0256                 if (b == NULL) {
0257                     ret = -ENOMEM;
0258                     break;
0259                 }
0260             }
0261             /* Largest write sets read block length */
0262             if (len > b->len)
0263                 b->len = len;
0264             for (i = 0; i < len; i++)
0265                 b->block[i] = data->block[i + 1];
0266             /* update for byte and word commands */
0267             chip->words[command] = (b->block[0] << 8) | b->len;
0268             dev_dbg(&adap->dev,
0269                 "smbus block data - addr 0x%02x, wrote %d bytes at 0x%02x.\n",
0270                 addr, len, command);
0271         } else {
0272             if (b == NULL) {
0273                 dev_dbg(&adap->dev,
0274                     "SMBus block read command without prior block write not supported\n");
0275                 ret = -EOPNOTSUPP;
0276                 break;
0277             }
0278             len = b->len;
0279             data->block[0] = len;
0280             for (i = 0; i < len; i++)
0281                 data->block[i + 1] = b->block[i];
0282             dev_dbg(&adap->dev,
0283                 "smbus block data - addr 0x%02x, read  %d bytes at 0x%02x.\n",
0284                 addr, len, command);
0285         }
0286
0287         ret = 0;
0288         break;
0289
0290     default:
0291         dev_dbg(&adap->dev, "Unsupported I2C/SMBus command\n");
0292         ret = -EOPNOTSUPP;
0293         break;
0294     } /* switch (size) */
0295
0296     return ret;
0297 }
0298
0299 static u32 stub_func(struct i2c_adapter *adapter)
0300 {
0301     return STUB_FUNC_ALL & functionality;
0302 }
0303
0304 static const struct i2c_algorithm smbus_algorithm = {
0305     .functionality  = stub_func,
0306     .smbus_xfer = stub_xfer,
0307 };
0308
0309 static struct i2c_adapter stub_adapter = {
0310     .owner      = THIS_MODULE,
0311     .class      = I2C_CLASS_HWMON | I2C_CLASS_SPD,
0312     .algo       = &smbus_algorithm,
0313     .name       = "SMBus stub driver",
0314 };
0315
0316 static int __init i2c_stub_allocate_banks(int i)
0317 {
0318     struct stub_chip *chip = stub_chips + i;
0319
0320     chip->bank_reg = bank_reg[i];
0321     chip->bank_start = bank_start[i];
0322     chip->bank_end = bank_end[i];
0323     chip->bank_size = bank_end[i] - bank_start[i] + 1;
0324
0325     /* We assume that all bits in the mask are contiguous */
0326     chip->bank_mask = bank_mask[i];
0327     while (!(chip->bank_mask & 1)) {
0328         chip->bank_shift++;
0329         chip->bank_mask >>= 1;
0330     }
0331
0332     chip->bank_words = kcalloc(chip->bank_mask * chip->bank_size,
0333                    sizeof(u16),
0334                    GFP_KERNEL);
0335     if (!chip->bank_words)
0336         return -ENOMEM;
0337
0338     pr_debug("Allocated %u banks of %u words each (registers 0x%02x to 0x%02x)\n",
0339          chip->bank_mask, chip->bank_size, chip->bank_start,
0340          chip->bank_end);
0341
0342     return 0;
0343 }
0344
0345 static void i2c_stub_free(void)
0346 {
0347     int i;
0348
0349     for (i = 0; i < stub_chips_nr; i++)
0350         kfree(stub_chips[i].bank_words);
0351     kfree(stub_chips);
0352 }
0353
0354 static int __init i2c_stub_init(void)
0355 {
0356     int i, ret;
0357
0358     if (!chip_addr[0]) {
0359         pr_err("Please specify a chip address\n");
0360         return -ENODEV;
0361     }
0362
0363     for (i = 0; i < MAX_CHIPS && chip_addr[i]; i++) {
0364         if (chip_addr[i] < 0x03 || chip_addr[i] > 0x77) {
0365             pr_err("Invalid chip address 0x%02x\n",
0366                    chip_addr[i]);
0367             return -EINVAL;
0368         }
0369
0370         pr_info("Virtual chip at 0x%02x\n", chip_addr[i]);
0371     }
0372
0373     /* Allocate memory for all chips at once */
0374     stub_chips_nr = i;
0375     stub_chips = kcalloc(stub_chips_nr, sizeof(struct stub_chip),
0376                  GFP_KERNEL);
0377     if (!stub_chips)
0378         return -ENOMEM;
0379
0380     for (i = 0; i < stub_chips_nr; i++) {
0381         INIT_LIST_HEAD(&stub_chips[i].smbus_blocks);
0382
0383         /* Allocate extra memory for banked register ranges */
0384         if (bank_mask[i]) {
0385             ret = i2c_stub_allocate_banks(i);
0386             if (ret)
0387                 goto fail_free;
0388         }
0389     }
0390
0391     ret = i2c_add_adapter(&stub_adapter);
0392     if (ret)
0393         goto fail_free;
0394
0395     return 0;
0396
0397  fail_free:
0398     i2c_stub_free();
0399     return ret;
0400 }
0401
0402 static void __exit i2c_stub_exit(void)
0403 {
0404     i2c_del_adapter(&stub_adapter);
0405     i2c_stub_free();
0406 }
0407
0408 MODULE_AUTHOR("Mark M. Hoffman <mhoffman@lightlink.com>");
0409 MODULE_DESCRIPTION("I2C stub driver");
0410 MODULE_LICENSE("GPL");
0411
0412 module_init(i2c_stub_init);
0413 module_exit(i2c_stub_exit);