static int lsm330dlc_gyro_resume(struct device *dev) { int err = 0; struct i2c_client *client = to_i2c_client(dev); struct lsm330dlc_gyro_data *data = i2c_get_clientdata(client); if (data->enable) { mutex_lock(&data->lock); err = i2c_smbus_write_i2c_block_data(client, CTRL_REG1 | AC, sizeof(data->ctrl_regs), data->ctrl_regs); if (data->interruptible) { enable_irq(data->client->irq); lsm330dlc_gyro_restart_fifo(data); } if (!data->interruptible) hrtimer_start(&data->timer, data->polling_delay, HRTIMER_MODE_REL); mutex_unlock(&data->lock); } return err; }
static int lsm330dlc_gyro_report_values\ (struct lsm330dlc_gyro_data *gyro_data) { int res; struct gyro_t data; res = lsm330dlc_gyro_read_values(gyro_data->client, &data, gyro_data->entries); if (res < 0) return res; data.x -= gyro_data->cal_data.x; data.y -= gyro_data->cal_data.y; data.z -= gyro_data->cal_data.z; input_report_rel(gyro_data->input_dev, REL_RX, data.x); input_report_rel(gyro_data->input_dev, REL_RY, data.y); input_report_rel(gyro_data->input_dev, REL_RZ, data.z); input_sync(gyro_data->input_dev); lsm330dlc_gyro_restart_fifo(gyro_data); #ifdef LOGGING_GYRO printk(KERN_INFO "%s, x = %d, y = %d, z = %d, count = %d\n" , __func__, data.x, data.y, data.z, gyro_data->count); #endif return res; }
static int lsm330dlc_gyro_report_values\ (struct lsm330dlc_gyro_data *data) { int res, i, j; s16 raw[3] = {0,}, gyro_adjusted[3] = {0,}; res = lsm330dlc_gyro_read_values(data->client, &data->xyz_data, data->entries); if (res < 0) return res; data->xyz_data.x -= data->cal_data.x; data->xyz_data.y -= data->cal_data.y; data->xyz_data.z -= data->cal_data.z; if (data->axis_adjust) { raw[0] = data->xyz_data.x; raw[1] = data->xyz_data.y; raw[2] = data->xyz_data.z; for (i = 0; i < 3; i++) { for (j = 0; j < 3; j++) gyro_adjusted[i] += position_map[data->position][i][j] * raw[j]; } } else { gyro_adjusted[0] = data->xyz_data.x; gyro_adjusted[1] = data->xyz_data.y; gyro_adjusted[2] = data->xyz_data.z; } input_report_rel(data->input_dev, REL_RX, gyro_adjusted[0]); input_report_rel(data->input_dev, REL_RY, gyro_adjusted[1]); input_report_rel(data->input_dev, REL_RZ, gyro_adjusted[2]); input_sync(data->input_dev); lsm330dlc_gyro_restart_fifo(data); #ifdef LOGGING_GYRO pr_info("%s, x = %d, y = %d, z = %d\n" , __func__, gyro_adjusted[0], gyro_adjusted[1], gyro_adjusted[2]); #endif return res; }
static int lsm330dlc_gyro_set_delay_ns(struct lsm330dlc_gyro_data *k3\ , u64 delay_ns) { int res = 0; int odr_value = ODR95_BW25; int i = 0, odr_table_size = ARRAY_SIZE(odr_delay_table) - 1; u8 ctrl; u64 new_delay = 0; mutex_lock(&k3->lock); if (!k3->interruptible) { hrtimer_cancel(&k3->timer); cancel_work_sync(&k3->work); } else disable_irq(k3->client->irq); /* round to the nearest supported ODR that is equal or above than * the requested value * 10ms(entries = 1), 20ms(entries = 2), * 67ms(entries = 6), 200ms(entries = 20) */ if (delay_ns == 10000000LL && k3->interruptible) delay_ns = odr_delay_table[odr_table_size].delay_ns; if (delay_ns >= MAX_DELAY) {/* > max delay */ k3->entries = MAX_ENTRY; odr_value = odr_delay_table[odr_table_size].odr; k3->time_to_read = odr_delay_table[odr_table_size].delay_ns; new_delay = MAX_DELAY; } else if (delay_ns <= odr_delay_table[0].delay_ns) { /* < min delay */ k3->entries = 1; odr_value = odr_delay_table[0].odr; k3->time_to_read = odr_delay_table[0].delay_ns; new_delay = odr_delay_table[0].delay_ns; } else { for (i = odr_table_size; i >= 0; i--) { if (delay_ns >= odr_delay_table[i].delay_ns) { new_delay = delay_ns; do_div(delay_ns, odr_delay_table[i].delay_ns); k3->entries = delay_ns; odr_value = odr_delay_table[i].odr; k3->time_to_read = odr_delay_table[i].delay_ns; break; } } } if (k3->interruptible) pr_info("%s, k3->entries=%d, odr_value=0x%x\n", __func__, k3->entries, odr_value); if (odr_value != (k3->ctrl_regs[0] & ODR_MASK)) { ctrl = (k3->ctrl_regs[0] & ~ODR_MASK); ctrl |= odr_value; k3->ctrl_regs[0] = ctrl; res = i2c_smbus_write_byte_data(k3->client, CTRL_REG1, ctrl); } if (k3->interruptible) { enable_irq(k3->client->irq); /* (re)start fifo */ lsm330dlc_gyro_restart_fifo(k3); } if (!k3->interruptible) { pr_info("%s, delay_ns=%lld, odr_value=0x%x\n", __func__, new_delay, odr_value); k3->polling_delay = ns_to_ktime(new_delay); if (k3->enable) hrtimer_start(&k3->timer, k3->polling_delay, HRTIMER_MODE_REL); } mutex_unlock(&k3->lock); return res; }
static ssize_t lsm330dlc_gyro_set_enable(struct device *dev, struct device_attribute *attr, const char *buf, size_t size) { int err = 0; struct lsm330dlc_gyro_data *data = dev_get_drvdata(dev); bool new_enable; 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; } printk(KERN_INFO "%s, %d enable : %d.\n", __func__, __LINE__,\ new_enable); if (new_enable == data->enable) return size; mutex_lock(&data->lock); if (new_enable) { /* load cal data */ err = lsm330dlc_gyro_open_calibration(data); if (err < 0 && err != -ENOENT) pr_err("%s: lsm330dlc_gyro_open_calibration() failed\n", __func__); /* turning on */ err = i2c_smbus_write_i2c_block_data(data->client, CTRL_REG1 | AC, sizeof(data->ctrl_regs), data->ctrl_regs); if (err < 0) { err = -EIO; goto unlock; } if (data->interruptible) { enable_irq(data->client->irq); /* reset fifo entries */ err = lsm330dlc_gyro_restart_fifo(data); if (err < 0) { err = -EIO; goto turn_off; } } if (!data->interruptible) { hrtimer_start(&data->timer, data->polling_delay, HRTIMER_MODE_REL); } } else { if (data->interruptible) disable_irq(data->client->irq); else { hrtimer_cancel(&data->timer); cancel_work_sync(&data->work); } /* turning off */ err = i2c_smbus_write_byte_data(data->client, CTRL_REG1, 0x00); if (err < 0) goto unlock; } data->enable = new_enable; printk(KERN_INFO "%s, %d lsm330dlc_gyro enable is done.\n",\ __func__, __LINE__); turn_off: if (err < 0) i2c_smbus_write_byte_data(data->client, CTRL_REG1, 0x00); unlock: mutex_unlock(&data->lock); return err ? err : size; }