drivers/misc/inv_mpu/compass/yas529-kernel.c

   1 /*
   2         $License:
   3         Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
   4
   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.
   9
  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.
  14
  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/>.
  17         $
  18  */
  19
  20 /* -------------------------------------------------------------------------- */
  21
  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>
  29 #include "mpu-dev.h"
  30
  31 #include <log.h>
  32 #include <linux/mpu.h>
  33 #include "mlsl.h"
  34 #include "mldl_cfg.h"
  35 #undef MPL_LOG_TAG
  36 #define MPL_LOG_TAG "MPL-acc"
  37
  38 /*----- YAMAHA YAS529 Registers ------*/
  39 enum YAS_REG {
  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 */
  48 };
  49
  50 /* -------------------------------------------------------------------------- */
  51
  52 static long a1;
  53 static long a2;
  54 static long a3;
  55 static long a4;
  56 static long a5;
  57 static long a6;
  58 static long a7;
  59 static long a8;
  60 static long a9;
  61
  62 /* -------------------------------------------------------------------------- */
  63 static int yas529_sensor_i2c_write(struct i2c_adapter *i2c_adap,
  64                                    unsigned char address,
  65                                    unsigned int len, unsigned char *data)
  66 {
  67         struct i2c_msg msgs[1];
  68         int res;
  69
  70         if (NULL == data || NULL == i2c_adap)
  71                 return -EINVAL;
  72
  73         msgs[0].addr = address;
  74         msgs[0].flags = 0;      /* write */
  75         msgs[0].buf = (unsigned char *)data;
  76         msgs[0].len = len;
  77
  78         res = i2c_transfer(i2c_adap, msgs, 1);
  79         if (res < 1)
  80                 return res;
  81         else
  82                 return 0;
  83 }
  84
  85 static int yas529_sensor_i2c_read(struct i2c_adapter *i2c_adap,
  86                                   unsigned char address,
  87                                   unsigned char reg,
  88                                   unsigned int len, unsigned char *data)
  89 {
  90         struct i2c_msg msgs[2];
  91         int res;
  92
  93         if (NULL == data || NULL == i2c_adap)
  94                 return -EINVAL;
  95
  96         msgs[0].addr = address;
  97         msgs[0].flags = I2C_M_RD;
  98         msgs[0].buf = data;
  99         msgs[0].len = len;
 100
 101         res = i2c_transfer(i2c_adap, msgs, 1);
 102         if (res < 1)
 103                 return res;
 104         else
 105                 return 0;
 106 }
 107
 108 static int yas529_suspend(void *mlsl_handle,
 109                           struct ext_slave_descr *slave,
 110                           struct ext_slave_platform_data *pdata)
 111 {
 112         int result = INV_SUCCESS;
 113
 114         return result;
 115 }
 116
 117 static int yas529_resume(void *mlsl_handle,
 118                          struct ext_slave_descr *slave,
 119                          struct ext_slave_platform_data *pdata)
 120 {
 121         int result = INV_SUCCESS;
 122
 123         unsigned char dummyData[1] = { 0 };
 124         unsigned char dummyRegister = 0;
 125         unsigned char rawData[6];
 126         unsigned char calData[9];
 127
 128         short xoffset, y1offset, y2offset;
 129         short d2, d3, d4, d5, d6, d7, d8, d9;
 130
 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;
 136         result =
 137             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 138         if (result) {
 139                 LOG_RESULT_LOCATION(result);
 140                 return result;
 141         }
 142         /* zero config register - "110 00 000" */
 143         dummyData[0] = YAS_REG_CONFR | 0x00;
 144         result =
 145             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 146         if (result) {
 147                 LOG_RESULT_LOCATION(result);
 148                 return result;
 149         }
 150
 151         /* Step 2 - initialization coil operation */
 152         dummyData[0] = YAS_REG_ICOILR | 0x11;
 153         result =
 154             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 155         if (result) {
 156                 LOG_RESULT_LOCATION(result);
 157                 return result;
 158         }
 159         dummyData[0] = YAS_REG_ICOILR | 0x01;
 160         result =
 161             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 162         if (result) {
 163                 LOG_RESULT_LOCATION(result);
 164                 return result;
 165         }
 166         dummyData[0] = YAS_REG_ICOILR | 0x12;
 167         result =
 168             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 169         if (result) {
 170                 LOG_RESULT_LOCATION(result);
 171                 return result;
 172         }
 173         dummyData[0] = YAS_REG_ICOILR | 0x02;
 174         result =
 175             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 176         if (result) {
 177                 LOG_RESULT_LOCATION(result);
 178                 return result;
 179         }
 180         dummyData[0] = YAS_REG_ICOILR | 0x13;
 181         result =
 182             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 183         if (result) {
 184                 LOG_RESULT_LOCATION(result);
 185                 return result;
 186         }
 187         dummyData[0] = YAS_REG_ICOILR | 0x03;
 188         result =
 189             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 190         if (result) {
 191                 LOG_RESULT_LOCATION(result);
 192                 return result;
 193         }
 194         dummyData[0] = YAS_REG_ICOILR | 0x14;
 195         result =
 196             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 197         if (result) {
 198                 LOG_RESULT_LOCATION(result);
 199                 return result;
 200         }
 201         dummyData[0] = YAS_REG_ICOILR | 0x04;
 202         result =
 203             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 204         if (result) {
 205                 LOG_RESULT_LOCATION(result);
 206                 return result;
 207         }
 208         dummyData[0] = YAS_REG_ICOILR | 0x15;
 209         result =
 210             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 211         if (result) {
 212                 LOG_RESULT_LOCATION(result);
 213                 return result;
 214         }
 215         dummyData[0] = YAS_REG_ICOILR | 0x05;
 216         result =
 217             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 218         if (result) {
 219                 LOG_RESULT_LOCATION(result);
 220                 return result;
 221         }
 222         dummyData[0] = YAS_REG_ICOILR | 0x16;
 223         result =
 224             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 225         if (result) {
 226                 LOG_RESULT_LOCATION(result);
 227                 return result;
 228         }
 229         dummyData[0] = YAS_REG_ICOILR | 0x06;
 230         result =
 231             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 232         if (result) {
 233                 LOG_RESULT_LOCATION(result);
 234                 return result;
 235         }
 236         dummyData[0] = YAS_REG_ICOILR | 0x17;
 237         result =
 238             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 239         if (result) {
 240                 LOG_RESULT_LOCATION(result);
 241                 return result;
 242         }
 243         dummyData[0] = YAS_REG_ICOILR | 0x07;
 244         result =
 245             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 246         if (result) {
 247                 LOG_RESULT_LOCATION(result);
 248                 return result;
 249         }
 250         dummyData[0] = YAS_REG_ICOILR | 0x10;
 251         result =
 252             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 253         if (result) {
 254                 LOG_RESULT_LOCATION(result);
 255                 return result;
 256         }
 257         dummyData[0] = YAS_REG_ICOILR | 0x00;
 258         result =
 259             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 260         if (result) {
 261                 LOG_RESULT_LOCATION(result);
 262                 return result;
 263         }
 264
 265         /* Step 3 - rough offset measurement */
 266         /* Config register - Measurements results - "110 00 000" */
 267         dummyData[0] = YAS_REG_CONFR | 0x00;
 268         result =
 269             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 270         if (result) {
 271                 LOG_RESULT_LOCATION(result);
 272                 return result;
 273         }
 274         /* Measurements command register - Rough offset measurement -
 275            "000 00001" */
 276         dummyData[0] = YAS_REG_CMDR | 0x01;
 277         result =
 278             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 279         if (result) {
 280                 LOG_RESULT_LOCATION(result);
 281                 return result;
 282         }
 283         msleep(2);              /* wait at least 1.5ms */
 284
 285         /* Measurement data read */
 286         result =
 287             yas529_sensor_i2c_read(mlsl_handle, pdata->address,
 288                                    dummyRegister, 6, rawData);
 289         if (result) {
 290                 LOG_RESULT_LOCATION(result);
 291                 return result;
 292         }
 293         xoffset =
 294             (short)((unsigned short)rawData[5] +
 295                     ((unsigned short)rawData[4] & 0x7) * 256) - 5;
 296         if (xoffset < 0)
 297                 xoffset = 0;
 298         y1offset =
 299             (short)((unsigned short)rawData[3] +
 300                     ((unsigned short)rawData[2] & 0x7) * 256) - 5;
 301         if (y1offset < 0)
 302                 y1offset = 0;
 303         y2offset =
 304             (short)((unsigned short)rawData[1] +
 305                     ((unsigned short)rawData[0] & 0x7) * 256) - 5;
 306         if (y2offset < 0)
 307                 y2offset = 0;
 308
 309         /* Step 4 - rough offset setting */
 310         /* Set rough offset register values */
 311         dummyData[0] = YAS_REG_XOFFSETR | xoffset;
 312         result =
 313             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 314         if (result) {
 315                 LOG_RESULT_LOCATION(result);
 316                 return result;
 317         }
 318         dummyData[0] = YAS_REG_Y1OFFSETR | y1offset;
 319         result =
 320             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 321         if (result) {
 322                 LOG_RESULT_LOCATION(result);
 323                 return result;
 324         }
 325         dummyData[0] = YAS_REG_Y2OFFSETR | y2offset;
 326         result =
 327             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 328         if (result) {
 329                 LOG_RESULT_LOCATION(result);
 330                 return result;
 331         }
 332
 333         /* CAL matrix read (first read is invalid) */
 334         /* Config register - CAL register read - "110 01 000" */
 335         dummyData[0] = YAS_REG_CONFR | 0x08;
 336         result =
 337             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 338         if (result) {
 339                 LOG_RESULT_LOCATION(result);
 340                 return result;
 341         }
 342         /* CAL data read */
 343         result =
 344             yas529_sensor_i2c_read(mlsl_handle, pdata->address,
 345                                    dummyRegister, 9, calData);
 346         if (result) {
 347                 LOG_RESULT_LOCATION(result);
 348                 return result;
 349         }
 350         /* Config register - CAL register read - "110 01 000" */
 351         dummyData[0] = YAS_REG_CONFR | 0x08;
 352         result =
 353             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 354         if (result) {
 355                 LOG_RESULT_LOCATION(result);
 356                 return result;
 357         }
 358         /* CAL data read */
 359         result =
 360             yas529_sensor_i2c_read(mlsl_handle, pdata->address,
 361                                    dummyRegister, 9, calData);
 362         if (result) {
 363                 LOG_RESULT_LOCATION(result);
 364                 return result;
 365         }
 366
 367         /* Calculate coefficients of the sensitivity correction matrix */
 368         a1 = 100;
 369         d2 = (calData[0] & 0xFC) >> 2;  /* [71..66] 6bit */
 370         a2 = (short)(d2 - 32);
 371         /* [65..62] 4bit */
 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;
 378         /* [49..44] 6bit */
 379         d6 = ((calData[2] & 0x03) << 4) | ((calData[3] & 0xF0) >> 4);
 380         a6 = (short)(d6 - 32);
 381         /* [43..38] 6bit */
 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;
 388
 389         return result;
 390 }
 391
 392 static int yas529_read(void *mlsl_handle,
 393                        struct ext_slave_descr *slave,
 394                        struct ext_slave_platform_data *pdata,
 395                        unsigned char *data)
 396 {
 397         unsigned char stat;
 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;
 404
 405         /* Config register - Measurements results - "110 00 000" */
 406         dummyData[0] = YAS_REG_CONFR | 0x00;
 407         result =
 408             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 409         if (result) {
 410                 LOG_RESULT_LOCATION(result);
 411                 return result;
 412         }
 413         /* Measurements command register - Normal magnetic field measurement -
 414            "000 00000" */
 415         dummyData[0] = YAS_REG_CMDR | 0x00;
 416         result =
 417             yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
 418         if (result) {
 419                 LOG_RESULT_LOCATION(result);
 420                 return result;
 421         }
 422         msleep(10);
 423         /* Measurement data read */
 424         result =
 425             yas529_sensor_i2c_read(mlsl_handle, pdata->address,
 426                                    dummyRegister, 6, (unsigned char *)&rawData);
 427         if (result) {
 428                 LOG_RESULT_LOCATION(result);
 429                 return result;
 430         }
 431
 432         stat = rawData[0] & 0x80;
 433         if (stat == 0x00) {
 434                 /* Extract raw data */
 435                 SX = (short)((unsigned short)rawData[5] +
 436                              ((unsigned short)rawData[4] & 0x7) * 256);
 437                 SY1 =
 438                     (short)((unsigned short)rawData[3] +
 439                             ((unsigned short)rawData[2] & 0x7) * 256);
 440                 SY2 =
 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 */
 448                 SX = -1 * SX;
 449                 SY = SY2 - SY1;
 450                 SZ = SY1 + SY2;
 451
 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;
 456
 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;
 463
 464                 return INV_SUCCESS;
 465         } else {
 466                 return INV_ERROR_COMPASS_DATA_NOT_READY;
 467         }
 468 }
 469
 470 static struct ext_slave_descr yas529_descr = {
 471         .init             = NULL,
 472         .exit             = NULL,
 473         .suspend          = yas529_suspend,
 474         .resume           = yas529_resume,
 475         .read             = yas529_read,
 476         .config           = NULL,
 477         .get_config       = NULL,
 478         .name             = "yas529",
 479         .type             = EXT_SLAVE_TYPE_COMPASS,
 480         .id               = COMPASS_ID_YAS529,
 481         .read_reg         = 0x06,
 482         .read_len         = 6,
 483         .endian           = EXT_SLAVE_BIG_ENDIAN,
 484         .range            = {19660, 8000},
 485         .trigger          = NULL,
 486 };
 487
 488 static
 489 struct ext_slave_descr *yas529_get_slave_descr(void)
 490 {
 491         return &yas529_descr;
 492 }
 493
 494 /* -------------------------------------------------------------------------- */
 495 struct yas529_mod_private_data {
 496         struct i2c_client *client;
 497         struct ext_slave_platform_data *pdata;
 498 };
 499
 500 static unsigned short normal_i2c[] = { I2C_CLIENT_END };
 501
 502 static int yas529_mod_probe(struct i2c_client *client,
 503                            const struct i2c_device_id *devid)
 504 {
 505         struct ext_slave_platform_data *pdata;
 506         struct yas529_mod_private_data *private_data;
 507         int result = 0;
 508
 509         dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
 510
 511         if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
 512                 result = -ENODEV;
 513                 goto out_no_free;
 514         }
 515
 516         pdata = client->dev.platform_data;
 517         if (!pdata) {
 518                 dev_err(&client->adapter->dev,
 519                         "Missing platform data for slave %s\n", devid->name);
 520                 result = -EFAULT;
 521                 goto out_no_free;
 522         }
 523
 524         private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
 525         if (!private_data) {
 526                 result = -ENOMEM;
 527                 goto out_no_free;
 528         }
 529
 530         i2c_set_clientdata(client, private_data);
 531         private_data->client = client;
 532         private_data->pdata = pdata;
 533
 534         result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
 535                                         yas529_get_slave_descr);
 536         if (result) {
 537                 dev_err(&client->adapter->dev,
 538                         "Slave registration failed: %s, %d\n",
 539                         devid->name, result);
 540                 goto out_free_memory;
 541         }
 542
 543         return result;
 544
 545 out_free_memory:
 546         kfree(private_data);
 547 out_no_free:
 548         dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
 549         return result;
 550
 551 }
 552
 553 static int yas529_mod_remove(struct i2c_client *client)
 554 {
 555         struct yas529_mod_private_data *private_data =
 556                 i2c_get_clientdata(client);
 557
 558         dev_dbg(&client->adapter->dev, "%s\n", __func__);
 559
 560         inv_mpu_unregister_slave(client, private_data->pdata,
 561                                 yas529_get_slave_descr);
 562
 563         kfree(private_data);
 564         return 0;
 565 }
 566
 567 static const struct i2c_device_id yas529_mod_id[] = {
 568         { "yas529", COMPASS_ID_YAS529 },
 569         {}
 570 };
 571
 572 MODULE_DEVICE_TABLE(i2c, yas529_mod_id);
 573
 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,
 579         .driver = {
 580                    .owner = THIS_MODULE,
 581                    .name = "yas529_mod",
 582                    },
 583         .address_list = normal_i2c,
 584 };
 585
 586 static int __init yas529_mod_init(void)
 587 {
 588         int res = i2c_add_driver(&yas529_mod_driver);
 589         pr_info("%s: Probe name %s\n", __func__, "yas529_mod");
 590         if (res)
 591                 pr_err("%s failed\n", __func__);
 592         return res;
 593 }
 594
 595 static void __exit yas529_mod_exit(void)
 596 {
 597         pr_info("%s\n", __func__);
 598         i2c_del_driver(&yas529_mod_driver);
 599 }
 600
 601 module_init(yas529_mod_init);
 602 module_exit(yas529_mod_exit);
 603
 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");
 608
 609 /**
 610  *  @}
 611  */