1 /* 2 i2c-stub.c - I2C/SMBus chip emulator 3 4 Copyright (c) 2004 Mark M. Hoffman <mhoffman@lightlink.com> 5 Copyright (C) 2007, 2012 Jean Delvare <jdelvare@suse.de> 6 7 This program is free software; you can redistribute it and/or modify 8 it under the terms of the GNU General Public License as published by 9 the Free Software Foundation; either version 2 of the License, or 10 (at your option) any later version. 11 12 This program is distributed in the hope that it will be useful, 13 but WITHOUT ANY WARRANTY; without even the implied warranty of 14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 GNU General Public License for more details. 16 17 You should have received a copy of the GNU General Public License 18 along with this program; if not, write to the Free Software 19 Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. 20 */ 21 22 #define DEBUG 1 23 24 #include <linux/init.h> 25 #include <linux/module.h> 26 #include <linux/kernel.h> 27 #include <linux/slab.h> 28 #include <linux/errno.h> 29 #include <linux/i2c.h> 30 #include <linux/list.h> 31 32 #define MAX_CHIPS 10 33 34 /* 35 * Support for I2C_FUNC_SMBUS_BLOCK_DATA is disabled by default and must 36 * be enabled explicitly by setting the I2C_FUNC_SMBUS_BLOCK_DATA bits 37 * in the 'functionality' module parameter. 38 */ 39 #define STUB_FUNC_DEFAULT \ 40 (I2C_FUNC_SMBUS_QUICK | I2C_FUNC_SMBUS_BYTE | \ 41 I2C_FUNC_SMBUS_BYTE_DATA | I2C_FUNC_SMBUS_WORD_DATA | \ 42 I2C_FUNC_SMBUS_I2C_BLOCK) 43 44 #define STUB_FUNC_ALL \ 45 (STUB_FUNC_DEFAULT | I2C_FUNC_SMBUS_BLOCK_DATA) 46 47 static unsigned short chip_addr[MAX_CHIPS]; 48 module_param_array(chip_addr, ushort, NULL, S_IRUGO); 49 MODULE_PARM_DESC(chip_addr, 50 "Chip addresses (up to 10, between 0x03 and 0x77)"); 51 52 static unsigned long functionality = STUB_FUNC_DEFAULT; 53 module_param(functionality, ulong, S_IRUGO | S_IWUSR); 54 MODULE_PARM_DESC(functionality, "Override functionality bitfield"); 55 56 struct smbus_block_data { 57 struct list_head node; 58 u8 command; 59 u8 len; 60 u8 block[I2C_SMBUS_BLOCK_MAX]; 61 }; 62 63 struct stub_chip { 64 u8 pointer; 65 u16 words[256]; /* Byte operations use the LSB as per SMBus 66 specification */ 67 struct list_head smbus_blocks; 68 }; 69 70 static struct stub_chip *stub_chips; 71 static int stub_chips_nr; 72 73 static struct smbus_block_data *stub_find_block(struct device *dev, 74 struct stub_chip *chip, 75 u8 command, bool create) 76 { 77 struct smbus_block_data *b, *rb = NULL; 78 79 list_for_each_entry(b, &chip->smbus_blocks, node) { 80 if (b->command == command) { 81 rb = b; 82 break; 83 } 84 } 85 if (rb == NULL && create) { 86 rb = devm_kzalloc(dev, sizeof(*rb), GFP_KERNEL); 87 if (rb == NULL) 88 return rb; 89 rb->command = command; 90 list_add(&rb->node, &chip->smbus_blocks); 91 } 92 return rb; 93 } 94 95 /* Return negative errno on error. */ 96 static s32 stub_xfer(struct i2c_adapter *adap, u16 addr, unsigned short flags, 97 char read_write, u8 command, int size, union i2c_smbus_data *data) 98 { 99 s32 ret; 100 int i, len; 101 struct stub_chip *chip = NULL; 102 struct smbus_block_data *b; 103 104 /* Search for the right chip */ 105 for (i = 0; i < stub_chips_nr; i++) { 106 if (addr == chip_addr[i]) { 107 chip = stub_chips + i; 108 break; 109 } 110 } 111 if (!chip) 112 return -ENODEV; 113 114 switch (size) { 115 116 case I2C_SMBUS_QUICK: 117 dev_dbg(&adap->dev, "smbus quick - addr 0x%02x\n", addr); 118 ret = 0; 119 break; 120 121 case I2C_SMBUS_BYTE: 122 if (read_write == I2C_SMBUS_WRITE) { 123 chip->pointer = command; 124 dev_dbg(&adap->dev, 125 "smbus byte - addr 0x%02x, wrote 0x%02x.\n", 126 addr, command); 127 } else { 128 data->byte = chip->words[chip->pointer++] & 0xff; 129 dev_dbg(&adap->dev, 130 "smbus byte - addr 0x%02x, read 0x%02x.\n", 131 addr, data->byte); 132 } 133 134 ret = 0; 135 break; 136 137 case I2C_SMBUS_BYTE_DATA: 138 if (read_write == I2C_SMBUS_WRITE) { 139 chip->words[command] &= 0xff00; 140 chip->words[command] |= data->byte; 141 dev_dbg(&adap->dev, 142 "smbus byte data - addr 0x%02x, wrote 0x%02x at 0x%02x.\n", 143 addr, data->byte, command); 144 } else { 145 data->byte = chip->words[command] & 0xff; 146 dev_dbg(&adap->dev, 147 "smbus byte data - addr 0x%02x, read 0x%02x at 0x%02x.\n", 148 addr, data->byte, command); 149 } 150 chip->pointer = command + 1; 151 152 ret = 0; 153 break; 154 155 case I2C_SMBUS_WORD_DATA: 156 if (read_write == I2C_SMBUS_WRITE) { 157 chip->words[command] = data->word; 158 dev_dbg(&adap->dev, 159 "smbus word data - addr 0x%02x, wrote 0x%04x at 0x%02x.\n", 160 addr, data->word, command); 161 } else { 162 data->word = chip->words[command]; 163 dev_dbg(&adap->dev, 164 "smbus word data - addr 0x%02x, read 0x%04x at 0x%02x.\n", 165 addr, data->word, command); 166 } 167 168 ret = 0; 169 break; 170 171 case I2C_SMBUS_I2C_BLOCK_DATA: 172 len = data->block[0]; 173 if (read_write == I2C_SMBUS_WRITE) { 174 for (i = 0; i < len; i++) { 175 chip->words[command + i] &= 0xff00; 176 chip->words[command + i] |= data->block[1 + i]; 177 } 178 dev_dbg(&adap->dev, 179 "i2c block data - addr 0x%02x, wrote %d bytes at 0x%02x.\n", 180 addr, len, command); 181 } else { 182 for (i = 0; i < len; i++) { 183 data->block[1 + i] = 184 chip->words[command + i] & 0xff; 185 } 186 dev_dbg(&adap->dev, 187 "i2c block data - addr 0x%02x, read %d bytes at 0x%02x.\n", 188 addr, len, command); 189 } 190 191 ret = 0; 192 break; 193 194 case I2C_SMBUS_BLOCK_DATA: 195 b = stub_find_block(&adap->dev, chip, command, false); 196 if (read_write == I2C_SMBUS_WRITE) { 197 len = data->block[0]; 198 if (len == 0 || len > I2C_SMBUS_BLOCK_MAX) { 199 ret = -EINVAL; 200 break; 201 } 202 if (b == NULL) { 203 b = stub_find_block(&adap->dev, chip, command, 204 true); 205 if (b == NULL) { 206 ret = -ENOMEM; 207 break; 208 } 209 } 210 /* Largest write sets read block length */ 211 if (len > b->len) 212 b->len = len; 213 for (i = 0; i < len; i++) 214 b->block[i] = data->block[i + 1]; 215 /* update for byte and word commands */ 216 chip->words[command] = (b->block[0] << 8) | b->len; 217 dev_dbg(&adap->dev, 218 "smbus block data - addr 0x%02x, wrote %d bytes at 0x%02x.\n", 219 addr, len, command); 220 } else { 221 if (b == NULL) { 222 dev_dbg(&adap->dev, 223 "SMBus block read command without prior block write not supported\n"); 224 ret = -EOPNOTSUPP; 225 break; 226 } 227 len = b->len; 228 data->block[0] = len; 229 for (i = 0; i < len; i++) 230 data->block[i + 1] = b->block[i]; 231 dev_dbg(&adap->dev, 232 "smbus block data - addr 0x%02x, read %d bytes at 0x%02x.\n", 233 addr, len, command); 234 } 235 236 ret = 0; 237 break; 238 239 default: 240 dev_dbg(&adap->dev, "Unsupported I2C/SMBus command\n"); 241 ret = -EOPNOTSUPP; 242 break; 243 } /* switch (size) */ 244 245 return ret; 246 } 247 248 static u32 stub_func(struct i2c_adapter *adapter) 249 { 250 return STUB_FUNC_ALL & functionality; 251 } 252 253 static const struct i2c_algorithm smbus_algorithm = { 254 .functionality = stub_func, 255 .smbus_xfer = stub_xfer, 256 }; 257 258 static struct i2c_adapter stub_adapter = { 259 .owner = THIS_MODULE, 260 .class = I2C_CLASS_HWMON | I2C_CLASS_SPD, 261 .algo = &smbus_algorithm, 262 .name = "SMBus stub driver", 263 }; 264 265 static int __init i2c_stub_init(void) 266 { 267 int i, ret; 268 269 if (!chip_addr[0]) { 270 pr_err("i2c-stub: Please specify a chip address\n"); 271 return -ENODEV; 272 } 273 274 for (i = 0; i < MAX_CHIPS && chip_addr[i]; i++) { 275 if (chip_addr[i] < 0x03 || chip_addr[i] > 0x77) { 276 pr_err("i2c-stub: Invalid chip address 0x%02x\n", 277 chip_addr[i]); 278 return -EINVAL; 279 } 280 281 pr_info("i2c-stub: Virtual chip at 0x%02x\n", chip_addr[i]); 282 } 283 284 /* Allocate memory for all chips at once */ 285 stub_chips_nr = i; 286 stub_chips = kcalloc(stub_chips_nr, sizeof(struct stub_chip), 287 GFP_KERNEL); 288 if (!stub_chips) { 289 pr_err("i2c-stub: Out of memory\n"); 290 return -ENOMEM; 291 } 292 for (i = 0; i < stub_chips_nr; i++) 293 INIT_LIST_HEAD(&stub_chips[i].smbus_blocks); 294 295 ret = i2c_add_adapter(&stub_adapter); 296 if (ret) 297 kfree(stub_chips); 298 return ret; 299 } 300 301 static void __exit i2c_stub_exit(void) 302 { 303 i2c_del_adapter(&stub_adapter); 304 kfree(stub_chips); 305 } 306 307 MODULE_AUTHOR("Mark M. Hoffman <mhoffman@lightlink.com>"); 308 MODULE_DESCRIPTION("I2C stub driver"); 309 MODULE_LICENSE("GPL"); 310 311 module_init(i2c_stub_init); 312 module_exit(i2c_stub_exit); 313