int acc_driver_add(struct acc_init_info* obj) { int err=0; int i =0; ACC_FUN(); for(i =0; i < MAX_CHOOSE_G_NUM; i++ ) { if(i == 0){ ACC_LOG("register gensor driver for the first time\n"); if(platform_driver_register(&gsensor_driver)) { ACC_ERR("failed to register gensor driver already exist\n"); } } if(NULL == gsensor_init_list[i]) { obj->platform_diver_addr = &gsensor_driver; gsensor_init_list[i] = obj; break; } } if(NULL==gsensor_init_list[i]) { ACC_ERR("ACC driver add err \n"); err=-1; } return err; }
static int acc_real_enable(int enable) { int err = 0; struct acc_context *cxt = NULL; cxt = acc_context_obj; if (1 == enable) { if (true == cxt->is_active_data || true == cxt->is_active_nodata) { err = cxt->acc_ctl.enable_nodata(1); if (err) { err = cxt->acc_ctl.enable_nodata(1); if (err) { err = cxt->acc_ctl.enable_nodata(1); if (err) ACC_ERR("acc enable(%d) err 3 timers = %d\n", enable, err); } } ACC_LOG("acc real enable\n"); } } if (0 == enable) { if (false == cxt->is_active_data && false == cxt->is_active_nodata) { err = cxt->acc_ctl.enable_nodata(0); if (err) ACC_ERR("acc enable(%d) err = %d\n", enable, err); ACC_LOG("acc real disable\n"); } } return err; }
int acc_register_control_path(struct acc_control_path *ctl) { struct acc_context *cxt = NULL; int err = 0; cxt = acc_context_obj; cxt->acc_ctl.set_delay = ctl->set_delay; cxt->acc_ctl.open_report_data = ctl->open_report_data; cxt->acc_ctl.enable_nodata = ctl->enable_nodata; cxt->acc_ctl.is_support_batch = ctl->is_support_batch; cxt->acc_ctl.is_report_input_direct = ctl->is_report_input_direct; cxt->acc_ctl.acc_calibration = ctl->acc_calibration; if (NULL == cxt->acc_ctl.set_delay || NULL == cxt->acc_ctl.open_report_data || NULL == cxt->acc_ctl.enable_nodata) { ACC_LOG("acc register control path fail\n"); return -1; } /* add misc dev for sensor hal control cmd */ err = acc_misc_init(acc_context_obj); if (err) { ACC_ERR("unable to register acc misc device!!\n"); return -2; } err = sysfs_create_group(&acc_context_obj->mdev.this_device->kobj, &acc_attribute_group); if (err < 0) { ACC_ERR("unable to create acc attribute file\n"); return -3; } kobject_uevent(&acc_context_obj->mdev.this_device->kobj, KOBJ_ADD); return 0; }
static ssize_t acc_store_delay(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { int64_t delay = 0; int64_t mdelay = 0; int ret = 0; struct acc_context *cxt = NULL; mutex_lock(&acc_context_obj->acc_op_mutex); cxt = acc_context_obj; if (NULL == cxt->acc_ctl.set_delay) { ACC_LOG("acc_ctl set_delay NULL\n"); mutex_unlock(&acc_context_obj->acc_op_mutex); return count; } ret = kstrtoll(buf, 10, &delay); if (ret != 0) { ACC_ERR("invalid format!!\n"); mutex_unlock(&acc_context_obj->acc_op_mutex); return count; } if (false == cxt->acc_ctl.is_report_input_direct) { mdelay = delay; do_div(mdelay, 1000000); atomic_set(&acc_context_obj->delay, mdelay); } cxt->acc_ctl.set_delay(delay); ACC_LOG(" acc_delay %lld ns\n", delay); mutex_unlock(&acc_context_obj->acc_op_mutex); return count; }
static ssize_t acc_store_enable_nodata(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { struct acc_context *cxt = NULL; ACC_LOG("acc_store_enable nodata buf=%s\n", buf); mutex_lock(&acc_context_obj->acc_op_mutex); cxt = acc_context_obj; if (NULL == cxt->acc_ctl.enable_nodata) { ACC_LOG("acc_ctl enable nodata NULL\n"); mutex_unlock(&acc_context_obj->acc_op_mutex); return count; } if (!strncmp(buf, "1", 1)) { /* cxt->acc_ctl.enable_nodata(1); */ acc_enable_nodata(1); } else if (!strncmp(buf, "0", 1)) { /* cxt->acc_ctl.enable_nodata(0); */ acc_enable_nodata(0); } else { ACC_ERR(" acc_store enable nodata cmd error !!\n"); } mutex_unlock(&acc_context_obj->acc_op_mutex); return count; }
static void startTimer(struct hrtimer *timer, int delay_ms, bool first) { struct acc_context *obj = (struct acc_context *)container_of(timer, struct acc_context, hrTimer); static int count; if (obj == NULL) { ACC_ERR("NULL pointer\n"); return; } if (first) { obj->target_ktime = ktime_add_ns(ktime_get(), (int64_t)delay_ms*1000000); /* ACC_LOG("%d, cur_nt = %lld, delay_ms = %d, target_nt = %lld\n", count, getCurNT(), delay_ms, ktime_to_us(obj->target_ktime)); */ count = 0; } else { do { obj->target_ktime = ktime_add_ns(obj->target_ktime, (int64_t)delay_ms*1000000); } while (ktime_to_ns(obj->target_ktime) < ktime_to_ns(ktime_get())); /* ACC_LOG("%d, cur_nt = %lld, delay_ms = %d, target_nt = %lld\n", count, getCurNT(), delay_ms, ktime_to_us(obj->target_ktime)); */ count++; } hrtimer_start(timer, obj->target_ktime, HRTIMER_MODE_ABS); }
static ssize_t acc_store_active(struct device* dev, struct device_attribute *attr, const char *buf, size_t count) { struct acc_context *cxt = NULL; //int err =0; ACC_LOG("acc_store_active buf=%s\n",buf); mutex_lock(&acc_context_obj->acc_op_mutex); cxt = acc_context_obj; if(NULL == cxt->acc_ctl.open_report_data) { ACC_LOG("acc_ctl enable NULL\n"); mutex_unlock(&acc_context_obj->acc_op_mutex); return count; } if (!strncmp(buf, "1", 1)) { // cxt->acc_ctl.enable(1); acc_enable_data(1); } else if (!strncmp(buf, "0", 1)) { //cxt->acc_ctl.enable(0); acc_enable_data(0); } else { ACC_ERR(" acc_store_active error !!\n"); } mutex_unlock(&acc_context_obj->acc_op_mutex); ACC_LOG(" acc_store_active done\n"); return count; }
static struct acc_context *acc_context_alloc_object(void) { struct acc_context *obj = kzalloc(sizeof(*obj), GFP_KERNEL); ACC_LOG("acc_context_alloc_object++++\n"); if (!obj) { ACC_ERR("Alloc accel 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, acc_work_func); obj->accel_workqueue = NULL; obj->accel_workqueue = create_workqueue("accel_polling"); if (!obj->accel_workqueue) { kfree(obj); return NULL; } initTimer(&obj->hrTimer, acc_poll); obj->is_first_data_after_enable = false; obj->is_polling_run = false; mutex_init(&obj->acc_op_mutex); obj->is_batch_enable = false;/* for batch mode init */ obj->cali_sw[ACC_AXIS_X] = 0; obj->cali_sw[ACC_AXIS_Y] = 0; obj->cali_sw[ACC_AXIS_Z] = 0; ACC_LOG("acc_context_alloc_object----\n"); return obj; }
static ssize_t acc_store_delay(struct device* dev, struct device_attribute *attr, const char *buf, size_t count) { // struct acc_context *devobj = (struct acc_context*)dev_get_drvdata(dev); int delay = 0; int mdelay=0; struct acc_context *cxt = NULL; //int err =0; mutex_lock(&acc_context_obj->acc_op_mutex); cxt = acc_context_obj; if(NULL == cxt->acc_ctl.set_delay) { ACC_LOG("acc_ctl set_delay NULL\n"); mutex_unlock(&acc_context_obj->acc_op_mutex); return count; } if (1 != sscanf(buf, "%d", &delay)) { ACC_ERR("invalid format!!\n"); mutex_unlock(&acc_context_obj->acc_op_mutex); return count; } if(false == cxt->acc_ctl.is_report_input_direct) { mdelay = (int)delay/1000/1000; atomic_set(&acc_context_obj->delay, mdelay); } cxt->acc_ctl.set_delay(delay); ACC_LOG(" acc_delay %d ns\n",delay); mutex_unlock(&acc_context_obj->acc_op_mutex); return count; }
static ssize_t acc_store_batch(struct device* dev, struct device_attribute *attr, const char *buf, size_t count) { struct acc_context *cxt = NULL; mutex_lock(&acc_context_obj->acc_op_mutex); ACC_LOG("acc_store_batch buf=%s\n",buf); cxt = acc_context_obj; if(cxt->acc_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 { ACC_ERR(" acc_store_batch error !!\n"); } } else { ACC_LOG(" acc_store_batch mot supported\n"); } mutex_unlock(&acc_context_obj->acc_op_mutex); ACC_LOG(" acc_store_batch done: %d\n", cxt->is_batch_enable); return count; }
static struct acc_context *acc_context_alloc_object(void) { struct acc_context *obj = kzalloc(sizeof(*obj), GFP_KERNEL); ACC_LOG("acc_context_alloc_object++++\n"); if(!obj) { ACC_ERR("Alloc accel 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, acc_work_func); init_timer(&obj->timer); obj->timer.expires = jiffies + atomic_read(&obj->delay)/(1000/HZ); obj->timer.function = acc_poll; obj->timer.data = (unsigned long)obj; obj->is_first_data_after_enable = false; obj->is_polling_run = false; mutex_init(&obj->acc_op_mutex); obj->is_batch_enable = false;//for batch mode init obj->cali_sw[ACC_AXIS_X]=0; obj->cali_sw[ACC_AXIS_Y]=0; obj->cali_sw[ACC_AXIS_Z]=0; ACC_LOG("acc_context_alloc_object----\n"); return obj; }
static int acc_probe(struct platform_device *pdev) { int err = 0; ACC_LOG("+++++++++++++accel_probe!!\n"); acc_context_obj = acc_context_alloc_object(); if (!acc_context_obj) { err = -ENOMEM; ACC_ERR("unable to allocate devobj!\n"); goto exit_alloc_data_failed; } //init real acceleration driver err = acc_real_driver_init(); if(err) { ACC_ERR("acc real driver init fail\n"); goto real_driver_init_fail; } //init input dev err = acc_input_init(acc_context_obj); if(err) { ACC_ERR("unable to register acc input device!\n"); goto exit_alloc_input_dev_failed; } err = sm_input_init(); if(err) { ACC_ERR("unable to register acc input device!\n"); goto exit_alloc_input_dev_failed; } atomic_set(&(acc_context_obj->early_suspend), 0); acc_context_obj->early_drv.level = EARLY_SUSPEND_LEVEL_STOP_DRAWING - 1, acc_context_obj->early_drv.suspend = acc_early_suspend, acc_context_obj->early_drv.resume = acc_late_resume, register_early_suspend(&acc_context_obj->early_drv); ACC_LOG("----accel_probe OK !!\n"); return 0; real_driver_init_fail: exit_alloc_input_dev_failed: kfree(acc_context_obj); exit_alloc_data_failed: ACC_LOG("----accel_probe fail !!!\n"); return err; }
static int acc_probe(void) { int err; ACC_LOG("+++++++++++++accel_probe!!\n"); acc_context_obj = acc_context_alloc_object(); if (!acc_context_obj) { err = -ENOMEM; ACC_ERR("unable to allocate devobj!\n"); goto exit_alloc_data_failed; } /* init real acceleration driver */ err = acc_real_driver_init(); if (err) { ACC_ERR("acc real driver init fail\n"); goto real_driver_init_fail; } /* init acc common factory mode misc device */ err = acc_factory_device_init(); if (err) ACC_ERR("acc factory device already registed\n"); /* init input dev */ err = acc_input_init(acc_context_obj); if (err) { ACC_ERR("unable to register acc input device!\n"); goto exit_alloc_input_dev_failed; } ACC_LOG("----accel_probe OK !!\n"); return 0; real_driver_init_fail: exit_alloc_input_dev_failed: kfree(acc_context_obj); exit_alloc_data_failed: ACC_ERR("----accel_probe fail !!!\n"); return err; }
static int __init acc_init(void) { ACC_FUN(); if(platform_driver_register(&acc_driver)) { ACC_ERR("failed to register acc driver\n"); return -ENODEV; } return 0; }
static int acc_factory_open(struct inode *inode, struct file *file) { file->private_data = acc_context_obj; if (file->private_data == NULL) { ACC_ERR("null pointer!!\n"); return -EINVAL; } return nonseekable_open(inode, file); }
static int __init acc_init(void) { ACC_LOG("acc_init\n"); if (acc_probe()) { ACC_ERR("failed to register acc driver\n"); return -ENODEV; } return 0; }
static int acc_misc_init(struct acc_context *cxt) { int err=0; cxt->mdev.minor = MISC_DYNAMIC_MINOR; cxt->mdev.name = ACC_MISC_DEV_NAME; if((err = misc_register(&cxt->mdev))) { ACC_ERR("unable to register acc misc device!!\n"); } //dev_set_drvdata(cxt->mdev.this_device, cxt); return err; }
static int acc_remove(struct platform_device *pdev) { int err=0; ACC_FUN(f); input_unregister_device(acc_context_obj->idev); sysfs_remove_group(&acc_context_obj->idev->dev.kobj, &acc_attribute_group); if((err = misc_deregister(&acc_context_obj->mdev))) { ACC_ERR("misc_deregister fail: %d\n", err); } kfree(acc_context_obj); return 0; }
static int acc_remove(void) { int err = 0; input_unregister_device(acc_context_obj->idev); sysfs_remove_group(&acc_context_obj->idev->dev.kobj, &acc_attribute_group); err = misc_deregister(&acc_context_obj->mdev); if (err) ACC_ERR("misc_deregister fail: %d\n", err); kfree(acc_context_obj); return 0; }
static int acc_enable_data(int enable) { struct acc_context *cxt = NULL; //int err =0; cxt = acc_context_obj; if(NULL == cxt->acc_ctl.open_report_data) { ACC_ERR("no acc control path\n"); return -1; } if(1 == enable) { ACC_LOG("ACC enable data\n"); cxt->is_active_data =true; cxt->is_first_data_after_enable = true; cxt->acc_ctl.open_report_data(1); if(false == cxt->is_polling_run && cxt->is_batch_enable == false) { if(false == cxt->acc_ctl.is_report_input_direct) { mod_timer(&cxt->timer, jiffies + atomic_read(&cxt->delay)/(1000/HZ)); cxt->is_polling_run = true; } } } if(0 == enable) { ACC_LOG("ACC disable \n"); cxt->is_active_data =false; cxt->acc_ctl.open_report_data(0); if(true == cxt->is_polling_run) { if(false == cxt->acc_ctl.is_report_input_direct) { cxt->is_polling_run = false; del_timer_sync(&cxt->timer); cancel_work_sync(&cxt->report); cxt->drv_data.acc_data.values[0] = ACC_INVALID_VALUE; cxt->drv_data.acc_data.values[1] = ACC_INVALID_VALUE; cxt->drv_data.acc_data.values[2] = ACC_INVALID_VALUE; } } } acc_real_enable(enable); return 0; }
static ssize_t acc_store_batch(struct device* dev, struct device_attribute *attr, const char *buf, size_t count) { struct acc_context *cxt = NULL; //int err =0; ACC_LOG("acc_store_batch buf=%s\n",buf); mutex_lock(&acc_context_obj->acc_op_mutex); cxt = acc_context_obj; if(cxt->acc_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.acc_data.values[0] = ACC_INVALID_VALUE; cxt->drv_data.acc_data.values[1] = ACC_INVALID_VALUE; cxt->drv_data.acc_data.values[2] = ACC_INVALID_VALUE; } } else if (!strncmp(buf, "0", 1)) { cxt->is_batch_enable = false; if(false == cxt->is_polling_run) { if(false == cxt->acc_ctl.is_report_input_direct) { mod_timer(&cxt->timer, jiffies + atomic_read(&cxt->delay)/(1000/HZ)); cxt->is_polling_run = true; } } } else { ACC_ERR(" acc_store_batch error !!\n"); } }else{ ACC_LOG(" acc_store_batch mot supported\n"); } mutex_unlock(&acc_context_obj->acc_op_mutex); ACC_LOG(" acc_store_batch done: %d\n", cxt->is_batch_enable); return count; }
int acc_enable_nodata(int enable) { struct acc_context *cxt = NULL; cxt = acc_context_obj; if (NULL == cxt->acc_ctl.enable_nodata) { ACC_ERR("acc_enable_nodata:acc ctl path is NULL\n"); return -1; } if (1 == enable) cxt->is_active_nodata = true; if (0 == enable) cxt->is_active_nodata = false; acc_real_enable(enable); return 0; }
static int acc_enable_data(int enable) { struct acc_context *cxt = NULL; cxt = acc_context_obj; if (NULL == cxt->acc_ctl.open_report_data) { ACC_ERR("no acc control path\n"); return -1; } if (1 == enable) { ACC_LOG("ACC enable data\n"); cxt->is_active_data = true; cxt->is_first_data_after_enable = true; cxt->acc_ctl.open_report_data(1); acc_real_enable(enable); if (false == cxt->is_polling_run && cxt->is_batch_enable == false) { if (false == cxt->acc_ctl.is_report_input_direct) { startTimer(&cxt->hrTimer, atomic_read(&cxt->delay), true); cxt->is_polling_run = true; } } } if (0 == enable) { ACC_LOG("ACC disable\n"); cxt->is_active_data = false; cxt->acc_ctl.open_report_data(0); if (true == cxt->is_polling_run) { if (false == cxt->acc_ctl.is_report_input_direct) { 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.acc_data.values[0] = ACC_INVALID_VALUE; cxt->drv_data.acc_data.values[1] = ACC_INVALID_VALUE; cxt->drv_data.acc_data.values[2] = ACC_INVALID_VALUE; } } acc_real_enable(enable); } return 0; }
static ssize_t acc_store_batch(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { struct acc_context *cxt = NULL; ACC_LOG("acc_store_batch buf=%s\n", buf); mutex_lock(&acc_context_obj->acc_op_mutex); cxt = acc_context_obj; if (cxt->acc_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.acc_data.values[0] = ACC_INVALID_VALUE; cxt->drv_data.acc_data.values[1] = ACC_INVALID_VALUE; cxt->drv_data.acc_data.values[2] = ACC_INVALID_VALUE; } } else if (!strncmp(buf, "0", 1)) { cxt->is_batch_enable = false; if (false == cxt->is_polling_run) { if (false == cxt->acc_ctl.is_report_input_direct && true == cxt->is_active_data) { startTimer(&cxt->hrTimer, atomic_read(&cxt->delay), true); cxt->is_polling_run = true; } } } else ACC_ERR(" acc_store_batch error !!\n"); } else ACC_LOG(" acc_store_batch mot supported\n"); mutex_unlock(&acc_context_obj->acc_op_mutex); ACC_LOG(" acc_store_batch done: %d\n", cxt->is_batch_enable); return count; }
static int acc_probe(struct platform_device *pdev) { int err; ACC_LOG("+++++++++++++accel_probe!!\n"); acc_context_obj = acc_context_alloc_object(); if (!acc_context_obj) { err = -ENOMEM; ACC_ERR("unable to allocate devobj!\n"); goto exit_alloc_data_failed; } //init real acceleration driver err = acc_real_driver_init(); if(err) { ACC_ERR("acc real driver init fail\n"); goto real_driver_init_fail; } //init acc common factory mode misc device err = acc_factory_device_init(); if(err) { ACC_ERR("acc factory device already registed\n"); } //init input dev err = acc_input_init(acc_context_obj); if(err) { ACC_ERR("unable to register acc input device!\n"); goto exit_alloc_input_dev_failed; } #if defined(CONFIG_HAS_EARLYSUSPEND) atomic_set(&(acc_context_obj->early_suspend), 0); acc_context_obj->early_drv.level = EARLY_SUSPEND_LEVEL_STOP_DRAWING - 1, acc_context_obj->early_drv.suspend = acc_early_suspend, acc_context_obj->early_drv.resume = acc_late_resume, register_early_suspend(&acc_context_obj->early_drv); #endif ACC_LOG("----accel_probe OK !!\n"); return 0; //exit_hwmsen_create_attr_failed: //exit_misc_register_failed: //exit_err_sysfs: if (err) { ACC_ERR("sysfs node creation error \n"); acc_input_destroy(acc_context_obj); } real_driver_init_fail: exit_alloc_input_dev_failed: kfree(acc_context_obj); exit_alloc_data_failed: ACC_LOG("----accel_probe fail !!!\n"); return err; }
static void acc_work_func(struct work_struct *work) { struct acc_context *cxt = NULL; //int out_size; //hwm_sensor_data sensor_data; int x,y,z,status; int64_t nt; struct timespec time; int err; cxt = acc_context_obj; if(NULL == cxt->acc_data.get_data) { ACC_LOG("acc 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; err = cxt->acc_data.get_data(&x,&y,&z,&status); if(err) { ACC_ERR("get acc data fails!!\n" ); goto acc_loop; } else { { if( 0 == x && 0==y && 0 == z) { goto acc_loop; } cxt->drv_data.acc_data.values[0] = x+cxt->cali_sw[0]; cxt->drv_data.acc_data.values[1] = y+cxt->cali_sw[1]; cxt->drv_data.acc_data.values[2] = z+cxt->cali_sw[2]; cxt->drv_data.acc_data.status = status; cxt->drv_data.acc_data.time = nt; } } if(true == cxt->is_first_data_after_enable) { cxt->is_first_data_after_enable = false; //filter -1 value if(ACC_INVALID_VALUE == cxt->drv_data.acc_data.values[0] || ACC_INVALID_VALUE == cxt->drv_data.acc_data.values[1] || ACC_INVALID_VALUE == cxt->drv_data.acc_data.values[2]) { ACC_LOG(" read invalid data \n"); goto acc_loop; } } //report data to input device //printk("new acc work run....\n"); //ACC_LOG("acc data[%d,%d,%d] \n" ,cxt->drv_data.acc_data.values[0], //cxt->drv_data.acc_data.values[1],cxt->drv_data.acc_data.values[2]); acc_data_report(cxt->drv_data.acc_data.values[0], cxt->drv_data.acc_data.values[1],cxt->drv_data.acc_data.values[2], cxt->drv_data.acc_data.status); acc_loop: if(true == cxt->is_polling_run) { mod_timer(&cxt->timer, jiffies + atomic_read(&cxt->delay)/(1000/HZ)); } }
static long acc_factory_unlocked_ioctl(struct file *file, unsigned int cmd, unsigned long arg) { void __user *data; int err = 0; struct acc_context *cxt = acc_context_obj; int x,y,z; char strbuf[256]; int cali[3] = {0}; SENSOR_DATA sensor_data = {0}; 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) { ACC_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd)); return -EFAULT; } switch (cmd) { case GSENSOR_IOCTL_INIT: break; case GSENSOR_IOCTL_READ_CHIPINFO: break; case GSENSOR_IOCTL_READ_SENSORDATA: data = (void __user *) arg; if (data == NULL) { err = -EINVAL; break; } if(cxt->acc_ctl.enable_nodata != NULL){ err = cxt->acc_ctl.enable_nodata(1); if(err) { err = cxt->acc_ctl.enable_nodata(1); if(err) { err = cxt->acc_ctl.enable_nodata(1); if(err) ACC_ERR("acc ioctl enable err 3 timers = %d\n", err); } } ACC_LOG("acc ioctl real enable \n" ); } if(cxt->acc_data.get_data != NULL){ //err = cxt->acc_data.get_data(&x, &y, &z, &status); err = cxt->acc_data.get_raw_data(&x, &y, &z); if(err < 0) { ACC_ERR("GSENSOR_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); ACC_LOG("GSENSOR_IOCTL_READ_SENSORDATA read data : (%d, %d, %d)!\n", x, y, z); ACC_LOG("GSENSOR_IOCTL_READ_SENSORDATA read strbuf : (%s)!\n", strbuf); if (copy_to_user(data, strbuf, strlen(strbuf)+1)) { err = -EFAULT; break; } }else{ ACC_ERR("GSENSOR_IOCTL_READ_SENSORDATA "); } break; case GSENSOR_IOCTL_READ_RAW_DATA: data = (void __user *) arg; if (data == NULL) { err = -EINVAL; break; } if(cxt->acc_data.get_raw_data != NULL){ err = cxt->acc_data.get_raw_data(&x, &y, &z); if(err < 0) { ACC_ERR("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); ACC_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{ ACC_ERR("GSENSOR_IOCTL_READ_SENSORDATA "); } break; case GSENSOR_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 ; ACC_LOG("GSENSOR_IOCTL_SET_CALI data : (%d, %d, %d)!\n", cali[0], cali[1], cali[2]); acc_set_cali(cali); /*if(cxt->acc_ctl.acc_calibration != NULL) { err = cxt->acc_ctl.acc_calibration(SETCALI, cali); if(err < 0) { ACC_ERR("GSENSOR_IOCTL_SET_CALI fail!\n"); break; } } */ break; case GSENSOR_IOCTL_CLR_CALI: /* if(cxt->acc_ctl.acc_calibration != NULL) { err = cxt->acc_ctl.acc_calibration(CLRCALI, cali); if(err < 0) { ACC_ERR("GSENSOR_IOCTL_CLR_CALI fail!\n"); break; } } */ acc_clear_cali(); break; case GSENSOR_IOCTL_GET_CALI: data = (void __user*)arg; if (data == NULL) { err = -EINVAL; break; } /* if(cxt->acc_ctl.acc_calibration != NULL) { err = cxt->acc_ctl.acc_calibration(GETCALI, cali); if(err < 0) { ACC_ERR("GSENSOR_IOCTL_GET_CALI fail!\n"); break; } } */ ACC_LOG("GSENSOR_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: ACC_ERR("unknown IOCTL: 0x%08x\n", cmd); err = -ENOIOCTLCMD; break; } return err; }
static void acc_work_func(struct work_struct *work) { struct acc_context *cxt = NULL; int x, y, z, status; int64_t pre_ns, cur_ns; int64_t delay_ms; int err; cxt = acc_context_obj; delay_ms = atomic_read(&cxt->delay); if (NULL == cxt->acc_data.get_data) { ACC_ERR("acc driver not register data path\n"); return; } cur_ns = getCurNS(); err = cxt->acc_data.get_data(&x, &y, &z, &status); if (err) { ACC_ERR("get acc data fails!!\n"); goto acc_loop; } else { if (0 == x && 0 == y && 0 == z) goto acc_loop; cxt->drv_data.acc_data.values[0] = x; cxt->drv_data.acc_data.values[1] = y; cxt->drv_data.acc_data.values[2] = z; cxt->drv_data.acc_data.status = status; pre_ns = cxt->drv_data.acc_data.time; cxt->drv_data.acc_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 (ACC_INVALID_VALUE == cxt->drv_data.acc_data.values[0] || ACC_INVALID_VALUE == cxt->drv_data.acc_data.values[1] || ACC_INVALID_VALUE == cxt->drv_data.acc_data.values[2]) { ACC_LOG(" read invalid data\n"); goto acc_loop; } } /* report data to input device */ /* printk("new acc work run....\n"); */ /* ACC_LOG("acc data[%d,%d,%d]\n" ,cxt->drv_data.acc_data.values[0], */ /* cxt->drv_data.acc_data.values[1],cxt->drv_data.acc_data.values[2]); */ while ((cur_ns - pre_ns) >= delay_ms*1800000LL) { pre_ns += delay_ms*1000000LL; acc_data_report(cxt->drv_data.acc_data.values[0], cxt->drv_data.acc_data.values[1], cxt->drv_data.acc_data.values[2], cxt->drv_data.acc_data.status, pre_ns); } acc_data_report(cxt->drv_data.acc_data.values[0], cxt->drv_data.acc_data.values[1], cxt->drv_data.acc_data.values[2], cxt->drv_data.acc_data.status, cxt->drv_data.acc_data.time); acc_loop: if (true == cxt->is_polling_run) startTimer(&cxt->hrTimer, atomic_read(&cxt->delay), false); }