afbr-s50: Add graceful error-handling on data-ready

Whenever the data-ready callback is invoked, there's a common pattern
that needs to take place:

- Stop measurement timer (to stop on-coming samples), which could be
restored by the application re-submitting the request.
- Set iodev_sqe to NULL, so new requests can come in.
- Report the error on the SQE.

This has been refactored in handle_error_on_result() and now it's
invoked throughout the code.

Signed-off-by: Luis Ubieda <[email protected]>

Author: ubieda

drivers/sensor/broadcom/afbr_s50/afbr_s50.c

@@ -40,6 +40,11 @@ struct afbr_s50_data {
 	 * for platform abstractions present under modules sub-directory.
 	 */
 	struct afbr_s50_platform_data platform;
+	/** Required in order to flush the data if resources are run out.
+	 * In any case we must call `Argus_EvaluateData` to free up the internal
+	 * buffers.
+	 */
+	struct argus_results_t buf;
 };
 
 struct afbr_s50_config {
@@ -58,6 +63,23 @@ struct afbr_s50_config {
 	} settings;
 };
 
+static inline void handle_error_on_result(struct afbr_s50_data *data, int result)
+{
+	struct rtio_iodev_sqe *iodev_sqe = data->rtio.iodev_sqe;
+	status_t status;
+
+	(void)Argus_StopMeasurementTimer(data->platform.argus.handle);
+	do {
+		/** Flush the existing data moving forward so the
+		 * internal buffers can be re-used afterwards.
+		 */
+		status = Argus_EvaluateData(data->platform.argus.handle, &data->buf);
+	} while (status == STATUS_OK);
+
+	data->rtio.iodev_sqe = NULL;
+	rtio_iodev_sqe_err(iodev_sqe, result);
+}
+
 static void data_ready_work_handler(struct rtio_iodev_sqe *iodev_sqe)
 {
 	const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data;
@@ -77,9 +99,7 @@ static void data_ready_work_handler(struct rtio_iodev_sqe *iodev_sqe)
 
 	CHECKIF(err || !edata || edata_len < sizeof(struct afbr_s50_edata)) {
 		LOG_ERR("Failed to get buffer for edata");
-
-		data->rtio.iodev_sqe = NULL;
-		rtio_iodev_sqe_err(iodev_sqe, -ENOMEM);
+		handle_error_on_result(data, -ENOMEM);
 		return;
 	}
 
@@ -88,6 +108,7 @@ static void data_ready_work_handler(struct rtio_iodev_sqe *iodev_sqe)
 	err = sensor_clock_get_cycles(&cycles);
 	CHECKIF(err != 0) {
 		LOG_ERR("Failed to get sensor clock cycles");
+		handle_error_on_result(data, -EIO);
 		return;
 	}
 
@@ -100,12 +121,10 @@ static void data_ready_work_handler(struct rtio_iodev_sqe *iodev_sqe)
 	status = Argus_EvaluateData(data->platform.argus.handle, &edata->payload);
 	if (status != STATUS_OK || edata->payload.Status != STATUS_OK) {
 		LOG_ERR("Data not valid: %d, %d", status, edata->payload.Status);
-
-		data->rtio.iodev_sqe = NULL;
-		rtio_iodev_sqe_err(iodev_sqe, -EIO);
-	} else {
-		data->rtio.iodev_sqe = NULL;
-		rtio_iodev_sqe_ok(iodev_sqe, 0);
+		handle_error_on_result(data, -EIO);
+	}
+	CHECKIF(Argus_IsDataEvaluationPending(data->platform.argus.handle)) {
+		LOG_WRN("Overrun. More pending data than what we've served.");
 	}
 
 	/** After freeing the buffer with EvaluateData, decide whether to
@@ -117,6 +136,9 @@ static void data_ready_work_handler(struct rtio_iodev_sqe *iodev_sqe)
 
 		(void)Argus_StopMeasurementTimer(data->platform.argus.handle);
 	}
+
+	data->rtio.iodev_sqe = NULL;
+	rtio_iodev_sqe_ok(iodev_sqe, 0);
 }
 
 static status_t data_ready_callback(status_t status, argus_hnd_t *hnd)
@@ -140,10 +162,7 @@ static status_t data_ready_callback(status_t status, argus_hnd_t *hnd)
 
 	if (status != STATUS_OK) {
 		LOG_ERR("Measurement failed: %d", status);
-
-		data->rtio.iodev_sqe = NULL;
-		rtio_iodev_sqe_err(iodev_sqe, -EIO);
-
+		handle_error_on_result(data, -EIO);
 		return status;
 	}
 
@@ -156,9 +175,7 @@ static status_t data_ready_callback(status_t status, argus_hnd_t *hnd)
 	CHECKIF(!req) {
 		LOG_ERR("RTIO work item allocation failed. Consider to increase "
 			"CONFIG_RTIO_WORKQ_POOL_ITEMS");
-
-		data->rtio.iodev_sqe = NULL;
-		rtio_iodev_sqe_err(iodev_sqe, -ENOMEM);
+		handle_error_on_result(data, -ENOMEM);
 		return ERROR_FAIL;
 	}