/*
$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 .
$
*/
/* -------------------------------------------------------------------------- */
#include
#include
#include
#include
#include
#include
#include
#include "mpu-dev.h"
#include
#include
#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");
/**
* @}
*/