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