static int gyro_probe(struct platform_device *pdev)
{
int err;
GYRO_LOG("+++++++++++++gyro_probe!!\n");
gyro_context_obj = gyro_context_alloc_object();
if (!gyro_context_obj)
{
err = -ENOMEM;
GYRO_ERR("unable to allocate devobj!\n");
goto exit_alloc_data_failed;
}
//init real gyroeleration driver
err = gyro_real_driver_init();
if(err)
{
GYRO_ERR("gyro real driver init fail\n");
goto real_driver_init_fail;
}
//init input dev
err = gyro_input_init(gyro_context_obj);
if(err)
{
GYRO_ERR("unable to register gyro input device!\n");
goto exit_alloc_input_dev_failed;
}
atomic_set(&(gyro_context_obj->early_suspend), 0);
gyro_context_obj->early_drv.level = EARLY_SUSPEND_LEVEL_STOP_DRAWING - 1,
gyro_context_obj->early_drv.suspend = gyro_early_suspend,
gyro_context_obj->early_drv.resume = gyro_late_resume,
register_early_suspend(&gyro_context_obj->early_drv);
GYRO_LOG("----gyro_probe OK !!\n");
return 0;
//exit_hwmsen_create_attr_failed:
//exit_misc_register_failed:
//exit_err_sysfs:
if (err)
{
GYRO_ERR("sysfs node creation error \n");
gyro_input_destroy(gyro_context_obj);
}
real_driver_init_fail:
exit_alloc_input_dev_failed:
kfree(gyro_context_obj);
exit_alloc_data_failed:
GYRO_LOG("----gyro_probe fail !!!\n");
return err;
}
static int gyro_real_driver_init(void)
{
int i =0;
int err=0;
GYRO_LOG(" gyro_real_driver_init +\n");
for(i = 0; i < MAX_CHOOSE_GYRO_NUM; i++)
{
GYRO_LOG(" i=%d\n",i);
if(0 != gyroscope_init_list[i])
{
GYRO_LOG(" gyro try to init driver %s\n", gyroscope_init_list[i]->name);
err = gyroscope_init_list[i]->init();
if(0 == err)
{
GYRO_LOG(" gyro real driver %s probe ok\n", gyroscope_init_list[i]->name);
break;
}
}
}
if(i == MAX_CHOOSE_GYRO_NUM)
{
GYRO_LOG(" gyro_real_driver_init fail\n");
err=-1;
}
return err;
}
static ssize_t gyro_store_delay(struct device* dev, struct device_attribute *attr,
const char *buf, size_t count)
{
// struct gyro_context *devobj = (struct gyro_context*)dev_get_drvdata(dev);
int delay;
int mdelay=0;
struct gyro_context *cxt = NULL;
//int err =0;
mutex_lock(&gyro_context_obj->gyro_op_mutex);
cxt = gyro_context_obj;
if(NULL == cxt->gyro_ctl.set_delay)
{
GYRO_LOG("gyro_ctl set_delay NULL\n");
mutex_unlock(&gyro_context_obj->gyro_op_mutex);
return count;
}
if (1 != sscanf(buf, "%d", &delay)) {
GYRO_ERR("invalid format!!\n");
mutex_unlock(&gyro_context_obj->gyro_op_mutex);
return count;
}
if(false == cxt->gyro_ctl.is_report_input_direct)
{
mdelay = (int)delay/1000/1000;
atomic_set(&gyro_context_obj->delay, mdelay);
}
cxt->gyro_ctl.set_delay(delay);
GYRO_LOG(" gyro_delay %d ns\n",delay);
mutex_unlock(&gyro_context_obj->gyro_op_mutex);
return count;
}
static ssize_t gyro_store_enable_nodata(struct device* dev, struct device_attribute *attr,
const char *buf, size_t count)
{
GYRO_LOG("gyro_store_enable nodata buf=%s\n",buf);
mutex_lock(&gyro_context_obj->gyro_op_mutex);
struct gyro_context *cxt = NULL;
int err =0;
cxt = gyro_context_obj;
if(NULL == cxt->gyro_ctl.enable_nodata)
{
GYRO_LOG("gyro_ctl enable nodata NULL\n");
mutex_unlock(&gyro_context_obj->gyro_op_mutex);
return count;
}
if (!strncmp(buf, "1", 1))
{
gyro_enable_nodata(1);
}
else if (!strncmp(buf, "0", 1))
{
gyro_enable_nodata(0);
}
else
{
GYRO_ERR(" gyro_store enable nodata cmd error !!\n");
}
mutex_unlock(&gyro_context_obj->gyro_op_mutex);
}
static struct gyro_context *gyro_context_alloc_object(void)
{
struct gyro_context *obj = kzalloc(sizeof(*obj), GFP_KERNEL);
GYRO_LOG("gyro_context_alloc_object++++\n");
if(!obj)
{
GYRO_ERR("Alloc gyro object error!\n");
return NULL;
}
atomic_set(&obj->delay, 200); /*5Hz*/// set work queue delay time 200ms
atomic_set(&obj->wake, 0);
INIT_WORK(&obj->report, gyro_work_func);
init_timer(&obj->timer);
obj->timer.expires = jiffies + atomic_read(&obj->delay)/(1000/HZ);
obj->timer.function = gyro_poll;
obj->timer.data = (unsigned long)obj;
obj->is_first_data_after_enable = false;
obj->is_polling_run = false;
obj->is_batch_enable = false;
obj->cali_sw[GYRO_AXIS_X]=0;
obj->cali_sw[GYRO_AXIS_Y]=0;
obj->cali_sw[GYRO_AXIS_Z]=0;
mutex_init(&obj->gyro_op_mutex);
GYRO_LOG("gyro_context_alloc_object----\n");
return obj;
}
static ssize_t gyro_store_delay(struct device *dev, struct device_attribute *attr,
const char *buf, size_t count)
{
int64_t delay;
int64_t mdelay = 0;
int ret = 0;
struct gyro_context *cxt = NULL;
mutex_lock(&gyro_context_obj->gyro_op_mutex);
cxt = gyro_context_obj;
if (NULL == cxt->gyro_ctl.set_delay) {
GYRO_LOG("gyro_ctl set_delay NULL\n");
mutex_unlock(&gyro_context_obj->gyro_op_mutex);
return count;
}
ret = kstrtoll(buf, 10, &delay);
if (ret != 0) {
GYRO_ERR("invalid format!!\n");
mutex_unlock(&gyro_context_obj->gyro_op_mutex);
return count;
}
if (false == cxt->gyro_ctl.is_report_input_direct) {
mdelay = delay;
do_div(mdelay, 1000000);
atomic_set(&gyro_context_obj->delay, mdelay);
}
cxt->gyro_ctl.set_delay(delay);
GYRO_LOG(" gyro_delay %lld ns\n", delay);
mutex_unlock(&gyro_context_obj->gyro_op_mutex);
return count;
}
static ssize_t gyro_store_batch(struct device* dev, struct device_attribute *attr,
const char *buf, size_t count)
{
struct gyro_context *cxt = NULL;
//int err =0;
GYRO_LOG("gyro_store_batch buf=%s\n",buf);
mutex_lock(&gyro_context_obj->gyro_op_mutex);
cxt = gyro_context_obj;
if(cxt->gyro_ctl.is_support_batch){
if (!strncmp(buf, "1", 1))
{
cxt->is_batch_enable = true;
}
else if (!strncmp(buf, "0", 1))
{
cxt->is_batch_enable = false;
}
else
{
GYRO_ERR(" gyro_store_batch error !!\n");
}
}else{
GYRO_LOG(" gyro_store_batch not support\n");
}
mutex_unlock(&gyro_context_obj->gyro_op_mutex);
GYRO_LOG(" gyro_store_batch done: %d\n", cxt->is_batch_enable);
return count;
}
static ssize_t gyro_store_active(struct device* dev, struct device_attribute *attr,
const char *buf, size_t count)
{
struct gyro_context *cxt = NULL;
//int err =0;
GYRO_LOG("gyro_store_active buf=%s\n",buf);
mutex_lock(&gyro_context_obj->gyro_op_mutex);
cxt = gyro_context_obj;
if(NULL == cxt->gyro_ctl.open_report_data)
{
GYRO_LOG("gyro_ctl enable NULL\n");
mutex_unlock(&gyro_context_obj->gyro_op_mutex);
return count;
}
if (!strncmp(buf, "1", 1))
{
// cxt->gyro_ctl.enable(1);
gyro_enable_data(1);
}
else if (!strncmp(buf, "0", 1))
{
//cxt->gyro_ctl.enable(0);
gyro_enable_data(0);
}
else
{
GYRO_ERR(" gyro_store_active error !!\n");
}
mutex_unlock(&gyro_context_obj->gyro_op_mutex);
GYRO_LOG(" gyro_store_active done\n");
return count;
}
static int gyro_real_enable(int enable)
{
int err = 0;
struct gyro_context *cxt = NULL;
cxt = gyro_context_obj;
if (1 == enable) {
if (true == cxt->is_active_data || true == cxt->is_active_nodata) {
err = cxt->gyro_ctl.enable_nodata(1);
if (err) {
err = cxt->gyro_ctl.enable_nodata(1);
if (err) {
err = cxt->gyro_ctl.enable_nodata(1);
if (err)
GYRO_ERR("gyro enable(%d) err 3 timers = %d\n", enable, err);
}
}
GYRO_LOG("gyro real enable\n");
}
}
if (0 == enable) {
if (false == cxt->is_active_data && false == cxt->is_active_nodata) {
err = cxt->gyro_ctl.enable_nodata(0);
if (err)
GYRO_ERR("gyro enable(%d) err = %d\n", enable, err);
GYRO_LOG("gyro real disable\n");
}
}
return err;
}
static struct gyro_context *gyro_context_alloc_object(void)
{
struct gyro_context *obj = kzalloc(sizeof(*obj), GFP_KERNEL);
GYRO_LOG("gyro_context_alloc_object++++\n");
if (!obj) {
GYRO_ERR("Alloc gyro object error!\n");
return NULL;
}
atomic_set(&obj->delay, 200); /*5Hz, set work queue delay time 200ms */
atomic_set(&obj->wake, 0);
INIT_WORK(&obj->report, gyro_work_func);
obj->gyro_workqueue = NULL;
obj->gyro_workqueue = create_workqueue("gyro_polling");
if (!obj->gyro_workqueue) {
kfree(obj);
return NULL;
}
initTimer(&obj->hrTimer, gyro_poll);
obj->is_first_data_after_enable = false;
obj->is_polling_run = false;
obj->is_batch_enable = false;
obj->cali_sw[GYRO_AXIS_X] = 0;
obj->cali_sw[GYRO_AXIS_Y] = 0;
obj->cali_sw[GYRO_AXIS_Z] = 0;
mutex_init(&obj->gyro_op_mutex);
GYRO_LOG("gyro_context_alloc_object----\n");
return obj;
}
static int gyro_set_cali(int data[GYRO_AXES_NUM])
{
struct gyro_context *cxt = gyro_context_obj;
GYRO_LOG(" factory gyro cali %d,%d,%d \n",data[GYRO_AXIS_X],data[GYRO_AXIS_Y],data[GYRO_AXIS_Z] );
GYRO_LOG(" original gyro cali %d,%d,%d \n",cxt->cali_sw[GYRO_AXIS_X],cxt->cali_sw[GYRO_AXIS_Y],cxt->cali_sw[GYRO_AXIS_Z]);
cxt->cali_sw[GYRO_AXIS_X] += data[GYRO_AXIS_X];
cxt->cali_sw[GYRO_AXIS_Y] += data[GYRO_AXIS_Y];
cxt->cali_sw[GYRO_AXIS_Z] += data[GYRO_AXIS_Z];
GYRO_LOG(" GYRO new cali %d,%d,%d \n",cxt->cali_sw[GYRO_AXIS_X],cxt->cali_sw[GYRO_AXIS_Y],cxt->cali_sw[GYRO_AXIS_Z]);
return 0;
}
/*----------------------------------------------------------------------------*/
static ssize_t gyro_show_active(struct device *dev,
struct device_attribute *attr, char *buf)
{
struct gyro_context *cxt = NULL;
int div = 0;
cxt = gyro_context_obj;
GYRO_LOG("gyro show active not support now\n");
div = cxt->gyro_data.vender_div;
GYRO_LOG("gyro vender_div value: %d\n", div);
return snprintf(buf, PAGE_SIZE, "%d\n", div);
}
static int gyro_enable_data(int enable)
{
struct gyro_context *cxt = NULL;
//int err =0;
cxt = gyro_context_obj;
if(NULL == cxt->gyro_ctl.open_report_data)
{
GYRO_ERR("no gyro control path\n");
return -1;
}
if(1 == enable)
{
GYRO_LOG("gyro enable data\n");
cxt->is_active_data =true;
cxt->is_first_data_after_enable = true;
cxt->gyro_ctl.open_report_data(1);
gyro_real_enable(enable);
if(false == cxt->is_polling_run && cxt->is_batch_enable == false)
{
if(false == cxt->gyro_ctl.is_report_input_direct)
{
mod_timer(&cxt->timer, jiffies + atomic_read(&cxt->delay)/(1000/HZ));
cxt->is_polling_run = true;
}
}
}
if(0 == enable)
{
GYRO_LOG("gyro disable \n");
cxt->is_active_data =false;
cxt->gyro_ctl.open_report_data(0);
if(true == cxt->is_polling_run)
{
if(false == cxt->gyro_ctl.is_report_input_direct)
{
cxt->is_polling_run = false;
smp_mb();
del_timer_sync(&cxt->timer);
smp_mb();
cancel_work_sync(&cxt->report);
cxt->drv_data.gyro_data.values[0] = GYRO_INVALID_VALUE;
cxt->drv_data.gyro_data.values[1] = GYRO_INVALID_VALUE;
cxt->drv_data.gyro_data.values[2] = GYRO_INVALID_VALUE;
}
}
gyro_real_enable(enable);
}
return 0;
}
int gyro_register_data_path(struct gyro_data_path *data)
{
struct gyro_context *cxt = NULL;
//int err =0;
cxt = gyro_context_obj;
cxt->gyro_data.get_data = data->get_data;
cxt->gyro_data.vender_div = data->vender_div;
GYRO_LOG("gyro register data path vender_div: %d\n", cxt->gyro_data.vender_div);
if(NULL == cxt->gyro_data.get_data)
{
GYRO_LOG("gyro register data path fail \n");
return -1;
}
return 0;
}
static int gyro_probe(void)
{
int err;
GYRO_LOG("+++++++++++++gyro_probe!!\n");
gyro_context_obj = gyro_context_alloc_object();
if (!gyro_context_obj) {
err = -ENOMEM;
GYRO_ERR("unable to allocate devobj!\n");
goto exit_alloc_data_failed;
}
/* init real gyroeleration driver */
err = gyro_real_driver_init(pltfm_dev);
if (err) {
GYRO_ERR("gyro real driver init fail\n");
goto real_driver_init_fail;
}
err = gyro_factory_device_init();
if (err)
GYRO_ERR("gyro_factory_device_init fail\n");
/* init input dev */
err = gyro_input_init(gyro_context_obj);
if (err) {
GYRO_ERR("unable to register gyro input device!\n");
goto exit_alloc_input_dev_failed;
}
GYRO_LOG("----gyro_probe OK !!\n");
return 0;
if (err) {
GYRO_ERR("sysfs node creation error\n");
gyro_input_destroy(gyro_context_obj);
}
real_driver_init_fail:
exit_alloc_input_dev_failed:
kfree(gyro_context_obj);
exit_alloc_data_failed:
GYRO_ERR("----gyro_probe fail !!!\n");
return err;
}
static ssize_t gyro_show_delay(struct device* dev,
struct device_attribute *attr, char *buf)
{
int len = 0;
GYRO_LOG(" not support now\n");
return len;
}
int gyro_factory_device_init()
{
int error = 0;
struct gyro_context *cxt = gyro_context_obj;
if (!cxt->gyro_ctl.is_use_common_factory) {
GYRO_LOG("Node of '/dev/gyroscope' has already existed!\n");
return -1;
}
if ((error = misc_register(&gyro_factory_device)))
{
GYRO_LOG("gyro_factory_device register failed\n");
error = -1;
}
return error;
}
int gyro_driver_add(struct gyro_init_info* obj)
{
int err=0;
int i =0;
GYRO_FUN(f);
for(i =0; i < MAX_CHOOSE_GYRO_NUM; i++ )
{
if((i == 0) && (NULL == gyroscope_init_list[0])){
GYRO_LOG("register gyro driver for the first time\n");
if(platform_driver_register(&gyroscope_driver))
{
GYRO_ERR("failed to register gyro driver already exist\n");
}
}
if(NULL == gyroscope_init_list[i])
{
obj->platform_diver_addr = &gyroscope_driver;
gyroscope_init_list[i] = obj;
break;
}
}
if(NULL==gyroscope_init_list[i])
{
GYRO_ERR("gyro driver add err \n");
err=-1;
}
return err;
}
static ssize_t gyro_store_batch(struct device* dev, struct device_attribute *attr,
const char *buf, size_t count)
{
struct gyro_context *cxt = NULL;
//int err =0;
GYRO_LOG("gyro_store_batch buf=%s\n",buf);
mutex_lock(&gyro_context_obj->gyro_op_mutex);
cxt = gyro_context_obj;
if(cxt->gyro_ctl.is_support_batch){
if (!strncmp(buf, "1", 1))
{
cxt->is_batch_enable = true;
if(true == cxt->is_polling_run)
{
cxt->is_polling_run = false;
del_timer_sync(&cxt->timer);
cancel_work_sync(&cxt->report);
cxt->drv_data.gyro_data.values[0] = GYRO_INVALID_VALUE;
cxt->drv_data.gyro_data.values[1] = GYRO_INVALID_VALUE;
cxt->drv_data.gyro_data.values[2] = GYRO_INVALID_VALUE;
}
}
else if (!strncmp(buf, "0", 1))
{
cxt->is_batch_enable = false;
if(false == cxt->is_polling_run)
{
if(false == cxt->gyro_ctl.is_report_input_direct)
{
mod_timer(&cxt->timer, jiffies + atomic_read(&cxt->delay)/(1000/HZ));
cxt->is_polling_run = true;
}
}
}
else
{
GYRO_ERR(" gyro_store_batch error !!\n");
}
}else{
GYRO_LOG(" gyro_store_batch not support\n");
}
mutex_unlock(&gyro_context_obj->gyro_op_mutex);
GYRO_LOG(" gyro_store_batch done: %d\n", cxt->is_batch_enable);
return count;
}
static int gyro_clear_cali(void)
{
struct gyro_context *cxt = gyro_context_obj;
cxt->cali_sw[GYRO_AXIS_X] = 0;
cxt->cali_sw[GYRO_AXIS_Y] = 0;
cxt->cali_sw[GYRO_AXIS_Z] = 0;
GYRO_LOG(" GYRO after clear cali %d,%d,%d \n",cxt->cali_sw[GYRO_AXIS_X],cxt->cali_sw[GYRO_AXIS_Y],cxt->cali_sw[GYRO_AXIS_Z] );
return 0;
}
static int __init gyro_init(void)
{
GYRO_LOG("gyro_init\n");
if (gyro_probe()) {
GYRO_ERR("failed to register gyro driver\n");
return -ENODEV;
}
return 0;
}
static ssize_t gyro_store_batch(struct device *dev, struct device_attribute *attr,
const char *buf, size_t count)
{
struct gyro_context *cxt = NULL;
GYRO_LOG("gyro_store_batch buf=%s\n", buf);
mutex_lock(&gyro_context_obj->gyro_op_mutex);
cxt = gyro_context_obj;
if (cxt->gyro_ctl.is_support_batch) {
if (!strncmp(buf, "1", 1)) {
cxt->is_batch_enable = true;
if (true == cxt->is_polling_run) {
cxt->is_polling_run = false;
smp_mb(); /* for memory barrier */
stopTimer(&cxt->hrTimer);
smp_mb(); /* for memory barrier */
cancel_work_sync(&cxt->report);
cxt->drv_data.gyro_data.values[0] = GYRO_INVALID_VALUE;
cxt->drv_data.gyro_data.values[1] = GYRO_INVALID_VALUE;
cxt->drv_data.gyro_data.values[2] = GYRO_INVALID_VALUE;
}
} else if (!strncmp(buf, "0", 1)) {
cxt->is_batch_enable = false;
if (false == cxt->is_polling_run) {
if (false == cxt->gyro_ctl.is_report_input_direct) {
startTimer(&cxt->hrTimer, atomic_read(&cxt->delay), true);
cxt->is_polling_run = true;
}
}
} else
GYRO_ERR(" gyro_store_batch error !!\n");
} else
GYRO_LOG(" gyro_store_batch not support\n");
mutex_unlock(&gyro_context_obj->gyro_op_mutex);
GYRO_LOG(" gyro_store_batch done: %d\n", cxt->is_batch_enable);
return count;
}
int gyro_register_control_path(struct gyro_control_path *ctl)
{
struct gyro_context *cxt = NULL;
int err =0;
cxt = gyro_context_obj;
cxt->gyro_ctl.set_delay = ctl->set_delay;
cxt->gyro_ctl.open_report_data= ctl->open_report_data;
cxt->gyro_ctl.enable_nodata = ctl->enable_nodata;
cxt->gyro_ctl.is_support_batch = ctl->is_support_batch;
cxt->gyro_ctl.gyro_calibration = ctl->gyro_calibration;
cxt->gyro_ctl.is_use_common_factory = ctl->is_use_common_factory;
if(NULL==cxt->gyro_ctl.set_delay || NULL==cxt->gyro_ctl.open_report_data
|| NULL==cxt->gyro_ctl.enable_nodata)
{
GYRO_LOG("gyro register control path fail \n");
return -1;
}
//add misc dev for sensor hal control cmd
err = gyro_misc_init(gyro_context_obj);
if(err)
{
GYRO_ERR("unable to register gyro misc device!!\n");
return -2;
}
err = sysfs_create_group(&gyro_context_obj->mdev.this_device->kobj,
&gyro_attribute_group);
if (err < 0)
{
GYRO_ERR("unable to create gyro attribute file\n");
return -3;
}
kobject_uevent(&gyro_context_obj->mdev.this_device->kobj, KOBJ_ADD);
return 0;
}
/*----------------------------------------------------------------------------*/
static void gyro_late_resume(struct early_suspend *h)
{
atomic_set(&(gyro_context_obj->early_suspend), 0);
GYRO_LOG(" gyro_late_resume ok------->hwm_obj->early_suspend=%d \n",atomic_read(&(gyro_context_obj->early_suspend)));
return ;
}
static long gyro_factory_unlocked_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
{
void __user *data;
long err = 0;
struct gyro_context *cxt = gyro_context_obj;
int x,y,z,status;
char strbuf[256];
int cali[3] = {0};
SENSOR_DATA sensor_data = {0};
int smtRes;
if (_IOC_DIR(cmd) & _IOC_READ)
{
err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
}
else if (_IOC_DIR(cmd) & _IOC_WRITE)
{
err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
}
if (err)
{
GYRO_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
return -EFAULT;
}
switch (cmd)
{
case GYROSCOPE_IOCTL_INIT:
if(cxt->gyro_ctl.enable_nodata!= NULL){
err = cxt->gyro_ctl.enable_nodata(1);
if(err < 0)
{
GYRO_LOG("GYROSCOPE_IOCTL_READ_SENSORDATA read data fail!\n");
break;
}
GYRO_LOG("GYROSCOPE_IOCTL_INIT\n");
}else{
GYRO_LOG("GYROSCOPE_IOCTL_READ_SENSORDATA ");
}
break;
case GYROSCOPE_IOCTL_SMT_DATA:
data = (void __user *) arg;
if (data == NULL)
{
err = -EINVAL;
break;
}
smtRes = 1;
err = copy_to_user(data, &smtRes, sizeof(smtRes));
if (err)
{
err = -EINVAL;
GYRO_ERR("copy gyro data to user failed!\n");
}
break;
case GYROSCOPE_IOCTL_READ_SENSORDATA:
data = (void __user *) arg;
if (data == NULL)
{
err = -EINVAL;
break;
}
if(cxt->gyro_data.get_data != NULL){
err = cxt->gyro_data.get_data(&x, &y, &z, &status);
if(err < 0)
{
GYRO_LOG("GYROSCOPE_IOCTL_READ_SENSORDATA read data fail!\n");
break;
}
x+=cxt->cali_sw[0];
y+=cxt->cali_sw[1];
z+=cxt->cali_sw[2];
sprintf(strbuf, "%x %x %x", x, y, z);
GYRO_LOG("GYROSCOPE_IOCTL_READ_SENSORDATA read data : (%d, %d, %d)!\n", x, y, z);
GYRO_LOG("GYROSCOPE_IOCTL_READ_SENSORDATA read strbuf : (%s)!\n", strbuf);
if (copy_to_user(data, strbuf, strlen(strbuf)+1))
{
err = -EFAULT;
break;
}
}else{
GYRO_LOG("GYROSCOPE_IOCTL_READ_SENSORDATA ");
}
break;
case GYROSCOPE_IOCTL_READ_SENSORDATA_RAW:
data = (void __user *) arg;
if (data == NULL)
{
err = -EINVAL;
break;
}
if(cxt->gyro_data.get_raw_data != NULL){
err = cxt->gyro_data.get_raw_data(&x, &y, &z);
if(err < 0)
{
GYRO_LOG("GSENSOR_IOCTL_READ_RAW_DATA read data fail!\n");
break;
}
x+=cxt->cali_sw[0];
y+=cxt->cali_sw[1];
z+=cxt->cali_sw[2];
sprintf(strbuf, "%x %x %x", x, y, z);
GYRO_LOG("GSENSOR_IOCTL_READ_RAW_DATA read data : (%d, %d, %d)!\n", x, y, z);
if (copy_to_user(data, strbuf, strlen(strbuf)+1))
{
err = -EFAULT;
break;
}
}else{
GYRO_LOG("GSENSOR_IOCTL_READ_RAW_DATA FAIL!\n ");
}
break;
case GYROSCOPE_IOCTL_SET_CALI:
data = (void __user*)arg;
if (data == NULL)
{
err = -EINVAL;
break;
}
if (copy_from_user(&sensor_data, data, sizeof(sensor_data)))
{
err = -EFAULT;
break;
}
cali[0] = sensor_data.x ;
cali[1] = sensor_data.y ;
cali[2] = sensor_data.z ;
GYRO_LOG("GYROSCOPE_IOCTL_SET_CALI data : (%d, %d, %d)!\n", cali[0], cali[1], cali[2]);
gyro_set_cali(cali);
/*
if(cxt->gyro_ctl.gyro_calibration != NULL)
{
err = cxt->gyro_ctl.gyro_calibration(SETCALI, cali);
if(err < 0)
{
GYRO_LOG("GYROSCOPE_IOCTL_SET_CALI fail!\n");
break;
}
}
*/
break;
case GYROSCOPE_IOCTL_CLR_CALI:
/*
if(cxt->gyro_ctl.gyro_calibration != NULL)
{
err = cxt->gyro_ctl.gyro_calibration(CLRCALI, cali);
if(err < 0)
{
GYRO_LOG("GYROSCOPE_IOCTL_CLR_CALI fail!\n");
break;
}
}
*/
gyro_clear_cali();
break;
case GYROSCOPE_IOCTL_GET_CALI:
data = (void __user*)arg;
if (data == NULL)
{
err = -EINVAL;
break;
}
/*
if(cxt->gyro_ctl.gyro_calibration != NULL)
{
err = cxt->gyro_ctl.gyro_calibration(GETCALI, cali);
if(err < 0)
{
GYRO_LOG("GYROSCOPE_IOCTL_GET_CALI fail!\n");
break;
}
}
*/
GYRO_LOG("GYROSCOPE_IOCTL_GET_CALI data : (%d, %d, %d)!\n", cxt->cali_sw[0] , cxt->cali_sw[1], cxt->cali_sw[2]);
sensor_data.x = cxt->cali_sw[0]; ;
sensor_data.y = cxt->cali_sw[1]; ;
sensor_data.z = cxt->cali_sw[2]; ;
if (copy_to_user(data, &sensor_data, sizeof(sensor_data)))
{
err = -EFAULT;
break;
}
break;
default:
GYRO_LOG("unknown IOCTL: 0x%08x\n", cmd);
err = -ENOIOCTLCMD;
break;
}
return err;
}
static void gyro_work_func(struct work_struct *work)
{
struct gyro_context *cxt = NULL;
int x, y, z, status;
int64_t pre_ns, cur_ns;
int64_t delay_ms;
int err = 0;
cxt = gyro_context_obj;
delay_ms = atomic_read(&cxt->delay);
if (NULL == cxt->gyro_data.get_data)
GYRO_ERR("gyro driver not register data path\n");
cur_ns = getCurNS();
/* add wake lock to make sure data can be read before system suspend */
cxt->gyro_data.get_data(&x, &y, &z, &status);
if (err) {
GYRO_ERR("get gyro data fails!!\n");
goto gyro_loop;
} else {
cxt->drv_data.gyro_data.values[0] = x+cxt->cali_sw[0];
cxt->drv_data.gyro_data.values[1] = y+cxt->cali_sw[1];
cxt->drv_data.gyro_data.values[2] = z+cxt->cali_sw[2];
cxt->drv_data.gyro_data.status = status;
pre_ns = cxt->drv_data.gyro_data.time;
cxt->drv_data.gyro_data.time = cur_ns;
}
if (true == cxt->is_first_data_after_enable) {
pre_ns = cur_ns;
cxt->is_first_data_after_enable = false;
/* filter -1 value */
if (GYRO_INVALID_VALUE == cxt->drv_data.gyro_data.values[0] ||
GYRO_INVALID_VALUE == cxt->drv_data.gyro_data.values[1] ||
GYRO_INVALID_VALUE == cxt->drv_data.gyro_data.values[2]) {
GYRO_LOG(" read invalid data\n");
goto gyro_loop;
}
}
/* GYRO_LOG("gyro data[%d,%d,%d]\n" ,cxt->drv_data.gyro_data.values[0], */
/* cxt->drv_data.gyro_data.values[1],cxt->drv_data.gyro_data.values[2]); */
while ((cur_ns - pre_ns) >= delay_ms*1800000LL) {
pre_ns += delay_ms*1000000LL;
gyro_data_report(cxt->drv_data.gyro_data.values[0],
cxt->drv_data.gyro_data.values[1], cxt->drv_data.gyro_data.values[2],
cxt->drv_data.gyro_data.status, pre_ns);
}
gyro_data_report(cxt->drv_data.gyro_data.values[0],
cxt->drv_data.gyro_data.values[1], cxt->drv_data.gyro_data.values[2],
cxt->drv_data.gyro_data.status, cxt->drv_data.gyro_data.time);
gyro_loop:
if (true == cxt->is_polling_run)
startTimer(&cxt->hrTimer, atomic_read(&cxt->delay), false);
}
static int gyroscope_probe(struct platform_device *pdev)
{
GYRO_LOG("gyroscope_probe\n");
return 0;
}
static void gyro_work_func(struct work_struct *work)
{
struct gyro_context *cxt = NULL;
//int out_size;
//hwm_sensor_data sensor_data;
int x,y,z,status;
int64_t nt;
struct timespec time;
int err = 0;
cxt = gyro_context_obj;
if(NULL == cxt->gyro_data.get_data)
{
GYRO_ERR("gyro driver not register data path\n");
}
time.tv_sec = time.tv_nsec = 0;
time = get_monotonic_coarse();
nt = time.tv_sec*1000000000LL+time.tv_nsec;
//add wake lock to make sure data can be read before system suspend
cxt->gyro_data.get_data(&x,&y,&z,&status);
if(err)
{
GYRO_ERR("get gyro data fails!!\n" );
goto gyro_loop;
}
else
{
if((x != cxt->drv_data.gyro_data.values[0])
|| (y != cxt->drv_data.gyro_data.values[1])
|| (z != cxt->drv_data.gyro_data.values[2]))
{
if( 0 == x && 0==y
&& 0 == z)
{
goto gyro_loop;
}
cxt->drv_data.gyro_data.values[0] = x;
cxt->drv_data.gyro_data.values[1] = y;
cxt->drv_data.gyro_data.values[2] = z;
cxt->drv_data.gyro_data.status = status;
cxt->drv_data.gyro_data.time = nt;
}
}
if(true == cxt->is_first_data_after_enable)
{
cxt->is_first_data_after_enable = false;
//filter -1 value
if(GYRO_INVALID_VALUE == cxt->drv_data.gyro_data.values[0] ||
GYRO_INVALID_VALUE == cxt->drv_data.gyro_data.values[1] ||
GYRO_INVALID_VALUE == cxt->drv_data.gyro_data.values[2])
{
GYRO_LOG(" read invalid data \n");
goto gyro_loop;
}
}
//report data to input device
//printk("new gyro work run....\n");
//GYRO_LOG("gyro data[%d,%d,%d] \n" ,cxt->drv_data.gyro_data.values[0],
//cxt->drv_data.gyro_data.values[1],cxt->drv_data.gyro_data.values[2]);
gyro_data_report(cxt->drv_data.gyro_data.values[0],
cxt->drv_data.gyro_data.values[1],cxt->drv_data.gyro_data.values[2],
cxt->drv_data.gyro_data.status);
gyro_loop:
if(true == cxt->is_polling_run)
{
{
mod_timer(&cxt->timer, jiffies + atomic_read(&cxt->delay)/(1000/HZ));
}
}
}