drivers/input/gyroscope/l3g4200d.c

   1 /* drivers/i2c/chips/l3g4200d.c - l3g4200d compass driver
   2  *
   3  * Copyright (C) 2007-2008 HTC Corporation.
   4  * Author: Hou-Kun Chen <houkun.chen@gmail.com>
   5  *
   6  * This software is licensed under the terms of the GNU General Public
   7  * License version 2, as published by the Free Software Foundation, and
   8  * may be copied, distributed, and modified under those terms.
   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  */
  16
  17 #include <linux/interrupt.h>
  18 #include <linux/i2c.h>
  19 #include <linux/slab.h>
  20 #include <linux/irq.h>
  21 #include <linux/miscdevice.h>
  22 #include <linux/gpio.h>
  23 #include <asm/uaccess.h>
  24 #include <linux/delay.h>
  25 #include <linux/input.h>
  26 #include <linux/workqueue.h>
  27 #include <linux/freezer.h>
  28 #include <linux/l3g4200d.h>
  29 #include <mach/gpio.h>
  30 #include <mach/board.h>
  31 #ifdef CONFIG_HAS_EARLYSUSPEND
  32 #include <linux/earlysuspend.h>
  33 #endif
  34
  35 #if 0
  36 #define DBG(x...) printk(x)
  37 #else
  38 #define DBG(x...)
  39 #endif
  40 static int  l3g4200d_probe(struct i2c_client *client, const struct i2c_device_id *id);
  41
  42 #define L3G4200D_SPEED          200 * 1000
  43 #define L3G4200D_DEVID          0xD3
  44
  45 #define L3G4200D_MAJOR   102
  46 #define L3G4200D_MINOR   4
  47
  48 /* l3g4200d gyroscope registers */
  49 #define WHO_AM_I    0x0F
  50
  51 #define CTRL_REG1       0x20    /* power control reg */
  52 #define CTRL_REG2       0x21    /* power control reg */
  53 #define CTRL_REG3       0x22    /* power control reg */
  54 #define CTRL_REG4       0x23    /* interrupt control reg */
  55 #define CTRL_REG5       0x24    /* interrupt control reg */
  56 #define AXISDATA_REG    0x28
  57
  58
  59 /* Addresses to scan -- protected by sense_data_mutex */
  60 //static char sense_data[RBUFF_SIZE + 1];
  61 static struct i2c_client *this_client;
  62 static struct l3g4200d_data *this_data;
  63 static struct miscdevice l3g4200d_device;
  64
  65 static DECLARE_WAIT_QUEUE_HEAD(data_ready_wq);
  66
  67 #ifdef CONFIG_HAS_EARLYSUSPEND
  68 static struct early_suspend l3g4200d_early_suspend;
  69 #endif
  70 static int revision = -1;
  71 /* AKM HW info */
  72 static ssize_t gsensor_vendor_show(struct device *dev,
  73                 struct device_attribute *attr, char *buf)
  74 {
  75         ssize_t ret = 0;
  76
  77         sprintf(buf, "%#x\n", revision);
  78         ret = strlen(buf) + 1;
  79
  80         return ret;
  81 }
  82
  83 static DEVICE_ATTR(vendor, 0444, gsensor_vendor_show, NULL);
  84
  85 static struct kobject *android_gsensor_kobj;
  86
  87 static int gsensor_sysfs_init(void)
  88 {
  89         int ret ;
  90
  91         android_gsensor_kobj = kobject_create_and_add("android_gyrosensor", NULL);
  92         if (android_gsensor_kobj == NULL) {
  93                 printk(KERN_ERR
  94                        "L3G4200D gsensor_sysfs_init:"\
  95                        "subsystem_register failed\n");
  96                 ret = -ENOMEM;
  97                 goto err;
  98         }
  99
 100         ret = sysfs_create_file(android_gsensor_kobj, &dev_attr_vendor.attr);
 101         if (ret) {
 102                 printk(KERN_ERR
 103                        "L3G4200D gsensor_sysfs_init:"\
 104                        "sysfs_create_group failed\n");
 105                 goto err4;
 106         }
 107
 108         return 0 ;
 109 err4:
 110         kobject_del(android_gsensor_kobj);
 111 err:
 112         return ret ;
 113 }
 114
 115 #if 0
 116 static int l3g4200d_rx_data(struct i2c_client *client, char *rxData, int length)
 117 {
 118         int ret = 0;
 119         char reg = rxData[0];
 120         ret = i2c_master_reg8_recv(client, reg, rxData, length, L3G4200D_SPEED);
 121         return (ret > 0)? 0 : ret;
 122 }
 123 #else
 124 static int l3g4200d_rx_data(struct i2c_client *client, char *rxData, int length)
 125 {
 126         int ret = 0;
 127         char reg = rxData[0];
 128         int i = 0;
 129         for(i=0; i<3; i++)
 130         {
 131                 ret = i2c_master_reg8_recv(client, reg, rxData, length, L3G4200D_SPEED);
 132                 if(ret < 0)
 133                 mdelay(1);
 134                 else
 135                 break;
 136         }
 137         return (ret > 0)? 0 : ret;
 138 }
 139 #endif
 140 #if 0
 141 static int l3g4200d_tx_data(struct i2c_client *client, char *txData, int length)
 142 {
 143         int ret = 0;
 144         char reg = txData[0];
 145         ret = i2c_master_reg8_send(client, reg, &txData[1], length-1, L3G4200D_SPEED);
 146         return (ret > 0)? 0 : ret;
 147 }
 148 #else
 149 static int l3g4200d_tx_data(struct i2c_client *client, char *txData, int length)
 150 {
 151         int ret = 0;
 152         char reg = txData[0];
 153         int i = 0;
 154         for(i=0; i<3; i++)
 155         {
 156                 ret = i2c_master_reg8_send(client, reg, &txData[1], length-1, L3G4200D_SPEED);
 157                 if(ret < 0)
 158                 mdelay(1);
 159                 else
 160                 break;
 161         }
 162         return (ret > 0)? 0 : ret;
 163 }
 164 #endif
 165
 166 #if 0
 167 static int gyro_rx_data(struct i2c_client *client, char *rxData, int length)
 168 {
 169         int ret = 0;
 170         int i = 0;
 171         char reg = rxData[0];
 172         for(i=0; i<3; i++)
 173         {
 174                 ret = i2c_master_reg8_recv(client, reg, rxData, length, L3G4200D_SPEED);
 175                 if(ret < 0)
 176                 mdelay(1);
 177                 else
 178                 break;
 179         }
 180         return (ret > 0)? 0 : ret;
 181 }
 182 #endif
 183
 184 static int gyro_tx_data(struct i2c_client *client, char *txData, int length)
 185 {
 186         int ret = 0;
 187         char reg = txData[0];
 188         int i = 0;
 189
 190         for(i=0; i<3; i++)
 191         {
 192                 ret = i2c_master_reg8_send(client, reg, &txData[1], length-1, L3G4200D_SPEED);
 193                 if(ret < 0)
 194                 mdelay(1);
 195                 else
 196                 break;
 197         }
 198         return (ret > 0)? 0 : ret;
 199 }
 200
 201 #if 0
 202 /*  i2c read routine for l3g4200d digital gyroscope */
 203 static char l3g4200d_i2c_read(unsigned char reg_addr,
 204                                    unsigned char *data,
 205                                    unsigned char len)
 206 {
 207         char tmp;
 208         int ret = 0;
 209         if (this_client == NULL)  /*  No global client pointer? */
 210                 return -1;
 211
 212         data[0]=reg_addr;
 213         ret = gyro_rx_data(this_client, data, len);
 214         return tmp;
 215 }
 216 #endif
 217 /*  i2c write routine for l3g4200d digital gyroscope */
 218 static char l3g4200d_i2c_write(unsigned char reg_addr,
 219                                     unsigned char *data,
 220                                     unsigned char len)
 221 {
 222
 223         char buffer[3];
 224         int ret = 0;
 225         int i = 0;
 226         if (this_client == NULL)  /*  No global client pointer? */
 227                 return -1;
 228         for (i = 0; i < len; i++)
 229                 {
 230                 buffer[0] = reg_addr+i;
 231                 buffer[1] = data[i];
 232                 ret = gyro_tx_data(this_client, &buffer[0], 2);
 233                 }
 234         return ret;
 235
 236 }
 237
 238 static char l3g4200d_read_reg(struct i2c_client *client,int addr)
 239 {
 240         char tmp;
 241         int ret = 0;
 242
 243         tmp = addr;
 244 //      ret = l3g4200d_tx_data(client, &tmp, 1);
 245         ret = l3g4200d_rx_data(client, &tmp, 1);
 246         return tmp;
 247 }
 248
 249 static int l3g4200d_write_reg(struct i2c_client *client,int addr,int value)
 250 {
 251         char buffer[3];
 252         int ret = 0;
 253
 254         buffer[0] = addr;
 255         buffer[1] = value;
 256         ret = l3g4200d_tx_data(client, &buffer[0], 2);
 257         return ret;
 258 }
 259
 260
 261 static char l3g4200d_get_devid(struct i2c_client *client)
 262 {
 263         unsigned int devid = 0;
 264         struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(client);
 265
 266         devid = l3g4200d_read_reg(client, WHO_AM_I)&0xff;
 267         if (devid == GYRO_DEVID_L3G4200D) {
 268                 l3g4200d->devid = devid;
 269                 printk(KERN_INFO "gyro is L3G4200D and devid=0x%x\n",devid);
 270                 return 0;
 271         } else if (devid == GYRO_DEVID_L3G20D)
 272         {
 273                 l3g4200d->devid = devid;
 274                 printk(KERN_INFO "gyro is L3G20D and devid=0x%x\n",devid);
 275                 return 0;
 276         }
 277         else
 278         {
 279                 printk(KERN_ERR "%s:gyro device id is error,devid=%d\n",__func__,devid);
 280                 return -1;
 281         }
 282 }
 283
 284 static int l3g4200d_active(struct i2c_client *client,int enable)
 285 {
 286         int tmp;
 287         int ret = 0;
 288
 289         tmp = l3g4200d_read_reg(client,CTRL_REG1);
 290         if(enable)
 291                 tmp |=ACTIVE_MASK;
 292         else
 293                 tmp &=~ACTIVE_MASK;
 294         DBG("l3g4200d_active %s (0x%x)\n",enable?"active":"standby",tmp);
 295         ret = l3g4200d_write_reg(client,CTRL_REG1,tmp);
 296         return ret;
 297 }
 298
 299
 300
 301 static int device_init(void)
 302 {
 303         int res;
 304         unsigned char buf[5];
 305         buf[0] = 0x07;  //27
 306         buf[1] = 0x00;
 307         buf[2] = 0x00;
 308         buf[3] = 0x20;  //0x00
 309         buf[4] = 0x00;
 310         res = l3g4200d_i2c_write(CTRL_REG1, &buf[0], 5);
 311         return res;
 312 }
 313
 314 int l3g4200d_set_bandwidth(char bw)
 315 {
 316         int res = 0;
 317         unsigned char data;
 318
 319         res = l3g4200d_read_reg(this_client, CTRL_REG1);
 320         if (res >= 0)
 321                 data = res & 0x000F;
 322
 323         data = data + bw;
 324         res = l3g4200d_i2c_write(CTRL_REG1, &data, 1);
 325         return res;
 326 }
 327
 328 static int l3g4200d_start_dev(struct i2c_client *client, char rate)
 329 {
 330         int ret = 0;
 331         //int tmp;
 332         //struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(client);
 333
 334         /* standby */
 335         l3g4200d_active(client,0);
 336         device_init();
 337         l3g4200d_set_bandwidth(rate);
 338         l3g4200d_active(client,1);
 339
 340         enable_irq(client->irq);
 341         return ret;
 342
 343 }
 344
 345 static int l3g4200d_start(struct i2c_client *client, char rate)
 346 {
 347     struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(client);
 348
 349     DBG("%s::enter\n",__FUNCTION__);
 350     if (l3g4200d->status == L3G4200D_OPEN) {
 351         return 0;
 352     }
 353     l3g4200d->status = L3G4200D_OPEN;
 354     return l3g4200d_start_dev(client, rate);
 355 }
 356
 357 static int l3g4200d_close_dev(struct i2c_client *client)
 358 {
 359         DBG("%s :enter\n",__FUNCTION__);
 360         disable_irq_nosync(client->irq);
 361         return l3g4200d_active(client,0);
 362 }
 363
 364 static int l3g4200d_close(struct i2c_client *client)
 365 {
 366         struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(client);
 367         DBG("%s::enter\n",__FUNCTION__);
 368         l3g4200d->status = L3G4200D_CLOSE;
 369
 370         return l3g4200d_close_dev(client);
 371 }
 372
 373 static int l3g4200d_reset_rate(struct i2c_client *client, char rate)
 374 {
 375         int ret = 0;
 376
 377         DBG("%s\n",__func__);
 378
 379  //   ret = l3g4200d_close_dev(client);
 380     ret = l3g4200d_start_dev(client, rate);
 381
 382         return ret ;
 383 }
 384
 385 static inline int l3g4200d_convert_to_int(char value)
 386 {
 387     int result;
 388
 389     if (value < 1) {
 390        result = value * 1;
 391     } else {
 392        result = ~(((~value & 0x7f) + 1)* 1) + 1;
 393     }
 394
 395     return result;
 396 }
 397
 398 static void l3g4200d_report_value(struct i2c_client *client, struct l3g4200d_axis *axis)
 399 {
 400         struct l3g4200d_data *l3g4200d = i2c_get_clientdata(client);
 401         //struct l3g4200d_axis *axis = (struct l3g4200d_axis *)rbuf;
 402
 403         /* Report acceleration sensor information */
 404         input_report_rel(l3g4200d->input_dev, ABS_RX, axis->x);
 405         input_report_rel(l3g4200d->input_dev, ABS_RY, axis->y);
 406         input_report_rel(l3g4200d->input_dev, ABS_RZ, axis->z);
 407         input_sync(l3g4200d->input_dev);
 408         DBG("%s:x==%d  y==%d z==%d\n",__func__,axis->x,axis->y,axis->z);
 409 }
 410
 411
 412 static int l3g4200d_get_data(struct i2c_client *client)
 413 {
 414         //char buffer[6];
 415         int ret,i;
 416         struct l3g4200d_axis axis;
 417         struct l3g4200d_platform_data *pdata = client->dev.platform_data;
 418         struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(client);
 419         //int res;
 420         unsigned char gyro_data[6];
 421         /* x,y,z hardware data */
 422         int x = 0, y = 0, z = 0;
 423
 424         for(i=0;i<6;i++)
 425         {
 426                 gyro_data[i] = AXISDATA_REG+i;
 427                 //ret = l3g4200d_tx_data(client, &buffer[0], 1);
 428                 ret = l3g4200d_rx_data(client, &gyro_data[i], 1);
 429         }
 430
 431         x = (short) (((gyro_data[1]) << 8) | gyro_data[0]);
 432         y = (short) (((gyro_data[3]) << 8) | gyro_data[2]);
 433         z = (short) (((gyro_data[5]) << 8) | gyro_data[4]);
 434
 435         DBG("%s: x=%d  y=%d z=%d \n",__func__, x,y,z);
 436         if(pdata && pdata->orientation)
 437         {
 438                 axis.x = (pdata->orientation[0])*x + (pdata->orientation[1])*y + (pdata->orientation[2])*z;
 439                 axis.y = (pdata->orientation[3])*x + (pdata->orientation[4])*y + (pdata->orientation[5])*z;
 440                 axis.z = (pdata->orientation[6])*x + (pdata->orientation[7])*y + (pdata->orientation[8])*z;
 441         }
 442         else
 443         {
 444                 axis.x = x;
 445                 axis.y = y;
 446                 axis.z = z;
 447         }
 448
 449         //filter gyro data
 450         if((abs(l3g4200d->axis.x - axis.x) > pdata->x_min)||(abs(l3g4200d->axis.y - axis.y) > pdata->y_min)||(abs(l3g4200d->axis.z - axis.z) > pdata->z_min))
 451         {
 452                 l3g4200d->axis.x = axis.x;
 453                 l3g4200d->axis.y = axis.y;
 454                 l3g4200d->axis.z = axis.z;
 455                 if(abs(l3g4200d->axis.x) <= pdata->x_min) l3g4200d->axis.x = 0;
 456                 if(abs(l3g4200d->axis.y) <= pdata->y_min) l3g4200d->axis.y = 0;
 457                 if(abs(l3g4200d->axis.z) <= pdata->z_min) l3g4200d->axis.z = 0;
 458
 459                 l3g4200d_report_value(client, &l3g4200d->axis);
 460         }
 461
 462         return 0;
 463 }
 464
 465 /*
 466 static int l3g4200d_trans_buff(char *rbuf, int size)
 467 {
 468         //wait_event_interruptible_timeout(data_ready_wq,
 469         //                               atomic_read(&data_ready), 1000);
 470         wait_event_interruptible(data_ready_wq,
 471                                          atomic_read(&data_ready));
 472
 473         atomic_set(&data_ready, 0);
 474         memcpy(rbuf, &sense_data[0], size);
 475
 476         return 0;
 477 }
 478 */
 479
 480 static int l3g4200d_open(struct inode *inode, struct file *file)
 481 {
 482         DBG("%s :enter\n",__FUNCTION__);
 483         return 0;//nonseekable_open(inode, file);
 484 }
 485
 486 static int l3g4200d_release(struct inode *inode, struct file *file)
 487 {
 488         DBG("%s :enter\n",__FUNCTION__);
 489         return 0;
 490 }
 491 #define RBUFF_SIZE              12      /* Rx buffer size */
 492
 493 static long l3g4200d_ioctl( struct file *file, unsigned int cmd,unsigned long arg)
 494 {
 495
 496         struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(this_client);
 497         void __user *argp = (void __user *)arg;
 498         //char msg[RBUFF_SIZE + 1];
 499         int ret = -1;
 500         char rate;
 501         struct i2c_client *client = container_of(l3g4200d_device.parent, struct i2c_client, dev);
 502
 503         switch (cmd) {
 504         case L3G4200D_IOCTL_GET_ENABLE:
 505                 DBG("%s :L3G4200D_IOCTL_GET_ENABLE\n",__FUNCTION__);
 506                 ret=!l3g4200d->status;
 507                 if (copy_to_user(argp, &ret, sizeof(ret)))
 508                 {
 509             printk("%s:failed to copy status to user space.\n",__FUNCTION__);
 510                         return -EFAULT;
 511                 }
 512                 break;
 513         case L3G4200D_IOCTL_SET_ENABLE:
 514                 DBG("%s :L3G4200D_IOCTL_SET_ENABLE,flag=%d\n",__FUNCTION__,*(unsigned int *)argp);
 515                 if(*(unsigned int *)argp)
 516                 {
 517                         ret = l3g4200d_start(client, ODR100_BW12_5);
 518                         if (ret < 0)
 519                         return ret;
 520                 }
 521                 else
 522                 {
 523                         ret = l3g4200d_close(client);
 524                         if (ret < 0)
 525                         return ret;
 526                 }
 527                 ret=l3g4200d->status;
 528                 if (copy_to_user(argp, &ret, sizeof(ret)))
 529                 {
 530             printk("%s:failed to copy sense data to user space.\n",__FUNCTION__);
 531                         return -EFAULT;
 532                 }
 533                 break;
 534         case L3G4200D_IOCTL_SET_DELAY:
 535                 DBG("%s :L3G4200D_IOCTL_SET_DELAY,rate=%d\n",__FUNCTION__,rate);
 536                 if (copy_from_user(&rate, argp, sizeof(rate)))
 537                 return -EFAULT;
 538                 ret = l3g4200d_reset_rate(client, 0x00);//rate<<4);//0x20
 539                 if (ret < 0)
 540                         return ret;
 541                 break;
 542 /*
 543         case ECS_IOCTL_GETDATA:
 544                 ret = l3g4200d_trans_buff(msg, RBUFF_SIZE);
 545                 if (ret < 0)
 546                         return ret;
 547                 if (copy_to_user(argp, &msg, sizeof(msg)))
 548                         return -EFAULT;
 549                 break;
 550 */
 551         default:
 552                 printk("%s:error,cmd=0x%x\n",__func__,cmd);
 553                 return -ENOTTY;
 554         }
 555
 556         DBG("%s:line=%d,cmd=0x%x\n",__func__,__LINE__,cmd);
 557         return 0;
 558 }
 559
 560 static void l3g4200d_work_func(struct work_struct *work)
 561 {
 562         struct l3g4200d_data *l3g4200d = container_of(work, struct l3g4200d_data, work);
 563         struct i2c_client *client = l3g4200d->client;
 564
 565         if (l3g4200d_get_data(client) < 0)
 566                 DBG(KERN_ERR "L3G4200D mma_work_func: Get data failed\n");
 567
 568         enable_irq(client->irq);
 569 }
 570
 571 static void  l3g4200d_delaywork_func(struct work_struct *work)
 572 {
 573         struct delayed_work *delaywork = container_of(work, struct delayed_work, work);
 574         struct l3g4200d_data *l3g4200d = container_of(delaywork, struct l3g4200d_data, delaywork);
 575         struct i2c_client *client = l3g4200d->client;
 576
 577         if (l3g4200d_get_data(client) < 0)
 578                 DBG(KERN_ERR "L3G4200D mma_work_func: Get data failed\n");
 579         enable_irq(client->irq);
 580 }
 581
 582 static irqreturn_t l3g4200d_interrupt(int irq, void *dev_id)
 583 {
 584         struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)dev_id;
 585
 586         disable_irq_nosync(irq);
 587         schedule_delayed_work(&l3g4200d->delaywork, msecs_to_jiffies(10));
 588         DBG("%s :enter\n",__FUNCTION__);
 589         return IRQ_HANDLED;
 590 }
 591
 592 static struct file_operations l3g4200d_fops = {
 593         .owner = THIS_MODULE,
 594         .open = l3g4200d_open,
 595         .release = l3g4200d_release,
 596         .unlocked_ioctl = l3g4200d_ioctl,
 597 };
 598
 599 static struct miscdevice l3g4200d_device = {
 600         .minor = MISC_DYNAMIC_MINOR,
 601         .name = "gyrosensor",//"l3g4200d_daemon",
 602         .fops = &l3g4200d_fops,
 603 };
 604
 605 static int l3g4200d_remove(struct i2c_client *client)
 606 {
 607         struct l3g4200d_data *l3g4200d = i2c_get_clientdata(client);
 608
 609     misc_deregister(&l3g4200d_device);
 610     input_unregister_device(l3g4200d->input_dev);
 611     input_free_device(l3g4200d->input_dev);
 612     free_irq(client->irq, l3g4200d);
 613     kfree(l3g4200d);
 614 #ifdef CONFIG_HAS_EARLYSUSPEND
 615     unregister_early_suspend(&l3g4200d_early_suspend);
 616 #endif
 617     this_client = NULL;
 618         return 0;
 619 }
 620
 621 #ifdef CONFIG_HAS_EARLYSUSPEND
 622 static void l3g4200d_suspend(struct early_suspend *h)
 623 {
 624         struct i2c_client *client = container_of(l3g4200d_device.parent, struct i2c_client, dev);
 625         struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(client);
 626         if(l3g4200d->status == L3G4200D_OPEN)
 627         {
 628                 l3g4200d_close_dev(client);
 629         }
 630
 631         DBG("%s:%d\n",__func__,l3g4200d->status);
 632 }
 633
 634 static void l3g4200d_resume(struct early_suspend *h)
 635 {
 636         struct i2c_client *client = container_of(l3g4200d_device.parent, struct i2c_client, dev);
 637         struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(client);
 638         if (l3g4200d->status == L3G4200D_OPEN)
 639                 l3g4200d_start_dev(client,l3g4200d->curr_tate);
 640
 641         DBG("%s:%d\n",__func__,l3g4200d->status);
 642 }
 643 #else
 644 static int l3g4200d_suspend(struct i2c_client *client, pm_message_t mesg)
 645 {
 646         int ret=0;
 647         //DBG("Gsensor mma7760 enter 2 level  suspend l3g4200d->status %d\n",l3g4200d->status);
 648         //struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(client);
 649         //if(l3g4200d->status == L3G4200D_OPEN)
 650         //{
 651         //l3g4200d->status = L3G4200D_SUSPEND;
 652         //ret = l3g4200d_close_dev(client);
 653         //}
 654         return ret;
 655 }
 656 static int l3g4200d_resume(struct i2c_client *client)
 657 {
 658         int ret=0;
 659         //struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(client);
 660         //DBG("Gsensor mma7760 2 level resume!! l3g4200d->status %d\n",l3g4200d->status);
 661         //if((l3g4200d->status == L3G4200D_SUSPEND) && (l3g4200d->status != L3G4200D_OPEN))
 662         //if (l3g4200d->status == L3G4200D_OPEN)
 663         //ret = l3g4200d_start_dev(client, l3g4200d->curr_tate);
 664         return ret;
 665 }
 666 #endif
 667
 668 static const struct i2c_device_id l3g4200d_id[] = {
 669                 {"l3g4200d_gryo", 0},
 670                 { }
 671 };
 672
 673 static struct i2c_driver l3g4200d_driver = {
 674         .driver = {
 675                 .name = "l3g4200d_gryo",
 676             },
 677         .id_table       = l3g4200d_id,
 678         .probe          = l3g4200d_probe,
 679         .remove         = __devexit_p(l3g4200d_remove),
 680 #ifndef CONFIG_HAS_EARLYSUSPEND
 681         .suspend = &l3g4200d_suspend,
 682         .resume = &l3g4200d_resume,
 683 #endif
 684 };
 685
 686
 687 static int l3g4200d_init_irq(struct i2c_client *client)
 688 {
 689         struct l3g4200d_data *l3g4200d;
 690         int ret;
 691         l3g4200d = i2c_get_clientdata(client);
 692         DBG("gpio_to_irq(%d) is %d\n",client->irq,gpio_to_irq(client->irq));
 693         if ( !gpio_is_valid(client->irq)) {
 694                 DBG("+++++++++++gpio_is_invalid\n");
 695                 return -EINVAL;
 696         }
 697         ret = gpio_request(client->irq, "l3g4200d_int");
 698         if (ret) {
 699                 DBG( "failed to request mma7990_trig GPIO%d\n",gpio_to_irq(client->irq));
 700                 return ret;
 701         }
 702         ret = gpio_direction_input(client->irq);
 703         if (ret) {
 704         DBG("failed to set mma7990_trig GPIO gpio input\n");
 705                 return ret;
 706         }
 707         gpio_pull_updown(client->irq, GPIOPullUp);
 708         client->irq = gpio_to_irq(client->irq);
 709         ret = request_irq(client->irq, l3g4200d_interrupt, IRQF_TRIGGER_LOW, client->dev.driver->name, l3g4200d);
 710         DBG("request irq is %d,ret is  0x%x\n",client->irq,ret);
 711         if (ret ) {
 712                 DBG(KERN_ERR "l3g4200d_init_irq: request irq failed,ret is %d\n",ret);
 713         return ret;
 714         }
 715         disable_irq(client->irq);
 716         init_waitqueue_head(&data_ready_wq);
 717
 718         return 0;
 719 }
 720
 721
 722 static int l3g4200d_validate_pdata(struct l3g4200d_data *gyro)
 723 {
 724         if (gyro->pdata->axis_map_x > 2 ||
 725             gyro->pdata->axis_map_y > 2 ||
 726             gyro->pdata->axis_map_z > 2) {
 727                 dev_err(&gyro->client->dev,
 728                         "invalid axis_map value x:%u y:%u z%u\n",
 729                         gyro->pdata->axis_map_x, gyro->pdata->axis_map_y,
 730                         gyro->pdata->axis_map_z);
 731                 return -EINVAL;
 732         }
 733
 734         /* Only allow 0 and 1 for negation boolean flag */
 735         if (gyro->pdata->negate_x > 1 ||
 736             gyro->pdata->negate_y > 1 ||
 737             gyro->pdata->negate_z > 1) {
 738                 dev_err(&gyro->client->dev,
 739                         "invalid negate value x:%u y:%u z:%u\n",
 740                         gyro->pdata->negate_x, gyro->pdata->negate_y,
 741                         gyro->pdata->negate_z);
 742                 return -EINVAL;
 743         }
 744
 745         return 0;
 746 }
 747
 748
 749
 750
 751 static int  l3g4200d_probe(struct i2c_client *client, const struct i2c_device_id *id)
 752 {
 753         struct l3g4200d_data *l3g4200d;
 754         struct l3g4200d_platform_data *pdata = pdata = client->dev.platform_data;
 755         int i,err;
 756
 757         if(pdata && pdata->init)
 758                 pdata->init();
 759
 760         l3g4200d = kzalloc(sizeof(struct l3g4200d_data), GFP_KERNEL);
 761         if (!l3g4200d) {
 762                 DBG("[l3g4200d]:alloc data failed.\n");
 763                 err = -ENOMEM;
 764                 goto exit_alloc_data_failed;
 765         }
 766
 767         INIT_WORK(&l3g4200d->work, l3g4200d_work_func);
 768         INIT_DELAYED_WORK(&l3g4200d->delaywork, l3g4200d_delaywork_func);
 769
 770         l3g4200d->client = client;
 771         i2c_set_clientdata(client, l3g4200d);
 772
 773         this_client = client;
 774
 775         l3g4200d->pdata = kmalloc(sizeof(*l3g4200d->pdata), GFP_KERNEL);
 776
 777         if (l3g4200d->pdata == NULL)
 778                 goto exit_kfree;
 779
 780         memcpy(l3g4200d->pdata, client->dev.platform_data, sizeof(*l3g4200d->pdata));
 781
 782         err = l3g4200d_validate_pdata(l3g4200d);
 783         if (err < 0) {
 784                 dev_err(&client->dev, "failed to validate platform data\n");
 785                 goto exit_kfree_pdata;
 786         }
 787         this_data=l3g4200d;
 788
 789         //try three times
 790         for(i=0; i<3; i++)
 791         {
 792                 err = l3g4200d_get_devid(client);
 793                 if (!err)
 794                 break;
 795         }
 796         if(err)
 797         {
 798                 printk("%s:fail\n",__func__);
 799                 goto exit_kfree_pdata;
 800         }
 801
 802
 803         err = l3g4200d_init_irq(client);
 804         if (err < 0) {
 805                 DBG(KERN_ERR
 806                        "l3g4200d_probe: l3g4200d_init_irq failed\n");
 807                 goto exit_request_gpio_irq_failed;
 808         }
 809
 810         l3g4200d->input_dev = input_allocate_device();
 811         if (!l3g4200d->input_dev) {
 812                 err = -ENOMEM;
 813                 DBG(KERN_ERR
 814                        "l3g4200d_probe: Failed to allocate input device\n");
 815                 goto exit_input_allocate_device_failed;
 816         }
 817
 818         //set_bit(EV_ABS, l3g4200d->input_dev->evbit);
 819
 820         /* x-axis acceleration */
 821         input_set_capability(l3g4200d->input_dev, EV_REL, REL_RX);
 822         input_set_abs_params(l3g4200d->input_dev, ABS_RX, -32768, 32768, 0, 0); //2g full scale range
 823         /* y-axis acceleration */
 824         input_set_capability(l3g4200d->input_dev, EV_REL, REL_RY);
 825         input_set_abs_params(l3g4200d->input_dev, ABS_RY, -32768, 32768, 0, 0); //2g full scale range
 826         /* z-axis acceleration */
 827         input_set_capability(l3g4200d->input_dev, EV_REL, REL_RZ);
 828         input_set_abs_params(l3g4200d->input_dev, ABS_RZ, -32768, 32768, 0, 0); //2g full scale range
 829
 830         l3g4200d->input_dev->name = "gyro";
 831         l3g4200d->input_dev->dev.parent = &client->dev;
 832         l3g4200d->axis.x = 0;
 833         l3g4200d->axis.y = 0;
 834         l3g4200d->axis.z = 0;
 835
 836         err = input_register_device(l3g4200d->input_dev);
 837         if (err < 0) {
 838                 DBG(KERN_ERR
 839                        "l3g4200d_probe: Unable to register input device: %s\n",
 840                        l3g4200d->input_dev->name);
 841                 goto exit_input_register_device_failed;
 842         }
 843
 844         l3g4200d_device.parent = &client->dev;
 845         err = misc_register(&l3g4200d_device);
 846         if (err < 0) {
 847                 DBG(KERN_ERR
 848                        "l3g4200d_probe: mmad_device register failed\n");
 849                 goto exit_misc_device_register_l3g4200d_device_failed;
 850         }
 851
 852         err = gsensor_sysfs_init();
 853         if (err < 0) {
 854                 DBG(KERN_ERR
 855             "l3g4200d_probe: gsensor sysfs init failed\n");
 856                 goto exit_gsensor_sysfs_init_failed;
 857         }
 858
 859 #ifdef CONFIG_HAS_EARLYSUSPEND
 860         l3g4200d_early_suspend.suspend = l3g4200d_suspend;
 861         l3g4200d_early_suspend.resume = l3g4200d_resume;
 862         l3g4200d_early_suspend.level = 0x2;
 863         register_early_suspend(&l3g4200d_early_suspend);
 864 #endif
 865
 866         l3g4200d->status = L3G4200D_CLOSE;
 867 #if  0
 868 //      l3g4200d_start_test(this_client);
 869         l3g4200d_start(client, L3G4200D_RATE_12P5);
 870 #endif
 871
 872         return 0;
 873
 874 exit_gsensor_sysfs_init_failed:
 875     misc_deregister(&l3g4200d_device);
 876 exit_misc_device_register_l3g4200d_device_failed:
 877     input_unregister_device(l3g4200d->input_dev);
 878 exit_input_register_device_failed:
 879         input_free_device(l3g4200d->input_dev);
 880 exit_input_allocate_device_failed:
 881     free_irq(client->irq, l3g4200d);
 882 exit_kfree_pdata:
 883 exit_request_gpio_irq_failed:
 884         kfree(l3g4200d->pdata);
 885 exit_kfree:
 886         kfree(l3g4200d);
 887 exit_alloc_data_failed:
 888         DBG("%s error\n",__FUNCTION__);
 889         return err;
 890 }
 891
 892
 893 static int __init l3g4200d_i2c_init(void)
 894 {
 895         return i2c_add_driver(&l3g4200d_driver);
 896 }
 897
 898 static void __exit l3g4200d_i2c_exit(void)
 899 {
 900         i2c_del_driver(&l3g4200d_driver);
 901 }
 902
 903 module_init(l3g4200d_i2c_init);
 904 module_exit(l3g4200d_i2c_exit);
 905
 906