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