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, 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 0000000..f53223f --- /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"); + +/** + * @} + */ |