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 ssize_t k3g_set_enable(struct device *dev,
struct device_attribute *attr, const char *buf, size_t size)
{
int err = 0;
struct k3g_platform_data *pdata;
bool new_enable;
struct k3g_data *k3g_data = dev_get_drvdata(dev);
struct i2c_client *client = to_i2c_client(dev);
pdata = k3g_i2c_client->dev.platform_data;
if(pdata == NULL)
{
dev_err(&client->dev, "failed to read platform data\n");
err = -ENODEV;
return err;
}
if(DEBUG_FUNC_TRACE & debug_mask)
printk(KERN_INFO "%s: line %d - enable %s\n", __func__, __LINE__, buf);
if (sysfs_streq(buf, "1"))
{
new_enable = true;
if(DEBUG_FUNC_TRACE & debug_mask||DEBUG_DEBUG_SYSFS & debug_mask)
printk(KERN_INFO "%s: line %d - enable\n", __func__, __LINE__);
}
else if (sysfs_streq(buf, "0"))
{
new_enable = false;
if(DEBUG_FUNC_TRACE & debug_mask||DEBUG_DEBUG_SYSFS & debug_mask)
printk(KERN_INFO "%s: line %d - disable\n", __func__, __LINE__);
}
else
{
pr_debug("%s: invalid value %d\n", __func__, *buf);
return -EINVAL;
}
if (new_enable == k3g_data->enable)
{
return size;
}
mutex_lock(&k3g_data->lock);
if (new_enable)
{
if(pdata->power_on){
if(DEBUG_FUNC_TRACE & debug_mask)
printk(KERN_INFO "%s: line %d, call power_on", __func__, __LINE__);
pdata->power_on(1<<SENSOR_TYPE_GYROSCOPE);
mdelay(1);
}
/* turning on */
err = i2c_smbus_write_i2c_block_data(k3g_data->client,
CTRL_REG1 | AC, sizeof(k3g_data->ctrl_regs),
k3g_data->ctrl_regs);
if (err < 0)
{
err = -EIO;
if(DEBUG_FUNC_TRACE & debug_mask)
printk(KERN_INFO "%s: line %d, failed turn on\n", __func__, __LINE__);
goto unlock;
}
mdelay(300);
/* reset fifo entries */
err = k3g_restart_fifo(k3g_data);
if (err < 0)
{
err = -EIO;
goto turn_off;
}
if (k3g_data->interruptible)
{
enable_irq(k3g_data->client->irq);
if(DEBUG_FUNC_TRACE & debug_mask)
printk(KERN_INFO "%s: line %d, enable interrupt\n", __func__, __LINE__);
}
else
{
set_polling_delay(k3g_data, 0);
hrtimer_start(&k3g_data->timer,
k3g_data->polling_delay, HRTIMER_MODE_REL);
if(DEBUG_FUNC_TRACE & debug_mask)
printk(KERN_INFO "%s: line %d, set_polling_delay\n", __func__, __LINE__);
}
report_cnt = 0;
}
else
{
if (k3g_data->interruptible)
{
printk(KERN_INFO "%s: line %d -disable_irq\n", __func__, __LINE__);
disable_irq(k3g_data->client->irq);
}
else
{
printk(KERN_INFO "%s: line %d - cancel timer\n", __func__, __LINE__);
hrtimer_cancel(&k3g_data->timer);
cancel_work_sync(&k3g_data->work);
flush_workqueue(k3g_data->k3g_wq);
}
/* turning off */
err = i2c_smbus_write_byte_data(k3g_data->client,
CTRL_REG1, 0x00);
if (err < 0)
{
printk(KERN_INFO "%s: line %d - i2c error\n", __func__, __LINE__);
goto unlock;
}
}
k3g_data->enable = new_enable;
turn_off:
if (err < 0)
i2c_smbus_write_byte_data(k3g_data->client,
CTRL_REG1, 0x00);
unlock:
mutex_unlock(&k3g_data->lock);
return err ? err : size;
}
static ssize_t k3g_set_enable(struct device *dev,
struct device_attribute *attr, const char *buf, size_t size)
{
int err = 0;
struct k3g_data *k3g_data = dev_get_drvdata(dev);
bool new_enable;
gyro_debug("GYRO sensor enter function %s\n",__FUNCTION__);
if (sysfs_streq(buf, "1"))
new_enable = true;
else if (sysfs_streq(buf, "0"))
new_enable = false;
else {
pr_debug("%s: invalid value %d\n", __func__, *buf);
return -EINVAL;
}
if (new_enable == k3g_data->enable)
return size;
mutex_lock(&k3g_data->lock);
if (new_enable) {
/* turning on */
err = i2c_smbus_write_i2c_block_data(k3g_data->client,
CTRL_REG1 | AC, sizeof(k3g_data->ctrl_regs),
k3g_data->ctrl_regs);
if (err < 0) {
err = -EIO;
goto unlock;
}
/* reset fifo entries */
err = k3g_restart_fifo(k3g_data);
if (err < 0) {
err = -EIO;
goto turn_off;
}
k3g_data->timer_enable = 1;
k3g_data->work_enable = 1;
if (k3g_data->interruptible)
enable_irq(k3g_data->client->irq);
else {
set_polling_delay(k3g_data, 0);
hrtimer_start(&k3g_data->timer,
k3g_data->polling_delay, HRTIMER_MODE_REL);
}
} else {
k3g_data->timer_enable = 0;
k3g_data->work_enable = 0;
if (k3g_data->interruptible)
disable_irq(k3g_data->client->irq);
else {
hrtimer_cancel(&k3g_data->timer);
cancel_work_sync(&k3g_data->work);
}
/* turning off */
err = i2c_smbus_write_byte_data(k3g_data->client,
CTRL_REG1, 0x00);
if (err < 0)
goto unlock;
}
k3g_data->enable = new_enable;
turn_off:
if (err < 0)
i2c_smbus_write_byte_data(k3g_data->client,
CTRL_REG1, 0x00);
unlock:
mutex_unlock(&k3g_data->lock);
return err ? err : size;
}
