zephyr: Rework CBI to be used as a driver

Rework the CBI support in Zephyr to be used via driver API.

Change also approach what to do with SSFC - let sensor drivers decide
how to handle alternative sensors.

BUG=b:183990188
BRANCH=none
TEST=Add alternative motion sensors to the device tree, modify CBI SSFC
with 'cbi set 8 value 4', reboot EC and verify that the new sensors are
used with the 'accelinfo' command.

Signed-off-by: Dawid Niedzwiecki <dn@semihalf.com>
Change-Id: I701af96bfa7a17333220530a5c63b8e8aaeb0d6b
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2862727
Reviewed-by: Yuval Peress <peress@chromium.org>

12 files changed, 393 insertions, 181 deletions

diff --git a/zephyr/drivers/CMakeLists.txt b/zephyr/drivers/CMakeLists.txt
index 5435ee6b84..6ab8383eb5 100644
--- a/zephyr/drivers/CMakeLists.txt
+++ b/zephyr/drivers/CMakeLists.txt
@@ -3,6 +3,7 @@
 # found in the LICENSE file.
 
 add_subdirectory(cros_bbram)
+add_subdirectory(cros_cbi)
 add_subdirectory(cros_flash)
 add_subdirectory(cros_kb_raw)
 add_subdirectory(cros_rtc)
