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 / bma050 / bma050.c
1 /* BMA250 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 2011 Bosch Sensortec GmbH
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
25 #include <mach/mt_typedefs.h>
26 #include <mach/mt_gpio.h>
27 #include <mach/mt_pm_ldo.h>
28
29 #include <cust_acc.h>
30 #include <linux/hwmsensor.h>
31 #include <linux/hwmsen_dev.h>
32 #include <linux/sensors_io.h>
33 #include "bma050.h"
34 #include <linux/hwmsen_helper.h>
35
36 #define POWER_NONE_MACRO MT65XX_POWER_NONE
37
38 /*----------------------------------------------------------------------------*/
39 #define I2C_DRIVERID_BMA250 250
40 /*----------------------------------------------------------------------------*/
41 #define DEBUG 1
42 /*----------------------------------------------------------------------------*/
43 //#define CONFIG_BMA250_LOWPASS /*apply low pass filter on output*/
44 #define SW_CALIBRATION
45
46 /*----------------------------------------------------------------------------*/
47 #define BMA250_AXIS_X 0
48 #define BMA250_AXIS_Y 1
49 #define BMA250_AXIS_Z 2
50 #define BMA250_AXES_NUM 3
51 #define BMA250_DATA_LEN 6
52 #define BMA250_DEV_NAME "BMA250"
53
54 #define BMA250_MODE_NORMAL 0
55 #define BMA250_MODE_LOWPOWER 1
56 #define BMA250_MODE_SUSPEND 2
57
58 #define BMA250_ACC_X_LSB__POS 6
59 #define BMA250_ACC_X_LSB__LEN 2
60 #define BMA250_ACC_X_LSB__MSK 0xC0
61 //#define BMA250_ACC_X_LSB__REG BMA250_X_AXIS_LSB_REG
62
63 #define BMA250_ACC_X_MSB__POS 0
64 #define BMA250_ACC_X_MSB__LEN 8
65 #define BMA250_ACC_X_MSB__MSK 0xFF
66 //#define BMA250_ACC_X_MSB__REG BMA250_X_AXIS_MSB_REG
67
68 #define BMA250_ACC_Y_LSB__POS 6
69 #define BMA250_ACC_Y_LSB__LEN 2
70 #define BMA250_ACC_Y_LSB__MSK 0xC0
71 //#define BMA250_ACC_Y_LSB__REG BMA250_Y_AXIS_LSB_REG
72
73 #define BMA250_ACC_Y_MSB__POS 0
74 #define BMA250_ACC_Y_MSB__LEN 8
75 #define BMA250_ACC_Y_MSB__MSK 0xFF
76 //#define BMA250_ACC_Y_MSB__REG BMA250_Y_AXIS_MSB_REG
77
78 #define BMA250_ACC_Z_LSB__POS 6
79 #define BMA250_ACC_Z_LSB__LEN 2
80 #define BMA250_ACC_Z_LSB__MSK 0xC0
81 //#define BMA250_ACC_Z_LSB__REG BMA250_Z_AXIS_LSB_REG
82
83 #define BMA250_ACC_Z_MSB__POS 0
84 #define BMA250_ACC_Z_MSB__LEN 8
85 #define BMA250_ACC_Z_MSB__MSK 0xFF
86 //#define BMA250_ACC_Z_MSB__REG BMA250_Z_AXIS_MSB_REG
87
88 #define BMA250_EN_LOW_POWER__POS 6
89 #define BMA250_EN_LOW_POWER__LEN 1
90 #define BMA250_EN_LOW_POWER__MSK 0x40
91 #define BMA250_EN_LOW_POWER__REG BMA250_REG_POWER_CTL
92
93 #define BMA250_EN_SUSPEND__POS 7
94 #define BMA250_EN_SUSPEND__LEN 1
95 #define BMA250_EN_SUSPEND__MSK 0x80
96 #define BMA250_EN_SUSPEND__REG BMA250_REG_POWER_CTL
97
98 #define BMA250_RANGE_SEL__POS 0
99 #define BMA250_RANGE_SEL__LEN 4
100 #define BMA250_RANGE_SEL__MSK 0x0F
101 #define BMA250_RANGE_SEL__REG BMA250_REG_DATA_FORMAT
102
103 #define BMA250_BANDWIDTH__POS 0
104 #define BMA250_BANDWIDTH__LEN 5
105 #define BMA250_BANDWIDTH__MSK 0x1F
106 #define BMA250_BANDWIDTH__REG BMA250_REG_BW_RATE
107
108 #define BMA250_GET_BITSLICE(regvar, bitname)\
109 ((regvar & bitname##__MSK) >> bitname##__POS)
110
111 #define BMA250_SET_BITSLICE(regvar, bitname, val)\
112 ((regvar & ~bitname##__MSK) | ((val<<bitname##__POS)&bitname##__MSK))
113
114 /*----------------------------------------------------------------------------*/
115
116 /*----------------------------------------------------------------------------*/
117 static const struct i2c_device_id bma250_i2c_id[] = {{BMA250_DEV_NAME,0},{}};
118 static struct i2c_board_info __initdata i2c_bma250={ I2C_BOARD_INFO("BMA250", (0x30>>1))};
119
120
121 /*the adapter id will be available in customization*/
122 //static unsigned short bma250_force[] = {0x00, BMA250_I2C_SLAVE_WRITE_ADDR, I2C_CLIENT_END, I2C_CLIENT_END};
123 //static const unsigned short *const bma250_forces[] = { bma250_force, NULL };
124 //static struct i2c_client_address_data bma250_addr_data = { .forces = bma250_forces,};
125
126 /*----------------------------------------------------------------------------*/
127 static int bma250_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id);
128 static int bma250_i2c_remove(struct i2c_client *client);
129 static int bma250_suspend(struct i2c_client *client, pm_message_t msg);
130 static int bma250_resume(struct i2c_client *client);
131
132
133 //static int bma250_i2c_detect(struct i2c_client *client, int kind, struct i2c_board_info *info);
134
135 /*----------------------------------------------------------------------------*/
136 typedef enum {
137 ADX_TRC_FILTER = 0x01,
138 ADX_TRC_RAWDATA = 0x02,
139 ADX_TRC_IOCTL = 0x04,
140 ADX_TRC_CALI = 0X08,
141 ADX_TRC_INFO = 0X10,
142 } ADX_TRC;
143 /*----------------------------------------------------------------------------*/
144 struct scale_factor{
145 u8 whole;
146 u8 fraction;
147 };
148 /*----------------------------------------------------------------------------*/
149 struct data_resolution {
150 struct scale_factor scalefactor;
151 int sensitivity;
152 };
153 /*----------------------------------------------------------------------------*/
154 #define C_MAX_FIR_LENGTH (32)
155 //#define USE_EARLY_SUSPEND
156 static bool enable_status = false;
157 static DEFINE_MUTEX(bma050_i2c_mutex);
158 static DEFINE_MUTEX(bma050_op_mutex);
159
160
161
162 /*----------------------------------------------------------------------------*/
163 struct data_filter {
164 s16 raw[C_MAX_FIR_LENGTH][BMA250_AXES_NUM];
165 int sum[BMA250_AXES_NUM];
166 int num;
167 int idx;
168 };
169 /*----------------------------------------------------------------------------*/
170 struct bma250_i2c_data {
171 struct i2c_client *client;
172 struct acc_hw *hw;
173 struct hwmsen_convert cvt;
174
175 /*misc*/
176 struct data_resolution *reso;
177 atomic_t trace;
178 atomic_t suspend;
179 atomic_t selftest;
180 atomic_t filter;
181 s16 cali_sw[BMA250_AXES_NUM+1];
182
183 /*data*/
184 s8 offset[BMA250_AXES_NUM+1]; /*+1: for 4-byte alignment*/
185 s16 data[BMA250_AXES_NUM+1];
186
187 #if defined(CONFIG_BMA250_LOWPASS)
188 atomic_t firlen;
189 atomic_t fir_en;
190 struct data_filter fir;
191 #endif
192 /*early suspend*/
193 #if defined(USE_EARLY_SUSPEND)
194 struct early_suspend early_drv;
195 #endif
196 };
197 /*----------------------------------------------------------------------------*/
198 static struct i2c_driver bma250_i2c_driver = {
199 .driver = {
200 .name = BMA250_DEV_NAME,
201 },
202 .probe = bma250_i2c_probe,
203 .remove = bma250_i2c_remove,
204 #if !defined(USE_EARLY_SUSPEND)
205 .suspend = bma250_suspend,
206 .resume = bma250_resume,
207 #endif
208 .id_table = bma250_i2c_id,
209 };
210
211 /*----------------------------------------------------------------------------*/
212 static struct i2c_client *bma250_i2c_client = NULL;
213 static struct platform_driver bma250_gsensor_driver;
214 static struct bma250_i2c_data *obj_i2c_data = NULL;
215 static bool sensor_power = true;
216 static GSENSOR_VECTOR3D gsensor_gain;
217 //static char selftestRes[8]= {0};
218
219 /*----------------------------------------------------------------------------*/
220 #define GSE_TAG "[Gsensor] "
221 #define GSE_FUN(f) printk(KERN_INFO GSE_TAG"%s\n", __FUNCTION__)
222 #define GSE_ERR(fmt, args...) printk(KERN_ERR GSE_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
223 #define GSE_LOG(fmt, args...) printk(KERN_INFO GSE_TAG fmt, ##args)
224 /*----------------------------------------------------------------------------*/
225 static struct data_resolution bma250_data_resolution[1] = {
226 /* combination by {FULL_RES,RANGE}*/
227 {{ 3, 9}, 256}, // dataformat +/-2g in 10-bit resolution; { 3, 9} = 3.9= (2*2*1000)/(2^10); 256 = (2^10)/(2*2)
228 };
229 /*----------------------------------------------------------------------------*/
230 static struct data_resolution bma250_offset_resolution = {{3, 9}, 256};
231
232 #define C_I2C_FIFO_SIZE 8
233 static int bma050_i2c_read_block(struct i2c_client *client, u8 addr, u8 *data, u8 len)
234 {
235 u8 beg = addr;
236 int err;
237 struct i2c_msg msgs[2]={{0},{0}};
238
239 mutex_lock(&bma050_i2c_mutex);
240
241 msgs[0].addr = client->addr;
242 msgs[0].flags = 0;
243 msgs[0].len =1;
244 msgs[0].buf = &beg;
245
246 msgs[1].addr = client->addr;
247 msgs[1].flags = I2C_M_RD;
248 msgs[1].len =len;
249 msgs[1].buf = data;
250
251 if (!client)
252 {
253 mutex_unlock(&bma050_i2c_mutex);
254 return -EINVAL;
255 }
256 else if (len > C_I2C_FIFO_SIZE)
257 {
258 GSE_ERR(" length %d exceeds %d\n", len, C_I2C_FIFO_SIZE);
259 mutex_unlock(&bma050_i2c_mutex);
260 return -EINVAL;
261 }
262 err = i2c_transfer(client->adapter, msgs, sizeof(msgs)/sizeof(msgs[0]));
263 if (err != 2)
264 {
265 GSE_ERR("i2c_transfer error: (%d %p %d) %d\n",addr, data, len, err);
266 err = -EIO;
267 }
268 else
269 {
270 err = 0;
271 }
272 mutex_unlock(&bma050_i2c_mutex);
273 return err;
274
275 }
276
277 static int bma050_i2c_write_block(struct i2c_client *client, u8 addr, u8 *data, u8 len)
278 { /*because address also occupies one byte, the maximum length for write is 7 bytes*/
279 int err, idx, num;
280 char buf[C_I2C_FIFO_SIZE];
281 err =0;
282 mutex_lock(&bma050_i2c_mutex);
283 if (!client)
284 {
285 mutex_unlock(&bma050_i2c_mutex);
286 return -EINVAL;
287 }
288 else if (len >= C_I2C_FIFO_SIZE)
289 {
290 GSE_ERR(" length %d exceeds %d\n", len, C_I2C_FIFO_SIZE);
291 mutex_unlock(&bma050_i2c_mutex);
292 return -EINVAL;
293 }
294
295 num = 0;
296 buf[num++] = addr;
297 for (idx = 0; idx < len; idx++)
298 {
299 buf[num++] = data[idx];
300 }
301
302 err = i2c_master_send(client, buf, num);
303 if (err < 0)
304 {
305 GSE_ERR("send command error!!\n");
306 mutex_unlock(&bma050_i2c_mutex);
307 return -EFAULT;
308 }
309 mutex_unlock(&bma050_i2c_mutex);
310 return err;
311 }
312
313
314 /*--------------------BMA250 power control function----------------------------------*/
315 static void BMA250_power(struct acc_hw *hw, unsigned int on)
316 {
317 static unsigned int power_on = 0;
318
319 if(hw->power_id != POWER_NONE_MACRO) // have externel LDO
320 {
321 GSE_LOG("power %s\n", on ? "on" : "off");
322 if(power_on == on) // power status not change
323 {
324 GSE_LOG("ignore power control: %d\n", on);
325 }
326 else if(on) // power on
327 {
328 if(!hwPowerOn(hw->power_id, hw->power_vol, "BMA250"))
329 {
330 GSE_ERR("power on fails!!\n");
331 }
332 }
333 else // power off
334 {
335 if (!hwPowerDown(hw->power_id, "BMA250"))
336 {
337 GSE_ERR("power off fail!!\n");
338 }
339 }
340 }
341 power_on = on;
342 }
343 /*----------------------------------------------------------------------------*/
344
345 /*----------------------------------------------------------------------------*/
346 static int BMA250_SetDataResolution(struct bma250_i2c_data *obj)
347 {
348
349 /*set g sensor dataresolution here*/
350
351 /*BMA250 only can set to 10-bit dataresolution, so do nothing in bma250 driver here*/
352
353 /*end of set dataresolution*/
354
355
356
357 /*we set measure range from -2g to +2g in BMA250_SetDataFormat(client, BMA250_RANGE_2G),
358 and set 10-bit dataresolution BMA250_SetDataResolution()*/
359
360 /*so bma250_data_resolution[0] set value as {{ 3, 9}, 256} when declaration, and assign the value to obj->reso here*/
361
362 obj->reso = &bma250_data_resolution[0];
363 return 0;
364
365 /*if you changed the measure range, for example call: BMA250_SetDataFormat(client, BMA250_RANGE_4G),
366 you must set the right value to bma250_data_resolution*/
367
368 }
369 /*----------------------------------------------------------------------------*/
370 static int BMA250_ReadData(struct i2c_client *client, s16 data[BMA250_AXES_NUM])
371 {
372 //struct bma250_i2c_data *priv = i2c_get_clientdata(client);
373 u8 addr = BMA250_REG_DATAXLOW;
374 u8 buf[BMA250_DATA_LEN] = {0};
375 int err = 0;
376
377 if(NULL == client)
378 {
379 err = -EINVAL;
380 }
381 else if((err = bma050_i2c_read_block(client, addr, buf, BMA250_DATA_LEN)))
382 {
383 GSE_ERR("error: %d\n", err);
384 }
385 else
386 {
387 /* Convert sensor raw data to 16-bit integer */
388 data[BMA250_AXIS_X] = BMA250_GET_BITSLICE(buf[0], BMA250_ACC_X_LSB)
389 |(BMA250_GET_BITSLICE(buf[1],
390 BMA250_ACC_X_MSB)<<BMA250_ACC_X_LSB__LEN);
391 data[BMA250_AXIS_X] = data[BMA250_AXIS_X] << (sizeof(short)*8-(BMA250_ACC_X_LSB__LEN
392 + BMA250_ACC_X_MSB__LEN));
393 data[BMA250_AXIS_X] = data[BMA250_AXIS_X] >> (sizeof(short)*8-(BMA250_ACC_X_LSB__LEN
394 + BMA250_ACC_X_MSB__LEN));
395 data[BMA250_AXIS_Y] = BMA250_GET_BITSLICE(buf[2], BMA250_ACC_Y_LSB)
396 | (BMA250_GET_BITSLICE(buf[3],
397 BMA250_ACC_Y_MSB)<<BMA250_ACC_Y_LSB__LEN);
398 data[BMA250_AXIS_Y] = data[BMA250_AXIS_Y] << (sizeof(short)*8-(BMA250_ACC_Y_LSB__LEN
399 + BMA250_ACC_Y_MSB__LEN));
400 data[BMA250_AXIS_Y] = data[BMA250_AXIS_Y] >> (sizeof(short)*8-(BMA250_ACC_Y_LSB__LEN
401 + BMA250_ACC_Y_MSB__LEN));
402 data[BMA250_AXIS_Z] = BMA250_GET_BITSLICE(buf[4], BMA250_ACC_Z_LSB)
403 | (BMA250_GET_BITSLICE(buf[5],
404 BMA250_ACC_Z_MSB)<<BMA250_ACC_Z_LSB__LEN);
405 data[BMA250_AXIS_Z] = data[BMA250_AXIS_Z] << (sizeof(short)*8-(BMA250_ACC_Z_LSB__LEN
406 + BMA250_ACC_Z_MSB__LEN));
407 data[BMA250_AXIS_Z] = data[BMA250_AXIS_Z] >> (sizeof(short)*8-(BMA250_ACC_Z_LSB__LEN
408 + BMA250_ACC_Z_MSB__LEN));
409
410 #ifdef CONFIG_BMA250_LOWPASS
411 if(atomic_read(&priv->filter))
412 {
413 if(atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
414 {
415 int idx, firlen = atomic_read(&priv->firlen);
416 if(priv->fir.num < firlen)
417 {
418 priv->fir.raw[priv->fir.num][BMA250_AXIS_X] = data[BMA250_AXIS_X];
419 priv->fir.raw[priv->fir.num][BMA250_AXIS_Y] = data[BMA250_AXIS_Y];
420 priv->fir.raw[priv->fir.num][BMA250_AXIS_Z] = data[BMA250_AXIS_Z];
421 priv->fir.sum[BMA250_AXIS_X] += data[BMA250_AXIS_X];
422 priv->fir.sum[BMA250_AXIS_Y] += data[BMA250_AXIS_Y];
423 priv->fir.sum[BMA250_AXIS_Z] += data[BMA250_AXIS_Z];
424 if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
425 {
426 GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
427 priv->fir.raw[priv->fir.num][BMA250_AXIS_X], priv->fir.raw[priv->fir.num][BMA250_AXIS_Y], priv->fir.raw[priv->fir.num][BMA250_AXIS_Z],
428 priv->fir.sum[BMA250_AXIS_X], priv->fir.sum[BMA250_AXIS_Y], priv->fir.sum[BMA250_AXIS_Z]);
429 }
430 priv->fir.num++;
431 priv->fir.idx++;
432 }
433 else
434 {
435 idx = priv->fir.idx % firlen;
436 priv->fir.sum[BMA250_AXIS_X] -= priv->fir.raw[idx][BMA250_AXIS_X];
437 priv->fir.sum[BMA250_AXIS_Y] -= priv->fir.raw[idx][BMA250_AXIS_Y];
438 priv->fir.sum[BMA250_AXIS_Z] -= priv->fir.raw[idx][BMA250_AXIS_Z];
439 priv->fir.raw[idx][BMA250_AXIS_X] = data[BMA250_AXIS_X];
440 priv->fir.raw[idx][BMA250_AXIS_Y] = data[BMA250_AXIS_Y];
441 priv->fir.raw[idx][BMA250_AXIS_Z] = data[BMA250_AXIS_Z];
442 priv->fir.sum[BMA250_AXIS_X] += data[BMA250_AXIS_X];
443 priv->fir.sum[BMA250_AXIS_Y] += data[BMA250_AXIS_Y];
444 priv->fir.sum[BMA250_AXIS_Z] += data[BMA250_AXIS_Z];
445 priv->fir.idx++;
446 data[BMA250_AXIS_X] = priv->fir.sum[BMA250_AXIS_X]/firlen;
447 data[BMA250_AXIS_Y] = priv->fir.sum[BMA250_AXIS_Y]/firlen;
448 data[BMA250_AXIS_Z] = priv->fir.sum[BMA250_AXIS_Z]/firlen;
449 if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
450 {
451 GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
452 priv->fir.raw[idx][BMA250_AXIS_X], priv->fir.raw[idx][BMA250_AXIS_Y], priv->fir.raw[idx][BMA250_AXIS_Z],
453 priv->fir.sum[BMA250_AXIS_X], priv->fir.sum[BMA250_AXIS_Y], priv->fir.sum[BMA250_AXIS_Z],
454 data[BMA250_AXIS_X], data[BMA250_AXIS_Y], data[BMA250_AXIS_Z]);
455 }
456 }
457 }
458 }
459 #endif
460 }
461 return err;
462 }
463 /*----------------------------------------------------------------------------*/
464 static int BMA250_ReadOffset(struct i2c_client *client, s8 ofs[BMA250_AXES_NUM])
465 {
466 int err = 0;
467 #ifdef SW_CALIBRATION
468 ofs[0]=ofs[1]=ofs[2]=0x0;
469 #else
470 if(err = bma050_i2c_read_block(client, BMA250_REG_OFSX, ofs, BMA250_AXES_NUM))
471 {
472 GSE_ERR("error: %d\n", err);
473 }
474 #endif
475 //printk("offesx=%x, y=%x, z=%x",ofs[0],ofs[1],ofs[2]);
476
477 return err;
478 }
479 /*----------------------------------------------------------------------------*/
480 static int BMA250_ResetCalibration(struct i2c_client *client)
481 {
482 struct bma250_i2c_data *obj = i2c_get_clientdata(client);
483 //u8 ofs[4]={0,0,0,0};
484 int err = 0;
485
486 #ifdef SW_CALIBRATION
487
488 #else
489 if(err = bma050_i2c_write_block(client, BMA250_REG_OFSX, ofs, 4))
490 {
491 GSE_ERR("error: %d\n", err);
492 }
493 #endif
494
495 memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
496 memset(obj->offset, 0x00, sizeof(obj->offset));
497 return err;
498 }
499 /*----------------------------------------------------------------------------*/
500 static int BMA250_ReadCalibration(struct i2c_client *client, int dat[BMA250_AXES_NUM])
501 {
502 struct bma250_i2c_data *obj = i2c_get_clientdata(client);
503 //int err;
504 int mul;
505
506 #ifdef SW_CALIBRATION
507 mul = 0;//only SW Calibration, disable HW Calibration
508 #else
509 if ((err = BMA250_ReadOffset(client, obj->offset))) {
510 GSE_ERR("read offset fail, %d\n", err);
511 return err;
512 }
513 mul = obj->reso->sensitivity/bma250_offset_resolution.sensitivity;
514 #endif
515
516 dat[obj->cvt.map[BMA250_AXIS_X]] = obj->cvt.sign[BMA250_AXIS_X]*(obj->offset[BMA250_AXIS_X]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[BMA250_AXIS_X]);
517 dat[obj->cvt.map[BMA250_AXIS_Y]] = obj->cvt.sign[BMA250_AXIS_Y]*(obj->offset[BMA250_AXIS_Y]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[BMA250_AXIS_Y]);
518 dat[obj->cvt.map[BMA250_AXIS_Z]] = obj->cvt.sign[BMA250_AXIS_Z]*(obj->offset[BMA250_AXIS_Z]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[BMA250_AXIS_Z]);
519
520 return 0;
521 }
522 /*----------------------------------------------------------------------------*/
523 static int BMA250_ReadCalibrationEx(struct i2c_client *client, int act[BMA250_AXES_NUM], int raw[BMA250_AXES_NUM])
524 {
525 /*raw: the raw calibration data; act: the actual calibration data*/
526 struct bma250_i2c_data *obj = i2c_get_clientdata(client);
527 //int err;
528 int mul;
529
530
531
532 #ifdef SW_CALIBRATION
533 mul = 0;//only SW Calibration, disable HW Calibration
534 #else
535 if(err = BMA250_ReadOffset(client, obj->offset))
536 {
537 GSE_ERR("read offset fail, %d\n", err);
538 return err;
539 }
540 mul = obj->reso->sensitivity/bma250_offset_resolution.sensitivity;
541 #endif
542
543 raw[BMA250_AXIS_X] = obj->offset[BMA250_AXIS_X]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[BMA250_AXIS_X];
544 raw[BMA250_AXIS_Y] = obj->offset[BMA250_AXIS_Y]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[BMA250_AXIS_Y];
545 raw[BMA250_AXIS_Z] = obj->offset[BMA250_AXIS_Z]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[BMA250_AXIS_Z];
546
547 act[obj->cvt.map[BMA250_AXIS_X]] = obj->cvt.sign[BMA250_AXIS_X]*raw[BMA250_AXIS_X];
548 act[obj->cvt.map[BMA250_AXIS_Y]] = obj->cvt.sign[BMA250_AXIS_Y]*raw[BMA250_AXIS_Y];
549 act[obj->cvt.map[BMA250_AXIS_Z]] = obj->cvt.sign[BMA250_AXIS_Z]*raw[BMA250_AXIS_Z];
550
551 return 0;
552 }
553 /*----------------------------------------------------------------------------*/
554 static int BMA250_WriteCalibration(struct i2c_client *client, int dat[BMA250_AXES_NUM])
555 {
556 struct bma250_i2c_data *obj = i2c_get_clientdata(client);
557 int err;
558 int cali[BMA250_AXES_NUM], raw[BMA250_AXES_NUM];
559 //int lsb = bma250_offset_resolution.sensitivity;
560 //int divisor = obj->reso->sensitivity/lsb;
561
562 if((err = BMA250_ReadCalibrationEx(client, cali, raw))) /*offset will be updated in obj->offset*/
563 {
564 GSE_ERR("read offset fail, %d\n", err);
565 return err;
566 }
567
568 GSE_LOG("OLDOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n",
569 raw[BMA250_AXIS_X], raw[BMA250_AXIS_Y], raw[BMA250_AXIS_Z],
570 obj->offset[BMA250_AXIS_X], obj->offset[BMA250_AXIS_Y], obj->offset[BMA250_AXIS_Z],
571 obj->cali_sw[BMA250_AXIS_X], obj->cali_sw[BMA250_AXIS_Y], obj->cali_sw[BMA250_AXIS_Z]);
572
573 /*calculate the real offset expected by caller*/
574 cali[BMA250_AXIS_X] += dat[BMA250_AXIS_X];
575 cali[BMA250_AXIS_Y] += dat[BMA250_AXIS_Y];
576 cali[BMA250_AXIS_Z] += dat[BMA250_AXIS_Z];
577
578 GSE_LOG("UPDATE: (%+3d %+3d %+3d)\n",
579 dat[BMA250_AXIS_X], dat[BMA250_AXIS_Y], dat[BMA250_AXIS_Z]);
580
581 #ifdef SW_CALIBRATION
582 obj->cali_sw[BMA250_AXIS_X] = obj->cvt.sign[BMA250_AXIS_X]*(cali[obj->cvt.map[BMA250_AXIS_X]]);
583 obj->cali_sw[BMA250_AXIS_Y] = obj->cvt.sign[BMA250_AXIS_Y]*(cali[obj->cvt.map[BMA250_AXIS_Y]]);
584 obj->cali_sw[BMA250_AXIS_Z] = obj->cvt.sign[BMA250_AXIS_Z]*(cali[obj->cvt.map[BMA250_AXIS_Z]]);
585 #else
586 obj->offset[BMA250_AXIS_X] = (s8)(obj->cvt.sign[BMA250_AXIS_X]*(cali[obj->cvt.map[BMA250_AXIS_X]])*(obj->reso->sensitivity)/GRAVITY_EARTH_1000/(divisor));
587 obj->offset[BMA250_AXIS_Y] = (s8)(obj->cvt.sign[BMA250_AXIS_Y]*(cali[obj->cvt.map[BMA250_AXIS_Y]])*(obj->reso->sensitivity)/GRAVITY_EARTH_1000/(divisor));
588 obj->offset[BMA250_AXIS_Z] = (s8)(obj->cvt.sign[BMA250_AXIS_Z]*(cali[obj->cvt.map[BMA250_AXIS_Z]])*(obj->reso->sensitivity)/GRAVITY_EARTH_1000/(divisor));
589
590 /*convert software calibration using standard calibration*/
591 obj->cali_sw[BMA250_AXIS_X] = obj->cvt.sign[BMA250_AXIS_X]*(cali[obj->cvt.map[BMA250_AXIS_X]])%(divisor);
592 obj->cali_sw[BMA250_AXIS_Y] = obj->cvt.sign[BMA250_AXIS_Y]*(cali[obj->cvt.map[BMA250_AXIS_Y]])%(divisor);
593 obj->cali_sw[BMA250_AXIS_Z] = obj->cvt.sign[BMA250_AXIS_Z]*(cali[obj->cvt.map[BMA250_AXIS_Z]])%(divisor);
594
595 GSE_LOG("NEWOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n",
596 obj->offset[BMA250_AXIS_X]*divisor + obj->cali_sw[BMA250_AXIS_X],
597 obj->offset[BMA250_AXIS_Y]*divisor + obj->cali_sw[BMA250_AXIS_Y],
598 obj->offset[BMA250_AXIS_Z]*divisor + obj->cali_sw[BMA250_AXIS_Z],
599 obj->offset[BMA250_AXIS_X], obj->offset[BMA250_AXIS_Y], obj->offset[BMA250_AXIS_Z],
600 obj->cali_sw[BMA250_AXIS_X], obj->cali_sw[BMA250_AXIS_Y], obj->cali_sw[BMA250_AXIS_Z]);
601
602 if(err = bma050_i2c_write_block(obj->client, BMA250_REG_OFSX, obj->offset, BMA250_AXES_NUM))
603 {
604 GSE_ERR("write offset fail: %d\n", err);
605 return err;
606 }
607 #endif
608
609 return err;
610 }
611 /*----------------------------------------------------------------------------*/
612 static int BMA250_CheckDeviceID(struct i2c_client *client)
613 {
614 u8 databuf[2];
615 int res = 0;
616
617 memset(databuf, 0, sizeof(u8)*2);
618 databuf[0] = BMA250_REG_DEVID;
619
620 res = bma050_i2c_read_block(client,BMA250_REG_DEVID,databuf,0x1);
621 if(res < 0)
622 {
623 goto exit_BMA250_CheckDeviceID;
624 }
625 if(databuf[0]!=BMA250_FIXED_DEVID)
626 {
627 printk("BMA250_CheckDeviceID %d failt!\n ", databuf[0]);
628 return BMA250_ERR_IDENTIFICATION;
629 }
630 else
631 {
632 printk("BMA250_CheckDeviceID %d pass!\n ", databuf[0]);
633 }
634
635 exit_BMA250_CheckDeviceID:
636 if (res < 0)
637 {
638 return BMA250_ERR_I2C;
639 }
640
641 return BMA250_SUCCESS;
642 }
643 /*----------------------------------------------------------------------------*/
644 static int BMA250_SetPowerMode(struct i2c_client *client, bool enable)
645 {
646 u8 databuf[2];
647 int res = 0;
648 u8 addr = BMA250_REG_POWER_CTL;
649 struct bma250_i2c_data *obj = i2c_get_clientdata(client);
650
651
652 if(enable == sensor_power )
653 {
654 GSE_LOG("Sensor power status is newest!\n");
655 return BMA250_SUCCESS;
656 }
657
658 if(bma050_i2c_read_block(client, addr, databuf, 0x01))
659 {
660 GSE_ERR("read power ctl register err!\n");
661 return BMA250_ERR_I2C;
662 }
663
664
665 if(enable == TRUE)
666 {
667 databuf[0] &= ~BMA250_MEASURE_MODE;
668 }
669 else
670 {
671 databuf[0] |= BMA250_MEASURE_MODE;
672 }
673 //databuf[1] = databuf[0];
674 //databuf[0] = BMA250_REG_POWER_CTL;
675
676 res = bma050_i2c_write_block(client,BMA250_REG_POWER_CTL,databuf,0x1);
677 if(res < 0)
678 {
679 GSE_LOG("set power mode failed!\n");
680 return BMA250_ERR_I2C;
681 }
682 else if(atomic_read(&obj->trace) & ADX_TRC_INFO)
683 {
684 GSE_LOG("set power mode ok %d!\n", databuf[1]);
685 }
686
687 //GSE_LOG("BMA250_SetPowerMode ok!\n");
688
689
690 sensor_power = enable;
691
692 mdelay(20);
693
694 return BMA250_SUCCESS;
695 }
696 /*----------------------------------------------------------------------------*/
697 static int BMA250_SetDataFormat(struct i2c_client *client, u8 dataformat)
698 {
699 struct bma250_i2c_data *obj = i2c_get_clientdata(client);
700 u8 databuf[10];
701 int res = 0;
702
703 memset(databuf, 0, sizeof(u8)*10);
704
705 if(bma050_i2c_read_block(client, BMA250_REG_DATA_FORMAT, databuf, 0x01))
706 {
707 printk("bma250 read Dataformat failt \n");
708 return BMA250_ERR_I2C;
709 }
710
711 databuf[0] &= ~BMA250_RANGE_MASK;
712 databuf[0] |= dataformat;
713
714 res = bma050_i2c_write_block(client,BMA250_REG_DATA_FORMAT,databuf,0x1);
715 if(res < 0)
716 {
717 return BMA250_ERR_I2C;
718 }
719
720 //printk("BMA250_SetDataFormat OK! \n");
721
722
723 return BMA250_SetDataResolution(obj);
724 }
725 /*----------------------------------------------------------------------------*/
726 static int BMA250_SetBWRate(struct i2c_client *client, u8 bwrate)
727 {
728 u8 databuf[10];
729 int res = 0;
730
731 memset(databuf, 0, sizeof(u8)*10);
732
733 if(bma050_i2c_read_block(client, BMA250_REG_BW_RATE, databuf, 0x01))
734 {
735 printk("bma250 read rate failt \n");
736 return BMA250_ERR_I2C;
737 }
738
739 databuf[0] &= ~BMA250_BW_MASK;
740 databuf[0] |= bwrate;
741
742 res = bma050_i2c_write_block(client,BMA250_REG_BW_RATE,databuf,0x1);
743 if(res < 0)
744 {
745 return BMA250_ERR_I2C;
746 }
747
748 //printk("BMA250_SetBWRate OK! \n");
749
750 return BMA250_SUCCESS;
751 }
752 /*----------------------------------------------------------------------------*/
753 static int BMA250_SetIntEnable(struct i2c_client *client, u8 intenable)
754 {
755 //u8 databuf[10];
756 int res = 0;
757
758 res = hwmsen_write_byte(client, BMA250_INT_REG_1, 0x00);
759 if(res != BMA250_SUCCESS)
760 {
761 return res;
762 }
763 res = hwmsen_write_byte(client, BMA250_INT_REG_2, 0x00);
764 if(res != BMA250_SUCCESS)
765 {
766 return res;
767 }
768 printk("BMA250 disable interrupt ...\n");
769
770 /*for disable interrupt function*/
771
772 return BMA250_SUCCESS;
773 }
774
775 /*----------------------------------------------------------------------------*/
776 static int bma250_init_client(struct i2c_client *client, int reset_cali)
777 {
778 struct bma250_i2c_data *obj = i2c_get_clientdata(client);
779 int res = 0;
780 int a = 0;
781 printk("bma250_init_client \n");
782
783 //for fix check device id error
784 do{
785 udelay(100);
786 res = BMA250_CheckDeviceID(client);
787 if(res == BMA250_SUCCESS)
788 {
789 printk("BMA250_CheckDeviceID ok \n");
790 break;
791 }
792 a++;
793 printk("bma250_init_client count: %d\n", a);
794 }while(a < 1000);
795
796 res = BMA250_SetBWRate(client, BMA250_BW_50HZ);
797 if(res != BMA250_SUCCESS )
798 {
799 return res;
800 }
801 printk("BMA250_SetBWRate OK!\n");
802
803 res = BMA250_SetDataFormat(client, BMA250_RANGE_2G);
804 if(res != BMA250_SUCCESS)
805 {
806 return res;
807 }
808 printk("BMA250_SetDataFormat OK!\n");
809
810 gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = obj->reso->sensitivity;
811
812
813 res = BMA250_SetIntEnable(client, 0x00);
814 if(res != BMA250_SUCCESS)
815 {
816 return res;
817 }
818 printk("BMA250 disable interrupt function!\n");
819
820 res = BMA250_SetPowerMode(client, enable_status);
821 if(res != BMA250_SUCCESS)
822 {
823 return res;
824 }
825 printk("BMA250_SetPowerMode OK!\n");
826
827
828 if(0 != reset_cali)
829 {
830 /*reset calibration only in power on*/
831 res = BMA250_ResetCalibration(client);
832 if(res != BMA250_SUCCESS)
833 {
834 return res;
835 }
836 }
837 printk("bma250_init_client OK!\n");
838 #ifdef CONFIG_BMA250_LOWPASS
839 memset(&obj->fir, 0x00, sizeof(obj->fir));
840 #endif
841
842 mdelay(20);
843
844 return BMA250_SUCCESS;
845 }
846 /*----------------------------------------------------------------------------*/
847 static int BMA250_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
848 {
849 u8 databuf[10];
850
851 memset(databuf, 0, sizeof(u8)*10);
852
853 if((NULL == buf)||(bufsize<=30))
854 {
855 return -1;
856 }
857
858 if(NULL == client)
859 {
860 *buf = 0;
861 return -2;
862 }
863
864 sprintf(buf, "BMA250 Chip");
865 return 0;
866 }
867 /*----------------------------------------------------------------------------*/
868 static int BMA250_CompassReadData(struct i2c_client *client, char *buf, int bufsize)
869 {
870 struct bma250_i2c_data *obj = (struct bma250_i2c_data*)i2c_get_clientdata(client);
871 u8 databuf[20];
872 int acc[BMA250_AXES_NUM];
873 int res = 0;
874 memset(databuf, 0, sizeof(u8)*10);
875
876 if(NULL == buf)
877 {
878 return -1;
879 }
880 if(NULL == client)
881 {
882 *buf = 0;
883 return -2;
884 }
885
886 if(sensor_power == FALSE)
887 {
888 res = BMA250_SetPowerMode(client, true);
889 if(res)
890 {
891 GSE_ERR("Power on bma250 error %d!\n", res);
892 }
893 }
894
895 if((res = BMA250_ReadData(client, obj->data)))
896 {
897 GSE_ERR("I2C error: ret value=%d", res);
898 return -3;
899 }
900 else
901 {
902 /*remap coordinate*/
903 acc[obj->cvt.map[BMA250_AXIS_X]] = obj->cvt.sign[BMA250_AXIS_X]*obj->data[BMA250_AXIS_X];
904 acc[obj->cvt.map[BMA250_AXIS_Y]] = obj->cvt.sign[BMA250_AXIS_Y]*obj->data[BMA250_AXIS_Y];
905 acc[obj->cvt.map[BMA250_AXIS_Z]] = obj->cvt.sign[BMA250_AXIS_Z]*obj->data[BMA250_AXIS_Z];
906 //printk("cvt x=%d, y=%d, z=%d \n",obj->cvt.sign[BMA250_AXIS_X],obj->cvt.sign[BMA250_AXIS_Y],obj->cvt.sign[BMA250_AXIS_Z]);
907
908 //GSE_LOG("Mapped gsensor data: %d, %d, %d!\n", acc[BMA250_AXIS_X], acc[BMA250_AXIS_Y], acc[BMA250_AXIS_Z]);
909
910 sprintf(buf, "%d %d %d", (s16)acc[BMA250_AXIS_X], (s16)acc[BMA250_AXIS_Y], (s16)acc[BMA250_AXIS_Z]);
911 if(atomic_read(&obj->trace) & ADX_TRC_IOCTL)
912 {
913 GSE_LOG("gsensor data for compass: %s!\n", buf);
914 }
915 }
916
917 return 0;
918 }
919 /*----------------------------------------------------------------------------*/
920 static int BMA250_ReadSensorData(struct i2c_client *client, char *buf, int bufsize)
921 {
922 struct bma250_i2c_data *obj = (struct bma250_i2c_data*)i2c_get_clientdata(client);
923 u8 databuf[20];
924 int acc[BMA250_AXES_NUM];
925 int res = 0;
926 memset(databuf, 0, sizeof(u8)*10);
927
928 if(NULL == buf)
929 {
930 return -1;
931 }
932 if(NULL == client)
933 {
934 *buf = 0;
935 return -2;
936 }
937 /*
938 if(false == enable_status )
939 {
940
941 acc[BMA250_AXIS_X]=-1;
942 acc[BMA250_AXIS_Y]=-1;
943 acc[BMA250_AXIS_Z]=-1;
944 sprintf(buf, "%04x %04x %04x", acc[BMA250_AXIS_X], acc[BMA250_AXIS_Y], acc[BMA250_AXIS_Z]);
945 GSE_ERR("sensor disable read invalid data!\n");
946 return 0;
947 }
948 */
949 if((res = BMA250_ReadData(client, obj->data)))
950 {
951 GSE_ERR("I2C error: ret value=%d", res);
952 return -3;
953 }
954 else
955 {
956 #if 0
957 obj->data[BMA250_AXIS_X] = obj->data[BMA250_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
958 obj->data[BMA250_AXIS_Y] = obj->data[BMA250_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
959 obj->data[BMA250_AXIS_Z] = obj->data[BMA250_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
960 #endif
961 //printk("raw data x=%d, y=%d, z=%d \n",obj->data[BMA250_AXIS_X],obj->data[BMA250_AXIS_Y],obj->data[BMA250_AXIS_Z]);
962 obj->data[BMA250_AXIS_X] += obj->cali_sw[BMA250_AXIS_X];
963 obj->data[BMA250_AXIS_Y] += obj->cali_sw[BMA250_AXIS_Y];
964 obj->data[BMA250_AXIS_Z] += obj->cali_sw[BMA250_AXIS_Z];
965
966 //printk("cali_sw x=%d, y=%d, z=%d \n",obj->cali_sw[BMA250_AXIS_X],obj->cali_sw[BMA250_AXIS_Y],obj->cali_sw[BMA250_AXIS_Z]);
967
968 /*remap coordinate*/
969 acc[obj->cvt.map[BMA250_AXIS_X]] = obj->cvt.sign[BMA250_AXIS_X]*obj->data[BMA250_AXIS_X];
970 acc[obj->cvt.map[BMA250_AXIS_Y]] = obj->cvt.sign[BMA250_AXIS_Y]*obj->data[BMA250_AXIS_Y];
971 acc[obj->cvt.map[BMA250_AXIS_Z]] = obj->cvt.sign[BMA250_AXIS_Z]*obj->data[BMA250_AXIS_Z];
972 //printk("cvt x=%d, y=%d, z=%d \n",obj->cvt.sign[BMA250_AXIS_X],obj->cvt.sign[BMA250_AXIS_Y],obj->cvt.sign[BMA250_AXIS_Z]);
973
974
975 //GSE_LOG("Mapped gsensor data: %d, %d, %d!\n", acc[BMA250_AXIS_X], acc[BMA250_AXIS_Y], acc[BMA250_AXIS_Z]);
976
977 //Out put the mg
978 //printk("mg acc=%d, GRAVITY=%d, sensityvity=%d \n",acc[BMA250_AXIS_X],GRAVITY_EARTH_1000,obj->reso->sensitivity);
979 #if 0
980 acc[BMA250_AXIS_X] = acc[BMA250_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
981 acc[BMA250_AXIS_Y] = acc[BMA250_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
982 acc[BMA250_AXIS_Z] = acc[BMA250_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
983 #endif
984
985
986 sprintf(buf, "%04x %04x %04x", acc[BMA250_AXIS_X], acc[BMA250_AXIS_Y], acc[BMA250_AXIS_Z]);
987 if(atomic_read(&obj->trace) & ADX_TRC_IOCTL)
988 {
989 GSE_LOG("gsensor data: %s!\n", buf);
990 }
991 }
992
993 return 0;
994 }
995 /*----------------------------------------------------------------------------*/
996 static int BMA250_ReadRawData(struct i2c_client *client, char *buf)
997 {
998 struct bma250_i2c_data *obj = (struct bma250_i2c_data*)i2c_get_clientdata(client);
999 int res = 0;
1000
1001 if (!buf || !client)
1002 {
1003 return EINVAL;
1004 }
1005
1006 if((res = BMA250_ReadData(client, obj->data)))
1007 {
1008 GSE_ERR("I2C error: ret value=%d", res);
1009 return EIO;
1010 }
1011 else
1012 {
1013 sprintf(buf, "BMA250_ReadRawData %04x %04x %04x", obj->data[BMA250_AXIS_X],
1014 obj->data[BMA250_AXIS_Y], obj->data[BMA250_AXIS_Z]);
1015
1016 }
1017
1018 return 0;
1019 }
1020 /*----------------------------------------------------------------------------*/
1021 static int bma250_set_mode(struct i2c_client *client, unsigned char mode)
1022 {
1023 int comres = 0;
1024 unsigned char data[2] = {BMA250_EN_LOW_POWER__REG};
1025
1026 if ((client == NULL) || (mode >= 3))
1027 {
1028 return -1;
1029 }
1030
1031 comres = bma050_i2c_read_block(client,
1032 BMA250_EN_LOW_POWER__REG, data, 1);
1033 switch (mode) {
1034 case BMA250_MODE_NORMAL:
1035 data[0] = BMA250_SET_BITSLICE(data[0],
1036 BMA250_EN_LOW_POWER, 0);
1037 data[0] = BMA250_SET_BITSLICE(data[0],
1038 BMA250_EN_SUSPEND, 0);
1039 break;
1040 case BMA250_MODE_LOWPOWER:
1041 data[0] = BMA250_SET_BITSLICE(data[0],
1042 BMA250_EN_LOW_POWER, 1);
1043 data[0] = BMA250_SET_BITSLICE(data[0],
1044 BMA250_EN_SUSPEND, 0);
1045 break;
1046 case BMA250_MODE_SUSPEND:
1047 data[0] = BMA250_SET_BITSLICE(data[0],
1048 BMA250_EN_LOW_POWER, 0);
1049 data[0] = BMA250_SET_BITSLICE(data[0],
1050 BMA250_EN_SUSPEND, 1);
1051 break;
1052 default:
1053 break;
1054 }
1055
1056 comres = bma050_i2c_write_block(client,BMA250_EN_LOW_POWER__REG,data,0x1);
1057 if(comres < 0)
1058 {
1059 return BMA250_ERR_I2C;
1060 }
1061 else
1062 {
1063 return comres;
1064 }
1065 }
1066 /*----------------------------------------------------------------------------*/
1067 static int bma250_get_mode(struct i2c_client *client, unsigned char *mode)
1068 {
1069 int comres = 0;
1070
1071 if (client == NULL)
1072 {
1073 return -1;
1074 }
1075 comres = bma050_i2c_read_block(client,
1076 BMA250_EN_LOW_POWER__REG, mode, 1);
1077 *mode = (*mode) >> 6;
1078
1079 return comres;
1080 }
1081
1082 /*----------------------------------------------------------------------------*/
1083 static int bma250_set_range(struct i2c_client *client, unsigned char range)
1084 {
1085 int comres = 0;
1086 unsigned char data[2] = {BMA250_RANGE_SEL__REG};
1087
1088 if (client == NULL)
1089 {
1090 return -1;
1091 }
1092
1093 comres = bma050_i2c_read_block(client,
1094 BMA250_RANGE_SEL__REG, data, 1);
1095
1096 data[0] = BMA250_SET_BITSLICE(data[0],
1097 BMA250_RANGE_SEL, range);
1098 comres= bma050_i2c_write_block(client,BMA250_RANGE_SEL__REG,data,0x1);
1099 if(comres < 0)
1100 {
1101 return BMA250_ERR_I2C;
1102 }
1103 else
1104 {
1105 return comres;
1106 }
1107 }
1108 /*----------------------------------------------------------------------------*/
1109 static int bma250_get_range(struct i2c_client *client, unsigned char *range)
1110 {
1111 int comres = 0;
1112 unsigned char data;
1113
1114 if (client == NULL)
1115 {
1116 return -1;
1117 }
1118
1119 comres = bma050_i2c_read_block(client, BMA250_RANGE_SEL__REG, &data, 1);
1120 *range = BMA250_GET_BITSLICE(data, BMA250_RANGE_SEL);
1121
1122 return comres;
1123 }
1124 /*----------------------------------------------------------------------------*/
1125 static int bma250_set_bandwidth(struct i2c_client *client, unsigned char bandwidth)
1126 {
1127 int comres = 0;
1128 unsigned char data[2] = {BMA250_BANDWIDTH__REG};
1129
1130 if (client == NULL)
1131 {
1132 return -1;
1133 }
1134
1135 comres = bma050_i2c_read_block(client,
1136 BMA250_BANDWIDTH__REG, data, 1);
1137
1138 data[0] = BMA250_SET_BITSLICE(data[0],
1139 BMA250_BANDWIDTH, bandwidth);
1140
1141 comres = bma050_i2c_write_block(client,BMA250_BANDWIDTH__REG,data,0x1);
1142 if(comres < 0)
1143 {
1144 return BMA250_ERR_I2C;
1145 }
1146 else
1147 {
1148 return comres;
1149 }
1150 }
1151 /*----------------------------------------------------------------------------*/
1152 static int bma250_get_bandwidth(struct i2c_client *client, unsigned char *bandwidth)
1153 {
1154 int comres = 0;
1155 unsigned char data;
1156
1157 if (client == NULL)
1158 {
1159 return -1;
1160 }
1161
1162 comres = bma050_i2c_read_block(client, BMA250_BANDWIDTH__REG, &data, 1);
1163 data = BMA250_GET_BITSLICE(data, BMA250_BANDWIDTH);
1164
1165 if (data < 0x08) //7.81Hz
1166 {
1167 *bandwidth = 0x08;
1168 }
1169 else if (data > 0x0f) // 1000Hz
1170 {
1171 *bandwidth = 0x0f;
1172 }
1173 else
1174 {
1175 *bandwidth = data;
1176 }
1177 return comres;
1178 }
1179 /*----------------------------------------------------------------------------*/
1180
1181 static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
1182 {
1183 struct i2c_client *client = bma250_i2c_client;
1184 char strbuf[BMA250_BUFSIZE];
1185 if(NULL == client)
1186 {
1187 GSE_ERR("i2c client is null!!\n");
1188 return 0;
1189 }
1190
1191 BMA250_ReadChipInfo(client, strbuf, BMA250_BUFSIZE);
1192 return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
1193 }
1194
1195 /*
1196 static ssize_t gsensor_init(struct device_driver *ddri, char *buf, size_t count)
1197 {
1198 struct i2c_client *client = bma250_i2c_client;
1199 char strbuf[BMA250_BUFSIZE];
1200
1201 if(NULL == client)
1202 {
1203 GSE_ERR("i2c client is null!!\n");
1204 return 0;
1205 }
1206 bma250_init_client(client, 1);
1207 return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
1208 }
1209 */
1210 /*----------------------------------------------------------------------------*/
1211 /*
1212 g sensor opmode for compass tilt compensation
1213 */
1214 static ssize_t show_cpsopmode_value(struct device_driver *ddri, char *buf)
1215 {
1216 unsigned char data;
1217
1218 if (bma250_get_mode(bma250_i2c_client, &data) < 0)
1219 {
1220 return sprintf(buf, "Read error\n");
1221 }
1222 else
1223 {
1224 return sprintf(buf, "%d\n", data);
1225 }
1226 }
1227
1228 /*----------------------------------------------------------------------------*/
1229 /*
1230 g sensor opmode for compass tilt compensation
1231 */
1232 static ssize_t store_cpsopmode_value(struct device_driver *ddri, const char *buf, size_t count)
1233 {
1234 unsigned long data;
1235 int error;
1236
1237 if ((error = strict_strtoul(buf, 10, &data)))
1238 {
1239 return error;
1240 }
1241 if (data == BMA250_MODE_NORMAL)
1242 {
1243 BMA250_SetPowerMode(bma250_i2c_client, true);
1244 }
1245 else if (data == BMA250_MODE_SUSPEND)
1246 {
1247 BMA250_SetPowerMode(bma250_i2c_client, false);
1248 }
1249 else if (bma250_set_mode(bma250_i2c_client, (unsigned char) data) < 0)
1250 {
1251 GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
1252 }
1253
1254 return count;
1255 }
1256
1257 /*----------------------------------------------------------------------------*/
1258 /*
1259 g sensor range for compass tilt compensation
1260 */
1261 static ssize_t show_cpsrange_value(struct device_driver *ddri, char *buf)
1262 {
1263 unsigned char data;
1264
1265 if (bma250_get_range(bma250_i2c_client, &data) < 0)
1266 {
1267 return sprintf(buf, "Read error\n");
1268 }
1269 else
1270 {
1271 return sprintf(buf, "%d\n", data);
1272 }
1273 }
1274
1275 /*----------------------------------------------------------------------------*/
1276 /*
1277 g sensor range for compass tilt compensation
1278 */
1279 static ssize_t store_cpsrange_value(struct device_driver *ddri, const char *buf, size_t count)
1280 {
1281 unsigned long data;
1282 int error;
1283
1284 if ((error = strict_strtoul(buf, 10, &data)))
1285 {
1286 return error;
1287 }
1288 if (bma250_set_range(bma250_i2c_client, (unsigned char) data) < 0)
1289 {
1290 GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
1291 }
1292
1293 return count;
1294 }
1295 /*----------------------------------------------------------------------------*/
1296 /*
1297 g sensor bandwidth for compass tilt compensation
1298 */
1299 static ssize_t show_cpsbandwidth_value(struct device_driver *ddri, char *buf)
1300 {
1301 unsigned char data;
1302
1303 if (bma250_get_bandwidth(bma250_i2c_client, &data) < 0)
1304 {
1305 return sprintf(buf, "Read error\n");
1306 }
1307 else
1308 {
1309 return sprintf(buf, "%d\n", data);
1310 }
1311 }
1312
1313 /*----------------------------------------------------------------------------*/
1314 /*
1315 g sensor bandwidth for compass tilt compensation
1316 */
1317 static ssize_t store_cpsbandwidth_value(struct device_driver *ddri, const char *buf, size_t count)
1318 {
1319 unsigned long data;
1320 int error;
1321
1322 if ((error = strict_strtoul(buf, 10, &data)))
1323 {
1324 return error;
1325 }
1326 if (bma250_set_bandwidth(bma250_i2c_client, (unsigned char) data) < 0)
1327 {
1328 GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
1329 }
1330
1331 return count;
1332 }
1333
1334 /*----------------------------------------------------------------------------*/
1335 /*
1336 g sensor data for compass tilt compensation
1337 */
1338 static ssize_t show_cpsdata_value(struct device_driver *ddri, char *buf)
1339 {
1340 struct i2c_client *client = bma250_i2c_client;
1341 char strbuf[BMA250_BUFSIZE];
1342
1343 if(NULL == client)
1344 {
1345 GSE_ERR("i2c client is null!!\n");
1346 return 0;
1347 }
1348 BMA250_CompassReadData(client, strbuf, BMA250_BUFSIZE);
1349 return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
1350 }
1351
1352 /*----------------------------------------------------------------------------*/
1353 static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
1354 {
1355 struct i2c_client *client = bma250_i2c_client;
1356 char strbuf[BMA250_BUFSIZE];
1357
1358 if(NULL == client)
1359 {
1360 GSE_ERR("i2c client is null!!\n");
1361 return 0;
1362 }
1363 BMA250_ReadSensorData(client, strbuf, BMA250_BUFSIZE);
1364 //BMA250_ReadRawData(client, strbuf);
1365 return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
1366 }
1367
1368 /*
1369 static ssize_t show_sensorrawdata_value(struct device_driver *ddri, char *buf, size_t count)
1370 {
1371 struct i2c_client *client = bma250_i2c_client;
1372 char strbuf[BMA250_BUFSIZE];
1373
1374 if(NULL == client)
1375 {
1376 GSE_ERR("i2c client is null!!\n");
1377 return 0;
1378 }
1379 //BMA250_ReadSensorData(client, strbuf, BMA250_BUFSIZE);
1380 BMA250_ReadRawData(client, strbuf);
1381 return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
1382 }
1383 */
1384 /*----------------------------------------------------------------------------*/
1385 static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
1386 {
1387 struct i2c_client *client = bma250_i2c_client;
1388 struct bma250_i2c_data *obj;
1389 int err, len = 0, mul;
1390 int tmp[BMA250_AXES_NUM];
1391
1392 if(NULL == client)
1393 {
1394 GSE_ERR("i2c client is null!!\n");
1395 return 0;
1396 }
1397
1398 obj = i2c_get_clientdata(client);
1399
1400
1401
1402 if((err = BMA250_ReadOffset(client, obj->offset)))
1403 {
1404 return -EINVAL;
1405 }
1406 else if((err = BMA250_ReadCalibration(client, tmp)))
1407 {
1408 return -EINVAL;
1409 }
1410 else
1411 {
1412 mul = obj->reso->sensitivity/bma250_offset_resolution.sensitivity;
1413 len += snprintf(buf+len, PAGE_SIZE-len, "[HW ][%d] (%+3d, %+3d, %+3d) : (0x%02X, 0x%02X, 0x%02X)\n", mul,
1414 obj->offset[BMA250_AXIS_X], obj->offset[BMA250_AXIS_Y], obj->offset[BMA250_AXIS_Z],
1415 obj->offset[BMA250_AXIS_X], obj->offset[BMA250_AXIS_Y], obj->offset[BMA250_AXIS_Z]);
1416 len += snprintf(buf+len, PAGE_SIZE-len, "[SW ][%d] (%+3d, %+3d, %+3d)\n", 1,
1417 obj->cali_sw[BMA250_AXIS_X], obj->cali_sw[BMA250_AXIS_Y], obj->cali_sw[BMA250_AXIS_Z]);
1418
1419 len += snprintf(buf+len, PAGE_SIZE-len, "[ALL] (%+3d, %+3d, %+3d) : (%+3d, %+3d, %+3d)\n",
1420 obj->offset[BMA250_AXIS_X]*mul + obj->cali_sw[BMA250_AXIS_X],
1421 obj->offset[BMA250_AXIS_Y]*mul + obj->cali_sw[BMA250_AXIS_Y],
1422 obj->offset[BMA250_AXIS_Z]*mul + obj->cali_sw[BMA250_AXIS_Z],
1423 tmp[BMA250_AXIS_X], tmp[BMA250_AXIS_Y], tmp[BMA250_AXIS_Z]);
1424
1425 return len;
1426 }
1427 }
1428 /*----------------------------------------------------------------------------*/
1429 static ssize_t store_cali_value(struct device_driver *ddri, const char *buf, size_t count)
1430 {
1431 struct i2c_client *client = bma250_i2c_client;
1432 int err, x, y, z;
1433 int dat[BMA250_AXES_NUM];
1434
1435 if(!strncmp(buf, "rst", 3))
1436 {
1437 if((err = BMA250_ResetCalibration(client)))
1438 {
1439 GSE_ERR("reset offset err = %d\n", err);
1440 }
1441 }
1442 else if(3 == sscanf(buf, "0x%02X 0x%02X 0x%02X", &x, &y, &z))
1443 {
1444 dat[BMA250_AXIS_X] = x;
1445 dat[BMA250_AXIS_Y] = y;
1446 dat[BMA250_AXIS_Z] = z;
1447 if((err = BMA250_WriteCalibration(client, dat)))
1448 {
1449 GSE_ERR("write calibration err = %d\n", err);
1450 }
1451 }
1452 else
1453 {
1454 GSE_ERR("invalid format\n");
1455 }
1456
1457 return count;
1458 }
1459
1460
1461 /*----------------------------------------------------------------------------*/
1462 static ssize_t show_firlen_value(struct device_driver *ddri, char *buf)
1463 {
1464 #ifdef CONFIG_BMA250_LOWPASS
1465 struct i2c_client *client = bma250_i2c_client;
1466 struct bma250_i2c_data *obj = i2c_get_clientdata(client);
1467 if(atomic_read(&obj->firlen))
1468 {
1469 int idx, len = atomic_read(&obj->firlen);
1470 GSE_LOG("len = %2d, idx = %2d\n", obj->fir.num, obj->fir.idx);
1471
1472 for(idx = 0; idx < len; idx++)
1473 {
1474 GSE_LOG("[%5d %5d %5d]\n", obj->fir.raw[idx][BMA250_AXIS_X], obj->fir.raw[idx][BMA250_AXIS_Y], obj->fir.raw[idx][BMA250_AXIS_Z]);
1475 }
1476
1477 GSE_LOG("sum = [%5d %5d %5d]\n", obj->fir.sum[BMA250_AXIS_X], obj->fir.sum[BMA250_AXIS_Y], obj->fir.sum[BMA250_AXIS_Z]);
1478 GSE_LOG("avg = [%5d %5d %5d]\n", obj->fir.sum[BMA250_AXIS_X]/len, obj->fir.sum[BMA250_AXIS_Y]/len, obj->fir.sum[BMA250_AXIS_Z]/len);
1479 }
1480 return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->firlen));
1481 #else
1482 return snprintf(buf, PAGE_SIZE, "not support\n");
1483 #endif
1484 }
1485 /*----------------------------------------------------------------------------*/
1486 static ssize_t store_firlen_value(struct device_driver *ddri, const char *buf, size_t count)
1487 {
1488 #ifdef CONFIG_BMA250_LOWPASS
1489 struct i2c_client *client = bma250_i2c_client;
1490 struct bma250_i2c_data *obj = i2c_get_clientdata(client);
1491 int firlen;
1492
1493 if(1 != sscanf(buf, "%d", &firlen))
1494 {
1495 GSE_ERR("invallid format\n");
1496 }
1497 else if(firlen > C_MAX_FIR_LENGTH)
1498 {
1499 GSE_ERR("exceeds maximum filter length\n");
1500 }
1501 else
1502 {
1503 atomic_set(&obj->firlen, firlen);
1504 if(NULL == firlen)
1505 {
1506 atomic_set(&obj->fir_en, 0);
1507 }
1508 else
1509 {
1510 memset(&obj->fir, 0x00, sizeof(obj->fir));
1511 atomic_set(&obj->fir_en, 1);
1512 }
1513 }
1514 #endif
1515 return count;
1516 }
1517 /*----------------------------------------------------------------------------*/
1518 static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
1519 {
1520 ssize_t res;
1521 struct bma250_i2c_data *obj = obj_i2c_data;
1522 if (obj == NULL)
1523 {
1524 GSE_ERR("i2c_data obj is null!!\n");
1525 return 0;
1526 }
1527
1528 res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));
1529 return res;
1530 }
1531 /*----------------------------------------------------------------------------*/
1532 static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
1533 {
1534 struct bma250_i2c_data *obj = obj_i2c_data;
1535 int trace;
1536 if (obj == NULL)
1537 {
1538 GSE_ERR("i2c_data obj is null!!\n");
1539 return 0;
1540 }
1541
1542 if(1 == sscanf(buf, "0x%x", &trace))
1543 {
1544 atomic_set(&obj->trace, trace);
1545 }
1546 else
1547 {
1548 GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
1549 }
1550
1551 return count;
1552 }
1553 /*----------------------------------------------------------------------------*/
1554 static ssize_t show_status_value(struct device_driver *ddri, char *buf)
1555 {
1556 ssize_t len = 0;
1557 struct bma250_i2c_data *obj = obj_i2c_data;
1558 if (obj == NULL)
1559 {
1560 GSE_ERR("i2c_data obj is null!!\n");
1561 return 0;
1562 }
1563
1564 if(obj->hw)
1565 {
1566 len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n",
1567 obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);
1568 }
1569 else
1570 {
1571 len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
1572 }
1573 return len;
1574 }
1575 /*----------------------------------------------------------------------------*/
1576 static ssize_t show_power_status_value(struct device_driver *ddri, char *buf)
1577 {
1578 if(sensor_power)
1579 printk("G sensor is in work mode, sensor_power = %d\n", sensor_power);
1580 else
1581 printk("G sensor is in standby mode, sensor_power = %d\n", sensor_power);
1582
1583 return 0;
1584 }
1585 /*----------------------------------------------------------------------------*/
1586 static DRIVER_ATTR(chipinfo, S_IWUSR | S_IRUGO, show_chipinfo_value, NULL);
1587 static DRIVER_ATTR(cpsdata, S_IWUSR | S_IRUGO, show_cpsdata_value, NULL);
1588 static DRIVER_ATTR(cpsopmode, S_IRUGO|S_IWUSR, show_cpsopmode_value, store_cpsopmode_value);
1589 static DRIVER_ATTR(cpsrange, S_IRUGO|S_IWUSR, show_cpsrange_value, store_cpsrange_value);
1590 static DRIVER_ATTR(cpsbandwidth, S_IRUGO|S_IWUSR, show_cpsbandwidth_value, store_cpsbandwidth_value);
1591 static DRIVER_ATTR(sensordata, S_IWUSR | S_IRUGO, show_sensordata_value, NULL);
1592 static DRIVER_ATTR(cali, S_IWUSR | S_IRUGO, show_cali_value, store_cali_value);
1593 static DRIVER_ATTR(firlen, S_IWUSR | S_IRUGO, show_firlen_value, store_firlen_value);
1594 static DRIVER_ATTR(trace, S_IWUSR | S_IRUGO, show_trace_value, store_trace_value);
1595 static DRIVER_ATTR(status, S_IRUGO, show_status_value, NULL);
1596 static DRIVER_ATTR(powerstatus, S_IRUGO, show_power_status_value, NULL);
1597
1598 /*----------------------------------------------------------------------------*/
1599 static struct driver_attribute *bma250_attr_list[] = {
1600 &driver_attr_chipinfo, /*chip information*/
1601 &driver_attr_sensordata, /*dump sensor data*/
1602 &driver_attr_cali, /*show calibration data*/
1603 &driver_attr_firlen, /*filter length: 0: disable, others: enable*/
1604 &driver_attr_trace, /*trace log*/
1605 &driver_attr_status,
1606 &driver_attr_powerstatus,
1607 &driver_attr_cpsdata, /*g sensor data for compass tilt compensation*/
1608 &driver_attr_cpsopmode, /*g sensor opmode for compass tilt compensation*/
1609 &driver_attr_cpsrange, /*g sensor range for compass tilt compensation*/
1610 &driver_attr_cpsbandwidth, /*g sensor bandwidth for compass tilt compensation*/
1611 };
1612 /*----------------------------------------------------------------------------*/
1613 static int bma250_create_attr(struct device_driver *driver)
1614 {
1615 int idx, err = 0;
1616 int num = (int)(sizeof(bma250_attr_list)/sizeof(bma250_attr_list[0]));
1617 if (driver == NULL)
1618 {
1619 return -EINVAL;
1620 }
1621
1622 for(idx = 0; idx < num; idx++)
1623 {
1624 if((err = driver_create_file(driver, bma250_attr_list[idx])))
1625 {
1626 GSE_ERR("driver_create_file (%s) = %d\n", bma250_attr_list[idx]->attr.name, err);
1627 break;
1628 }
1629 }
1630 return err;
1631 }
1632 /*----------------------------------------------------------------------------*/
1633 static int bma250_delete_attr(struct device_driver *driver)
1634 {
1635 int idx ,err = 0;
1636 int num = (int)(sizeof(bma250_attr_list)/sizeof(bma250_attr_list[0]));
1637
1638 if(driver == NULL)
1639 {
1640 return -EINVAL;
1641 }
1642
1643
1644 for(idx = 0; idx < num; idx++)
1645 {
1646 driver_remove_file(driver, bma250_attr_list[idx]);
1647 }
1648
1649
1650 return err;
1651 }
1652
1653 /*----------------------------------------------------------------------------*/
1654 int gsensor_operate(void* self, uint32_t command, void* buff_in, int size_in,
1655 void* buff_out, int size_out, int* actualout)
1656 {
1657 int err = 0;
1658 int value, sample_delay;
1659 struct bma250_i2c_data *priv = (struct bma250_i2c_data*)self;
1660 hwm_sensor_data* gsensor_data;
1661 char buff[BMA250_BUFSIZE];
1662
1663 //GSE_FUN(f);
1664 switch (command)
1665 {
1666 case SENSOR_DELAY:
1667 if((buff_in == NULL) || (size_in < sizeof(int)))
1668 {
1669 GSE_ERR("Set delay parameter error!\n");
1670 err = -EINVAL;
1671 }
1672 else
1673 {
1674 value = *(int *)buff_in;
1675 if(value <= 5)
1676 {
1677 sample_delay = BMA250_BW_200HZ;
1678 }
1679 else if(value <= 10)
1680 {
1681 sample_delay = BMA250_BW_100HZ;
1682 }
1683 else
1684 {
1685 sample_delay = BMA250_BW_50HZ;
1686 }
1687
1688 err = BMA250_SetBWRate(priv->client, sample_delay);
1689 if(err != BMA250_SUCCESS ) //0x2C->BW=100Hz
1690 {
1691 GSE_ERR("Set delay parameter error!\n");
1692 }
1693
1694 if(value >= 50)
1695 {
1696 atomic_set(&priv->filter, 0);
1697 }
1698 else
1699 {
1700 #if defined(CONFIG_BMA250_LOWPASS)
1701 priv->fir.num = 0;
1702 priv->fir.idx = 0;
1703 priv->fir.sum[BMA250_AXIS_X] = 0;
1704 priv->fir.sum[BMA250_AXIS_Y] = 0;
1705 priv->fir.sum[BMA250_AXIS_Z] = 0;
1706 atomic_set(&priv->filter, 1);
1707 #endif
1708 }
1709 }
1710 break;
1711
1712 case SENSOR_ENABLE:
1713 if((buff_in == NULL) || (size_in < sizeof(int)))
1714 {
1715 GSE_ERR("Enable sensor parameter error!\n");
1716 err = -EINVAL;
1717 }
1718 else
1719 {
1720 value = *(int *)buff_in;
1721 mutex_lock(&bma050_op_mutex);
1722 GSE_LOG("Gsensor enable_status value = %d,sensor_power=%d\n",value,sensor_power);
1723 if(((value == 0) && (sensor_power == false)) ||((value == 1) && (sensor_power == true)))
1724 {
1725 GSE_LOG("Gsensor device have updated!\n");
1726 enable_status = sensor_power;
1727 }
1728 else
1729 {
1730 enable_status = !sensor_power;
1731 err = BMA250_SetPowerMode( priv->client, !sensor_power);
1732
1733 }
1734 GSE_LOG("Gsensor enable_status = %d\n",enable_status);
1735 mutex_unlock(&bma050_op_mutex);
1736 }
1737 break;
1738
1739 case SENSOR_GET_DATA:
1740 if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
1741 {
1742 GSE_ERR("get sensor data parameter error!\n");
1743 err = -EINVAL;
1744 }
1745 else
1746 {
1747 gsensor_data = (hwm_sensor_data *)buff_out;
1748 BMA250_ReadSensorData(priv->client, buff, BMA250_BUFSIZE);
1749 sscanf(buff, "%x %x %x", &gsensor_data->values[0],
1750 &gsensor_data->values[1], &gsensor_data->values[2]);
1751 gsensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;
1752 gsensor_data->value_divide = 1000;
1753 }
1754 break;
1755 default:
1756 GSE_ERR("gsensor operate function no this parameter %d!\n", command);
1757 err = -1;
1758 break;
1759 }
1760
1761 return err;
1762 }
1763
1764 /******************************************************************************
1765 * Function Configuration
1766 ******************************************************************************/
1767 static int bma250_open(struct inode *inode, struct file *file)
1768 {
1769 file->private_data = bma250_i2c_client;
1770
1771 if(file->private_data == NULL)
1772 {
1773 GSE_ERR("null pointer!!\n");
1774 return -EINVAL;
1775 }
1776 return nonseekable_open(inode, file);
1777 }
1778 /*----------------------------------------------------------------------------*/
1779 static int bma250_release(struct inode *inode, struct file *file)
1780 {
1781 file->private_data = NULL;
1782 return 0;
1783 }
1784 /*----------------------------------------------------------------------------*/
1785 //static int bma250_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
1786 // unsigned long arg)
1787 static long bma250_unlocked_ioctl(struct file *file, unsigned int cmd,unsigned long arg)
1788 {
1789 struct i2c_client *client = (struct i2c_client*)file->private_data;
1790 struct bma250_i2c_data *obj = (struct bma250_i2c_data*)i2c_get_clientdata(client);
1791 char strbuf[BMA250_BUFSIZE];
1792 void __user *data;
1793 SENSOR_DATA sensor_data;
1794 long err = 0;
1795 int cali[3];
1796
1797 //GSE_FUN(f);
1798 if(_IOC_DIR(cmd) & _IOC_READ)
1799 {
1800 err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
1801 }
1802 else if(_IOC_DIR(cmd) & _IOC_WRITE)
1803 {
1804 err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
1805 }
1806
1807 if(err)
1808 {
1809 GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
1810 return -EFAULT;
1811 }
1812
1813 switch(cmd)
1814 {
1815 case GSENSOR_IOCTL_INIT:
1816 bma250_init_client(client, 0);
1817 break;
1818
1819 case GSENSOR_IOCTL_READ_CHIPINFO:
1820 data = (void __user *) arg;
1821 if(data == NULL)
1822 {
1823 err = -EINVAL;
1824 break;
1825 }
1826
1827 BMA250_ReadChipInfo(client, strbuf, BMA250_BUFSIZE);
1828 if(copy_to_user(data, strbuf, strlen(strbuf)+1))
1829 {
1830 err = -EFAULT;
1831 break;
1832 }
1833 break;
1834
1835 case GSENSOR_IOCTL_READ_SENSORDATA:
1836 data = (void __user *) arg;
1837 if(data == NULL)
1838 {
1839 err = -EINVAL;
1840 break;
1841 }
1842 BMA250_SetPowerMode(client,true);
1843 BMA250_ReadSensorData(client, strbuf, BMA250_BUFSIZE);
1844 if(copy_to_user(data, strbuf, strlen(strbuf)+1))
1845 {
1846 err = -EFAULT;
1847 break;
1848 }
1849 break;
1850
1851 case GSENSOR_IOCTL_READ_GAIN:
1852 data = (void __user *) arg;
1853 if(data == NULL)
1854 {
1855 err = -EINVAL;
1856 break;
1857 }
1858
1859 if(copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
1860 {
1861 err = -EFAULT;
1862 break;
1863 }
1864 break;
1865
1866 case GSENSOR_IOCTL_READ_RAW_DATA:
1867 data = (void __user *) arg;
1868 if(data == NULL)
1869 {
1870 err = -EINVAL;
1871 break;
1872 }
1873 BMA250_ReadRawData(client, strbuf);
1874 if(copy_to_user(data, &strbuf, strlen(strbuf)+1))
1875 {
1876 err = -EFAULT;
1877 break;
1878 }
1879 break;
1880
1881 case GSENSOR_IOCTL_SET_CALI:
1882 data = (void __user*)arg;
1883 if(data == NULL)
1884 {
1885 err = -EINVAL;
1886 break;
1887 }
1888 if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
1889 {
1890 err = -EFAULT;
1891 break;
1892 }
1893 if(atomic_read(&obj->suspend))
1894 {
1895 GSE_ERR("Perform calibration in suspend state!!\n");
1896 err = -EINVAL;
1897 }
1898 else
1899 {
1900 #if 0
1901 cali[BMA250_AXIS_X] = sensor_data.x * obj->reso->sensitivity / GRAVITY_EARTH_1000;
1902 cali[BMA250_AXIS_Y] = sensor_data.y * obj->reso->sensitivity / GRAVITY_EARTH_1000;
1903 cali[BMA250_AXIS_Z] = sensor_data.z * obj->reso->sensitivity / GRAVITY_EARTH_1000;
1904 #else
1905 cali[BMA250_AXIS_X] = sensor_data.x;
1906 cali[BMA250_AXIS_Y] = sensor_data.y;
1907 cali[BMA250_AXIS_Z] = sensor_data.z;
1908
1909
1910 #endif
1911 err = BMA250_WriteCalibration(client, cali);
1912 }
1913 break;
1914
1915 case GSENSOR_IOCTL_CLR_CALI:
1916 err = BMA250_ResetCalibration(client);
1917 break;
1918
1919 case GSENSOR_IOCTL_GET_CALI:
1920 data = (void __user*)arg;
1921 if(data == NULL)
1922 {
1923 err = -EINVAL;
1924 break;
1925 }
1926 if((err = BMA250_ReadCalibration(client, cali)))
1927 {
1928 break;
1929 }
1930 #if 0
1931 sensor_data.x = cali[BMA250_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
1932 sensor_data.y = cali[BMA250_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
1933 sensor_data.z = cali[BMA250_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
1934 #else
1935 sensor_data.x = cali[BMA250_AXIS_X];
1936 sensor_data.y = cali[BMA250_AXIS_Y];
1937 sensor_data.z = cali[BMA250_AXIS_Z];
1938
1939 #endif
1940 if(copy_to_user(data, &sensor_data, sizeof(sensor_data)))
1941 {
1942 err = -EFAULT;
1943 break;
1944 }
1945 break;
1946
1947
1948 default:
1949 GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
1950 err = -ENOIOCTLCMD;
1951 break;
1952
1953 }
1954
1955 return err;
1956 }
1957
1958
1959 /*----------------------------------------------------------------------------*/
1960 static struct file_operations bma250_fops = {
1961 //.owner = THIS_MODULE,
1962 .open = bma250_open,
1963 .release = bma250_release,
1964 .unlocked_ioctl = bma250_unlocked_ioctl,
1965 };
1966 /*----------------------------------------------------------------------------*/
1967 static struct miscdevice bma250_device = {
1968 .minor = MISC_DYNAMIC_MINOR,
1969 .name = "gsensor",
1970 .fops = &bma250_fops,
1971 };
1972 /*----------------------------------------------------------------------------*/
1973 #ifndef USE_EARLY_SUSPEND
1974 /*----------------------------------------------------------------------------*/
1975 static int bma250_suspend(struct i2c_client *client, pm_message_t msg)
1976 {
1977 struct bma250_i2c_data *obj = i2c_get_clientdata(client);
1978 int err = 0;
1979 GSE_FUN();
1980 mutex_lock(&bma050_op_mutex);
1981 if(msg.event == PM_EVENT_SUSPEND)
1982 {
1983 if(obj == NULL)
1984 {
1985 GSE_ERR("null pointer!!\n");
1986 mutex_unlock(&bma050_op_mutex);
1987 return -EINVAL;
1988 }
1989 atomic_set(&obj->suspend, 1);
1990 if((err = BMA250_SetPowerMode(obj->client, false)))
1991 {
1992 GSE_ERR("write power control fail!!\n");
1993 mutex_unlock(&bma050_op_mutex);
1994 return -EINVAL;
1995 }
1996 sensor_power = false;
1997 BMA250_power(obj->hw, 0);
1998 }
1999 mutex_unlock(&bma050_op_mutex);
2000 return err;
2001 }
2002 /*----------------------------------------------------------------------------*/
2003 static int bma250_resume(struct i2c_client *client)
2004 {
2005 struct bma250_i2c_data *obj = i2c_get_clientdata(client);
2006 int err;
2007 GSE_FUN();
2008 udelay(500);//for fix resume check device id error
2009 if(obj == NULL)
2010 {
2011 GSE_ERR("null pointer!!\n");
2012 return -EINVAL;
2013 }
2014 mutex_lock(&bma050_op_mutex);
2015 BMA250_power(obj->hw, 1);
2016 if((err = bma250_init_client(client, 0)))
2017 {
2018 GSE_ERR("initialize client fail!!\n");
2019 mutex_unlock(&bma050_op_mutex);
2020 return err;
2021 }
2022 atomic_set(&obj->suspend, 0);
2023 mutex_unlock(&bma050_op_mutex);
2024 return 0;
2025 }
2026 /*----------------------------------------------------------------------------*/
2027 #else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
2028 /*----------------------------------------------------------------------------*/
2029 static void bma250_early_suspend(struct early_suspend *h)
2030 {
2031 struct bma250_i2c_data *obj = container_of(h, struct bma250_i2c_data, early_drv);
2032 int err;
2033 GSE_FUN();
2034
2035 if(obj == NULL)
2036 {
2037 GSE_ERR("null pointer!!\n");
2038 return;
2039 }
2040 atomic_set(&obj->suspend, 1);
2041 if((err = BMA250_SetPowerMode(obj->client, false)))
2042 {
2043 GSE_ERR("write power control fail!!\n");
2044 return;
2045 }
2046
2047 sensor_power = false;
2048
2049 BMA250_power(obj->hw, 0);
2050 }
2051 /*----------------------------------------------------------------------------*/
2052 static void bma250_late_resume(struct early_suspend *h)
2053 {
2054 struct bma250_i2c_data *obj = container_of(h, struct bma250_i2c_data, early_drv);
2055 int err;
2056 GSE_FUN();
2057
2058 if(obj == NULL)
2059 {
2060 GSE_ERR("null pointer!!\n");
2061 return;
2062 }
2063
2064 BMA250_power(obj->hw, 1);
2065 if((err = bma250_init_client(obj->client, 0)))
2066 {
2067 GSE_ERR("initialize client fail!!\n");
2068 return;
2069 }
2070 atomic_set(&obj->suspend, 0);
2071 }
2072 /*----------------------------------------------------------------------------*/
2073 #endif /*USE_EARLY_SUSPEND*/
2074 /*----------------------------------------------------------------------------*/
2075 //static int bma250_i2c_detect(struct i2c_client *client, int kind, struct i2c_board_info *info)
2076 //{
2077 // strcpy(info->type, BMA250_DEV_NAME);
2078 // return 0;
2079 //}
2080
2081 /*----------------------------------------------------------------------------*/
2082 static int bma250_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
2083 {
2084 struct i2c_client *new_client;
2085 struct bma250_i2c_data *obj;
2086 struct hwmsen_object sobj;
2087 int err = 0;
2088 int retry = 0;
2089 GSE_FUN();
2090
2091 if(!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
2092 {
2093 err = -ENOMEM;
2094 goto exit;
2095 }
2096
2097 memset(obj, 0, sizeof(struct bma250_i2c_data));
2098
2099 obj->hw = get_cust_acc_hw();
2100
2101 if((err = hwmsen_get_convert(obj->hw->direction, &obj->cvt)))
2102 {
2103 GSE_ERR("invalid direction: %d\n", obj->hw->direction);
2104 goto exit;
2105 }
2106
2107 obj_i2c_data = obj;
2108 obj->client = client;
2109 new_client = obj->client;
2110 i2c_set_clientdata(new_client,obj);
2111
2112 atomic_set(&obj->trace, 0);
2113 atomic_set(&obj->suspend, 0);
2114
2115 #ifdef CONFIG_BMA250_LOWPASS
2116 if(obj->hw->firlen > C_MAX_FIR_LENGTH)
2117 {
2118 atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
2119 }
2120 else
2121 {
2122 atomic_set(&obj->firlen, obj->hw->firlen);
2123 }
2124
2125 if(atomic_read(&obj->firlen) > 0)
2126 {
2127 atomic_set(&obj->fir_en, 1);
2128 }
2129
2130 #endif
2131
2132 bma250_i2c_client = new_client;
2133
2134 for(retry = 0; retry < 3; retry++){
2135 err = bma250_init_client(new_client, 1);
2136 if(err == 0)
2137 {
2138 GSE_LOG("init client done\n");
2139 break;
2140 }
2141 GSE_LOG("init client fail\n");
2142 }
2143
2144 if(err != 0)
2145 goto exit_init_failed;
2146
2147 if((err = misc_register(&bma250_device)))
2148 {
2149 GSE_ERR("bma250_device register failed\n");
2150 goto exit_misc_device_register_failed;
2151 }
2152
2153 if((err = bma250_create_attr(&bma250_gsensor_driver.driver)))
2154 {
2155 GSE_ERR("create attribute err = %d\n", err);
2156 goto exit_create_attr_failed;
2157 }
2158
2159 sobj.self = obj;
2160 sobj.polling = 1;
2161 sobj.sensor_operate = gsensor_operate;
2162 if((err = hwmsen_attach(ID_ACCELEROMETER, &sobj)))
2163 {
2164 GSE_ERR("attach fail = %d\n", err);
2165 goto exit_kfree;
2166 }
2167
2168 #ifdef USE_EARLY_SUSPEND
2169 obj->early_drv.level = EARLY_SUSPEND_LEVEL_DISABLE_FB - 1,
2170 obj->early_drv.suspend = bma250_early_suspend,
2171 obj->early_drv.resume = bma250_late_resume,
2172 register_early_suspend(&obj->early_drv);
2173 #endif
2174
2175 GSE_LOG("%s: OK\n", __func__);
2176 return 0;
2177
2178 exit_create_attr_failed:
2179 misc_deregister(&bma250_device);
2180 exit_misc_device_register_failed:
2181 exit_init_failed:
2182 //i2c_detach_client(new_client);
2183 exit_kfree:
2184 kfree(obj);
2185 exit:
2186 GSE_ERR("%s: err = %d\n", __func__, err);
2187 return err;
2188 }
2189
2190 /*----------------------------------------------------------------------------*/
2191 static int bma250_i2c_remove(struct i2c_client *client)
2192 {
2193 int err = 0;
2194
2195 if((err = bma250_delete_attr(&bma250_gsensor_driver.driver)))
2196 {
2197 GSE_ERR("bma150_delete_attr fail: %d\n", err);
2198 }
2199
2200 if((err = misc_deregister(&bma250_device)))
2201 {
2202 GSE_ERR("misc_deregister fail: %d\n", err);
2203 }
2204
2205 if((err = hwmsen_detach(ID_ACCELEROMETER)))
2206
2207
2208 bma250_i2c_client = NULL;
2209 i2c_unregister_device(client);
2210 kfree(i2c_get_clientdata(client));
2211 return 0;
2212 }
2213 /*----------------------------------------------------------------------------*/
2214 static int bma250_probe(struct platform_device *pdev)
2215 {
2216 struct acc_hw *hw = get_cust_acc_hw();
2217 GSE_FUN();
2218
2219 BMA250_power(hw, 1);
2220 //bma250_force[0] = hw->i2c_num;
2221 if(i2c_add_driver(&bma250_i2c_driver))
2222 {
2223 GSE_ERR("add driver error\n");
2224 return -1;
2225 }
2226 return 0;
2227 }
2228 /*----------------------------------------------------------------------------*/
2229 static int bma250_remove(struct platform_device *pdev)
2230 {
2231 struct acc_hw *hw = get_cust_acc_hw();
2232
2233 GSE_FUN();
2234 BMA250_power(hw, 0);
2235 i2c_del_driver(&bma250_i2c_driver);
2236 return 0;
2237 }
2238 /*----------------------------------------------------------------------------*/
2239
2240 #ifdef CONFIG_OF
2241 static const struct of_device_id gsensor_of_match[] = {
2242 { .compatible = "mediatek,gsensor", },
2243 {},
2244 };
2245 #endif
2246
2247 static struct platform_driver bma250_gsensor_driver = {
2248 .probe = bma250_probe,
2249 .remove = bma250_remove,
2250 .driver =
2251 {
2252 .name = "gsensor",
2253 #ifdef CONFIG_OF
2254 .of_match_table = gsensor_of_match,
2255 #endif
2256 }
2257 };
2258
2259 /*----------------------------------------------------------------------------*/
2260 static int __init bma250_init(void)
2261 {
2262 //GSE_FUN();
2263 struct acc_hw *hw = get_cust_acc_hw();
2264 GSE_LOG("%s: i2c_number=%d\n", __func__,hw->i2c_num);
2265 i2c_register_board_info(hw->i2c_num, &i2c_bma250, 1);
2266 if(platform_driver_register(&bma250_gsensor_driver))
2267 {
2268 GSE_ERR("failed to register driver");
2269 return -ENODEV;
2270 }
2271 return 0;
2272 }
2273 /*----------------------------------------------------------------------------*/
2274 static void __exit bma250_exit(void)
2275 {
2276 GSE_FUN();
2277 platform_driver_unregister(&bma250_gsensor_driver);
2278 }
2279 /*----------------------------------------------------------------------------*/
2280 module_init(bma250_init);
2281 module_exit(bma250_exit);
2282 /*----------------------------------------------------------------------------*/
2283 MODULE_LICENSE("GPL");
2284 MODULE_DESCRIPTION("BMA250 I2C driver");
2285 MODULE_AUTHOR("hongji.zhou@bosch-sensortec.com");
kernel source for telekom puls (alcatel/mediatek)