projects / GitHub / mt8127 / android_kernel_alcatel_ttab.git / blame
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (incremental) | history | HEAD
import PULS_20160108
[GitHub/mt8127/android_kernel_alcatel_ttab.git] / drivers / misc / mediatek / gyroscope / mpu3000-new / mpu3000.c
CommitLineData
6fa3eb70
S		 1/* MPU3000 motion sensor driver
2 *
3 *
4 *
5 * This software is licensed under the terms of the GNU General Public
6 * License version 2, as published by the Free Software Foundation, and
7 * may be copied, distributed, and modified under those terms.
8 *
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
13 *
14 */
15
16#include <linux/interrupt.h>
17#include <linux/i2c.h>
18#include <linux/slab.h>
19#include <linux/irq.h>
20#include <linux/miscdevice.h>
21#include <asm/uaccess.h>
22#include <linux/delay.h>
23#include <linux/input.h>
24#include <linux/workqueue.h>
25#include <linux/kobject.h>
26#include <linux/earlysuspend.h>
27#include <linux/platform_device.h>
28
29#include <cust_gyro.h>
30#include <linux/hwmsensor.h>
31#include <linux/hwmsen_dev.h>
32#include <linux/sensors_io.h>
33#include "mpu3000.h"
34#include <linux/hwmsen_helper.h>
35#include <linux/kernel.h>
36
37#include <mach/mt_typedefs.h>
38#include <mach/mt_gpio.h>
39#include <mach/mt_pm_ldo.h>
40#include <mach/mt_boot.h>
41#include "gyroscope.h"
42
43#define POWER_NONE_MACRO MT65XX_POWER_NONE
44
45/*----------------------------------------------------------------------------*/
46#define I2C_DRIVERID_MPU3000 3000
47/*----------------------------------------------------------------------------*/
48#define MPU3000_DEFAULT_FS MPU3000_FS_1000
49#define MPU3000_DEFAULT_LSB MPU3000_FS_1000_LSB
50/*---------------------------------------------------------------------------*/
51#define DEBUG 0
52/*----------------------------------------------------------------------------*/
53#define CONFIG_MPU3000_LOWPASS /*apply low pass filter on output*/
54/*----------------------------------------------------------------------------*/
55#define MPU3000_AXIS_X 0
56#define MPU3000_AXIS_Y 1
57#define MPU3000_AXIS_Z 2
58#define MPU3000_AXES_NUM 3
59#define MPU3000_DATA_LEN 6
60#define MPU3000_DEV_NAME "MPU3000"
61/*----------------------------------------------------------------------------*/
62static const struct i2c_device_id mpu3000_i2c_id[] = {{MPU3000_DEV_NAME,0},{}};
63static struct i2c_board_info __initdata i2c_mpu3000={ I2C_BOARD_INFO("MPU3000", (0xD0>>1))};
64/*the adapter id will be available in customization*/
65static int mpu3000_init_flag =-1; // 0<==>OK -1 <==> fail
66static int mpu3000_local_init(void);
67static int mpu3000_remove(void);
68
69static struct gyro_init_info mpu3000_init_info = {
70 .name = "MPU3000",
71 .init = mpu3000_local_init,
72 .uninit = mpu3000_remove,
73};
74
75int packet_thresh = 75; // 600 ms / 8ms/sample
76
77/*----------------------------------------------------------------------------*/
78static int mpu3000_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id);
79static int mpu3000_i2c_remove(struct i2c_client *client);
80//static int mpu3000_i2c_detect(struct i2c_client *client, int kind, struct i2c_board_info *info);
81//static int mpu3000_suspend(struct i2c_client *client, pm_message_t msg) ;
82//static int mpu3000_resume(struct i2c_client *client);
83/*----------------------------------------------------------------------------*/
84typedef enum {
85 GYRO_TRC_FILTER = 0x01,
86 GYRO_TRC_RAWDATA = 0x02,
87 GYRO_TRC_IOCTL = 0x04,
88 GYRO_TRC_CALI = 0X08,
89 GYRO_TRC_INFO = 0X10,
90 GYRO_TRC_DATA = 0X20,
91} GYRO_TRC;
92/*----------------------------------------------------------------------------*/
93struct scale_factor{
94 u8 whole;
95 u8 fraction;
96};
97/*----------------------------------------------------------------------------*/
98struct data_resolution {
99 struct scale_factor scalefactor;
100 int sensitivity;
101};
102/*----------------------------------------------------------------------------*/
103#define C_MAX_FIR_LENGTH (32)
104/*----------------------------------------------------------------------------*/
105struct data_filter {
106 s16 raw[C_MAX_FIR_LENGTH][MPU3000_AXES_NUM];
107 int sum[MPU3000_AXES_NUM];
108 int num;
109 int idx;
110};
111/*----------------------------------------------------------------------------*/
112struct mpu3000_i2c_data {
113 struct i2c_client *client;
114 struct gyro_hw *hw;
115 struct hwmsen_convert cvt;
116
117 /*misc*/
118 struct data_resolution *reso;
119 atomic_t trace;
120 atomic_t suspend;
121 atomic_t selftest;
122 atomic_t filter;
123 s16 cali_sw[MPU3000_AXES_NUM+1];
124
125 /*data*/
126 s8 offset[MPU3000_AXES_NUM+1]; /*+1: for 4-byte alignment*/
127 s16 data[MPU3000_AXES_NUM+1];
128
129#if defined(CONFIG_MPU3000_LOWPASS)
130 atomic_t firlen;
131 atomic_t fir_en;
132 struct data_filter fir;
133#endif
134 /*early suspend*/
135#if defined(CONFIG_HAS_EARLYSUSPEND)
136 struct early_suspend early_drv;
137#endif
138};
139/*----------------------------------------------------------------------------*/
140static struct i2c_driver mpu3000_i2c_driver = {
141 .driver = {
142// .owner = THIS_MODULE,//modified
143 .name = MPU3000_DEV_NAME,
144 },
145 .probe = mpu3000_i2c_probe,
146 .remove = mpu3000_i2c_remove,
147// .detect = mpu3000_i2c_detect,
148#if !defined(CONFIG_HAS_EARLYSUSPEND)
149 .suspend = mpu3000_suspend,
150 .resume = mpu3000_resume,
151#endif
152 .id_table = mpu3000_i2c_id,
153 //.address_list = mpu3000_forces,//modified
154};
155
156/*----------------------------------------------------------------------------*/
157static struct i2c_client *mpu3000_i2c_client = NULL;
158static struct platform_driver mpu3000_gyro_driver;
159static struct mpu3000_i2c_data *obj_i2c_data = NULL;
160static bool sensor_power = false;
161
162
163
164/*----------------------------------------------------------------------------*/
165#if 0
166#define GYRO_TAG "[Mpu3000] "
167#define GYRO_FUN(f) printk(KERN_INFO GYRO_TAG"%s\n", __FUNCTION__)
168#define GYRO_ERR(fmt, args...) printk(KERN_ERR GYRO_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
169#define GYRO_LOG(fmt, args...) printk(KERN_ERR GYRO_TAG fmt, ##args)
170#endif
171/*----------------------------------------------------------------------------*/
172/*--------------------gyroscopy power control function----------------------------------*/
173static void MPU3000_power(struct gyro_hw *hw, unsigned int on)
174{
175 static unsigned int power_on = 0;
176
177 if(hw->power_id != POWER_NONE_MACRO) // have externel LDO
178 {
179 GYRO_LOG("power %s\n", on ? "on" : "off");
180 if(power_on == on) // power status not change
181 {
182 GYRO_LOG("ignore power control: %d\n", on);
183 }
184 else if(on) // power on
185 {
186 if(!hwPowerOn(hw->power_id, hw->power_vol, "MPU3000"))
187 {
188 GYRO_ERR("power on fails!!\n");
189 }
190 }
191 else // power off
192 {
193 if (!hwPowerDown(hw->power_id, "MPU3000"))
194 {
195 GYRO_ERR("power off fail!!\n");
196 }
197 }
198 }
199 power_on = on;
200}
201/*----------------------------------------------------------------------------*/
202
203
204/*----------------------------------------------------------------------------*/
205static int MPU3000_write_rel_calibration(struct mpu3000_i2c_data *obj, int dat[MPU3000_AXES_NUM])
206{
207 obj->cali_sw[MPU3000_AXIS_X] = obj->cvt.sign[MPU3000_AXIS_X]*dat[obj->cvt.map[MPU3000_AXIS_X]];
208 obj->cali_sw[MPU3000_AXIS_Y] = obj->cvt.sign[MPU3000_AXIS_Y]*dat[obj->cvt.map[MPU3000_AXIS_Y]];
209 obj->cali_sw[MPU3000_AXIS_Z] = obj->cvt.sign[MPU3000_AXIS_Z]*dat[obj->cvt.map[MPU3000_AXIS_Z]];
210#if DEBUG
211 if(atomic_read(&obj->trace) & GYRO_TRC_CALI)
212 {
213 GYRO_LOG("test (%5d, %5d, %5d) ->(%5d, %5d, %5d)->(%5d, %5d, %5d))\n",
214 obj->cvt.sign[MPU3000_AXIS_X],obj->cvt.sign[MPU3000_AXIS_Y],obj->cvt.sign[MPU3000_AXIS_Z],
215 dat[MPU3000_AXIS_X], dat[MPU3000_AXIS_Y], dat[MPU3000_AXIS_Z],
216 obj->cvt.map[MPU3000_AXIS_X],obj->cvt.map[MPU3000_AXIS_Y],obj->cvt.map[MPU3000_AXIS_Z]);
217 GYRO_LOG("write gyro calibration data (%5d, %5d, %5d)\n",
218 obj->cali_sw[MPU3000_AXIS_X],obj->cali_sw[MPU3000_AXIS_Y],obj->cali_sw[MPU3000_AXIS_Z]);
219 }
220#endif
221 return 0;
222}
223
224
225/*----------------------------------------------------------------------------*/
226static int MPU3000_ResetCalibration(struct i2c_client *client)
227{
228 struct mpu3000_i2c_data *obj = i2c_get_clientdata(client);
229
230 memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
231 return 0;
232}
233/*----------------------------------------------------------------------------*/
234static int MPU3000_ReadCalibration(struct i2c_client *client, int dat[MPU3000_AXES_NUM])
235{
236 struct mpu3000_i2c_data *obj = i2c_get_clientdata(client);
237
238 dat[obj->cvt.map[MPU3000_AXIS_X]] = obj->cvt.sign[MPU3000_AXIS_X]*obj->cali_sw[MPU3000_AXIS_X];
239 dat[obj->cvt.map[MPU3000_AXIS_Y]] = obj->cvt.sign[MPU3000_AXIS_Y]*obj->cali_sw[MPU3000_AXIS_Y];
240 dat[obj->cvt.map[MPU3000_AXIS_Z]] = obj->cvt.sign[MPU3000_AXIS_Z]*obj->cali_sw[MPU3000_AXIS_Z];
241
242#if DEBUG
243 if(atomic_read(&obj->trace) & GYRO_TRC_CALI)
244 {
245 GYRO_LOG("Read gyro calibration data (%5d, %5d, %5d)\n",
246 dat[MPU3000_AXIS_X],dat[MPU3000_AXIS_Y],dat[MPU3000_AXIS_Z]);
247 }
248#endif
249
250 return 0;
251}
252/*----------------------------------------------------------------------------*/
253/*----------------------------------------------------------------------------*/
254static int MPU3000_WriteCalibration(struct i2c_client *client, int dat[MPU3000_AXES_NUM])
255{
256 struct mpu3000_i2c_data *obj = i2c_get_clientdata(client);
257 int err = 0;
258 int cali[MPU3000_AXES_NUM];
259
260
261 GYRO_FUN();
262 if(!obj || ! dat)
263 {
264 GYRO_ERR("null ptr!!\n");
265 return -EINVAL;
266 }
267 else
268 {
269 cali[obj->cvt.map[MPU3000_AXIS_X]] = obj->cvt.sign[MPU3000_AXIS_X]*obj->cali_sw[MPU3000_AXIS_X];
270 cali[obj->cvt.map[MPU3000_AXIS_Y]] = obj->cvt.sign[MPU3000_AXIS_Y]*obj->cali_sw[MPU3000_AXIS_Y];
271 cali[obj->cvt.map[MPU3000_AXIS_Z]] = obj->cvt.sign[MPU3000_AXIS_Z]*obj->cali_sw[MPU3000_AXIS_Z];
272 cali[MPU3000_AXIS_X] += dat[MPU3000_AXIS_X];
273 cali[MPU3000_AXIS_Y] += dat[MPU3000_AXIS_Y];
274 cali[MPU3000_AXIS_Z] += dat[MPU3000_AXIS_Z];
275#if DEBUG
276 if(atomic_read(&obj->trace) & GYRO_TRC_CALI)
277 {
278 GYRO_LOG("write gyro calibration data (%5d, %5d, %5d)-->(%5d, %5d, %5d)\n",
279 dat[MPU3000_AXIS_X], dat[MPU3000_AXIS_Y], dat[MPU3000_AXIS_Z],
280 cali[MPU3000_AXIS_X],cali[MPU3000_AXIS_Y],cali[MPU3000_AXIS_Z]);
281 }
282#endif
283 return MPU3000_write_rel_calibration(obj, cali);
284 }
285
286 return err;
287}
288/*----------------------------------------------------------------------------*/
289
290
291/*----------------------------------------------------------------------------*/
292static int MPU3000_ReadStart(struct i2c_client *client, bool enable)
293{
294 u8 databuf[2] = {0};
295 int res = 0;
296 GYRO_FUN();
297
298 databuf[0] = MPU3000_REG_FIFO_EN;
299
300 if(enable)
301 {
302 //enable xyz gyro in FIFO
303 databuf[1] = (MPU3000_FIFO_GYROX_EN|MPU3000_FIFO_GYROY_EN|MPU3000_FIFO_GYROZ_EN);
304 }
305 else
306 {
307 //disable xyz gyro in FIFO
308 databuf[1] = 0;
309 }
310
311 res = i2c_master_send(client, databuf, 0x2);
312 if(res <= 0)
313 {
314 GYRO_ERR(" enable xyz gyro in FIFO error,enable: 0x%x!\n", databuf[1]);
315 return MPU3000_ERR_I2C;
316 }
317 GYRO_LOG("MPU3000_ReadStart: enable xyz gyro in FIFO: 0x%x\n", databuf[1]);
318 return MPU3000_SUCCESS;
319}
320
321
322
323//----------------------------------------------------------------------------//
324static int MPU3000_SetPowerMode(struct i2c_client *client, bool enable)
325{
326 u8 databuf[2] = {0};
327 int res = 0;
328
329 if(enable == sensor_power)
330 {
331 GYRO_LOG("Sensor power status is newest!\n");
332 return MPU3000_SUCCESS;
333 }
334
335 if(hwmsen_read_byte(client, MPU3000_REG_PWR_CTL, databuf))
336 {
337 GYRO_ERR("read power ctl register err!\n");
338 return MPU3000_ERR_I2C;
339 }
340
341 databuf[0] &= ~MPU3000_SLEEP;
342 if(enable == FALSE)
343 {
344 databuf[0] |= MPU3000_SLEEP;
345 }
346 else
347 {
348 // do nothing
349 }
350
351
352 databuf[1] = databuf[0];
353 databuf[0] = MPU3000_REG_PWR_CTL;
354 res = i2c_master_send(client, databuf, 0x2);
355 if(res <= 0)
356 {
357 GYRO_LOG("set power mode failed!\n");
358 return MPU3000_ERR_I2C;
359 }
360 else
361 {
362 GYRO_LOG("set power mode ok %d!\n", enable);
363 }
364
365 sensor_power = enable;
366
367 return MPU3000_SUCCESS;
368}
369
370/*----------------------------------------------------------------------------*/
371static int MPU3000_SetDataFormat(struct i2c_client *client, u8 dataformat)
372{
373 u8 databuf[2] = {0};
374 int res = 0;
375 GYRO_FUN();
376
377 databuf[0] = MPU3000_REG_DATA_FMT;
378 databuf[1] = dataformat;
379 res = i2c_master_send(client, databuf, 0x2);
380 if(res <= 0)
381 {
382 return MPU3000_ERR_I2C;
383 }
384
385 //read sample rate after written for test
386 udelay(500);
387 if(hwmsen_read_byte(client, MPU3000_REG_DATA_FMT, databuf))
388 {
389 GYRO_ERR("read data format register err!\n");
390 return MPU3000_ERR_I2C;
391 }
392 else
393 {
394 GYRO_LOG("read data format: 0x%x\n", databuf[0]);
395 }
396
397 //return MPU3000_SetDataResolution(obj);
398 return MPU3000_SUCCESS;
399}
400
401// set the sample rate
402static int MPU3000_SetSampleRate(struct i2c_client *client, int sample_rate)
403{
404 u8 databuf[2] = {0};
405 int rate_div = 0;
406 int res = 0;
407 GYRO_FUN();
408
409 if(hwmsen_read_byte(client, MPU3000_REG_DATA_FMT, databuf))
410 {
411 GYRO_ERR("read gyro data format register err!\n");
412 return MPU3000_ERR_I2C;
413 }
414 else
415 {
416 GYRO_LOG("read gyro data format register: 0x%x\n", databuf[0]);
417 }
418
419 if((databuf[0] & 0x07) == 0) //Analog sample rate is 8KHz
420 {
421 rate_div = 8 * 1024 / sample_rate - 1;
422 }
423 else // 1kHz
424 {
425 rate_div = 1024 / sample_rate - 1;
426 }
427
428 if(rate_div > 255) // rate_div: 0 to 255;
429 {
430 rate_div = 255;
431 }
432 else if(rate_div < 0)
433 {
434 rate_div = 0;
435 }
436
437 databuf[0] = MPU3000_REG_SAMRT_DIV;
438 databuf[1] = rate_div;
439 res = i2c_master_send(client, databuf, 0x2);
440 if(res <= 0)
441 {
442 GYRO_ERR("write sample rate register err!\n");
443 return MPU3000_ERR_I2C;
444 }
445
446 //read sample div after written for test
447 udelay(500);
448 if(hwmsen_read_byte(client, MPU3000_REG_SAMRT_DIV, databuf))
449 {
450 GYRO_ERR("read gyro sample rate register err!\n");
451 return MPU3000_ERR_I2C;
452 }
453 else
454 {
455 GYRO_LOG("read gyro sample rate: 0x%x\n", databuf[0]);
456 }
457
458 return MPU3000_SUCCESS;
459}
460/*----------------------------------------------------------------------------*/
461/*
462static int MPU3000_SetIntEnable(struct i2c_client *client, u8 intenable)
463{
464 u8 databuf[2] = {0};
465 int res = 0;
466 GYRO_FUN();
467
468 databuf[0] = MPU3000_REG_INT_EN;
469 databuf[1] = intenable;
470 res = i2c_master_send(client, databuf, 0x2);
471 if(res <= 0)
472 {
473 return MPU3000_ERR_I2C;
474 }
475
476 return MPU3000_SUCCESS;
477}
478*/
479
480#if 0
481/*----------------------------------------------------------------------------*/
482static int MPU3000_Reset(struct i2c_client *client, u8 reset)
483{
484 u8 databuf[2] = {0};
485 int res = 0;
486 GYRO_FUN();
487
488 //read FIFO CTL register
489 if(hwmsen_read_byte(client, MPU3000_REG_FIFO_CTL, databuf))
490 {
491 GYRO_ERR("read gyro FIFO CTRL register err!\n");
492 return MPU3000_ERR_I2C;
493 }
494 else
495 {
496 GYRO_LOG("read gyro FIFO CTRL: 0x%x\n", databuf[0]);
497 }
498
499 //write the reset flag of this register
500 databuf[1] = databuf[0] |reset;
501 databuf[0] = MPU3000_REG_FIFO_CTL;
502 res = i2c_master_send(client, databuf, 0x2);
503 if(res <= 0)
504 {
505 GYRO_ERR("write FIFO CTRL register err!\n");
506 return MPU3000_ERR_I2C;
507 }
508
509 GYRO_LOG("MPU3000_Reset OK!\n");
510
511 return MPU3000_SUCCESS;
512}
513#endif
514
515/*----------------------------------------------------------------------------*/
516static int MPU3000_FIFOConfig(struct i2c_client *client, u8 clk)
517{
518 u8 databuf[2] = {0};
519 int res = 0;
520 GYRO_FUN();
521
522 //use gyro X, Y or Z for clocking
523 databuf[0] = MPU3000_REG_PWR_CTL;
524 databuf[1] = clk;
525 res = i2c_master_send(client, databuf, 0x2);
526 if(res <= 0)
527 {
528 GYRO_ERR("write Power CTRL register err!\n");
529 return MPU3000_ERR_I2C;
530 }
531 GYRO_LOG("MPU3000 use gyro X for clocking OK!\n");
532
533 mdelay(50);
534
535 //enable xyz gyro in FIFO
536 databuf[0] = MPU3000_REG_FIFO_EN;
537 databuf[1] = (MPU3000_FIFO_GYROX_EN|MPU3000_FIFO_GYROY_EN|MPU3000_FIFO_GYROZ_EN);
538 res = i2c_master_send(client, databuf, 0x2);
539 if(res <= 0)
540 {
541 GYRO_ERR("write Power CTRL register err!\n");
542 return MPU3000_ERR_I2C;
543 }
544 GYRO_LOG("MPU3000 enable xyz gyro in FIFO OK!\n");
545
546 //disable AUX_VDDIO
547 databuf[0] = MPU3000_REG_AUX_VDD;
548 databuf[1] = MPU3000_AUX_VDDIO_DIS;
549 res = i2c_master_send(client, databuf, 0x2);
550 if(res <= 0)
551 {
552 GYRO_ERR("write AUX_VDD register err!\n");
553 return MPU3000_ERR_I2C;
554 }
555 GYRO_LOG("MPU3000 disable AUX_VDDIO OK!\n");
556
557 //enable FIFO and reset FIFO
558 databuf[0] = MPU3000_REG_FIFO_CTL;
559 databuf[1] = (MPU3000_FIFO_EN | MPU3000_FIFO_RST);
560 res = i2c_master_send(client, databuf, 0x2);
561 if(res <= 0)
562 {
563 GYRO_ERR("write FIFO CTRL register err!\n");
564 return MPU3000_ERR_I2C;
565 }
566
567 GYRO_LOG("MPU3000_FIFOConfig OK!\n");
568 return MPU3000_SUCCESS;
569}
570
571/*----------------------------------------------------------------------------*/
572static int MPU3000_ReadFifoData(struct i2c_client *client, s16 *data, int* datalen)
573{
574 struct mpu3000_i2c_data *obj = i2c_get_clientdata(client);
575 u8 buf[MPU3000_DATA_LEN] = {0};
576 s16 tmp1[MPU3000_AXES_NUM] = {0};
577 s16 tmp2[MPU3000_AXES_NUM] = {0};
578 int err = 0;
579 u8 tmp = 0;
580 int packet_cnt = 0;
581 int i;
582 GYRO_FUN();
583
584 if(NULL == client)
585 {
586 return -EINVAL;
587 }
588
589 //stop putting data in FIFO
590 MPU3000_ReadStart(client, FALSE);
591
592 //read data number of bytes in FIFO
593 err = hwmsen_read_byte(client, MPU3000_REG_FIFO_CNTH, &tmp);
594 if(err)
595 {
596 GYRO_ERR("read data high number of bytes error: %d\n", err);
597 return -1;
598 }
599 packet_cnt = tmp<< 8;
600
601 err = hwmsen_read_byte(client, MPU3000_REG_FIFO_CNTL, &tmp);
602 if(err)
603 {
604 GYRO_ERR("read data low number of bytes error: %d\n", err);
605 return -1;
606 }
607 packet_cnt = (packet_cnt + tmp) /MPU3000_DATA_LEN;
608
609 GYRO_LOG("MPU3000 Read Data packet number OK: %d\n", packet_cnt);
610
611 *datalen = packet_cnt;
612
613 //Within +-3% range: timing_tolerance * packet_thresh=0.03*75
614 if(packet_cnt && (abs(packet_thresh -packet_cnt) < 4))
615 {
616 //read data in FIFO
617 for(i = 0; i < packet_cnt; i++)
618 {
619 if(hwmsen_read_block(client, MPU3000_REG_FIFO_DATA, buf, MPU3000_DATA_LEN))
620 {
621 GYRO_ERR("MPU3000 read data from FIFO error: %d\n", err);
622 return -2;
623 }
624 else
625 {
626 GYRO_LOG("MPU3000 read Data of diff address from FIFO OK !\n");
627 }
628
629 tmp1[MPU3000_AXIS_X] = (s16)((buf[MPU3000_AXIS_X*2+1]) | (buf[MPU3000_AXIS_X*2] << 8));
630 tmp1[MPU3000_AXIS_Y] = (s16)((buf[MPU3000_AXIS_Y*2+1]) | (buf[MPU3000_AXIS_Y*2] << 8));
631 tmp1[MPU3000_AXIS_Z] = (s16)((buf[MPU3000_AXIS_Z*2+1]) | (buf[MPU3000_AXIS_Z*2] << 8));
632
633 //remap coordinate//
634 tmp2[obj->cvt.map[MPU3000_AXIS_X]] = obj->cvt.sign[MPU3000_AXIS_X]*tmp1[MPU3000_AXIS_X];
635 tmp2[obj->cvt.map[MPU3000_AXIS_Y]] = obj->cvt.sign[MPU3000_AXIS_Y]*tmp1[MPU3000_AXIS_Y];
636 tmp2[obj->cvt.map[MPU3000_AXIS_Z]] = obj->cvt.sign[MPU3000_AXIS_Z]*tmp1[MPU3000_AXIS_Z];
637
638 data[3* i +MPU3000_AXIS_X] = tmp2[MPU3000_AXIS_X];
639 data[3* i +MPU3000_AXIS_Y] = tmp2[MPU3000_AXIS_Y];
640 data[3* i +MPU3000_AXIS_Z] = tmp2[MPU3000_AXIS_Z];
641
642 GYRO_LOG("gyro FIFO packet[%d]:[%04X %04X %04X] => [%5d %5d %5d]\n", i,
643 data[3*i +MPU3000_AXIS_X], data[3*i +MPU3000_AXIS_Y], data[3*i +MPU3000_AXIS_Z],
644 data[3*i +MPU3000_AXIS_X], data[3*i +MPU3000_AXIS_Y], data[3*i +MPU3000_AXIS_Z]);
645 }
646
647 }
648 else
649 {
650 GYRO_ERR("MPU3000 Incorrect packet count: %d\n", packet_cnt);
651 return -3;
652 }
653
654 return 0;
655}
656
657/*----------------------------------------------------------------------------*/
658static int MPU3000_ReadGyroData(struct i2c_client *client, char *buf, int bufsize)
659{
660 char databuf[6];
661 int data[3];
662 struct mpu3000_i2c_data *obj = i2c_get_clientdata(client);
663
664 if(sensor_power == false)
665 {
666 MPU3000_SetPowerMode(client, true);
667 msleep(50);
668 }
669
670 if(hwmsen_read_block(client, MPU3000_REG_GYRO_XH, databuf, 6))
671 {
672 GYRO_ERR("MPU3000 read gyroscope data error\n");
673 return -2;
674 }
675 else
676 {
677 obj->data[MPU3000_AXIS_X] = ((s16)((databuf[MPU3000_AXIS_X*2+1]) | (databuf[MPU3000_AXIS_X*2] << 8)));
678 obj->data[MPU3000_AXIS_Y] = ((s16)((databuf[MPU3000_AXIS_Y*2+1]) | (databuf[MPU3000_AXIS_Y*2] << 8)));
679 obj->data[MPU3000_AXIS_Z] = ((s16)((databuf[MPU3000_AXIS_Z*2+1]) | (databuf[MPU3000_AXIS_Z*2] << 8)));
680#if DEBUG
681 if(atomic_read(&obj->trace) & GYRO_TRC_RAWDATA)
682 {
683 GYRO_LOG("read gyro register: %d, %d, %d, %d, %d, %d",
684 databuf[0], databuf[1], databuf[2], databuf[3], databuf[4], databuf[5]);
685 GYRO_LOG("get gyro raw data (0x%08X, 0x%08X, 0x%08X) -> (%5d, %5d, %5d)\n",
686 obj->data[MPU3000_AXIS_X],obj->data[MPU3000_AXIS_Y],obj->data[MPU3000_AXIS_Z],
687 obj->data[MPU3000_AXIS_X],obj->data[MPU3000_AXIS_Y],obj->data[MPU3000_AXIS_Z]);
688 }
689#endif
690
691 //Out put the degree/second(o/s)
692 obj->data[MPU3000_AXIS_X] = obj->data[MPU3000_AXIS_X] * MPU3000_FS_MAX_LSB / MPU3000_DEFAULT_LSB + obj->cali_sw[MPU3000_AXIS_X];
693 obj->data[MPU3000_AXIS_Y] = obj->data[MPU3000_AXIS_Y] * MPU3000_FS_MAX_LSB / MPU3000_DEFAULT_LSB + obj->cali_sw[MPU3000_AXIS_Y];
694 obj->data[MPU3000_AXIS_Z] = obj->data[MPU3000_AXIS_Z] * MPU3000_FS_MAX_LSB / MPU3000_DEFAULT_LSB + obj->cali_sw[MPU3000_AXIS_Z];
695
696
697 /*remap coordinate*/
698 data[obj->cvt.map[MPU3000_AXIS_X]] = obj->cvt.sign[MPU3000_AXIS_X]*obj->data[MPU3000_AXIS_X];
699 data[obj->cvt.map[MPU3000_AXIS_Y]] = obj->cvt.sign[MPU3000_AXIS_Y]*obj->data[MPU3000_AXIS_Y];
700 data[obj->cvt.map[MPU3000_AXIS_Z]] = obj->cvt.sign[MPU3000_AXIS_Z]*obj->data[MPU3000_AXIS_Z];
701
702 }
703
704 sprintf(buf, "%04x %04x %04x", data[MPU3000_AXIS_X],data[MPU3000_AXIS_Y],data[MPU3000_AXIS_Z]);
705
706#if DEBUG
707 if(atomic_read(&obj->trace) & GYRO_TRC_DATA)
708 {
709 GYRO_LOG("get gyro data packet:[%d %d %d]\n", data[0], data[1], data[2]);
710 }
711#endif
712
713 return 0;
714
715}
716
717//for factory mode
718static int MPU3000_PROCESS_SMT_DATA(struct i2c_client *client, short *data)
719{
720 int total_num = 0;
721 int retval =0;
722 long xSum = 0;
723 long ySum = 0;
724 long zSum = 0;
725 long xAvg, yAvg, zAvg;
726 long xRMS, yRMS, zRMS;
727 int i=0;
728
729 int bias_thresh = 5242; // 40 dps * 131.072 LSB/dps
730 //float RMS_thresh = 687.19f; // (.2 dps * 131.072) ^ 2
731 long RMS_thresh = 68719; // (.2 dps * 131.072) ^ 2
732
733 total_num = data[0];
734 retval = data[1];
735 GYRO_LOG("MPU3000 read gyro data OK, total number: %d \n", total_num);
736 for(i = 0; i < total_num; i++)
737 {
738 xSum =xSum + data[MPU3000_AXES_NUM*i + MPU3000_AXIS_X +2];
739 ySum =ySum + data[MPU3000_AXES_NUM*i + MPU3000_AXIS_Y +2];
740 zSum =zSum + data[MPU3000_AXES_NUM*i + MPU3000_AXIS_Z +2];
741
742 /*
743 FLPLOGD("read gyro data OK: packet_num:%d, [X:%5d, Y:%5d, Z:%5d]\n", i, data[MPU3000_AXES_NUM*i + MPU3000_AXIS_X +2],
744 data[MPU3000_AXES_NUM*i + MPU3000_AXIS_Y +2], data[MPU3000_AXES_NUM*i + MPU3000_AXIS_Z +2]);
745 FLPLOGD("MPU3000 xSum: %5d, ySum: %5d, zSum: %5d \n", xSum, ySum, zSum);
746 */
747 }
748 GYRO_LOG("MPU3000 xSum: %5ld, ySum: %5ld, zSum: %5ld \n", xSum, ySum, zSum);
749
750 if (total_num != 0)
751 {
752 xAvg = (xSum / total_num);
753 yAvg = (ySum / total_num);
754 zAvg = (zSum / total_num);
755 }
756 else
757 {
758 xAvg = xSum;
759 yAvg = ySum;
760 zAvg = zSum;
761 }
762
763 GYRO_LOG("MPU3000 xAvg: %ld, yAvg: %ld, zAvg: %ld \n", xAvg, yAvg, zAvg);
764
765 if ( abs(xAvg) >bias_thresh)
766 {
767 GYRO_LOG("X-Gyro bias exceeded threshold \n");
768 retval |= 1 << 3;
769 }
770 if ( abs(yAvg) > bias_thresh)
771 {
772 GYRO_LOG("Y-Gyro bias exceeded threshold \n");
773 retval |= 1 << 4;
774 }
775 if ( abs(zAvg ) > bias_thresh)
776 {
777 GYRO_LOG("Z-Gyro bias exceeded threshold \n");
778 retval |= 1 << 5;
779 }
780
781 xRMS = 0;
782 yRMS = 0;
783 zRMS = 0;
784
785 //Finally, check RMS
786 for ( i = 0; i < total_num ; i++)
787 {
788 xRMS += (data[MPU3000_AXES_NUM*i + MPU3000_AXIS_X+2]-xAvg)*(data[MPU3000_AXES_NUM*i + MPU3000_AXIS_X+2]-xAvg);
789 yRMS += (data[MPU3000_AXES_NUM*i + MPU3000_AXIS_Y+2]-yAvg)*(data[MPU3000_AXES_NUM*i + MPU3000_AXIS_Y+2]-yAvg);
790 zRMS += (data[MPU3000_AXES_NUM*i + MPU3000_AXIS_Z+2]-zAvg)*(data[MPU3000_AXES_NUM*i + MPU3000_AXIS_Z+2]-zAvg);
791 }
792
793 GYRO_LOG("MPU3000 xRMS: %ld, yRMS: %ld, zRMS: %ld \n", xRMS, yRMS, zRMS);
794 xRMS = 100*xRMS;
795 yRMS = 100*yRMS;
796 zRMS = 100*zRMS;
797
798 if (FACTORY_BOOT == get_boot_mode())
799 return retval;
800 if ( xRMS > RMS_thresh * total_num)
801 {
802 GYRO_LOG("X-Gyro RMS exceeded threshold, RMS_thresh: %ld \n", RMS_thresh * total_num);
803 retval |= 1 << 6;
804 }
805 if ( yRMS > RMS_thresh * total_num )
806 {
807 GYRO_LOG("Y-Gyro RMS exceeded threshold, RMS_thresh: %ld \n", RMS_thresh * total_num);
808 retval |= 1 << 7;
809 }
810 if ( zRMS > RMS_thresh * total_num )
811 {
812 GYRO_LOG("Z-Gyro RMS exceeded threshold, RMS_thresh: %ld \n", RMS_thresh * total_num);
813 retval |= 1 << 8;
814 }
815 if ( xRMS == 0 || yRMS == 0 || zRMS == 0)
816 //If any of the RMS noise value returns zero, then we might have dead gyro or FIFO/register failure
817 retval |= 1 << 9;
818
819 return retval;
820
821}
822
823
824
825/*----------------------------------------------------------------------------*/
826static int MPU3000_SMTReadSensorData(struct i2c_client *client, s16 *buf, int bufsize)
827{
828 //S16 gyro[MPU3000_AXES_NUM*MPU3000_FIFOSIZE];
829 int res = 0;
830 int i;
831 int datalen, total_num= 0;
832
833 GYRO_FUN();
834
835 if(sensor_power == false)
836 {
837 MPU3000_SetPowerMode(client, true);
838 }
839
840 if(NULL == buf)
841 {
842 return -1;
843 }
844 if(NULL == client)
845 {
846 *buf = 0;
847 return -2;
848 }
849
850 for(i = 0; i < MPU3000_AXES_NUM; i++)
851 {
852 res = MPU3000_FIFOConfig(client, (i+1));
853 if(res)
854 {
855 GYRO_ERR("MPU3000_FIFOConfig error:%d!\n", res);
856 return -3;
857 }
858
859 //putting data in FIFO during the delayed 600ms
860 mdelay(600);
861
862 res = MPU3000_ReadFifoData(client, &(buf[total_num+2]), &datalen);
863 if(res)
864 {
865 if(res == (-3))
866 {
867 buf[1] = (1<< i);
868 }
869 else
870 {
871 GYRO_ERR("MPU3000_ReadData error:%d!\n", res);
872 return -3;
873 }
874 }
875 else
876 {
877 buf[0] = datalen;
878 total_num+=datalen*MPU3000_AXES_NUM;
879 }
880 }
881
882 GYRO_LOG("gyroscope read data OK, total packet: %d", buf[0] );
883
884 return 0;
885}
886
887/*----------------------------------------------------------------------------*/
888static int MPU3000_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
889{
890 u8 databuf[10];
891
892 memset(databuf, 0, sizeof(u8)*10);
893
894 if((NULL == buf)||(bufsize<=30))
895 {
896 return -1;
897 }
898
899 if(NULL == client)
900 {
901 *buf = 0;
902 return -2;
903 }
904
905 sprintf(buf, "MPU3000 Chip");
906 return 0;
907}
908
909
910/*----------------------------------------------------------------------------*/
911static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
912{
913 struct i2c_client *client = mpu3000_i2c_client;
914 char strbuf[MPU3000_BUFSIZE];
915 if(NULL == client)
916 {
917 GYRO_ERR("i2c client is null!!\n");
918 return 0;
919 }
920
921 MPU3000_ReadChipInfo(client, strbuf, MPU3000_BUFSIZE);
922 return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
923}
924/*----------------------------------------------------------------------------*/
925static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
926{
927 struct i2c_client *client = mpu3000_i2c_client;
928 char strbuf[MPU3000_BUFSIZE];
929
930 if(NULL == client)
931 {
932 GYRO_ERR("i2c client is null!!\n");
933 return 0;
934 }
935
936 MPU3000_ReadGyroData(client, strbuf, MPU3000_BUFSIZE);
937 return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);;
938}
939
940/*----------------------------------------------------------------------------*/
941static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
942{
943 ssize_t res;
944 struct mpu3000_i2c_data *obj = obj_i2c_data;
945 if (obj == NULL)
946 {
947 GYRO_ERR("i2c_data obj is null!!\n");
948 return 0;
949 }
950
951 res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));
952 return res;
953}
954/*----------------------------------------------------------------------------*/
955static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
956{
957 struct mpu3000_i2c_data *obj = obj_i2c_data;
958 int trace;
959 if (obj == NULL)
960 {
961 GYRO_ERR("i2c_data obj is null!!\n");
962 return 0;
963 }
964
965 if(1 == sscanf(buf, "0x%x", &trace))
966 {
967 atomic_set(&obj->trace, trace);
968 }
969 else
970 {
971 GYRO_ERR("invalid content: '%s', length = %d\n", buf, count);
972 }
973
974 return count;
975}
976/*----------------------------------------------------------------------------*/
977static ssize_t show_status_value(struct device_driver *ddri, char *buf)
978{
979 ssize_t len = 0;
980 struct mpu3000_i2c_data *obj = obj_i2c_data;
981 if (obj == NULL)
982 {
983 GYRO_ERR("i2c_data obj is null!!\n");
984 return 0;
985 }
986
987 if(obj->hw)
988 {
989 len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n",
990 obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);
991 }
992 else
993 {
994 len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
995 }
996 return len;
997}
998/*----------------------------------------------------------------------------*/
999static DRIVER_ATTR(chipinfo, S_IRUGO, show_chipinfo_value, NULL);
1000static DRIVER_ATTR(sensordata, S_IRUGO, show_sensordata_value, NULL);
1001static DRIVER_ATTR(trace, S_IWUSR | S_IRUGO, show_trace_value, store_trace_value);
1002static DRIVER_ATTR(status, S_IRUGO, show_status_value, NULL);
1003/*----------------------------------------------------------------------------*/
1004static struct driver_attribute *MPU3000_attr_list[] = {
1005 &driver_attr_chipinfo, /*chip information*/
1006 &driver_attr_sensordata, /*dump sensor data*/
1007 &driver_attr_trace, /*trace log*/
1008 &driver_attr_status,
1009};
1010/*----------------------------------------------------------------------------*/
1011static int mpu3000_create_attr(struct device_driver *driver)
1012{
1013 int idx, err = 0;
1014 int num = (int)(sizeof(MPU3000_attr_list)/sizeof(MPU3000_attr_list[0]));
1015 if (driver == NULL)
1016 {
1017 return -EINVAL;
1018 }
1019
1020 for(idx = 0; idx < num; idx++)
1021 {
1022 if(0 != (err = driver_create_file(driver, MPU3000_attr_list[idx])))
1023 {
1024 GYRO_ERR("driver_create_file (%s) = %d\n", MPU3000_attr_list[idx]->attr.name, err);
1025 break;
1026 }
1027 }
1028 return err;
1029}
1030/*----------------------------------------------------------------------------*/
1031static int mpu3000_delete_attr(struct device_driver *driver)
1032{
1033 int idx ,err = 0;
1034 int num = (int)(sizeof(MPU3000_attr_list)/sizeof(MPU3000_attr_list[0]));
1035
1036 if(driver == NULL)
1037 {
1038 return -EINVAL;
1039 }
1040
1041
1042 for(idx = 0; idx < num; idx++)
1043 {
1044 driver_remove_file(driver, MPU3000_attr_list[idx]);
1045 }
1046
1047
1048 return err;
1049}
1050
1051/*----------------------------------------------------------------------------*/
1052static int mpu3000_gpio_config(void)
1053{
1054 //because we donot use EINT ,to support low power
1055 // config to GPIO input mode + PD
1056 //set GPIO_MSE_EINT_PIN
1057 mt_set_gpio_mode(GPIO_GYRO_EINT_PIN, GPIO_GYRO_EINT_PIN_M_GPIO);
1058 mt_set_gpio_dir(GPIO_GYRO_EINT_PIN, GPIO_DIR_IN);
1059 mt_set_gpio_pull_enable(GPIO_GYRO_EINT_PIN, GPIO_PULL_ENABLE);
1060 mt_set_gpio_pull_select(GPIO_GYRO_EINT_PIN, GPIO_PULL_DOWN);
1061 return 0;
1062}
1063static int mpu3000_init_client(struct i2c_client *client, bool enable)
1064{
1065 struct mpu3000_i2c_data *obj = i2c_get_clientdata(client);
1066 int res = 0;
1067 GYRO_FUN();
1068 mpu3000_gpio_config();
1069
1070 res = MPU3000_SetPowerMode(client, enable);
1071 if(res != MPU3000_SUCCESS)
1072 {
1073 return res;
1074 }
1075
1076
1077
1078 // The range should at least be 17.45 rad/s (ie: ~1000 deg/s).
1079 res = MPU3000_SetDataFormat(client, (MPU3000_SYNC_GYROX << MPU3000_EXT_SYNC)|
1080 (MPU3000_DEFAULT_FS << MPU3000_FS_RANGE)|
1081 MPU3000_RATE_1K_LPFB_188HZ);
1082 if(res != MPU3000_SUCCESS)
1083 {
1084 return res;
1085 }
1086
1087 // Set 125HZ sample rate
1088 res = MPU3000_SetSampleRate(client, 125);
1089 if(res != MPU3000_SUCCESS )
1090 {
1091 return res;
1092 }
1093
1094 GYRO_LOG("mpu3000_init_client OK!\n");
1095
1096#ifdef CONFIG_MPU3000_LOWPASS
1097 memset(&obj->fir, 0x00, sizeof(obj->fir));
1098#endif
1099
1100 return MPU3000_SUCCESS;
1101}
1102
1103/*----------------------------------------------------------------------------*/
1104int mpu3000_operate(void* self, uint32_t command, void* buff_in, int size_in,
1105 void* buff_out, int size_out, int* actualout)
1106{
1107 int err = 0;
1108 int value;
1109 struct mpu3000_i2c_data *priv = (struct mpu3000_i2c_data*)self;
1110 hwm_sensor_data* gyro_data;
1111 char buff[MPU3000_BUFSIZE];
1112
1113 switch (command)
1114 {
1115 case SENSOR_DELAY:
1116 if((buff_in == NULL) || (size_in < sizeof(int)))
1117 {
1118 GYRO_ERR("Set delay parameter error!\n");
1119 err = -EINVAL;
1120 }
1121 else
1122 {
1123
1124 }
1125 break;
1126
1127 case SENSOR_ENABLE:
1128 if((buff_in == NULL) || (size_in < sizeof(int)))
1129 {
1130 GYRO_ERR("Enable gyroscope parameter error!\n");
1131 err = -EINVAL;
1132 }
1133 else
1134 {
1135 value = *(int *)buff_in;
1136 if(((value == 0) && (sensor_power == false)) ||((value == 1) && (sensor_power == true)))
1137 {
1138 GYRO_LOG("gyroscope device have updated!\n");
1139 }
1140 else
1141 {
1142 err = MPU3000_SetPowerMode(priv->client, !sensor_power);
1143 }
1144 }
1145 break;
1146
1147 case SENSOR_GET_DATA:
1148 if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
1149 {
1150 GYRO_ERR("get gyroscope data parameter error!\n");
1151 err = -EINVAL;
1152 }
1153 else
1154 {
1155 gyro_data = (hwm_sensor_data *)buff_out;
1156 MPU3000_ReadGyroData(priv->client, buff, MPU3000_BUFSIZE);
1157 sscanf(buff, "%x %x %x", &gyro_data->values[0],
1158 &gyro_data->values[1], &gyro_data->values[2]);
1159 gyro_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;
1160 gyro_data->value_divide = DEGREE_TO_RAD;
1161 }
1162 break;
1163 default:
1164 GYRO_ERR("gyroscope operate function no this parameter %d!\n", command);
1165 err = -1;
1166 break;
1167 }
1168
1169 return err;
1170}
1171
1172/******************************************************************************
1173 * Function Configuration
1174******************************************************************************/
1175static int mpu3000_open(struct inode *inode, struct file *file)
1176{
1177 file->private_data = mpu3000_i2c_client;
1178
1179 if(file->private_data == NULL)
1180 {
1181 GYRO_ERR("null pointer!!\n");
1182 return -EINVAL;
1183 }
1184 return nonseekable_open(inode, file);
1185}
1186/*----------------------------------------------------------------------------*/
1187static int mpu3000_release(struct inode *inode, struct file *file)
1188{
1189 file->private_data = NULL;
1190 return 0;
1191}
1192/*----------------------------------------------------------------------------*/
1193//static int mpu3000_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
1194// unsigned long arg)
1195static long mpu3000_unlocked_ioctl(struct file *file, unsigned int cmd,
1196 unsigned long arg)
1197{
1198 struct i2c_client *client = (struct i2c_client*)file->private_data;
1199 //struct mpu3000_i2c_data *obj = (struct mpu3000_i2c_data*)i2c_get_clientdata(client);
1200 char strbuf[MPU3000_BUFSIZE] = {0};
1201 s16 *SMTdata;
1202 void __user *data;
1203 long err = 0;
1204 int copy_cnt = 0;
1205 SENSOR_DATA sensor_data;
1206 int cali[3];
1207 int smtRes=0;
1208 //GYRO_FUN();
1209
1210 if(_IOC_DIR(cmd) & _IOC_READ)
1211 {
1212 err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
1213 }
1214 else if(_IOC_DIR(cmd) & _IOC_WRITE)
1215 {
1216 err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
1217 }
1218
1219 if(err)
1220 {
1221 GYRO_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
1222 return -EFAULT;
1223 }
1224
1225 switch(cmd)
1226 {
1227 case GYROSCOPE_IOCTL_INIT:
1228 mpu3000_init_client(client, false);
1229 break;
1230
1231 case GYROSCOPE_IOCTL_SMT_DATA:
1232 data = (void __user *) arg;
1233 if(data == NULL)
1234 {
1235 err = -EINVAL;
1236 break;
1237 }
1238
1239 SMTdata = kzalloc(sizeof(*SMTdata) * 800, GFP_KERNEL);
1240 if(SMTdata == NULL)
1241 {
1242 err = -ENOMEM;
1243 break;
1244 }
1245 memset(SMTdata, 0, sizeof(*SMTdata) * 800);
1246 MPU3000_SMTReadSensorData(client, SMTdata, 800);
1247 //GYRO_LOG("gyroscope read data from kernel OK: sizeof:%d, strlen:%d, packet:%d!\n",
1248 //sizeof(SMTdata), strlen(SMTdata), SMTdata[0]);
1249 GYRO_LOG("gyroscope read data from kernel OK: SMTdata[0]:%d, copied packet:%d!\n", SMTdata[0],
1250 ((SMTdata[0]*MPU3000_AXES_NUM+2)*sizeof(s16)+1));
1251
1252 smtRes = MPU3000_PROCESS_SMT_DATA(client,SMTdata);
1253 copy_cnt = copy_to_user(data, &smtRes, sizeof(smtRes));
1254 kfree(SMTdata);
1255 if(copy_cnt)
1256 {
1257 err = -EFAULT;
1258 GYRO_ERR("copy gyro data to user failed!\n");
1259 }
1260 GYRO_LOG("copy gyro data to user OK: %d!\n", copy_cnt);
1261 break;
1262
1263 case GYROSCOPE_IOCTL_READ_SENSORDATA:
1264 data = (void __user *) arg;
1265 if(data == NULL)
1266 {
1267 err = -EINVAL;
1268 break;
1269 }
1270
1271 MPU3000_ReadGyroData(client, strbuf, MPU3000_BUFSIZE);
1272 if(copy_to_user(data, strbuf, sizeof(strbuf)))
1273 {
1274 err = -EFAULT;
1275 break;
1276 }
1277 break;
1278
1279 case GYROSCOPE_IOCTL_SET_CALI:
1280 data = (void __user*)arg;
1281 if(data == NULL)
1282 {
1283 err = -EINVAL;
1284 break;
1285 }
1286 if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
1287 {
1288 err = -EFAULT;
1289 break;
1290 }
1291
1292 else
1293 {
1294 cali[MPU3000_AXIS_X] = sensor_data.x ;
1295 cali[MPU3000_AXIS_Y] = sensor_data.y ;
1296 cali[MPU3000_AXIS_Z] = sensor_data.z ;
1297 err = MPU3000_WriteCalibration(client, cali);
1298 }
1299 break;
1300
1301 case GYROSCOPE_IOCTL_CLR_CALI:
1302 err = MPU3000_ResetCalibration(client);
1303 break;
1304
1305 case GYROSCOPE_IOCTL_GET_CALI:
1306 data = (void __user*)arg;
1307 if(data == NULL)
1308 {
1309 err = -EINVAL;
1310 break;
1311 }
1312 err = MPU3000_ReadCalibration(client, cali);
1313 if(err)
1314 {
1315 break;
1316 }
1317 sensor_data.x = cali[MPU3000_AXIS_X] ;
1318 sensor_data.y = cali[MPU3000_AXIS_Y] ;
1319 sensor_data.z = cali[MPU3000_AXIS_Z] ;
1320 if(copy_to_user(data, &sensor_data, sizeof(sensor_data)))
1321 {
1322 err = -EFAULT;
1323 break;
1324 }
1325 break;
1326
1327 default:
1328 GYRO_ERR("unknown IOCTL: 0x%08x\n", cmd);
1329 err = -ENOIOCTLCMD;
1330 break;
1331 }
1332 return err;
1333}
1334
1335
1336/*----------------------------------------------------------------------------*/
1337static struct file_operations mpu3000_fops = {
1338// .owner = THIS_MODULE,//modified
1339 .open = mpu3000_open,
1340 .release = mpu3000_release,
1341 .unlocked_ioctl = mpu3000_unlocked_ioctl,
1342};
1343/*----------------------------------------------------------------------------*/
1344static struct miscdevice mpu3000_device = {
1345 .minor = MISC_DYNAMIC_MINOR,
1346 .name = "gyroscope",
1347 .fops = &mpu3000_fops,
1348};
1349/*----------------------------------------------------------------------------*/
1350#ifndef CONFIG_HAS_EARLYSUSPEND
1351/*----------------------------------------------------------------------------*/
1352static int mpu3000_suspend(struct i2c_client *client, pm_message_t msg)
1353{
1354 struct mpu3000_i2c_data *obj = i2c_get_clientdata(client);
1355 int err;
1356 GYRO_FUN();
1357
1358 if(msg.event == PM_EVENT_SUSPEND)
1359 {
1360 if(obj == NULL)
1361 {
1362 GYRO_ERR("null pointer!!\n");
1363 return -EINVAL;
1364 }
1365 atomic_set(&obj->suspend, 1);
1366
1367 err = MPU3000_SetPowerMode(client, false);
1368 if(err <= 0)
1369 {
1370 return err;
1371 }
1372 }
1373 return 0;//modified
1374}
1375/*----------------------------------------------------------------------------*/
1376static int mpu3000_resume(struct i2c_client *client)
1377{
1378 struct mpu3000_i2c_data *obj = i2c_get_clientdata(client);
1379 int err;
1380 GYRO_FUN();
1381
1382 if(obj == NULL)
1383 {
1384 GYRO_ERR("null pointer!!\n");
1385 return -EINVAL;
1386 }
1387
1388 MPU3000_power(obj->hw, 1);
1389 err = mpu3000_init_client(client, false);
1390 if(err)
1391 {
1392 GYRO_ERR("initialize client fail!!\n");
1393 return err;
1394 }
1395 atomic_set(&obj->suspend, 0);
1396
1397 return 0;
1398}
1399/*----------------------------------------------------------------------------*/
1400#else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
1401/*----------------------------------------------------------------------------*/
1402static void mpu3000_early_suspend(struct early_suspend *h)
1403{
1404 struct mpu3000_i2c_data *obj = container_of(h, struct mpu3000_i2c_data, early_drv);
1405 int err;
1406 u8 databuf[2];
1407 GYRO_FUN();
1408
1409 if(obj == NULL)
1410 {
1411 GYRO_ERR("null pointer!!\n");
1412 return;
1413 }
1414 atomic_set(&obj->suspend, 1);
1415 err = MPU3000_SetPowerMode(obj->client, false);
1416 if(err)
1417 {
1418 GYRO_ERR("write power control fail!!\n");
1419 return;
1420 }
1421
1422 databuf[0] = MPU3000_REG_PWR_CTL;
1423 databuf[1] = MPU3000_SLEEP;
1424 err = i2c_master_send(obj->client, databuf, 0x2);
1425 if(err <= 0)
1426 {
1427 return;
1428 }
1429
1430 sensor_power = false;
1431
1432 MPU3000_power(obj->hw, 0);
1433}
1434/*----------------------------------------------------------------------------*/
1435static void mpu3000_late_resume(struct early_suspend *h)
1436{
1437 struct mpu3000_i2c_data *obj = container_of(h, struct mpu3000_i2c_data, early_drv);
1438 int err;
1439 GYRO_FUN();
1440
1441 if(obj == NULL)
1442 {
1443 GYRO_ERR("null pointer!!\n");
1444 return;
1445 }
1446
1447 MPU3000_power(obj->hw, 1);
1448 err = mpu3000_init_client(obj->client, false);
1449 if(err)
1450 {
1451 GYRO_ERR("initialize client fail! err code %d!\n", err);
1452 return;
1453 }
1454 atomic_set(&obj->suspend, 0);
1455}
1456/*----------------------------------------------------------------------------*/
1457#endif /*CONFIG_HAS_EARLYSUSPEND*/
1458/*----------------------------------------------------------------------------*/
1459
1460// if use this typ of enable , Gsensor should report inputEvent(x, y, z ,stats, div) to HAL
1461static int mpu3000_open_report_data(int open)
1462{
1463 //should queuq work to report event if is_report_input_direct=true
1464 return 0;
1465}
1466/*----------------------------------------------------------------------------*/
1467// if use this typ of enable , Gsensor only enabled but not report inputEvent to HAL
1468static int mpu3000_enable_nodata(int en)
1469{
1470 int err = 0;
1471 int retry = 0;
1472
1473 if(((en == 0) && (sensor_power == false)) ||((en == 1) && (sensor_power == true)))
1474 {
1475 GYRO_LOG("gyroscope device have updated!\n");
1476 }
1477 else
1478 {
1479 for(retry = 0; retry < 3; retry++) {
1480 err = MPU3000_SetPowerMode(obj_i2c_data->client, !sensor_power);
1481 if (err == 0)
1482 break;
1483 }
1484 }
1485
1486 if(err != MPU3000_SUCCESS)
1487 {
1488 GYRO_ERR("gsensor_enable_nodata fail!\n");
1489 return -1;
1490 }
1491
1492 GYRO_LOG("gsensor_enable_nodata OK!\n");
1493 return 0;
1494}
1495/*----------------------------------------------------------------------------*/
1496static int mpu3000_set_delay(u64 ns)
1497{
1498 return 0;
1499}
1500/*----------------------------------------------------------------------------*/
1501static int mpu3000_get_data(int* x ,int* y,int* z, int* status)
1502{
1503 char buff[MPU3000_BUFSIZE];
1504
1505 MPU3000_ReadGyroData(obj_i2c_data->client, buff, MPU3000_BUFSIZE);
1506 sscanf(buff, "%x %x %x", x, y, z);
1507 *status = SENSOR_STATUS_ACCURACY_MEDIUM;
1508
1509 return 0;
1510}
1511
1512
1513/*----------------------------------------------------------------------------*/
1514static int mpu3000_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
1515{
1516 int err = 0;
1517 struct i2c_client *new_client;
1518 struct mpu3000_i2c_data *obj;
1519 struct gyro_control_path ctl={0};
1520 struct gyro_data_path data={0};
1521
1522 GYRO_FUN();
1523 if(!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
1524 {
1525 err = -ENOMEM;
1526 goto exit;
1527 }
1528
1529 obj->hw = get_cust_gyro_hw();
1530 err = hwmsen_get_convert(obj->hw->direction, &obj->cvt);
1531 if(err)
1532 {
1533 GYRO_ERR("invalid direction: %d\n", obj->hw->direction);
1534 goto exit;
1535 }
1536
1537 GYRO_LOG("gyro_default_i2c_addr: %x\n", client->addr);
1538 GYRO_LOG("gyro_custom_i2c_addr: %x\n", obj->hw->addr);
1539 if(0!=obj->hw->addr)
1540 {
1541 client->addr = obj->hw->addr >> 1;
1542 GYRO_LOG("gyro_use_i2c_addr: %x\n", client->addr);
1543 }
1544
1545 obj_i2c_data = obj;
1546 obj->client = client;
1547 new_client = obj->client;
1548 i2c_set_clientdata(new_client,obj);
1549 atomic_set(&obj->trace, 0);
1550 atomic_set(&obj->suspend, 0);
1551 mpu3000_i2c_client = new_client;
1552
1553 err = mpu3000_init_client(new_client, false);
1554 if(err)
1555 {
1556 goto exit_init_failed;
1557 }
1558
1559 err = misc_register(&mpu3000_device);
1560 if(err)
1561 {
1562 GYRO_ERR("mpu3000_device misc register failed!\n");
1563 goto exit_misc_device_register_failed;
1564 }
1565 ctl.is_use_common_factory = false;
1566 err = mpu3000_create_attr(&mpu3000_init_info.platform_diver_addr->driver);
1567 if(err)
1568 {
1569 GYRO_ERR("mpu3000 create attribute err = %d\n", err);
1570 goto exit_create_attr_failed;
1571 }
1572
1573 ctl.open_report_data= mpu3000_open_report_data;
1574 ctl.enable_nodata = mpu3000_enable_nodata;
1575 ctl.set_delay = mpu3000_set_delay;
1576 ctl.is_report_input_direct = false;
1577 ctl.is_support_batch = obj->hw->is_batch_supported;
1578
1579 err = gyro_register_control_path(&ctl);
1580 if(err)
1581 {
1582 GYRO_ERR("register gyro control path err\n");
1583 goto exit_kfree;
1584 }
1585
1586 data.get_data = mpu3000_get_data;
1587 data.vender_div = DEGREE_TO_RAD;
1588 err = gyro_register_data_path(&data);
1589 if(err)
1590 {
1591 GYRO_ERR("register acc data path err\n");
1592 goto exit_kfree;
1593 }
1594
1595#ifdef CONFIG_HAS_EARLYSUSPEND
1596 obj->early_drv.level = EARLY_SUSPEND_LEVEL_STOP_DRAWING - 2,
1597 obj->early_drv.suspend = mpu3000_early_suspend,
1598 obj->early_drv.resume = mpu3000_late_resume,
1599 register_early_suspend(&obj->early_drv);
1600#endif
1601 mpu3000_init_flag = 0;
1602 GYRO_LOG("%s: OK\n", __func__);
1603 return 0;
1604
1605 exit_create_attr_failed:
1606 misc_deregister(&mpu3000_device);
1607 exit_misc_device_register_failed:
1608 exit_init_failed:
1609 exit_kfree:
1610 kfree(obj);
1611 obj = NULL;
1612 exit:
1613 mpu3000_init_flag = -1;
1614 GYRO_ERR("%s: err = %d\n", __func__, err);
1615 return err;
1616}
1617
1618/*----------------------------------------------------------------------------*/
1619static int mpu3000_i2c_remove(struct i2c_client *client)
1620{
1621 int err = 0;
1622
1623 err = mpu3000_delete_attr(&mpu3000_init_info.platform_diver_addr->driver);
1624 if(err)
1625 {
1626 GYRO_ERR("mpu3000_delete_attr fail: %d\n", err);
1627 }
1628
1629 err = misc_deregister(&mpu3000_device);
1630 if(err)
1631 {
1632 GYRO_ERR("misc_deregister fail: %d\n", err);
1633 }
1634
1635 mpu3000_i2c_client = NULL;
1636 i2c_unregister_device(client);
1637 kfree(i2c_get_clientdata(client));
1638 return 0;
1639}
1640/*----------------------------------------------------------------------------*/
1641
1642static int mpu3000_local_init(void)
1643{
1644 struct gyro_hw *hw = get_cust_gyro_hw();
1645 GYRO_FUN();
1646 MPU3000_power(hw, 1);
1647 if (i2c_add_driver(&mpu3000_i2c_driver))
1648 {
1649 GYRO_ERR("add driver error\n");
1650 return -1;
1651 }
1652
1653 if(-1 == mpu3000_init_flag)
1654 {
1655 return -1;
1656 }
1657
1658 return 0;
1659}
1660
1661static int mpu3000_remove(void)
1662{
1663 struct gyro_hw *hw = get_cust_gyro_hw();
1664
1665 GYRO_FUN();
1666 MPU3000_power(hw, 0);
1667 i2c_del_driver(&mpu3000_i2c_driver);
1668 return 0;
1669}
1670
1671/*----------------------------------------------------------------------------*/
1672static int __init mpu3000_init(void)
1673{
1674 struct gyro_hw *hw = get_cust_gyro_hw();
1675 GYRO_LOG("%s: i2c_number=%d\n", __func__,hw->i2c_num);
1676 i2c_register_board_info(hw->i2c_num, &i2c_mpu3000, 1);
1677 gyro_driver_add(&mpu3000_init_info);
1678
1679 return 0;
1680}
1681
1682static void __exit mpu3000_exit(void)
1683{
1684 GYRO_FUN();
1685}
1686/*----------------------------------------------------------------------------*/
1687module_init(mpu3000_init);
1688module_exit(mpu3000_exit);
1689/*----------------------------------------------------------------------------*/
1690MODULE_LICENSE("GPL");
1691MODULE_DESCRIPTION("MPU3000 gyroscope driver");
1692MODULE_AUTHOR("Chunlei.Wang@mediatek.com");
kernel source for telekom puls (alcatel/mediatek)