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>

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;
@@ -332,6 +385,20 @@ static int stk8312_write_raw(struct iio_dev *indio_dev,
 		ret = stk8312_set_range(data, index);
 		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;
 	}
 
@@ -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;