[linux.git] / drivers / i2c / i2c-stub.c

/*
    i2c-stub.c - I2C/SMBus chip emulator

    Copyright (c) 2004 Mark M. Hoffman <[email protected]>
    Copyright (C) 2007-2014 Jean Delvare <[email protected]>

    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
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.
*/

#define DEBUG 1
#define pr_fmt(fmt) "i2c-stub: " fmt

#include <linux/errno.h>
#include <linux/i2c.h>
#include <linux/init.h>
#include <linux/kernel.h>
#include <linux/list.h>
#include <linux/module.h>
#include <linux/slab.h>

#define MAX_CHIPS 10

/*
 * 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_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;
	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;

	/* 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;
static int stub_chips_nr;

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;
}

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)
{
	s32 ret;
	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++) {
		if (addr == chip_addr[i]) {
			chip = stub_chips + i;
			break;
		}
	}
	if (!chip)
		return -ENODEV;

	switch (size) {

	case I2C_SMBUS_QUICK:
		dev_dbg(&adap->dev, "smbus quick - addr 0x%02x\n", addr);
		ret = 0;
		break;

	case I2C_SMBUS_BYTE:
		if (read_write == I2C_SMBUS_WRITE) {
			chip->pointer = command;
			dev_dbg(&adap->dev,
				"smbus byte - addr 0x%02x, wrote 0x%02x.\n",
				addr, command);
		} else {
			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);
		}

		ret = 0;
		break;

	case I2C_SMBUS_BYTE_DATA:
		wordp = stub_get_wordp(chip, command);
		if (read_write == I2C_SMBUS_WRITE) {
			*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 = *wordp & 0xff;
			dev_dbg(&adap->dev,
				"smbus byte data - addr 0x%02x, read  0x%02x at 0x%02x.\n",
				addr, data->byte, command);
		}
		chip->pointer = command + 1;

		ret = 0;
		break;

	case I2C_SMBUS_WORD_DATA:
		wordp = stub_get_wordp(chip, command);
		if (read_write == I2C_SMBUS_WRITE) {
			*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 = *wordp;
			dev_dbg(&adap->dev,
				"smbus word data - addr 0x%02x, read  0x%04x at 0x%02x.\n",
				addr, data->word, command);
		}

		ret = 0;
		break;

	case I2C_SMBUS_I2C_BLOCK_DATA:
		/*
		 * 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++) {
				chip->words[command + i] &= 0xff00;
				chip->words[command + i] |= data->block[1 + i];
			}
			dev_dbg(&adap->dev,
				"i2c block data - addr 0x%02x, wrote %d bytes at 0x%02x.\n",
				addr, len, command);
		} else {
			for (i = 0; i < len; i++) {
				data->block[1 + i] =
					chip->words[command + i] & 0xff;
			}
			dev_dbg(&adap->dev,
				"i2c block data - addr 0x%02x, read  %d bytes at 0x%02x.\n",
				addr, len, command);
		}

		ret = 0;
		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];
			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;
		break;
	} /* switch (size) */

	return ret;
}

static u32 stub_func(struct i2c_adapter *adapter)
{
	return STUB_FUNC_ALL & functionality;
}

static const struct i2c_algorithm smbus_algorithm = {
	.functionality	= stub_func,
	.smbus_xfer	= stub_xfer,
};

static struct i2c_adapter stub_adapter = {
	.owner		= THIS_MODULE,
	.class		= I2C_CLASS_HWMON | I2C_CLASS_SPD,
	.algo		= &smbus_algorithm,
	.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 = kcalloc(chip->bank_mask * chip->bank_size,
				   sizeof(u16),
				   GFP_KERNEL);
	if (!chip->bank_words)
		return -ENOMEM;

	pr_debug("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;

	if (!chip_addr[0]) {
		pr_err("Please specify a chip address\n");
		return -ENODEV;
	}

	for (i = 0; i < MAX_CHIPS && chip_addr[i]; i++) {
		if (chip_addr[i] < 0x03 || chip_addr[i] > 0x77) {
			pr_err("Invalid chip address 0x%02x\n",
			       chip_addr[i]);
			return -EINVAL;
		}

		pr_info("Virtual chip at 0x%02x\n", chip_addr[i]);
	}

	/* Allocate memory for all chips at once */
	stub_chips_nr = i;
	stub_chips = kcalloc(stub_chips_nr, sizeof(struct stub_chip),
			     GFP_KERNEL);
	if (!stub_chips)
		return -ENOMEM;

	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)
		goto fail_free;

	return 0;

 fail_free:
	i2c_stub_free();
	return ret;
}

static void __exit i2c_stub_exit(void)
{
	i2c_del_adapter(&stub_adapter);
	i2c_stub_free();
}

MODULE_AUTHOR("Mark M. Hoffman <[email protected]>");
MODULE_DESCRIPTION("I2C stub driver");
MODULE_LICENSE("GPL");

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