int mag_driver_add(struct mag_init_info* obj) 
{
    int err=0;
	int i =0;
	
	MAG_FUN();

	for(i =0; i < MAX_CHOOSE_G_NUM; i++ )
	{
		if(i == 0){
			MAG_LOG("register mensor driver for the first time\n");
			if(platform_driver_register(&msensor_driver))
			{
				MAG_ERR("failed to register msensor driver already exist\n");
			}
		}
	    if(NULL == msensor_init_list[i])
	    {
	    	obj->platform_diver_addr = &msensor_driver;
	      	msensor_init_list[i] = obj;
		  	break;
	    }
	}
	if(NULL==msensor_init_list[i])
	{
	   MAG_ERR("MAG driver add err \n");
	   err =-1;
	}
		
	return err;
}
Exemple #2
0
static int mag_probe(struct platform_device *pdev) 
{

	int err;
	MAG_LOG("+++++++++++++magel_probe!!\n");
	mag_context_obj = mag_context_alloc_object();
	if (!mag_context_obj)
	{
		err = -ENOMEM;
		MAG_ERR("unable to allocate devobj!\n");
		goto exit_alloc_data_failed;
	}

	//init real mageleration driver
    err = mag_real_driver_init();
	if(err)
	{
		goto mag_real_driver_init_fail;
		MAG_ERR("mag_real_driver_init fail\n");
	}
    //init input dev
	err = mag_input_init(mag_context_obj);
	if(err)
	{
		MAG_ERR("unable to register mag input device!\n");
		goto exit_alloc_input_dev_failed;
	}

    atomic_set(&(mag_context_obj->early_suspend), 0);
	mag_context_obj->early_drv.level    = EARLY_SUSPEND_LEVEL_STOP_DRAWING - 1,
	mag_context_obj->early_drv.suspend  = mag_early_suspend,
	mag_context_obj->early_drv.resume   = mag_late_resume,    
	register_early_suspend(&mag_context_obj->early_drv);

	wake_lock_init(&(mag_context_obj->read_data_wake_lock),WAKE_LOCK_SUSPEND,"read_data_wake_lock");
  
	MAG_LOG("----magel_probe OK !!\n");
	return 0;

	exit_hwmsen_create_attr_failed:
	exit_misc_register_failed:    

	exit_err_sysfs:
	
	if (err)
	{
	   MAG_ERR("sysfs node creation error \n");
	   mag_input_destroy(mag_context_obj);
	}

	mag_real_driver_init_fail:
	exit_alloc_input_dev_failed:    
	kfree(mag_context_obj);
	
	exit_alloc_data_failed:

	MAG_LOG("----magel_probe fail !!!\n");
	return err;
}
static ssize_t mag_store_delay(struct device* dev, struct device_attribute *attr,
                                  const char *buf, size_t count)
{
	mutex_lock(&mag_context_obj->mag_op_mutex);
    struct mag_context *devobj = (struct mag_context*)dev_get_drvdata(dev);
    int delay=0;
	int mdelay=0;
	struct mag_context *cxt = NULL;
	int err =0;
	cxt = mag_context_obj;
	if(NULL == cxt->mag_ctl.m_set_delay)
	{
		mutex_unlock(&mag_context_obj->mag_op_mutex);
		MAG_LOG("mag_ctl m_delay NULL\n");
	 	return count;
	}
	
    MAG_LOG(" mag_delay ++ \n");
  
	if (1 != sscanf(buf, "%d", &delay)) {
		mutex_unlock(&mag_context_obj->mag_op_mutex);
        MAG_ERR("invalid format!!\n");
        return count;
    }
	if(false == cxt->mag_ctl.is_report_input_direct)
    {
    	mdelay = (int)delay/1000/1000;
    	atomic_set(&mag_context_obj->delay, mdelay);
    }
	cxt->mag_ctl.m_set_delay(delay);
	mutex_unlock(&mag_context_obj->mag_op_mutex);
	MAG_LOG(" mag_delay %d ns done\n",delay);
    return count;
}
static ssize_t mag_store_active(struct device* dev, struct device_attribute *attr,
                                  const char *buf, size_t count)
{
    MAG_LOG("mag_store_active buf=%s\n",buf);
	mutex_lock(&mag_context_obj->mag_op_mutex);
	struct mag_context *cxt = NULL;
	int err =0;
	cxt = mag_context_obj;
	if(NULL == cxt->mag_ctl.m_enable)
	{
		mutex_unlock(&mag_context_obj->mag_op_mutex);
		MAG_LOG("mag_ctl path is NULL\n");
	 	return count;
	}
	
    if (!strncmp(buf, "1", 1)) 
	{
       mag_enable_data(ID_M_V_MAGNETIC,1);
       //cxt->mag_ctl.m_enable(1);
    } 
	else if (!strncmp(buf, "0", 1))
	{
       mag_enable_data(ID_M_V_MAGNETIC,0);    
       //cxt->mag_ctl.m_enable(0);
    }
	else
	{
	  MAG_ERR(" mag_store_active error !!\n");
	}
	mutex_unlock(&mag_context_obj->mag_op_mutex);
	MAG_LOG(" mag_store_active done\n");
    return count;
}
static struct mag_context *mag_context_alloc_object(void)
{
	
