Added missing tegra files.HEADmaster

1 files changed, 611 insertions, 0 deletions

diff --git a/drivers/misc/inv_mpu/compass/yas529-kernel.c b/drivers/misc/inv_mpu/compass/yas529-kernel.c
new file mode 100644
index 00000000000..f53223fba64
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/yas529-kernel.c
@@ -0,0 +1,611 @@
+/*
+        $License:
+        Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+
+        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, see <http://www.gnu.org/licenses/>.
+        $
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+/*----- YAMAHA YAS529 Registers ------*/
+enum YAS_REG {
+        YAS_REG_CMDR            = 0x00, /* 000 < 5 */
+        YAS_REG_XOFFSETR        = 0x20, /* 001 < 5 */
+        YAS_REG_Y1OFFSETR       = 0x40, /* 010 < 5 */
+        YAS_REG_Y2OFFSETR       = 0x60, /* 011 < 5 */
+        YAS_REG_ICOILR          = 0x80, /* 100 < 5 */
+        YAS_REG_CAL             = 0xA0, /* 101 < 5 */
+        YAS_REG_CONFR           = 0xC0, /* 110 < 5 */
+        YAS_REG_DOUTR           = 0xE0  /* 111 < 5 */
+};
+
+/* -------------------------------------------------------------------------- */
+
+static long a1;
+static long a2;
+static long a3;
+static long a4;
+static long a5;
+static long a6;
+static long a7;
+static long a8;
+static long a9;
+
+/* -------------------------------------------------------------------------- */
+static int yas529_sensor_i2c_write(struct i2c_adapter *i2c_adap,
+                                   unsigned char address,
+                                   unsigned int len, unsigned char *data)
+{
+        struct i2c_msg msgs[1];
+        int res;
+
+        if (NULL == data || NULL == i2c_adap)
+                return -EINVAL;
+
+        msgs[0].addr = address;
+        msgs[0].flags = 0;      /* write */
+        msgs[0].buf = (unsigned char *)data;
+        msgs[0].len = len;
+
+        res = i2c_transfer(i2c_adap, msgs, 1);
+        if (res < 1)
+                return res;
+        else
+                return 0;
+}
+
+static int yas529_sensor_i2c_read(struct i2c_adapter *i2c_adap,
+                                  unsigned char address,
+                                  unsigned char reg,
+                                  unsigned int len, unsigned char *data)
+{
+        struct i2c_msg msgs[2];
+        int res;
+
+        if (NULL == data || NULL == i2c_adap)
+                return -EINVAL;
+
+        msgs[0].addr = address;
+        msgs[0].flags = I2C_M_RD;
+        msgs[0].buf = data;
+        msgs[0].len = len;
+
+        res = i2c_transfer(i2c_adap, msgs, 1);
+        if (res < 1)
+                return res;
+        else
+                return 0;
+}
+
+static int yas529_suspend(void *mlsl_handle,
+                          struct ext_slave_descr *slave,
+                          struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+
+        return result;
+}
+
+static int yas529_resume(void *mlsl_handle,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata)
+{
+        int result = INV_SUCCESS;
+
+        unsigned char dummyData[1] = { 0 };
+        unsigned char dummyRegister = 0;
+        unsigned char rawData[6];
+        unsigned char calData[9];
+
+        short xoffset, y1offset, y2offset;
+        short d2, d3, d4, d5, d6, d7, d8, d9;
+
+        /* YAS529 Application Manual MS-3C - Section 4.4.5 */
+        /* =============================================== */
+        /* Step 1 - register initialization */
+        /* zero initialization coil register - "100 00 000" */
+        dummyData[0] = YAS_REG_ICOILR | 0x00;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* zero config register - "110 00 000" */
+        dummyData[0] = YAS_REG_CONFR | 0x00;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Step 2 - initialization coil operation */
+        dummyData[0] = YAS_REG_ICOILR | 0x11;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x01;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x12;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x02;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x13;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x03;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x14;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x04;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x15;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x05;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x16;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x06;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x17;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x07;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x10;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_ICOILR | 0x00;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Step 3 - rough offset measurement */
+        /* Config register - Measurements results - "110 00 000" */
+        dummyData[0] = YAS_REG_CONFR | 0x00;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Measurements command register - Rough offset measurement -
+           "000 00001" */
+        dummyData[0] = YAS_REG_CMDR | 0x01;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(2);              /* wait at least 1.5ms */
+
+        /* Measurement data read */
+        result =
+            yas529_sensor_i2c_read(mlsl_handle, pdata->address,
+                                   dummyRegister, 6, rawData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        xoffset =
+            (short)((unsigned short)rawData[5] +
+                    ((unsigned short)rawData[4] & 0x7) * 256) - 5;
+        if (xoffset < 0)
+                xoffset = 0;
+        y1offset =
+            (short)((unsigned short)rawData[3] +
+                    ((unsigned short)rawData[2] & 0x7) * 256) - 5;
+        if (y1offset < 0)
+                y1offset = 0;
+        y2offset =
+            (short)((unsigned short)rawData[1] +
+                    ((unsigned short)rawData[0] & 0x7) * 256) - 5;
+        if (y2offset < 0)
+                y2offset = 0;
+
+        /* Step 4 - rough offset setting */
+        /* Set rough offset register values */
+        dummyData[0] = YAS_REG_XOFFSETR | xoffset;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_Y1OFFSETR | y1offset;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        dummyData[0] = YAS_REG_Y2OFFSETR | y2offset;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* CAL matrix read (first read is invalid) */
+        /* Config register - CAL register read - "110 01 000" */
+        dummyData[0] = YAS_REG_CONFR | 0x08;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* CAL data read */
+        result =
+            yas529_sensor_i2c_read(mlsl_handle, pdata->address,
+                                   dummyRegister, 9, calData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Config register - CAL register read - "110 01 000" */
+        dummyData[0] = YAS_REG_CONFR | 0x08;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* CAL data read */
+        result =
+            yas529_sensor_i2c_read(mlsl_handle, pdata->address,
+                                   dummyRegister, 9, calData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        /* Calculate coefficients of the sensitivity correction matrix */
+        a1 = 100;
+        d2 = (calData[0] & 0xFC) >> 2;  /* [71..66] 6bit */
+        a2 = (short)(d2 - 32);
+        /* [65..62] 4bit */
+        d3 = ((calData[0] & 0x03) << 2) | ((calData[1] & 0xC0) >> 6);
+        a3 = (short)(d3 - 8);
+        d4 = (calData[1] & 0x3F);       /* [61..56] 6bit */
+        a4 = (short)(d4 - 32);
+        d5 = (calData[2] & 0xFC) >> 2;  /* [55..50] 6bit */
+        a5 = (short)(d5 - 32) + 70;
+        /* [49..44] 6bit */
+        d6 = ((calData[2] & 0x03) << 4) | ((calData[3] & 0xF0) >> 4);
+        a6 = (short)(d6 - 32);
+        /* [43..38] 6bit */
+        d7 = ((calData[3] & 0x0F) << 2) | ((calData[4] & 0xC0) >> 6);
+        a7 = (short)(d7 - 32);
+        d8 = (calData[4] & 0x3F);       /* [37..32] 6bit */
+        a8 = (short)(d8 - 32);
+        d9 = (calData[5] & 0xFE) >> 1;  /* [31..25] 7bit */
+        a9 = (short)(d9 - 64) + 130;
+
+        return result;
+}
+
+static int yas529_read(void *mlsl_handle,
+                       struct ext_slave_descr *slave,
+                       struct ext_slave_platform_data *pdata,
+                       unsigned char *data)
+{
+        unsigned char stat;
+        unsigned char rawData[6];
+        unsigned char dummyData[1] = { 0 };
+        unsigned char dummyRegister = 0;
+        int result = INV_SUCCESS;
+        short SX, SY1, SY2, SY, SZ;
+        short row1fixed, row2fixed, row3fixed;
+
+        /* Config register - Measurements results - "110 00 000" */
+        dummyData[0] = YAS_REG_CONFR | 0x00;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        /* Measurements command register - Normal magnetic field measurement -
+           "000 00000" */
+        dummyData[0] = YAS_REG_CMDR | 0x00;
+        result =
+            yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+        msleep(10);
+        /* Measurement data read */
+        result =
+            yas529_sensor_i2c_read(mlsl_handle, pdata->address,
+                                   dummyRegister, 6, (unsigned char *)&rawData);
+        if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+        }
+
+        stat = rawData[0] & 0x80;
+        if (stat == 0x00) {
+                /* Extract raw data */
+                SX = (short)((unsigned short)rawData[5] +
+                             ((unsigned short)rawData[4] & 0x7) * 256);
+                SY1 =
+                    (short)((unsigned short)rawData[3] +
+                            ((unsigned short)rawData[2] & 0x7) * 256);
+                SY2 =
+                    (short)((unsigned short)rawData[1] +
+                            ((unsigned short)rawData[0] & 0x7) * 256);
+                if ((SX <= 1) || (SY1 <= 1) || (SY2 <= 1))
+                        return INV_ERROR_COMPASS_DATA_UNDERFLOW;
+                if ((SX >= 1024) || (SY1 >= 1024) || (SY2 >= 1024))
+                        return INV_ERROR_COMPASS_DATA_OVERFLOW;
+                /* Convert to XYZ axis */
+                SX = -1 * SX;
+                SY = SY2 - SY1;
+                SZ = SY1 + SY2;
+
+                /* Apply sensitivity correction matrix */
+                row1fixed = (short)((a1 * SX + a2 * SY + a3 * SZ) >> 7) * 41;
+                row2fixed = (short)((a4 * SX + a5 * SY + a6 * SZ) >> 7) * 41;
+                row3fixed = (short)((a7 * SX + a8 * SY + a9 * SZ) >> 7) * 41;
+
+                data[0] = row1fixed >> 8;
+                data[1] = row1fixed & 0xFF;
+                data[2] = row2fixed >> 8;
+                data[3] = row2fixed & 0xFF;
+                data[4] = row3fixed >> 8;
+                data[5] = row3fixed & 0xFF;
+
+                return INV_SUCCESS;
+        } else {
+                return INV_ERROR_COMPASS_DATA_NOT_READY;
+        }
+}
+
+static struct ext_slave_descr yas529_descr = {
+        .init             = NULL,
+        .exit             = NULL,
+        .suspend          = yas529_suspend,
+        .resume           = yas529_resume,
+        .read             = yas529_read,
+        .config           = NULL,
+        .get_config       = NULL,
+        .name             = "yas529",
+        .type             = EXT_SLAVE_TYPE_COMPASS,
+        .id               = COMPASS_ID_YAS529,
+        .read_reg         = 0x06,
+        .read_len         = 6,
+        .endian           = EXT_SLAVE_BIG_ENDIAN,
+        .range            = {19660, 8000},
+        .trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *yas529_get_slave_descr(void)
+{
+        return &yas529_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct yas529_mod_private_data {
+        struct i2c_client *client;
+        struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int yas529_mod_probe(struct i2c_client *client,
+                           const struct i2c_device_id *devid)
+{
+        struct ext_slave_platform_data *pdata;
+        struct yas529_mod_private_data *private_data;
+        int result = 0;
+
+        dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                result = -ENODEV;
+                goto out_no_free;
+        }
+
+        pdata = client->dev.platform_data;
+        if (!pdata) {
+                dev_err(&client->adapter->dev,
+                        "Missing platform data for slave %s\n", devid->name);
+                result = -EFAULT;
+                goto out_no_free;
+        }
+
+        private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+        if (!private_data) {
+                result = -ENOMEM;
+                goto out_no_free;
+        }
+
+        i2c_set_clientdata(client, private_data);
+        private_data->client = client;
+        private_data->pdata = pdata;
+
+        result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+                                        yas529_get_slave_descr);
+        if (result) {
+                dev_err(&client->adapter->dev,
+                        "Slave registration failed: %s, %d\n",
+                        devid->name, result);
+                goto out_free_memory;
+        }
+
+        return result;
+
+out_free_memory:
+        kfree(private_data);
+out_no_free:
+        dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+        return result;
+
+}
+
+static int yas529_mod_remove(struct i2c_client *client)
+{
+        struct yas529_mod_private_data *private_data =
+                i2c_get_clientdata(client);
+
+        dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+        inv_mpu_unregister_slave(client, private_data->pdata,
+                                yas529_get_slave_descr);
+
+        kfree(private_data);
+        return 0;
+}
+
+static const struct i2c_device_id yas529_mod_id[] = {
+        { "yas529", COMPASS_ID_YAS529 },
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, yas529_mod_id);
+
+static struct i2c_driver yas529_mod_driver = {
+        .class = I2C_CLASS_HWMON,
+        .probe = yas529_mod_probe,
+        .remove = yas529_mod_remove,
+        .id_table = yas529_mod_id,
+        .driver = {
+                   .owner = THIS_MODULE,
+                   .name = "yas529_mod",
+                   },
+        .address_list = normal_i2c,
+};
+
+static int __init yas529_mod_init(void)
+{
+        int res = i2c_add_driver(&yas529_mod_driver);
+        pr_info("%s: Probe name %s\n", __func__, "yas529_mod");
+        if (res)
+                pr_err("%s failed\n", __func__);
+        return res;
+}
+
+static void __exit yas529_mod_exit(void)
+{
+        pr_info("%s\n", __func__);
+        i2c_del_driver(&yas529_mod_driver);
+}
+
+module_init(yas529_mod_init);
+module_exit(yas529_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate YAS529 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("yas529_mod");
+
+/**
+ *  @}
+ */