iio: accel: Add sampling rate support for STK8312

Added support for setting the STK8312 accelerometer's
sampling rate.

Signed-off-by: Tiberiu Breana <tiberiu.a.breana@intel.com>
Signed-off-by: Jonathan Cameron <jic23@kernel.org>

diff --git a/drivers/iio/accel/stk8312.c b/drivers/iio/accel/stk8312.c
index be58d66..c2bd144 100644 (file)
--- a/drivers/iio/accel/stk8312.c
+++ b/drivers/iio/accel/stk8312.c
@@ -29,6 +29,7 @@
 #define STK8312_REG_ZOUT               0x02
 #define STK8312_REG_INTSU              0x06
 #define STK8312_REG_MODE               0x07
+#define STK8312_REG_SR                 0x08
 #define STK8312_REG_STH                        0x13
 #define STK8312_REG_RESET              0x20
 #define STK8312_REG_AFECTRL            0x24
@@ -41,6 +42,8 @@
 #define STK8312_DREADY_BIT             0x10
 #define STK8312_INT_MODE               0xC0
 #define STK8312_RNG_MASK               0xC0
+#define STK8312_SR_MASK                        0x07
+#define STK8312_SR_400HZ_IDX           0
 #define STK8312_RNG_SHIFT              6
 #define STK8312_READ_RETRIES           16
 #define STK8312_ALL_CHANNEL_MASK       7
@@ -65,20 +68,29 @@ static const int stk8312_scale_table[][2] = {
        {0, 461600}, {1, 231100}
 };
 
-#define STK8312_ACCEL_CHANNEL(index, reg, axis) {              \
-       .type = IIO_ACCEL,                                      \
-       .address = reg,                                         \
-       .modified = 1,                                          \
-       .channel2 = IIO_MOD_##axis,                             \
-       .info_mask_separate = BIT(IIO_CHAN_INFO_RAW),           \
-       .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),   \
-       .scan_index = index,                                    \
-       .scan_type = {                                          \
-               .sign = 's',                                    \
-               .realbits = 8,                                  \
-               .storagebits = 8,                               \
-               .endianness = IIO_CPU,                          \
-       },                                                      \
+static const struct {
+       u16 val;
+       u32 val2;
+} stk8312_samp_freq_table[] = {
+       {400, 0}, {200, 0}, {100, 0}, {50, 0}, {25, 0},
+       {12, 500000}, {6, 250000}, {3, 125000}
+};
+
+#define STK8312_ACCEL_CHANNEL(index, reg, axis) {                      \
+       .type = IIO_ACCEL,                                              \
+       .address = reg,                                                 \
+       .modified = 1,                                                  \
+       .channel2 = IIO_MOD_##axis,                                     \
+       .info_mask_separate = BIT(IIO_CHAN_INFO_RAW),                   \
+       .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) |          \
+                                   BIT(IIO_CHAN_INFO_SAMP_FREQ),       \
+       .scan_index = index,                                            \
+       .scan_type = {                                                  \
+               .sign = 's',                                            \
+               .realbits = 8,                                          \
+               .storagebits = 8,                                       \
+               .endianness = IIO_CPU,                                  \
+       },                                                              \
 }
 
 static const struct iio_chan_spec stk8312_channels[] = {
@@ -92,6 +104,7 @@ struct stk8312_data {
        struct i2c_client *client;
        struct mutex lock;
        int range;
+       u8 sample_rate_idx;
        u8 mode;
        struct iio_trigger *dready_trig;
        bool dready_trigger_on;
@@ -100,8 +113,11 @@ struct stk8312_data {
 
 static IIO_CONST_ATTR(in_accel_scale_available, STK8312_SCALE_AVAIL);
 
+static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("3.125 6.25 12.5 25 50 100 200 400");
+
 static struct attribute *stk8312_attributes[] = {
        &iio_const_attr_in_accel_scale_available.dev_attr.attr,
+       &iio_const_attr_sampling_frequency_available.dev_attr.attr,
        NULL,
 };
 
@@ -220,6 +236,39 @@ static const struct iio_trigger_ops stk8312_trigger_ops = {
        .owner = THIS_MODULE,
 };
 
+static int stk8312_set_sample_rate(struct stk8312_data *data, int rate)
+{
+       int ret;
+       u8 masked_reg;
+       u8 mode;
+       struct i2c_client *client = data->client;
+
+       if (rate == data->sample_rate_idx)
+               return 0;
+
+       mode = data->mode;
+       /* We need to go in standby mode to modify registers */
+       ret = stk8312_set_mode(data, STK8312_MODE_STANDBY);
+       if (ret < 0)
+               return ret;
+
+       ret = i2c_smbus_read_byte_data(client, STK8312_REG_SR);
+       if (ret < 0) {
+               dev_err(&client->dev, "failed to set sampling rate\n");
+               return ret;
+       }
+
+       masked_reg = (ret & (~STK8312_SR_MASK)) | rate;
+
+       ret = i2c_smbus_write_byte_data(client, STK8312_REG_SR, masked_reg);
+       if (ret < 0)
+               dev_err(&client->dev, "failed to set sampling rate\n");
+       else
+               data->sample_rate_idx = rate;
+
+       return stk8312_set_mode(data, mode);
+}
+
 static int stk8312_set_range(struct stk8312_data *data, u8 range)
 {
        int ret;
@@ -303,6 +352,10 @@ static int stk8312_read_raw(struct iio_dev *indio_dev,
                *val = stk8312_scale_table[data->range - 1][0];
                *val2 = stk8312_scale_table[data->range - 1][1];
                return IIO_VAL_INT_PLUS_MICRO;
+       case IIO_CHAN_INFO_SAMP_FREQ:
+               *val = stk8312_samp_freq_table[data->sample_rate_idx].val;
+               *val2 = stk8312_samp_freq_table[data->sample_rate_idx].val2;
+               return IIO_VAL_INT_PLUS_MICRO;
        }
 
        return -EINVAL;
@@ -333,6 +386,20 @@ static int stk8312_write_raw(struct iio_dev *indio_dev,
                mutex_unlock(&data->lock);
 
                return ret;
+       case IIO_CHAN_INFO_SAMP_FREQ:
+               for (i = 0; i < ARRAY_SIZE(stk8312_samp_freq_table); i++)
+                       if (val == stk8312_samp_freq_table[i].val &&
+                           val2 == stk8312_samp_freq_table[i].val2) {
+                               index = i;
+                               break;
+                       }
+               if (index < 0)
+                       return -EINVAL;
+               mutex_lock(&data->lock);
+               ret = stk8312_set_sample_rate(data, index);
+               mutex_unlock(&data->lock);
+
+               return ret;
        }
 
        return -EINVAL;
@@ -479,6 +546,7 @@ static int stk8312_probe(struct i2c_client *client,
                dev_err(&client->dev, "failed to reset sensor\n");
                return ret;
        }
+       data->sample_rate_idx = STK8312_SR_400HZ_IDX;
        ret = stk8312_set_range(data, 1);
        if (ret < 0)
                return ret;