From c6da2cfeb05178a11c6d062a06f8078150ee492f Mon Sep 17 00:00:00 2001 From: codeworkx Date: Sat, 2 Jun 2012 13:09:29 +0200 Subject: samsung update 1 --- drivers/misc/mpu3050/compass/ami304.c | 164 +++++++++ drivers/misc/mpu3050/compass/ami30x.c | 167 ++++++++++ drivers/misc/mpu3050/compass/hmc5883.c | 254 ++++++++++++++ drivers/misc/mpu3050/compass/hscdtd002b.c | 163 +++++++++ drivers/misc/mpu3050/compass/hscdtd004a.c | 162 +++++++++ drivers/misc/mpu3050/compass/lsm303m.c | 244 ++++++++++++++ drivers/misc/mpu3050/compass/mmc314x.c | 184 +++++++++++ drivers/misc/mpu3050/compass/mmc328x.c | 220 ++++++++++++ drivers/misc/mpu3050/compass/mpuak8975.c | 188 +++++++++++ drivers/misc/mpu3050/compass/yas529-kernel.c | 477 +++++++++++++++++++++++++++ 10 files changed, 2223 insertions(+) create mode 100755 drivers/misc/mpu3050/compass/ami304.c create mode 100755 drivers/misc/mpu3050/compass/ami30x.c create mode 100755 drivers/misc/mpu3050/compass/hmc5883.c create mode 100755 drivers/misc/mpu3050/compass/hscdtd002b.c create mode 100755 drivers/misc/mpu3050/compass/hscdtd004a.c create mode 100755 drivers/misc/mpu3050/compass/lsm303m.c create mode 100755 drivers/misc/mpu3050/compass/mmc314x.c create mode 100755 drivers/misc/mpu3050/compass/mmc328x.c create mode 100755 drivers/misc/mpu3050/compass/mpuak8975.c create mode 100755 drivers/misc/mpu3050/compass/yas529-kernel.c (limited to 'drivers/misc/mpu3050/compass') diff --git a/drivers/misc/mpu3050/compass/ami304.c b/drivers/misc/mpu3050/compass/ami304.c new file mode 100755 index 00000000000..5c33861fc8b --- /dev/null +++ b/drivers/misc/mpu3050/compass/ami304.c @@ -0,0 +1,164 @@ +/* + $License: + Copyright (C) 2010 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 . + $ + */ + +/** + * @defgroup COMPASSDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file ami304.c + * @brief Magnetometer setup and handling methods for Aichi ami304 compass. +*/ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-compass" + +#define AMI304_REG_DATAX (0x10) +#define AMI304_REG_STAT1 (0x18) +#define AMI304_REG_CNTL1 (0x1B) +#define AMI304_REG_CNTL2 (0x1C) +#define AMI304_REG_CNTL3 (0x1D) + +#define AMI304_BIT_CNTL1_PC1 (0x80) +#define AMI304_BIT_CNTL1_ODR1 (0x10) +#define AMI304_BIT_CNTL1_FS1 (0x02) + +#define AMI304_BIT_CNTL2_IEN (0x10) +#define AMI304_BIT_CNTL2_DREN (0x08) +#define AMI304_BIT_CNTL2_DRP (0x04) +#define AMI304_BIT_CNTL3_F0RCE (0x40) + +int ami304_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result; + unsigned char reg; + result = + MLSLSerialRead(mlsl_handle, pdata->address, AMI304_REG_CNTL1, + 1, ®); + ERROR_CHECK(result); + + reg &= ~(AMI304_BIT_CNTL1_PC1|AMI304_BIT_CNTL1_FS1); + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AMI304_REG_CNTL1, reg); + ERROR_CHECK(result); + + return result; +} + +int ami304_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + /* Set CNTL1 reg to power model active */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AMI304_REG_CNTL1, + AMI304_BIT_CNTL1_PC1|AMI304_BIT_CNTL1_FS1); + ERROR_CHECK(result); + /* Set CNTL2 reg to DRDY active high and enabled */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AMI304_REG_CNTL2, + AMI304_BIT_CNTL2_DREN | + AMI304_BIT_CNTL2_DRP); + ERROR_CHECK(result); + /* Set CNTL3 reg to forced measurement period */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AMI304_REG_CNTL3, AMI304_BIT_CNTL3_F0RCE); + + return result; +} + +int ami304_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, unsigned char *data) +{ + unsigned char stat; + int result = ML_SUCCESS; + + /* Read status reg and check if data ready (DRDY) */ + result = + MLSLSerialRead(mlsl_handle, pdata->address, AMI304_REG_STAT1, + 1, &stat); + ERROR_CHECK(result); + + if (stat & 0x40) { + result = + MLSLSerialRead(mlsl_handle, pdata->address, + AMI304_REG_DATAX, 6, + (unsigned char *) data); + ERROR_CHECK(result); + /* start another measurement */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AMI304_REG_CNTL3, + AMI304_BIT_CNTL3_F0RCE); + ERROR_CHECK(result); + + return ML_SUCCESS; + } + + return ML_ERROR_COMPASS_DATA_NOT_READY; +} + +struct ext_slave_descr ami304_descr = { + /*.suspend = */ ami304_suspend, + /*.resume = */ ami304_resume, + /*.read = */ ami304_read, + /*.name = */ "ami304", + /*.type = */ EXT_SLAVE_TYPE_COMPASS, + /*.id = */ COMPASS_ID_AICHI, + /*.reg = */ 0x06, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_LITTLE_ENDIAN, + /*.range = */ {5461, 3333} +}; + +struct ext_slave_descr *ami304_get_slave_descr(void) +{ + return &ami304_descr; +} + +#ifdef __KERNEL__ +EXPORT_SYMBOL(ami304_get_slave_descr); +#endif + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/compass/ami30x.c b/drivers/misc/mpu3050/compass/ami30x.c new file mode 100755 index 00000000000..5e4a33efc7f --- /dev/null +++ b/drivers/misc/mpu3050/compass/ami30x.c @@ -0,0 +1,167 @@ +/* + $License: + Copyright (C) 2010 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 . + $ + */ + +/** + * @defgroup COMPASSDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file ami30x.c + * @brief Magnetometer setup and handling methods for Aichi AMI304/AMI305 + * compass. +*/ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-compass" + +#define AMI30X_REG_DATAX (0x10) +#define AMI30X_REG_STAT1 (0x18) +#define AMI30X_REG_CNTL1 (0x1B) +#define AMI30X_REG_CNTL2 (0x1C) +#define AMI30X_REG_CNTL3 (0x1D) + +#define AMI30X_BIT_CNTL1_PC1 (0x80) +#define AMI30X_BIT_CNTL1_ODR1 (0x10) +#define AMI30X_BIT_CNTL1_FS1 (0x02) + +#define AMI30X_BIT_CNTL2_IEN (0x10) +#define AMI30X_BIT_CNTL2_DREN (0x08) +#define AMI30X_BIT_CNTL2_DRP (0x04) +#define AMI30X_BIT_CNTL3_F0RCE (0x40) + +int ami30x_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result; + unsigned char reg; + result = + MLSLSerialRead(mlsl_handle, pdata->address, AMI30X_REG_CNTL1, + 1, ®); + ERROR_CHECK(result); + + reg &= ~(AMI30X_BIT_CNTL1_PC1|AMI30X_BIT_CNTL1_FS1); + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AMI30X_REG_CNTL1, reg); + ERROR_CHECK(result); + + return result; +} + +int ami30x_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + /* Set CNTL1 reg to power model active */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AMI30X_REG_CNTL1, + AMI30X_BIT_CNTL1_PC1|AMI30X_BIT_CNTL1_FS1); + ERROR_CHECK(result); + /* Set CNTL2 reg to DRDY active high and enabled */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AMI30X_REG_CNTL2, + AMI30X_BIT_CNTL2_DREN | + AMI30X_BIT_CNTL2_DRP); + ERROR_CHECK(result); + /* Set CNTL3 reg to forced measurement period */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AMI30X_REG_CNTL3, AMI30X_BIT_CNTL3_F0RCE); + + return result; +} + +int ami30x_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, unsigned char *data) +{ + unsigned char stat; + int result = ML_SUCCESS; + + /* Read status reg and check if data ready (DRDY) */ + result = + MLSLSerialRead(mlsl_handle, pdata->address, AMI30X_REG_STAT1, + 1, &stat); + ERROR_CHECK(result); + + if (stat & 0x40) { + result = + MLSLSerialRead(mlsl_handle, pdata->address, + AMI30X_REG_DATAX, 6, + (unsigned char *) data); + ERROR_CHECK(result); + /* start another measurement */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AMI30X_REG_CNTL3, + AMI30X_BIT_CNTL3_F0RCE); + ERROR_CHECK(result); + + return ML_SUCCESS; + } + + return ML_ERROR_COMPASS_DATA_NOT_READY; +} + +struct ext_slave_descr ami30x_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ ami30x_suspend, + /*.resume = */ ami30x_resume, + /*.read = */ ami30x_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "ami30x", + /*.type = */ EXT_SLAVE_TYPE_COMPASS, + /*.id = */ COMPASS_ID_AMI30X, + /*.reg = */ 0x06, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_LITTLE_ENDIAN, + /*.range = */ {5461, 3333} + /* For AMI305,the range field needs to be modified to {9830.4f}*/ +}; + +struct ext_slave_descr *ami30x_get_slave_descr(void) +{ + return &ami30x_descr; +} +EXPORT_SYMBOL(ami30x_get_slave_descr); + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/compass/hmc5883.c b/drivers/misc/mpu3050/compass/hmc5883.c new file mode 100755 index 00000000000..051b071fe7f --- /dev/null +++ b/drivers/misc/mpu3050/compass/hmc5883.c @@ -0,0 +1,254 @@ +/* + $License: + Copyright (C) 2010 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 . + $ + */ + +/** + * @brief Provides the interface to setup and handle a compass + * connected to the primary I2C interface of the gyroscope. + * + * @{ + * @file hmc5883.c + * @brief Magnetometer setup and handling methods for honeywell hmc5883 + * compass. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-compass" + +/*-----HONEYWELL HMC5883 Registers ------*/ +enum HMC_REG { + HMC_REG_CONF_A = 0x0, + HMC_REG_CONF_B = 0x1, + HMC_REG_MODE = 0x2, + HMC_REG_X_M = 0x3, + HMC_REG_X_L = 0x4, + HMC_REG_Z_M = 0x5, + HMC_REG_Z_L = 0x6, + HMC_REG_Y_M = 0x7, + HMC_REG_Y_L = 0x8, + HMC_REG_STATUS = 0x9, + HMC_REG_ID_A = 0xA, + HMC_REG_ID_B = 0xB, + HMC_REG_ID_C = 0xC +}; + +enum HMC_CONF_A { + HMC_CONF_A_DRATE_MASK = 0x1C, + HMC_CONF_A_DRATE_0_75 = 0x00, + HMC_CONF_A_DRATE_1_5 = 0x04, + HMC_CONF_A_DRATE_3 = 0x08, + HMC_CONF_A_DRATE_7_5 = 0x0C, + HMC_CONF_A_DRATE_15 = 0x10, + HMC_CONF_A_DRATE_30 = 0x14, + HMC_CONF_A_DRATE_75 = 0x18, + HMC_CONF_A_MEAS_MASK = 0x3, + HMC_CONF_A_MEAS_NORM = 0x0, + HMC_CONF_A_MEAS_POS = 0x1, + HMC_CONF_A_MEAS_NEG = 0x2 +}; + +enum HMC_CONF_B { + HMC_CONF_B_GAIN_MASK = 0xE0, + HMC_CONF_B_GAIN_0_9 = 0x00, + HMC_CONF_B_GAIN_1_2 = 0x20, + HMC_CONF_B_GAIN_1_9 = 0x40, + HMC_CONF_B_GAIN_2_5 = 0x60, + HMC_CONF_B_GAIN_4_0 = 0x80, + HMC_CONF_B_GAIN_4_6 = 0xA0, + HMC_CONF_B_GAIN_5_5 = 0xC0, + HMC_CONF_B_GAIN_7_9 = 0xE0 +}; + +enum HMC_MODE { + HMC_MODE_MASK = 0x3, + HMC_MODE_CONT = 0x0, + HMC_MODE_SINGLE = 0x1, + HMC_MODE_IDLE = 0x2, + HMC_MODE_SLEEP = 0x3 +}; + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +/***************************************** + Accelerometer Initialization Functions +*****************************************/ + +int hmc5883_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + HMC_REG_MODE, HMC_MODE_SLEEP); + ERROR_CHECK(result); + MLOSSleep(3); + + return result; +} + +int hmc5883_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + /* Use single measurement mode. Start at sleep state. */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + HMC_REG_MODE, HMC_MODE_SLEEP); + ERROR_CHECK(result); + /* Config normal measurement */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + HMC_REG_CONF_A, 0); + ERROR_CHECK(result); + /* Adjust gain to 307 LSB/Gauss */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + HMC_REG_CONF_B, HMC_CONF_B_GAIN_5_5); + ERROR_CHECK(result); + + return result; +} + +int hmc5883_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + unsigned char stat; + tMLError result = ML_SUCCESS; + unsigned char tmp; + short axisFixed; + + /* Read status reg. to check if data is ready */ + result = + MLSLSerialRead(mlsl_handle, pdata->address, HMC_REG_STATUS, 1, + &stat); + ERROR_CHECK(result); + if (stat & 0x01) { + result = + MLSLSerialRead(mlsl_handle, pdata->address, + HMC_REG_X_M, 6, (unsigned char *) data); + ERROR_CHECK(result); + + /* switch YZ axis to proper position */ + tmp = data[2]; + data[2] = data[4]; + data[4] = tmp; + tmp = data[3]; + data[3] = data[5]; + data[5] = tmp; + + /*drop data if overflows */ + if ((data[0] == 0xf0) || (data[2] == 0xf0) + || (data[4] == 0xf0)) { + /* trigger next measurement read */ + result = + MLSLSerialWriteSingle(mlsl_handle, + pdata->address, + HMC_REG_MODE, + HMC_MODE_SINGLE); + ERROR_CHECK(result); + return ML_ERROR_COMPASS_DATA_OVERFLOW; + } + /* convert to fixed point and apply sensitivity correction for + Z-axis */ + axisFixed = + (short) ((unsigned short) data[5] + + (unsigned short) data[4] * 256); + /* scale up by 1.125 (36/32) */ + axisFixed = (short) (axisFixed * 36); + data[4] = axisFixed >> 8; + data[5] = axisFixed & 0xFF; + + axisFixed = + (short) ((unsigned short) data[3] + + (unsigned short) data[2] * 256); + axisFixed = (short) (axisFixed * 32); + data[2] = axisFixed >> 8; + data[3] = axisFixed & 0xFF; + + axisFixed = + (short) ((unsigned short) data[1] + + (unsigned short) data[0] * 256); + axisFixed = (short) (axisFixed * 32); + data[0] = axisFixed >> 8; + data[1] = axisFixed & 0xFF; + + /* trigger next measurement read */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + HMC_REG_MODE, HMC_MODE_SINGLE); + ERROR_CHECK(result); + + return ML_SUCCESS; + } else { + /* trigger next measurement read */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + HMC_REG_MODE, HMC_MODE_SINGLE); + ERROR_CHECK(result); + + return ML_ERROR_COMPASS_DATA_NOT_READY; + } +} + +struct ext_slave_descr hmc5883_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ hmc5883_suspend, + /*.resume = */ hmc5883_resume, + /*.read = */ hmc5883_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "hmc5883", + /*.type = */ EXT_SLAVE_TYPE_COMPASS, + /*.id = */ COMPASS_ID_HMC5883, + /*.reg = */ 0x06, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_BIG_ENDIAN, + /*.range = */ {10673, 6156}, +}; + +struct ext_slave_descr *hmc5883_get_slave_descr(void) +{ + return &hmc5883_descr; +} +EXPORT_SYMBOL(hmc5883_get_slave_descr); + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/compass/hscdtd002b.c b/drivers/misc/mpu3050/compass/hscdtd002b.c new file mode 100755 index 00000000000..bf26cae06d2 --- /dev/null +++ b/drivers/misc/mpu3050/compass/hscdtd002b.c @@ -0,0 +1,163 @@ +/* + $License: + Copyright (C) 2010 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 . + $ + */ + +/** + * @brief Provides the interface to setup and handle a compass + * connected to the primary I2C interface of the gyroscope. + * + * @{ + * @file hscdtd002b.c + * @brief Magnetometer setup and handling methods for Alps hscdtd002b + * compass. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-compass" + +/*----- ALPS HSCDTD002B Registers ------*/ +#define COMPASS_HSCDTD002B_STAT (0x18) +#define COMPASS_HSCDTD002B_CTRL1 (0x1B) +#define COMPASS_HSCDTD002B_CTRL2 (0x1C) +#define COMPASS_HSCDTD002B_CTRL3 (0x1D) +#define COMPASS_HSCDTD002B_DATAX (0x10) + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +/***************************************** + Compass Initialization Functions +*****************************************/ + +int hscdtd002b_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + /* Power mode: stand-by */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + COMPASS_HSCDTD002B_CTRL1, 0x00); + ERROR_CHECK(result); + MLOSSleep(1); /* turn-off time */ + + return result; +} + +int hscdtd002b_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + /* Soft reset */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + COMPASS_HSCDTD002B_CTRL3, 0x80); + ERROR_CHECK(result); + /* Force state; Power mode: active */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + COMPASS_HSCDTD002B_CTRL1, 0x82); + ERROR_CHECK(result); + /* Data ready enable */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + COMPASS_HSCDTD002B_CTRL2, 0x08); + ERROR_CHECK(result); + MLOSSleep(1); /* turn-on time */ + + return result; +} + +int hscdtd002b_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + unsigned char stat; + tMLError result = ML_SUCCESS; + int status = ML_SUCCESS; + + /* Read status reg. to check if data is ready */ + result = + MLSLSerialRead(mlsl_handle, pdata->address, + COMPASS_HSCDTD002B_STAT, 1, &stat); + ERROR_CHECK(result); + if (stat & 0x40) { + result = + MLSLSerialRead(mlsl_handle, pdata->address, + COMPASS_HSCDTD002B_DATAX, 6, + (unsigned char *) data); + ERROR_CHECK(result); + status = ML_SUCCESS; + } else if (stat & 0x20) { + status = ML_ERROR_COMPASS_DATA_OVERFLOW; + } else { + status = ML_ERROR_COMPASS_DATA_NOT_READY; + } + /* trigger next measurement read */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + COMPASS_HSCDTD002B_CTRL3, 0x40); + ERROR_CHECK(result); + + return status; +} + +struct ext_slave_descr hscdtd002b_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ hscdtd002b_suspend, + /*.resume = */ hscdtd002b_resume, + /*.read = */ hscdtd002b_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "hscdtd002b", + /*.type = */ EXT_SLAVE_TYPE_COMPASS, + /*.id = */ COMPASS_ID_HSCDTD002B, + /*.reg = */ 0x10, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_LITTLE_ENDIAN, + /*.range = */ {9830, 4000}, +}; + +struct ext_slave_descr *hscdtd002b_get_slave_descr(void) +{ + return &hscdtd002b_descr; +} +EXPORT_SYMBOL(hscdtd002b_get_slave_descr); + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/compass/hscdtd004a.c b/drivers/misc/mpu3050/compass/hscdtd004a.c new file mode 100755 index 00000000000..43fc14a23fc --- /dev/null +++ b/drivers/misc/mpu3050/compass/hscdtd004a.c @@ -0,0 +1,162 @@ +/* + $License: + Copyright (C) 2010 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 . + $ + */ + +/** + * @brief Provides the interface to setup and handle a compass + * connected to the primary I2C interface of the gyroscope. + * + * @{ + * @file hscdtd004a.c + * @brief Magnetometer setup and handling methods for Alps hscdtd004a + * compass. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-compass" + +/*----- ALPS HSCDTD004A Registers ------*/ +#define COMPASS_HSCDTD004A_STAT (0x18) +#define COMPASS_HSCDTD004A_CTRL1 (0x1B) +#define COMPASS_HSCDTD004A_CTRL2 (0x1C) +#define COMPASS_HSCDTD004A_CTRL3 (0x1D) +#define COMPASS_HSCDTD004A_DATAX (0x10) + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +/***************************************** + Compass Initialization Functions +*****************************************/ + +int hscdtd004a_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + /* Power mode: stand-by */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + COMPASS_HSCDTD004A_CTRL1, 0x00); + ERROR_CHECK(result); + MLOSSleep(1); /* turn-off time */ + + return result; +} + +int hscdtd004a_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + /* Soft reset */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + COMPASS_HSCDTD004A_CTRL3, 0x80); + ERROR_CHECK(result); + /* Normal state; Power mode: active */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + COMPASS_HSCDTD004A_CTRL1, 0x82); + ERROR_CHECK(result); + /* Data ready enable */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + COMPASS_HSCDTD004A_CTRL2, 0x7C); + ERROR_CHECK(result); + MLOSSleep(1); /* turn-on time */ + return result; +} + +int hscdtd004a_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + unsigned char stat; + tMLError result = ML_SUCCESS; + int status = ML_SUCCESS; + + /* Read status reg. to check if data is ready */ + result = + MLSLSerialRead(mlsl_handle, pdata->address, + COMPASS_HSCDTD004A_STAT, 1, &stat); + ERROR_CHECK(result); + if (stat & 0x48) { + result = + MLSLSerialRead(mlsl_handle, pdata->address, + COMPASS_HSCDTD004A_DATAX, 6, + (unsigned char *) data); + ERROR_CHECK(result); + status = ML_SUCCESS; + } else if (stat & 0x68) { + status = ML_ERROR_COMPASS_DATA_OVERFLOW; + } else { + status = ML_ERROR_COMPASS_DATA_NOT_READY; + } + /* trigger next measurement read */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + COMPASS_HSCDTD004A_CTRL3, 0x40); + ERROR_CHECK(result); + return status; + +} + +struct ext_slave_descr hscdtd004a_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ hscdtd004a_suspend, + /*.resume = */ hscdtd004a_resume, + /*.read = */ hscdtd004a_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "hscdtd004a", + /*.type = */ EXT_SLAVE_TYPE_COMPASS, + /*.id = */ COMPASS_ID_HSCDTD004A, + /*.reg = */ 0x10, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_LITTLE_ENDIAN, + /*.range = */ {9830, 4000}, +}; + +struct ext_slave_descr *hscdtd004a_get_slave_descr(void) +{ + return &hscdtd004a_descr; +} +EXPORT_SYMBOL(hscdtd004a_get_slave_descr); + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/compass/lsm303m.c b/drivers/misc/mpu3050/compass/lsm303m.c new file mode 100755 index 00000000000..871d002fd36 --- /dev/null +++ b/drivers/misc/mpu3050/compass/lsm303m.c @@ -0,0 +1,244 @@ +/* + $License: + Copyright (C) 2010 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 . + $ + */ + +/** + * @brief Provides the interface to setup and handle a compass + * connected to the primary I2C interface of the gyroscope. + * + * @{ + * @file lsm303m.c + * @brief Magnetometer setup and handling methods for ST LSM303. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-compass" + +/*----- ST LSM303 Registers ------*/ +enum LSM_REG { + LSM_REG_CONF_A = 0x0, + LSM_REG_CONF_B = 0x1, + LSM_REG_MODE = 0x2, + LSM_REG_X_M = 0x3, + LSM_REG_X_L = 0x4, + LSM_REG_Z_M = 0x5, + LSM_REG_Z_L = 0x6, + LSM_REG_Y_M = 0x7, + LSM_REG_Y_L = 0x8, + LSM_REG_STATUS = 0x9, + LSM_REG_ID_A = 0xA, + LSM_REG_ID_B = 0xB, + LSM_REG_ID_C = 0xC +}; + +enum LSM_CONF_A { + LSM_CONF_A_DRATE_MASK = 0x1C, + LSM_CONF_A_DRATE_0_75 = 0x00, + LSM_CONF_A_DRATE_1_5 = 0x04, + LSM_CONF_A_DRATE_3 = 0x08, + LSM_CONF_A_DRATE_7_5 = 0x0C, + LSM_CONF_A_DRATE_15 = 0x10, + LSM_CONF_A_DRATE_30 = 0x14, + LSM_CONF_A_DRATE_75 = 0x18, + LSM_CONF_A_MEAS_MASK = 0x3, + LSM_CONF_A_MEAS_NORM = 0x0, + LSM_CONF_A_MEAS_POS = 0x1, + LSM_CONF_A_MEAS_NEG = 0x2 +}; + +enum LSM_CONF_B { + LSM_CONF_B_GAIN_MASK = 0xE0, + LSM_CONF_B_GAIN_0_9 = 0x00, + LSM_CONF_B_GAIN_1_2 = 0x20, + LSM_CONF_B_GAIN_1_9 = 0x40, + LSM_CONF_B_GAIN_2_5 = 0x60, + LSM_CONF_B_GAIN_4_0 = 0x80, + LSM_CONF_B_GAIN_4_6 = 0xA0, + LSM_CONF_B_GAIN_5_5 = 0xC0, + LSM_CONF_B_GAIN_7_9 = 0xE0 +}; + +enum LSM_MODE { + LSM_MODE_MASK = 0x3, + LSM_MODE_CONT = 0x0, + LSM_MODE_SINGLE = 0x1, + LSM_MODE_IDLE = 0x2, + LSM_MODE_SLEEP = 0x3 +}; + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +/***************************************** + Accelerometer Initialization Functions +*****************************************/ + +int lsm303dlhm_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LSM_REG_MODE, LSM_MODE_SLEEP); + ERROR_CHECK(result); + MLOSSleep(3); + + return result; +} + +int lsm303dlhm_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + /* Use single measurement mode. Start at sleep state. */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LSM_REG_MODE, LSM_MODE_SLEEP); + ERROR_CHECK(result); + /* Config normal measurement */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LSM_REG_CONF_A, 0); + ERROR_CHECK(result); + /* Adjust gain to 320 LSB/Gauss */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LSM_REG_CONF_B, LSM_CONF_B_GAIN_5_5); + ERROR_CHECK(result); + + return result; +} + +int lsm303dlhm_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + unsigned char stat; + tMLError result = ML_SUCCESS; + short axisFixed; + + /* Read status reg. to check if data is ready */ + result = + MLSLSerialRead(mlsl_handle, pdata->address, LSM_REG_STATUS, 1, + &stat); + ERROR_CHECK(result); + if (stat & 0x01) { + result = + MLSLSerialRead(mlsl_handle, pdata->address, + LSM_REG_X_M, 6, (unsigned char *) data); + ERROR_CHECK(result); + + /*drop data if overflows */ + if ((data[0] == 0xf0) || (data[2] == 0xf0) + || (data[4] == 0xf0)) { + /* trigger next measurement read */ + result = + MLSLSerialWriteSingle(mlsl_handle, + pdata->address, + LSM_REG_MODE, + LSM_MODE_SINGLE); + ERROR_CHECK(result); + return ML_ERROR_COMPASS_DATA_OVERFLOW; + } + /* convert to fixed point and apply sensitivity correction for + Z-axis */ + axisFixed = + (short) ((unsigned short) data[5] + + (unsigned short) data[4] * 256); + /* scale up by 1.125 (36/32) approximate of 1.122 (320/285) */ + axisFixed = (short) (axisFixed * 36); + data[4] = axisFixed >> 8; + data[5] = axisFixed & 0xFF; + + axisFixed = + (short) ((unsigned short) data[3] + + (unsigned short) data[2] * 256); + axisFixed = (short) (axisFixed * 32); + data[2] = axisFixed >> 8; + data[3] = axisFixed & 0xFF; + + axisFixed = + (short) ((unsigned short) data[1] + + (unsigned short) data[0] * 256); + axisFixed = (short) (axisFixed * 32); + data[0] = axisFixed >> 8; + data[1] = axisFixed & 0xFF; + + /* trigger next measurement read */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LSM_REG_MODE, LSM_MODE_SINGLE); + ERROR_CHECK(result); + + return ML_SUCCESS; + } else { + /* trigger next measurement read */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LSM_REG_MODE, LSM_MODE_SINGLE); + ERROR_CHECK(result); + + return ML_ERROR_COMPASS_DATA_NOT_READY; + } +} + +struct ext_slave_descr lsm303dlhm_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ lsm303dlhm_suspend, + /*.resume = */ lsm303dlhm_resume, + /*.read = */ lsm303dlhm_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "lsm303dlhm", + /*.type = */ EXT_SLAVE_TYPE_COMPASS, + /*.id = */ COMPASS_ID_LSM303, + /*.reg = */ 0x06, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_BIG_ENDIAN, + /*.range = */ {10240, 0}, +}; + +struct ext_slave_descr *lsm303dlhm_get_slave_descr(void) +{ + return &lsm303dlhm_descr; +} +EXPORT_SYMBOL(lsm303dlhm_get_slave_descr); + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/compass/mmc314x.c b/drivers/misc/mpu3050/compass/mmc314x.c new file mode 100755 index 00000000000..010d7a78d14 --- /dev/null +++ b/drivers/misc/mpu3050/compass/mmc314x.c @@ -0,0 +1,184 @@ +/* + $License: + Copyright (C) 2010 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 . + $ + */ + +/** + * @defgroup ACCELDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file mmc314x.c + * @brief Magnetometer setup and handling methods for ???? compass. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-compass" + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +static int reset_int = 1000; +static int read_count = 1; +static char reset_mode; /* in Z-init section */ + +#define MMC314X_REG_ST (0x00) +#define MMC314X_REG_X_MSB (0x01) + +#define MMC314X_CNTL_MODE_WAKE_UP (0x01) +#define MMC314X_CNTL_MODE_SET (0x02) +#define MMC314X_CNTL_MODE_RESET (0x04) + +/***************************************** + Accelerometer Initialization Functions +*****************************************/ + +int mmc314x_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + return result; +} + +int mmc314x_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + + int result; + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + MMC314X_REG_ST, MMC314X_CNTL_MODE_RESET); + ERROR_CHECK(result); + MLOSSleep(10); + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + MMC314X_REG_ST, MMC314X_CNTL_MODE_SET); + ERROR_CHECK(result); + MLOSSleep(10); + read_count = 1; + return ML_SUCCESS; +} + +int mmc314x_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + int result, ii; + short tmp[3]; + unsigned char tmpdata[6]; + + + if (read_count > 1000) + read_count = 1; + + result = + MLSLSerialRead(mlsl_handle, pdata->address, MMC314X_REG_X_MSB, + 6, (unsigned char *) data); + ERROR_CHECK(result); + + for (ii = 0; ii < 6; ii++) + tmpdata[ii] = data[ii]; + + for (ii = 0; ii < 3; ii++) { + tmp[ii] = + (short) ((tmpdata[2 * ii] << 8) + tmpdata[2 * ii + 1]); + tmp[ii] = tmp[ii] - 4096; + tmp[ii] = tmp[ii] * 16; + } + + for (ii = 0; ii < 3; ii++) { + data[2 * ii] = (unsigned char) (tmp[ii] >> 8); + data[2 * ii + 1] = (unsigned char) (tmp[ii]); + } + + if (read_count % reset_int == 0) { + if (reset_mode) { + result = + MLSLSerialWriteSingle(mlsl_handle, + pdata->address, + MMC314X_REG_ST, + MMC314X_CNTL_MODE_RESET); + ERROR_CHECK(result); + reset_mode = 0; + return ML_ERROR_COMPASS_DATA_NOT_READY; + } else { + result = + MLSLSerialWriteSingle(mlsl_handle, + pdata->address, + MMC314X_REG_ST, + MMC314X_CNTL_MODE_SET); + ERROR_CHECK(result); + reset_mode = 1; + read_count++; + return ML_ERROR_COMPASS_DATA_NOT_READY; + } + } + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + MMC314X_REG_ST, + MMC314X_CNTL_MODE_WAKE_UP); + ERROR_CHECK(result); + read_count++; + + return ML_SUCCESS; +} + +struct ext_slave_descr mmc314x_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ mmc314x_suspend, + /*.resume = */ mmc314x_resume, + /*.read = */ mmc314x_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "mmc314x", + /*.type = */ EXT_SLAVE_TYPE_COMPASS, + /*.id = */ COMPASS_ID_MMC314X, + /*.reg = */ 0x01, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_BIG_ENDIAN, + /*.range = */ {400, 0}, +}; + +struct ext_slave_descr *mmc314x_get_slave_descr(void) +{ + return &mmc314x_descr; +} +EXPORT_SYMBOL(mmc314x_get_slave_descr); + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/compass/mmc328x.c b/drivers/misc/mpu3050/compass/mmc328x.c new file mode 100755 index 00000000000..d2914f3ef62 --- /dev/null +++ b/drivers/misc/mpu3050/compass/mmc328x.c @@ -0,0 +1,220 @@ +/* + * Copyright (C) 2010 MEMSIC, Inc. + * + * 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, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +#include + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include + +#define MMC328X_I2C_ADDR 0x30 + +#define MMC328X_REG_CTRL 0x07 +#define MMC328X_REG_DATA 0x00 +#define MMC328X_REG_DS 0x06 + +#define MMC328X_CTRL_TM 0x01 +#define MMC328X_CTRL_RM 0x20 + +#define MMC328X_DELAY_TM 10 /* ms */ +#define MMC328X_DELAY_RM 10 /* ms */ +#define MMC328X_DELAY_STDN 1 /* ms */ + +static int mmc328x_i2c_rx_data(void *mlsl_handle, + struct ext_slave_platform_data *pdata, char *buf, int len) +{ + uint8_t i; + struct i2c_msg msgs[2] = { 0, }; + + if (NULL == buf || NULL == mlsl_handle) + return -EINVAL; + + msgs[0].addr = pdata->address; + msgs[0].flags = 0; + msgs[0].len = 1; + msgs[0].buf = buf; + + msgs[1].addr = pdata->address; + msgs[1].flags = I2C_M_RD; + msgs[1].len = len; + msgs[1].buf = buf; + + if (i2c_transfer(mlsl_handle, msgs, 2) >= 0) + return -1; + + return 0; +} + +static int mmc328x_i2c_tx_data(void *mlsl_handle, + struct ext_slave_platform_data *pdata, char *buf, int len) +{ + struct i2c_msg msgs[1]; + int res; + + if (NULL == buf || NULL == mlsl_handle) + return -EINVAL; + + msgs[0].addr = pdata->address; + msgs[0].flags = 0; /* write */ + msgs[0].buf = (unsigned char *)buf; + msgs[0].len = len; + + res = i2c_transfer(mlsl_handle, msgs, 1); + if (res < 1) + return res; + else + return 0; +} + +int memsic_api_resume(void *mlsl_handle, + struct ext_slave_platform_data *pdata) +{ +#if 0 + unsigned char data[2] = { 0 }; + + data[0] = MMC328X_REG_CTRL; + data[1] = MMC328X_CTRL_RM; + if (mmc328x_i2c_tx_data(mlsl_handle, pdata, data, 2) < 0) + return -1; + + msleep(MMC328X_DELAY_RM); +#endif + return 0; +} + +int memsic_api_suspend(void *mlsl_handle, + struct ext_slave_platform_data *pdata) +{ +#if 0 + unsigned char data[2] = { 0 }; + + data[0] = MMC328X_REG_CTRL; + data[1] = 0; + if (mmc328x_i2c_tx_data(mlsl_handle, pdata, data, 2) < 0) + return -1; + msleep(MMC328X_DELAY_RM); + + data[0] = MMC328X_REG_CTRL; + data[1] = 0; + mmc328x_i2c_tx_data(mlsl_handle, pdata, data, 2); + msleep(MMC328X_DELAY_TM); +#endif + return 0; +} + +int memsic_api_read(void *mlsl_handle, struct ext_slave_platform_data *pdata, + unsigned char *raw_data, int *accuracy) +{ + unsigned char data[6] = { 0 }; + int MD_times = 0; + + data[0] = MMC328X_REG_CTRL; + data[1] = MMC328X_CTRL_TM; + mmc328x_i2c_tx_data(mlsl_handle, pdata, data, 2); + msleep(MMC328X_DELAY_TM); + + data[0] = MMC328X_REG_DS; + if (mmc328x_i2c_rx_data(mlsl_handle, pdata, data, 1) < 0) + return -EFAULT; + + while (!(data[0] & 0x01)) { + msleep(20); + data[0] = MMC328X_REG_DS; + + if (mmc328x_i2c_rx_data(mlsl_handle, pdata, data, 1) < 0) + return -EFAULT; + + if (data[0] & 0x01) + break; + + MD_times++; + + if (MD_times > 2) + return -EFAULT; + } + + data[0] = MMC328X_REG_DATA; + if (mmc328x_i2c_rx_data(mlsl_handle, pdata, data, 6) < 0) + return -EFAULT; + + memcpy(raw_data, data, sizeof(unsigned char) * 6); + + *accuracy = 0; + + return 0; +} + +int mmc328x_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, unsigned char *data) +{ + int xyz[3] = { 0, }; + int accuracy = 0; + + memsic_api_read(mlsl_handle, pdata, data, &accuracy); + + return ML_SUCCESS; +} + +int mmc328x_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + memsic_api_suspend(mlsl_handle, pdata); + return result; +} + +int mmc328x_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + + memsic_api_resume(mlsl_handle, pdata); + return ML_SUCCESS; +} + +struct ext_slave_descr mmc328x_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ mmc328x_suspend, + /*.resume = */ mmc328x_resume, + /*.read = */ mmc328x_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "mmc328x", + /*.type = */ EXT_SLAVE_TYPE_COMPASS, + /*.id = */ COMPASS_ID_MMC328X, + /*.reg = */ 0x01, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_BIG_ENDIAN, + /*.range = */ {400, 0}, +}; + +struct ext_slave_descr *mmc328x_get_slave_descr(void) +{ + return &mmc328x_descr; +} +EXPORT_SYMBOL(mmc328x_get_slave_descr); + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/compass/mpuak8975.c b/drivers/misc/mpu3050/compass/mpuak8975.c new file mode 100755 index 00000000000..991de77dbd0 --- /dev/null +++ b/drivers/misc/mpu3050/compass/mpuak8975.c @@ -0,0 +1,188 @@ +/* + $License: + Copyright (C) 2010 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 . + $ + */ + +/** + * @defgroup COMPASSDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file AK8975.c + * @brief Magnetometer setup and handling methods for AKM 8975 compass. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#include + +#ifdef __KERNEL__ +#include +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-compass" + + +#define AK8975_REG_ST1 (0x02) +#define AK8975_REG_HXL (0x03) +#define AK8975_REG_ST2 (0x09) + +#define AK8975_REG_CNTL (0x0A) + +#define AK8975_CNTL_MODE_POWER_DOWN (0x00) +#define AK8975_CNTL_MODE_SINGLE_MEASUREMENT (0x01) + +int ak8975_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AK8975_REG_CNTL, + AK8975_CNTL_MODE_POWER_DOWN); + MLOSSleep(1); /* wait at least 100us */ + ERROR_CHECK(result); + return result; +} + +int ak8975_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AK8975_REG_CNTL, + AK8975_CNTL_MODE_SINGLE_MEASUREMENT); + ERROR_CHECK(result); + return result; +} + +int ak8975_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, unsigned char *data) +{ + unsigned char regs[8]; + unsigned char *stat = ®s[0]; + unsigned char *stat2 = ®s[7]; + int result = ML_SUCCESS; + int status = ML_SUCCESS; + + result = + MLSLSerialRead(mlsl_handle, pdata->address, AK8975_REG_ST1, + 8, regs); + ERROR_CHECK(result); + + /* + * ST : data ready - + * Measurement has been completed and data is ready to be read. + */ + if (*stat & 0x01) { + memcpy(data, ®s[1], 6); + status = ML_SUCCESS; + } + + /* + * ST2 : data error - + * occurs when data read is started outside of a readable period; + * data read would not be correct. + * Valid in continuous measurement mode only. + * In single measurement mode this error should not occour but we + * stil account for it and return an error, since the data would be + * corrupted. + * DERR bit is self-clearing when ST2 register is read. + */ + if (*stat2 & 0x04) + status = ML_ERROR_COMPASS_DATA_ERROR; + /* + * ST2 : overflow - + * the sum of the absolute values of all axis |X|+|Y|+|Z| < 2400uT. + * This is likely to happen in presence of an external magnetic + * disturbance; it indicates, the sensor data is incorrect and should + * be ignored. + * An error is returned. + * HOFL bit clears when a new measurement starts. + */ + if (*stat2 & 0x08) + status = ML_ERROR_COMPASS_DATA_OVERFLOW; + /* + * ST : overrun - + * the previous sample was not fetched and lost. + * Valid in continuous measurement mode only. + * In single measurement mode this error should not occour and we + * don't consider this condition an error. + * DOR bit is self-clearing when ST2 or any meas. data register is + * read. + */ + if (*stat & 0x02) { + /* status = ML_ERROR_COMPASS_DATA_UNDERFLOW; */ + status = ML_SUCCESS; + } + + /* + * trigger next measurement if: + * - stat is non zero; + * - if stat is zero and stat2 is non zero. + * Won't trigger if data is not ready and there was no error. + */ + if (*stat != 0x00 || *stat2 != 0x00) { + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AK8975_REG_CNTL, + AK8975_CNTL_MODE_SINGLE_MEASUREMENT); + ERROR_CHECK(result); + } + + return status; +} + +struct ext_slave_descr ak8975_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ ak8975_suspend, + /*.resume = */ ak8975_resume, + /*.read = */ ak8975_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "ak8975", + /*.type = */ EXT_SLAVE_TYPE_COMPASS, + /*.id = */ COMPASS_ID_AKM, + /*.reg = */ 0x01, + /*.len = */ 9, + /*.endian = */ EXT_SLAVE_LITTLE_ENDIAN, + /*.range = */ {9830, 4000} +}; + +struct ext_slave_descr *ak8975_get_slave_descr(void) +{ + return &ak8975_descr; +} +EXPORT_SYMBOL(ak8975_get_slave_descr); + +/** + * @} + */ diff --git a/drivers/misc/mpu3050/compass/yas529-kernel.c b/drivers/misc/mpu3050/compass/yas529-kernel.c new file mode 100755 index 00000000000..239ab669cc1 --- /dev/null +++ b/drivers/misc/mpu3050/compass/yas529-kernel.c @@ -0,0 +1,477 @@ +/* + $License: + Copyright (C) 2010 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 . + $ + */ +/** + * @defgroup ACCELDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file yas529.c + * @brief Magnetometer setup and handling methods for Yamaha yas529 + * compass. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include +#include +#endif + +#include "mpu.h" +#include "mlos.h" + +#include +#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 */ +}; + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +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; + +/***************************************** + Yamaha I2C access functions +*****************************************/ + +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; +} + +/***************************************** + Accelerometer Initialization Functions +*****************************************/ + +int yas529_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + return result; +} + +int yas529_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_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); + ERROR_CHECK(result); + /* zero config register - "110 00 000" */ + dummyData[0] = YAS_REG_CONFR | 0x00; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + + /* Step 2 - initialization coil operation */ + dummyData[0] = YAS_REG_ICOILR | 0x11; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x01; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x12; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x02; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x13; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x03; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x14; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x04; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x15; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x05; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x16; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x06; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x17; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x07; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x10; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x00; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(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); + ERROR_CHECK(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); + ERROR_CHECK(result); + MLOSSleep(2); /* wait at least 1.5ms */ + + /* Measurement data read */ + result = + yas529_sensor_i2c_read(mlsl_handle, pdata->address, + dummyRegister, 6, rawData); + ERROR_CHECK(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); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_Y1OFFSETR | y1offset; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_Y2OFFSETR | y2offset; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(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); + ERROR_CHECK(result); + /* CAL data read */ + result = + yas529_sensor_i2c_read(mlsl_handle, pdata->address, + dummyRegister, 9, calData); + ERROR_CHECK(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); + ERROR_CHECK(result); + /* CAL data read */ + result = + yas529_sensor_i2c_read(mlsl_handle, pdata->address, + dummyRegister, 9, calData); + ERROR_CHECK(result); + + /* Calculate coefficients of the sensitivity corrcetion matrix */ +#if 1 /* production sensor */ + 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; +#else /* evaluation sensor */ + a1 = 1.0f; + /* [71..66] 6bit */ + d2 = (calData[0] & 0xFC) >> 2; + a2 = (short) d2; + /* [65..60] 6bit */ + d3 = ((calData[0] & 0x03) << 4) | ((calData[1] & 0xF0) >> 4); + a3 = (short) d3; + /* [59..54] 6bit */ + d4 = ((calData[1] & 0x0F) << 2) | ((calData[2] & 0xC0) >> 6); + a4 = (short) d4; + /* [53..48] 6bit */ + d5 = (calData[2] & 0x3F); + a5 = (short) (d5 + 70); + /* [47..42] 6bit */ + d6 = ((calData[3] & 0xFC) >> 2); + a6 = (short) d6; + /* [41..36] 6bit */ + d7 = ((calData[3] & 0x03) << 4) | ((calData[4] & 0xF0) >> 4); + a7 = (short) d7; + /* [35..30] 6bit */ + d8 = ((calData[4] & 0x0F) << 2) | ((calData[5] & 0xC0) >> 6); + a8 = (short) d8; + /* [29..24] 6bit */ + d9 = (calData[5] & 0x3F); + a9 = (short) (d9 + 150); +#endif + + return result; +} + +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; + tMLError result = ML_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); + ERROR_CHECK(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); + ERROR_CHECK(result); + MLOSSleep(10); + /* Measurement data read */ + result = + yas529_sensor_i2c_read(mlsl_handle, pdata->address, + dummyRegister, 6, + (unsigned char *) &rawData); + ERROR_CHECK(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 ML_ERROR_COMPASS_DATA_UNDERFLOW; + if ((SX >= 1024) || (SY1 >= 1024) || (SY2 >= 1024)) + return ML_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 ML_SUCCESS; + } else { + return ML_ERROR_COMPASS_DATA_NOT_READY; + } +} + +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, + /*.reg = */ 0x06, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_BIG_ENDIAN, + /*.range = */ {19660, 8000}, +}; + +struct ext_slave_descr *yas529_get_slave_descr(void) +{ + return &yas529_descr; +} +EXPORT_SYMBOL(yas529_get_slave_descr); + +/** + * @} + */ -- cgit v1.2.3