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 / accelerometer / k2dh / k2dh.c
1 /* K2DH motion sensor driver
2 *
3 *
4 * This software program is licensed subject to the GNU General Public License
5 * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
6
7 * (C) Copyright 2010 STMicroelectronics
8 * All Rights Reserved
9 */
10
11 #include <linux/interrupt.h>
12 #include <linux/i2c.h>
13 #include <linux/slab.h>
14 #include <linux/irq.h>
15 #include <linux/miscdevice.h>
16 #include <asm/uaccess.h>
17 #include <linux/delay.h>
18 #include <linux/input.h>
19 #include <linux/workqueue.h>
20 #include <linux/kobject.h>
21 #include <linux/earlysuspend.h>
22 #include <linux/platform_device.h>
23 #include <asm/atomic.h>
24 #include <linux/module.h>
25
26 #define MT6582
27
28 #ifdef MT6516
29 #include <mach/mt6516_devs.h>
30 #include <mach/mt6516_typedefs.h>
31 #include <mach/mt6516_gpio.h>
32 #include <mach/mt6516_pll.h>
33 #endif
34
35 #ifdef MT6573
36 #include <mach/mt6573_devs.h>
37 #include <mach/mt6573_typedefs.h>
38 #include <mach/mt6573_gpio.h>
39 #include <mach/mt6573_pll.h>
40 #endif
41
42 #ifdef MT6575
43 #include <mach/mt_devs.h>
44 #include <mach/mt_typedefs.h>
45 #include <mach/mt_gpio.h>
46 #include <mach/mt_pm_ldo.h>
47 #endif
48
49 #ifdef MT6577
50 #include <mach/mt6577_devs.h>
51 #include <mach/mt6577_typedefs.h>
52 #include <mach/mt6577_gpio.h>
53 #include <mach/mt6577_pm_ldo.h>
54 #endif
55
56 #ifdef MT6572
57 #include <mach/devs.h>
58 #include <mach/mt_typedefs.h>
59 #include <mach/mt_gpio.h>
60 #include <mach/mt_pm_ldo.h>
61 #endif
62
63 #ifdef MT6582
64 #include <mach/devs.h>
65 #include <mach/mt_typedefs.h>
66 #include <mach/mt_gpio.h>
67 #include <mach/mt_pm_ldo.h>
68 #endif
69
70 #ifdef MT6516
71 #define POWER_NONE_MACRO MT6516_POWER_NONE
72 #endif
73
74 #ifdef MT6573
75 #define POWER_NONE_MACRO MT65XX_POWER_NONE
76 #endif
77
78 #ifdef MT6575
79 #define POWER_NONE_MACRO MT65XX_POWER_NONE
80 #endif
81
82 #ifdef MT6577
83 #define POWER_NONE_MACRO MT65XX_POWER_NONE
84 #endif
85
86 #ifdef MT6572
87 #define POWER_NONE_MACRO MT65XX_POWER_NONE
88 #endif
89
90 #ifdef MT6582
91 #define POWER_NONE_MACRO MT65XX_POWER_NONE
92 #endif
93
94
95 #include <cust_acc.h>
96 #include <linux/hwmsensor.h>
97 #include <linux/hwmsen_dev.h>
98 #include <linux/sensors_io.h>
99 #include "k2dh.h"
100 #include <linux/hwmsen_helper.h>
101
102 /*----------------------------------------------------------------------------*/
103 #define DEBUG 1
104 /*----------------------------------------------------------------------------*/
105 //#define CONFIG_K2DH_LOWPASS /*apply low pass filter on output*/
106
107 #define K2DH_AXIS_X 0
108 #define K2DH_AXIS_Y 1
109 #define K2DH_AXIS_Z 2
110 #define K2DH_AXES_NUM 3
111 #define K2DH_DATA_LEN 6
112
113 #define K2DH_MODE_LOWPOWER 0
114 #define K2DH_MODE_NORMAL 1
115 #define K2DH_MODE_HIGH_RESOLUTION 2
116
117 #define CALIBRATION_DATA_AMOUNT 10
118
119 #define K2DH_ACCEL_CALIBRATION // Accelerometer Sensor Calibration Enable Feature
120 /*----------------------------------------------------------------------------*/
121 #define HIGH_RESOLUTION 0x08
122
123 #define AXISDATA_REG 0x28
124 #define WHOAMI_K2DH_ACC 0x33 /* Expctd content for WAI */
125
126 /* CONTROL REGISTERS */
127 #define WHO_AM_I 0x0F /* WhoAmI register */
128 #define TEMP_CFG_REG 0x1F /* temper sens control reg */
129 /* ctrl 1: ODR3 ODR2 ODR ODR0 LPen Zenable Yenable Zenable */
130 #define CTRL_REG1 0x20 /* control reg 1 */
131 #define CTRL_REG2 0x21 /* control reg 2 */
132 #define CTRL_REG3 0x22 /* control reg 3 */
133 #define CTRL_REG4 0x23 /* control reg 4 */
134 #define CTRL_REG5 0x24 /* control reg 5 */
135 #define CTRL_REG6 0x25 /* control reg 6 */
136
137 #define FIFO_CTRL_REG 0x2E /* FiFo control reg */
138
139 #define INT_CFG1 0x30 /* interrupt 1 config */
140 #define INT_SRC1 0x31 /* interrupt 1 source */
141 #define INT_THS1 0x32 /* interrupt 1 threshold */
142 #define INT_DUR1 0x33 /* interrupt 1 duration */
143
144 #define INT_CFG2 0x34 /* interrupt 2 config */
145 #define INT_SRC2 0x35 /* interrupt 2 source */
146 #define INT_THS2 0x36 /* interrupt 2 threshold */
147 #define INT_DUR2 0x37 /* interrupt 2 duration */
148
149 #define TT_CFG 0x38 /* tap config */
150 #define TT_SRC 0x39 /* tap source */
151 #define TT_THS 0x3A /* tap threshold */
152 #define TT_LIM 0x3B /* tap time limit */
153 #define TT_TLAT 0x3C /* tap time latency */
154 #define TT_TW 0x3D /* tap time window */
155 /* end CONTROL REGISTRES */
156
157 #define K2DH_ACC_ENABLE_ALL_AXES 0x07
158 #define K2DH_ACC_ENABLE_ALL_AXES_MSK 0x07
159
160 #define ODR_POWERDOWN_MODE 0x00 /* Power Down Mode */
161 #define ODR1 0x10 /* 1Hz output data rate */
162 #define ODR10 0x20 /* 10Hz output data rate */
163 #define ODR25 0x30 /* 25Hz output data rate */
164 #define ODR50 0x40 /* 50Hz output data rate */
165 #define ODR100 0x50 /* 100Hz output data rate */
166 #define ODR200 0x60 /* 200Hz output data rate */ // default speed
167 #define ODR400 0x70 /* 400Hz output data rate */
168 #define ODR1344 0x90 /* 1344Hz output data rate */
169
170 #define I2C_RETRY_DELAY 5
171 #define I2C_RETRIES 5
172 #define I2C_AUTO_INCREMENT 0x80
173
174 #define K2DH_LP_EN_MSK 0x08
175 #define K2DH_LP_EN_POS 3
176 #define K2DH_LP_EN_REG CTRL_REG1
177
178 #define K2DH_HR_MSK 0x08
179 #define K2DH_HR_POS 3
180 #define K2DH_HR_REG CTRL_REG4
181
182 #define K2DH_FS_MSK 0x30
183 #define K2DH_FS_POS 4
184 #define K2DH_FS_REG CTRL_REG4
185
186 #define K2DH_ODR_MSK 0xF0
187 #define K2DH_ODR_POS 4
188 #define K2DH_ODR_REG CTRL_REG1
189
190 #ifdef K2DH_ACCEL_CALIBRATION
191 #define K2DH_SHAKING_DETECT_THRESHOLD (200) //clubsh cal2 50 -> 200
192
193 struct K2DHacc{
194 s16 x,
195 y,
196 z;
197 } ;
198 #endif
199
200 #define SELF_TEST_2G_MAX_LSB (360)
201 #define SELF_TEST_2G_MIN_LSB (17)
202 #define TESTLIMIT_XY (175)
203 #define TESTLIMIT_Z_USL_LSB (1270)
204 #define TESTLIMIT_Z_LSL_LSB (778)
205
206 #define READ_DATA_CHECK_CNT (20)
207
208 /*----------------------------------------------------------------------------*/
209 static const struct i2c_device_id k2dh_i2c_id[] = {{K2DH_ACC_DEV_NAME,0},{}};
210 static struct i2c_board_info __initdata k2dh_i2c_info ={ I2C_BOARD_INFO(K2DH_ACC_DEV_NAME, K2DH_ACC_I2C_SAD_L)};
211
212 /*----------------------------------------------------------------------------*/
213 static int k2dh_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id);
214 static int k2dh_i2c_remove(struct i2c_client *client);
215
216 /*----------------------------------------------------------------------------*/
217 typedef enum {
218 K2DH_TRC_FILTER = 0x01,
219 K2DH_TRC_RAWDATA = 0x02,
220 K2DH_TRC_IOCTL = 0x04,
221 K2DH_TRC_CALI = 0X08,
222 K2DH_TRC_INFO = 0X10,
223 } K2DH_TRC;
224 /*----------------------------------------------------------------------------*/
225 struct scale_factor{
226 u8 whole;
227 u8 fraction;
228 };
229 /*----------------------------------------------------------------------------*/
230 struct data_resolution {
231 struct scale_factor scalefactor;
232 int sensitivity;
233 };
234 /*----------------------------------------------------------------------------*/
235 #ifdef CONFIG_K2DH_LOWPASS
236 #define C_MAX_FIR_LENGTH (32)
237
238 struct data_filter {
239 s16 raw[C_MAX_FIR_LENGTH][K2DH_AXES_NUM];
240 int sum[K2DH_AXES_NUM];
241 int num;
242 int idx;
243 };
244 #endif
245 /*----------------------------------------------------------------------------*/
246 struct k2dh_i2c_data {
247 struct i2c_client *client;
248 struct acc_hw *hw;
249 struct hwmsen_convert cvt;
250
251 /*misc*/
252 struct data_resolution *reso;
253 atomic_t trace;
254 atomic_t suspend;
255 atomic_t selftest_rslt;
256 atomic_t filter;
257 s16 cali_sw[K2DH_AXES_NUM+1];
258
259 /*data*/
260 //s8 offset[K2DH_AXES_NUM+1]; /*+1: for 4-byte alignment*/
261 s16 data[K2DH_AXES_NUM+1];
262
263 #if defined(CONFIG_K2DH_LOWPASS)
264 atomic_t firlen;
265 atomic_t fir_en;
266 struct data_filter fir;
267 #endif
268 /*early suspend*/
269 #if defined(CONFIG_HAS_EARLYSUSPEND)
270 struct early_suspend early_drv;
271 #endif
272
273 #ifdef K2DH_ACCEL_CALIBRATION
274 atomic_t fast_calib_x_rslt;
275 atomic_t fast_calib_y_rslt;
276 atomic_t fast_calib_z_rslt;
277 atomic_t fast_calib_rslt;
278 #endif
279 };
280 /*----------------------------------------------------------------------------*/
281 static struct i2c_driver k2dh_i2c_driver = {
282 .driver = {
283 .name = K2DH_ACC_DEV_NAME,
284 },
285 .probe = k2dh_i2c_probe,
286 .remove = k2dh_i2c_remove,
287 #if !defined(CONFIG_HAS_EARLYSUSPEND)
288 .suspend = k2dh_suspend,
289 .resume = k2dh_resume,
290 #endif
291 .id_table = k2dh_i2c_id,
292 };
293
294 /*----------------------------------------------------------------------------*/
295 static struct i2c_client *k2dh_i2c_client = NULL;
296 static struct platform_driver k2dh_gsensor_driver;
297 static struct k2dh_i2c_data *obj_i2c_data = NULL;
298 static bool sensor_power = true;
299 static int test_status = 0;
300 static UINT32 data_read_count = 0;
301 static GSENSOR_VECTOR3D gsensor_gain;
302
303 /*----------------------------------------------------------------------------*/
304 #define GSE_TAG "[Gsensor] "
305 #define GSE_FUN(f) printk(KERN_ERR GSE_TAG"%s()\n", __FUNCTION__)
306 #define GSE_ERR(fmt, args...) printk(KERN_ERR GSE_TAG"[ERROR] %s() line=%d : "fmt, __FUNCTION__, __LINE__, ##args)
307 #define GSE_LOG(fmt, args...) printk(KERN_ERR GSE_TAG fmt, ##args)
308 /*----------------------------------------------------------------------------*/
309 static struct data_resolution K2DH_data_resolution[1] = {
310 /* combination by {FULL_RES,RANGE}*/
311 {{ 1, 0}, 1024}, // dataformat +/-2g in 12-bit resolution; { 1, 0} = 1.0= (2*2*1000)/(2^12); 1024 = (2^12)/(2*2)
312 };
313 /*--------------------K2DH power control function----------------------------------*/
314 static void k2dh_power(struct acc_hw *hw, unsigned int on)
315 {
316 static unsigned int power_on = 0;
317
318 //if(hw->power_id != POWER_NONE_MACRO) // have externel LDO
319 {
320 GSE_LOG("power %s\n", on ? "on" : "off");
321 if(power_on == on) // power status not change
322 {
323 GSE_LOG("ignore power control: %d\n", on);
324 }
325 else if(on) // power on
326 {
327 if(!(hw->power(hw, true, "K2DH")))
328 {
329 GSE_ERR("power on fails!!\n");
330 }
331 }
332 else // power off
333 {
334 if(!(hw->power(hw, false, "K2DH")))
335 {
336 GSE_ERR("power off fail!!\n");
337 }
338 }
339 }
340 power_on = on;
341 }
342
343 /*----------------------------------------------------------------------------*/
344 static int k2dh_SetDataResolution(struct k2dh_i2c_data *obj)
345 {
346 /*set g sensor dataresolution here*/
347 /*end of set dataresolution*/
348
349 obj->reso = &K2DH_data_resolution[0];
350 return 0;
351
352 /*if you changed the measure range, for example call: K2DH_SetDataFormat(client, K2DH_ACC_G_4G),
353 you must set the right value to K2DH_data_resolution*/
354 }
355
356 /*----------------------------------------------------------------------------*/
357 static int k2dh_ResetCalibration(struct i2c_client *client)
358 {
359 struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
360 u8 ofs[4]={0,0,0,0};
361 int err = 0;
362
363 memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
364 //memset(obj->offset, 0x00, sizeof(obj->offset));
365
366 return err;
367 }
368
369 static int k2dh_acc_i2c_read(struct i2c_client *client, u8 * buf, int len)
370 {
371 int err = -1;
372 int tries = 0;
373
374 struct i2c_msg msgs[] = {
375 {
376 .addr = client->addr,
377 .flags = client->flags & I2C_M_TEN,
378 .len = 1,
379 .buf = buf, },
380 {
381 .addr = client->addr,
382 .flags = (client->flags & I2C_M_TEN) | I2C_M_RD,
383 .len = len,
384 .buf = buf, },
385 };
386
387 #if 1 // SENSOR_I2C_FIX
388 client->adapter->retries = I2C_RETRIES;
389 err = i2c_transfer(client->adapter, msgs, 2);
390 #else
391 do {
392 err = i2c_transfer(client->adapter, msgs, 2);
393 if (err != 2)
394 msleep_interruptible(I2C_RETRY_DELAY);
395 } while ((err != 2) && (++tries < I2C_RETRIES));
396 #endif
397
398 if (err != 2) {
399 GSE_ERR("read transfer error: %d\n", err);
400 err = -EIO;
401 } else {
402 err = 0;
403 }
404
405 return err;
406 }
407
408 static int k2dh_acc_i2c_write(struct i2c_client *client, u8 * buf, int len)
409 {
410 int err = -1;
411 int tries = 0;
412
413 struct i2c_msg msgs[] = { { .addr = client->addr,
414 .flags = client->flags & I2C_M_TEN,
415 .len = len + 1, .buf = buf, }, };
416
417 #if 1 // SENSOR_I2C_FIX
418 client->adapter->retries = I2C_RETRIES;
419 err = i2c_transfer(client->adapter, msgs, 1);
420 #else
421 do {
422 err = i2c_transfer(client->adapter, msgs, 1);
423 if (err != 1)
424 msleep_interruptible(I2C_RETRY_DELAY);
425 } while ((err != 1) && (++tries < I2C_RETRIES));
426 #endif
427
428 if (err != 1) {
429 GSE_ERR("write transfer error: %d\n", err);
430 err = -EIO;
431 } else {
432 err = 0;
433 }
434
435 return err;
436 }
437
438 static int k2dh_acc_register_write(struct i2c_client *client, u8 *buf, u8 reg_address, u8 new_value)
439 {
440 int err = -1;
441
442 buf[0] = reg_address;
443 buf[1] = new_value;
444
445 err = k2dh_acc_i2c_write(client, buf, 1);
446 if(err != K2DH_SUCCESS)
447 {
448 return err;
449 }
450 return err;
451 }
452
453 static int k2dh_acc_register_read(struct i2c_client *client, u8 *buf, u8 reg_address)
454 {
455 int err = -1;
456 buf[0] = (reg_address);
457 err = k2dh_acc_i2c_read(client, buf, 1);
458 return err;
459 }
460
461 static int k2dh_acc_register_update(struct i2c_client *client, u8 *buf, u8 reg_address, u8 mask, u8 new_bit_values)
462 {
463 int err = -1;
464 u8 init_val;
465 u8 updated_val;
466
467 err = k2dh_acc_register_read(client, buf, reg_address);
468 if(err == K2DH_SUCCESS)
469 {
470 init_val = buf[0];
471 updated_val = ((mask & new_bit_values) | ((~mask) & init_val));
472 err = k2dh_acc_register_write(client, buf, reg_address, updated_val);
473 }
474 return err;
475 }
476
477 static int k2dh_CheckDeviceID(struct i2c_client *client)
478 {
479 u8 databuf[2] = {0,0};
480 int res = -1;
481
482 GSE_FUN();
483
484 memset(databuf, 0, sizeof(u8)*2);
485 databuf[0] = WHO_AM_I;
486
487 res = k2dh_acc_i2c_read(client, databuf, 1);
488 if(res != K2DH_SUCCESS)
489 {
490 res = k2dh_acc_i2c_read(client, databuf, 1);
491 if(res != K2DH_SUCCESS)
492 {
493 goto exit_k2dh_CheckDeviceID;
494 }
495 }
496
497 //udelay(500);
498
499 if(databuf[0] != WHOAMI_K2DH_ACC)
500 {
501 GSE_ERR("device unknown. Expected: 0x%x, Replies: 0x%x\n", WHOAMI_K2DH_ACC, databuf[0]);
502 return K2DH_ERR_IDENTIFICATION;
503 }
504 else
505 {
506 GSE_LOG("k2dh_CheckDeviceID %d pass! \n", databuf[0]);
507 }
508
509 exit_k2dh_CheckDeviceID:
510 if(res != K2DH_SUCCESS)
511 {
512 return K2DH_ERR_I2C;
513 }
514
515 return K2DH_SUCCESS;
516 }
517
518 /*----------------------------------------------------------------------------*/
519 static int k2dh_SetBWRate(struct i2c_client *client, u8 bwrate)
520 { // bwrate : ODRxxx
521 int res = -1;
522 u8 databuf[2] = {0,0};
523
524 u8 new_val = bwrate | K2DH_ACC_ENABLE_ALL_AXES;
525 u8 mask = K2DH_ODR_MSK | K2DH_ACC_ENABLE_ALL_AXES_MSK;
526
527 res = k2dh_acc_register_update(client, databuf, CTRL_REG1, mask, new_val);
528 if(res != K2DH_SUCCESS)
529 {
530 GSE_ERR("update odr failed, 0x%x, 0x%x : %d\n", databuf[0], databuf[1], res);
531 return K2DH_ERR_I2C;
532 }
533 else
534 {
535 GSE_LOG("Set BWrate : 0x%02x\n", bwrate);
536 }
537
538 return K2DH_SUCCESS;
539 }
540
541 /*----------------------------------------------------------------------------*/
542 static int k2dh_SetPowerMode(struct i2c_client *client, bool enable)
543 { // power mode controled by operating Bandwidth(ODR-Hz)
544 GSE_FUN();
545
546 int res = -1;
547 struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
548
549 if(enable == sensor_power )
550 {
551 GSE_LOG("Sensor power status is newest!\n");
552 return K2DH_SUCCESS;
553 }
554
555 if(enable == TRUE)
556 {
557 res = k2dh_SetBWRate(client, ODR200); // operating mode as 200HZ
558 }
559 else
560 {
561 res = k2dh_SetBWRate(client, ODR_POWERDOWN_MODE); // power down mode
562 }
563
564 if(res != K2DH_SUCCESS)
565 {
566 GSE_LOG("set power mode failed!\n");
567 return K2DH_ERR_I2C;
568 }
569 else
570 {
571 GSE_LOG("set power mode ok: ");
572 }
573
574 sensor_power = enable;
575 test_status = sensor_power;
576
577 //mdelay(20);
578
579 return K2DH_SUCCESS;
580 }
581
582 /*----------------------------------------------------------------------------*/
583 static int k2dh_SetDataFormat(struct i2c_client *client, u8 dataformat)
584 { // dataformat : K2DH_ACC_G_2G . K2DH_ACC_G_4G. K2DH_ACC_G_8G. K2DH_ACC_G_16G
585
586 int res = -1;
587
588 u8 buf[2] = {0,0};
589 u8 updated_val = 0;
590 u8 init_val = 0;
591 u8 new_val = 0;
592 u8 mask = K2DH_ACC_FS_MASK | HIGH_RESOLUTION;
593
594 struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
595
596
597 buf[0] = CTRL_REG4;
598 res = k2dh_acc_i2c_read(client, buf, 1);
599 if(res != K2DH_SUCCESS)
600 {
601 goto error;
602 }
603 else
604 {
605 init_val = buf[0];
606 new_val = dataformat | HIGH_RESOLUTION;
607 updated_val = ((mask & new_val) | ((~mask) & init_val));
608 buf[1] = updated_val;
609 buf[0] = CTRL_REG4;
610 res = k2dh_acc_i2c_write(client, buf, 1);
611 if(res != K2DH_SUCCESS)
612 {
613 goto error;
614 }
615 else
616 {
617 GSE_LOG("Set DataFormat: 0x%02x\n", dataformat);
618 }
619 }
620
621 return k2dh_SetDataResolution(obj);
622
623 error:
624 GSE_ERR("update g range failed 0x%x, 0x%x: %d\n", buf[0], buf[1], res);
625
626 return res;
627 }
628
629 /*----------------------------------------------------------------------------*/
630 static int k2dh_SetIntEnable(struct i2c_client *client, u8 intenable)
631 {
632 int res = -1;
633 u8 databuf[2] = {0, 0};
634
635 databuf[1] = 0x00;
636 databuf[0] = INT_CFG1;
637
638 res = k2dh_acc_i2c_read(client, databuf, 1);
639 if(res != K2DH_SUCCESS)
640 {
641 GSE_ERR("Interrupt configuration failed, 0x%x, 0x%x : %d\n", databuf[0], databuf[1], res);
642 return K2DH_ERR_I2C;
643 }
644
645 databuf[0] = INT_CFG2;
646 res = k2dh_acc_i2c_read(client, databuf, 1);
647 if(res != K2DH_SUCCESS)
648 {
649 GSE_ERR("Interrupt configuration failed, 0x%x, 0x%x : %d\n", databuf[0], databuf[1], res);
650 return K2DH_ERR_I2C;
651 }
652 GSE_LOG("K2DH Interrupt was configured\n");
653
654 return K2DH_SUCCESS;
655 }
656
657
658 static int k2dh_init_client(struct i2c_client *client)
659 {
660 int err = -1;
661 u8 buf[7] = {0};
662
663 buf[0] = CTRL_REG1;
664 buf[1] = K2DH_ACC_ENABLE_ALL_AXES;
665 err = k2dh_acc_i2c_write(client, buf, 1);
666 if (err < 0)
667 goto error;
668
669 buf[0] = TEMP_CFG_REG;
670 buf[1] = 0x00;
671 err = k2dh_acc_i2c_write(client, buf, 1);
672 if (err < 0)
673 goto error;
674
675 buf[0] = FIFO_CTRL_REG;
676 buf[1] = 0x00;
677 err = k2dh_acc_i2c_write(client, buf, 1);
678 if (err < 0)
679 goto error;
680
681 buf[0] = (I2C_AUTO_INCREMENT | TT_THS);
682 buf[1] = 0x00;
683 buf[2] = 0x00;
684 buf[3] = 0x00;
685 buf[4] = 0x00;
686 err = k2dh_acc_i2c_write(client, buf, 4);
687 if (err < 0)
688 goto error;
689 buf[0] = TT_CFG;
690 buf[1] = 0x00;
691 err = k2dh_acc_i2c_write(client, buf, 1);
692 if (err < 0)
693 goto error;
694
695 buf[0] = (I2C_AUTO_INCREMENT | INT_THS1);
696 buf[1] = 0x00;
697 buf[2] = 0x00;
698 err = k2dh_acc_i2c_write(client, buf, 2);
699 if (err < 0)
700 goto error;
701 buf[0] = INT_CFG1;
702 buf[1] = 0x00;
703 err = k2dh_acc_i2c_write(client, buf, 1);
704 if (err < 0)
705 goto error;
706
707 buf[0] = (I2C_AUTO_INCREMENT | INT_THS2);
708 buf[1] = 0x00;
709 buf[2] = 0x00;
710 err = k2dh_acc_i2c_write(client, buf, 2);
711 if (err < 0)
712 goto error;
713 buf[0] = INT_CFG2;
714 buf[1] = 0x00;
715 err = k2dh_acc_i2c_write(client, buf, 1);
716 if (err < 0)
717 goto error;
718
719 buf[0] = (I2C_AUTO_INCREMENT | CTRL_REG2);
720 buf[1] = 0x00;
721 buf[2] = 0x00;
722 buf[3] = 0x00;
723 buf[4] = 0x00;
724 buf[5] = 0x00;
725 err = k2dh_acc_i2c_write(client, buf, 5);
726 if (err < 0)
727 goto error;
728
729 GSE_LOG("k2dh_init_client : hw init done\n");
730 return K2DH_SUCCESS;
731
732 error:
733 GSE_ERR("hw init error 0x%x, 0x%x: %d\n", buf[0], buf[1], err);
734
735 return err;
736 }
737
738 static int k2dh_acc_hw_init(struct i2c_client *client, int reset_cali)
739 {
740 int res = -1;
741 struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
742
743 GSE_FUN();
744
745 res = k2dh_CheckDeviceID(client);
746 if(res != K2DH_SUCCESS)
747 {
748 return res;
749 }
750
751 res = k2dh_init_client(client);
752 if(res != K2DH_SUCCESS)
753 {
754 return res;
755 }
756
757 res = k2dh_SetBWRate(client, ODR200);
758 if(res != K2DH_SUCCESS )
759 {
760 return res;
761 }
762
763 res = k2dh_SetDataFormat(client, K2DH_ACC_G_2G);
764 if(res != K2DH_SUCCESS)
765 {
766 return res;
767 }
768
769 gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = obj->reso->sensitivity;
770
771 res = k2dh_SetIntEnable(client, 0x00); // interrupt disable
772 if(res != K2DH_SUCCESS)
773 {
774 return res;
775 }
776
777 res = k2dh_SetPowerMode(client, false);
778 if(res != K2DH_SUCCESS)
779 {
780 return res;
781 }
782
783 if(0 != reset_cali)
784 {
785 /*reset calibration only in power on*/
786 res = k2dh_ResetCalibration(client);
787 if(res != K2DH_SUCCESS)
788 {
789 return res;
790 }
791 }
792
793 #ifdef CONFIG_K2DH_LOWPASS
794 memset(&obj->fir, 0x00, sizeof(obj->fir));
795 #endif
796
797 //mdelay(20);
798
799 return K2DH_SUCCESS;
800 }
801 /*----------------------------------------------------------------------------*/
802 static int k2dh_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
803 {
804 u8 databuf[10] = {0};
805
806 memset(databuf, 0, sizeof(u8)*10);
807
808 if((NULL == buf)||(bufsize<=30))
809 {
810 return -1;
811 }
812
813 if(NULL == client)
814 {
815 *buf = 0;
816 return -2;
817 }
818
819 sprintf(buf, "K2DH Chip");
820 return 0;
821 }
822 /*----------------------------------------------------------------------------*/
823
824
825 /*----------------------------------------------------------------------------*/
826 #if 1
827 static s16 prev_data[K2DH_AXES_NUM] = {0,0,0};
828 static s16 check_cnt = 0;
829 #endif
830
831 static int k2dh_ReadData(struct i2c_client *client, s16 data[K2DH_AXES_NUM])
832 {
833 struct k2dh_i2c_data *priv = i2c_get_clientdata(client);
834 u8 acc_data[K2DH_DATA_LEN] = {0};
835 int err = -1;
836
837 acc_data[0] = (I2C_AUTO_INCREMENT | AXISDATA_REG);
838
839 if(NULL == client)
840 {
841 err = -EINVAL;
842 }
843 else if((err = k2dh_acc_i2c_read(client, acc_data, K2DH_DATA_LEN)) != K2DH_SUCCESS)
844 {
845 GSE_ERR("I2C read error: %d\n", err);
846 }
847 else
848 {
849 /* Convert sensor raw data to 16-bit integer */
850 data[K2DH_AXIS_X] = (((s16) ((acc_data[1] << 8) | acc_data[0])) >> 4);
851 data[K2DH_AXIS_Y] = (((s16) ((acc_data[3] << 8) | acc_data[2])) >> 4);
852 data[K2DH_AXIS_Z] = (((s16) ((acc_data[5] << 8) | acc_data[4])) >> 4);
853
854 #if 1
855 if((prev_data[K2DH_AXIS_X] == data[K2DH_AXIS_X]) && (prev_data[K2DH_AXIS_Y] == data[K2DH_AXIS_Y]) && (prev_data[K2DH_AXIS_Z] == data[K2DH_AXIS_Z]))
856 {
857 check_cnt++;
858 if(check_cnt >= READ_DATA_CHECK_CNT) // read data same case
859 {
860 GSE_ERR("READ DATA SAME CASE : (%d %d %d)\n", data[K2DH_AXIS_X], data[K2DH_AXIS_Y],data[K2DH_AXIS_Z]);
861
862 // Try recovery the malfunction by change Power Down mode and ODR200
863 k2dh_SetBWRate(client, ODR_POWERDOWN_MODE); // power down mode
864 msleep(100);
865 k2dh_SetBWRate(client, ODR200); // operating mode as 200HZ
866 check_cnt = 0;
867 }
868 }
869 else
870 {
871 prev_data[K2DH_AXIS_X] =data[K2DH_AXIS_X];
872 prev_data[K2DH_AXIS_Y] =data[K2DH_AXIS_Y];
873 prev_data[K2DH_AXIS_Z] =data[K2DH_AXIS_Z];
874 check_cnt = 0;
875 }
876 #endif
877
878 if(atomic_read(&priv->trace) & K2DH_TRC_RAWDATA)
879 {
880 GSE_LOG("gsensor raw data: x=%d, y=%d, z=%d!\n", data[K2DH_AXIS_X], data[K2DH_AXIS_Y], data[K2DH_AXIS_Z]);
881 }
882 #ifdef CONFIG_K2DH_LOWPASS
883 if(atomic_read(&priv->filter))
884 {
885 if(atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
886 {
887 int idx, firlen = atomic_read(&priv->firlen);
888 if(priv->fir.num < firlen)
889 {
890 priv->fir.raw[priv->fir.num][K2DH_AXIS_X] = data[K2DH_AXIS_X];
891 priv->fir.raw[priv->fir.num][K2DH_AXIS_Y] = data[K2DH_AXIS_Y];
892 priv->fir.raw[priv->fir.num][K2DH_AXIS_Z] = data[K2DH_AXIS_Z];
893 priv->fir.sum[K2DH_AXIS_X] += data[K2DH_AXIS_X];
894 priv->fir.sum[K2DH_AXIS_Y] += data[K2DH_AXIS_Y];
895 priv->fir.sum[K2DH_AXIS_Z] += data[K2DH_AXIS_Z];
896 if(atomic_read(&priv->trace) & K2DH_TRC_FILTER)
897 {
898 GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
899 priv->fir.raw[priv->fir.num][K2DH_AXIS_X], priv->fir.raw[priv->fir.num][K2DH_AXIS_Y], priv->fir.raw[priv->fir.num][K2DH_AXIS_Z],
900 priv->fir.sum[K2DH_AXIS_X], priv->fir.sum[K2DH_AXIS_Y], priv->fir.sum[K2DH_AXIS_Z]);
901 }
902 priv->fir.num++;
903 priv->fir.idx++;
904 }
905 else
906 {
907 idx = priv->fir.idx % firlen;
908 priv->fir.sum[K2DH_AXIS_X] -= priv->fir.raw[idx][K2DH_AXIS_X];
909 priv->fir.sum[K2DH_AXIS_Y] -= priv->fir.raw[idx][K2DH_AXIS_Y];
910 priv->fir.sum[K2DH_AXIS_Z] -= priv->fir.raw[idx][K2DH_AXIS_Z];
911 priv->fir.raw[idx][K2DH_AXIS_X] = data[K2DH_AXIS_X];
912 priv->fir.raw[idx][K2DH_AXIS_Y] = data[K2DH_AXIS_Y];
913 priv->fir.raw[idx][K2DH_AXIS_Z] = data[K2DH_AXIS_Z];
914 priv->fir.sum[K2DH_AXIS_X] += data[K2DH_AXIS_X];
915 priv->fir.sum[K2DH_AXIS_Y] += data[K2DH_AXIS_Y];
916 priv->fir.sum[K2DH_AXIS_Z] += data[K2DH_AXIS_Z];
917 priv->fir.idx++;
918 data[K2DH_AXIS_X] = priv->fir.sum[K2DH_AXIS_X]/firlen;
919 data[K2DH_AXIS_Y] = priv->fir.sum[K2DH_AXIS_Y]/firlen;
920 data[K2DH_AXIS_Z] = priv->fir.sum[K2DH_AXIS_Z]/firlen;
921 if(atomic_read(&priv->trace) & K2DH_TRC_FILTER)
922 {
923 GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
924 priv->fir.raw[idx][K2DH_AXIS_X], priv->fir.raw[idx][K2DH_AXIS_Y], priv->fir.raw[idx][K2DH_AXIS_Z],
925 priv->fir.sum[K2DH_AXIS_X], priv->fir.sum[K2DH_AXIS_Y], priv->fir.sum[K2DH_AXIS_Z],
926 data[K2DH_AXIS_X], data[K2DH_AXIS_Y], data[K2DH_AXIS_Z]);
927 }
928 }
929 }
930 }
931 #endif
932 }
933 return err;
934 }
935 /*----------------------------------------------------------------------------*/
936 static int k2dh_ReadSensorData(struct i2c_client *client, char *buf, int bufsize)
937 {
938 struct k2dh_i2c_data *obj = (struct k2dh_i2c_data*)i2c_get_clientdata(client);
939 u8 databuf[20] = {0};
940 int acc[K2DH_AXES_NUM] = {0};
941 int res = -1;
942 memset(databuf, 0, sizeof(u8)*10);
943
944 if(NULL == buf)
945 {
946 return -1;
947 }
948 if(NULL == client)
949 {
950 *buf = 0;
951 return -2;
952 }
953
954 if(sensor_power == FALSE)
955 {
956 res = k2dh_SetPowerMode(client, true);
957 if(res)
958 {
959 GSE_ERR("Power on K2DH error %d!\n", res);
960 }
961 }
962
963 if(res = k2dh_ReadData(client, obj->data))
964 {
965 GSE_ERR("I2C error: ret value=%d", res);
966 return -3;
967 }
968 else
969 {
970 data_read_count++;
971 if(data_read_count >= 2147483647)
972 {
973 data_read_count =0;
974 }
975
976 obj->data[K2DH_AXIS_X] -= obj->cali_sw[K2DH_AXIS_X];
977 obj->data[K2DH_AXIS_Y] -= obj->cali_sw[K2DH_AXIS_Y];
978 obj->data[K2DH_AXIS_Z] -= obj->cali_sw[K2DH_AXIS_Z];
979
980 //printk("cali_sw x=%d, y=%d, z=%d \n",obj->cali_sw[K2DH_AXIS_X],obj->cali_sw[K2DH_AXIS_Y],obj->cali_sw[K2DH_AXIS_Z]);
981
982 /*remap coordinate*/
983 acc[obj->cvt.map[K2DH_AXIS_X]] = obj->cvt.sign[K2DH_AXIS_X]*obj->data[K2DH_AXIS_X];
984 acc[obj->cvt.map[K2DH_AXIS_Y]] = obj->cvt.sign[K2DH_AXIS_Y]*obj->data[K2DH_AXIS_Y];
985 acc[obj->cvt.map[K2DH_AXIS_Z]] = obj->cvt.sign[K2DH_AXIS_Z]*obj->data[K2DH_AXIS_Z];
986 //printk("cvt x=%d, y=%d, z=%d \n",obj->cvt.sign[K2DH_AXIS_X],obj->cvt.sign[K2DH_AXIS_Y],obj->cvt.sign[K2DH_AXIS_Z]);
987
988 if(atomic_read(&obj->trace) & K2DH_TRC_RAWDATA)
989 {
990 //GSE_LOG("Mapped gsensor data: x=%d, y=%d, z=%d!\n", acc[K2DH_AXIS_X], acc[K2DH_AXIS_Y], acc[K2DH_AXIS_Z]);
991 }
992
993 //Out put the mg
994 //printk("mg acc=%d, GRAVITY=%d, sensityvity=%d \n",acc[K2DH_AXIS_X],GRAVITY_EARTH_1000,obj->reso->sensitivity);
995 acc[K2DH_AXIS_X] = acc[K2DH_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
996 acc[K2DH_AXIS_Y] = acc[K2DH_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
997 acc[K2DH_AXIS_Z] = acc[K2DH_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
998
999 sprintf(buf, "%04x %04x %04x %x", acc[K2DH_AXIS_X], acc[K2DH_AXIS_Y], acc[K2DH_AXIS_Z], data_read_count);
1000 if(atomic_read(&obj->trace) & K2DH_TRC_IOCTL)
1001 {
1002 GSE_LOG("gsensor data: x=%d, y=%d, z=%d!\n", acc[K2DH_AXIS_X], acc[K2DH_AXIS_Y], acc[K2DH_AXIS_Z]);
1003 }
1004 }
1005
1006 return 0;
1007 }
1008
1009 static int k2dh_read_accel_raw_data(struct k2dh_acc *acc)
1010 {
1011 int err;
1012 unsigned char acc_data[6] = {0};
1013 s16 temp;
1014
1015 acc_data[0] = (I2C_AUTO_INCREMENT | AXISDATA_REG);
1016 }
1017
1018 static int k2dh_do_calibration(void)
1019 {
1020 struct K2DHacc acc_cal = {0, };
1021 struct K2DHacc acc_cal_pre = {0, };
1022 int sum[3] = {0, };
1023 int err = 0;
1024 int i;
1025
1026 struct i2c_client *client = k2dh_i2c_client;
1027 struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
1028
1029 err = k2dh_ReadData(client, &acc_cal_pre);
1030 if(err < 0)
1031 {
1032 GSE_ERR("%s : k2dh_ReadData() failed\n", __func__);
1033 return err;
1034 }
1035
1036 for(i = 0; i < CALIBRATION_DATA_AMOUNT; i++)
1037 {
1038 mdelay(20);
1039 err = k2dh_ReadData(client, &acc_cal);
1040 if(err < 0)
1041 {
1042 GSE_ERR("%s : k2dh_ReadData() failed in the %dth loop\n", __func__, i);
1043 return err;
1044 }
1045
1046 GSE_LOG(KERN_INFO "===============moved x=============== timeout = %d\n", i);
1047 GSE_LOG(KERN_INFO "(%d, %d, %d) (%d, %d, %d)\n", acc_cal_pre.x, acc_cal_pre.y, acc_cal_pre.z, acc_cal.x,acc_cal.y,acc_cal.z );
1048
1049 if((abs(acc_cal.x - acc_cal_pre.x) > K2DH_SHAKING_DETECT_THRESHOLD)
1050 || (abs((acc_cal.y - acc_cal_pre.y)) > K2DH_SHAKING_DETECT_THRESHOLD)
1051 || (abs((acc_cal.z - acc_cal_pre.z)) > K2DH_SHAKING_DETECT_THRESHOLD))
1052 {
1053 atomic_set(&obj->fast_calib_rslt, 0);
1054 GSE_LOG(KERN_INFO "===============shaking x===============\n");
1055 return -EINVAL;
1056 }
1057 else
1058 {
1059 sum[K2DH_AXIS_X] += acc_cal.x;
1060 sum[K2DH_AXIS_Y] += acc_cal.y;
1061 sum[K2DH_AXIS_Z] += acc_cal.z;
1062
1063 acc_cal_pre.x = acc_cal.x;
1064 acc_cal_pre.y = acc_cal.y;
1065 acc_cal_pre.z = acc_cal.z;
1066 }
1067
1068 GSE_LOG("calibration sum data (%d, %d, %d)\n", sum[K2DH_AXIS_X], sum[K2DH_AXIS_Y], sum[K2DH_AXIS_Z]);
1069 GSE_LOG(KERN_INFO "===============timeout_shaking: %d=============== \n",i);
1070 }
1071
1072 GSE_LOG(KERN_INFO "===============complete shaking x check===============\n");
1073
1074 #if 1
1075 // check zero-g offset
1076 if((abs(sum[K2DH_AXIS_X]/CALIBRATION_DATA_AMOUNT) >TESTLIMIT_XY) ||
1077 (abs(sum[K2DH_AXIS_Y]/CALIBRATION_DATA_AMOUNT) >TESTLIMIT_XY) ||
1078 ((abs(sum[K2DH_AXIS_Z]/CALIBRATION_DATA_AMOUNT) > TESTLIMIT_Z_USL_LSB) || (abs(sum[K2DH_AXIS_Z]/CALIBRATION_DATA_AMOUNT) < TESTLIMIT_Z_LSL_LSB)))
1079 {
1080 GSE_LOG("Calibration zero-g offset check failed (%d, %d, %d)\n", sum[K2DH_AXIS_X]/CALIBRATION_DATA_AMOUNT,
1081 sum[K2DH_AXIS_Y]/CALIBRATION_DATA_AMOUNT, sum[K2DH_AXIS_Z]/CALIBRATION_DATA_AMOUNT);
1082 atomic_set(&obj->fast_calib_rslt, 0);
1083 return -EINVAL;
1084 }
1085 #endif
1086
1087 obj->cali_sw[K2DH_AXIS_X] = sum[K2DH_AXIS_X] / CALIBRATION_DATA_AMOUNT; //K2DH(12bit) 0+-154
1088 obj->cali_sw[K2DH_AXIS_Y] = sum[K2DH_AXIS_Y] / CALIBRATION_DATA_AMOUNT; //K2DH(12bit) 0+-154
1089
1090 if(sum[K2DH_AXIS_Z] < 0)
1091 {
1092 obj->cali_sw[K2DH_AXIS_Z] = (sum[K2DH_AXIS_Z] / CALIBRATION_DATA_AMOUNT) + 1024; // K2DH(12bit) 1024 +- 226
1093 }
1094 else
1095 {
1096 obj->cali_sw[K2DH_AXIS_Z] = (sum[K2DH_AXIS_Z] / CALIBRATION_DATA_AMOUNT) - 1024; // K2DH(12bit) 1024 +- 226
1097 }
1098
1099 GSE_LOG("%s : calibration data (%d, %d, %d)\n", __func__, obj->cali_sw[K2DH_AXIS_X], obj->cali_sw[K2DH_AXIS_Y], obj->cali_sw[K2DH_AXIS_Z]);
1100
1101 return err;
1102 }
1103
1104 #if 1
1105 static int k2dh_get_selftest(struct i2c_client *client)
1106 {
1107 int val, i;
1108 int en_state = 0;
1109
1110 unsigned char x[8];
1111 int NO_ST[3] = {0, 0, 0};
1112 int NO_ST_ZOFF[3] = {0, 0, 0};
1113 int ST[3] = {0, 0, 0};
1114 s16 tmp = 0;
1115
1116 struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
1117
1118 // ODR setting
1119 x[0] = CTRL_REG1;
1120 x[1] = 0x67; // 0x47
1121 k2dh_acc_i2c_write(client, x, 1);
1122
1123 x[0] = (I2C_AUTO_INCREMENT | CTRL_REG4);
1124 x[1] = 0x88;
1125 x[2] = 0x00;
1126 x[3] = 0x00;
1127 k2dh_acc_i2c_write(client, x, 3);
1128
1129 mdelay(80);
1130
1131 x[0] = (I2C_AUTO_INCREMENT | AXISDATA_REG);
1132 k2dh_acc_i2c_read(client, x, 6);
1133
1134 for(i = 0; i < 5; i++)
1135 {
1136 while(1)
1137 {
1138 x[0] = 0x27; // status_reg
1139 val = k2dh_acc_i2c_read(client, x, 1);
1140 if(val < 0)
1141 {
1142 GSE_ERR("[SELFTEST] I2C fail point1\n");
1143 goto ST_EXIT;
1144 }
1145 if(x[0] & 0x08) // x,y,z asix new data Available
1146 {
1147 break;
1148 }
1149 }
1150
1151 x[0] = (I2C_AUTO_INCREMENT | AXISDATA_REG);
1152 k2dh_acc_i2c_read(client, x, 6);
1153
1154 NO_ST_ZOFF[0] += (((s16) ((x[1] << 8) | x[0])) >> 4); // 12 bit resolution 1LSB= 0.997mg
1155 NO_ST_ZOFF[1] += (((s16) ((x[3] << 8) | x[2])) >> 4);
1156 NO_ST_ZOFF[2] += (((s16) ((x[5] << 8) | x[4])) >> 4);
1157
1158 NO_ST[0] += (((s16) ((x[1] << 8) | x[0])) >> 6); //10 bit resolution 1LSB=4mg
1159 NO_ST[1] += (((s16) ((x[3] << 8) | x[2])) >> 6);
1160 NO_ST[2] += (((s16) ((x[5] << 8) | x[4])) >> 6);
1161
1162 if(atomic_read(&obj->trace) & K2DH_TRC_CALI)
1163 {
1164 GSE_LOG("[SELFTEST] NO_ST(%d) : %d, %d, %d\n", i, NO_ST[0], NO_ST[1], NO_ST[2]);
1165 }
1166 }
1167
1168 NO_ST_ZOFF[0] /= 5;
1169 NO_ST_ZOFF[1] /= 5;
1170 NO_ST_ZOFF[2] /= 5;
1171
1172 NO_ST[0] /= 5;
1173 NO_ST[1] /= 5;
1174 NO_ST[2] /= 5;
1175
1176 //if(atomic_read(&obj->trace) & K2DH_TRC_CALI)
1177 {
1178 GSE_LOG("[SELFTEST] AVE_NO_ST : %d, %d, %d\n", NO_ST[0], NO_ST[1], NO_ST[2]);
1179 }
1180
1181 x[0] = CTRL_REG4;
1182 x[1] = 0x8A; // ST enable
1183 k2dh_acc_i2c_write(client, x, 1);
1184
1185 mdelay(80);
1186
1187 x[0] = (I2C_AUTO_INCREMENT | AXISDATA_REG);
1188 k2dh_acc_i2c_read(client, x, 6);
1189
1190 for(i = 0; i < 5; i++)
1191 {
1192 while(1)
1193 {
1194 x[0] = 0x27;
1195 val = k2dh_acc_i2c_read(client, x, 1);
1196
1197 if(val < 0)
1198 {
1199 GSE_ERR("[SELFTEST] I2C fail point2\n");
1200 goto ST_EXIT;
1201 }
1202
1203 if(x[0] & 0x08)
1204 {
1205 break;
1206 }
1207 }
1208
1209 x[0] = (I2C_AUTO_INCREMENT | AXISDATA_REG);
1210 k2dh_acc_i2c_read(client, x, 6);
1211
1212 ST[0] += (((s16) ((x[1] << 8) | x[0])) >> 6);
1213 ST[1] += (((s16) ((x[3] << 8) | x[2])) >> 6);
1214 ST[2] += (((s16) ((x[5] << 8) | x[4])) >> 6);
1215
1216 if(atomic_read(&obj->trace) & K2DH_TRC_CALI)
1217 {
1218 GSE_LOG("[SELFTEST] ST(%d) : %d, %d, %d\n", i, ST[0], ST[1], ST[2]);
1219 }
1220 }
1221 ST[0] /= 5;
1222 ST[1] /= 5;
1223 ST[2] /= 5;
1224 //if(atomic_read(&obj->trace) & K2DH_TRC_CALI)
1225 {
1226 GSE_LOG("[SELFTEST] AVE_ST : %d, %d, %d\n", ST[0], ST[1], ST[2]);
1227 }
1228
1229 for(val = 0, i = 0; i < 3; i++)
1230 {
1231 // calculate differece between SelfTest value and zoro g offset in 10bit resolution
1232 ST[i] -= NO_ST[i];
1233 ST[i] = abs(ST[i]);
1234
1235 // range compare of the self test
1236 if((SELF_TEST_2G_MIN_LSB > ST[i]) || (ST[i] > SELF_TEST_2G_MAX_LSB))
1237 {
1238 GSE_ERR("[SELFTEST] ST[%d] : Out of range!! (%d)\n", i, ST[i]);
1239 val = -1;
1240 }
1241 }
1242
1243 // check zero-g offset
1244 if(val >= 0)
1245 {
1246 for(val = 1, i = 0; i < 3; i++)
1247 {
1248 if(i < 2)
1249 {
1250 if(abs(NO_ST_ZOFF[i]) > TESTLIMIT_XY) // X, Y axis flat check
1251 {
1252 GSE_ERR("[SELFTEST] NO_ST[%d] : Out of ZOffset!! (%d)\n", i, NO_ST_ZOFF[i]);
1253 val = -1;
1254 }
1255 }
1256 else
1257 { // Z axis flat check
1258 if((abs(NO_ST_ZOFF[i]) > TESTLIMIT_Z_USL_LSB) || (abs(NO_ST_ZOFF[i]) < TESTLIMIT_Z_LSL_LSB))
1259 {
1260 GSE_ERR("[SELFTEST] NO_ST[%d] : Out of ZOffset!! (%d)\n", i, NO_ST[i]);
1261 val = -1;
1262 }
1263 }
1264 }
1265 }
1266
1267 //if(atomic_read(&obj->trace) & K2DH_TRC_CALI)
1268 {
1269 if(val >= 0)
1270 {
1271 GSE_LOG("[SELFTEST] OK!! val : %d, (%d, %d, %d) ||| (%d, %d, %d)\n", val, ST[0], ST[1], ST[2], NO_ST_ZOFF[0], NO_ST_ZOFF[1], NO_ST_ZOFF[2]);
1272 }
1273 else
1274 {
1275 GSE_LOG("[SELFTEST] NG!! val : %d, (%d, %d, %d) ||| (%d, %d, %d)\n", val, ST[0], ST[1], ST[2], NO_ST_ZOFF[0], NO_ST_ZOFF[1], NO_ST_ZOFF[2]);
1276 }
1277 }
1278
1279 ST_EXIT:
1280 #if 0 // do not control ODR rate
1281 x[0] = CTRL_REG1;
1282 x[1] = 0x00;
1283 k2dh_acc_i2c_write(client, x, 1);
1284 #endif
1285
1286 x[0] = CTRL_REG4;
1287 x[1] = 0x08; // ST disable
1288 k2dh_acc_i2c_write(client, x, 1);
1289
1290
1291 x[0] = CTRL_REG5;
1292 x[1] = 0x88;
1293 k2dh_acc_i2c_write(client, x, 1);
1294
1295 return val;
1296 }
1297
1298 #endif
1299
1300
1301 static int k2dh_set_mode(struct i2c_client *client, unsigned char mode)
1302 {
1303 int comres = -1;
1304 unsigned char data[2] ={0, 0};
1305 unsigned char LPen = 0;
1306 unsigned char HR = 0;
1307
1308 if((client == NULL) || (mode >= 3))
1309 {
1310 return -1;
1311 }
1312
1313 switch(mode)
1314 {
1315 case K2DH_MODE_LOWPOWER :
1316 LPen = 1;
1317 HR = 0;
1318 break;
1319 case K2DH_MODE_NORMAL:
1320 LPen = 0;
1321 HR = 0;
1322 break;
1323 case K2DH_MODE_HIGH_RESOLUTION :
1324 LPen = 0;
1325 HR = 1;
1326 break;
1327 default :
1328 break;
1329 }
1330
1331 comres = k2dh_acc_register_update(client, (u8 *) data, K2DH_LP_EN_REG, K2DH_LP_EN_MSK, LPen << K2DH_LP_EN_POS);
1332 if(comres != K2DH_SUCCESS)
1333 {
1334 return comres;
1335 }
1336 else
1337 {
1338 comres = k2dh_acc_register_update(client, (u8 *) data, K2DH_HR_REG, K2DH_HR_MSK, HR << K2DH_HR_POS);
1339 if(comres != K2DH_SUCCESS)
1340 {
1341 return comres;
1342 }
1343 }
1344 GSE_LOG("k2dh_set_mode, mode : %d, LPen : %d, HR : %d\n", mode, LPen, HR);
1345 return K2DH_SUCCESS;
1346 }
1347
1348 static int k2dh_get_mode(struct i2c_client *client, unsigned char *mode)
1349 {
1350 int comres = -1;
1351 unsigned char data[2] ={0, 0};
1352 unsigned char LPen = 0;
1353 unsigned char HR = 0;
1354
1355 comres = k2dh_acc_register_read(client, data, K2DH_LP_EN_REG);
1356 if(comres != K2DH_SUCCESS)
1357 {
1358 return comres;
1359 }
1360 else
1361 {
1362 LPen = (data[0]& K2DH_LP_EN_MSK)?1:0;
1363 comres = k2dh_acc_register_read(client, data, K2DH_HR_REG);
1364 if(comres != K2DH_SUCCESS)
1365 {
1366 return comres;
1367 }
1368 else
1369 {
1370 HR = (data[0]& K2DH_HR_MSK)?1:0;
1371 }
1372
1373 if(LPen == 1 && HR == 0)
1374 {
1375 *mode = K2DH_MODE_LOWPOWER;
1376 }
1377 else if(LPen == 0 && HR == 0)
1378 {
1379 *mode = K2DH_MODE_NORMAL;
1380 }
1381 else if(LPen == 0 && HR == 1)
1382 {
1383 *mode = K2DH_MODE_HIGH_RESOLUTION;
1384 }
1385 else
1386 {
1387 *mode = -1;
1388 }
1389 }
1390 GSE_LOG("k2dh_get_mode, LPen : %d, HR : %d\n", LPen, HR);
1391 return comres;
1392 }
1393
1394 static int k2dh_set_range(struct i2c_client *client, unsigned char range)
1395 {
1396 int comres = -1;
1397 unsigned char data[2] ={0, 0};
1398
1399 if((client == NULL) || (range >= 4))
1400 {
1401 return -1;
1402 }
1403
1404 comres = k2dh_acc_register_update(client, (u8 *) data, K2DH_FS_REG, K2DH_FS_MSK, range << K2DH_FS_POS);
1405 if(comres != K2DH_SUCCESS)
1406 {
1407 return comres;
1408 }
1409
1410 GSE_LOG("k2dh_set_range, range : %d\n", range);
1411 return K2DH_SUCCESS;
1412
1413 }
1414 static int k2dh_get_range(struct i2c_client *client, unsigned char *range)
1415 {
1416 int comres = -1;
1417 unsigned char data[2] ={0, 0};
1418
1419 comres = k2dh_acc_register_read(client, data, K2DH_FS_REG);
1420 if(comres != K2DH_SUCCESS)
1421 {
1422 return comres;
1423 }
1424 data[0] = (data[0] & K2DH_FS_MSK) >> K2DH_FS_POS;
1425 *range = data[0];
1426 GSE_LOG("k2dh_get_range, range : %d\n", data[0]);
1427 return comres;
1428 }
1429 static int k2dh_set_bandwidth(struct i2c_client *client, unsigned char bandwidth)
1430 {
1431 int comres = -1;
1432 unsigned char data[2] ={0, 0};
1433
1434 if((client == NULL) || (bandwidth >= 10))
1435 {
1436 return -1;
1437 }
1438
1439 comres = k2dh_acc_register_update(client, (u8 *) data, K2DH_ODR_REG, K2DH_ODR_MSK, bandwidth << K2DH_ODR_POS);
1440 if(comres != K2DH_SUCCESS)
1441 {
1442 return comres;
1443 }
1444
1445 GSE_LOG("k2dh_set_bandwidth, bandwidth : %d\n", bandwidth);
1446 return K2DH_SUCCESS;
1447 }
1448 static int k2dh_get_bandwidth(struct i2c_client *client, unsigned char *bandwidth)
1449 {
1450 int comres = -1;
1451 unsigned char data[2] ={0, 0};
1452
1453 comres = k2dh_acc_register_read(client, data, K2DH_ODR_REG);
1454 if(comres != K2DH_SUCCESS)
1455 {
1456 return comres;
1457 }
1458 data[0] = (data[0] & K2DH_ODR_MSK) >> K2DH_ODR_POS;
1459 *bandwidth = data[0];
1460 GSE_LOG("k2dh_get_bandwidth, bandwidth : %d\n", data[0]);
1461 return comres;
1462 }
1463
1464 /*----------------------------------------------------------------------------*/
1465 static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
1466 {
1467 struct i2c_client *client = k2dh_i2c_client;
1468 char strbuf[K2DH_BUFSIZE] = {0};
1469 if(NULL == client)
1470 {
1471 GSE_ERR("i2c client is null!!\n");
1472 return 0;
1473 }
1474
1475 k2dh_ReadChipInfo(client, strbuf, K2DH_BUFSIZE);
1476 return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
1477 }
1478
1479 static ssize_t gsensor_init(struct device_driver *ddri, char *buf, size_t count)
1480 {
1481 struct i2c_client *client = k2dh_i2c_client;
1482 char strbuf[K2DH_BUFSIZE] = {0};
1483
1484 if(NULL == client)
1485 {
1486 GSE_ERR("i2c client is null!!\n");
1487 return 0;
1488 }
1489 k2dh_acc_hw_init(client, 1);
1490 return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
1491 }
1492
1493 static ssize_t show_opmode_value(struct device_driver *ddri, char *buf)
1494 {
1495 unsigned char data = 0;
1496
1497 if (k2dh_get_mode(k2dh_i2c_client, &data) < 0)
1498 {
1499 return sprintf(buf, "Read error\n");
1500 }
1501 else
1502 {
1503 if(data == 0)
1504 return sprintf(buf, "0:LP MODE\n");
1505 else if(data == 1)
1506 return sprintf(buf, "1:Normal MODE\n");
1507 else if(data == 2)
1508 return sprintf(buf, "2:HR MODE\n");
1509 else
1510 return sprintf(buf, "Error\n");
1511 }
1512 }
1513
1514 static ssize_t store_opmode_value(struct device_driver *ddri, char *buf, size_t count)
1515 {
1516 unsigned long data = 0;
1517 int error = -1;
1518
1519 if (error = strict_strtoul(buf, 10, &data))
1520 {
1521 return error;
1522 }
1523
1524 if (k2dh_set_mode(k2dh_i2c_client, (unsigned char) data) < 0)
1525 {
1526 GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
1527 }
1528
1529 return count;
1530 }
1531
1532 static ssize_t show_range_value(struct device_driver *ddri, char *buf)
1533 {
1534 unsigned char data = 0;
1535
1536 if (k2dh_get_range(k2dh_i2c_client, &data) < 0)
1537 {
1538 return sprintf(buf, "Read error\n");
1539 }
1540 else
1541 {
1542 return sprintf(buf, "%d\n", data);
1543 }
1544 }
1545
1546 static ssize_t store_range_value(struct device_driver *ddri, char *buf, size_t count)
1547 {
1548 unsigned long data = 0;
1549 int error = -1;
1550
1551 if (error = strict_strtoul(buf, 10, &data))
1552 {
1553 return error;
1554 }
1555 if (k2dh_set_range(k2dh_i2c_client, (unsigned char) data) < 0)
1556 {
1557 GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
1558 }
1559
1560 return count;
1561 }
1562
1563 static ssize_t show_bandwidth_value(struct device_driver *ddri, char *buf)
1564 {
1565 unsigned char data = 0;
1566
1567 if (k2dh_get_bandwidth(k2dh_i2c_client, &data) < 0)
1568 {
1569 return sprintf(buf, "Read error\n");
1570 }
1571 else
1572 {
1573 return sprintf(buf, "%d\n", data);
1574 }
1575 }
1576
1577 static ssize_t store_bandwidth_value(struct device_driver *ddri, char *buf, size_t count)
1578 {
1579 unsigned long data = 0;
1580 int error = -1;
1581
1582 if (error = strict_strtoul(buf, 10, &data))
1583 {
1584 return error;
1585 }
1586 if (k2dh_set_bandwidth(k2dh_i2c_client, (unsigned char) data) < 0)
1587 {
1588 GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
1589 }
1590
1591 return count;
1592 }
1593
1594 static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
1595 {
1596 struct i2c_client *client = k2dh_i2c_client;
1597 char strbuf[K2DH_BUFSIZE] = {0};;
1598
1599 if(NULL == client)
1600 {
1601 GSE_ERR("i2c client is null!!\n");
1602 return 0;
1603 }
1604 k2dh_ReadSensorData(client, strbuf, K2DH_BUFSIZE);
1605 return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
1606 }
1607
1608 static ssize_t show_sensorrawdata_value(struct device_driver *ddri, char *buf)
1609 {
1610 struct i2c_client *client = k2dh_i2c_client;
1611 s16 data[K2DH_AXES_NUM];
1612
1613 if(NULL == client)
1614 {
1615 GSE_ERR("i2c client is null!!\n");
1616 return 0;
1617 }
1618
1619 k2dh_ReadData(client, data);
1620 return snprintf(buf, PAGE_SIZE, "Read RawData : x=%04x, y=%04x, z=%04x\n", data[0], data[1], data[2]);
1621 }
1622
1623
1624
1625 /*----------------------------------------------------------------------------*/
1626 static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
1627 {
1628 ssize_t res = -1;
1629 struct k2dh_i2c_data *obj = obj_i2c_data;
1630 if (obj == NULL)
1631 {
1632 GSE_ERR("i2c_data obj is null!!\n");
1633 return 0;
1634 }
1635
1636 res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));
1637 return res;
1638 }
1639 /*----------------------------------------------------------------------------*/
1640 static ssize_t store_trace_value(struct device_driver *ddri, char *buf, size_t count)
1641 {
1642 struct k2dh_i2c_data *obj = obj_i2c_data;
1643 unsigned long data = 0;
1644 int error = -1;
1645
1646 if (obj == NULL)
1647 {
1648 GSE_ERR("i2c_data obj is null!!\n");
1649 return 0;
1650 }
1651
1652 if (error = strict_strtoul(buf, 10, &data))
1653 {
1654 return error;
1655 }
1656
1657 atomic_set(&obj->trace, data);
1658
1659 //GSE_LOG("data : %d saved_data : %d , buf : %s, count : %d\n", data, atomic_read(&obj->trace), buf, count);
1660
1661 return count;
1662 }
1663 /*----------------------------------------------------------------------------*/
1664 static ssize_t show_status_value(struct device_driver *ddri, char *buf)
1665 {
1666 ssize_t len = 0;
1667 struct k2dh_i2c_data *obj = obj_i2c_data;
1668 if (obj == NULL)
1669 {
1670 GSE_ERR("i2c_data obj is null!!\n");
1671 return 0;
1672 }
1673
1674 if(obj->hw)
1675 {
1676 len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n", obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);
1677 }
1678 else
1679 {
1680 len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
1681 }
1682 return len;
1683 }
1684 /*----------------------------------------------------------------------------*/
1685 static ssize_t show_power_status_value(struct device_driver *ddri, char *buf)
1686 {
1687 if(sensor_power)
1688 GSE_LOG("G sensor is in work mode, sensor_power = %d\n", sensor_power);
1689 else
1690 GSE_LOG("G sensor is in standby mode, sensor_power = %d\n", sensor_power);
1691
1692 return sprintf(buf, "%d\n", sensor_power);
1693 }
1694
1695 static ssize_t show_teststatus_value(struct device_driver *ddri, char *buf)
1696 {
1697 struct i2c_client *client = k2dh_i2c_client;
1698 if(NULL == client)
1699 {
1700 GSE_ERR("i2c client is null!!\n");
1701 return 0;
1702 }
1703
1704 return snprintf(buf, PAGE_SIZE, "%d\n", test_status);
1705 }
1706
1707
1708 #ifdef K2DH_ACCEL_CALIBRATION
1709 static int k2dh_read_accel_xyz(struct i2c_client *client, struct K2DHacc *acc)
1710 {
1711 int comres = 0;
1712 unsigned char data[6];
1713
1714 return comres;
1715 }
1716
1717 static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
1718 {
1719 GSE_FUN();
1720 struct i2c_client *client = k2dh_i2c_client;
1721 struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
1722 int offset_x,offset_y,offset_z;
1723
1724 offset_x = obj->cali_sw[K2DH_AXIS_X];
1725 offset_y = obj->cali_sw[K2DH_AXIS_Y];
1726 offset_z = obj->cali_sw[K2DH_AXIS_Z];
1727
1728 GSE_LOG("offset_x: %d, offset_y: %d, offset_z: %d\n",offset_x,offset_y,offset_z);
1729
1730 return snprintf(buf, PAGE_SIZE, "%d %d %d \n", offset_x, offset_y, offset_z);
1731 }
1732
1733 static ssize_t store_cali_value(struct device_driver *ddri, char *buf, size_t count)
1734 {
1735 struct i2c_client *client = k2dh_i2c_client;
1736 struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
1737 int err;
1738 int offset_x,offset_y,offset_z;
1739 int dat[K2DH_AXES_NUM];
1740
1741 if(!strncmp(buf, "rst", 3))
1742 {
1743 GSE_FUN();
1744 if(err = k2dh_ResetCalibration(client))
1745 {
1746 GSE_ERR("reset offset err = %d\n", err);
1747 }
1748 }
1749 else if(3 == sscanf(buf, "%d %d %d", &offset_x, &offset_y, &offset_z))
1750 {
1751 GSE_LOG("store_cali_value: x=%d, y=%d, z=%d\n", offset_x, offset_y, offset_z);
1752
1753 obj->cali_sw[K2DH_AXIS_X] = offset_x;
1754 obj->cali_sw[K2DH_AXIS_Y] = offset_y;
1755 obj->cali_sw[K2DH_AXIS_Z] = offset_z;
1756 }
1757 else
1758 {
1759 GSE_ERR("invalid format\n");
1760 }
1761
1762 return count;
1763 }
1764
1765 static ssize_t k2dh_fast_calibration_x_show(struct device_driver *ddri, char *buf)
1766 {
1767 struct i2c_client *client = k2dh_i2c_client;
1768 struct k2dh_i2c_data *k2dh = i2c_get_clientdata(client);
1769
1770 return sprintf(buf, "%d\n", atomic_read(&k2dh->fast_calib_x_rslt));
1771 }
1772
1773 static ssize_t k2dh_fast_calibration_x_store(struct device_driver *ddri, char *buf, size_t count)
1774 {
1775 unsigned long data;
1776 signed char tmp;
1777 unsigned char timeout = 0;
1778 unsigned int timeout_shaking = 0;
1779 int error;
1780 struct i2c_client *client = k2dh_i2c_client;
1781 struct k2dh_i2c_data *k2dh = i2c_get_clientdata(client);
1782 struct K2DHacc acc_cal;
1783 struct K2DHacc acc_cal_pre;
1784
1785 test_status = 4; // calibration status
1786
1787 if (error = strict_strtoul(buf, 10, &data))
1788 return error;
1789
1790 if(k2dh_do_calibration() != K2DH_SUCCESS)
1791 {
1792 atomic_set(&k2dh->fast_calib_x_rslt, 0);
1793 return -EINVAL;
1794 }
1795
1796 atomic_set(&k2dh->fast_calib_x_rslt, 1);
1797 GSE_LOG(KERN_INFO "x axis fast calibration finished\n");
1798
1799 return count;
1800 }
1801
1802 static ssize_t k2dh_fast_calibration_y_show(struct device_driver *ddri, char *buf)
1803 {
1804
1805 struct i2c_client *client = k2dh_i2c_client;
1806 struct k2dh_i2c_data *k2dh = i2c_get_clientdata(client);
1807
1808 return sprintf(buf, "%d\n", atomic_read(&k2dh->fast_calib_y_rslt));
1809 }
1810
1811 static ssize_t k2dh_fast_calibration_y_store(struct device_driver *ddri, char *buf, size_t count)
1812 {
1813 unsigned long data;
1814 int error;
1815 struct i2c_client *client = k2dh_i2c_client;
1816 struct k2dh_i2c_data *k2dh = i2c_get_clientdata(client);
1817
1818 if (error = strict_strtoul(buf, 10, &data))
1819 return error;
1820
1821 atomic_set(&k2dh->fast_calib_y_rslt, 1);
1822 GSE_LOG(KERN_INFO "y axis fast calibration finished\n");
1823
1824 return count;
1825 }
1826
1827 static ssize_t k2dh_fast_calibration_z_show(struct device_driver *ddri, char *buf)
1828 {
1829 struct i2c_client *client = k2dh_i2c_client;
1830 struct k2dh_i2c_data *k2dh = i2c_get_clientdata(client);
1831
1832 return sprintf(buf, "%d\n", atomic_read(&k2dh->fast_calib_z_rslt));
1833 }
1834
1835 static ssize_t k2dh_fast_calibration_z_store(struct device_driver *ddri, char *buf, size_t count)
1836 {
1837 unsigned long data;
1838 int error;
1839 struct i2c_client *client = k2dh_i2c_client;
1840 struct k2dh_i2c_data *k2dh = i2c_get_clientdata(client);
1841
1842 if (error = strict_strtoul(buf, 10, &data))
1843 return error;
1844
1845 atomic_set(&k2dh->fast_calib_z_rslt, 1);
1846 GSE_LOG(KERN_INFO "z axis fast calibration finished\n");
1847
1848 test_status = sensor_power;
1849
1850 return count;
1851 }
1852
1853 #if 1
1854 static int k2dh_runCalibration(void)
1855 {
1856 if(k2dh_do_calibration() != K2DH_SUCCESS)
1857 {
1858 return -EINVAL;
1859 }
1860 GSE_LOG(KERN_INFO "self calibration Done\n");
1861
1862 return K2DH_SUCCESS;
1863 }
1864 #endif
1865
1866 static ssize_t k2dh_eeprom_writing_show(struct device_driver *ddri, char *buf)
1867 {
1868 struct i2c_client *client = k2dh_i2c_client;
1869 struct k2dh_i2c_data *k2dh = i2c_get_clientdata(client);
1870
1871 return sprintf(buf, "%d\n", atomic_read(&k2dh->fast_calib_rslt));
1872 }
1873
1874 static ssize_t k2dh_eeprom_writing_store(struct device_driver *ddri, char *buf, size_t count)
1875 {
1876 unsigned char offset_x,offset_y,offset_z;
1877 struct i2c_client *client = k2dh_i2c_client;
1878 struct k2dh_i2c_data *k2dh = i2c_get_clientdata(client);
1879
1880 atomic_set(&k2dh->fast_calib_rslt, 1);
1881
1882 return count;
1883 }
1884
1885 static int k2dh_soft_reset(struct i2c_client *client)
1886 {
1887 int comres = 0;
1888 unsigned char data;
1889
1890 GSE_FUN();
1891 // no soft_reset need
1892
1893 return comres;
1894 }
1895 static ssize_t k2dh_softreset_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
1896 {
1897 struct i2c_client *client = k2dh_i2c_client;
1898 struct k2dh_i2c_data *k2dh = i2c_get_clientdata(client);
1899
1900 if (k2dh_soft_reset(k2dh->client) < 0)
1901 return -EINVAL;
1902
1903 return count;
1904 }
1905
1906 #if 0
1907 static ssize_t k2dh_runCalibration_store(struct device_driver *ddri, char *buf, size_t count)
1908 {
1909 unsigned long data = 0;
1910 int error = -1;
1911 int cali[3];
1912
1913
1914 struct i2c_client *client = k2dh_i2c_client;
1915 struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
1916
1917 if (error = strict_strtoul(buf, 10, &data))
1918 {
1919 return error;
1920 }
1921 GSE_LOG("Calibration CMD value : %d\n", (int)data);
1922
1923 if(data == 1 || data == 2) // calibration start command
1924 {
1925 if(k2dh_runCalibration() == K2DH_SUCCESS)
1926 {
1927 cali[0] = obj->cali_sw[0];
1928 cali[1] = obj->cali_sw[1];
1929 cali[2] = obj->cali_sw[2];
1930 if(LGE_FacWriteAccelerometerCalibration((unsigned int*)cali) == TRUE)
1931 {
1932 atomic_set(&obj->fast_calib_rslt, 1);
1933 }
1934 }
1935 else // wrong input
1936 {
1937 GSE_LOG("Calibration FAIL\n");
1938 return -EINVAL;
1939 }
1940 }
1941 return count;
1942 }
1943
1944 static ssize_t k2dh_runCalibration_show(struct device_driver *ddri, char *buf)
1945 {
1946 int selfCalibration = 1; // fail
1947
1948 struct i2c_client *client = k2dh_i2c_client;
1949 struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
1950
1951 if(atomic_read(&obj->fast_calib_rslt) == 1) // calibration success
1952 {
1953 selfCalibration = 0; // success
1954 }
1955 else
1956 {
1957 selfCalibration = 1; // fail
1958 }
1959 return sprintf(buf, "%d\n", selfCalibration);
1960 }
1961
1962 #endif
1963
1964 #endif // END OF K2DH_ACCEL_CALIBRATION
1965
1966 #if 1
1967 static ssize_t k2dh_SelfTest_store(struct device_driver *ddri, char *buf, size_t count)
1968 {
1969 unsigned long data = 0;
1970 int error = -1;
1971
1972 struct i2c_client *client = k2dh_i2c_client;
1973 struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
1974
1975 if (error = strict_strtoul(buf, 10, &data))
1976 {
1977 return error;
1978 }
1979 GSE_LOG("Self test CMD value : %d\n", (int)data);
1980
1981 if(data == 1) // self test start command
1982 {
1983 if(k2dh_get_selftest(client) >= 0)
1984 {
1985 atomic_set(&obj->selftest_rslt, 1);
1986 }
1987 else
1988 {
1989 atomic_set(&obj->selftest_rslt, 0);
1990 }
1991 }
1992 else // wrong input
1993 {
1994 GSE_LOG("SelfTest FAIL\n");
1995 return -EINVAL;
1996 }
1997 return count;
1998 }
1999
2000 static ssize_t k2dh_SelfTest_show(struct device_driver *ddri, char *buf)
2001 {
2002 int selftest = 1; // fail
2003
2004 struct i2c_client *client = k2dh_i2c_client;
2005 struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
2006
2007 if(atomic_read(&obj->selftest_rslt) == 1) // selftest success
2008 {
2009 selftest = 0; // success
2010 }
2011 else
2012 {
2013 selftest = 1; // fail
2014 }
2015 return sprintf(buf, "%d\n", selftest);
2016 }
2017 #endif
2018
2019 /*----------------------------------------------------------------------------*/
2020 static DRIVER_ATTR(chipinfo, S_IWUSR|S_IRUGO, show_chipinfo_value, NULL);
2021 static DRIVER_ATTR(opmode, S_IWUSR|S_IRUGO|S_IWGRP, show_opmode_value, store_opmode_value);
2022 static DRIVER_ATTR(range, S_IWUSR|S_IRUGO|S_IWGRP, show_range_value, store_range_value);
2023 static DRIVER_ATTR(bandwidth, S_IWUSR|S_IRUGO|S_IWGRP, show_bandwidth_value, store_bandwidth_value);
2024 static DRIVER_ATTR(sensordata, S_IWUSR|S_IRUGO, show_sensordata_value, NULL);
2025 static DRIVER_ATTR(sensorrawdata, S_IWUSR|S_IRUGO, show_sensorrawdata_value, NULL);
2026 static DRIVER_ATTR(trace, S_IWUSR|S_IRUGO|S_IWGRP, show_trace_value, store_trace_value);
2027 static DRIVER_ATTR(status, S_IRUGO, show_status_value, NULL);
2028 static DRIVER_ATTR(powerstatus, S_IRUGO, show_power_status_value, NULL);
2029 static DRIVER_ATTR(teststatus, S_IWUSR|S_IRUGO, show_teststatus_value, NULL);
2030 #ifdef K2DH_ACCEL_CALIBRATION
2031 static DRIVER_ATTR(softreset, S_IWUSR|S_IWGRP, NULL, k2dh_softreset_store);
2032 static DRIVER_ATTR(cali, S_IWUSR|S_IRUGO|S_IWGRP, show_cali_value, store_cali_value);
2033 static DRIVER_ATTR(fast_calibration_x, S_IRUGO|S_IWUSR|S_IWGRP, k2dh_fast_calibration_x_show, k2dh_fast_calibration_x_store);
2034 static DRIVER_ATTR(fast_calibration_y, S_IRUGO|S_IWUSR|S_IWGRP, k2dh_fast_calibration_y_show, k2dh_fast_calibration_y_store);
2035 static DRIVER_ATTR(fast_calibration_z, S_IRUGO|S_IWUSR|S_IWGRP, k2dh_fast_calibration_z_show, k2dh_fast_calibration_z_store);
2036 static DRIVER_ATTR(eeprom_writing, S_IRUGO|S_IWUSR|S_IWGRP, k2dh_eeprom_writing_show, k2dh_eeprom_writing_store);
2037
2038 #if 0
2039 static DRIVER_ATTR(run_fast_calibration, S_IRUGO|S_IWUSR|S_IWGRP, k2dh_runCalibration_show, k2dh_runCalibration_store);
2040 #endif
2041 #endif
2042 #ifdef CONFIG_K2DH_LOWPASS
2043 static DRIVER_ATTR(firlen, S_IWUSR|S_IRUGO|S_IWGRP, show_firlen_value, store_firlen_value);
2044 #endif
2045 #if 1
2046 static DRIVER_ATTR(selftest, S_IRUGO|S_IWUSR|S_IWGRP, k2dh_SelfTest_show, k2dh_SelfTest_store);
2047 #endif
2048
2049
2050 /*----------------------------------------------------------------------------*/
2051 static struct driver_attribute *k2dh_attr_list[] = {
2052 &driver_attr_chipinfo, /*chip information*/
2053 &driver_attr_opmode,
2054 &driver_attr_range,
2055 &driver_attr_bandwidth,
2056 &driver_attr_sensordata, /*dump sensor data*/
2057 &driver_attr_sensorrawdata,
2058 &driver_attr_trace, /*trace log*/
2059 &driver_attr_status,
2060 &driver_attr_powerstatus,
2061 &driver_attr_teststatus,
2062 #ifdef K2DH_ACCEL_CALIBRATION
2063 &driver_attr_softreset,
2064 &driver_attr_cali, /*show calibration data*/
2065 &driver_attr_fast_calibration_x,
2066 &driver_attr_fast_calibration_y,
2067 &driver_attr_fast_calibration_z,
2068 &driver_attr_eeprom_writing,
2069 #if 0
2070 &driver_attr_run_fast_calibration,
2071 #endif
2072 #endif
2073 #ifdef CONFIG_K2DH_LOWPASS
2074 &driver_attr_firlen, /*filter length: 0: disable, others: enable*/
2075 #endif
2076 #if 1
2077 &driver_attr_selftest,
2078 #endif
2079 };
2080 /*----------------------------------------------------------------------------*/
2081 static int k2dh_create_attr(struct device_driver *driver)
2082 {
2083 int idx = 0, err = 0;
2084 int num = (int)(sizeof(k2dh_attr_list)/sizeof(k2dh_attr_list[0]));
2085 if (driver == NULL)
2086 {
2087 return -EINVAL;
2088 }
2089
2090 for(idx = 0; idx < num; idx++)
2091 {
2092 if(err = driver_create_file(driver, k2dh_attr_list[idx]))
2093 {
2094 GSE_ERR("driver_create_file (%s) = %d\n", k2dh_attr_list[idx]->attr.name, err);
2095 break;
2096 }
2097 }
2098 return err;
2099 }
2100 /*----------------------------------------------------------------------------*/
2101 static int k2dh_delete_attr(struct device_driver *driver)
2102 {
2103 int idx = 0 ,err = 0;
2104 int num = (int)(sizeof(k2dh_attr_list)/sizeof(k2dh_attr_list[0]));
2105
2106 if(driver == NULL)
2107 {
2108 return -EINVAL;
2109 }
2110
2111 for(idx = 0; idx < num; idx++)
2112 {
2113 driver_remove_file(driver, k2dh_attr_list[idx]);
2114 }
2115
2116 return err;
2117 }
2118
2119 /*----------------------------------------------------------------------------*/
2120 int gsensor_operate(void* self, uint32_t command, void* buff_in, int size_in, void* buff_out, int size_out, int* actualout)
2121 {
2122 int err = 0;
2123 int value = 0, sample_delay = 0;
2124 struct k2dh_i2c_data *priv = (struct k2dh_i2c_data*)self;
2125 hwm_sensor_data* gsensor_data;
2126 char buff[K2DH_BUFSIZE] = {0};
2127
2128 //GSE_FUN(f);
2129 switch (command)
2130 {
2131 case SENSOR_DELAY:
2132 if((buff_in == NULL) || (size_in < sizeof(int)))
2133 {
2134 GSE_ERR("Set delay parameter error!\n");
2135 err = -EINVAL;
2136 }
2137 else
2138 {
2139 value = *(int *)buff_in;
2140
2141 #if 1
2142 sample_delay = ODR200; // 5ms
2143 #else
2144 if(value >= 100)
2145 {
2146 sample_delay = ODR10; // 100ms
2147 }
2148 else if(value >= 40)
2149 {
2150 sample_delay = ODR25; // 40ms
2151 }
2152 else if(value >= 20)
2153 {
2154 sample_delay = ODR50; // 20ms
2155 }
2156 else if(value >= 10)
2157 {
2158 sample_delay = ODR100; // 10ms
2159 }
2160 else
2161 {
2162 sample_delay = ODR200; // 5ms
2163 }
2164 #endif
2165 err = k2dh_SetBWRate(priv->client, sample_delay);
2166 if(err != K2DH_SUCCESS )
2167 {
2168 GSE_ERR("Set delay parameter error!\n");
2169 }
2170
2171 if(value >= 50)
2172 {
2173 atomic_set(&priv->filter, 0);
2174 }
2175 else
2176 {
2177 #if defined(CONFIG_K2DH_LOWPASS)
2178 priv->fir.num = 0;
2179 priv->fir.idx = 0;
2180 priv->fir.sum[K2DH_AXIS_X] = 0;
2181 priv->fir.sum[K2DH_AXIS_Y] = 0;
2182 priv->fir.sum[K2DH_AXIS_Z] = 0;
2183 atomic_set(&priv->filter, 1);
2184 #endif
2185 }
2186 }
2187 break;
2188
2189 case SENSOR_ENABLE:
2190 if((buff_in == NULL) || (size_in < sizeof(int)))
2191 {
2192 GSE_ERR("Enable sensor parameter error!\n");
2193 err = -EINVAL;
2194 }
2195 else
2196 {
2197 value = *(int *)buff_in;
2198 if(((value == 0) && (sensor_power == false)) || ((value == 1) && (sensor_power == true)))
2199 {
2200 GSE_LOG("Gsensor device have updated!, power: %d\n", sensor_power);
2201 }
2202 else
2203 {
2204 err = k2dh_SetPowerMode( priv->client, !sensor_power);
2205 }
2206 }
2207 break;
2208
2209 case SENSOR_GET_DATA:
2210 if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
2211 {
2212 GSE_ERR("get sensor data parameter error!\n");
2213 err = -EINVAL;
2214 }
2215 else
2216 {
2217 gsensor_data = (hwm_sensor_data *)buff_out;
2218 k2dh_ReadSensorData(priv->client, buff, K2DH_BUFSIZE);
2219 sscanf(buff, "%x %x %x", &gsensor_data->values[0], &gsensor_data->values[1], &gsensor_data->values[2]);
2220 gsensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;
2221 gsensor_data->value_divide = 1000;
2222 }
2223 break;
2224
2225 default:
2226 GSE_ERR("gsensor operate function no this parameter %d!\n", command);
2227 err = -1;
2228 break;
2229 }
2230
2231 return err;
2232 }
2233
2234 /******************************************************************************
2235 * Function Configuration
2236 ******************************************************************************/
2237 static int k2dh_open(struct inode *inode, struct file *file)
2238 {
2239 file->private_data = k2dh_i2c_client;
2240
2241 if(file->private_data == NULL)
2242 {
2243 GSE_ERR("null pointer!!\n");
2244 return -EINVAL;
2245 }
2246 return nonseekable_open(inode, file);
2247 }
2248 /*----------------------------------------------------------------------------*/
2249 static int k2dh_release(struct inode *inode, struct file *file)
2250 {
2251 file->private_data = NULL;
2252 return 0;
2253 }
2254 /*----------------------------------------------------------------------------*/
2255 static long k2dh_unlocked_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
2256 {
2257 struct i2c_client *client = (struct i2c_client*)file->private_data;
2258 struct k2dh_i2c_data *obj = (struct k2dh_i2c_data*)i2c_get_clientdata(client);
2259 char strbuf[K2DH_BUFSIZE] = {0};
2260 void __user *data;
2261 SENSOR_DATA sensor_data;
2262 long err = 0;
2263 int cali[3] = {0};
2264 uint32_t enable = 0;
2265
2266 //GSE_FUN(f);
2267 if(_IOC_DIR(cmd) & _IOC_READ)
2268 {
2269 err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
2270 }
2271 else if(_IOC_DIR(cmd) & _IOC_WRITE)
2272 {
2273 err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
2274 }
2275
2276 if(err)
2277 {
2278 GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
2279 return -EFAULT;
2280 }
2281
2282 switch(cmd)
2283 {
2284 #if 1
2285 #if 0
2286 case GSENSOR_IOCTL_SET_ENABLE:
2287 GSE_LOG("GSENSOR_IOCTL_SET_ENABLE\n");
2288 data = (void __user *) arg;
2289 if(data == NULL)
2290 {
2291 err = -EINVAL;
2292 break;
2293 }
2294 if(copy_from_user(&enable, data, sizeof(enable)))
2295 {
2296 return -EFAULT;
2297 }
2298 else
2299 {
2300 if(enable == 1)
2301 {
2302 k2dh_SetPowerMode( obj_i2c_data->client, 1);
2303 }
2304 else if(enable == 0)
2305 {
2306 k2dh_SetPowerMode( obj_i2c_data->client, 0);
2307 GSE_LOG("coolpisoo, test 1: %d\n", data2[0]);
2308 msleep(10);
2309 k2dh_acc_register_read(client, data2, CTRL_REG2);
2310 GSE_LOG("coolpisoo, test 2: %d\n", data2[0]);
2311 msleep(10);
2312 k2dh_acc_register_read(client, data2, CTRL_REG3);
2313 GSE_LOG("coolpisoo, test 3: %d\n", data2[0]);
2314 msleep(10);
2315 k2dh_acc_register_read(client, data2, CTRL_REG4);
2316 GSE_LOG("coolpisoo, test 4: %d\n", data2[0]);
2317 msleep(10);
2318 k2dh_acc_register_read(client, data2, CTRL_REG5);
2319 GSE_LOG("coolpisoo, test 5: %d\n", data2[0]);
2320 msleep(10);
2321 k2dh_acc_register_read(client, data2, CTRL_REG6);
2322 GSE_LOG("coolpisoo, test 6: %d\n", data2[0]);
2323 msleep(10);
2324 k2dh_acc_register_read(client, data2, FIFO_CTRL_REG);
2325 GSE_LOG("coolpisoo, test 7: %d\n", data2[0]);
2326 msleep(10);
2327 k2dh_acc_register_read(client, data2, TEMP_CFG_REG);
2328 GSE_LOG("coolpisoo, test 8: %d\n", data2[0]);
2329 msleep(10);
2330 }
2331 }
2332 break;
2333 case GSENSOR_IOCTL_GET_STATUS:
2334 GSE_LOG("GSENSOR_IOCTL_GET_STATUS\n");
2335 data = (void __user *) arg;
2336 if(data == NULL)
2337 {
2338 err = -EINVAL;
2339 break;
2340 }
2341 if(copy_to_user(data, &sensor_power, sizeof(sensor_power)))
2342 {
2343 return -EFAULT;
2344 }
2345 break;
2346 #endif /* 0 */
2347 case GSENSOR_IOCTL_INIT:
2348 k2dh_acc_hw_init(client, 0);
2349 break;
2350
2351 case GSENSOR_IOCTL_READ_CHIPINFO:
2352 data = (void __user *) arg;
2353 if(data == NULL)
2354 {
2355 err = -EINVAL;
2356 break;
2357 }
2358
2359 k2dh_ReadChipInfo(client, strbuf, K2DH_BUFSIZE);
2360 if(copy_to_user(data, strbuf, strlen(strbuf)+1))
2361 {
2362 err = -EFAULT;
2363 break;
2364 }
2365 break;
2366
2367 case GSENSOR_IOCTL_READ_SENSORDATA:
2368 data = (void __user *) arg;
2369 if(data == NULL)
2370 {
2371 err = -EINVAL;
2372 break;
2373 }
2374
2375 k2dh_ReadSensorData(client, strbuf, K2DH_BUFSIZE);
2376 if(copy_to_user(data, strbuf, strlen(strbuf)+1))
2377 {
2378 err = -EFAULT;
2379 break;
2380 }
2381 break;
2382
2383 case GSENSOR_IOCTL_READ_GAIN:
2384 data = (void __user *) arg;
2385 if(data == NULL)
2386 {
2387 err = -EINVAL;
2388 break;
2389 }
2390
2391 if(copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
2392 {
2393 err = -EFAULT;
2394 break;
2395 }
2396 break;
2397
2398 case GSENSOR_IOCTL_READ_RAW_DATA:
2399 data = (void __user *) arg;
2400 if(data == NULL)
2401 {
2402 err = -EINVAL;
2403 break;
2404 }
2405 //k2dh_ReadRawData(client, strbuf);
2406 if(copy_to_user(data, &strbuf, strlen(strbuf)+1))
2407 {
2408 err = -EFAULT;
2409 break;
2410 }
2411 break;
2412
2413 case GSENSOR_IOCTL_SET_CALI:
2414 data = (void __user*)arg;
2415 if(data == NULL)
2416 {
2417 err = -EINVAL;
2418 break;
2419 }
2420 if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
2421 {
2422 err = -EFAULT;
2423 break;
2424 }
2425
2426 GSE_LOG("GSENSOR_IOCTL_SET_CALI, Cal data : 0x%x, 0x%x, 0x%x\n", sensor_data.x, sensor_data.y, sensor_data.z);
2427 if((sensor_data.x == 0) && (sensor_data.y == 0) && (sensor_data.z == 0))
2428 { // temp defensive code for nvram_daemon ioctl
2429 break;
2430 }
2431
2432 obj->cali_sw[K2DH_AXIS_X] = sensor_data.x;
2433 obj->cali_sw[K2DH_AXIS_Y] = sensor_data.y;
2434 obj->cali_sw[K2DH_AXIS_Z] = sensor_data.z;
2435 if(obj->cali_sw[K2DH_AXIS_X]>1000|| obj->cali_sw[K2DH_AXIS_X]<-1000||
2436 obj->cali_sw[K2DH_AXIS_Y]>1000|| obj->cali_sw[K2DH_AXIS_Y]<-1000||
2437 obj->cali_sw[K2DH_AXIS_Z]>1000|| obj->cali_sw[K2DH_AXIS_Z]<-1000){
2438 GSE_LOG("Unnormal Cal Data");
2439 obj->cali_sw[K2DH_AXIS_X] = 0;
2440 obj->cali_sw[K2DH_AXIS_Y] = 0;
2441 obj->cali_sw[K2DH_AXIS_Z] = 0;
2442 }
2443 break;
2444
2445 case GSENSOR_IOCTL_CLR_CALI:
2446 err = k2dh_ResetCalibration(client);
2447 break;
2448
2449 case GSENSOR_IOCTL_GET_CALI:
2450 GSE_LOG("GSENSOR_IOCTL_GET_CALI\n");
2451 break;
2452 #endif /* 0 */
2453
2454 default:
2455 GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
2456 err = -ENOIOCTLCMD;
2457 break;
2458 }
2459
2460 return err;
2461 }
2462
2463 /*----------------------------------------------------------------------------*/
2464 static struct file_operations k2dh_fops = {
2465 //.owner = THIS_MODULE,
2466 .open = k2dh_open,
2467 .release = k2dh_release,
2468 .unlocked_ioctl = k2dh_unlocked_ioctl,
2469 };
2470 /*----------------------------------------------------------------------------*/
2471 static struct miscdevice k2dh_device = {
2472 .minor = MISC_DYNAMIC_MINOR,
2473 .name = "gsensor",
2474 .fops = &k2dh_fops,
2475 };
2476 /*----------------------------------------------------------------------------*/
2477 #ifndef CONFIG_HAS_EARLYSUSPEND
2478 /*----------------------------------------------------------------------------*/
2479 static int k2dh_suspend(struct i2c_client *client, pm_message_t msg)
2480 {
2481 GSE_FUN();
2482
2483 return 0;
2484 }
2485 /*----------------------------------------------------------------------------*/
2486 static int k2dh_resume(struct i2c_client *client)
2487 {
2488 GSE_FUN();
2489
2490 return 0;
2491 }
2492 /*----------------------------------------------------------------------------*/
2493 #else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
2494 /*----------------------------------------------------------------------------*/
2495 static void k2dh_early_suspend(struct early_suspend *h)
2496 {
2497 GSE_FUN();
2498 }
2499 /*----------------------------------------------------------------------------*/
2500 static void k2dh_late_resume(struct early_suspend *h)
2501 {
2502 GSE_FUN();
2503 }
2504
2505 /*----------------------------------------------------------------------------*/
2506 #endif /*CONFIG_HAS_EARLYSUSPEND*/
2507 /*----------------------------------------------------------------------------*/
2508 static int k2dh_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
2509 {
2510 struct i2c_client *new_client;
2511 struct k2dh_i2c_data *obj;
2512 struct hwmsen_object sobj;
2513 int err = 0;
2514 GSE_FUN();
2515
2516 if(!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
2517 {
2518 err = -ENOMEM;
2519 goto exit;
2520 }
2521
2522 memset(obj, 0, sizeof(struct k2dh_i2c_data));
2523
2524 obj->hw = get_cust_acc_hw();
2525 if(err = hwmsen_get_convert(obj->hw->direction, &obj->cvt))
2526 {
2527 GSE_ERR("invalid direction: %d\n", obj->hw->direction);
2528 goto exit;
2529 }
2530
2531 obj_i2c_data = obj;
2532 obj->client = client;
2533 new_client = obj->client;
2534 i2c_set_clientdata(new_client,obj);
2535
2536 atomic_set(&obj->trace, 0);
2537 atomic_set(&obj->suspend, 0);
2538
2539 #ifdef CONFIG_K2DH_LOWPASS
2540 if(obj->hw->firlen > C_MAX_FIR_LENGTH)
2541 {
2542 atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
2543 }
2544 else
2545 {
2546 atomic_set(&obj->firlen, obj->hw->firlen);
2547 }
2548
2549 if(atomic_read(&obj->firlen) > 0)
2550 {
2551 atomic_set(&obj->fir_en, 1);
2552 }
2553
2554 #endif
2555
2556 k2dh_i2c_client = new_client;
2557
2558 if(err = k2dh_acc_hw_init(new_client, 1))
2559 {
2560 GSE_ERR ( "failed to init K2DH ( err = %d )\n", err );
2561 goto exit_init_failed;
2562 }
2563
2564 if(err = misc_register(&k2dh_device))
2565 {
2566 GSE_ERR("k2dh_device register failed\n");
2567 goto exit_misc_device_register_failed;
2568 }
2569
2570 if(err = k2dh_create_attr(&k2dh_gsensor_driver.driver))
2571 {
2572 GSE_ERR("create attribute err = %d\n", err);
2573 goto exit_create_attr_failed;
2574 }
2575
2576 sobj.self = obj;
2577 sobj.polling = 1;
2578 sobj.sensor_operate = gsensor_operate;
2579 if(err = hwmsen_attach(ID_ACCELEROMETER, &sobj))
2580 {
2581 GSE_ERR("attach fail = %d\n", err);
2582 goto exit_kfree;
2583 }
2584
2585 #ifdef CONFIG_HAS_EARLYSUSPEND
2586 obj->early_drv.level = EARLY_SUSPEND_LEVEL_DISABLE_FB - 1,
2587 obj->early_drv.suspend = k2dh_early_suspend,
2588 obj->early_drv.resume = k2dh_late_resume,
2589 register_early_suspend(&obj->early_drv);
2590 #endif
2591
2592 GSE_LOG("%s: OK\n", __func__);
2593 return 0;
2594
2595 exit_create_attr_failed:
2596 misc_deregister(&k2dh_device);
2597 exit_misc_device_register_failed:
2598 exit_init_failed:
2599 //i2c_detach_client(new_client);
2600 exit_kfree:
2601 kfree(obj);
2602 exit:
2603 GSE_ERR("%s: err = %d\n", __func__, err);
2604 return err;
2605 }
2606
2607 /*----------------------------------------------------------------------------*/
2608 static int k2dh_i2c_remove(struct i2c_client *client)
2609 {
2610 int err = 0;
2611
2612 if(err = k2dh_delete_attr(&k2dh_gsensor_driver.driver))
2613 {
2614 GSE_ERR("k2dh_delete_attr fail: %d\n", err);
2615 }
2616
2617 if(err = misc_deregister(&k2dh_device))
2618 {
2619 GSE_ERR("misc_deregister fail: %d\n", err);
2620 }
2621
2622 if(err = hwmsen_detach(ID_ACCELEROMETER))
2623
2624
2625 k2dh_i2c_client = NULL;
2626 i2c_unregister_device(client);
2627 kfree(i2c_get_clientdata(client));
2628 return 0;
2629 }
2630 /*----------------------------------------------------------------------------*/
2631 static int k2dh_probe(struct platform_device *pdev)
2632 {
2633 struct acc_hw *hw = get_cust_acc_hw();
2634
2635 GSE_FUN();
2636
2637 if(i2c_add_driver(&k2dh_i2c_driver))
2638 {
2639 GSE_ERR("add driver error\n");
2640 return -1;
2641 }
2642 return 0;
2643 }
2644 /*----------------------------------------------------------------------------*/
2645 static int k2dh_remove(struct platform_device *pdev)
2646 {
2647 struct acc_hw *hw = get_cust_acc_hw();
2648
2649 GSE_FUN();
2650 k2dh_power(hw, 0);
2651 i2c_del_driver(&k2dh_i2c_driver);
2652 return 0;
2653 }
2654 /*----------------------------------------------------------------------------*/
2655 static struct platform_driver k2dh_gsensor_driver = {
2656 .probe = k2dh_probe,
2657 .remove = k2dh_remove,
2658 .driver = {
2659 .name = "gsensor",
2660 }
2661 };
2662
2663 /*----------------------------------------------------------------------------*/
2664 static int __init k2dh_init(void)
2665 {
2666 struct acc_hw *hw = get_cust_acc_hw();
2667
2668 GSE_FUN();
2669 i2c_register_board_info(hw->i2c_num, &k2dh_i2c_info, 1);
2670 if(platform_driver_register(&k2dh_gsensor_driver))
2671 {
2672 GSE_ERR("failed to register driver");
2673 return -ENODEV;
2674 }
2675 return 0;
2676 }
2677 /*----------------------------------------------------------------------------*/
2678 static void __exit k2dh_exit(void)
2679 {
2680 GSE_FUN();
2681 platform_driver_unregister(&k2dh_gsensor_driver);
2682 }
2683 /*----------------------------------------------------------------------------*/
2684 module_init(k2dh_init);
2685 module_exit(k2dh_exit);
2686 /*----------------------------------------------------------------------------*/
2687 MODULE_LICENSE("GPL");
2688 MODULE_DESCRIPTION("K2DH Accelerometer misc driver");
2689 MODULE_AUTHOR("STMcroelectronics");
kernel source for telekom puls (alcatel/mediatek)