linux/drivers/iio/imu/kmx61.c
<<
>>
Prefs
   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
1446#ifdef CONFIG_PM_SLEEP
1447static int kmx61_suspend(struct device *dev)
1448{
1449        int ret;
1450        struct kmx61_data *data = i2c_get_clientdata(to_i2c_client(dev));
1451
1452        mutex_lock(&data->lock);
1453        ret = kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG,
1454                             false);
1455        mutex_unlock(&data->lock);
1456
1457        return ret;
1458}
1459
1460static int kmx61_resume(struct device *dev)
1461{
1462        u8 stby = 0;
1463        struct kmx61_data *data = i2c_get_clientdata(to_i2c_client(dev));
1464
1465        if (data->acc_stby)
1466                stby |= KMX61_ACC_STBY_BIT;
1467        if (data->mag_stby)
1468                stby |= KMX61_MAG_STBY_BIT;
1469
1470        return kmx61_set_mode(data, stby, KMX61_ACC | KMX61_MAG, true);
1471}
1472#endif
1473
1474#ifdef CONFIG_PM
1475static int kmx61_runtime_suspend(struct device *dev)
1476{
1477        struct kmx61_data *data = i2c_get_clientdata(to_i2c_client(dev));
1478        int ret;
1479
1480        mutex_lock(&data->lock);
1481        ret = kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG, true);
1482        mutex_unlock(&data->lock);
1483
1484        return ret;
1485}
1486
1487static int kmx61_runtime_resume(struct device *dev)
1488{
1489        struct kmx61_data *data = i2c_get_clientdata(to_i2c_client(dev));
1490        u8 stby = 0;
1491
1492        if (!data->acc_ps)
1493                stby |= KMX61_ACC_STBY_BIT;
1494        if (!data->mag_ps)
1495                stby |= KMX61_MAG_STBY_BIT;
1496
1497        return kmx61_set_mode(data, stby, KMX61_ACC | KMX61_MAG, true);
1498}
1499#endif
1500
1501static const struct dev_pm_ops kmx61_pm_ops = {
1502        SET_SYSTEM_SLEEP_PM_OPS(kmx61_suspend, kmx61_resume)
1503        SET_RUNTIME_PM_OPS(kmx61_runtime_suspend, kmx61_runtime_resume, NULL)
1504};
1505
1506static const struct acpi_device_id kmx61_acpi_match[] = {
1507        {"KMX61021", 0},
1508        {}
1509};
1510
1511MODULE_DEVICE_TABLE(acpi, kmx61_acpi_match);
1512
1513static const struct i2c_device_id kmx61_id[] = {
1514        {"kmx611021", 0},
1515        {}
1516};
1517
1518MODULE_DEVICE_TABLE(i2c, kmx61_id);
1519
1520static struct i2c_driver kmx61_driver = {
1521        .driver = {
1522                .name = KMX61_DRV_NAME,
1523                .acpi_match_table = ACPI_PTR(kmx61_acpi_match),
1524                .pm = &kmx61_pm_ops,
1525        },
1526        .probe          = kmx61_probe,
1527        .remove         = kmx61_remove,
1528        .id_table       = kmx61_id,
1529};
1530
1531module_i2c_driver(kmx61_driver);
1532
1533MODULE_AUTHOR("Daniel Baluta <daniel.baluta@intel.com>");
1534MODULE_DESCRIPTION("KMX61 accelerometer/magnetometer driver");
1535MODULE_LICENSE("GPL v2");
1536