NED
/
tegrakernel
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
tegrakernel/kernel/nvidia/drivers/iio/imu/nvi_mpu/nvi_icm.c

1322 lines
30 KiB
C
Raw Permalink Blame History

/* Copyright (c) 2016, NVIDIA CORPORATION. All rights reserved.
*
* This software is licensed under the terms of the GNU General Public
* License version 2, as published by the Free Software Foundation, and
* may be copied, distributed, and modified under those terms.
*
* 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.
*/
#include <linux/kernel.h>
#include <linux/time.h>
#include <linux/delay.h>
#include "nvi.h"
#include "nvi_dmp_icm.h"
#define BYTES_PER_SENSOR 6
/* full scale and LPF setting */
#define ICM_SELFTEST_GYRO_FS ((0 << 3) | 1)
#define ICM_SELFTEST_ACCEL_FS ((7 << 3) | 1)
/* register settings */
#define ICM_SELFTEST_GYRO_SMPLRT_DIV 10
#define ICM_SELFTEST_GYRO_AVGCFG 3
#define ICM_SELFTEST_ACCEL_SMPLRT_DIV 10
#define ICM_SELFTEST_ACCEL_DEC3_CFG 2
#define ICM_SELFTEST_GYRO_SENS (32768 / 250)
#define BIT_ACCEL_CTEN 0x1C
#define BIT_GYRO_CTEN 0x38
/* wait time before collecting data */
#define ICM_MAX_PACKETS 20
#define ICM_SELFTEST_WAIT_TIME (ICM_MAX_PACKETS * 10)
#define ICM_GYRO_ENGINE_UP_TIME 50
#define ICM_ST_STABLE_TIME 20
#define ICM_GYRO_SCALE 131
#define ICM_ST_PRECISION 1000
#define ICM_ST_ACCEL_FS_MG 2000UL
#define ICM_ST_SCALE 32768
#define ICM_ST_TRY_TIMES 2
#define ICM_ST_SAMPLES 200
#define ICM_ACCEL_ST_SHIFT_DELTA_MIN 500
#define ICM_ACCEL_ST_SHIFT_DELTA_MAX 1500
#define ICM_GYRO_CT_SHIFT_DELTA 500
/* Gyro Offset Max Value (dps) */
#define ICM_GYRO_OFFSET_MAX 20
/* Gyro Self Test Absolute Limits ST_AL (dps) */
#define ICM_GYRO_ST_AL 60
/* Accel Self Test Absolute Limits ST_AL (mg) */
#define ICM_ACCEL_ST_AL_MIN ((225 * ICM_ST_SCALE \
/ ICM_ST_ACCEL_FS_MG) * ICM_ST_PRECISION)
#define ICM_ACCEL_ST_AL_MAX ((675 * ICM_ST_SCALE \
/ ICM_ST_ACCEL_FS_MG) * ICM_ST_PRECISION)
static const u16 icm_st_tb[256] = {
2620, 2646, 2672, 2699, 2726, 2753, 2781, 2808,
2837, 2865, 2894, 2923, 2952, 2981, 3011, 3041,
3072, 3102, 3133, 3165, 3196, 3228, 3261, 3293,
3326, 3359, 3393, 3427, 3461, 3496, 3531, 3566,
3602, 3638, 3674, 3711, 3748, 3786, 3823, 3862,
3900, 3939, 3979, 4019, 4059, 4099, 4140, 4182,
4224, 4266, 4308, 4352, 4395, 4439, 4483, 4528,
4574, 4619, 4665, 4712, 4759, 4807, 4855, 4903,
4953, 5002, 5052, 5103, 5154, 5205, 5257, 5310,
5363, 5417, 5471, 5525, 5581, 5636, 5693, 5750,
5807, 5865, 5924, 5983, 6043, 6104, 6165, 6226,
6289, 6351, 6415, 6479, 6544, 6609, 6675, 6742,
6810, 6878, 6946, 7016, 7086, 7157, 7229, 7301,
7374, 7448, 7522, 7597, 7673, 7750, 7828, 7906,
7985, 8065, 8145, 8227, 8309, 8392, 8476, 8561,
8647, 8733, 8820, 8909, 8998, 9088, 9178, 9270,
9363, 9457, 9551, 9647, 9743, 9841, 9939, 10038,
10139, 10240, 10343, 10446, 10550, 10656, 10763, 10870,
10979, 11089, 11200, 11312, 11425, 11539, 11654, 11771,
11889, 12008, 12128, 12249, 12371, 12495, 12620, 12746,
12874, 13002, 13132, 13264, 13396, 13530, 13666, 13802,
13940, 14080, 14221, 14363, 14506, 14652, 14798, 14946,
15096, 15247, 15399, 15553, 15709, 15866, 16024, 16184,
16346, 16510, 16675, 16842, 17010, 17180, 17352, 17526,
17701, 17878, 18057, 18237, 18420, 18604, 18790, 18978,
19167, 19359, 19553, 19748, 19946, 20145, 20347, 20550,
20756, 20963, 21173, 21385, 21598, 21814, 22033, 22253,
22475, 22700, 22927, 23156, 23388, 23622, 23858, 24097,
24338, 24581, 24827, 25075, 25326, 25579, 25835, 26093,
26354, 26618, 26884, 27153, 27424, 27699, 27976, 28255,
28538, 28823, 29112, 29403, 29697, 29994, 30294, 30597,
30903, 31212, 31524, 31839, 32157, 32479, 32804
};
/**
* inv_icm_st_gyr_chk() - check gyro self test.
* this function returns zero as success. A non-zero return
* value indicates failure in self test.
* @*st: main data structure.
* @*reg_avg: average value of normal test.
* @*st_avg: average value of self test
*/
static int inv_icm_st_gyr_chk(struct nvi_state *st, int *reg_avg, int *st_avg)
{
u8 *regs;
int ret = 0;
int otp_value_zero = 0;
int st_shift_prod[AXIS_N];
int st_shift_cust[AXIS_N];
int i;
regs = &st->st_data[DEV_GYR][0];
if (st->sts & NVI_DBG_SPEW_MSG)
dev_info(&st->i2c->dev, "%s data: %02x %02x %02x\n",
__func__, regs[0], regs[1], regs[2]);
for (i = 0; i < AXIS_N; i++) {
if (regs[i] != 0) {
st_shift_prod[i] = icm_st_tb[regs[i] - 1];
} else {
st_shift_prod[i] = 0;
otp_value_zero = 1;
}
}
if (st->sts & NVI_DBG_SPEW_MSG)
dev_info(&st->i2c->dev,
"%s st_shift_prod: %+d %+d %+d\n", __func__,
st_shift_prod[0], st_shift_prod[1], st_shift_prod[2]);
for (i = 0; i < AXIS_N; i++) {
st_shift_cust[i] = st_avg[i] - reg_avg[i];
if (!otp_value_zero) {
/* Self Test Pass/Fail Criteria A */
if (st_shift_cust[i] < (ICM_GYRO_CT_SHIFT_DELTA *
st_shift_prod[i])) {
ret = 1;
if (st->sts & NVI_DBG_SPEW_MSG)
dev_info(&st->i2c->dev,
"%s FAIL A axis %d\n",
__func__, i);
}
} else {
/* Self Test Pass/Fail Criteria B */
if (st_shift_cust[i] < (ICM_GYRO_ST_AL *
ICM_SELFTEST_GYRO_SENS *
ICM_ST_PRECISION)) {
ret = 1;
if (st->sts & NVI_DBG_SPEW_MSG)
dev_info(&st->i2c->dev,
"%s FAIL B axis %d\n",
__func__, i);
}
}
}
if (st->sts & NVI_DBG_SPEW_MSG)
dev_info(&st->i2c->dev, "%s st_shift_cust: %+d %+d %+d\n",
__func__,
st_shift_cust[0], st_shift_cust[1], st_shift_cust[2]);
if (ret == 0) {
/* Self Test Pass/Fail Criteria C */
for (i = 0; i < AXIS_N; i++) {
if (abs(reg_avg[i]) > (ICM_GYRO_OFFSET_MAX *
ICM_SELFTEST_GYRO_SENS *
ICM_ST_PRECISION)) {
ret = 1;
if (st->sts & NVI_DBG_SPEW_MSG)
dev_info(&st->i2c->dev,
"%s FAIL C axis %d\n",
__func__, i);
}
}
}
return ret;
}
/**
* inv_icm_st_acc_chk() - check accel self test.
* this function returns zero as success. A non-zero return
* value indicates failure in self test.
* @*st: main data structure.
* @*reg_avg: average value of normal test.
* @*st_avg: average value of self test
*/
static int inv_icm_st_acc_chk(struct nvi_state *st, int *reg_avg, int *st_avg)
{
u8 *regs;
int i;
int st_shift_prod[AXIS_N];
int st_shift_cust[AXIS_N];
int otp_value_zero = 0;
int ret = 0;
regs = &st->st_data[DEV_ACC][0];
if (st->sts & NVI_DBG_SPEW_MSG)
dev_info(&st->i2c->dev, "%s data: %02x %02x %02x\n",
__func__, regs[0], regs[1], regs[2]);
for (i = 0; i < AXIS_N; i++) {
if (regs[i] != 0) {
st_shift_prod[i] = icm_st_tb[regs[i] - 1];
} else {
st_shift_prod[i] = 0;
otp_value_zero = 1;
}
}
if (st->sts & NVI_DBG_SPEW_MSG)
dev_info(&st->i2c->dev,
"%s st_shift_prod: %+d %+d %+d\n", __func__,
st_shift_prod[0], st_shift_prod[1], st_shift_prod[2]);
if (!otp_value_zero) {
/* Self Test Pass/Fail Criteria A */
for (i = 0; i < AXIS_N; i++) {
st_shift_cust[i] = st_avg[i] - reg_avg[i];
if (st_shift_cust[i] < (ICM_ACCEL_ST_SHIFT_DELTA_MIN *
st_shift_prod[i])) {
ret = 1;
if (st->sts & NVI_DBG_SPEW_MSG)
dev_info(&st->i2c->dev,
"%s FAIL A (min) axis %d\n",
__func__, i);
}
if (st_shift_cust[i] > (ICM_ACCEL_ST_SHIFT_DELTA_MAX *
st_shift_prod[i])) {
ret = 1;
if (st->sts & NVI_DBG_SPEW_MSG)
dev_info(&st->i2c->dev,
"%s FAIL A (max) axis %d\n",
__func__, i);
}
}
} else {
/* Self Test Pass/Fail Criteria B */
for (i = 0; i < AXIS_N; i++) {
st_shift_cust[i] = abs(st_avg[i] - reg_avg[i]);
if ((st_shift_cust[i] < ICM_ACCEL_ST_AL_MIN) ||
(st_shift_cust[i] > ICM_ACCEL_ST_AL_MAX)) {
ret = 1;
if (st->sts & NVI_DBG_SPEW_MSG)
dev_info(&st->i2c->dev,
"%s FAIL B axis %d\n",
__func__, i);
}
}
}
if (st->sts & NVI_DBG_SPEW_MSG)
dev_info(&st->i2c->dev, "%s st_shift_cust: %+d %+d %+d\n",
__func__,
st_shift_cust[0], st_shift_cust[1], st_shift_cust[2]);
return ret;
}
static int inv_icm_st_setup(struct nvi_state *st)
{
int axis;
int ret;
/* Wake up and stop sensors */
ret = nvi_pm_wr(st, __func__, INV_CLK_PLL, BIT_PWR_PRESSURE_STBY |
BIT_PWR_ACCEL_STBY | BIT_PWR_GYRO_STBY, 0);
/* Perform a soft-reset of the chip
* This will clear any prior states in the chip
*/
ret |= nvi_wr_pm1(st, __func__, BIT_H_RESET);
if (ret)
return ret;
msleep(POWER_UP_TIME);
/* Wake up */
ret = nvi_wr_pm1(st, __func__, INV_CLK_PLL);
ret |= nvi_i2c_wr_rc(st, &st->hal->reg->user_ctrl, BIT_FIFO_EN,
__func__, &st->rc.user_ctrl);
/* Configure FIFO */
ret |= nvi_wr_fifo_cfg(st, -1);
/* Set cycle mode */
ret |= nvi_i2c_wr_rc(st, &st->hal->reg->lp_config, 0x70,
__func__, &st->rc.lp_config);
/* Configure FSR and DLPF */
ret |= nvi_i2c_write_rc(st, &st->hal->reg->smplrt[DEV_GYR],
ICM_SELFTEST_GYRO_SMPLRT_DIV, __func__,
(u8 *)&st->rc.smplrt[DEV_GYR], true);
ret |= nvi_i2c_wr_rc(st, &st->hal->reg->gyro_config1,
ICM_SELFTEST_GYRO_FS,
__func__, &st->rc.gyro_config1);
ret |= nvi_i2c_wr_rc(st, &st->hal->reg->gyro_config2,
ICM_SELFTEST_GYRO_AVGCFG,
__func__, &st->rc.gyro_config2);
ret |= nvi_i2c_write_rc(st, &st->hal->reg->smplrt[DEV_ACC],
ICM_SELFTEST_ACCEL_SMPLRT_DIV, __func__,
(u8 *)&st->rc.smplrt[DEV_ACC], true);
ret |= nvi_i2c_wr_rc(st, &st->hal->reg->accel_config,
ICM_SELFTEST_ACCEL_FS,
__func__, &st->rc.accel_config);
ret |= nvi_i2c_wr_rc(st, &st->hal->reg->accel_config2,
ICM_SELFTEST_ACCEL_DEC3_CFG,
__func__, &st->rc.accel_config2);
if (ret)
return ret;
/* Read selftest values */
for (axis = 0; axis < AXIS_N; axis++)
ret |= nvi_i2c_rd(st, &st->hal->reg->self_test_g[axis],
&st->st_data[DEV_GYR][axis]);
for (axis = 0; axis < AXIS_N; axis++)
ret |= nvi_i2c_rd(st, &st->hal->reg->self_test_a[axis],
&st->st_data[DEV_ACC][axis]);
ret |= nvi_pm_wr(st, __func__, INV_CLK_PLL,
BIT_PWR_PRESSURE_STBY, 0x70);
if (ret)
return ret;
msleep(ICM_GYRO_ENGINE_UP_TIME);
return ret;
}
static int inv_icm_st_rd(struct nvi_state *st, int dev,
int *sum_result, int *s)
{
u16 fifo_count;
s16 vals[AXIS_N];
u8 d[ICM_MAX_PACKETS * BYTES_PER_SENSOR];
int packet_count;
int i;
int j;
int t;
int ret;
ret = nvi_i2c_write_rc(st, &st->hal->reg->fifo_en, 0,
__func__, (u8 *)&st->rc.fifo_en, false);
/* Reset FIFO */
ret |= nvi_i2c_wr(st, &st->hal->reg->fifo_rst,
0x1F, __func__);
ret |= nvi_i2c_wr(st, &st->hal->reg->fifo_rst,
0x1E, __func__);
if (ret)
return ret;
while (*s < ICM_ST_SAMPLES) {
ret = nvi_i2c_write_rc(st, &st->hal->reg->fifo_en,
st->hal->dev[dev]->fifo_en_msk,
__func__, (u8 *)&st->rc.fifo_en, false);
if (ret)
return ret;
msleep(ICM_SELFTEST_WAIT_TIME);
ret = nvi_i2c_write_rc(st, &st->hal->reg->fifo_en, 0,
__func__, (u8 *)&st->rc.fifo_en, false);
if (ret)
return ret;
ret = nvi_i2c_rd(st, &st->hal->reg->fifo_count_h, d);
if (ret)
return ret;
fifo_count = be16_to_cpup((__be16 *)(&d[0]));
if (st->sts & NVI_DBG_SPEW_MSG)
dev_info(&st->i2c->dev, "%s fifo_count=%d\n",
__func__, fifo_count);
if (ICM_MAX_PACKETS * BYTES_PER_SENSOR < fifo_count) {
ret = nvi_i2c_r(st, st->hal->reg->fifo_rw.bank,
st->hal->reg->fifo_rw.reg,
ICM_MAX_PACKETS * BYTES_PER_SENSOR, d);
packet_count = ICM_MAX_PACKETS;
} else {
ret = nvi_i2c_r(st, st->hal->reg->fifo_rw.bank,
st->hal->reg->fifo_rw.reg,
fifo_count, d);
packet_count = fifo_count / BYTES_PER_SENSOR;
}
if (ret)
return ret;
i = 0;
while (i < packet_count) {
for (j = 0; j < AXIS_N; j++) {
t = 2 * j + i * BYTES_PER_SENSOR;
vals[j] = (s16)be16_to_cpup((__be16 *)(&d[t]));
sum_result[j] += vals[j];
}
if (st->sts & NVI_DBG_SPEW_MSG)
dev_info(&st->i2c->dev,
"%s %s %d: %+d %+d %+d\n",
__func__, st->snsr[dev].cfg.name,
*s, vals[0], vals[1], vals[2]);
(*s)++;
i++;
}
}
return 0;
}
/*
* inv_icm_st_acc_do() - do the actual test of self testing
*/
static int inv_icm_st_acc_do(struct nvi_state *st,
int *accel_result, int *accel_st_result)
{
int accel_s;
int i;
int j;
int ret;
for (i = 0; i < AXIS_N; i++) {
accel_result[i] = 0;
accel_st_result[i] = 0;
}
accel_s = 0;
ret = inv_icm_st_rd(st, DEV_ACC, accel_result, &accel_s);
if (ret || accel_s <= 0)
return -1;
for (j = 0; j < AXIS_N; j++) {
accel_result[j] = accel_result[j] / accel_s;
accel_result[j] *= ICM_ST_PRECISION;
}
/* Set Self-Test Bit */
ret = nvi_i2c_wr_rc(st, &st->hal->reg->accel_config2,
BIT_ACCEL_CTEN | ICM_SELFTEST_ACCEL_DEC3_CFG,
__func__, &st->rc.accel_config2);
if (ret)
return ret;
msleep(ICM_ST_STABLE_TIME);
accel_s = 0;
ret = inv_icm_st_rd(st, DEV_ACC, accel_st_result, &accel_s);
if (ret || accel_s <= 0)
return -1;
for (j = 0; j < AXIS_N; j++) {
accel_st_result[j] = accel_st_result[j] / accel_s;
accel_st_result[j] *= ICM_ST_PRECISION;
}
if (st->sts & NVI_DBG_SPEW_MSG)
dev_info(&st->i2c->dev, "%s %d, %d, %d\n", __func__,
accel_result[0], accel_result[1], accel_result[2]);
return 0;
}
/*
* inv_icm_st_gyr_do() - do the actual test of self testing
*/
static int inv_icm_st_gyr_do(struct nvi_state *st,
int *gyro_result, int *gyro_st_result)
{
int gyro_s;
int i;
int j;
int ret;
for (i = 0; i < AXIS_N; i++) {
gyro_result[i] = 0;
gyro_st_result[i] = 0;
}
gyro_s = 0;
ret = inv_icm_st_rd(st, DEV_GYR, gyro_result, &gyro_s);
if (ret || gyro_s <= 0)
return -1;
for (j = 0; j < AXIS_N; j++) {
gyro_result[j] = gyro_result[j] / gyro_s;
gyro_result[j] *= ICM_ST_PRECISION;
}
/* Set Self-Test Bit */
ret = nvi_i2c_wr_rc(st, &st->hal->reg->gyro_config2,
BIT_GYRO_CTEN | ICM_SELFTEST_GYRO_AVGCFG,
__func__, &st->rc.gyro_config2);
if (ret)
return ret;
msleep(ICM_ST_STABLE_TIME);
gyro_s = 0;
ret = inv_icm_st_rd(st, DEV_GYR, gyro_st_result, &gyro_s);
if (ret || gyro_s <= 0)
return -1;
for (j = 0; j < AXIS_N; j++) {
gyro_st_result[j] = gyro_st_result[j] / gyro_s;
gyro_st_result[j] *= ICM_ST_PRECISION;
}
if (st->sts & NVI_DBG_SPEW_MSG)
dev_info(&st->i2c->dev, "%s %d, %d, %d\n", __func__,
gyro_result[0], gyro_result[1], gyro_result[2]);
return 0;
}
static int inv_st_gyr_icm(struct nvi_state *st)
{
int ret;
int gyr_bias_st[AXIS_N];
int gyr_bias_regular[AXIS_N];
int test_times;
int i;
char gyro_result;
ret = inv_icm_st_setup(st);
if (ret)
return ret;
test_times = ICM_ST_TRY_TIMES;
while (test_times > 0) {
ret = inv_icm_st_gyr_do(st, gyr_bias_regular, gyr_bias_st);
if (ret == -EAGAIN)
test_times--;
else
test_times = 0;
}
if (ret)
return ret;
if (st->sts & NVI_DBG_SPEW_MSG) {
dev_info(&st->i2c->dev, "%s gyro bias_regular: %+d %+d %+d\n",
__func__, gyr_bias_regular[0], gyr_bias_regular[1],
gyr_bias_regular[2]);
dev_info(&st->i2c->dev, "%s gyro bias_st: %+d %+d %+d\n",
__func__, gyr_bias_st[0], gyr_bias_st[1],
gyr_bias_st[2]);
}
for (i = 0; i < AXIS_N; i++)
st->bias[DEV_GYR][i] = (gyr_bias_regular[i] /
ICM_ST_PRECISION);
gyro_result = inv_icm_st_gyr_chk(st, gyr_bias_regular, gyr_bias_st);
if (st->sts & NVI_DBG_SPEW_MSG)
dev_info(&st->i2c->dev, "%s gyro_result %hhd\n",
__func__, gyro_result);
return gyro_result;
}
static int inv_st_acc_icm(struct nvi_state *st)
{
int ret;
int acc_bias_st[AXIS_N];
int acc_bias_regular[AXIS_N];
int test_times;
int i;
char accel_result;
ret = inv_icm_st_setup(st);
if (ret)
return ret;
test_times = ICM_ST_TRY_TIMES;
while (test_times > 0) {
ret = inv_icm_st_acc_do(st, acc_bias_regular, acc_bias_st);
if (ret == -EAGAIN)
test_times--;
else
break;
}
if (ret)
return ret;
if (st->sts & NVI_DBG_SPEW_MSG) {
dev_info(&st->i2c->dev, "%s acc_bias_regular: %+d %+d %+d\n",
__func__, acc_bias_regular[0],
acc_bias_regular[1], acc_bias_regular[2]);
dev_info(&st->i2c->dev, "%s acc_bias_st: %+d %+d %+d\n",
__func__, acc_bias_st[0], acc_bias_st[1],
acc_bias_st[2]);
}
for (i = 0; i < AXIS_N; i++)
st->bias[DEV_ACC][i] = (acc_bias_regular[i] /
ICM_ST_PRECISION);
accel_result = inv_icm_st_acc_chk(st, acc_bias_regular, acc_bias_st);
if (st->sts & NVI_DBG_SPEW_MSG)
dev_info(&st->i2c->dev, "%s accel_result %hhd\n",
__func__, accel_result);
return accel_result;
}
static int nvi_pm_icm(struct nvi_state *st, u8 pm1, u8 pm2, u8 lp)
{
int ret;
ret = nvi_i2c_wr_rc(st, &st->hal->reg->lp_config, 0x70,
__func__, &st->rc.lp_config);
/* the DMP changes pm2 so we can't use the cache */
ret |= nvi_i2c_wr(st, &st->hal->reg->pm2, pm2, __func__);
ret |= nvi_i2c_wr_rc(st, &st->hal->reg->pm1, pm1,
__func__, &st->rc.pm1);
return ret;
}
static int nvi_en_gyr_icm(struct nvi_state *st)
{
u8 val;
int i;
int ret = 0;
st->snsr[DEV_GYR].buf_n = 6;
for (i = 0; i < AXIS_N; i++)
ret |= nvi_wr_gyro_offset(st, i,
(u16)(st->rom_offset[DEV_GYR][i] +
st->dev_offset[DEV_GYR][i]));
ret |= nvi_i2c_wr_rc(st, &st->hal->reg->gyro_config2, 0,
__func__, &st->rc.gyro_config2);
val = (st->snsr[DEV_GYR].usr_cfg << 1) | 0x01;
ret |= nvi_i2c_wr_rc(st, &st->hal->reg->gyro_config1, val,
__func__, &st->rc.gyro_config1);
return ret;
}
static int nvi_en_acc_icm(struct nvi_state *st)
{
u8 val;
int i;
int ret = 0;
st->snsr[DEV_ACC].buf_n = 6;
st->snsr[DEV_ACC].buf_shft = 0;
for (i = 0; i < AXIS_N; i++)
ret |= nvi_wr_accel_offset(st, i,
(u16)(st->rom_offset[DEV_ACC][i] +
(st->dev_offset[DEV_ACC][i] << 1)));
ret |= nvi_i2c_wr_rc(st, &st->hal->reg->accel_config2, 0,
__func__, &st->rc.accel_config2);
val = (st->snsr[DEV_ACC].usr_cfg << 1);
ret |= nvi_i2c_wr_rc(st, &st->hal->reg->accel_config, val,
__func__, &st->rc.accel_config);
return ret;
}
static int nvi_init_icm(struct nvi_state *st)
{
u8 val;
s8 t;
unsigned int src;
int ret;
st->snsr[DEV_ACC].cfg.thresh_hi = 0; /* no ACC LP on ICM */
st->snsr[DEV_SM].cfg.thresh_lo = ICM_SMD_THLD_INIT;
st->snsr[DEV_SM].cfg.thresh_hi = ICM_SMD_THLD_N_INIT;
st->snsr[DEV_SM].cfg.delay_us_min = ICM_SMD_TIMER_INIT;
st->snsr[DEV_SM].cfg.delay_us_max = ICM_SMD_TIMER2_INIT;
st->snsr[DEV_SM].cfg.report_n = ICM_SMD_RESET_INIT;
ret = nvi_i2c_rd(st, &st->hal->reg->tbc_pll, &val);
if (ret)
return ret;
t = abs(val & 0x7F);
if (val & 0x80)
t = -t;
st->src[SRC_GYR].base_t = NSEC_PER_SEC - t * 769903 + ((t * 769903) /
1270) * t;
st->src[SRC_ACC].base_t = NSEC_PER_SEC;
st->src[SRC_AUX].base_t = NSEC_PER_SEC;
for (src = 0; src < st->hal->src_n; src++)
st->src[src].base_t /= ICM_BASE_SAMPLE_RATE;
/* calculate the period_us_max */
st->src[SRC_GYR].period_us_max = (st->src[SRC_GYR].base_t *
0xFF) / 1000;
/* WAR: limit SRC_ACC to SRC_GYR since same period required by WAR */
st->src[SRC_ACC].period_us_max = st->src[SRC_GYR].period_us_max;
st->src[SRC_AUX].period_us_max = st->hal->src[SRC_AUX].period_us_max;
for (src = 0; src < st->hal->src_n; src++)
st->src[src].period_us_min = st->hal->src[src].period_us_min;
nvi_en_gyr_icm(st);
nvi_en_acc_icm(st);
return 0;
}
struct nvi_fn nvi_fn_icm = {
.pm = nvi_pm_icm,
.init = nvi_init_icm,
.st_acc = inv_st_acc_icm,
.st_gyr = inv_st_gyr_icm,
.en_acc = nvi_en_acc_icm,
.en_gyr = nvi_en_gyr_icm,
};
static int nvi_src(struct nvi_state *st, unsigned int src)
{
unsigned int rate;
int ret;
rate = (st->src[src].period_us_req * 1000) / st->src[src].base_t;
if (rate)
rate--;
ret = nvi_i2c_write_rc(st, &st->hal->reg->smplrt[src], rate,
__func__, (u8 *)&st->rc.smplrt[src], true);
if (!ret)
st->src[src].period_us_src = ((rate + 1) *
st->src[src].base_t) / 1000;
if (st->sts & (NVS_STS_SPEW_MSG | NVI_DBG_SPEW_MSG))
dev_info(&st->i2c->dev,
"%s src[%u] period_req=%u period_src=%u err=%d\n",
__func__, src, st->src[src].period_us_req,
st->src[src].period_us_src, ret);
return ret;
}
static int nvi_src_gyr(struct nvi_state *st)
{
return nvi_src(st, SRC_GYR);
}
static int nvi_src_acc(struct nvi_state *st)
{
return nvi_src(st, SRC_ACC);
}
static unsigned int nvi_aux_period_us[] = {
29127111,
14563556,
7281778,
3640889,
1820444,
910222,
455111,
227556,
113778,
56889,
28444,
14222,
};
static int nvi_src_aux(struct nvi_state *st)
{
u8 val;
unsigned int i;
int ret;
for (i = 0; i < ARRAY_SIZE(nvi_aux_period_us); i++) {
if (st->src[SRC_AUX].period_us_req >= nvi_aux_period_us[i])
break;
}
if (i >= ARRAY_SIZE(nvi_aux_period_us))
i = ARRAY_SIZE(nvi_aux_period_us) - 1;
val = 0x0F - i;
ret = nvi_i2c_wr_rc(st, &st->hal->reg->i2c_mst_odr_config, val,
__func__, &st->rc.i2c_mst_odr_config);
if (!ret) {
st->src[SRC_AUX].period_us_src = nvi_aux_period_us[i];
ret = nvi_aux_delay(st, __func__);
}
if (st->sts & (NVS_STS_SPEW_MSG | NVI_DBG_SPEW_MSG))
dev_info(&st->i2c->dev,
"%s src[SRC_AUX] period_req=%u period_src=%u err=%d\n",
__func__, st->src[SRC_AUX].period_us_req,
st->src[SRC_AUX].period_us_src, ret);
return ret;
}
static const struct nvi_hal_src src[] = {
[SRC_GYR] {
.dev_msk = (1 << DEV_GYR) | (1 << DEV_GYU),
.period_us_min = 10000,
.period_us_max = 200000,
.fn_period = nvi_src_gyr,
},
[SRC_ACC] {
.dev_msk = (1 << DEV_ACC),
.period_us_min = 10000,
.period_us_max = 200000,
.fn_period = nvi_src_acc,
},
[SRC_AUX] {
.dev_msk = (1 << DEV_AUX),
.period_us_min = 14222,
.period_us_max = 29127111,
.fn_period = nvi_src_aux,
},
};
static int nvi_fifo_devs[] = {
DEV_GYR,
DEV_ACC,
-1,
DEV_AUX,
};
static const unsigned long nvi_lp_dly_us_tbl[] = {
4096000,/* 4096ms */
2048000,/* 2048ms */
1024000,/* 1024ms */
512000, /* 512ms */
256000, /* 256ms */
128000, /* 128ms */
64000, /* 64ms */
32000, /* 32ms */
16000, /* 16ms */
8000, /* 8ms */
4000, /* 4ms */
/* 2000, 2ms */
};
static struct nvi_rr nvi_rr_acc[] = {
/* all accelerometer values are in g's fval = NVS_FLOAT_NANO */
{
.max_range = {
.ival = 19,
.fval = 613300000,
},
.resolution = {
.ival = 0,
.fval = 598550,
},
},
{
.max_range = {
.ival = 39,
.fval = 226600000,
},
.resolution = {
.ival = 0,
.fval = 1197101,
},
},
{
.max_range = {
.ival = 78,
.fval = 453200000,
},
.resolution = {
.ival = 0,
.fval = 2394202,
},
},
{
.max_range = {
.ival = 156,
.fval = 906400000,
},
.resolution = {
.ival = 0,
.fval = 4788403,
},
},
};
static struct nvi_rr nvi_rr_gyr[] = {
/* rad / sec fval = NVS_FLOAT_NANO */
{
.max_range = {
.ival = 4,
.fval = 363323130,
},
.resolution = {
.ival = 0,
.fval = 133231,
},
},
{
.max_range = {
.ival = 8,
.fval = 726646260,
},
.resolution = {
.ival = 0,
.fval = 266462,
},
},
{
.max_range = {
.ival = 17,
.fval = 453292520,
},
.resolution = {
.ival = 0,
.fval = 532113,
},
},
{
.max_range = {
.ival = 34,
.fval = 906585040,
},
.resolution = {
.ival = 0,
.fval = 1064225,
},
},
};
static struct nvi_rr nvi_rr_tmp[] = {
{
.max_range = {
.ival = 125,
.fval = 0,
},
.resolution = {
.ival = 1,
.fval = 0,
},
},
};
static struct nvi_rr nvi_rr_dmp[] = {
{
.max_range = {
.ival = 1,
.fval = 0,
},
.resolution = {
.ival = 1,
.fval = 0,
},
},
};
static const struct nvi_hal_dev nvi_hal_acc = {
.version = 3,
.src = SRC_ACC,
.rr_0n = ARRAY_SIZE(nvi_rr_acc) - 1,
.rr = nvi_rr_acc,
.milliamp = {
.ival = 0,
.fval = 500000000, /* NVS_FLOAT_NANO */
},
.fifo_en_msk = 0x1000,
.fifo = 1,
.fifo_data_n = 6,
};
static const struct nvi_hal_dev nvi_hal_gyr = {
.version = 3,
.src = SRC_GYR,
.rr_0n = ARRAY_SIZE(nvi_rr_gyr) - 1,
.rr = nvi_rr_gyr,
.milliamp = {
.ival = 3,
.fval = 700000000, /* NVS_FLOAT_NANO */
},
.fifo_en_msk = 0x0E00,
.fifo = 0,
.fifo_data_n = 6,
};
static const struct nvi_hal_dev nvi_hal_tmp = {
.version = 2,
.src = -1,
.rr_0n = ARRAY_SIZE(nvi_rr_tmp) - 1,
.rr = nvi_rr_tmp,
.scale = {
.ival = 0,
.fval = 334082700, /* NVS_FLOAT_NANO */
},
.offset = {
.ival = 0,
.fval = 137625600, /* NVS_FLOAT_NANO */
},
.milliamp = {
.ival = 3,
.fval = 700000000, /* NVS_FLOAT_NANO */
},
};
static const struct nvi_hal_dev nvi_hal_aux = {
.src = SRC_AUX,
.fifo_en_msk = 0x000F,
};
static const struct nvi_hal_dev nvi_hal_dmp = {
.version = 1,
.src = SRC_GYR,
.rr_0n = ARRAY_SIZE(nvi_rr_dmp) - 1,
.rr = nvi_rr_dmp,
.milliamp = {
.ival = 0,
.fval = 500000, /* NVS_FLOAT_MICRO */
},
};
static const struct nvi_hal_reg nvi_hal_reg_icm = {
/* register bank 0 */
.lp_config = {
.bank = 0,
.reg = 0x05,
.dflt = 0x70,
},
.i2c_mst_status = {
.bank = 0,
.reg = 0x17,
},
.int_pin_cfg = {
.bank = 0,
.reg = 0x0F,
},
.int_enable = {
.bank = 0,
.reg = 0x10,
.len = 4,
},
.int_dmp = {
.bank = 0,
.reg = 0x18,
},
.int_status = {
.bank = 0,
.reg = 0x19,
.len = 4,
},
.out_h[DEV_ACC] = {
.bank = 0,
.reg = 0x2D,
},
.out_h[DEV_GYR] = {
.bank = 0,
.reg = 0x33,
},
.out_h[DEV_TMP] = {
.bank = 0,
.reg = 0x39,
},
.ext_sens_data_00 = {
.bank = 0,
.reg = 0x3B,
},
.signal_path_reset = {
.bank = 0,
.reg = 0x04,
},
.user_ctrl = {
.bank = 0,
.reg = 0x03,
},
.pm1 = {
.bank = 0,
.reg = 0x06,
.dflt = 0x01,
},
.pm2 = {
.bank = 0,
.reg = 0x07,
.dflt = 0x40,
},
.fifo_en = {
.bank = 0,
.reg = 0x66,
.len = 2,
},
.fifo_rst = {
.bank = 0,
.reg = 0x68,
},
.fifo_sz = {
.bank = 0,
.reg = 0x6E,
},
.fifo_count_h = {
.bank = 0,
.reg = 0x70,
.len = 2,
},
.fifo_rw = {
.bank = 0,
.reg = 0x72,
},
.fifo_cfg = {
.bank = 0,
.reg = 0x76,
},
.who_am_i = {
.bank = 0,
.reg = 0x00,
},
.mem_addr = {
.bank = 0,
.reg = 0x7C,
},
.mem_rw = {
.bank = 0,
.reg = 0x7D,
},
.mem_bank = {
.bank = 0,
.reg = 0x7E,
},
.reg_bank = {
.bank = 0,
.reg = 0x7F,
},
/* register bank 1 */
.self_test_g[AXIS_X] = {
.bank = 1,
.reg = 0x02,
},
.self_test_g[AXIS_Y] = {
.bank = 1,
.reg = 0x03,
},
.self_test_g[AXIS_Z] = {
.bank = 1,
.reg = 0x04,
},
.self_test_a[AXIS_X] = {
.bank = 1,
.reg = 0x0E,
},
.self_test_a[AXIS_Y] = {
.bank = 1,
.reg = 0x0F,
},
.self_test_a[AXIS_Z] = {
.bank = 1,
.reg = 0x10,
},
.a_offset_h[AXIS_X] = {
.bank = 1,
.reg = 0x14,
.len = 2,
},
.a_offset_h[AXIS_Y] = {
.bank = 1,
.reg = 0x17,
.len = 2,
},
.a_offset_h[AXIS_Z] = {
.bank = 1,
.reg = 0x1A,
.len = 2,
},
.tbc_pll = {
.bank = 1,
.reg = 0x28,
},
.tbc_rcosc = {
.bank = 1,
.reg = 0x29,
},
/* register bank 2 */
.smplrt[SRC_GYR] = {
.bank = 2,
.reg = 0x00,
},
.gyro_config1 = {
.bank = 2,
.reg = 0x01,
.dflt = 0x01,
},
.gyro_config2 = {
.bank = 2,
.reg = 0x02,
},
.g_offset_h[AXIS_X] = {
.bank = 2,
.reg = 0x03,
.len = 2,
},
.g_offset_h[AXIS_Y] = {
.bank = 2,
.reg = 0x05,
.len = 2,
},
.g_offset_h[AXIS_Z] = {
.bank = 2,
.reg = 0x07,
.len = 2,
},
.smplrt[SRC_ACC] = {
.bank = 2,
.reg = 0x10,
.len = 2,
},
.accel_config = {
.bank = 2,
.reg = 0x14,
},
.accel_config2 = {
.bank = 2,
.reg = 0x15,
},
.fw_start = {
.bank = 2,
.reg = 0x50,
.len = 2,
},
/* register bank 3 */
.i2c_mst_odr_config = {
.bank = 3,
.reg = 0x00,
},
.i2c_mst_ctrl = {
.bank = 3,
.reg = 0x01,
},
.i2c_mst_delay_ctrl = {
.bank = 3,
.reg = 0x02,
},
.i2c_slv_addr[0] = {
.bank = 3,
.reg = 0x03,
},
.i2c_slv_addr[1] = {
.bank = 3,
.reg = 0x07,
},
.i2c_slv_addr[2] = {
.bank = 3,
.reg = 0x0B,
},
.i2c_slv_addr[3] = {
.bank = 3,
.reg = 0x0F,
},
.i2c_slv_addr[4] = {
.bank = 3,
.reg = 0x13,
},
.i2c_slv_reg[0] = {
.bank = 3,
.reg = 0x04,
},
.i2c_slv_reg[1] = {
.bank = 3,
.reg = 0x08,
},
.i2c_slv_reg[2] = {
.bank = 3,
.reg = 0x0C,
},
.i2c_slv_reg[3] = {
.bank = 3,
.reg = 0x10,
},
.i2c_slv_reg[4] = {
.bank = 3,
.reg = 0x14,
},
.i2c_slv_ctrl[0] = {
.bank = 3,
.reg = 0x05,
},
.i2c_slv_ctrl[1] = {
.bank = 3,
.reg = 0x09,
},
.i2c_slv_ctrl[2] = {
.bank = 3,
.reg = 0x0D,
},
.i2c_slv_ctrl[3] = {
.bank = 3,
.reg = 0x11,
},
.i2c_slv4_ctrl = {
.bank = 3,
.reg = 0x15,
},
.i2c_slv_do[0] = {
.bank = 3,
.reg = 0x06,
},
.i2c_slv_do[1] = {
.bank = 3,
.reg = 0x0A,
},
.i2c_slv_do[2] = {
.bank = 3,
.reg = 0x0E,
},
.i2c_slv_do[3] = {
.bank = 3,
.reg = 0x12,
},
.i2c_slv_do[4] = {
.bank = 3,
.reg = 0x16,
},
.i2c_slv4_di = {
.bank = 3,
.reg = 0x17,
},
};
static const struct nvi_hal_bit nvi_hal_bit_icm = {
.dmp_int_sm = 2,
.dmp_int_stp = 3,
.int_i2c_mst = 0,
.int_dmp = 1,
.int_pll_rdy = 2,
.int_wom = 3,
.int_wof = 7,
.int_data_rdy_0 = 8,
.int_data_rdy_1 = 9,
.int_data_rdy_2 = 10,
.int_data_rdy_3 = 11,
.int_fifo_ovrflw_0 = 16,
.int_fifo_ovrflw_1 = 17,
.int_fifo_ovrflw_2 = 18,
.int_fifo_ovrflw_3 = 19,
.int_fifo_wm_0 = 24,
.int_fifo_wm_1 = 25,
.int_fifo_wm_2 = 26,
.int_fifo_wm_3 = 27,
.slv_fifo_en[0] = 0,
.slv_fifo_en[1] = 1,
.slv_fifo_en[2] = 2,
.slv_fifo_en[3] = 3,
};
const struct nvi_hal nvi_hal_20628 = {
.regs_n = 128,
.reg_bank_n = 4,
.src = src,
.src_n = ARRAY_SIZE(src),
.fifo_dev = nvi_fifo_devs,
.fifo_n = ARRAY_SIZE(nvi_fifo_devs),
.lp_tbl = nvi_lp_dly_us_tbl,
.lp_tbl_n = ARRAY_SIZE(nvi_lp_dly_us_tbl),
.dev[DEV_ACC] = &nvi_hal_acc,
.dev[DEV_GYR] = &nvi_hal_gyr,
.dev[DEV_TMP] = &nvi_hal_tmp,
.dev[DEV_SM] = &nvi_hal_dmp,
.dev[DEV_STP] = &nvi_hal_dmp,
.dev[DEV_QTN] = &nvi_hal_dmp,
.dev[DEV_GMR] = &nvi_hal_dmp,
.dev[DEV_GYU] = &nvi_hal_gyr,
.dev[DEV_AUX] = &nvi_hal_aux,
.reg = &nvi_hal_reg_icm,
.bit = &nvi_hal_bit_icm,
.fn = &nvi_fn_icm,
.dmp = &nvi_dmp_icm,
};
EXPORT_SYMBOL(nvi_hal_20628);
Reference in New Issue View Git Blame Copy Permalink