3 Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
5 This program is free software; you can redistribute it and/or modify
6 it under the terms of the GNU General Public License as published by
7 the Free Software Foundation; either version 2 of the License, or
8 (at your option) any later version.
10 This program is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU General Public License for more details.
15 You should have received a copy of the GNU General Public License
16 along with this program. If not, see <http://www.gnu.org/licenses/>.
20 /* -------------------------------------------------------------------------- */
22 #include <linux/i2c.h>
23 #include <linux/module.h>
24 #include <linux/moduleparam.h>
25 #include <linux/kernel.h>
26 #include <linux/errno.h>
27 #include <linux/slab.h>
28 #include <linux/delay.h>
32 #include <linux/mpu.h>
36 #define MPL_LOG_TAG "MPL-acc"
38 /*----- YAMAHA YAS529 Registers ------*/
40 YAS_REG_CMDR = 0x00, /* 000 < 5 */
41 YAS_REG_XOFFSETR = 0x20, /* 001 < 5 */
42 YAS_REG_Y1OFFSETR = 0x40, /* 010 < 5 */
43 YAS_REG_Y2OFFSETR = 0x60, /* 011 < 5 */
44 YAS_REG_ICOILR = 0x80, /* 100 < 5 */
45 YAS_REG_CAL = 0xA0, /* 101 < 5 */
46 YAS_REG_CONFR = 0xC0, /* 110 < 5 */
47 YAS_REG_DOUTR = 0xE0 /* 111 < 5 */
50 /* -------------------------------------------------------------------------- */
62 /* -------------------------------------------------------------------------- */
63 static int yas529_sensor_i2c_write(struct i2c_adapter *i2c_adap,
64 unsigned char address,
65 unsigned int len, unsigned char *data)
67 struct i2c_msg msgs[1];
70 if (NULL == data || NULL == i2c_adap)
73 msgs[0].addr = address;
74 msgs[0].flags = 0; /* write */
75 msgs[0].buf = (unsigned char *)data;
78 res = i2c_transfer(i2c_adap, msgs, 1);
85 static int yas529_sensor_i2c_read(struct i2c_adapter *i2c_adap,
86 unsigned char address,
88 unsigned int len, unsigned char *data)
90 struct i2c_msg msgs[2];
93 if (NULL == data || NULL == i2c_adap)
96 msgs[0].addr = address;
97 msgs[0].flags = I2C_M_RD;
101 res = i2c_transfer(i2c_adap, msgs, 1);
108 static int yas529_suspend(void *mlsl_handle,
109 struct ext_slave_descr *slave,
110 struct ext_slave_platform_data *pdata)
112 int result = INV_SUCCESS;
117 static int yas529_resume(void *mlsl_handle,
118 struct ext_slave_descr *slave,
119 struct ext_slave_platform_data *pdata)
121 int result = INV_SUCCESS;
123 unsigned char dummyData[1] = { 0 };
124 unsigned char dummyRegister = 0;
125 unsigned char rawData[6];
126 unsigned char calData[9];
128 short xoffset, y1offset, y2offset;
129 short d2, d3, d4, d5, d6, d7, d8, d9;
131 /* YAS529 Application Manual MS-3C - Section 4.4.5 */
132 /* =============================================== */
133 /* Step 1 - register initialization */
134 /* zero initialization coil register - "100 00 000" */
135 dummyData[0] = YAS_REG_ICOILR | 0x00;
137 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
139 LOG_RESULT_LOCATION(result);
142 /* zero config register - "110 00 000" */
143 dummyData[0] = YAS_REG_CONFR | 0x00;
145 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
147 LOG_RESULT_LOCATION(result);
151 /* Step 2 - initialization coil operation */
152 dummyData[0] = YAS_REG_ICOILR | 0x11;
154 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
156 LOG_RESULT_LOCATION(result);
159 dummyData[0] = YAS_REG_ICOILR | 0x01;
161 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
163 LOG_RESULT_LOCATION(result);
166 dummyData[0] = YAS_REG_ICOILR | 0x12;
168 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
170 LOG_RESULT_LOCATION(result);
173 dummyData[0] = YAS_REG_ICOILR | 0x02;
175 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
177 LOG_RESULT_LOCATION(result);
180 dummyData[0] = YAS_REG_ICOILR | 0x13;
182 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
184 LOG_RESULT_LOCATION(result);
187 dummyData[0] = YAS_REG_ICOILR | 0x03;
189 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
191 LOG_RESULT_LOCATION(result);
194 dummyData[0] = YAS_REG_ICOILR | 0x14;
196 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
198 LOG_RESULT_LOCATION(result);
201 dummyData[0] = YAS_REG_ICOILR | 0x04;
203 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
205 LOG_RESULT_LOCATION(result);
208 dummyData[0] = YAS_REG_ICOILR | 0x15;
210 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
212 LOG_RESULT_LOCATION(result);
215 dummyData[0] = YAS_REG_ICOILR | 0x05;
217 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
219 LOG_RESULT_LOCATION(result);
222 dummyData[0] = YAS_REG_ICOILR | 0x16;
224 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
226 LOG_RESULT_LOCATION(result);
229 dummyData[0] = YAS_REG_ICOILR | 0x06;
231 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
233 LOG_RESULT_LOCATION(result);
236 dummyData[0] = YAS_REG_ICOILR | 0x17;
238 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
240 LOG_RESULT_LOCATION(result);
243 dummyData[0] = YAS_REG_ICOILR | 0x07;
245 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
247 LOG_RESULT_LOCATION(result);
250 dummyData[0] = YAS_REG_ICOILR | 0x10;
252 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
254 LOG_RESULT_LOCATION(result);
257 dummyData[0] = YAS_REG_ICOILR | 0x00;
259 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
261 LOG_RESULT_LOCATION(result);
265 /* Step 3 - rough offset measurement */
266 /* Config register - Measurements results - "110 00 000" */
267 dummyData[0] = YAS_REG_CONFR | 0x00;
269 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
271 LOG_RESULT_LOCATION(result);
274 /* Measurements command register - Rough offset measurement -
276 dummyData[0] = YAS_REG_CMDR | 0x01;
278 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
280 LOG_RESULT_LOCATION(result);
283 msleep(2); /* wait at least 1.5ms */
285 /* Measurement data read */
287 yas529_sensor_i2c_read(mlsl_handle, pdata->address,
288 dummyRegister, 6, rawData);
290 LOG_RESULT_LOCATION(result);
294 (short)((unsigned short)rawData[5] +
295 ((unsigned short)rawData[4] & 0x7) * 256) - 5;
299 (short)((unsigned short)rawData[3] +
300 ((unsigned short)rawData[2] & 0x7) * 256) - 5;
304 (short)((unsigned short)rawData[1] +
305 ((unsigned short)rawData[0] & 0x7) * 256) - 5;
309 /* Step 4 - rough offset setting */
310 /* Set rough offset register values */
311 dummyData[0] = YAS_REG_XOFFSETR | xoffset;
313 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
315 LOG_RESULT_LOCATION(result);
318 dummyData[0] = YAS_REG_Y1OFFSETR | y1offset;
320 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
322 LOG_RESULT_LOCATION(result);
325 dummyData[0] = YAS_REG_Y2OFFSETR | y2offset;
327 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
329 LOG_RESULT_LOCATION(result);
333 /* CAL matrix read (first read is invalid) */
334 /* Config register - CAL register read - "110 01 000" */
335 dummyData[0] = YAS_REG_CONFR | 0x08;
337 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
339 LOG_RESULT_LOCATION(result);
344 yas529_sensor_i2c_read(mlsl_handle, pdata->address,
345 dummyRegister, 9, calData);
347 LOG_RESULT_LOCATION(result);
350 /* Config register - CAL register read - "110 01 000" */
351 dummyData[0] = YAS_REG_CONFR | 0x08;
353 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
355 LOG_RESULT_LOCATION(result);
360 yas529_sensor_i2c_read(mlsl_handle, pdata->address,
361 dummyRegister, 9, calData);
363 LOG_RESULT_LOCATION(result);
367 /* Calculate coefficients of the sensitivity correction matrix */
369 d2 = (calData[0] & 0xFC) >> 2; /* [71..66] 6bit */
370 a2 = (short)(d2 - 32);
372 d3 = ((calData[0] & 0x03) << 2) | ((calData[1] & 0xC0) >> 6);
373 a3 = (short)(d3 - 8);
374 d4 = (calData[1] & 0x3F); /* [61..56] 6bit */
375 a4 = (short)(d4 - 32);
376 d5 = (calData[2] & 0xFC) >> 2; /* [55..50] 6bit */
377 a5 = (short)(d5 - 32) + 70;
379 d6 = ((calData[2] & 0x03) << 4) | ((calData[3] & 0xF0) >> 4);
380 a6 = (short)(d6 - 32);
382 d7 = ((calData[3] & 0x0F) << 2) | ((calData[4] & 0xC0) >> 6);
383 a7 = (short)(d7 - 32);
384 d8 = (calData[4] & 0x3F); /* [37..32] 6bit */
385 a8 = (short)(d8 - 32);
386 d9 = (calData[5] & 0xFE) >> 1; /* [31..25] 7bit */
387 a9 = (short)(d9 - 64) + 130;
392 static int yas529_read(void *mlsl_handle,
393 struct ext_slave_descr *slave,
394 struct ext_slave_platform_data *pdata,
398 unsigned char rawData[6];
399 unsigned char dummyData[1] = { 0 };
400 unsigned char dummyRegister = 0;
401 int result = INV_SUCCESS;
402 short SX, SY1, SY2, SY, SZ;
403 short row1fixed, row2fixed, row3fixed;
405 /* Config register - Measurements results - "110 00 000" */
406 dummyData[0] = YAS_REG_CONFR | 0x00;
408 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
410 LOG_RESULT_LOCATION(result);
413 /* Measurements command register - Normal magnetic field measurement -
415 dummyData[0] = YAS_REG_CMDR | 0x00;
417 yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
419 LOG_RESULT_LOCATION(result);
423 /* Measurement data read */
425 yas529_sensor_i2c_read(mlsl_handle, pdata->address,
426 dummyRegister, 6, (unsigned char *)&rawData);
428 LOG_RESULT_LOCATION(result);
432 stat = rawData[0] & 0x80;
434 /* Extract raw data */
435 SX = (short)((unsigned short)rawData[5] +
436 ((unsigned short)rawData[4] & 0x7) * 256);
438 (short)((unsigned short)rawData[3] +
439 ((unsigned short)rawData[2] & 0x7) * 256);
441 (short)((unsigned short)rawData[1] +
442 ((unsigned short)rawData[0] & 0x7) * 256);
443 if ((SX <= 1) || (SY1 <= 1) || (SY2 <= 1))
444 return INV_ERROR_COMPASS_DATA_UNDERFLOW;
445 if ((SX >= 1024) || (SY1 >= 1024) || (SY2 >= 1024))
446 return INV_ERROR_COMPASS_DATA_OVERFLOW;
447 /* Convert to XYZ axis */
452 /* Apply sensitivity correction matrix */
453 row1fixed = (short)((a1 * SX + a2 * SY + a3 * SZ) >> 7) * 41;
454 row2fixed = (short)((a4 * SX + a5 * SY + a6 * SZ) >> 7) * 41;
455 row3fixed = (short)((a7 * SX + a8 * SY + a9 * SZ) >> 7) * 41;
457 data[0] = row1fixed >> 8;
458 data[1] = row1fixed & 0xFF;
459 data[2] = row2fixed >> 8;
460 data[3] = row2fixed & 0xFF;
461 data[4] = row3fixed >> 8;
462 data[5] = row3fixed & 0xFF;
466 return INV_ERROR_COMPASS_DATA_NOT_READY;
470 static struct ext_slave_descr yas529_descr = {
473 .suspend = yas529_suspend,
474 .resume = yas529_resume,
479 .type = EXT_SLAVE_TYPE_COMPASS,
480 .id = COMPASS_ID_YAS529,
483 .endian = EXT_SLAVE_BIG_ENDIAN,
484 .range = {19660, 8000},
489 struct ext_slave_descr *yas529_get_slave_descr(void)
491 return &yas529_descr;
494 /* -------------------------------------------------------------------------- */
495 struct yas529_mod_private_data {
496 struct i2c_client *client;
497 struct ext_slave_platform_data *pdata;
500 static unsigned short normal_i2c[] = { I2C_CLIENT_END };
502 static int yas529_mod_probe(struct i2c_client *client,
503 const struct i2c_device_id *devid)
505 struct ext_slave_platform_data *pdata;
506 struct yas529_mod_private_data *private_data;
509 dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
511 if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
516 pdata = client->dev.platform_data;
518 dev_err(&client->adapter->dev,
519 "Missing platform data for slave %s\n", devid->name);
524 private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
530 i2c_set_clientdata(client, private_data);
531 private_data->client = client;
532 private_data->pdata = pdata;
534 result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
535 yas529_get_slave_descr);
537 dev_err(&client->adapter->dev,
538 "Slave registration failed: %s, %d\n",
539 devid->name, result);
540 goto out_free_memory;
548 dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
553 static int yas529_mod_remove(struct i2c_client *client)
555 struct yas529_mod_private_data *private_data =
556 i2c_get_clientdata(client);
558 dev_dbg(&client->adapter->dev, "%s\n", __func__);
560 inv_mpu_unregister_slave(client, private_data->pdata,
561 yas529_get_slave_descr);
567 static const struct i2c_device_id yas529_mod_id[] = {
568 { "yas529", COMPASS_ID_YAS529 },
572 MODULE_DEVICE_TABLE(i2c, yas529_mod_id);
574 static struct i2c_driver yas529_mod_driver = {
575 .class = I2C_CLASS_HWMON,
576 .probe = yas529_mod_probe,
577 .remove = yas529_mod_remove,
578 .id_table = yas529_mod_id,
580 .owner = THIS_MODULE,
581 .name = "yas529_mod",
583 .address_list = normal_i2c,
586 static int __init yas529_mod_init(void)
588 int res = i2c_add_driver(&yas529_mod_driver);
589 pr_info("%s: Probe name %s\n", __func__, "yas529_mod");
591 pr_err("%s failed\n", __func__);
595 static void __exit yas529_mod_exit(void)
597 pr_info("%s\n", __func__);
598 i2c_del_driver(&yas529_mod_driver);
601 module_init(yas529_mod_init);
602 module_exit(yas529_mod_exit);
604 MODULE_AUTHOR("Invensense Corporation");
605 MODULE_DESCRIPTION("Driver to integrate YAS529 sensor with the MPU");
606 MODULE_LICENSE("GPL");
607 MODULE_ALIAS("yas529_mod");