Implement arbitration

.gitignore

@@ -10,6 +10,7 @@
 .\#*
 \#*\#
 build*/
+/build*
 !doc/build/
 !scripts/build
 !tests/drivers/build_all

drivers/sensor/icm42688/icm42688_emul.c

@@ -56,6 +56,9 @@ static void icm42688_emul_handle_write(const struct emul *target, uint8_t regn,
 			data->reg[REG_INT_STATUS] |= BIT_INT_STATUS_RESET_DONE;
 		}
 		break;
+	default:
+		data->reg[regn] = value;
+		break;
 	}
 }
 

include/zephyr/sensing/sensing.h

@@ -32,7 +32,6 @@
 extern "C" {
 #endif
 
-
 /**
  * @struct sensing_sensor_version
  * @brief Sensor Version
@@ -49,20 +48,17 @@ struct sensing_sensor_version {
 	};
 };
 
-#define SENSING_SENSOR_VERSION(_major, _minor, _hotfix, _build)         \
-				(FIELD_PREP(GENMASK(31, 24), _major) |  \
-				 FIELD_PREP(GENMASK(23, 16), _minor) |  \
-				 FIELD_PREP(GENMASK(15, 8), _hotfix) |  \
-				 FIELD_PREP(GENMASK(7, 0), _build))
-
+#define SENSING_SENSOR_VERSION(_major, _minor, _hotfix, _build)                                    \
+	(FIELD_PREP(GENMASK(31, 24), _major) | FIELD_PREP(GENMASK(23, 16), _minor) |               \
+	 FIELD_PREP(GENMASK(15, 8), _hotfix) | FIELD_PREP(GENMASK(7, 0), _build))
 
 /**
  * @brief Sensor flag indicating if this sensor is on event reporting data.
  *
  * Reporting sensor data when the sensor event occurs, such as a motion detect sensor reporting
  * a motion or motionless detected event.
  */
-#define SENSING_SENSOR_FLAG_REPORT_ON_EVENT			BIT(0)
+#define SENSING_SENSOR_FLAG_REPORT_ON_EVENT BIT(0)
 
 /**
  * @brief Sensor flag indicating if this sensor is on change reporting data.
@@ -71,8 +67,7 @@ struct sensing_sensor_version {
  *
  * Exclusive with \ref SENSING_SENSOR_FLAG_REPORT_ON_EVENT
  */
-#define SENSING_SENSOR_FLAG_REPORT_ON_CHANGE			BIT(1)
-
+#define SENSING_SENSOR_FLAG_REPORT_ON_CHANGE BIT(1)
 
 /**
  * @brief Sensing subsystem sensor state.
@@ -83,32 +78,30 @@ enum sensing_sensor_state {
 	SENSING_SENSOR_STATE_OFFLINE = 1,
 };
 
-
 /**
  * @brief Define Sensing subsystem sensor handle
  *
  */
 typedef void *sensing_sensor_handle_t;
 
-
 /**
  * @brief Sensor data event receive callback.
  *
  * @param handle The sensor instance handle.
  *
  * @param buf The data buffer with sensor data.
  */
-typedef void (*sensing_data_event_t)(
-		sensing_sensor_handle_t handle,
-		const void *buf);
+typedef void (*sensing_data_event_t)(sensing_sensor_handle_t handle, const void *buf);
 
 /**
  * @struct sensing_sensor_info
  * @brief Sensor basic constant information
  *
  */
 struct sensing_sensor_info {
-	const struct sensor_info * info;
+	const struct sensor_info *info;
+
+	const struct device *dev;
 
 	/** Sensor type */
 	int32_t type;
@@ -123,20 +116,19 @@ struct sensing_callback_list {
 	sensing_data_event_t on_data_event;
 };
 
-
- /**
-  * @brief Get all supported sensor instances' information.
-  *
-  * This API just returns read only information of sensor instances, pointer info will
-  * directly point to internal buffer, no need for caller to allocate buffer,
-  * no side effect to sensor instances.
-  *
-  * @param num_sensors Get number of sensor instances.
-  *
-  * @param info For receiving sensor instances' information array pointer.
-  *
-  * @return 0 on success or negative error value on failure.
-  */
+/**
+ * @brief Get all supported sensor instances' information.
+ *
+ * This API just returns read only information of sensor instances, pointer info will
+ * directly point to internal buffer, no need for caller to allocate buffer,
+ * no side effect to sensor instances.
+ *
+ * @param num_sensors Get number of sensor instances.
+ *
+ * @param info For receiving sensor instances' information array pointer.
+ *
+ * @return 0 on success or negative error value on failure.
+ */
 int sensing_get_sensors(int *num_sensors, const struct sensing_sensor_info **info);
 
 /**
@@ -155,10 +147,9 @@ int sensing_get_sensors(int *num_sensors, const struct sensing_sensor_info **inf
  *
  * @return 0 on success or negative error value on failure.
  */
-int sensing_open_sensor(
-		const struct sensing_sensor_info *info,
-		const struct sensing_callback_list *cb_list,
-		sensing_sensor_handle_t *handle);
+int sensing_open_sensor(const struct sensing_sensor_info *info,
+			const struct sensing_callback_list *cb_list,
+			sensing_sensor_handle_t *handle);
 
 /**
  * @brief Close sensor instance.
@@ -167,8 +158,16 @@ int sensing_open_sensor(
  *
  * @return 0 on success or negative error value on failure.
  */
-int sensing_close_sensor(
-		sensing_sensor_handle_t handle);
+int sensing_close_sensor(sensing_sensor_handle_t handle);
+
+struct sensing_sensor_attribute {
+	enum sensor_attribute attribute;
+	q31_t value;
+	int8_t shift;
+};
+
+int sensing_set_attributes(sensing_sensor_handle_t handle,
+			   struct sensing_sensor_attribute *attributes, size_t count);
 
 /**
  * @brief Get sensor information from sensor instance handle.
@@ -177,8 +176,7 @@ int sensing_close_sensor(
  *
  * @return a const pointer to \ref sensing_sensor_info on success or NULL on failure.
  */
-const struct sensing_sensor_info *sensing_get_sensor_info(
-		sensing_sensor_handle_t handle);
+const struct sensing_sensor_info *sensing_get_sensor_info(sensing_sensor_handle_t handle);
 
 __test_only void sensing_reset_connections(void);
 
@@ -190,5 +188,4 @@ __test_only void sensing_reset_connections(void);
  * @}
  */
 
-
 #endif /*ZEPHYR_INCLUDE_SENSING_H_*/

include/zephyr/sensing/sensor.h

@@ -19,6 +19,7 @@
 		sensing_sensor_info,                                                               \
 		SENSING_SENSOR_INFO_DT_NAME(node_id, DT_PROP_BY_IDX(node_id, prop, idx))) = {      \
 		.info = &SENSOR_INFO_DT_NAME(DT_PHANDLE(node_id, dev)),                            \
+		.dev = DEVICE_DT_GET(node_id),                                                     \
 		.type = DT_PROP_BY_IDX(node_id, prop, idx),                                        \
 	};
 