diff --git a/zephyr/drivers/cros_cbi/CMakeLists.txt b/zephyr/drivers/cros_cbi/CMakeLists.txt
new file mode 100644
index 0000000000..1ef8eccf1f
--- /dev/null
+++ b/zephyr/drivers/cros_cbi/CMakeLists.txt
@@ -0,0 +1,3 @@
+# SPDX-License-Identifier: Apache-2.0
+
+zephyr_library_sources_ifdef(CONFIG_PLATFORM_EC_CBI	cros_cbi.c)
diff --git a/zephyr/drivers/cros_cbi/cros_cbi.c b/zephyr/drivers/cros_cbi/cros_cbi.c
new file mode 100644
index 0000000000..0aaa5e2b1a
--- /dev/null
+++ b/zephyr/drivers/cros_cbi/cros_cbi.c
@@ -0,0 +1,218 @@
+/* Copyright 2021 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#include <drivers/cros_cbi.h>
+#include "cros_board_info.h"
+#include <logging/log.h>
+
+LOG_MODULE_REGISTER(cros_cbi, LOG_LEVEL_ERR);
+
+/* CBI SSFC part */
+
+/* This part of the driver is about CBI SSFC part.
+ * Actually, two "compatible" values are handle here -
+ * named_cbi_ssfc_value and named_cbi_ssfc. named_cbi_ssfc_value nodes are
+ * grandchildren of the named_cbi_ssfc node. named_cbi_ssfc_value is introduced
+ * to iterate over grandchildren of the named_cbi_ssfc(macro
+ * DT_FOREACH_CHILD can not be nested) and it can be pointed by a sensor dts to
+ * indicate alternative usage.
+ */
+#define DT_DRV_COMPAT named_cbi_ssfc_value
+
+BUILD_ASSERT(DT_NUM_INST_STATUS_OKAY(named_cbi_ssfc) < 2,
+	     "More than 1 CBI SSFS node");
+#define CBI_SSFC_NODE DT_INST(0, named_cbi_ssfc)
+
+#define CBI_SSFC_INIT_DEFAULT_ID(id)                                           \
+	do {                                                                   \
+		if (DT_PROP(id, default)) {                                    \
+			cached_ssfc.CBI_SSFC_UNION_ENTRY_NAME(DT_PARENT(id)) = \
+				DT_PROP(id, value);                            \
+		}                                                              \
+	} while (0);
+
+#define CBI_SSFC_INIT_DEFAULT(inst) CBI_SSFC_INIT_DEFAULT_ID(DT_DRV_INST(inst))
+
+#define CBI_SSFC_VALUE_ARRAY_ID(id) \
+	[CBI_SSFC_VALUE_ID(id)] = DT_PROP(id, value),
+
+#define CBI_SSFC_VALUE_ARRAY(inst) CBI_SSFC_VALUE_ARRAY_ID(DT_DRV_INST(inst))
+
+#define CBI_SSFC_VALUE_BUILD_ASSERT(inst)                    \
+	BUILD_ASSERT(DT_INST_PROP(inst, value) <= UINT8_MAX, \
+		     "CBI SSFS value too big");
+
+#define CBI_SSFC_PARENT_VALUE_CASE_GENERATE(value_id, value_parent) \
+	case value_id:                                              \
+		return value_parent;
+
+#define CBI_SSFC_PARENT_VALUE_CASE_ID(id)    \
+	CBI_SSFC_PARENT_VALUE_CASE_GENERATE( \
+		CBI_SSFC_VALUE_ID(id),       \
+		cached_ssfc.CBI_SSFC_UNION_ENTRY_NAME(DT_PARENT(id)))
+
+#define CBI_SSFC_PARENT_VALUE_CASE(inst) \
+	CBI_SSFC_PARENT_VALUE_CASE_ID(DT_DRV_INST(inst))
+
+#define CBI_SSFC_UNION_ENTRY_NAME(id) DT_CAT(cbi_ssfc_, id)
+#define CBI_SSFC_UNION_ENTRY(id)               \
+	uint32_t CBI_SSFC_UNION_ENTRY_NAME(id) \
+		: DT_PROP(id, size);
+
+#define CBI_SSFC_PLUS_FIELD_SIZE(id) +DT_PROP(id, size)
+#define CBI_SSFC_FIELDS_SIZE                                                 \
+	(0 COND_CODE_1(                                                      \
+		DT_NODE_EXISTS(CBI_SSFC_NODE),                               \
+		(DT_FOREACH_CHILD(CBI_SSFC_NODE, CBI_SSFC_PLUS_FIELD_SIZE)), \
+		()))
+
+BUILD_ASSERT(CBI_SSFC_FIELDS_SIZE <= 32, "CBI SSFS is bigger than 32 bits");
+
+/*
+ * Define union bit fields based on the device tree entries. Example:
+ * cbi-ssfc {
+ *	compatible = "named-cbi-ssfc";
+ *
+ *	base_sensor {
+ *		enum-name = "BASE_SENSOR";
+ *		size = <3>;
+ *		bmi160 {
+ *			compatible = "named-cbi-ssfc-value";
+ *			status = "okay";
+ *			value = <1>;
+ *		};
+ *	};
+ *	lid_sensor {
+ *		enum-name = "LID_SENSOR";
+ *		size = <3>;
+ *		bma255 {
+ *			compatible = "named-cbi-ssfc-value";
+ *			status = "okay";
+ *			value = <1>;
+ *		};
+ *	};
+ *	lightbar {
+ *		enum-name = "LIGHTBAR";
+ *		size = <2>;
+ *		10_led {
+ *			compatible = "named-cbi-ssfc-value";
+ *			status = "okay";
+ *			value = <1>;
+ *		};
+ *	};
+ * };
+ * Should be converted into
+ * union cbi_ssfc {
+ *	struct {
+ *		uint32_t cbi_ssfc_DT_N_S_cbi_ssfc_S_base_sensor:3
+ *		uint32_t cbi_ssfc_DT_N_S_cbi_ssfc_S_lid_sensor:3
+ *		uint32_t cbi_ssfc_DT_N_S_cbi_ssfc_S_lightbar:2
+ *		uint32_t reserved : 24;
+ *	};
+ *	uint32_t raw_value;
+ * };
+ */
+union cbi_ssfc {
+	struct {
+#if DT_NODE_EXISTS(CBI_SSFC_NODE)
+		DT_FOREACH_CHILD(CBI_SSFC_NODE, CBI_SSFC_UNION_ENTRY)
+		uint32_t reserved : (32 - CBI_SSFC_FIELDS_SIZE);
+#endif
+	};
+	uint32_t raw_value;
+};
+
+BUILD_ASSERT(sizeof(union cbi_ssfc) == sizeof(uint32_t),
+	     "CBI SSFS structure exceedes 32 bits");
+
+DT_INST_FOREACH_STATUS_OKAY(CBI_SSFC_VALUE_BUILD_ASSERT)
+
+static const uint8_t ssfc_values[] = {
+	DT_INST_FOREACH_STATUS_OKAY(CBI_SSFC_VALUE_ARRAY)
+};
+static union cbi_ssfc cached_ssfc;
+
+/* CBI SSFC part end */
+
+/* Device config */
+struct cros_cbi_config {
+	/* SSFC values for specific configs */
+	const uint8_t *ssfc_values;
+};
+
+/* Device data */
+struct cros_cbi_data {
+	/* Cached SSFC configs */
+	union cbi_ssfc cached_ssfc;
+};
+
+/* CBI SSFC part */
+
+static void cros_cbi_ssfc_init(const struct device *dev)
+{
+	struct cros_cbi_data *data = (struct cros_cbi_data *)(dev->data);
+
+	if (cbi_get_ssfc(&data->cached_ssfc.raw_value) != EC_SUCCESS) {
+		DT_INST_FOREACH_STATUS_OKAY(CBI_SSFC_INIT_DEFAULT)
+	}
+
+	LOG_INF("Read CBI SSFC : 0x%08X\n", data->cached_ssfc.raw_value);
+}
+
+static uint32_t cros_cbi_ssfc_get_parent_field_value(union cbi_ssfc cached_ssfc,
+						enum cbi_ssfc_value_id value_id)
+{
+	switch (value_id) {
+		DT_INST_FOREACH_STATUS_OKAY(CBI_SSFC_PARENT_VALUE_CASE)
+	default:
+		LOG_ERR("CBI SSFC parent field value not found: %d\n",
+		        value_id);
+		return 0;
+	}
+}
+
+static int cros_cbi_ec_ssfc_check_match(const struct device *dev,
+					enum cbi_ssfc_value_id value_id)
+{
+	struct cros_cbi_data *data = (struct cros_cbi_data *)(dev->data);
+	struct cros_cbi_config *cfg = (struct cros_cbi_config *)(dev->config);
+
+	return cros_cbi_ssfc_get_parent_field_value(data->cached_ssfc,
+						    value_id) ==
+	       cfg->ssfc_values[value_id];
+}
+
+/* CBI SSFC part end */
+#undef DT_DRV_COMPAT
+
+static int cros_cbi_ec_init(const struct device *dev)
+{
+	cros_cbi_ssfc_init(dev);
+
+	return 0;
+}
+
+/* cros ec cbi driver registration */
+static const struct cros_cbi_driver_api cros_cbi_driver_api = {
+	.init = cros_cbi_ec_init,
+	.ssfc_check_match = cros_cbi_ec_ssfc_check_match,
+};
+
+static int cbi_init(const struct device *dev)
+{
+	ARG_UNUSED(dev);
+
+	return 0;
+}
+
+static const struct cros_cbi_config cros_cbi_cfg = {
+	.ssfc_values = ssfc_values,
+};
+
+static struct cros_cbi_data cros_cbi_data;
+
+DEVICE_DEFINE(cros_cbi, CROS_CBI_LABEL, cbi_init, NULL, &cros_cbi_data,
+	      &cros_cbi_cfg, PRE_KERNEL_1, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT,
+	      &cros_cbi_driver_api);
diff --git a/zephyr/dts/bindings/cbi/named-cbi-ssfc-value.yaml b/zephyr/dts/bindings/cbi/named-cbi-ssfc-value.yaml
index 8755c8ae3d..f97d688727 100644
--- a/zephyr/dts/bindings/cbi/named-cbi-ssfc-value.yaml
+++ b/zephyr/dts/bindings/cbi/named-cbi-ssfc-value.yaml
@@ -14,12 +14,6 @@ properties:
         required: true
         description:
           Unique value of CBI SSFC field
