1 files changed, 103 insertions, 88 deletions

diff --git a/drivers/iio/industrialio-buffer.c b/drivers/iio/industrialio-buffer.c
index 0b4fe63c529e..b4d7dba163cf 100644
--- a/drivers/iio/industrialio-buffer.c
+++ b/drivers/iio/industrialio-buffer.c
@@ -539,28 +539,6 @@ static void iio_buffer_deactivate(struct iio_buffer *buffer)
 	iio_buffer_put(buffer);
 }
 
-void iio_disable_all_buffers(struct iio_dev *indio_dev)
-{
-	struct iio_buffer *buffer, *_buffer;
-
-	if (list_empty(&indio_dev->buffer_list))
-		return;
-
-	if (indio_dev->setup_ops->predisable)
-		indio_dev->setup_ops->predisable(indio_dev);
-
-	list_for_each_entry_safe(buffer, _buffer,
-			&indio_dev->buffer_list, buffer_list)
-		iio_buffer_deactivate(buffer);
-
-	indio_dev->currentmode = INDIO_DIRECT_MODE;
-	if (indio_dev->setup_ops->postdisable)
-		indio_dev->setup_ops->postdisable(indio_dev);
-
-	if (indio_dev->available_scan_masks == NULL)
-		kfree(indio_dev->active_scan_mask);
-}
-
 static void iio_buffer_update_bytes_per_datum(struct iio_dev *indio_dev,
 	struct iio_buffer *buffer)
 {
@@ -683,59 +661,14 @@ static int iio_verify_update(struct iio_dev *indio_dev,
 	return 0;
 }
 
-static int __iio_update_buffers(struct iio_dev *indio_dev,
-		       struct iio_buffer *insert_buffer,
-		       struct iio_buffer *remove_buffer)
+static int iio_enable_buffers(struct iio_dev *indio_dev,
+	struct iio_device_config *config)
 {
 	int ret;
-	const unsigned long *old_mask;
-	struct iio_device_config new_config;
-
-	ret = iio_verify_update(indio_dev, insert_buffer, remove_buffer,
-		&new_config);
-	if (ret)
-		return ret;
-
-	if (insert_buffer) {
-		ret = iio_buffer_request_update(indio_dev, insert_buffer);
-		if (ret)
-			goto err_free_config;
-	}
 
-	/* Wind down existing buffers - iff there are any */
-	if (!list_empty(&indio_dev->buffer_list)) {
-		if (indio_dev->setup_ops->predisable) {
-			ret = indio_dev->setup_ops->predisable(indio_dev);
-			if (ret)
-				goto err_free_config;
-		}
-		indio_dev->currentmode = INDIO_DIRECT_MODE;
-		if (indio_dev->setup_ops->postdisable) {
-			ret = indio_dev->setup_ops->postdisable(indio_dev);
-			if (ret)
-				goto err_free_config;
-		}
-	}
-	/* Keep a copy of current setup to allow roll back */
-	old_mask = indio_dev->active_scan_mask;
-	if (!indio_dev->available_scan_masks)
-		indio_dev->active_scan_mask = NULL;
-
-	if (remove_buffer)
-		iio_buffer_deactivate(remove_buffer);
-	if (insert_buffer)
-		iio_buffer_activate(indio_dev, insert_buffer);
-
-	/* If no buffers in list, we are done */
-	if (list_empty(&indio_dev->buffer_list)) {
-		indio_dev->currentmode = INDIO_DIRECT_MODE;
-		iio_free_scan_mask(indio_dev, old_mask);
-		return 0;
-	}
-
-	indio_dev->active_scan_mask = new_config.scan_mask;
-	indio_dev->scan_timestamp = new_config.scan_timestamp;
-	indio_dev->scan_bytes = new_config.scan_bytes;
+	indio_dev->active_scan_mask = config->scan_mask;
+	indio_dev->scan_timestamp = config->scan_timestamp;
+	indio_dev->scan_bytes = config->scan_bytes;
 
 	iio_update_demux(indio_dev);
 
@@ -745,7 +678,7 @@ static int __iio_update_buffers(struct iio_dev *indio_dev,
 		if (ret) {
 			dev_dbg(&indio_dev->dev,
 			       "Buffer not started: buffer preenable failed (%d)\n", ret);
-			goto error_remove_inserted;
+			goto err_undo_config;
 		}
 	}
 
@@ -757,39 +690,108 @@ static int __iio_update_buffers(struct iio_dev *indio_dev,
 			dev_dbg(&indio_dev->dev,
 				"Buffer not started: update scan mode failed (%d)\n",
 				ret);
-			goto error_run_postdisable;
+			goto err_run_postdisable;
 		}
 	}
 
-	indio_dev->currentmode = new_config.mode;
+	indio_dev->currentmode = config->mode;
 
 	if (indio_dev->setup_ops->postenable) {
 		ret = indio_dev->setup_ops->postenable(indio_dev);
 		if (ret) {
 			dev_dbg(&indio_dev->dev,
 			       "Buffer not started: postenable failed (%d)\n", ret);
-			indio_dev->currentmode = INDIO_DIRECT_MODE;
-			if (indio_dev->setup_ops->postdisable)
-				indio_dev->setup_ops->postdisable(indio_dev);
-			goto error_disable_all_buffers;
+			goto err_run_postdisable;
 		}
 	}
 
-	iio_free_scan_mask(indio_dev, old_mask);
-
 	return 0;
 
-error_disable_all_buffers:
+err_run_postdisable:
 	indio_dev->currentmode = INDIO_DIRECT_MODE;
-error_run_postdisable:
 	if (indio_dev->setup_ops->postdisable)
 		indio_dev->setup_ops->postdisable(indio_dev);
-error_remove_inserted:
-	if (insert_buffer)
-		iio_buffer_deactivate(insert_buffer);
-	iio_free_scan_mask(indio_dev, indio_dev->active_scan_mask);
-	indio_dev->active_scan_mask = old_mask;
+err_undo_config:
+	indio_dev->active_scan_mask = NULL;
+
 	return ret;
+}
+
+static int iio_disable_buffers(struct iio_dev *indio_dev)
+{
+	int ret;
+
+	/* Wind down existing buffers - iff there are any */
+	if (list_empty(&indio_dev->buffer_list))
+		return 0;
+
+	if (indio_dev->setup_ops->predisable) {
+		ret = indio_dev->setup_ops->predisable(indio_dev);
+		if (ret)
+			return ret;
+	}
+
+	indio_dev->currentmode = INDIO_DIRECT_MODE;
+
+	if (indio_dev->setup_ops->postdisable) {
+		ret = indio_dev->setup_ops->postdisable(indio_dev);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int __iio_update_buffers(struct iio_dev *indio_dev,
+		       struct iio_buffer *insert_buffer,
+		       struct iio_buffer *remove_buffer)
+{
+	int ret;
+	const unsigned long *old_mask;
+	struct iio_device_config new_config;
+
+	ret = iio_verify_update(indio_dev, insert_buffer, remove_buffer,
+		&new_config);
+	if (ret)
+		return ret;
+
+	if (insert_buffer) {
+		ret = iio_buffer_request_update(indio_dev, insert_buffer);
+		if (ret)
+			goto err_free_config;
+	}
+
+	/* Keep a copy of current setup to allow roll back */
+	old_mask = indio_dev->active_scan_mask;
+	indio_dev->active_scan_mask = NULL;
+
+	ret = iio_disable_buffers(indio_dev);
+	if (ret) {
+		iio_free_scan_mask(indio_dev, old_mask);
+		goto err_free_config;
+	}
+
+	if (remove_buffer)
+		iio_buffer_deactivate(remove_buffer);
+	if (insert_buffer)
+		iio_buffer_activate(indio_dev, insert_buffer);
+
+	/* If no buffers in list, we are done */
+	if (list_empty(&indio_dev->buffer_list)) {
+		iio_free_scan_mask(indio_dev, old_mask);
+		return 0;
+	}
+
+	ret = iio_enable_buffers(indio_dev, &new_config);
+	if (ret) {
+		if (insert_buffer)
+			iio_buffer_deactivate(insert_buffer);
+		indio_dev->active_scan_mask = old_mask;
+		goto err_free_config;
+	}
+
+	iio_free_scan_mask(indio_dev, old_mask);
+	return 0;
 
 err_free_config:
 	iio_free_scan_mask(indio_dev, new_config.scan_mask);
@@ -834,6 +836,19 @@ out_unlock:
 }
 EXPORT_SYMBOL_GPL(iio_update_buffers);
 
+void iio_disable_all_buffers(struct iio_dev *indio_dev)
+{
+	struct iio_buffer *buffer, *_buffer;
+
+	iio_disable_buffers(indio_dev);
+	iio_free_scan_mask(indio_dev, indio_dev->active_scan_mask);
+	indio_dev->active_scan_mask = NULL;
+
+	list_for_each_entry_safe(buffer, _buffer,
+			&indio_dev->buffer_list, buffer_list)
+		iio_buffer_deactivate(buffer);
+}
+
 static ssize_t iio_buffer_store_enable(struct device *dev,
 				       struct device_attribute *attr,
 				       const char *buf,