4 static struct gyro_context
*gyro_context_obj
= NULL
;
7 static struct gyro_init_info
* gyroscope_init_list
[MAX_CHOOSE_GYRO_NUM
]= {0}; //modified
8 static void gyro_early_suspend(struct early_suspend
*h
);
9 static void gyro_late_resume(struct early_suspend
*h
);
11 static void gyro_work_func(struct work_struct
*work
)
14 struct gyro_context
*cxt
= NULL
;
16 //hwm_sensor_data sensor_data;
22 cxt
= gyro_context_obj
;
24 if(NULL
== cxt
->gyro_data
.get_data
)
26 GYRO_ERR("gyro driver not register data path\n");
30 time
.tv_sec
= time
.tv_nsec
= 0;
31 time
= get_monotonic_coarse();
32 nt
= time
.tv_sec
*1000000000LL+time
.tv_nsec
;
34 //add wake lock to make sure data can be read before system suspend
35 cxt
->gyro_data
.get_data(&x
,&y
,&z
,&status
);
39 GYRO_ERR("get gyro data fails!!\n" );
44 if((x
!= cxt
->drv_data
.gyro_data
.values
[0])
45 || (y
!= cxt
->drv_data
.gyro_data
.values
[1])
46 || (z
!= cxt
->drv_data
.gyro_data
.values
[2]))
54 cxt
->drv_data
.gyro_data
.values
[0] = x
;
55 cxt
->drv_data
.gyro_data
.values
[1] = y
;
56 cxt
->drv_data
.gyro_data
.values
[2] = z
;
57 cxt
->drv_data
.gyro_data
.status
= status
;
58 cxt
->drv_data
.gyro_data
.time
= nt
;
63 if(true == cxt
->is_first_data_after_enable
)
65 cxt
->is_first_data_after_enable
= false;
67 if(GYRO_INVALID_VALUE
== cxt
->drv_data
.gyro_data
.values
[0] ||
68 GYRO_INVALID_VALUE
== cxt
->drv_data
.gyro_data
.values
[1] ||
69 GYRO_INVALID_VALUE
== cxt
->drv_data
.gyro_data
.values
[2])
71 GYRO_LOG(" read invalid data \n");
76 //report data to input device
77 //printk("new gyro work run....\n");
78 //GYRO_LOG("gyro data[%d,%d,%d] \n" ,cxt->drv_data.gyro_data.values[0],
79 //cxt->drv_data.gyro_data.values[1],cxt->drv_data.gyro_data.values[2]);
81 gyro_data_report(cxt
->drv_data
.gyro_data
.values
[0],
82 cxt
->drv_data
.gyro_data
.values
[1],cxt
->drv_data
.gyro_data
.values
[2],
83 cxt
->drv_data
.gyro_data
.status
);
86 if(true == cxt
->is_polling_run
)
89 mod_timer(&cxt
->timer
, jiffies
+ atomic_read(&cxt
->delay
)/(1000/HZ
));
95 static void gyro_poll(unsigned long data
)
97 struct gyro_context
*obj
= (struct gyro_context
*)data
;
100 schedule_work(&obj
->report
);
104 static struct gyro_context
*gyro_context_alloc_object(void)
107 struct gyro_context
*obj
= kzalloc(sizeof(*obj
), GFP_KERNEL
);
108 GYRO_LOG("gyro_context_alloc_object++++\n");
111 GYRO_ERR("Alloc gyro object error!\n");
114 atomic_set(&obj
->delay
, 200); /*5Hz*/// set work queue delay time
200ms
115 atomic_set(&obj
->wake
, 0);
116 INIT_WORK(&obj
->report
, gyro_work_func
);
117 init_timer(&obj
->timer
);
118 obj
->timer
.expires
= jiffies
+ atomic_read(&obj
->delay
)/(1000/HZ
);
119 obj
->timer
.function
= gyro_poll
;
120 obj
->timer
.data
= (unsigned long)obj
;
121 obj
->is_first_data_after_enable
= false;
122 obj
->is_polling_run
= false;
123 obj
->is_batch_enable
= false;
124 mutex_init(&obj
->gyro_op_mutex
);
125 GYRO_LOG("gyro_context_alloc_object----\n");
129 static int gyro_real_enable(int enable
)
132 struct gyro_context
*cxt
= NULL
;
133 cxt
= gyro_context_obj
;
137 if(true==cxt
->is_active_data
|| true ==cxt
->is_active_nodata
)
139 err
= cxt
->gyro_ctl
.enable_nodata(1);
142 err
= cxt
->gyro_ctl
.enable_nodata(1);
145 err
= cxt
->gyro_ctl
.enable_nodata(1);
147 GYRO_ERR("gyro enable(%d) err 3 timers = %d\n", enable
, err
);
150 GYRO_LOG("gyro real enable \n" );
156 if(false==cxt
->is_active_data
&& false ==cxt
->is_active_nodata
)
158 err
= cxt
->gyro_ctl
.enable_nodata(0);
161 GYRO_ERR("gyro enable(%d) err = %d\n", enable
, err
);
163 GYRO_LOG("gyro real disable \n" );
170 static int gyro_enable_data(int enable
)
172 struct gyro_context
*cxt
= NULL
;
174 cxt
= gyro_context_obj
;
175 if(NULL
== cxt
->gyro_ctl
.open_report_data
)
177 GYRO_ERR("no gyro control path\n");
183 GYRO_LOG("gyro enable data\n");
184 cxt
->is_active_data
=true;
185 cxt
->is_first_data_after_enable
= true;
186 cxt
->gyro_ctl
.open_report_data(1);
187 if(false == cxt
->is_polling_run
&& cxt
->is_batch_enable
== false)
189 if(false == cxt
->gyro_ctl
.is_report_input_direct
)
191 mod_timer(&cxt
->timer
, jiffies
+ atomic_read(&cxt
->delay
)/(1000/HZ
));
192 cxt
->is_polling_run
= true;
198 GYRO_LOG("gyro disable \n");
200 cxt
->is_active_data
=false;
201 cxt
->gyro_ctl
.open_report_data(0);
202 if(true == cxt
->is_polling_run
)
204 if(false == cxt
->gyro_ctl
.is_report_input_direct
)
206 cxt
->is_polling_run
= false;
207 del_timer_sync(&cxt
->timer
);
208 cancel_work_sync(&cxt
->report
);
209 cxt
->drv_data
.gyro_data
.values
[0] = GYRO_INVALID_VALUE
;
210 cxt
->drv_data
.gyro_data
.values
[1] = GYRO_INVALID_VALUE
;
211 cxt
->drv_data
.gyro_data
.values
[2] = GYRO_INVALID_VALUE
;
216 gyro_real_enable(enable
);
222 int gyro_enable_nodata(int enable
)
224 struct gyro_context
*cxt
= NULL
;
226 cxt
= gyro_context_obj
;
227 if(NULL
== cxt
->gyro_ctl
.enable_nodata
)
229 GYRO_ERR("gyro_enable_nodata:gyro ctl path is NULL\n");
235 cxt
->is_active_nodata
= true;
240 cxt
->is_active_nodata
= false;
242 gyro_real_enable(enable
);
247 static ssize_t
gyro_show_enable_nodata(struct device
* dev
,
248 struct device_attribute
*attr
, char *buf
)
251 GYRO_LOG(" not support now\n");
255 static ssize_t
gyro_store_enable_nodata(struct device
* dev
, struct device_attribute
*attr
,
256 const char *buf
, size_t count
)
258 GYRO_LOG("gyro_store_enable nodata buf=%s\n",buf
);
259 mutex_lock(&gyro_context_obj
->gyro_op_mutex
);
260 struct gyro_context
*cxt
= NULL
;
262 cxt
= gyro_context_obj
;
263 if(NULL
== cxt
->gyro_ctl
.enable_nodata
)
265 GYRO_LOG("gyro_ctl enable nodata NULL\n");
266 mutex_unlock(&gyro_context_obj
->gyro_op_mutex
);
269 if (!strncmp(buf
, "1", 1))
271 gyro_enable_nodata(1);
273 else if (!strncmp(buf
, "0", 1))
275 gyro_enable_nodata(0);
279 GYRO_ERR(" gyro_store enable nodata cmd error !!\n");
281 mutex_unlock(&gyro_context_obj
->gyro_op_mutex
);
284 static ssize_t
gyro_store_active(struct device
* dev
, struct device_attribute
*attr
,
285 const char *buf
, size_t count
)
287 GYRO_LOG("gyro_store_active buf=%s\n",buf
);
288 mutex_lock(&gyro_context_obj
->gyro_op_mutex
);
289 struct gyro_context
*cxt
= NULL
;
291 cxt
= gyro_context_obj
;
292 if(NULL
== cxt
->gyro_ctl
.open_report_data
)
294 GYRO_LOG("gyro_ctl enable NULL\n");
295 mutex_unlock(&gyro_context_obj
->gyro_op_mutex
);
298 if (!strncmp(buf
, "1", 1))
300 // cxt->gyro_ctl.enable(1);
304 else if (!strncmp(buf
, "0", 1))
307 //cxt->gyro_ctl.enable(0);
312 GYRO_ERR(" gyro_store_active error !!\n");
314 mutex_unlock(&gyro_context_obj
->gyro_op_mutex
);
315 GYRO_LOG(" gyro_store_active done\n");
318 /*----------------------------------------------------------------------------*/
319 static ssize_t
gyro_show_active(struct device
* dev
,
320 struct device_attribute
*attr
, char *buf
)
322 struct gyro_context
*cxt
= NULL
;
323 cxt
= gyro_context_obj
;
326 GYRO_LOG("gyro show active not support now\n");
327 int div
=cxt
->gyro_data
.vender_div
;
328 GYRO_LOG("gyro vender_div value: %d\n", div
);
329 return snprintf(buf
, PAGE_SIZE
, "%d\n", div
);
334 static ssize_t
gyro_store_delay(struct device
* dev
, struct device_attribute
*attr
,
335 const char *buf
, size_t count
)
337 mutex_lock(&gyro_context_obj
->gyro_op_mutex
);
338 struct gyro_context
*devobj
= (struct gyro_context
*)dev_get_drvdata(dev
);
341 struct gyro_context
*cxt
= NULL
;
343 cxt
= gyro_context_obj
;
344 if(NULL
== cxt
->gyro_ctl
.set_delay
)
346 GYRO_LOG("gyro_ctl set_delay NULL\n");
347 mutex_unlock(&gyro_context_obj
->gyro_op_mutex
);
351 if (1 != sscanf(buf
, "%d", &delay
)) {
352 GYRO_ERR("invalid format!!\n");
353 mutex_unlock(&gyro_context_obj
->gyro_op_mutex
);
357 if(false == cxt
->gyro_ctl
.is_report_input_direct
)
359 mdelay
= (int)delay
/1000/1000;
360 atomic_set(&gyro_context_obj
->delay
, mdelay
);
362 cxt
->gyro_ctl
.set_delay(delay
);
363 GYRO_LOG(" gyro_delay %d ns\n",delay
);
364 mutex_unlock(&gyro_context_obj
->gyro_op_mutex
);
368 static ssize_t
gyro_show_delay(struct device
* dev
,
369 struct device_attribute
*attr
, char *buf
)
372 GYRO_LOG(" not support now\n");
376 static ssize_t
gyro_store_batch(struct device
* dev
, struct device_attribute
*attr
,
377 const char *buf
, size_t count
)
379 GYRO_LOG("gyro_store_batch buf=%s\n",buf
);
380 mutex_lock(&gyro_context_obj
->gyro_op_mutex
);
381 struct gyro_context
*cxt
= NULL
;
383 cxt
= gyro_context_obj
;
384 if(cxt
->gyro_ctl
.is_support_batch
){
386 if (!strncmp(buf
, "1", 1))
388 cxt
->is_batch_enable
= true;
390 else if (!strncmp(buf
, "0", 1))
392 cxt
->is_batch_enable
= false;
396 GYRO_ERR(" gyro_store_batch error !!\n");
399 GYRO_LOG(" gyro_store_batch not support\n");
401 mutex_unlock(&gyro_context_obj
->gyro_op_mutex
);
402 GYRO_LOG(" gyro_store_batch done: %d\n", cxt
->is_batch_enable
);
407 static ssize_t
gyro_show_batch(struct device
* dev
,
408 struct device_attribute
*attr
, char *buf
)
410 return snprintf(buf
, PAGE_SIZE
, "%d\n", 0);
413 static ssize_t
gyro_store_flush(struct device
* dev
, struct device_attribute
*attr
,
414 const char *buf
, size_t count
)
416 mutex_lock(&gyro_context_obj
->gyro_op_mutex
);
417 struct gyro_context
*devobj
= (struct gyro_context
*)dev_get_drvdata(dev
);
418 //do read FIFO data function and report data immediately
419 mutex_unlock(&gyro_context_obj
->gyro_op_mutex
);
423 static ssize_t
gyro_show_flush(struct device
* dev
,
424 struct device_attribute
*attr
, char *buf
)
426 return snprintf(buf
, PAGE_SIZE
, "%d\n", 0);
429 static ssize_t
gyro_show_devnum(struct device
* dev
,
430 struct device_attribute
*attr
, char *buf
)
432 char *devname
= NULL
;
433 devname
= dev_name(&gyro_context_obj
->idev
->dev
);
434 return snprintf(buf
, PAGE_SIZE
, "%s\n", devname
+5);
436 static int gyroscope_remove(struct platform_device
*pdev
)
438 GYRO_LOG("gyroscope_remove\n");
442 static int gyroscope_probe(struct platform_device
*pdev
)
444 GYRO_LOG("gyroscope_probe\n");
448 static struct platform_driver gyroscope_driver
= {
449 .probe
= gyroscope_probe
,
450 .remove
= gyroscope_remove
,
457 static int gyro_real_driver_init(void)
461 GYRO_LOG(" gyro_real_driver_init +\n");
462 for(i
= 0; i
< MAX_CHOOSE_GYRO_NUM
; i
++)
464 GYRO_LOG(" i=%d\n",i
);
465 if(0 != gyroscope_init_list
[i
])
467 GYRO_LOG(" gyro try to init driver %s\n", gyroscope_init_list
[i
]->name
);
468 err
= gyroscope_init_list
[i
]->init();
471 GYRO_LOG(" gyro real driver %s probe ok\n", gyroscope_init_list
[i
]->name
);
477 if(i
== MAX_CHOOSE_GYRO_NUM
)
479 GYRO_LOG(" gyro_real_driver_init fail\n");
485 int gyro_driver_add(struct gyro_init_info
* obj
)
492 for(i
=0; i
< MAX_CHOOSE_GYRO_NUM
; i
++ )
495 GYRO_LOG("register gyro driver for the first time\n");
496 if(platform_driver_register(&gyroscope_driver
))
498 GYRO_ERR("failed to register gyro driver already exist\n");
502 if(NULL
== gyroscope_init_list
[i
])
504 obj
->platform_diver_addr
= &gyroscope_driver
;
505 gyroscope_init_list
[i
] = obj
;
509 if(NULL
==gyroscope_init_list
[i
])
511 GYRO_ERR("gyro driver add err \n");
517 EXPORT_SYMBOL_GPL(gyro_driver_add
);
519 static int gyro_misc_init(struct gyro_context
*cxt
)
523 cxt
->mdev
.minor
= MISC_DYNAMIC_MINOR
;
524 cxt
->mdev
.name
= GYRO_MISC_DEV_NAME
;
525 if((err
= misc_register(&cxt
->mdev
)))
527 GYRO_ERR("unable to register gyro misc device!!\n");
532 static void gyro_input_destroy(struct gyro_context
*cxt
)
534 struct input_dev
*dev
= cxt
->idev
;
536 input_unregister_device(dev
);
537 input_free_device(dev
);
540 static int gyro_input_init(struct gyro_context
*cxt
)
542 struct input_dev
*dev
;
545 dev
= input_allocate_device();
549 dev
->name
= GYRO_INPUTDEV_NAME
;
551 input_set_capability(dev
, EV_ABS
, EVENT_TYPE_GYRO_X
);
552 input_set_capability(dev
, EV_ABS
, EVENT_TYPE_GYRO_Y
);
553 input_set_capability(dev
, EV_ABS
, EVENT_TYPE_GYRO_Z
);
554 input_set_capability(dev
, EV_ABS
, EVENT_TYPE_GYRO_STATUS
);
556 input_set_abs_params(dev
, EVENT_TYPE_GYRO_X
, GYRO_VALUE_MIN
, GYRO_VALUE_MAX
, 0, 0);
557 input_set_abs_params(dev
, EVENT_TYPE_GYRO_Y
, GYRO_VALUE_MIN
, GYRO_VALUE_MAX
, 0, 0);
558 input_set_abs_params(dev
, EVENT_TYPE_GYRO_Z
, GYRO_VALUE_MIN
, GYRO_VALUE_MAX
, 0, 0);
559 input_set_abs_params(dev
, EVENT_TYPE_GYRO_STATUS
, GYRO_STATUS_MIN
, GYRO_STATUS_MAX
, 0, 0);
560 input_set_drvdata(dev
, cxt
);
562 err
= input_register_device(dev
);
564 input_free_device(dev
);
572 DEVICE_ATTR(gyroenablenodata
, S_IWUSR
| S_IRUGO
, gyro_show_enable_nodata
, gyro_store_enable_nodata
);
573 DEVICE_ATTR(gyroactive
, S_IWUSR
| S_IRUGO
, gyro_show_active
, gyro_store_active
);
574 DEVICE_ATTR(gyrodelay
, S_IWUSR
| S_IRUGO
, gyro_show_delay
, gyro_store_delay
);
575 DEVICE_ATTR(gyrobatch
, S_IWUSR
| S_IRUGO
, gyro_show_batch
, gyro_store_batch
);
576 DEVICE_ATTR(gyroflush
, S_IWUSR
| S_IRUGO
, gyro_show_flush
, gyro_store_flush
);
577 DEVICE_ATTR(gyrodevnum
, S_IWUSR
| S_IRUGO
, gyro_show_devnum
, NULL
);
579 static struct attribute
*gyro_attributes
[] = {
580 &dev_attr_gyroenablenodata
.attr
,
581 &dev_attr_gyroactive
.attr
,
582 &dev_attr_gyrodelay
.attr
,
583 &dev_attr_gyrobatch
.attr
,
584 &dev_attr_gyroflush
.attr
,
585 &dev_attr_gyrodevnum
.attr
,
589 static struct attribute_group gyro_attribute_group
= {
590 .attrs
= gyro_attributes
593 int gyro_register_data_path(struct gyro_data_path
*data
)
595 struct gyro_context
*cxt
= NULL
;
597 cxt
= gyro_context_obj
;
598 cxt
->gyro_data
.get_data
= data
->get_data
;
599 cxt
->gyro_data
.vender_div
= data
->vender_div
;
600 GYRO_LOG("gyro register data path vender_div: %d\n", cxt
->gyro_data
.vender_div
);
601 if(NULL
== cxt
->gyro_data
.get_data
)
603 GYRO_LOG("gyro register data path fail \n");
609 int gyro_register_control_path(struct gyro_control_path
*ctl
)
611 struct gyro_context
*cxt
= NULL
;
613 cxt
= gyro_context_obj
;
614 cxt
->gyro_ctl
.set_delay
= ctl
->set_delay
;
615 cxt
->gyro_ctl
.open_report_data
= ctl
->open_report_data
;
616 cxt
->gyro_ctl
.enable_nodata
= ctl
->enable_nodata
;
617 cxt
->gyro_ctl
.is_support_batch
= ctl
->is_support_batch
;
619 if(NULL
==cxt
->gyro_ctl
.set_delay
|| NULL
==cxt
->gyro_ctl
.open_report_data
620 || NULL
==cxt
->gyro_ctl
.enable_nodata
)
622 GYRO_LOG("gyro register control path fail \n");
626 //add misc dev for sensor hal control cmd
627 err
= gyro_misc_init(gyro_context_obj
);
630 GYRO_ERR("unable to register gyro misc device!!\n");
633 err
= sysfs_create_group(&gyro_context_obj
->mdev
.this_device
->kobj
,
634 &gyro_attribute_group
);
637 GYRO_ERR("unable to create gyro attribute file\n");
641 kobject_uevent(&gyro_context_obj
->mdev
.this_device
->kobj
, KOBJ_ADD
);
646 int gyro_data_report(int x
, int y
, int z
,int status
)
648 //GYRO_LOG("+gyro_data_report! %d, %d, %d, %d\n",x,y,z,status);
649 struct gyro_context
*cxt
= NULL
;
651 cxt
= gyro_context_obj
;
652 input_report_abs(cxt
->idev
, EVENT_TYPE_GYRO_X
, x
);
653 input_report_abs(cxt
->idev
, EVENT_TYPE_GYRO_Y
, y
);
654 input_report_abs(cxt
->idev
, EVENT_TYPE_GYRO_Z
, z
);
655 input_report_abs(cxt
->idev
, EVENT_TYPE_GYRO_STATUS
, status
);
656 input_sync(cxt
->idev
);
659 static int gyro_probe(struct platform_device
*pdev
)
663 GYRO_LOG("+++++++++++++gyro_probe!!\n");
665 gyro_context_obj
= gyro_context_alloc_object();
666 if (!gyro_context_obj
)
669 GYRO_ERR("unable to allocate devobj!\n");
670 goto exit_alloc_data_failed
;
673 //init real gyroeleration driver
674 err
= gyro_real_driver_init();
677 GYRO_ERR("gyro real driver init fail\n");
678 goto real_driver_init_fail
;
682 err
= gyro_input_init(gyro_context_obj
);
685 GYRO_ERR("unable to register gyro input device!\n");
686 goto exit_alloc_input_dev_failed
;
689 atomic_set(&(gyro_context_obj
->early_suspend
), 0);
690 gyro_context_obj
->early_drv
.level
= EARLY_SUSPEND_LEVEL_STOP_DRAWING
- 1,
691 gyro_context_obj
->early_drv
.suspend
= gyro_early_suspend
,
692 gyro_context_obj
->early_drv
.resume
= gyro_late_resume
,
693 register_early_suspend(&gyro_context_obj
->early_drv
);
695 GYRO_LOG("----gyro_probe OK !!\n");
698 exit_hwmsen_create_attr_failed
:
699 exit_misc_register_failed
:
705 GYRO_ERR("sysfs node creation error \n");
706 gyro_input_destroy(gyro_context_obj
);
709 real_driver_init_fail
:
710 exit_alloc_input_dev_failed
:
711 kfree(gyro_context_obj
);
713 exit_alloc_data_failed
:
716 GYRO_LOG("----gyro_probe fail !!!\n");
722 static int gyro_remove(struct platform_device
*pdev
)
726 input_unregister_device(gyro_context_obj
->idev
);
727 sysfs_remove_group(&gyro_context_obj
->idev
->dev
.kobj
,
728 &gyro_attribute_group
);
730 if((err
= misc_deregister(&gyro_context_obj
->mdev
)))
732 GYRO_ERR("misc_deregister fail: %d\n", err
);
734 kfree(gyro_context_obj
);
739 static void gyro_early_suspend(struct early_suspend
*h
)
741 atomic_set(&(gyro_context_obj
->early_suspend
), 1);
742 GYRO_LOG(" gyro_early_suspend ok------->hwm_obj->early_suspend=%d \n",atomic_read(&(gyro_context_obj
->early_suspend
)));
745 /*----------------------------------------------------------------------------*/
746 static void gyro_late_resume(struct early_suspend
*h
)
748 atomic_set(&(gyro_context_obj
->early_suspend
), 0);
749 GYRO_LOG(" gyro_late_resume ok------->hwm_obj->early_suspend=%d \n",atomic_read(&(gyro_context_obj
->early_suspend
)));
753 static int gyro_suspend(struct platform_device
*dev
, pm_message_t state
)
757 /*----------------------------------------------------------------------------*/
758 static int gyro_resume(struct platform_device
*dev
)
763 static struct platform_driver gyro_driver
=
766 .remove
= gyro_remove
,
767 .suspend
= gyro_suspend
,
768 .resume
= gyro_resume
,
771 .name
= GYRO_PL_DEV_NAME
,//mt_gyro_pl
775 static int __init
gyro_init(void)
779 if(platform_driver_register(&gyro_driver
))
781 GYRO_ERR("failed to register gyro driver\n");
788 static void __exit
gyro_exit(void)
790 platform_driver_unregister(&gyro_driver
);
791 platform_driver_unregister(&gyroscope_driver
);
794 module_init(gyro_init
);
795 module_exit(gyro_exit
);
796 MODULE_LICENSE("GPL");
797 MODULE_DESCRIPTION("GYROSCOPE device driver");
798 MODULE_AUTHOR("Mediatek");