	struct mag_context *obj = kzalloc(sizeof(*obj), GFP_KERNEL); 
    MAG_LOG("mag_context_alloc_object++++\n");
	if(!obj)
	{
		MAG_ERR("Alloc magel 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, mag_work_func);
	init_timer(&obj->timer);
	obj->timer.expires	= jiffies + atomic_read(&obj->delay)/(1000/HZ);
	obj->timer.function	= mag_poll;
	obj->timer.data		= (unsigned long)obj;
	obj->is_first_data_after_enable = false;
	obj->is_polling_run = false;
	obj->active_data_sensor = 0;
	obj->active_nodata_sensor = 0;
	obj->is_batch_enable = false;
	mutex_init(&obj->mag_op_mutex);
	MAG_LOG("mag_context_alloc_object----\n");
	return obj;
}
static ssize_t mag_store_obatch(struct device* dev, struct device_attribute *attr,
                                  const char *buf, size_t count)
{
    	MAG_LOG("mag_store_obatch buf=%s\n",buf);
	mutex_lock(&mag_context_obj->mag_op_mutex);
	struct mag_context *cxt = NULL;
	int err =0;
	cxt = mag_context_obj;
	if(cxt->mag_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
		{
			MAG_ERR(" mag_store_obatch error !!\n");
		}
	}else{
		MAG_LOG(" mag_store_obatch not supported\n");		
	}
	mutex_unlock(&mag_context_obj->mag_op_mutex);
	MAG_LOG(" mag_store_obatch done: %d\n", cxt->is_batch_enable);
    	return count;

}
int mag_register_control_path(struct mag_control_path *ctl)
{
	struct mag_context *cxt = NULL;
	int err =0;
	cxt = mag_context_obj;

	cxt->mag_ctl.m_set_delay = ctl->m_set_delay;
	cxt->mag_ctl.m_enable= ctl->m_enable;
	cxt->mag_ctl.m_open_report_data= ctl->m_open_report_data;
	cxt->mag_ctl.o_set_delay = ctl->o_set_delay;
	cxt->mag_ctl.o_open_report_data= ctl->o_open_report_data;
	cxt->mag_ctl.o_enable= ctl->o_enable;
	cxt->mag_ctl.is_report_input_direct = ctl->is_report_input_direct;
	cxt->mag_ctl.is_support_batch = ctl->is_support_batch;
	cxt->mag_ctl.is_use_common_factory = ctl->is_use_common_factory;
	
	if(NULL==cxt->mag_ctl.m_set_delay || NULL==cxt->mag_ctl.m_enable
		|| NULL==cxt->mag_ctl.m_open_report_data
		|| NULL==cxt->mag_ctl.o_set_delay || NULL==cxt->mag_ctl.o_open_report_data
		|| NULL==cxt->mag_ctl.o_enable)
	{
		MAG_LOG("mag register control path fail \n");
	 	return -1;
	}

	//add misc dev for sensor hal control cmd
	err = mag_misc_init(mag_context_obj);
	if(err)
	{
	   MAG_ERR("unable to register acc misc device!!\n");
	   return -2;
	}
	err = sysfs_create_group(&mag_context_obj->mdev.this_device->kobj,
			&mag_attribute_group);
	if (err < 0)
	{
	  MAG_ERR("unable to create acc attribute file\n");
	   return -3;
	}

	kobject_uevent(&mag_context_obj->mdev.this_device->kobj, KOBJ_ADD);

	return 0;	
}
int mag_attach(int sensor,struct mag_drv_obj *obj)
{
	int err = 0;
	MAG_FUN();
	//mag_context_obj->drv_obj[sensor] =  obj;
	mag_context_obj->drv_obj[sensor] = kzalloc(sizeof(struct mag_drv_obj), GFP_KERNEL);
	if(mag_context_obj->drv_obj[sensor] == NULL)
	{
		err = -EPERM;
		MAG_ERR(" mag attatch alloc fail \n");
		return err;
	}				
		
	memcpy(mag_context_obj->drv_obj[sensor], obj, sizeof(*obj));
	if(NULL == mag_context_obj->drv_obj[sensor])
	{
	  err  =-1;
	  MAG_ERR(" mag attatch fail \n");
	}
	return err;
}
static int __init mag_init(void) 
{
	MAG_FUN();

	if(platform_driver_register(&mag_driver))
	{
		MAG_ERR("failed to register mag driver\n");
		return -ENODEV;
	}

	return 0;
}
static int mag_misc_init(struct mag_context *cxt)
{

    int err=0;
    cxt->mdev.minor = MISC_DYNAMIC_MINOR;
	cxt->mdev.name  = MAG_MISC_DEV_NAME;
	
	if((err = misc_register(&cxt->mdev)))
	{
		MAG_ERR("unable to register mag misc device!!\n");
	}
	return err;
}
static int check_abnormal_data(int x, int y, int z, int status)
{
    long total;
    total = (x*x + y*y + z*z)/16;
    if ((total <100) || (total >10000)) {
        if (count %10 == 0) {
             MAG_ERR("mag sensor abnormal data: x=%d,y=%d,z=%d, status=%d \n",x,y,z,status);
        }
        count++;
        if (count >1000)
            count=0;
    }
    return 0;
}
static int check_repeat_data(int x, int y, int z)
{
    if ((x1==x) && (y1==y) &&(z1==z))
        pc++;
    else
        pc = 0;

    x1=x;y1=y;z1=z;

    if (pc >100) {
        MAG_ERR("Mag sensor output repeat data\n");
        pc=0;
    }
    return 0;
}
static int __init mag_init(void) 
{
	MAG_FUN();

	devinfo_msensor = get_deviceinfo_dev();
	if (device_create_file(devinfo_msensor, &dev_attr_msensor) < 0)
	pr_err("Failed to create device file(%s)!\n", dev_attr_msensor.attr.name);

	if(platform_driver_register(&mag_driver))
	{
		MAG_ERR("failed to register mag driver\n");
		return -ENODEV;
	}

	return 0;
}
static int mag_remove(struct platform_device *pdev)
{
	MAG_FUN(f);
	int err=0;
	input_unregister_device(mag_context_obj->idev);        
	sysfs_remove_group(&mag_context_obj->idev->dev.kobj,
				&mag_attribute_group);
	
	if((err = misc_deregister(&mag_context_obj->mdev)))
	{
		MAG_ERR("misc_deregister fail: %d\n", err);
	}
	kfree(mag_context_obj);

	return 0;
}
static ssize_t mag_store_obatch(struct device* dev, struct device_attribute *attr,
                                  const char *buf, size_t count)
{
	struct mag_context *cxt = NULL;
	//int err =0;
	MAG_LOG("mag_store_obatch buf=%s\n",buf);
	mutex_lock(&mag_context_obj->mag_op_mutex);
	cxt = mag_context_obj;
	if(cxt->mag_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[ID_M_V_ORIENTATION].mag_data.values[0] = MAG_INVALID_VALUE;
                    cxt->drv_data[ID_M_V_ORIENTATION].mag_data.values[1] = MAG_INVALID_VALUE;
                    cxt->drv_data[ID_M_V_ORIENTATION].mag_data.values[2] = MAG_INVALID_VALUE;
                }
	   	 } 
		else if (!strncmp(buf, "0", 1))
		{
			cxt->is_batch_enable = false;
                if(false == cxt->is_polling_run)
                {
                    if(false == cxt->mag_ctl.is_report_input_direct)
                    {
                        mod_timer(&cxt->timer, jiffies + atomic_read(&cxt->delay)/(1000/HZ));
                        cxt->is_polling_run = true;
                    }
                }
	    	}
		else
		{
			MAG_ERR(" mag_store_obatch error !!\n");
		}
	}else{
		MAG_LOG(" mag_store_obatch not supported\n");		
	}
	mutex_unlock(&mag_context_obj->mag_op_mutex);
	MAG_LOG(" mag_store_obatch done: %d\n", cxt->is_batch_enable);
    	return count;

}
static int mag_enable_data(int handle,int enable)
{
    struct mag_context *cxt = NULL;
	int err =0;
	cxt = mag_context_obj;
	if(NULL  == cxt->drv_obj[handle])
	{
	  MAG_ERR("no real mag driver\n");
	  return -1;
	}
    
    if(1 == enable)
    {
       MAG_LOG("MAG(%d) enable \n",handle);
       cxt->is_first_data_after_enable = true;
	   cxt->active_data_sensor |= 1<<handle;
	   
	   if(ID_M_V_ORIENTATION == handle)
	   {
	   		cxt->mag_ctl.o_enable(1);
		  	cxt->mag_ctl.o_open_report_data(1);
	   }
	   if(ID_M_V_MAGNETIC == handle)
	   {
	   		cxt->mag_ctl.m_enable(1);
	   		cxt->mag_ctl.m_open_report_data(1);
	   }
	   
	   if((0!=cxt->active_data_sensor) && (false == cxt->is_polling_run)&&(false == cxt->is_batch_enable))
	   {
	      if(false == cxt->mag_ctl.is_report_input_direct)
	      {
	       		MAG_LOG("MAG(%d)  mod timer \n",handle);
	      		mod_timer(&cxt->timer, jiffies + atomic_read(&cxt->delay)/(1000/HZ));
		  		cxt->is_polling_run = true;
	      }
	   }
	  
	   
    }
	if(0 == enable)
	{
	   MAG_LOG("MAG(%d) disable \n",handle);
	   cxt->active_data_sensor &= ~(1<<handle);
	   if(ID_M_V_ORIENTATION == handle)
	   {
	   		cxt->mag_ctl.o_enable(0);
		  	cxt->mag_ctl.o_open_report_data(0);
	   }
	   if(ID_M_V_MAGNETIC == handle)
	   {
	   		cxt->mag_ctl.m_enable(0);
	   		cxt->mag_ctl.m_open_report_data(0);
	   }
	   
	   if(0 == cxt->active_data_sensor && true == cxt->is_polling_run)
	   {
	   		if(false == cxt->mag_ctl.is_report_input_direct)
	   		{
	   			MAG_LOG("MAG(%d)  del timer \n",handle);
	      		cxt->is_polling_run = false;
	      		del_timer_sync(&cxt->timer);
	      		cancel_work_sync(&cxt->report);
				cxt->drv_data[handle].mag_data.values[0] = MAG_INVALID_VALUE;
	   			cxt->drv_data[handle].mag_data.values[1] = MAG_INVALID_VALUE;
	   			cxt->drv_data[handle].mag_data.values[2] = MAG_INVALID_VALUE;
	   		}
	      
	   }

	}
	//mag_real_enable(handle,enable);
	return 0;
}
static void mag_work_func(struct work_struct *work)
{

	struct mag_context *cxt = NULL;
	int out_size;
	hwm_sensor_data sensor_data;
	int64_t  nt;
	struct timespec time; 
	int err, idx;	
	int i;

	cxt  = mag_context_obj;
    memset(&sensor_data, 0, sizeof(sensor_data));	
	time.tv_sec = time.tv_nsec = 0;    
	time = get_monotonic_coarse(); 
	nt = time.tv_sec*1000000000LL+time.tv_nsec;
	
	for(i = 0; i < MAX_M_V_SENSOR; i++)
	{
	   if (NULL == cxt->drv_obj[i])
	   {
		  //MAG_LOG("%d driver not atteched\n",i);
		  continue;
	   }

	   if((0 == cxt->drv_obj[i]->polling) || !(cxt->active_data_sensor&(0x01<<i)))
	   {
	       MAG_LOG("mag_type(%d) polling(%d) enabled(%d)\n",i, cxt->drv_obj[i]->polling,cxt->active_data_sensor);
		   continue;
	   }
      
       err = cxt->drv_obj[i]->mag_operate(cxt->drv_obj[i]->self,OP_MAG_GET_DATA, NULL, 0, 
			&sensor_data, sizeof(hwm_sensor_data), &out_size);
       
	   if(err)
	   {
		  MAG_ERR("get %d data fails!!\n" ,i);
		  return;
	   }
	   else
	   {
		   {	
			   if( 0 == sensor_data.values[0] && 0==sensor_data.values[1] 
						&& 0 == sensor_data.values[2])
			   {
			        MAG_ERR("data is zero.\n" );
				    continue;
			   }

			   cxt->drv_data[i].mag_data.values[0] = sensor_data.values[0];
			   cxt->drv_data[i].mag_data.values[1] = sensor_data.values[1];
			   cxt->drv_data[i].mag_data.values[2] = sensor_data.values[2];
			   cxt->drv_data[i].mag_data.status = sensor_data.status;
			   cxt->drv_data[i].mag_data.time = nt;	
		   }			
	    }
       
	    if(true ==  cxt->is_first_data_after_enable)
	    {
		   cxt->is_first_data_after_enable = false;
		   //filter -1 value
	       if(MAG_INVALID_VALUE == cxt->drv_data[i].mag_data.values[0] ||
		   	     MAG_INVALID_VALUE == cxt->drv_data[i].mag_data.values[1] ||
		   	     MAG_INVALID_VALUE == cxt->drv_data[i].mag_data.values[2])
	       {
	          MAG_LOG(" read invalid data \n");
	       	  continue;
			
	       }
	    }
		
		if(ID_M_V_MAGNETIC ==i)
		{
			mag_data_report(MAGNETIC,cxt->drv_data[i].mag_data.values[0],
				cxt->drv_data[i].mag_data.values[1],
				cxt->drv_data[i].mag_data.values[2],
				cxt->drv_data[i].mag_data.status);
		
		  MAG_LOG("mag_type(%d) data[%d,%d,%d] \n" ,i,cxt->drv_data[i].mag_data.values[0],
	    cxt->drv_data[i].mag_data.values[1],cxt->drv_data[i].mag_data.values[2]);
		}
		
		if(ID_M_V_ORIENTATION ==i)
		{
			mag_data_report(ORIENTATION,cxt->drv_data[i].mag_data.values[0],
				cxt->drv_data[i].mag_data.values[1],
				cxt->drv_data[i].mag_data.values[2],
				cxt->drv_data[i].mag_data.status);
		
		  MAG_LOG("mag_type(%d) data[%d,%d,%d] \n" ,i,cxt->drv_data[i].mag_data.values[0],
	    cxt->drv_data[i].mag_data.values[1],cxt->drv_data[i].mag_data.values[2]);
		}
	    

	}

	//report data to input device
	//printk("new mag work run....\n");
	
	if(true == cxt->is_polling_run)
	{
		  mod_timer(&cxt->timer, jiffies + atomic_read(&cxt->delay)/(1000/HZ)); 
	}
}
static void mag_work_func(struct work_struct *work)
{

	struct mag_context *cxt = NULL;
	hwm_sensor_data sensor_data;
	int64_t  nt;
	struct timespec time; 
	int err;	
	int i;
	int x,y,z,status;
	cxt  = mag_context_obj;
    memset(&sensor_data, 0, sizeof(sensor_data));	
	time.tv_sec = time.tv_nsec = 0;    
	time = get_monotonic_coarse(); 
	nt = time.tv_sec*1000000000LL+time.tv_nsec;
	
	for(i = 0; i < MAX_M_V_SENSOR; i++)
	{
	   
	   if(!(cxt->active_data_sensor&(0x01<<i)))
	   {
	       MAG_LOG("mag_type(%d)  enabled(%d)\n",i,cxt->active_data_sensor);
		   continue;
	   }

		if(ID_M_V_MAGNETIC ==i)
		{
			err = cxt->mag_dev_data.get_data_m(&x,&y,&z,&status);
			if(err)
	   		{
		  		MAG_ERR("get %d data fails!!\n" ,i);
		  		return;
	   		}
			cxt->drv_data[i].mag_data.values[0]=x;
			cxt->drv_data[i].mag_data.values[1]=y;
			cxt->drv_data[i].mag_data.values[2]=z;
			cxt->drv_data[i].mag_data.status = status;
			if(true ==  cxt->is_first_data_after_enable)
	    	{
		   		cxt->is_first_data_after_enable = false;
		   		//filter -1 value
	       		if(MAG_INVALID_VALUE == cxt->drv_data[i].mag_data.values[0] ||
		   	     MAG_INVALID_VALUE == cxt->drv_data[i].mag_data.values[1] ||
		   	     MAG_INVALID_VALUE == cxt->drv_data[i].mag_data.values[2])
	       		{
	          		MAG_LOG(" read invalid data \n");
	       	  		continue;
			
	       		}
	    	}
			mag_data_report(MAGNETIC,cxt->drv_data[i].mag_data.values[0],
				cxt->drv_data[i].mag_data.values[1],
				cxt->drv_data[i].mag_data.values[2],
				cxt->drv_data[i].mag_data.status);
		
		  	//MAG_LOG("mag_type(%d) data[%d,%d,%d] \n" ,i,cxt->drv_data[i].mag_data.values[0],
	    	//cxt->drv_data[i].mag_data.values[1],cxt->drv_data[i].mag_data.values[2]);
		}
		
		if(ID_M_V_ORIENTATION ==i)
		{
			
			err = cxt->mag_dev_data.get_data_o(&x,&y,&z,&status);
			if(err)
	   		{
		  		MAG_ERR("get %d data fails!!\n" ,i);
		  		return;
	   		}
			cxt->drv_data[i].mag_data.values[0]=x;
			cxt->drv_data[i].mag_data.values[1]=y;
			cxt->drv_data[i].mag_data.values[2]=z;
			cxt->drv_data[i].mag_data.status = status;
			if(true ==  cxt->is_first_data_after_enable)
	    	{
		   		cxt->is_first_data_after_enable = false;
		   		//filter -1 value
	       		if(MAG_INVALID_VALUE == cxt->drv_data[i].mag_data.values[0] ||
		   	     MAG_INVALID_VALUE == cxt->drv_data[i].mag_data.values[1] ||
		   	     MAG_INVALID_VALUE == cxt->drv_data[i].mag_data.values[2])
	       		{
	          		MAG_LOG(" read invalid data \n");
	       	  		continue;
			
	       		}
	    	}
			mag_data_report(ORIENTATION,cxt->drv_data[i].mag_data.values[0],
				cxt->drv_data[i].mag_data.values[1],
				cxt->drv_data[i].mag_data.values[2],
				cxt->drv_data[i].mag_data.status);
		
		  	//MAG_LOG("mag_type(%d) data[%d,%d,%d] \n" ,i,cxt->drv_data[i].mag_data.values[0],
	    	//cxt->drv_data[i].mag_data.values[1],cxt->drv_data[i].mag_data.values[2]);
		}
	    

	}

	//report data to input device
	//printk("new mag work run....\n");
	
	if(true == cxt->is_polling_run)
	{
		  mod_timer(&cxt->timer, jiffies + atomic_read(&cxt->delay)/(1000/HZ)); 
	}
}