iio: imu: st_lsm6dsx: split fifo mode and fifo odr configuration

Separate fifo mode and max fifo sample rate configuration.
That change will be necessary to reuse st_lsm6dsx_set_fifo_mode()
routine and to support more devices in st_lsm6dsx driver

Signed-off-by: Lorenzo Bianconi <lorenzo.bianconi@st.com>
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c

@@ -139,23 +139,10 @@ static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
 int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
 			     enum st_lsm6dsx_fifo_mode fifo_mode)
 {
-	u8 data;
 	int err;
 
-	switch (fifo_mode) {
-	case ST_LSM6DSX_FIFO_BYPASS:
-		data = fifo_mode;
-		break;
-	case ST_LSM6DSX_FIFO_CONT:
-		data = (ST_LSM6DSX_MAX_FIFO_ODR_VAL <<
-			__ffs(ST_LSM6DSX_FIFO_ODR_MASK)) | fifo_mode;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
-			    sizeof(data), &data);
+	err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
+					 ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode);
 	if (err < 0)
 		return err;
 
@@ -164,6 +151,17 @@ int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
 	return 0;
 }
 
+static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
+				   bool enable)
+{
+	struct st_lsm6dsx_hw *hw = sensor->hw;
+	u8 data;
+
+	data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0;
+	return st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
+					  ST_LSM6DSX_FIFO_ODR_MASK, data);
+}
+
 int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
 {
 	u16 fifo_watermark = ~0, cur_watermark, sip = 0;
@@ -345,6 +343,10 @@ static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable)
 			return err;
 	}
 
+	err = st_lsm6dsx_set_fifo_odr(sensor, enable);
+	if (err < 0)
+		return err;
+
 	err = st_lsm6dsx_update_decimators(hw);
 	if (err < 0)
 		return err;