2 * Copyright (C) 2012 Senodia.
4 * Author: Tori Xu <xuezhi_xu@senodia.com>
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.
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.
16 #include <linux/st480.h>
17 #include <linux/module.h>
18 #include <linux/of_device.h>
21 struct i2c_client *client;
22 struct input_dev *input_dev;
23 struct st480_platform_data *pdata;
24 struct delayed_work work;
27 static struct st480_data *st480;
29 static atomic_t m_flag;
30 static atomic_t mv_flag;
32 static atomic_t open_count;
33 static atomic_t open_flag;
34 static atomic_t reserve_open_flag;
36 volatile static short st480_delay = ST480_DEFAULT_DELAY;
43 volatile static struct mag_3 mag;
49 #if ST480_DATA_TRANSFER
50 static int st480_i2c_data_transfer(struct i2c_client *client, int len, char *buf, int length)
52 struct i2c_msg msgs[] = {
67 if(i2c_transfer(client->adapter, msgs, 2) < 0) {
68 pr_err("megnetic_i2c_read_data: transfer error\n");
75 #if ST480_SMBUS_READ_BYTE
76 static int st480_smbus_read_byte(struct i2c_client *client,
77 unsigned char reg_addr, unsigned char *data)
80 dummy = i2c_smbus_read_byte_data(client, reg_addr);
83 *data = dummy & 0x000000ff;
89 #if ST480_SMBUS_WRITE_BYTE
90 static int st480_smbus_write_byte(struct i2c_client *client,
91 unsigned char reg_addr, unsigned char *data)
94 dummy = i2c_smbus_write_byte_data(client, reg_addr, *data);
101 #if ST480_SMBUS_READ_BYTE_BLOCK
102 static int st480_smbus_read_byte_block(struct i2c_client *client,
103 unsigned char reg_addr, unsigned char *data, unsigned char len)
106 dummy = i2c_smbus_read_i2c_block_data(client, reg_addr, len, data);
113 #if ST480_SMBUS_WRITE_BYTE_BLOCK
114 static int st480_smbus_write_byte_block(struct i2c_client *client,
115 unsigned char reg_addr, unsigned char *data, unsigned char len)
118 dummy = i2c_smbus_write_i2c_block_data(client, reg_addr, len, data);
126 * Device detect and init
129 static int st480_setup(struct i2c_client *client)
132 unsigned char buf[5];
137 buf[0] = READ_REGISTER_CMD;
140 while(st480_i2c_data_transfer(client, 2, buf, 3)!=0) {
143 if(st480_i2c_data_transfer(client, 2, buf, 3)==0) {
146 if(ret > MAX_FAILURE_COUNT) {
151 if(buf[2] != ST480_DEVICE_ID) {
156 //init register step 1
157 buf[0] = WRITE_REGISTER_CMD;
158 buf[1] = ONE_INIT_DATA_HIGH;
159 buf[2] = ONE_INIT_DATA_LOW;
160 buf[3] = ONE_INIT_REG;
162 while(st480_i2c_data_transfer(client, 4, buf, 1)!=0) {
165 if(st480_i2c_data_transfer(client, 4, buf, 1)==0) {
168 if(ret > MAX_FAILURE_COUNT) {
173 //init register step 2
174 buf[0] = WRITE_REGISTER_CMD;
175 buf[1] = TWO_INIT_DATA_HIGH;
176 buf[2] = TWO_INIT_DATA_LOW;
177 buf[3] = TWO_INIT_REG;
179 while(st480_i2c_data_transfer(client, 4, buf, 1)!=0) {
182 if(st480_i2c_data_transfer(client, 4, buf, 1)==0) {
185 if(ret > MAX_FAILURE_COUNT) {
190 //set calibration register
191 buf[0] = WRITE_REGISTER_CMD;
192 buf[1] = CALIBRATION_DATA_HIGH;
193 buf[2] = CALIBRATION_DATA_LOW;
194 buf[3] = CALIBRATION_REG;
196 while(st480_i2c_data_transfer(client, 4, buf, 1)!=0) {
199 if(st480_i2c_data_transfer(client, 4, buf, 1)==0) {
202 if(ret > MAX_FAILURE_COUNT) {
208 buf[0] = SINGLE_MEASUREMENT_MODE_CMD;
210 while(st480_i2c_data_transfer(client, 1, buf, 1)!=0) {
213 if(st480_i2c_data_transfer(client, 1, buf, 1)==0) {
216 if(ret > MAX_FAILURE_COUNT) {
224 static void st480_work_func(void)
229 /* x,y,z hardware data */
232 memset(buffer, 0, 9);
234 buffer[0] = READ_MEASUREMENT_CMD;
236 while(st480_i2c_data_transfer(st480->client, 1, buffer, 9)!=0) {
239 if(st480_i2c_data_transfer(st480->client, 1, buffer, 9)==0) {
242 if(ret > MAX_FAILURE_COUNT) {
247 if(!((buffer[0]>>4) & 0X01)) {
248 hw_d[0] = ((buffer[3]<<8)|buffer[4]);
249 hw_d[1] = ((buffer[5]<<8)|buffer[6]);
250 hw_d[2] = ((buffer[7]<<8)|buffer[8]);
252 mag.mag_x = ((st480->pdata->negate_x) ? (-hw_d[st480->pdata->axis_map_x])
253 : (hw_d[st480->pdata->axis_map_x]));
254 mag.mag_y = ((st480->pdata->negate_y) ? (-hw_d[st480->pdata->axis_map_y])
255 : (hw_d[st480->pdata->axis_map_y]));
256 mag.mag_z = ((st480->pdata->negate_z) ? (-hw_d[st480->pdata->axis_map_z])
257 : (hw_d[st480->pdata->axis_map_z]));
259 if( ((buffer[1]<<8)|(buffer[2])) > 46244) {
260 mag.mag_x = mag.mag_x * (1 + (70/128/4096) * (((buffer[1]<<8)|(buffer[2])) - 46244));
261 mag.mag_y = mag.mag_y * (1 + (70/128/4096) * (((buffer[1]<<8)|(buffer[2])) - 46244));
262 mag.mag_z = mag.mag_z * (1 + (70/128/4096) * (((buffer[1]<<8)|(buffer[2])) - 46244));
263 } else if( ((buffer[1]<<8)|(buffer[2])) < 46244) {
264 mag.mag_x = mag.mag_x * (1 + (60/128/4096) * (((buffer[1]<<8)|(buffer[2])) - 46244));
265 mag.mag_y = mag.mag_y * (1 + (60/128/4096) * (((buffer[1]<<8)|(buffer[2])) - 46244));
266 mag.mag_z = mag.mag_z * (1 + (60/128/4096) * (((buffer[1]<<8)|(buffer[2])) - 46244));
269 SENODIADBG("st480 raw data: x = %d, y = %d, z = %d \n",mag.mag_x,mag.mag_y,mag.mag_z);
273 buffer[0] = SINGLE_MEASUREMENT_MODE_CMD;
275 while(st480_i2c_data_transfer(st480->client, 1, buffer, 1)!=0) {
278 if(st480_i2c_data_transfer(st480->client, 1, buffer, 1)==0) {
281 if(ret > MAX_FAILURE_COUNT) {
287 static void st480_input_func(struct work_struct *work)
289 SENODIAFUNC("st480_input_func");
292 if (atomic_read(&m_flag) || atomic_read(&mv_flag)) {
293 input_report_abs(st480->input_dev, ABS_RX, mag.mag_x);
294 input_report_abs(st480->input_dev, ABS_RY, mag.mag_y);
295 input_report_abs(st480->input_dev, ABS_RZ, mag.mag_z);
298 input_sync(st480->input_dev);
300 schedule_delayed_work(&st480->work,msecs_to_jiffies(st480_delay));
303 static void ecs_closedone(void)
305 SENODIAFUNC("ecs_closedone");
306 atomic_set(&m_flag, 0);
307 atomic_set(&mv_flag, 0);
310 /***** st480 functions ***************************************/
311 static int st480_open(struct inode *inode, struct file *file)
315 SENODIAFUNC("st480_open");
317 if (atomic_cmpxchg(&open_count, 0, 1) == 0) {
318 if (atomic_cmpxchg(&open_flag, 0, 1) == 0) {
319 atomic_set(&reserve_open_flag, 1);
324 if(atomic_read(&reserve_open_flag)) {
325 schedule_delayed_work(&st480->work,msecs_to_jiffies(st480_delay));
330 static int st480_release(struct inode *inode, struct file *file)
332 SENODIAFUNC("st480_release");
334 atomic_set(&reserve_open_flag, 0);
335 atomic_set(&open_flag, 0);
336 atomic_set(&open_count, 0);
340 cancel_delayed_work(&st480->work);
344 #if OLD_KERNEL_VERSION
346 st480_ioctl(struct inode *inode, struct file *file,
347 unsigned int cmd, unsigned long arg)
350 st480_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
353 void __user *argp = (void __user *)arg;
356 SENODIADBG("enter %s\n", __func__);
359 case ECS_IOCTL_APP_SET_MFLAG:
360 case ECS_IOCTL_APP_SET_MVFLAG:
361 if (copy_from_user(&flag, argp, sizeof(flag))) {
364 if (flag < 0 || flag > 1) {
368 case ECS_IOCTL_APP_SET_DELAY:
369 if (copy_from_user(&flag, argp, sizeof(flag))) {
378 case ECS_IOCTL_APP_SET_MFLAG:
379 atomic_set(&m_flag, flag);
380 SENODIADBG("MFLAG is set to %d", flag);
382 case ECS_IOCTL_APP_GET_MFLAG:
383 flag = atomic_read(&m_flag);
384 SENODIADBG("Mflag = %d\n",flag);
386 case ECS_IOCTL_APP_SET_MVFLAG:
387 atomic_set(&mv_flag, flag);
388 SENODIADBG("MVFLAG is set to %d", flag);
390 case ECS_IOCTL_APP_GET_MVFLAG:
391 flag = atomic_read(&mv_flag);
392 SENODIADBG("MVflag = %d\n",flag);
394 case ECS_IOCTL_APP_SET_DELAY:
397 case ECS_IOCTL_APP_GET_DELAY:
405 case ECS_IOCTL_APP_GET_MFLAG:
406 case ECS_IOCTL_APP_GET_MVFLAG:
407 case ECS_IOCTL_APP_GET_DELAY:
408 if (copy_to_user(argp, &flag, sizeof(flag))) {
420 static struct file_operations st480_fops = {
421 .owner = THIS_MODULE,
423 .release = st480_release,
424 #if OLD_KERNEL_VERSION
425 .ioctl = st480_ioctl,
427 .unlocked_ioctl = st480_ioctl,
432 static struct miscdevice st480_device = {
433 .minor = MISC_DYNAMIC_MINOR,
438 /*********************************************/
440 static int sensor_test_read(void)
447 static int auto_test_read(void *unused)
458 static struct st480_platform_data *st480_parse_dt(struct device *dev)
460 struct st480_platform_data *pdata;
461 struct device_node *np = dev->of_node;
463 pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
465 dev_err(dev, "Could not allocate struct st480_platform_data");
468 ret = of_property_read_u32(np, "axis_map_x", &pdata->axis_map_x);
470 dev_err(dev, "fail to get axis_map_x\n");
473 ret = of_property_read_u32(np, "axis_map_y", &pdata->axis_map_y);
475 dev_err(dev, "fail to get axis_map_y\n");
478 ret = of_property_read_u32(np, "axis_map_z", &pdata->axis_map_z);
480 dev_err(dev, "fail to get axis_map_z\n");
483 ret = of_property_read_u32(np, "negate_x", &pdata->negate_x);
485 dev_err(dev, "fail to get negate_x\n");
488 ret = of_property_read_u32(np, "negate_y", &pdata->negate_y);
490 dev_err(dev, "fail to get negate_y\n");
493 ret = of_property_read_u32(np, "negate_z", &pdata->negate_z);
495 dev_err(dev, "fail to get negate_z\n");
504 int st480_probe(struct i2c_client *client, const struct i2c_device_id *id)
507 struct st480_platform_data *pdata = client->dev.platform_data;
510 struct task_struct *thread;
513 SENODIAFUNC("st480_probe");
516 struct device_node *np = client->dev.of_node;
518 pdata = st480_parse_dt(&client->dev);
520 client->dev.platform_data = pdata;
524 goto exit_alloc_platform_data_failed;
528 if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
529 printk(KERN_ERR "SENODIA st480_probe: check_functionality failed.\n");
534 /* Allocate memory for driver data */
535 st480 = kzalloc(sizeof(struct st480_data), GFP_KERNEL);
537 printk(KERN_ERR "SENODIA st480_probe: memory allocation failed.\n");
542 st480->client = client;
544 i2c_set_clientdata(client, st480);
546 INIT_DELAYED_WORK(&st480->work, st480_input_func);
548 st480->pdata = kmalloc(sizeof(*st480->pdata), GFP_KERNEL);
549 if (st480->pdata == NULL) {
551 dev_err(&client->dev, "failed to allocate memory for pdata: %d\n", err);
555 memcpy(st480->pdata, client->dev.platform_data, sizeof(*st480->pdata));
558 if(st480_setup(st480->client) != 0) {
559 printk("st480 setup error!\n");
563 /* Declare input device */
564 st480->input_dev = input_allocate_device();
565 if (!st480->input_dev) {
568 "SENODIA st480_probe: Failed to allocate input device\n");
572 /* Setup input device */
573 set_bit(EV_ABS, st480->input_dev->evbit);
575 /* x-axis of raw magnetic vector (-32768, 32767) */
576 input_set_abs_params(st480->input_dev, ABS_RX, ABSMIN_MAG, ABSMAX_MAG, 0, 0);
577 /* y-axis of raw magnetic vector (-32768, 32767) */
578 input_set_abs_params(st480->input_dev, ABS_RY, ABSMIN_MAG, ABSMAX_MAG, 0, 0);
579 /* z-axis of raw magnetic vector (-32768, 32767) */
580 input_set_abs_params(st480->input_dev, ABS_RZ, ABSMIN_MAG, ABSMAX_MAG, 0, 0);
582 st480->input_dev->name = "compass";
585 err = input_register_device(st480->input_dev);
588 "SENODIA st480_probe: Unable to register input device\n");
592 err = misc_register(&st480_device);
595 "SENODIA st480_probe: st480_device register failed\n");
599 /* As default, report all information */
600 atomic_set(&m_flag, 1);
601 atomic_set(&mv_flag, 1);
604 thread=kthread_run(auto_test_read,NULL,"st480_read_test");
607 printk("st480 probe done.");
611 misc_deregister(&st480_device);
612 input_unregister_device(st480->input_dev);
614 input_free_device(st480->input_dev);
620 exit_alloc_platform_data_failed:
625 static int st480_remove(struct i2c_client *client)
627 misc_deregister(&st480_device);
628 input_unregister_device(st480->input_dev);
629 input_free_device(st480->input_dev);
630 cancel_delayed_work(&st480->work);
635 static const struct i2c_device_id st480_id[] = {
636 {ST480_I2C_NAME, 0 },
640 static const struct of_device_id st480_of_match[] = {
641 { .compatible = "ST,st480", },
644 MODULE_DEVICE_TABLE(of, st480_of_match);
645 static struct i2c_driver st480_driver = {
646 .probe = st480_probe,
647 .remove = st480_remove,
648 .id_table = st480_id,
650 .name = ST480_I2C_NAME,
651 .of_match_table = st480_of_match,
655 static int __init st480_init(void)
657 return i2c_add_driver(&st480_driver);
660 static void __exit st480_exit(void)
662 i2c_del_driver(&st480_driver);
665 module_init(st480_init);
666 module_exit(st480_exit);
668 MODULE_AUTHOR("Tori Xu <xuezhi_xu@senodia.com>");
669 MODULE_DESCRIPTION("senodia st480 linux driver");
670 MODULE_LICENSE("GPL");
671 MODULE_VERSION("9.0");