subsys/sensing/CMakeLists.txt

@@ -2,7 +2,9 @@
 
 zephyr_library()
 zephyr_library_sources(
+    src/sensor_arbitrate.c
     src/sensor_info.c
     src/sensor_pipe.c
     src/sensor_connections.c
 )
+zephyr_library_include_directories(include)

subsys/sensing/include/sensing/internal/sensing.h

@@ -0,0 +1,36 @@
+//
+// Created by peress on 27/06/23.
+//
+
+#ifndef ZEPHYR_SUBSYS_SENSING_INCLUDE_SENSING_INTERNAL_SENSING_H
+#define ZEPHYR_SUBSYS_SENSING_INCLUDE_SENSING_INTERNAL_SENSING_H
+
+#include <zephyr/drivers/sensor.h>
+#include <zephyr/sensing/sensing.h>
+#include <zephyr/dsp/types.h>
+
+#define __SENSING_POOL_MASK_BUNDLE_COUNT                                                           \
+	(DIV_ROUND_UP(DIV_ROUND_UP(CONFIG_SENSING_MAX_CONNECTIONS, 8), sizeof(uint32_t)))
+
+struct sensing_connection {
+	const struct sensing_sensor_info *info;
+	const struct sensing_callback_list *cb_list;
+	q31_t attributes[SENSOR_ATTR_COMMON_COUNT];
+	uint32_t attribute_mask;
+	struct {
+		uint8_t in_use: 1;
+		uint8_t reserved: 7;
+	} flags;
+} __packed __aligned(4);
+
+extern struct sensing_connection_pool {
+	struct sensing_connection pool[CONFIG_SENSING_MAX_CONNECTIONS];
+	sys_bitarray_t *bitarray;
+	struct k_mutex *lock;
+} __sensing_connection_pool;
+
+BUILD_ASSERT(SENSOR_ATTR_COMMON_COUNT <= 32, "Too many sensor attributes");
+
+void __sensing_arbitrate();
+
+#endif // ZEPHYR_SUBSYS_SENSING_INCLUDE_SENSING_INTERNAL_SENSING_H

