1
2
3
4
5
6
7
8
9
10
11
12#include <asm/unaligned.h>
13
14#include <linux/delay.h>
15#include <linux/i2c.h>
16
17#include "ccs.h"
18#include "ccs-limits.h"
19
20static u32 float_to_u32_mul_1000000(struct i2c_client *client, u32 phloat)
21{
22 s32 exp;
23 u64 man;
24
25 if (phloat >= 0x80000000) {
26 dev_err(&client->dev, "this is a negative number\n");
27 return 0;
28 }
29
30 if (phloat == 0x7f800000)
31 return ~0;
32
33 if ((phloat & 0x7f800000) == 0x7f800000) {
34 dev_err(&client->dev, "NaN or other special number\n");
35 return 0;
36 }
37
38
39 if (phloat == 0)
40 return 0;
41
42 if (phloat > 0x4f800000)
43 return ~0;
44
45
46
47
48
49 exp = ((int32_t)phloat >> 23) - 127;
50
51
52 man = ((phloat & 0x7fffff) | 0x800000) * 1000000ULL;
53
54 if (exp < 0)
55 man >>= -exp;
56 else
57 man <<= exp;
58
59 man >>= 23;
60
61 return man & 0xffffffff;
62}
63
64
65
66
67
68
69static int ____ccs_read_addr(struct ccs_sensor *sensor, u16 reg, u16 len,
70 u32 *val)
71{
72 struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
73 struct i2c_msg msg;
74 unsigned char data_buf[sizeof(u32)] = { 0 };
75 unsigned char offset_buf[sizeof(u16)];
76 int r;
77
78 if (len > sizeof(data_buf))
79 return -EINVAL;
80
81 msg.addr = client->addr;
82 msg.flags = 0;
83 msg.len = sizeof(offset_buf);
84 msg.buf = offset_buf;
85 put_unaligned_be16(reg, offset_buf);
86
87 r = i2c_transfer(client->adapter, &msg, 1);
88 if (r != 1) {
89 if (r >= 0)
90 r = -EBUSY;
91 goto err;
92 }
93
94 msg.len = len;
95 msg.flags = I2C_M_RD;
96 msg.buf = &data_buf[sizeof(data_buf) - len];
97
98 r = i2c_transfer(client->adapter, &msg, 1);
99 if (r != 1) {
100 if (r >= 0)
101 r = -EBUSY;
102 goto err;
103 }
104
105 *val = get_unaligned_be32(data_buf);
106
107 return 0;
108
109err:
110 dev_err(&client->dev, "read from offset 0x%x error %d\n", reg, r);
111
112 return r;
113}
114
115
116static int ____ccs_read_addr_8only(struct ccs_sensor *sensor, u16 reg,
117 u16 len, u32 *val)
118{
119 unsigned int i;
120 int rval;
121
122 *val = 0;
123
124 for (i = 0; i < len; i++) {
125 u32 val8;
126
127 rval = ____ccs_read_addr(sensor, reg + i, 1, &val8);
128 if (rval < 0)
129 return rval;
130 *val |= val8 << ((len - i - 1) << 3);
131 }
132
133 return 0;
134}
135
136unsigned int ccs_reg_width(u32 reg)
137{
138 if (reg & CCS_FL_16BIT)
139 return sizeof(u16);
140 if (reg & CCS_FL_32BIT)
141 return sizeof(u32);
142
143 return sizeof(u8);
144}
145
146static u32 ireal32_to_u32_mul_1000000(struct i2c_client *client, u32 val)
147{
148 if (val >> 10 > U32_MAX / 15625) {
149 dev_warn(&client->dev, "value %u overflows!\n", val);
150 return U32_MAX;
151 }
152
153 return ((val >> 10) * 15625) +
154 (val & GENMASK(9, 0)) * 15625 / 1024;
155}
156
157u32 ccs_reg_conv(struct ccs_sensor *sensor, u32 reg, u32 val)
158{
159 struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
160
161 if (reg & CCS_FL_FLOAT_IREAL) {
162 if (CCS_LIM(sensor, CLOCK_CAPA_TYPE_CAPABILITY) &
163 CCS_CLOCK_CAPA_TYPE_CAPABILITY_IREAL)
164 val = ireal32_to_u32_mul_1000000(client, val);
165 else
166 val = float_to_u32_mul_1000000(client, val);
167 } else if (reg & CCS_FL_IREAL) {
168 val = ireal32_to_u32_mul_1000000(client, val);
169 }
170
171 return val;
172}
173
174
175
176
177
178static int __ccs_read_addr(struct ccs_sensor *sensor, u32 reg, u32 *val,
179 bool only8, bool conv)
180{
181 unsigned int len = ccs_reg_width(reg);
182 int rval;
183
184 if (!only8)
185 rval = ____ccs_read_addr(sensor, CCS_REG_ADDR(reg), len, val);
186 else
187 rval = ____ccs_read_addr_8only(sensor, CCS_REG_ADDR(reg), len,
188 val);
189 if (rval < 0)
190 return rval;
191
192 if (!conv)
193 return 0;
194
195 *val = ccs_reg_conv(sensor, reg, *val);
196
197 return 0;
198}
199
200static int __ccs_read_data(struct ccs_reg *regs, size_t num_regs,
201 u32 reg, u32 *val)
202{
203 unsigned int width = ccs_reg_width(reg);
204 size_t i;
205
206 for (i = 0; i < num_regs; i++, regs++) {
207 u8 *data;
208
209 if (regs->addr + regs->len < CCS_REG_ADDR(reg) + width)
210 continue;
211
212 if (regs->addr > CCS_REG_ADDR(reg))
213 break;
214
215 data = ®s->value[CCS_REG_ADDR(reg) - regs->addr];
216
217 switch (width) {
218 case sizeof(u8):
219 *val = *data;
220 break;
221 case sizeof(u16):
222 *val = get_unaligned_be16(data);
223 break;
224 case sizeof(u32):
225 *val = get_unaligned_be32(data);
226 break;
227 default:
228 WARN_ON(1);
229 return -EINVAL;
230 }
231
232 return 0;
233 }
234
235 return -ENOENT;
236}
237
238static int ccs_read_data(struct ccs_sensor *sensor, u32 reg, u32 *val)
239{
240 if (!__ccs_read_data(sensor->sdata.sensor_read_only_regs,
241 sensor->sdata.num_sensor_read_only_regs,
242 reg, val))
243 return 0;
244
245 return __ccs_read_data(sensor->mdata.module_read_only_regs,
246 sensor->mdata.num_module_read_only_regs,
247 reg, val);
248}
249
250static int ccs_read_addr_raw(struct ccs_sensor *sensor, u32 reg, u32 *val,
251 bool force8, bool quirk, bool conv, bool data)
252{
253 int rval;
254
255 if (data) {
256 rval = ccs_read_data(sensor, reg, val);
257 if (!rval)
258 return 0;
259 }
260
261 if (quirk) {
262 *val = 0;
263 rval = ccs_call_quirk(sensor, reg_access, false, ®, val);
264 if (rval == -ENOIOCTLCMD)
265 return 0;
266 if (rval < 0)
267 return rval;
268
269 if (force8)
270 return __ccs_read_addr(sensor, reg, val, true, conv);
271 }
272
273 return __ccs_read_addr(sensor, reg, val,
274 ccs_needs_quirk(sensor,
275 CCS_QUIRK_FLAG_8BIT_READ_ONLY),
276 conv);
277}
278
279int ccs_read_addr(struct ccs_sensor *sensor, u32 reg, u32 *val)
280{
281 return ccs_read_addr_raw(sensor, reg, val, false, true, true, true);
282}
283
284int ccs_read_addr_8only(struct ccs_sensor *sensor, u32 reg, u32 *val)
285{
286 return ccs_read_addr_raw(sensor, reg, val, true, true, true, true);
287}
288
289int ccs_read_addr_noconv(struct ccs_sensor *sensor, u32 reg, u32 *val)
290{
291 return ccs_read_addr_raw(sensor, reg, val, false, true, false, true);
292}
293
294static int ccs_write_retry(struct i2c_client *client, struct i2c_msg *msg)
295{
296 unsigned int retries;
297 int r;
298
299 for (retries = 0; retries < 10; retries++) {
300
301
302
303
304
305 r = i2c_transfer(client->adapter, msg, 1);
306 if (r != 1) {
307 usleep_range(1000, 2000);
308 continue;
309 }
310
311 if (retries)
312 dev_err(&client->dev,
313 "sensor i2c stall encountered. retries: %d\n",
314 retries);
315 return 0;
316 }
317
318 return r;
319}
320
321int ccs_write_addr_no_quirk(struct ccs_sensor *sensor, u32 reg, u32 val)
322{
323 struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
324 struct i2c_msg msg;
325 unsigned char data[6];
326 unsigned int len = ccs_reg_width(reg);
327 int r;
328
329 if (len > sizeof(data) - 2)
330 return -EINVAL;
331
332 msg.addr = client->addr;
333 msg.flags = 0;
334 msg.len = 2 + len;
335 msg.buf = data;
336
337 put_unaligned_be16(CCS_REG_ADDR(reg), data);
338 put_unaligned_be32(val << (8 * (sizeof(val) - len)), data + 2);
339
340 dev_dbg(&client->dev, "writing reg 0x%4.4x value 0x%*.*x (%u)\n",
341 CCS_REG_ADDR(reg), ccs_reg_width(reg) << 1,
342 ccs_reg_width(reg) << 1, val, val);
343
344 r = ccs_write_retry(client, &msg);
345 if (r)
346 dev_err(&client->dev,
347 "wrote 0x%x to offset 0x%x error %d\n", val,
348 CCS_REG_ADDR(reg), r);
349
350 return r;
351}
352
353
354
355
356
357int ccs_write_addr(struct ccs_sensor *sensor, u32 reg, u32 val)
358{
359 int rval;
360
361 rval = ccs_call_quirk(sensor, reg_access, true, ®, &val);
362 if (rval == -ENOIOCTLCMD)
363 return 0;
364 if (rval < 0)
365 return rval;
366
367 return ccs_write_addr_no_quirk(sensor, reg, val);
368}
369
370#define MAX_WRITE_LEN 32U
371
372int ccs_write_data_regs(struct ccs_sensor *sensor, struct ccs_reg *regs,
373 size_t num_regs)
374{
375 struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
376 unsigned char buf[2 + MAX_WRITE_LEN];
377 struct i2c_msg msg = {
378 .addr = client->addr,
379 .buf = buf,
380 };
381 size_t i;
382
383 for (i = 0; i < num_regs; i++, regs++) {
384 unsigned char *regdata = regs->value;
385 unsigned int j;
386
387 for (j = 0; j < regs->len;
388 j += msg.len - 2, regdata += msg.len - 2) {
389 char printbuf[(MAX_WRITE_LEN << 1) +
390 1 ] = { 0 };
391 int rval;
392
393 msg.len = min(regs->len - j, MAX_WRITE_LEN);
394
395 bin2hex(printbuf, regdata, msg.len);
396 dev_dbg(&client->dev,
397 "writing msr reg 0x%4.4x value 0x%s\n",
398 regs->addr + j, printbuf);
399
400 put_unaligned_be16(regs->addr + j, buf);
401 memcpy(buf + 2, regdata, msg.len);
402
403 msg.len += 2;
404
405 rval = ccs_write_retry(client, &msg);
406 if (rval) {
407 dev_err(&client->dev,
408 "error writing %u octets to address 0x%4.4x\n",
409 msg.len, regs->addr + j);
410 return rval;
411 }
412 }
413 }
414
415 return 0;
416}
417
