cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

kmx61.c (36803B)


      1// SPDX-License-Identifier: GPL-2.0-only
      2/*
      3 * KMX61 - Kionix 6-axis Accelerometer/Magnetometer
      4 *
      5 * Copyright (c) 2014, Intel Corporation.
      6 *
      7 * IIO driver for KMX61 (7-bit I2C slave address 0x0E or 0x0F).
      8 */
      9
     10#include <linux/module.h>
     11#include <linux/i2c.h>
     12#include <linux/acpi.h>
     13#include <linux/interrupt.h>
     14#include <linux/pm.h>
     15#include <linux/pm_runtime.h>
     16#include <linux/iio/iio.h>
     17#include <linux/iio/sysfs.h>
     18#include <linux/iio/events.h>
     19#include <linux/iio/trigger.h>
     20#include <linux/iio/buffer.h>
     21#include <linux/iio/triggered_buffer.h>
     22#include <linux/iio/trigger_consumer.h>
     23
     24#define KMX61_DRV_NAME "kmx61"
     25#define KMX61_IRQ_NAME "kmx61_event"
     26
     27#define KMX61_REG_WHO_AM_I	0x00
     28#define KMX61_REG_INS1		0x01
     29#define KMX61_REG_INS2		0x02
     30
     31/*
     32 * three 16-bit accelerometer output registers for X/Y/Z axis
     33 * we use only XOUT_L as a base register, all other addresses
     34 * can be obtained by applying an offset and are provided here
     35 * only for clarity.
     36 */
     37#define KMX61_ACC_XOUT_L	0x0A
     38#define KMX61_ACC_XOUT_H	0x0B
     39#define KMX61_ACC_YOUT_L	0x0C
     40#define KMX61_ACC_YOUT_H	0x0D
     41#define KMX61_ACC_ZOUT_L	0x0E
     42#define KMX61_ACC_ZOUT_H	0x0F
     43
     44/*
     45 * one 16-bit temperature output register
     46 */
     47#define KMX61_TEMP_L		0x10
     48#define KMX61_TEMP_H		0x11
     49
     50/*
     51 * three 16-bit magnetometer output registers for X/Y/Z axis
     52 */
     53#define KMX61_MAG_XOUT_L	0x12
     54#define KMX61_MAG_XOUT_H	0x13
     55#define KMX61_MAG_YOUT_L	0x14
     56#define KMX61_MAG_YOUT_H	0x15
     57#define KMX61_MAG_ZOUT_L	0x16
     58#define KMX61_MAG_ZOUT_H	0x17
     59
     60#define KMX61_REG_INL		0x28
     61#define KMX61_REG_STBY		0x29
     62#define KMX61_REG_CTRL1		0x2A
     63#define KMX61_REG_CTRL2		0x2B
     64#define KMX61_REG_ODCNTL	0x2C
     65#define KMX61_REG_INC1		0x2D
     66
     67#define KMX61_REG_WUF_THRESH	0x3D
     68#define KMX61_REG_WUF_TIMER	0x3E
     69
     70#define KMX61_ACC_STBY_BIT	BIT(0)
     71#define KMX61_MAG_STBY_BIT	BIT(1)
     72#define KMX61_ACT_STBY_BIT	BIT(7)
     73
     74#define KMX61_ALL_STBY		(KMX61_ACC_STBY_BIT | KMX61_MAG_STBY_BIT)
     75
     76#define KMX61_REG_INS1_BIT_WUFS		BIT(1)
     77
     78#define KMX61_REG_INS2_BIT_ZP		BIT(0)
     79#define KMX61_REG_INS2_BIT_ZN		BIT(1)
     80#define KMX61_REG_INS2_BIT_YP		BIT(2)
     81#define KMX61_REG_INS2_BIT_YN		BIT(3)
     82#define KMX61_REG_INS2_BIT_XP		BIT(4)
     83#define KMX61_REG_INS2_BIT_XN		BIT(5)
     84
     85#define KMX61_REG_CTRL1_GSEL_MASK	0x03
     86
     87#define KMX61_REG_CTRL1_BIT_RES		BIT(4)
     88#define KMX61_REG_CTRL1_BIT_DRDYE	BIT(5)
     89#define KMX61_REG_CTRL1_BIT_WUFE	BIT(6)
     90#define KMX61_REG_CTRL1_BIT_BTSE	BIT(7)
     91
     92#define KMX61_REG_INC1_BIT_WUFS		BIT(0)
     93#define KMX61_REG_INC1_BIT_DRDYM	BIT(1)
     94#define KMX61_REG_INC1_BIT_DRDYA	BIT(2)
     95#define KMX61_REG_INC1_BIT_IEN		BIT(5)
     96
     97#define KMX61_ACC_ODR_SHIFT	0
     98#define KMX61_MAG_ODR_SHIFT	4
     99#define KMX61_ACC_ODR_MASK	0x0F
    100#define KMX61_MAG_ODR_MASK	0xF0
    101
    102#define KMX61_OWUF_MASK		0x7
    103
    104#define KMX61_DEFAULT_WAKE_THRESH	1
    105#define KMX61_DEFAULT_WAKE_DURATION	1
    106
    107#define KMX61_SLEEP_DELAY_MS	2000
    108
    109#define KMX61_CHIP_ID		0x12
    110
    111/* KMX61 devices */
    112#define KMX61_ACC	0x01
    113#define KMX61_MAG	0x02
    114
    115struct kmx61_data {
    116	struct i2c_client *client;
    117
    118	/* serialize access to non-atomic ops, e.g set_mode */
    119	struct mutex lock;
    120
    121	/* standby state */
    122	bool acc_stby;
    123	bool mag_stby;
    124
    125	/* power state */
    126	bool acc_ps;
    127	bool mag_ps;
    128
    129	/* config bits */
    130	u8 range;
    131	u8 odr_bits;
    132	u8 wake_thresh;
    133	u8 wake_duration;
    134
    135	/* accelerometer specific data */
    136	struct iio_dev *acc_indio_dev;
    137	struct iio_trigger *acc_dready_trig;
    138	struct iio_trigger *motion_trig;
    139	bool acc_dready_trig_on;
    140	bool motion_trig_on;
    141	bool ev_enable_state;
    142
    143	/* magnetometer specific data */
    144	struct iio_dev *mag_indio_dev;
    145	struct iio_trigger *mag_dready_trig;
    146	bool mag_dready_trig_on;
    147};
    148
    149enum kmx61_range {
    150	KMX61_RANGE_2G,
    151	KMX61_RANGE_4G,
    152	KMX61_RANGE_8G,
    153};
    154
    155enum kmx61_axis {
    156	KMX61_AXIS_X,
    157	KMX61_AXIS_Y,
    158	KMX61_AXIS_Z,
    159};
    160
    161static const u16 kmx61_uscale_table[] = {9582, 19163, 38326};
    162
    163static const struct {
    164	int val;
    165	int val2;
    166} kmx61_samp_freq_table[] = { {12, 500000},
    167			{25, 0},
    168			{50, 0},
    169			{100, 0},
    170			{200, 0},
    171			{400, 0},
    172			{800, 0},
    173			{1600, 0},
    174			{0, 781000},
    175			{1, 563000},
    176			{3, 125000},
    177			{6, 250000} };
    178
    179static const struct {
    180	int val;
    181	int val2;
    182	int odr_bits;
    183} kmx61_wake_up_odr_table[] = { {0, 781000, 0x00},
    184				 {1, 563000, 0x01},
    185				 {3, 125000, 0x02},
    186				 {6, 250000, 0x03},
    187				 {12, 500000, 0x04},
    188				 {25, 0, 0x05},
    189				 {50, 0, 0x06},
    190				 {100, 0, 0x06},
    191				 {200, 0, 0x06},
    192				 {400, 0, 0x06},
    193				 {800, 0, 0x06},
    194				 {1600, 0, 0x06} };
    195
    196static IIO_CONST_ATTR(accel_scale_available, "0.009582 0.019163 0.038326");
    197static IIO_CONST_ATTR(magn_scale_available, "0.001465");
    198static IIO_CONST_ATTR_SAMP_FREQ_AVAIL(
    199	"0.781000 1.563000 3.125000 6.250000 12.500000 25 50 100 200 400 800");
    200
    201static struct attribute *kmx61_acc_attributes[] = {
    202	&iio_const_attr_accel_scale_available.dev_attr.attr,
    203	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
    204	NULL,
    205};
    206
    207static struct attribute *kmx61_mag_attributes[] = {
    208	&iio_const_attr_magn_scale_available.dev_attr.attr,
    209	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
    210	NULL,
    211};
    212
    213static const struct attribute_group kmx61_acc_attribute_group = {
    214	.attrs = kmx61_acc_attributes,
    215};
    216
    217static const struct attribute_group kmx61_mag_attribute_group = {
    218	.attrs = kmx61_mag_attributes,
    219};
    220
    221static const struct iio_event_spec kmx61_event = {
    222	.type = IIO_EV_TYPE_THRESH,
    223	.dir = IIO_EV_DIR_EITHER,
    224	.mask_separate = BIT(IIO_EV_INFO_VALUE) |
    225			 BIT(IIO_EV_INFO_ENABLE) |
    226			 BIT(IIO_EV_INFO_PERIOD),
    227};
    228
    229#define KMX61_ACC_CHAN(_axis) { \
    230	.type = IIO_ACCEL, \
    231	.modified = 1, \
    232	.channel2 = IIO_MOD_ ## _axis, \
    233	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
    234	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) | \
    235				BIT(IIO_CHAN_INFO_SAMP_FREQ), \
    236	.address = KMX61_ACC, \
    237	.scan_index = KMX61_AXIS_ ## _axis, \
    238	.scan_type = { \
    239		.sign = 's', \
    240		.realbits = 12, \
    241		.storagebits = 16, \
    242		.shift = 4, \
    243		.endianness = IIO_LE, \
    244	}, \
    245	.event_spec = &kmx61_event, \
    246	.num_event_specs = 1 \
    247}
    248
    249#define KMX61_MAG_CHAN(_axis) { \
    250	.type = IIO_MAGN, \
    251	.modified = 1, \
    252	.channel2 = IIO_MOD_ ## _axis, \
    253	.address = KMX61_MAG, \
    254	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
    255	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) | \
    256				BIT(IIO_CHAN_INFO_SAMP_FREQ), \
    257	.scan_index = KMX61_AXIS_ ## _axis, \
    258	.scan_type = { \
    259		.sign = 's', \
    260		.realbits = 14, \
    261		.storagebits = 16, \
    262		.shift = 2, \
    263		.endianness = IIO_LE, \
    264	}, \
    265}
    266
    267static const struct iio_chan_spec kmx61_acc_channels[] = {
    268	KMX61_ACC_CHAN(X),
    269	KMX61_ACC_CHAN(Y),
    270	KMX61_ACC_CHAN(Z),
    271};
    272
    273static const struct iio_chan_spec kmx61_mag_channels[] = {
    274	KMX61_MAG_CHAN(X),
    275	KMX61_MAG_CHAN(Y),
    276	KMX61_MAG_CHAN(Z),
    277};
    278
    279static void kmx61_set_data(struct iio_dev *indio_dev, struct kmx61_data *data)
    280{
    281	struct kmx61_data **priv = iio_priv(indio_dev);
    282
    283	*priv = data;
    284}
    285
    286static struct kmx61_data *kmx61_get_data(struct iio_dev *indio_dev)
    287{
    288	return *(struct kmx61_data **)iio_priv(indio_dev);
    289}
    290
    291static int kmx61_convert_freq_to_bit(int val, int val2)
    292{
    293	int i;
    294
    295	for (i = 0; i < ARRAY_SIZE(kmx61_samp_freq_table); i++)
    296		if (val == kmx61_samp_freq_table[i].val &&
    297		    val2 == kmx61_samp_freq_table[i].val2)
    298			return i;
    299	return -EINVAL;
    300}
    301
    302static int kmx61_convert_wake_up_odr_to_bit(int val, int val2)
    303{
    304	int i;
    305
    306	for (i = 0; i < ARRAY_SIZE(kmx61_wake_up_odr_table); ++i)
    307		if (kmx61_wake_up_odr_table[i].val == val &&
    308			kmx61_wake_up_odr_table[i].val2 == val2)
    309				return kmx61_wake_up_odr_table[i].odr_bits;
    310	return -EINVAL;
    311}
    312
    313/**
    314 * kmx61_set_mode() - set KMX61 device operating mode
    315 * @data: kmx61 device private data pointer
    316 * @mode: bitmask, indicating operating mode for @device
    317 * @device: bitmask, indicating device for which @mode needs to be set
    318 * @update: update stby bits stored in device's private  @data
    319 *
    320 * For each sensor (accelerometer/magnetometer) there are two operating modes
    321 * STANDBY and OPERATION. Neither accel nor magn can be disabled independently
    322 * if they are both enabled. Internal sensors state is saved in acc_stby and
    323 * mag_stby members of driver's private @data.
    324 */
    325static int kmx61_set_mode(struct kmx61_data *data, u8 mode, u8 device,
    326			  bool update)
    327{
    328	int ret;
    329	int acc_stby = -1, mag_stby = -1;
    330
    331	ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_STBY);
    332	if (ret < 0) {
    333		dev_err(&data->client->dev, "Error reading reg_stby\n");
    334		return ret;
    335	}
    336	if (device & KMX61_ACC) {
    337		if (mode & KMX61_ACC_STBY_BIT) {
    338			ret |= KMX61_ACC_STBY_BIT;
    339			acc_stby = 1;
    340		} else {
    341			ret &= ~KMX61_ACC_STBY_BIT;
    342			acc_stby = 0;
    343		}
    344	}
    345
    346	if (device & KMX61_MAG) {
    347		if (mode & KMX61_MAG_STBY_BIT) {
    348			ret |= KMX61_MAG_STBY_BIT;
    349			mag_stby = 1;
    350		} else {
    351			ret &= ~KMX61_MAG_STBY_BIT;
    352			mag_stby = 0;
    353		}
    354	}
    355
    356	if (mode & KMX61_ACT_STBY_BIT)
    357		ret |= KMX61_ACT_STBY_BIT;
    358
    359	ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_STBY, ret);
    360	if (ret < 0) {
    361		dev_err(&data->client->dev, "Error writing reg_stby\n");
    362		return ret;
    363	}
    364
    365	if (acc_stby != -1 && update)
    366		data->acc_stby = acc_stby;
    367	if (mag_stby != -1 && update)
    368		data->mag_stby = mag_stby;
    369
    370	return 0;
    371}
    372
    373static int kmx61_get_mode(struct kmx61_data *data, u8 *mode, u8 device)
    374{
    375	int ret;
    376
    377	ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_STBY);
    378	if (ret < 0) {
    379		dev_err(&data->client->dev, "Error reading reg_stby\n");
    380		return ret;
    381	}
    382	*mode = 0;
    383
    384	if (device & KMX61_ACC) {
    385		if (ret & KMX61_ACC_STBY_BIT)
    386			*mode |= KMX61_ACC_STBY_BIT;
    387		else
    388			*mode &= ~KMX61_ACC_STBY_BIT;
    389	}
    390
    391	if (device & KMX61_MAG) {
    392		if (ret & KMX61_MAG_STBY_BIT)
    393			*mode |= KMX61_MAG_STBY_BIT;
    394		else
    395			*mode &= ~KMX61_MAG_STBY_BIT;
    396	}
    397
    398	return 0;
    399}
    400
    401static int kmx61_set_wake_up_odr(struct kmx61_data *data, int val, int val2)
    402{
    403	int ret, odr_bits;
    404
    405	odr_bits = kmx61_convert_wake_up_odr_to_bit(val, val2);
    406	if (odr_bits < 0)
    407		return odr_bits;
    408
    409	ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_CTRL2,
    410					odr_bits);
    411	if (ret < 0)
    412		dev_err(&data->client->dev, "Error writing reg_ctrl2\n");
    413	return ret;
    414}
    415
    416static int kmx61_set_odr(struct kmx61_data *data, int val, int val2, u8 device)
    417{
    418	int ret;
    419	u8 mode;
    420	int lodr_bits, odr_bits;
    421
    422	ret = kmx61_get_mode(data, &mode, KMX61_ACC | KMX61_MAG);
    423	if (ret < 0)
    424		return ret;
    425
    426	lodr_bits = kmx61_convert_freq_to_bit(val, val2);
    427	if (lodr_bits < 0)
    428		return lodr_bits;
    429
    430	/* To change ODR, accel and magn must be in STDBY */
    431	ret = kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG,
    432			     true);
    433	if (ret < 0)
    434		return ret;
    435
    436	odr_bits = 0;
    437	if (device & KMX61_ACC)
    438		odr_bits |= lodr_bits << KMX61_ACC_ODR_SHIFT;
    439	if (device & KMX61_MAG)
    440		odr_bits |= lodr_bits << KMX61_MAG_ODR_SHIFT;
    441
    442	ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_ODCNTL,
    443					odr_bits);
    444	if (ret < 0)
    445		return ret;
    446
    447	data->odr_bits = odr_bits;
    448
    449	if (device & KMX61_ACC) {
    450		ret = kmx61_set_wake_up_odr(data, val, val2);
    451		if (ret)
    452			return ret;
    453	}
    454
    455	return kmx61_set_mode(data, mode, KMX61_ACC | KMX61_MAG, true);
    456}
    457
    458static int kmx61_get_odr(struct kmx61_data *data, int *val, int *val2,
    459			 u8 device)
    460{
    461	u8 lodr_bits;
    462
    463	if (device & KMX61_ACC)
    464		lodr_bits = (data->odr_bits >> KMX61_ACC_ODR_SHIFT) &
    465			     KMX61_ACC_ODR_MASK;
    466	else if (device & KMX61_MAG)
    467		lodr_bits = (data->odr_bits >> KMX61_MAG_ODR_SHIFT) &
    468			     KMX61_MAG_ODR_MASK;
    469	else
    470		return -EINVAL;
    471
    472	if (lodr_bits >= ARRAY_SIZE(kmx61_samp_freq_table))
    473		return -EINVAL;
    474
    475	*val = kmx61_samp_freq_table[lodr_bits].val;
    476	*val2 = kmx61_samp_freq_table[lodr_bits].val2;
    477
    478	return 0;
    479}
    480
    481static int kmx61_set_range(struct kmx61_data *data, u8 range)
    482{
    483	int ret;
    484
    485	ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_CTRL1);
    486	if (ret < 0) {
    487		dev_err(&data->client->dev, "Error reading reg_ctrl1\n");
    488		return ret;
    489	}
    490
    491	ret &= ~KMX61_REG_CTRL1_GSEL_MASK;
    492	ret |= range & KMX61_REG_CTRL1_GSEL_MASK;
    493
    494	ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_CTRL1, ret);
    495	if (ret < 0) {
    496		dev_err(&data->client->dev, "Error writing reg_ctrl1\n");
    497		return ret;
    498	}
    499
    500	data->range = range;
    501
    502	return 0;
    503}
    504
    505static int kmx61_set_scale(struct kmx61_data *data, u16 uscale)
    506{
    507	int ret, i;
    508	u8  mode;
    509
    510	for (i = 0; i < ARRAY_SIZE(kmx61_uscale_table); i++) {
    511		if (kmx61_uscale_table[i] == uscale) {
    512			ret = kmx61_get_mode(data, &mode,
    513					     KMX61_ACC | KMX61_MAG);
    514			if (ret < 0)
    515				return ret;
    516
    517			ret = kmx61_set_mode(data, KMX61_ALL_STBY,
    518					     KMX61_ACC | KMX61_MAG, true);
    519			if (ret < 0)
    520				return ret;
    521
    522			ret = kmx61_set_range(data, i);
    523			if (ret < 0)
    524				return ret;
    525
    526			return  kmx61_set_mode(data, mode,
    527					       KMX61_ACC | KMX61_MAG, true);
    528		}
    529	}
    530	return -EINVAL;
    531}
    532
    533static int kmx61_chip_init(struct kmx61_data *data)
    534{
    535	int ret, val, val2;
    536
    537	ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_WHO_AM_I);
    538	if (ret < 0) {
    539		dev_err(&data->client->dev, "Error reading who_am_i\n");
    540		return ret;
    541	}
    542
    543	if (ret != KMX61_CHIP_ID) {
    544		dev_err(&data->client->dev,
    545			"Wrong chip id, got %x expected %x\n",
    546			 ret, KMX61_CHIP_ID);
    547		return -EINVAL;
    548	}
    549
    550	/* set accel 12bit, 4g range */
    551	ret = kmx61_set_range(data, KMX61_RANGE_4G);
    552	if (ret < 0)
    553		return ret;
    554
    555	ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_ODCNTL);
    556	if (ret < 0) {
    557		dev_err(&data->client->dev, "Error reading reg_odcntl\n");
    558		return ret;
    559	}
    560	data->odr_bits = ret;
    561
    562	/*
    563	 * set output data rate for wake up (motion detection) function
    564	 * to match data rate for accelerometer sampling
    565	 */
    566	ret = kmx61_get_odr(data, &val, &val2, KMX61_ACC);
    567	if (ret < 0)
    568		return ret;
    569
    570	ret = kmx61_set_wake_up_odr(data, val, val2);
    571	if (ret < 0)
    572		return ret;
    573
    574	/* set acc/magn to OPERATION mode */
    575	ret = kmx61_set_mode(data, 0, KMX61_ACC | KMX61_MAG, true);
    576	if (ret < 0)
    577		return ret;
    578
    579	data->wake_thresh = KMX61_DEFAULT_WAKE_THRESH;
    580	data->wake_duration = KMX61_DEFAULT_WAKE_DURATION;
    581
    582	return 0;
    583}
    584
    585static int kmx61_setup_new_data_interrupt(struct kmx61_data *data,
    586					  bool status, u8 device)
    587{
    588	u8 mode;
    589	int ret;
    590
    591	ret = kmx61_get_mode(data, &mode, KMX61_ACC | KMX61_MAG);
    592	if (ret < 0)
    593		return ret;
    594
    595	ret = kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG, true);
    596	if (ret < 0)
    597		return ret;
    598
    599	ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_INC1);
    600	if (ret < 0) {
    601		dev_err(&data->client->dev, "Error reading reg_ctrl1\n");
    602		return ret;
    603	}
    604
    605	if (status) {
    606		ret |= KMX61_REG_INC1_BIT_IEN;
    607		if (device & KMX61_ACC)
    608			ret |= KMX61_REG_INC1_BIT_DRDYA;
    609		if (device & KMX61_MAG)
    610			ret |=  KMX61_REG_INC1_BIT_DRDYM;
    611	} else {
    612		ret &= ~KMX61_REG_INC1_BIT_IEN;
    613		if (device & KMX61_ACC)
    614			ret &= ~KMX61_REG_INC1_BIT_DRDYA;
    615		if (device & KMX61_MAG)
    616			ret &= ~KMX61_REG_INC1_BIT_DRDYM;
    617	}
    618	ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_INC1, ret);
    619	if (ret < 0) {
    620		dev_err(&data->client->dev, "Error writing reg_int_ctrl1\n");
    621		return ret;
    622	}
    623
    624	ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_CTRL1);
    625	if (ret < 0) {
    626		dev_err(&data->client->dev, "Error reading reg_ctrl1\n");
    627		return ret;
    628	}
    629
    630	if (status)
    631		ret |= KMX61_REG_CTRL1_BIT_DRDYE;
    632	else
    633		ret &= ~KMX61_REG_CTRL1_BIT_DRDYE;
    634
    635	ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_CTRL1, ret);
    636	if (ret < 0) {
    637		dev_err(&data->client->dev, "Error writing reg_ctrl1\n");
    638		return ret;
    639	}
    640
    641	return kmx61_set_mode(data, mode, KMX61_ACC | KMX61_MAG, true);
    642}
    643
    644static int kmx61_chip_update_thresholds(struct kmx61_data *data)
    645{
    646	int ret;
    647
    648	ret = i2c_smbus_write_byte_data(data->client,
    649					KMX61_REG_WUF_TIMER,
    650					data->wake_duration);
    651	if (ret < 0) {
    652		dev_err(&data->client->dev, "Errow writing reg_wuf_timer\n");
    653		return ret;
    654	}
    655
    656	ret = i2c_smbus_write_byte_data(data->client,
    657					KMX61_REG_WUF_THRESH,
    658					data->wake_thresh);
    659	if (ret < 0)
    660		dev_err(&data->client->dev, "Error writing reg_wuf_thresh\n");
    661
    662	return ret;
    663}
    664
    665static int kmx61_setup_any_motion_interrupt(struct kmx61_data *data,
    666					    bool status)
    667{
    668	u8 mode;
    669	int ret;
    670
    671	ret = kmx61_get_mode(data, &mode, KMX61_ACC | KMX61_MAG);
    672	if (ret < 0)
    673		return ret;
    674
    675	ret = kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG, true);
    676	if (ret < 0)
    677		return ret;
    678
    679	ret = kmx61_chip_update_thresholds(data);
    680	if (ret < 0)
    681		return ret;
    682
    683	ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_INC1);
    684	if (ret < 0) {
    685		dev_err(&data->client->dev, "Error reading reg_inc1\n");
    686		return ret;
    687	}
    688	if (status)
    689		ret |= (KMX61_REG_INC1_BIT_IEN | KMX61_REG_INC1_BIT_WUFS);
    690	else
    691		ret &= ~(KMX61_REG_INC1_BIT_IEN | KMX61_REG_INC1_BIT_WUFS);
    692
    693	ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_INC1, ret);
    694	if (ret < 0) {
    695		dev_err(&data->client->dev, "Error writing reg_inc1\n");
    696		return ret;
    697	}
    698
    699	ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_CTRL1);
    700	if (ret < 0) {
    701		dev_err(&data->client->dev, "Error reading reg_ctrl1\n");
    702		return ret;
    703	}
    704
    705	if (status)
    706		ret |= KMX61_REG_CTRL1_BIT_WUFE | KMX61_REG_CTRL1_BIT_BTSE;
    707	else
    708		ret &= ~(KMX61_REG_CTRL1_BIT_WUFE | KMX61_REG_CTRL1_BIT_BTSE);
    709
    710	ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_CTRL1, ret);
    711	if (ret < 0) {
    712		dev_err(&data->client->dev, "Error writing reg_ctrl1\n");
    713		return ret;
    714	}
    715	mode |= KMX61_ACT_STBY_BIT;
    716	return kmx61_set_mode(data, mode, KMX61_ACC | KMX61_MAG, true);
    717}
    718
    719/**
    720 * kmx61_set_power_state() - set power state for kmx61 @device
    721 * @data: kmx61 device private pointer
    722 * @on: power state to be set for @device
    723 * @device: bitmask indicating device for which @on state needs to be set
    724 *
    725 * Notice that when ACC power state needs to be set to ON and MAG is in
    726 * OPERATION then we know that kmx61_runtime_resume was already called
    727 * so we must set ACC OPERATION mode here. The same happens when MAG power
    728 * state needs to be set to ON and ACC is in OPERATION.
    729 */
    730static int kmx61_set_power_state(struct kmx61_data *data, bool on, u8 device)
    731{
    732#ifdef CONFIG_PM
    733	int ret;
    734
    735	if (device & KMX61_ACC) {
    736		if (on && !data->acc_ps && !data->mag_stby) {
    737			ret = kmx61_set_mode(data, 0, KMX61_ACC, true);
    738			if (ret < 0)
    739				return ret;
    740		}
    741		data->acc_ps = on;
    742	}
    743	if (device & KMX61_MAG) {
    744		if (on && !data->mag_ps && !data->acc_stby) {
    745			ret = kmx61_set_mode(data, 0, KMX61_MAG, true);
    746			if (ret < 0)
    747				return ret;
    748		}
    749		data->mag_ps = on;
    750	}
    751
    752	if (on) {
    753		ret = pm_runtime_resume_and_get(&data->client->dev);
    754	} else {
    755		pm_runtime_mark_last_busy(&data->client->dev);
    756		ret = pm_runtime_put_autosuspend(&data->client->dev);
    757	}
    758	if (ret < 0) {
    759		dev_err(&data->client->dev,
    760			"Failed: kmx61_set_power_state for %d, ret %d\n",
    761			on, ret);
    762
    763		return ret;
    764	}
    765#endif
    766	return 0;
    767}
    768
    769static int kmx61_read_measurement(struct kmx61_data *data, u8 base, u8 offset)
    770{
    771	int ret;
    772	u8 reg = base + offset * 2;
    773
    774	ret = i2c_smbus_read_word_data(data->client, reg);
    775	if (ret < 0)
    776		dev_err(&data->client->dev, "failed to read reg at %x\n", reg);
    777
    778	return ret;
    779}
    780
    781static int kmx61_read_raw(struct iio_dev *indio_dev,
    782			  struct iio_chan_spec const *chan, int *val,
    783			  int *val2, long mask)
    784{
    785	int ret;
    786	u8 base_reg;
    787	struct kmx61_data *data = kmx61_get_data(indio_dev);
    788
    789	switch (mask) {
    790	case IIO_CHAN_INFO_RAW:
    791		switch (chan->type) {
    792		case IIO_ACCEL:
    793			base_reg = KMX61_ACC_XOUT_L;
    794			break;
    795		case IIO_MAGN:
    796			base_reg = KMX61_MAG_XOUT_L;
    797			break;
    798		default:
    799			return -EINVAL;
    800		}
    801		mutex_lock(&data->lock);
    802
    803		ret = kmx61_set_power_state(data, true, chan->address);
    804		if (ret) {
    805			mutex_unlock(&data->lock);
    806			return ret;
    807		}
    808
    809		ret = kmx61_read_measurement(data, base_reg, chan->scan_index);
    810		if (ret < 0) {
    811			kmx61_set_power_state(data, false, chan->address);
    812			mutex_unlock(&data->lock);
    813			return ret;
    814		}
    815		*val = sign_extend32(ret >> chan->scan_type.shift,
    816				     chan->scan_type.realbits - 1);
    817		ret = kmx61_set_power_state(data, false, chan->address);
    818
    819		mutex_unlock(&data->lock);
    820		if (ret)
    821			return ret;
    822		return IIO_VAL_INT;
    823	case IIO_CHAN_INFO_SCALE:
    824		switch (chan->type) {
    825		case IIO_ACCEL:
    826			*val = 0;
    827			*val2 = kmx61_uscale_table[data->range];
    828			return IIO_VAL_INT_PLUS_MICRO;
    829		case IIO_MAGN:
    830			/* 14 bits res, 1465 microGauss per magn count */
    831			*val = 0;
    832			*val2 = 1465;
    833			return IIO_VAL_INT_PLUS_MICRO;
    834		default:
    835			return -EINVAL;
    836		}
    837	case IIO_CHAN_INFO_SAMP_FREQ:
    838		if (chan->type != IIO_ACCEL && chan->type != IIO_MAGN)
    839			return -EINVAL;
    840
    841		mutex_lock(&data->lock);
    842		ret = kmx61_get_odr(data, val, val2, chan->address);
    843		mutex_unlock(&data->lock);
    844		if (ret)
    845			return -EINVAL;
    846		return IIO_VAL_INT_PLUS_MICRO;
    847	}
    848	return -EINVAL;
    849}
    850
    851static int kmx61_write_raw(struct iio_dev *indio_dev,
    852			   struct iio_chan_spec const *chan, int val,
    853			   int val2, long mask)
    854{
    855	int ret;
    856	struct kmx61_data *data = kmx61_get_data(indio_dev);
    857
    858	switch (mask) {
    859	case IIO_CHAN_INFO_SAMP_FREQ:
    860		if (chan->type != IIO_ACCEL && chan->type != IIO_MAGN)
    861			return -EINVAL;
    862
    863		mutex_lock(&data->lock);
    864		ret = kmx61_set_odr(data, val, val2, chan->address);
    865		mutex_unlock(&data->lock);
    866		return ret;
    867	case IIO_CHAN_INFO_SCALE:
    868		switch (chan->type) {
    869		case IIO_ACCEL:
    870			if (val != 0)
    871				return -EINVAL;
    872			mutex_lock(&data->lock);
    873			ret = kmx61_set_scale(data, val2);
    874			mutex_unlock(&data->lock);
    875			return ret;
    876		default:
    877			return -EINVAL;
    878		}
    879	default:
    880		return -EINVAL;
    881	}
    882}
    883
    884static int kmx61_read_event(struct iio_dev *indio_dev,
    885			    const struct iio_chan_spec *chan,
    886			    enum iio_event_type type,
    887			    enum iio_event_direction dir,
    888			    enum iio_event_info info,
    889			    int *val, int *val2)
    890{
    891	struct kmx61_data *data = kmx61_get_data(indio_dev);
    892
    893	*val2 = 0;
    894	switch (info) {
    895	case IIO_EV_INFO_VALUE:
    896		*val = data->wake_thresh;
    897		return IIO_VAL_INT;
    898	case IIO_EV_INFO_PERIOD:
    899		*val = data->wake_duration;
    900		return IIO_VAL_INT;
    901	default:
    902		return -EINVAL;
    903	}
    904}
    905
    906static int kmx61_write_event(struct iio_dev *indio_dev,
    907			     const struct iio_chan_spec *chan,
    908			     enum iio_event_type type,
    909			     enum iio_event_direction dir,
    910			     enum iio_event_info info,
    911			     int val, int val2)
    912{
    913	struct kmx61_data *data = kmx61_get_data(indio_dev);
    914
    915	if (data->ev_enable_state)
    916		return -EBUSY;
    917
    918	switch (info) {
    919	case IIO_EV_INFO_VALUE:
    920		data->wake_thresh = val;
    921		return IIO_VAL_INT;
    922	case IIO_EV_INFO_PERIOD:
    923		data->wake_duration = val;
    924		return IIO_VAL_INT;
    925	default:
    926		return -EINVAL;
    927	}
    928}
    929
    930static int kmx61_read_event_config(struct iio_dev *indio_dev,
    931				   const struct iio_chan_spec *chan,
    932				   enum iio_event_type type,
    933				   enum iio_event_direction dir)
    934{
    935	struct kmx61_data *data = kmx61_get_data(indio_dev);
    936
    937	return data->ev_enable_state;
    938}
    939
    940static int kmx61_write_event_config(struct iio_dev *indio_dev,
    941				    const struct iio_chan_spec *chan,
    942				    enum iio_event_type type,
    943				    enum iio_event_direction dir,
    944				    int state)
    945{
    946	struct kmx61_data *data = kmx61_get_data(indio_dev);
    947	int ret = 0;
    948
    949	if (state && data->ev_enable_state)
    950		return 0;
    951
    952	mutex_lock(&data->lock);
    953
    954	if (!state && data->motion_trig_on) {
    955		data->ev_enable_state = false;
    956		goto err_unlock;
    957	}
    958
    959	ret = kmx61_set_power_state(data, state, KMX61_ACC);
    960	if (ret < 0)
    961		goto err_unlock;
    962
    963	ret = kmx61_setup_any_motion_interrupt(data, state);
    964	if (ret < 0) {
    965		kmx61_set_power_state(data, false, KMX61_ACC);
    966		goto err_unlock;
    967	}
    968
    969	data->ev_enable_state = state;
    970
    971err_unlock:
    972	mutex_unlock(&data->lock);
    973
    974	return ret;
    975}
    976
    977static int kmx61_acc_validate_trigger(struct iio_dev *indio_dev,
    978				      struct iio_trigger *trig)
    979{
    980	struct kmx61_data *data = kmx61_get_data(indio_dev);
    981
    982	if (data->acc_dready_trig != trig && data->motion_trig != trig)
    983		return -EINVAL;
    984
    985	return 0;
    986}
    987
    988static int kmx61_mag_validate_trigger(struct iio_dev *indio_dev,
    989				      struct iio_trigger *trig)
    990{
    991	struct kmx61_data *data = kmx61_get_data(indio_dev);
    992
    993	if (data->mag_dready_trig != trig)
    994		return -EINVAL;
    995
    996	return 0;
    997}
    998
    999static const struct iio_info kmx61_acc_info = {
   1000	.read_raw		= kmx61_read_raw,
   1001	.write_raw		= kmx61_write_raw,
   1002	.attrs			= &kmx61_acc_attribute_group,
   1003	.read_event_value	= kmx61_read_event,
   1004	.write_event_value	= kmx61_write_event,
   1005	.read_event_config	= kmx61_read_event_config,
   1006	.write_event_config	= kmx61_write_event_config,
   1007	.validate_trigger	= kmx61_acc_validate_trigger,
   1008};
   1009
   1010static const struct iio_info kmx61_mag_info = {
   1011	.read_raw		= kmx61_read_raw,
   1012	.write_raw		= kmx61_write_raw,
   1013	.attrs			= &kmx61_mag_attribute_group,
   1014	.validate_trigger	= kmx61_mag_validate_trigger,
   1015};
   1016
   1017
   1018static int kmx61_data_rdy_trigger_set_state(struct iio_trigger *trig,
   1019					    bool state)
   1020{
   1021	int ret = 0;
   1022	u8 device;
   1023
   1024	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
   1025	struct kmx61_data *data = kmx61_get_data(indio_dev);
   1026
   1027	mutex_lock(&data->lock);
   1028
   1029	if (!state && data->ev_enable_state && data->motion_trig_on) {
   1030		data->motion_trig_on = false;
   1031		goto err_unlock;
   1032	}
   1033
   1034	if (data->acc_dready_trig == trig || data->motion_trig == trig)
   1035		device = KMX61_ACC;
   1036	else
   1037		device = KMX61_MAG;
   1038
   1039	ret = kmx61_set_power_state(data, state, device);
   1040	if (ret < 0)
   1041		goto err_unlock;
   1042
   1043	if (data->acc_dready_trig == trig || data->mag_dready_trig == trig)
   1044		ret = kmx61_setup_new_data_interrupt(data, state, device);
   1045	else
   1046		ret = kmx61_setup_any_motion_interrupt(data, state);
   1047	if (ret < 0) {
   1048		kmx61_set_power_state(data, false, device);
   1049		goto err_unlock;
   1050	}
   1051
   1052	if (data->acc_dready_trig == trig)
   1053		data->acc_dready_trig_on = state;
   1054	else if (data->mag_dready_trig == trig)
   1055		data->mag_dready_trig_on = state;
   1056	else
   1057		data->motion_trig_on = state;
   1058err_unlock:
   1059	mutex_unlock(&data->lock);
   1060
   1061	return ret;
   1062}
   1063
   1064static void kmx61_trig_reenable(struct iio_trigger *trig)
   1065{
   1066	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
   1067	struct kmx61_data *data = kmx61_get_data(indio_dev);
   1068	int ret;
   1069
   1070	ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_INL);
   1071	if (ret < 0)
   1072		dev_err(&data->client->dev, "Error reading reg_inl\n");
   1073}
   1074
   1075static const struct iio_trigger_ops kmx61_trigger_ops = {
   1076	.set_trigger_state = kmx61_data_rdy_trigger_set_state,
   1077	.reenable = kmx61_trig_reenable,
   1078};
   1079
   1080static irqreturn_t kmx61_event_handler(int irq, void *private)
   1081{
   1082	struct kmx61_data *data = private;
   1083	struct iio_dev *indio_dev = data->acc_indio_dev;
   1084	int ret;
   1085
   1086	ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_INS1);
   1087	if (ret < 0) {
   1088		dev_err(&data->client->dev, "Error reading reg_ins1\n");
   1089		goto ack_intr;
   1090	}
   1091
   1092	if (ret & KMX61_REG_INS1_BIT_WUFS) {
   1093		ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_INS2);
   1094		if (ret < 0) {
   1095			dev_err(&data->client->dev, "Error reading reg_ins2\n");
   1096			goto ack_intr;
   1097		}
   1098
   1099		if (ret & KMX61_REG_INS2_BIT_XN)
   1100			iio_push_event(indio_dev,
   1101				       IIO_MOD_EVENT_CODE(IIO_ACCEL,
   1102				       0,
   1103				       IIO_MOD_X,
   1104				       IIO_EV_TYPE_THRESH,
   1105				       IIO_EV_DIR_FALLING),
   1106				       0);
   1107
   1108		if (ret & KMX61_REG_INS2_BIT_XP)
   1109			iio_push_event(indio_dev,
   1110				       IIO_MOD_EVENT_CODE(IIO_ACCEL,
   1111				       0,
   1112				       IIO_MOD_X,
   1113				       IIO_EV_TYPE_THRESH,
   1114				       IIO_EV_DIR_RISING),
   1115				       0);
   1116
   1117		if (ret & KMX61_REG_INS2_BIT_YN)
   1118			iio_push_event(indio_dev,
   1119				       IIO_MOD_EVENT_CODE(IIO_ACCEL,
   1120				       0,
   1121				       IIO_MOD_Y,
   1122				       IIO_EV_TYPE_THRESH,
   1123				       IIO_EV_DIR_FALLING),
   1124				       0);
   1125
   1126		if (ret & KMX61_REG_INS2_BIT_YP)
   1127			iio_push_event(indio_dev,
   1128				       IIO_MOD_EVENT_CODE(IIO_ACCEL,
   1129				       0,
   1130				       IIO_MOD_Y,
   1131				       IIO_EV_TYPE_THRESH,
   1132				       IIO_EV_DIR_RISING),
   1133				       0);
   1134
   1135		if (ret & KMX61_REG_INS2_BIT_ZN)
   1136			iio_push_event(indio_dev,
   1137				       IIO_MOD_EVENT_CODE(IIO_ACCEL,
   1138				       0,
   1139				       IIO_MOD_Z,
   1140				       IIO_EV_TYPE_THRESH,
   1141				       IIO_EV_DIR_FALLING),
   1142				       0);
   1143
   1144		if (ret & KMX61_REG_INS2_BIT_ZP)
   1145			iio_push_event(indio_dev,
   1146				       IIO_MOD_EVENT_CODE(IIO_ACCEL,
   1147				       0,
   1148				       IIO_MOD_Z,
   1149				       IIO_EV_TYPE_THRESH,
   1150				       IIO_EV_DIR_RISING),
   1151				       0);
   1152	}
   1153
   1154ack_intr:
   1155	ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_CTRL1);
   1156	if (ret < 0)
   1157		dev_err(&data->client->dev, "Error reading reg_ctrl1\n");
   1158
   1159	ret |= KMX61_REG_CTRL1_BIT_RES;
   1160	ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_CTRL1, ret);
   1161	if (ret < 0)
   1162		dev_err(&data->client->dev, "Error writing reg_ctrl1\n");
   1163
   1164	ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_INL);
   1165	if (ret < 0)
   1166		dev_err(&data->client->dev, "Error reading reg_inl\n");
   1167
   1168	return IRQ_HANDLED;
   1169}
   1170
   1171static irqreturn_t kmx61_data_rdy_trig_poll(int irq, void *private)
   1172{
   1173	struct kmx61_data *data = private;
   1174
   1175	if (data->acc_dready_trig_on)
   1176		iio_trigger_poll(data->acc_dready_trig);
   1177	if (data->mag_dready_trig_on)
   1178		iio_trigger_poll(data->mag_dready_trig);
   1179
   1180	if (data->motion_trig_on)
   1181		iio_trigger_poll(data->motion_trig);
   1182
   1183	if (data->ev_enable_state)
   1184		return IRQ_WAKE_THREAD;
   1185	return IRQ_HANDLED;
   1186}
   1187
   1188static irqreturn_t kmx61_trigger_handler(int irq, void *p)
   1189{
   1190	struct iio_poll_func *pf = p;
   1191	struct iio_dev *indio_dev = pf->indio_dev;
   1192	struct kmx61_data *data = kmx61_get_data(indio_dev);
   1193	int bit, ret, i = 0;
   1194	u8 base;
   1195	s16 buffer[8];
   1196
   1197	if (indio_dev == data->acc_indio_dev)
   1198		base = KMX61_ACC_XOUT_L;
   1199	else
   1200		base = KMX61_MAG_XOUT_L;
   1201
   1202	mutex_lock(&data->lock);
   1203	for_each_set_bit(bit, indio_dev->active_scan_mask,
   1204			 indio_dev->masklength) {
   1205		ret = kmx61_read_measurement(data, base, bit);
   1206		if (ret < 0) {
   1207			mutex_unlock(&data->lock);
   1208			goto err;
   1209		}
   1210		buffer[i++] = ret;
   1211	}
   1212	mutex_unlock(&data->lock);
   1213
   1214	iio_push_to_buffers(indio_dev, buffer);
   1215err:
   1216	iio_trigger_notify_done(indio_dev->trig);
   1217
   1218	return IRQ_HANDLED;
   1219}
   1220
   1221static const char *kmx61_match_acpi_device(struct device *dev)
   1222{
   1223	const struct acpi_device_id *id;
   1224
   1225	id = acpi_match_device(dev->driver->acpi_match_table, dev);
   1226	if (!id)
   1227		return NULL;
   1228	return dev_name(dev);
   1229}
   1230
   1231static struct iio_dev *kmx61_indiodev_setup(struct kmx61_data *data,
   1232					    const struct iio_info *info,
   1233					    const struct iio_chan_spec *chan,
   1234					    int num_channels,
   1235					    const char *name)
   1236{
   1237	struct iio_dev *indio_dev;
   1238
   1239	indio_dev = devm_iio_device_alloc(&data->client->dev, sizeof(data));
   1240	if (!indio_dev)
   1241		return ERR_PTR(-ENOMEM);
   1242
   1243	kmx61_set_data(indio_dev, data);
   1244
   1245	indio_dev->channels = chan;
   1246	indio_dev->num_channels = num_channels;
   1247	indio_dev->name = name;
   1248	indio_dev->modes = INDIO_DIRECT_MODE;
   1249	indio_dev->info = info;
   1250
   1251	return indio_dev;
   1252}
   1253
   1254static struct iio_trigger *kmx61_trigger_setup(struct kmx61_data *data,
   1255					       struct iio_dev *indio_dev,
   1256					       const char *tag)
   1257{
   1258	struct iio_trigger *trig;
   1259	int ret;
   1260
   1261	trig = devm_iio_trigger_alloc(&data->client->dev,
   1262				      "%s-%s-dev%d",
   1263				      indio_dev->name,
   1264				      tag,
   1265				      iio_device_id(indio_dev));
   1266	if (!trig)
   1267		return ERR_PTR(-ENOMEM);
   1268
   1269	trig->ops = &kmx61_trigger_ops;
   1270	iio_trigger_set_drvdata(trig, indio_dev);
   1271
   1272	ret = iio_trigger_register(trig);
   1273	if (ret)
   1274		return ERR_PTR(ret);
   1275
   1276	return trig;
   1277}
   1278
   1279static int kmx61_probe(struct i2c_client *client,
   1280		       const struct i2c_device_id *id)
   1281{
   1282	int ret;
   1283	struct kmx61_data *data;
   1284	const char *name = NULL;
   1285
   1286	data = devm_kzalloc(&client->dev, sizeof(*data), GFP_KERNEL);
   1287	if (!data)
   1288		return -ENOMEM;
   1289
   1290	i2c_set_clientdata(client, data);
   1291	data->client = client;
   1292
   1293	mutex_init(&data->lock);
   1294
   1295	if (id)
   1296		name = id->name;
   1297	else if (ACPI_HANDLE(&client->dev))
   1298		name = kmx61_match_acpi_device(&client->dev);
   1299	else
   1300		return -ENODEV;
   1301
   1302	data->acc_indio_dev =
   1303		kmx61_indiodev_setup(data, &kmx61_acc_info,
   1304				     kmx61_acc_channels,
   1305				     ARRAY_SIZE(kmx61_acc_channels),
   1306				     name);
   1307	if (IS_ERR(data->acc_indio_dev))
   1308		return PTR_ERR(data->acc_indio_dev);
   1309
   1310	data->mag_indio_dev =
   1311		kmx61_indiodev_setup(data, &kmx61_mag_info,
   1312				     kmx61_mag_channels,
   1313				     ARRAY_SIZE(kmx61_mag_channels),
   1314				     name);
   1315	if (IS_ERR(data->mag_indio_dev))
   1316		return PTR_ERR(data->mag_indio_dev);
   1317
   1318	ret = kmx61_chip_init(data);
   1319	if (ret < 0)
   1320		return ret;
   1321
   1322	if (client->irq > 0) {
   1323		ret = devm_request_threaded_irq(&client->dev, client->irq,
   1324						kmx61_data_rdy_trig_poll,
   1325						kmx61_event_handler,
   1326						IRQF_TRIGGER_RISING,
   1327						KMX61_IRQ_NAME,
   1328						data);
   1329		if (ret)
   1330			goto err_chip_uninit;
   1331
   1332		data->acc_dready_trig =
   1333			kmx61_trigger_setup(data, data->acc_indio_dev,
   1334					    "dready");
   1335		if (IS_ERR(data->acc_dready_trig)) {
   1336			ret = PTR_ERR(data->acc_dready_trig);
   1337			goto err_chip_uninit;
   1338		}
   1339
   1340		data->mag_dready_trig =
   1341			kmx61_trigger_setup(data, data->mag_indio_dev,
   1342					    "dready");
   1343		if (IS_ERR(data->mag_dready_trig)) {
   1344			ret = PTR_ERR(data->mag_dready_trig);
   1345			goto err_trigger_unregister_acc_dready;
   1346		}
   1347
   1348		data->motion_trig =
   1349			kmx61_trigger_setup(data, data->acc_indio_dev,
   1350					    "any-motion");
   1351		if (IS_ERR(data->motion_trig)) {
   1352			ret = PTR_ERR(data->motion_trig);
   1353			goto err_trigger_unregister_mag_dready;
   1354		}
   1355
   1356		ret = iio_triggered_buffer_setup(data->acc_indio_dev,
   1357						 &iio_pollfunc_store_time,
   1358						 kmx61_trigger_handler,
   1359						 NULL);
   1360		if (ret < 0) {
   1361			dev_err(&data->client->dev,
   1362				"Failed to setup acc triggered buffer\n");
   1363			goto err_trigger_unregister_motion;
   1364		}
   1365
   1366		ret = iio_triggered_buffer_setup(data->mag_indio_dev,
   1367						 &iio_pollfunc_store_time,
   1368						 kmx61_trigger_handler,
   1369						 NULL);
   1370		if (ret < 0) {
   1371			dev_err(&data->client->dev,
   1372				"Failed to setup mag triggered buffer\n");
   1373			goto err_buffer_cleanup_acc;
   1374		}
   1375	}
   1376
   1377	ret = pm_runtime_set_active(&client->dev);
   1378	if (ret < 0)
   1379		goto err_buffer_cleanup_mag;
   1380
   1381	pm_runtime_enable(&client->dev);
   1382	pm_runtime_set_autosuspend_delay(&client->dev, KMX61_SLEEP_DELAY_MS);
   1383	pm_runtime_use_autosuspend(&client->dev);
   1384
   1385	ret = iio_device_register(data->acc_indio_dev);
   1386	if (ret < 0) {
   1387		dev_err(&client->dev, "Failed to register acc iio device\n");
   1388		goto err_pm_cleanup;
   1389	}
   1390
   1391	ret = iio_device_register(data->mag_indio_dev);
   1392	if (ret < 0) {
   1393		dev_err(&client->dev, "Failed to register mag iio device\n");
   1394		goto err_iio_unregister_acc;
   1395	}
   1396
   1397	return 0;
   1398
   1399err_iio_unregister_acc:
   1400	iio_device_unregister(data->acc_indio_dev);
   1401err_pm_cleanup:
   1402	pm_runtime_dont_use_autosuspend(&client->dev);
   1403	pm_runtime_disable(&client->dev);
   1404err_buffer_cleanup_mag:
   1405	if (client->irq > 0)
   1406		iio_triggered_buffer_cleanup(data->mag_indio_dev);
   1407err_buffer_cleanup_acc:
   1408	if (client->irq > 0)
   1409		iio_triggered_buffer_cleanup(data->acc_indio_dev);
   1410err_trigger_unregister_motion:
   1411	iio_trigger_unregister(data->motion_trig);
   1412err_trigger_unregister_mag_dready:
   1413	iio_trigger_unregister(data->mag_dready_trig);
   1414err_trigger_unregister_acc_dready:
   1415	iio_trigger_unregister(data->acc_dready_trig);
   1416err_chip_uninit:
   1417	kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG, true);
   1418	return ret;
   1419}
   1420
   1421static int kmx61_remove(struct i2c_client *client)
   1422{
   1423	struct kmx61_data *data = i2c_get_clientdata(client);
   1424
   1425	iio_device_unregister(data->acc_indio_dev);
   1426	iio_device_unregister(data->mag_indio_dev);
   1427
   1428	pm_runtime_disable(&client->dev);
   1429	pm_runtime_set_suspended(&client->dev);
   1430
   1431	if (client->irq > 0) {
   1432		iio_triggered_buffer_cleanup(data->acc_indio_dev);
   1433		iio_triggered_buffer_cleanup(data->mag_indio_dev);
   1434		iio_trigger_unregister(data->acc_dready_trig);
   1435		iio_trigger_unregister(data->mag_dready_trig);
   1436		iio_trigger_unregister(data->motion_trig);
   1437	}
   1438
   1439	mutex_lock(&data->lock);
   1440	kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG, true);
   1441	mutex_unlock(&data->lock);
   1442
   1443	return 0;
   1444}
   1445
   1446static int kmx61_suspend(struct device *dev)
   1447{
   1448	int ret;
   1449	struct kmx61_data *data = i2c_get_clientdata(to_i2c_client(dev));
   1450
   1451	mutex_lock(&data->lock);
   1452	ret = kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG,
   1453			     false);
   1454	mutex_unlock(&data->lock);
   1455
   1456	return ret;
   1457}
   1458
   1459static int kmx61_resume(struct device *dev)
   1460{
   1461	u8 stby = 0;
   1462	struct kmx61_data *data = i2c_get_clientdata(to_i2c_client(dev));
   1463
   1464	if (data->acc_stby)
   1465		stby |= KMX61_ACC_STBY_BIT;
   1466	if (data->mag_stby)
   1467		stby |= KMX61_MAG_STBY_BIT;
   1468
   1469	return kmx61_set_mode(data, stby, KMX61_ACC | KMX61_MAG, true);
   1470}
   1471
   1472static int kmx61_runtime_suspend(struct device *dev)
   1473{
   1474	struct kmx61_data *data = i2c_get_clientdata(to_i2c_client(dev));
   1475	int ret;
   1476
   1477	mutex_lock(&data->lock);
   1478	ret = kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG, true);
   1479	mutex_unlock(&data->lock);
   1480
   1481	return ret;
   1482}
   1483
   1484static int kmx61_runtime_resume(struct device *dev)
   1485{
   1486	struct kmx61_data *data = i2c_get_clientdata(to_i2c_client(dev));
   1487	u8 stby = 0;
   1488
   1489	if (!data->acc_ps)
   1490		stby |= KMX61_ACC_STBY_BIT;
   1491	if (!data->mag_ps)
   1492		stby |= KMX61_MAG_STBY_BIT;
   1493
   1494	return kmx61_set_mode(data, stby, KMX61_ACC | KMX61_MAG, true);
   1495}
   1496
   1497static const struct dev_pm_ops kmx61_pm_ops = {
   1498	SYSTEM_SLEEP_PM_OPS(kmx61_suspend, kmx61_resume)
   1499	RUNTIME_PM_OPS(kmx61_runtime_suspend, kmx61_runtime_resume, NULL)
   1500};
   1501
   1502static const struct acpi_device_id kmx61_acpi_match[] = {
   1503	{"KMX61021", 0},
   1504	{}
   1505};
   1506
   1507MODULE_DEVICE_TABLE(acpi, kmx61_acpi_match);
   1508
   1509static const struct i2c_device_id kmx61_id[] = {
   1510	{"kmx611021", 0},
   1511	{}
   1512};
   1513
   1514MODULE_DEVICE_TABLE(i2c, kmx61_id);
   1515
   1516static struct i2c_driver kmx61_driver = {
   1517	.driver = {
   1518		.name = KMX61_DRV_NAME,
   1519		.acpi_match_table = ACPI_PTR(kmx61_acpi_match),
   1520		.pm = pm_ptr(&kmx61_pm_ops),
   1521	},
   1522	.probe		= kmx61_probe,
   1523	.remove		= kmx61_remove,
   1524	.id_table	= kmx61_id,
   1525};
   1526
   1527module_i2c_driver(kmx61_driver);
   1528
   1529MODULE_AUTHOR("Daniel Baluta <daniel.baluta@intel.com>");
   1530MODULE_DESCRIPTION("KMX61 accelerometer/magnetometer driver");
   1531MODULE_LICENSE("GPL v2");