static int posix_get_monotonic_coarse(clockid_t which_clock,
struct timespec *tp)
{
*tp = get_monotonic_coarse();
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->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;
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));
}
}
}
static void act_work_func(struct work_struct *work)
{
struct act_context *cxt = NULL;
//int out_size;
//hwm_sensor_data sensor_data;
//u64 data64[6]; //for unify get_data parameter type
//u16 data32[6]; //for hwm_sensor_data.values as int
u16 data16[6]; //for hwm_sensor_data.values as int
int status;
int64_t nt;
struct timespec time;
int err = 0;
cxt = act_context_obj;
if(NULL == cxt->act_data.get_data)
{
ACT_ERR("act 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
//initial data
ACT_LOG("act_data.values %d,%d,%d,%d,%d,%d \n" ,
cxt->drv_data.act_data.values[0],
cxt->drv_data.act_data.values[1],
cxt->drv_data.act_data.values[2],
cxt->drv_data.act_data.values[3],
cxt->drv_data.act_data.values[4],
cxt->drv_data.act_data.values[5]);
err = cxt->act_data.get_data(data16,&status);
ACT_LOG("get_data 16 %d,%d,%d,%d,%d,%d \n" ,
data16[0],data16[1],data16[2],data16[3],data16[4],data16[5]);
if(err)
{
ACT_ERR("get act data fails!!\n" );
goto act_loop;
}
else
{
if((data16[0] == cxt->drv_data.act_data.values[0])
&& (data16[1] == cxt->drv_data.act_data.values[1])
&& (data16[2] == cxt->drv_data.act_data.values[2])
&& (data16[3] == cxt->drv_data.act_data.values[3])
&& (data16[4] == cxt->drv_data.act_data.values[4])
&& (data16[5] == cxt->drv_data.act_data.values[5]))
{
goto act_loop;
}
else
{
cxt->drv_data.act_data.values[0] = data16[0];
cxt->drv_data.act_data.values[1] = data16[1];
cxt->drv_data.act_data.values[2] = data16[2];
cxt->drv_data.act_data.values[3] = data16[3];
cxt->drv_data.act_data.values[4] = data16[4];
cxt->drv_data.act_data.values[5] = data16[5];
ACT_LOG("act values %d,%d,%d,%d,%d,%d\n" ,
cxt->drv_data.act_data.values[0],
cxt->drv_data.act_data.values[1],
cxt->drv_data.act_data.values[2],
cxt->drv_data.act_data.values[3],
cxt->drv_data.act_data.values[4],
cxt->drv_data.act_data.values[5]);
cxt->drv_data.act_data.status = status;
cxt->drv_data.act_data.time = nt;
}
}
if(true == cxt->is_first_data_after_enable)
{
cxt->is_first_data_after_enable = false;
//filter -1 value
if( ACT_INVALID_VALUE == cxt->drv_data.act_data.values[0] ||
ACT_INVALID_VALUE == cxt->drv_data.act_data.values[1] ||
ACT_INVALID_VALUE == cxt->drv_data.act_data.values[2] ||
ACT_INVALID_VALUE == cxt->drv_data.act_data.values[3] ||
ACT_INVALID_VALUE == cxt->drv_data.act_data.values[4] ||
ACT_INVALID_VALUE == cxt->drv_data.act_data.values[5] )
{
ACT_LOG(" read invalid data \n");
goto act_loop;
}
}
//report data to input device
//printk("new act work run....\n");
//ACT_LOG("act data[%d,%d,%d] \n" ,cxt->drv_data.act_data.values[0],
//cxt->drv_data.act_data.values[1],cxt->drv_data.act_data.values[2]);
act_data_report(cxt->drv_data.act_data, cxt->drv_data.act_data.status);
act_loop:
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));
}
}
static void gmrv_work_func(struct work_struct *work)
{
struct gmrv_context *cxt = NULL;
int x, y, z, scalar, status;
int64_t nt;
struct timespec time;
int err;
cxt = gmrv_context_obj;
if (NULL == cxt->gmrv_data.get_data)
GMRV_LOG("gmrv 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->gmrv_data.get_data(&x, &y, &z, &scalar, &status);
if (err) {
GMRV_ERR("get gmrv data fails!!\n");
goto gmrv_loop;
} else {
{
if (0 == x && 0 == y && 0 == z)
goto gmrv_loop;
cxt->drv_data.gmrv_data.values[0] = x;
cxt->drv_data.gmrv_data.values[1] = y;
cxt->drv_data.gmrv_data.values[2] = z;
cxt->drv_data.gmrv_data.values[3] = scalar;
cxt->drv_data.gmrv_data.status = status;
cxt->drv_data.gmrv_data.time = nt;
}
}
if (true == cxt->is_first_data_after_enable) {
cxt->is_first_data_after_enable = false;
/* filter -1 value */
if (GMRV_INVALID_VALUE == cxt->drv_data.gmrv_data.values[0] ||
GMRV_INVALID_VALUE == cxt->drv_data.gmrv_data.values[1] ||
GMRV_INVALID_VALUE == cxt->drv_data.gmrv_data.values[2] ||
GMRV_INVALID_VALUE == cxt->drv_data.gmrv_data.values[3]
) {
GMRV_LOG(" read invalid data\n");
goto gmrv_loop;
}
}
/* report data to input device */
/* printk("new gmrv work run....\n"); */
/* GMRV_LOG("gmrv data[%d,%d,%d]\n" ,cxt->drv_data.gmrv_data.values[0], */
/* cxt->drv_data.gmrv_data.values[1],cxt->drv_data.gmrv_data.values[2]); */
gmrv_data_report(cxt->drv_data.gmrv_data.values[0],
cxt->drv_data.gmrv_data.values[1],
cxt->drv_data.gmrv_data.values[2],
cxt->drv_data.gmrv_data.values[3], cxt->drv_data.gmrv_data.status);
gmrv_loop:
if (true == cxt->is_polling_run)
mod_timer(&cxt->timer, jiffies + atomic_read(&cxt->delay) / (1000 / HZ));
}
