projects / GitHub / mt8127 / android_kernel_alcatel_ttab.git / blob
? search:
summary | shortlog | log | commit | commitdiff | tree
blame | history | raw | HEAD
import PULS_20160108
[GitHub/mt8127/android_kernel_alcatel_ttab.git] / arch / arm / mach-mt8127 / kr076_nand / accelerometer / mpu6050.c
1 /* MPU6050 motion sensor driver
2 *
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.
6 *
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.
11 *
12 */
13
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>
26 #include <asm/atomic.h>
27
28 #include <cust_acc.h>
29 #include <linux/hwmsensor.h>
30 #include <linux/hwmsen_dev.h>
31 #include <linux/sensors_io.h>
32 #include "mpu6050.h"
33 #include <linux/hwmsen_helper.h>
34
35 #include <mach/mt_typedefs.h>
36 #include <mach/mt_gpio.h>
37 #include <mach/mt_pm_ldo.h>
38
39 #define POWER_NONE_MACRO MT65XX_POWER_NONE
40
41 /*----------------------------------------------------------------------------*/
42 #define DEBUG 1
43 /*----------------------------------------------------------------------------*/
44 #define CONFIG_MPU6050_LOWPASS /*apply low pass filter on output*/
45 #define SW_CALIBRATION
46 /*----------------------------------------------------------------------------*/
47 #define MPU6050_AXIS_X 0
48 #define MPU6050_AXIS_Y 1
49 #define MPU6050_AXIS_Z 2
50 #define MPU6050_AXES_NUM 3
51 #define MPU6050_DATA_LEN 6
52 #define MPU6050_DEV_NAME "MPU6050G" /* name must different with gyro mpu6050 */
53 /*----------------------------------------------------------------------------*/
54 static const struct i2c_device_id mpu6050_i2c_id[] = {{MPU6050_DEV_NAME,0},{}};
55 static struct i2c_board_info __initdata i2c_mpu6050={ I2C_BOARD_INFO(MPU6050_DEV_NAME, (MPU6050_I2C_SLAVE_ADDR>>1))};
56
57 /*----------------------------------------------------------------------------*/
58 static int mpu6050_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id);
59 static int mpu6050_i2c_remove(struct i2c_client *client);
60 static int mpu6050_i2c_detect(struct i2c_client *client, struct i2c_board_info *info);
61 #ifndef CONFIG_HAS_EARLYSUSPEND
62 static int mpu6050_suspend(struct i2c_client *client, pm_message_t msg) ;
63 static int mpu6050_resume(struct i2c_client *client);
64 #endif
65 /*----------------------------------------------------------------------------*/
66 typedef enum
67 {
68 MPU6050_TRC_FILTER = 0x01,
69 MPU6050_TRC_RAWDATA = 0x02,
70 MPU6050_TRC_IOCTL = 0x04,
71 MPU6050_TRC_CALI = 0X08,
72 MPU6050_TRC_INFO = 0X10,
73 } MPU6050_TRC;
74 /*----------------------------------------------------------------------------*/
75 struct scale_factor
76 {
77 u8 whole;
78 u8 fraction;
79 };
80 /*----------------------------------------------------------------------------*/
81 struct data_resolution
82 {
83 struct scale_factor scalefactor;
84 int sensitivity;
85 };
86 /*----------------------------------------------------------------------------*/
87 #define C_MAX_FIR_LENGTH (32)
88 /*----------------------------------------------------------------------------*/
89 struct data_filter
90 {
91 s16 raw[C_MAX_FIR_LENGTH][MPU6050_AXES_NUM];
92 int sum[MPU6050_AXES_NUM];
93 int num;
94 int idx;
95 };
96 /*----------------------------------------------------------------------------*/
97 struct mpu6050_i2c_data
98 {
99 struct i2c_client *client;
100 struct acc_hw *hw;
101 struct hwmsen_convert cvt;
102
103 /*misc*/
104 struct data_resolution *reso;
105 atomic_t trace;
106 atomic_t suspend;
107 atomic_t selftest;
108 atomic_t filter;
109 s16 cali_sw[MPU6050_AXES_NUM+1];
110
111 /*data*/
112 s8 offset[MPU6050_AXES_NUM+1]; /*+1: for 4-byte alignment*/
113 s16 data[MPU6050_AXES_NUM+1];
114
115 #if defined(CONFIG_MPU6050_LOWPASS)
116 atomic_t firlen;
117 atomic_t fir_en;
118 struct data_filter fir;
119 #endif
120 /*early suspend*/
121 #if defined(CONFIG_HAS_EARLYSUSPEND)
122 struct early_suspend early_drv;
123 #endif
124 u8 bandwidth;
125 };
126 /*----------------------------------------------------------------------------*/
127 static struct i2c_driver mpu6050_i2c_driver = {
128 .driver = {
129 .name = MPU6050_DEV_NAME,
130 },
131 .probe = mpu6050_i2c_probe,
132 .remove = mpu6050_i2c_remove,
133 .detect = mpu6050_i2c_detect,
134 #if !defined(CONFIG_HAS_EARLYSUSPEND)
135 .suspend = mpu6050_suspend,
136 .resume = mpu6050_resume,
137 #endif
138 .id_table = mpu6050_i2c_id,
139 };
140
141 /*----------------------------------------------------------------------------*/
142 static struct i2c_client *mpu6050_i2c_client = NULL;
143 static struct platform_driver mpu6050_gsensor_driver;
144 static struct mpu6050_i2c_data *obj_i2c_data = NULL;
145 static bool sensor_power = false;
146 static GSENSOR_VECTOR3D gsensor_gain;
147 static char selftestRes[8]= {0};
148
149
150 /*----------------------------------------------------------------------------*/
151 #define GSE_TAG "[Gsensor] "
152 #define GSE_FUN(f) printk(GSE_TAG"%s\n", __FUNCTION__)
153 #define GSE_ERR(fmt, args...) printk(GSE_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
154 #define GSE_LOG(fmt, args...) printk(GSE_TAG fmt, ##args)
155 /*----------------------------------------------------------------------------*/
156 static struct data_resolution mpu6050_data_resolution[] = {
157 /*8 combination by {FULL_RES,RANGE}*/
158 {{ 0, 6}, 16384}, /*+/-2g in 16-bit resolution: 0.06 mg/LSB*/
159 {{ 0, 12}, 8192}, /*+/-4g in 16-bit resolution: 0.12 mg/LSB*/
160 {{ 0, 24}, 4096}, /*+/-8g in 16-bit resolution: 0.24 mg/LSB*/
161 {{ 0, 5}, 2048}, /*+/-16g in 16-bit resolution: 0.49 mg/LSB*/
162 };
163 /*----------------------------------------------------------------------------*/
164 static struct data_resolution mpu6050_offset_resolution = {{ 0, 5}, 2048};
165
166 static unsigned int power_on = 0;
167
168 extern int MPU6050_gyro_power(void);
169 extern int MPU6050_gyro_mode(void);
170
171
172 int MPU6050_gse_power( void)
173 {
174 return(power_on);
175 }
176 EXPORT_SYMBOL(MPU6050_gse_power);
177
178 int MPU6050_gse_mode(void)
179 {
180 return sensor_power;
181 }
182 EXPORT_SYMBOL(MPU6050_gse_mode);
183
184
185 int MPU6050_i2c_master_send(u8 *buf, u8 len)
186 {
187 int res = 0;
188 if (NULL == mpu6050_i2c_client)
189 {
190 GSE_ERR("MPU6050_i2c_master_send null ptr!!\n");
191 }
192 else
193 {
194 res = i2c_master_send(mpu6050_i2c_client, buf, len);
195 }
196
197 return res;
198 }
199 EXPORT_SYMBOL(MPU6050_i2c_master_send);
200
201 int MPU6050_i2c_master_recv(u8 *buf, u8 len)
202 {
203 int res = 0;
204 if (NULL == mpu6050_i2c_client)
205 {
206 GSE_ERR("MPU6050_i2c_master_recv null ptr!!\n");
207 }
208 else
209 {
210 res = i2c_master_recv(mpu6050_i2c_client, buf, len);
211 }
212
213 return res;
214 }
215 EXPORT_SYMBOL(MPU6050_i2c_master_recv);
216 /*----------------------------------------------------------------------------*/
217 static int mpu_i2c_read_block(struct i2c_client *client, u8 addr, u8 *data, u8 len){
218 u8 beg = addr;
219 struct i2c_msg msgs[2] = {
220 {
221 .addr = client->addr, .flags = 0,
222 .len = 1, .buf = &beg
223 },
224 {
225 .addr = client->addr, .flags = I2C_M_RD,
226 .len = len, .buf = data,
227 }
228 };
229 int err;
230
231 if (!client)
232 return -EINVAL;
233 else if (len > C_I2C_FIFO_SIZE) {
234 GSE_ERR(" length %d exceeds %d\n", len, C_I2C_FIFO_SIZE);
235 return -EINVAL;
236 }
237
238 err = i2c_transfer(client->adapter, msgs, sizeof(msgs)/sizeof(msgs[0]));
239 if (err != 2) {
240 GSE_ERR("i2c_transfer error: (%d %p %d) %d\n",
241 addr, data, len, err);
242 err = -EIO;
243 } else {
244 err = 0;
245 }
246 return err;
247
248 }
249 int MPU6050_hwmsen_read_block(u8 addr, u8 *buf, u8 len)
250 {
251 if (NULL == mpu6050_i2c_client)
252 {
253 GSE_ERR("MPU6050_hwmsen_read_block null ptr!!\n");
254 return MPU6050_ERR_I2C;
255 }
256 return mpu_i2c_read_block(mpu6050_i2c_client, addr, buf, len);
257 }
258 EXPORT_SYMBOL(MPU6050_hwmsen_read_block);
259
260
261 int MPU6050_hwmsen_read_byte(u8 addr, u8 *buf)
262 {
263 if (NULL == mpu6050_i2c_client)
264 {
265 GSE_ERR("MPU6050_hwmsen_read_byte null ptr!!\n");
266 return MPU6050_ERR_I2C;
267 }
268 return mpu_i2c_read_block(mpu6050_i2c_client, addr, buf, 1);
269 }
270 EXPORT_SYMBOL(MPU6050_hwmsen_read_byte);
271 /*--------------------mpu6050 power control function----------------------------------*/
272 static void MPU6050_power(struct acc_hw *hw, unsigned int on)
273 {
274
275
276 if (hw->power_id != POWER_NONE_MACRO) // have externel LDO
277 {
278 GSE_LOG("power %s\n", on ? "on" : "off");
279 if (power_on == on) // power status not change
280 {
281 GSE_LOG("ignore power control: %d\n", on);
282 }
283 else if (on) // power on
284 {
285 if (!hwPowerOn(hw->power_id, hw->power_vol, "MPU6050G"))
286 {
287 GSE_ERR("power on fails!!\n");
288 }
289 }
290 else // power off
291 {
292 if (MPU6050_gyro_power() == false)
293 {
294 if (!hwPowerDown(hw->power_id, "MPU6050G"))
295 {
296 GSE_ERR("power off fail!!\n");
297 }
298 }
299 }
300 }
301 power_on = on;
302 }
303
304 /*----------------------------------------------------------------------------*/
305 static int MPU6050_SetPowerMode(struct i2c_client *client, bool enable)
306 {
307 u8 databuf[2];
308 int res = 0;
309 //u8 addr = MPU6050_REG_POWER_CTL;
310 struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
311
312
313 if (enable == sensor_power)
314 {
315 GSE_LOG("Sensor power status is newest!\n");
316 return MPU6050_SUCCESS;
317 }
318
319 databuf[0] = MPU6050_REG_POWER_CTL;
320 res = i2c_master_send(client, databuf, 0x1);
321 if (res <= 0)
322 {
323 return MPU6050_ERR_I2C;
324 }
325
326 udelay(500);
327
328 databuf[0] = 0x0;
329 /*
330 res = i2c_master_recv(client, databuf, 1);
331 if (res <= 0)
332 {
333 return MPU6050_ERR_I2C;
334 }
335 */
336
337 databuf[0] &= ~MPU6050_SLEEP;
338
339 if (enable == FALSE)
340 {
341 if (MPU6050_gyro_mode() == false)
342 {
343 databuf[0] |= MPU6050_SLEEP;
344 }
345 }
346 else
347 {
348 // do nothing
349 }
350 databuf[1] = databuf[0];
351 databuf[0] = MPU6050_REG_POWER_CTL;
352
353 res = i2c_master_send(client, databuf, 0x2);
354
355 if (res <= 0)
356 {
357 GSE_LOG("set power mode failed!\n");
358 return MPU6050_ERR_I2C;
359 }
360 else if (atomic_read(&obj->trace) & MPU6050_TRC_INFO)
361 {
362 GSE_LOG("set power mode ok %d!\n", databuf[1]);
363 }
364
365 msleep(200);
366
367 sensor_power = enable;
368 return MPU6050_SUCCESS;
369 }
370 /*----------------------------------------------------------------------------*/
371 static int MPU6050_SetDataResolution(struct mpu6050_i2c_data *obj)
372 {
373 int err;
374 u8 dat, reso;
375
376 if ((err = mpu_i2c_read_block(obj->client, MPU6050_REG_DATA_FORMAT, &dat, 1)))
377 {
378 GSE_ERR("write data format fail!!\n");
379 return err;
380 }
381
382 /*the data_reso is combined by 3 bits: {FULL_RES, DATA_RANGE}*/
383 reso = 0x00;
384 reso = (dat & MPU6050_RANGE_16G) >> 3;
385
386 if (reso < sizeof(mpu6050_data_resolution)/sizeof(mpu6050_data_resolution[0]))
387 {
388 obj->reso = &mpu6050_data_resolution[reso];
389 return 0;
390 }
391 else
392 {
393 return -EINVAL;
394 }
395 }
396 /*----------------------------------------------------------------------------*/
397 static int MPU6050_ReadData(struct i2c_client *client, s16 data[MPU6050_AXES_NUM])
398 {
399 struct mpu6050_i2c_data *priv = i2c_get_clientdata(client);
400 u8 buf[MPU6050_DATA_LEN] = {0};
401 int err = 0;
402
403
404 if (NULL == client)
405 {
406 return -EINVAL;
407 }
408
409 {
410 /* write then burst read */
411 mpu_i2c_read_block(client, MPU6050_REG_DATAX0, buf, MPU6050_DATA_LEN);
412
413 data[MPU6050_AXIS_X] = (s16)((buf[MPU6050_AXIS_X*2] << 8) |
414 (buf[MPU6050_AXIS_X*2+1] ));
415 data[MPU6050_AXIS_Y] = (s16)((buf[MPU6050_AXIS_Y*2] << 8) |
416 (buf[MPU6050_AXIS_Y*2+1] ));
417 data[MPU6050_AXIS_Z] = (s16)((buf[MPU6050_AXIS_Z*2] << 8) |
418 (buf[MPU6050_AXIS_Z*2+1] ));
419
420 if (atomic_read(&priv->trace) & MPU6050_TRC_RAWDATA)
421 {
422 GSE_LOG("[%08X %08X %08X] => [%5d %5d %5d]\n", data[MPU6050_AXIS_X], data[MPU6050_AXIS_Y], data[MPU6050_AXIS_Z],
423 data[MPU6050_AXIS_X], data[MPU6050_AXIS_Y], data[MPU6050_AXIS_Z]);
424 }
425 #ifdef CONFIG_MPU6050_LOWPASS
426 if (atomic_read(&priv->filter))
427 {
428 if (atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
429 {
430 int idx, firlen = atomic_read(&priv->firlen);
431 if (priv->fir.num < firlen)
432 {
433 priv->fir.raw[priv->fir.num][MPU6050_AXIS_X] = data[MPU6050_AXIS_X];
434 priv->fir.raw[priv->fir.num][MPU6050_AXIS_Y] = data[MPU6050_AXIS_Y];
435 priv->fir.raw[priv->fir.num][MPU6050_AXIS_Z] = data[MPU6050_AXIS_Z];
436 priv->fir.sum[MPU6050_AXIS_X] += data[MPU6050_AXIS_X];
437 priv->fir.sum[MPU6050_AXIS_Y] += data[MPU6050_AXIS_Y];
438 priv->fir.sum[MPU6050_AXIS_Z] += data[MPU6050_AXIS_Z];
439 if (atomic_read(&priv->trace) & MPU6050_TRC_FILTER)
440 {
441 GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
442 priv->fir.raw[priv->fir.num][MPU6050_AXIS_X], priv->fir.raw[priv->fir.num][MPU6050_AXIS_Y], priv->fir.raw[priv->fir.num][MPU6050_AXIS_Z],
443 priv->fir.sum[MPU6050_AXIS_X], priv->fir.sum[MPU6050_AXIS_Y], priv->fir.sum[MPU6050_AXIS_Z]);
444 }
445 priv->fir.num++;
446 priv->fir.idx++;
447 }
448 else
449 {
450 idx = priv->fir.idx % firlen;
451 priv->fir.sum[MPU6050_AXIS_X] -= priv->fir.raw[idx][MPU6050_AXIS_X];
452 priv->fir.sum[MPU6050_AXIS_Y] -= priv->fir.raw[idx][MPU6050_AXIS_Y];
453 priv->fir.sum[MPU6050_AXIS_Z] -= priv->fir.raw[idx][MPU6050_AXIS_Z];
454 priv->fir.raw[idx][MPU6050_AXIS_X] = data[MPU6050_AXIS_X];
455 priv->fir.raw[idx][MPU6050_AXIS_Y] = data[MPU6050_AXIS_Y];
456 priv->fir.raw[idx][MPU6050_AXIS_Z] = data[MPU6050_AXIS_Z];
457 priv->fir.sum[MPU6050_AXIS_X] += data[MPU6050_AXIS_X];
458 priv->fir.sum[MPU6050_AXIS_Y] += data[MPU6050_AXIS_Y];
459 priv->fir.sum[MPU6050_AXIS_Z] += data[MPU6050_AXIS_Z];
460 priv->fir.idx++;
461 data[MPU6050_AXIS_X] = priv->fir.sum[MPU6050_AXIS_X]/firlen;
462 data[MPU6050_AXIS_Y] = priv->fir.sum[MPU6050_AXIS_Y]/firlen;
463 data[MPU6050_AXIS_Z] = priv->fir.sum[MPU6050_AXIS_Z]/firlen;
464 if (atomic_read(&priv->trace) & MPU6050_TRC_FILTER)
465 {
466 GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
467 priv->fir.raw[idx][MPU6050_AXIS_X], priv->fir.raw[idx][MPU6050_AXIS_Y], priv->fir.raw[idx][MPU6050_AXIS_Z],
468 priv->fir.sum[MPU6050_AXIS_X], priv->fir.sum[MPU6050_AXIS_Y], priv->fir.sum[MPU6050_AXIS_Z],
469 data[MPU6050_AXIS_X], data[MPU6050_AXIS_Y], data[MPU6050_AXIS_Z]);
470 }
471 }
472 }
473 }
474 #endif
475 }
476 return err;
477 }
478 /*----------------------------------------------------------------------------*/
479 static int MPU6050_ReadOffset(struct i2c_client *client, s8 ofs[MPU6050_AXES_NUM])
480 {
481 int err = 0;
482 #ifdef SW_CALIBRATION
483 ofs[0]=ofs[1]=ofs[2]=0x0;
484 #else
485 if ((err = mpu_i2c_read_block(client, MPU6050_REG_OFSX, ofs, MPU6050_AXES_NUM)))
486 {
487 GSE_ERR("error: %d\n", err);
488 }
489 #endif
490 //GSE_LOG("offesx=%x, y=%x, z=%x",ofs[0],ofs[1],ofs[2]);
491
492 return err;
493 }
494 /*----------------------------------------------------------------------------*/
495 static int MPU6050_ResetCalibration(struct i2c_client *client)
496 {
497 struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
498 #ifndef SW_CALIBRATION
499 s8 ofs[MPU6050_AXES_NUM] = {0x00, 0x00, 0x00};
500 #endif
501 int err = 0;
502 #ifdef SW_CALIBRATION
503 /* do not thing */
504 #else
505
506 if ((err = hwmsen_write_block(client, MPU6050_REG_OFSX, ofs, MPU6050_AXES_NUM)))
507 {
508 GSE_ERR("error: %d\n", err);
509 }
510 #endif
511
512 memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
513 memset(obj->offset, 0x00, sizeof(obj->offset));
514
515 return err;
516 }
517 /*----------------------------------------------------------------------------*/
518 static int MPU6050_ReadCalibration(struct i2c_client *client, int dat[MPU6050_AXES_NUM])
519 {
520 struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
521 #ifdef SW_CALIBRATION
522 int mul;
523 #else
524 int err;
525 #endif
526 #ifdef SW_CALIBRATION
527 mul = 0;//only SW Calibration, disable HW Calibration
528 #else
529
530 if ((err = MPU6050_ReadOffset(client, obj->offset)))
531 {
532 GSE_ERR("read offset fail, %d\n", err);
533 return err;
534 }
535 mul = obj->reso->sensitivity/mpu6050_offset_resolution.sensitivity;
536 #endif
537
538 dat[obj->cvt.map[MPU6050_AXIS_X]] = obj->cvt.sign[MPU6050_AXIS_X]*(obj->offset[MPU6050_AXIS_X]*mul + obj->cali_sw[MPU6050_AXIS_X]);
539 dat[obj->cvt.map[MPU6050_AXIS_Y]] = obj->cvt.sign[MPU6050_AXIS_Y]*(obj->offset[MPU6050_AXIS_Y]*mul + obj->cali_sw[MPU6050_AXIS_Y]);
540 dat[obj->cvt.map[MPU6050_AXIS_Z]] = obj->cvt.sign[MPU6050_AXIS_Z]*(obj->offset[MPU6050_AXIS_Z]*mul + obj->cali_sw[MPU6050_AXIS_Z]);
541
542 return 0;
543 }
544 /*----------------------------------------------------------------------------*/
545 static int MPU6050_ReadCalibrationEx(struct i2c_client *client, int act[MPU6050_AXES_NUM], int raw[MPU6050_AXES_NUM])
546 {
547 /*raw: the raw calibration data; act: the actual calibration data*/
548 struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
549 #ifdef SW_CALIBRATION
550 int mul;
551 #else
552 int err;
553 #endif
554 #ifdef SW_CALIBRATION
555 mul = 0;//only SW Calibration, disable HW Calibration
556 #else
557
558 if ((err = MPU6050_ReadOffset(client, obj->offset)))
559 {
560 GSE_ERR("read offset fail, %d\n", err);
561 return err;
562 }
563 mul = obj->reso->sensitivity/mpu6050_offset_resolution.sensitivity;
564 #endif
565
566 raw[MPU6050_AXIS_X] = obj->offset[MPU6050_AXIS_X]*mul + obj->cali_sw[MPU6050_AXIS_X];
567 raw[MPU6050_AXIS_Y] = obj->offset[MPU6050_AXIS_Y]*mul + obj->cali_sw[MPU6050_AXIS_Y];
568 raw[MPU6050_AXIS_Z] = obj->offset[MPU6050_AXIS_Z]*mul + obj->cali_sw[MPU6050_AXIS_Z];
569
570 act[obj->cvt.map[MPU6050_AXIS_X]] = obj->cvt.sign[MPU6050_AXIS_X]*raw[MPU6050_AXIS_X];
571 act[obj->cvt.map[MPU6050_AXIS_Y]] = obj->cvt.sign[MPU6050_AXIS_Y]*raw[MPU6050_AXIS_Y];
572 act[obj->cvt.map[MPU6050_AXIS_Z]] = obj->cvt.sign[MPU6050_AXIS_Z]*raw[MPU6050_AXIS_Z];
573
574 return 0;
575 }
576 /*----------------------------------------------------------------------------*/
577 static int MPU6050_WriteCalibration(struct i2c_client *client, int dat[MPU6050_AXES_NUM])
578 {
579 struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
580 int err;
581 int cali[MPU6050_AXES_NUM], raw[MPU6050_AXES_NUM];
582 #ifndef SW_CALIBRATION
583 int lsb = mpu6050_offset_resolution.sensitivity;
584 int divisor = obj->reso->sensitivity/lsb;
585 #endif
586 if ((err = MPU6050_ReadCalibrationEx(client, cali, raw))) /*offset will be updated in obj->offset*/
587 {
588 GSE_ERR("read offset fail, %d\n", err);
589 return err;
590 }
591
592 GSE_LOG("OLDOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n",
593 raw[MPU6050_AXIS_X], raw[MPU6050_AXIS_Y], raw[MPU6050_AXIS_Z],
594 obj->offset[MPU6050_AXIS_X], obj->offset[MPU6050_AXIS_Y], obj->offset[MPU6050_AXIS_Z],
595 obj->cali_sw[MPU6050_AXIS_X], obj->cali_sw[MPU6050_AXIS_Y], obj->cali_sw[MPU6050_AXIS_Z]);
596
597 /*calculate the real offset expected by caller*/
598 cali[MPU6050_AXIS_X] += dat[MPU6050_AXIS_X];
599 cali[MPU6050_AXIS_Y] += dat[MPU6050_AXIS_Y];
600 cali[MPU6050_AXIS_Z] += dat[MPU6050_AXIS_Z];
601
602 GSE_LOG("UPDATE: (%+3d %+3d %+3d)\n",
603 dat[MPU6050_AXIS_X], dat[MPU6050_AXIS_Y], dat[MPU6050_AXIS_Z]);
604 #ifdef SW_CALIBRATION
605 obj->cali_sw[MPU6050_AXIS_X] = obj->cvt.sign[MPU6050_AXIS_X]*(cali[obj->cvt.map[MPU6050_AXIS_X]]);
606 obj->cali_sw[MPU6050_AXIS_Y] = obj->cvt.sign[MPU6050_AXIS_Y]*(cali[obj->cvt.map[MPU6050_AXIS_Y]]);
607 obj->cali_sw[MPU6050_AXIS_Z] = obj->cvt.sign[MPU6050_AXIS_Z]*(cali[obj->cvt.map[MPU6050_AXIS_Z]]);
608 #else
609
610 obj->offset[MPU6050_AXIS_X] = (s8)(obj->cvt.sign[MPU6050_AXIS_X]*(cali[obj->cvt.map[MPU6050_AXIS_X]])/(divisor));
611 obj->offset[MPU6050_AXIS_Y] = (s8)(obj->cvt.sign[MPU6050_AXIS_Y]*(cali[obj->cvt.map[MPU6050_AXIS_Y]])/(divisor));
612 obj->offset[MPU6050_AXIS_Z] = (s8)(obj->cvt.sign[MPU6050_AXIS_Z]*(cali[obj->cvt.map[MPU6050_AXIS_Z]])/(divisor));
613
614 /*convert software calibration using standard calibration*/
615 obj->cali_sw[MPU6050_AXIS_X] = obj->cvt.sign[MPU6050_AXIS_X]*(cali[obj->cvt.map[MPU6050_AXIS_X]])%(divisor);
616 obj->cali_sw[MPU6050_AXIS_Y] = obj->cvt.sign[MPU6050_AXIS_Y]*(cali[obj->cvt.map[MPU6050_AXIS_Y]])%(divisor);
617 obj->cali_sw[MPU6050_AXIS_Z] = obj->cvt.sign[MPU6050_AXIS_Z]*(cali[obj->cvt.map[MPU6050_AXIS_Z]])%(divisor);
618
619 GSE_LOG("NEWOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n",
620 obj->offset[MPU6050_AXIS_X]*divisor + obj->cali_sw[MPU6050_AXIS_X],
621 obj->offset[MPU6050_AXIS_Y]*divisor + obj->cali_sw[MPU6050_AXIS_Y],
622 obj->offset[MPU6050_AXIS_Z]*divisor + obj->cali_sw[MPU6050_AXIS_Z],
623 obj->offset[MPU6050_AXIS_X], obj->offset[MPU6050_AXIS_Y], obj->offset[MPU6050_AXIS_Z],
624 obj->cali_sw[MPU6050_AXIS_X], obj->cali_sw[MPU6050_AXIS_Y], obj->cali_sw[MPU6050_AXIS_Z]);
625
626 if ((err = hwmsen_write_block(obj->client, MPU6050_REG_OFSX, obj->offset, MPU6050_AXES_NUM)))
627 {
628 GSE_ERR("write offset fail: %d\n", err);
629 return err;
630 }
631 #endif
632
633 return err;
634 }
635 /*----------------------------------------------------------------------------*/
636 static int MPU6050_CheckDeviceID(struct i2c_client *client)
637 {
638 u8 databuf[10];
639 int res = 0;
640
641 memset(databuf, 0, sizeof(u8)*10);
642 databuf[0] = MPU6050_REG_DEVID;
643
644 res = i2c_master_send(client, databuf, 0x1);
645 if (res <= 0)
646 {
647 goto exit_MPU6050_CheckDeviceID;
648 }
649
650 udelay(500);
651
652 databuf[0] = 0x0;
653 res = i2c_master_recv(client, databuf, 0x01);
654 if (res <= 0)
655 {
656 goto exit_MPU6050_CheckDeviceID;
657 }
658
659 GSE_LOG("MPU6050_CheckDeviceID 0x%x\n", databuf[0]);
660
661 exit_MPU6050_CheckDeviceID:
662 if (res <= 0)
663 {
664 return MPU6050_ERR_I2C;
665 }
666 return MPU6050_SUCCESS;
667 }
668
669 /*----------------------------------------------------------------------------*/
670 static int MPU6050_SetDataFormat(struct i2c_client *client, u8 dataformat)
671 {
672 struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
673 u8 databuf[2];
674 int res = 0;
675
676 memset(databuf, 0, sizeof(u8)*2);
677 databuf[0] = MPU6050_REG_DATA_FORMAT;
678 res = i2c_master_send(client, databuf, 0x1);
679 if (res <= 0)
680 {
681 return MPU6050_ERR_I2C;
682 }
683
684 udelay(500);
685
686 databuf[0] = 0x0;
687 res = i2c_master_recv(client, databuf, 0x01);
688 if (res <= 0)
689 {
690 return MPU6050_ERR_I2C;
691 }
692
693 /* write */
694 databuf[1] = databuf[0] | dataformat;
695 databuf[0] = MPU6050_REG_DATA_FORMAT;
696 res = i2c_master_send(client, databuf, 0x2);
697
698 if (res <= 0)
699 {
700 return MPU6050_ERR_I2C;
701 }
702 return MPU6050_SetDataResolution(obj);
703 }
704 /*----------------------------------------------------------------------------*/
705 static int MPU6050_SetBWRate(struct i2c_client *client, u8 bwrate)
706 {
707 struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
708 u8 databuf[10];
709 int res = 0;
710
711 if( (obj->bandwidth != bwrate) || (atomic_read(&obj->suspend)) )
712 {
713 memset(databuf, 0, sizeof(u8)*10);
714
715 /* read */
716 databuf[0] = MPU6050_REG_BW_RATE;
717 res = i2c_master_send(client, databuf, 0x1);
718 if (res <= 0)
719 {
720 return MPU6050_ERR_I2C;
721 }
722
723 udelay(500);
724
725 databuf[0] = 0x0;
726 res = i2c_master_recv(client, databuf, 0x01);
727 if (res <= 0)
728 {
729 return MPU6050_ERR_I2C;
730 }
731
732
733 /* write */
734 databuf[1] = databuf[0] | bwrate;
735 databuf[0] = MPU6050_REG_BW_RATE;
736
737 res = i2c_master_send(client, databuf, 0x2);
738
739 if (res <= 0)
740 {
741 return MPU6050_ERR_I2C;
742 }
743
744 obj->bandwidth = bwrate;
745 }
746
747 return MPU6050_SUCCESS;
748 }
749
750 /*----------------------------------------------------------------------------*/
751 static int MPU6050_Dev_Reset(struct i2c_client *client)
752 {
753 u8 databuf[10];
754 int res = 0;
755
756 memset(databuf, 0, sizeof(u8)*10);
757
758 /* read */
759 databuf[0] = MPU6050_REG_POWER_CTL;
760 res = i2c_master_send(client, databuf, 0x1);
761 if (res <= 0)
762 {
763 return MPU6050_ERR_I2C;
764 }
765
766 udelay(500);
767
768 databuf[0] = 0x0;
769 res = i2c_master_recv(client, databuf, 0x01);
770 if (res <= 0)
771 {
772 return MPU6050_ERR_I2C;
773 }
774
775
776 /* write */
777 databuf[1] = databuf[0] | MPU6050_DEV_RESET;
778 databuf[0] = MPU6050_REG_POWER_CTL;
779
780 res = i2c_master_send(client, databuf, 0x2);
781
782 if (res <= 0)
783 {
784 return MPU6050_ERR_I2C;
785 }
786
787 do
788 {
789 databuf[0] = MPU6050_REG_POWER_CTL;
790 res = i2c_master_send(client, databuf, 0x1);
791
792 udelay(500);
793
794 databuf[0] = 0x0;
795 res = i2c_master_recv(client, databuf, 0x01);
796
797 printk("[Gsensor] check reset bit");
798
799 }while((databuf[0]&MPU6050_DEV_RESET) != 0);
800
801 msleep(50);
802 return MPU6050_SUCCESS;
803 }
804
805
806 /*----------------------------------------------------------------------------*/
807 static int MPU6050_Reset(struct i2c_client *client)
808 {
809 u8 databuf[10];
810 int res = 0;
811
812 /* write */
813 databuf[1] = 0x7; /* reset gyro, g-sensor, temperature */
814 databuf[0] = MPU6050_REG_RESET;
815
816 res = i2c_master_send(client, databuf, 0x2);
817
818 if (res <= 0)
819 {
820 return MPU6050_ERR_I2C;
821 }
822
823 msleep(20);
824 return MPU6050_SUCCESS;
825 }
826
827
828 /*----------------------------------------------------------------------------*/
829 static int MPU6050_SetIntEnable(struct i2c_client *client, u8 intenable)
830 {
831 u8 databuf[2];
832 int res = 0;
833
834 memset(databuf, 0, sizeof(u8)*2);
835 databuf[0] = MPU6050_REG_INT_ENABLE;
836 databuf[1] = intenable;
837
838 res = i2c_master_send(client, databuf, 0x2);
839
840 if (res <= 0)
841 {
842 return MPU6050_ERR_I2C;
843 }
844
845 return MPU6050_SUCCESS;
846 }
847 /*----------------------------------------------------------------------------*/
848 static int mpu6050_gpio_config(void)
849 {
850 //because we donot use EINT to support low power
851 // config to GPIO input mode + PD
852
853 //set to GPIO_GSE_1_EINT_PIN
854 /*
855 mt_set_gpio_mode(GPIO_GSE_1_EINT_PIN, GPIO_GSE_1_EINT_PIN_M_GPIO);
856 mt_set_gpio_dir(GPIO_GSE_1_EINT_PIN, GPIO_DIR_IN);
857 mt_set_gpio_pull_enable(GPIO_GSE_1_EINT_PIN, GPIO_PULL_ENABLE);
858 mt_set_gpio_pull_select(GPIO_GSE_1_EINT_PIN, GPIO_PULL_DOWN);
859 */
860 //set to GPIO_GSE_2_EINT_PIN
861 /*
862 mt_set_gpio_mode(GPIO_GSE_2_EINT_PIN, GPIO_GSE_2_EINT_PIN_M_GPIO);
863 mt_set_gpio_dir(GPIO_GSE_2_EINT_PIN, GPIO_DIR_IN);
864 mt_set_gpio_pull_enable(GPIO_GSE_2_EINT_PIN, GPIO_PULL_ENABLE);
865 mt_set_gpio_pull_select(GPIO_GSE_2_EINT_PIN, GPIO_PULL_DOWN);
866 */
867 return 0;
868 }
869
870 static int mpu6050_init_client(struct i2c_client *client, int reset_cali)
871 {
872 struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
873 int res = 0;
874 bool sensor_power_org;
875
876 mpu6050_gpio_config();
877
878 sensor_power_org = sensor_power;
879 res = MPU6050_SetPowerMode(client, true);
880 if (res != MPU6050_SUCCESS)
881 {
882 GSE_ERR("set power error\n");
883 return res;
884 }
885 res = MPU6050_CheckDeviceID(client);
886 if (res != MPU6050_SUCCESS)
887 {
888 GSE_ERR("Check ID error\n");
889 return res;
890 }
891
892 res = MPU6050_SetBWRate(client, MPU6050_BW_184HZ);
893 if (res != MPU6050_SUCCESS ) //0x2C->BW=100Hz
894 {
895 GSE_ERR("set power error\n");
896 return res;
897 }
898
899 res = MPU6050_SetDataFormat(client, MPU6050_RANGE_16G);
900 if (res != MPU6050_SUCCESS) //0x2C->BW=100Hz
901 {
902 GSE_ERR("set data format error\n");
903 return res;
904 }
905
906 gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = obj->reso->sensitivity;
907
908 res = MPU6050_SetIntEnable(client, 0x00);//disable INT
909 if (res != MPU6050_SUCCESS)
910 {
911 GSE_ERR("mpu6050_SetIntEnable error\n");
912 return res;
913 }
914
915 if (0 != reset_cali)
916 {
917 /*reset calibration only in power on*/
918 res = MPU6050_ResetCalibration(client);
919 if (res != MPU6050_SUCCESS)
920 {
921 return res;
922 }
923 }
924
925 res = MPU6050_SetPowerMode(client, sensor_power_org);
926 if (res != MPU6050_SUCCESS)
927 {
928 GSE_ERR("set power error\n");
929 return res;
930 }
931
932 #ifdef CONFIG_MPU6050_LOWPASS
933 memset(&obj->fir, 0x00, sizeof(obj->fir));
934 #endif
935 msleep(20);
936 return MPU6050_SUCCESS;
937 }
938 /*----------------------------------------------------------------------------*/
939 static int MPU6050_ReadAllReg(struct i2c_client *client, char *buf, int bufsize)
940 {
941 u8 total_len= 0x5C; //(0x75-0x19);
942
943 u8 addr = 0x19;
944 u8 buff[total_len+1];
945 int err = 0;
946 int i;
947
948
949 if (sensor_power == FALSE)
950 {
951 err = MPU6050_SetPowerMode(client, true);
952 if (err)
953 {
954 GSE_ERR("Power on mpu6050 error %d!\n", err);
955 }
956 }
957
958 mpu_i2c_read_block(client, addr, buff, total_len);
959
960 for ( i=0; i<=total_len; i++)
961 {
962 GSE_LOG("MPU6050 reg=0x%x, data=0x%x \n",(addr+i), buff[i]);
963 }
964
965 return 0;
966 }
967 /*----------------------------------------------------------------------------*/
968 static int MPU6050_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
969 {
970 u8 databuf[10];
971
972 memset(databuf, 0, sizeof(u8)*10);
973
974 if ((NULL == buf)||(bufsize<=30))
975 {
976 return -1;
977 }
978
979 if (NULL == client)
980 {
981 *buf = 0;
982 return -2;
983 }
984
985 sprintf(buf, "MPU6050 Chip");
986 return 0;
987 }
988 /*----------------------------------------------------------------------------*/
989 static int MPU6050_ReadSensorData(struct i2c_client *client, char *buf, int bufsize)
990 {
991 struct mpu6050_i2c_data *obj = obj_i2c_data; //(struct mpu6050_i2c_data*)i2c_get_clientdata(client);
992 int acc[MPU6050_AXES_NUM];
993 int res = 0;
994 client = obj->client;
995
996 if (atomic_read(&obj->suspend))
997 {
998 return -3;
999 }
1000
1001 if (NULL == buf)
1002 {
1003 return -1;
1004 }
1005 if (NULL == client)
1006 {
1007 *buf = 0;
1008 return -2;
1009 }
1010
1011 if (sensor_power == FALSE)
1012 {
1013 res = MPU6050_SetPowerMode(client, true);
1014 if (res)
1015 {
1016 GSE_ERR("Power on mpu6050 error %d!\n", res);
1017 }
1018 }
1019
1020 if ((res = MPU6050_ReadData(client, obj->data)))
1021 {
1022 GSE_ERR("I2C error: ret value=%d", res);
1023 return -3;
1024 }
1025 else
1026 {
1027 obj->data[MPU6050_AXIS_X] += obj->cali_sw[MPU6050_AXIS_X];
1028 obj->data[MPU6050_AXIS_Y] += obj->cali_sw[MPU6050_AXIS_Y];
1029 obj->data[MPU6050_AXIS_Z] += obj->cali_sw[MPU6050_AXIS_Z];
1030
1031 /*remap coordinate*/
1032 acc[obj->cvt.map[MPU6050_AXIS_X]] = obj->cvt.sign[MPU6050_AXIS_X]*obj->data[MPU6050_AXIS_X];
1033 acc[obj->cvt.map[MPU6050_AXIS_Y]] = obj->cvt.sign[MPU6050_AXIS_Y]*obj->data[MPU6050_AXIS_Y];
1034 acc[obj->cvt.map[MPU6050_AXIS_Z]] = obj->cvt.sign[MPU6050_AXIS_Z]*obj->data[MPU6050_AXIS_Z];
1035
1036 //Out put the mg
1037 acc[MPU6050_AXIS_X] = acc[MPU6050_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
1038 acc[MPU6050_AXIS_Y] = acc[MPU6050_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
1039 acc[MPU6050_AXIS_Z] = acc[MPU6050_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
1040
1041 sprintf(buf, "%04x %04x %04x", acc[MPU6050_AXIS_X], acc[MPU6050_AXIS_Y], acc[MPU6050_AXIS_Z]);
1042 if (atomic_read(&obj->trace) & MPU6050_TRC_IOCTL)
1043 {
1044 GSE_LOG("gsensor data: %s!\n", buf);
1045 }
1046 }
1047
1048 return 0;
1049 }
1050 /*----------------------------------------------------------------------------*/
1051 static int MPU6050_ReadRawData(struct i2c_client *client, char *buf)
1052 {
1053 struct mpu6050_i2c_data *obj = (struct mpu6050_i2c_data*)i2c_get_clientdata(client);
1054 int res = 0;
1055
1056 if (!buf || !client)
1057 {
1058 return EINVAL;
1059 }
1060
1061
1062 if (atomic_read(&obj->suspend))
1063 {
1064 return EIO;
1065 }
1066
1067 if ((res = MPU6050_ReadData(client, obj->data)))
1068 {
1069 GSE_ERR("I2C error: ret value=%d", res);
1070 return EIO;
1071 }
1072 else
1073 {
1074 sprintf(buf, "%04x %04x %04x", obj->data[MPU6050_AXIS_X],
1075 obj->data[MPU6050_AXIS_Y], obj->data[MPU6050_AXIS_Z]);
1076
1077 }
1078
1079 return 0;
1080 }
1081 /*----------------------------------------------------------------------------*/
1082 static int MPU6050_InitSelfTest(struct i2c_client *client)
1083 {
1084 int res = 0;
1085 u8 data;
1086
1087 res = MPU6050_SetPowerMode(client, true);
1088 if (res != MPU6050_SUCCESS)
1089 {
1090 GSE_ERR("set power error\n");
1091 return res;
1092 }
1093
1094 res = MPU6050_SetBWRate(client, MPU6050_BW_184HZ);
1095 if (res != MPU6050_SUCCESS ) //0x2C->BW=100Hz
1096 {
1097 return res;
1098 }
1099
1100 res = mpu_i2c_read_block(client, MPU6050_REG_DATA_FORMAT, &data, 1);
1101
1102 if (res != MPU6050_SUCCESS)
1103 {
1104 return res;
1105 }
1106
1107 return MPU6050_SUCCESS;
1108 }
1109 /*----------------------------------------------------------------------------*/
1110 static int MPU6050_JudgeTestResult(struct i2c_client *client, s32 prv[MPU6050_AXES_NUM], s32 nxt[MPU6050_AXES_NUM])
1111 {
1112 struct criteria
1113 {
1114 int min;
1115 int max;
1116 };
1117
1118 struct criteria self[4][3] = {
1119 {{ 0, 540}, { 0, 540}, { 0, 875}},
1120 {{ 0, 270}, { 0, 270}, { 0, 438}},
1121 {{ 0, 135}, { 0, 135}, { 0, 219}},
1122 {{ 0, 67}, { 0, 67}, { 0, 110}},
1123 };
1124 struct criteria (*ptr)[3] = NULL;
1125 u8 format;
1126 int res;
1127 if ((res = mpu_i2c_read_block(client, MPU6050_REG_DATA_FORMAT, &format, 1)))
1128 return res;
1129
1130 format = format & MPU6050_RANGE_16G;
1131
1132 switch (format)
1133 {
1134 case MPU6050_RANGE_2G:
1135 GSE_LOG("format use self[0]\n");
1136 ptr = &self[0];
1137 break;
1138
1139 case MPU6050_RANGE_4G:
1140 GSE_LOG("format use self[1]\n");
1141 ptr = &self[1];
1142 break;
1143
1144 case MPU6050_RANGE_8G:
1145 GSE_LOG("format use self[2]\n");
1146 ptr = &self[2];
1147 break;
1148
1149 case MPU6050_RANGE_16G:
1150 GSE_LOG("format use self[3]\n");
1151 ptr = &self[3];
1152 break;
1153
1154 default:
1155 GSE_LOG("format unknow use \n");
1156 break;
1157 }
1158
1159 if (!ptr)
1160 {
1161 GSE_ERR("null pointer\n");
1162 return -EINVAL;
1163 }
1164 GSE_LOG("format=0x%x\n",format);
1165
1166 GSE_LOG("X diff is %ld\n",abs(nxt[MPU6050_AXIS_X] - prv[MPU6050_AXIS_X]));
1167 GSE_LOG("Y diff is %ld\n",abs(nxt[MPU6050_AXIS_Y] - prv[MPU6050_AXIS_Y]));
1168 GSE_LOG("Z diff is %ld\n",abs(nxt[MPU6050_AXIS_Z] - prv[MPU6050_AXIS_Z]));
1169
1170
1171 if ((abs(nxt[MPU6050_AXIS_X] - prv[MPU6050_AXIS_X]) > (*ptr)[MPU6050_AXIS_X].max) ||
1172 (abs(nxt[MPU6050_AXIS_X] - prv[MPU6050_AXIS_X]) < (*ptr)[MPU6050_AXIS_X].min))
1173 {
1174 GSE_ERR("X is over range\n");
1175 res = -EINVAL;
1176 }
1177 if ((abs(nxt[MPU6050_AXIS_Y] - prv[MPU6050_AXIS_Y]) > (*ptr)[MPU6050_AXIS_Y].max) ||
1178 (abs(nxt[MPU6050_AXIS_Y] - prv[MPU6050_AXIS_Y]) < (*ptr)[MPU6050_AXIS_Y].min))
1179 {
1180 GSE_ERR("Y is over range\n");
1181 res = -EINVAL;
1182 }
1183 if ((abs(nxt[MPU6050_AXIS_Z] - prv[MPU6050_AXIS_Z]) > (*ptr)[MPU6050_AXIS_Z].max) ||
1184 (abs(nxt[MPU6050_AXIS_Z] - prv[MPU6050_AXIS_Z]) < (*ptr)[MPU6050_AXIS_Z].min))
1185 {
1186 GSE_ERR("Z is over range\n");
1187 res = -EINVAL;
1188 }
1189 return res;
1190 }
1191 /*----------------------------------------------------------------------------*/
1192 static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
1193 {
1194 struct i2c_client *client = mpu6050_i2c_client;
1195 char strbuf[MPU6050_BUFSIZE];
1196 if (NULL == client)
1197 {
1198 GSE_ERR("i2c client is null!!\n");
1199 return 0;
1200 }
1201
1202 if (sensor_power == false)
1203 {
1204 MPU6050_SetPowerMode(client, true);
1205 }
1206
1207 MPU6050_ReadAllReg(client, strbuf, MPU6050_BUFSIZE);
1208
1209 MPU6050_ReadChipInfo(client, strbuf, MPU6050_BUFSIZE);
1210 return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
1211 }
1212 /*----------------------------------------------------------------------------*/
1213 static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
1214 {
1215 struct i2c_client *client = mpu6050_i2c_client;
1216 char strbuf[MPU6050_BUFSIZE];
1217
1218 if (NULL == client)
1219 {
1220 GSE_ERR("i2c client is null!!\n");
1221 return 0;
1222 }
1223 MPU6050_ReadSensorData(client, strbuf, MPU6050_BUFSIZE);
1224 return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
1225 }
1226 /*----------------------------------------------------------------------------*/
1227 static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
1228 {
1229 struct i2c_client *client = mpu6050_i2c_client;
1230 struct mpu6050_i2c_data *obj;
1231 int err, len = 0, mul;
1232 int tmp[MPU6050_AXES_NUM];
1233
1234 if (NULL == client)
1235 {
1236 GSE_ERR("i2c client is null!!\n");
1237 return 0;
1238 }
1239
1240 obj = i2c_get_clientdata(client);
1241
1242
1243 if ((err = MPU6050_ReadOffset(client, obj->offset)))
1244 {
1245 return -EINVAL;
1246 }
1247 else if ((err = MPU6050_ReadCalibration(client, tmp)))
1248 {
1249 return -EINVAL;
1250 }
1251 else
1252 {
1253 mul = obj->reso->sensitivity/mpu6050_offset_resolution.sensitivity;
1254 len += snprintf(buf+len, PAGE_SIZE-len, "[HW ][%d] (%+3d, %+3d, %+3d) : (0x%02X, 0x%02X, 0x%02X)\n", mul,
1255 obj->offset[MPU6050_AXIS_X], obj->offset[MPU6050_AXIS_Y], obj->offset[MPU6050_AXIS_Z],
1256 obj->offset[MPU6050_AXIS_X], obj->offset[MPU6050_AXIS_Y], obj->offset[MPU6050_AXIS_Z]);
1257 len += snprintf(buf+len, PAGE_SIZE-len, "[SW ][%d] (%+3d, %+3d, %+3d)\n", 1,
1258 obj->cali_sw[MPU6050_AXIS_X], obj->cali_sw[MPU6050_AXIS_Y], obj->cali_sw[MPU6050_AXIS_Z]);
1259
1260 len += snprintf(buf+len, PAGE_SIZE-len, "[ALL] (%+3d, %+3d, %+3d) : (%+3d, %+3d, %+3d)\n",
1261 obj->offset[MPU6050_AXIS_X]*mul + obj->cali_sw[MPU6050_AXIS_X],
1262 obj->offset[MPU6050_AXIS_Y]*mul + obj->cali_sw[MPU6050_AXIS_Y],
1263 obj->offset[MPU6050_AXIS_Z]*mul + obj->cali_sw[MPU6050_AXIS_Z],
1264 tmp[MPU6050_AXIS_X], tmp[MPU6050_AXIS_Y], tmp[MPU6050_AXIS_Z]);
1265
1266 return len;
1267 }
1268 }
1269 /*----------------------------------------------------------------------------*/
1270 static ssize_t store_cali_value(struct device_driver *ddri, const char *buf, size_t count)
1271 {
1272 struct i2c_client *client = mpu6050_i2c_client;
1273 int err, x, y, z;
1274 int dat[MPU6050_AXES_NUM];
1275
1276 if (!strncmp(buf, "rst", 3))
1277 {
1278 if ((err = MPU6050_ResetCalibration(client)))
1279 {
1280 GSE_ERR("reset offset err = %d\n", err);
1281 }
1282 }
1283 else if (3 == sscanf(buf, "0x%02X 0x%02X 0x%02X", &x, &y, &z))
1284 {
1285 dat[MPU6050_AXIS_X] = x;
1286 dat[MPU6050_AXIS_Y] = y;
1287 dat[MPU6050_AXIS_Z] = z;
1288 if ((err = MPU6050_WriteCalibration(client, dat)))
1289 {
1290 GSE_ERR("write calibration err = %d\n", err);
1291 }
1292 }
1293 else
1294 {
1295 GSE_ERR("invalid format\n");
1296 }
1297
1298 return count;
1299 }
1300 /*----------------------------------------------------------------------------*/
1301 static ssize_t show_self_value(struct device_driver *ddri, char *buf)
1302 {
1303 struct i2c_client *client = mpu6050_i2c_client;
1304
1305 if (NULL == client)
1306 {
1307 GSE_ERR("i2c client is null!!\n");
1308 return 0;
1309 }
1310
1311 return snprintf(buf, 8, "%s\n", selftestRes);
1312 }
1313 /*----------------------------------------------------------------------------*/
1314 static ssize_t store_self_value(struct device_driver *ddri, const char *buf, size_t count)
1315 { /*write anything to this register will trigger the process*/
1316 struct item
1317 {
1318 s16 raw[MPU6050_AXES_NUM];
1319 };
1320
1321 struct i2c_client *client = mpu6050_i2c_client;
1322 int idx, res, num;
1323 struct item *prv = NULL, *nxt = NULL;
1324 s32 avg_prv[MPU6050_AXES_NUM] = {0, 0, 0};
1325 s32 avg_nxt[MPU6050_AXES_NUM] = {0, 0, 0};
1326
1327
1328 if (1 != sscanf(buf, "%d", &num))
1329 {
1330 GSE_ERR("parse number fail\n");
1331 return count;
1332 }
1333 else if (num == 0)
1334 {
1335 GSE_ERR("invalid data count\n");
1336 return count;
1337 }
1338
1339 prv = kzalloc(sizeof(*prv) * num, GFP_KERNEL);
1340 nxt = kzalloc(sizeof(*nxt) * num, GFP_KERNEL);
1341 if (!prv || !nxt)
1342 {
1343 goto exit;
1344 }
1345
1346
1347 GSE_LOG("NORMAL:\n");
1348 MPU6050_SetPowerMode(client,true);
1349
1350 for (idx = 0; idx < num; idx++)
1351 {
1352 if ((res = MPU6050_ReadData(client, prv[idx].raw)))
1353 {
1354 GSE_ERR("read data fail: %d\n", res);
1355 goto exit;
1356 }
1357
1358 avg_prv[MPU6050_AXIS_X] += prv[idx].raw[MPU6050_AXIS_X];
1359 avg_prv[MPU6050_AXIS_Y] += prv[idx].raw[MPU6050_AXIS_Y];
1360 avg_prv[MPU6050_AXIS_Z] += prv[idx].raw[MPU6050_AXIS_Z];
1361 GSE_LOG("[%5d %5d %5d]\n", prv[idx].raw[MPU6050_AXIS_X], prv[idx].raw[MPU6050_AXIS_Y], prv[idx].raw[MPU6050_AXIS_Z]);
1362 }
1363
1364 avg_prv[MPU6050_AXIS_X] /= num;
1365 avg_prv[MPU6050_AXIS_Y] /= num;
1366 avg_prv[MPU6050_AXIS_Z] /= num;
1367
1368 /*initial setting for self test*/
1369 GSE_LOG("SELFTEST:\n");
1370 for (idx = 0; idx < num; idx++)
1371 {
1372 if ((res = MPU6050_ReadData(client, nxt[idx].raw)))
1373 {
1374 GSE_ERR("read data fail: %d\n", res);
1375 goto exit;
1376 }
1377 avg_nxt[MPU6050_AXIS_X] += nxt[idx].raw[MPU6050_AXIS_X];
1378 avg_nxt[MPU6050_AXIS_Y] += nxt[idx].raw[MPU6050_AXIS_Y];
1379 avg_nxt[MPU6050_AXIS_Z] += nxt[idx].raw[MPU6050_AXIS_Z];
1380 GSE_LOG("[%5d %5d %5d]\n", nxt[idx].raw[MPU6050_AXIS_X], nxt[idx].raw[MPU6050_AXIS_Y], nxt[idx].raw[MPU6050_AXIS_Z]);
1381 }
1382
1383 avg_nxt[MPU6050_AXIS_X] /= num;
1384 avg_nxt[MPU6050_AXIS_Y] /= num;
1385 avg_nxt[MPU6050_AXIS_Z] /= num;
1386
1387 GSE_LOG("X: %5d - %5d = %5d \n", avg_nxt[MPU6050_AXIS_X], avg_prv[MPU6050_AXIS_X], avg_nxt[MPU6050_AXIS_X] - avg_prv[MPU6050_AXIS_X]);
1388 GSE_LOG("Y: %5d - %5d = %5d \n", avg_nxt[MPU6050_AXIS_Y], avg_prv[MPU6050_AXIS_Y], avg_nxt[MPU6050_AXIS_Y] - avg_prv[MPU6050_AXIS_Y]);
1389 GSE_LOG("Z: %5d - %5d = %5d \n", avg_nxt[MPU6050_AXIS_Z], avg_prv[MPU6050_AXIS_Z], avg_nxt[MPU6050_AXIS_Z] - avg_prv[MPU6050_AXIS_Z]);
1390
1391 if (!MPU6050_JudgeTestResult(client, avg_prv, avg_nxt))
1392 {
1393 GSE_LOG("SELFTEST : PASS\n");
1394 strcpy(selftestRes,"y");
1395 }
1396 else
1397 {
1398 GSE_LOG("SELFTEST : FAIL\n");
1399 strcpy(selftestRes,"n");
1400 }
1401
1402 exit:
1403 /*restore the setting*/
1404 mpu6050_init_client(client, 0);
1405 kfree(prv);
1406 kfree(nxt);
1407 return count;
1408 }
1409 /*----------------------------------------------------------------------------*/
1410 static ssize_t show_selftest_value(struct device_driver *ddri, char *buf)
1411 {
1412 struct i2c_client *client = mpu6050_i2c_client;
1413 struct mpu6050_i2c_data *obj;
1414
1415 if (NULL == client)
1416 {
1417 GSE_ERR("i2c client is null!!\n");
1418 return 0;
1419 }
1420
1421 obj = i2c_get_clientdata(client);
1422 return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->selftest));
1423 }
1424 /*----------------------------------------------------------------------------*/
1425 static ssize_t store_selftest_value(struct device_driver *ddri, const char *buf, size_t count)
1426 {
1427 struct mpu6050_i2c_data *obj = obj_i2c_data;
1428 int tmp;
1429
1430 if (NULL == obj)
1431 {
1432 GSE_ERR("i2c data obj is null!!\n");
1433 return 0;
1434 }
1435
1436
1437 if (1 == sscanf(buf, "%d", &tmp))
1438 {
1439 if (atomic_read(&obj->selftest) && !tmp)
1440 {
1441 /*enable -> disable*/
1442 mpu6050_init_client(obj->client, 0);
1443 }
1444 else if (!atomic_read(&obj->selftest) && tmp)
1445 {
1446 /*disable -> enable*/
1447 MPU6050_InitSelfTest(obj->client);
1448 }
1449
1450 GSE_LOG("selftest: %d => %d\n", atomic_read(&obj->selftest), tmp);
1451 atomic_set(&obj->selftest, tmp);
1452 }
1453 else
1454 {
1455 GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
1456 }
1457 return count;
1458 }
1459 /*----------------------------------------------------------------------------*/
1460 static ssize_t show_firlen_value(struct device_driver *ddri, char *buf)
1461 {
1462 #ifdef CONFIG_MPU6050_LOWPASS
1463 struct i2c_client *client = mpu6050_i2c_client;
1464 struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
1465 if (atomic_read(&obj->firlen))
1466 {
1467 int idx, len = atomic_read(&obj->firlen);
1468 GSE_LOG("len = %2d, idx = %2d\n", obj->fir.num, obj->fir.idx);
1469
1470 for (idx = 0; idx < len; idx++)
1471 {
1472 GSE_LOG("[%5d %5d %5d]\n", obj->fir.raw[idx][MPU6050_AXIS_X], obj->fir.raw[idx][MPU6050_AXIS_Y], obj->fir.raw[idx][MPU6050_AXIS_Z]);
1473 }
1474
1475 GSE_LOG("sum = [%5d %5d %5d]\n", obj->fir.sum[MPU6050_AXIS_X], obj->fir.sum[MPU6050_AXIS_Y], obj->fir.sum[MPU6050_AXIS_Z]);
1476 GSE_LOG("avg = [%5d %5d %5d]\n", obj->fir.sum[MPU6050_AXIS_X]/len, obj->fir.sum[MPU6050_AXIS_Y]/len, obj->fir.sum[MPU6050_AXIS_Z]/len);
1477 }
1478 return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->firlen));
1479 #else
1480 return snprintf(buf, PAGE_SIZE, "not support\n");
1481 #endif
1482 }
1483 /*----------------------------------------------------------------------------*/
1484 static ssize_t store_firlen_value(struct device_driver *ddri, const char *buf, size_t count)
1485 {
1486 #ifdef CONFIG_MPU6050_LOWPASS
1487 struct i2c_client *client = mpu6050_i2c_client;
1488 struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
1489 int firlen;
1490
1491 if (1 != sscanf(buf, "%d", &firlen))
1492 {
1493 GSE_ERR("invallid format\n");
1494 }
1495 else if (firlen > C_MAX_FIR_LENGTH)
1496 {
1497 GSE_ERR("exceeds maximum filter length\n");
1498 }
1499 else
1500 {
1501 atomic_set(&obj->firlen, firlen);
1502 if (0 == firlen)
1503 {
1504 atomic_set(&obj->fir_en, 0);
1505 }
1506 else
1507 {
1508 memset(&obj->fir, 0x00, sizeof(obj->fir));
1509 atomic_set(&obj->fir_en, 1);
1510 }
1511 }
1512 #endif
1513 return count;
1514 }
1515 /*----------------------------------------------------------------------------*/
1516 static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
1517 {
1518 ssize_t res;
1519 struct mpu6050_i2c_data *obj = obj_i2c_data;
1520 if (obj == NULL)
1521 {
1522 GSE_ERR("i2c_data obj is null!!\n");
1523 return 0;
1524 }
1525
1526 res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));
1527 return res;
1528 }
1529 /*----------------------------------------------------------------------------*/
1530 static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
1531 {
1532 struct mpu6050_i2c_data *obj = obj_i2c_data;
1533 int trace;
1534 if (obj == NULL)
1535 {
1536 GSE_ERR("i2c_data obj is null!!\n");
1537 return 0;
1538 }
1539
1540 if (1 == sscanf(buf, "0x%x", &trace))
1541 {
1542 atomic_set(&obj->trace, trace);
1543 }
1544 else
1545 {
1546 GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
1547 }
1548
1549 return count;
1550 }
1551 /*----------------------------------------------------------------------------*/
1552 static ssize_t show_status_value(struct device_driver *ddri, char *buf)
1553 {
1554 ssize_t len = 0;
1555 struct mpu6050_i2c_data *obj = obj_i2c_data;
1556 if (obj == NULL)
1557 {
1558 GSE_ERR("i2c_data obj is null!!\n");
1559 return 0;
1560 }
1561
1562 if (obj->hw)
1563 {
1564 len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n",
1565 obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);
1566 }
1567 else
1568 {
1569 len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
1570 }
1571 return len;
1572 }
1573 /*----------------------------------------------------------------------------*/
1574 static DRIVER_ATTR(chipinfo, S_IRUGO, show_chipinfo_value, NULL);
1575 static DRIVER_ATTR(sensordata, S_IRUGO, show_sensordata_value, NULL);
1576 static DRIVER_ATTR(cali, S_IWUSR | S_IRUGO, show_cali_value, store_cali_value);
1577 static DRIVER_ATTR(self, S_IWUSR | S_IRUGO, show_selftest_value, store_selftest_value);
1578 static DRIVER_ATTR(selftest, S_IWUSR | S_IRUGO, show_self_value , store_self_value );
1579 static DRIVER_ATTR(firlen, S_IWUSR | S_IRUGO, show_firlen_value, store_firlen_value);
1580 static DRIVER_ATTR(trace, S_IWUSR | S_IRUGO, show_trace_value, store_trace_value);
1581 static DRIVER_ATTR(status, S_IRUGO, show_status_value, NULL);
1582 /*----------------------------------------------------------------------------*/
1583 static struct driver_attribute *mpu6050_attr_list[] = {
1584 &driver_attr_chipinfo, /*chip information*/
1585 &driver_attr_sensordata, /*dump sensor data*/
1586 &driver_attr_cali, /*show calibration data*/
1587 &driver_attr_self, /*self test demo*/
1588 &driver_attr_selftest, /*self control: 0: disable, 1: enable*/
1589 &driver_attr_firlen, /*filter length: 0: disable, others: enable*/
1590 &driver_attr_trace, /*trace log*/
1591 &driver_attr_status,
1592 };
1593 /*----------------------------------------------------------------------------*/
1594 static int mpu6050_create_attr(struct device_driver *driver)
1595 {
1596 int idx, err = 0;
1597 int num = (int)(sizeof(mpu6050_attr_list)/sizeof(mpu6050_attr_list[0]));
1598 if (driver == NULL)
1599 {
1600 return -EINVAL;
1601 }
1602
1603 for (idx = 0; idx < num; idx++)
1604 {
1605 if (0 != (err = driver_create_file(driver, mpu6050_attr_list[idx])))
1606 {
1607 GSE_ERR("driver_create_file (%s) = %d\n", mpu6050_attr_list[idx]->attr.name, err);
1608 break;
1609 }
1610 }
1611 return err;
1612 }
1613 /*----------------------------------------------------------------------------*/
1614 static int mpu6050_delete_attr(struct device_driver *driver)
1615 {
1616 int idx ,err = 0;
1617 int num = (int)(sizeof(mpu6050_attr_list)/sizeof(mpu6050_attr_list[0]));
1618
1619 if (driver == NULL)
1620 {
1621 return -EINVAL;
1622 }
1623
1624 for (idx = 0; idx < num; idx++)
1625 {
1626 driver_remove_file(driver, mpu6050_attr_list[idx]);
1627 }
1628
1629 return err;
1630 }
1631
1632 /*----------------------------------------------------------------------------*/
1633 int gsensor_operate(void* self, uint32_t command, void* buff_in, int size_in,
1634 void* buff_out, int size_out, int* actualout)
1635 {
1636 int err = 0;
1637 int value, sample_delay;
1638 struct mpu6050_i2c_data *priv = (struct mpu6050_i2c_data*)self;
1639 hwm_sensor_data* gsensor_data;
1640 char buff[MPU6050_BUFSIZE];
1641
1642
1643 switch (command)
1644 {
1645 case SENSOR_DELAY:
1646 if ((buff_in == NULL) || (size_in < sizeof(int)))
1647 {
1648 GSE_ERR("Set delay parameter error!\n");
1649 err = -EINVAL;
1650 }
1651 else
1652 {
1653 value = *(int *)buff_in;
1654
1655 if(value <= 5)
1656 {
1657 sample_delay = MPU6050_BW_184HZ;
1658 }
1659 else if(value <= 10)
1660 {
1661 sample_delay = MPU6050_BW_94HZ;
1662 }
1663 else
1664 {
1665 sample_delay = MPU6050_BW_44HZ;
1666 }
1667 GSE_LOG("Set delay parameter value:%d \n", value);
1668
1669
1670 err = MPU6050_SetBWRate(priv->client, sample_delay);
1671 if (err != MPU6050_SUCCESS ) //0x2C->BW=100Hz
1672 {
1673 GSE_ERR("Set delay parameter error!\n");
1674 }
1675
1676 if (value >= 50)
1677 {
1678 atomic_set(&priv->filter, 0);
1679 }
1680 else
1681 {
1682 #if defined(CONFIG_MPU6050_LOWPASS)
1683 priv->fir.num = 0;
1684 priv->fir.idx = 0;
1685 priv->fir.sum[MPU6050_AXIS_X] = 0;
1686 priv->fir.sum[MPU6050_AXIS_Y] = 0;
1687 priv->fir.sum[MPU6050_AXIS_Z] = 0;
1688 #endif
1689 atomic_set(&priv->filter, 1);
1690 }
1691 }
1692 break;
1693
1694 case SENSOR_ENABLE:
1695 if ((buff_in == NULL) || (size_in < sizeof(int)))
1696 {
1697 GSE_ERR("Enable sensor parameter error!\n");
1698 err = -EINVAL;
1699 }
1700 else
1701 {
1702 value = *(int *)buff_in;
1703 if (((value == 0) && (sensor_power == false)) ||((value == 1) && (sensor_power == true)))
1704 {
1705 GSE_LOG("Gsensor device have updated!\n");
1706 }
1707 else
1708 {
1709 err = MPU6050_SetPowerMode( priv->client, !sensor_power);
1710 }
1711 }
1712 break;
1713
1714 case SENSOR_GET_DATA:
1715 if ((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
1716 {
1717 GSE_ERR("get sensor data parameter error!\n");
1718 err = -EINVAL;
1719 }
1720 else
1721 {
1722 gsensor_data = (hwm_sensor_data *)buff_out;
1723 err = MPU6050_ReadSensorData(priv->client, buff, MPU6050_BUFSIZE);
1724 if (!err)
1725 {
1726 sscanf(buff, "%x %x %x", &gsensor_data->values[0],
1727 &gsensor_data->values[1], &gsensor_data->values[2]);
1728 gsensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;
1729 gsensor_data->value_divide = 1000;
1730 }
1731 }
1732 break;
1733 default:
1734 GSE_ERR("gsensor operate function no this parameter %d!\n", command);
1735 err = -1;
1736 break;
1737 }
1738
1739 return err;
1740 }
1741
1742 /******************************************************************************
1743 * Function Configuration
1744 ******************************************************************************/
1745 static int mpu6050_open(struct inode *inode, struct file *file)
1746 {
1747 file->private_data = mpu6050_i2c_client;
1748
1749 if (file->private_data == NULL)
1750 {
1751 GSE_ERR("null pointer!!\n");
1752 return -EINVAL;
1753 }
1754 return nonseekable_open(inode, file);
1755 }
1756 /*----------------------------------------------------------------------------*/
1757 static int mpu6050_release(struct inode *inode, struct file *file)
1758 {
1759 file->private_data = NULL;
1760 return 0;
1761 }
1762 /*----------------------------------------------------------------------------*/
1763 static long mpu6050_unlocked_ioctl(struct file *file, unsigned int cmd,
1764 unsigned long arg)
1765 {
1766 struct i2c_client *client = (struct i2c_client*)file->private_data;
1767 struct mpu6050_i2c_data *obj = (struct mpu6050_i2c_data*)i2c_get_clientdata(client);
1768 char strbuf[MPU6050_BUFSIZE];
1769 void __user *data;
1770 SENSOR_DATA sensor_data;
1771 long err = 0;
1772 int cali[3];
1773
1774 if (_IOC_DIR(cmd) & _IOC_READ)
1775 {
1776 err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
1777 }
1778 else if (_IOC_DIR(cmd) & _IOC_WRITE)
1779 {
1780 err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
1781 }
1782
1783 if (err)
1784 {
1785 GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
1786 return -EFAULT;
1787 }
1788
1789 switch (cmd)
1790 {
1791 case GSENSOR_IOCTL_INIT:
1792 mpu6050_init_client(client, 0);
1793 break;
1794
1795 case GSENSOR_IOCTL_READ_CHIPINFO:
1796 data = (void __user *) arg;
1797 if (data == NULL)
1798 {
1799 err = -EINVAL;
1800 break;
1801 }
1802
1803 MPU6050_ReadChipInfo(client, strbuf, MPU6050_BUFSIZE);
1804 if (copy_to_user(data, strbuf, strlen(strbuf)+1))
1805 {
1806 err = -EFAULT;
1807 break;
1808 }
1809 break;
1810
1811 case GSENSOR_IOCTL_READ_SENSORDATA:
1812 data = (void __user *) arg;
1813 if (data == NULL)
1814 {
1815 err = -EINVAL;
1816 break;
1817 }
1818
1819 MPU6050_ReadSensorData(client, strbuf, MPU6050_BUFSIZE);
1820 if (copy_to_user(data, strbuf, strlen(strbuf)+1))
1821 {
1822 err = -EFAULT;
1823 break;
1824 }
1825 break;
1826
1827 case GSENSOR_IOCTL_READ_GAIN:
1828 data = (void __user *) arg;
1829 if (data == NULL)
1830 {
1831 err = -EINVAL;
1832 break;
1833 }
1834
1835 if (copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
1836 {
1837 err = -EFAULT;
1838 break;
1839 }
1840 break;
1841
1842 case GSENSOR_IOCTL_READ_RAW_DATA:
1843 data = (void __user *) arg;
1844 if (data == NULL)
1845 {
1846 err = -EINVAL;
1847 break;
1848 }
1849
1850 if (atomic_read(&obj->suspend))
1851 {
1852 err = -EINVAL;
1853 }
1854 else
1855 {
1856 MPU6050_ReadRawData(client, strbuf);
1857 if (copy_to_user(data, strbuf, strlen(strbuf)+1))
1858 {
1859 err = -EFAULT;
1860 break;
1861 }
1862 }
1863 break;
1864
1865 case GSENSOR_IOCTL_SET_CALI:
1866 data = (void __user*)arg;
1867 if (data == NULL)
1868 {
1869 err = -EINVAL;
1870 break;
1871 }
1872 if (copy_from_user(&sensor_data, data, sizeof(sensor_data)))
1873 {
1874 err = -EFAULT;
1875 break;
1876 }
1877 if (atomic_read(&obj->suspend))
1878 {
1879 GSE_ERR("Perform calibration in suspend state!!\n");
1880 err = -EINVAL;
1881 }
1882 else
1883 {
1884 cali[MPU6050_AXIS_X] = sensor_data.x * obj->reso->sensitivity / GRAVITY_EARTH_1000;
1885 cali[MPU6050_AXIS_Y] = sensor_data.y * obj->reso->sensitivity / GRAVITY_EARTH_1000;
1886 cali[MPU6050_AXIS_Z] = sensor_data.z * obj->reso->sensitivity / GRAVITY_EARTH_1000;
1887 err = MPU6050_WriteCalibration(client, cali);
1888 }
1889 break;
1890
1891 case GSENSOR_IOCTL_CLR_CALI:
1892 err = MPU6050_ResetCalibration(client);
1893 break;
1894
1895 case GSENSOR_IOCTL_GET_CALI:
1896 data = (void __user*)arg;
1897 if (data == NULL)
1898 {
1899 err = -EINVAL;
1900 break;
1901 }
1902 if ((err = MPU6050_ReadCalibration(client, cali)))
1903 {
1904 break;
1905 }
1906
1907 sensor_data.x = cali[MPU6050_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
1908 sensor_data.y = cali[MPU6050_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
1909 sensor_data.z = cali[MPU6050_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
1910 if (copy_to_user(data, &sensor_data, sizeof(sensor_data)))
1911 {
1912 err = -EFAULT;
1913 break;
1914 }
1915 break;
1916
1917
1918 default:
1919 GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
1920 err = -ENOIOCTLCMD;
1921 break;
1922
1923 }
1924
1925 return err;
1926 }
1927
1928
1929 /*----------------------------------------------------------------------------*/
1930 static struct file_operations mpu6050_fops = {
1931 .open = mpu6050_open,
1932 .release = mpu6050_release,
1933 .unlocked_ioctl = mpu6050_unlocked_ioctl,
1934 };
1935 /*----------------------------------------------------------------------------*/
1936 static struct miscdevice mpu6050_device = {
1937 .minor = MISC_DYNAMIC_MINOR,
1938 .name = "gsensor",
1939 .fops = &mpu6050_fops,
1940 };
1941 /*----------------------------------------------------------------------------*/
1942 #ifndef CONFIG_HAS_EARLYSUSPEND
1943 /*----------------------------------------------------------------------------*/
1944 static int mpu6050_suspend(struct i2c_client *client, pm_message_t msg)
1945 {
1946 struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
1947 int err = 0;
1948 GSE_FUN();
1949
1950 if (msg.event == PM_EVENT_SUSPEND)
1951 {
1952 if (obj == NULL)
1953 {
1954 GSE_ERR("null pointer!!\n");
1955 return -EINVAL;
1956 }
1957 atomic_set(&obj->suspend, 1);
1958
1959 if ((err = MPU6050_SetPowerMode(obj->client, false)))
1960 {
1961 GSE_ERR("write power control fail!!\n");
1962 return err;
1963 }
1964 MPU6050_power(obj->hw, 0);
1965 GSE_LOG("mpu6050_suspend ok\n");
1966 }
1967 return err;
1968 }
1969 /*----------------------------------------------------------------------------*/
1970 static int mpu6050_resume(struct i2c_client *client)
1971 {
1972 struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
1973 int err;
1974 GSE_FUN();
1975
1976 if (obj == NULL)
1977 {
1978 GSE_ERR("null pointer!!\n");
1979 return -EINVAL;
1980 }
1981
1982 MPU6050_power(obj->hw, 1);
1983
1984 if ((err = mpu6050_init_client(client, 0)))
1985 {
1986 GSE_ERR("initialize client fail!!\n");
1987 return err;
1988 }
1989 atomic_set(&obj->suspend, 0);
1990 GSE_LOG("mpu6050_resume ok\n");
1991
1992 return 0;
1993 }
1994 /*----------------------------------------------------------------------------*/
1995 #else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
1996 /*----------------------------------------------------------------------------*/
1997 static void mpu6050_early_suspend(struct early_suspend *h)
1998 {
1999 struct mpu6050_i2c_data *obj = container_of(h, struct mpu6050_i2c_data, early_drv);
2000 int err;
2001 GSE_FUN();
2002
2003 if (obj == NULL)
2004 {
2005 GSE_ERR("null pointer!!\n");
2006 return;
2007 }
2008 atomic_set(&obj->suspend, 1);
2009
2010 if ((err = MPU6050_SetPowerMode(obj->client, false)))
2011 {
2012 GSE_ERR("write power control fail!!\n");
2013 return;
2014 }
2015
2016 /*
2017 if (MPU6050_gyro_mode() == false)
2018 {
2019 MPU6050_Dev_Reset(obj->client);
2020 MPU6050_Reset(obj->client);
2021 }
2022 */
2023
2024 obj->bandwidth = 0;
2025
2026 sensor_power = false;
2027
2028 MPU6050_power(obj->hw, 0);
2029 }
2030 /*----------------------------------------------------------------------------*/
2031 static void mpu6050_late_resume(struct early_suspend *h)
2032 {
2033 struct mpu6050_i2c_data *obj = container_of(h, struct mpu6050_i2c_data, early_drv);
2034 int err;
2035 GSE_FUN();
2036
2037 if (obj == NULL)
2038 {
2039 GSE_ERR("null pointer!!\n");
2040 return;
2041 }
2042
2043 MPU6050_power(obj->hw, 1);
2044
2045 if ((err = mpu6050_init_client(obj->client, 0)))
2046 {
2047 GSE_ERR("initialize client fail!!\n");
2048 return;
2049 }
2050 atomic_set(&obj->suspend, 0);
2051 }
2052 /*----------------------------------------------------------------------------*/
2053 #endif /*CONFIG_HAS_EARLYSUSPEND*/
2054 /*----------------------------------------------------------------------------*/
2055 static int mpu6050_i2c_detect(struct i2c_client *client, struct i2c_board_info *info)
2056 {
2057 strcpy(info->type, MPU6050_DEV_NAME);
2058 return 0;
2059 }
2060 /*----------------------------------------------------------------------------*/
2061 static int mpu6050_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
2062 {
2063 struct i2c_client *new_client;
2064 struct mpu6050_i2c_data *obj;
2065 struct hwmsen_object sobj;
2066 int err = 0;
2067 GSE_FUN();
2068
2069 if (!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
2070 {
2071 err = -ENOMEM;
2072 goto exit;
2073 }
2074
2075 memset(obj, 0, sizeof(struct mpu6050_i2c_data));
2076
2077 obj->hw = get_cust_acc_hw();
2078
2079 if ((err = hwmsen_get_convert(obj->hw->direction, &obj->cvt)))
2080 {
2081 GSE_ERR("invalid direction: %d\n", obj->hw->direction);
2082 goto exit;
2083 }
2084
2085 obj_i2c_data = obj;
2086 obj->client = client;
2087 obj->client->timing = 400;
2088
2089 new_client = obj->client;
2090 i2c_set_clientdata(new_client,obj);
2091
2092 atomic_set(&obj->trace, 0);
2093 atomic_set(&obj->suspend, 0);
2094
2095 #ifdef CONFIG_MPU6050_LOWPASS
2096 if (obj->hw->firlen > C_MAX_FIR_LENGTH)
2097 {
2098 atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
2099 }
2100 else
2101 {
2102 atomic_set(&obj->firlen, obj->hw->firlen);
2103 }
2104
2105 if (atomic_read(&obj->firlen) > 0)
2106 {
2107 atomic_set(&obj->fir_en, 1);
2108 }
2109
2110 #endif
2111
2112 mpu6050_i2c_client = new_client;
2113 MPU6050_Dev_Reset(new_client);
2114 MPU6050_Reset(new_client);
2115
2116 if ((err = mpu6050_init_client(new_client, 1)))
2117 {
2118 goto exit_init_failed;
2119 }
2120
2121
2122 if ((err = misc_register(&mpu6050_device)))
2123 {
2124 GSE_ERR("mpu6050_device register failed\n");
2125 goto exit_misc_device_register_failed;
2126 }
2127
2128
2129 if ((err = mpu6050_create_attr(&mpu6050_gsensor_driver.driver)))
2130 {
2131 GSE_ERR("create attribute err = %d\n", err);
2132 goto exit_create_attr_failed;
2133 }
2134
2135 sobj.self = obj;
2136 sobj.polling = 1;
2137 sobj.sensor_operate = gsensor_operate;
2138 if ((err = hwmsen_attach(ID_ACCELEROMETER, &sobj)))
2139 {
2140 GSE_ERR("attach fail = %d\n", err);
2141 goto exit_kfree;
2142 }
2143
2144 #ifdef CONFIG_HAS_EARLYSUSPEND
2145 obj->early_drv.level = EARLY_SUSPEND_LEVEL_STOP_DRAWING - 2,
2146 obj->early_drv.suspend = mpu6050_early_suspend,
2147 obj->early_drv.resume = mpu6050_late_resume,
2148 register_early_suspend(&obj->early_drv);
2149 #endif
2150
2151 GSE_LOG("%s: OK\n", __func__);
2152 return 0;
2153
2154 exit_create_attr_failed:
2155 misc_deregister(&mpu6050_device);
2156 exit_misc_device_register_failed:
2157 exit_init_failed:
2158 //i2c_detach_client(new_client);
2159 exit_kfree:
2160 kfree(obj);
2161 exit:
2162 GSE_ERR("%s: err = %d\n", __func__, err);
2163 return err;
2164 }
2165
2166 /*----------------------------------------------------------------------------*/
2167 static int mpu6050_i2c_remove(struct i2c_client *client)
2168 {
2169 int err = 0;
2170
2171 if ((err = mpu6050_delete_attr(&mpu6050_gsensor_driver.driver)))
2172 {
2173 GSE_ERR("mpu6050_delete_attr fail: %d\n", err);
2174 }
2175
2176 if ((err = misc_deregister(&mpu6050_device)))
2177 {
2178 GSE_ERR("misc_deregister fail: %d\n", err);
2179 }
2180
2181 if ((err = hwmsen_detach(ID_ACCELEROMETER)))
2182 {
2183 GSE_ERR("hwmsen_detach fail: %d\n", err);
2184 }
2185
2186 mpu6050_i2c_client = NULL;
2187 i2c_unregister_device(client);
2188 kfree(i2c_get_clientdata(client));
2189 return 0;
2190 }
2191 /*----------------------------------------------------------------------------*/
2192 static int mpu6050_probe(struct platform_device *pdev)
2193 {
2194 struct acc_hw *hw = get_cust_acc_hw();
2195 GSE_FUN();
2196
2197 MPU6050_power(hw, 1);
2198 if (i2c_add_driver(&mpu6050_i2c_driver))
2199 {
2200 GSE_ERR("add driver error\n");
2201 return -1;
2202 }
2203 return 0;
2204 }
2205 /*----------------------------------------------------------------------------*/
2206 static int mpu6050_remove(struct platform_device *pdev)
2207 {
2208 struct acc_hw *hw = get_cust_acc_hw();
2209
2210 GSE_FUN();
2211 MPU6050_power(hw, 0);
2212 i2c_del_driver(&mpu6050_i2c_driver);
2213 return 0;
2214 }
2215 /*----------------------------------------------------------------------------*/
2216 static struct platform_driver mpu6050_gsensor_driver = {
2217 .probe = mpu6050_probe,
2218 .remove = mpu6050_remove,
2219 .driver = {
2220 .name = "gsensor",
2221 }
2222 };
2223
2224 /*----------------------------------------------------------------------------*/
2225 static int __init mpu6050gse_init(void)
2226 {
2227 struct acc_hw *hw = get_cust_acc_hw();
2228 GSE_LOG("%s: i2c_number=%d\n", __func__,hw->i2c_num);
2229 i2c_register_board_info(hw->i2c_num, &i2c_mpu6050, 1);
2230 if (platform_driver_register(&mpu6050_gsensor_driver))
2231 {
2232 GSE_ERR("failed to register driver");
2233 return -ENODEV;
2234 }
2235 return 0;
2236 }
2237 /*----------------------------------------------------------------------------*/
2238 static void __exit mpu6050gse_exit(void)
2239 {
2240 GSE_FUN();
2241 platform_driver_unregister(&mpu6050_gsensor_driver);
2242 }
2243 /*----------------------------------------------------------------------------*/
2244 module_init(mpu6050gse_init);
2245 module_exit(mpu6050gse_exit);
2246 /*----------------------------------------------------------------------------*/
2247 MODULE_LICENSE("GPL");
2248 MODULE_DESCRIPTION("MPU6050 gse driver");
2249 MODULE_AUTHOR("Yucong.Xiong@mediatek.com");
kernel source for telekom puls (alcatel/mediatek)