| /* |
| * Copyright (C) 2012-2018 InvenSense, Inc. |
| * |
| * 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. |
| */ |
| #define pr_fmt(fmt) "inv_mpu: " fmt |
| |
| #include <linux/export.h> |
| #include <linux/kernel.h> |
| #include <linux/device.h> |
| #include <linux/slab.h> |
| #include <linux/err.h> |
| #include <linux/delay.h> |
| #include <linux/sysfs.h> |
| #include <linux/jiffies.h> |
| #include <linux/irq.h> |
| #include <linux/interrupt.h> |
| #include <linux/kfifo.h> |
| #include <linux/poll.h> |
| #include <linux/math64.h> |
| #include <linux/miscdevice.h> |
| |
| #include "inv_mpu_iio.h" |
| |
| static void inv_push_timestamp(struct iio_dev *indio_dev, u64 t) |
| { |
| u8 buf[IIO_BUFFER_BYTES]; |
| struct inv_mpu_state *st; |
| |
| st = iio_priv(indio_dev); |
| if (st->poke_mode_on) |
| memcpy(buf, &st->poke_ts, sizeof(t)); |
| else |
| memcpy(buf, &t, sizeof(t)); |
| iio_push_to_buffers(indio_dev, buf); |
| } |
| |
| int inv_push_marker_to_buffer(struct inv_mpu_state *st, u16 hdr, int data) |
| { |
| struct iio_dev *indio_dev = iio_priv_to_dev(st); |
| u8 buf[IIO_BUFFER_BYTES]; |
| |
| memcpy(buf, &hdr, sizeof(hdr)); |
| memcpy(&buf[4], &data, sizeof(data)); |
| iio_push_to_buffers(indio_dev, buf); |
| |
| return 0; |
| } |
| static int inv_calc_precision(struct inv_mpu_state *st) |
| { |
| int diff; |
| int init; |
| |
| if (st->eis.voting_state != 8) |
| return 0; |
| diff = abs(st->eis.fsync_delay_s[1] - st->eis.fsync_delay_s[0]); |
| init = 0; |
| if (diff) |
| init = st->sensor[SENSOR_GYRO].dur / diff; |
| |
| if (abs(init - NSEC_PER_USEC) < (NSEC_PER_USEC >> 3)) |
| st->eis.count_precision = init; |
| else |
| st->eis.voting_state = 0; |
| |
| pr_debug("dur= %d prc= %d\n", st->sensor[SENSOR_GYRO].dur, |
| st->eis.count_precision); |
| |
| return 0; |
| } |
| |
| static s64 calc_frame_ave(struct inv_mpu_state *st, int delay) |
| { |
| s64 ts; |
| |
| ts = st->eis.current_timestamp - delay; |
| #if defined(CONFIG_INV_MPU_IIO_ICM20648) | defined(CONFIG_INV_MPU_IIO_ICM20690) |
| ts -= st->ts_algo.gyro_ts_shift; |
| #endif |
| pr_debug("shift= %d ts = %lld\n", st->ts_algo.gyro_ts_shift, ts); |
| |
| return ts; |
| } |
| |
| static void inv_push_eis_ring(struct inv_mpu_state *st, int *q, bool sync, |
| s64 t) |
| { |
| struct iio_dev *indio_dev = iio_priv_to_dev(st); |
| struct inv_eis *eis = &st->eis; |
| u8 buf[IIO_BUFFER_BYTES]; |
| int tmp, ii; |
| |
| buf[0] = (EIS_GYRO_HDR & 0xff); |
| buf[1] = (EIS_GYRO_HDR >> 8); |
| memcpy(buf + 4, &q[0], sizeof(q[0])); |
| iio_push_to_buffers(indio_dev, buf); |
| for (ii = 0; ii < 2; ii++) |
| memcpy(buf + 4 * ii, &q[ii + 1], sizeof(q[ii])); |
| iio_push_to_buffers(indio_dev, buf); |
| tmp = eis->frame_count; |
| if (sync) |
| tmp |= 0x80000000; |
| memcpy(buf, &tmp, sizeof(tmp)); |
| iio_push_to_buffers(indio_dev, buf); |
| inv_push_timestamp(indio_dev, t); |
| } |
| static int inv_do_interpolation_gyro(struct inv_mpu_state *st, int *prev, |
| s64 prev_t, int *curr, s64 curr_t, s64 t, bool trigger) |
| { |
| int i; |
| int out[3]; |
| #if defined(CONFIG_INV_MPU_IIO_ICM20648) | defined(CONFIG_INV_MPU_IIO_ICM20690) |
| prev_t -= st->ts_algo.gyro_ts_shift; |
| prev_t += MPU_4X_TS_GYRO_SHIFT; |
| curr_t -= st->ts_algo.gyro_ts_shift; |
| curr_t += MPU_4X_TS_GYRO_SHIFT; |
| #endif |
| if ((t > prev_t) && (t < curr_t)) { |
| for (i = 0; i < 3; i++) |
| out[i] = (int)div_s64((s64)(curr[i] - prev[i]) * |
| (s64)(t - prev_t), curr_t - prev_t) + prev[i]; |
| } else if (t < prev_t) { |
| for (i = 0; i < 3; i++) |
| out[i] = prev[i]; |
| } else { |
| for (i = 0; i < 3; i++) |
| out[i] = curr[i]; |
| } |
| pr_debug("prev= %lld t = %lld curr= %lld\n", prev_t, t, curr_t); |
| pr_debug("prev = %d, %d, %d\n", prev[0], prev[1], prev[2]); |
| pr_debug("curr = %d, %d, %d\n", curr[0], curr[1], curr[2]); |
| pr_debug("out = %d, %d, %d\n", out[0], out[1], out[2]); |
| inv_push_eis_ring(st, out, trigger, t); |
| |
| return 0; |
| } |
| #if defined(CONFIG_INV_MPU_IIO_ICM20648) | defined(CONFIG_INV_MPU_IIO_ICM20690) |
| static void inv_handle_triggered_eis(struct inv_mpu_state *st) |
| { |
| struct inv_eis *eis = &st->eis; |
| int delay; |
| |
| if (st->eis.eis_frame) { |
| inv_calc_precision(st); |
| delay = ((int)st->eis.fsync_delay) * st->eis.count_precision; |
| eis->fsync_timestamp = calc_frame_ave(st, delay); |
| inv_do_interpolation_gyro(st, |
| st->eis.prev_gyro, st->eis.prev_timestamp, |
| st->eis.current_gyro, st->eis.current_timestamp, |
| eis->fsync_timestamp, true); |
| pr_debug("fsync=%lld, curr=%lld, delay=%d\n", |
| eis->fsync_timestamp, eis->current_timestamp, delay); |
| inv_push_eis_ring(st, st->eis.current_gyro, false, |
| st->eis.current_timestamp - st->ts_algo.gyro_ts_shift |
| + MPU_4X_TS_GYRO_SHIFT); |
| eis->last_fsync_timestamp = eis->fsync_timestamp; |
| } else { |
| pr_debug("cur= %lld\n", st->eis.current_timestamp); |
| inv_push_eis_ring(st, st->eis.current_gyro, false, |
| st->eis.current_timestamp - st->ts_algo.gyro_ts_shift |
| + MPU_4X_TS_GYRO_SHIFT); |
| } |
| } |
| #else |
| static void inv_handle_triggered_eis(struct inv_mpu_state *st) |
| { |
| struct inv_eis *eis = &st->eis; |
| int delay; |
| |
| if ((st->eis.eis_frame && (st->eis.fsync_delay != 5)) || |
| (st->eis.eis_frame && (st->eis.fsync_delay == 5) && |
| (!st->eis.current_sync)) |
| ) { |
| inv_calc_precision(st); |
| delay = ((int)st->eis.fsync_delay) * st->eis.count_precision; |
| eis->fsync_timestamp = calc_frame_ave(st, delay); |
| inv_do_interpolation_gyro(st, |
| st->eis.prev_gyro, st->eis.prev_timestamp, |
| st->eis.current_gyro, st->eis.current_timestamp, |
| eis->fsync_timestamp, true); |
| pr_debug("fsync=%lld, curr=%lld, delay=%d\n", |
| eis->fsync_timestamp, eis->current_timestamp, delay); |
| inv_push_eis_ring(st, st->eis.current_gyro, false, |
| st->eis.current_timestamp); |
| eis->last_fsync_timestamp = eis->fsync_timestamp; |
| st->eis.eis_frame = false; |
| } else { |
| st->eis.current_sync = false; |
| pr_debug("cur= %lld\n", st->eis.current_timestamp); |
| inv_push_eis_ring(st, st->eis.current_gyro, false, |
| st->eis.current_timestamp); |
| } |
| } |
| #endif |
| static void inv_push_eis_buffer(struct inv_mpu_state *st, u64 t, int *q) |
| { |
| int ii; |
| |
| if (st->eis.eis_triggered) { |
| for (ii = 0; ii < 3; ii++) |
| st->eis.prev_gyro[ii] = st->eis.current_gyro[ii]; |
| st->eis.prev_timestamp = st->eis.current_timestamp; |
| |
| for (ii = 0; ii < 3; ii++) |
| st->eis.current_gyro[ii] = q[ii]; |
| st->eis.current_timestamp = t; |
| inv_handle_triggered_eis(st); |
| } else { |
| for (ii = 0; ii < 3; ii++) |
| st->eis.current_gyro[ii] = q[ii]; |
| st->eis.current_timestamp = t; |
| } |
| } |
| static int inv_push_16bytes_final(struct inv_mpu_state *st, int j, |
| s32 *q, u64 t, s16 accur) |
| { |
| struct iio_dev *indio_dev = iio_priv_to_dev(st); |
| u8 buf[IIO_BUFFER_BYTES]; |
| int ii; |
| |
| memcpy(buf, &st->sensor_l[j].header, sizeof(st->sensor_l[j].header)); |
| memcpy(buf + 2, &accur, sizeof(accur)); |
| memcpy(buf + 4, &q[0], sizeof(q[0])); |
| iio_push_to_buffers(indio_dev, buf); |
| for (ii = 0; ii < 2; ii++) |
| memcpy(buf + 4 * ii, &q[ii + 1], sizeof(q[ii])); |
| iio_push_to_buffers(indio_dev, buf); |
| inv_push_timestamp(indio_dev, t); |
| st->sensor_l[j].counter = 0; |
| if (st->sensor_l[j].wake_on) |
| st->wake_sensor_received = true; |
| |
| return 0; |
| } |
| int inv_push_16bytes_buffer(struct inv_mpu_state *st, u16 sensor, |
| u64 t, int *q, s16 accur) |
| { |
| int j; |
| |
| for (j = 0; j < SENSOR_L_NUM_MAX; j++) { |
| if (st->sensor_l[j].on && (st->sensor_l[j].base == sensor)) { |
| st->sensor_l[j].counter++; |
| if ((st->sensor_l[j].div != 0xffff) && |
| (st->sensor_l[j].counter >= |
| st->sensor_l[j].div)) { |
| pr_debug( |
| "Sensor_l = %d sensor = %d header [%04X] div [%d] ts [%lld] %d %d %d\n", |
| j, sensor, |
| st->sensor_l[j].header, |
| st->sensor_l[j].div, |
| t, q[0], q[1], q[2]); |
| inv_push_16bytes_final(st, j, q, t, accur); |
| } |
| } |
| } |
| return 0; |
| } |
| |
| void inv_convert_and_push_16bytes(struct inv_mpu_state *st, u16 hdr, |
| u8 *d, u64 t, s8 *m) |
| { |
| int i, j; |
| s32 in[3], out[3]; |
| |
| for (i = 0; i < 3; i++) |
| in[i] = be32_to_int(d + i * 4); |
| /* multiply with orientation matrix can be optimized like this */ |
| for (i = 0; i < 3; i++) |
| for (j = 0; j < 3; j++) |
| if (m[i * 3 + j]) |
| out[i] = in[j] * m[i * 3 + j]; |
| |
| inv_push_16bytes_buffer(st, hdr, t, out, 0); |
| } |
| |
| void inv_convert_and_push_8bytes(struct inv_mpu_state *st, u16 hdr, |
| u8 *d, u64 t, s8 *m) |
| { |
| int i, j; |
| s16 in[3], out[3]; |
| |
| for (i = 0; i < 3; i++) |
| in[i] = be16_to_cpup((__be16 *) (d + i * 2)); |
| |
| /* multiply with orientation matrix can be optimized like this */ |
| for (i = 0; i < 3; i++) |
| for (j = 0; j < 3; j++) |
| if (m[i * 3 + j]) |
| out[i] = in[j] * m[i * 3 + j]; |
| |
| inv_push_8bytes_buffer(st, hdr, t, out); |
| } |
| |
| int inv_push_special_8bytes_buffer(struct inv_mpu_state *st, |
| u16 hdr, u64 t, s16 *d) |
| { |
| struct iio_dev *indio_dev = iio_priv_to_dev(st); |
| u8 buf[IIO_BUFFER_BYTES]; |
| int j; |
| |
| memcpy(buf, &hdr, sizeof(hdr)); |
| memcpy(&buf[2], &d[0], sizeof(d[0])); |
| for (j = 0; j < 2; j++) |
| memcpy(&buf[4 + j * 2], &d[j + 1], sizeof(d[j])); |
| iio_push_to_buffers(indio_dev, buf); |
| inv_push_timestamp(indio_dev, t); |
| |
| return 0; |
| } |
| |
| static int inv_s16_gyro_push(struct inv_mpu_state *st, int i, s16 *raw, u64 t) |
| { |
| if (st->sensor_l[i].on) { |
| st->sensor_l[i].counter++; |
| if ((st->sensor_l[i].div != 0xffff) && |
| (st->sensor_l[i].counter >= st->sensor_l[i].div)) { |
| inv_push_special_8bytes_buffer(st, |
| st->sensor_l[i].header, t, raw); |
| st->sensor_l[i].counter = 0; |
| if (st->sensor_l[i].wake_on) |
| st->wake_sensor_received = true; |
| } |
| } |
| |
| return 0; |
| } |
| |
| static int inv_s32_gyro_push(struct inv_mpu_state *st, int i, s32 *calib, u64 t) |
| { |
| if (st->sensor_l[i].on) { |
| st->sensor_l[i].counter++; |
| if ((st->sensor_l[i].div != 0xffff) && |
| (st->sensor_l[i].counter >= st->sensor_l[i].div)) { |
| inv_push_16bytes_final(st, i, calib, t, 0); |
| st->sensor_l[i].counter = 0; |
| if (st->sensor_l[i].wake_on) |
| st->wake_sensor_received = true; |
| } |
| } |
| |
| return 0; |
| } |
| |
| int inv_push_gyro_data(struct inv_mpu_state *st, s16 *raw, s32 *calib, u64 t) |
| { |
| int gyro_data[] = {SENSOR_L_GYRO, SENSOR_L_GYRO_WAKE}; |
| int calib_data[] = {SENSOR_L_GYRO_CAL, SENSOR_L_GYRO_CAL_WAKE}; |
| int i; |
| |
| if (st->sensor_l[SENSOR_L_EIS_GYRO].on) |
| inv_push_eis_buffer(st, t, calib); |
| |
| for (i = 0; i < 2; i++) |
| inv_s16_gyro_push(st, gyro_data[i], raw, t); |
| for (i = 0; i < 2; i++) |
| inv_s32_gyro_push(st, calib_data[i], calib, t); |
| |
| return 0; |
| } |
| int inv_push_8bytes_buffer(struct inv_mpu_state *st, u16 sensor, u64 t, s16 *d) |
| { |
| struct iio_dev *indio_dev = iio_priv_to_dev(st); |
| u8 buf[IIO_BUFFER_BYTES]; |
| int ii, j; |
| |
| if ((sensor == STEP_DETECTOR_HDR) || |
| (sensor == STEP_DETECTOR_WAKE_HDR)) { |
| memcpy(buf, &sensor, sizeof(sensor)); |
| memcpy(&buf[2], &d[0], sizeof(d[0])); |
| for (j = 0; j < 2; j++) |
| memcpy(&buf[4 + j * 2], &d[j + 1], sizeof(d[j])); |
| iio_push_to_buffers(indio_dev, buf); |
| inv_push_timestamp(indio_dev, t); |
| if (sensor == STEP_DETECTOR_WAKE_HDR) |
| st->wake_sensor_received = true; |
| return 0; |
| } |
| for (ii = 0; ii < SENSOR_L_NUM_MAX; ii++) { |
| if (st->sensor_l[ii].on && |
| (st->sensor_l[ii].base == sensor) && |
| (st->sensor_l[ii].div != 0xffff)) { |
| st->sensor_l[ii].counter++; |
| if (st->sensor_l[ii].counter >= st->sensor_l[ii].div) { |
| pr_debug( |
| "Sensor_l = %d sensor = %d header [%04X] div [%d] ts [%lld] %d %d %d\n", |
| ii, sensor, st->sensor_l[ii].header, |
| st->sensor_l[ii].div, t, d[0], d[1], d[2]); |
| |
| memcpy(buf, &st->sensor_l[ii].header, |
| sizeof(st->sensor_l[ii].header)); |
| memcpy(&buf[2], &d[0], sizeof(d[0])); |
| for (j = 0; j < 2; j++) |
| memcpy(&buf[4 + j * 2], &d[j + 1], |
| sizeof(d[j])); |
| |
| iio_push_to_buffers(indio_dev, buf); |
| inv_push_timestamp(indio_dev, t); |
| st->sensor_l[ii].counter = 0; |
| if (st->sensor_l[ii].wake_on) |
| st->wake_sensor_received = true; |
| } |
| } |
| } |
| |
| return 0; |
| } |
| #ifdef CONFIG_INV_MPU_IIO_ICM20648 |
| /* Implemented activity to string function for BAC test */ |
| #define TILT_DETECTED 0x1000 |
| #define NONE 0x00 |
| #define DRIVE 0x01 |
| #define WALK 0x02 |
| #define RUN 0x04 |
| #define BIKE 0x08 |
| #define TILT 0x10 |
| #define STILL 0x20 |
| #define DRIVE_WALK (DRIVE | WALK) |
| #define DRIVE_RUN (DRIVE | RUN) |
| |
| char *act_string(s16 data) |
| { |
| data &= (~TILT); |
| switch (data) { |
| case NONE: |
| return "None"; |
| case DRIVE: |
| return "Drive"; |
| case WALK: |
| return "Walk"; |
| case RUN: |
| return "Run"; |
| case BIKE: |
| return "Bike"; |
| case STILL: |
| return "Still"; |
| case DRIVE_WALK: |
| return "drive and walk"; |
| case DRIVE_RUN: |
| return "drive and run"; |
| default: |
| return "Unknown"; |
| } |
| return "Unknown"; |
| } |
| |
| char *inv_tilt_check(s16 data) |
| { |
| if (data & TILT) |
| return "Tilt"; |
| else |
| return "None"; |
| } |
| |
| int inv_push_8bytes_kf(struct inv_mpu_state *st, u16 hdr, u64 t, s16 *d) |
| { |
| struct iio_dev *indio_dev = iio_priv_to_dev(st); |
| u8 buf[IIO_BUFFER_BYTES]; |
| int i; |
| |
| if (st->chip_config.activity_on) { |
| memcpy(buf, &hdr, sizeof(hdr)); |
| for (i = 0; i < 3; i++) |
| memcpy(&buf[2 + i * 2], &d[i], sizeof(d[i])); |
| |
| kfifo_in(&st->kf, buf, IIO_BUFFER_BYTES); |
| memcpy(buf, &t, sizeof(t)); |
| kfifo_in(&st->kf, buf, IIO_BUFFER_BYTES); |
| st->activity_size += IIO_BUFFER_BYTES * 2; |
| } |
| if (st->chip_config.tilt_enable) { |
| pr_debug("d[0] = %04X, [%X : %s] to [%X : %s]", |
| d[0], d[0] & 0x00FF, |
| inv_tilt_check(d[0] & 0x00FF), |
| (d[0] & 0xFF00) >> 8, inv_tilt_check((d[0] & 0xFF00) >> 8)); |
| sysfs_notify(&indio_dev->dev.kobj, NULL, "poll_tilt"); |
| } |
| |
| pr_debug("d[0] = %04X, [%X : %s] to [%X : %s]", d[0], d[0] & 0x00FF, |
| act_string(d[0] & 0x00FF), |
| (d[0] & 0xFF00) >> 8, act_string((d[0] & 0xFF00) >> 8)); |
| |
| read_be32_from_mem(st, &st->bac_drive_conf, BAC_DRIVE_CONFIDENCE); |
| read_be32_from_mem(st, &st->bac_walk_conf, BAC_WALK_CONFIDENCE); |
| read_be32_from_mem(st, &st->bac_smd_conf, BAC_SMD_CONFIDENCE); |
| read_be32_from_mem(st, &st->bac_bike_conf, BAC_BIKE_CONFIDENCE); |
| read_be32_from_mem(st, &st->bac_still_conf, BAC_STILL_CONFIDENCE); |
| read_be32_from_mem(st, &st->bac_run_conf, BAC_RUN_CONFIDENCE); |
| |
| return 0; |
| } |
| #endif |
| |
| int inv_send_steps(struct inv_mpu_state *st, int step, u64 ts) |
| { |
| s16 s[3]; |
| |
| s[0] = 0; |
| s[1] = (s16) (step & 0xffff); |
| s[2] = (s16) ((step >> 16) & 0xffff); |
| if (st->step_counter_l_on) |
| inv_push_special_8bytes_buffer(st, STEP_COUNTER_HDR, ts, s); |
| if (st->step_counter_wake_l_on) { |
| inv_push_special_8bytes_buffer(st, STEP_COUNTER_WAKE_HDR, |
| ts, s); |
| st->wake_sensor_received = true; |
| } |
| return 0; |
| } |
| |
| void inv_push_step_indicator(struct inv_mpu_state *st, u64 t) |
| { |
| s16 sen[3]; |
| #define STEP_INDICATOR_HEADER 0x0001 |
| |
| sen[0] = 0; |
| sen[1] = 0; |
| sen[2] = 0; |
| inv_push_8bytes_buffer(st, STEP_INDICATOR_HEADER, t, sen); |
| } |
| |
| /* |
| * inv_irq_handler() - Cache a timestamp at each data ready interrupt. |
| */ |
| static irqreturn_t inv_irq_handler(int irq, void *dev_id) |
| { |
| return IRQ_WAKE_THREAD; |
| } |
| |
| #ifdef TIMER_BASED_BATCHING |
| static enum hrtimer_restart inv_batch_timer_handler(struct hrtimer *timer) |
| { |
| struct inv_mpu_state *st = |
| container_of(timer, struct inv_mpu_state, hr_batch_timer); |
| |
| if (st->chip_config.gyro_enable || st->chip_config.accel_enable) { |
| hrtimer_forward_now(&st->hr_batch_timer, |
| ns_to_ktime(st->batch_timeout)); |
| schedule_work(&st->batch_work); |
| return HRTIMER_RESTART; |
| } |
| st->is_batch_timer_running = 0; |
| return HRTIMER_NORESTART; |
| } |
| #endif |
| |
| void inv_mpu_unconfigure_ring(struct iio_dev *indio_dev) |
| { |
| struct inv_mpu_state *st = iio_priv(indio_dev); |
| #ifdef KERNEL_VERSION_4_X |
| devm_free_irq(st->dev, st->irq, st); |
| devm_iio_kfifo_free(st->dev, indio_dev->buffer); |
| #else |
| free_irq(st->irq, st); |
| iio_kfifo_free(indio_dev->buffer); |
| #endif |
| }; |
| EXPORT_SYMBOL_GPL(inv_mpu_unconfigure_ring); |
| |
| #ifndef KERNEL_VERSION_4_X |
| static int inv_predisable(struct iio_dev *indio_dev) |
| { |
| return 0; |
| } |
| |
| static int inv_preenable(struct iio_dev *indio_dev) |
| { |
| return 0; |
| } |
| |
| static const struct iio_buffer_setup_ops inv_mpu_ring_setup_ops = { |
| .preenable = &inv_preenable, |
| .predisable = &inv_predisable, |
| }; |
| #endif |
| |
| int inv_mpu_configure_ring(struct iio_dev *indio_dev) |
| { |
| int ret; |
| struct inv_mpu_state *st = iio_priv(indio_dev); |
| struct iio_buffer *ring; |
| |
| #ifdef TIMER_BASED_BATCHING |
| /* configure hrtimer */ |
| hrtimer_init(&st->hr_batch_timer, CLOCK_BOOTTIME, HRTIMER_MODE_REL); |
| st->hr_batch_timer.function = inv_batch_timer_handler; |
| INIT_WORK(&st->batch_work, inv_batch_work); |
| #endif |
| #ifdef KERNEL_VERSION_4_X |
| ring = devm_iio_kfifo_allocate(st->dev); |
| if (!ring) |
| return -ENOMEM; |
| ring->scan_timestamp = true; |
| iio_device_attach_buffer(indio_dev, ring); |
| ret = devm_request_threaded_irq(st->dev, |
| st->irq, |
| inv_irq_handler, |
| inv_read_fifo, |
| IRQF_TRIGGER_RISING | IRQF_SHARED, |
| "inv_irq", |
| st); |
| if (ret) { |
| devm_iio_kfifo_free(st->dev, ring); |
| return ret; |
| } |
| |
| // this mode does not use ops |
| indio_dev->modes = INDIO_ALL_BUFFER_MODES; |
| |
| return ret; |
| #else |
| ring = iio_kfifo_allocate(indio_dev); |
| if (!ring) |
| return -ENOMEM; |
| indio_dev->buffer = ring; |
| /* setup ring buffer */ |
| ring->scan_timestamp = true; |
| indio_dev->setup_ops = &inv_mpu_ring_setup_ops; |
| ret = request_threaded_irq(st->irq, |
| inv_irq_handler, |
| inv_read_fifo, |
| IRQF_TRIGGER_RISING | IRQF_SHARED, |
| "inv_irq", |
| st); |
| if (ret) |
| goto error_iio_sw_rb_free; |
| |
| indio_dev->modes |= INDIO_BUFFER_HARDWARE; |
| |
| return 0; |
| error_iio_sw_rb_free: |
| iio_kfifo_free(indio_dev->buffer); |
| |
| return ret; |
| #endif |
| } |
| EXPORT_SYMBOL_GPL(inv_mpu_configure_ring); |