joy/android_kernel_sony_sdm845
Watch
1
Fork
You've already forked android_kernel_sony_sdm845
 0
Code Issues Pull requests Projects Releases Wiki Activity

icm20602 driver: fix read fifo and timestamp

Browse source 
Fixed some errors on fifo read interface, and the timestamp also
need to be push to userspace

Change-Id: I765cb6570b62d7fc4cf3af805aa0ca0e024ba6fa
Signed-off-by: xuji <xuji@codeaurora.org>
This commit is contained in:
Ji Xu 2018-05-22 20:14:20 +08:00
commit 201bf295a8
4 changed files with 99 additions and 62 deletions
Download patch file Download diff file Expand all files Collapse all files

86
drivers/iio/imu/inv_icm20602/inv_icm20602_bsp.c
View file

@ -95,12 +95,12 @@ int icm20602_init_reg_map(void)
#define W_FLG 0
#define R_FLG 1
int icm20602_bulk_read(struct inv_icm20602_state *st,
int reg, char *buf, int size)
int reg, u8 *buf, int size)
{
int result = MPU_SUCCESS;
char tx_buf[2] = {0x0, 0x0};
int tmp_size = size;
int tmp_buf = buf;
u8 *tmp_buf = buf;
struct i2c_msg msg[2];
if (!st || !buf)
@ -109,38 +109,31 @@ int icm20602_bulk_read(struct inv_icm20602_state *st,
if (st->interface == ICM20602_SPI) {
tx_buf[0] = ICM20602_READ_REG(reg);
result = spi_write_then_read(st->spi, &tx_buf[0],
1, tmp_buf, size);
1, tmp_buf, size);
if (result) {
pr_err("mpu read reg %u failed, rc %d\n",
reg, result);
reg, result);
result = -MPU_READ_FAIL;
}
} else {
result = size;
while (tmp_size > 0) {
#ifdef ICM20602_I2C_SMBUS
result += i2c_smbus_read_i2c_block_data(st->client,
reg, (tmp_size < 32)?tmp_size:32, tmp_buf);
tmp_size -= 32;
tmp_buf += tmp_size;
result += i2c_smbus_read_i2c_block_data(st->client,
reg, size, tmp_buf);
#else
tx_buf[0] = reg;
msg[0].addr = st->client->addr;
msg[0].flags = W_FLG;
msg[0].len = 1;
msg[0].buf = tx_buf;
tx_buf[0] = reg;
msg[0].addr = st->client->addr;
msg[0].flags = W_FLG;
msg[0].len = 1;
msg[0].buf = tx_buf;
msg[1].addr = st->client->addr;
msg[1].flags = I2C_M_RD;
msg[1].len = (tmp_size < 32)?tmp_size:32;
msg[1].buf = tmp_buf;
i2c_transfer(st->client->adapter, msg, ARRAY_SIZE(msg));
tmp_size -= 32;
tmp_buf += tmp_size;
msg[1].addr = st->client->addr;
msg[1].flags = I2C_M_RD;
msg[1].len = size;
msg[1].buf = tmp_buf;
i2c_transfer(st->client->adapter, msg, ARRAY_SIZE(msg));
#endif
}
}
return result;
}
@ -355,6 +348,33 @@ static int icm20602_stop_fifo(struct inv_icm20602_state *st)
return MPU_SUCCESS;
}
int icm20602_int_status(struct inv_icm20602_state *st,
u8 *int_status)
{
return icm20602_read_reg(st,
reg_set_20602.INT_STATUS.address, int_status);
}
int icm20602_int_wm_status(struct inv_icm20602_state *st,
u8 *int_status)
{
return icm20602_read_reg(st,
reg_set_20602.FIFO_WM_INT_STATUS.address, int_status);
}
int icm20602_fifo_count(struct inv_icm20602_state *st,
u16 *fifo_count)
{
u8 count_h, count_l;
*fifo_count = 0;
icm20602_read_reg(st, reg_set_20602.FIFO_COUNTH.address, &count_h);
icm20602_read_reg(st, reg_set_20602.FIFO_COUNTL.address, &count_l);
*fifo_count |= (count_h << 8);
*fifo_count |= count_l;
return MPU_SUCCESS;
}
static int icm20602_config_waterlevel(struct inv_icm20602_state *st)
{
struct icm20602_user_config *config = NULL;
@ -809,6 +829,7 @@ int icm20602_init_device(struct inv_icm20602_state *st)
int result = MPU_SUCCESS;
struct icm20602_user_config *config = NULL;
int package_count;
int i;
config = st->config;
if (st == NULL || st->config == NULL) {
@ -860,7 +881,7 @@ int icm20602_init_device(struct inv_icm20602_state *st)
/* buffer malloc */
package_count = config->fifo_waterlevel / ICM20602_PACKAGE_SIZE;
st->buf = kzalloc(sizeof(config->fifo_waterlevel * 2), GFP_ATOMIC);
st->buf = kzalloc(config->fifo_waterlevel * 2, GFP_ATOMIC);
if (!st->buf)
return -ENOMEM;
@ -869,9 +890,26 @@ int icm20602_init_device(struct inv_icm20602_state *st)
if (!st->data_push)
return -ENOMEM;
for (i = 0; i < package_count; i++) {
st->data_push[i].raw_data =
kzalloc(ICM20602_PACKAGE_SIZE, GFP_ATOMIC);
}
return result;
}
int icm20602_reset_fifo(struct inv_icm20602_state *st)
{
reg_set_20602.USER_CTRL.reg_u.REG.FIFO_RST = 0x1;
if (icm20602_write_reg_simple(st, reg_set_20602.USER_CTRL)) {
reg_set_20602.USER_CTRL.reg_u.REG.FIFO_RST = 0x0;
return -MPU_FAIL;
}
reg_set_20602.USER_CTRL.reg_u.REG.FIFO_RST = 0x0;
return MPU_SUCCESS;
}
void icm20602_rw_test(struct inv_icm20602_state *st)
{
uint8_t val = 0;

4
drivers/iio/imu/inv_icm20602/inv_icm20602_core.c
View file

@ -41,12 +41,12 @@ static void inv_icm20602_def_config(struct inv_icm20602_state *st)
{
struct icm20602_user_config *config = st->config;
config->user_fps_in_ms = 10;
config->user_fps_in_ms = 20;
config->gyro_lpf = INV_ICM20602_GYRO_LFP_92HZ;
config->gyro_fsr = ICM20602_GYRO_FSR_1000DPS;
config->acc_lpf = ICM20602_ACCLFP_99;
config->acc_fsr = ICM20602_ACC_FSR_4G;
config->gyro_accel_sample_rate = ICM20602_SAMPLE_RATE_100HZ;
config->gyro_accel_sample_rate = ICM20602_SAMPLE_RATE_200HZ;
config->fifo_enabled = true;
}

10
drivers/iio/imu/inv_icm20602/inv_icm20602_iio.h
View file

@ -180,7 +180,7 @@ struct icm20602_user_config {
uint32_t user_fps_in_ms;
bool fifo_enabled;
uint32_t fifo_waterlevel;
uint16_t fifo_waterlevel;
struct X_Y_Z wake_on_motion;
};
@ -257,19 +257,21 @@ struct struct_icm20602_real_data {
struct struct_icm20602_data {
s64 timestamps;
struct struct_icm20602_raw_data raw_data;
struct struct_icm20602_real_data real_data;
u8 *raw_data;
};
extern struct iio_trigger *inv_trig;
irqreturn_t inv_icm20602_irq_handler(int irq, void *p);
irqreturn_t inv_icm20602_read_fifo_fn(int irq, void *p);
int inv_icm20602_reset_fifo(struct iio_dev *indio_dev);
int inv_icm20602_probe_trigger(struct iio_dev *indio_dev);
void inv_icm20602_remove_trigger(struct inv_icm20602_state *st);
int inv_icm20602_validate_trigger(struct iio_dev *indio_dev,
struct iio_trigger *trig);
int icm20602_int_status(struct inv_icm20602_state *st, u8 *int_status);
int icm20602_int_wm_status(struct inv_icm20602_state *st, u8 *int_status);
int icm20602_reset_fifo(struct inv_icm20602_state *st);
int icm20602_fifo_count(struct inv_icm20602_state *st, u16 *fifo_count);
int icm20602_read_raw(struct inv_icm20602_state *st,
struct struct_icm20602_real_data *real_data, uint32_t type);

65
drivers/iio/imu/inv_icm20602/inv_icm20602_ring.c
View file

@ -40,15 +40,6 @@ static void inv_clear_kfifo(struct inv_icm20602_state *st)
spin_unlock_irqrestore(&st->time_stamp_lock, flags);
}
int inv_icm20602_reset_fifo(struct iio_dev *indio_dev)
{
int result;
//u8 d;
//struct inv_icm20602_state *st = iio_priv(indio_dev);
return result;
}
/*
* inv_icm20602_irq_handler() - Cache a timestamp at each data ready interrupt.
*/
@ -67,44 +58,50 @@ irqreturn_t inv_icm20602_irq_handler(int irq, void *p)
return IRQ_WAKE_THREAD;
}
#define BIT_FIFO_OFLOW_INT 0x10
#define BIT_FIFO_WM_INT 0x40
static int inv_icm20602_read_data(struct iio_dev *indio_dev)
{
int result = MPU_SUCCESS;
struct inv_icm20602_state *st = iio_priv(indio_dev);
struct icm20602_user_config *config = st->config;
int package_count;
//char *buf = st->buf;
//struct struct_icm20602_data *data_push = st->data_push;
char *buf = st->buf;
struct struct_icm20602_data *data_push = st->data_push;
s64 timestamp;
u8 int_status, int_wm_status;
u16 fifo_count;
int i;
if (!st)
return -MPU_FAIL;
package_count = config->fifo_waterlevel / ICM20602_PACKAGE_SIZE;
mutex_lock(&indio_dev->mlock);
if (config->fifo_enabled) {
result = icm20602_read_fifo(st,
st->buf, config->fifo_waterlevel);
if (result != config->fifo_waterlevel) {
pr_err("icm20602 read fifo failed, result = %d\n",
result);
goto flush_fifo;
}
for (i = 0; i < package_count; i++) {
memcpy((char *)(&st->data_push[i].raw_data),
st->buf, ICM20602_PACKAGE_SIZE);
result = kfifo_out(&st->timestamps,
&timestamp, 1);
/* when there is no timestamp, put it as 0 */
if (result == 0)
timestamp = 0;
st->data_push[i].timestamps = timestamp;
iio_push_to_buffers(indio_dev, st->data_push+i);
st->buf += ICM20602_PACKAGE_SIZE;
}
icm20602_int_status(st, &int_status);
if (int_status & BIT_FIFO_OFLOW_INT) {
icm20602_fifo_count(st, &fifo_count);
pr_debug("fifo_count = %d\n", fifo_count);
icm20602_reset_fifo(st);
goto end_session;
}
//end_session:
if (config->fifo_enabled) {
result = kfifo_out(&st->timestamps,
&timestamp, 1);
/* when there is no timestamp, put it as 0 */
if (result == 0)
timestamp = 0;
for (i = 0; i < package_count; i++) {
result = icm20602_read_fifo(st,
buf, ICM20602_PACKAGE_SIZE);
memcpy(st->data_push[i].raw_data,
buf, ICM20602_PACKAGE_SIZE);
iio_push_to_buffers_with_timestamp(indio_dev,
st->data_push[i].raw_data, timestamp);
buf += ICM20602_PACKAGE_SIZE;
}
memset(st->buf, 0, config->fifo_waterlevel);
}
end_session:
mutex_unlock(&indio_dev->mlock);
iio_trigger_notify_done(indio_dev->trig);
return MPU_SUCCESS;
@ -112,7 +109,7 @@ static int inv_icm20602_read_data(struct iio_dev *indio_dev)
flush_fifo:
/* Flush HW and SW FIFOs. */
inv_clear_kfifo(st);
inv_icm20602_reset_fifo(indio_dev);
icm20602_reset_fifo(st);
mutex_unlock(&indio_dev->mlock);
iio_trigger_notify_done(indio_dev->trig);
return MPU_SUCCESS;