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