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