drivers/input/gyroscope/k3g.c

   1 /*
   2  *  Copyright (C) 2010, Samsung Electronics Co. Ltd. All Rights Reserved.
   3  *
   4  *  This program is free software; you can redistribute it and/or modify
   5  *  it under the terms of the GNU General Public License as published by
   6  *  the Free Software Foundation; either version 2 of the License, or
   7  *  (at your option) any later version.
   8  *
   9  *  This program is distributed in the hope that it will be useful,
  10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  *  GNU General Public License for more details.
  13  *
  14  */
  15
  16 #include <linux/kernel.h>
  17 #include <linux/module.h>
  18 #include <linux/i2c.h>
  19 #include <linux/uaccess.h>
  20 #include <linux/poll.h>
  21 #include <linux/slab.h>
  22 #include <linux/input.h>
  23 #include <linux/interrupt.h>
  24 #include <asm/div64.h>
  25 //#include <linux/input/k3g.h>
  26 #include <linux/delay.h>
  27
  28 /* k3g chip id */
  29 #define DEVICE_ID       0xD3
  30 /* k3g gyroscope registers */
  31 #define WHO_AM_I        0x0F
  32 #define CTRL_REG1       0x20  /* power control reg */
  33 #define CTRL_REG2       0x21  /* power control reg */
  34 #define CTRL_REG3       0x22  /* power control reg */
  35 #define CTRL_REG4       0x23  /* interrupt control reg */
  36 #define CTRL_REG5       0x24  /* interrupt control reg */
  37 #define OUT_TEMP        0x26  /* Temperature data */
  38 #define STATUS_REG      0x27
  39 #define AXISDATA_REG    0x28
  40 #define OUT_Y_L         0x2A
  41 #define FIFO_CTRL_REG   0x2E
  42 #define FIFO_SRC_REG    0x2F
  43 #define PM_OFF          0x00
  44 #define PM_NORMAL       0x08
  45 #define ENABLE_ALL_AXES 0x07
  46 #define BYPASS_MODE     0x00
  47 #define FIFO_MODE       0x20
  48
  49 #define FIFO_EMPTY      0x20
  50 #define FSS_MASK        0x1F
  51 #define ODR_MASK        0xF0
  52 #define ODR105_BW12_5   0x00  /* ODR = 105Hz; BW = 12.5Hz */
  53 #define ODR105_BW25     0x10  /* ODR = 105Hz; BW = 25Hz   */
  54 #define ODR210_BW12_5   0x40  /* ODR = 210Hz; BW = 12.5Hz */
  55 #define ODR210_BW25     0x50  /* ODR = 210Hz; BW = 25Hz   */
  56 #define ODR210_BW50     0x60  /* ODR = 210Hz; BW = 50Hz   */
  57 #define ODR210_BW70     0x70  /* ODR = 210Hz; BW = 70Hz   */
  58 #define ODR420_BW20     0x80  /* ODR = 420Hz; BW = 20Hz   */
  59 #define ODR420_BW25     0x90  /* ODR = 420Hz; BW = 25Hz   */
  60 #define ODR420_BW50     0xA0  /* ODR = 420Hz; BW = 50Hz   */
  61 #define ODR420_BW110    0xB0  /* ODR = 420Hz; BW = 110Hz  */
  62 #define ODR840_BW30     0xC0  /* ODR = 840Hz; BW = 30Hz   */
  63 #define ODR840_BW35     0xD0  /* ODR = 840Hz; BW = 35Hz   */
  64 #define ODR840_BW50     0xE0  /* ODR = 840Hz; BW = 50Hz   */
  65 #define ODR840_BW110    0xF0  /* ODR = 840Hz; BW = 110Hz  */
  66
  67 #define MIN_ST          175
  68 #define MAX_ST          875
  69 #define AC              (1 << 7) /* register auto-increment bit */
  70 #define MAX_ENTRY       1
  71 #define MAX_DELAY       (MAX_ENTRY * 9523809LL)
  72
  73 /* default register setting for device init */
  74 static const char default_ctrl_regs[] = {
  75         0x3F,   /* 105HZ, PM-normal, xyz enable */
  76         0x00,   /* normal mode */
  77         0x04,   /* fifo wtm interrupt on */
  78         0xA0,   /* block data update, 2000d/s */
  79         0x40,   /* fifo enable */
  80 };
  81
  82 static const struct odr_delay {
  83         u8 odr; /* odr reg setting */
  84         u32 delay_ns; /* odr in ns */
  85 } odr_delay_table[] = {
  86         {  ODR840_BW110, 1190476LL }, /* 840Hz */
  87         {  ODR420_BW110, 2380952LL }, /* 420Hz */
  88         {   ODR210_BW70, 4761904LL }, /* 210Hz */
  89         {   ODR105_BW25, 9523809LL }, /* 105Hz */
  90 };
  91
  92 /*
  93  * K3G gyroscope data
  94  * brief structure containing gyroscope values for yaw, pitch and roll in
  95  * signed short
  96  */
  97 struct k3g_t {
  98         s16 x;
  99         s16 y;
 100         s16 z;
 101 };
 102
 103 struct k3g_data {
 104         struct i2c_client *client;
 105         struct input_dev *input_dev;
 106         struct mutex lock;
 107         struct workqueue_struct *k3g_wq;
 108         struct work_struct work;
 109         struct hrtimer timer;
 110         bool enable;
 111         bool drop_next_event;
 112         bool interruptible;     /* interrupt or polling? */
 113         int entries;            /* number of fifo entries */
 114         u8 ctrl_regs[5];        /* saving register settings */
 115         u32 time_to_read;       /* time needed to read one entry */
 116         ktime_t polling_delay;  /* polling time for timer */
 117 };
 118
 119 static int k3g_read_fifo_status(struct k3g_data *k3g_data)
 120 {
 121         int fifo_status;
 122
 123         fifo_status = i2c_smbus_read_byte_data(k3g_data->client, FIFO_SRC_REG);
 124         if (fifo_status < 0) {
 125                 pr_err("%s: failed to read fifo source register\n",
 126                                                         __func__);
 127                 return fifo_status;
 128         }
 129         return (fifo_status & FSS_MASK) + !(fifo_status & FIFO_EMPTY);
 130 }
 131
 132 static int k3g_restart_fifo(struct k3g_data *k3g_data)
 133 {
 134         int res = 0;
 135
 136         res = i2c_smbus_write_byte_data(k3g_data->client,
 137                         FIFO_CTRL_REG, BYPASS_MODE);
 138         if (res < 0) {
 139                 pr_err("%s : failed to set bypass_mode\n", __func__);
 140                 return res;
 141         }
 142
 143         res = i2c_smbus_write_byte_data(k3g_data->client,
 144                         FIFO_CTRL_REG, FIFO_MODE | (k3g_data->entries - 1));
 145
 146         if (res < 0)
 147                 pr_err("%s : failed to set fifo_mode\n", __func__);
 148
 149         return res;
 150 }
 151
 152 static void set_polling_delay(struct k3g_data *k3g_data, int res)
 153 {
 154         s64 delay_ns;
 155
 156         delay_ns = k3g_data->entries + 1 - res;
 157         if (delay_ns < 0)
 158                 delay_ns = 0;
 159
 160         delay_ns = delay_ns * k3g_data->time_to_read;
 161         k3g_data->polling_delay = ns_to_ktime(delay_ns);
 162 }
 163
 164 /* gyroscope data readout */
 165 static int k3g_read_gyro_values(struct i2c_client *client,
 166                                 struct k3g_t *data, int total_read)
 167 {
 168         int err;
 169         struct i2c_msg msg[2];
 170         u8 reg_buf;
 171         u8 gyro_data[sizeof(*data) * (total_read ? (total_read - 1) : 1)];
 172
 173         msg[0].addr = client->addr;
 174         msg[0].buf = &reg_buf;
 175         msg[0].flags = 0;
 176         msg[0].len = 1;
 177
 178         msg[1].addr = client->addr;
 179         msg[1].flags = I2C_M_RD;
 180         msg[1].buf = gyro_data;
 181
 182         if (total_read > 1) {
 183                 reg_buf = AXISDATA_REG | AC;
 184                 msg[1].len = sizeof(gyro_data);
 185
 186                 err = i2c_transfer(client->adapter, msg, 2);
 187                 if (err != 2)
 188                         return (err < 0) ? err : -EIO;
 189         }
 190
 191         reg_buf = AXISDATA_REG;
 192         msg[1].len = 1;
 193         err = i2c_transfer(client->adapter, msg, 2);
 194         if (err != 2)
 195                 return (err < 0) ? err : -EIO;
 196
 197         reg_buf = OUT_Y_L | AC;
 198         msg[1].len = sizeof(*data);
 199         err = i2c_transfer(client->adapter, msg, 2);
 200         if (err != 2)
 201                 return (err < 0) ? err : -EIO;
 202
 203         data->y = (gyro_data[1] << 8) | gyro_data[0];
 204         data->z = (gyro_data[3] << 8) | gyro_data[2];
 205         data->x = (gyro_data[5] << 8) | gyro_data[4];
 206
 207         return 0;
 208 }
 209
 210 static int k3g_report_gyro_values(struct k3g_data *k3g_data)
 211 {
 212         int res;
 213         struct k3g_t data;
 214
 215         res = k3g_read_gyro_values(k3g_data->client, &data,
 216                                 k3g_data->entries + k3g_data->drop_next_event);
 217         if (res < 0)
 218                 return res;
 219
 220         res = k3g_read_fifo_status(k3g_data);
 221
 222         k3g_data->drop_next_event = !res;
 223
 224         if (res >= 31 - k3g_data->entries) {
 225                 /* reset fifo to start again - data isn't trustworthy,
 226                  * our locked read might not have worked and we
 227                  * could have done i2c read in mid register update
 228                  */
 229                 return k3g_restart_fifo(k3g_data);
 230         }
 231
 232         input_report_rel(k3g_data->input_dev, REL_RX, data.x);
 233         input_report_rel(k3g_data->input_dev, REL_RY, data.y);
 234         input_report_rel(k3g_data->input_dev, REL_RZ, data.z);
 235         input_sync(k3g_data->input_dev);
 236
 237         return res;
 238 }
 239
 240 static enum hrtimer_restart k3g_timer_func(struct hrtimer *timer)
 241 {
 242         struct k3g_data *k3g_data = container_of(timer, struct k3g_data, timer);
 243         queue_work(k3g_data->k3g_wq, &k3g_data->work);
 244         return HRTIMER_NORESTART;
 245 }
 246
 247 static void k3g_work_func(struct work_struct *work)
 248 {
 249         int res;
 250         struct k3g_data *k3g_data = container_of(work, struct k3g_data, work);
 251
 252         do {
 253                 res = k3g_read_fifo_status(k3g_data);
 254                 if (res < 0)
 255                         return;
 256
 257                 if (res < k3g_data->entries) {
 258                         pr_warning("%s: fifo entries are less than we want\n",
 259                                                                 __func__);
 260                         goto timer_set;
 261                 }
 262
 263                 res = k3g_report_gyro_values(k3g_data);
 264                 if (res < 0)
 265                         return;
 266 timer_set:
 267                 set_polling_delay(k3g_data, res);
 268
 269         } while (!ktime_to_ns(k3g_data->polling_delay));
 270
 271         hrtimer_start(&k3g_data->timer,
 272                 k3g_data->polling_delay, HRTIMER_MODE_REL);
 273 }
 274
 275 static irqreturn_t k3g_interrupt_thread(int irq, void *k3g_data_p)
 276 {
 277         int res;
 278         struct k3g_data *k3g_data = k3g_data_p;
 279         res = k3g_report_gyro_values(k3g_data);
 280         if (res < 0)
 281                 pr_err("%s: failed to report gyro values\n", __func__);
 282
 283         return IRQ_HANDLED;
 284 }
 285
 286 static ssize_t k3g_show_enable(struct device *dev,
 287                         struct device_attribute *attr, char *buf)
 288 {
 289         struct k3g_data *k3g_data  = dev_get_drvdata(dev);
 290         return sprintf(buf, "%d\n", k3g_data->enable);
 291 }
 292
 293 static ssize_t k3g_set_enable(struct device *dev,
 294                 struct device_attribute *attr, const char *buf, size_t size)
 295 {
 296         int err = 0;
 297         struct k3g_data *k3g_data  = dev_get_drvdata(dev);
 298         bool new_enable;
 299
 300         if (sysfs_streq(buf, "1"))
 301                 new_enable = true;
 302         else if (sysfs_streq(buf, "0"))
 303                 new_enable = false;
 304         else {
 305                 pr_debug("%s: invalid value %d\n", __func__, *buf);
 306                 return -EINVAL;
 307         }
 308
 309         if (new_enable == k3g_data->enable)
 310                 return size;
 311
 312         mutex_lock(&k3g_data->lock);
 313         if (new_enable) {
 314                 /* turning on */
 315                 err = i2c_smbus_write_i2c_block_data(k3g_data->client,
 316                         CTRL_REG1 | AC, sizeof(k3g_data->ctrl_regs),
 317                                                 k3g_data->ctrl_regs);
 318                 if (err < 0) {
 319                         err = -EIO;
 320                         goto unlock;
 321                 }
 322
 323                 /* reset fifo entries */
 324                 err = k3g_restart_fifo(k3g_data);
 325                 if (err < 0) {
 326                         err = -EIO;
 327                         goto turn_off;
 328                 }
 329
 330                 if (k3g_data->interruptible)
 331                         enable_irq(k3g_data->client->irq);
 332                 else {
 333                         set_polling_delay(k3g_data, 0);
 334                         hrtimer_start(&k3g_data->timer,
 335                                 k3g_data->polling_delay, HRTIMER_MODE_REL);
 336                 }
 337         } else {
 338                 if (k3g_data->interruptible)
 339                         disable_irq(k3g_data->client->irq);
 340                 else {
 341                         hrtimer_cancel(&k3g_data->timer);
 342                         cancel_work_sync(&k3g_data->work);
 343                 }
 344                 /* turning off */
 345                 err = i2c_smbus_write_byte_data(k3g_data->client,
 346                                                 CTRL_REG1, 0x00);
 347                 if (err < 0)
 348                         goto unlock;
 349         }
 350         k3g_data->enable = new_enable;
 351
 352 turn_off:
 353         if (err < 0)
 354                 i2c_smbus_write_byte_data(k3g_data->client,
 355                                                 CTRL_REG1, 0x00);
 356 unlock:
 357         mutex_unlock(&k3g_data->lock);
 358
 359         return err ? err : size;
 360 }
 361
 362 static ssize_t k3g_show_delay(struct device *dev,
 363                         struct device_attribute *attr, char *buf)
 364 {
 365         struct k3g_data *k3g_data  = dev_get_drvdata(dev);
 366         u64 delay;
 367
 368         delay = k3g_data->time_to_read * k3g_data->entries;
 369         delay = ktime_to_ns(ns_to_ktime(delay));
 370
 371         return sprintf(buf, "%lld\n", delay);
 372 }
 373
 374 static ssize_t k3g_set_delay(struct device *dev,
 375                 struct device_attribute *attr, const char *buf, size_t size)
 376 {
 377         struct k3g_data *k3g_data  = dev_get_drvdata(dev);
 378         int odr_value = ODR105_BW25;
 379         int res = 0;
 380         int i;
 381         u64 delay_ns;
 382         u8 ctrl;
 383
 384         res = strict_strtoll(buf, 10, &delay_ns);
 385         if (res < 0)
 386                 return res;
 387
 388         mutex_lock(&k3g_data->lock);
 389         if (!k3g_data->interruptible)
 390                 hrtimer_cancel(&k3g_data->timer);
 391         else
 392                 disable_irq(k3g_data->client->irq);
 393
 394         /* round to the nearest supported ODR that is less than
 395          * the requested value
 396          */
 397         for (i = 0; i < ARRAY_SIZE(odr_delay_table); i++)
 398                 if (delay_ns <= odr_delay_table[i].delay_ns) {
 399                         odr_value = odr_delay_table[i].odr;
 400                         delay_ns = odr_delay_table[i].delay_ns;
 401                         k3g_data->time_to_read = delay_ns;
 402                         k3g_data->entries = 1;
 403                         break;
 404                 }
 405
 406         if (delay_ns >= odr_delay_table[3].delay_ns) {
 407                 if (delay_ns >= MAX_DELAY) {
 408                         k3g_data->entries = MAX_ENTRY;
 409                         delay_ns = MAX_DELAY;
 410                 } else {
 411                         do_div(delay_ns, odr_delay_table[3].delay_ns);
 412                         k3g_data->entries = delay_ns;
 413                 }
 414                 k3g_data->time_to_read = odr_delay_table[3].delay_ns;
 415         }
 416
 417         if (odr_value != (k3g_data->ctrl_regs[0] & ODR_MASK)) {
 418                 ctrl = (k3g_data->ctrl_regs[0] & ~ODR_MASK);
 419                 ctrl |= odr_value;
 420                 k3g_data->ctrl_regs[0] = ctrl;
 421                 res = i2c_smbus_write_byte_data(k3g_data->client,
 422                                                 CTRL_REG1, ctrl);
 423         }
 424
 425         /* we see a noise in the first sample or two after we
 426          * change rates.  this delay helps eliminate that noise.
 427          */
 428         msleep((u32)delay_ns * 2 / NSEC_PER_MSEC);
 429
 430         /* (re)start fifo */
 431         k3g_restart_fifo(k3g_data);
 432
 433         if (!k3g_data->interruptible) {
 434                 delay_ns = k3g_data->entries * k3g_data->time_to_read;
 435                 k3g_data->polling_delay = ns_to_ktime(delay_ns);
 436                 if (k3g_data->enable)
 437                         hrtimer_start(&k3g_data->timer,
 438                                 k3g_data->polling_delay, HRTIMER_MODE_REL);
 439         } else
 440                 enable_irq(k3g_data->client->irq);
 441
 442         mutex_unlock(&k3g_data->lock);
 443
 444         return size;
 445 }
 446
 447 static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR | S_IWGRP,
 448                         k3g_show_enable, k3g_set_enable);
 449 static DEVICE_ATTR(poll_delay, S_IRUGO | S_IWUSR | S_IWGRP,
 450                         k3g_show_delay, k3g_set_delay);
 451
 452 static int k3g_probe(struct i2c_client *client,
 453                                const struct i2c_device_id *devid)
 454 {
 455         int ret;
 456         int err = 0;
 457         struct k3g_data *data;
 458         struct input_dev *input_dev;
 459
 460         if (client->dev.platform_data == NULL) {
 461                 dev_err(&client->dev, "platform data is NULL. exiting.\n");
 462                 err = -ENODEV;
 463                 goto exit;
 464         }
 465
 466         data = kzalloc(sizeof(*data), GFP_KERNEL);
 467         if (data == NULL) {
 468                 dev_err(&client->dev,
 469                         "failed to allocate memory for module data\n");
 470                 err = -ENOMEM;
 471                 goto exit;
 472         }
 473
 474         data->client = client;
 475
 476         /* read chip id */
 477         ret = i2c_smbus_read_byte_data(client, WHO_AM_I);
 478         if (ret != DEVICE_ID) {
 479                 if (ret < 0) {
 480                         pr_err("%s: i2c for reading chip id failed\n",
 481                                                                 __func__);
 482                         err = ret;
 483                 } else {
 484                         pr_err("%s : Device identification failed\n",
 485                                                                 __func__);
 486                         err = -ENODEV;
 487                 }
 488                 goto err_read_reg;
 489         }
 490
 491         mutex_init(&data->lock);
 492
 493         /* allocate gyro input_device */
 494         input_dev = input_allocate_device();
 495         if (!input_dev) {
 496                 pr_err("%s: could not allocate input device\n", __func__);
 497                 err = -ENOMEM;
 498                 goto err_input_allocate_device;
 499         }
 500
 501         data->input_dev = input_dev;
 502         input_set_drvdata(input_dev, data);
 503         input_dev->name = "gyro";
 504         /* X */
 505         input_set_capability(input_dev, EV_REL, REL_RX);
 506         input_set_abs_params(input_dev, REL_RX, -2048, 2047, 0, 0);
 507         /* Y */
 508         input_set_capability(input_dev, EV_REL, REL_RY);
 509         input_set_abs_params(input_dev, REL_RY, -2048, 2047, 0, 0);
 510         /* Z */
 511         input_set_capability(input_dev, EV_REL, REL_RZ);
 512         input_set_abs_params(input_dev, REL_RZ, -2048, 2047, 0, 0);
 513
 514         err = input_register_device(input_dev);
 515         if (err < 0) {
 516                 pr_err("%s: could not register input device\n", __func__);
 517                 input_free_device(data->input_dev);
 518                 goto err_input_register_device;
 519         }
 520
 521         memcpy(&data->ctrl_regs, &default_ctrl_regs, sizeof(default_ctrl_regs));
 522         if (data->client->irq >= 0) { /* interrupt */
 523                 data->interruptible = true;
 524                 err = request_threaded_irq(data->client->irq, NULL,
 525                         k3g_interrupt_thread, IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
 526                                 "k3g", data);
 527                 if (err < 0) {
 528                         pr_err("%s: can't allocate irq.\n", __func__);
 529                         goto err_request_irq;
 530                 }
 531                 disable_irq(data->client->irq);
 532
 533         } else { /* polling */
 534                 u64 delay_ns;
 535                 data->ctrl_regs[2] = 0x00; /* disable interrupt */
 536                 /* hrtimer settings.  we poll for gyro values using a timer. */
 537                 hrtimer_init(&data->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
 538                 data->polling_delay = ns_to_ktime(200 * NSEC_PER_MSEC);
 539                 data->time_to_read = 10000000LL;
 540                 delay_ns = ktime_to_ns(data->polling_delay);
 541                 do_div(delay_ns, data->time_to_read);
 542                 data->entries = delay_ns;
 543                 data->timer.function = k3g_timer_func;
 544
 545                 /* the timer just fires off a work queue request.
 546                    We need a thread to read i2c (can be slow and blocking). */
 547                 data->k3g_wq = create_singlethread_workqueue("k3g_wq");
 548                 if (!data->k3g_wq) {
 549                         err = -ENOMEM;
 550                         pr_err("%s: could not create workqueue\n", __func__);
 551                         goto err_create_workqueue;
 552                 }
 553                 /* this is the thread function we run on the work queue */
 554                 INIT_WORK(&data->work, k3g_work_func);
 555         }
 556
 557         if (device_create_file(&input_dev->dev,
 558                                 &dev_attr_enable) < 0) {
 559                 pr_err("Failed to create device file(%s)!\n",
 560                                 dev_attr_enable.attr.name);
 561                 goto err_device_create_file;
 562         }
 563
 564         if (device_create_file(&input_dev->dev,
 565                                 &dev_attr_poll_delay) < 0) {
 566                 pr_err("Failed to create device file(%s)!\n",
 567                                 dev_attr_poll_delay.attr.name);
 568                 goto err_device_create_file2;
 569         }
 570
 571         i2c_set_clientdata(client, data);
 572         dev_set_drvdata(&input_dev->dev, data);
 573
 574         return 0;
 575
 576 err_device_create_file2:
 577         device_remove_file(&input_dev->dev, &dev_attr_enable);
 578 err_device_create_file:
 579         if (data->interruptible) {
 580                 enable_irq(data->client->irq);
 581                 free_irq(data->client->irq, data);
 582         } else
 583                 destroy_workqueue(data->k3g_wq);
 584         input_unregister_device(data->input_dev);
 585 err_create_workqueue:
 586 err_request_irq:
 587 err_input_register_device:
 588 err_input_allocate_device:
 589         mutex_destroy(&data->lock);
 590 err_read_reg:
 591         kfree(data);
 592 exit:
 593         return err;
 594 }
 595
 596 static int k3g_remove(struct i2c_client *client)
 597 {
 598         int err = 0;
 599         struct k3g_data *k3g_data = i2c_get_clientdata(client);
 600
 601         device_remove_file(&k3g_data->input_dev->dev, &dev_attr_enable);
 602         device_remove_file(&k3g_data->input_dev->dev, &dev_attr_poll_delay);
 603
 604         if (k3g_data->enable)
 605                 err = i2c_smbus_write_byte_data(k3g_data->client,
 606                                         CTRL_REG1, 0x00);
 607         if (k3g_data->interruptible) {
 608                 if (!k3g_data->enable) /* no disable_irq before free_irq */
 609                         enable_irq(k3g_data->client->irq);
 610                 free_irq(k3g_data->client->irq, k3g_data);
 611
 612         } else {
 613                 hrtimer_cancel(&k3g_data->timer);
 614                 cancel_work_sync(&k3g_data->work);
 615                 destroy_workqueue(k3g_data->k3g_wq);
 616         }
 617
 618         input_unregister_device(k3g_data->input_dev);
 619         mutex_destroy(&k3g_data->lock);
 620         kfree(k3g_data);
 621
 622         return err;
 623 }
 624
 625 static int k3g_suspend(struct device *dev)
 626 {
 627         int err = 0;
 628         struct i2c_client *client = to_i2c_client(dev);
 629         struct k3g_data *k3g_data = i2c_get_clientdata(client);
 630
 631         if (k3g_data->enable) {
 632                 mutex_lock(&k3g_data->lock);
 633                 if (!k3g_data->interruptible) {
 634                         hrtimer_cancel(&k3g_data->timer);
 635                         cancel_work_sync(&k3g_data->work);
 636                 }
 637                 err = i2c_smbus_write_byte_data(k3g_data->client,
 638                                                 CTRL_REG1, 0x00);
 639                 mutex_unlock(&k3g_data->lock);
 640         }
 641
 642         return err;
 643 }
 644
 645 static int k3g_resume(struct device *dev)
 646 {
 647         int err = 0;
 648         struct i2c_client *client = to_i2c_client(dev);
 649         struct k3g_data *k3g_data = i2c_get_clientdata(client);
 650
 651         if (k3g_data->enable) {
 652                 mutex_lock(&k3g_data->lock);
 653                 if (!k3g_data->interruptible)
 654                         hrtimer_start(&k3g_data->timer,
 655                                 k3g_data->polling_delay, HRTIMER_MODE_REL);
 656                 err = i2c_smbus_write_i2c_block_data(client,
 657                                 CTRL_REG1 | AC, sizeof(k3g_data->ctrl_regs),
 658                                                         k3g_data->ctrl_regs);
 659                 mutex_unlock(&k3g_data->lock);
 660         }
 661
 662         return err;
 663 }
 664
 665 static const struct dev_pm_ops k3g_pm_ops = {
 666         .suspend = k3g_suspend,
 667         .resume = k3g_resume
 668 };
 669
 670 static const struct i2c_device_id k3g_id[] = {
 671         { "k3g", 0 },
 672         { }
 673 };
 674
 675 MODULE_DEVICE_TABLE(i2c, k3g_id);
 676
 677 static struct i2c_driver k3g_driver = {
 678         .probe = k3g_probe,
 679         .remove = __devexit_p(k3g_remove),
 680         .id_table = k3g_id,
 681         .driver = {
 682                 .pm = &k3g_pm_ops,
 683                 .owner = THIS_MODULE,
 684                 .name = "k3g"
 685         },
 686 };
 687
 688 static int __init k3g_init(void)
 689 {
 690         return i2c_add_driver(&k3g_driver);
 691 }
 692
 693 static void __exit k3g_exit(void)
 694 {
 695         i2c_del_driver(&k3g_driver);
 696 }
 697
 698 module_init(k3g_init);
 699 module_exit(k3g_exit);
 700
 701 MODULE_DESCRIPTION("k3g digital gyroscope driver");
 702 MODULE_AUTHOR("Tim SK Lee Samsung Electronics <tim.sk.lee@samsung.com>");
 703 MODULE_LICENSE("GPL");