1 files changed, 38 insertions, 59 deletions

diff --git a/zephyr/shim/src/motionsense_sensors.c b/zephyr/shim/src/motionsense_sensors.c
index b884f890b2..7ddc22968e 100644
--- a/zephyr/shim/src/motionsense_sensors.c
+++ b/zephyr/shim/src/motionsense_sensors.c
@@ -8,6 +8,7 @@
 #include "accelgyro.h"
 #include "hooks.h"
 #include "drivers/cros_cbi.h"
+#include "motionsense_sensors.h"
 
 LOG_MODULE_REGISTER(shim_cros_motionsense_sensors);
 
@@ -390,75 +391,53 @@ DECLARE_HOOK(HOOK_INIT, sensor_enable_irqs, HOOK_PRIO_DEFAULT);
 #endif
 
 /* Handle the alternative motion sensors */
-#define REPLACE_ALT_MOTION_SENSOR(new_id, old_id) \
-	motion_sensors[SENSOR_ID(old_id)] =       \
-		motion_sensors_alt[SENSOR_ID(new_id)];
-
-#define CHECK_AND_REPLACE_ALT_MOTION_SENSOR(id)                        \
-	do {                                                           \
-		if (cros_cbi_ssfc_check_match(                         \
-			    dev, CBI_SSFC_VALUE_ID(DT_PHANDLE(         \
-					 id, alternate_indicator)))) { \
-			REPLACE_ALT_MOTION_SENSOR(                     \
-				id, DT_PHANDLE(id, alternate_for))     \
-		}                                                      \
-	} while (0);
-
-#define ALT_MOTION_SENSOR_INIT_ID(id)                                    \
-	COND_CODE_1(UTIL_AND(DT_NODE_HAS_PROP(id, alternate_for),        \
-			     DT_NODE_HAS_PROP(id, alternate_indicator)), \
-		    (CHECK_AND_REPLACE_ALT_MOTION_SENSOR(id)), ())
-
-#define PROBE_SENSOR(id)						\
-{									\
-	int res;							\
-									\
-	LOG_INF("Probing \"%s\" chip %d type %d loc %d",		\
-		motion_sensors_alt[SENSOR_ID(id)].name,			\
-		motion_sensors_alt[SENSOR_ID(id)].chip,			\
-		motion_sensors_alt[SENSOR_ID(id)].type,			\
-		motion_sensors_alt[SENSOR_ID(id)].location);		\
-									\
-	__ASSERT(motion_sensors_alt[SENSOR_ID(id)].drv->probe != NULL,	\
-		"No probing function for alt sensor: %d", SENSOR_ID(id)); \
-	res = motion_sensors_alt[SENSOR_ID(id)].drv->probe(		\
-			&motion_sensors_alt[SENSOR_ID(id)]);		\
-	LOG_INF("%sfound\n", (res != EC_SUCCESS ? "not " : ""));	\
-									\
-	if (res == EC_SUCCESS) {					\
-		REPLACE_ALT_MOTION_SENSOR(id,				\
-					DT_PHANDLE(id, alternate_for));	\
-	}								\
-}
-
-#define PROBE_IF_NEEDED(id)						\
-	COND_CODE_1(DT_PROP(id, runtime_probe),				\
-		(PROBE_SENSOR(id)),					\
-		())
+#define CHECK_SSFC_AND_ENABLE_ALT_SENSOR(id, cbi_dev)                         \
+	do {                                                                  \
+		if (cros_cbi_ssfc_check_match(cbi_dev, CBI_SSFC_VALUE_ID(     \
+				DT_PHANDLE(id, alternate_ssfc_indicator)))) { \
+			LOG_INF("Replacing \"%s\" for \"%s\" based on SSFC",  \
+				motion_sensors[SENSOR_ID(DT_PHANDLE(id,       \
+					alternate_for))].name,                \
+				motion_sensors_alt[SENSOR_ID(id)].name);      \
+			ENABLE_ALT_MOTION_SENSOR(id);                         \
+		}                                                             \
+	} while (0)
+
+#define ALT_SENSOR_CHECK_SSFC_ID(id, cbi_dev)                                 \
+	COND_CODE_1(UTIL_AND(DT_NODE_HAS_PROP(id, alternate_for),             \
+			     DT_NODE_HAS_PROP(id, alternate_ssfc_indicator)), \
+		    (CHECK_SSFC_AND_ENABLE_ALT_SENSOR(id, cbi_dev);), ())
 
 #if DT_NODE_EXISTS(SENSOR_ALT_NODE)
-#ifndef CONFIG_PLATFORM_EC_MOTION_SENSE_RUNTIME_PROBE_CUSTOM_CALL
-static void motion_sense_probe_sensors(void)
-#else
-void motion_sense_probe_sensors(void)
-#endif
+
+int motion_sense_probe(enum sensor_alt_id alt_idx)
 {
-	DT_FOREACH_CHILD(SENSOR_ALT_NODE, PROBE_IF_NEEDED);
+	int res;
+
+	LOG_INF("Probing \"%s\" chip %d type %d loc %d",
+		motion_sensors_alt[alt_idx].name,
+		motion_sensors_alt[alt_idx].chip,
+		motion_sensors_alt[alt_idx].type,
+		motion_sensors_alt[alt_idx].location);
+
+	__ASSERT(motion_sensors_alt[alt_idx].drv->probe != NULL,
+		 "No probing function for alt sensor: %d", alt_idx);
+	res = motion_sensors_alt[alt_idx].drv->probe(
+		&motion_sensors_alt[alt_idx]);
+	LOG_INF("%sfound\n", (res != EC_SUCCESS ? "not " : ""));
+
+	return res;
 }
 
-static void motion_sensors_init_alt(void)
+void motion_sensors_check_ssfc(void)
 {
-	const struct device *dev = device_get_binding("cros_cbi");
+	const struct device *dev = device_get_binding(CROS_CBI_LABEL);
 
 	if (dev != NULL) {
-		DT_FOREACH_CHILD(SENSOR_ALT_NODE, ALT_MOTION_SENSOR_INIT_ID)
+		DT_FOREACH_CHILD_VARGS(SENSOR_ALT_NODE,
+				       ALT_SENSOR_CHECK_SSFC_ID, dev)
 	}
-
-	if (!IS_ENABLED(
-	    CONFIG_PLATFORM_EC_MOTION_SENSE_RUNTIME_PROBE_CUSTOM_CALL))
-		motion_sense_probe_sensors();
 }
-DECLARE_HOOK(HOOK_INIT, motion_sensors_init_alt, HOOK_PRIO_INIT_I2C + 1);
 #endif /* DT_NODE_EXISTS(SENSOR_ALT_NODE) */
 
 #define DEF_MOTION_ISR_NAME_ENUM(id) \