static void k3g_work_func(struct work_struct *work)
{
int res;
struct k3g_data *k3g_data = container_of(work, struct k3g_data, work);
if(DEBUG_FUNC_TRACE & debug_mask)
printk(KERN_INFO "%s: line %d\n", __func__, __LINE__);
if(k3g_data->enable == 0)
{
return;
}
do {
res = k3g_read_fifo_status(k3g_data);
if (res < 0)
return;
if (res < k3g_data->entries) {
pr_warn("%s: fifo entries are less than we want\n",
__func__);
goto timer_set;
}
res = k3g_report_gyro_values(k3g_data);
if (res < 0)
return;
timer_set:
set_polling_delay(k3g_data, res);
} while (!ktime_to_ns(k3g_data->polling_delay));
hrtimer_start(&k3g_data->timer,
k3g_data->polling_delay, HRTIMER_MODE_REL);
}
static void k3g_work_func(struct work_struct *work)
{
int res;
struct k3g_data *k3g_data = container_of(work, struct k3g_data, work);
gyro_debug("GYRO sensor enter function %s\n",__FUNCTION__);
do {
res = k3g_read_fifo_status(k3g_data);
if (res < 0)
return;
if (res < k3g_data->entries) {
pr_warn("%s: fifo entries are less than we want\n",
__func__);
goto timer_set;
}
res = k3g_report_gyro_values(k3g_data);
if (res < 0)
return;
timer_set:
set_polling_delay(k3g_data, res);
} while (!ktime_to_ns(k3g_data->polling_delay));
if(likely(k3g_data->work_enable))
hrtimer_start(&k3g_data->timer,
k3g_data->polling_delay, HRTIMER_MODE_REL);
}
static int k3g_report_gyro_values(struct k3g_data *k3g_data)
{
int res;
struct k3g_t data;
gyro_debug("GYRO sensor enter function %s\n",__FUNCTION__);
res = k3g_read_gyro_values(k3g_data->client, &data,
k3g_data->entries + k3g_data->drop_next_event);
if (res < 0)
return res;
res = k3g_read_fifo_status(k3g_data);
k3g_data->drop_next_event = !res;
if (res >= 31 - k3g_data->entries) {
/* reset fifo to start again - data isn't trustworthy,
* our locked read might not have worked and we
* could have done i2c read in mid register update
*/
return k3g_restart_fifo(k3g_data);
}
input_report_rel(k3g_data->input_dev, REL_RX, data.x);
input_report_rel(k3g_data->input_dev, REL_RY, data.y);
input_report_rel(k3g_data->input_dev, REL_RZ, data.z);
input_sync(k3g_data->input_dev);
gyro_debug("GYRO sensor report x %d\n",data.x);
gyro_debug("GYRO sensor report y %d\n",data.y);
gyro_debug("GYRO sensor report z %d\n",data.z);
return res;
}
static int k3g_report_gyro_values(struct k3g_data *k3g_data)
{
int res;
struct k3g_t data;
if(DEBUG_FUNC_TRACE & debug_mask)
printk(KERN_INFO "%s: line %d\n", __func__, __LINE__);
res = k3g_read_gyro_values(k3g_data->client, &data,
k3g_data->entries + k3g_data->drop_next_event);
if (res < 0)
return res;
res = k3g_read_fifo_status(k3g_data);
if(DEBUG_DEV_STATUS& debug_mask)
printk(KERN_INFO "[k3g] read_fifo_status(%d)\n", res);
k3g_data->drop_next_event = !res;
if(DEBUG_DEV_STATUS& debug_mask)
printk(KERN_INFO "[k3g] entries=%d\n", k3g_data->entries);
if (res >= 31 - k3g_data->entries) {
/* reset fifo to start again - data isn't trustworthy,
* our locked read might not have worked and we
* could have done i2c read in mid register update
*/
if(DEBUG_DEV_STATUS& debug_mask)
printk(KERN_INFO "[k3g] call restart_fifo, entries=%d\n", k3g_data->entries);
return k3g_restart_fifo(k3g_data);
}
#ifndef CONFIG_LGE_SENSOR_FUSION
input_report_rel(k3g_data->input_dev, REL_RX, data.x);
input_report_rel(k3g_data->input_dev, REL_RY, data.y);
input_report_rel(k3g_data->input_dev, REL_RZ, data.z);
input_sync(k3g_data->input_dev);
#else
k3g_gyro_data[0] = (int) data.x;
k3g_gyro_data[1] = (int) data.y;
k3g_gyro_data[2] = (int) data.z;
#endif
#ifdef CONFIG_MACH_LGE_I_BOARD_VZW
//BEGIN:seungkwan.jung
gyro_xyz[0] = data.x;
gyro_xyz[1] = data.y;
gyro_xyz[2] = data.z;
//END:seungkwan.jung
#endif
if(DEBUG_DEV_DEBOUNCE & debug_mask)
printk(KERN_INFO "%s: [k3g] x(%d), y(%d), z(%d)\n", __func__, data.x, data.y, data.z);
report_cnt++;
return res;
}
