diff options
| -rw-r--r-- | drivers/iio/magnetometer/mag3110.c | 158 |
1 files changed, 140 insertions, 18 deletions
diff --git a/drivers/iio/magnetometer/mag3110.c b/drivers/iio/magnetometer/mag3110.c index b34ace76d31b..f063355480ba 100644 --- a/drivers/iio/magnetometer/mag3110.c +++ b/drivers/iio/magnetometer/mag3110.c @@ -26,6 +26,7 @@ #define MAG3110_OUT_Y 0x03 #define MAG3110_OUT_Z 0x05 #define MAG3110_WHO_AM_I 0x07 +#define MAG3110_SYSMOD 0x08 #define MAG3110_OFF_X 0x09 /* MSB first */ #define MAG3110_OFF_Y 0x0b #define MAG3110_OFF_Z 0x0d @@ -39,6 +40,8 @@ #define MAG3110_CTRL_DR_SHIFT 5 #define MAG3110_CTRL_DR_DEFAULT 0 +#define MAG3110_SYSMOD_MODE_MASK GENMASK(1, 0) + #define MAG3110_CTRL_TM BIT(1) /* trigger single measurement */ #define MAG3110_CTRL_AC BIT(0) /* continuous measurements */ @@ -52,17 +55,20 @@ struct mag3110_data { struct i2c_client *client; struct mutex lock; u8 ctrl_reg1; + int sleep_val; }; static int mag3110_request(struct mag3110_data *data) { int ret, tries = 150; - /* trigger measurement */ - ret = i2c_smbus_write_byte_data(data->client, MAG3110_CTRL_REG1, - data->ctrl_reg1 | MAG3110_CTRL_TM); - if (ret < 0) - return ret; + if ((data->ctrl_reg1 & MAG3110_CTRL_AC) == 0) { + /* trigger measurement */ + ret = i2c_smbus_write_byte_data(data->client, MAG3110_CTRL_REG1, + data->ctrl_reg1 | MAG3110_CTRL_TM); + if (ret < 0) + return ret; + } while (tries-- > 0) { ret = i2c_smbus_read_byte_data(data->client, MAG3110_STATUS); @@ -71,7 +77,11 @@ static int mag3110_request(struct mag3110_data *data) /* wait for data ready */ if ((ret & MAG3110_STATUS_DRDY) == MAG3110_STATUS_DRDY) break; - msleep(20); + + if (data->sleep_val <= 20) + usleep_range(data->sleep_val * 250, data->sleep_val * 500); + else + msleep(20); } if (tries < 0) { @@ -144,6 +154,117 @@ static int mag3110_get_samp_freq_index(struct mag3110_data *data, val2); } +static int mag3110_calculate_sleep(struct mag3110_data *data) +{ + int ret, i = data->ctrl_reg1 >> MAG3110_CTRL_DR_SHIFT; + + if (mag3110_samp_freq[i][0] > 0) + ret = 1000 / mag3110_samp_freq[i][0]; + else + ret = 1000; + + return ret == 0 ? 1 : ret; +} + +static int mag3110_standby(struct mag3110_data *data) +{ + return i2c_smbus_write_byte_data(data->client, MAG3110_CTRL_REG1, + data->ctrl_reg1 & ~MAG3110_CTRL_AC); +} + +static int mag3110_wait_standby(struct mag3110_data *data) +{ + int ret, tries = 30; + + /* + * Takes up to 1/ODR to come out of active mode into stby + * Longest expected period is 12.5seconds. + * We'll sleep for 500ms between checks + */ + while (tries-- > 0) { + ret = i2c_smbus_read_byte_data(data->client, MAG3110_SYSMOD); + if (ret < 0) { + dev_err(&data->client->dev, "i2c error\n"); + return ret; + } + /* wait for standby */ + if ((ret & MAG3110_SYSMOD_MODE_MASK) == 0) + break; + + msleep_interruptible(500); + } + + if (tries < 0) { + dev_err(&data->client->dev, "device not entering standby mode\n"); + return -EIO; + } + + return 0; +} + +static int mag3110_active(struct mag3110_data *data) +{ + return i2c_smbus_write_byte_data(data->client, MAG3110_CTRL_REG1, + data->ctrl_reg1); +} + +/* returns >0 if active, 0 if in standby and <0 on error */ +static int mag3110_is_active(struct mag3110_data *data) +{ + int reg; + + reg = i2c_smbus_read_byte_data(data->client, MAG3110_CTRL_REG1); + if (reg < 0) + return reg; + + return reg & MAG3110_CTRL_AC; +} + +static int mag3110_change_config(struct mag3110_data *data, u8 reg, u8 val) +{ + int ret; + int is_active; + + mutex_lock(&data->lock); + + is_active = mag3110_is_active(data); + if (is_active < 0) { + ret = is_active; + goto fail; + } + + /* config can only be changed when in standby */ + if (is_active > 0) { + ret = mag3110_standby(data); + if (ret < 0) + goto fail; + } + + /* + * After coming out of active we must wait for the part + * to transition to STBY. This can take up to 1 /ODR to occur + */ + ret = mag3110_wait_standby(data); + if (ret < 0) + goto fail; + + ret = i2c_smbus_write_byte_data(data->client, reg, val); + if (ret < 0) + goto fail; + + if (is_active > 0) { + ret = mag3110_active(data); + if (ret < 0) + goto fail; + } + + ret = 0; +fail: + mutex_unlock(&data->lock); + + return ret; +} + static int mag3110_read_raw(struct iio_dev *indio_dev, struct iio_chan_spec const *chan, int *val, int *val2, long mask) @@ -235,11 +356,15 @@ static int mag3110_write_raw(struct iio_dev *indio_dev, ret = -EINVAL; break; } - - data->ctrl_reg1 &= ~MAG3110_CTRL_DR_MASK; + data->ctrl_reg1 &= 0xff & ~MAG3110_CTRL_DR_MASK + & ~MAG3110_CTRL_AC; data->ctrl_reg1 |= rate << MAG3110_CTRL_DR_SHIFT; - ret = i2c_smbus_write_byte_data(data->client, - MAG3110_CTRL_REG1, data->ctrl_reg1); + data->sleep_val = mag3110_calculate_sleep(data); + if (data->sleep_val < 40) + data->ctrl_reg1 |= MAG3110_CTRL_AC; + + ret = mag3110_change_config(data, MAG3110_CTRL_REG1, + data->ctrl_reg1); break; case IIO_CHAN_INFO_CALIBBIAS: if (val < -10000 || val > 10000) { @@ -337,12 +462,6 @@ static const struct iio_info mag3110_info = { static const unsigned long mag3110_scan_masks[] = {0x7, 0xf, 0}; -static int mag3110_standby(struct mag3110_data *data) -{ - return i2c_smbus_write_byte_data(data->client, MAG3110_CTRL_REG1, - data->ctrl_reg1 & ~MAG3110_CTRL_AC); -} - static int mag3110_probe(struct i2c_client *client, const struct i2c_device_id *id) { @@ -374,8 +493,11 @@ static int mag3110_probe(struct i2c_client *client, indio_dev->available_scan_masks = mag3110_scan_masks; data->ctrl_reg1 = MAG3110_CTRL_DR_DEFAULT << MAG3110_CTRL_DR_SHIFT; - ret = i2c_smbus_write_byte_data(client, MAG3110_CTRL_REG1, - data->ctrl_reg1); + data->sleep_val = mag3110_calculate_sleep(data); + if (data->sleep_val < 40) + data->ctrl_reg1 |= MAG3110_CTRL_AC; + + ret = mag3110_change_config(data, MAG3110_CTRL_REG1, data->ctrl_reg1); if (ret < 0) return ret; |