From ecbfa821ee56767c53ae66746ea79c6c675ac5a7 Mon Sep 17 00:00:00 2001 From: Uzi Date: Sun, 1 Mar 2026 09:58:16 +0200 Subject: [PATCH 01/11] d4xx: fix HW reset recovery for GMSL cameras Fix two HW reset issues: 1. [D457 GMSL] No device found after HW Reset - caused by chip-wide deserializer reset_oneshot disrupting sibling camera links 2. [D401 GMSL] Fail to stream after HW reset during active streaming - caused by stale sensor->streaming flags blocking subsequent stream starts Changes: - Stop active streams before sending HW reset command to ensure clean state - Clear sensor->streaming flags and release SERDES pipes eagerly during reset (uncomment and enhance the previously disabled invalidation block) - Replace chip-wide reset_oneshot with tiered SERDES recovery: Phase 1: serializer-only re-init (per-camera, non-disruptive) Phase 2: full deserializer reset only when all siblings are also dead, with I2C health check gate to protect actively streaming cameras - Replace global ds5_reset_gen counter with per-deserializer counters to prevent cross-camera state invalidation across independent GMSL links - Relocate serdes_inited[] and MAX_DEV_NUM earlier for visibility Fixes: RSDSO-21151, RSDSO-21257, RSDSO-21254 --- kernel/realsense/d4xx.c | 316 ++++++++++++++++++++++++++++++++++------ 1 file changed, 274 insertions(+), 42 deletions(-) diff --git a/kernel/realsense/d4xx.c b/kernel/realsense/d4xx.c index bea6b11f..0566d123 100644 --- a/kernel/realsense/d4xx.c +++ b/kernel/realsense/d4xx.c @@ -499,6 +499,55 @@ static atomic_t ds5_probe_reset_once = ATOMIC_INIT(0); #ifdef CONFIG_VIDEO_D4XX_SERDES static DEFINE_MUTEX(serdes_lock__); +/* + * Per-deserializer reset generation counter. + * Replaces the old global ds5_reset_gen for SERDES builds so that + * resetting camera A does not force camera B (on a different deserializer) + * to invalidate its state. Cameras sharing the same deserializer still + * see each other's resets through the shared counter. + */ +#define MAX_DSER_NUM 8 +struct dser_reset_gen { + struct device *dser_dev; + atomic_t gen; +}; +static struct dser_reset_gen dser_reset_gens[MAX_DSER_NUM]; + +static atomic_t *ds5_get_reset_gen(struct ds5 *state) +{ + int i; + + if (state->dser_dev) { + for (i = 0; i < MAX_DSER_NUM; i++) { + if (dser_reset_gens[i].dser_dev == state->dser_dev) + return &dser_reset_gens[i].gen; + if (!dser_reset_gens[i].dser_dev) { + dser_reset_gens[i].dser_dev = state->dser_dev; + atomic_set(&dser_reset_gens[i].gen, 0); + return &dser_reset_gens[i].gen; + } + } + } + /* Fallback: table full or no deserializer */ + return &ds5_reset_gen; +} +#else +static inline atomic_t *ds5_get_reset_gen(struct ds5 *state) +{ + return &ds5_reset_gen; +} +#endif + +/* + * max 24 number from this link: + * https://docs.nvidia.com/jetson/archives/r35.1/DeveloperGuide/text/ + * SD/CameraDevelopment/JetsonVirtualChannelWithGmslCameraFramework.html + * #jetson-agx-xavier-series + */ +#define MAX_DEV_NUM 24 +#ifdef CONFIG_VIDEO_D4XX_SERDES +static struct ds5 *serdes_inited[MAX_DEV_NUM]; + /* MAX9296 deserializer interface implementation */ static const struct dser_interface max9296_interface = { .get_available_pipe_id = max9296_get_available_pipe_id, @@ -1711,7 +1760,7 @@ static int ds5_configure(struct ds5 *state) u16 height_value = 0; int ret; - current_reset_gen = atomic_read(&ds5_reset_gen); + current_reset_gen = atomic_read(ds5_get_reset_gen(state)); if (state->reset_gen != current_reset_gen) { struct ds5_sensor *active = ds5_get_active_sensor(state); @@ -2238,14 +2287,165 @@ static int ds5_set_calibration_data(struct ds5 *state, #define DS5_HW_RESET_STATUS_READY 0xDEAD #define DS5_HW_RESET_DFU_MAGIC_LSW 0x0201 /* Lower 16 bits of 0x04030201 */ +/* + * ds5_hw_reset_serdes_recovery - Tiered SERDES recovery after HW reset. + * + * Phase 1: Re-init serializer only (per-camera, non-disruptive). + * Phase 2: If I2C still fails, check whether all sibling cameras on the + * same deserializer are also dead. Only then perform a full + * deserializer reset_oneshot (chip-wide, disruptive). + * + * Returns 0 on success, negative errno on failure. + */ +#ifdef CONFIG_VIDEO_D4XX_SERDES +static int ds5_hw_reset_serdes_recovery(struct ds5 *state) +{ + int ret; + int i; + u16 tmp = 0; + bool sibling_alive = false; + + if (!state->ser_dev || !state->dser_dev) + return 0; + + /* Phase 1 — serializer-only re-init (per-camera, safe) */ + dev_info(&state->client->dev, + "%s(): Phase 1 - re-initializing serializer\n", __func__); + ret = max9295_init_settings(state->ser_dev); + if (ret < 0) + dev_warn(&state->client->dev, + "%s(): serializer init_settings failed: %d\n", + __func__, ret); + + msleep(100); + + /* Verify I2C link to camera is working */ + ret = ds5_read(state, DS5_FW_VERSION, &tmp); + if (ret == 0) { + dev_info(&state->client->dev, + "%s(): Phase 1 succeeded, I2C link OK\n", __func__); + return 0; + } + + /* Phase 2 — I2C still broken; consider full deserializer reset */ + dev_warn(&state->client->dev, + "%s(): Phase 1 failed (I2C err %d), checking siblings before deser reset\n", + __func__, ret); + + for (i = 0; i < MAX_DEV_NUM; i++) { + struct ds5 *sib = serdes_inited[i]; + + if (!sib || sib == state || sib->dser_dev != state->dser_dev) + continue; + + /* Check if sibling can still communicate over I2C */ + if (ds5_read(sib, DS5_FW_VERSION, &tmp) == 0) { + /* Check if sibling has any active stream */ + if (sib->depth.sensor.streaming || + sib->ir.sensor.streaming || + sib->rgb.sensor.streaming || + sib->imu.sensor.streaming) { + sibling_alive = true; + dev_warn(&state->client->dev, + "%s(): sibling %s alive & streaming, skipping deser reset\n", + __func__, dev_name(&sib->client->dev)); + break; + } + } + } + + if (sibling_alive) { + dev_err(&state->client->dev, + "%s(): GMSL link broken but sibling streaming — cannot reset deserializer. " + "Manual recovery (driver reload / camera power cycle) may be needed.\n", + __func__); + return -EIO; + } + + /* All siblings dead or none exist — safe to reset entire deserializer */ + dev_info(&state->client->dev, + "%s(): Phase 2 - performing full deserializer reset\n", __func__); + + mutex_lock(&serdes_lock__); + state->dser_ops->reset_oneshot(state->dser_dev); + msleep(300); + + /* Re-establish link & control for this camera's serializer */ + ret = state->dser_ops->setup_link(state->dser_dev, &state->client->dev); + if (ret) + dev_warn(&state->client->dev, + "%s(): deser setup_link failed: %d\n", __func__, ret); + + msleep(100); + + ret = max9295_setup_control(state->ser_dev); + if (ret) + dev_warn(&state->client->dev, + "%s(): ser setup_control failed: %d\n", __func__, ret); + + ret = state->dser_ops->setup_control(state->dser_dev, &state->client->dev); + if (ret) + dev_warn(&state->client->dev, + "%s(): deser setup_control failed: %d\n", __func__, ret); + + ret = max9295_init_settings(state->ser_dev); + if (ret) + dev_warn(&state->client->dev, + "%s(): ser init_settings failed: %d\n", __func__, ret); + + ret = state->dser_ops->init_settings(state->dser_dev); + if (ret) + dev_warn(&state->client->dev, + "%s(): deser init_settings failed: %d\n", __func__, ret); + + mutex_unlock(&serdes_lock__); + + /* Verify I2C link after full recovery */ + ret = ds5_read(state, DS5_FW_VERSION, &tmp); + if (ret < 0) { + dev_err(&state->client->dev, + "%s(): Phase 2 failed, I2C still broken: %d\n", + __func__, ret); + return ret; + } + + dev_info(&state->client->dev, + "%s(): Phase 2 succeeded, full deser recovery complete\n", __func__); + + /* Invalidate all sibling cameras so they re-configure on next stream start */ + for (i = 0; i < MAX_DEV_NUM; i++) { + struct ds5 *sib = serdes_inited[i]; + + if (!sib || sib == state || sib->dser_dev != state->dser_dev) + continue; + + ds5_invalidate_sensor(sib, &sib->depth.sensor); + ds5_invalidate_sensor(sib, &sib->ir.sensor); + ds5_invalidate_sensor(sib, &sib->rgb.sensor); + ds5_invalidate_sensor(sib, &sib->imu.sensor); + sib->depth.sensor.streaming = false; + sib->ir.sensor.streaming = false; + sib->rgb.sensor.streaming = false; + sib->imu.sensor.streaming = false; + + dev_warn(&state->client->dev, + "%s(): invalidated sibling %s after deser reset\n", + __func__, dev_name(&sib->client->dev)); + } + + return 0; +} +#endif /* CONFIG_VIDEO_D4XX_SERDES */ + /* * ds5_hw_reset_with_recovery - Perform hardware reset with GMSL recovery * @state: Driver state structure * - * This function sends a hardware reset command to the D4XX device and - * waits for it to come back online. For GMSL connections, unlike USB, - * there is no automatic re-enumeration, so we must poll the device - * until it becomes responsive again. + * Sends a hardware reset command to the D4XX device and waits for it to + * come back online. Before resetting, stops active streams and invalidates + * all driver-side sensor state (streaming flags, SERDES pipes, config cache). + * After the device responds, performs tiered SERDES recovery (serializer-only + * first, full deserializer reset only when all siblings are also dead). * * Returns 0 on success, negative error code on failure. */ @@ -2253,45 +2453,75 @@ static int ds5_hw_reset_with_recovery(struct ds5 *state) { int ret; int retry; + int i; u16 status = 0; bool device_went_down = false; - - dev_info(&state->client->dev, "%s(): Initiating HW reset with recovery\n", - __func__); - - /* Invalidate cached sensor configs and release SERDES pipes before reset, since reset will clear device state but not driver state. - This ensures we don't try to reuse stale configs or pipes after reset. */ - /* TBD: Open for single instance architecture and remove ds5_invalidate_sensor multi instance solution struct ds5_sensor *sensors[] = { &state->depth.sensor, &state->ir.sensor, &state->rgb.sensor, &state->imu.sensor, }; - int i; + static const u16 stream_ids[] = { + DS5_STREAM_DEPTH, + DS5_STREAM_IR, + DS5_STREAM_RGB, + DS5_STREAM_IMU, + }; - if (state->dser_dev) - { + dev_info(&state->client->dev, "%s(): Initiating HW reset with recovery\n", + __func__); + + /* 1. Stop active streams on the device before reset. + * This ensures FW and SERDES are in a clean state. + */ + for (i = 0; i < ARRAY_SIZE(sensors); i++) { + if (sensors[i]->streaming) { + dev_info(&state->client->dev, + "%s(): stopping active stream %d before reset\n", + __func__, stream_ids[i]); + ds5_write(state, DS5_START_STOP_STREAM, + DS5_STREAM_STOP | stream_ids[i]); + } + } + + /* 2. Invalidate all sensor state and release SERDES pipes. + * After HW reset the device loses all configuration, so driver + * state must be brought in sync. Clear streaming flags so that + * ds5_mux_s_stream() won't silently skip the next stream-start. + */ +#ifdef CONFIG_VIDEO_D4XX_SERDES + if (state->dser_dev) { mutex_lock(&serdes_lock__); - for (i = 0; i < ARRAY_SIZE(sensors); i++) - { + for (i = 0; i < ARRAY_SIZE(sensors); i++) { struct ds5_sensor *sensor = sensors[i]; ds5_config_cache_clear(sensor); + sensor->streaming = false; - if (sensor->pipe_id >= 0) - { - int release_ret = max9296_release_pipe(state->dser_dev, sensor->pipe_id); - dev_warn(&state->client->dev, "release pipe %d (%d)\n", - sensor->pipe_id, release_ret); + if (sensor->pipe_id >= 0) { + int release_ret = state->dser_ops->release_pipe( + state->dser_dev, sensor->pipe_id); + dev_info(&state->client->dev, + "%s(): released pipe %d (%d)\n", + __func__, sensor->pipe_id, release_ret); sensor->pipe_id = PIPE_NOT_CONFIGURED; } + sensor->pipe_data_type1 = 0; + sensor->pipe_data_type2 = 0; + sensor->pipe_vc_id = 0; } mutex_unlock(&serdes_lock__); + } else +#endif + { + for (i = 0; i < ARRAY_SIZE(sensors); i++) { + ds5_config_cache_clear(sensors[i]); + sensors[i]->streaming = false; + } } - */ - /* 1. Send HW reset command */ + /* 3. Send HW reset command */ ret = ds5_raw_write(state, DS5_HWMC_DATA, &cmd_hw_reset, sizeof(cmd_hw_reset)); if (ret < 0) { dev_err(&state->client->dev, "%s(): Failed to write HW reset command: %d\n", @@ -2305,15 +2535,15 @@ static int ds5_hw_reset_with_recovery(struct ds5 *state) __func__, ret); return ret; } - atomic_inc(&ds5_reset_gen); + atomic_inc(ds5_get_reset_gen(state)); dev_info(&state->client->dev, "%s(): HW reset command sent, waiting for device...\n", __func__); - /* 2. Brief delay to allow reset to begin */ + /* 4. Brief delay to allow reset to begin */ msleep(DS5_HW_RESET_INITIAL_DELAY_MS); - /* 3. Poll for device to come back online */ + /* 5. Poll for device to come back online */ for (retry = 0; retry < DS5_HW_RESET_MAX_RETRIES; retry++) { msleep(DS5_HW_RESET_POLL_INTERVAL_MS); @@ -2363,16 +2593,19 @@ static int ds5_hw_reset_with_recovery(struct ds5 *state) } #ifdef CONFIG_VIDEO_D4XX_SERDES - /* 4. Re-initialize SERDES link if available */ - if (state->dser_dev) { - dev_info(&state->client->dev, - "%s(): Re-initializing SERDES link\n", __func__); - state->dser_ops->reset_oneshot(state->dser_dev); - msleep(300); + /* 6. Tiered SERDES recovery: + * Phase 1: serializer-only re-init (per-camera, non-disruptive). + * Phase 2: full deserializer reset only if ALL siblings are also dead. + */ + ret = ds5_hw_reset_serdes_recovery(state); + if (ret < 0) { + dev_err(&state->client->dev, + "%s(): SERDES recovery failed: %d\n", __func__, ret); + return ret; } #endif - /* 5. Verify device is operational by reading firmware version */ + /* 7. Verify device is operational by reading firmware version */ ret = ds5_read(state, DS5_FW_VERSION, &state->fw_version); if (ret < 0) { dev_err(&state->client->dev, @@ -2387,7 +2620,7 @@ static int ds5_hw_reset_with_recovery(struct ds5 *state) return ret; } - /* 6. Wait for device type register to be populated. + /* 8. Wait for device type register to be populated. * After HW reset the firmware sets status to 0xDEAD before fully * initializing configuration registers (DS5_DEVICE_TYPE, stream DT, * resolution, etc.). If ds5_fixed_configuration() reads device type @@ -3386,13 +3619,9 @@ static const struct v4l2_subdev_internal_ops ds5_sensor_internal_ops = { * sensors into one i2c device. Only first sensor node per max9295 sets up the * link. * - * max 24 number from this link: - * https://docs.nvidia.com/jetson/archives/r35.1/DeveloperGuide/text/ - * SD/CameraDevelopment/JetsonVirtualChannelWithGmslCameraFramework.html - * #jetson-agx-xavier-series + * serdes_inited[] and MAX_DEV_NUM are defined near the top of the file + * (after serdes_lock__). */ -#define MAX_DEV_NUM 24 -static struct ds5 *serdes_inited[MAX_DEV_NUM]; #ifdef CONFIG_OF static int ds5_board_setup(struct ds5 *state) { @@ -5819,7 +6048,6 @@ static int ds5_probe(struct i2c_client *c, const struct i2c_device_id *id) mutex_init(&state->lock); state->client = c; - state->reset_gen = atomic_read(&ds5_reset_gen); dev_warn(&c->dev, "Probing driver for D4xx\n"); #ifdef CONFIG_OF ret = of_property_read_u32(c->dev.of_node, "override_reg", &override_addr); @@ -5858,6 +6086,10 @@ static int ds5_probe(struct i2c_client *c, const struct i2c_device_id *id) if (ret < 0) goto e_regulator; #endif + /* Initialize reset generation counter after SERDES setup so that + * ds5_get_reset_gen() can resolve the per-deserializer counter. */ + state->reset_gen = atomic_read(ds5_get_reset_gen(state)); + // Verify communication retry = 5; do { From 97b91b913baff0a460a73e5fbe979b4ee52be756 Mon Sep 17 00:00:00 2001 From: Uzi Date: Sun, 1 Mar 2026 14:35:58 +0200 Subject: [PATCH 02/11] d4xx: fix HW reset recovery for per-stream-instance architecture Each physical D4XX camera has 4 driver instances (Depth, RGB, IR, IMU) sharing the same MAX9295 serializer (ser_dev). The initial HW reset recovery implementation treated all instances on the same deserializer as siblings, which caused incorrect behavior: 1. Same-camera instances (same ser_dev) were treated as 'siblings with active streams' in Phase 2, blocking deserializer reset even when no true sibling camera was streaming. 2. The streaming flag check examined all 4 sensor types per instance, but each instance only manages ONE sensor type (via is_depth/is_rgb/ is_y8/is_imu flags). 3. Steps 1 and 2 (stop streams, invalidate state) only operated on the current instance, leaving peer instances of the same physical camera with stale streaming flags and SERDES pipe state after HW reset. Fix by: - Distinguishing same-camera peers (same ser_dev) from true siblings (different ser_dev, same dser_dev) in SERDES recovery Phase 2 - Using ds5_get_active_sensor() to check only the managed sensor type per instance instead of all 4 sensor structs - Iterating serdes_inited[] to stop streams and invalidate state on all peer instances of the same physical camera in Steps 1 and 2 - Using __maybe_unused for loop variable only needed in SERDES path JIRA: RSDSO-21151, RSDSO-21257, RSDSO-21254 --- kernel/realsense/d4xx.c | 211 +++++++++++++++++++++++++++------------- 1 file changed, 141 insertions(+), 70 deletions(-) diff --git a/kernel/realsense/d4xx.c b/kernel/realsense/d4xx.c index 0566d123..9ae10f02 100644 --- a/kernel/realsense/d4xx.c +++ b/kernel/realsense/d4xx.c @@ -2332,37 +2332,59 @@ static int ds5_hw_reset_serdes_recovery(struct ds5 *state) "%s(): Phase 1 failed (I2C err %d), checking siblings before deser reset\n", __func__, ret); + /* + * In the D4XX architecture each physical camera has 4 driver instances + * (Depth, RGB, IR, IMU) sharing the same ser_dev. A "true sibling" is + * a different physical camera on the same deserializer: same dser_dev + * but different ser_dev. + * + * Same-camera instances are always safe to reset together since they + * share the same camera ASIC — the HW reset already killed them all. + */ for (i = 0; i < MAX_DEV_NUM; i++) { struct ds5 *sib = serdes_inited[i]; + bool sib_streaming; - if (!sib || sib == state || sib->dser_dev != state->dser_dev) + if (!sib || sib == state) continue; - /* Check if sibling can still communicate over I2C */ - if (ds5_read(sib, DS5_FW_VERSION, &tmp) == 0) { - /* Check if sibling has any active stream */ - if (sib->depth.sensor.streaming || - sib->ir.sensor.streaming || - sib->rgb.sensor.streaming || - sib->imu.sensor.streaming) { - sibling_alive = true; - dev_warn(&state->client->dev, - "%s(): sibling %s alive & streaming, skipping deser reset\n", - __func__, dev_name(&sib->client->dev)); - break; - } + /* Skip instances of the SAME physical camera (same serializer) */ + if (sib->ser_dev == state->ser_dev) + continue; + + /* Only check cameras on the same deserializer (true siblings) */ + if (sib->dser_dev != state->dser_dev) + continue; + + /* True sibling — check if its I2C link is alive */ + if (ds5_read(sib, DS5_FW_VERSION, &tmp) != 0) + continue; + + /* Check only the sensor type this instance actually manages */ + sib_streaming = (sib->is_depth && sib->depth.sensor.streaming) || + (sib->is_rgb && sib->rgb.sensor.streaming) || + (sib->is_y8 && sib->ir.sensor.streaming) || + (sib->is_imu && sib->imu.sensor.streaming); + + if (sib_streaming) { + sibling_alive = true; + dev_warn(&state->client->dev, + "%s(): true sibling %s alive & streaming, skipping deser reset\n", + __func__, dev_name(&sib->client->dev)); + break; } } if (sibling_alive) { dev_err(&state->client->dev, - "%s(): GMSL link broken but sibling streaming — cannot reset deserializer. " - "Manual recovery (driver reload / camera power cycle) may be needed.\n", + "%s(): GMSL link broken but sibling camera streaming — " + "cannot reset deserializer. Manual recovery " + "(driver reload / camera power cycle) may be needed.\n", __func__); return -EIO; } - /* All siblings dead or none exist — safe to reset entire deserializer */ + /* All true siblings dead or none streaming — safe to reset entire deserializer */ dev_info(&state->client->dev, "%s(): Phase 2 - performing full deserializer reset\n", __func__); @@ -2412,25 +2434,31 @@ static int ds5_hw_reset_serdes_recovery(struct ds5 *state) dev_info(&state->client->dev, "%s(): Phase 2 succeeded, full deser recovery complete\n", __func__); - /* Invalidate all sibling cameras so they re-configure on next stream start */ + /* Invalidate all cameras on this deserializer so they re-configure + * on next stream start. This covers both same-camera peer instances + * (same ser_dev) and true sibling cameras (different ser_dev). + */ for (i = 0; i < MAX_DEV_NUM; i++) { struct ds5 *sib = serdes_inited[i]; + struct ds5_sensor *active; - if (!sib || sib == state || sib->dser_dev != state->dser_dev) + if (!sib || sib == state) + continue; + if (sib->dser_dev != state->dser_dev) continue; - ds5_invalidate_sensor(sib, &sib->depth.sensor); - ds5_invalidate_sensor(sib, &sib->ir.sensor); - ds5_invalidate_sensor(sib, &sib->rgb.sensor); - ds5_invalidate_sensor(sib, &sib->imu.sensor); - sib->depth.sensor.streaming = false; - sib->ir.sensor.streaming = false; - sib->rgb.sensor.streaming = false; - sib->imu.sensor.streaming = false; + /* Invalidate only the sensor type this instance manages */ + active = ds5_get_active_sensor(sib); + if (active) { + ds5_invalidate_sensor(sib, active); + active->streaming = false; + } dev_warn(&state->client->dev, - "%s(): invalidated sibling %s after deser reset\n", - __func__, dev_name(&sib->client->dev)); + "%s(): invalidated %s %s after deser reset\n", + __func__, + (sib->ser_dev == state->ser_dev) ? "same-cam" : "sibling", + dev_name(&sib->client->dev)); } return 0; @@ -2453,74 +2481,117 @@ static int ds5_hw_reset_with_recovery(struct ds5 *state) { int ret; int retry; - int i; + int __maybe_unused i; u16 status = 0; bool device_went_down = false; - struct ds5_sensor *sensors[] = { - &state->depth.sensor, - &state->ir.sensor, - &state->rgb.sensor, - &state->imu.sensor, - }; - static const u16 stream_ids[] = { - DS5_STREAM_DEPTH, - DS5_STREAM_IR, - DS5_STREAM_RGB, - DS5_STREAM_IMU, - }; dev_info(&state->client->dev, "%s(): Initiating HW reset with recovery\n", __func__); /* 1. Stop active streams on the device before reset. * This ensures FW and SERDES are in a clean state. + * + * In the D4XX architecture each physical camera has 4 driver + * instances (Depth, RGB, IR, IMU) sharing the same ser_dev. + * HW reset kills all streams on the camera ASIC, so we must + * stop and invalidate all peer instances of the same camera. */ - for (i = 0; i < ARRAY_SIZE(sensors); i++) { - if (sensors[i]->streaming) { + { + struct ds5_sensor *active = ds5_get_active_sensor(state); + + if (active && active->streaming) { + u16 sid = state->is_depth ? DS5_STREAM_DEPTH : + state->is_rgb ? DS5_STREAM_RGB : + state->is_y8 ? DS5_STREAM_IR : + DS5_STREAM_IMU; dev_info(&state->client->dev, - "%s(): stopping active stream %d before reset\n", - __func__, stream_ids[i]); + "%s(): stopping own stream %d before reset\n", + __func__, sid); ds5_write(state, DS5_START_STOP_STREAM, - DS5_STREAM_STOP | stream_ids[i]); + DS5_STREAM_STOP | sid); } } - /* 2. Invalidate all sensor state and release SERDES pipes. +#ifdef CONFIG_VIDEO_D4XX_SERDES + /* Stop streams on peer instances of the same physical camera */ + for (i = 0; i < MAX_DEV_NUM; i++) { + struct ds5 *peer = serdes_inited[i]; + struct ds5_sensor *peer_active; + + if (!peer || peer == state) + continue; + if (peer->ser_dev != state->ser_dev) + continue; + + peer_active = ds5_get_active_sensor(peer); + if (peer_active && peer_active->streaming) { + u16 sid = peer->is_depth ? DS5_STREAM_DEPTH : + peer->is_rgb ? DS5_STREAM_RGB : + peer->is_y8 ? DS5_STREAM_IR : + DS5_STREAM_IMU; + dev_info(&state->client->dev, + "%s(): stopping peer %s stream %d before reset\n", + __func__, dev_name(&peer->client->dev), sid); + ds5_write(peer, DS5_START_STOP_STREAM, + DS5_STREAM_STOP | sid); + } + } +#endif + + /* 2. Invalidate sensor state and release SERDES pipes. * After HW reset the device loses all configuration, so driver * state must be brought in sync. Clear streaming flags so that * ds5_mux_s_stream() won't silently skip the next stream-start. + * Covers this instance AND all peer instances of the same camera. */ -#ifdef CONFIG_VIDEO_D4XX_SERDES - if (state->dser_dev) { - mutex_lock(&serdes_lock__); - for (i = 0; i < ARRAY_SIZE(sensors); i++) { - struct ds5_sensor *sensor = sensors[i]; + { + struct ds5_sensor *active = ds5_get_active_sensor(state); - ds5_config_cache_clear(sensor); - sensor->streaming = false; + if (active) { + ds5_config_cache_clear(active); + active->streaming = false; +#ifdef CONFIG_VIDEO_D4XX_SERDES + if (active->pipe_id >= 0 && state->dser_dev) { + int release_ret; - if (sensor->pipe_id >= 0) { - int release_ret = state->dser_ops->release_pipe( - state->dser_dev, sensor->pipe_id); + mutex_lock(&serdes_lock__); + release_ret = state->dser_ops->release_pipe( + state->dser_dev, active->pipe_id); + mutex_unlock(&serdes_lock__); dev_info(&state->client->dev, "%s(): released pipe %d (%d)\n", - __func__, sensor->pipe_id, release_ret); - sensor->pipe_id = PIPE_NOT_CONFIGURED; + __func__, active->pipe_id, release_ret); + active->pipe_id = PIPE_NOT_CONFIGURED; } - sensor->pipe_data_type1 = 0; - sensor->pipe_data_type2 = 0; - sensor->pipe_vc_id = 0; - } - mutex_unlock(&serdes_lock__); - } else + active->pipe_data_type1 = 0; + active->pipe_data_type2 = 0; + active->pipe_vc_id = 0; #endif - { - for (i = 0; i < ARRAY_SIZE(sensors); i++) { - ds5_config_cache_clear(sensors[i]); - sensors[i]->streaming = false; } } +#ifdef CONFIG_VIDEO_D4XX_SERDES + /* Invalidate peer instances of the same physical camera */ + for (i = 0; i < MAX_DEV_NUM; i++) { + struct ds5 *peer = serdes_inited[i]; + struct ds5_sensor *peer_active; + + if (!peer || peer == state) + continue; + if (peer->ser_dev != state->ser_dev) + continue; + + peer_active = ds5_get_active_sensor(peer); + if (peer_active) { + ds5_invalidate_sensor(peer, peer_active); + peer_active->streaming = false; + dev_info(&state->client->dev, + "%s(): invalidated peer %s\n", + __func__, dev_name(&peer->client->dev)); + } + } +#endif + /* 3. Send HW reset command */ ret = ds5_raw_write(state, DS5_HWMC_DATA, &cmd_hw_reset, sizeof(cmd_hw_reset)); if (ret < 0) { From 0e205e538015e8e99dffe4f0f537d7ff78b8528a Mon Sep 17 00:00:00 2001 From: Uzi Date: Sun, 1 Mar 2026 16:30:54 +0200 Subject: [PATCH 03/11] d4xx: register all stream instances in serdes_inited[] and fix EBUSY loop Two bugs found via post-HW-reset log analysis: Bug A: Only the first stream instance (Depth) per physical camera was registered in serdes_inited[]. RGB, Y8, and IMU instances returned -ENOTSUPP from ds5_board_setup() and were never registered. This made them invisible to peer iteration in ds5_hw_reset_with_recovery() steps 1 and 2, so their streaming flags and SERDES pipe state were never cleared after HW reset. Fix: Register ALL instances in serdes_inited[], even when they share the same serializer. The -ENOTSUPP still prevents duplicate SERDES setup but the instance is now registered before returning. Add a serdes_primary flag to struct ds5 so that only the primary instance tears down the serializer/deserializer on ds5_remove(). Bug B: After HW reset, all FW streams return to idle. When the VI capture engine detects frame loss and initiates error recovery, it calls ds5_mux_s_stream(off) to stop the stream. The pre-condition check at the top of ds5_mux_s_stream() sees the stream is already stopped (status 0x0000) and returns -EBUSY, which causes VI to fail the restart and enter an infinite retry loop. Fix: Treat stream-already-in-target-state as a no-op (return 0) instead of -EBUSY, allowing VI error recovery to proceed. Observed in log: d4xx 9-001b (RGB) stuck in endless 'stream 1 in 0 state already, return busy' loop after d4xx 9-001a (Depth) triggered HW reset. JIRA: RSDSO-21151, RSDSO-21257, RSDSO-21254 --- kernel/realsense/d4xx.c | 68 ++++++++++++++++++++++++++++++++++++----- 1 file changed, 61 insertions(+), 7 deletions(-) diff --git a/kernel/realsense/d4xx.c b/kernel/realsense/d4xx.c index 9ae10f02..d4215f95 100644 --- a/kernel/realsense/d4xx.c +++ b/kernel/realsense/d4xx.c @@ -484,6 +484,7 @@ struct ds5 { struct i2c_client *ser_i2c; struct i2c_client *dser_i2c; const struct dser_interface *dser_ops; + bool serdes_primary; /* true for the instance that ran SERDES setup */ #endif }; @@ -3856,12 +3857,31 @@ static int ds5_board_setup(struct ds5 *state) state->g_ctx.num_csi_lanes = value; state->g_ctx.s_dev = dev; + for (i = 0; i < MAX_DEV_NUM; i++) { + if (serdes_inited[i] && serdes_inited[i]->ser_dev == state->ser_dev) { + /* Same camera, different stream instance. + * Register in serdes_inited[] so peer iteration + * can find it, but return -ENOTSUPP to skip + * duplicate SERDES setup. + */ + int j; + + for (j = i + 1; j < MAX_DEV_NUM; j++) { + if (!serdes_inited[j]) { + serdes_inited[j] = state; + dev_info(dev, "registered peer instance in serdes_inited[%d]\n", j); + return -ENOTSUPP; + } + } + dev_err(dev, "cannot register more than %d D4XX instances\n", MAX_DEV_NUM); + return -ENOTSUPP; + } + } + /* First instance of a new camera */ for (i = 0; i < MAX_DEV_NUM; i++) { if (!serdes_inited[i]) { serdes_inited[i] = state; return 0; - } else if (serdes_inited[i]->ser_dev == state->ser_dev) { - return -ENOTSUPP; } } err = -EINVAL; @@ -4003,12 +4023,25 @@ static int ds5_board_setup(struct ds5 *state) state->g_ctx.num_csi_lanes = 2; state->g_ctx.s_dev = dev; + for (i = 0; i < MAX_DEV_NUM; i++) { + if (serdes_inited[i] && serdes_inited[i]->ser_dev == state->ser_dev) { + int j; + + for (j = i + 1; j < MAX_DEV_NUM; j++) { + if (!serdes_inited[j]) { + serdes_inited[j] = state; + dev_info(dev, "registered peer instance in serdes_inited[%d]\n", j); + return -ENOTSUPP; + } + } + dev_err(dev, "cannot register more than %d D4XX instances\n", MAX_DEV_NUM); + return -ENOTSUPP; + } + } for (i = 0; i < MAX_DEV_NUM; i++) { if (!serdes_inited[i]) { serdes_inited[i] = state; return 0; - } else if (serdes_inited[i]->ser_dev == state->ser_dev) { - return -ENOTSUPP; } } err = -EINVAL; @@ -4070,11 +4103,17 @@ static int ds5_serdes_setup(struct ds5 *state) ret = ds5_board_setup(state); if (ret) { - if (ret == -ENOTSUPP) + if (ret == -ENOTSUPP) { + /* Peer instance of already-initialized camera. + * Registered in serdes_inited[] but skips SERDES setup. + */ + state->serdes_primary = false; return 0; + } dev_err(&c->dev, "board setup failed\n"); return ret; } + state->serdes_primary = true; /* Pair sensor to serializer dev */ ret = max9295_sdev_pair(state->ser_dev, &state->g_ctx); @@ -4858,10 +4897,17 @@ static int ds5_mux_s_stream(struct v4l2_subdev *sd, int on) "stream %d in expected state, toggling to %d (status: 0x%04x) %dms\n", stream_id, on, status, jiffies_to_msecs(jiffies - ts)); } else { + /* After HW reset the FW reboots and all streams return to + * idle. If VI error recovery tries to stop a stream that + * is already stopped (or start one already started), treat + * it as a no-op so the upper layer can proceed with + * restart instead of getting stuck in an EBUSY loop. + */ dev_warn(&state->client->dev, - "stream %d in %d state already (status: 0x%04x) %dms, return busy\n", + "stream %d in %d state already (status: 0x%04x) %dms, treating as no-op\n", stream_id, on, status, jiffies_to_msecs(jiffies - ts)); - return -EBUSY; + sensor->streaming = on; + return 0; } restore_val = sensor->streaming; @@ -6303,6 +6349,14 @@ static int ds5_remove(struct i2c_client *c) for (i = 0; i < MAX_DEV_NUM; i++) { if (serdes_inited[i] && serdes_inited[i] == state) { serdes_inited[i] = NULL; + + /* Only the primary instance (the one that did SERDES + * setup) should tear down the serializer/deserializer. + * Peer instances just unregister from the array. + */ + if (!state->serdes_primary) + break; + mutex_lock(&serdes_lock__); ret = max9295_reset_control(state->ser_dev); From 5742c3aedc9afac7d3e2c53cc88f8ff9a70ba4bf Mon Sep 17 00:00:00 2001 From: Nikolai-L <36960789+Nikolai-L@users.noreply.github.com> Date: Sun, 1 Mar 2026 17:13:38 +0200 Subject: [PATCH 04/11] Bump driver version to 1.0.2.18 --- kernel/realsense/d4xx.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kernel/realsense/d4xx.c b/kernel/realsense/d4xx.c index d4215f95..103badfe 100644 --- a/kernel/realsense/d4xx.c +++ b/kernel/realsense/d4xx.c @@ -6450,4 +6450,4 @@ MODULE_AUTHOR("Guennadi Liakhovetski ,\n\ Shikun Ding ,\n\ Dmitry Perchanov "); MODULE_LICENSE("GPL v2"); -MODULE_VERSION("1.0.2.17"); +MODULE_VERSION("1.0.2.18"); From 4e8e62cbe500932fcc1aae59ded63414ad0d69ab Mon Sep 17 00:00:00 2001 From: Uzi Date: Mon, 2 Mar 2026 10:47:12 +0200 Subject: [PATCH 05/11] d4xx: fix D457 probe failure on 4th instance (IMU) after HW reset The 4th probe instance (IMU at 9-001d) fails to communicate with the D457 camera at the second I2C check in ds5_probe(), even though the first 3 instances succeed. Root cause analysis of the dmesg logs shows the first I2C check (with retries) passes, but the second check (single ds5_read, no outer retry) fails with -121 (EREMOTEIO / NAK). The D457 firmware has a brief I2C-unresponsive window during post-HW- reset initialization that the D401 does not exhibit. By the time the 4th instance reaches the second I2C check (~50ms after HW reset completion), the camera is momentarily unreachable. Three bugs fixed: 1. Second I2C check had no retry loop: replaced single ds5_read() with a retry loop (10 iterations, 50ms delay between retries) matching the pattern used by the first communication check. 2. Spurious pipe 0 release at probe: sensor pipe_ids default to 0 (kzalloc) instead of PIPE_NOT_CONFIGURED (-1). When hw_reset_with_recovery runs during probe (before ds5_sensor_init), step 2 sees pipe_id=0 >= 0 and releases pipe 0 from the MAX9296. Fix: initialize all four sensor pipe_ids to PIPE_NOT_CONFIGURED before calling hw_reset_with_recovery. 3. No stabilization delay after probe HW reset: added msleep(100) after hw_reset_with_recovery returns, giving the D457 firmware time to complete post-reset initialization before subsequent instances attempt I2C communication. Tested against logs: v1.0.2.16 (0/4 instances, old full SERDES re-init killed GMSL link), v1.0.2.18 pre-fix (3/4 instances, IMU fails at 2nd I2C check). --- docs/hw-reset-followup-plan.md | 81 ++++++++++++++++++++++++++++++++++ kernel/realsense/d4xx.c | 30 ++++++++++++- 2 files changed, 110 insertions(+), 1 deletion(-) create mode 100644 docs/hw-reset-followup-plan.md diff --git a/docs/hw-reset-followup-plan.md b/docs/hw-reset-followup-plan.md new file mode 100644 index 00000000..7a696fea --- /dev/null +++ b/docs/hw-reset-followup-plan.md @@ -0,0 +1,81 @@ +# HW Reset Follow-up Plan: Circuit-Breaker & Phase 1 Dual-Camera Safety + +**Date:** 2026-03-02 +**Branch:** `fix/hw-reset-gmsl-recovery` +**Commits:** ecbfa82 → 97b91b9 → 0e205e5 +**JIRA:** RSDSO-21151, RSDSO-21257, RSDSO-21254 + +## Summary + +Two follow-up items from the log analysis. **Item 1** (circuit-breaker) adds rate-limiting and consecutive-failure tracking to `ds5_hw_reset_with_recovery()` to break the 35-reset infinite loop seen in `reset_issue_dmesg.txt`. **Item 2** (Phase 1 dual-camera impact) is largely resolved by analysis — `max9295_init_settings()` only writes serializer-local registers and cannot disrupt siblings — but a lightweight safety check should be added. + +--- + +## Item 1: Reset Circuit-Breaker (High Priority) + +The `reset_issue_dmesg.txt` log shows 35 consecutive userspace-triggered HW resets (~30s apart) via `HWMC_RW` opcode 0x20, each triggering full SERDES recovery. After ~3 resets, I2C errors (err -121) appear and the device never recovers — yet the driver keeps accepting reset commands indefinitely. + +### Steps + +1. **Add tracking fields to `struct ds5`** (`kernel/realsense/d4xx.c`): + - `int consecutive_reset_failures` — incremented when `ds5_hw_reset_with_recovery()` returns an error, reset to 0 on success + - `unsigned long last_reset_jiffies` — timestamp of the last reset attempt + - `int total_resets` — lifetime counter for diagnostics (never reset) + +2. **Add circuit-breaker constants** near the existing `DS5_HW_RESET_*` defines: + - `DS5_HW_RESET_MAX_CONSECUTIVE_FAILURES` = 3 — after 3 consecutive failed resets, refuse further resets + - `DS5_HW_RESET_COOLDOWN_MS` = 5000 — minimum interval between resets (5 seconds) + - `DS5_HW_RESET_BREAKER_RESET_MS` = 60000 — if no reset for 60s, clear the failure counter (auto-recovery) + +3. **Add gating logic at top of `ds5_hw_reset_with_recovery()`**: + - Check `consecutive_reset_failures >= MAX_CONSECUTIVE_FAILURES`: + - If the breaker timeout (`DS5_HW_RESET_BREAKER_RESET_MS`) has elapsed since `last_reset_jiffies`, auto-clear the counter and allow the reset (self-healing) + - Otherwise, log `dev_err` "circuit breaker tripped — %d consecutive failures, refusing HW reset. Will auto-reset after %d seconds of inactivity" and return `-EBUSY` + - Check cooldown: if `jiffies - last_reset_jiffies < msecs_to_jiffies(COOLDOWN_MS)`, log `dev_warn` "HW reset throttled — last reset was %d ms ago" and return `-EAGAIN` + +4. **Update success/failure tracking at bottom of function**: + - On success (return 0): set `state->consecutive_reset_failures = 0`, update `state->last_reset_jiffies = jiffies`, increment `state->total_resets` + - On failure: increment `state->consecutive_reset_failures`, update `state->last_reset_jiffies = jiffies`, increment `state->total_resets` + +5. **Expose diagnostic counters via existing V4L2 log** — add the counters to the final `dev_info` message at the end of `ds5_hw_reset_with_recovery()` so they appear in dmesg. + +6. **Skip circuit-breaker for probe path** — the `ds5_probe()` call should bypass the circuit-breaker since it's a one-time initialization, not a userspace retry loop. Add a `bool force` parameter to `ds5_hw_reset_with_recovery()`, or handle it by checking `last_reset_jiffies == 0` (uninitialized). + +--- + +## Item 2: Phase 1 Dual-Camera Safety (Low Priority) + +### Analysis Conclusion + +The D401 dual-camera log errors (bus 12 I2C failures 12ms after SERDES re-init) occurred with the **old driver code** that did full SERDES re-init including deserializer `reset_oneshot`. Our new Phase 1 calls only `max9295_init_settings()`, which writes exclusively to serializer-local registers (`PIPE_EN`=0x2, `START_PIPE`=0x311, `MIPI_RX1`=0x331, `CSI_PORT_SEL`). This **cannot** disrupt sibling cameras on a different serializer. + +### Remaining Risk + +Theoretical — if the GMSL link is unstable, a serializer register write could briefly glitch the shared GMSL bus during the I2C transaction. This is extremely unlikely with MAX9295/MAX9296 but worth monitoring. + +### Steps + +7. **Add post-Phase-1 sibling health check** in `ds5_hw_reset_serdes_recovery()` — after the Phase 1 success path (`return 0`), before returning: + - Iterate `serdes_inited[]` for true siblings (same `dser_dev`, different `ser_dev`) + - For each streaming sibling, do a quick I2C read (`ds5_read(sib, DS5_FW_VERSION, &tmp)`) + - If any fail: log `dev_warn` "Phase 1 serializer re-init may have disrupted sibling %s" (informational only — do NOT fail the reset) + - This is a **diagnostic-only** check that gives us data to decide if future mitigation is needed + +8. **Add a brief stabilization delay** — increase the post-Phase-1 `msleep(100)` to `msleep(150)` to give the GMSL link a bit more time to re-lock after serializer reconfiguration, before verifying. This is conservative and costs only 50ms. + +--- + +## Verification Plan + +- **Circuit-breaker**: Trigger 5+ rapid HW resets on a camera with a broken GMSL link. Verify that after 3 failures, the driver logs "circuit breaker tripped" and returns `-EBUSY`. Wait 60s, verify reset is accepted again. +- **Cooldown**: Send two HW resets <5s apart. Verify the second one returns `-EAGAIN` with a throttle warning. +- **Probe bypass**: Verify `modprobe d4xx` still performs initial HW reset successfully without tripping the circuit-breaker. +- **Phase 1 safety**: On a dual-camera Orin setup, HW-reset camera A while camera B is streaming. Check dmesg for "may have disrupted sibling" warning. Confirm camera B's stream is unaffected. +- **Regression**: Run `python3 run_ci.py` from `test/` on a D457 to verify no existing tests break. + +## Design Decisions + +- **Circuit-breaker is per-instance, not per-camera**: Each `struct ds5` has its own counter. Since userspace sends HW reset to one instance (typically Depth at `9-001a`), this naturally tracks per-camera. Peer instances aren't reset independently. +- **Return -EBUSY for tripped breaker, -EAGAIN for cooldown**: Distinct error codes let userspace distinguish "permanently failed" from "try again later". +- **Auto-clear after 60s inactivity**: Avoids permanent lockout. If the camera recovers externally (e.g., power cycle), the driver will accept resets again. +- **Phase 1 dual-camera is diagnostic only**: No blocking behavior added — just logging. If field data shows disruption, we can add a sibling-streaming gate in a future commit. diff --git a/kernel/realsense/d4xx.c b/kernel/realsense/d4xx.c index 103badfe..b095fcf3 100644 --- a/kernel/realsense/d4xx.c +++ b/kernel/realsense/d4xx.c @@ -6270,15 +6270,43 @@ static int ds5_probe(struct i2c_client *c, const struct i2c_device_id *id) if (atomic_cmpxchg(&ds5_probe_reset_once, 0, 1) == 0) { dev_info(&c->dev, "%s(): first probe instance, running HW reset recovery\n", __func__); + + /* Initialize sensor pipe_ids to PIPE_NOT_CONFIGURED before + * hw_reset_with_recovery, otherwise the kzalloc default of 0 + * causes a spurious release_pipe(0) in step 2. + */ + state->depth.sensor.pipe_id = PIPE_NOT_CONFIGURED; + state->ir.sensor.pipe_id = PIPE_NOT_CONFIGURED; + state->rgb.sensor.pipe_id = PIPE_NOT_CONFIGURED; + state->imu.sensor.pipe_id = PIPE_NOT_CONFIGURED; + ret = ds5_hw_reset_with_recovery(state); if (ret < 0) { dev_err(&c->dev, "%s(): probe HW reset recovery failed: %d\n", __func__, ret); goto e_chardev; } + + /* Allow the camera firmware to fully stabilize after HW reset + * before subsequent driver instances attempt I2C communication. + * Without this delay, the D457's FW may be briefly unresponsive + * during a post-reset initialization phase, causing later probe + * instances (especially the 4th/IMU) to fail I2C checks. + */ + msleep(100); } - ret = ds5_read(state, 0x5020, &rec_state); + /* Verify communication with retries and delay. + * After HW reset (done by the first probe instance), the camera + * firmware may still be initializing and briefly NAK I2C requests. + * Use the same retry pattern as the initial communication check. + */ + retry = 10; + do { + ret = ds5_read(state, 0x5020, &rec_state); + if (ret < 0) + msleep(50); + } while (retry-- && ret < 0); if (ret < 0) { dev_err(&c->dev, "%s(): cannot communicate with D4XX: %d\n", __func__, ret); From 5b5a43c00f04f3141144f8ec17067590d6dba69f Mon Sep 17 00:00:00 2001 From: Nikolai-L <36960789+Nikolai-L@users.noreply.github.com> Date: Mon, 2 Mar 2026 13:45:19 +0200 Subject: [PATCH 06/11] Bump driver version to 1.0.2.19 --- kernel/realsense/d4xx.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kernel/realsense/d4xx.c b/kernel/realsense/d4xx.c index b095fcf3..125b83c3 100644 --- a/kernel/realsense/d4xx.c +++ b/kernel/realsense/d4xx.c @@ -6478,4 +6478,4 @@ MODULE_AUTHOR("Guennadi Liakhovetski ,\n\ Shikun Ding ,\n\ Dmitry Perchanov "); MODULE_LICENSE("GPL v2"); -MODULE_VERSION("1.0.2.18"); +MODULE_VERSION("1.0.2.19"); From 06f04955bb177dd2cf66a23bd5bd83edca483cb7 Mon Sep 17 00:00:00 2001 From: Nikolai-L <36960789+Nikolai-L@users.noreply.github.com> Date: Mon, 2 Mar 2026 15:49:29 +0200 Subject: [PATCH 07/11] Copilot update Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- kernel/realsense/d4xx.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/kernel/realsense/d4xx.c b/kernel/realsense/d4xx.c index 125b83c3..9e47eeaf 100644 --- a/kernel/realsense/d4xx.c +++ b/kernel/realsense/d4xx.c @@ -2292,9 +2292,9 @@ static int ds5_set_calibration_data(struct ds5 *state, * ds5_hw_reset_serdes_recovery - Tiered SERDES recovery after HW reset. * * Phase 1: Re-init serializer only (per-camera, non-disruptive). - * Phase 2: If I2C still fails, check whether all sibling cameras on the - * same deserializer are also dead. Only then perform a full - * deserializer reset_oneshot (chip-wide, disruptive). + * Phase 2: If I2C still fails, perform a full deserializer reset_oneshot + * (chip-wide, disruptive) only when no sibling camera on the same + * deserializer is currently reachable over I2C and streaming. * * Returns 0 on success, negative errno on failure. */ From 2726ce1dd4b9e03c64b16cd00e670187cb486b5a Mon Sep 17 00:00:00 2001 From: Nikolai-L <36960789+Nikolai-L@users.noreply.github.com> Date: Mon, 2 Mar 2026 15:50:39 +0200 Subject: [PATCH 08/11] Copilot update Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- kernel/realsense/d4xx.c | 1 - 1 file changed, 1 deletion(-) diff --git a/kernel/realsense/d4xx.c b/kernel/realsense/d4xx.c index 9e47eeaf..8d5310e3 100644 --- a/kernel/realsense/d4xx.c +++ b/kernel/realsense/d4xx.c @@ -2484,7 +2484,6 @@ static int ds5_hw_reset_with_recovery(struct ds5 *state) int retry; int __maybe_unused i; u16 status = 0; - bool device_went_down = false; dev_info(&state->client->dev, "%s(): Initiating HW reset with recovery\n", __func__); From 9fce732fb02ef263c7d9adcb97f054ed6fba6ed5 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Mon, 2 Mar 2026 13:52:02 +0000 Subject: [PATCH 09/11] Initial plan From bd179eb3b098407f29ec2ac014e8d7b3b15b3564 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Mon, 2 Mar 2026 13:53:51 +0000 Subject: [PATCH 10/11] Fix peer slot search to scan entire serdes_inited[] array Co-authored-by: Nikolai-L <36960789+Nikolai-L@users.noreply.github.com> --- _codeql_detected_source_root | 1 + kernel/realsense/d4xx.c | 8 ++++---- 2 files changed, 5 insertions(+), 4 deletions(-) create mode 120000 _codeql_detected_source_root diff --git a/_codeql_detected_source_root b/_codeql_detected_source_root new file mode 120000 index 00000000..f586edda --- /dev/null +++ b/_codeql_detected_source_root @@ -0,0 +1 @@ +./nvidia-oot \ No newline at end of file diff --git a/kernel/realsense/d4xx.c b/kernel/realsense/d4xx.c index 8d5310e3..63951de3 100644 --- a/kernel/realsense/d4xx.c +++ b/kernel/realsense/d4xx.c @@ -3865,8 +3865,8 @@ static int ds5_board_setup(struct ds5 *state) */ int j; - for (j = i + 1; j < MAX_DEV_NUM; j++) { - if (!serdes_inited[j]) { + for (j = 0; j < MAX_DEV_NUM; j++) { + if (j != i && !serdes_inited[j]) { serdes_inited[j] = state; dev_info(dev, "registered peer instance in serdes_inited[%d]\n", j); return -ENOTSUPP; @@ -4026,8 +4026,8 @@ static int ds5_board_setup(struct ds5 *state) if (serdes_inited[i] && serdes_inited[i]->ser_dev == state->ser_dev) { int j; - for (j = i + 1; j < MAX_DEV_NUM; j++) { - if (!serdes_inited[j]) { + for (j = 0; j < MAX_DEV_NUM; j++) { + if (j != i && !serdes_inited[j]) { serdes_inited[j] = state; dev_info(dev, "registered peer instance in serdes_inited[%d]\n", j); return -ENOTSUPP; From d8002b625b9b6b4202bb7fedebe58de64815abe1 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Sun, 8 Mar 2026 11:06:39 +0000 Subject: [PATCH 11/11] Rebase: fix peer slot search to scan entire serdes_inited[] array Co-authored-by: Nikolai-L <36960789+Nikolai-L@users.noreply.github.com> --- kernel/realsense/d4xx.c | 725 ++++++++++++++++++++++++++++++++++------ 1 file changed, 631 insertions(+), 94 deletions(-) diff --git a/kernel/realsense/d4xx.c b/kernel/realsense/d4xx.c index 63951de3..476b4696 100644 --- a/kernel/realsense/d4xx.c +++ b/kernel/realsense/d4xx.c @@ -75,6 +75,15 @@ struct dser_interface { #define GMSL_CSI_DT_EMBED 0x12 #endif +/* IMU pipe defaults — used by ds5_configure() and post-reset pipe restoration. + * The D457 FW configures pipes 0-2 (Depth/RGB/IR) but NOT pipe 3 (IMU). + * The driver must configure pipe 3 explicitly with these values. + */ +#define DS5_IMU_PIPE_ID 3 +#define DS5_IMU_VC_ID 3 +#define DS5_IMU_DT1 GMSL_CSI_DT_YUV422_8 +#define DS5_IMU_DT2 GMSL_CSI_DT_EMBED + //#define DS5_DRIVER_NAME "DS5 RealSense camera driver" #define DS5_DRIVER_NAME "d4xx" #define DS5_DRIVER_NAME_AWG "d4xx-awg" @@ -497,6 +506,20 @@ struct ds5_counters { static atomic_t ds5_reset_gen = ATOMIC_INIT(0); static atomic_t ds5_probe_reset_once = ATOMIC_INIT(0); +/* Timestamp (jiffies) of last completed HW reset. + * Used to enforce DS5_HW_RESET_COOLDOWN_MS between consecutive resets + * and prevent GMSL link degradation from rapid reset cycles. + */ +static unsigned long ds5_last_reset_jiffies; + +/* Cached device type from HW reset Step 8. + * During probe the first instance resets the camera, causing DS5_DEVICE_TYPE + * to temporarily return 0. Step 8 polls until the register is valid and + * stores the result here so that all four probe instances (and any post-reset + * code path) can use the confirmed value even if the register read returns 0. + */ +static u16 ds5_cached_device_type; + #ifdef CONFIG_VIDEO_D4XX_SERDES static DEFINE_MUTEX(serdes_lock__); @@ -517,20 +540,31 @@ static struct dser_reset_gen dser_reset_gens[MAX_DSER_NUM]; static atomic_t *ds5_get_reset_gen(struct ds5 *state) { int i; + int free_idx = -1; + atomic_t *gen = &ds5_reset_gen; if (state->dser_dev) { + mutex_lock(&serdes_lock__); for (i = 0; i < MAX_DSER_NUM; i++) { - if (dser_reset_gens[i].dser_dev == state->dser_dev) - return &dser_reset_gens[i].gen; - if (!dser_reset_gens[i].dser_dev) { - dser_reset_gens[i].dser_dev = state->dser_dev; - atomic_set(&dser_reset_gens[i].gen, 0); - return &dser_reset_gens[i].gen; + if (dser_reset_gens[i].dser_dev == state->dser_dev) { + gen = &dser_reset_gens[i].gen; + goto out_unlock; } + if (!dser_reset_gens[i].dser_dev && free_idx < 0) + free_idx = i; } + + if (free_idx >= 0) { + dser_reset_gens[free_idx].dser_dev = state->dser_dev; + atomic_set(&dser_reset_gens[free_idx].gen, 0); + gen = &dser_reset_gens[free_idx].gen; + } +out_unlock: + mutex_unlock(&serdes_lock__); } + /* Fallback: table full or no deserializer */ - return &ds5_reset_gen; + return gen; } #else static inline atomic_t *ds5_get_reset_gen(struct ds5 *state) @@ -1731,14 +1765,13 @@ static struct ds5_sensor *ds5_get_active_sensor(struct ds5 *state) static void ds5_invalidate_sensor(struct ds5 *state, struct ds5_sensor *sensor) { ds5_config_cache_clear(sensor); -#ifdef CONFIG_VIDEO_D4XX_SERDES - if (sensor->pipe_id >= 0 && state->dser_dev) { - mutex_lock(&serdes_lock__); - state->dser_ops->release_pipe(state->dser_dev, sensor->pipe_id); - mutex_unlock(&serdes_lock__); - } -#endif - sensor->pipe_id = PIPE_NOT_CONFIGURED; + /* Do NOT release SERDES pipes or clear pipe_id here. + * Preserve the existing pipe_id so that ds5_configure() can + * release-then-reallocate the pipe at stream-start time, when + * the camera FW has finished its post-boot serializer init. + * Clearing pipe_data_type forces ds5_configure() to enter the + * re-allocation path (data_type mismatch triggers pipe setup). + */ sensor->pipe_data_type1 = 0; sensor->pipe_data_type2 = 0; sensor->pipe_vc_id = 0; @@ -1805,7 +1838,7 @@ static int ds5_configure(struct ds5 *state) fps_addr = DS5_IMU_FPS; width_addr = DS5_IMU_RES_WIDTH; height_addr = DS5_IMU_RES_HEIGHT; - vc_id = 3; + vc_id = DS5_IMU_VC_ID; } else { return -EINVAL; } @@ -2166,7 +2199,12 @@ static int ds5_get_hwmc_status(struct ds5 *state) if (retries != 100) msleep_range(1); ret = ds5_read(state, DS5_HWMC_STATUS, &status); - } while (!ret && retries-- && status == DS5_HWMC_STATUS_WIP); + if (ret) { + dev_dbg(&state->client->dev, + "%s(): I2C read failed (%d), retries left: %d\n", + __func__, ret, retries); + } + } while (retries-- && (ret || status == DS5_HWMC_STATUS_WIP)); dev_dbg(&state->client->dev, "%s(): ret: 0x%x, status: 0x%x\n", __func__, ret, status); @@ -2279,6 +2317,22 @@ static int ds5_set_calibration_data(struct ds5 *state, #define DS5_HW_RESET_TIMEOUT_MS 10000 #define DS5_HW_RESET_MAX_RETRIES (DS5_HW_RESET_TIMEOUT_MS / DS5_HW_RESET_POLL_INTERVAL_MS) +/* Post-reset I2C stability verification (Step 10). + * After FW reports ready and HWMC passes, verify the I2C link is + * *sustained* — some camera SKUs (D401) have a secondary FW init phase + * that briefly drops I2C ~65ms after the initial checks pass. + */ +#define DS5_HW_RESET_STABILITY_READS 3 /* consecutive successful reads */ +#define DS5_HW_RESET_STABILITY_INTERVAL_MS 200 /* between each check */ +#define DS5_HW_RESET_STABILITY_TIMEOUT_MS 3000 /* max wait for stable link */ + +/* Minimum interval between consecutive HW resets (ms). + * Rapid back-to-back resets degrade the GMSL link because each + * serializer re-init (Phase 1) conflicts with the camera FW's own + * I2C bus reconfiguration after reboot. + */ +#define DS5_HW_RESET_COOLDOWN_MS 2000 + /* * Register 0x5020 status values (from firmware): * - 0xDEAD: Device in normal UVC mode (ready) - ICT returns error for unhandled cmd @@ -2290,48 +2344,108 @@ static int ds5_set_calibration_data(struct ds5 *state, /* * ds5_hw_reset_serdes_recovery - Tiered SERDES recovery after HW reset. + * @state: Driver state + * @force_phase2: When true, skip Phase 1 and go straight to Phase 2 + * (full deserializer reset). Used by Step 10 when the + * I2C link is unstable despite Phase 1 having passed. * * Phase 1: Re-init serializer only (per-camera, non-disruptive). - * Phase 2: If I2C still fails, perform a full deserializer reset_oneshot - * (chip-wide, disruptive) only when no sibling camera on the same - * deserializer is currently reachable over I2C and streaming. + * Phase 2: If I2C still fails (or force_phase2), perform a full deserializer + * reset_oneshot (chip-wide, disruptive) only when no sibling camera + * on the same deserializer is currently reachable over I2C and + * streaming. * * Returns 0 on success, negative errno on failure. */ #ifdef CONFIG_VIDEO_D4XX_SERDES -static int ds5_hw_reset_serdes_recovery(struct ds5 *state) +static int ds5_hw_reset_serdes_recovery(struct ds5 *state, bool force_phase2) { int ret; int i; u16 tmp = 0; bool sibling_alive = false; + struct ds5 *primary = state; /* the instance that ran SERDES setup */ if (!state->ser_dev || !state->dser_dev) return 0; - /* Phase 1 — serializer-only re-init (per-camera, safe) */ - dev_info(&state->client->dev, - "%s(): Phase 1 - re-initializing serializer\n", __func__); - ret = max9295_init_settings(state->ser_dev); - if (ret < 0) - dev_warn(&state->client->dev, - "%s(): serializer init_settings failed: %d\n", - __func__, ret); + /* The deserializer/serializer API calls (setup_link, setup_control) + * require the device that was originally paired via sdev_pair/ + * sdev_register during probe. Only the primary instance (the first + * probe of each physical camera) performs that pairing. If we are + * called from a non-primary instance (e.g. IMU = 9-001d) we must + * find and use the primary's device, otherwise the deserializer + * returns "no sdev found" (-EINVAL). + */ + if (!state->serdes_primary) { + for (i = 0; i < MAX_DEV_NUM; i++) { + struct ds5 *p = serdes_inited[i]; - msleep(100); + if (p && p->serdes_primary && + p->ser_dev == state->ser_dev) { + primary = p; + dev_dbg(&state->client->dev, + "%s(): using primary instance %s for SERDES API calls\n", + __func__, dev_name(&primary->client->dev)); + break; + } + } + } - /* Verify I2C link to camera is working */ - ret = ds5_read(state, DS5_FW_VERSION, &tmp); - if (ret == 0) { + if (!force_phase2) { + /* Phase 1 — serializer-only re-init (per-camera, safe) */ dev_info(&state->client->dev, - "%s(): Phase 1 succeeded, I2C link OK\n", __func__); - return 0; - } + "%s(): Phase 1 - re-initializing serializer\n", __func__); + ret = max9295_init_settings(state->ser_dev); + if (ret < 0) + dev_warn(&state->client->dev, + "%s(): serializer init_settings failed: %d\n", + __func__, ret); - /* Phase 2 — I2C still broken; consider full deserializer reset */ - dev_warn(&state->client->dev, - "%s(): Phase 1 failed (I2C err %d), checking siblings before deser reset\n", - __func__, ret); + msleep(100); + + /* Verify I2C link to camera is working AND stable. + * A single successful read is not enough — the D457 FW can + * momentarily restore the GMSL link during its late-boot + * serializer reconfiguration, only to kill it again ~100 ms + * later. Do multiple reads with delays to catch oscillation. + */ + ret = ds5_read(state, DS5_FW_VERSION, &tmp); + if (ret == 0) { + int k; + bool stable = true; + + for (k = 0; k < 3; k++) { + msleep(200); + ret = ds5_read(state, DS5_FW_VERSION, &tmp); + if (ret < 0) { + dev_warn(&state->client->dev, + "%s(): Phase 1 stability check %d/3 failed (err %d)\n", + __func__, k + 1, ret); + stable = false; + break; + } + } + if (stable) { + dev_info(&state->client->dev, + "%s(): Phase 1 succeeded, I2C link stable (3 consecutive reads OK)\n", + __func__); + return 0; + } + dev_warn(&state->client->dev, + "%s(): Phase 1 link unstable, escalating to Phase 2\n", + __func__); + } else { + /* Phase 2 — I2C still broken; consider full deserializer reset */ + dev_warn(&state->client->dev, + "%s(): Phase 1 failed (I2C err %d), checking siblings before deser reset\n", + __func__, ret); + } + } else { + dev_info(&state->client->dev, + "%s(): Phase 2 forced (I2C link unstable after Phase 1), checking siblings\n", + __func__); + } /* * In the D4XX architecture each physical camera has 4 driver instances @@ -2389,24 +2503,63 @@ static int ds5_hw_reset_serdes_recovery(struct ds5 *state) dev_info(&state->client->dev, "%s(): Phase 2 - performing full deserializer reset\n", __func__); + /* Use reset_oneshot (software GMSL link reset) rather than a full + * GPIO power cycle. reset_oneshot preserves the MAX9296's I2C + * pass-through table and per-pipe config while re-initializing the + * GMSL link channels. Testing shows the GMSL link reliably + * re-establishes after reset_oneshot (typically 2-4 s), whereas a + * full power_off/power_on GPIO reset destroys all deserializer + * state and the link does NOT come back. + */ mutex_lock(&serdes_lock__); state->dser_ops->reset_oneshot(state->dser_dev); msleep(300); - /* Re-establish link & control for this camera's serializer */ - ret = state->dser_ops->setup_link(state->dser_dev, &state->client->dev); + /* setup_link writes to deserializer registers via host I2C (always + * reachable, no GMSL needed). This configures the link mode so + * GMSL auto-negotiation can proceed. + */ + ret = state->dser_ops->setup_link(state->dser_dev, &primary->client->dev); if (ret) dev_warn(&state->client->dev, "%s(): deser setup_link failed: %d\n", __func__, ret); + mutex_unlock(&serdes_lock__); - msleep(100); + /* The GMSL link may take several seconds to fully re-establish + * after reset_oneshot. In testing, the D457 camera's serializer + * becomes reachable 2-4 s after the reset. Poll for I2C recovery + * before attempting serializer/deserializer control setup. + */ + for (i = 0; i < 8; i++) { + msleep(500); + ret = ds5_read(state, DS5_FW_VERSION, &tmp); + if (ret == 0) { + dev_info(&state->client->dev, + "%s(): Phase 2: I2C link up after %d ms\n", + __func__, (i + 1) * 500 + 300); + break; + } + } + + if (ret < 0) { + dev_err(&state->client->dev, + "%s(): Phase 2 failed, I2C unrecoverable after %d ms\n", + __func__, 8 * 500 + 300); + return ret; + } + + /* GMSL link is up — now configure serializer and deserializer. + * Use the primary instance's device for deserializer API calls + * (the one paired via sdev_pair/sdev_register during probe). + */ + mutex_lock(&serdes_lock__); ret = max9295_setup_control(state->ser_dev); if (ret) dev_warn(&state->client->dev, "%s(): ser setup_control failed: %d\n", __func__, ret); - ret = state->dser_ops->setup_control(state->dser_dev, &state->client->dev); + ret = state->dser_ops->setup_control(state->dser_dev, &primary->client->dev); if (ret) dev_warn(&state->client->dev, "%s(): deser setup_control failed: %d\n", __func__, ret); @@ -2423,13 +2576,25 @@ static int ds5_hw_reset_serdes_recovery(struct ds5 *state) mutex_unlock(&serdes_lock__); - /* Verify I2C link after full recovery */ - ret = ds5_read(state, DS5_FW_VERSION, &tmp); - if (ret < 0) { - dev_err(&state->client->dev, - "%s(): Phase 2 failed, I2C still broken: %d\n", - __func__, ret); - return ret; + /* Final stability check — ensure the link is reliably up */ + { + int k; + bool stable = true; + + for (k = 0; k < 3; k++) { + msleep(200); + ret = ds5_read(state, DS5_FW_VERSION, &tmp); + if (ret < 0) { + stable = false; + break; + } + } + if (!stable) { + dev_err(&state->client->dev, + "%s(): Phase 2 setup done but I2C unstable: %d\n", + __func__, ret); + return ret; + } } dev_info(&state->client->dev, @@ -2484,10 +2649,35 @@ static int ds5_hw_reset_with_recovery(struct ds5 *state) int retry; int __maybe_unused i; u16 status = 0; +#ifdef CONFIG_VIDEO_D4XX_SERDES + bool serdes_recovery_ran = false; +#endif dev_info(&state->client->dev, "%s(): Initiating HW reset with recovery\n", __func__); + /* 0. Reset cooldown — prevent rapid consecutive resets. + * Repeated HW reset + Phase-1 serializer re-init without letting + * the camera FW finish its post-boot I2C bus reconfiguration + * progressively degrades the GMSL link until even SERDES pipe + * setup fails. Enforce a minimum interval between resets. + * Skip check on the very first reset (ds5_last_reset_jiffies == 0). + */ + if (ds5_last_reset_jiffies) { + unsigned long elapsed = jiffies - ds5_last_reset_jiffies; + unsigned long cooldown = msecs_to_jiffies(DS5_HW_RESET_COOLDOWN_MS); + + if (time_before(jiffies, ds5_last_reset_jiffies + cooldown)) { + unsigned long remaining = cooldown - elapsed; + + dev_info(&state->client->dev, + "%s(): Reset cooldown — last reset %u ms ago, waiting %u ms\n", + __func__, jiffies_to_msecs(elapsed), + jiffies_to_msecs(remaining)); + msleep(jiffies_to_msecs(remaining)); + } + } + /* 1. Stop active streams on the device before reset. * This ensures FW and SERDES are in a clean state. * @@ -2538,11 +2728,18 @@ static int ds5_hw_reset_with_recovery(struct ds5 *state) } #endif - /* 2. Invalidate sensor state and release SERDES pipes. + /* 2. Invalidate sensor state (defer SERDES pipe release). * After HW reset the device loses all configuration, so driver * state must be brought in sync. Clear streaming flags so that * ds5_mux_s_stream() won't silently skip the next stream-start. * Covers this instance AND all peer instances of the same camera. + * + * Do NOT release SERDES pipes here — the D457 FW is still + * reconfiguring the MAX9295 serializer after reporting 0xDEAD. + * Releasing + re-allocating pipes now would race with FW init. + * Instead, clear pipe_data_type to force ds5_configure() to + * release-then-reallocate at stream-start time, when the FW + * has long finished its init (matching v1.0.1.33 behavior). */ { struct ds5_sensor *active = ds5_get_active_sensor(state); @@ -2551,18 +2748,6 @@ static int ds5_hw_reset_with_recovery(struct ds5 *state) ds5_config_cache_clear(active); active->streaming = false; #ifdef CONFIG_VIDEO_D4XX_SERDES - if (active->pipe_id >= 0 && state->dser_dev) { - int release_ret; - - mutex_lock(&serdes_lock__); - release_ret = state->dser_ops->release_pipe( - state->dser_dev, active->pipe_id); - mutex_unlock(&serdes_lock__); - dev_info(&state->client->dev, - "%s(): released pipe %d (%d)\n", - __func__, active->pipe_id, release_ret); - active->pipe_id = PIPE_NOT_CONFIGURED; - } active->pipe_data_type1 = 0; active->pipe_data_type2 = 0; active->pipe_vc_id = 0; @@ -2622,7 +2807,6 @@ static int ds5_hw_reset_with_recovery(struct ds5 *state) if (ret < 0) { /* I2C failed - device is resetting (expected during reset) */ - device_went_down = true; dev_dbg(&state->client->dev, "%s(): Device not responding (resetting), retry %d\n", __func__, retry); @@ -2664,15 +2848,47 @@ static int ds5_hw_reset_with_recovery(struct ds5 *state) } #ifdef CONFIG_VIDEO_D4XX_SERDES - /* 6. Tiered SERDES recovery: - * Phase 1: serializer-only re-init (per-camera, non-disruptive). - * Phase 2: full deserializer reset only if ALL siblings are also dead. + /* 6. SERDES recovery (conditional). + * Step 5 polled the device over I2C until 0xDEAD was returned, + * which means the GMSL link is up. Probe once more: if the + * link is still alive, the GMSL link recovered naturally — no + * full tiered SERDES recovery needed (the common case for D457). + * + * Do NOT call max9295_init_settings() here. That function writes + * global serializer registers (0x02, 0x308, 0x311, 0x331) that + * disrupt the active GMSL link. Per-pipe reconfiguration is + * handled by callers: + * - First-probe: ds5_probe() configures pipe 3 (IMU) after + * this function returns, before IMU peer probes. + * - HWMC reset: ds5_invalidate_sensor() clears pipe state for + * all peers; ds5_configure() re-runs ds5_setup_pipeline() + * at the next STREAMON for each stream. + * + * Only run full tiered recovery (including init_settings + + * stability checks + Phase 2 escalation) when the I2C link + * is actually broken. */ - ret = ds5_hw_reset_serdes_recovery(state); - if (ret < 0) { - dev_err(&state->client->dev, - "%s(): SERDES recovery failed: %d\n", __func__, ret); - return ret; + { + u16 link_probe = 0; + + ret = ds5_read(state, DS5_FW_VERSION, &link_probe); + if (ret < 0 || link_probe == 0) { + dev_info(&state->client->dev, + "%s(): I2C link dead after Step 5 (ret=%d, val=0x%x), running full SERDES recovery\n", + __func__, ret, link_probe); + ret = ds5_hw_reset_serdes_recovery(state, false); + if (ret < 0) { + dev_err(&state->client->dev, + "%s(): SERDES recovery failed: %d\n", + __func__, ret); + return ret; + } + serdes_recovery_ran = true; + } else { + dev_info(&state->client->dev, + "%s(): GMSL link recovered naturally (FW ver 0x%04x), no SERDES intervention needed\n", + __func__, link_probe); + } } #endif @@ -2705,10 +2921,11 @@ static int ds5_hw_reset_with_recovery(struct ds5 *state) for (retry = 0; retry < DS5_HW_RESET_MAX_RETRIES; retry++) { ret = ds5_read(state, DS5_DEVICE_TYPE, &dev_type); if (ret == 0 && dev_type != 0) { - dev_dbg(&state->client->dev, + dev_info(&state->client->dev, "%s(): Device type 0x%x ready after %d ms\n", __func__, dev_type, (retry + 1) * DS5_HW_RESET_POLL_INTERVAL_MS); + ds5_cached_device_type = dev_type; break; } msleep(DS5_HW_RESET_POLL_INTERVAL_MS); @@ -2720,12 +2937,200 @@ static int ds5_hw_reset_with_recovery(struct ds5 *state) __func__, dev_type, DS5_HW_RESET_TIMEOUT_MS); } + /* 9. Verify HWMC subsystem is ready to accept commands. + * After HW reset the FW reports 0xDEAD and populates device type + * before the HWM command processor is fully initialized. If + * userspace queries GVD/HWMC immediately after we return, the + * I2C reads NAK (EREMOTEIO) or return stale status (WIP/ERR), + * which can crash RS Viewer. Send a lightweight GVD command and + * poll HWMC_STATUS until it completes successfully. + */ + { + struct hwm_cmd hwmc_probe; + u16 hwmc_status = DS5_HWMC_STATUS_WIP; + int hwmc_ret; + + memcpy(&hwmc_probe, &gvd, sizeof(gvd)); + + for (retry = 0; retry < DS5_HW_RESET_MAX_RETRIES; retry++) { + hwmc_ret = ds5_raw_write(state, DS5_HWMC_DATA, + &hwmc_probe, sizeof(hwmc_probe)); + if (hwmc_ret) { + dev_dbg(&state->client->dev, + "%s(): HWMC probe write failed (%d), retry %d\n", + __func__, hwmc_ret, retry); + msleep(DS5_HW_RESET_POLL_INTERVAL_MS); + continue; + } + + hwmc_ret = ds5_write(state, DS5_HWMC_EXEC, 0x01); + if (hwmc_ret) { + dev_dbg(&state->client->dev, + "%s(): HWMC probe exec failed (%d), retry %d\n", + __func__, hwmc_ret, retry); + msleep(DS5_HW_RESET_POLL_INTERVAL_MS); + continue; + } + + /* Poll status — allow both I2C and WIP retries */ + { + int poll; + + for (poll = 0; poll < 100; poll++) { + msleep_range(5); + hwmc_ret = ds5_read(state, DS5_HWMC_STATUS, + &hwmc_status); + if (hwmc_ret == 0 && + hwmc_status == DS5_HWMC_STATUS_OK) + break; + } + } + + if (hwmc_ret == 0 && hwmc_status == DS5_HWMC_STATUS_OK) { + dev_info(&state->client->dev, + "%s(): HWMC ready after %d ms\n", + __func__, + (retry + 1) * DS5_HW_RESET_POLL_INTERVAL_MS); + break; + } + + dev_dbg(&state->client->dev, + "%s(): HWMC not ready (status: 0x%x, ret: %d), retry %d\n", + __func__, hwmc_status, hwmc_ret, retry); + msleep(DS5_HW_RESET_POLL_INTERVAL_MS); + } + + if (retry >= DS5_HW_RESET_MAX_RETRIES) + dev_warn(&state->client->dev, + "%s(): HWMC subsystem not ready after %d ms, userspace queries may fail\n", + __func__, DS5_HW_RESET_TIMEOUT_MS); + } + +#ifdef CONFIG_VIDEO_D4XX_SERDES + /* 10. Post-reset I2C stability verification (conditional). + * Only needed when SERDES recovery was actively performed in Step 6, + * because the serializer/deserializer re-init can cause transient + * I2C instability. When the GMSL link recovered naturally (Step 6 + * was skipped), Steps 7-9 already verified the link via multiple + * I2C reads — an additional 600ms+ stability loop is unnecessary + * and causes timing-sensitive CI test failures. + * + * Some camera SKUs have a secondary FW initialization phase that + * briefly drops the I2C bus ~65-110ms after the initial readiness + * checks (Steps 7-9) pass. This has been observed on both D401 + * (FW 5.17.x) and D457 (FW 5.17.2.7). If we return now, + * userspace HWMC queries NAK, the viewer triggers another reset, + * and repeated rapid resets degrade the GMSL link until SERDES pipe + * setup fails and the device is unrecoverable without a host reboot. + * + * Verify I2C stability by performing DS5_HW_RESET_STABILITY_READS + * consecutive successful reads spaced DS5_HW_RESET_STABILITY_INTERVAL_MS + * apart. If the link drops mid-verification, reset the counter and + * keep waiting (up to DS5_HW_RESET_STABILITY_TIMEOUT_MS). If it + * remains unstable, escalate to Phase 2 (full deserializer reset). + */ + if (!serdes_recovery_ran) { + dev_info(&state->client->dev, + "%s(): SERDES recovery was skipped (natural link recovery), " + "bypassing Step 10 stability verification\n", __func__); + } else { + int stable_count = 0; + unsigned long stab_ts = jiffies; + unsigned long stab_timeout = + stab_ts + msecs_to_jiffies(DS5_HW_RESET_STABILITY_TIMEOUT_MS); + u16 stab_val = 0; + + while (time_before(jiffies, stab_timeout)) { + msleep(DS5_HW_RESET_STABILITY_INTERVAL_MS); + + ret = ds5_read(state, DS5_FW_VERSION, &stab_val); + if (ret == 0 && stab_val != 0) { + stable_count++; + if (stable_count >= DS5_HW_RESET_STABILITY_READS) { + dev_info(&state->client->dev, + "%s(): I2C link stable after %d ms (%d consecutive reads OK)\n", + __func__, + jiffies_to_msecs(jiffies - stab_ts), + stable_count); + break; + } + } else { + if (stable_count > 0) + dev_warn(&state->client->dev, + "%s(): I2C stability check failed after %d OK reads (ret=%d, val=0x%x), resetting counter\n", + __func__, stable_count, ret, stab_val); + stable_count = 0; + } + } + + if (stable_count < DS5_HW_RESET_STABILITY_READS) { + /* I2C link is unstable — escalate to Phase 2 (full deser reset) */ + dev_warn(&state->client->dev, + "%s(): I2C link unstable after %d ms, escalating to Phase 2 SERDES recovery\n", + __func__, + jiffies_to_msecs(jiffies - stab_ts)); + + ret = ds5_hw_reset_serdes_recovery(state, true); + if (ret < 0) { + dev_err(&state->client->dev, + "%s(): Phase 2 escalation failed: %d\n", + __func__, ret); + /* Continue anyway — the device might still be partially usable */ + } else { + /* Re-verify stability after Phase 2 */ + stable_count = 0; + stab_ts = jiffies; + stab_timeout = stab_ts + msecs_to_jiffies( + DS5_HW_RESET_STABILITY_TIMEOUT_MS); + + while (time_before(jiffies, stab_timeout)) { + msleep(DS5_HW_RESET_STABILITY_INTERVAL_MS); + ret = ds5_read(state, DS5_FW_VERSION, &stab_val); + if (ret == 0 && stab_val != 0) { + stable_count++; + if (stable_count >= DS5_HW_RESET_STABILITY_READS) { + dev_info(&state->client->dev, + "%s(): I2C link stable after Phase 2 (%d ms)\n", + __func__, + jiffies_to_msecs(jiffies - stab_ts)); + break; + } + } else { + stable_count = 0; + } + } + + if (stable_count < DS5_HW_RESET_STABILITY_READS) + dev_warn(&state->client->dev, + "%s(): I2C still unstable after Phase 2, proceeding anyway\n", + __func__); + } + } + } /* if (serdes_recovery_ran) */ + + /* 11. Per-pipe reconfiguration is NOT done here. + * - First-probe: ds5_probe() configures pipe 3 (IMU) after + * this function returns, before IMU peer probes. + * - HWMC reset: ds5_invalidate_sensor() (Step 2) already + * cleared pipe_data_type1/2/vc_id for all peers. At next + * STREAMON, ds5_configure() detects the mismatch and calls + * ds5_setup_pipeline() to reconfigure each pipe. + * + * Do NOT call max9295_init_settings() or ds5_setup_pipeline() + * here. init_settings() writes global MAX9295 registers that + * kill the GMSL link. And for HWMC resets, the existing + * ds5_configure() path handles pipe reconfiguration cleanly. + */ +#endif + dev_info(&state->client->dev, "%s(): HW reset complete. Firmware: %d.%d.%d.%d\n", __func__, (state->fw_version >> 8) & 0xff, state->fw_version & 0xff, (state->fw_build >> 8) & 0xff, state->fw_build & 0xff); + ds5_last_reset_jiffies = jiffies; + return 0; } @@ -3116,24 +3521,24 @@ static int ds5_gvd(struct ds5 *state, unsigned char *data) struct hwm_cmd cmd; int ret = -1; u16 length = 0; - u16 status = 2; - u8 retries = 3; + u16 status = DS5_HWMC_STATUS_WIP; + u8 retries = 20; memcpy(&cmd, &gvd, sizeof(gvd)); ds5_raw_write_with_check(state, DS5_HWMC_DATA, &cmd, sizeof(cmd)); /* Write command data */ ds5_write_with_check(state, DS5_HWMC_EXEC, 0x01); /* execute cmd */ do { - if (retries != 3) - msleep_range(10); + if (retries != 20) + msleep_range(50); ret = ds5_read(state, DS5_HWMC_STATUS, &status); - } while (ret && retries-- && status != 0); + } while ((ret || status == DS5_HWMC_STATUS_WIP) && retries--); - if (ret || status != 0) { + if (ret || status != DS5_HWMC_STATUS_OK) { dev_err(&state->client->dev, - "%s(): Failed to read GVD, HWM cmd status: %x\n", - __func__, status); - return status; + "%s(): Failed to read GVD, HWM cmd status: %x, ret: %d\n", + __func__, status, ret); + return -EIO; } ret = ds5_raw_read(state, DS5_HWMC_RESP_LEN, &length, sizeof(length)); /* Read response length */ @@ -3873,7 +4278,7 @@ static int ds5_board_setup(struct ds5 *state) } } dev_err(dev, "cannot register more than %d D4XX instances\n", MAX_DEV_NUM); - return -ENOTSUPP; + return -ENOSPC; } } /* First instance of a new camera */ @@ -4034,7 +4439,7 @@ static int ds5_board_setup(struct ds5 *state) } } dev_err(dev, "cannot register more than %d D4XX instances\n", MAX_DEV_NUM); - return -ENOTSUPP; + return -ENOSPC; } } for (i = 0; i < MAX_DEV_NUM; i++) { @@ -4835,6 +5240,9 @@ static int ds5_mux_s_stream(struct v4l2_subdev *sd, int on) struct ds5_sensor *sensor = state->mux.last_set; u16 expected_streaming_state; bool ds5_config_done = !on; /* for stop, skip config */ +#ifdef CONFIG_VIDEO_D4XX_SERDES + int serdes_recovery_attempts = 0; +#endif // spare duplicate calls if (sensor->streaming == on) @@ -4880,6 +5288,9 @@ static int ds5_mux_s_stream(struct v4l2_subdev *sd, int on) status = DS5_STATUS_STREAMING; } +#ifdef CONFIG_VIDEO_D4XX_SERDES +retry_after_serdes_recovery: +#endif /* Verify stream is in the expected state before issuing command */ ts = jiffies; for (timeout = ts + msecs_to_jiffies(DS5_START_MAX_TIME), i = 0; @@ -4985,6 +5396,58 @@ static int ds5_mux_s_stream(struct v4l2_subdev *sd, int on) dev_warn(&state->client->dev, "stream %d toggle to %d timeout in %dms, retries %d\n", stream_id, on, jiffies_to_msecs(jiffies - ts), i); + +#ifdef CONFIG_VIDEO_D4XX_SERDES + /* Stream start timed out. Probe the I2C link: if it is dead + * (EREMOTEIO / -121), the GMSL link dropped — possibly a + * delayed secondary failure after a prior HW reset. Attempt + * SerDes recovery (Phase 1: serializer re-init, Phase 2: full + * deserializer reset if serializer is also unreachable) and + * retry the stream start once. + */ + if (on && serdes_recovery_attempts < 2 && + state->ser_dev && state->dser_dev) { + u16 probe_val; + int probe_ret = ds5_read(state, DS5_FW_VERSION, &probe_val); + + if (probe_ret < 0) { + serdes_recovery_attempts++; + dev_warn(&state->client->dev, + "stream %d: I2C link dead (err %d), " + "attempting GMSL recovery (attempt %d/2)\n", + stream_id, probe_ret, + serdes_recovery_attempts); + + /* Clean up partial state from the failed attempt */ + sensor->streaming = restore_val; + ds5_invalidate_sensor(state, sensor); + + /* 1st attempt: Phase 1 (ser re-init), escalate to + * Phase 2 (reset_oneshot + poll) if unstable. + * 2nd attempt: skip Phase 1, go straight to Phase 2. + * The reset_oneshot from attempt 1 may have already + * started the link recovery — Phase 2 polling will + * detect it. + */ + if (ds5_hw_reset_serdes_recovery(state, + serdes_recovery_attempts > 1) == 0) { + dev_info(&state->client->dev, + "stream %d: GMSL recovery OK, " + "retrying stream start\n", + stream_id); + ds5_config_done = false; + ds5_config_retries = MAX_DS5_CONFIG_RETRIES; + status = on ? 0 : DS5_STATUS_STREAMING; + goto retry_after_serdes_recovery; + } + dev_err(&state->client->dev, + "stream %d: GMSL recovery failed " + "(attempt %d/2), stream start aborted\n", + stream_id, serdes_recovery_attempts); + } + } +#endif + if (streaming == expected_streaming_state) { /* try to toggle stream back on timeout */ ds5_write(state, DS5_START_STOP_STREAM, (on ? DS5_STREAM_STOP : DS5_STREAM_START) | stream_id); @@ -5306,6 +5769,16 @@ static int ds5_fixed_configuration(struct i2c_client *client, struct ds5 *state) if (ret < 0) return ret; + /* After HW reset the FW may not have populated DS5_DEVICE_TYPE yet. + * Use the cached value confirmed by Step 8 in hw_reset_with_recovery. + */ + if (dev_type == 0 && ds5_cached_device_type != 0) { + dev_info(&client->dev, + "%s(): device type register returned 0, using cached type 0x%x\n", + __func__, ds5_cached_device_type); + dev_type = ds5_cached_device_type; + } + dev_dbg(&client->dev, "%s(): cfg0 %x %ux%u cfg0_md %x %ux%u\n", __func__, cfg0, dw, dh, cfg0_md, yw, yh); @@ -5330,7 +5803,10 @@ static int ds5_fixed_configuration(struct i2c_client *client, struct ds5 *state) sensor->formats = ds5_depth_formats_d46x; break; default: - sensor->formats = ds5_depth_formats_d46x; + dev_warn(&client->dev, + "%s(): unknown device type 0x%x, using D43X format tables\n", + __func__, dev_type); + sensor->formats = ds5_depth_formats_d43x; } sensor->n_formats = 1; sensor->mux_pad = DS5_MUX_PAD_DEPTH; @@ -5793,6 +6269,13 @@ static void ds5_adjust_sync_mode_control(struct i2c_client *client, struct ds5 * return; } + if (dev_type == 0 && ds5_cached_device_type != 0) { + dev_info(&client->dev, + "%s(): device type register returned 0, using cached type 0x%x\n", + __func__, ds5_cached_device_type); + dev_type = ds5_cached_device_type; + } + switch (dev_type) { case DS5_DEVICE_TYPE_D41X: /* D41X does not support sync mode */ @@ -6286,13 +6769,67 @@ static int ds5_probe(struct i2c_client *c, const struct i2c_device_id *id) goto e_chardev; } - /* Allow the camera firmware to fully stabilize after HW reset - * before subsequent driver instances attempt I2C communication. - * Without this delay, the D457's FW may be briefly unresponsive - * during a post-reset initialization phase, causing later probe - * instances (especially the 4th/IMU) to fail I2C checks. + /* Wait for D457 FW to finish reconfiguring the MAX9295 + * serializer. The FW continues writing to MAX9295 for + * ~50-100ms after reporting 0xDEAD. 200ms total provides + * margin before we touch serializer registers. */ - msleep(100); + msleep(200); + +#ifdef CONFIG_VIDEO_D4XX_SERDES + /* Configure pipe 3 (IMU) on the serializer/deserializer, + * but only if an IMU peer actually exists in the device tree. + * + * The D457 FW configures pipes 0-2 (Depth/RGB/IR) during + * boot but does NOT configure pipe 3. Without pipe 3, + * the serializer has no I2C address translation for the + * IMU instance and its probe will fail with -EREMOTEIO. + * + * Only needed at first-probe time. For HWMC resets, + * ds5_configure() handles pipe reconfiguration at STREAMON. + * + * Uses ds5_setup_pipeline() (per-pipe registers only), + * NOT max9295_init_settings() which writes global registers + * (0x02, 0x308, 0x311) that disrupt the active GMSL link. + */ + { + struct device_node *bus_node, *peer; + const char *peer_cam_type; + bool has_imu_peer = false; + + bus_node = of_get_parent(c->dev.of_node); + if (bus_node) { + for_each_child_of_node(bus_node, peer) { + if (!of_property_read_string(peer, "cam-type", + &peer_cam_type) && + !strcmp(peer_cam_type, "IMU")) { + has_imu_peer = true; + of_node_put(peer); + break; + } + } + of_node_put(bus_node); + } + + if (has_imu_peer) { + int pipe3_ret = ds5_setup_pipeline(state, + DS5_IMU_DT1, DS5_IMU_DT2, + DS5_IMU_PIPE_ID, DS5_IMU_VC_ID); + if (pipe3_ret) + dev_warn(&c->dev, + "%s(): first-probe pipe 3 (IMU) setup failed: %d\n", + __func__, pipe3_ret); + else + dev_info(&c->dev, + "%s(): first-probe pipe 3 (IMU) configured\n", + __func__); + } else { + dev_dbg(&c->dev, + "%s(): no IMU peer in DT, skipping pipe 3 setup\n", + __func__); + } + } +#endif } /* Verify communication with retries and delay. @@ -6413,7 +6950,7 @@ static int ds5_remove(struct i2c_client *c) } if (state->ser_i2c) i2c_unregister_device(state->ser_i2c); - if (state->dser_i2c) + if (state->dser_i2c && !state->aggregated) i2c_unregister_device(state->dser_i2c); #endif #ifndef CONFIG_TEGRA_CAMERA_PLATFORM @@ -6477,4 +7014,4 @@ MODULE_AUTHOR("Guennadi Liakhovetski ,\n\ Shikun Ding ,\n\ Dmitry Perchanov "); MODULE_LICENSE("GPL v2"); -MODULE_VERSION("1.0.2.19"); +MODULE_VERSION("1.0.2.21");