diff options
Diffstat (limited to 'drivers/misc/inv_mpu/compass/yas530.c')
| -rw-r--r-- | drivers/misc/inv_mpu/compass/yas530.c | 596 |
1 files changed, 596 insertions, 0 deletions
diff --git a/drivers/misc/inv_mpu/compass/yas530.c b/drivers/misc/inv_mpu/compass/yas530.c new file mode 100644 index 0000000..263990f --- /dev/null +++ b/drivers/misc/inv_mpu/compass/yas530.c @@ -0,0 +1,596 @@ +/* + $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/>. + $ + */ + +/** + * @addtogroup COMPASSDL + * + * @{ + * @file yas530.c + * @brief Magnetometer setup and handling methods for Yamaha YAS530 + * compass when used in a user-space solution (no kernel driver). + */ + +/* -------------------------------------------------------------------------- */ + +#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/module.h> +#include <linux/delay.h> +#include "mpu-dev.h" + +#include "log.h" +#include <linux/mpu_411.h> +#include "mlsl.h" +#include "mldl_cfg.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-compass" + +/* -------------------------------------------------------------------------- */ +#define YAS530_REGADDR_DEVICE_ID (0x80) +#define YAS530_REGADDR_ACTUATE_INIT_COIL (0x81) +#define YAS530_REGADDR_MEASURE_COMMAND (0x82) +#define YAS530_REGADDR_CONFIG (0x83) +#define YAS530_REGADDR_MEASURE_INTERVAL (0x84) +#define YAS530_REGADDR_OFFSET_X (0x85) +#define YAS530_REGADDR_OFFSET_Y1 (0x86) +#define YAS530_REGADDR_OFFSET_Y2 (0x87) +#define YAS530_REGADDR_TEST1 (0x88) +#define YAS530_REGADDR_TEST2 (0x89) +#define YAS530_REGADDR_CAL (0x90) +#define YAS530_REGADDR_MEASURE_DATA (0xb0) + +/* -------------------------------------------------------------------------- */ +static int Cx, Cy1, Cy2; +static int /*a1, */ a2, a3, a4, a5, a6, a7, a8, a9; +static int k; + +static unsigned char dx, dy1, dy2; +static unsigned char d2, d3, d4, d5, d6, d7, d8, d9, d0; +static unsigned char dck; + +/* -------------------------------------------------------------------------- */ + +static int set_hardware_offset(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + char offset_x, char offset_y1, char offset_y2) +{ + char data; + int result = INV_SUCCESS; + + data = offset_x & 0x3f; + result = inv_serial_single_write(mlsl_handle, pdata->address, + YAS530_REGADDR_OFFSET_X, data); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + data = offset_y1 & 0x3f; + result = inv_serial_single_write(mlsl_handle, pdata->address, + YAS530_REGADDR_OFFSET_Y1, data); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + data = offset_y2 & 0x3f; + result = inv_serial_single_write(mlsl_handle, pdata->address, + YAS530_REGADDR_OFFSET_Y2, data); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return result; +} + +static int set_measure_command(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + int ldtc, int fors, int dlymes) +{ + int result = INV_SUCCESS; + + result = inv_serial_single_write(mlsl_handle, pdata->address, + YAS530_REGADDR_MEASURE_COMMAND, 0x01); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return result; +} + +static int measure_normal(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + int *busy, unsigned short *t, + unsigned short *x, unsigned short *y1, + unsigned short *y2) +{ + unsigned char data[8]; + unsigned short b, to, xo, y1o, y2o; + int result; + ktime_t sleeptime; + result = set_measure_command(mlsl_handle, slave, pdata, 0, 0, 0); + sleeptime = ktime_set(0, 2 * NSEC_PER_MSEC); + set_current_state(TASK_UNINTERRUPTIBLE); + schedule_hrtimeout(&sleeptime, HRTIMER_MODE_REL); + + result = inv_serial_read(mlsl_handle, pdata->address, + YAS530_REGADDR_MEASURE_DATA, 8, data); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + b = (data[0] >> 7) & 0x01; + to = ((data[0] << 2) & 0x1fc) | ((data[1] >> 6) & 0x03); + xo = ((data[2] << 5) & 0xfe0) | ((data[3] >> 3) & 0x1f); + y1o = ((data[4] << 5) & 0xfe0) | ((data[5] >> 3) & 0x1f); + y2o = ((data[6] << 5) & 0xfe0) | ((data[7] >> 3) & 0x1f); + + *busy = b; + *t = to; + *x = xo; + *y1 = y1o; + *y2 = y2o; + + return result; +} + +static int check_offset(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + char offset_x, char offset_y1, char offset_y2, + int *flag_x, int *flag_y1, int *flag_y2) +{ + int result; + int busy; + short t, x, y1, y2; + + result = set_hardware_offset(mlsl_handle, slave, pdata, + offset_x, offset_y1, offset_y2); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = measure_normal(mlsl_handle, slave, pdata, + &busy, &t, &x, &y1, &y2); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + *flag_x = 0; + *flag_y1 = 0; + *flag_y2 = 0; + + if (x > 2048) + *flag_x = 1; + if (y1 > 2048) + *flag_y1 = 1; + if (y2 > 2048) + *flag_y2 = 1; + if (x < 2048) + *flag_x = -1; + if (y1 < 2048) + *flag_y1 = -1; + if (y2 < 2048) + *flag_y2 = -1; + + return result; +} + +static int measure_and_set_offset(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + char *offset) +{ + int i; + int result = INV_SUCCESS; + char offset_x = 0, offset_y1 = 0, offset_y2 = 0; + int flag_x = 0, flag_y1 = 0, flag_y2 = 0; + static const int correct[5] = { 16, 8, 4, 2, 1 }; + + for (i = 0; i < 5; i++) { + result = check_offset(mlsl_handle, slave, pdata, + offset_x, offset_y1, offset_y2, + &flag_x, &flag_y1, &flag_y2); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + if (flag_x) + offset_x += flag_x * correct[i]; + if (flag_y1) + offset_y1 += flag_y1 * correct[i]; + if (flag_y2) + offset_y2 += flag_y2 * correct[i]; + } + + result = set_hardware_offset(mlsl_handle, slave, pdata, + offset_x, offset_y1, offset_y2); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + offset[0] = offset_x; + offset[1] = offset_y1; + offset[2] = offset_y2; + + return result; +} + +static void coordinate_conversion(short x, short y1, short y2, short t, + int32_t *xo, int32_t *yo, int32_t *zo) +{ + int32_t sx, sy1, sy2, sy, sz; + int32_t hx, hy, hz; + + sx = x - (Cx * t) / 100; + sy1 = y1 - (Cy1 * t) / 100; + sy2 = y2 - (Cy2 * t) / 100; + + sy = sy1 - sy2; + sz = -sy1 - sy2; + + hx = k * ((100 * sx + a2 * sy + a3 * sz) / 10); + hy = k * ((a4 * sx + a5 * sy + a6 * sz) / 10); + hz = k * ((a7 * sx + a8 * sy + a9 * sz) / 10); + + *xo = hx; + *yo = hy; + *zo = hz; +} + +static int yas530_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = INV_SUCCESS; + + return result; +} + +static int yas530_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = INV_SUCCESS; + + unsigned char dummyData = 0x00; + char offset[3] = { 0, 0, 0 }; + unsigned char data[16]; + unsigned char read_reg[1]; + + /* =============================================== */ + + /* Step 1 - Test register initialization */ + dummyData = 0x00; + result = inv_serial_single_write(mlsl_handle, pdata->address, + YAS530_REGADDR_TEST1, dummyData); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = + inv_serial_single_write(mlsl_handle, pdata->address, + YAS530_REGADDR_TEST2, dummyData); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* Device ID read */ + result = inv_serial_read(mlsl_handle, pdata->address, + YAS530_REGADDR_DEVICE_ID, 1, read_reg); + + /*Step 2 Read the CAL register */ + /* CAL data read */ + result = inv_serial_read(mlsl_handle, pdata->address, + YAS530_REGADDR_CAL, 16, data); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + /* CAL data Second Read */ + result = inv_serial_read(mlsl_handle, pdata->address, + YAS530_REGADDR_CAL, 16, data); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /*Cal data */ + dx = data[0]; + dy1 = data[1]; + dy2 = data[2]; + d2 = (data[3] >> 2) & 0x03f; + d3 = ((data[3] << 2) & 0x0c) | ((data[4] >> 6) & 0x03); + d4 = data[4] & 0x3f; + d5 = (data[5] >> 2) & 0x3f; + d6 = ((data[5] << 4) & 0x30) | ((data[6] >> 4) & 0x0f); + d7 = ((data[6] << 3) & 0x78) | ((data[7] >> 5) & 0x07); + d8 = ((data[7] << 1) & 0x3e) | ((data[8] >> 7) & 0x01); + d9 = ((data[8] << 1) & 0xfe) | ((data[9] >> 7) & 0x01); + d0 = (data[9] >> 2) & 0x1f; + dck = ((data[9] << 1) & 0x06) | ((data[10] >> 7) & 0x01); + + /*Correction Data */ + Cx = (int)dx * 6 - 768; + Cy1 = (int)dy1 * 6 - 768; + Cy2 = (int)dy2 * 6 - 768; + a2 = (int)d2 - 32; + a3 = (int)d3 - 8; + a4 = (int)d4 - 32; + a5 = (int)d5 + 38; + a6 = (int)d6 - 32; + a7 = (int)d7 - 64; + a8 = (int)d8 - 32; + a9 = (int)d9; + k = (int)d0 + 10; + + /*Obtain the [49:47] bits */ + dck &= 0x07; + + /*Step 3 : Storing the CONFIG with the CLK value */ + dummyData = 0x00 | (dck << 2); + result = inv_serial_single_write(mlsl_handle, pdata->address, + YAS530_REGADDR_CONFIG, dummyData); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /*Step 4 : Set Acquisition Interval Register */ + dummyData = 0x00; + result = inv_serial_single_write(mlsl_handle, pdata->address, + YAS530_REGADDR_MEASURE_INTERVAL, + dummyData); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /*Step 5 : Reset Coil */ + dummyData = 0x00; + result = inv_serial_single_write(mlsl_handle, pdata->address, + YAS530_REGADDR_ACTUATE_INIT_COIL, + dummyData); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* Offset Measurement and Set */ + result = measure_and_set_offset(mlsl_handle, slave, pdata, offset); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return result; +} + +static int yas530_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + int result = INV_SUCCESS; + + int busy; + short t, x, y1, y2; + int32_t xyz[3]; + short rawfixed[3]; + + result = measure_normal(mlsl_handle, slave, pdata, + &busy, &t, &x, &y1, &y2); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + coordinate_conversion(x, y1, y2, t, &xyz[0], &xyz[1], &xyz[2]); + + rawfixed[0] = (short)(xyz[0] / 100); + rawfixed[1] = (short)(xyz[1] / 100); + rawfixed[2] = (short)(xyz[2] / 100); + + data[0] = rawfixed[0] >> 8; + data[1] = rawfixed[0] & 0xFF; + data[2] = rawfixed[1] >> 8; + data[3] = rawfixed[1] & 0xFF; + data[4] = rawfixed[2] >> 8; + data[5] = rawfixed[2] & 0xFF; + + if (busy) + return INV_ERROR_COMPASS_DATA_NOT_READY; + return result; +} + +static int yas530_get_config(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + struct ext_slave_config *data) +{ + int result = INV_SUCCESS; + //struct yas530_private_data *private_data = pdata->private_data; + + switch (data->key) + { + default: + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + } + return result; +} + +static struct ext_slave_descr yas530_descr = { + .init = NULL, + .exit = NULL, + .suspend = yas530_suspend, + .resume = yas530_resume, + .read = yas530_read, + .config = NULL, + .get_config = yas530_get_config, + .name = "yas530", + .type = EXT_SLAVE_TYPE_COMPASS, + .id = COMPASS_ID_YAS530, + .read_reg = 0x06, + .read_len = 6, + .endian = EXT_SLAVE_BIG_ENDIAN, + .range = {3276, 8001}, + .trigger = NULL, +}; + +static +struct ext_slave_descr *yas530_get_slave_descr(void) +{ + return &yas530_descr; +} + +/* -------------------------------------------------------------------------- */ +struct yas530_mod_private_data { + struct i2c_client *client; + struct ext_slave_platform_data *pdata; +}; + +static unsigned short normal_i2c[] = { I2C_CLIENT_END }; + +static int yas530_mod_probe(struct i2c_client *client, + const struct i2c_device_id *devid) +{ + struct ext_slave_platform_data *pdata; + struct yas530_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, + yas530_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 yas530_mod_remove(struct i2c_client *client) +{ + struct yas530_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, + yas530_get_slave_descr); + + kfree(private_data); + return 0; +} + +static const struct i2c_device_id yas530_mod_id[] = { + { "yas530", COMPASS_ID_YAS530 }, + {} +}; + +MODULE_DEVICE_TABLE(i2c, yas530_mod_id); + +static struct i2c_driver yas530_mod_driver = { + .class = I2C_CLASS_HWMON, + .probe = yas530_mod_probe, + .remove = yas530_mod_remove, + .id_table = yas530_mod_id, + .driver = { + .owner = THIS_MODULE, + .name = "yas530_mod", + }, + .address_list = normal_i2c, +}; + +static int __init yas530_mod_init(void) +{ + int res = i2c_add_driver(&yas530_mod_driver); + pr_info("%s: Probe name %s\n", __func__, "yas530_mod"); + if (res) + pr_err("%s failed\n", __func__); + return res; +} + +static void __exit yas530_mod_exit(void) +{ + pr_info("%s\n", __func__); + i2c_del_driver(&yas530_mod_driver); +} + +module_init(yas530_mod_init); +module_exit(yas530_mod_exit); + +MODULE_AUTHOR("Invensense Corporation"); +MODULE_DESCRIPTION("Driver to integrate YAS530 sensor with the MPU"); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("yas530_mod"); + +/** + * @} + */ |