-    devices:
-        type: phandles
-        required: true
-        description:
-          Pointers to alternative devices which has to be used if
-          CBI SSFC matches the defined value
     default:
         type: boolean
         description:
diff --git a/zephyr/dts/bindings/motionsense/motionsense-sensor-base.yaml b/zephyr/dts/bindings/motionsense/motionsense-sensor-base.yaml
index 9e8b7be2c3..6948ce6997 100644
--- a/zephyr/dts/bindings/motionsense/motionsense-sensor-base.yaml
+++ b/zephyr/dts/bindings/motionsense/motionsense-sensor-base.yaml
@@ -58,6 +58,10 @@ properties:
     type: phandle
     description: phandle to another sensor that can be swapped with this one
       at runtime.
+  alternate-indicator:
+    type: phandle
+    description: phandle to CBI SSGC value indicating that the sensor
+      should be used
 
 #
 # examples:
diff --git a/zephyr/include/drivers/cros_cbi.h b/zephyr/include/drivers/cros_cbi.h
new file mode 100644
index 0000000000..aa55e03b77
--- /dev/null
+++ b/zephyr/include/drivers/cros_cbi.h
@@ -0,0 +1,102 @@
+/* Copyright 2021 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+/**
+ * @file
+ * @brief Chrome OS-specific API for access to Cros Board Info(CBI)
+ */
+
+#ifndef ZEPHYR_INCLUDE_DRIVERS_CROS_CBI_H_
+#define ZEPHYR_INCLUDE_DRIVERS_CROS_CBI_H_
+
+#include <kernel.h>
+#include <device.h>
+#include <devicetree.h>
+
+#define CBI_SSFC_VALUE_COMPAT			named_cbi_ssfc_value
+#define CBI_SSFC_VALUE_ID(id)			DT_CAT(CBI_SSFC_VALUE_, id)
+#define CBI_SSFC_VALUE_ID_WITH_COMMA(id)	CBI_SSFC_VALUE_ID(id),
+#define CBI_SSFC_VALUE_INST_ENUM(inst, _) \
+	CBI_SSFC_VALUE_ID_WITH_COMMA(DT_INST(inst, CBI_SSFC_VALUE_COMPAT))
+#define CROS_CBI_LABEL				"cros_cbi"
+
+enum cbi_ssfc_value_id {
+	UTIL_LISTIFY(DT_NUM_INST_STATUS_OKAY(CBI_SSFC_VALUE_COMPAT),
+		     CBI_SSFC_VALUE_INST_ENUM)
+	CBI_SSFC_VALUE_COUNT
+};
+
+/**
+ * @cond INTERNAL_HIDDEN
+ *
+ * cros cbi raw driver API definition and system call entry points
+ *
+ * (Internal use only.)
+ */
+typedef int (*cros_cbi_api_init)(const struct device *dev);
+typedef int (*cros_cbi_api_ssfc_check_match)(const struct device *dev,
+					     enum cbi_ssfc_value_id value_id);
+
+__subsystem struct cros_cbi_driver_api {
+	cros_cbi_api_init init;
+	cros_cbi_api_ssfc_check_match ssfc_check_match;
+};
+
+/**
+ * @endcond
+ */
+
+/**
+ * @brief Initialize CBI.
+ *
+ * @param dev Pointer to the device structure for the CBI instance.
+ *
+ * @return 0 If successful.
+ * @retval -ENOTSUP Not supported api function.
+ */
+__syscall int cros_cbi_init(const struct device *dev);
+
+static inline int z_impl_cros_cbi_init(const struct device *dev)
+{
+	const struct cros_cbi_driver_api *api =
+		(const struct cros_cbi_driver_api *)dev->api;
+
+	if (!api->init) {
+		return -ENOTSUP;
+	}
+
+	return api->init(dev);
+}
+
+/**
+ * @brief Check if the CBI SSFC value matches the one in the EEPROM
+ *
+ * @param dev Pointer to the device.
+ *
+ * @return 1 If matches, 0 if not.
+ * @retval -ENOTSUP Not supported api function.
+ */
+__syscall int cros_cbi_ssfc_check_match(const struct device *dev,
+					enum cbi_ssfc_value_id value_id);
+
+static inline int
+z_impl_cros_cbi_ssfc_check_match(const struct device *dev,
+				 enum cbi_ssfc_value_id value_id)
+{
+	const struct cros_cbi_driver_api *api =
+		(const struct cros_cbi_driver_api *)dev->api;
+
+	if (!api->ssfc_check_match) {
+		return -ENOTSUP;
+	}
+
+	return api->ssfc_check_match(dev, value_id);
+}
+
+/**
+ * @}
+ */
+#include <syscalls/cros_cbi.h>
+#endif /* ZEPHYR_INCLUDE_DRIVERS_CROS_CBI_H_ */
diff --git a/zephyr/projects/volteer/volteer/cbi_eeprom.dts b/zephyr/projects/volteer/volteer/cbi_eeprom.dts
index ac2dff6f3a..65248a5f48 100644
--- a/zephyr/projects/volteer/volteer/cbi_eeprom.dts
+++ b/zephyr/projects/volteer/volteer/cbi_eeprom.dts
@@ -17,59 +17,52 @@
 };
 
 &cbi_ssfc_base_sensor {
-	bmi160 {
+	base_sensor_bmi160: bmi160 {
 		compatible = "named-cbi-ssfc-value";
 		status = "okay";
 
 		value = <1>;
-		devices = <>;
 	};
-	icm426xx {
+	base_sensor_icm426xx: icm426xx {
 		compatible = "named-cbi-ssfc-value";
 		status = "okay";
 
 		value = <2>;
-		devices = <>;
 	};
-	kx022 {
+	base_sensor_kx022: kx022 {
 		compatible = "named-cbi-ssfc-value";
 		status = "okay";
 
 		value = <3>;
-		devices = <>;
 	};
 };
 
 &cbi_ssfc_lid_sensor {
-	bma255 {
+	lid_sensor_bma255: bma255 {
 		compatible = "named-cbi-ssfc-value";
 		status = "okay";
 
 		value = <1>;
-		devices = <&lid_accel>;
 	};
-	kx022 {
+	lid_sensor_kx022: kx022 {
 		compatible = "named-cbi-ssfc-value";
 		status = "okay";
 
 		value = <2>;
-		devices = <>;
 	};
 };
 
 &cbi_ssfc_lightbar {
-	10_led {
+	lightbar_10_led: 10_led {
 		compatible = "named-cbi-ssfc-value";
 		status = "okay";
 
 		value = <1>;
-		devices = <>;
 	};
-	12_led {
+	lightbar_12_led: 12_led {
 		compatible = "named-cbi-ssfc-value";
 		status = "okay";
 
 		value = <2>;
-		devices = <>;
 	};
 };
diff --git a/zephyr/shim/include/cbi_ssfc.h b/zephyr/shim/include/cbi_ssfc.h
deleted file mode 100644
index 08b1223de2..0000000000
--- a/zephyr/shim/include/cbi_ssfc.h
+++ /dev/null
@@ -1,91 +0,0 @@
-/* Copyright 2021 The Chromium OS Authors. All rights reserved.
- * Use of this source code is governed by a BSD-style license that can be
- * found in the LICENSE file.
- */
-
-#ifndef ZEPHYR_SHIM_INCLUDE_CBI_SSFC_H_
-#define ZEPHYR_SHIM_INCLUDE_CBI_SSFC_H_
-
-#include <devicetree.h>
-#include <device.h>
-
-#define CBI_SSFC_NODE			DT_PATH(cbi_ssfc)
-
-#define CBI_SSFC_UNION_ENTRY_NAME(id)	DT_CAT(cbi_ssfc_, id)
-#define CBI_SSFC_UNION_ENTRY(id)               \
-	uint32_t CBI_SSFC_UNION_ENTRY_NAME(id) \
-		: DT_PROP(id, size);
-
-#define CBI_SSFC_PLUS_FIELD_SIZE(id)	+ DT_PROP(id, size)
-#define CBI_SSFC_FIELDS_SIZE                                         \
-	(0 COND_CODE_1(DT_NODE_EXISTS(CBI_SSFC_NODE),                \
-		       (DT_FOREACH_CHILD(CBI_SSFC_NODE,              \
-					 CBI_SSFC_PLUS_FIELD_SIZE)), \
-		       ()))
-
-BUILD_ASSERT(CBI_SSFC_FIELDS_SIZE <= 32, "CBI SSFS is bigger than 32 bits");
-
-/*
- * Define union bit fields based on the device tree entries. Example:
- * cbi-ssfc {
- *	compatible = "named-cbi-ssfc";
- *
- *	base_sensor {
- *		enum-name = "BASE_SENSOR";
- *		size = <3>;
- *		bmi160 {
- *			compatible = "named-cbi-ssfc-value";
- *			status = "okay";
- *
- *			value = <1>;
- *			devices = <>;
- *		};
- *	};
- *	lid_sensor {
- *		enum-name = "LID_SENSOR";
- *		size = <3>;
- *		bma255 {
- *			compatible = "named-cbi-ssfc-value";
- *			status = "okay";
- *
- *			value = <1>;
- *			devices = <&lid_accel>;
- *		};
- *	};
- *	lightbar {
- *		enum-name = "LIGHTBAR";
- *		size = <2>;
- *		10_led {
- *			compatible = "named-cbi-ssfc-value";
- *			status = "okay";
- *
- *			value = <1>;
- *			devices = <>;
- *		};
- *	};
- * };
- * Should be converted into
- * union cbi_ssfc {
- *	struct {
- *		uint32_t cbi_ssfc_DT_N_S_cbi_ssfc_S_base_sensor:3
- *		uint32_t cbi_ssfc_DT_N_S_cbi_ssfc_S_lid_sensor:3
- *		uint32_t cbi_ssfc_DT_N_S_cbi_ssfc_S_lightbar:2
- *		uint32_t reserved : 24;
- *	};
- *	uint32_t raw_value;
- * };
- */
-union cbi_ssfc {
-	struct {
-#if DT_NODE_EXISTS(CBI_SSFC_NODE)
-		DT_FOREACH_CHILD(CBI_SSFC_NODE, CBI_SSFC_UNION_ENTRY)
-		uint32_t reserved : (32 - CBI_SSFC_FIELDS_SIZE);
-#endif
-	};
-	uint32_t raw_value;
-};
-
-BUILD_ASSERT(sizeof(union cbi_ssfc) == sizeof(uint32_t),
-	"CBI SSFS structure exceedes 32 bits");
-
-#endif /* ZEPHYR_SHIM_INCLUDE_CBI_SSFC_H_ */
diff --git a/zephyr/shim/src/CMakeLists.txt b/zephyr/shim/src/CMakeLists.txt
index 5515a103c3..b29c9600b7 100644
--- a/zephyr/shim/src/CMakeLists.txt
+++ b/zephyr/shim/src/CMakeLists.txt
@@ -16,7 +16,7 @@ endif()
 zephyr_library_sources_ifdef(no_libgcc                  libgcc_${ARCH}.S)
 
 zephyr_library_sources_ifdef(CONFIG_PLATFORM_EC_ADC         adc.c)
-zephyr_library_sources_ifdef(CONFIG_PLATFORM_EC_CBI         cbi_ssfc.c)
+zephyr_library_sources_ifdef(CONFIG_PLATFORM_EC_CBI         cbi.c)
 zephyr_library_sources_ifdef(CONFIG_PLATFORM_EC_ESPI        espi.c)
 zephyr_library_sources_ifdef(CONFIG_PLATFORM_EC_FAN         fan.c)
 zephyr_library_sources_ifdef(CONFIG_PLATFORM_EC_FLASH_CROS  flash.c)
diff --git a/zephyr/shim/src/cbi.c b/zephyr/shim/src/cbi.c
new file mode 100644
index 0000000000..e9d85b6088
--- /dev/null
+++ b/zephyr/shim/src/cbi.c
@@ -0,0 +1,22 @@
+/* Copyright 2021 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#include <drivers/cros_cbi.h>
+#include <logging/log.h>
+#include "hooks.h"
+
+LOG_MODULE_REGISTER(shim_cbi, LOG_LEVEL_ERR);
+
+static void cbi_dev_init(void)
+{
+	const struct device *dev = device_get_binding(CROS_CBI_LABEL);
+
+	if (!dev)
+		LOG_ERR("Fail to find %s", CROS_CBI_LABEL);
+
+	cros_cbi_init(dev);
+}
+
+DECLARE_HOOK(HOOK_INIT, cbi_dev_init, HOOK_PRIO_FIRST);
diff --git a/zephyr/shim/src/cbi_ssfc.c b/zephyr/shim/src/cbi_ssfc.c
deleted file mode 100644
index 36f0e0eacc..0000000000
--- a/zephyr/shim/src/cbi_ssfc.c
+++ /dev/null
@@ -1,69 +0,0 @@
-/* Copyright 2021 The Chromium OS Authors. All rights reserved.
- * Use of this source code is governed by a BSD-style license that can be
- * found in the LICENSE file.
- */
-
-#include "cbi_ssfc.h"
-#include "cros_board_info.h"
-#include "hooks.h"
-#include "motionsense_sensors.h"
-#include "motion_sense.h"
-#include <logging/log.h>
-
-#define DT_DRV_COMPAT named_cbi_ssfc_value
-
-LOG_MODULE_REGISTER(cbi_ssfc_shim);
-
-static union cbi_ssfc cached_ssfc;
-
-#define REPLACE_ALT_MOTION_SENSOR(new_id, old_id) \
-	motion_sensors[SENSOR_ID(old_id)] =       \
-		motion_sensors_alt[SENSOR_ID(new_id)];
-
-#define ALT_MOTION_SENSOR_INIT_ID(id)                            \
-	COND_CODE_1(DT_NODE_HAS_PROP(id, alternate_for),         \
-		    (REPLACE_ALT_MOTION_SENSOR(                  \
-			    id, DT_PHANDLE(id, alternate_for))), \
-		    ())
-
-#define ALT_MOTION_SENSOR_INIT(i, id) \
-	ALT_MOTION_SENSOR_INIT_ID(DT_PHANDLE_BY_IDX(id, devices, i))
-
-#define SSFC_ALT_MOTION_SENSOR_INIT_ID(id)                                  \
-	do {                                                                \
-		if (DT_PROP(id, value) ==                                   \
-		    cached_ssfc.CBI_SSFC_UNION_ENTRY_NAME(DT_PARENT(id))) { \
-			UTIL_LISTIFY(DT_PROP_LEN(id, devices),              \
-				     ALT_MOTION_SENSOR_INIT, id)            \
-		}                                                           \
-	} while (0);
-
-#define SSFC_ALT_MOTION_SENSOR_INIT(inst) \
-	SSFC_ALT_MOTION_SENSOR_INIT_ID(DT_DRV_INST(inst))
-
-#define SSFC_INIT_DEFAULT_ID(id)                                               \
-	do {                                                                   \
-		if (DT_PROP(id, default)) {                                    \
-			cached_ssfc.CBI_SSFC_UNION_ENTRY_NAME(DT_PARENT(id)) = \
-			    DT_PROP(id, value);                                \
-		}                                                              \
-	} while (0);
-
-#define SSFC_INIT_DEFAULT(inst) \
-	SSFC_INIT_DEFAULT_ID(DT_DRV_INST(inst))
-
-static void cbi_ssfc_init(void)
-{
-	if (cbi_get_ssfc(&cached_ssfc.raw_value) != EC_SUCCESS) {
-		/* Default to values specified in DTS */
-		DT_INST_FOREACH_STATUS_OKAY(SSFC_INIT_DEFAULT)
-	}
-
-	LOG_INF("Read CBI SSFC : 0x%08X \n", cached_ssfc.raw_value);
-	/*
-	 * Adjust the motion_sensors array as soon as possible to initialize
-	 * correct sensors
-	 */
-	DT_INST_FOREACH_STATUS_OKAY(SSFC_ALT_MOTION_SENSOR_INIT)
-}
-DECLARE_HOOK(HOOK_INIT, cbi_ssfc_init, HOOK_PRIO_FIRST);
diff --git a/zephyr/shim/src/motionsense_sensors.c b/zephyr/shim/src/motionsense_sensors.c
index 329c7a4cbe..a7df4dbe5c 100644
--- a/zephyr/shim/src/motionsense_sensors.c
+++ b/zephyr/shim/src/motionsense_sensors.c
@@ -6,6 +6,7 @@
 #include "common.h"
 #include "accelgyro.h"
 #include "hooks.h"
+#include "drivers/cros_cbi.h"
 
 #define SENSOR_MUTEX_NODE		DT_PATH(motionsense_mutex)
 #define SENSOR_MUTEX_NAME(id)		DT_CAT(MUTEX_, id)
@@ -366,3 +367,37 @@ static void sensor_enable_irqs(void)
 }
 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)), ())
+
+void motion_sensors_init_alt(void)
+{
+	const struct device *dev = device_get_binding("cros_cbi");
+
+	if (dev == NULL)
+		return;
+
+#if DT_NODE_EXISTS(SENSOR_ALT_NODE)
+	DT_FOREACH_CHILD(SENSOR_ALT_NODE, ALT_MOTION_SENSOR_INIT_ID)
+#endif
+}
+
+DECLARE_HOOK(HOOK_INIT, motion_sensors_init_alt, HOOK_PRIO_INIT_I2C + 1);