1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
|
/*
i2c-stub.c - I2C/SMBus chip emulator
Copyright (c) 2004 Mark M. Hoffman <mhoffman@lightlink.com>
Copyright (C) 2007, 2012 Jean Delvare <khali@linux-fr.org>
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.
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#define DEBUG 1
#include <linux/init.h>
#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/slab.h>
#include <linux/errno.h>
#include <linux/i2c.h>
#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)
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;
module_param(functionality, ulong, S_IRUGO | S_IWUSR);
MODULE_PARM_DESC(functionality, "Override functionality bitfield");
struct stub_chip {
u8 pointer;
u16 words[256]; /* Byte operations use the LSB as per SMBus
specification */
};
static struct stub_chip *stub_chips;
/* 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;
/* Search for the right chip */
for (i = 0; i < MAX_CHIPS && chip_addr[i]; 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 {
data->byte = chip->words[chip->pointer++] & 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:
if (read_write == I2C_SMBUS_WRITE) {
chip->words[command] &= 0xff00;
chip->words[command] |= data->byte;
dev_dbg(&adap->dev,
"smbus byte data - addr 0x%02x, wrote 0x%02x at 0x%02x.\n",
addr, data->byte, command);
} else {
data->byte = chip->words[command] & 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:
if (read_write == I2C_SMBUS_WRITE) {
chip->words[command] = 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];
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:
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;
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 & 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_init(void)
{
int i, ret;
if (!chip_addr[0]) {
pr_err("i2c-stub: 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("i2c-stub: Invalid chip address 0x%02x\n",
chip_addr[i]);
return -EINVAL;
}
pr_info("i2c-stub: Virtual chip at 0x%02x\n", chip_addr[i]);
}
/* Allocate memory for all chips at once */
stub_chips = kzalloc(i * sizeof(struct stub_chip), GFP_KERNEL);
if (!stub_chips) {
pr_err("i2c-stub: Out of memory\n");
return -ENOMEM;
}
ret = i2c_add_adapter(&stub_adapter);
if (ret)
kfree(stub_chips);
return ret;
}
static void __exit i2c_stub_exit(void)
{
i2c_del_adapter(&stub_adapter);
kfree(stub_chips);
}
MODULE_AUTHOR("Mark M. Hoffman <mhoffman@lightlink.com>");
MODULE_DESCRIPTION("I2C stub driver");
MODULE_LICENSE("GPL");
module_init(i2c_stub_init);
module_exit(i2c_stub_exit);
|