subsys/sensing/src/sensor_arbitrate.c

@@ -0,0 +1,96 @@
+//
+// Created by peress on 27/06/23.
+//
+
+#include <zephyr/sensing/sensing.h>
+#include <zephyr/sys/util.h>
+#include <zephyr/logging/log.h>
+
+#include "sensing/internal/sensing.h"
+
+LOG_MODULE_REGISTER(sensing_arbitrate, CONFIG_SENSING_LOG_LEVEL);
+
+static q31_t arbitrate_attribute_value(enum sensor_attribute attribute, q31_t current_value,
+				       q31_t new_value)
+{
+	switch (attribute) {
+	case SENSOR_ATTR_SAMPLING_FREQUENCY:
+		return MAX(current_value, new_value);
+	default:
+		return current_value;
+	}
+}
+
+static void set_arbitrated_value(const struct device *dev, int32_t type,
+				 enum sensor_attribute attribute, q31_t value)
+{
+	enum sensor_channel chan;
+	switch (type) {
+	case SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D:
+		chan = SENSOR_CHAN_ACCEL_XYZ;
+		break;
+	case SENSING_SENSOR_TYPE_MOTION_GYROMETER_3D:
+		chan = SENSOR_CHAN_GYRO_XYZ;
+		break;
+	default:
+		return;
+	}
+
+	struct sensor_value val = {
+		.val1 = FIELD_GET(GENMASK(31, 16), value),
+		.val2 = (FIELD_GET(GENMASK(15, 0), value) * 1000000) / INT16_MAX,
+	};
+	LOG_DBG("Updating attribute chan=%d, value=%d/%d", chan, val.val1, val.val2);
+	((const struct sensor_driver_api *)(dev->api))->attr_set(dev, chan, attribute, &val);
+}
+
+static int arbitrate_sensor_attribute(const struct sensing_sensor_info *info,
+				      enum sensor_attribute attribute)
+{
+	const struct sensing_connection *connections = __sensing_connection_pool.pool;
+	int connection_count = 0;
+	q31_t value;
+
+	for (int i = 0; i < CONFIG_SENSING_MAX_CONNECTIONS; ++i) {
+		if (connections[i].info != info || !connections[i].flags.in_use) {
+//			LOG_DBG("Skipping connection %p, info mismatch", (void*)&connections[i]);
+			continue;
+		}
+		if (FIELD_GET(BIT(attribute), connections[i].attribute_mask) == 0) {
+//			LOG_DBG("Skipping connection %p, attribute not set", (void*)&connections[i]);
+			continue;
+		}
+		if (connection_count == 0) {
+			/* First connection */
+			value = connections[i].attributes[attribute];
+			LOG_DBG("Arbitrating '%s'@%p type=%d attribute=%d", info->info->dev->name,
+				info->info->dev, info->type, attribute);
+			LOG_DBG("    First connection %p, value=0x%08x", (void*)&connections[i], value);
+		} else {
+			value = arbitrate_attribute_value(attribute, value,
+							  connections[i].attributes[attribute]);
+			LOG_DBG("    Updating         %p, value=0x%08x", (void*)&connections[i], value);
+		}
+		connection_count++;
+	}
+
+	if (connection_count != 0) {
+		set_arbitrated_value(info->dev, info->type, attribute, value);
+	}
+
+	return connection_count;
+}
+
+static void arbitrate_sensor_instance(const struct sensing_sensor_info *info)
+{
+	for (int i = 0; i < SENSOR_ATTR_COMMON_COUNT; ++i) {
+		arbitrate_sensor_attribute(info, i);
+	}
+}
+
+void __sensing_arbitrate(void)
+{
+	STRUCT_SECTION_FOREACH(sensing_sensor_info, info) {
+		arbitrate_sensor_instance(info);
+	}
+}