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 / magnetometer / lsm303m / lsm303m.c
1
2 /* LSM303M motion sensor driver
3 *
4 *
5 *
6 * This software is licensed under the terms of the GNU General Public
7 * License version 2, as published by the Free Software Foundation, and
8 * may be copied, distributed, and modified under those terms.
9 *
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
14 *
15 */
16
17
18 #include <linux/interrupt.h>
19 #include <linux/i2c.h>
20 #include <linux/slab.h>
21 #include <linux/irq.h>
22 #include <linux/miscdevice.h>
23 #include <asm/uaccess.h>
24 #include <asm/atomic.h>
25 #include <linux/delay.h>
26 #include <linux/input.h>
27 #include <linux/workqueue.h>
28 #include <linux/kobject.h>
29 #include <linux/platform_device.h>
30 #include <linux/earlysuspend.h>
31
32 #include <linux/hwmsensor.h>
33 #include <linux/hwmsen_dev.h>
34 #include <linux/sensors_io.h>
35
36
37 #include <cust_mag.h>
38 #include "lsm303m.h"
39 #include <linux/hwmsen_helper.h>
40
41
42
43
44 #include <mach/mt_typedefs.h>
45 #include <mach/mt_gpio.h>
46 #include <mach/mt_pm_ldo.h>
47
48
49
50 /*-------------------------MT6516&MT6573 define-------------------------------*/
51
52 #define POWER_NONE_MACRO MT65XX_POWER_NONE
53
54
55
56
57 #define SW_CALIBRATION
58
59 /*----------------------------------------------------------------------------*/
60 #define I2C_DRIVERID_LSM303M 345
61 /*----------------------------------------------------------------------------*/
62 #define DEBUG 1
63 /*----------------------------------------------------------------------------*/
64 #define CONFIG_LSM303M_LOWPASS /*apply low pass filter on output*/
65 /*----------------------------------------------------------------------------*/
66 #define LSM303M_AXIS_X 0
67 #define LSM303M_AXIS_Y 1
68 #define LSM303M_AXIS_Z 2
69 #define LSM303M_AXES_NUM 3
70 #define LSM303M_DATA_LEN 6
71 #define LSM303M_DEV_NAME "lsm303m"
72
73
74 static DECLARE_WAIT_QUEUE_HEAD(data_ready_wq);
75 static DECLARE_WAIT_QUEUE_HEAD(open_wq);
76
77 static atomic_t open_flag = ATOMIC_INIT(0);
78 static atomic_t m_flag = ATOMIC_INIT(0);
79 static atomic_t o_flag = ATOMIC_INIT(0);
80 /*----------------------------------------------------------------------------*/
81 static atomic_t dev_open_count;
82
83 /*----------------------------------------------------------------------------*/
84 static const struct i2c_device_id lsm303m_i2c_id[] = {{LSM303M_DEV_NAME,0},{}};
85 static struct i2c_board_info __initdata i2c_lsm303m={ I2C_BOARD_INFO("lsm303m", 0x3B>>1)};
86 /*the adapter id will be available in customization*/
87 //static unsigned short lsm303m_force[] = {0x00, LSM303M_I2C_SLAVE_ADDR, I2C_CLIENT_END, I2C_CLIENT_END};
88 //static const unsigned short *const lsm303m_forces[] = { lsm303m_force, NULL };
89 //static struct i2c_client_address_data lsm303m_addr_data = { .forces = lsm303m_forces,};
90
91 /*----------------------------------------------------------------------------*/
92 static int lsm303m_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id);
93 static int lsm303m_i2c_remove(struct i2c_client *client);
94 //static int lsm303m_i2c_detect(struct i2c_client *client, int kind, struct i2c_board_info *info);
95 static int lsm303m_suspend(struct i2c_client *client, pm_message_t msg) ;
96 static int lsm303m_resume(struct i2c_client *client);
97
98 /*----------------------------------------------------------------------------*/
99 typedef enum {
100 ADX_TRC_FILTER = 0x01,
101 ADX_TRC_RAWDATA = 0x02,
102 ADX_TRC_IOCTL = 0x04,
103 ADX_TRC_CALI = 0X08,
104 ADX_TRC_INFO = 0X10,
105
106 } ADX_TRC;
107 /*----------------------------------------------------------------------------*/
108 struct scale_factor{
109 u8 whole;
110 u8 fraction;
111 };
112 /*----------------------------------------------------------------------------*/
113 struct data_resolution {
114 struct scale_factor scalefactor;
115 int sensitivity;
116 };
117 /*----------------------------------------------------------------------------*/
118 #define C_MAX_FIR_LENGTH (32)
119 /*----------------------------------------------------------------------------*/
120 struct data_filter {
121 s16 raw[C_MAX_FIR_LENGTH][LSM303M_AXES_NUM];
122 int sum[LSM303M_AXES_NUM];
123 int num;
124 int idx;
125 };
126 /*----------------------------------------------------------------------------*/
127 struct _lsm303m_data {
128 rwlock_t lock;
129 int mode;
130 int rate;
131 volatile int updated;
132 } lsm303m_data;
133
134 /*----------------------------------------------------------------------------*/
135 struct lsm303m_data {
136 rwlock_t datalock;
137 rwlock_t ctrllock;
138 int controldata[10];
139 unsigned int debug;
140 int yaw;
141 int roll;
142 int pitch;
143 int nmx;
144 int nmy;
145 int nmz;
146 int nax;
147 int nay;
148 int naz;
149 int mag_status;
150 }lsm303mmid_data;
151
152 /*----------------------------------------------------------------------------*/
153 struct lsm303m_i2c_data {
154 struct i2c_client *client;
155 struct mag_hw *hw;
156 struct hwmsen_convert cvt;
157
158 /*misc*/
159 struct data_resolution reso;
160 atomic_t trace;
161 atomic_t layout;
162 atomic_t suspend;
163 atomic_t selftest;
164 atomic_t filter;
165 s16 cali_sw[LSM303M_AXES_NUM+1];
166
167 /*data*/
168 s8 offset[LSM303M_AXES_NUM+1]; /*+1: for 4-byte alignment*/
169 s32 data[LSM303M_AXES_NUM+1];
170
171 #if defined(CONFIG_LSM303M_LOWPASS)
172 atomic_t firlen;
173 atomic_t fir_en;
174 struct data_filter fir;
175 #endif
176 /*early suspend*/
177 #if defined(CONFIG_HAS_EARLYSUSPEND)
178 struct early_suspend early_drv;
179 #endif
180 };
181 /*----------------------------------------------------------------------------*/
182 static struct i2c_driver lsm303m_i2c_driver = {
183 .driver = {
184 // .owner = THIS_MODULE,
185 .name = LSM303M_DEV_NAME,
186 },
187 .probe = lsm303m_i2c_probe,
188 .remove = lsm303m_i2c_remove,
189 // .detect = lsm303m_i2c_detect,
190 //#if !defined(CONFIG_HAS_EARLYSUSPEND)
191 .suspend = lsm303m_suspend,
192 .resume = lsm303m_resume,
193 //#endif
194 .id_table = lsm303m_i2c_id,
195 //.address_list= lsm303m_forces,
196 };
197
198 /*----------------------------------------------------------------------------*/
199 static struct i2c_client *lsm303m_i2c_client = NULL;
200 static struct platform_driver lsm303m_msensor_driver;
201 static struct lsm303m_i2c_data *obj_i2c_data = NULL;
202 static bool sensor_power = false;
203 //static MSENSOR_VECTOR3D msensor_gain;
204 static char selftestRes[8]= {0};
205
206 /*----------------------------------------------------------------------------*/
207 #define MSE_TAG "MSENSOR"
208 #define MSE_FUN(f) printk(MSE_TAG" %s\r\n", __FUNCTION__)
209 #define MSE_ERR(fmt, args...) printk(KERN_ERR MSE_TAG" %s %d : \r\n" fmt, __FUNCTION__, __LINE__, ##args)
210 #define MSE_LOG(fmt, args...) printk(MSE_TAG fmt, ##args)
211 #define MSE_VER(fmt, args...) ((void)0)
212
213
214 /*----------------------------------------------------------------------------*/
215 static struct data_resolution lsm303m_data_resolution[] = {
216 /*8 combination by {FULL_RES,RANGE}*/
217 {{ 3, 9}, 256}, /*+/-2g in 10-bit resolution: 3.9 mg/LSB*/
218 {{ 7, 8}, 128}, /*+/-4g in 10-bit resolution: 7.8 mg/LSB*/
219 {{15, 6}, 64}, /*+/-8g in 10-bit resolution: 15.6 mg/LSB*/
220 {{31, 2}, 32}, /*+/-16g in 10-bit resolution: 31.2 mg/LSB*/
221 {{ 3, 9}, 256}, /*+/-2g in 10-bit resolution: 3.9 mg/LSB (full-resolution)*/
222 {{ 3, 9}, 256}, /*+/-4g in 11-bit resolution: 3.9 mg/LSB (full-resolution)*/
223 {{ 3, 9}, 256}, /*+/-8g in 12-bit resolution: 3.9 mg/LSB (full-resolution)*/
224 {{ 3, 9}, 256}, /*+/-16g in 13-bit resolution: 3.9 mg/LSB (full-resolution)*/
225 };
226 /*----------------------------------------------------------------------------*/
227 static struct data_resolution lsm303m_offset_resolution = {{15, 6}, 64};
228
229 /*--------------------ADXL power control function----------------------------------*/
230 static void lsm303m_power(struct mag_hw *hw, unsigned int on)
231 {
232 static unsigned int power_on = 0;
233
234 if(hw->power_id != POWER_NONE_MACRO) // have externel LDO
235 {
236 MSE_LOG("power %s\n", on ? "on" : "off");
237 if(power_on == on) // power status not change
238 {
239 MSE_LOG("ignore power control: %d\n", on);
240 }
241 else if(on) // power on
242 {
243 if(!hwPowerOn(hw->power_id, hw->power_vol, "LSM303M"))
244 {
245 MSE_ERR("power on fails!!\n");
246 }
247 }
248 else // power off
249 {
250 if (!hwPowerDown(hw->power_id, "LSM303M"))
251 {
252 MSE_ERR("power off fail!!\n");
253 }
254 }
255 }
256 power_on = on;
257 }
258
259
260
261 /*----------------------------------------- OK -----------------------------------*/
262 static int lsm303m_CheckDeviceID(struct i2c_client *client)
263 {
264 //MSE_LOG("++++++++++++++++++++++LSM303M_CheckDeviceID!");
265
266 u8 databuf[10];
267 int res = 0;
268
269 memset(databuf, 0, sizeof(u8)*10);
270 databuf[0] = LSM303M_REG_DEVID;
271
272 res = i2c_master_send(client, databuf, 0x1);
273 if(res <= 0)
274 {
275 goto exit_LSM303M_CheckDeviceID;
276 }
277
278 udelay(500);
279
280 databuf[0] = 0x0;
281 res = i2c_master_recv(client, databuf, 0x01);
282 if(res <= 0)
283 {
284 goto exit_LSM303M_CheckDeviceID;
285 }
286
287
288 if(databuf[0]!=LSM303M_FIXED_DEVID)
289 {
290 return LSM303M_ERR_IDENTIFICATION;
291 }
292
293 exit_LSM303M_CheckDeviceID:
294 if (res <= 0)
295 {
296 return LSM303M_ERR_I2C;
297 }
298
299 return LSM303M_SUCCESS;
300 }
301
302
303
304 /*--------------------------------------- YES -------------------------------------*/
305 static int lsm303m_SetPowerMode(struct i2c_client *client, bool enable)
306 {
307 u8 databuf[2];
308 int res = 0;
309 u8 addr = LSM303M_REG_POWER_CTL;
310 struct lsm303m_i2c_data *obj = i2c_get_clientdata(client);
311
312 MSE_FUN();
313 if(enable == sensor_power)
314 {
315 MSE_LOG("Sensor power status is newest!\n");
316 return LSM303M_SUCCESS;
317 }
318
319
320 if(enable == TRUE)
321 {
322 databuf[0] = LSM303M_MAG_POWER_ON;
323 }
324 else
325 {
326 databuf[0] = LSM303M_MAG_POWER_OFF;
327 }
328 databuf[1] = databuf[0];
329 databuf[0] = LSM303M_REG_POWER_CTL;
330
331
332 res = i2c_master_send(client, databuf, 0x2);
333
334 if(res <= 0)
335 {
336 MSE_LOG("set power mode failed!\n");
337 return LSM303M_ERR_I2C;
338 }
339 else if(atomic_read(&obj->trace) & ADX_TRC_INFO)
340 {
341 MSE_LOG("set power mode ok %d!\n", databuf[1]);
342 }
343
344 sensor_power = enable;
345 return 0;
346 }
347
348
349 /*----------------------------------- ok -----------------------------------------*/
350 static int lsm303m_SetBWRate(struct i2c_client *client, u8 bwrate)
351 {
352 u8 databuf[10];
353 int res = 0;
354
355 MSE_FUN();
356
357 memset(databuf, 0, sizeof(u8)*10);
358
359 bwrate = (ODR_MAG_MASK & bwrate);
360
361
362 databuf[0] = LSM303M_REG_BW_RATE;
363 databuf[1] = bwrate;
364
365 res = i2c_master_send(client, databuf, 0x2);
366
367 if(res <= 0)
368 {
369 return LSM303M_ERR_I2C;
370 }
371
372 return LSM303M_SUCCESS;
373 }
374
375
376 /*---------------------------------- done ------------------------------------------*/
377 static int lsm303m_SetDataResolution(struct lsm303m_i2c_data *obj, u8 new_fs_range)
378 {
379 MSE_FUN();
380
381 int err;
382 u8 dat, reso;
383
384 switch (new_fs_range) {
385 case LSM303M_MAG_FS_2G:
386 obj->reso.sensitivity = SENSITIVITY_MAG_2G;
387 break;
388 case LSM303M_MAG_FS_4G:
389 obj->reso.sensitivity = SENSITIVITY_MAG_4G;
390 break;
391 case LSM303M_MAG_FS_8G:
392 obj->reso.sensitivity = SENSITIVITY_MAG_8G;
393 break;
394 case LSM303M_MAG_FS_12G:
395 obj->reso.sensitivity = SENSITIVITY_MAG_12G;
396 break;
397 default:
398 obj->reso.sensitivity = SENSITIVITY_MAG_4G;
399 MSE_LOG("invalid magnetometer fs range requested: %u\n", new_fs_range);
400 return -1;
401 }
402
403 return 0;
404
405
406 }
407
408
409 /*--------------------------------- ok -------------------------------------------*/
410 static int lsm303m_SetDataFormat(struct i2c_client *client, u8 dataformat)
411 {
412 struct lsm303m_i2c_data *obj = i2c_get_clientdata(client);
413 u8 databuf[10];
414 int res = 0;
415
416 MSE_FUN();
417
418 memset(databuf, 0, sizeof(u8)*10);
419
420 dataformat = ((LSM303M_MAG_FS_MASK & dataformat));
421
422
423 databuf[0] = LSM303M_REG_DATA_FORMAT;
424 databuf[1] = dataformat;
425
426 res = i2c_master_send(client, databuf, 0x2);
427
428 if(res <= 0)
429 {
430 return LSM303M_ERR_I2C;
431 }
432
433 return lsm303m_SetDataResolution(obj, dataformat);
434
435 }
436
437
438
439
440
441 /*----------------------------------------------------------------------------*/
442 static int lsm303m_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
443 {
444 u8 databuf[10];
445
446 memset(databuf, 0, sizeof(u8)*10);
447
448 if((NULL == buf)||(bufsize<=30))
449 {
450 return -1;
451 }
452
453 if(NULL == client)
454 {
455 *buf = 0;
456 return -2;
457 }
458
459 sprintf(buf, "LSM303M Chip");
460 return 0;
461 }
462
463
464 static int lsm303m_init_client(struct i2c_client *client, int reset_cali)
465 {
466 struct lsm303m_i2c_data *obj = i2c_get_clientdata(client);
467 int res = 0;
468
469 MSE_FUN();
470
471 // 1 check ID ok
472 res = lsm303m_CheckDeviceID(client);
473 if(res != LSM303M_SUCCESS)
474 {
475 MSE_ERR("Check ID error\n");
476 return res;
477 }
478
479 // 2 POWER MODE no
480 res = lsm303m_SetPowerMode(client, false);
481 if(res != LSM303M_SUCCESS)
482 {
483 MSE_ERR("set power error\n");
484 return res;
485 }
486
487 // 3 RATE YES
488 res = lsm303m_SetBWRate(client, LSM303M_MAG_ODR50);
489 if(res != LSM303M_SUCCESS ) //0x2C->BW=100Hz
490 {
491 MSE_ERR("set power error\n");
492 return res;
493 }
494
495 // 4 RANGE ok
496 res = lsm303m_SetDataFormat(client, LSM303M_MAG_FS_4G);
497 if(res != LSM303M_SUCCESS) //0x2C->BW=100Hz
498 {
499 MSE_ERR("set data format error\n");
500 return res;
501 }
502
503
504
505 #ifdef CONFIG_LSM303M_LOWPASS
506 memset(&obj->fir, 0x00, sizeof(obj->fir));
507 #endif
508
509 return LSM303M_SUCCESS;
510 }
511
512 /*----------------------------------------------------------------------------*/
513 static int lsm303m_ReadData(struct i2c_client *client, s32 data[LSM303M_AXES_NUM])
514 {
515 struct lsm303m_i2c_data *priv = i2c_get_clientdata(client);
516 u8 addr = LSM303M_REG_DATAX0 | I2C_AUTO_INCREMENT;
517 u8 buf[LSM303M_DATA_LEN] = {0};
518 int err = 0;
519
520 if(NULL == client)
521 {
522 err = -EINVAL;
523 }
524 else if((err = hwmsen_read_block(client, addr, buf, 0x06)))
525 {
526 MSE_ERR("error: %d\n", err);
527 }
528 else
529 {
530 data[LSM303M_AXIS_X] = ((s32)( (s16)((buf[LSM303M_AXIS_X*2+1] << 8) | (buf[LSM303M_AXIS_X*2]))));
531 data[LSM303M_AXIS_Y] = ((s32)( (s16)((buf[LSM303M_AXIS_Y*2+1] << 8) | (buf[LSM303M_AXIS_Y*2]))));
532 data[LSM303M_AXIS_Z] = ((s32)( (s16)((buf[LSM303M_AXIS_Z*2+1] << 8) | (buf[LSM303M_AXIS_Z*2]))));
533
534 //MSE_LOG("lsm303m_ReadData [%08X %08X %08X] => [%5d %5d %5d]\n", data[LSM303M_AXIS_X], data[LSM303M_AXIS_Y], data[LSM303M_AXIS_Z],
535 // data[LSM303M_AXIS_X], data[LSM303M_AXIS_Y], data[LSM303M_AXIS_Z]);
536 if(atomic_read(&priv->trace) & ADX_TRC_RAWDATA)
537 {
538 MSE_LOG("[%08X %08X %08X] => [%5d %5d %5d]\n", data[LSM303M_AXIS_X], data[LSM303M_AXIS_Y], data[LSM303M_AXIS_Z],
539 data[LSM303M_AXIS_X], data[LSM303M_AXIS_Y], data[LSM303M_AXIS_Z]);
540 }
541
542 }
543 return err;
544 }
545
546
547 /*----------------------------------------------------------------------------*/
548 static int lsm303m_ReadRawData(struct i2c_client *client, char *buf)
549 {
550 struct lsm303m_i2c_data *obj = (struct lsm303m_i2c_data*)i2c_get_clientdata(client);
551 int res = 0;
552
553 if (!buf || !client)
554 {
555 return EINVAL;
556 }
557
558 if((res = lsm303m_ReadData(client, obj->data)))
559 {
560 MSE_ERR("I2C error: ret value=%d", res);
561 return EIO;
562 }
563 else
564 {
565 sprintf(buf, "%04x %04x %04x", obj->data[LSM303M_AXIS_X],
566 obj->data[LSM303M_AXIS_Y], obj->data[LSM303M_AXIS_Z]);
567
568 }
569
570 return 0;
571 }
572
573
574 /*----------------------------------------------------------------------------*/
575 static int lsm303m_ReadSensorData(struct i2c_client *client, char *buf, int bufsize)
576 {
577 int mag[LSM303M_DATA_LEN];
578 int res = 0;
579 struct lsm303m_i2c_data *obj = obj_i2c_data; //(struct lsm303m_i2c_data*)i2c_get_clientdata(client);
580 client = obj->client;
581 //u8 databuf[20];
582
583 //memset(databuf, 0, sizeof(u8)*10);
584
585 if(NULL == buf)
586 {
587 return -1;
588 }
589 if(NULL == client)
590 {
591 *buf = 0;
592 return -2;
593 }
594
595 if(sensor_power == FALSE)
596 {
597 res = lsm303m_SetPowerMode(client, true);
598 if(res)
599 {
600 MSE_ERR("Power on lsm303m error %d!\n", res);
601 }
602 msleep(20);
603 }
604
605 if((res = lsm303m_ReadData(client, obj->data)))
606 {
607 MSE_ERR("I2C error: ret value=%d", res);
608 return -3;
609 }
610 else
611 {
612
613 /*remap coordinate*/
614
615 mag[obj->cvt.map[LSM303M_AXIS_X]] = obj->cvt.sign[LSM303M_AXIS_X]*obj->data[LSM303M_AXIS_X] * obj->reso.sensitivity / 1000;
616 mag[obj->cvt.map[LSM303M_AXIS_Y]] = obj->cvt.sign[LSM303M_AXIS_Y]*obj->data[LSM303M_AXIS_Y] * obj->reso.sensitivity / 1000;
617 mag[obj->cvt.map[LSM303M_AXIS_Z]] = obj->cvt.sign[LSM303M_AXIS_Z]*obj->data[LSM303M_AXIS_Z] * obj->reso.sensitivity / 1000;
618
619 /**
620 mag[obj->cvt.map[LSM303M_AXIS_X]] = obj->cvt.sign[LSM303M_AXIS_X]*obj->data[LSM303M_AXIS_X];
621 mag[obj->cvt.map[LSM303M_AXIS_Y]] = obj->cvt.sign[LSM303M_AXIS_Y]*obj->data[LSM303M_AXIS_Y];
622 mag[obj->cvt.map[LSM303M_AXIS_Z]] = obj->cvt.sign[LSM303M_AXIS_Z]*obj->data[LSM303M_AXIS_Z];
623 **/
624
625 //MSE_LOG("Mapped msensor data: %d, %d, %d!\n", mag[LSM303M_AXIS_X], mag[LSM303M_AXIS_Y], mag[LSM303M_AXIS_Z]);
626
627 sprintf(buf, "%04x %04x %04x", mag[LSM303M_AXIS_X], mag[LSM303M_AXIS_Y], mag[LSM303M_AXIS_Z]);
628 if(atomic_read(&obj->trace) & ADX_TRC_IOCTL)
629 {
630 MSE_LOG("msensor data: %s!\n", buf);
631 }
632 }
633
634
635 //MSE_LOG("io read msensor data: %d, %d, %d!\n", mag[LSM303M_AXIS_X],mag[LSM303M_AXIS_Y],mag[LSM303M_AXIS_Z]);
636 return 0;
637 }
638
639
640
641 /*----------------------------------------------------------------------------*/
642 static int lsm303m_InitSelfTest(struct i2c_client *client)
643 {
644
645
646 return LSM303M_SUCCESS;
647 }
648 /*----------------------------------------------------------------------------*/
649 static int lsm303m_JudgeTestResult(struct i2c_client *client, s32 prv[LSM303M_AXES_NUM], s32 nxt[LSM303M_AXES_NUM])
650 {
651
652 int res = 0;
653 return res;
654 }
655 /*----------------------------------------------------------------------------*/
656 static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
657 {
658 struct i2c_client *client = lsm303m_i2c_client;
659 char strbuf[LSM303M_BUFSIZE];
660 if(NULL == client)
661 {
662 MSE_ERR("i2c client is null!!\n");
663 return 0;
664 }
665
666 lsm303m_ReadChipInfo(client, strbuf, LSM303M_BUFSIZE);
667 return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
668 }
669 /*----------------------------------------------------------------------------*/
670 static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
671 {
672 struct i2c_client *client = lsm303m_i2c_client;
673 char strbuf[LSM303M_BUFSIZE];
674
675 if(NULL == client)
676 {
677 MSE_ERR("i2c client is null!!\n");
678 return 0;
679 }
680 lsm303m_ReadSensorData(client, strbuf, LSM303M_BUFSIZE);
681 return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
682 }
683
684
685 /*--------------------------------- NO -------------------------------------------*/
686 static ssize_t show_reginfo_value(struct device_driver *ddri, char *buffer)
687 {
688
689 struct i2c_client *client = lsm303m_i2c_client;
690
691 struct lsm303m_i2c_data *priv = i2c_get_clientdata(client);
692 u8 addr = LSM303M_REG_CTL0 | I2C_AUTO_INCREMENT;
693 u8 buf[8] = {0};
694 int err = 0;
695 ssize_t len = 0;
696
697
698 if(NULL == client)
699 {
700 err = -EINVAL;
701 }
702 else if((err = hwmsen_read_block(client, addr, buf, 0x08)))
703 {
704 MSE_ERR("error: %d\n", err);
705 }
706
707 len += snprintf(buffer+len, PAGE_SIZE, "0x%04X , \t 0x%04X , \t 0x%04X, \t0x%04X , \n 0x%04X , \t 0x%04X, \t0x%04X, \t 0x%04X , \t \n ",
708 buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7]);
709
710
711 return len;
712 }
713 /*-------------------------------- NO --------------------------------------------*/
714 static ssize_t store_selftest_value(struct device_driver *ddri, const char *buf, size_t count)
715 {
716 struct lsm303m_i2c_data *obj = obj_i2c_data;
717 int tmp;
718
719 if(NULL == obj)
720 {
721 MSE_ERR("i2c data obj is null!!\n");
722 return 0;
723 }
724
725
726 if(1 == sscanf(buf, "%d", &tmp))
727 {
728 if(atomic_read(&obj->selftest) && !tmp)
729 {
730 /*enable -> disable*/
731 lsm303m_init_client(obj->client, 0);
732 }
733 else if(!atomic_read(&obj->selftest) && tmp)
734 {
735 /*disable -> enable*/
736 lsm303m_InitSelfTest(obj->client);
737 }
738
739 MSE_LOG("selftest: %d => %d\n", atomic_read(&obj->selftest), tmp);
740 atomic_set(&obj->selftest, tmp);
741 }
742 else
743 {
744 MSE_ERR("invalid content: '%s', length = %d\n", buf, count);
745 }
746 return count;
747 }
748
749 /*----------------------------------------------------------------------------*/
750 static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
751 {
752 ssize_t res;
753 struct lsm303m_i2c_data *obj = obj_i2c_data;
754 if (obj == NULL)
755 {
756 MSE_ERR("i2c_data obj is null!!\n");
757 return 0;
758 }
759
760 res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));
761 return res;
762 }
763 /*----------------------------------------------------------------------------*/
764 static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
765 {
766 struct lsm303m_i2c_data *obj = obj_i2c_data;
767 int trace;
768 if (obj == NULL)
769 {
770 MSE_ERR("i2c_data obj is null!!\n");
771 return 0;
772 }
773
774 if(1 == sscanf(buf, "0x%x", &trace))
775 {
776 atomic_set(&obj->trace, trace);
777 }
778 else
779 {
780 MSE_ERR("invalid content: '%s', length = %d\n", buf, count);
781 }
782
783 return count;
784 }
785 /*----------------------------------------------------------------------------*/
786 static ssize_t show_status_value(struct device_driver *ddri, char *buf)
787 {
788 ssize_t len = 0;
789 struct lsm303m_i2c_data *obj = obj_i2c_data;
790 if (obj == NULL)
791 {
792 MSE_ERR("i2c_data obj is null!!\n");
793 return 0;
794 }
795
796 if(obj->hw)
797 {
798 len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n",
799 obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);
800 }
801 else
802 {
803 len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
804 }
805 return len;
806 }
807
808 static ssize_t show_power_status_value(struct device_driver *ddri, char *buf)
809 {
810 int relv = 0;
811 if(sensor_power)
812 relv = snprintf(buf, PAGE_SIZE, "1\n");
813 else
814 relv = snprintf(buf, PAGE_SIZE, "0\n");
815
816 return relv;
817 }
818
819
820 static ssize_t store_power_status_value(struct device_driver *ddri, const char *buf, size_t count)
821 {
822 int mode = 0;
823 int res = 0;
824 sscanf(buf, "%d", &mode);
825 res = lsm303m_SetPowerMode(lsm303m_i2c_client, mode);
826 return count;
827 }
828
829
830
831 /*----------------------------------------------------------------------------*/
832 static ssize_t show_daemon_name(struct device_driver *ddri, char *buf)
833 {
834 char strbuf[LSM303M_BUFSIZE];
835 sprintf(strbuf, "lsm303md");
836 return sprintf(buf, "%s", strbuf);
837 }
838 /*----------------------------------------------------------------------------*/
839
840 /*----------------------------------------------------------------------------*/
841 static ssize_t show_debug_value(struct device_driver *ddri, char *buf)
842 {
843 ssize_t res;
844 struct lsm303m_i2c_data *obj = i2c_get_clientdata(lsm303m_i2c_client);
845 if(NULL == obj)
846 {
847 MSE_ERR("lsm303m_i2c_data is null!!\n");
848 return 0;
849 }
850
851 res = snprintf(buf, PAGE_SIZE, "0x%04X\n", lsm303mmid_data.controldata[8]);
852 return res;
853 }
854
855 static ssize_t store_debug_value(struct device_driver *ddri, const char *buf, size_t count)
856
857 {
858 struct lsm303m_i2c_data *obj = i2c_get_clientdata(lsm303m_i2c_client);
859 int trace;
860 if(NULL == obj)
861 {
862 MSE_ERR("lsm303m_i2c_data is null!!\n");
863 return 0;
864 }
865
866 if(1 == sscanf(buf, "0x%x", &trace))
867 {
868 //atomic_set(&obj->trace, trace);
869 lsm303mmid_data.controldata[8] = trace;
870 }
871 else
872 {
873 MSE_ERR("invalid content: '%s', length = %d\n", buf, count);
874 }
875
876 return count;
877 }
878
879 /*----------------------------------------------------------------------------*/
880 static ssize_t show_layout_value(struct device_driver *ddri, char *buf)
881 {
882 struct i2c_client *client = lsm303m_i2c_client;
883 struct lsm303m_i2c_data *data = i2c_get_clientdata(client);
884
885 return sprintf(buf, "(%d, %d)\n[%+2d %+2d %+2d]\n[%+2d %+2d %+2d]\n",
886 data->hw->direction,atomic_read(&data->layout), data->cvt.sign[0], data->cvt.sign[1],
887 data->cvt.sign[2],data->cvt.map[0], data->cvt.map[1], data->cvt.map[2]);
888 }
889
890 /*----------------------------------------------------------------------------*/
891 static ssize_t store_layout_value(struct device_driver *ddri, const char *buf, size_t count)
892 {
893 struct i2c_client *client = lsm303m_i2c_client;
894 struct lsm303m_i2c_data *data = i2c_get_clientdata(client);
895 int layout = 0;
896
897 if(1 == sscanf(buf, "%d", &layout))
898 {
899 atomic_set(&data->layout, layout);
900 if(!hwmsen_get_convert(layout, &data->cvt))
901 {
902 MSE_ERR("HWMSEN_GET_CONVERT function ok!\r\n");
903 data->hw->direction = layout;
904 }
905 else if(!hwmsen_get_convert(data->hw->direction, &data->cvt))
906 {
907 MSE_ERR("invalid layout: %d, restore to %d, no changed \n", layout, data->hw->direction);
908
909 }
910 else
911 {
912 MSE_ERR("invalid layout: (%d, %d)\n", layout, data->hw->direction);
913 hwmsen_get_convert(0, &data->cvt);
914 data->hw->direction = 0;
915 }
916 }
917 else
918 {
919 MSE_ERR("invalid format = '%s'\n", buf);
920 }
921
922 return count;
923 }
924
925
926
927 /*----------------------------------------------------------------------------*/
928 static DRIVER_ATTR(daemon, S_IRUGO, show_daemon_name, NULL);
929 static DRIVER_ATTR(chipinfo, S_IRUGO, show_chipinfo_value, NULL);
930 static DRIVER_ATTR(sensordata, S_IRUGO, show_sensordata_value, NULL);
931 static DRIVER_ATTR(reginfo, S_IRUGO, show_reginfo_value, NULL);
932 static DRIVER_ATTR(debugon, S_IWUSR | S_IRUGO, show_debug_value, store_debug_value);
933 static DRIVER_ATTR(trace, S_IWUSR | S_IRUGO, show_trace_value, store_trace_value);
934 static DRIVER_ATTR(status, S_IRUGO, show_status_value, NULL);
935 static DRIVER_ATTR(powerstatus, S_IWUSR | S_IRUGO, show_power_status_value, store_power_status_value);
936 static DRIVER_ATTR(layout, S_IRUGO | S_IWUSR, show_layout_value, store_layout_value );
937
938
939 /*----------------------------------------------------------------------------*/
940 static struct driver_attribute *lsm303m_attr_list[] = {
941 &driver_attr_daemon,
942 &driver_attr_chipinfo, /*chip information*/
943 &driver_attr_sensordata, /*dump sensor data*/
944 &driver_attr_reginfo, /*self test demo*/
945 &driver_attr_debugon, /*self test demo*/
946 &driver_attr_trace, /*trace log*/
947 &driver_attr_status,
948 &driver_attr_powerstatus,
949 &driver_attr_layout,
950 };
951
952
953 /*----------------------------------------------------------------------------*/
954 static int lsm303m_create_attr(struct device_driver *driver)
955 {
956 int idx, err = 0;
957 int num = (int)(sizeof(lsm303m_attr_list)/sizeof(lsm303m_attr_list[0]));
958 if (driver == NULL)
959 {
960 return -EINVAL;
961 }
962
963 for(idx = 0; idx < num; idx++)
964 {
965 if((err = driver_create_file(driver, lsm303m_attr_list[idx])))
966 {
967 MSE_ERR("driver_create_file (%s) = %d\n", lsm303m_attr_list[idx]->attr.name, err);
968 break;
969 }
970 }
971 return err;
972 }
973 /*----------------------------------------------------------------------------*/
974 static int lsm303m_delete_attr(struct device_driver *driver)
975 {
976 int idx ,err = 0;
977 int num = (int)(sizeof(lsm303m_attr_list)/sizeof(lsm303m_attr_list[0]));
978
979 if(driver == NULL)
980 {
981 return -EINVAL;
982 }
983
984
985 for(idx = 0; idx < num; idx++)
986 {
987 driver_remove_file(driver, lsm303m_attr_list[idx]);
988 }
989
990
991 return err;
992 }
993
994
995 /*----------------------------------------------------------------------------*/
996 static int lsm303m_ReadCaliData(char *buf, int bufsize)
997 {
998 if((!buf)||(bufsize<=80))
999 {
1000 return -1;
1001 }
1002
1003 MSE_FUN(f);
1004
1005 read_lock(&lsm303mmid_data.datalock);
1006 sprintf(buf, "%d %d %d %d %d %d %d", lsm303mmid_data.nmx, lsm303mmid_data.nmy,
1007 lsm303mmid_data.nmz,lsm303mmid_data.nax,lsm303mmid_data.nay,lsm303mmid_data.naz,lsm303mmid_data.mag_status);
1008 read_unlock(&lsm303mmid_data.datalock);
1009 return 0;
1010 }
1011
1012
1013 /*----------------------------------------------------------------------------*/
1014 static int lsm303m_ReadPostureData(char *buf, int bufsize)
1015 {
1016 MSE_FUN(f);
1017
1018 if((!buf)||(bufsize<=80))
1019 {
1020 return -1;
1021 }
1022
1023 read_lock(&lsm303mmid_data.datalock);
1024 sprintf(buf, "%d %d %d %d", lsm303mmid_data.yaw, lsm303mmid_data.pitch,
1025 lsm303mmid_data.roll, lsm303mmid_data.mag_status);
1026 read_unlock(&lsm303mmid_data.datalock);
1027 return 0;
1028 }
1029
1030
1031
1032 /*----------------------------------------------------------------------------*/
1033 int lsm303m_operate(void* self, uint32_t command, void* buff_in, int size_in,
1034 void* buff_out, int size_out, int* actualout)
1035 {
1036 int err = 0;
1037 int value, sample_delay, status, delay_time;
1038 hwm_sensor_data* msensor_data;
1039 struct lsm303m_i2c_data *priv = (struct lsm303m_i2c_data*)self;
1040 char buff[LSM303M_BUFSIZE];
1041
1042
1043 //MSE_FUN(f);
1044
1045 //MSE_FUN(f);
1046 switch (command)
1047 {
1048 case SENSOR_DELAY:
1049
1050
1051 if((buff_in == NULL) || (size_in < sizeof(int)))
1052 {
1053 MSE_ERR("Set delay parameter error!\n");
1054 err = -EINVAL;
1055 }
1056 else
1057 {
1058 value = *(int *)buff_in;
1059 MSE_LOG("Set delay parameter %d!\n", value);
1060 if(value <= 5)
1061 {
1062 sample_delay = LSM303M_MAG_ODR50;
1063 }
1064 else if(value <= 10)
1065 {
1066 sample_delay = LSM303M_MAG_ODR50;
1067 }
1068 else
1069 {
1070 sample_delay = LSM303M_MAG_ODR50;
1071 }
1072
1073 err = lsm303m_SetBWRate(priv->client, sample_delay);
1074 if(err != LSM303M_SUCCESS ) //0x2C->BW=100Hz
1075 {
1076 MSE_ERR("Set delay parameter error!\n");
1077 }
1078
1079 value = *(int *)buff_in;
1080 if(value <= 20)
1081 {
1082 value = 20;
1083 }
1084
1085 lsm303mmid_data.controldata[0] = value; // Loop Delay
1086 }
1087 break;
1088
1089 case SENSOR_ENABLE:
1090 //MSE_LOG("***lsm303m SENSOR_ENABLE****");
1091 if((buff_in == NULL) || (size_in < sizeof(int)))
1092 {
1093 MSE_ERR("Enable sensor parameter error!\n");
1094 err = -EINVAL;
1095 }
1096 else
1097 {
1098 value = *(int *)buff_in;
1099 read_lock(&lsm303mmid_data.ctrllock);
1100 if(value == 1)
1101 {
1102 MSE_LOG("lsm303m SENSOR_ENABLE");
1103 lsm303m_SetPowerMode( priv->client, 1);
1104 lsm303mmid_data.controldata[7] |= SENSOR_MAGNETIC;
1105 atomic_set(&m_flag, 1);
1106 atomic_set(&open_flag, 1);
1107 }
1108 else
1109 {
1110 lsm303m_SetPowerMode( priv->client, 0);
1111 lsm303mmid_data.controldata[7] &= ~SENSOR_MAGNETIC;
1112 atomic_set(&m_flag, 0);
1113 if(atomic_read(&o_flag) == 0)
1114 {
1115 atomic_set(&open_flag, 0);
1116 }
1117 }
1118 wake_up(&open_wq);
1119 read_unlock(&lsm303mmid_data.ctrllock);
1120 // TODO: turn device into standby or normal mode
1121 }
1122 break;
1123
1124 case SENSOR_GET_DATA:
1125 //MSE_LOG("++++++++++++++++++++++++MSENSOR_GET_DATA");
1126 //MSE_LOG("***lsm303m SENSOR_GET_DATA****");
1127 if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
1128 {
1129 MSE_ERR("get sensor data parameter error!\n");
1130 err = -EINVAL;
1131 }
1132 else
1133 {
1134
1135 msensor_data = (hwm_sensor_data *)buff_out;
1136 read_lock(&lsm303mmid_data.datalock);
1137 msensor_data->values[0] = lsm303mmid_data.nmx;
1138 msensor_data->values[1] = lsm303mmid_data.nmy;
1139 msensor_data->values[2] = lsm303mmid_data.nmz;
1140 msensor_data->status = lsm303mmid_data.mag_status;
1141
1142 read_unlock(&lsm303mmid_data.datalock);
1143
1144
1145 msensor_data->values[0] = msensor_data->values[0] / 10;
1146 msensor_data->values[1] = msensor_data->values[1] / 10;
1147 msensor_data->values[2] = msensor_data->values[2] / 10;
1148 msensor_data->value_divide = 1;
1149
1150 switch (status)
1151 {
1152 case 1: case 2:
1153 msensor_data->status = SENSOR_STATUS_ACCURACY_HIGH;
1154 break;
1155 case 3:
1156 msensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;
1157 break;
1158 case 4:
1159 msensor_data->status = SENSOR_STATUS_ACCURACY_LOW;
1160 break;
1161 default:
1162 msensor_data->status = SENSOR_STATUS_UNRELIABLE;
1163 break;
1164 }
1165
1166 msensor_data->status = SENSOR_STATUS_ACCURACY_HIGH;
1167
1168 //MSE_LOG("get msensor data: %d, %d, %d, %d!\n", msensor_data->values[0],msensor_data->values[1], msensor_data->values[2], msensor_data->status);
1169
1170 }
1171 break;
1172 default:
1173 MSE_ERR("msensor operate function no this parameter %d!\n", command);
1174 err = -1;
1175 break;
1176 }
1177
1178 return err;
1179 }
1180
1181
1182
1183 /*----------------------------------------------------------------------------*/
1184 int lsm303m_orientation_operate(void* self, uint32_t command, void* buff_in, int size_in,
1185 void* buff_out, int size_out, int* actualout)
1186 {
1187 int err = 0;
1188 int value, sample_delay, status=0;
1189 hwm_sensor_data* osensor_data=NULL;
1190 struct lsm303m_i2c_data *priv = (struct lsm303m_i2c_data*)self;
1191
1192
1193 MSE_FUN(f);
1194 switch (command)
1195 {
1196 case SENSOR_DELAY:
1197
1198 MSE_LOG("***orientation SENSOR_DELAY****");
1199 if((buff_in == NULL) || (size_in < sizeof(int)))
1200 {
1201 MSE_ERR("Set delay parameter error!\n");
1202 err = -EINVAL;
1203 }
1204 else
1205 {
1206 value = *(int *)buff_in;
1207 if(value <= 20)
1208 {
1209 sample_delay = 20;
1210 }
1211
1212
1213 lsm303mmid_data.controldata[0] = sample_delay; // Loop Delay
1214 }
1215 break;
1216
1217 case SENSOR_ENABLE:
1218 MSE_LOG("***orientation SENSOR_ENABLE****");
1219 if((buff_in == NULL) || (size_in < sizeof(int)))
1220 {
1221 MSE_ERR("Enable sensor parameter error!\n");
1222 err = -EINVAL;
1223 }
1224 else
1225 {
1226 value = *(int *)buff_in;
1227 read_lock(&lsm303mmid_data.ctrllock);
1228 if(value == 1)
1229 {
1230 lsm303m_SetPowerMode( priv->client, 1);
1231 lsm303mmid_data.controldata[7] |= SENSOR_ORIENTATION;
1232 atomic_set(&o_flag, 1);
1233 atomic_set(&open_flag, 1);
1234 }
1235 else
1236 {
1237 lsm303m_SetPowerMode( priv->client, 0);
1238 lsm303mmid_data.controldata[7] &= ~SENSOR_ORIENTATION;
1239 atomic_set(&o_flag, 0);
1240 if(atomic_read(&m_flag) == 0)
1241 {
1242 atomic_set(&open_flag, 0);
1243 }
1244 }
1245 wake_up(&open_wq);
1246 read_unlock(&lsm303mmid_data.ctrllock);
1247 // Do nothing
1248 }
1249 break;
1250
1251 case SENSOR_GET_DATA:
1252 //MSE_LOG("+++++++++++MSENSOR_GET_ORIENTATION_DATA");
1253 MSE_LOG("************OSENSOR_GET_DATA***********\r\n");
1254 if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
1255 {
1256 MSE_ERR("get sensor data parameter error!\n");
1257 err = -EINVAL;
1258 }
1259 else
1260 {
1261 osensor_data = (hwm_sensor_data *)buff_out;
1262 read_lock(&lsm303mmid_data.datalock);
1263 osensor_data->values[0] = lsm303mmid_data.yaw;
1264 osensor_data->values[1] = lsm303mmid_data.pitch;
1265 osensor_data->values[2] = lsm303mmid_data.roll;
1266 status = lsm303mmid_data.mag_status;
1267 read_unlock(&lsm303mmid_data.datalock);
1268
1269 osensor_data->value_divide = 1;
1270
1271
1272 //MSE_LOG(" get osensor data: %d, %d, %d, %d!\n", osensor_data->values[0],osensor_data->values[1], osensor_data->values[2], osensor_data->status);
1273 }
1274
1275 switch (status)
1276 {
1277 case 1: case 2:
1278 osensor_data->status = SENSOR_STATUS_ACCURACY_HIGH;
1279 break;
1280 case 3:
1281 osensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;
1282 break;
1283 case 4:
1284 osensor_data->status = SENSOR_STATUS_ACCURACY_LOW;
1285 break;
1286 default:
1287 osensor_data->status = SENSOR_STATUS_UNRELIABLE;
1288 break;
1289 }
1290 break;
1291 default:
1292 MSE_ERR("msensor operate function no this parameter %d!\n", command);
1293 err = -1;
1294 break;
1295 }
1296
1297 return err;
1298 }
1299
1300
1301
1302 /******************************************************************************
1303 * Function Configuration
1304 ******************************************************************************/
1305 static int lsm303m_open(struct inode *inode, struct file *file)
1306 {
1307 file->private_data = lsm303m_i2c_client;
1308 atomic_inc(&dev_open_count);
1309
1310 if(file->private_data == NULL)
1311 {
1312 MSE_ERR("null pointer!!\n");
1313 return -EINVAL;
1314 }
1315 return nonseekable_open(inode, file);
1316 }
1317
1318
1319 /*----------------------------------------------------------------------------*/
1320 static int lsm303m_release(struct inode *inode, struct file *file)
1321 {
1322 struct lsm303m_i2c_data *obj = i2c_get_clientdata(lsm303m_i2c_client);
1323 atomic_dec(&dev_open_count);
1324 file->private_data = NULL;
1325 if(atomic_read(&obj->trace) & ADX_TRC_INFO)
1326 {
1327 MSE_LOG("Release device node:ami304\n");
1328 }
1329 return 0;
1330 }
1331
1332
1333 static int lsm303m_GetOpenStatus(void)
1334 {
1335 wait_event_interruptible(open_wq, (atomic_read(&open_flag) != 0));
1336 return atomic_read(&open_flag);
1337 }
1338
1339
1340
1341 /*----------------------------------------------------------------------------*/
1342 //static int lsm303m_ioctl(struct inode *inode, struct file *file, unsigned int cmd,unsigned long arg)//modified here
1343 static long lsm303m_unlocked_ioctl(struct file *file, unsigned int cmd,
1344 unsigned long arg)
1345 {
1346 void __user *argp = (void __user *)arg;
1347 int valuebuf[4];
1348 int YPRdata[4];
1349 int controlbuf[10];
1350 char strbuf[LSM303M_BUFSIZE];
1351 void __user *data;
1352 long retval=0;
1353 int mode=0;
1354 hwm_sensor_data* osensor_data;
1355 uint32_t enable;
1356 char buff[512];
1357 int status; /* for OPEN/CLOSE_STATUS */
1358 short sensor_status; /* for Orientation and Msensor status */
1359 struct lsm303m_i2c_data *priv = obj_i2c_data;
1360
1361 // MSE_FUN(f);
1362
1363 // MSE_FUN(f);
1364
1365
1366 switch (cmd)
1367 {
1368 case MSENSOR_IOCTL_INIT:
1369 MSE_LOG("===========IOCTL_INIT=======\r\n");
1370 read_lock(&lsm303m_data.lock);
1371 mode = lsm303m_data.mode;
1372 read_unlock(&lsm303m_data.lock);
1373 lsm303m_init_client(lsm303m_i2c_client, 0);
1374 break;
1375
1376 case ECOMPASS_IOC_GET_OFLAG:
1377 sensor_status = atomic_read(&o_flag);
1378 if(copy_to_user(argp, &sensor_status, sizeof(sensor_status)))
1379 {
1380 MSE_ERR("copy_to_user failed.");
1381 return -EFAULT;
1382 }
1383 break;
1384
1385 case ECOMPASS_IOC_GET_MFLAG:
1386 sensor_status = atomic_read(&m_flag);
1387 if(copy_to_user(argp, &sensor_status, sizeof(sensor_status)))
1388 {
1389 MSE_ERR("copy_to_user failed.");
1390 return -EFAULT;
1391 }
1392 break;
1393
1394 case ECOMPASS_IOC_GET_OPEN_STATUS:
1395 MSE_LOG("===========GET__OPEN_STATU=======\r\n");
1396 status = lsm303m_GetOpenStatus();
1397 if(copy_to_user(argp, &status, sizeof(status)))
1398 {
1399 MSE_LOG("copy_to_user failed.");
1400 return -EFAULT;
1401 }
1402 MSE_LOG("===========GET__OPEN_STATU DONE=======\r\n");
1403 break;
1404
1405 case MSENSOR_IOCTL_SET_POSTURE:
1406 //MSE_LOG("===========SET_POSTURE=======\r\n");
1407 data = (void __user *) arg;
1408 if(data == NULL)
1409 {
1410 MSE_ERR("IO parameter pointer is NULL!\r\n");
1411 break;
1412 }
1413
1414 if(copy_from_user(&valuebuf, data, sizeof(valuebuf)))
1415 {
1416 retval = -EFAULT;
1417 goto err_out;
1418 }
1419
1420 write_lock(&lsm303mmid_data.datalock);
1421 lsm303mmid_data.yaw = valuebuf[0];
1422 lsm303mmid_data.pitch = valuebuf[1];
1423 lsm303mmid_data.roll = valuebuf[2];
1424 lsm303mmid_data.mag_status = valuebuf[3];
1425 write_unlock(&lsm303mmid_data.datalock);
1426
1427
1428 //MSE_LOG("SET_POSTURE osensor data: %d, %d, %d!\n", lsm303mmid_data.yaw ,lsm303mmid_data.pitch ,lsm303mmid_data.pitch);
1429 break;
1430
1431
1432 case MSENSOR_IOCTL_SET_CALIDATA:
1433 //MSE_LOG("===========IOCTL_SET_YPRdata=======\r\n");
1434 data = (void __user *) arg;
1435 if (data == NULL)
1436 {
1437 MSE_ERR("IO parameter pointer is NULL!\r\n");
1438 break;
1439 }
1440 if(copy_from_user(&YPRdata, data, sizeof(YPRdata)))
1441 {
1442 retval = -EFAULT;
1443 goto err_out;
1444 }
1445
1446 write_lock(&lsm303mmid_data.datalock);
1447 lsm303mmid_data.nmx = YPRdata[0];
1448 lsm303mmid_data.nmy = YPRdata[1];
1449 lsm303mmid_data.nmz = YPRdata[2];
1450 lsm303mmid_data.mag_status = YPRdata[3];
1451 write_unlock(&lsm303mmid_data.datalock);
1452 //MSE_LOG("IOCTL_SET_YPRdata msensor data: %d, %d, %d!\n", lsm303mmid_data.nmx ,lsm303mmid_data.nmy ,lsm303mmid_data.nmz);
1453 break;
1454
1455 case MSENSOR_IOCTL_READ_CHIPINFO:
1456 data = (void __user *) arg;
1457 if(data == NULL)
1458 {
1459 MSE_ERR("IO parameter pointer is NULL!\r\n");
1460 break;
1461 }
1462
1463 lsm303m_ReadChipInfo(lsm303m_i2c_client, strbuf, LSM303M_BUFSIZE);
1464 if(copy_to_user(data, strbuf, strlen(strbuf)+1))
1465 {
1466 retval = -EFAULT;
1467 goto err_out;
1468 }
1469 break;
1470
1471 case MSENSOR_IOCTL_SENSOR_ENABLE:
1472 //MSE_LOG("===========IOCTL_SENSOR_ENABLE=======\r\n");
1473
1474 data = (void __user *) arg;
1475 if (data == NULL)
1476 {
1477 MSE_ERR("IO parameter pointer is NULL!\r\n");
1478 break;
1479 }
1480 if(copy_from_user(&enable, data, sizeof(enable)))
1481 {
1482 MSE_ERR("copy_from_user failed.");
1483 return -EFAULT;
1484 }
1485 else
1486 {
1487 printk( "MSENSOR_IOCTL_SENSOR_ENABLE enable=%d!\r\n",enable);
1488 read_lock(&lsm303mmid_data.ctrllock);
1489 if(enable == 1)
1490 {
1491 lsm303m_SetPowerMode( priv->client, 1);
1492 lsm303mmid_data.controldata[7] |= SENSOR_ORIENTATION;
1493 atomic_set(&o_flag, 1);
1494 atomic_set(&open_flag, 1);
1495 }
1496 else
1497 {
1498 lsm303m_SetPowerMode( priv->client, 0);
1499 lsm303mmid_data.controldata[7] &= ~SENSOR_ORIENTATION;
1500 atomic_set(&o_flag, 0);
1501 if(atomic_read(&m_flag) == 0)
1502 {
1503 atomic_set(&open_flag, 0);
1504 }
1505 }
1506 wake_up(&open_wq);
1507 read_unlock(&lsm303mmid_data.ctrllock);
1508
1509 }
1510
1511 break;
1512
1513 case MSENSOR_IOCTL_READ_SENSORDATA:
1514 //MSE_LOG("===========IOCTL_READ_SENSORDATA=======\r\n");
1515 data = (void __user *) arg;
1516 if(data == NULL)
1517 {
1518 MSE_ERR("IO parameter pointer is NULL!\r\n");
1519 break;
1520 }
1521 lsm303m_ReadSensorData(lsm303m_i2c_client, strbuf, LSM303M_BUFSIZE);
1522 if(copy_to_user(data, strbuf, strlen(strbuf)+1))
1523 {
1524 retval = -EFAULT;
1525 goto err_out;
1526 }
1527
1528 break;
1529
1530 case MSENSOR_IOCTL_READ_FACTORY_SENSORDATA:
1531
1532 //MSE_LOG("===========IOCTL_READ_FACTORY_SENSORDATA=======\r\n");
1533 data = (void __user *) arg;
1534 if (data == NULL)
1535 {
1536 MSE_ERR("IO parameter pointer is NULL!\r\n");
1537 break;
1538 }
1539
1540 osensor_data = (hwm_sensor_data *)buff;
1541
1542 read_lock(&lsm303mmid_data.datalock);
1543 osensor_data->values[0] = lsm303mmid_data.yaw;
1544 osensor_data->values[1] = lsm303mmid_data.pitch;
1545 osensor_data->values[2] = lsm303mmid_data.roll;
1546 //status = lsm303mmid_data.mag_status;
1547 read_unlock(&lsm303mmid_data.datalock);
1548
1549 osensor_data->value_divide = 1;
1550
1551 switch (lsm303mmid_data.mag_status)
1552 {
1553 case 1: case 2:
1554 osensor_data->status = SENSOR_STATUS_ACCURACY_HIGH;
1555 break;
1556 case 3:
1557 osensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;
1558 break;
1559 case 4:
1560 osensor_data->status = SENSOR_STATUS_ACCURACY_LOW;
1561 break;
1562 default:
1563 osensor_data->status = SENSOR_STATUS_UNRELIABLE;
1564 break;
1565 }
1566
1567
1568 sprintf(buff, "%x %x %x %x %x", osensor_data->values[0], osensor_data->values[1],
1569 osensor_data->values[2],osensor_data->status,osensor_data->value_divide);
1570 if(copy_to_user(data, buff, strlen(buff)+1))
1571 {
1572 return -EFAULT;
1573 }
1574
1575 break;
1576
1577 case MSENSOR_IOCTL_READ_POSTUREDATA:
1578 //MSE_LOG("===========IOCTL_READ_READ_POSTUREDATA=======\r\n");
1579 data = (void __user *) arg;
1580 if(data == NULL)
1581 {
1582 MSE_ERR("IO parameter pointer is NULL!\r\n");
1583 break;
1584 }
1585
1586 lsm303m_ReadPostureData(strbuf, LSM303M_BUFSIZE);
1587 if(copy_to_user(data, strbuf, strlen(strbuf)+1))
1588 {
1589 retval = -EFAULT;
1590 goto err_out;
1591 }
1592 break;
1593
1594 case MSENSOR_IOCTL_READ_CALIDATA:
1595 //MSE_LOG("===========IOCTL_READ_READ_CALIDATA=======\r\n");
1596 data = (void __user *) arg;
1597 if(data == NULL)
1598 {
1599 break;
1600 }
1601 lsm303m_ReadCaliData(strbuf, LSM303M_BUFSIZE);
1602 if(copy_to_user(data, strbuf, strlen(strbuf)+1))
1603 {
1604 retval = -EFAULT;
1605 goto err_out;
1606 }
1607 break;
1608
1609 case MSENSOR_IOCTL_READ_CONTROL:
1610 read_lock(&lsm303mmid_data.ctrllock);
1611 memcpy(controlbuf, &lsm303mmid_data.controldata[0], sizeof(controlbuf));
1612 read_unlock(&lsm303mmid_data.ctrllock);
1613 data = (void __user *) arg;
1614 if(data == NULL)
1615 {
1616 break;
1617 }
1618 if(copy_to_user(data, controlbuf, sizeof(controlbuf)))
1619 {
1620 retval = -EFAULT;
1621 goto err_out;
1622 }
1623 break;
1624
1625 case MSENSOR_IOCTL_SET_CONTROL:
1626 //MSE_LOG("===========IOCTL_SET_CONTROL=======\r\n");
1627 data = (void __user *) arg;
1628 if(data == NULL)
1629 {
1630 break;
1631 }
1632 if(copy_from_user(controlbuf, data, sizeof(controlbuf)))
1633 {
1634 retval = -EFAULT;
1635 goto err_out;
1636 }
1637 write_lock(&lsm303mmid_data.ctrllock);
1638 memcpy(&lsm303mmid_data.controldata[0], controlbuf, sizeof(controlbuf));
1639 write_unlock(&lsm303mmid_data.ctrllock);
1640 break;
1641
1642 case MSENSOR_IOCTL_SET_MODE:
1643
1644 break;
1645
1646 default:
1647 MSE_ERR("%s not supported = 0x%04x", __FUNCTION__, cmd);
1648 retval = -ENOIOCTLCMD;
1649 break;
1650 }
1651
1652 err_out:
1653 //MSE_LOG("===========liangruo lsm303m ioctl done err = %d, =======\r\n", retval);
1654 return retval;
1655 }
1656
1657
1658
1659 /*----------------------------------------------------------------------------*/
1660 static struct file_operations lsm303m_fops = {
1661 // .owner = THIS_MODULE,
1662 .open = lsm303m_open,
1663 .release = lsm303m_release,
1664 .unlocked_ioctl = lsm303m_unlocked_ioctl,
1665 };
1666 /*----------------------------------------------------------------------------*/
1667 static struct miscdevice lsm303m_device = {
1668 .minor = MISC_DYNAMIC_MINOR,
1669 .name = "msensor",
1670 .fops = &lsm303m_fops,
1671 };
1672
1673
1674 /*----------------------------------------------------------------------------*/
1675 //#ifndef CONFIG_HAS_EARLYSUSPEND
1676 /*----------------------------------------------------------------------------*/
1677 static int lsm303m_suspend(struct i2c_client *client, pm_message_t msg)
1678 {
1679 struct lsm303m_i2c_data *obj = i2c_get_clientdata(client);
1680 int err = 0;
1681 MSE_FUN();
1682
1683 if(msg.event == PM_EVENT_SUSPEND)
1684 {
1685 if(obj == NULL)
1686 {
1687 MSE_ERR("null pointer!!\n");
1688 return -EINVAL;
1689 }
1690 atomic_set(&obj->suspend, 1);
1691 /**
1692 if((err = lsm303m_SetPowerMode(obj->client, false)))
1693 {
1694 MSE_ERR("write power control fail!!\n");
1695 return err;
1696 }
1697 ***/
1698 lsm303m_power(obj->hw, 0);
1699 MSE_LOG("lsm303m_suspend ok\n");
1700 }
1701 return err;
1702 }
1703 /*----------------------------------------------------------------------------*/
1704 static int lsm303m_resume(struct i2c_client *client)
1705 {
1706 struct lsm303m_i2c_data *obj = i2c_get_clientdata(client);
1707 int err;
1708 MSE_FUN();
1709
1710 if(obj == NULL)
1711 {
1712 MSE_ERR("null pointer!!\n");
1713 return -EINVAL;
1714 }
1715
1716 lsm303m_power(obj->hw, 1);
1717 /***
1718 if((err = lsm303m_SetPowerMode(obj->client, true)))
1719 {
1720 MSE_ERR("write power control fail!!\n");
1721 return err;
1722 }
1723
1724 ***/
1725 atomic_set(&obj->suspend, 0);
1726 MSE_LOG("lsm303m_resume ok\n");
1727
1728 return 0;
1729 }
1730 /*----------------------------------------------------------------------------*/
1731 //#else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
1732 /*----------------------------------------------------------------------------*/
1733 static void lsm303m_early_suspend(struct early_suspend *h)
1734 {
1735 struct lsm303m_i2c_data *obj = container_of(h, struct lsm303m_i2c_data, early_drv);
1736 int err;
1737 MSE_FUN();
1738
1739 if(obj == NULL)
1740 {
1741 MSE_ERR("null pointer!!\n");
1742 return;
1743 }
1744 atomic_set(&obj->suspend, 1);
1745 /***
1746 if((err = lsm303m_SetPowerMode(obj->client, false)))
1747 {
1748 MSE_ERR("write power control fail!!\n");
1749 return;
1750 }
1751 **/
1752
1753 sensor_power = false;
1754
1755 lsm303m_power(obj->hw, 0);
1756 }
1757 /*----------------------------------------------------------------------------*/
1758 static void lsm303m_late_resume(struct early_suspend *h)
1759 {
1760 struct lsm303m_i2c_data *obj = container_of(h, struct lsm303m_i2c_data, early_drv);
1761 int err;
1762 MSE_FUN();
1763
1764 if(obj == NULL)
1765 {
1766 MSE_ERR("null pointer!!\n");
1767 return;
1768 }
1769
1770 lsm303m_power(obj->hw, 1);
1771 /***
1772
1773 if((err = lsm303m_SetPowerMode(obj->client, true)))
1774 {
1775 MSE_ERR("write power control fail!!\n");
1776 return err;
1777 }
1778 ***/
1779 atomic_set(&obj->suspend, 0);
1780 }
1781
1782
1783
1784 /*----------------------------------------------------------------------------*/
1785 static int lsm303m_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
1786 {
1787 MSE_FUN();
1788
1789 struct i2c_client *new_client;
1790 struct lsm303m_i2c_data *data;
1791 int err = 0;
1792 struct hwmsen_object sobj_m, sobj_o;
1793
1794 MSE_FUN(f);
1795
1796 client->addr = 0x3C>>1;
1797
1798 if (!(data = kmalloc(sizeof(struct lsm303m_i2c_data), GFP_KERNEL)))
1799 {
1800 err = -ENOMEM;
1801 goto exit;
1802 }
1803 memset(data, 0, sizeof(struct lsm303m_i2c_data));
1804
1805 data->hw = get_cust_mag_hw();
1806 if((err = hwmsen_get_convert(data->hw->direction, &data->cvt)))
1807 {
1808 MSE_ERR("invalid direction: %d\n", data->hw->direction);
1809 goto exit;
1810 }
1811
1812 atomic_set(&data->layout, data->hw->direction);
1813 atomic_set(&data->trace, 0);
1814 init_waitqueue_head(&data_ready_wq);
1815 init_waitqueue_head(&open_wq);
1816
1817 data->client = client;
1818 new_client = data->client;
1819 i2c_set_clientdata(new_client, data);
1820
1821 lsm303m_i2c_client = new_client;
1822 obj_i2c_data = data;
1823
1824 if((err = lsm303m_init_client(new_client, 1)))
1825 {
1826 goto exit_init_failed;
1827 }
1828
1829 /* Register sysfs attribute */
1830 if((err = lsm303m_create_attr(&lsm303m_msensor_driver.driver)))
1831 {
1832 MSE_ERR("create attribute err = %d\n", err);
1833 goto exit_sysfs_create_group_failed;
1834 }
1835
1836
1837 if((err = misc_register(&lsm303m_device)))
1838 {
1839 MSE_ERR("lsm303m_device register failed\n");
1840 goto exit_misc_device_register_failed;
1841 }
1842
1843 sobj_m.self = data;
1844 sobj_m.polling = 1;
1845 sobj_m.sensor_operate = lsm303m_operate;
1846 if((err = hwmsen_attach(ID_MAGNETIC, &sobj_m)))
1847 {
1848 MSE_ERR("attach fail = %d\n", err);
1849 goto exit_kfree;
1850 }
1851
1852 sobj_o.self = data;
1853 sobj_o.polling = 1;
1854 sobj_o.sensor_operate = lsm303m_orientation_operate;
1855 if((err = hwmsen_attach(ID_ORIENTATION, &sobj_o)))
1856 {
1857 MSE_ERR("attach fail = %d\n", err);
1858 goto exit_kfree;
1859 }
1860
1861 #if CONFIG_HAS_EARLYSUSPEND
1862 data->early_drv.level = EARLY_SUSPEND_LEVEL_DISABLE_FB - 1,
1863 data->early_drv.suspend = lsm303m_early_suspend,
1864 data->early_drv.resume = lsm303m_late_resume,
1865 register_early_suspend(&data->early_drv);
1866 #endif
1867
1868 MSE_LOG("%s: OK\n", __func__);
1869 return 0;
1870
1871 exit_sysfs_create_group_failed:
1872 exit_init_failed:
1873 //i2c_detach_client(new_client);
1874 exit_misc_device_register_failed:
1875 exit_kfree:
1876 kfree(data);
1877 exit:
1878 MSE_ERR("%s: err = %d\n", __func__, err);
1879 return err;
1880
1881 }
1882
1883
1884
1885 /*----------------------------------------------------------------------------*/
1886 static int lsm303m_i2c_remove(struct i2c_client *client)
1887 {
1888 int err = 0;
1889
1890 if((err = lsm303m_delete_attr(&lsm303m_msensor_driver.driver)))
1891 {
1892 MSE_ERR("lsm303m_delete_attr fail: %d\n", err);
1893 }
1894
1895 if((err = misc_deregister(&lsm303m_device)))
1896 {
1897 MSE_ERR("misc_deregister fail: %d\n", err);
1898 }
1899
1900 if((err = hwmsen_detach(ID_MAGNETIC)))
1901 {
1902 MSE_ERR("hwmsen_detach fail: %d\n", err);
1903 }
1904 if((err = hwmsen_detach(ID_ORIENTATION)))
1905 {
1906 MSE_ERR("hwmsen_detach fail: %d\n", err);
1907 }
1908
1909
1910 lsm303m_i2c_client = NULL;
1911 i2c_unregister_device(client);
1912 kfree(i2c_get_clientdata(client));
1913
1914 return 0;
1915 }
1916 /*----------------------------------------------------------------------------*/
1917 static int lsm303m_probe(struct platform_device *pdev)
1918 {
1919 struct mag_hw *hw = get_cust_mag_hw();
1920
1921 MSE_FUN();
1922
1923 lsm303m_power(hw, 1);
1924 rwlock_init(&lsm303mmid_data.ctrllock);
1925 rwlock_init(&lsm303mmid_data.datalock);
1926 rwlock_init(&lsm303m_data.lock);
1927 memset(&lsm303mmid_data.controldata[0], 0, sizeof(int)*10);
1928
1929 atomic_set(&dev_open_count, 0);
1930
1931 MSE_ERR("i2c add driver \n");
1932
1933 //lsm303m_force[0] = hw->i2c_num;//modified
1934 if(i2c_add_driver(&lsm303m_i2c_driver))
1935 {
1936 MSE_ERR("add driver error\n");
1937 return -1;
1938 }
1939 return 0;
1940 }
1941 /*----------------------------------------------------------------------------*/
1942 static int lsm303m_remove(struct platform_device *pdev)
1943 {
1944 struct mag_hw *hw = get_cust_mag_hw();
1945
1946 MSE_FUN();
1947 lsm303m_power(hw, 0);
1948 atomic_set(&dev_open_count, 0);
1949 i2c_del_driver(&lsm303m_i2c_driver);
1950 return 0;
1951 }
1952 /*----------------------------------------------------------------------------*/
1953 #if 0
1954 static struct platform_driver lsm303m_msensor_driver = {
1955 .probe = lsm303m_probe,
1956 .remove = lsm303m_remove,
1957 .driver = {
1958 .name = "msensor",
1959 // .owner = THIS_MODULE,
1960 }
1961 };
1962 #endif
1963
1964 #ifdef CONFIG_OF
1965 static const struct of_device_id lsm303m_of_match[] = {
1966 { .compatible = "mediatek,msensor", },
1967 {},
1968 };
1969 #endif
1970
1971 static struct platform_driver lsm303m_msensor_driver =
1972 {
1973 .probe = lsm303m_probe,
1974 .remove = lsm303m_remove,
1975 .driver =
1976 {
1977 .name = "msensor",
1978 #ifdef CONFIG_OF
1979 .of_match_table = lsm303m_of_match,
1980 #endif
1981 }
1982 };
1983
1984 /*----------------------------------------------------------------------------*/
1985 static int __init lsm303m_init(void)
1986 {
1987 MSE_FUN();
1988
1989 struct mag_hw *hw = get_cust_mag_hw();
1990 MSE_LOG("%s: i2c_number=%d\n", __func__,hw->i2c_num);
1991
1992 i2c_register_board_info(hw->i2c_num, &i2c_lsm303m, 1);
1993 if(platform_driver_register(&lsm303m_msensor_driver))
1994 {
1995 MSE_ERR("failed to register driver");
1996 return -ENODEV;
1997 }
1998 return 0;
1999 }
2000 /*----------------------------------------------------------------------------*/
2001 static void __exit lsm303m_exit(void)
2002 {
2003 MSE_FUN();
2004 platform_driver_unregister(&lsm303m_msensor_driver);
2005 }
2006
2007 /*----------------------------------------------------------------------------*/
2008 module_init(lsm303m_init);
2009 module_exit(lsm303m_exit);
2010 /*----------------------------------------------------------------------------*/
2011 MODULE_AUTHOR("Ruo Liang");
2012 MODULE_DESCRIPTION("LSM303M MI-Sensor driver without DRDY");
2013 MODULE_LICENSE("GPL");
2014 MODULE_VERSION("1.0.1");
2015
2016
kernel source for telekom puls (alcatel/mediatek)