Commit | Line | Data |
---|---|---|
6fa3eb70 S |
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"); |