drivers/misc/inv_mpu/mpu-dev.c

   1 /*
   2         $License:
   3         Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
   4
   5         This program is free software; you can redistribute it and/or modify
   6         it under the terms of the GNU General Public License as published by
   7         the Free Software Foundation; either version 2 of the License, or
   8         (at your option) any later version.
   9
  10         This program is distributed in the hope that it will be useful,
  11         but WITHOUT ANY WARRANTY; without even the implied warranty of
  12         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13         GNU General Public License for more details.
  14
  15         You should have received a copy of the GNU General Public License
  16         along with this program.  If not, see <http://www.gnu.org/licenses/>.
  17         $
  18  */
  19 #define DEBUG
  20 #include <linux/i2c.h>
  21 #include <linux/i2c-dev.h>
  22 #include <linux/interrupt.h>
  23 #include <linux/module.h>
  24 #include <linux/moduleparam.h>
  25 #include <linux/kernel.h>
  26 #include <linux/init.h>
  27 #include <linux/stat.h>
  28 #include <linux/irq.h>
  29 #include <linux/gpio.h>
  30 #include <linux/signal.h>
  31 #include <linux/miscdevice.h>
  32 #include <linux/slab.h>
  33 #include <linux/version.h>
  34 #include <linux/pm.h>
  35 #include <linux/mutex.h>
  36 #include <linux/suspend.h>
  37 #include <linux/poll.h>
  38
  39 #include <linux/errno.h>
  40 #include <linux/fs.h>
  41 #include <linux/mm.h>
  42 #include <linux/sched.h>
  43 #include <linux/wait.h>
  44 #include <linux/uaccess.h>
  45 #include <linux/io.h>
  46
  47 #include "mpuirq.h"
  48 #include "slaveirq.h"
  49 #include "mlsl.h"
  50 #include "mldl_cfg.h"
  51 #include <linux/mpu.h>
  52
  53
  54 /* Platform data for the MPU */
  55 struct mpu_private_data {
  56         struct miscdevice dev;
  57         struct i2c_client *client;
  58
  59         /* mldl_cfg data */
  60         struct mldl_cfg mldl_cfg;
  61         struct mpu_ram          mpu_ram;
  62         struct mpu_gyro_cfg     mpu_gyro_cfg;
  63         struct mpu_offsets      mpu_offsets;
  64         struct mpu_chip_info    mpu_chip_info;
  65         struct inv_mpu_cfg      inv_mpu_cfg;
  66         struct inv_mpu_state    inv_mpu_state;
  67
  68         struct mutex mutex;
  69         wait_queue_head_t mpu_event_wait;
  70         struct completion completion;
  71         struct timer_list timeout;
  72         struct notifier_block nb;
  73         struct mpuirq_data mpu_pm_event;
  74         int response_timeout;   /* In seconds */
  75         unsigned long event;
  76         int pid;
  77         struct module *slave_modules[EXT_SLAVE_NUM_TYPES];
  78 };
  79
  80 struct mpu_private_data *mpu_private_data;
  81 static struct i2c_client *this_client;
  82
  83 static void mpu_pm_timeout(u_long data)
  84 {
  85         struct mpu_private_data *mpu = (struct mpu_private_data *)data;
  86         struct i2c_client *client = mpu->client;
  87         dev_dbg(&client->adapter->dev, "%s\n", __func__);
  88         complete(&mpu->completion);
  89 }
  90
  91 static int mpu_pm_notifier_callback(struct notifier_block *nb,
  92                                     unsigned long event, void *unused)
  93 {
  94         struct mpu_private_data *mpu =
  95             container_of(nb, struct mpu_private_data, nb);
  96         struct i2c_client *client = mpu->client;
  97         struct timeval event_time;
  98         dev_dbg(&client->adapter->dev, "%s: %ld\n", __func__, event);
  99
 100         /* Prevent the file handle from being closed before we initialize
 101            the completion event */
 102         mutex_lock(&mpu->mutex);
 103         if (!(mpu->pid) ||
 104             (event != PM_SUSPEND_PREPARE && event != PM_POST_SUSPEND)) {
 105                 mutex_unlock(&mpu->mutex);
 106                 return NOTIFY_OK;
 107         }
 108
 109         if (event == PM_SUSPEND_PREPARE)
 110                 mpu->event = MPU_PM_EVENT_SUSPEND_PREPARE;
 111         if (event == PM_POST_SUSPEND)
 112                 mpu->event = MPU_PM_EVENT_POST_SUSPEND;
 113
 114         do_gettimeofday(&event_time);
 115         mpu->mpu_pm_event.interruptcount++;
 116         mpu->mpu_pm_event.irqtime =
 117             (((long long)event_time.tv_sec) << 32) + event_time.tv_usec;
 118         mpu->mpu_pm_event.data_type = MPUIRQ_DATA_TYPE_PM_EVENT;
 119         mpu->mpu_pm_event.data = mpu->event;
 120
 121         if (mpu->response_timeout > 0) {
 122                 mpu->timeout.expires = jiffies + mpu->response_timeout * HZ;
 123                 add_timer(&mpu->timeout);
 124         }
 125         INIT_COMPLETION(mpu->completion);
 126         mutex_unlock(&mpu->mutex);
 127
 128         wake_up_interruptible(&mpu->mpu_event_wait);
 129         wait_for_completion(&mpu->completion);
 130         del_timer_sync(&mpu->timeout);
 131         dev_dbg(&client->adapter->dev, "%s: %ld DONE\n", __func__, event);
 132         return NOTIFY_OK;
 133 }
 134
 135 static int mpu_dev_open(struct inode *inode, struct file *file)
 136 {
 137         struct mpu_private_data *mpu =
 138             (struct mpu_private_data *) i2c_get_clientdata(this_client);
 139         struct i2c_client *client = mpu->client;
 140         int result;
 141         int ii;
 142         dev_dbg(&client->adapter->dev, "%s\n", __func__);
 143         dev_dbg(&client->adapter->dev, "current->pid %d\n", current->pid);
 144
 145         result = mutex_lock_interruptible(&mpu->mutex);
 146         if (mpu->pid) {
 147                 mutex_unlock(&mpu->mutex);
 148                 return -EBUSY;
 149         }
 150         mpu->pid = current->pid;
 151         file->private_data = &mpu->dev;
 152
 153         /* Reset the sensors to the default */
 154         if (result) {
 155                 dev_err(&client->adapter->dev,
 156                         "%s: mutex_lock_interruptible returned %d\n",
 157                         __func__, result);
 158                 return result;
 159         }
 160
 161         for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++)
 162                 __module_get(mpu->slave_modules[ii]);
 163
 164         mutex_unlock(&mpu->mutex);
 165         return 0;
 166 }
 167
 168 /* close function - called when the "file" /dev/mpu is closed in userspace   */
 169 static int mpu_release(struct inode *inode, struct file *file)
 170 {
 171         struct mpu_private_data *mpu =
 172             container_of(file->private_data, struct mpu_private_data, dev);
 173         struct i2c_client *client = mpu->client;
 174         struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
 175         int result = 0;
 176         int ii;
 177         struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
 178         struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
 179
 180         for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
 181                 if (!pdata_slave[ii])
 182                         slave_adapter[ii] = NULL;
 183                 else
 184                         slave_adapter[ii] =
 185                                 i2c_get_adapter(pdata_slave[ii]->adapt_num);
 186         }
 187         slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
 188
 189         mutex_lock(&mpu->mutex);
 190         mldl_cfg->inv_mpu_cfg->requested_sensors = 0;
 191         result = inv_mpu_suspend(mldl_cfg,
 192                                 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
 193                                 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
 194                                 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
 195                                 slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
 196                                 INV_ALL_SENSORS);
 197         mpu->pid = 0;
 198         for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++)
 199                 module_put(mpu->slave_modules[ii]);
 200
 201         mutex_unlock(&mpu->mutex);
 202         complete(&mpu->completion);
 203         dev_dbg(&client->adapter->dev, "mpu_release\n");
 204
 205         return result;
 206 }
 207
 208 /* read function called when from /dev/mpu is read.  Read from the FIFO */
 209 static ssize_t mpu_read(struct file *file,
 210                         char __user *buf, size_t count, loff_t *offset)
 211 {
 212         struct mpu_private_data *mpu =
 213             container_of(file->private_data, struct mpu_private_data, dev);
 214         struct i2c_client *client = mpu->client;
 215         size_t len = sizeof(mpu->mpu_pm_event) + sizeof(unsigned long);
 216         int err;
 217
 218         if (!mpu->event && (!(file->f_flags & O_NONBLOCK)))
 219                 wait_event_interruptible(mpu->mpu_event_wait, mpu->event);
 220
 221         if (!mpu->event || !buf
 222             || count < sizeof(mpu->mpu_pm_event))
 223                 return 0;
 224
 225         err = copy_to_user(buf, &mpu->mpu_pm_event, sizeof(mpu->mpu_pm_event));
 226         if (err) {
 227                 dev_err(&client->adapter->dev,
 228                         "Copy to user returned %d\n", err);
 229                 return -EFAULT;
 230         }
 231         mpu->event = 0;
 232         return len;
 233 }
 234
 235 static unsigned int mpu_poll(struct file *file, struct poll_table_struct *poll)
 236 {
 237         struct mpu_private_data *mpu =
 238             container_of(file->private_data, struct mpu_private_data, dev);
 239         int mask = 0;
 240
 241         poll_wait(file, &mpu->mpu_event_wait, poll);
 242         if (mpu->event)
 243                 mask |= POLLIN | POLLRDNORM;
 244         return mask;
 245 }
 246
 247 static int mpu_dev_ioctl_get_ext_slave_platform_data(
 248         struct i2c_client *client,
 249         struct ext_slave_platform_data __user *arg)
 250 {
 251         struct mpu_private_data *mpu =
 252             (struct mpu_private_data *)i2c_get_clientdata(client);
 253         struct ext_slave_platform_data *pdata_slave;
 254         struct ext_slave_platform_data local_pdata_slave;
 255
 256         if (copy_from_user(&local_pdata_slave, arg, sizeof(local_pdata_slave)))
 257                 return -EFAULT;
 258
 259         if (local_pdata_slave.type >= EXT_SLAVE_NUM_TYPES)
 260                 return -EINVAL;
 261
 262         pdata_slave = mpu->mldl_cfg.pdata_slave[local_pdata_slave.type];
 263         /* All but private data and irq_data */
 264         if (!pdata_slave)
 265                 return -ENODEV;
 266         if (copy_to_user(arg, pdata_slave, sizeof(*pdata_slave)))
 267                 return -EFAULT;
 268         return 0;
 269 }
 270
 271 static int mpu_dev_ioctl_get_mpu_platform_data(
 272         struct i2c_client *client,
 273         struct mpu_platform_data __user *arg)
 274 {
 275         struct mpu_private_data *mpu =
 276             (struct mpu_private_data *)i2c_get_clientdata(client);
 277         struct mpu_platform_data *pdata = mpu->mldl_cfg.pdata;
 278
 279         if (copy_to_user(arg, pdata, sizeof(*pdata)))
 280                 return -EFAULT;
 281         return 0;
 282 }
 283
 284 static int mpu_dev_ioctl_get_ext_slave_descr(
 285         struct i2c_client *client,
 286         struct ext_slave_descr __user *arg)
 287 {
 288         struct mpu_private_data *mpu =
 289             (struct mpu_private_data *)i2c_get_clientdata(client);
 290         struct ext_slave_descr *slave;
 291         struct ext_slave_descr local_slave;
 292
 293         if (copy_from_user(&local_slave, arg, sizeof(local_slave)))
 294                 return -EFAULT;
 295
 296         if (local_slave.type >= EXT_SLAVE_NUM_TYPES)
 297                 return -EINVAL;
 298
 299         slave = mpu->mldl_cfg.slave[local_slave.type];
 300         /* All but private data and irq_data */
 301         if (!slave)
 302                 return -ENODEV;
 303         if (copy_to_user(arg, slave, sizeof(*slave)))
 304                 return -EFAULT;
 305         return 0;
 306 }
 307
 308
 309 /**
 310  * slave_config() - Pass a requested slave configuration to the slave sensor
 311  *
 312  * @adapter the adaptor to use to communicate with the slave
 313  * @mldl_cfg the mldl configuration structuer
 314  * @slave pointer to the slave descriptor
 315  * @usr_config The configuration to pass to the slave sensor
 316  *
 317  * returns 0 or non-zero error code
 318  */
 319 static int inv_mpu_config(struct mldl_cfg *mldl_cfg,
 320                         void *gyro_adapter,
 321                         struct ext_slave_config __user *usr_config)
 322 {
 323         int retval = 0;
 324         struct ext_slave_config config;
 325
 326         retval = copy_from_user(&config, usr_config, sizeof(config));
 327         if (retval)
 328                 return -EFAULT;
 329
 330         if (config.len && config.data) {
 331                 void *data;
 332                 data = kmalloc(config.len, GFP_KERNEL);
 333                 if (!data)
 334                         return -ENOMEM;
 335
 336                 retval = copy_from_user(data,
 337                                         (void __user *)config.data, config.len);
 338                 if (retval) {
 339                         retval = -EFAULT;
 340                         kfree(data);
 341                         return retval;
 342                 }
 343                 config.data = data;
 344         }
 345         retval = gyro_config(gyro_adapter, mldl_cfg, &config);
 346         kfree(config.data);
 347         return retval;
 348 }
 349
 350 static int inv_mpu_get_config(struct mldl_cfg *mldl_cfg,
 351                             void *gyro_adapter,
 352                             struct ext_slave_config __user *usr_config)
 353 {
 354         int retval = 0;
 355         struct ext_slave_config config;
 356         void *user_data;
 357
 358         retval = copy_from_user(&config, usr_config, sizeof(config));
 359         if (retval)
 360                 return -EFAULT;
 361
 362         user_data = config.data;
 363         if (config.len && config.data) {
 364                 void *data;
 365                 data = kmalloc(config.len, GFP_KERNEL);
 366                 if (!data)
 367                         return -ENOMEM;
 368
 369                 retval = copy_from_user(data,
 370                                         (void __user *)config.data, config.len);
 371                 if (retval) {
 372                         retval = -EFAULT;
 373                         kfree(data);
 374                         return retval;
 375                 }
 376                 config.data = data;
 377         }
 378         retval = gyro_get_config(gyro_adapter, mldl_cfg, &config);
 379         if (!retval)
 380                 retval = copy_to_user((unsigned char __user *)user_data,
 381                                 config.data, config.len);
 382         kfree(config.data);
 383         return retval;
 384 }
 385
 386 static int slave_config(struct mldl_cfg *mldl_cfg,
 387                         void *gyro_adapter,
 388                         void *slave_adapter,
 389                         struct ext_slave_descr *slave,
 390                         struct ext_slave_platform_data *pdata,
 391                         struct ext_slave_config __user *usr_config)
 392 {
 393         int retval = 0;
 394         struct ext_slave_config config;
 395         if ((!slave) || (!slave->config))
 396                 return -ENODEV;
 397
 398         retval = copy_from_user(&config, usr_config, sizeof(config));
 399         if (retval)
 400                 return -EFAULT;
 401
 402         if (config.len && config.data) {
 403                 void *data;
 404                 data = kmalloc(config.len, GFP_KERNEL);
 405                 if (!data)
 406                         return -ENOMEM;
 407
 408                 retval = copy_from_user(data,
 409                                         (void __user *)config.data, config.len);
 410                 if (retval) {
 411                         retval = -EFAULT;
 412                         kfree(data);
 413                         return retval;
 414                 }
 415                 config.data = data;
 416         }
 417         retval = inv_mpu_slave_config(mldl_cfg, gyro_adapter, slave_adapter,
 418                                       &config, slave, pdata);
 419         kfree(config.data);
 420         return retval;
 421 }
 422
 423 static int slave_get_config(struct mldl_cfg *mldl_cfg,
 424                             void *gyro_adapter,
 425                             void *slave_adapter,
 426                             struct ext_slave_descr *slave,
 427                             struct ext_slave_platform_data *pdata,
 428                             struct ext_slave_config __user *usr_config)
 429 {
 430         int retval = 0;
 431         struct ext_slave_config config;
 432         void *user_data;
 433         if (!(slave) || !(slave->get_config))
 434                 return -ENODEV;
 435
 436         retval = copy_from_user(&config, usr_config, sizeof(config));
 437         if (retval)
 438                 return -EFAULT;
 439
 440         user_data = config.data;
 441         if (config.len && config.data) {
 442                 void *data;
 443                 data = kmalloc(config.len, GFP_KERNEL);
 444                 if (!data)
 445                         return -ENOMEM;
 446
 447                 retval = copy_from_user(data,
 448                                         (void __user *)config.data, config.len);
 449                 if (retval) {
 450                         retval = -EFAULT;
 451                         kfree(data);
 452                         return retval;
 453                 }
 454                 config.data = data;
 455         }
 456         retval = inv_mpu_get_slave_config(mldl_cfg, gyro_adapter,
 457                                           slave_adapter, &config, slave, pdata);
 458         if (retval) {
 459                 kfree(config.data);
 460                 return retval;
 461         }
 462         retval = copy_to_user((unsigned char __user *)user_data,
 463                               config.data, config.len);
 464         kfree(config.data);
 465         return retval;
 466 }
 467
 468 static int inv_slave_read(struct mldl_cfg *mldl_cfg,
 469                           void *gyro_adapter,
 470                           void *slave_adapter,
 471                           struct ext_slave_descr *slave,
 472                           struct ext_slave_platform_data *pdata,
 473                           void __user *usr_data)
 474 {
 475         int retval;
 476         unsigned char *data;
 477         data = kzalloc(slave->read_len, GFP_KERNEL);
 478         if (!data)
 479                 return -EFAULT;
 480
 481         retval = inv_mpu_slave_read(mldl_cfg, gyro_adapter, slave_adapter,
 482                                     slave, pdata, data);
 483
 484         if ((!retval) &&
 485             (copy_to_user((unsigned char __user *)usr_data,
 486                           data, slave->read_len)))
 487                 retval = -EFAULT;
 488
 489         kfree(data);
 490         return retval;
 491 }
 492
 493 static int mpu_handle_mlsl(void *sl_handle,
 494                            unsigned char addr,
 495                            unsigned int cmd,
 496                            struct mpu_read_write __user *usr_msg)
 497 {
 498         int retval = 0;
 499         struct mpu_read_write msg;
 500         unsigned char *user_data;
 501         retval = copy_from_user(&msg, usr_msg, sizeof(msg));
 502         if (retval)
 503                 return -EFAULT;
 504
 505         user_data = msg.data;
 506         if (msg.length && msg.data) {
 507                 unsigned char *data;
 508                 data = kmalloc(msg.length, GFP_KERNEL);
 509                 if (!data)
 510                         return -ENOMEM;
 511
 512                 retval = copy_from_user(data,
 513                                         (void __user *)msg.data, msg.length);
 514                 if (retval) {
 515                         retval = -EFAULT;
 516                         kfree(data);
 517                         return retval;
 518                 }
 519                 msg.data = data;
 520         } else {
 521                 return -EPERM;
 522         }
 523
 524         switch (cmd) {
 525         case MPU_READ:
 526                 retval = inv_serial_read(sl_handle, addr,
 527                                          msg.address, msg.length, msg.data);
 528                 break;
 529         case MPU_WRITE:
 530                 retval = inv_serial_write(sl_handle, addr,
 531                                           msg.length, msg.data);
 532                 break;
 533         case MPU_READ_MEM:
 534                 retval = inv_serial_read_mem(sl_handle, addr,
 535                                              msg.address, msg.length, msg.data);
 536                 break;
 537         case MPU_WRITE_MEM:
 538                 retval = inv_serial_write_mem(sl_handle, addr,
 539                                               msg.address, msg.length,
 540                                               msg.data);
 541                 break;
 542         case MPU_READ_FIFO:
 543                 retval = inv_serial_read_fifo(sl_handle, addr,
 544                                               msg.length, msg.data);
 545                 break;
 546         case MPU_WRITE_FIFO:
 547                 retval = inv_serial_write_fifo(sl_handle, addr,
 548                                                msg.length, msg.data);
 549                 break;
 550
 551         };
 552         if (retval) {
 553                 dev_err(&((struct i2c_adapter *)sl_handle)->dev,
 554                         "%s: i2c %d error %d\n",
 555                         __func__, cmd, retval);
 556                 kfree(msg.data);
 557                 return retval;
 558         }
 559         retval = copy_to_user((unsigned char __user *)user_data,
 560                               msg.data, msg.length);
 561         kfree(msg.data);
 562         return retval;
 563 }
 564
 565 /* ioctl - I/O control */
 566 static long mpu_dev_ioctl(struct file *file,
 567                           unsigned int cmd, unsigned long arg)
 568 {
 569         struct mpu_private_data *mpu =
 570             container_of(file->private_data, struct mpu_private_data, dev);
 571         struct i2c_client *client = mpu->client;
 572         struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
 573         int retval = 0;
 574         struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
 575         struct ext_slave_descr **slave = mldl_cfg->slave;
 576         struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
 577         int ii;
 578
 579         for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
 580                 if (!pdata_slave[ii])
 581                         slave_adapter[ii] = NULL;
 582                 else
 583                         slave_adapter[ii] =
 584                                 i2c_get_adapter(pdata_slave[ii]->adapt_num);
 585         }
 586         slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
 587
 588         retval = mutex_lock_interruptible(&mpu->mutex);
 589         if (retval) {
 590                 dev_err(&this_client->adapter->dev,
 591                         "%s: mutex_lock_interruptible returned %d\n",
 592                         __func__, retval);
 593                 return retval;
 594         }
 595
 596         switch (cmd) {
 597         case MPU_GET_EXT_SLAVE_PLATFORM_DATA:
 598                 retval = mpu_dev_ioctl_get_ext_slave_platform_data(
 599                         client,
 600                         (struct ext_slave_platform_data __user *)arg);
 601                 break;
 602         case MPU_GET_MPU_PLATFORM_DATA:
 603                 retval = mpu_dev_ioctl_get_mpu_platform_data(
 604                         client,
 605                         (struct mpu_platform_data __user *)arg);
 606                 break;
 607         case MPU_GET_EXT_SLAVE_DESCR:
 608                 retval = mpu_dev_ioctl_get_ext_slave_descr(
 609                         client,
 610                         (struct ext_slave_descr __user *)arg);
 611                 break;
 612         case MPU_READ:
 613         case MPU_WRITE:
 614         case MPU_READ_MEM:
 615         case MPU_WRITE_MEM:
 616         case MPU_READ_FIFO:
 617         case MPU_WRITE_FIFO:
 618                 retval = mpu_handle_mlsl(
 619                         slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
 620                         mldl_cfg->mpu_chip_info->addr, cmd,
 621                         (struct mpu_read_write __user *)arg);
 622                 break;
 623         case MPU_CONFIG_GYRO:
 624                 retval = inv_mpu_config(
 625                         mldl_cfg,
 626                         slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
 627                         (struct ext_slave_config __user *)arg);
 628                 break;
 629         case MPU_CONFIG_ACCEL:
 630                 retval = slave_config(
 631                         mldl_cfg,
 632                         slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
 633                         slave_adapter[EXT_SLAVE_TYPE_ACCEL],
 634                         slave[EXT_SLAVE_TYPE_ACCEL],
 635                         pdata_slave[EXT_SLAVE_TYPE_ACCEL],
 636                         (struct ext_slave_config __user *)arg);
 637                 break;
 638         case MPU_CONFIG_COMPASS:
 639                 retval = slave_config(
 640                         mldl_cfg,
 641                         slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
 642                         slave_adapter[EXT_SLAVE_TYPE_COMPASS],
 643                         slave[EXT_SLAVE_TYPE_COMPASS],
 644                         pdata_slave[EXT_SLAVE_TYPE_COMPASS],
 645                         (struct ext_slave_config __user *)arg);
 646                 break;
 647         case MPU_CONFIG_PRESSURE:
 648                 retval = slave_config(
 649                         mldl_cfg,
 650                         slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
 651                         slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
 652                         slave[EXT_SLAVE_TYPE_PRESSURE],
 653                         pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
 654                         (struct ext_slave_config __user *)arg);
 655                 break;
 656         case MPU_GET_CONFIG_GYRO:
 657                 retval = inv_mpu_get_config(
 658                         mldl_cfg,
 659                         slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
 660                         (struct ext_slave_config __user *)arg);
 661                 break;
 662         case MPU_GET_CONFIG_ACCEL:
 663                 retval = slave_get_config(
 664                         mldl_cfg,
 665                         slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
 666                         slave_adapter[EXT_SLAVE_TYPE_ACCEL],
 667                         slave[EXT_SLAVE_TYPE_ACCEL],
 668                         pdata_slave[EXT_SLAVE_TYPE_ACCEL],
 669                         (struct ext_slave_config __user *)arg);
 670                 break;
 671         case MPU_GET_CONFIG_COMPASS:
 672                 retval = slave_get_config(
 673                         mldl_cfg,
 674                         slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
 675                         slave_adapter[EXT_SLAVE_TYPE_COMPASS],
 676                         slave[EXT_SLAVE_TYPE_COMPASS],
 677                         pdata_slave[EXT_SLAVE_TYPE_COMPASS],
 678                         (struct ext_slave_config __user *)arg);
 679                 break;
 680         case MPU_GET_CONFIG_PRESSURE:
 681                 retval = slave_get_config(
 682                         mldl_cfg,
 683                         slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
 684                         slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
 685                         slave[EXT_SLAVE_TYPE_PRESSURE],
 686                         pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
 687                         (struct ext_slave_config __user *)arg);
 688                 break;
 689         case MPU_SUSPEND:
 690                 retval = inv_mpu_suspend(
 691                         mldl_cfg,
 692                         slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
 693                         slave_adapter[EXT_SLAVE_TYPE_ACCEL],
 694                         slave_adapter[EXT_SLAVE_TYPE_COMPASS],
 695                         slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
 696                         arg);
 697                 break;
 698         case MPU_RESUME:
 699                 retval = inv_mpu_resume(
 700                         mldl_cfg,
 701                         slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
 702                         slave_adapter[EXT_SLAVE_TYPE_ACCEL],
 703                         slave_adapter[EXT_SLAVE_TYPE_COMPASS],
 704                         slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
 705                         arg);
 706                 break;
 707         case MPU_PM_EVENT_HANDLED:
 708                 dev_dbg(&client->adapter->dev, "%s: %d\n", __func__, cmd);
 709                 complete(&mpu->completion);
 710                 break;
 711         case MPU_READ_ACCEL:
 712                 retval = inv_slave_read(
 713                         mldl_cfg,
 714                         slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
 715                         slave_adapter[EXT_SLAVE_TYPE_ACCEL],
 716                         slave[EXT_SLAVE_TYPE_ACCEL],
 717                         pdata_slave[EXT_SLAVE_TYPE_ACCEL],
 718                         (unsigned char __user *)arg);
 719                 break;
 720         case MPU_READ_COMPASS:
 721                 retval = inv_slave_read(
 722                         mldl_cfg,
 723                         slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
 724                         slave_adapter[EXT_SLAVE_TYPE_COMPASS],
 725                         slave[EXT_SLAVE_TYPE_COMPASS],
 726                         pdata_slave[EXT_SLAVE_TYPE_COMPASS],
 727                         (unsigned char __user *)arg);
 728                 break;
 729         case MPU_READ_PRESSURE:
 730                 retval = inv_slave_read(
 731                         mldl_cfg,
 732                         slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
 733                         slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
 734                         slave[EXT_SLAVE_TYPE_PRESSURE],
 735                         pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
 736                         (unsigned char __user *)arg);
 737                 break;
 738         case MPU_GET_REQUESTED_SENSORS:
 739                 if (copy_to_user(
 740                            (__u32 __user *)arg,
 741                            &mldl_cfg->inv_mpu_cfg->requested_sensors,
 742                            sizeof(mldl_cfg->inv_mpu_cfg->requested_sensors)))
 743                         retval = -EFAULT;
 744                 break;
 745         case MPU_SET_REQUESTED_SENSORS:
 746                 mldl_cfg->inv_mpu_cfg->requested_sensors = arg;
 747                 break;
 748         case MPU_GET_IGNORE_SYSTEM_SUSPEND:
 749                 if (copy_to_user(
 750                         (unsigned char __user *)arg,
 751                         &mldl_cfg->inv_mpu_cfg->ignore_system_suspend,
 752                         sizeof(mldl_cfg->inv_mpu_cfg->ignore_system_suspend)))
 753                         retval = -EFAULT;
 754                 break;
 755         case MPU_SET_IGNORE_SYSTEM_SUSPEND:
 756                 mldl_cfg->inv_mpu_cfg->ignore_system_suspend = arg;
 757                 break;
 758         case MPU_GET_MLDL_STATUS:
 759                 if (copy_to_user(
 760                         (unsigned char __user *)arg,
 761                         &mldl_cfg->inv_mpu_state->status,
 762                         sizeof(mldl_cfg->inv_mpu_state->status)))
 763                         retval = -EFAULT;
 764                 break;
 765         case MPU_GET_I2C_SLAVES_ENABLED:
 766                 if (copy_to_user(
 767                         (unsigned char __user *)arg,
 768                         &mldl_cfg->inv_mpu_state->i2c_slaves_enabled,
 769                         sizeof(mldl_cfg->inv_mpu_state->i2c_slaves_enabled)))
 770                         retval = -EFAULT;
 771                 break;
 772         default:
 773                 dev_err(&client->adapter->dev,
 774                         "%s: Unknown cmd %x, arg %lu\n",
 775                         __func__, cmd, arg);
 776                 retval = -EINVAL;
 777         };
 778
 779         mutex_unlock(&mpu->mutex);
 780
 781
 782         //dev_dbg(&client->adapter->dev, "%s: %08x, %08lx, %d\n",__func__, cmd, arg, retval);
 783
 784         if (retval > 0)
 785                 retval = -retval;
 786
 787         return retval;
 788 }
 789
 790 void mpu_shutdown(struct i2c_client *client)
 791 {
 792         struct mpu_private_data *mpu =
 793             (struct mpu_private_data *)i2c_get_clientdata(client);
 794         struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
 795         struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
 796         struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
 797         int ii;
 798
 799         for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
 800                 if (!pdata_slave[ii])
 801                         slave_adapter[ii] = NULL;
 802                 else
 803                         slave_adapter[ii] =
 804                                 i2c_get_adapter(pdata_slave[ii]->adapt_num);
 805         }
 806         slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
 807
 808         mutex_lock(&mpu->mutex);
 809         (void)inv_mpu_suspend(mldl_cfg,
 810                         slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
 811                         slave_adapter[EXT_SLAVE_TYPE_ACCEL],
 812                         slave_adapter[EXT_SLAVE_TYPE_COMPASS],
 813                         slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
 814                         INV_ALL_SENSORS);
 815         mutex_unlock(&mpu->mutex);
 816         dev_dbg(&client->adapter->dev, "%s\n", __func__);
 817 }
 818
 819 int mpu_dev_suspend(struct i2c_client *client, pm_message_t mesg)
 820 {
 821         struct mpu_private_data *mpu =
 822             (struct mpu_private_data *)i2c_get_clientdata(client);
 823         struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
 824         struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
 825         struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
 826         int ii;
 827
 828         for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
 829                 if (!pdata_slave[ii])
 830                         slave_adapter[ii] = NULL;
 831                 else
 832                         slave_adapter[ii] =
 833                                 i2c_get_adapter(pdata_slave[ii]->adapt_num);
 834         }
 835         slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
 836
 837         mutex_lock(&mpu->mutex);
 838         if (!mldl_cfg->inv_mpu_cfg->ignore_system_suspend) {
 839                 dev_dbg(&client->adapter->dev,
 840                         "%s: suspending on event %d\n", __func__, mesg.event);
 841                 (void)inv_mpu_suspend(mldl_cfg,
 842                                 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
 843                                 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
 844                                 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
 845                                 slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
 846                                 INV_ALL_SENSORS);
 847         } else {
 848                 dev_dbg(&client->adapter->dev,
 849                         "%s: Already suspended %d\n", __func__, mesg.event);
 850         }
 851         mutex_unlock(&mpu->mutex);
 852         return 0;
 853 }
 854
 855 int mpu_dev_resume(struct i2c_client *client)
 856 {
 857         struct mpu_private_data *mpu =
 858             (struct mpu_private_data *)i2c_get_clientdata(client);
 859         struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
 860         struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
 861         struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
 862         int ii;
 863
 864         for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
 865                 if (!pdata_slave[ii])
 866                         slave_adapter[ii] = NULL;
 867                 else
 868                         slave_adapter[ii] =
 869                                 i2c_get_adapter(pdata_slave[ii]->adapt_num);
 870         }
 871         slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
 872
 873         mutex_lock(&mpu->mutex);
 874         if (mpu->pid && !mldl_cfg->inv_mpu_cfg->ignore_system_suspend) {
 875                 (void)inv_mpu_resume(mldl_cfg,
 876                                 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
 877                                 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
 878                                 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
 879                                 slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
 880                                 mldl_cfg->inv_mpu_cfg->requested_sensors);
 881                 dev_dbg(&client->adapter->dev,
 882                         "%s for pid %d\n", __func__, mpu->pid);
 883         }
 884         mutex_unlock(&mpu->mutex);
 885         return 0;
 886 }
 887
 888 /* define which file operations are supported */
 889 static const struct file_operations mpu_fops = {
 890         .owner = THIS_MODULE,
 891         .read = mpu_read,
 892         .poll = mpu_poll,
 893 #if HAVE_COMPAT_IOCTL
 894         .compat_ioctl = mpu_dev_ioctl,
 895 #endif
 896 #if HAVE_UNLOCKED_IOCTL
 897         .unlocked_ioctl = mpu_dev_ioctl,
 898 #endif
 899         //.unlocked_ioctl = mpu_dev_ioctl,
 900         .open = mpu_dev_open,
 901         .release = mpu_release,
 902 };
 903
 904 int inv_mpu_register_slave(struct module *slave_module,
 905                         struct i2c_client *slave_client,
 906                         struct ext_slave_platform_data *slave_pdata,
 907                         struct ext_slave_descr *(*get_slave_descr)(void))
 908 {
 909         struct mpu_private_data *mpu = mpu_private_data;
 910         struct mldl_cfg *mldl_cfg;
 911         struct ext_slave_descr *slave_descr;
 912         struct ext_slave_platform_data **pdata_slave;
 913         char *irq_name = NULL;
 914         int result = 0;
 915
 916         if (!slave_client || !slave_pdata || !get_slave_descr)
 917                 return -EINVAL;
 918
 919         if (!mpu) {
 920                 dev_err(&slave_client->adapter->dev,
 921                         "%s: Null mpu_private_data\n", __func__);
 922                 return -EINVAL;
 923         }
 924         mldl_cfg    = &mpu->mldl_cfg;
 925         pdata_slave = mldl_cfg->pdata_slave;
 926         slave_descr = get_slave_descr();
 927
 928         if (!slave_descr) {
 929                 dev_err(&slave_client->adapter->dev,
 930                         "%s: Null ext_slave_descr\n", __func__);
 931                 return -EINVAL;
 932         }
 933
 934         mutex_lock(&mpu->mutex);
 935         if (mpu->pid) {
 936                 mutex_unlock(&mpu->mutex);
 937                 return -EBUSY;
 938         }
 939
 940         if (pdata_slave[slave_descr->type]) {
 941                 result = -EBUSY;
 942                 goto out_unlock_mutex;
 943         }
 944
 945         slave_pdata->address    = slave_client->addr;
 946         slave_pdata->irq        = gpio_to_irq(slave_client->irq);
 947         slave_pdata->adapt_num  = i2c_adapter_id(slave_client->adapter);
 948
 949         dev_info(&slave_client->adapter->dev,
 950                 "%s: +%s Type %d: Addr: %2x IRQ: %2d, Adapt: %2d\n",
 951                 __func__,
 952                 slave_descr->name,
 953                 slave_descr->type,
 954                 slave_pdata->address,
 955                 slave_pdata->irq,
 956                 slave_pdata->adapt_num);
 957
 958         switch (slave_descr->type) {
 959         case EXT_SLAVE_TYPE_ACCEL:
 960                 irq_name = "accelirq";
 961                 break;
 962         case EXT_SLAVE_TYPE_COMPASS:
 963                 irq_name = "compassirq";
 964                 break;
 965         case EXT_SLAVE_TYPE_PRESSURE:
 966                 irq_name = "pressureirq";
 967                 break;
 968         default:
 969                 irq_name = "none";
 970         };
 971         if (slave_descr->init) {
 972                 result = slave_descr->init(slave_client->adapter,
 973                                         slave_descr,
 974                                         slave_pdata);
 975                 if (result) {
 976                         dev_err(&slave_client->adapter->dev,
 977                                 "%s init failed %d\n",
 978                                 slave_descr->name, result);
 979                         goto out_unlock_mutex;
 980                 }
 981         }
 982
 983         pdata_slave[slave_descr->type] = slave_pdata;
 984         mpu->slave_modules[slave_descr->type] = slave_module;
 985         mldl_cfg->slave[slave_descr->type] = slave_descr;
 986
 987         goto out_unlock_mutex;
 988
 989 out_unlock_mutex:
 990         mutex_unlock(&mpu->mutex);
 991
 992         if (!result && irq_name && (slave_pdata->irq > 0)) {
 993                 int warn_result;
 994                 dev_info(&slave_client->adapter->dev,
 995                         "Installing %s irq using %d\n",
 996                         irq_name,
 997                         slave_pdata->irq);
 998                 warn_result = slaveirq_init(slave_client->adapter,
 999                                         slave_pdata, irq_name);
1000                 if (result)
1001                         dev_err(&slave_client->adapter->dev,
1002                                 "%s irq assigned error: %d\n",
1003                                 slave_descr->name, warn_result);
1004         } else {
1005                 dev_warn(&slave_client->adapter->dev,
1006                         "%s irq not assigned: %d %d %d\n",
1007                         slave_descr->name,
1008                         result, (int)irq_name, slave_pdata->irq);
1009         }
1010
1011         return result;
1012 }
1013 EXPORT_SYMBOL(inv_mpu_register_slave);
1014
1015 void inv_mpu_unregister_slave(struct i2c_client *slave_client,
1016                         struct ext_slave_platform_data *slave_pdata,
1017                         struct ext_slave_descr *(*get_slave_descr)(void))
1018 {
1019         struct mpu_private_data *mpu = mpu_private_data;
1020         struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
1021         struct ext_slave_descr *slave_descr;
1022         int result;
1023
1024         dev_info(&slave_client->adapter->dev, "%s\n", __func__);
1025
1026         if (!slave_client || !slave_pdata || !get_slave_descr)
1027                 return;
1028
1029         if (slave_pdata->irq)
1030                 slaveirq_exit(slave_pdata);
1031
1032         slave_descr = get_slave_descr();
1033         if (!slave_descr)
1034                 return;
1035
1036         mutex_lock(&mpu->mutex);
1037
1038         if (slave_descr->exit) {
1039                 result = slave_descr->exit(slave_client->adapter,
1040                                         slave_descr,
1041                                         slave_pdata);
1042                 if (result)
1043                         dev_err(&slave_client->adapter->dev,
1044                                 "Accel exit failed %d\n", result);
1045         }
1046         mldl_cfg->slave[slave_descr->type] = NULL;
1047         mldl_cfg->pdata_slave[slave_descr->type] = NULL;
1048         mpu->slave_modules[slave_descr->type] = NULL;
1049
1050         mutex_unlock(&mpu->mutex);
1051
1052 }
1053 EXPORT_SYMBOL(inv_mpu_unregister_slave);
1054
1055 static unsigned short normal_i2c[] = { I2C_CLIENT_END };
1056
1057 static const struct i2c_device_id mpu_id[] = {
1058         {"mpu3050", 0},
1059         {"mpu6050", 0},
1060         {"mpu6050_no_accel", 0},
1061         {}
1062 };
1063 MODULE_DEVICE_TABLE(i2c, mpu_id);
1064
1065 int mpu_probe(struct i2c_client *client, const struct i2c_device_id *devid)
1066 {
1067         struct mpu_platform_data *pdata;
1068         struct mpu_private_data *mpu;
1069         struct mldl_cfg *mldl_cfg;
1070         int res = 0;
1071         int ii = 0;
1072
1073         dev_info(&client->adapter->dev, "%s: %d\n", __func__, ii++);
1074
1075         if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
1076                 res = -ENODEV;
1077                 goto out_check_functionality_failed;
1078         }
1079
1080         mpu = kzalloc(sizeof(struct mpu_private_data), GFP_KERNEL);
1081         if (!mpu) {
1082                 res = -ENOMEM;
1083                 goto out_alloc_data_failed;
1084         }
1085         mldl_cfg = &mpu->mldl_cfg;
1086         mldl_cfg->mpu_ram       = &mpu->mpu_ram;
1087         mldl_cfg->mpu_gyro_cfg  = &mpu->mpu_gyro_cfg;
1088         mldl_cfg->mpu_offsets   = &mpu->mpu_offsets;
1089         mldl_cfg->mpu_chip_info = &mpu->mpu_chip_info;
1090         mldl_cfg->inv_mpu_cfg   = &mpu->inv_mpu_cfg;
1091         mldl_cfg->inv_mpu_state = &mpu->inv_mpu_state;
1092
1093         mldl_cfg->mpu_ram->length = MPU_MEM_NUM_RAM_BANKS * MPU_MEM_BANK_SIZE;
1094         mldl_cfg->mpu_ram->ram = kzalloc(mldl_cfg->mpu_ram->length, GFP_KERNEL);
1095         if (!mldl_cfg->mpu_ram->ram) {
1096                 res = -ENOMEM;
1097                 goto out_alloc_ram_failed;
1098         }
1099         mpu_private_data = mpu;
1100         i2c_set_clientdata(client, mpu);
1101         this_client = client;
1102         mpu->client = client;
1103
1104         init_waitqueue_head(&mpu->mpu_event_wait);
1105         mutex_init(&mpu->mutex);
1106         init_completion(&mpu->completion);
1107
1108         mpu->response_timeout = 60;     /* Seconds */
1109         mpu->timeout.function = mpu_pm_timeout;
1110         mpu->timeout.data = (u_long) mpu;
1111         init_timer(&mpu->timeout);
1112
1113         mpu->nb.notifier_call = mpu_pm_notifier_callback;
1114         mpu->nb.priority = 0;
1115         res = register_pm_notifier(&mpu->nb);
1116         if (res) {
1117                 dev_err(&client->adapter->dev,
1118                         "Unable to register pm_notifier %d\n", res);
1119                 goto out_register_pm_notifier_failed;
1120         }
1121
1122         pdata = (struct mpu_platform_data *)client->dev.platform_data;
1123         if (!pdata) {
1124                 dev_WARN(&client->adapter->dev,
1125                          "Missing platform data for mpu\n");
1126         }
1127         mldl_cfg->pdata = pdata;
1128
1129         mldl_cfg->mpu_chip_info->addr = client->addr;
1130         res = inv_mpu_open(&mpu->mldl_cfg, client->adapter, NULL, NULL, NULL);
1131
1132         if (res) {
1133                 dev_err(&client->adapter->dev,
1134                         "Unable to open %s %d\n", MPU_NAME, res);
1135                 res = -ENODEV;
1136                 goto out_whoami_failed;
1137         }
1138
1139         mpu->dev.minor = MISC_DYNAMIC_MINOR;
1140         mpu->dev.name = "mpu";
1141         mpu->dev.fops = &mpu_fops;
1142         res = misc_register(&mpu->dev);
1143         if (res < 0) {
1144                 dev_err(&client->adapter->dev,
1145                         "ERROR: misc_register returned %d\n", res);
1146                 goto out_misc_register_failed;
1147         }
1148
1149         if (client->irq) {
1150                 dev_info(&client->adapter->dev,
1151                          "Installing irq using %d\n", client->irq);
1152                 res = mpuirq_init(client, mldl_cfg);
1153                 if (res)
1154                         goto out_mpuirq_failed;
1155         } else {
1156                 dev_WARN(&client->adapter->dev,
1157                          "Missing %s IRQ\n", MPU_NAME);
1158         }
1159         return res;
1160
1161 out_mpuirq_failed:
1162         misc_deregister(&mpu->dev);
1163 out_misc_register_failed:
1164         inv_mpu_close(&mpu->mldl_cfg, client->adapter, NULL, NULL, NULL);
1165 out_whoami_failed:
1166         unregister_pm_notifier(&mpu->nb);
1167 out_register_pm_notifier_failed:
1168         kfree(mldl_cfg->mpu_ram->ram);
1169         mpu_private_data = NULL;
1170 out_alloc_ram_failed:
1171         kfree(mpu);
1172 out_alloc_data_failed:
1173 out_check_functionality_failed:
1174         dev_err(&client->adapter->dev, "%s failed %d\n", __func__, res);
1175         return res;
1176
1177 }
1178
1179 static int mpu_remove(struct i2c_client *client)
1180 {
1181         struct mpu_private_data *mpu = i2c_get_clientdata(client);
1182         struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
1183         struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
1184         struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
1185         int ii;
1186
1187         for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
1188                 if (!pdata_slave[ii])
1189                         slave_adapter[ii] = NULL;
1190                 else
1191                         slave_adapter[ii] =
1192                                 i2c_get_adapter(pdata_slave[ii]->adapt_num);
1193         }
1194
1195         slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
1196         dev_dbg(&client->adapter->dev, "%s\n", __func__);
1197
1198         inv_mpu_close(mldl_cfg,
1199                 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
1200                 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
1201                 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
1202                 slave_adapter[EXT_SLAVE_TYPE_PRESSURE]);
1203
1204
1205         if (client->irq)
1206                 mpuirq_exit();
1207
1208         misc_deregister(&mpu->dev);
1209
1210         unregister_pm_notifier(&mpu->nb);
1211
1212         kfree(mpu->mldl_cfg.mpu_ram->ram);
1213         kfree(mpu);
1214
1215         return 0;
1216 }
1217
1218 static struct i2c_driver mpu_driver = {
1219         .class = I2C_CLASS_HWMON,
1220         .probe = mpu_probe,
1221         .remove = mpu_remove,
1222         .id_table = mpu_id,
1223         .driver = {
1224                    .owner = THIS_MODULE,
1225                    .name = MPU_NAME,
1226                    },
1227         .address_list = normal_i2c,
1228         .shutdown = mpu_shutdown,       /* optional */
1229         .suspend = mpu_dev_suspend,     /* optional */
1230         .resume = mpu_dev_resume,       /* optional */
1231
1232 };
1233
1234 static int __init mpu_init(void)
1235 {
1236         int res = i2c_add_driver(&mpu_driver);
1237         pr_info("%s: Probe name %s\n", __func__, MPU_NAME);
1238         if (res)
1239                 pr_err("%s failed\n", __func__);
1240         return res;
1241 }
1242
1243 static void __exit mpu_exit(void)
1244 {
1245         pr_info("%s\n", __func__);
1246         i2c_del_driver(&mpu_driver);
1247 }
1248
1249 module_init(mpu_init);
1250 module_exit(mpu_exit);
1251
1252 MODULE_AUTHOR("Invensense Corporation");
1253 MODULE_DESCRIPTION("User space character device interface for MPU");
1254 MODULE_LICENSE("GPL");
1255 MODULE_ALIAS(MPU_NAME);