|
| 1 | +/* |
| 2 | + * SPDX-License-Identifier: Apache-2.0 |
| 3 | + * |
| 4 | + * Command-line control for enabling/disabling emulated I2C devices |
| 5 | + * on native_sim. This allows testing device hot-plug and error scenarios. |
| 6 | + */ |
| 7 | + |
| 8 | +#include <zephyr/kernel.h> |
| 9 | +#include <zephyr/device.h> |
| 10 | +#include <zephyr/drivers/emul.h> |
| 11 | +#include <zephyr/drivers/i2c_emul.h> |
| 12 | +#include <zephyr/init.h> |
| 13 | +#include <zephyr/logging/log.h> |
| 14 | + |
| 15 | +#include "nsi_cmdline.h" |
| 16 | +#include "posix_native_task.h" |
| 17 | + |
| 18 | +LOG_MODULE_REGISTER(i2c_emul_control, LOG_LEVEL_INF); |
| 19 | + |
| 20 | +#define MAX_DISABLED_DEVICES 16 |
| 21 | + |
| 22 | +struct disabled_device { |
| 23 | + const char *name; |
| 24 | + const struct emul *emul; |
| 25 | + struct i2c_emul_api mock_api; |
| 26 | + bool disabled; |
| 27 | +}; |
| 28 | + |
| 29 | +static struct disabled_device disabled_devices[MAX_DISABLED_DEVICES]; |
| 30 | +static int num_disabled_devices = 0; |
| 31 | + |
| 32 | +static char *disabled_device_args[MAX_DISABLED_DEVICES]; |
| 33 | +static int num_disabled_device_args = 0; |
| 34 | + |
| 35 | +/* |
| 36 | + * Mock transfer function that returns -EIO (NACK) when device is disabled, |
| 37 | + * or -ENOSYS to fall back to the real emulator. |
| 38 | + */ |
| 39 | +static int disabled_device_transfer(const struct emul *target, |
| 40 | + struct i2c_msg *msgs, |
| 41 | + int num_msgs, |
| 42 | + int addr) { |
| 43 | + ARG_UNUSED(msgs); |
| 44 | + ARG_UNUSED(num_msgs); |
| 45 | + ARG_UNUSED(addr); |
| 46 | + |
| 47 | + for (int i = 0; i < num_disabled_devices; i++) { |
| 48 | + if (disabled_devices[i].emul == target) { |
| 49 | + if (disabled_devices[i].disabled) { |
| 50 | + LOG_DBG("Device %s is disabled, returning -EIO", |
| 51 | + disabled_devices[i].name); |
| 52 | + return -EIO; |
| 53 | + } |
| 54 | + break; |
| 55 | + } |
| 56 | + } |
| 57 | + /* Fall back to normal emulator behavior */ |
| 58 | + return -ENOSYS; |
| 59 | +} |
| 60 | + |
| 61 | +int i2c_emul_control_disable_device(const char *name) { |
| 62 | + const struct emul *emul = emul_get_binding(name); |
| 63 | + if (!emul) { |
| 64 | + LOG_ERR("Emulator '%s' not found", name); |
| 65 | + return -ENODEV; |
| 66 | + } |
| 67 | + |
| 68 | + if (emul->bus_type != EMUL_BUS_TYPE_I2C) { |
| 69 | + LOG_ERR("Emulator '%s' is not an I2C device", name); |
| 70 | + return -EINVAL; |
| 71 | + } |
| 72 | + |
| 73 | + /* Find existing entry or create new one */ |
| 74 | + int idx = -1; |
| 75 | + for (int i = 0; i < num_disabled_devices; i++) { |
| 76 | + if (disabled_devices[i].emul == emul) { |
| 77 | + idx = i; |
| 78 | + break; |
| 79 | + } |
| 80 | + } |
| 81 | + |
| 82 | + if (idx < 0) { |
| 83 | + if (num_disabled_devices >= MAX_DISABLED_DEVICES) { |
| 84 | + LOG_ERR("Too many disabled devices"); |
| 85 | + return -ENOMEM; |
| 86 | + } |
| 87 | + idx = num_disabled_devices++; |
| 88 | + disabled_devices[idx].name = name; |
| 89 | + disabled_devices[idx].emul = emul; |
| 90 | + disabled_devices[idx].mock_api.transfer = disabled_device_transfer; |
| 91 | + |
| 92 | + /* Install our mock_api to intercept transfers */ |
| 93 | + emul->bus.i2c->mock_api = &disabled_devices[idx].mock_api; |
| 94 | + } |
| 95 | + |
| 96 | + disabled_devices[idx].disabled = true; |
| 97 | + LOG_INF("Disabled I2C emulator: %s", name); |
| 98 | + return 0; |
| 99 | +} |
| 100 | + |
| 101 | +int i2c_emul_control_enable_device(const char *name) { |
| 102 | + for (int i = 0; i < num_disabled_devices; i++) { |
| 103 | + if (strcmp(disabled_devices[i].name, name) == 0) { |
| 104 | + disabled_devices[i].disabled = false; |
| 105 | + LOG_INF("Enabled I2C emulator: %s", name); |
| 106 | + return 0; |
| 107 | + } |
| 108 | + } |
| 109 | + LOG_ERR("Device '%s' not in disabled list", name); |
| 110 | + return -ENODEV; |
| 111 | +} |
| 112 | + |
| 113 | +bool i2c_emul_control_is_disabled(const char *name) { |
| 114 | + for (int i = 0; i < num_disabled_devices; i++) { |
| 115 | + if (strcmp(disabled_devices[i].name, name) == 0) { |
| 116 | + return disabled_devices[i].disabled; |
| 117 | + } |
| 118 | + } |
| 119 | + return false; |
| 120 | +} |
| 121 | + |
| 122 | +/* Command-line option handler */ |
| 123 | +static void cmd_disable_i2c_device(char *argv, int offset) { |
| 124 | + /* The value is at argv + offset (after the '=' in --disable-i2c=value) */ |
| 125 | + char *value = argv + offset; |
| 126 | + if (num_disabled_device_args < MAX_DISABLED_DEVICES) { |
| 127 | + disabled_device_args[num_disabled_device_args++] = value; |
| 128 | + } else { |
| 129 | + printk("i2c_emul_control: Too many --disable-i2c arguments, ignoring: %s\n", value); |
| 130 | + } |
| 131 | +} |
| 132 | + |
| 133 | +static struct args_struct_t i2c_emul_args[] = { |
| 134 | + { |
| 135 | + .option = "disable-i2c", |
| 136 | + .name = "device", |
| 137 | + .type = 's', |
| 138 | + .call_when_found = cmd_disable_i2c_device, |
| 139 | + .descript = "Disable an emulated I2C device by name (can be repeated). " |
| 140 | + "Example: --disable-i2c=bmi160" |
| 141 | + }, |
| 142 | + ARG_TABLE_ENDMARKER |
| 143 | +}; |
| 144 | + |
| 145 | +static void register_cmdline_opts(void) { |
| 146 | + nsi_add_command_line_opts(i2c_emul_args); |
| 147 | +} |
| 148 | + |
| 149 | +/* Register command-line options early in boot */ |
| 150 | +NATIVE_TASK(register_cmdline_opts, PRE_BOOT_1, 0); |
| 151 | + |
| 152 | +static int apply_disabled_devices(void) { |
| 153 | + LOG_DBG("Applying %d disabled device(s)", num_disabled_device_args); |
| 154 | + for (int i = 0; i < num_disabled_device_args; i++) { |
| 155 | + int rc = i2c_emul_control_disable_device(disabled_device_args[i]); |
| 156 | + if (rc != 0) { |
| 157 | + LOG_WRN("Failed to disable I2C device '%s': %d", |
| 158 | + disabled_device_args[i], rc); |
| 159 | + } |
| 160 | + } |
| 161 | + return 0; |
| 162 | +} |
| 163 | + |
| 164 | +/* |
| 165 | + * Apply after emulators are initialized. |
| 166 | + * I2C emulators are registered at POST_KERNEL level, so we need to run |
| 167 | + * at APPLICATION level to ensure they exist. |
| 168 | + */ |
| 169 | +SYS_INIT(apply_disabled_devices, APPLICATION, 99); |
0 commit comments