414 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*3 i2c-stub.c - I2C/SMBus chip emulator4 5 Copyright (c) 2004 Mark M. Hoffman <mhoffman@lightlink.com>6 Copyright (C) 2007-2014 Jean Delvare <jdelvare@suse.de>7 8*/9 10#define pr_fmt(fmt) "i2c-stub: " fmt11 12#include <linux/errno.h>13#include <linux/i2c.h>14#include <linux/init.h>15#include <linux/kernel.h>16#include <linux/list.h>17#include <linux/module.h>18#include <linux/slab.h>19 20#define MAX_CHIPS 1021 22/*23 * Support for I2C_FUNC_SMBUS_BLOCK_DATA is disabled by default and must24 * be enabled explicitly by setting the I2C_FUNC_SMBUS_BLOCK_DATA bits25 * in the 'functionality' module parameter.26 */27#define STUB_FUNC_DEFAULT \28 (I2C_FUNC_SMBUS_QUICK | I2C_FUNC_SMBUS_BYTE | \29 I2C_FUNC_SMBUS_BYTE_DATA | I2C_FUNC_SMBUS_WORD_DATA | \30 I2C_FUNC_SMBUS_I2C_BLOCK)31 32#define STUB_FUNC_ALL \33 (STUB_FUNC_DEFAULT | I2C_FUNC_SMBUS_BLOCK_DATA)34 35static unsigned short chip_addr[MAX_CHIPS];36module_param_array(chip_addr, ushort, NULL, S_IRUGO);37MODULE_PARM_DESC(chip_addr,38 "Chip addresses (up to 10, between 0x03 and 0x77)");39 40static unsigned long functionality = STUB_FUNC_DEFAULT;41module_param(functionality, ulong, S_IRUGO | S_IWUSR);42MODULE_PARM_DESC(functionality, "Override functionality bitfield");43 44/* Some chips have banked register ranges */45 46static u8 bank_reg[MAX_CHIPS];47module_param_array(bank_reg, byte, NULL, S_IRUGO);48MODULE_PARM_DESC(bank_reg, "Bank register");49 50static u8 bank_mask[MAX_CHIPS];51module_param_array(bank_mask, byte, NULL, S_IRUGO);52MODULE_PARM_DESC(bank_mask, "Bank value mask");53 54static u8 bank_start[MAX_CHIPS];55module_param_array(bank_start, byte, NULL, S_IRUGO);56MODULE_PARM_DESC(bank_start, "First banked register");57 58static u8 bank_end[MAX_CHIPS];59module_param_array(bank_end, byte, NULL, S_IRUGO);60MODULE_PARM_DESC(bank_end, "Last banked register");61 62struct smbus_block_data {63 struct list_head node;64 u8 command;65 u8 len;66 u8 block[I2C_SMBUS_BLOCK_MAX];67};68 69struct stub_chip {70 u8 pointer;71 u16 words[256]; /* Byte operations use the LSB as per SMBus72 specification */73 struct list_head smbus_blocks;74 75 /* For chips with banks, extra registers are allocated dynamically */76 u8 bank_reg;77 u8 bank_shift;78 u8 bank_mask;79 u8 bank_sel; /* Currently selected bank */80 u8 bank_start;81 u8 bank_end;82 u16 bank_size;83 u16 *bank_words; /* Room for bank_mask * bank_size registers */84};85 86static struct stub_chip *stub_chips;87static int stub_chips_nr;88 89static struct smbus_block_data *stub_find_block(struct device *dev,90 struct stub_chip *chip,91 u8 command, bool create)92{93 struct smbus_block_data *b, *rb = NULL;94 95 list_for_each_entry(b, &chip->smbus_blocks, node) {96 if (b->command == command) {97 rb = b;98 break;99 }100 }101 if (rb == NULL && create) {102 rb = devm_kzalloc(dev, sizeof(*rb), GFP_KERNEL);103 if (rb == NULL)104 return rb;105 rb->command = command;106 list_add(&rb->node, &chip->smbus_blocks);107 }108 return rb;109}110 111static u16 *stub_get_wordp(struct stub_chip *chip, u8 offset)112{113 if (chip->bank_sel &&114 offset >= chip->bank_start && offset <= chip->bank_end)115 return chip->bank_words +116 (chip->bank_sel - 1) * chip->bank_size +117 offset - chip->bank_start;118 else119 return chip->words + offset;120}121 122/* Return negative errno on error. */123static s32 stub_xfer(struct i2c_adapter *adap, u16 addr, unsigned short flags,124 char read_write, u8 command, int size, union i2c_smbus_data *data)125{126 s32 ret;127 int i, len;128 struct stub_chip *chip = NULL;129 struct smbus_block_data *b;130 u16 *wordp;131 132 /* Search for the right chip */133 for (i = 0; i < stub_chips_nr; i++) {134 if (addr == chip_addr[i]) {135 chip = stub_chips + i;136 break;137 }138 }139 if (!chip)140 return -ENODEV;141 142 switch (size) {143 144 case I2C_SMBUS_QUICK:145 dev_dbg(&adap->dev, "smbus quick - addr 0x%02x\n", addr);146 ret = 0;147 break;148 149 case I2C_SMBUS_BYTE:150 if (read_write == I2C_SMBUS_WRITE) {151 chip->pointer = command;152 dev_dbg(&adap->dev,153 "smbus byte - addr 0x%02x, wrote 0x%02x.\n",154 addr, command);155 } else {156 wordp = stub_get_wordp(chip, chip->pointer++);157 data->byte = *wordp & 0xff;158 dev_dbg(&adap->dev,159 "smbus byte - addr 0x%02x, read 0x%02x.\n",160 addr, data->byte);161 }162 163 ret = 0;164 break;165 166 case I2C_SMBUS_BYTE_DATA:167 wordp = stub_get_wordp(chip, command);168 if (read_write == I2C_SMBUS_WRITE) {169 *wordp &= 0xff00;170 *wordp |= data->byte;171 dev_dbg(&adap->dev,172 "smbus byte data - addr 0x%02x, wrote 0x%02x at 0x%02x.\n",173 addr, data->byte, command);174 175 /* Set the bank as needed */176 if (chip->bank_words && command == chip->bank_reg) {177 chip->bank_sel =178 (data->byte >> chip->bank_shift)179 & chip->bank_mask;180 dev_dbg(&adap->dev,181 "switching to bank %u.\n",182 chip->bank_sel);183 }184 } else {185 data->byte = *wordp & 0xff;186 dev_dbg(&adap->dev,187 "smbus byte data - addr 0x%02x, read 0x%02x at 0x%02x.\n",188 addr, data->byte, command);189 }190 chip->pointer = command + 1;191 192 ret = 0;193 break;194 195 case I2C_SMBUS_WORD_DATA:196 wordp = stub_get_wordp(chip, command);197 if (read_write == I2C_SMBUS_WRITE) {198 *wordp = data->word;199 dev_dbg(&adap->dev,200 "smbus word data - addr 0x%02x, wrote 0x%04x at 0x%02x.\n",201 addr, data->word, command);202 } else {203 data->word = *wordp;204 dev_dbg(&adap->dev,205 "smbus word data - addr 0x%02x, read 0x%04x at 0x%02x.\n",206 addr, data->word, command);207 }208 209 ret = 0;210 break;211 212 case I2C_SMBUS_I2C_BLOCK_DATA:213 /*214 * We ignore banks here, because banked chips don't use I2C215 * block transfers216 */217 if (data->block[0] > 256 - command) /* Avoid overrun */218 data->block[0] = 256 - command;219 len = data->block[0];220 if (read_write == I2C_SMBUS_WRITE) {221 for (i = 0; i < len; i++) {222 chip->words[command + i] &= 0xff00;223 chip->words[command + i] |= data->block[1 + i];224 }225 dev_dbg(&adap->dev,226 "i2c block data - addr 0x%02x, wrote %d bytes at 0x%02x.\n",227 addr, len, command);228 } else {229 for (i = 0; i < len; i++) {230 data->block[1 + i] =231 chip->words[command + i] & 0xff;232 }233 dev_dbg(&adap->dev,234 "i2c block data - addr 0x%02x, read %d bytes at 0x%02x.\n",235 addr, len, command);236 }237 238 ret = 0;239 break;240 241 case I2C_SMBUS_BLOCK_DATA:242 /*243 * We ignore banks here, because chips typically don't use both244 * banks and SMBus block transfers245 */246 b = stub_find_block(&adap->dev, chip, command, false);247 if (read_write == I2C_SMBUS_WRITE) {248 len = data->block[0];249 if (len == 0 || len > I2C_SMBUS_BLOCK_MAX) {250 ret = -EINVAL;251 break;252 }253 if (b == NULL) {254 b = stub_find_block(&adap->dev, chip, command,255 true);256 if (b == NULL) {257 ret = -ENOMEM;258 break;259 }260 }261 /* Largest write sets read block length */262 if (len > b->len)263 b->len = len;264 for (i = 0; i < len; i++)265 b->block[i] = data->block[i + 1];266 /* update for byte and word commands */267 chip->words[command] = (b->block[0] << 8) | b->len;268 dev_dbg(&adap->dev,269 "smbus block data - addr 0x%02x, wrote %d bytes at 0x%02x.\n",270 addr, len, command);271 } else {272 if (b == NULL) {273 dev_dbg(&adap->dev,274 "SMBus block read command without prior block write not supported\n");275 ret = -EOPNOTSUPP;276 break;277 }278 len = b->len;279 data->block[0] = len;280 for (i = 0; i < len; i++)281 data->block[i + 1] = b->block[i];282 dev_dbg(&adap->dev,283 "smbus block data - addr 0x%02x, read %d bytes at 0x%02x.\n",284 addr, len, command);285 }286 287 ret = 0;288 break;289 290 default:291 dev_dbg(&adap->dev, "Unsupported I2C/SMBus command\n");292 ret = -EOPNOTSUPP;293 break;294 } /* switch (size) */295 296 return ret;297}298 299static u32 stub_func(struct i2c_adapter *adapter)300{301 return STUB_FUNC_ALL & functionality;302}303 304static const struct i2c_algorithm smbus_algorithm = {305 .functionality = stub_func,306 .smbus_xfer = stub_xfer,307};308 309static struct i2c_adapter stub_adapter = {310 .owner = THIS_MODULE,311 .class = I2C_CLASS_HWMON,312 .algo = &smbus_algorithm,313 .name = "SMBus stub driver",314};315 316static int __init i2c_stub_allocate_banks(int i)317{318 struct stub_chip *chip = stub_chips + i;319 320 chip->bank_reg = bank_reg[i];321 chip->bank_start = bank_start[i];322 chip->bank_end = bank_end[i];323 chip->bank_size = bank_end[i] - bank_start[i] + 1;324 325 /* We assume that all bits in the mask are contiguous */326 chip->bank_mask = bank_mask[i];327 while (!(chip->bank_mask & 1)) {328 chip->bank_shift++;329 chip->bank_mask >>= 1;330 }331 332 chip->bank_words = kcalloc(chip->bank_mask * chip->bank_size,333 sizeof(u16),334 GFP_KERNEL);335 if (!chip->bank_words)336 return -ENOMEM;337 338 pr_debug("Allocated %u banks of %u words each (registers 0x%02x to 0x%02x)\n",339 chip->bank_mask, chip->bank_size, chip->bank_start,340 chip->bank_end);341 342 return 0;343}344 345static void i2c_stub_free(void)346{347 int i;348 349 for (i = 0; i < stub_chips_nr; i++)350 kfree(stub_chips[i].bank_words);351 kfree(stub_chips);352}353 354static int __init i2c_stub_init(void)355{356 int i, ret;357 358 if (!chip_addr[0]) {359 pr_err("Please specify a chip address\n");360 return -ENODEV;361 }362 363 for (i = 0; i < MAX_CHIPS && chip_addr[i]; i++) {364 if (chip_addr[i] < 0x03 || chip_addr[i] > 0x77) {365 pr_err("Invalid chip address 0x%02x\n",366 chip_addr[i]);367 return -EINVAL;368 }369 370 pr_info("Virtual chip at 0x%02x\n", chip_addr[i]);371 }372 373 /* Allocate memory for all chips at once */374 stub_chips_nr = i;375 stub_chips = kcalloc(stub_chips_nr, sizeof(struct stub_chip),376 GFP_KERNEL);377 if (!stub_chips)378 return -ENOMEM;379 380 for (i = 0; i < stub_chips_nr; i++) {381 INIT_LIST_HEAD(&stub_chips[i].smbus_blocks);382 383 /* Allocate extra memory for banked register ranges */384 if (bank_mask[i]) {385 ret = i2c_stub_allocate_banks(i);386 if (ret)387 goto fail_free;388 }389 }390 391 ret = i2c_add_adapter(&stub_adapter);392 if (ret)393 goto fail_free;394 395 return 0;396 397 fail_free:398 i2c_stub_free();399 return ret;400}401 402static void __exit i2c_stub_exit(void)403{404 i2c_del_adapter(&stub_adapter);405 i2c_stub_free();406}407 408MODULE_AUTHOR("Mark M. Hoffman <mhoffman@lightlink.com>");409MODULE_DESCRIPTION("I2C stub driver");410MODULE_LICENSE("GPL");411 412module_init(i2c_stub_init);413module_exit(i2c_stub_exit);414