diff options
Diffstat (limited to 'drivers/misc/inv_mpu/compass/yas529-kernel.c')
| -rw-r--r-- | drivers/misc/inv_mpu/compass/yas529-kernel.c | 611 |
1 files changed, 0 insertions, 611 deletions
diff --git a/drivers/misc/inv_mpu/compass/yas529-kernel.c b/drivers/misc/inv_mpu/compass/yas529-kernel.c deleted file mode 100644 index f53223f..0000000 --- a/drivers/misc/inv_mpu/compass/yas529-kernel.c +++ /dev/null @@ -1,611 +0,0 @@ -/* - $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"); - -/** - * @} - */ |