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)); } } }