From 6f16b75a41abbbd11c4c8b7c62eb66604879b981 Mon Sep 17 00:00:00 2001 From: Guenter Roeck Date: Thu, 17 Jul 2014 09:56:03 -0700 Subject: i2c: stub: Add support for SMBus block commands SMBus block commands are different to I2C block commands since the returned data is not normally accessible with byte or word commands on other command offsets. Add linked list of 'block' commands to support those commands. Access mechanism is quite simple: Block commands must be written before they can be read. Subsequent writes can be partial. Block read commands always return the number of bytes associated with the longest previous write. Signed-off-by: Guenter Roeck Reviewed-by: Jean Delvare Signed-off-by: Wolfram Sang --- drivers/i2c/i2c-stub.c | 99 +++++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 94 insertions(+), 5 deletions(-) (limited to 'drivers/i2c/i2c-stub.c') diff --git a/drivers/i2c/i2c-stub.c b/drivers/i2c/i2c-stub.c index 77e4849d2f2..e0bb4655661 100644 --- a/drivers/i2c/i2c-stub.c +++ b/drivers/i2c/i2c-stub.c @@ -27,29 +27,70 @@ #include #include #include +#include #define MAX_CHIPS 10 -#define STUB_FUNC (I2C_FUNC_SMBUS_QUICK | I2C_FUNC_SMBUS_BYTE | \ - I2C_FUNC_SMBUS_BYTE_DATA | I2C_FUNC_SMBUS_WORD_DATA | \ - I2C_FUNC_SMBUS_I2C_BLOCK) + +/* + * Support for I2C_FUNC_SMBUS_BLOCK_DATA is disabled by default and must + * be enabled explicitly by setting the I2C_FUNC_SMBUS_BLOCK_DATA bits + * in the 'functionality' module parameter. + */ +#define STUB_FUNC_DEFAULT \ + (I2C_FUNC_SMBUS_QUICK | I2C_FUNC_SMBUS_BYTE | \ + I2C_FUNC_SMBUS_BYTE_DATA | I2C_FUNC_SMBUS_WORD_DATA | \ + I2C_FUNC_SMBUS_I2C_BLOCK) + +#define STUB_FUNC_ALL \ + (STUB_FUNC_DEFAULT | I2C_FUNC_SMBUS_BLOCK_DATA) static unsigned short chip_addr[MAX_CHIPS]; module_param_array(chip_addr, ushort, NULL, S_IRUGO); MODULE_PARM_DESC(chip_addr, "Chip addresses (up to 10, between 0x03 and 0x77)"); -static unsigned long functionality = STUB_FUNC; +static unsigned long functionality = STUB_FUNC_DEFAULT; module_param(functionality, ulong, S_IRUGO | S_IWUSR); MODULE_PARM_DESC(functionality, "Override functionality bitfield"); +struct smbus_block_data { + struct list_head node; + u8 command; + u8 len; + u8 block[I2C_SMBUS_BLOCK_MAX]; +}; + struct stub_chip { u8 pointer; u16 words[256]; /* Byte operations use the LSB as per SMBus specification */ + struct list_head smbus_blocks; }; static struct stub_chip *stub_chips; +static struct smbus_block_data *stub_find_block(struct device *dev, + struct stub_chip *chip, + u8 command, bool create) +{ + struct smbus_block_data *b, *rb = NULL; + + list_for_each_entry(b, &chip->smbus_blocks, node) { + if (b->command == command) { + rb = b; + break; + } + } + if (rb == NULL && create) { + rb = devm_kzalloc(dev, sizeof(*rb), GFP_KERNEL); + if (rb == NULL) + return rb; + rb->command = command; + list_add(&rb->node, &chip->smbus_blocks); + } + return rb; +} + /* Return negative errno on error. */ static s32 stub_xfer(struct i2c_adapter *adap, u16 addr, unsigned short flags, char read_write, u8 command, int size, union i2c_smbus_data *data) @@ -57,6 +98,7 @@ static s32 stub_xfer(struct i2c_adapter *adap, u16 addr, unsigned short flags, s32 ret; int i, len; struct stub_chip *chip = NULL; + struct smbus_block_data *b; /* Search for the right chip */ for (i = 0; i < MAX_CHIPS && chip_addr[i]; i++) { @@ -148,6 +190,51 @@ static s32 stub_xfer(struct i2c_adapter *adap, u16 addr, unsigned short flags, ret = 0; break; + case I2C_SMBUS_BLOCK_DATA: + b = stub_find_block(&adap->dev, chip, command, false); + if (read_write == I2C_SMBUS_WRITE) { + len = data->block[0]; + if (len == 0 || len > I2C_SMBUS_BLOCK_MAX) { + ret = -EINVAL; + break; + } + if (b == NULL) { + b = stub_find_block(&adap->dev, chip, command, + true); + if (b == NULL) { + ret = -ENOMEM; + break; + } + } + /* Largest write sets read block length */ + if (len > b->len) + b->len = len; + for (i = 0; i < len; i++) + b->block[i] = data->block[i + 1]; + /* update for byte and word commands */ + chip->words[command] = (b->block[0] << 8) | b->len; + dev_dbg(&adap->dev, + "smbus block data - addr 0x%02x, wrote %d bytes at 0x%02x.\n", + addr, len, command); + } else { + if (b == NULL) { + dev_dbg(&adap->dev, + "SMBus block read command without prior block write not supported\n"); + ret = -EOPNOTSUPP; + break; + } + len = b->len; + data->block[0] = len; + for (i = 0; i < len; i++) + data->block[i + 1] = b->block[i]; + dev_dbg(&adap->dev, + "smbus block data - addr 0x%02x, read %d bytes at 0x%02x.\n", + addr, len, command); + } + + ret = 0; + break; + default: dev_dbg(&adap->dev, "Unsupported I2C/SMBus command\n"); ret = -EOPNOTSUPP; @@ -159,7 +246,7 @@ static s32 stub_xfer(struct i2c_adapter *adap, u16 addr, unsigned short flags, static u32 stub_func(struct i2c_adapter *adapter) { - return STUB_FUNC & functionality; + return STUB_FUNC_ALL & functionality; } static const struct i2c_algorithm smbus_algorithm = { @@ -199,6 +286,8 @@ static int __init i2c_stub_init(void) pr_err("i2c-stub: Out of memory\n"); return -ENOMEM; } + for (i--; i >= 0; i--) + INIT_LIST_HEAD(&stub_chips[i].smbus_blocks); ret = i2c_add_adapter(&stub_adapter); if (ret) -- cgit v1.2.3-70-g09d2 From 1dff59831b2b78122a883f44269be6597b378afa Mon Sep 17 00:00:00 2001 From: Jean Delvare Date: Thu, 10 Jul 2014 12:45:11 +0200 Subject: i2c: stub: Remember the number of emulated chips This makes initialization, cleanup and look-up easier. Signed-off-by: Jean Delvare Reviewed-by: Guenter Roeck Signed-off-by: Wolfram Sang --- drivers/i2c/i2c-stub.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) (limited to 'drivers/i2c/i2c-stub.c') diff --git a/drivers/i2c/i2c-stub.c b/drivers/i2c/i2c-stub.c index e0bb4655661..ad52f070119 100644 --- a/drivers/i2c/i2c-stub.c +++ b/drivers/i2c/i2c-stub.c @@ -68,6 +68,7 @@ struct stub_chip { }; static struct stub_chip *stub_chips; +static int stub_chips_nr; static struct smbus_block_data *stub_find_block(struct device *dev, struct stub_chip *chip, @@ -101,7 +102,7 @@ static s32 stub_xfer(struct i2c_adapter *adap, u16 addr, unsigned short flags, struct smbus_block_data *b; /* Search for the right chip */ - for (i = 0; i < MAX_CHIPS && chip_addr[i]; i++) { + for (i = 0; i < stub_chips_nr; i++) { if (addr == chip_addr[i]) { chip = stub_chips + i; break; @@ -281,12 +282,14 @@ static int __init i2c_stub_init(void) } /* Allocate memory for all chips at once */ - stub_chips = kzalloc(i * sizeof(struct stub_chip), GFP_KERNEL); + stub_chips_nr = i; + stub_chips = kcalloc(stub_chips_nr, sizeof(struct stub_chip), + GFP_KERNEL); if (!stub_chips) { pr_err("i2c-stub: Out of memory\n"); return -ENOMEM; } - for (i--; i >= 0; i--) + for (i = 0; i < stub_chips_nr; i++) INIT_LIST_HEAD(&stub_chips[i].smbus_blocks); ret = i2c_add_adapter(&stub_adapter); -- cgit v1.2.3-70-g09d2 From 2408c17fce9236d929b15a975cbd8129de089d62 Mon Sep 17 00:00:00 2001 From: Jean Delvare Date: Thu, 10 Jul 2014 12:56:59 +0200 Subject: i2c: stub: Add support for banked register ranges Some chips implement banked register ranges. This allows implementing more registers than the limited 8-bit address space originally allows. In order to access a register on these chips, you must first select the proper bank. Add support for this mechanism to the i2c-stub driver so that such chips can be emulated. All the bank settings are passed as module parameters. Signed-off-by: Jean Delvare Tested-by: Guenter Roeck Signed-off-by: Wolfram Sang --- Documentation/i2c/i2c-stub | 11 ++-- drivers/i2c/i2c-stub.c | 131 +++++++++++++++++++++++++++++++++++++++++---- 2 files changed, 128 insertions(+), 14 deletions(-) (limited to 'drivers/i2c/i2c-stub.c') diff --git a/Documentation/i2c/i2c-stub b/Documentation/i2c/i2c-stub index a0fe7a04a3b..a16924fbd28 100644 --- a/Documentation/i2c/i2c-stub +++ b/Documentation/i2c/i2c-stub @@ -47,15 +47,18 @@ unsigned long functionality: value 0x1f0000 would only enable the quick, byte and byte data commands. +u8 bank_reg[10] +u8 bank_mask[10] +u8 bank_start[10] +u8 bank_end[10]: + Optional bank settings. They tell which bits in which register + select the active bank, as well as the range of banked registers. + CAVEATS: If your target driver polls some byte or word waiting for it to change, the stub could lock it up. Use i2cset to unlock it. -If the hardware for your driver has banked registers (e.g. Winbond sensors -chips) this module will not work well - although it could be extended to -support that pretty easily. - If you spam it hard enough, printk can be lossy. This module really wants something like relayfs. diff --git a/drivers/i2c/i2c-stub.c b/drivers/i2c/i2c-stub.c index ad52f070119..e815c6067ff 100644 --- a/drivers/i2c/i2c-stub.c +++ b/drivers/i2c/i2c-stub.c @@ -2,7 +2,7 @@ i2c-stub.c - I2C/SMBus chip emulator Copyright (c) 2004 Mark M. Hoffman - Copyright (C) 2007, 2012 Jean Delvare + Copyright (C) 2007-2014 Jean Delvare This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -53,6 +53,24 @@ static unsigned long functionality = STUB_FUNC_DEFAULT; module_param(functionality, ulong, S_IRUGO | S_IWUSR); MODULE_PARM_DESC(functionality, "Override functionality bitfield"); +/* Some chips have banked register ranges */ + +static u8 bank_reg[MAX_CHIPS]; +module_param_array(bank_reg, byte, NULL, S_IRUGO); +MODULE_PARM_DESC(bank_reg, "Bank register"); + +static u8 bank_mask[MAX_CHIPS]; +module_param_array(bank_mask, byte, NULL, S_IRUGO); +MODULE_PARM_DESC(bank_mask, "Bank value mask"); + +static u8 bank_start[MAX_CHIPS]; +module_param_array(bank_start, byte, NULL, S_IRUGO); +MODULE_PARM_DESC(bank_start, "First banked register"); + +static u8 bank_end[MAX_CHIPS]; +module_param_array(bank_end, byte, NULL, S_IRUGO); +MODULE_PARM_DESC(bank_end, "Last banked register"); + struct smbus_block_data { struct list_head node; u8 command; @@ -65,6 +83,16 @@ struct stub_chip { u16 words[256]; /* Byte operations use the LSB as per SMBus specification */ struct list_head smbus_blocks; + + /* For chips with banks, extra registers are allocated dynamically */ + u8 bank_reg; + u8 bank_shift; + u8 bank_mask; + u8 bank_sel; /* Currently selected bank */ + u8 bank_start; + u8 bank_end; + u16 bank_size; + u16 *bank_words; /* Room for bank_mask * bank_size registers */ }; static struct stub_chip *stub_chips; @@ -92,6 +120,17 @@ static struct smbus_block_data *stub_find_block(struct device *dev, return rb; } +static u16 *stub_get_wordp(struct stub_chip *chip, u8 offset) +{ + if (chip->bank_sel && + offset >= chip->bank_start && offset <= chip->bank_end) + return chip->bank_words + + (chip->bank_sel - 1) * chip->bank_size + + offset - chip->bank_start; + else + return chip->words + offset; +} + /* Return negative errno on error. */ static s32 stub_xfer(struct i2c_adapter *adap, u16 addr, unsigned short flags, char read_write, u8 command, int size, union i2c_smbus_data *data) @@ -100,6 +139,7 @@ static s32 stub_xfer(struct i2c_adapter *adap, u16 addr, unsigned short flags, int i, len; struct stub_chip *chip = NULL; struct smbus_block_data *b; + u16 *wordp; /* Search for the right chip */ for (i = 0; i < stub_chips_nr; i++) { @@ -125,7 +165,8 @@ static s32 stub_xfer(struct i2c_adapter *adap, u16 addr, unsigned short flags, "smbus byte - addr 0x%02x, wrote 0x%02x.\n", addr, command); } else { - data->byte = chip->words[chip->pointer++] & 0xff; + wordp = stub_get_wordp(chip, chip->pointer++); + data->byte = *wordp & 0xff; dev_dbg(&adap->dev, "smbus byte - addr 0x%02x, read 0x%02x.\n", addr, data->byte); @@ -135,14 +176,25 @@ static s32 stub_xfer(struct i2c_adapter *adap, u16 addr, unsigned short flags, break; case I2C_SMBUS_BYTE_DATA: + wordp = stub_get_wordp(chip, command); if (read_write == I2C_SMBUS_WRITE) { - chip->words[command] &= 0xff00; - chip->words[command] |= data->byte; + *wordp &= 0xff00; + *wordp |= data->byte; dev_dbg(&adap->dev, "smbus byte data - addr 0x%02x, wrote 0x%02x at 0x%02x.\n", addr, data->byte, command); + + /* Set the bank as needed */ + if (chip->bank_words && command == chip->bank_reg) { + chip->bank_sel = + (data->byte >> chip->bank_shift) + & chip->bank_mask; + dev_dbg(&adap->dev, + "switching to bank %u.\n", + chip->bank_sel); + } } else { - data->byte = chip->words[command] & 0xff; + data->byte = *wordp & 0xff; dev_dbg(&adap->dev, "smbus byte data - addr 0x%02x, read 0x%02x at 0x%02x.\n", addr, data->byte, command); @@ -153,13 +205,14 @@ static s32 stub_xfer(struct i2c_adapter *adap, u16 addr, unsigned short flags, break; case I2C_SMBUS_WORD_DATA: + wordp = stub_get_wordp(chip, command); if (read_write == I2C_SMBUS_WRITE) { - chip->words[command] = data->word; + *wordp = data->word; dev_dbg(&adap->dev, "smbus word data - addr 0x%02x, wrote 0x%04x at 0x%02x.\n", addr, data->word, command); } else { - data->word = chip->words[command]; + data->word = *wordp; dev_dbg(&adap->dev, "smbus word data - addr 0x%02x, read 0x%04x at 0x%02x.\n", addr, data->word, command); @@ -169,6 +222,10 @@ static s32 stub_xfer(struct i2c_adapter *adap, u16 addr, unsigned short flags, break; case I2C_SMBUS_I2C_BLOCK_DATA: + /* + * We ignore banks here, because banked chips don't use I2C + * block transfers + */ len = data->block[0]; if (read_write == I2C_SMBUS_WRITE) { for (i = 0; i < len; i++) { @@ -192,6 +249,10 @@ static s32 stub_xfer(struct i2c_adapter *adap, u16 addr, unsigned short flags, break; case I2C_SMBUS_BLOCK_DATA: + /* + * We ignore banks here, because chips typically don't use both + * banks and SMBus block transfers + */ b = stub_find_block(&adap->dev, chip, command, false); if (read_write == I2C_SMBUS_WRITE) { len = data->block[0]; @@ -262,6 +323,43 @@ static struct i2c_adapter stub_adapter = { .name = "SMBus stub driver", }; +static int __init i2c_stub_allocate_banks(int i) +{ + struct stub_chip *chip = stub_chips + i; + + chip->bank_reg = bank_reg[i]; + chip->bank_start = bank_start[i]; + chip->bank_end = bank_end[i]; + chip->bank_size = bank_end[i] - bank_start[i] + 1; + + /* We assume that all bits in the mask are contiguous */ + chip->bank_mask = bank_mask[i]; + while (!(chip->bank_mask & 1)) { + chip->bank_shift++; + chip->bank_mask >>= 1; + } + + chip->bank_words = kzalloc(chip->bank_mask * chip->bank_size * + sizeof(u16), GFP_KERNEL); + if (!chip->bank_words) + return -ENOMEM; + + pr_debug("i2c-stub: Allocated %u banks of %u words each (registers 0x%02x to 0x%02x)\n", + chip->bank_mask, chip->bank_size, chip->bank_start, + chip->bank_end); + + return 0; +} + +static void i2c_stub_free(void) +{ + int i; + + for (i = 0; i < stub_chips_nr; i++) + kfree(stub_chips[i].bank_words); + kfree(stub_chips); +} + static int __init i2c_stub_init(void) { int i, ret; @@ -289,19 +387,32 @@ static int __init i2c_stub_init(void) pr_err("i2c-stub: Out of memory\n"); return -ENOMEM; } - for (i = 0; i < stub_chips_nr; i++) + for (i = 0; i < stub_chips_nr; i++) { INIT_LIST_HEAD(&stub_chips[i].smbus_blocks); + /* Allocate extra memory for banked register ranges */ + if (bank_mask[i]) { + ret = i2c_stub_allocate_banks(i); + if (ret) + goto fail_free; + } + } + ret = i2c_add_adapter(&stub_adapter); if (ret) - kfree(stub_chips); + goto fail_free; + + return 0; + + fail_free: + i2c_stub_free(); return ret; } static void __exit i2c_stub_exit(void) { i2c_del_adapter(&stub_adapter); - kfree(stub_chips); + i2c_stub_free(); } MODULE_AUTHOR("Mark M. Hoffman "); -- cgit v1.2.3-70-g09d2 From 0f6ba0d15f43e2be8cae9e3906aa03fcfe231e5b Mon Sep 17 00:00:00 2001 From: Jean Delvare Date: Sun, 13 Jul 2014 17:17:17 +0200 Subject: i2c: stub: Avoid an array overrun on I2C block transfers I2C block transfers can have a size up to 32 bytes. If starting close to the end of the address space, there may not be enough room to write that many bytes (on I2C block writes) or not enough bytes to be read (on I2C block reads.) In that case, we must shorten the transfer so that it does not exceed the address space. Signed-off-by: Jean Delvare Reviewed-by: Guenter Roeck Signed-off-by: Wolfram Sang --- drivers/i2c/i2c-stub.c | 2 ++ 1 file changed, 2 insertions(+) (limited to 'drivers/i2c/i2c-stub.c') diff --git a/drivers/i2c/i2c-stub.c b/drivers/i2c/i2c-stub.c index e815c6067ff..d241aa295d9 100644 --- a/drivers/i2c/i2c-stub.c +++ b/drivers/i2c/i2c-stub.c @@ -226,6 +226,8 @@ static s32 stub_xfer(struct i2c_adapter *adap, u16 addr, unsigned short flags, * We ignore banks here, because banked chips don't use I2C * block transfers */ + if (data->block[0] > 256 - command) /* Avoid overrun */ + data->block[0] = 256 - command; len = data->block[0]; if (read_write == I2C_SMBUS_WRITE) { for (i = 0; i < len; i++) { -- cgit v1.2.3-70-g09d2