1 /* MPU6050 motion sensor driver
3 * This software is licensed under the terms of the GNU General Public
4 * License version 2, as published by the Free Software Foundation, and
5 * may be copied, distributed, and modified under those terms.
7 * This program is distributed in the hope that it will be useful,
8 * but WITHOUT ANY WARRANTY; without even the implied warranty of
9 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 * GNU General Public License for more details.
14 #include <linux/interrupt.h>
15 #include <linux/i2c.h>
16 #include <linux/slab.h>
17 #include <linux/irq.h>
18 #include <linux/miscdevice.h>
19 #include <asm/uaccess.h>
20 #include <linux/delay.h>
21 #include <linux/input.h>
22 #include <linux/workqueue.h>
23 #include <linux/kobject.h>
24 #include <linux/earlysuspend.h>
25 #include <linux/platform_device.h>
27 #include <cust_gyro.h>
28 #include <linux/hwmsensor.h>
29 #include <linux/hwmsen_dev.h>
30 #include <linux/sensors_io.h>
32 #include <linux/hwmsen_helper.h>
33 #include <linux/kernel.h>
35 #include <mach/mt_typedefs.h>
36 #include <mach/mt_gpio.h>
37 #include <mach/mt_pm_ldo.h>
38 #include <mach/mt_boot.h>
40 #define INV_GYRO_AUTO_CALI 1
42 #define POWER_NONE_MACRO MT65XX_POWER_NONE
43 /*----------------------------------------------------------------------------*/
44 #define MPU6050_DEFAULT_FS MPU6050_FS_1000
45 #define MPU6050_DEFAULT_LSB MPU6050_FS_1000_LSB
46 /*---------------------------------------------------------------------------*/
48 /*----------------------------------------------------------------------------*/
49 #define CONFIG_MPU6050_LOWPASS /*apply low pass filter on output*/
50 /*----------------------------------------------------------------------------*/
51 #define MPU6050_AXIS_X 0
52 #define MPU6050_AXIS_Y 1
53 #define MPU6050_AXIS_Z 2
54 #define MPU6050_AXES_NUM 3
55 #define MPU6050_DATA_LEN 6
56 #define MPU6050_DEV_NAME "MPU6050GY" /* name must different with gsensor mpu6050 */
57 /*----------------------------------------------------------------------------*/
58 static const struct i2c_device_id mpu6050_i2c_id
[] = {{MPU6050_DEV_NAME
,0},{}};
59 static struct i2c_board_info __initdata i2c_mpu6050
={ I2C_BOARD_INFO(MPU6050_DEV_NAME
, (MPU6050_I2C_SLAVE_ADDR
>>1))};
61 int packet_thresh
= 75; // 600 ms / 8ms/sample
63 /*----------------------------------------------------------------------------*/
64 static int mpu6050_i2c_probe(struct i2c_client
*client
, const struct i2c_device_id
*id
);
65 static int mpu6050_i2c_remove(struct i2c_client
*client
);
66 static int mpu6050_i2c_detect(struct i2c_client
*client
, struct i2c_board_info
*info
);
68 /*----------------------------------------------------------------------------*/
71 GYRO_TRC_FILTER
= 0x01,
72 GYRO_TRC_RAWDATA
= 0x02,
73 GYRO_TRC_IOCTL
= 0x04,
78 /*----------------------------------------------------------------------------*/
84 /*----------------------------------------------------------------------------*/
85 struct data_resolution
87 struct scale_factor scalefactor
;
90 /*----------------------------------------------------------------------------*/
91 #define C_MAX_FIR_LENGTH (32)
92 /*----------------------------------------------------------------------------*/
95 s16 raw
[C_MAX_FIR_LENGTH
][MPU6050_AXES_NUM
];
96 int sum
[MPU6050_AXES_NUM
];
100 /*----------------------------------------------------------------------------*/
101 struct mpu6050_i2c_data
103 struct i2c_client
*client
;
105 struct hwmsen_convert cvt
;
108 struct data_resolution
*reso
;
113 s16 cali_sw
[MPU6050_AXES_NUM
+1];
116 s8 offset
[MPU6050_AXES_NUM
+1]; /*+1: for 4-byte alignment*/
117 s16 data
[MPU6050_AXES_NUM
+1];
119 #if defined(CONFIG_MPU6050_LOWPASS)
122 struct data_filter fir
;
125 #if defined(CONFIG_HAS_EARLYSUSPEND)
126 struct early_suspend early_drv
;
128 #if INV_GYRO_AUTO_CALI==1
129 s16 inv_cali_raw
[MPU6050_AXES_NUM
+1];
131 struct mutex temperature_mutex
;//for temperature protection
132 struct mutex raw_data_mutex
;//for inv_cali_raw[] protection
135 /*----------------------------------------------------------------------------*/
136 static struct i2c_driver mpu6050_i2c_driver
= {
138 .name
= MPU6050_DEV_NAME
,
140 .probe
= mpu6050_i2c_probe
,
141 .remove
= mpu6050_i2c_remove
,
142 .detect
= mpu6050_i2c_detect
,
143 #if !defined(CONFIG_HAS_EARLYSUSPEND)
144 .suspend
= mpu6050_suspend
,
145 .resume
= mpu6050_resume
,
147 .id_table
= mpu6050_i2c_id
,
150 /*----------------------------------------------------------------------------*/
151 static struct i2c_client
*mpu6050_i2c_client
= NULL
;
152 static struct platform_driver mpu6050_gyro_driver
;
153 static struct mpu6050_i2c_data
*obj_i2c_data
= NULL
;
154 static bool sensor_power
= false;
158 /*----------------------------------------------------------------------------*/
159 #define GYRO_TAG "[Gyroscope] "
160 #define GYRO_FUN(f) printk(KERN_INFO GYRO_TAG"%s\n", __FUNCTION__)
161 #define GYRO_ERR(fmt, args...) printk(KERN_ERR GYRO_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
162 #define GYRO_LOG(fmt, args...) printk(KERN_INFO GYRO_TAG fmt, ##args)
163 /*----------------------------------------------------------------------------*/
165 extern int MPU6050_gse_power(void);
166 extern int MPU6050_gse_mode(void);
168 #ifdef MPU6050_ACCESS_BY_GSE_I2C
169 extern int MPU6050_i2c_master_send(u8
*buf
, u8 len
);
170 extern int MPU6050_i2c_master_recv(u8
*buf
, u8 len
);
171 extern int MPU6050_hwmsen_read_block(u8 addr
, u8
*buf
, u8 len
);
172 extern int MPU6050_hwmsen_read_byte(u8 addr
, u8
*buf
);
175 static unsigned int power_on
= 0;
177 int MPU6050_gyro_power(void)
181 EXPORT_SYMBOL(MPU6050_gyro_power
);
183 int MPU6050_gyro_mode(void)
187 EXPORT_SYMBOL(MPU6050_gyro_mode
);
189 /*--------------------gyroscopy power control function----------------------------------*/
190 static void MPU6050_power(struct gyro_hw
*hw
, unsigned int on
)
193 if (hw
->power_id
!= POWER_NONE_MACRO
) // have externel LDO
195 GYRO_LOG("power %s\n", on
? "on" : "off");
196 if (power_on
== on
) // power status not change
198 GYRO_LOG("ignore power control: %d\n", on
);
200 else if (on
) // power on
202 if (!hwPowerOn(hw
->power_id
, hw
->power_vol
, "MPU6050GY"))
204 GYRO_ERR("power on fails!!\n");
209 if (MPU6050_gse_power() == false)
211 if (!hwPowerDown(hw
->power_id
, "MPU6050GY"))
213 GYRO_ERR("power off fail!!\n");
220 /*----------------------------------------------------------------------------*/
223 /*----------------------------------------------------------------------------*/
224 static int MPU6050_write_rel_calibration(struct mpu6050_i2c_data
*obj
, int dat
[MPU6050_AXES_NUM
])
226 obj
->cali_sw
[MPU6050_AXIS_X
] = obj
->cvt
.sign
[MPU6050_AXIS_X
]*dat
[obj
->cvt
.map
[MPU6050_AXIS_X
]];
227 obj
->cali_sw
[MPU6050_AXIS_Y
] = obj
->cvt
.sign
[MPU6050_AXIS_Y
]*dat
[obj
->cvt
.map
[MPU6050_AXIS_Y
]];
228 obj
->cali_sw
[MPU6050_AXIS_Z
] = obj
->cvt
.sign
[MPU6050_AXIS_Z
]*dat
[obj
->cvt
.map
[MPU6050_AXIS_Z
]];
230 if (atomic_read(&obj
->trace
) & GYRO_TRC_CALI
)
232 GYRO_LOG("test (%5d, %5d, %5d) ->(%5d, %5d, %5d)->(%5d, %5d, %5d))\n",
233 obj
->cvt
.sign
[MPU6050_AXIS_X
],obj
->cvt
.sign
[MPU6050_AXIS_Y
],obj
->cvt
.sign
[MPU6050_AXIS_Z
],
234 dat
[MPU6050_AXIS_X
], dat
[MPU6050_AXIS_Y
], dat
[MPU6050_AXIS_Z
],
235 obj
->cvt
.map
[MPU6050_AXIS_X
],obj
->cvt
.map
[MPU6050_AXIS_Y
],obj
->cvt
.map
[MPU6050_AXIS_Z
]);
236 GYRO_LOG("write gyro calibration data (%5d, %5d, %5d)\n",
237 obj
->cali_sw
[MPU6050_AXIS_X
],obj
->cali_sw
[MPU6050_AXIS_Y
],obj
->cali_sw
[MPU6050_AXIS_Z
]);
244 /*----------------------------------------------------------------------------*/
245 static int MPU6050_ResetCalibration(struct i2c_client
*client
)
247 struct mpu6050_i2c_data
*obj
= i2c_get_clientdata(client
);
249 memset(obj
->cali_sw
, 0x00, sizeof(obj
->cali_sw
));
252 /*----------------------------------------------------------------------------*/
253 static int MPU6050_ReadCalibration(struct i2c_client
*client
, int dat
[MPU6050_AXES_NUM
])
255 struct mpu6050_i2c_data
*obj
= i2c_get_clientdata(client
);
257 dat
[obj
->cvt
.map
[MPU6050_AXIS_X
]] = obj
->cvt
.sign
[MPU6050_AXIS_X
]*obj
->cali_sw
[MPU6050_AXIS_X
];
258 dat
[obj
->cvt
.map
[MPU6050_AXIS_Y
]] = obj
->cvt
.sign
[MPU6050_AXIS_Y
]*obj
->cali_sw
[MPU6050_AXIS_Y
];
259 dat
[obj
->cvt
.map
[MPU6050_AXIS_Z
]] = obj
->cvt
.sign
[MPU6050_AXIS_Z
]*obj
->cali_sw
[MPU6050_AXIS_Z
];
262 if (atomic_read(&obj
->trace
) & GYRO_TRC_CALI
)
264 GYRO_LOG("Read gyro calibration data (%5d, %5d, %5d)\n",
265 dat
[MPU6050_AXIS_X
],dat
[MPU6050_AXIS_Y
],dat
[MPU6050_AXIS_Z
]);
271 /*----------------------------------------------------------------------------*/
272 /*----------------------------------------------------------------------------*/
273 static int MPU6050_WriteCalibration(struct i2c_client
*client
, int dat
[MPU6050_AXES_NUM
])
275 struct mpu6050_i2c_data
*obj
= i2c_get_clientdata(client
);
277 int cali
[MPU6050_AXES_NUM
];
283 GYRO_ERR("null ptr!!\n");
288 cali
[obj
->cvt
.map
[MPU6050_AXIS_X
]] = obj
->cvt
.sign
[MPU6050_AXIS_X
]*obj
->cali_sw
[MPU6050_AXIS_X
];
289 cali
[obj
->cvt
.map
[MPU6050_AXIS_Y
]] = obj
->cvt
.sign
[MPU6050_AXIS_Y
]*obj
->cali_sw
[MPU6050_AXIS_Y
];
290 cali
[obj
->cvt
.map
[MPU6050_AXIS_Z
]] = obj
->cvt
.sign
[MPU6050_AXIS_Z
]*obj
->cali_sw
[MPU6050_AXIS_Z
];
291 cali
[MPU6050_AXIS_X
] += dat
[MPU6050_AXIS_X
];
292 cali
[MPU6050_AXIS_Y
] += dat
[MPU6050_AXIS_Y
];
293 cali
[MPU6050_AXIS_Z
] += dat
[MPU6050_AXIS_Z
];
295 if (atomic_read(&obj
->trace
) & GYRO_TRC_CALI
)
297 GYRO_LOG("write gyro calibration data (%5d, %5d, %5d)-->(%5d, %5d, %5d)\n",
298 dat
[MPU6050_AXIS_X
], dat
[MPU6050_AXIS_Y
], dat
[MPU6050_AXIS_Z
],
299 cali
[MPU6050_AXIS_X
],cali
[MPU6050_AXIS_Y
],cali
[MPU6050_AXIS_Z
]);
302 return MPU6050_write_rel_calibration(obj
, cali
);
307 /*----------------------------------------------------------------------------*/
310 /*----------------------------------------------------------------------------*/
311 static int MPU6050_ReadStart(struct i2c_client
*client
, bool enable
)
317 databuf
[0] = MPU6050_REG_FIFO_EN
;
321 //enable xyz gyro in FIFO
322 databuf
[1] = (MPU6050_FIFO_GYROX_EN
|MPU6050_FIFO_GYROY_EN
|MPU6050_FIFO_GYROZ_EN
);
326 //disable xyz gyro in FIFO
330 #ifdef MPU6050_ACCESS_BY_GSE_I2C
331 res
= MPU6050_i2c_master_send(databuf
, 0x2);
333 res
= i2c_master_send(client
, databuf
, 0x2);
337 GYRO_ERR(" enable xyz gyro in FIFO error,enable: 0x%x!\n", databuf
[1]);
338 return MPU6050_ERR_I2C
;
340 GYRO_LOG("MPU6050_ReadStart: enable xyz gyro in FIFO: 0x%x\n", databuf
[1]);
341 return MPU6050_SUCCESS
;
346 //----------------------------------------------------------------------------//
347 static int MPU6050_SetPowerMode(struct i2c_client
*client
, bool enable
)
352 if (enable
== sensor_power
)
354 GYRO_LOG("Sensor power status is newest!\n");
355 return MPU6050_SUCCESS
;
358 #ifdef MPU6050_ACCESS_BY_GSE_I2C
359 if (MPU6050_hwmsen_read_byte(MPU6050_REG_PWR_CTL
, databuf
))
361 if (hwmsen_read_byte(client
, MPU6050_REG_PWR_CTL
, databuf
))
364 GYRO_ERR("read power ctl register err!\n");
365 return MPU6050_ERR_I2C
;
368 databuf
[0] &= ~MPU6050_SLEEP
;
371 if (MPU6050_gse_mode() == false)
373 databuf
[0] |= MPU6050_SLEEP
;
382 databuf
[1] = databuf
[0];
383 databuf
[0] = MPU6050_REG_PWR_CTL
;
385 #ifdef MPU6050_ACCESS_BY_GSE_I2C
386 res
= MPU6050_i2c_master_send(databuf
, 0x2);
388 res
= i2c_master_send(client
, databuf
, 0x2);
393 GYRO_LOG("set power mode failed!\n");
394 return MPU6050_ERR_I2C
;
398 GYRO_LOG("set power mode ok %d!\n", enable
);
402 sensor_power
= enable
;
404 return MPU6050_SUCCESS
;
407 /*----------------------------------------------------------------------------*/
408 static int MPU6050_SetDataFormat(struct i2c_client
*client
, u8 dataformat
)
414 databuf
[0] = MPU6050_REG_CFG
;
415 databuf
[1] = dataformat
;
416 #ifdef MPU6050_ACCESS_BY_GSE_I2C
417 res
= MPU6050_i2c_master_send(databuf
, 0x2);
419 res
= i2c_master_send(client
, databuf
, 0x2);
423 return MPU6050_ERR_I2C
;
426 //read sample rate after written for test
428 #ifdef MPU6050_ACCESS_BY_GSE_I2C
429 if (MPU6050_hwmsen_read_byte(MPU6050_REG_CFG
, databuf
))
431 if (hwmsen_read_byte(client
, MPU6050_REG_CFG
, databuf
))
434 GYRO_ERR("read data format register err!\n");
435 return MPU6050_ERR_I2C
;
439 GYRO_LOG("read data format: 0x%x\n", databuf
[0]);
442 return MPU6050_SUCCESS
;
445 static int MPU6050_SetFullScale(struct i2c_client
*client
, u8 dataformat
)
451 databuf
[0] = MPU6050_REG_GYRO_CFG
;
452 databuf
[1] = dataformat
;
453 #ifdef MPU6050_ACCESS_BY_GSE_I2C
454 res
= MPU6050_i2c_master_send(databuf
, 0x2);
456 res
= i2c_master_send(client
, databuf
, 0x2);
460 return MPU6050_ERR_I2C
;
463 //read sample rate after written for test
465 #ifdef MPU6050_ACCESS_BY_GSE_I2C
466 if (MPU6050_hwmsen_read_byte(MPU6050_REG_GYRO_CFG
, databuf
))
468 if (hwmsen_read_byte(client
, MPU6050_REG_GYRO_CFG
, databuf
))
471 GYRO_ERR("read data format register err!\n");
472 return MPU6050_ERR_I2C
;
476 GYRO_LOG("read data format: 0x%x\n", databuf
[0]);
479 return MPU6050_SUCCESS
;
483 // set the sample rate
484 static int MPU6050_SetSampleRate(struct i2c_client
*client
, int sample_rate
)
490 #ifdef MPU6050_ACCESS_BY_GSE_I2C
491 if (MPU6050_hwmsen_read_byte(MPU6050_REG_CFG
, databuf
))
493 if (hwmsen_read_byte(client
, MPU6050_REG_CFG
, databuf
))
496 GYRO_ERR("read gyro data format register err!\n");
497 return MPU6050_ERR_I2C
;
501 GYRO_LOG("read gyro data format register: 0x%x\n", databuf
[0]);
504 if ((databuf
[0] & 0x07) == 0) //Analog sample rate is 8KHz
506 rate_div
= 8 * 1024 / sample_rate
- 1;
510 rate_div
= 1024 / sample_rate
- 1;
513 if (rate_div
> 255) // rate_div: 0 to 255;
517 else if (rate_div
< 0)
522 databuf
[0] = MPU6050_REG_SAMRT_DIV
;
523 databuf
[1] = rate_div
;
524 #ifdef MPU6050_ACCESS_BY_GSE_I2C
525 res
= MPU6050_i2c_master_send(databuf
, 0x2);
527 res
= i2c_master_send(client
, databuf
, 0x2);
531 GYRO_ERR("write sample rate register err!\n");
532 return MPU6050_ERR_I2C
;
535 //read sample div after written for test
537 #ifdef MPU6050_ACCESS_BY_GSE_I2C
538 if (MPU6050_hwmsen_read_byte(MPU6050_REG_SAMRT_DIV
, databuf
))
540 if (hwmsen_read_byte(client
, MPU6050_REG_SAMRT_DIV
, databuf
))
543 GYRO_ERR("read gyro sample rate register err!\n");
544 return MPU6050_ERR_I2C
;
548 GYRO_LOG("read gyro sample rate: 0x%x\n", databuf
[0]);
551 return MPU6050_SUCCESS
;
553 /*----------------------------------------------------------------------------*/
554 /*----------------------------------------------------------------------------*/
555 static int MPU6050_FIFOConfig(struct i2c_client
*client
, u8 clk
)
561 //use gyro X, Y or Z for clocking
562 databuf
[0] = MPU6050_REG_PWR_CTL
;
564 #ifdef MPU6050_ACCESS_BY_GSE_I2C
565 res
= MPU6050_i2c_master_send(databuf
, 0x2);
567 res
= i2c_master_send(client
, databuf
, 0x2);
571 GYRO_ERR("write Power CTRL register err!\n");
572 return MPU6050_ERR_I2C
;
574 GYRO_LOG("MPU6050 use gyro X for clocking OK!\n");
578 //enable xyz gyro in FIFO
579 databuf
[0] = MPU6050_REG_FIFO_EN
;
580 databuf
[1] = (MPU6050_FIFO_GYROX_EN
|MPU6050_FIFO_GYROY_EN
|MPU6050_FIFO_GYROZ_EN
);
581 #ifdef MPU6050_ACCESS_BY_GSE_I2C
582 res
= MPU6050_i2c_master_send(databuf
, 0x2);
584 res
= i2c_master_send(client
, databuf
, 0x2);
588 GYRO_ERR("write Power CTRL register err!\n");
589 return MPU6050_ERR_I2C
;
591 GYRO_LOG("MPU6050 enable xyz gyro in FIFO OK!\n");
594 databuf
[0] = MPU6050_REG_AUX_VDD
;
595 databuf
[1] = MPU6050_AUX_VDDIO_DIS
;
596 #ifdef MPU6050_ACCESS_BY_GSE_I2C
597 res
= MPU6050_i2c_master_send(databuf
, 0x2);
599 res
= i2c_master_send(client
, databuf
, 0x2);
603 GYRO_ERR("write AUX_VDD register err!\n");
604 return MPU6050_ERR_I2C
;
606 GYRO_LOG("MPU6050 disable AUX_VDDIO OK!\n");
608 //enable FIFO and reset FIFO
609 databuf
[0] = MPU6050_REG_FIFO_CTL
;
610 databuf
[1] = (MPU6050_FIFO_EN
| MPU6050_FIFO_RST
);
611 #ifdef MPU6050_ACCESS_BY_GSE_I2C
612 res
= MPU6050_i2c_master_send(databuf
, 0x2);
614 res
= i2c_master_send(client
, databuf
, 0x2);
618 GYRO_ERR("write FIFO CTRL register err!\n");
619 return MPU6050_ERR_I2C
;
622 GYRO_LOG("MPU6050_FIFOConfig OK!\n");
623 return MPU6050_SUCCESS
;
626 /*----------------------------------------------------------------------------*/
627 static int MPU6050_ReadFifoData(struct i2c_client
*client
, s16
*data
, int* datalen
)
629 struct mpu6050_i2c_data
*obj
= i2c_get_clientdata(client
);
630 u8 buf
[MPU6050_DATA_LEN
] = {0};
631 s16 tmp1
[MPU6050_AXES_NUM
] = {0};
632 s16 tmp2
[MPU6050_AXES_NUM
] = {0};
644 //stop putting data in FIFO
645 MPU6050_ReadStart(client
, FALSE
);
647 //read data number of bytes in FIFO
648 #ifdef MPU6050_ACCESS_BY_GSE_I2C
649 err
= MPU6050_hwmsen_read_byte(MPU6050_REG_FIFO_CNTH
, &tmp
);
651 err
= hwmsen_read_byte(client
, MPU6050_REG_FIFO_CNTH
, &tmp
);
655 GYRO_ERR("read data high number of bytes error: %d\n", err
);
658 packet_cnt
= tmp
<< 8;
660 #ifdef MPU6050_ACCESS_BY_GSE_I2C
661 err
= MPU6050_hwmsen_read_byte(MPU6050_REG_FIFO_CNTL
, &tmp
);
663 err
= hwmsen_read_byte(client
, MPU6050_REG_FIFO_CNTL
, &tmp
);
667 GYRO_ERR("read data low number of bytes error: %d\n", err
);
670 packet_cnt
= (packet_cnt
+ tmp
) /MPU6050_DATA_LEN
;
672 GYRO_LOG("MPU6050 Read Data packet number OK: %d\n", packet_cnt
);
674 *datalen
= packet_cnt
;
676 //Within +-5% range: timing_tolerance * packet_thresh=0.05*75
677 if (packet_cnt
&& (abs(packet_thresh
-packet_cnt
) < 4))
680 for (i
= 0; i
< packet_cnt
; i
++)
682 #ifdef MPU6050_ACCESS_BY_GSE_I2C
683 if (MPU6050_hwmsen_read_block(MPU6050_REG_FIFO_DATA
, buf
, MPU6050_DATA_LEN
))
685 if (hwmsen_read_block(client
, MPU6050_REG_FIFO_DATA
, buf
, MPU6050_DATA_LEN
))
688 GYRO_ERR("MPU6050 read data from FIFO error: %d\n", err
);
693 GYRO_LOG("MPU6050 read Data of diff address from FIFO OK !\n");
696 tmp1
[MPU6050_AXIS_X
] = (s16
)((buf
[MPU6050_AXIS_X
*2+1]) | (buf
[MPU6050_AXIS_X
*2] << 8));
697 tmp1
[MPU6050_AXIS_Y
] = (s16
)((buf
[MPU6050_AXIS_Y
*2+1]) | (buf
[MPU6050_AXIS_Y
*2] << 8));
698 tmp1
[MPU6050_AXIS_Z
] = (s16
)((buf
[MPU6050_AXIS_Z
*2+1]) | (buf
[MPU6050_AXIS_Z
*2] << 8));
701 tmp2
[obj
->cvt
.map
[MPU6050_AXIS_X
]] = obj
->cvt
.sign
[MPU6050_AXIS_X
]*tmp1
[MPU6050_AXIS_X
];
702 tmp2
[obj
->cvt
.map
[MPU6050_AXIS_Y
]] = obj
->cvt
.sign
[MPU6050_AXIS_Y
]*tmp1
[MPU6050_AXIS_Y
];
703 tmp2
[obj
->cvt
.map
[MPU6050_AXIS_Z
]] = obj
->cvt
.sign
[MPU6050_AXIS_Z
]*tmp1
[MPU6050_AXIS_Z
];
705 data
[3* i
+MPU6050_AXIS_X
] = tmp2
[MPU6050_AXIS_X
];
706 data
[3* i
+MPU6050_AXIS_Y
] = tmp2
[MPU6050_AXIS_Y
];
707 data
[3* i
+MPU6050_AXIS_Z
] = tmp2
[MPU6050_AXIS_Z
];
709 GYRO_LOG("gyro FIFO packet[%d]:[%04X %04X %04X] => [%5d %5d %5d]\n", i
,
710 data
[3*i
+MPU6050_AXIS_X
], data
[3*i
+MPU6050_AXIS_Y
], data
[3*i
+MPU6050_AXIS_Z
],
711 data
[3*i
+MPU6050_AXIS_X
], data
[3*i
+MPU6050_AXIS_Y
], data
[3*i
+MPU6050_AXIS_Z
]);
717 GYRO_ERR("MPU6050 Incorrect packet count: %d\n", packet_cnt
);
724 /*----------------------------------------------------------------------------*/
725 static int MPU6050_ReadGyroData(struct i2c_client
*client
, char *buf
, int bufsize
)
729 struct mpu6050_i2c_data
*obj
= i2c_get_clientdata(client
);
731 if (atomic_read(&obj
->suspend
))
736 if (sensor_power
== false)
738 MPU6050_SetPowerMode(client
, true);
741 #if INV_GYRO_AUTO_CALI==1
742 #ifdef MPU6050_ACCESS_BY_GSE_I2C
743 if (MPU6050_hwmsen_read_block(MPU6050_REG_TEMPH
, databuf
, 2))
745 if (hwmsen_read_block(client
, MPU6050_REG_TEMPH
, databuf
, 2))
748 GYRO_ERR("MPU6050 read temperature data error\n");
753 mutex_lock(&obj
->temperature_mutex
);
754 obj
->temperature
= ((s16
)((databuf
[1]) | (databuf
[0] << 8)));
755 mutex_unlock(&obj
->temperature_mutex
);
759 #ifdef MPU6050_ACCESS_BY_GSE_I2C
760 if (MPU6050_hwmsen_read_block(MPU6050_REG_GYRO_XH
, databuf
, 6))
762 if (hwmsen_read_block(client
, MPU6050_REG_GYRO_XH
, databuf
, 6))
765 GYRO_ERR("MPU6050 read gyroscope data error\n");
770 obj
->data
[MPU6050_AXIS_X
] = ((s16
)((databuf
[MPU6050_AXIS_X
*2+1]) | (databuf
[MPU6050_AXIS_X
*2] << 8)));
771 obj
->data
[MPU6050_AXIS_Y
] = ((s16
)((databuf
[MPU6050_AXIS_Y
*2+1]) | (databuf
[MPU6050_AXIS_Y
*2] << 8)));
772 obj
->data
[MPU6050_AXIS_Z
] = ((s16
)((databuf
[MPU6050_AXIS_Z
*2+1]) | (databuf
[MPU6050_AXIS_Z
*2] << 8)));
774 if (atomic_read(&obj
->trace
) & GYRO_TRC_RAWDATA
)
776 GYRO_LOG("read gyro register: %d, %d, %d, %d, %d, %d",
777 databuf
[0], databuf
[1], databuf
[2], databuf
[3], databuf
[4], databuf
[5]);
778 GYRO_LOG("get gyro raw data (0x%08X, 0x%08X, 0x%08X) -> (%5d, %5d, %5d)\n",
779 obj
->data
[MPU6050_AXIS_X
],obj
->data
[MPU6050_AXIS_Y
],obj
->data
[MPU6050_AXIS_Z
],
780 obj
->data
[MPU6050_AXIS_X
],obj
->data
[MPU6050_AXIS_Y
],obj
->data
[MPU6050_AXIS_Z
]);
783 #if INV_GYRO_AUTO_CALI==1
784 mutex_lock(&obj
->raw_data_mutex
);
786 obj
->inv_cali_raw
[obj
->cvt
.map
[MPU6050_AXIS_X
]] = obj
->cvt
.sign
[MPU6050_AXIS_X
]*obj
->data
[MPU6050_AXIS_X
];
787 obj
->inv_cali_raw
[obj
->cvt
.map
[MPU6050_AXIS_Y
]] = obj
->cvt
.sign
[MPU6050_AXIS_Y
]*obj
->data
[MPU6050_AXIS_Y
];
788 obj
->inv_cali_raw
[obj
->cvt
.map
[MPU6050_AXIS_Z
]] = obj
->cvt
.sign
[MPU6050_AXIS_Z
]*obj
->data
[MPU6050_AXIS_Z
];
789 mutex_unlock(&obj
->raw_data_mutex
);
791 obj
->data
[MPU6050_AXIS_X
] = obj
->data
[MPU6050_AXIS_X
] + obj
->cali_sw
[MPU6050_AXIS_X
];
792 obj
->data
[MPU6050_AXIS_Y
] = obj
->data
[MPU6050_AXIS_Y
] + obj
->cali_sw
[MPU6050_AXIS_Y
];
793 obj
->data
[MPU6050_AXIS_Z
] = obj
->data
[MPU6050_AXIS_Z
] + obj
->cali_sw
[MPU6050_AXIS_Z
];
796 data
[obj
->cvt
.map
[MPU6050_AXIS_X
]] = obj
->cvt
.sign
[MPU6050_AXIS_X
]*obj
->data
[MPU6050_AXIS_X
];
797 data
[obj
->cvt
.map
[MPU6050_AXIS_Y
]] = obj
->cvt
.sign
[MPU6050_AXIS_Y
]*obj
->data
[MPU6050_AXIS_Y
];
798 data
[obj
->cvt
.map
[MPU6050_AXIS_Z
]] = obj
->cvt
.sign
[MPU6050_AXIS_Z
]*obj
->data
[MPU6050_AXIS_Z
];
800 //Out put the degree/second(o/s)
801 data
[MPU6050_AXIS_X
] = data
[MPU6050_AXIS_X
] * MPU6050_FS_MAX_LSB
/ MPU6050_DEFAULT_LSB
;
802 data
[MPU6050_AXIS_Y
] = data
[MPU6050_AXIS_Y
] * MPU6050_FS_MAX_LSB
/ MPU6050_DEFAULT_LSB
;
803 data
[MPU6050_AXIS_Z
] = data
[MPU6050_AXIS_Z
] * MPU6050_FS_MAX_LSB
/ MPU6050_DEFAULT_LSB
;
808 sprintf(buf
, "%04x %04x %04x", data
[MPU6050_AXIS_X
],data
[MPU6050_AXIS_Y
],data
[MPU6050_AXIS_Z
]);
811 if (atomic_read(&obj
->trace
) & GYRO_TRC_DATA
)
813 GYRO_LOG("get gyro data packet:[%d %d %d]\n", data
[0], data
[1], data
[2]);
822 static int MPU6050_PROCESS_SMT_DATA(struct i2c_client
*client
, short *data
)
829 long xAvg
, yAvg
, zAvg
;
830 long xRMS
, yRMS
, zRMS
;
833 int bias_thresh
= 5242; // 40 dps * 131.072 LSB/dps
834 //float RMS_thresh = 687.19f; // (.2 dps * 131.072) ^ 2
835 long RMS_thresh
= 68719; // (.2 dps * 131.072) ^ 2
839 GYRO_LOG("MPU6050 read gyro data OK, total number: %d \n", total_num
);
840 for (i
= 0; i
< total_num
; i
++)
842 xSum
=xSum
+ data
[MPU6050_AXES_NUM
*i
+ MPU6050_AXIS_X
+2];
843 ySum
=ySum
+ data
[MPU6050_AXES_NUM
*i
+ MPU6050_AXIS_Y
+2];
844 zSum
=zSum
+ data
[MPU6050_AXES_NUM
*i
+ MPU6050_AXIS_Z
+2];
847 FLPLOGD("read gyro data OK: packet_num:%d, [X:%5d, Y:%5d, Z:%5d]\n", i, data[MPU6050_AXES_NUM*i + MPU6050_AXIS_X +2], data[MPU6050_AXES_NUM*i + MPU6050_AXIS_Y +2], data[MPU6050_AXES_NUM*i + MPU6050_AXIS_Z +2]);
848 FLPLOGD("MPU6050 xSum: %5d, ySum: %5d, zSum: %5d \n", xSum, ySum, zSum);
852 GYRO_LOG("MPU6050 xSum: %5ld, ySum: %5ld, zSum: %5ld \n", xSum
, ySum
, zSum
);
856 xAvg
= (xSum
/ total_num
);
857 yAvg
= (ySum
/ total_num
);
858 zAvg
= (zSum
/ total_num
);
867 GYRO_LOG("MPU6050 xAvg: %ld, yAvg: %ld, zAvg: %ld \n", xAvg
, yAvg
, zAvg
);
869 if ( abs(xAvg
) >bias_thresh
)
871 GYRO_LOG("X-Gyro bias exceeded threshold \n");
874 if ( abs(yAvg
) > bias_thresh
)
876 GYRO_LOG("Y-Gyro bias exceeded threshold \n");
879 if ( abs(zAvg
) > bias_thresh
)
881 GYRO_LOG("Z-Gyro bias exceeded threshold \n");
890 for ( i
= 0; i
< total_num
; i
++)
892 xRMS
+= (data
[MPU6050_AXES_NUM
*i
+ MPU6050_AXIS_X
+2]-xAvg
)*(data
[MPU6050_AXES_NUM
*i
+ MPU6050_AXIS_X
+2]-xAvg
);
893 yRMS
+= (data
[MPU6050_AXES_NUM
*i
+ MPU6050_AXIS_Y
+2]-yAvg
)*(data
[MPU6050_AXES_NUM
*i
+ MPU6050_AXIS_Y
+2]-yAvg
);
894 zRMS
+= (data
[MPU6050_AXES_NUM
*i
+ MPU6050_AXIS_Z
+2]-zAvg
)*(data
[MPU6050_AXES_NUM
*i
+ MPU6050_AXIS_Z
+2]-zAvg
);
897 GYRO_LOG("MPU6050 xRMS: %ld, yRMS: %ld, zRMS: %ld \n", xRMS
, yRMS
, zRMS
);
902 if (FACTORY_BOOT
== get_boot_mode())
904 if ( xRMS
> RMS_thresh
* total_num
)
906 GYRO_LOG("X-Gyro RMS exceeded threshold, RMS_thresh: %ld \n", RMS_thresh
* total_num
);
909 if ( yRMS
> RMS_thresh
* total_num
)
911 GYRO_LOG("Y-Gyro RMS exceeded threshold, RMS_thresh: %ld \n", RMS_thresh
* total_num
);
914 if ( zRMS
> RMS_thresh
* total_num
)
916 GYRO_LOG("Z-Gyro RMS exceeded threshold, RMS_thresh: %ld \n", RMS_thresh
* total_num
);
919 if ( xRMS
== 0 || yRMS
== 0 || zRMS
== 0)
920 //If any of the RMS noise value returns zero, then we might have dead gyro or FIFO/register failure
922 GYRO_LOG("retval %d \n", retval
);
929 /*----------------------------------------------------------------------------*/
930 static int MPU6050_SMTReadSensorData(struct i2c_client
*client
, s16
*buf
, int bufsize
)
932 //S16 gyro[MPU6050_AXES_NUM*MPU6050_FIFOSIZE];
935 int datalen
, total_num
= 0;
939 if (sensor_power
== false)
941 MPU6050_SetPowerMode(client
, true);
954 for (i
= 0; i
< MPU6050_AXES_NUM
; i
++)
956 res
= MPU6050_FIFOConfig(client
, (i
+1));
959 GYRO_ERR("MPU6050_FIFOConfig error:%d!\n", res
);
963 //putting data in FIFO during the delayed 600ms
966 res
= MPU6050_ReadFifoData(client
, &(buf
[total_num
+2]), &datalen
);
975 GYRO_ERR("MPU6050_ReadData error:%d!\n", res
);
982 total_num
+=datalen
*MPU6050_AXES_NUM
;
986 GYRO_LOG("gyroscope read data OK, total packet: %d", buf
[0] );
991 /*----------------------------------------------------------------------------*/
992 static int MPU6050_ReadChipInfo(struct i2c_client
*client
, char *buf
, int bufsize
)
996 memset(databuf
, 0, sizeof(u8
)*10);
998 if ((NULL
== buf
)||(bufsize
<=30))
1009 sprintf(buf
, "MPU6050 Chip");
1013 #if INV_GYRO_AUTO_CALI==1
1014 /*----------------------------------------------------------------------------*/
1015 static int MPU6050_ReadGyroDataRaw(struct i2c_client
*client
, char *buf
, int bufsize
)
1017 struct mpu6050_i2c_data
*obj
= i2c_get_clientdata(client
);
1019 mutex_lock(&obj
->raw_data_mutex
);
1020 //return gyro raw LSB in device orientation
1021 sprintf(buf
, "%x %x %x", obj
->inv_cali_raw
[MPU6050_AXIS_X
],obj
->inv_cali_raw
[MPU6050_AXIS_Y
],obj
->inv_cali_raw
[MPU6050_AXIS_Z
]);
1024 if (atomic_read(&obj
->trace
) & GYRO_TRC_DATA
)
1026 GYRO_LOG("get gyro raw data packet:[%d %d %d]\n", obj
->inv_cali_raw
[0], obj
->inv_cali_raw
[1], obj
->inv_cali_raw
[2]);
1029 mutex_unlock(&obj
->raw_data_mutex
);
1035 /*----------------------------------------------------------------------------*/
1036 static int MPU6050_ReadTemperature(struct i2c_client
*client
, char *buf
, int bufsize
)
1038 struct mpu6050_i2c_data
*obj
= i2c_get_clientdata(client
);
1040 mutex_lock(&obj
->temperature_mutex
);
1041 sprintf(buf
, "%x", obj
->temperature
);
1044 if (atomic_read(&obj
->trace
) & GYRO_TRC_DATA
)
1046 GYRO_LOG("get gyro temperature:[%d]\n", obj
->temperature
);
1049 mutex_unlock(&obj
->temperature_mutex
);
1055 /*----------------------------------------------------------------------------*/
1056 static int MPU6050_ReadPowerStatus(struct i2c_client
*client
, char *buf
, int bufsize
)
1059 GYRO_LOG("get gyro PowerStatus:[%d]\n", sensor_power
);
1062 sprintf(buf
, "%x", sensor_power
);
1069 /*----------------------------------------------------------------------------*/
1070 static ssize_t
show_chipinfo_value(struct device_driver
*ddri
, char *buf
)
1072 struct i2c_client
*client
= mpu6050_i2c_client
;
1073 char strbuf
[MPU6050_BUFSIZE
];
1076 GYRO_ERR("i2c client is null!!\n");
1080 MPU6050_ReadChipInfo(client
, strbuf
, MPU6050_BUFSIZE
);
1081 return snprintf(buf
, PAGE_SIZE
, "%s\n", strbuf
);
1083 /*----------------------------------------------------------------------------*/
1084 static ssize_t
show_sensordata_value(struct device_driver
*ddri
, char *buf
)
1086 struct i2c_client
*client
= mpu6050_i2c_client
;
1087 char strbuf
[MPU6050_BUFSIZE
];
1091 GYRO_ERR("i2c client is null!!\n");
1095 MPU6050_ReadGyroData(client
, strbuf
, MPU6050_BUFSIZE
);
1096 return snprintf(buf
, PAGE_SIZE
, "%s\n", strbuf
);;
1099 /*----------------------------------------------------------------------------*/
1100 static ssize_t
show_trace_value(struct device_driver
*ddri
, char *buf
)
1103 struct mpu6050_i2c_data
*obj
= obj_i2c_data
;
1106 GYRO_ERR("i2c_data obj is null!!\n");
1110 res
= snprintf(buf
, PAGE_SIZE
, "0x%04X\n", atomic_read(&obj
->trace
));
1113 /*----------------------------------------------------------------------------*/
1114 static ssize_t
store_trace_value(struct device_driver
*ddri
, const char *buf
, size_t count
)
1116 struct mpu6050_i2c_data
*obj
= obj_i2c_data
;
1120 GYRO_ERR("i2c_data obj is null!!\n");
1124 if (1 == sscanf(buf
, "0x%x", &trace
))
1126 atomic_set(&obj
->trace
, trace
);
1130 GYRO_ERR("invalid content: '%s', length = %d\n", buf
, count
);
1135 /*----------------------------------------------------------------------------*/
1136 static ssize_t
show_status_value(struct device_driver
*ddri
, char *buf
)
1139 struct mpu6050_i2c_data
*obj
= obj_i2c_data
;
1142 GYRO_ERR("i2c_data obj is null!!\n");
1148 len
+= snprintf(buf
+len
, PAGE_SIZE
-len
, "CUST: %d %d (%d %d)\n",
1149 obj
->hw
->i2c_num
, obj
->hw
->direction
, obj
->hw
->power_id
, obj
->hw
->power_vol
);
1153 len
+= snprintf(buf
+len
, PAGE_SIZE
-len
, "CUST: NULL\n");
1157 /*----------------------------------------------------------------------------*/
1158 static DRIVER_ATTR(chipinfo
, S_IRUGO
, show_chipinfo_value
, NULL
);
1159 static DRIVER_ATTR(sensordata
, S_IRUGO
, show_sensordata_value
, NULL
);
1160 static DRIVER_ATTR(trace
, S_IWUSR
| S_IRUGO
, show_trace_value
, store_trace_value
);
1161 static DRIVER_ATTR(status
, S_IRUGO
, show_status_value
, NULL
);
1162 /*----------------------------------------------------------------------------*/
1163 static struct driver_attribute
*MPU6050_attr_list
[] = {
1164 &driver_attr_chipinfo
, /*chip information*/
1165 &driver_attr_sensordata
, /*dump sensor data*/
1166 &driver_attr_trace
, /*trace log*/
1167 &driver_attr_status
,
1169 /*----------------------------------------------------------------------------*/
1170 static int mpu6050_create_attr(struct device_driver
*driver
)
1173 int num
= (int)(sizeof(MPU6050_attr_list
)/sizeof(MPU6050_attr_list
[0]));
1179 for (idx
= 0; idx
< num
; idx
++)
1181 if (0 != (err
= driver_create_file(driver
, MPU6050_attr_list
[idx
])))
1183 GYRO_ERR("driver_create_file (%s) = %d\n", MPU6050_attr_list
[idx
]->attr
.name
, err
);
1189 /*----------------------------------------------------------------------------*/
1190 static int mpu6050_delete_attr(struct device_driver
*driver
)
1193 int num
= (int)(sizeof(MPU6050_attr_list
)/sizeof(MPU6050_attr_list
[0]));
1201 for (idx
= 0; idx
< num
; idx
++)
1203 driver_remove_file(driver
, MPU6050_attr_list
[idx
]);
1210 /*----------------------------------------------------------------------------*/
1211 static int mpu6050_gpio_config(void)
1213 //because we donot use EINT ,to support low power
1214 // config to GPIO input mode + PD
1215 //set GPIO_MSE_EINT_PIN
1216 mt_set_gpio_mode(GPIO_GYRO_EINT_PIN
, GPIO_GYRO_EINT_PIN_M_GPIO
);
1217 mt_set_gpio_dir(GPIO_GYRO_EINT_PIN
, GPIO_DIR_IN
);
1218 mt_set_gpio_pull_enable(GPIO_GYRO_EINT_PIN
, GPIO_PULL_ENABLE
);
1219 mt_set_gpio_pull_select(GPIO_GYRO_EINT_PIN
, GPIO_PULL_DOWN
);
1222 static int mpu6050_init_client(struct i2c_client
*client
, bool enable
)
1224 struct mpu6050_i2c_data
*obj
= i2c_get_clientdata(client
);
1226 bool sensor_power_org
;
1229 mpu6050_gpio_config();
1231 sensor_power_org
= sensor_power
;
1232 res
= MPU6050_SetPowerMode(client
, true);
1233 if (res
!= MPU6050_SUCCESS
)
1240 // The range should at least be 17.45 rad/s (ie: ~1000 deg/s).
1241 res
= MPU6050_SetDataFormat(client
, (MPU6050_SYNC_GYROX
<< MPU6050_EXT_SYNC
)|
1242 MPU6050_RATE_1K_LPFB_188HZ
);
1244 res
= MPU6050_SetFullScale(client
, (MPU6050_DEFAULT_FS
<< MPU6050_FS_RANGE
));
1245 if (res
!= MPU6050_SUCCESS
)
1250 // Set 125HZ sample rate
1251 res
= MPU6050_SetSampleRate(client
, 125);
1252 if (res
!= MPU6050_SUCCESS
)
1257 res
= MPU6050_SetPowerMode(client
, sensor_power_org
);
1258 if (res
!= MPU6050_SUCCESS
)
1263 GYRO_LOG("mpu6050_init_client OK!\n");
1265 #ifdef CONFIG_MPU6050_LOWPASS
1266 memset(&obj
->fir
, 0x00, sizeof(obj
->fir
));
1269 return MPU6050_SUCCESS
;
1272 /*----------------------------------------------------------------------------*/
1273 int mpu6050_operate(void* self
, uint32_t command
, void* buff_in
, int size_in
,
1274 void* buff_out
, int size_out
, int* actualout
)
1278 struct mpu6050_i2c_data
*priv
= (struct mpu6050_i2c_data
*)self
;
1279 hwm_sensor_data
* gyro_data
;
1280 char buff
[MPU6050_BUFSIZE
];
1285 if ((buff_in
== NULL
) || (size_in
< sizeof(int)))
1287 GYRO_ERR("Set delay parameter error!\n");
1297 if ((buff_in
== NULL
) || (size_in
< sizeof(int)))
1299 GYRO_ERR("Enable gyroscope parameter error!\n");
1304 value
= *(int *)buff_in
;
1305 if (((value
== 0) && (sensor_power
== false)) ||((value
== 1) && (sensor_power
== true)))
1307 GYRO_LOG("gyroscope device have updated!\n");
1311 err
= MPU6050_SetPowerMode(priv
->client
, !sensor_power
);
1316 case SENSOR_GET_DATA
:
1317 if ((buff_out
== NULL
) || (size_out
< sizeof(hwm_sensor_data
)))
1319 GYRO_ERR("get gyroscope data parameter error!\n");
1324 gyro_data
= (hwm_sensor_data
*)buff_out
;
1325 err
= MPU6050_ReadGyroData(priv
->client
, buff
, MPU6050_BUFSIZE
);
1328 sscanf(buff
, "%x %x %x", &gyro_data
->values
[0],
1329 &gyro_data
->values
[1], &gyro_data
->values
[2]);
1330 gyro_data
->status
= SENSOR_STATUS_ACCURACY_MEDIUM
;
1331 gyro_data
->value_divide
= DEGREE_TO_RAD
;
1336 GYRO_ERR("gyroscope operate function no this parameter %d!\n", command
);
1344 /******************************************************************************
1345 * Function Configuration
1346 ******************************************************************************/
1347 static int mpu6050_open(struct inode
*inode
, struct file
*file
)
1349 file
->private_data
= mpu6050_i2c_client
;
1351 if (file
->private_data
== NULL
)
1353 GYRO_ERR("null pointer!!\n");
1356 return nonseekable_open(inode
, file
);
1358 /*----------------------------------------------------------------------------*/
1359 static int mpu6050_release(struct inode
*inode
, struct file
*file
)
1361 file
->private_data
= NULL
;
1364 /*----------------------------------------------------------------------------*/
1365 static long mpu6050_unlocked_ioctl(struct file
*file
, unsigned int cmd
,
1368 struct i2c_client
*client
= (struct i2c_client
*)file
->private_data
;
1369 char strbuf
[MPU6050_BUFSIZE
] = {0};
1374 SENSOR_DATA sensor_data
;
1378 if (_IOC_DIR(cmd
) & _IOC_READ
)
1380 err
= !access_ok(VERIFY_WRITE
, (void __user
*)arg
, _IOC_SIZE(cmd
));
1382 else if (_IOC_DIR(cmd
) & _IOC_WRITE
)
1384 err
= !access_ok(VERIFY_READ
, (void __user
*)arg
, _IOC_SIZE(cmd
));
1389 GYRO_ERR("access error: %08X, (%2d, %2d)\n", cmd
, _IOC_DIR(cmd
), _IOC_SIZE(cmd
));
1395 case GYROSCOPE_IOCTL_INIT
:
1396 mpu6050_init_client(client
, false);
1399 case GYROSCOPE_IOCTL_SMT_DATA
:
1400 data
= (void __user
*) arg
;
1407 SMTdata
= kzalloc(sizeof(*SMTdata
) * 800, GFP_KERNEL
);
1408 if (SMTdata
== NULL
)
1413 memset(SMTdata
, 0, sizeof(*SMTdata
) * 800);
1414 MPU6050_SMTReadSensorData(client
, SMTdata
, 800);
1416 GYRO_LOG("gyroscope read data from kernel OK: SMTdata[0]:%d, copied packet:%d!\n", SMTdata
[0],
1417 ((SMTdata
[0]*MPU6050_AXES_NUM
+2)*sizeof(s16
)+1));
1419 smtRes
= MPU6050_PROCESS_SMT_DATA(client
,SMTdata
);
1420 GYRO_LOG("ioctl smtRes: %d!\n", smtRes
);
1421 copy_cnt
= copy_to_user(data
, &smtRes
, sizeof(smtRes
));
1426 GYRO_ERR("copy gyro data to user failed!\n");
1428 GYRO_LOG("copy gyro data to user OK: %d!\n", copy_cnt
);
1431 case GYROSCOPE_IOCTL_READ_SENSORDATA
:
1432 data
= (void __user
*) arg
;
1439 MPU6050_ReadGyroData(client
, strbuf
, MPU6050_BUFSIZE
);
1440 if (copy_to_user(data
, strbuf
, sizeof(strbuf
)))
1447 case GYROSCOPE_IOCTL_SET_CALI
:
1448 data
= (void __user
*)arg
;
1454 if (copy_from_user(&sensor_data
, data
, sizeof(sensor_data
)))
1462 cali
[MPU6050_AXIS_X
] = sensor_data
.x
* MPU6050_DEFAULT_LSB
/ MPU6050_FS_MAX_LSB
;
1463 cali
[MPU6050_AXIS_Y
] = sensor_data
.y
* MPU6050_DEFAULT_LSB
/ MPU6050_FS_MAX_LSB
;
1464 cali
[MPU6050_AXIS_Z
] = sensor_data
.z
* MPU6050_DEFAULT_LSB
/ MPU6050_FS_MAX_LSB
;
1465 GYRO_LOG("gyro set cali:[%5d %5d %5d]\n",
1466 cali
[MPU6050_AXIS_X
], cali
[MPU6050_AXIS_Y
], cali
[MPU6050_AXIS_Z
]);
1467 err
= MPU6050_WriteCalibration(client
, cali
);
1471 case GYROSCOPE_IOCTL_CLR_CALI
:
1472 err
= MPU6050_ResetCalibration(client
);
1475 case GYROSCOPE_IOCTL_GET_CALI
:
1476 data
= (void __user
*)arg
;
1482 err
= MPU6050_ReadCalibration(client
, cali
);
1488 sensor_data
.x
= cali
[MPU6050_AXIS_X
] * MPU6050_FS_MAX_LSB
/ MPU6050_DEFAULT_LSB
;
1489 sensor_data
.y
= cali
[MPU6050_AXIS_Y
] * MPU6050_FS_MAX_LSB
/ MPU6050_DEFAULT_LSB
;
1490 sensor_data
.z
= cali
[MPU6050_AXIS_Z
] * MPU6050_FS_MAX_LSB
/ MPU6050_DEFAULT_LSB
;
1491 if (copy_to_user(data
, &sensor_data
, sizeof(sensor_data
)))
1498 #if INV_GYRO_AUTO_CALI==1
1499 case GYROSCOPE_IOCTL_READ_SENSORDATA_RAW
:
1500 data
= (void __user
*) arg
;
1507 MPU6050_ReadGyroDataRaw(client
, strbuf
, MPU6050_BUFSIZE
);
1508 if (copy_to_user(data
, strbuf
, sizeof(strbuf
)))
1515 case GYROSCOPE_IOCTL_READ_TEMPERATURE
:
1516 data
= (void __user
*) arg
;
1523 MPU6050_ReadTemperature(client
, strbuf
, MPU6050_BUFSIZE
);
1524 if (copy_to_user(data
, strbuf
, sizeof(strbuf
)))
1531 case GYROSCOPE_IOCTL_GET_POWER_STATUS
:
1532 data
= (void __user
*) arg
;
1539 MPU6050_ReadPowerStatus(client
, strbuf
, MPU6050_BUFSIZE
);
1540 if (copy_to_user(data
, strbuf
, sizeof(strbuf
)))
1549 GYRO_ERR("unknown IOCTL: 0x%08x\n", cmd
);
1557 /*----------------------------------------------------------------------------*/
1558 static struct file_operations mpu6050_fops
= {
1559 .open
= mpu6050_open
,
1560 .release
= mpu6050_release
,
1561 .unlocked_ioctl
= mpu6050_unlocked_ioctl
,
1563 /*----------------------------------------------------------------------------*/
1564 static struct miscdevice mpu6050_device
= {
1565 .minor
= MISC_DYNAMIC_MINOR
,
1566 .name
= "gyroscope",
1567 .fops
= &mpu6050_fops
,
1569 /*----------------------------------------------------------------------------*/
1570 #ifndef CONFIG_HAS_EARLYSUSPEND
1571 /*----------------------------------------------------------------------------*/
1572 static int mpu6050_suspend(struct i2c_client
*client
, pm_message_t msg
)
1574 struct mpu6050_i2c_data
*obj
= i2c_get_clientdata(client
);
1577 if (msg
.event
== PM_EVENT_SUSPEND
)
1581 GYRO_ERR("null pointer!!\n");
1584 atomic_set(&obj
->suspend
, 1);
1586 err
= MPU6050_SetPowerMode(client
, false);
1594 /*----------------------------------------------------------------------------*/
1595 static int mpu6050_resume(struct i2c_client
*client
)
1597 struct mpu6050_i2c_data
*obj
= i2c_get_clientdata(client
);
1603 GYRO_ERR("null pointer!!\n");
1607 MPU6050_power(obj
->hw
, 1);
1608 err
= mpu6050_init_client(client
, false);
1611 GYRO_ERR("initialize client fail!!\n");
1614 atomic_set(&obj
->suspend
, 0);
1618 /*----------------------------------------------------------------------------*/
1619 #else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
1620 /*----------------------------------------------------------------------------*/
1621 static void mpu6050_early_suspend(struct early_suspend
*h
)
1623 struct mpu6050_i2c_data
*obj
= container_of(h
, struct mpu6050_i2c_data
, early_drv
);
1630 GYRO_ERR("null pointer!!\n");
1633 atomic_set(&obj
->suspend
, 1);
1634 err
= MPU6050_SetPowerMode(obj
->client
, false);
1637 GYRO_ERR("write power control fail!!\n");
1641 sensor_power
= false;
1643 MPU6050_power(obj
->hw
, 0);
1645 /*----------------------------------------------------------------------------*/
1646 static void mpu6050_late_resume(struct early_suspend
*h
)
1648 struct mpu6050_i2c_data
*obj
= container_of(h
, struct mpu6050_i2c_data
, early_drv
);
1654 GYRO_ERR("null pointer!!\n");
1658 MPU6050_power(obj
->hw
, 1);
1659 err
= mpu6050_init_client(obj
->client
, false);
1662 GYRO_ERR("initialize client fail! err code %d!\n", err
);
1665 atomic_set(&obj
->suspend
, 0);
1667 /*----------------------------------------------------------------------------*/
1668 #endif /*CONFIG_HAS_EARLYSUSPEND*/
1669 /*----------------------------------------------------------------------------*/
1670 static int mpu6050_i2c_detect(struct i2c_client
*client
, struct i2c_board_info
*info
)
1672 strcpy(info
->type
, MPU6050_DEV_NAME
);
1676 /*----------------------------------------------------------------------------*/
1677 static int mpu6050_i2c_probe(struct i2c_client
*client
, const struct i2c_device_id
*id
)
1679 struct i2c_client
*new_client
;
1680 struct mpu6050_i2c_data
*obj
;
1681 struct hwmsen_object sobj
;
1685 if (!(obj
= kzalloc(sizeof(*obj
), GFP_KERNEL
)))
1691 memset(obj
, 0, sizeof(struct mpu6050_i2c_data
));
1693 obj
->hw
= get_cust_gyro_hw();
1694 err
= hwmsen_get_convert(obj
->hw
->direction
, &obj
->cvt
);
1697 GYRO_ERR("invalid direction: %d\n", obj
->hw
->direction
);
1702 GYRO_LOG("gyro_default_i2c_addr: %x\n", client
->addr
);
1703 #ifdef MPU6050_ACCESS_BY_GSE_I2C
1704 obj
->hw
->addr
= MPU6050_I2C_SLAVE_ADDR
; /* mtk i2c not allow to probe two same address */
1707 GYRO_LOG("gyro_custom_i2c_addr: %x\n", obj
->hw
->addr
);
1708 if (0!=obj
->hw
->addr
)
1710 client
->addr
= obj
->hw
->addr
>> 1;
1711 GYRO_LOG("gyro_use_i2c_addr: %x\n", client
->addr
);
1715 obj
->client
= client
;
1716 new_client
= obj
->client
;
1717 i2c_set_clientdata(new_client
,obj
);
1719 atomic_set(&obj
->trace
, 0);
1720 atomic_set(&obj
->suspend
, 0);
1724 mpu6050_i2c_client
= new_client
;
1725 err
= mpu6050_init_client(new_client
, false);
1728 goto exit_init_failed
;
1732 err
= misc_register(&mpu6050_device
);
1735 GYRO_ERR("mpu6050_device misc register failed!\n");
1736 goto exit_misc_device_register_failed
;
1739 err
= mpu6050_create_attr(&mpu6050_gyro_driver
.driver
);
1742 GYRO_ERR("mpu6050 create attribute err = %d\n", err
);
1743 goto exit_create_attr_failed
;
1749 sobj
.sensor_operate
= mpu6050_operate
;
1750 err
= hwmsen_attach(ID_GYROSCOPE
, &sobj
);
1753 GYRO_ERR("hwmsen_attach fail = %d\n", err
);
1758 #ifdef CONFIG_HAS_EARLYSUSPEND
1759 obj
->early_drv
.level
= EARLY_SUSPEND_LEVEL_STOP_DRAWING
- 2,
1760 obj
->early_drv
.suspend
= mpu6050_early_suspend
,
1761 obj
->early_drv
.resume
= mpu6050_late_resume
,
1762 register_early_suspend(&obj
->early_drv
);
1765 #if INV_GYRO_AUTO_CALI==1
1766 mutex_init(&obj
->temperature_mutex
);
1767 mutex_init(&obj
->raw_data_mutex
);
1770 GYRO_LOG("%s: OK\n", __func__
);
1773 exit_create_attr_failed
:
1774 misc_deregister(&mpu6050_device
);
1775 exit_misc_device_register_failed
:
1777 //i2c_detach_client(new_client);
1781 GYRO_ERR("%s: err = %d\n", __func__
, err
);
1785 /*----------------------------------------------------------------------------*/
1786 static int mpu6050_i2c_remove(struct i2c_client
*client
)
1790 err
= mpu6050_delete_attr(&mpu6050_gyro_driver
.driver
);
1793 GYRO_ERR("mpu6050_delete_attr fail: %d\n", err
);
1796 err
= misc_deregister(&mpu6050_device
);
1799 GYRO_ERR("misc_deregister fail: %d\n", err
);
1802 err
= hwmsen_detach(ID_ACCELEROMETER
);
1805 GYRO_ERR("hwmsen_detach fail: %d\n", err
);
1808 mpu6050_i2c_client
= NULL
;
1809 i2c_unregister_device(client
);
1810 kfree(i2c_get_clientdata(client
));
1813 /*----------------------------------------------------------------------------*/
1814 static int mpu6050_probe(struct platform_device
*pdev
)
1816 struct gyro_hw
*hw
= get_cust_gyro_hw();
1818 MPU6050_power(hw
, 1);
1819 if (i2c_add_driver(&mpu6050_i2c_driver
))
1821 GYRO_ERR("add driver error\n");
1826 /*----------------------------------------------------------------------------*/
1827 static int mpu6050_remove(struct platform_device
*pdev
)
1829 struct gyro_hw
*hw
= get_cust_gyro_hw();
1831 MPU6050_power(hw
, 0);
1832 i2c_del_driver(&mpu6050_i2c_driver
);
1835 /*----------------------------------------------------------------------------*/
1836 static struct platform_driver mpu6050_gyro_driver
= {
1837 .probe
= mpu6050_probe
,
1838 .remove
= mpu6050_remove
,
1840 .name
= "gyroscope",
1844 /*----------------------------------------------------------------------------*/
1845 static int __init
mpu6050_init(void)
1847 struct gyro_hw
*hw
= get_cust_gyro_hw();
1848 GYRO_LOG("%s: i2c_number=%d\n", __func__
,hw
->i2c_num
);
1849 i2c_register_board_info(hw
->i2c_num
, &i2c_mpu6050
, 1);
1850 if (platform_driver_register(&mpu6050_gyro_driver
))
1852 GYRO_ERR("failed to register driver");
1857 /*----------------------------------------------------------------------------*/
1858 static void __exit
mpu6050_exit(void)
1861 platform_driver_unregister(&mpu6050_gyro_driver
);
1863 /*----------------------------------------------------------------------------*/
1864 module_init(mpu6050_init
);
1865 module_exit(mpu6050_exit
);
1866 /*----------------------------------------------------------------------------*/
1867 MODULE_LICENSE("GPL");
1868 MODULE_DESCRIPTION("MPU6050 gyroscope driver");
1869 MODULE_AUTHOR("Yucong.Xiong@mediatek.com");