1
2
3
4
5
6
7
8
9
10
11#include <linux/delay.h>
12#include <linux/i2c.h>
13
14#include "smiapp.h"
15#include "smiapp-regs.h"
16
17static uint32_t float_to_u32_mul_1000000(struct i2c_client *client,
18 uint32_t phloat)
19{
20 int32_t exp;
21 uint64_t man;
22
23 if (phloat >= 0x80000000) {
24 dev_err(&client->dev, "this is a negative number\n");
25 return 0;
26 }
27
28 if (phloat == 0x7f800000)
29 return ~0;
30
31 if ((phloat & 0x7f800000) == 0x7f800000) {
32 dev_err(&client->dev, "NaN or other special number\n");
33 return 0;
34 }
35
36
37 if (phloat == 0)
38 return 0;
39
40 if (phloat > 0x4f800000)
41 return ~0;
42
43
44
45
46
47 exp = ((int32_t)phloat >> 23) - 127;
48
49
50 man = ((phloat & 0x7fffff) | 0x800000) * 1000000ULL;
51
52 if (exp < 0)
53 man >>= -exp;
54 else
55 man <<= exp;
56
57 man >>= 23;
58
59 return man & 0xffffffff;
60}
61
62
63
64
65
66
67static int ____smiapp_read(struct smiapp_sensor *sensor, u16 reg,
68 u16 len, u32 *val)
69{
70 struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
71 struct i2c_msg msg;
72 unsigned char data[4];
73 u16 offset = reg;
74 int r;
75
76 msg.addr = client->addr;
77 msg.flags = 0;
78 msg.len = 2;
79 msg.buf = data;
80
81
82 data[0] = (u8) (offset >> 8);
83 data[1] = (u8) offset;
84 r = i2c_transfer(client->adapter, &msg, 1);
85 if (r != 1) {
86 if (r >= 0)
87 r = -EBUSY;
88 goto err;
89 }
90
91 msg.len = len;
92 msg.flags = I2C_M_RD;
93 r = i2c_transfer(client->adapter, &msg, 1);
94 if (r != 1) {
95 if (r >= 0)
96 r = -EBUSY;
97 goto err;
98 }
99
100 *val = 0;
101
102 switch (len) {
103 case SMIAPP_REG_32BIT:
104 *val = (data[0] << 24) + (data[1] << 16) + (data[2] << 8) +
105 data[3];
106 break;
107 case SMIAPP_REG_16BIT:
108 *val = (data[0] << 8) + data[1];
109 break;
110 case SMIAPP_REG_8BIT:
111 *val = data[0];
112 break;
113 default:
114 BUG();
115 }
116
117 return 0;
118
119err:
120 dev_err(&client->dev, "read from offset 0x%x error %d\n", offset, r);
121
122 return r;
123}
124
125
126static int ____smiapp_read_8only(struct smiapp_sensor *sensor, u16 reg,
127 u16 len, u32 *val)
128{
129 unsigned int i;
130 int rval;
131
132 *val = 0;
133
134 for (i = 0; i < len; i++) {
135 u32 val8;
136
137 rval = ____smiapp_read(sensor, reg + i, 1, &val8);
138 if (rval < 0)
139 return rval;
140 *val |= val8 << ((len - i - 1) << 3);
141 }
142
143 return 0;
144}
145
146
147
148
149
150static int __smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val,
151 bool only8)
152{
153 struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
154 u8 len = SMIAPP_REG_WIDTH(reg);
155 int rval;
156
157 if (len != SMIAPP_REG_8BIT && len != SMIAPP_REG_16BIT
158 && len != SMIAPP_REG_32BIT)
159 return -EINVAL;
160
161 if (len == SMIAPP_REG_8BIT || !only8)
162 rval = ____smiapp_read(sensor, SMIAPP_REG_ADDR(reg), len, val);
163 else
164 rval = ____smiapp_read_8only(sensor, SMIAPP_REG_ADDR(reg), len,
165 val);
166 if (rval < 0)
167 return rval;
168
169 if (reg & SMIAPP_REG_FLAG_FLOAT)
170 *val = float_to_u32_mul_1000000(client, *val);
171
172 return 0;
173}
174
175int smiapp_read_no_quirk(struct smiapp_sensor *sensor, u32 reg, u32 *val)
176{
177 return __smiapp_read(
178 sensor, reg, val,
179 smiapp_needs_quirk(sensor,
180 SMIAPP_QUIRK_FLAG_8BIT_READ_ONLY));
181}
182
183static int smiapp_read_quirk(struct smiapp_sensor *sensor, u32 reg, u32 *val,
184 bool force8)
185{
186 int rval;
187
188 *val = 0;
189 rval = smiapp_call_quirk(sensor, reg_access, false, ®, val);
190 if (rval == -ENOIOCTLCMD)
191 return 0;
192 if (rval < 0)
193 return rval;
194
195 if (force8)
196 return __smiapp_read(sensor, reg, val, true);
197
198 return smiapp_read_no_quirk(sensor, reg, val);
199}
200
201int smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val)
202{
203 return smiapp_read_quirk(sensor, reg, val, false);
204}
205
206int smiapp_read_8only(struct smiapp_sensor *sensor, u32 reg, u32 *val)
207{
208 return smiapp_read_quirk(sensor, reg, val, true);
209}
210
211int smiapp_write_no_quirk(struct smiapp_sensor *sensor, u32 reg, u32 val)
212{
213 struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
214 struct i2c_msg msg;
215 unsigned char data[6];
216 unsigned int retries;
217 u8 flags = SMIAPP_REG_FLAGS(reg);
218 u8 len = SMIAPP_REG_WIDTH(reg);
219 u16 offset = SMIAPP_REG_ADDR(reg);
220 int r;
221
222 if ((len != SMIAPP_REG_8BIT && len != SMIAPP_REG_16BIT &&
223 len != SMIAPP_REG_32BIT) || flags)
224 return -EINVAL;
225
226 if (!sensor->active)
227 return 0;
228
229 msg.addr = client->addr;
230 msg.flags = 0;
231 msg.len = 2 + len;
232 msg.buf = data;
233
234
235 data[0] = (u8) (reg >> 8);
236 data[1] = (u8) (reg & 0xff);
237
238 switch (len) {
239 case SMIAPP_REG_8BIT:
240 data[2] = val;
241 break;
242 case SMIAPP_REG_16BIT:
243 data[2] = val >> 8;
244 data[3] = val;
245 break;
246 case SMIAPP_REG_32BIT:
247 data[2] = val >> 24;
248 data[3] = val >> 16;
249 data[4] = val >> 8;
250 data[5] = val;
251 break;
252 default:
253 BUG();
254 }
255
256 for (retries = 0; retries < 5; retries++) {
257
258
259
260
261
262 r = i2c_transfer(client->adapter, &msg, 1);
263 if (r == 1) {
264 if (retries)
265 dev_err(&client->dev,
266 "sensor i2c stall encountered. retries: %d\n",
267 retries);
268 return 0;
269 }
270
271 usleep_range(2000, 2000);
272 }
273
274 dev_err(&client->dev,
275 "wrote 0x%x to offset 0x%x error %d\n", val, offset, r);
276
277 return r;
278}
279
280
281
282
283
284int smiapp_write(struct smiapp_sensor *sensor, u32 reg, u32 val)
285{
286 int rval;
287
288 rval = smiapp_call_quirk(sensor, reg_access, true, ®, &val);
289 if (rval == -ENOIOCTLCMD)
290 return 0;
291 if (rval < 0)
292 return rval;
293
294 return smiapp_write_no_quirk(sensor, reg, val);
295}
296