drivers/hwmon/lis3dh_acc.c

   1 /******************** (C) COPYRIGHT 2010 STMicroelectronics ********************
   2  *
   3  * File Name          : lis3dh_acc.c
   4  * Authors            : MSH - Motion Mems BU - Application Team
   5  *                    : Matteo Dameno (matteo.dameno@st.com)
   6  *                    : Carmine Iascone (carmine.iascone@st.com)
   7  *                    : Both authors are willing to be considered the contact
   8  *                    : and update points for the driver.
   9  * Version            : V.1.0.8
  10  * Date               : 2010/Apr/01
  11  * Description        : LIS3DH accelerometer sensor API
  12  *
  13  *******************************************************************************
  14  *
  15  * This program is free software; you can redistribute it and/or modify
  16  * it under the terms of the GNU General Public License version 2 as
  17  * published by the Free Software Foundation.
  18  *
  19  * THE PRESENT SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES
  20  * OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED, FOR THE SOLE
  21  * PURPOSE TO SUPPORT YOUR APPLICATION DEVELOPMENT.
  22  * AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
  23  * INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
  24  * CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
  25  * INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
  26  *
  27  * THIS SOFTWARE IS SPECIFICALLY DESIGNED FOR EXCLUSIVE USE WITH ST PARTS.
  28  *
  29  ******************************************************************************
  30  Revision 1.0.0 05/11/09
  31  First Release;
  32  Revision 1.0.3 22/01/2010
  33   Linux K&R Compliant Release;
  34  Revision 1.0.5 16/08/2010
  35   modified _get_acceleration_data function;
  36   modified _update_odr function;
  37   manages 2 interrupts;
  38  Revision 1.0.6 15/11/2010
  39   supports sysfs;
  40   no more support for ioctl;
  41  Revision 1.0.7 26/11/2010
  42   checks for availability of interrupts pins
  43   correction on FUZZ and FLAT values;
  44  Revision 1.0.8 2010/Apr/01
  45   corrects a bug in interrupt pin management in 1.0.7
  46
  47  ******************************************************************************/
  48
  49 #include <linux/err.h>
  50 #include <linux/errno.h>
  51 #include <linux/delay.h>
  52 #include <linux/fs.h>
  53 #include <linux/i2c.h>
  54 #include <linux/input.h>
  55 #include <linux/uaccess.h>
  56 #include <linux/workqueue.h>
  57 #include <linux/irq.h>
  58 #include <linux/gpio.h>
  59 #include <linux/interrupt.h>
  60 #include <linux/slab.h>
  61 #include <linux/input/lis3dh.h>
  62 #include <linux/earlysuspend.h>
  63 #include <drm/drm_mode.h>
  64
  65 #define DEBUG   0
  66 #define DEBUG_DATA_LOG 0
  67
  68 #define G_MAX           16000
  69
  70 #define LIS3DH_6D_REPORT_DELAY 200
  71 #define LIS3DH_6D_REPORT_CNT   40
  72
  73 #define SENSITIVITY_2G          1       /**     mg/LSB  */
  74 #define SENSITIVITY_4G          2       /**     mg/LSB  */
  75 #define SENSITIVITY_8G          4       /**     mg/LSB  */
  76 #define SENSITIVITY_16G         12      /**     mg/LSB  */
  77
  78 /* Accelerometer Sensor Full Scale */
  79 #define LIS3DH_ACC_FS_MASK              0x30
  80 #define LIS3DH_ACC_G_2G         0x00
  81 #define LIS3DH_ACC_G_4G         0x10
  82 #define LIS3DH_ACC_G_8G         0x20
  83 #define LIS3DH_ACC_G_16G                0x30
  84
  85 /* Accelerometer Sensor Operating Mode */
  86 #define LIS3DH_ACC_ENABLE       0x01
  87 #define LIS3DH_ACC_DISABLE      0x00
  88
  89 #define HIGH_RESOLUTION         0x08
  90
  91 #define AXISDATA_REG            0x28
  92 #define WHOAMI_LIS3DH_ACC       0x32    /*      Expected content for WAI */
  93
  94 /*      CONTROL REGISTERS       */
  95 #define WHO_AM_I                0x0F    /*      WhoAmI register         */
  96 #define TEMP_CFG_REG            0x1F    /*      temper sens control reg */
  97 /* ctrl 1: ODR3 ODR2 ODR ODR0 LPen Zenable Yenable Zenable */
  98 #define CTRL_REG1               0x20    /*      control reg 1           */
  99 #define CTRL_REG2               0x21    /*      control reg 2           */
 100 #define CTRL_REG3               0x22    /*      control reg 3           */
 101 #define CTRL_REG4               0x23    /*      control reg 4           */
 102 #define CTRL_REG5               0x24    /*      control reg 5           */
 103 #define CTRL_REG6               0x25    /*      control reg 6           */
 104
 105 #define FIFO_CTRL_REG           0x2E    /*      FiFo control reg        */
 106
 107 #define INT_CFG1                0x30    /*      interrupt 1 config      */
 108 #define INT_SRC1                0x31    /*      interrupt 1 source      */
 109 #define INT_THS1                0x32    /*      interrupt 1 threshold   */
 110 #define INT_DUR1                0x33    /*      interrupt 1 duration    */
 111
 112 #define TT_CFG                  0x38    /*      tap config              */
 113 #define TT_SRC                  0x39    /*      tap source              */
 114 #define TT_THS                  0x3A    /*      tap threshold           */
 115 #define TT_LIM                  0x3B    /*      tap time limit          */
 116 #define TT_TLAT                 0x3C    /*      tap time latency        */
 117 #define TT_TW                   0x3D    /*      tap time window         */
 118 /*      end CONTROL REGISTRES   */
 119
 120 #define ENABLE_HIGH_RESOLUTION  1
 121
 122 #define LIS3DH_ACC_PM_OFF               0x00
 123 #define LIS3DH_ACC_ENABLE_ALL_AXES      0x07
 124
 125 #define PMODE_MASK                      0x08
 126 #define ODR_MASK                        0XF0
 127
 128 #define ODR1            0x10  /* 1Hz output data rate */
 129 #define ODR10           0x20  /* 10Hz output data rate */
 130 #define ODR25           0x30  /* 25Hz output data rate */
 131 #define ODR50           0x40  /* 50Hz output data rate */
 132 #define ODR100          0x50  /* 100Hz output data rate */
 133 #define ODR200          0x60  /* 200Hz output data rate */
 134 #define ODR400          0x70  /* 400Hz output data rate */
 135 #define ODR1250         0x90  /* 1250Hz output data rate */
 136
 137 #define IA                      0x40
 138 #define ZH                      0x20
 139 #define ZL                      0x10
 140 #define YH                      0x08
 141 #define YL                      0x04
 142 #define XH                      0x02
 143 #define XL                      0x01
 144
 145 /* CTRL REG BITS*/
 146 #define CTRL_REG3_I1_AOI1       0x40
 147 #define CTRL_REG6_I2_TAPEN      0x80
 148 #define CTRL_REG6_HLACTIVE      0x02
 149
 150 #define NO_MASK                 0xFF
 151 #define INT1_DURATION_MASK      0x7F
 152 #define INT1_THRESHOLD_MASK     0x7F
 153 #define TAP_CFG_MASK            0x3F
 154 #define TAP_THS_MASK            0x7F
 155 #define TAP_TLIM_MASK           0x7F
 156 #define TAP_TLAT_MASK           NO_MASK
 157 #define TAP_TW_MASK             NO_MASK
 158
 159 /* TAP_SOURCE_REG BIT */
 160 #define DTAP                    0x20
 161 #define STAP                    0x10
 162 #define SIGNTAP                 0x08
 163 #define ZTAP                    0x04
 164 #define YTAP                    0x02
 165 #define XTAZ                    0x01
 166
 167 #define FUZZ                    0
 168 #define FLAT                    0
 169 #define I2C_RETRY_DELAY         5
 170 #define I2C_RETRIES             5
 171 #define I2C_AUTO_INCREMENT      0x80
 172
 173 /* RESUME STATE INDICES */
 174 #define RES_CTRL_REG1           0
 175 #define RES_CTRL_REG2           1
 176 #define RES_CTRL_REG3           2
 177 #define RES_CTRL_REG4           3
 178 #define RES_CTRL_REG5           4
 179 #define RES_CTRL_REG6           5
 180
 181 #define RES_INT_CFG1            6
 182 #define RES_INT_THS1            7
 183 #define RES_INT_DUR1            8
 184
 185 #define RES_TT_CFG              9
 186 #define RES_TT_THS              10
 187 #define RES_TT_LIM              11
 188 #define RES_TT_TLAT             12
 189 #define RES_TT_TW               13
 190
 191 #define RES_TEMP_CFG_REG        14
 192 #define RES_REFERENCE_REG       15
 193 #define RES_FIFO_CTRL_REG       16
 194
 195 #define RESUME_ENTRIES          17
 196 /* end RESUME STATE INDICES */
 197
 198 struct {
 199         unsigned int cutoff_ms;
 200         unsigned int mask;
 201 } lis3dh_acc_odr_table[] = {
 202                 {    1, ODR1250 },
 203                 {    3, ODR400  },
 204                 {    5, ODR200  },
 205                 {   10, ODR100  },
 206                 {   20, ODR50   },
 207                 {   40, ODR25   },
 208                 {  100, ODR10   },
 209                 { 1000, ODR1    },
 210 };
 211
 212 struct lis3dh_acc_data {
 213         struct input_dev *input_dev;
 214         struct i2c_client *client;
 215         struct lis3dh_acc_platform_data *pdata;
 216
 217         struct mutex lock;
 218         struct delayed_work input_work;
 219         int report_cnt;
 220         int report_interval;
 221
 222         int hw_initialized;
 223         /* hw_working=-1 means not tested yet */
 224         int hw_working;
 225         int enabled;
 226         int need_resume;
 227 #ifdef CONFIG_HAS_EARLYSUSPEND
 228         struct early_suspend es;
 229 #else
 230         struct notifier_block screen_notifier;
 231 #endif
 232
 233         u8 sensitivity;
 234
 235         u8 resume_state[RESUME_ENTRIES];
 236
 237         int irq1;
 238         int irq2;
 239
 240 #ifdef DEBUG
 241         u8 reg_addr;
 242 #endif
 243 };
 244
 245 static int lis3dh_acc_i2c_read(struct lis3dh_acc_data *acc,
 246                                u8 reg, u8 *buf, int len)
 247 {
 248         if (len > 1)
 249                 reg |= I2C_AUTO_INCREMENT;
 250
 251         return i2c_smbus_read_i2c_block_data(acc->client, reg, len, buf);
 252 }
 253
 254 static int lis3dh_acc_i2c_write(struct lis3dh_acc_data *acc,
 255                                 u8 reg, u8 *buf, int len)
 256 {
 257         if (len > 1)
 258                 reg |= I2C_AUTO_INCREMENT;
 259
 260         return i2c_smbus_write_i2c_block_data(acc->client, reg, len, buf);
 261 }
 262
 263 static int lis3dh_acc_hw_init(struct lis3dh_acc_data *acc)
 264 {
 265         int err = -1;
 266         u8 buf[7];
 267
 268         dev_dbg(&acc->client->dev, "hw init start\n");
 269
 270         /* LSM303DLHC doesn't have WHO_AM_I register */
 271         if (acc->pdata->model != MODEL_LSM303DLHC) {
 272                 err = lis3dh_acc_i2c_read(acc, WHO_AM_I, buf, 1);
 273                 if (err < 0) {
 274                         dev_warn(&acc->client->dev, "Error reading WHO_AM_I: "
 275                                         "is device available/working?\n");
 276                         goto err_firstread;
 277                 }
 278         }
 279
 280         acc->hw_working = 1;
 281
 282         buf[0] = acc->resume_state[RES_CTRL_REG1];
 283         err = lis3dh_acc_i2c_write(acc, CTRL_REG1, buf, 1);
 284         if (err < 0)
 285                 goto err_resume_state;
 286
 287         buf[0] = acc->resume_state[RES_TEMP_CFG_REG];
 288         err = lis3dh_acc_i2c_write(acc, TEMP_CFG_REG, buf, 1);
 289         if (err < 0)
 290                 goto err_resume_state;
 291
 292         buf[0] = acc->resume_state[RES_FIFO_CTRL_REG];
 293         err = lis3dh_acc_i2c_write(acc, FIFO_CTRL_REG, buf, 1);
 294         if (err < 0)
 295                 goto err_resume_state;
 296
 297         buf[0] = acc->resume_state[RES_INT_THS1];
 298         err = lis3dh_acc_i2c_write(acc, INT_THS1, buf, 1);
 299         if (err < 0)
 300                 goto err_resume_state;
 301         buf[0] = acc->resume_state[RES_INT_DUR1];
 302         err = lis3dh_acc_i2c_write(acc, INT_DUR1, buf, 1);
 303         if (err < 0)
 304                 goto err_resume_state;
 305
 306         buf[0] = acc->resume_state[RES_INT_CFG1];
 307         err = lis3dh_acc_i2c_write(acc, INT_CFG1, buf, 1);
 308         if (err < 0)
 309                 goto err_resume_state;
 310
 311         buf[0] = acc->resume_state[RES_CTRL_REG3];
 312         err = lis3dh_acc_i2c_write(acc, CTRL_REG3, buf, 1);
 313         if (err < 0)
 314                 goto err_resume_state;
 315
 316         acc->hw_initialized = 1;
 317         dev_dbg(&acc->client->dev, "hw init done\n");
 318         return 0;
 319
 320 err_firstread:
 321         acc->hw_working = 0;
 322 err_resume_state:
 323         acc->hw_initialized = 0;
 324         dev_err(&acc->client->dev, "hw init error 0x%x,0x%x: %d\n", buf[0],
 325                         buf[1], err);
 326         return err;
 327 }
 328
 329 static void lis3dh_acc_device_power_off(struct lis3dh_acc_data *acc)
 330 {
 331         int err;
 332         u8 buf;
 333
 334         buf = LIS3DH_ACC_PM_OFF;
 335         err = lis3dh_acc_i2c_write(acc, CTRL_REG1, &buf, 1);
 336         if (err < 0)
 337                 dev_err(&acc->client->dev, "soft power off failed: %d\n", err);
 338
 339         if (acc->hw_initialized) {
 340                 if (acc->pdata->power_off)
 341                         acc->pdata->power_off();
 342                 acc->hw_initialized = 0;
 343         }
 344 }
 345
 346 static int lis3dh_acc_device_power_on(struct lis3dh_acc_data *acc)
 347 {
 348         int err;
 349
 350         if (!acc->hw_initialized) {
 351                 err = lis3dh_acc_hw_init(acc);
 352                 if (acc->hw_working == 1 && err < 0) {
 353                         lis3dh_acc_device_power_off(acc);
 354                         return err;
 355                 }
 356         }
 357
 358         return 0;
 359 }
 360
 361 int lis3dh_acc_update_g_range(struct lis3dh_acc_data *acc, u8 new_g_range)
 362 {
 363         int err = -1;
 364         u8 sensitivity;
 365         u8 buf;
 366         u8 updated_val;
 367         u8 init_val;
 368         u8 new_val;
 369         u8 mask = LIS3DH_ACC_FS_MASK | HIGH_RESOLUTION;
 370         u8 fs_bits = 0;
 371
 372         switch (new_g_range) {
 373         case 2:
 374                 fs_bits = LIS3DH_ACC_G_2G;
 375                 sensitivity = SENSITIVITY_2G;
 376                 break;
 377         case 4:
 378                 fs_bits = LIS3DH_ACC_G_4G;
 379                 sensitivity = SENSITIVITY_4G;
 380                 break;
 381         case 8:
 382                 fs_bits = LIS3DH_ACC_G_8G;
 383                 sensitivity = SENSITIVITY_8G;
 384                 break;
 385         case 16:
 386                 fs_bits = LIS3DH_ACC_G_16G;
 387                 sensitivity = SENSITIVITY_16G;
 388                 break;
 389         default:
 390                 dev_err(&acc->client->dev, "invalid grange requested: %u\n",
 391                                 new_g_range);
 392                 return -EINVAL;
 393         }
 394
 395         /* Updates configuration register 4,
 396          * which contains g range setting */
 397         err = lis3dh_acc_i2c_read(acc, CTRL_REG4, &buf, 1);
 398         if (err < 0)
 399                 goto error;
 400         init_val = buf;
 401         acc->resume_state[RES_CTRL_REG4] = init_val;
 402         new_val = fs_bits | HIGH_RESOLUTION;
 403         updated_val = ((mask & new_val) | ((~mask) & init_val));
 404         buf = updated_val;
 405         err = lis3dh_acc_i2c_write(acc, CTRL_REG4, &buf, 1);
 406         if (err < 0)
 407                 goto error;
 408         acc->resume_state[RES_CTRL_REG4] = updated_val;
 409         acc->sensitivity = sensitivity;
 410
 411         return err;
 412 error:
 413         dev_err(&acc->client->dev, "update grange failed 0x%x: %d\n", buf, err);
 414         return err;
 415 }
 416
 417 int lis3dh_acc_update_odr(struct lis3dh_acc_data *acc, int poll_interval_ms)
 418 {
 419         int err = -1;
 420         int i;
 421         u8 config;
 422
 423         /* Following, looks for the longest possible odr interval scrolling the
 424          * odr_table vector from the end (shortest interval) backward (longest
 425          * interval), to support the poll_interval requested by the system.
 426          * It must be the longest interval lower then the poll interval.*/
 427         for (i = ARRAY_SIZE(lis3dh_acc_odr_table) - 1; i >= 0; i--) {
 428                 if (lis3dh_acc_odr_table[i].cutoff_ms <= poll_interval_ms)
 429                         break;
 430         }
 431
 432         /* i < 0 should not happen ... */
 433         if (i < 0)
 434                 goto error;
 435
 436         config = lis3dh_acc_odr_table[i].mask;
 437         config |= LIS3DH_ACC_ENABLE_ALL_AXES;
 438
 439         err = lis3dh_acc_i2c_write(acc, CTRL_REG1, &config, 1);
 440         if (err < 0)
 441                 goto error;
 442         acc->resume_state[RES_CTRL_REG1] = config;
 443
 444         return err;
 445
 446 error:
 447         dev_err(&acc->client->dev, "update odr failed 0x%x: %d\n",
 448                 config, err);
 449
 450         return err;
 451 }
 452
 453 static int lis3dh_acc_get_acceleration_data(struct lis3dh_acc_data *acc,
 454                 int *xyz)
 455 {
 456         int err = -1;
 457         /* Data bytes from hardware xL, xH, yL, yH, zL, zH */
 458         u8 acc_data[6];
 459         /* x,y,z hardware data */
 460         s16 hw_d[3] = { 0 };
 461
 462         err = lis3dh_acc_i2c_read(acc, AXISDATA_REG, acc_data, 6);
 463         if (err < 0)
 464                 return err;
 465
 466         hw_d[0] = (((s16) ((acc_data[1] << 8) | acc_data[0])) >> 4);
 467         hw_d[1] = (((s16) ((acc_data[3] << 8) | acc_data[2])) >> 4);
 468         hw_d[2] = (((s16) ((acc_data[5] << 8) | acc_data[4])) >> 4);
 469
 470         hw_d[0] = hw_d[0] * acc->sensitivity;
 471         hw_d[1] = hw_d[1] * acc->sensitivity;
 472         hw_d[2] = hw_d[2] * acc->sensitivity;
 473
 474
 475         xyz[0] = ((acc->pdata->negate_x) ? (-hw_d[acc->pdata->axis_map_x])
 476                    : (hw_d[acc->pdata->axis_map_x]));
 477         xyz[1] = ((acc->pdata->negate_y) ? (-hw_d[acc->pdata->axis_map_y])
 478                    : (hw_d[acc->pdata->axis_map_y]));
 479         xyz[2] = ((acc->pdata->negate_z) ? (-hw_d[acc->pdata->axis_map_z])
 480                    : (hw_d[acc->pdata->axis_map_z]));
 481
 482 #if DEBUG_DATA_LOG
 483         printk(KERN_INFO "%s read x=%d, y=%d, z=%d\n",
 484                 LIS3DH_ACC_DEV_NAME, xyz[0], xyz[1], xyz[2]);
 485 #endif
 486         return err;
 487 }
 488
 489 static void lis3dh_acc_report_values(struct lis3dh_acc_data *acc, int *xyz)
 490 {
 491         input_report_rel(acc->input_dev, REL_X, xyz[0]);
 492         input_report_rel(acc->input_dev, REL_Y, xyz[1]);
 493         input_report_rel(acc->input_dev, REL_Z, xyz[2]);
 494         input_sync(acc->input_dev);
 495 }
 496
 497 /* mutex lock must be held when calling this function */
 498 static void lis3dh_acc_launch_work(struct lis3dh_acc_data *acc)
 499 {
 500         if (!acc->enabled)
 501                 return;
 502         if (acc->pdata->poll_interval > 0) {
 503                 schedule_delayed_work(&acc->input_work,
 504                                 msecs_to_jiffies(acc->pdata->poll_interval));
 505         } else {
 506                 acc->report_cnt = LIS3DH_6D_REPORT_CNT;
 507                 schedule_delayed_work(&acc->input_work,
 508                                 msecs_to_jiffies(acc->report_interval));
 509         }
 510 }
 511
 512 static int lis3dh_acc_get_int1_source(struct lis3dh_acc_data *acc)
 513 {
 514         u8 data;
 515         int ret;
 516
 517         ret = lis3dh_acc_i2c_read(acc, INT_SRC1, &data, 1);
 518         if (ret < 0) {
 519                 printk(KERN_INFO "Error when read int1_source\n");
 520                 return ret;
 521         }
 522
 523         return data;
 524 }
 525
 526 #define INT1_IA (1 << 6)
 527 static irqreturn_t lis3dh_acc_isr1(int irq, void *data)
 528 {
 529         struct lis3dh_acc_data *acc = data;
 530         struct device *dev = &acc->client->dev;
 531         int int1_stat;
 532         int irq_ret = IRQ_HANDLED,ret=0;
 533         int xyz[3];
 534
 535         mutex_lock(&acc->lock);
 536
 537         if (!acc->enabled)
 538                 goto out;
 539
 540         int1_stat = lis3dh_acc_get_int1_source(acc);
 541         if (int1_stat < 0) {
 542                 irq_ret = IRQ_NONE;
 543                 goto out;
 544         }
 545
 546         if (int1_stat & INT1_IA) {
 547                 dev_dbg(dev, "6d interrupt reported\n");
 548                 lis3dh_acc_launch_work(acc);
 549         }
 550
 551 out:
 552         mutex_unlock(&acc->lock);
 553         return irq_ret;
 554 }
 555
 556 static int lis3dh_acc_enable(struct lis3dh_acc_data *acc)
 557 {
 558         /* clear interrupt source */
 559         lis3dh_acc_get_int1_source(acc);
 560         lis3dh_acc_device_power_on(acc);
 561         acc->enabled = 1;
 562
 563         /* Send initial events to userspace */
 564         lis3dh_acc_launch_work(acc);
 565         return 0;
 566 }
 567
 568 static int lis3dh_acc_disable(struct lis3dh_acc_data *acc)
 569 {
 570         /*
 571          * cancel_delayed_work_sync can't be used since both worker
 572          * and this function are protected with the same mutex.
 573          */
 574         cancel_delayed_work(&acc->input_work);
 575         lis3dh_acc_device_power_off(acc);
 576         acc->enabled = 0;
 577
 578         return 0;
 579 }
 580
 581
 582 static ssize_t read_single_reg(struct device *dev, char *buf, u8 reg)
 583 {
 584         ssize_t ret;
 585         struct lis3dh_acc_data *acc = dev_get_drvdata(dev);
 586         int err;
 587         u8 data;
 588
 589         err = lis3dh_acc_i2c_read(acc, reg, &data, 1);
 590         if (err < 0)
 591                 return err;
 592         ret = sprintf(buf, "0x%02x\n", data);
 593         return ret;
 594 }
 595
 596 static int write_reg(struct device *dev, const char *buf, u8 reg,
 597                 u8 mask, int resumeIndex)
 598 {
 599         int err;
 600         struct lis3dh_acc_data *acc = dev_get_drvdata(dev);
 601         u8 new_val;
 602         unsigned long val;
 603
 604         if (strict_strtoul(buf, 16, &val))
 605                 return -EINVAL;
 606
 607         new_val = ((u8) val & mask);
 608         err = lis3dh_acc_i2c_write(acc, reg, &new_val, 1);
 609         if (err < 0)
 610                 return err;
 611         acc->resume_state[resumeIndex] = new_val;
 612         return err;
 613 }
 614
 615 static ssize_t attr_get_polling_rate(struct device *dev,
 616                                      struct device_attribute *attr,
 617                                      char *buf)
 618 {
 619         int val;
 620         struct lis3dh_acc_data *acc = dev_get_drvdata(dev);
 621
 622         mutex_lock(&acc->lock);
 623         val = acc->pdata->poll_interval;
 624         mutex_unlock(&acc->lock);
 625         return sprintf(buf, "%d\n", val);
 626 }
 627
 628 static ssize_t attr_set_polling_rate(struct device *dev,
 629                                      struct device_attribute *attr,
 630                                      const char *buf, size_t size)
 631 {
 632         struct lis3dh_acc_data *acc = dev_get_drvdata(dev);
 633         unsigned long interval_ms;
 634
 635         if (strict_strtoul(buf, 10, &interval_ms))
 636                 return -EINVAL;
 637
 638         if (!interval_ms)
 639                 cancel_delayed_work_sync(&acc->input_work);
 640
 641         mutex_lock(&acc->lock);
 642         acc->pdata->poll_interval = interval_ms;
 643
 644         /* Send initial events to userspace */
 645         if (acc->enabled)
 646                 lis3dh_acc_launch_work(acc);
 647         mutex_unlock(&acc->lock);
 648         return size;
 649 }
 650 static DEVICE_ATTR(poll, S_IRUGO | S_IWUSR,
 651                    attr_get_polling_rate, attr_set_polling_rate);
 652
 653 static ssize_t attr_get_range(struct device *dev,
 654                                struct device_attribute *attr, char *buf)
 655 {
 656         char val;
 657         struct lis3dh_acc_data *acc = dev_get_drvdata(dev);
 658         char range;
 659
 660         mutex_lock(&acc->lock);
 661         range = acc->pdata->g_range ;
 662         mutex_unlock(&acc->lock);
 663         return sprintf(buf, "%d\n", range);
 664 }
 665
 666 static ssize_t attr_set_range(struct device *dev,
 667                               struct device_attribute *attr,
 668                               const char *buf, size_t size)
 669 {
 670         struct lis3dh_acc_data *acc = dev_get_drvdata(dev);
 671         unsigned long val;
 672
 673         if (strict_strtoul(buf, 10, &val))
 674                 return -EINVAL;
 675
 676         if (val != 2 && val != 4 && val != 8 && val != 16)
 677                 return -EINVAL;
 678
 679
 680         mutex_lock(&acc->lock);
 681         acc->pdata->g_range = (u8) val;
 682         lis3dh_acc_update_g_range(acc, acc->pdata->g_range);
 683         mutex_unlock(&acc->lock);
 684
 685         return size;
 686 }
 687 static DEVICE_ATTR(range, S_IRUGO | S_IWUSR, attr_get_range, attr_set_range);
 688
 689 static ssize_t attr_get_enable(struct device *dev,
 690                                struct device_attribute *attr, char *buf)
 691 {
 692         struct lis3dh_acc_data *acc = dev_get_drvdata(dev);
 693         int val;
 694
 695         mutex_lock(&acc->lock);
 696         val = acc->enabled;
 697         mutex_unlock(&acc->lock);
 698
 699         return sprintf(buf, "%d\n", val);
 700 }
 701
 702 static ssize_t attr_set_enable(struct device *dev,
 703                                struct device_attribute *attr,
 704                                const char *buf, size_t size)
 705 {
 706         struct lis3dh_acc_data *acc = dev_get_drvdata(dev);
 707         unsigned long val;
 708
 709         if (strict_strtoul(buf, 10, &val))
 710                 return -EINVAL;
 711
 712         if (val != 0 && val != 1)
 713                 return -EINVAL;
 714
 715         dev_dbg(dev, "enable %ld\n", val);
 716
 717         mutex_lock(&acc->lock);
 718         if (val)
 719                 lis3dh_acc_enable(acc);
 720         else
 721                 lis3dh_acc_disable(acc);
 722         mutex_unlock(&acc->lock);
 723
 724         return size;
 725 }
 726 static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR, attr_get_enable, attr_set_enable);
 727
 728 static ssize_t attr_set_intconfig1(struct device *dev,
 729                 struct device_attribute *attr, const char *buf, size_t size)
 730 {
 731         return write_reg(dev, buf, INT_CFG1, NO_MASK, RES_INT_CFG1);
 732 }
 733
 734 static ssize_t attr_get_intconfig1(struct device *dev,
 735                 struct device_attribute *attr, char *buf)
 736 {
 737         return read_single_reg(dev, buf, INT_CFG1);
 738 }
 739 static DEVICE_ATTR(int1_config, S_IRUGO | S_IWUSR,
 740                    attr_get_intconfig1, attr_set_intconfig1);
 741
 742 static ssize_t attr_set_duration1(struct device *dev,
 743                 struct device_attribute *attr, const char *buf, size_t size)
 744 {
 745         return write_reg(dev, buf, INT_DUR1, INT1_DURATION_MASK, RES_INT_DUR1);
 746 }
 747
 748 static ssize_t attr_get_duration1(struct device *dev,
 749                 struct device_attribute *attr, char *buf)
 750 {
 751         return read_single_reg(dev, buf, INT_DUR1);
 752 }
 753 static DEVICE_ATTR(int1_duration, S_IRUGO | S_IWUSR,
 754                    attr_get_duration1, attr_set_duration1);
 755
 756 static ssize_t attr_set_thresh1(struct device *dev,
 757                 struct device_attribute *attr, const char *buf, size_t size)
 758 {
 759         return write_reg(dev, buf, INT_THS1, INT1_THRESHOLD_MASK, RES_INT_THS1);
 760 }
 761
 762 static ssize_t attr_get_thresh1(struct device *dev,
 763                 struct device_attribute *attr, char *buf)
 764 {
 765         return read_single_reg(dev, buf, INT_THS1);
 766 }
 767 static DEVICE_ATTR(int1_threshold, S_IRUGO | S_IWUSR,
 768                    attr_get_thresh1, attr_set_thresh1);
 769
 770 static ssize_t attr_get_source1(struct device *dev,
 771                 struct device_attribute *attr, char *buf)
 772 {
 773         return read_single_reg(dev, buf, INT_SRC1);
 774 }
 775 static DEVICE_ATTR(int1_source, S_IRUGO, attr_get_source1, NULL);
 776
 777 static ssize_t attr_set_click_cfg(struct device *dev,
 778                 struct device_attribute *attr, const char *buf, size_t size)
 779 {
 780         return write_reg(dev, buf, TT_CFG, TAP_CFG_MASK, RES_TT_CFG);
 781 }
 782
 783 static ssize_t attr_get_click_cfg(struct device *dev,
 784                 struct device_attribute *attr, char *buf)
 785 {
 786         return read_single_reg(dev, buf, TT_CFG);
 787 }
 788 static DEVICE_ATTR(click_config, S_IRUGO | S_IWUSR,
 789                    attr_get_click_cfg, attr_set_click_cfg);
 790
 791 static ssize_t attr_get_click_source(struct device *dev,
 792                 struct device_attribute *attr, char *buf)
 793 {
 794         return read_single_reg(dev, buf, TT_SRC);
 795 }
 796 static DEVICE_ATTR(click_source, S_IRUGO, attr_get_click_source, NULL);
 797
 798 static ssize_t attr_set_click_ths(struct device *dev,
 799                 struct device_attribute *attr, const char *buf, size_t size)
 800 {
 801         return write_reg(dev, buf, TT_THS, TAP_THS_MASK, RES_TT_THS);
 802 }
 803
 804 static ssize_t attr_get_click_ths(struct device *dev,
 805                 struct device_attribute *attr, char *buf)
 806 {
 807         return read_single_reg(dev, buf, TT_THS);
 808 }
 809 static DEVICE_ATTR(click_threshold, S_IRUGO | S_IWUSR,
 810                    attr_get_click_ths, attr_set_click_ths);
 811
 812 static ssize_t attr_set_click_tlim(struct device *dev,
 813                 struct device_attribute *attr, const char *buf, size_t size)
 814 {
 815         return write_reg(dev, buf, TT_LIM, TAP_TLIM_MASK, RES_TT_LIM);
 816 }
 817
 818 static ssize_t attr_get_click_tlim(struct device *dev,
 819                 struct device_attribute *attr, char *buf)
 820 {
 821         return read_single_reg(dev, buf, TT_LIM);
 822 }
 823 static DEVICE_ATTR(click_timelimit, S_IRUGO | S_IWUSR,
 824                    attr_get_click_tlim, attr_set_click_tlim);
 825
 826 static ssize_t attr_set_click_tlat(struct device *dev,
 827                 struct device_attribute *attr, const char *buf, size_t size)
 828 {
 829         return write_reg(dev, buf, TT_TLAT, TAP_TLAT_MASK, RES_TT_TLAT);
 830 }
 831
 832 static ssize_t attr_get_click_tlat(struct device *dev,
 833                 struct device_attribute *attr, char *buf)
 834 {
 835         return read_single_reg(dev, buf, TT_TLAT);
 836 }
 837 static DEVICE_ATTR(click_timelatency, S_IRUGO | S_IWUSR,
 838                    attr_get_click_tlat, attr_set_click_tlat);
 839
 840 static ssize_t attr_set_click_tw(struct device *dev,
 841                 struct device_attribute *attr, const char *buf, size_t size)
 842 {
 843         return write_reg(dev, buf, TT_TLAT, TAP_TW_MASK, RES_TT_TLAT);
 844 }
 845
 846 static ssize_t attr_get_click_tw(struct device *dev,
 847                 struct device_attribute *attr, char *buf)
 848 {
 849         return read_single_reg(dev, buf, TT_TLAT);
 850 }
 851 static DEVICE_ATTR(click_timewindow, S_IRUGO | S_IWUSR,
 852                    attr_get_click_tw, attr_set_click_tw);
 853
 854
 855 #ifdef DEBUG
 856 /* PAY ATTENTION: These DEBUG funtions don't manage resume_state */
 857 static ssize_t attr_reg_set(struct device *dev, struct device_attribute *attr,
 858                                 const char *buf, size_t size)
 859 {
 860         int rc;
 861         struct lis3dh_acc_data *acc = dev_get_drvdata(dev);
 862         unsigned long val;
 863
 864         if (strict_strtoul(buf, 16, &val))
 865                 return -EINVAL;
 866
 867         mutex_lock(&acc->lock);
 868         rc = lis3dh_acc_i2c_write(acc, acc->reg_addr, (u8 *)&val, 1);
 869         mutex_unlock(&acc->lock);
 870
 871         if (rc == 0)
 872                 rc = size;
 873         return rc;
 874 }
 875
 876 static ssize_t attr_reg_get(struct device *dev, struct device_attribute *attr,
 877                                 char *buf)
 878 {
 879         struct lis3dh_acc_data *acc = dev_get_drvdata(dev);
 880         int rc;
 881         u8 data;
 882
 883         mutex_lock(&acc->lock);
 884         rc = lis3dh_acc_i2c_read(acc, acc->reg_addr, &data, 1);
 885         mutex_unlock(&acc->lock);
 886
 887         if (rc < 0)
 888                 return rc;
 889         else
 890                 return sprintf(buf, "0x%02x\n", data);
 891 }
 892 static DEVICE_ATTR(reg_value, S_IRUGO | S_IWUSR, attr_reg_get, attr_reg_set);
 893
 894 static ssize_t attr_addr_set(struct device *dev, struct device_attribute *attr,
 895                                 const char *buf, size_t size)
 896 {
 897         struct lis3dh_acc_data *acc = dev_get_drvdata(dev);
 898         unsigned long val;
 899
 900         if (strict_strtoul(buf, 16, &val))
 901                 return -EINVAL;
 902
 903         mutex_lock(&acc->lock);
 904         acc->reg_addr = val;
 905         mutex_unlock(&acc->lock);
 906
 907         return size;
 908 }
 909 static DEVICE_ATTR(reg_addr, S_IWUSR, NULL, attr_addr_set);
 910 #endif
 911
 912 static struct attribute *lis3dh_attrs[] = {
 913         &dev_attr_poll.attr,
 914         &dev_attr_range.attr,
 915         &dev_attr_enable.attr,
 916         &dev_attr_int1_config.attr,
 917         &dev_attr_int1_duration.attr,
 918         &dev_attr_int1_threshold.attr,
 919         &dev_attr_int1_source.attr,
 920         &dev_attr_click_config.attr,
 921         &dev_attr_click_source.attr,
 922         &dev_attr_click_threshold.attr,
 923         &dev_attr_click_timelimit.attr,
 924         &dev_attr_click_timelatency.attr,
 925         &dev_attr_click_timewindow.attr,
 926 #ifdef DEBUG
 927         &dev_attr_reg_value.attr,
 928         &dev_attr_reg_addr.attr,
 929 #endif
 930         NULL
 931 };
 932
 933 static struct attribute_group lis3dh_attr_group = {
 934         .name = "lis3dh",
 935         .attrs = lis3dh_attrs
 936 };
 937
 938 static void lis3dh_acc_input_work_func(struct work_struct *work)
 939 {
 940         struct lis3dh_acc_data *acc;
 941
 942         int xyz[3] = { 0 };
 943         int err;
 944
 945         acc = container_of((struct delayed_work *)work,
 946                         struct lis3dh_acc_data, input_work);
 947
 948         mutex_lock(&acc->lock);
 949         /*
 950          * If the disable function was run after the work has been changed to
 951          * running state but before we got the mutex the worker keeps on
 952          * running. Check the enable status after the mutex is locked to
 953          * see if the HW is still running.
 954          */
 955         if (!acc->enabled)
 956                 goto leave;
 957
 958         err = lis3dh_acc_get_acceleration_data(acc, xyz);
 959         if (err < 0)
 960                 dev_err(&acc->client->dev, "get_acceleration_data failed\n");
 961         else
 962                 lis3dh_acc_report_values(acc, xyz);
 963
 964         if (acc->pdata->poll_interval > 0) {
 965                 schedule_delayed_work(&acc->input_work,
 966                                 msecs_to_jiffies(acc->pdata->poll_interval));
 967         } else if (acc->report_cnt-- > 0) {
 968                 schedule_delayed_work(&acc->input_work,
 969                                 msecs_to_jiffies(acc->report_interval));
 970         }
 971 leave:
 972         mutex_unlock(&acc->lock);
 973 }
 974
 975 static int lis3dh_acc_validate_pdata(struct lis3dh_acc_data *acc)
 976 {
 977         acc->pdata->poll_interval = max(acc->pdata->poll_interval,
 978                         acc->pdata->min_interval);
 979
 980         if (acc->pdata->axis_map_x > 2 ||
 981             acc->pdata->axis_map_y > 2 ||
 982             acc->pdata->axis_map_z > 2) {
 983                 dev_err(&acc->client->dev, "invalid axis_map value "
 984                         "x:%u y:%u z%u\n", acc->pdata->axis_map_x,
 985                                 acc->pdata->axis_map_y, acc->pdata->axis_map_z);
 986                 return -EINVAL;
 987         }
 988
 989         /* Only allow 0 and 1 for negation boolean flag */
 990         if (acc->pdata->negate_x > 1 || acc->pdata->negate_y > 1
 991                         || acc->pdata->negate_z > 1) {
 992                 dev_err(&acc->client->dev, "invalid negate value "
 993                         "x:%u y:%u z:%u\n", acc->pdata->negate_x,
 994                                 acc->pdata->negate_y, acc->pdata->negate_z);
 995                 return -EINVAL;
 996         }
 997
 998         /* Enforce minimum polling interval */
 999         if (acc->pdata->poll_interval < acc->pdata->min_interval) {
1000                 dev_err(&acc->client->dev, "minimum poll interval violated\n");
1001                 return -EINVAL;
1002         }
1003
1004         return 0;
1005 }
1006
1007 static int lis3dh_acc_input_init(struct lis3dh_acc_data *acc)
1008 {
1009         int err;
1010
1011         INIT_DELAYED_WORK(&acc->input_work, lis3dh_acc_input_work_func);
1012         acc->report_interval = LIS3DH_6D_REPORT_DELAY;
1013
1014         acc->input_dev = input_allocate_device();
1015         if (!acc->input_dev) {
1016                 err = -ENOMEM;
1017                 dev_err(&acc->client->dev, "input device allocation failed\n");
1018                 goto err0;
1019         }
1020
1021         acc->input_dev->name = LIS3DH_ACC_DEV_NAME;
1022         acc->input_dev->id.bustype = BUS_I2C;
1023         acc->input_dev->dev.parent = &acc->client->dev;
1024
1025         input_set_drvdata(acc->input_dev, acc);
1026
1027         set_bit(EV_REL, acc->input_dev->evbit);
1028         set_bit(REL_X, acc->input_dev->relbit);
1029         set_bit(REL_Y, acc->input_dev->relbit);
1030         set_bit(REL_Z, acc->input_dev->relbit);
1031         /*      next is used for interruptA sources data if the case */
1032         set_bit(ABS_MISC, acc->input_dev->absbit);
1033         /*      next is used for interruptB sources data if the case */
1034         set_bit(ABS_WHEEL, acc->input_dev->absbit);
1035
1036         /*      next is used for interruptA sources data if the case */
1037         input_set_abs_params(acc->input_dev, ABS_MISC, INT_MIN, INT_MAX, 0, 0);
1038         /*      next is used for interruptB sources data if the case */
1039         input_set_abs_params(acc->input_dev, ABS_WHEEL, INT_MIN, INT_MAX, 0, 0);
1040
1041         err = input_register_device(acc->input_dev);
1042         if (err) {
1043                 dev_err(&acc->client->dev,
1044                                 "unable to register input device %s\n",
1045                                 acc->input_dev->name);
1046                 goto err1;
1047         }
1048
1049         err = sysfs_create_group(&acc->client->dev.kobj, &lis3dh_attr_group);
1050         if (err < 0) {
1051                 dev_err(&acc->client->dev,
1052                    "device LIS3DH_ACC_DEV_NAME sysfs register failed\n");
1053                 goto err0;
1054         }
1055
1056         return 0;
1057
1058 err1:
1059         input_free_device(acc->input_dev);
1060 err0:
1061         return err;
1062 }
1063
1064
1065 static void lis3dh_init_resume_state(struct lis3dh_acc_data *acc)
1066 {
1067         memset(acc->resume_state, 0, ARRAY_SIZE(acc->resume_state));
1068         acc->resume_state[RES_CTRL_REG1] = LIS3DH_ACC_ENABLE_ALL_AXES;
1069         acc->resume_state[RES_CTRL_REG2] = 0x00;
1070         acc->resume_state[RES_CTRL_REG3] = 0x40;
1071         acc->resume_state[RES_CTRL_REG4] = 0x00;
1072         acc->resume_state[RES_CTRL_REG5] = 0x08;
1073         acc->resume_state[RES_CTRL_REG6] = 0x00;
1074
1075         acc->resume_state[RES_TEMP_CFG_REG] = 0x00;
1076         acc->resume_state[RES_FIFO_CTRL_REG] = 0x00;
1077         acc->resume_state[RES_INT_CFG1] = 0x7f;
1078         acc->resume_state[RES_INT_THS1] = 0x30;
1079         acc->resume_state[RES_INT_DUR1] = 5;
1080
1081         acc->resume_state[RES_TT_CFG] = 0x00;
1082         acc->resume_state[RES_TT_THS] = 0x00;
1083         acc->resume_state[RES_TT_LIM] = 0x00;
1084         acc->resume_state[RES_TT_TLAT] = 0x00;
1085         acc->resume_state[RES_TT_TW] = 0x00;
1086 }
1087
1088 #ifdef CONFIG_HAS_EARLYSUSPEND
1089 static void lis3dh_early_suspend(struct early_suspend *h)
1090 {
1091         struct lis3dh_acc_data *acc = container_of(h,
1092                                         struct lis3dh_acc_data, es);
1093
1094 #else
1095 static void lis3dh_first_suspend(struct device *dev)
1096 {
1097         struct lis3dh_acc_data *acc = dev_get_drvdata(dev);
1098
1099 #endif
1100         dev_dbg(&acc->client->dev, "enter early_suspend\n");
1101         disable_irq(acc->irq1);
1102
1103         mutex_lock(&acc->lock);
1104         acc->need_resume = acc->enabled;
1105         if (acc->enabled)
1106                 lis3dh_acc_disable(acc);
1107         mutex_unlock(&acc->lock);
1108 }
1109
1110 #ifdef CONFIG_HAS_EARLYSUSPEND
1111 static void lis3dh_late_resume(struct early_suspend *h)
1112 {
1113         struct lis3dh_acc_data *acc = container_of(h,
1114                                         struct lis3dh_acc_data, es);
1115
1116 #else
1117 static void lis3dh_last_resume(struct device *dev)
1118 {
1119         struct lis3dh_acc_data *acc = dev_get_drvdata(dev);
1120
1121 #endif
1122         dev_dbg(&acc->client->dev, "enter late_resume\n");
1123         enable_irq(acc->irq1);
1124
1125         mutex_lock(&acc->lock);
1126         if (acc->need_resume)
1127                 lis3dh_acc_enable(acc);
1128         mutex_unlock(&acc->lock);
1129 }
1130
1131 #ifndef CONFIG_HAS_EARLYSUSPEND
1132 extern int screen_register_receiver(struct notifier_block *nb);
1133 extern int screen_unregister_receiver(struct notifier_block *nb);
1134
1135 static bool lis3dh_enabled;
1136
1137 static int lis3dh_screen_notifier_callback(struct notifier_block *self,
1138                                     unsigned long event_type, void *nt_data)
1139 {
1140         struct lis3dh_acc_data *acc = container_of(self,
1141                                 struct lis3dh_acc_data, screen_notifier);
1142         struct device *dev = &acc->client->dev;
1143
1144         if(dev == NULL)
1145                 return 0;
1146
1147         switch (event_type) {
1148         case DRM_MODE_DPMS_ON:
1149                 if (lis3dh_enabled == false) {
1150                         lis3dh_last_resume(dev);
1151                         lis3dh_enabled = true;
1152                 }
1153                 break;
1154         case DRM_MODE_DPMS_OFF:
1155                 if (lis3dh_enabled == true) {
1156                         lis3dh_first_suspend(dev);
1157                         lis3dh_enabled = false;
1158                 }
1159                 break;
1160         default:
1161                 break;
1162         }
1163
1164         return 0;
1165 }
1166 #endif
1167
1168 static int lis3dh_acc_probe(struct i2c_client *client,
1169                             const struct i2c_device_id *id)
1170 {
1171         struct lis3dh_acc_data *acc;
1172         int err;
1173
1174         if (!client->dev.platform_data) {
1175                 dev_err(&client->dev, "platform data is NULL. exiting.\n");
1176                 err = -ENODEV;
1177                 goto out;
1178         }
1179
1180         if (!i2c_check_functionality(client->adapter,
1181                                 I2C_FUNC_I2C | I2C_FUNC_SMBUS_I2C_BLOCK)) {
1182                 dev_err(&client->dev, "client not i2c capable\n");
1183                 err = -ENODEV;
1184                 goto out;
1185         }
1186
1187         acc = kzalloc(sizeof(struct lis3dh_acc_data), GFP_KERNEL);
1188         if (!acc) {
1189                 err = -ENOMEM;
1190                 dev_err(&client->dev, "failed to allocate memory\n");
1191                 goto out;
1192         }
1193
1194         mutex_init(&acc->lock);
1195         mutex_lock(&acc->lock);
1196
1197         acc->client = client;
1198         i2c_set_clientdata(client, acc);
1199
1200         acc->pdata = kmalloc(sizeof(*acc->pdata), GFP_KERNEL);
1201         if (!acc->pdata) {
1202                 err = -ENOMEM;
1203                 dev_err(&client->dev, "failed to allocate memory for pdata\n");
1204                 goto out_unlock;
1205         }
1206
1207         memcpy(acc->pdata, client->dev.platform_data, sizeof(*acc->pdata));
1208         err = lis3dh_acc_validate_pdata(acc);
1209         if (err < 0) {
1210                 dev_err(&client->dev, "failed to validate platform data\n");
1211                 goto out_free_pdata;
1212         }
1213
1214         if (acc->pdata->init) {
1215                 err = acc->pdata->init();
1216                 if (err < 0) {
1217                         dev_err(&client->dev, "init failed: %d\n", err);
1218                         goto out_free_pdata;
1219                 }
1220         }
1221
1222         lis3dh_init_resume_state(acc);
1223         err = lis3dh_acc_device_power_on(acc);
1224         if (err < 0) {
1225                 dev_err(&client->dev, "power on failed: %d\n", err);
1226                 goto out_pdata_exit;
1227         }
1228
1229         err = lis3dh_acc_update_g_range(acc, acc->pdata->g_range);
1230         if (err < 0) {
1231                 dev_err(&client->dev, "update_g_range failed\n");
1232                 goto  out_power_off;
1233         }
1234
1235         err = lis3dh_acc_update_odr(acc, 10);
1236         if (err < 0) {
1237                 dev_err(&client->dev, "update_odr failed\n");
1238                 goto  out_power_off;
1239         }
1240
1241         err = lis3dh_acc_input_init(acc);
1242         if (err < 0) {
1243                 dev_err(&client->dev, "input init failed\n");
1244                 goto out_power_off;
1245         }
1246
1247         if (acc->pdata->gpio_int1 >= 0) {
1248                 gpio_request(acc->pdata->gpio_int1, "accel_int1");
1249                 gpio_direction_input(acc->pdata->gpio_int1);
1250                 acc->irq1 = gpio_to_irq(acc->pdata->gpio_int1);
1251                 dev_dbg(&client->dev, "irq1:%d mapped on gpio:%d\n",
1252                          acc->irq1, acc->pdata->gpio_int1);
1253
1254                 err = request_threaded_irq(acc->irq1, NULL, lis3dh_acc_isr1,
1255                                 IRQF_TRIGGER_RISING, "lis3dh_acc_irq1", acc);
1256                 if (err < 0) {
1257                         dev_err(&client->dev, "request irq1 failed: %d\n", err);
1258                         goto out_free_input;
1259                 }
1260         }
1261
1262         lis3dh_acc_disable(acc);
1263
1264 #ifdef CONFIG_HAS_EARLYSUSPEND
1265         acc->es.level = EARLY_SUSPEND_LEVEL_DISABLE_FB + 10;
1266         acc->es.suspend = lis3dh_early_suspend;
1267         acc->es.resume = lis3dh_late_resume;
1268         register_early_suspend(&acc->es);
1269 #else
1270         lis3dh_enabled = true;
1271         acc->screen_notifier.notifier_call = lis3dh_screen_notifier_callback;
1272         screen_register_receiver(&acc->screen_notifier);
1273 #endif
1274
1275         mutex_unlock(&acc->lock);
1276         dev_info(&client->dev, "successfully probed\n");
1277
1278         return 0;
1279
1280 out_free_input:
1281         sysfs_remove_group(&client->dev.kobj, &lis3dh_attr_group);
1282         input_unregister_device(acc->input_dev);
1283 out_power_off:
1284         lis3dh_acc_device_power_off(acc);
1285 out_pdata_exit:
1286         if (acc->pdata->exit)
1287                 acc->pdata->exit();
1288 out_free_pdata:
1289         kfree(acc->pdata);
1290 out_unlock:
1291         mutex_unlock(&acc->lock);
1292         kfree(acc);
1293 out:
1294         return err;
1295 }
1296
1297 static int __devexit lis3dh_acc_remove(struct i2c_client *client)
1298 {
1299         struct lis3dh_acc_data *acc = i2c_get_clientdata(client);
1300
1301         if (acc->pdata->gpio_int1 >= 0) {
1302                 free_irq(acc->irq1, acc);
1303                 gpio_free(acc->pdata->gpio_int1);
1304         }
1305
1306         input_unregister_device(acc->input_dev);
1307         lis3dh_acc_device_power_off(acc);
1308
1309         sysfs_remove_group(&client->dev.kobj, &lis3dh_attr_group);
1310 #ifdef CONFIG_HAS_EARLYSUSPEND
1311         unregister_early_suspend(&acc->es);
1312 #else
1313         screen_unregister_receiver(&acc->screen_notifier);
1314 #endif
1315
1316         if (acc->pdata->exit)
1317                 acc->pdata->exit();
1318         kfree(acc->pdata);
1319         kfree(acc);
1320
1321         return 0;
1322 }
1323
1324 #ifdef CONFIG_PM_SLEEP
1325 static int lis3dh_acc_resume(struct device *dev)
1326 {
1327         return 0;
1328 }
1329
1330 static int lis3dh_acc_suspend(struct device *dev)
1331 {
1332         return 0;
1333 }
1334 #endif /* CONFIG_PM_SLEEP */
1335
1336 static const struct dev_pm_ops lis3dh_acc_pm_ops = {
1337         SET_SYSTEM_SLEEP_PM_OPS(lis3dh_acc_suspend,
1338                         lis3dh_acc_resume)
1339 };
1340
1341 static const struct i2c_device_id lis3dh_acc_id[]
1342                 = { { LIS3DH_ACC_DEV_NAME, 0 }, { "lsm303dl", 0 }, { }, };
1343
1344 MODULE_DEVICE_TABLE(i2c, lis3dh_acc_id);
1345
1346 static struct i2c_driver lis3dh_acc_driver = {
1347         .driver = {
1348                 .owner = THIS_MODULE,
1349                 .name = LIS3DH_ACC_DEV_NAME,
1350                 .pm = &lis3dh_acc_pm_ops,
1351         },
1352         .probe = lis3dh_acc_probe,
1353         .remove = __devexit_p(lis3dh_acc_remove),
1354         .id_table = lis3dh_acc_id,
1355 };
1356
1357 static int __init lis3dh_acc_init(void)
1358 {
1359         pr_info("lis3dh driver: init\n");
1360         return i2c_add_driver(&lis3dh_acc_driver);
1361 }
1362
1363 static void __exit lis3dh_acc_exit(void)
1364 {
1365         pr_info("lis3dh driver exit\n");
1366         i2c_del_driver(&lis3dh_acc_driver);
1367 }
1368
1369 module_init(lis3dh_acc_init);
1370 module_exit(lis3dh_acc_exit);
1371
1372 MODULE_DESCRIPTION("lis3dh digital accelerometer sysfs driver");
1373 MODULE_AUTHOR("Matteo Dameno, Carmine Iascone, STMicroelectronics");
1374 MODULE_LICENSE("GPL");