projects / GitHub / mt8127 / android_kernel_alcatel_ttab.git / blob
? search:
summary | shortlog | log | commit | commitdiff | tree
blame | history | raw | HEAD
import PULS_20160108
[GitHub/mt8127/android_kernel_alcatel_ttab.git] / drivers / misc / mediatek / gyroscope / lsm6ds0 / lsm6ds0.c
1 /* LSM6DS0 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 "lsm6ds0.h"
34 #include <linux/hwmsen_helper.h>
35 #include <linux/kernel.h>
36
37
38 #include <mach/mt_typedefs.h>
39 #include <mach/mt_gpio.h>
40 #include <mach/mt_pm_ldo.h>
41 #include <mach/mt_boot.h>
42
43 /*-------------------------MT6516&MT6573 define-------------------------------*/
44
45 #define POWER_NONE_MACRO MT65XX_POWER_NONE
46
47 /*----------------------------------------------------------------------------*/
48 #define I2C_DRIVERID_LSM6DS0 3000
49 /*----------------------------------------------------------------------------*/
50 //#define LSM6DS0_DEFAULT_FS LSM6DS0_FS_1000
51 //#define LSM6DS0_DEFAULT_LSB LSM6DS0_FS_250_LSB
52 /*---------------------------------------------------------------------------*/
53 #define DEBUG 1
54 /*----------------------------------------------------------------------------*/
55 #define CONFIG_LSM6DS0_LOWPASS /*apply low pass filter on output*/
56 /*----------------------------------------------------------------------------*/
57 #define LSM6DS0_AXIS_X 0
58 #define LSM6DS0_AXIS_Y 1
59 #define LSM6DS0_AXIS_Z 2
60 #define LSM6DS0_AXES_NUM 3
61 #define LSM6DS0_DATA_LEN 6
62 #define LSM6DS0_DEV_NAME "lsm6ds0"
63 /*----------------------------------------------------------------------------*/
64 static const struct i2c_device_id LSM6DS0_i2c_id[] = {{LSM6DS0_DEV_NAME,0},{}};
65 static struct i2c_board_info __initdata i2c_LSM6DS0={ I2C_BOARD_INFO(LSM6DS0_DEV_NAME, (LSM6DS0_I2C_SLAVE_ADDR>>1))};
66 /*the adapter id will be available in customization*/
67 //static unsigned short LSM6DS0_force[] = {0x00, LSM6DS0_I2C_SLAVE_ADDR, I2C_CLIENT_END, I2C_CLIENT_END};
68 //static const unsigned short *const LSM6DS0_forces[] = { LSM6DS0_force, NULL };
69 //static struct i2c_client_address_data LSM6DS0_addr_data = { .forces = LSM6DS0_forces,};
70
71 int packet_thresh = 75; // 600 ms / 8ms/sample
72
73 /*----------------------------------------------------------------------------*/
74 static int LSM6DS0_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id);
75 static int LSM6DS0_i2c_remove(struct i2c_client *client);
76 static int LSM6DS0_i2c_detect(struct i2c_client *client, int kind, struct i2c_board_info *info);
77 static int LSM6DS0_init_client(struct i2c_client *client, bool enable);
78 static int LSM6DS0_read_byte_sr(struct i2c_client *client, u8 addr, u8 *data, u8 len);
79
80 /*----------------------------------------------------------------------------*/
81 typedef enum {
82 GYRO_TRC_FILTER = 0x01,
83 GYRO_TRC_RAWDATA = 0x02,
84 GYRO_TRC_IOCTL = 0x04,
85 GYRO_TRC_CALI = 0X08,
86 GYRO_TRC_INFO = 0X10,
87 GYRO_TRC_DATA = 0X20,
88 } GYRO_TRC;
89 /*----------------------------------------------------------------------------*/
90 struct scale_factor{
91 u8 whole;
92 u8 fraction;
93 };
94 /*----------------------------------------------------------------------------*/
95 struct data_resolution {
96 struct scale_factor scalefactor;
97 int sensitivity;
98 };
99 /*----------------------------------------------------------------------------*/
100 #define C_MAX_FIR_LENGTH (32)
101 /*----------------------------------------------------------------------------*/
102 struct data_filter {
103 s16 raw[C_MAX_FIR_LENGTH][LSM6DS0_AXES_NUM];
104 int sum[LSM6DS0_AXES_NUM];
105 int num;
106 int idx;
107 };
108 /*----------------------------------------------------------------------------*/
109 struct LSM6DS0_i2c_data {
110 struct i2c_client *client;
111 struct gyro_hw *hw;
112 struct hwmsen_convert cvt;
113
114 /*misc*/
115 struct data_resolution *reso;
116 atomic_t trace;
117 atomic_t suspend;
118 atomic_t selftest;
119 atomic_t filter;
120 s16 cali_sw[LSM6DS0_AXES_NUM+1];
121
122 /*data*/
123 s8 offset[LSM6DS0_AXES_NUM+1]; /*+1: for 4-byte alignment*/
124 s16 data[LSM6DS0_AXES_NUM+1];
125
126 #if defined(CONFIG_LSM6DS0_LOWPASS)
127 atomic_t firlen;
128 atomic_t fir_en;
129 struct data_filter fir;
130 #endif
131 /*early suspend*/
132 #if defined(CONFIG_HAS_EARLYSUSPEND)
133 struct early_suspend early_drv;
134 #endif
135 };
136 /*----------------------------------------------------------------------------*/
137 static struct i2c_driver LSM6DS0_i2c_driver = {
138 .driver = {
139 // .owner = THIS_MODULE,
140 .name = LSM6DS0_DEV_NAME,
141 },
142 .probe = LSM6DS0_i2c_probe,
143 .remove = LSM6DS0_i2c_remove,
144 .detect = LSM6DS0_i2c_detect,
145 #if !defined(CONFIG_HAS_EARLYSUSPEND)
146 .suspend = LSM6DS0_suspend,
147 .resume = LSM6DS0_resume,
148 #endif
149 .id_table = LSM6DS0_i2c_id,
150 // .address_data = &LSM6DS0_addr_data,
151 };
152
153 /*----------------------------------------------------------------------------*/
154 static struct i2c_client *LSM6DS0_i2c_client = NULL;
155 static struct platform_driver LSM6DS0_gyro_driver;
156 static struct LSM6DS0_i2c_data *obj_i2c_data = NULL;
157 static bool sensor_power = false;
158
159
160
161 /*----------------------------------------------------------------------------*/
162 #define GYRO_TAG "[Gyroscope] "
163 //#define GYRO_FUN(f) printk(KERN_INFO GYRO_TAG"%s\n", __FUNCTION__)
164 //#define GYRO_ERR(fmt, args...) printk(KERN_ERR GYRO_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
165
166 //#define GYRO_LOG(fmt, args...) printk(KERN_INFO GYRO_TAG fmt, ##args)
167
168 #define GYRO_FUN(f) printk(GYRO_TAG"%s\n", __FUNCTION__)
169 #define GYRO_ERR(fmt, args...) printk(KERN_ERR GYRO_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
170 #define GYRO_LOG(fmt, args...) printk(GYRO_TAG fmt, ##args)
171
172 /*----------------------------------------------------------------------------*/
173
174 static void LSM6DS0_dumpReg(struct i2c_client *client)
175 {
176 int i=0;
177 u8 addr = 0x20;
178 u8 regdata=0;
179 for(i=0; i<25 ; i++)
180 {
181 //dump all
182 LSM6DS0_read_byte_sr(client,addr,&regdata,1);
183 HWM_LOG("Reg addr=%x regdata=%x\n",addr,regdata);
184 addr++;
185
186 }
187
188 }
189
190
191 /*--------------------gyroscopy power control function----------------------------------*/
192 static void LSM6DS0_power(struct gyro_hw *hw, unsigned int on)
193 {
194 static unsigned int power_on = 0;
195
196 if(hw->power_id != POWER_NONE_MACRO) // have externel LDO
197 {
198 GYRO_LOG("power %s\n", on ? "on" : "off");
199 if(power_on == on) // power status not change
200 {
201 GYRO_LOG("ignore power control: %d\n", on);
202 }
203 else if(on) // power on
204 {
205 if(!hwPowerOn(hw->power_id, hw->power_vol, "LSM6DS0"))
206 {
207 GYRO_ERR("power on fails!!\n");
208 }
209 }
210 else // power off
211 {
212 if (!hwPowerDown(hw->power_id, "LSM6DS0"))
213 {
214 GYRO_ERR("power off fail!!\n");
215 }
216 }
217 }
218 power_on = on;
219 }
220 /*----------------------------------------------------------------------------*/
221
222
223 /*----------------------------------------------------------------------------*/
224 static int LSM6DS0_write_rel_calibration(struct LSM6DS0_i2c_data *obj, int dat[LSM6DS0_AXES_NUM])
225 {
226 obj->cali_sw[LSM6DS0_AXIS_X] = obj->cvt.sign[LSM6DS0_AXIS_X]*dat[obj->cvt.map[LSM6DS0_AXIS_X]];
227 obj->cali_sw[LSM6DS0_AXIS_Y] = obj->cvt.sign[LSM6DS0_AXIS_Y]*dat[obj->cvt.map[LSM6DS0_AXIS_Y]];
228 obj->cali_sw[LSM6DS0_AXIS_Z] = obj->cvt.sign[LSM6DS0_AXIS_Z]*dat[obj->cvt.map[LSM6DS0_AXIS_Z]];
229 #if DEBUG
230 if(atomic_read(&obj->trace) & GYRO_TRC_CALI)
231 {
232 GYRO_LOG("test (%5d, %5d, %5d) ->(%5d, %5d, %5d)->(%5d, %5d, %5d))\n",
233 obj->cvt.sign[LSM6DS0_AXIS_X],obj->cvt.sign[LSM6DS0_AXIS_Y],obj->cvt.sign[LSM6DS0_AXIS_Z],
234 dat[LSM6DS0_AXIS_X], dat[LSM6DS0_AXIS_Y], dat[LSM6DS0_AXIS_Z],
235 obj->cvt.map[LSM6DS0_AXIS_X],obj->cvt.map[LSM6DS0_AXIS_Y],obj->cvt.map[LSM6DS0_AXIS_Z]);
236 GYRO_LOG("write gyro calibration data (%5d, %5d, %5d)\n",
237 obj->cali_sw[LSM6DS0_AXIS_X],obj->cali_sw[LSM6DS0_AXIS_Y],obj->cali_sw[LSM6DS0_AXIS_Z]);
238 }
239 #endif
240 return 0;
241 }
242
243
244 /*----------------------------------------------------------------------------*/
245 static int LSM6DS0_ResetCalibration(struct i2c_client *client)
246 {
247 struct LSM6DS0_i2c_data *obj = i2c_get_clientdata(client);
248
249 memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
250 return 0;
251 }
252 /*----------------------------------------------------------------------------*/
253 static int LSM6DS0_ReadCalibration(struct i2c_client *client, int dat[LSM6DS0_AXES_NUM])
254 {
255 struct LSM6DS0_i2c_data *obj = i2c_get_clientdata(client);
256
257 dat[obj->cvt.map[LSM6DS0_AXIS_X]] = obj->cvt.sign[LSM6DS0_AXIS_X]*obj->cali_sw[LSM6DS0_AXIS_X];
258 dat[obj->cvt.map[LSM6DS0_AXIS_Y]] = obj->cvt.sign[LSM6DS0_AXIS_Y]*obj->cali_sw[LSM6DS0_AXIS_Y];
259 dat[obj->cvt.map[LSM6DS0_AXIS_Z]] = obj->cvt.sign[LSM6DS0_AXIS_Z]*obj->cali_sw[LSM6DS0_AXIS_Z];
260
261 #if DEBUG
262 if(atomic_read(&obj->trace) & GYRO_TRC_CALI)
263 {
264 GYRO_LOG("Read gyro calibration data (%5d, %5d, %5d)\n",
265 dat[LSM6DS0_AXIS_X],dat[LSM6DS0_AXIS_Y],dat[LSM6DS0_AXIS_Z]);
266 }
267 #endif
268 GYRO_LOG("Read gyro calibration data (%5d, %5d, %5d)\n",
269 dat[LSM6DS0_AXIS_X],dat[LSM6DS0_AXIS_Y],dat[LSM6DS0_AXIS_Z]);
270 return 0;
271 }
272 /*----------------------------------------------------------------------------*/
273 /*----------------------------------------------------------------------------*/
274 static int LSM6DS0_WriteCalibration(struct i2c_client *client, int dat[LSM6DS0_AXES_NUM])
275 {
276 struct LSM6DS0_i2c_data *obj = i2c_get_clientdata(client);
277 int err = 0;
278 int cali[LSM6DS0_AXES_NUM];
279
280
281 GYRO_FUN();
282 if(!obj || ! dat)
283 {
284 GYRO_ERR("null ptr!!\n");
285 return -EINVAL;
286 }
287 else
288 {
289 cali[obj->cvt.map[LSM6DS0_AXIS_X]] = obj->cvt.sign[LSM6DS0_AXIS_X]*obj->cali_sw[LSM6DS0_AXIS_X];
290 cali[obj->cvt.map[LSM6DS0_AXIS_Y]] = obj->cvt.sign[LSM6DS0_AXIS_Y]*obj->cali_sw[LSM6DS0_AXIS_Y];
291 cali[obj->cvt.map[LSM6DS0_AXIS_Z]] = obj->cvt.sign[LSM6DS0_AXIS_Z]*obj->cali_sw[LSM6DS0_AXIS_Z];
292 cali[LSM6DS0_AXIS_X] += dat[LSM6DS0_AXIS_X];
293 cali[LSM6DS0_AXIS_Y] += dat[LSM6DS0_AXIS_Y];
294 cali[LSM6DS0_AXIS_Z] += dat[LSM6DS0_AXIS_Z];
295 #if DEBUG
296 if(atomic_read(&obj->trace) & GYRO_TRC_CALI)
297 {
298 GYRO_LOG("write gyro calibration data (%5d, %5d, %5d)-->(%5d, %5d, %5d)\n",
299 dat[LSM6DS0_AXIS_X], dat[LSM6DS0_AXIS_Y], dat[LSM6DS0_AXIS_Z],
300 cali[LSM6DS0_AXIS_X],cali[LSM6DS0_AXIS_Y],cali[LSM6DS0_AXIS_Z]);
301 }
302 #endif
303 return LSM6DS0_write_rel_calibration(obj, cali);
304 }
305
306 return err;
307 }
308 /*----------------------------------------------------------------------------*/
309 static int LSM6DS0_CheckDeviceID(struct i2c_client *client)
310 {
311 u8 databuf[10];
312 int res = 0;
313
314 memset(databuf, 0, sizeof(u8)*10);
315 databuf[0] = LSM6DS0_FIXED_DEVID;
316
317 res = LSM6DS0_read_byte_sr(client,LSM6DS0_REG_DEVID,databuf,1);
318 GYRO_LOG(" LSM6DS0 id %x!\n",databuf[0]);
319 if(databuf[0]!=LSM6DS0_FIXED_DEVID)
320 {
321 return LSM6DS0_ERR_IDENTIFICATION;
322 }
323
324 //exit_MMA8453Q_CheckDeviceID:
325 if (res < 0)
326 {
327 return LSM6DS0_ERR_I2C;
328 }
329
330 return LSM6DS0_SUCCESS;
331 }
332
333
334 //----------------------------------------------------------------------------//
335 static int LSM6DS0_SetPowerMode(struct i2c_client *client, bool enable)
336 {
337 u8 databuf[2] = {0};
338 int res = 0;
339
340 if(enable == sensor_power)
341 {
342 GYRO_LOG("Sensor power status is newest!\n");
343 return LSM6DS0_SUCCESS;
344 }
345
346 if(LSM6DS0_read_byte_sr(client, LSM6DS0_CTL_REG1, databuf,1))
347 {
348 GYRO_ERR("read power ctl register err!\n");
349 return LSM6DS0_ERR_I2C;
350 }
351
352 databuf[0] &= ~LSM6DS0_POWER_ON;//clear power on bit
353 if(true == enable )
354 {
355 databuf[0] |= LSM6DS0_POWER_ON;
356 }
357 else
358 {
359 // do nothing
360 }
361 databuf[1] = databuf[0];
362 databuf[0] = LSM6DS0_CTL_REG1;
363 res = i2c_master_send(client, databuf, 0x2);
364 if(res <= 0)
365 {
366 GYRO_LOG("set power mode failed!\n");
367 return LSM6DS0_ERR_I2C;
368 }
369 else
370 {
371 GYRO_LOG("set power mode ok %d!\n", enable);
372 }
373
374 sensor_power = enable;
375
376 return LSM6DS0_SUCCESS;
377 }
378
379 /*----------------------------------------------------------------------------*/
380
381
382 static int LSM6DS0_SetDataResolution(struct i2c_client *client, u8 dataResolution)
383 {
384 u8 databuf[2] = {0};
385 int res = 0;
386 GYRO_FUN();
387
388 if(LSM6DS0_read_byte_sr(client, LSM6DS0_CTL_REG1, databuf,1))
389 {
390 GYRO_ERR("read LSM6DS0_CTL_REG4 err!\n");
391 return LSM6DS0_ERR_I2C;
392 }
393 else
394 {
395 GYRO_LOG("read LSM6DS0_CTL_REG4 register: 0x%x\n", databuf[0]);
396 }
397
398 databuf[0] &= 0xE7;//clear
399 databuf[0] |= dataResolution;
400
401 databuf[1] = databuf[0];
402 databuf[0] = LSM6DS0_CTL_REG1;
403
404
405 res = i2c_master_send(client, databuf, 0x2);
406 if(res <= 0)
407 {
408 GYRO_ERR("write SetDataResolution register err!\n");
409 return LSM6DS0_ERR_I2C;
410 }
411 return LSM6DS0_SUCCESS;
412 }
413
414 // set the sample rate
415 static int LSM6DS0_SetSampleRate(struct i2c_client *client, u8 sample_rate)
416 {
417 u8 databuf[2] = {0};
418 int res = 0;
419 GYRO_FUN();
420
421 if(LSM6DS0_read_byte_sr(client, LSM6DS0_CTL_REG1, databuf,1))
422 {
423 GYRO_ERR("read gyro data format register err!\n");
424 return LSM6DS0_ERR_I2C;
425 }
426 else
427 {
428 GYRO_LOG("read gyro data format register: 0x%x\n", databuf[0]);
429 }
430
431 databuf[0] &= 0x3f;//clear
432 databuf[0] |= sample_rate;
433
434 databuf[1] = databuf[0];
435 databuf[0] = LSM6DS0_CTL_REG1;
436
437
438 res = i2c_master_send(client, databuf, 0x2);
439 if(res <= 0)
440 {
441 GYRO_ERR("write sample rate register err!\n");
442 return LSM6DS0_ERR_I2C;
443 }
444
445 return LSM6DS0_SUCCESS;
446 }
447 /*----------------------------------------------------------------------------*/
448
449 static int LSM6DS0_read_byte_sr(struct i2c_client *client, u8 addr, u8 *data, u8 len)
450 {
451 int ret = 0;
452 unsigned short length = 0;
453
454 client->addr = (client->addr & I2C_MASK_FLAG) | I2C_WR_FLAG |I2C_RS_FLAG;
455 data[0] = addr;
456 length = (len << 8) | 1;
457
458 ret = i2c_master_send(client, (const char*)data, length);
459 if (ret < 0) {
460 GYRO_ERR("send command error!!\n");
461 return -EFAULT;
462 }
463
464 client->addr &= I2C_MASK_FLAG;
465
466 return 0;
467 }
468
469 /*----------------------------------------------------------------------------*/
470 static int LSM6DS0_ReadGyroData(struct i2c_client *client, char *buf, int bufsize)
471 {
472 char databuf[6]= {0};
473 int data[3];
474 struct LSM6DS0_i2c_data *obj = i2c_get_clientdata(client);
475
476 if(sensor_power == false)
477 {
478 LSM6DS0_SetPowerMode(client, true);
479 }
480
481 if(LSM6DS0_read_byte_sr(client, LSM6DS0_REG_GYRO_XL, databuf, 0x06))
482 {
483 GYRO_ERR("LSM6DS0 read gyroscope data error\n");
484 return LSM6DS0_ERR_GETGSENSORDATA;
485 }
486 else
487 {
488 obj->data[LSM6DS0_AXIS_X] = (s16)((databuf[LSM6DS0_AXIS_X*2+1] << 8) | (databuf[LSM6DS0_AXIS_X*2]));
489 obj->data[LSM6DS0_AXIS_Y] = (s16)((databuf[LSM6DS0_AXIS_Y*2+1] << 8) | (databuf[LSM6DS0_AXIS_Y*2]));
490 obj->data[LSM6DS0_AXIS_Z] = (s16)((databuf[LSM6DS0_AXIS_Z*2+1] << 8) | (databuf[LSM6DS0_AXIS_Z*2]));
491
492 #if DEBUG
493 if(atomic_read(&obj->trace) & GYRO_TRC_RAWDATA)
494 {
495 GYRO_LOG("read gyro register: %x, %x, %x, %x, %x, %x",
496 databuf[0], databuf[1], databuf[2], databuf[3], databuf[4], databuf[5]);
497 GYRO_LOG("get gyro raw data (0x%08X, 0x%08X, 0x%08X) -> (%5d, %5d, %5d)\n",
498 obj->data[LSM6DS0_AXIS_X],obj->data[LSM6DS0_AXIS_Y],obj->data[LSM6DS0_AXIS_Z],
499 obj->data[LSM6DS0_AXIS_X],obj->data[LSM6DS0_AXIS_Y],obj->data[LSM6DS0_AXIS_Z]);
500 }
501 #endif
502
503 obj->data[LSM6DS0_AXIS_X] = obj->data[LSM6DS0_AXIS_X] + obj->cali_sw[LSM6DS0_AXIS_X];
504 obj->data[LSM6DS0_AXIS_Y] = obj->data[LSM6DS0_AXIS_Y] + obj->cali_sw[LSM6DS0_AXIS_Y];
505 obj->data[LSM6DS0_AXIS_Z] = obj->data[LSM6DS0_AXIS_Z] + obj->cali_sw[LSM6DS0_AXIS_Z];
506
507 /*remap coordinate*/
508 data[obj->cvt.map[LSM6DS0_AXIS_X]] = obj->cvt.sign[LSM6DS0_AXIS_X]*obj->data[LSM6DS0_AXIS_X];
509 data[obj->cvt.map[LSM6DS0_AXIS_Y]] = obj->cvt.sign[LSM6DS0_AXIS_Y]*obj->data[LSM6DS0_AXIS_Y];
510 data[obj->cvt.map[LSM6DS0_AXIS_Z]] = obj->cvt.sign[LSM6DS0_AXIS_Z]*obj->data[LSM6DS0_AXIS_Z];
511
512
513 //Out put the degree/second(o/s)
514 data[LSM6DS0_AXIS_X] = data[LSM6DS0_AXIS_X] * LSM6DS0_OUT_MAGNIFY / LSM6DS0_FS_2000_LSB;
515 data[LSM6DS0_AXIS_Y] = data[LSM6DS0_AXIS_Y] * LSM6DS0_OUT_MAGNIFY / LSM6DS0_FS_2000_LSB;
516 data[LSM6DS0_AXIS_Z] = data[LSM6DS0_AXIS_Z] * LSM6DS0_OUT_MAGNIFY / LSM6DS0_FS_2000_LSB;
517
518 }
519
520 sprintf(buf, "%04x %04x %04x", data[LSM6DS0_AXIS_X],data[LSM6DS0_AXIS_Y],data[LSM6DS0_AXIS_Z]);
521
522 #if DEBUG
523 if(atomic_read(&obj->trace) & GYRO_TRC_DATA)
524 {
525 GYRO_LOG("get gyro data packet:[%d %d %d]\n", data[0], data[1], data[2]);
526 }
527 #endif
528
529 return 0;
530
531 }
532
533
534 #if 0
535 static int LSM6DS0_SelfTest(struct i2c_client *client)
536 {
537 int err =0;
538 u8 data=0;
539 char strbuf[LSM6DS0_BUFSIZE] = {0};
540 int avgx_NOST,avgy_NOST,avgz_NOST;
541 int sumx,sumy,sumz;
542 int avgx_ST,avgy_ST,avgz_ST;
543 int nost_x,nost_y,nost_z=0;
544 int st_x,st_y,st_z=0;
545
546 int resx,resy,resz=-1;
547 int i=0;
548 int testRes=0;
549 int sampleNum =5;
550
551 sumx=sumy=sumz=0;
552 // 1 init
553 err = LSM6DS0_init_client(client, true);
554 if(err)
555 {
556 GYRO_ERR("initialize client fail! err code %d!\n", err);
557 return - 2;
558 }
559 LSM6DS0_dumpReg(client);
560 // 2 check ZYXDA bit
561 LSM6DS0_read_byte_sr(client,LSM6DS0_STATUS_REG,&data,1);
562 GYRO_LOG("LSM6DS0_STATUS_REG=%d\n",data );
563 while(0x04 != (data&0x04))
564 {
565 msleep(10);
566 }
567 msleep(1000); //wait for stable
568 // 3 read raw data no self test data
569 for(i=0; i<sampleNum; i++)
570 {
571 LSM6DS0_ReadGyroData(client, strbuf, LSM6DS0_BUFSIZE);
572 sscanf(strbuf, "%x %x %x", &nost_x, &nost_y, &nost_z);
573 GYRO_LOG("NOst %d %d %d!\n", nost_x,nost_y,nost_z);
574 sumx += nost_x;
575 sumy += nost_y;
576 sumz += nost_z;
577 msleep(10);
578 }
579 //calculate avg x y z
580 avgx_NOST = sumx/sampleNum;
581 avgy_NOST = sumy/sampleNum;
582 avgz_NOST = sumz/sampleNum;
583 GYRO_LOG("avg NOST %d %d %d!\n", avgx_NOST,avgy_NOST,avgz_NOST);
584
585 // 4 enalbe selftest
586 LSM6DS0_read_byte_sr(client,LSM6DS0_CTL_REG4,&data,1);
587 data = data | 0x02;
588 hwmsen_write_byte(client,LSM6DS0_CTL_REG4,data);
589
590 msleep(1000);//wait for stable
591
592 LSM6DS0_dumpReg(client);
593 // 5 check ZYXDA bit
594
595 //6 read raw data self test data
596 sumx=0;
597 sumy=0;
598 sumz=0;
599 for(i=0; i<sampleNum; i++)
600 {
601 LSM6DS0_ReadGyroData(client, strbuf, LSM6DS0_BUFSIZE);
602 sscanf(strbuf, "%x %x %x", &st_x, &st_y, &st_z);
603 GYRO_LOG("st %d %d %d!\n", st_x,st_y,st_z);
604
605 sumx += st_x;
606 sumy += st_y;
607 sumz += st_z;
608
609 msleep(10);
610 }
611 // 7 calc calculate avg x y z ST
612 avgx_ST = sumx/sampleNum;
613 avgy_ST = sumy/sampleNum;
614 avgz_ST = sumz/sampleNum;
615 //GYRO_LOG("avg ST %d %d %d!\n", avgx_ST,avgy_ST,avgz_ST);
616 //GYRO_LOG("abs(avgx_ST-avgx_NOST): %ld \n", abs(avgx_ST-avgx_NOST));
617 //GYRO_LOG("abs(avgy_ST-avgy_NOST): %ld \n", abs(avgy_ST-avgy_NOST));
618 //GYRO_LOG("abs(avgz_ST-avgz_NOST): %ld \n", abs(avgz_ST-avgz_NOST));
619
620 if((abs(avgx_ST-avgx_NOST)>=175*131) && (abs(avgx_ST-avgx_NOST)<=875*131))
621 {
622 resx =0; //x axis pass
623 GYRO_LOG(" x axis pass\n" );
624 }
625 if((abs(avgy_ST-avgy_NOST)>=175*131) && (abs(avgy_ST-avgy_NOST)<=875*131))
626 {
627 resy =0; //y axis pass
628 GYRO_LOG(" y axis pass\n" );
629 }
630 if((abs(avgz_ST-avgz_NOST)>=175*131) && (abs(avgz_ST-avgz_NOST)<=875*131))
631 {
632 resz =0; //z axis pass
633 GYRO_LOG(" z axis pass\n" );
634 }
635
636 if(0==resx && 0==resy && 0==resz)
637 {
638 testRes = 0;
639 }
640 else
641 {
642 testRes = -1;
643 }
644
645 hwmsen_write_byte(client,LSM6DS0_CTL_REG4,0x00);
646 err = LSM6DS0_init_client(client, false);
647 if(err)
648 {
649 GYRO_ERR("initialize client fail! err code %d!\n", err);
650 return -2;
651 }
652 GYRO_LOG("testRes %d!\n", testRes);
653 return testRes;
654
655 }
656
657 //self test for factory
658 static int LSM6DS0_SMTReadSensorData(struct i2c_client *client)
659 {
660 //S16 gyro[LSM6DS0_AXES_NUM*LSM6DS0_FIFOSIZE];
661 int res = 0;
662
663 GYRO_FUN();
664 res = LSM6DS0_SelfTest(client);
665
666 GYRO_LOG(" LSM6DS0_SMTReadSensorData %d", res );
667
668 return res;
669 }
670
671 #endif
672 /*----------------------------------------------------------------------------*/
673 static int LSM6DS0_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
674 {
675 u8 databuf[10];
676
677 memset(databuf, 0, sizeof(u8)*10);
678
679 if((NULL == buf)||(bufsize<=30))
680 {
681 return -1;
682 }
683
684 if(NULL == client)
685 {
686 *buf = 0;
687 return -2;
688 }
689
690 sprintf(buf, "LSM6DS0 Chip");
691 return 0;
692 }
693
694
695 /*----------------------------------------------------------------------------*/
696 static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
697 {
698 struct i2c_client *client = LSM6DS0_i2c_client;
699 char strbuf[LSM6DS0_BUFSIZE];
700 if(NULL == client)
701 {
702 GYRO_ERR("i2c client is null!!\n");
703 return 0;
704 }
705
706 LSM6DS0_ReadChipInfo(client, strbuf, LSM6DS0_BUFSIZE);
707 return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
708 }
709 /*----------------------------------------------------------------------------*/
710 static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
711 {
712 struct i2c_client *client = LSM6DS0_i2c_client;
713 char strbuf[LSM6DS0_BUFSIZE]= {0};
714
715 if(NULL == client)
716 {
717 GYRO_ERR("i2c client is null!!\n");
718 return 0;
719 }
720
721 LSM6DS0_ReadGyroData(client, strbuf, LSM6DS0_BUFSIZE);
722 return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);;
723 }
724
725 /*----------------------------------------------------------------------------*/
726 static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
727 {
728 ssize_t res;
729 struct LSM6DS0_i2c_data *obj = obj_i2c_data;
730 if (obj == NULL)
731 {
732 GYRO_ERR("i2c_data obj is null!!\n");
733 return 0;
734 }
735
736 res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));
737 return res;
738 }
739 /*----------------------------------------------------------------------------*/
740 static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
741 {
742 struct LSM6DS0_i2c_data *obj = obj_i2c_data;
743 int trace;
744 if (obj == NULL)
745 {
746 GYRO_ERR("i2c_data obj is null!!\n");
747 return 0;
748 }
749
750 if(1 == sscanf(buf, "0x%x", &trace))
751 {
752 atomic_set(&obj->trace, trace);
753 }
754 else
755 {
756 GYRO_ERR("invalid content: '%s', length = %d\n", buf, count);
757 }
758
759 return count;
760 }
761 /*----------------------------------------------------------------------------*/
762 static ssize_t show_status_value(struct device_driver *ddri, char *buf)
763 {
764 ssize_t len = 0;
765 struct LSM6DS0_i2c_data *obj = obj_i2c_data;
766 if (obj == NULL)
767 {
768 GYRO_ERR("i2c_data obj is null!!\n");
769 return 0;
770 }
771
772 if(obj->hw)
773 {
774 len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n",
775 obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);
776 }
777 else
778 {
779 len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
780 }
781 return len;
782 }
783 /*----------------------------------------------------------------------------*/
784
785 static DRIVER_ATTR(chipinfo, S_IRUGO, show_chipinfo_value, NULL);
786 static DRIVER_ATTR(sensordata, S_IRUGO, show_sensordata_value, NULL);
787 static DRIVER_ATTR(trace, S_IWUGO | S_IRUGO, show_trace_value, store_trace_value);
788 static DRIVER_ATTR(status, S_IRUGO, show_status_value, NULL);
789 /*----------------------------------------------------------------------------*/
790 static struct driver_attribute *LSM6DS0_attr_list[] = {
791 &driver_attr_chipinfo, /*chip information*/
792 &driver_attr_sensordata, /*dump sensor data*/
793 &driver_attr_trace, /*trace log*/
794 &driver_attr_status,
795 };
796 /*----------------------------------------------------------------------------*/
797 static int LSM6DS0_create_attr(struct device_driver *driver)
798 {
799 int idx, err = 0;
800 int num = (int)(sizeof(LSM6DS0_attr_list)/sizeof(LSM6DS0_attr_list[0]));
801 if (driver == NULL)
802 {
803 return -EINVAL;
804 }
805
806 for(idx = 0; idx < num; idx++)
807 {
808 if(0 != (err = driver_create_file(driver, LSM6DS0_attr_list[idx])))
809 {
810 GYRO_ERR("driver_create_file (%s) = %d\n", LSM6DS0_attr_list[idx]->attr.name, err);
811 break;
812 }
813 }
814 return err;
815 }
816 /*----------------------------------------------------------------------------*/
817 static int LSM6DS0_delete_attr(struct device_driver *driver)
818 {
819 int idx ,err = 0;
820 int num = (int)(sizeof(LSM6DS0_attr_list)/sizeof(LSM6DS0_attr_list[0]));
821
822 if(driver == NULL)
823 {
824 return -EINVAL;
825 }
826
827
828 for(idx = 0; idx < num; idx++)
829 {
830 driver_remove_file(driver, LSM6DS0_attr_list[idx]);
831 }
832
833
834 return err;
835 }
836
837 /*open drain mode set*/
838 static int LSM6DS0_PP_OD_init(client)
839 {
840 u8 databuf[2] = {0};
841 int res = 0;
842 GYRO_FUN();
843
844 databuf[1] = 0x14;
845 databuf[0] = LSM6DS0_CTL_REG8;
846 res = i2c_master_send(client, databuf, 0x2);
847 if(res <= 0)
848 {
849 GYRO_LOG("set PP_OD mode failed!\n");
850 return LSM6DS0_ERR_I2C;
851 }
852
853 return LSM6DS0_SUCCESS;
854 }
855
856 /*----------------------------------------------------------------------------*/
857 static int LSM6DS0_init_client(struct i2c_client *client, bool enable)
858 {
859 struct LSM6DS0_i2c_data *obj = i2c_get_clientdata(client);
860 int res = 0;
861 int i = 0;
862 GYRO_FUN();
863 GYRO_LOG(" fwq LSM6DS0 addr %x!\n",client->addr);
864 #if 0
865 res = LSM6DS0_PP_OD_init(client);
866 if(res != LSM6DS0_SUCCESS)
867 {
868 GYRO_LOG("set PP_OD mode failed!\n");
869 return res;
870 }
871 #endif
872 for(i = 0; i < 5; i++)
873 {
874 res = LSM6DS0_CheckDeviceID(client);
875 if(res = LSM6DS0_SUCCESS)
876 {
877 GYRO_LOG("check success time %d !\n", i);
878 break;
879 }
880 }
881
882 res = LSM6DS0_SetPowerMode(client, enable);
883 GYRO_LOG("LSM6DS0_SetPowerMode res = %x\n", res);
884
885 if(res != LSM6DS0_SUCCESS)
886 {
887 return res;
888 }
889
890 // The range should at least be 17.45 rad/s (ie: ~1000 deg/s).
891 res = LSM6DS0_SetDataResolution(client,LSM6DS0_RANGE_2000);
892 GYRO_LOG("LSM6DS0_SetDataResolution res = %x\n", res);
893
894 if(res != LSM6DS0_SUCCESS)
895 {
896 return res;
897 }
898
899 #if 0
900 res = LSM6DS0_SetSampleRate(client, LSM6DS0_100HZ);
901 GYRO_LOG("LSM6DS0_SetSampleRate res = %x\n", res);
902
903 if(res != LSM6DS0_SUCCESS )
904 {
905 return res;
906 }
907 #endif
908
909 GYRO_LOG("LSM6DS0_init_client OK!\n");
910
911 #ifdef CONFIG_LSM6DS0_LOWPASS
912 memset(&obj->fir, 0x00, sizeof(obj->fir));
913 #endif
914
915 return LSM6DS0_SUCCESS;
916 }
917
918 /*----------------------------------------------------------------------------*/
919 int LSM6DS0_operate(void* self, uint32_t command, void* buff_in, int size_in,
920 void* buff_out, int size_out, int* actualout)
921 {
922 int err = 0;
923 int value;
924 struct LSM6DS0_i2c_data *priv = (struct LSM6DS0_i2c_data*)self;
925 hwm_sensor_data* gyro_data;
926 char buff[LSM6DS0_BUFSIZE];
927
928 switch (command)
929 {
930 case SENSOR_DELAY:
931 if((buff_in == NULL) || (size_in < sizeof(int)))
932 {
933 GYRO_ERR("Set delay parameter error!\n");
934 err = -EINVAL;
935 }
936 else
937 {
938
939 }
940 break;
941
942 case SENSOR_ENABLE:
943 if((buff_in == NULL) || (size_in < sizeof(int)))
944 {
945 GYRO_ERR("Enable gyroscope parameter error!\n");
946 err = -EINVAL;
947 }
948 else
949 {
950 value = *(int *)buff_in;
951 if(((value == 0) && (sensor_power == false)) ||((value == 1) && (sensor_power == true)))
952 {
953 GYRO_LOG("gyroscope device have updated!\n");
954 }
955 else
956 {
957 err = LSM6DS0_SetPowerMode(priv->client, !sensor_power);
958 }
959 }
960 break;
961
962 case SENSOR_GET_DATA:
963 if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
964 {
965 GYRO_ERR("get gyroscope data parameter error!\n");
966 err = -EINVAL;
967 }
968 else
969 {
970 gyro_data = (hwm_sensor_data *)buff_out;
971 LSM6DS0_ReadGyroData(priv->client, buff, LSM6DS0_BUFSIZE);
972 sscanf(buff, "%x %x %x", &gyro_data->values[0],
973 &gyro_data->values[1], &gyro_data->values[2]);
974 gyro_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;
975 gyro_data->value_divide = DEGREE_TO_RAD;
976 }
977 break;
978 default:
979 GYRO_ERR("gyroscope operate function no this parameter %d!\n", command);
980 err = -1;
981 break;
982 }
983
984 return err;
985 }
986
987 /******************************************************************************
988 * Function Configuration
989 ******************************************************************************/
990 static int LSM6DS0_open(struct inode *inode, struct file *file)
991 {
992 file->private_data = LSM6DS0_i2c_client;
993
994 if(file->private_data == NULL)
995 {
996 GYRO_ERR("null pointer!!\n");
997 return -EINVAL;
998 }
999 return nonseekable_open(inode, file);
1000 }
1001 /*----------------------------------------------------------------------------*/
1002 static int LSM6DS0_release(struct inode *inode, struct file *file)
1003 {
1004 file->private_data = NULL;
1005 return 0;
1006 }
1007 /*----------------------------------------------------------------------------*/
1008 //static int LSM6DS0_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
1009 // unsigned long arg)
1010 static long LSM6DS0_ioctl(struct file *file, unsigned int cmd,
1011 unsigned long arg)
1012 {
1013 struct i2c_client *client = (struct i2c_client*)file->private_data;
1014 //struct LSM6DS0_i2c_data *obj = (struct LSM6DS0_i2c_data*)i2c_get_clientdata(client);
1015 char strbuf[LSM6DS0_BUFSIZE] = {0};
1016 void __user *data;
1017 long err = 0;
1018 int copy_cnt = 0;
1019 SENSOR_DATA sensor_data;
1020 int cali[3];
1021 int smtRes=0;
1022 //GYRO_FUN();
1023
1024 if(_IOC_DIR(cmd) & _IOC_READ)
1025 {
1026 err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
1027 }
1028 else if(_IOC_DIR(cmd) & _IOC_WRITE)
1029 {
1030 err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
1031 }
1032
1033 if(err)
1034 {
1035 GYRO_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
1036 return -EFAULT;
1037 }
1038
1039 switch(cmd)
1040 {
1041 case GYROSCOPE_IOCTL_INIT:
1042 LSM6DS0_init_client(client, false);
1043 break;
1044
1045 case GYROSCOPE_IOCTL_SMT_DATA:
1046 #if 0
1047 data = (void __user *) arg;
1048 if(data == NULL)
1049 {
1050 err = -EINVAL;
1051 break;
1052 }
1053
1054 smtRes = LSM6DS0_SMTReadSensorData(client);
1055 GYRO_LOG("IOCTL smtRes: %d!\n", smtRes);
1056 copy_cnt = copy_to_user(data, &smtRes, sizeof(smtRes));
1057
1058 if(copy_cnt)
1059 {
1060 err = -EFAULT;
1061 GYRO_ERR("copy gyro data to user failed!\n");
1062 }
1063 #endif
1064
1065 GYRO_LOG("GYROSCOPE SMT Test not support !\n");
1066 break;
1067
1068
1069 case GYROSCOPE_IOCTL_READ_SENSORDATA:
1070 data = (void __user *) arg;
1071 if(data == NULL)
1072 {
1073 err = -EINVAL;
1074 break;
1075 }
1076
1077 LSM6DS0_ReadGyroData(client, strbuf, LSM6DS0_BUFSIZE);
1078 if(copy_to_user(data, strbuf, sizeof(strbuf)))
1079 {
1080 err = -EFAULT;
1081 break;
1082 }
1083 break;
1084
1085 case GYROSCOPE_IOCTL_SET_CALI:
1086 data = (void __user*)arg;
1087 if(data == NULL)
1088 {
1089 err = -EINVAL;
1090 break;
1091 }
1092 if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
1093 {
1094 err = -EFAULT;
1095 break;
1096 }
1097
1098 else
1099 {
1100 cali[LSM6DS0_AXIS_X] = sensor_data.x * LSM6DS0_FS_2000_LSB / LSM6DS0_OUT_MAGNIFY;
1101 cali[LSM6DS0_AXIS_Y] = sensor_data.y * LSM6DS0_FS_2000_LSB / LSM6DS0_OUT_MAGNIFY;
1102 cali[LSM6DS0_AXIS_Z] = sensor_data.z * LSM6DS0_FS_2000_LSB / LSM6DS0_OUT_MAGNIFY;
1103 err = LSM6DS0_WriteCalibration(client, cali);
1104 }
1105 break;
1106
1107 case GYROSCOPE_IOCTL_CLR_CALI:
1108 err = LSM6DS0_ResetCalibration(client);
1109 break;
1110
1111 case GYROSCOPE_IOCTL_GET_CALI:
1112 data = (void __user*)arg;
1113 if(data == NULL)
1114 {
1115 err = -EINVAL;
1116 break;
1117 }
1118 err = LSM6DS0_ReadCalibration(client, cali);
1119 if(err)
1120 {
1121 break;
1122 }
1123
1124 sensor_data.x = cali[LSM6DS0_AXIS_X] * LSM6DS0_OUT_MAGNIFY / LSM6DS0_FS_2000_LSB;
1125 sensor_data.y = cali[LSM6DS0_AXIS_Y] * LSM6DS0_OUT_MAGNIFY / LSM6DS0_FS_2000_LSB;
1126 sensor_data.z = cali[LSM6DS0_AXIS_Z] * LSM6DS0_OUT_MAGNIFY / LSM6DS0_FS_2000_LSB;
1127 if(copy_to_user(data, &sensor_data, sizeof(sensor_data)))
1128 {
1129 err = -EFAULT;
1130 break;
1131 }
1132 break;
1133
1134 default:
1135 GYRO_ERR("unknown IOCTL: 0x%08x\n", cmd);
1136 err = -ENOIOCTLCMD;
1137 break;
1138 }
1139 return err;
1140 }
1141
1142
1143 /*----------------------------------------------------------------------------*/
1144 static struct file_operations LSM6DS0_fops = {
1145 // .owner = THIS_MODULE,
1146 .open = LSM6DS0_open,
1147 .release = LSM6DS0_release,
1148 .unlocked_ioctl = LSM6DS0_ioctl,
1149 };
1150 /*----------------------------------------------------------------------------*/
1151 static struct miscdevice LSM6DS0_device = {
1152 .minor = MISC_DYNAMIC_MINOR,
1153 .name = "gyroscope",
1154 .fops = &LSM6DS0_fops,
1155 };
1156 /*----------------------------------------------------------------------------*/
1157 #ifndef CONFIG_HAS_EARLYSUSPEND
1158 /*----------------------------------------------------------------------------*/
1159 static int LSM6DS0_suspend(struct i2c_client *client, pm_message_t msg)
1160 {
1161 struct LSM6DS0_i2c_data *obj = i2c_get_clientdata(client);
1162 GYRO_FUN();
1163
1164 if(msg.event == PM_EVENT_SUSPEND)
1165 {
1166 if(obj == NULL)
1167 {
1168 GYRO_ERR("null pointer!!\n");
1169 return -EINVAL;
1170 }
1171 atomic_set(&obj->suspend, 1);
1172
1173 err = LSM6DS0_SetPowerMode(client, false);
1174 if(err <= 0)
1175 {
1176 return err;
1177 }
1178 }
1179 return err;
1180 }
1181 /*----------------------------------------------------------------------------*/
1182 static int LSM6DS0_resume(struct i2c_client *client)
1183 {
1184 struct LSM6DS0_i2c_data *obj = i2c_get_clientdata(client);
1185 int err;
1186 GYRO_FUN();
1187
1188 if(obj == NULL)
1189 {
1190 GYRO_ERR("null pointer!!\n");
1191 return -EINVAL;
1192 }
1193
1194 LSM6DS0_power(obj->hw, 1);
1195 err = LSM6DS0_init_client(client, false);
1196 if(err)
1197 {
1198 GYRO_ERR("initialize client fail!!\n");
1199 return err;
1200 }
1201 atomic_set(&obj->suspend, 0);
1202
1203 return 0;
1204 }
1205 /*----------------------------------------------------------------------------*/
1206 #else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
1207 /*----------------------------------------------------------------------------*/
1208 static void LSM6DS0_early_suspend(struct early_suspend *h)
1209 {
1210 struct LSM6DS0_i2c_data *obj = container_of(h, struct LSM6DS0_i2c_data, early_drv);
1211 int err;
1212 GYRO_FUN();
1213
1214 if(obj == NULL)
1215 {
1216 GYRO_ERR("null pointer!!\n");
1217 return;
1218 }
1219
1220 atomic_set(&obj->suspend, 1);
1221 err = LSM6DS0_SetPowerMode(obj->client, false);
1222 if(err)
1223 {
1224 GYRO_ERR(" LSM6DS0_early_suspend write power control fail!!\n");
1225 return;
1226 }
1227
1228 LSM6DS0_power(obj->hw, 0);
1229
1230 return;
1231 }
1232 /*----------------------------------------------------------------------------*/
1233 static void LSM6DS0_late_resume(struct early_suspend *h)
1234 {
1235 struct LSM6DS0_i2c_data *obj = container_of(h, struct LSM6DS0_i2c_data, early_drv);
1236 int err;
1237 GYRO_FUN();
1238
1239 if(obj == NULL)
1240 {
1241 GYRO_ERR("null pointer!!\n");
1242 return;
1243 }
1244
1245 atomic_set(&obj->suspend, 0);
1246 err = LSM6DS0_SetPowerMode(obj->client, true);
1247 if(err)
1248 {
1249 GYRO_ERR("LSM6DS0_late_resume write power control fail!!\n");
1250 return;
1251 }
1252
1253 LSM6DS0_power(obj->hw, 1);
1254
1255 return;
1256 }
1257 /*----------------------------------------------------------------------------*/
1258 #endif /*CONFIG_HAS_EARLYSUSPEND*/
1259 /*----------------------------------------------------------------------------*/
1260 static int LSM6DS0_i2c_detect(struct i2c_client *client, int kind, struct i2c_board_info *info)
1261 {
1262 strcpy(info->type, LSM6DS0_DEV_NAME);
1263 return 0;
1264 }
1265
1266 /*----------------------------------------------------------------------------*/
1267 static int LSM6DS0_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
1268 {
1269 struct i2c_client *new_client;
1270 struct LSM6DS0_i2c_data *obj;
1271 struct hwmsen_object sobj;
1272 int err = 0;
1273 GYRO_FUN();
1274
1275 if(!(obj = kzalloc(sizeof(struct LSM6DS0_i2c_data), GFP_KERNEL)))
1276 {
1277 err = -ENOMEM;
1278 goto exit;
1279 }
1280
1281 memset(obj, 0, sizeof(struct LSM6DS0_i2c_data));
1282
1283 obj->hw = get_cust_gyro_hw();
1284 err = hwmsen_get_convert(obj->hw->direction, &obj->cvt);
1285 if(err)
1286 {
1287 GYRO_ERR("invalid direction: %d\n", obj->hw->direction);
1288 goto exit;
1289 }
1290
1291 obj_i2c_data = obj;
1292 obj->client = client;
1293 new_client = obj->client;
1294 i2c_set_clientdata(new_client,obj);
1295
1296 atomic_set(&obj->trace, 0);
1297 atomic_set(&obj->suspend, 0);
1298
1299 LSM6DS0_i2c_client = new_client;
1300 err = LSM6DS0_init_client(new_client, false);
1301 if(err)
1302 {
1303 goto exit_init_failed;
1304 }
1305
1306 err = misc_register(&LSM6DS0_device);
1307 if(err)
1308 {
1309 GYRO_ERR("LSM6DS0_device misc register failed!\n");
1310 goto exit_misc_device_register_failed;
1311 }
1312
1313 err = LSM6DS0_create_attr(&LSM6DS0_gyro_driver.driver);
1314 if(err)
1315 {
1316 GYRO_ERR("LSM6DS0 create attribute err = %d\n", err);
1317 goto exit_create_attr_failed;
1318 }
1319
1320 sobj.self = obj;
1321 sobj.polling = 1;
1322 sobj.sensor_operate = LSM6DS0_operate;
1323 err = hwmsen_attach(ID_GYROSCOPE, &sobj);
1324 if(err)
1325 {
1326 GYRO_ERR("hwmsen_attach fail = %d\n", err);
1327 goto exit_kfree;
1328 }
1329
1330 #ifdef CONFIG_HAS_EARLYSUSPEND
1331 obj->early_drv.level = EARLY_SUSPEND_LEVEL_STOP_DRAWING - 2,
1332 obj->early_drv.suspend = LSM6DS0_early_suspend,
1333 obj->early_drv.resume = LSM6DS0_late_resume,
1334 register_early_suspend(&obj->early_drv);
1335 #endif
1336
1337 GYRO_LOG("%s: OK\n", __func__);
1338 return 0;
1339
1340 exit_create_attr_failed:
1341 misc_deregister(&LSM6DS0_device);
1342 exit_misc_device_register_failed:
1343 exit_init_failed:
1344 //i2c_detach_client(new_client);
1345 exit_kfree:
1346 kfree(obj);
1347 exit:
1348 GYRO_ERR("%s: err = %d\n", __func__, err);
1349 return err;
1350 }
1351
1352 /*----------------------------------------------------------------------------*/
1353 static int LSM6DS0_i2c_remove(struct i2c_client *client)
1354 {
1355 int err = 0;
1356
1357 err = LSM6DS0_delete_attr(&LSM6DS0_gyro_driver.driver);
1358 if(err)
1359 {
1360 GYRO_ERR("LSM6DS0_delete_attr fail: %d\n", err);
1361 }
1362
1363 err = misc_deregister(&LSM6DS0_device);
1364 if(err)
1365 {
1366 GYRO_ERR("misc_deregister fail: %d\n", err);
1367 }
1368
1369 err = hwmsen_detach(ID_ACCELEROMETER);
1370 if(err)
1371 {
1372 GYRO_ERR("hwmsen_detach fail: %d\n", err);
1373 }
1374
1375 LSM6DS0_i2c_client = NULL;
1376 i2c_unregister_device(client);
1377 kfree(i2c_get_clientdata(client));
1378 return 0;
1379 }
1380 /*----------------------------------------------------------------------------*/
1381 static int LSM6DS0_probe(struct platform_device *pdev)
1382 {
1383 struct gyro_hw *hw = get_cust_gyro_hw();
1384 GYRO_FUN();
1385
1386 LSM6DS0_power(hw, 1);
1387 // LSM6DS0_force[0] = hw->i2c_num;
1388 if(i2c_add_driver(&LSM6DS0_i2c_driver))
1389 {
1390 GYRO_ERR("add driver error\n");
1391 return -1;
1392 }
1393 return 0;
1394 }
1395 /*----------------------------------------------------------------------------*/
1396 static int LSM6DS0_remove(struct platform_device *pdev)
1397 {
1398 struct gyro_hw *hw = get_cust_gyro_hw();
1399
1400 GYRO_FUN();
1401 LSM6DS0_power(hw, 0);
1402 i2c_del_driver(&LSM6DS0_i2c_driver);
1403 return 0;
1404 }
1405 /*----------------------------------------------------------------------------*/
1406 #if 1
1407 #ifdef CONFIG_OF
1408 static const struct of_device_id gyroscope_of_match[] = {
1409 { .compatible = "mediatek,gyroscope", },
1410 {},
1411 };
1412 #endif
1413
1414 static struct platform_driver LSM6DS0_gyro_driver = {
1415 .probe = LSM6DS0_probe,
1416 .remove = LSM6DS0_remove,
1417 .driver =
1418 {
1419 .name = "gyroscope",
1420 // .owner = THIS_MODULE,
1421 #ifdef CONFIG_OF
1422 .of_match_table = gyroscope_of_match,
1423 #endif
1424 }
1425 };
1426 #else
1427
1428 static struct platform_driver LSM6DS0_gyro_driver = {
1429 .probe = LSM6DS0_probe,
1430 .remove = LSM6DS0_remove,
1431 .driver = {
1432 .name = "gyroscope",
1433 // .owner = THIS_MODULE,
1434 }
1435 };
1436 #endif
1437 /*----------------------------------------------------------------------------*/
1438 static int __init LSM6DS0_init(void)
1439 {
1440 struct gyro_hw *hw = get_cust_gyro_hw();
1441 GYRO_LOG("%s: i2c_number=%d\n", __func__,hw->i2c_num);
1442 i2c_register_board_info(hw->i2c_num, &i2c_LSM6DS0, 1);
1443 if(platform_driver_register(&LSM6DS0_gyro_driver))
1444 {
1445 GYRO_ERR("failed to register driver");
1446 return -ENODEV;
1447 }
1448 return 0;
1449 }
1450 /*----------------------------------------------------------------------------*/
1451 static void __exit LSM6DS0_exit(void)
1452 {
1453 GYRO_FUN();
1454 platform_driver_unregister(&LSM6DS0_gyro_driver);
1455 }
1456 /*----------------------------------------------------------------------------*/
1457 module_init(LSM6DS0_init);
1458 module_exit(LSM6DS0_exit);
1459 /*----------------------------------------------------------------------------*/
1460 MODULE_LICENSE("GPL");
1461 MODULE_DESCRIPTION("LSM6DS0 gyroscope driver");
1462 MODULE_AUTHOR("Chunlei.Wang@mediatek.com");
kernel source for telekom puls (alcatel/mediatek)