Ejemplo n.º 1
0
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;
}
Ejemplo n.º 2
0
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;
}
Ejemplo n.º 3
0
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;
}
Ejemplo n.º 4
0
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);
}
Ejemplo n.º 5
0
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;
}
Ejemplo n.º 6
0
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;
}
Ejemplo n.º 7
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;
    //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;

}
Ejemplo n.º 8
0
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;
}
Ejemplo n.º 9
0
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;
}
Ejemplo n.º 10
0
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;
}
Ejemplo n.º 12
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);
}
Ejemplo n.º 13
0
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;
}
Ejemplo n.º 14
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;
}
Ejemplo n.º 15
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;
}
Ejemplo n.º 16
0
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;
}
Ejemplo n.º 18
0
  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;
}
Ejemplo n.º 19
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;
	//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;
}
Ejemplo n.º 21
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;
}
Ejemplo n.º 22
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;
}
Ejemplo n.º 23
0
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;	
}
Ejemplo n.º 24
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;
}
Ejemplo n.º 26
0
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);
}
Ejemplo n.º 27
0
static int gyroscope_probe(struct platform_device *pdev)
{
    GYRO_LOG("gyroscope_probe\n");
    return 0;
}
Ejemplo n.º 28
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));
        }

    }
}