2 i2c-stub.c - I2C/SMBus chip emulator
4 Copyright (c) 2004 Mark M. Hoffman <mhoffman@lightlink.com>
5 Copyright (C) 2007-2014 Jean Delvare <jdelvare@suse.de>
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.
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.
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.
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>
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.
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)
44 #define STUB_FUNC_ALL \
45 (STUB_FUNC_DEFAULT | I2C_FUNC_SMBUS_BLOCK_DATA)
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)");
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");
56 /* Some chips have banked register ranges */
58 static u8 bank_reg
[MAX_CHIPS
];
59 module_param_array(bank_reg
, byte
, NULL
, S_IRUGO
);
60 MODULE_PARM_DESC(bank_reg
, "Bank register");
62 static u8 bank_mask
[MAX_CHIPS
];
63 module_param_array(bank_mask
, byte
, NULL
, S_IRUGO
);
64 MODULE_PARM_DESC(bank_mask
, "Bank value mask");
66 static u8 bank_start
[MAX_CHIPS
];
67 module_param_array(bank_start
, byte
, NULL
, S_IRUGO
);
68 MODULE_PARM_DESC(bank_start
, "First banked register");
70 static u8 bank_end
[MAX_CHIPS
];
71 module_param_array(bank_end
, byte
, NULL
, S_IRUGO
);
72 MODULE_PARM_DESC(bank_end
, "Last banked register");
74 struct smbus_block_data
{
75 struct list_head node
;
78 u8 block
[I2C_SMBUS_BLOCK_MAX
];
83 u16 words
[256]; /* Byte operations use the LSB as per SMBus
85 struct list_head smbus_blocks
;
87 /* For chips with banks, extra registers are allocated dynamically */
91 u8 bank_sel
; /* Currently selected bank */
95 u16
*bank_words
; /* Room for bank_mask * bank_size registers */
98 static struct stub_chip
*stub_chips
;
99 static int stub_chips_nr
;
101 static struct smbus_block_data
*stub_find_block(struct device
*dev
,
102 struct stub_chip
*chip
,
103 u8 command
, bool create
)
105 struct smbus_block_data
*b
, *rb
= NULL
;
107 list_for_each_entry(b
, &chip
->smbus_blocks
, node
) {
108 if (b
->command
== command
) {
113 if (rb
== NULL
&& create
) {
114 rb
= devm_kzalloc(dev
, sizeof(*rb
), GFP_KERNEL
);
117 rb
->command
= command
;
118 list_add(&rb
->node
, &chip
->smbus_blocks
);
123 static u16
*stub_get_wordp(struct stub_chip
*chip
, u8 offset
)
125 if (chip
->bank_sel
&&
126 offset
>= chip
->bank_start
&& offset
<= chip
->bank_end
)
127 return chip
->bank_words
+
128 (chip
->bank_sel
- 1) * chip
->bank_size
+
129 offset
- chip
->bank_start
;
131 return chip
->words
+ offset
;
134 /* Return negative errno on error. */
135 static s32
stub_xfer(struct i2c_adapter
*adap
, u16 addr
, unsigned short flags
,
136 char read_write
, u8 command
, int size
, union i2c_smbus_data
*data
)
140 struct stub_chip
*chip
= NULL
;
141 struct smbus_block_data
*b
;
144 /* Search for the right chip */
145 for (i
= 0; i
< stub_chips_nr
; i
++) {
146 if (addr
== chip_addr
[i
]) {
147 chip
= stub_chips
+ i
;
156 case I2C_SMBUS_QUICK
:
157 dev_dbg(&adap
->dev
, "smbus quick - addr 0x%02x\n", addr
);
162 if (read_write
== I2C_SMBUS_WRITE
) {
163 chip
->pointer
= command
;
165 "smbus byte - addr 0x%02x, wrote 0x%02x.\n",
168 wordp
= stub_get_wordp(chip
, chip
->pointer
++);
169 data
->byte
= *wordp
& 0xff;
171 "smbus byte - addr 0x%02x, read 0x%02x.\n",
178 case I2C_SMBUS_BYTE_DATA
:
179 wordp
= stub_get_wordp(chip
, command
);
180 if (read_write
== I2C_SMBUS_WRITE
) {
182 *wordp
|= data
->byte
;
184 "smbus byte data - addr 0x%02x, wrote 0x%02x at 0x%02x.\n",
185 addr
, data
->byte
, command
);
187 /* Set the bank as needed */
188 if (chip
->bank_words
&& command
== chip
->bank_reg
) {
190 (data
->byte
>> chip
->bank_shift
)
193 "switching to bank %u.\n",
197 data
->byte
= *wordp
& 0xff;
199 "smbus byte data - addr 0x%02x, read 0x%02x at 0x%02x.\n",
200 addr
, data
->byte
, command
);
202 chip
->pointer
= command
+ 1;
207 case I2C_SMBUS_WORD_DATA
:
208 wordp
= stub_get_wordp(chip
, command
);
209 if (read_write
== I2C_SMBUS_WRITE
) {
212 "smbus word data - addr 0x%02x, wrote 0x%04x at 0x%02x.\n",
213 addr
, data
->word
, command
);
217 "smbus word data - addr 0x%02x, read 0x%04x at 0x%02x.\n",
218 addr
, data
->word
, command
);
224 case I2C_SMBUS_I2C_BLOCK_DATA
:
226 * We ignore banks here, because banked chips don't use I2C
229 if (data
->block
[0] > 256 - command
) /* Avoid overrun */
230 data
->block
[0] = 256 - command
;
231 len
= data
->block
[0];
232 if (read_write
== I2C_SMBUS_WRITE
) {
233 for (i
= 0; i
< len
; i
++) {
234 chip
->words
[command
+ i
] &= 0xff00;
235 chip
->words
[command
+ i
] |= data
->block
[1 + i
];
238 "i2c block data - addr 0x%02x, wrote %d bytes at 0x%02x.\n",
241 for (i
= 0; i
< len
; i
++) {
243 chip
->words
[command
+ i
] & 0xff;
246 "i2c block data - addr 0x%02x, read %d bytes at 0x%02x.\n",
253 case I2C_SMBUS_BLOCK_DATA
:
255 * We ignore banks here, because chips typically don't use both
256 * banks and SMBus block transfers
258 b
= stub_find_block(&adap
->dev
, chip
, command
, false);
259 if (read_write
== I2C_SMBUS_WRITE
) {
260 len
= data
->block
[0];
261 if (len
== 0 || len
> I2C_SMBUS_BLOCK_MAX
) {
266 b
= stub_find_block(&adap
->dev
, chip
, command
,
273 /* Largest write sets read block length */
276 for (i
= 0; i
< len
; i
++)
277 b
->block
[i
] = data
->block
[i
+ 1];
278 /* update for byte and word commands */
279 chip
->words
[command
] = (b
->block
[0] << 8) | b
->len
;
281 "smbus block data - addr 0x%02x, wrote %d bytes at 0x%02x.\n",
286 "SMBus block read command without prior block write not supported\n");
291 data
->block
[0] = len
;
292 for (i
= 0; i
< len
; i
++)
293 data
->block
[i
+ 1] = b
->block
[i
];
295 "smbus block data - addr 0x%02x, read %d bytes at 0x%02x.\n",
303 dev_dbg(&adap
->dev
, "Unsupported I2C/SMBus command\n");
306 } /* switch (size) */
311 static u32
stub_func(struct i2c_adapter
*adapter
)
313 return STUB_FUNC_ALL
& functionality
;
316 static const struct i2c_algorithm smbus_algorithm
= {
317 .functionality
= stub_func
,
318 .smbus_xfer
= stub_xfer
,
321 static struct i2c_adapter stub_adapter
= {
322 .owner
= THIS_MODULE
,
323 .class = I2C_CLASS_HWMON
| I2C_CLASS_SPD
,
324 .algo
= &smbus_algorithm
,
325 .name
= "SMBus stub driver",
328 static int __init
i2c_stub_allocate_banks(int i
)
330 struct stub_chip
*chip
= stub_chips
+ i
;
332 chip
->bank_reg
= bank_reg
[i
];
333 chip
->bank_start
= bank_start
[i
];
334 chip
->bank_end
= bank_end
[i
];
335 chip
->bank_size
= bank_end
[i
] - bank_start
[i
] + 1;
337 /* We assume that all bits in the mask are contiguous */
338 chip
->bank_mask
= bank_mask
[i
];
339 while (!(chip
->bank_mask
& 1)) {
341 chip
->bank_mask
>>= 1;
344 chip
->bank_words
= kzalloc(chip
->bank_mask
* chip
->bank_size
*
345 sizeof(u16
), GFP_KERNEL
);
346 if (!chip
->bank_words
)
349 pr_debug("i2c-stub: Allocated %u banks of %u words each (registers 0x%02x to 0x%02x)\n",
350 chip
->bank_mask
, chip
->bank_size
, chip
->bank_start
,
356 static void i2c_stub_free(void)
360 for (i
= 0; i
< stub_chips_nr
; i
++)
361 kfree(stub_chips
[i
].bank_words
);
365 static int __init
i2c_stub_init(void)
370 pr_err("i2c-stub: Please specify a chip address\n");
374 for (i
= 0; i
< MAX_CHIPS
&& chip_addr
[i
]; i
++) {
375 if (chip_addr
[i
] < 0x03 || chip_addr
[i
] > 0x77) {
376 pr_err("i2c-stub: Invalid chip address 0x%02x\n",
381 pr_info("i2c-stub: Virtual chip at 0x%02x\n", chip_addr
[i
]);
384 /* Allocate memory for all chips at once */
386 stub_chips
= kcalloc(stub_chips_nr
, sizeof(struct stub_chip
),
389 pr_err("i2c-stub: Out of memory\n");
392 for (i
= 0; i
< stub_chips_nr
; i
++) {
393 INIT_LIST_HEAD(&stub_chips
[i
].smbus_blocks
);
395 /* Allocate extra memory for banked register ranges */
397 ret
= i2c_stub_allocate_banks(i
);
403 ret
= i2c_add_adapter(&stub_adapter
);
414 static void __exit
i2c_stub_exit(void)
416 i2c_del_adapter(&stub_adapter
);
420 MODULE_AUTHOR("Mark M. Hoffman <mhoffman@lightlink.com>");
421 MODULE_DESCRIPTION("I2C stub driver");
422 MODULE_LICENSE("GPL");
424 module_init(i2c_stub_init
);
425 module_exit(i2c_stub_exit
);