static int lsm330dlc_gyro_report_values\
(struct lsm330dlc_gyro_data *gyro_data)
{
int res;
struct gyro_t data;
res = lsm330dlc_gyro_read_values(gyro_data->client, &data,
gyro_data->entries);
if (res < 0)
return res;
data.x -= gyro_data->cal_data.x;
data.y -= gyro_data->cal_data.y;
data.z -= gyro_data->cal_data.z;
input_report_rel(gyro_data->input_dev, REL_RX, data.x);
input_report_rel(gyro_data->input_dev, REL_RY, data.y);
input_report_rel(gyro_data->input_dev, REL_RZ, data.z);
input_sync(gyro_data->input_dev);
lsm330dlc_gyro_restart_fifo(gyro_data);
#ifdef LOGGING_GYRO
printk(KERN_INFO "%s, x = %d, y = %d, z = %d, count = %d\n"
, __func__, data.x, data.y, data.z, gyro_data->count);
#endif
return res;
}
static void lsm330dlc_gyro_work_func(struct work_struct *work)
{
int res, retry = 3, i, j;
struct lsm330dlc_gyro_data *data\
= container_of(work, struct lsm330dlc_gyro_data, work);
s32 status = 0;
s16 raw[3] = {0,}, gyro_adjusted[3] = {0,};
do {
status = i2c_smbus_read_byte_data(data->client,
STATUS_REG);
if (status & 0x08)
break;
} while (retry--);
if (status & 0x08 && data->self_test == false) {
res = lsm330dlc_gyro_read_values(data->client,
&data->xyz_data, 0);
if (res < 0)
pr_err("%s, reading data fail(res = %d)\n",
__func__, res);
if (data->dps == DPS500) {
data->xyz_data.x -= data->cal_data.x;
data->xyz_data.y -= data->cal_data.y;
data->xyz_data.z -= data->cal_data.z;
}
} else {
pr_warn("%s, use last data(%d, %d, %d), status=%d, selftest=%d\n",
__func__, data->xyz_data.x, data->xyz_data.y,
data->xyz_data.z, status, data->self_test);
}
if (data->axis_adjust) {
raw[0] = data->xyz_data.x;
raw[1] = data->xyz_data.y;
raw[2] = data->xyz_data.z;
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++)
gyro_adjusted[i]
+= position_map[data->position][i][j] * raw[j];
}
} else {
gyro_adjusted[0] = data->xyz_data.x;
gyro_adjusted[1] = data->xyz_data.y;
gyro_adjusted[2] = data->xyz_data.z;
}
input_report_rel(data->input_dev, REL_RX, gyro_adjusted[0]);
input_report_rel(data->input_dev, REL_RY, gyro_adjusted[1]);
input_report_rel(data->input_dev, REL_RZ, gyro_adjusted[2]);
input_sync(data->input_dev);
#ifdef LOGGING_GYRO
pr_info("%s, x = %d, y = %d, z = %d\n"
, __func__, gyro_adjusted[0], gyro_adjusted[1],
gyro_adjusted[2]);
#endif
}
static int lsm330dlc_gyro_report_values\
(struct lsm330dlc_gyro_data *data)
{
int res, i, j;
s16 raw[3] = {0,}, gyro_adjusted[3] = {0,};
res = lsm330dlc_gyro_read_values(data->client,
&data->xyz_data, data->entries);
if (res < 0)
return res;
data->xyz_data.x -= data->cal_data.x;
data->xyz_data.y -= data->cal_data.y;
data->xyz_data.z -= data->cal_data.z;
if (data->axis_adjust) {
raw[0] = data->xyz_data.x;
raw[1] = data->xyz_data.y;
raw[2] = data->xyz_data.z;
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++)
gyro_adjusted[i]
+= position_map[data->position][i][j] * raw[j];
}
} else {
gyro_adjusted[0] = data->xyz_data.x;
gyro_adjusted[1] = data->xyz_data.y;
gyro_adjusted[2] = data->xyz_data.z;
}
input_report_rel(data->input_dev, REL_RX, gyro_adjusted[0]);
input_report_rel(data->input_dev, REL_RY, gyro_adjusted[1]);
input_report_rel(data->input_dev, REL_RZ, gyro_adjusted[2]);
input_sync(data->input_dev);
lsm330dlc_gyro_restart_fifo(data);
#ifdef LOGGING_GYRO
pr_info("%s, x = %d, y = %d, z = %d\n"
, __func__, gyro_adjusted[0], gyro_adjusted[1],
gyro_adjusted[2]);
#endif
return res;
}
static void lsm330dlc_gyro_work_func(struct work_struct *work)
{
int res, retry = 3;
struct lsm330dlc_gyro_data *gyro_data\
= container_of(work, struct lsm330dlc_gyro_data, work);
s32 status = 0;
do {
status = i2c_smbus_read_byte_data(gyro_data->client,
STATUS_REG);
if (status & 0x08)
break;
} while (retry--);
if (status & 0x08 && gyro_data->self_test == false) {
res = lsm330dlc_gyro_read_values(gyro_data->client,
&gyro_data->xyz_data, 0);
if (res < 0)
pr_err("%s, reading data fail(res = %d)\n",
__func__, res);
gyro_data->xyz_data.x -= gyro_data->cal_data.x;
gyro_data->xyz_data.y -= gyro_data->cal_data.y;
gyro_data->xyz_data.z -= gyro_data->cal_data.z;
}else {
pr_warn("%s, use last data(%d, %d, %d), status=%d, selftest=%d\n",
__func__, gyro_data->xyz_data.x, gyro_data->xyz_data.y,
gyro_data->xyz_data.z, status, gyro_data->self_test);
}
input_report_rel(gyro_data->input_dev, REL_RX, gyro_data->xyz_data.x);
input_report_rel(gyro_data->input_dev, REL_RY, gyro_data->xyz_data.y);
input_report_rel(gyro_data->input_dev, REL_RZ, gyro_data->xyz_data.z);
input_sync(gyro_data->input_dev);
#ifdef LOGGING_GYRO
pr_info("%s, x = %d, y = %d, z = %d\n"
, __func__, gyro_data->xyz_data.x, gyro_data->xyz_data.y,
gyro_data->xyz_data.z);
#endif
}
static int lsm330dlc_gyro_fifo_self_test(struct lsm330dlc_gyro_data *data,\
u8 *cal_pass, s16 *zero_rate_data)
{
struct gyro_t raw_data;
int err;
int i, j;
s16 raw[3] = {0,}, zero_rate_delta[3] = {0,};
int sum_raw[3] = {0,};
bool zero_rate_read_2nd = false;
u8 reg[5];
u8 fifo_pass = 2;
u8 status_reg;
struct file *cal_filp = NULL;
mm_segment_t old_fs;
/* fifo mode, enable interrupt, 500dps */
reg[0] = 0x6F;
reg[1] = 0x00;
reg[2] = 0x04;
reg[3] = 0x90;
reg[4] = 0x40;
for (i = 0; i < 10; i++) {
err = i2c_smbus_write_i2c_block_data(data->client,
CTRL_REG1 | AC, sizeof(reg), reg);
if (err >= 0)
break;
}
if (err < 0) {
pr_err("%s: CTRL_REGs i2c writing failed\n", __func__);
goto exit;
}
/* Power up, wait for 800ms for stable output */
msleep(800);
read_zero_rate_again:
for (i = 0; i < 10; i++) {
err = i2c_smbus_write_byte_data(data->client,
FIFO_CTRL_REG, BYPASS_MODE);
if (err >= 0)
break;
}
if (err < 0) {
pr_err("%s : failed to set bypass_mode\n", __func__);
goto exit;
}
for (i = 0; i < 10; i++) {
err = i2c_smbus_write_byte_data(data->client,
FIFO_CTRL_REG, FIFO_MODE | FIFO_TEST_WTM);
if (err >= 0)
break;
}
if (err < 0) {
pr_err("%s: failed to set fifo_mode\n", __func__);
goto exit;
}
/* if interrupt mode */
if (!data->enable && data->interruptible) {
enable_irq(data->client->irq);
err = wait_for_completion_timeout(&data->data_ready, 5*HZ);
msleep(200);
if (err <= 0) {
disable_irq(data->client->irq);
if (!err)
pr_err("%s: wait timed out\n", __func__);
goto exit;
}
/* if polling mode */
} else
msleep(200);
/* check out watermark status */
status_reg = i2c_smbus_read_byte_data(data->client, FIFO_SRC_REG);
if (!(status_reg & 0x80)) {
pr_err("%s: Watermark level is not enough\n", __func__);
goto exit;
}
/* read fifo entries */
err = lsm330dlc_gyro_read_values(data->client,
&raw_data, FIFO_TEST_WTM + 2);
if (err < 0) {
pr_err("%s: lsm330dlc_gyro_read_values() failed\n", __func__);
goto exit;
}
/* print out fifo data */
printk(KERN_INFO "[gyro_self_test] fifo data\n");
for (i = 0; i < sizeof(raw_data) * (FIFO_TEST_WTM + 1);
i += sizeof(raw_data)) {
raw[0] = (data->fifo_data[i+1] << 8)
| data->fifo_data[i];
raw[1] = (data->fifo_data[i+3] << 8)
| data->fifo_data[i+2];
raw[2] = (data->fifo_data[i+5] << 8)
| data->fifo_data[i+4];
pr_info("%2dth: %8d %8d %8d\n", i/6, raw[0], raw[1], raw[2]);
/* for calibration of gyro sensor data */
sum_raw[0] += raw[0];
sum_raw[1] += raw[1];
sum_raw[2] += raw[2];
for (j = 0; j < 3; j++) {
if (raw[j] < MIN_ZERO_RATE || raw[j] > MAX_ZERO_RATE) {
pr_err("%s: %dth data(%d) is out of zero-rate",
__func__, i/6, raw[j]);
pr_err("%s: fifo test failed\n", __func__);
fifo_pass = 0;
*cal_pass = 0;
goto exit;
}
}
}
/* zero_rate_data */
zero_rate_data[0] = raw[0] * 175 / 10000;
zero_rate_data[1] = raw[1] * 175 / 10000;
zero_rate_data[2] = raw[2] * 175 / 10000;
if (zero_rate_read_2nd == true) {
/* check zero_rate second time */
zero_rate_delta[0] -= zero_rate_data[0];
zero_rate_delta[1] -= zero_rate_data[1];
zero_rate_delta[2] -= zero_rate_data[2];
pr_info("[gyro_self_test] zero rate second: %8d %8d %8d\n",
zero_rate_data[0], zero_rate_data[1],
zero_rate_data[2]);
pr_info("[gyro_self_test] zero rate delta: %8d %8d %8d\n",
zero_rate_delta[0], zero_rate_delta[1],
zero_rate_delta[2]);
if ((-5 < zero_rate_delta[0] && zero_rate_delta[0] < 5) &&
(-5 < zero_rate_delta[1] && zero_rate_delta[1] < 5) &&
(-5 < zero_rate_delta[2] && zero_rate_delta[2] < 5)) {
/* calibration of gyro sensor data */
data->cal_data.x = sum_raw[0]/(FIFO_TEST_WTM + 1);
data->cal_data.y = sum_raw[1]/(FIFO_TEST_WTM + 1);
data->cal_data.z = sum_raw[2]/(FIFO_TEST_WTM + 1);
pr_info("%s: cal data (%d,%d,%d)\n", __func__,
data->cal_data.x, data->cal_data.y,
data->cal_data.z);
/* save cal data */
old_fs = get_fs();
set_fs(KERNEL_DS);
cal_filp = filp_open(CALIBRATION_FILE_PATH,
O_CREAT | O_TRUNC | O_WRONLY, 0666);
if (IS_ERR(cal_filp)) {
pr_err("%s: Can't open calibration file\n",
__func__);
set_fs(old_fs);
err = PTR_ERR(cal_filp);
return err;
}
err = cal_filp->f_op->write(cal_filp,
(char *)&data->cal_data, 3 * sizeof(s16),
&cal_filp->f_pos);
if (err != 3 * sizeof(s16)) {
pr_err("%s: Can't write the cal data to file\n",
__func__);
err = -EIO;
}
filp_close(cal_filp, current->files);
set_fs(old_fs);
*cal_pass = 1;
} /* else calibration is failed */
} else { /* check zero_rate first time, go to check again */
zero_rate_read_2nd = true;
sum_raw[0] = 0;
sum_raw[1] = 0;
sum_raw[2] = 0;
zero_rate_delta[0] = zero_rate_data[0];
zero_rate_delta[1] = zero_rate_data[1];
zero_rate_delta[2] = zero_rate_data[2];
pr_info("[gyro_self_test] zero rate first: %8d %8d %8d\n",
zero_rate_data[0], zero_rate_data[1],
zero_rate_data[2]);
goto read_zero_rate_again;
}
fifo_pass = 1;
exit:
/* 1: success, 0: fail, 2: retry */
return fifo_pass;
}
