Skip to content

Enhance DS5 sensor configuration with retry logic for I2C operations and caching of parameters #348

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
Nikolai-L wants to merge 1 commit into from
Closed

Enhance DS5 sensor configuration with retry logic for I2C operations and caching of parameters #348

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
206 changes: 167 additions & 39 deletions kernel/realsense/d4xx.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -419,6 +419,17 @@ struct ds5_sensor {
const struct ds5_format *formats;
unsigned int n_formats;
int pipe_id;
int last_pipe_id;
u16 pipe_data_type1;
u16 pipe_data_type2;
u32 pipe_vc_id;
bool pipe_configured;
u16 cached_dt_value;
u16 cached_md_value;
u16 cached_override_value;
u16 cached_fps_value;
u16 cached_width_value;
u16 cached_height_value;
};

#ifdef CONFIG_TEGRA_CAMERA_PLATFORM
Expand Down Expand Up @@ -536,6 +547,8 @@ static int ds5_write(struct ds5 *state, u16 reg, u16 val)
{
int ret;
u8 value[2];
int retry;
int delay_ms = 10;

value[1] = val >> 8;
value[0] = val & 0x00FF;
Expand All @@ -544,7 +557,28 @@ static int ds5_write(struct ds5 *state, u16 reg, u16 val)
"%s(): writing to register: 0x%04x, value1: 0x%x, value2:0x%x\n",
__func__, reg, value[1], value[0]);

ret = regmap_raw_write(state->regmap, reg, value, sizeof(value));
for (retry = 0; retry < 3; retry++) {
ret = regmap_raw_write(state->regmap, reg, value, sizeof(value));
if (ret == 0)
break;

/* Retry only on timeout/remote IO errors */
if (ret != -ETIMEDOUT && ret != -EREMOTEIO) {
dev_err(&state->client->dev,
"%s(): i2c write failed %d, 0x%04x = 0x%x\n",
__func__, ret, reg, val);
return ret;
}

if (retry < 2) {
dev_dbg(&state->client->dev,
"%s(): retry %d after %dms delay, 0x%04x\n",
__func__, retry + 1, delay_ms, reg);
msleep(delay_ms);
delay_ms *= 2;
}
}
Comment on lines +560 to +580

Copilot AI Feb 9, 2026

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The retry/backoff logic is duplicated across ds5_write(), ds5_raw_write(), and ds5_read(). To reduce the chance of these drifting (different retry counts, delays, error handling, log messages), consider factoring the common retry policy into a shared helper (e.g., a small internal function that accepts an op callback or separate helpers for read/write) and reuse it in all three call sites.

Copilot uses AI. Check for mistakes.

if (ret < 0)
dev_err(&state->client->dev,
"%s(): i2c write failed %d, 0x%04x = 0x%x\n",
Expand All @@ -560,7 +594,32 @@ static int ds5_write(struct ds5 *state, u16 reg, u16 val)
static int ds5_raw_write(struct ds5 *state, u16 reg,
const void *val, size_t val_len)
{
int ret = regmap_raw_write(state->regmap, reg, val, val_len);
int ret;
int retry;
int delay_ms = 10;

for (retry = 0; retry < 3; retry++) {
ret = regmap_raw_write(state->regmap, reg, val, val_len);
if (ret == 0)
break;

/* Retry only on timeout/remote IO errors */
if (ret != -ETIMEDOUT && ret != -EREMOTEIO) {
dev_err(&state->client->dev,
"%s(): i2c raw write failed %d, %04x size(%d) bytes\n",
__func__, ret, reg, (int)val_len);
return ret;
}

if (retry < 2) {
dev_dbg(&state->client->dev,
"%s(): retry %d after %dms delay, 0x%04x\n",
__func__, retry + 1, delay_ms, reg);
msleep(delay_ms);
delay_ms *= 2;
}
}

if (ret < 0)
dev_err(&state->client->dev,
"%s(): i2c raw write failed %d, %04x size(%d) bytes\n",
Expand All @@ -576,7 +635,31 @@ static int ds5_raw_write(struct ds5 *state, u16 reg,

static int ds5_read(struct ds5 *state, u16 reg, u16 *val)
{
int ret = regmap_raw_read(state->regmap, reg, val, 2);
int ret;
int retry;
int delay_ms = 10;

for (retry = 0; retry < 3; retry++) {
ret = regmap_raw_read(state->regmap, reg, val, 2);
if (ret == 0)
break;

/* Retry only on timeout/remote IO errors */
if (ret != -ETIMEDOUT && ret != -EREMOTEIO) {
dev_err(&state->client->dev, "%s(): i2c read failed %d, 0x%04x\n",
__func__, ret, reg);
return ret;
}

if (retry < 2) {
dev_dbg(&state->client->dev,
"%s(): retry %d after %dms delay, 0x%04x\n",
__func__, retry + 1, delay_ms, reg);
msleep(delay_ms);
delay_ms *= 2;
}
}

if (ret < 0)
dev_err(&state->client->dev, "%s(): i2c read failed %d, 0x%04x\n",
__func__, ret, reg);
Expand Down Expand Up @@ -1827,6 +1910,11 @@ static int ds5_configure(struct ds5 *state)
u16 data_type1, data_type2;
#endif
u16 dt_addr, md_addr, override_addr, fps_addr, width_addr, height_addr;
u16 dt_value = 0;
u16 md_value = 0;
u16 fps_value = 0;
u16 width_value = 0;
u16 height_value = 0;
int ret;

if (state->is_depth) {
Expand Down Expand Up @@ -1878,14 +1966,31 @@ static int ds5_configure(struct ds5 *state)
data_type2 = state->is_imu ? 0x00 : md_fmt;

vc_id = state->g_ctx.dst_vc;

ret = ds5_setup_pipeline(state, data_type1, data_type2, sensor->pipe_id,
vc_id);
// reset data path when switching to Y12I
if (state->is_y8 && data_type1 == GMSL_CSI_DT_RGB_888)
max9296_reset_oneshot(state->dser_dev);
if (ret < 0)
return ret;
if (!sensor->pipe_configured ||
sensor->pipe_id != sensor->last_pipe_id ||
sensor->pipe_data_type1 != data_type1 ||
sensor->pipe_data_type2 != data_type2 ||
sensor->pipe_vc_id != vc_id) {
ret = ds5_setup_pipeline(state, data_type1, data_type2,
sensor->pipe_id, vc_id);
// reset data path when switching to Y12I
if (state->is_y8 && data_type1 == GMSL_CSI_DT_RGB_888)
max9296_reset_oneshot(state->dser_dev);
if (ret < 0)
return ret;
sensor->pipe_configured = true;
sensor->last_pipe_id = sensor->pipe_id;
sensor->pipe_data_type1 = data_type1;
sensor->pipe_data_type2 = data_type2;
sensor->pipe_vc_id = vc_id;
dev_warn(&state->client->dev,
"pipe %d configured (dt1=0x%x dt2=0x%x vc=%u)\n",
sensor->pipe_id, data_type1, data_type2, vc_id);
} else {
dev_warn(&state->client->dev,
Comment on lines +1986 to +1990

Copilot AI Feb 9, 2026

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

These dev_warn() logs will likely fire on every start/configure call and can spam dmesg in normal operation (especially the 'already configured' case). Consider switching these to dev_dbg(), or dev_warn_ratelimited() if you want visibility without flooding logs.

Suggested change
dev_warn(&state->client->dev,
"pipe %d configured (dt1=0x%x dt2=0x%x vc=%u)\n",
sensor->pipe_id, data_type1, data_type2, vc_id);
} else {
dev_warn(&state->client->dev,
dev_dbg(&state->client->dev,
"pipe %d configured (dt1=0x%x dt2=0x%x vc=%u)\n",
sensor->pipe_id, data_type1, data_type2, vc_id);
} else {
dev_dbg(&state->client->dev,

Copilot uses AI. Check for mistakes.
"pipe %d already configured (dt1=0x%x dt2=0x%x vc=%u)\n",
sensor->pipe_id, data_type1, data_type2, vc_id);
}
#endif

fmt = sensor->streaming ? sensor->config.format->data_type : 0;
Expand All @@ -1895,55 +2000,72 @@ static int ds5_configure(struct ds5 *state)
* Set IR stream Y8I data type as 0x32
*/
if (state->is_depth && fmt != 0)
ret = ds5_write(state, dt_addr, 0x31);
dt_value = 0x31;
else if (state->is_y8 && fmt != 0 &&
sensor->config.format->data_type == GMSL_CSI_DT_YUV422_8) {
if (sensor->config.format->mbus_code == MEDIA_BUS_FMT_VYUY8_1X16)
{
/* This is the custom Y8I format -
* telling FW to enable "etMipiDataType_UserDefined3_R8L8"
*/
ret = ds5_write(state, dt_addr, GMSL_CSI_DT_CUSTOM_Y8I_16);
if (sensor->config.format->mbus_code == MEDIA_BUS_FMT_VYUY8_1X16) {
dt_value = GMSL_CSI_DT_CUSTOM_Y8I_16;
} else if (sensor->config.format->mbus_code == MEDIA_BUS_FMT_YUYV8_1X16) {
/* This is the custom RGB through IR format -
* telling FW to enable "etMipiDataType_UserDefined0_IR_RGB"
*/
ret = ds5_write(state, dt_addr, GMSL_CSI_DT_CUSTOM_IR_RGB_16);
dt_value = GMSL_CSI_DT_CUSTOM_IR_RGB_16;
} else {
dev_err(sensor->sd.dev, "%s(): Illegal mbus_code %u for IR sensor\n",
__func__, sensor->config.format->mbus_code);
return -EINVAL;
}
} else {
ret = ds5_write(state, dt_addr, fmt);
dt_value = fmt;
}
if (ret < 0)
return ret;

ret = ds5_write(state, md_addr, (vc_id << 8) | md_fmt);
if (ret < 0)
return ret;
if (sensor->cached_dt_value != dt_value) {
ret = ds5_write(state, dt_addr, dt_value);
if (ret < 0)
return ret;
sensor->cached_dt_value = dt_value;
}
Comment on lines +2019 to +2024

Copilot AI Feb 9, 2026

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The cache fields appear to rely on default initialization, which can cause the very first write to be skipped when the desired value is 0 (e.g., dt_value == 0 and cached_dt_value defaults to 0). That changes behavior from always writing to potentially never programming the register. Fix by initializing cached_* to an invalid sentinel (e.g., 0xFFFF for u16, 0xFFFFFFFF for u32) or by adding per-field 'valid' flags so the first configuration always writes.

Copilot uses AI. Check for mistakes.

md_value = (vc_id << 8) | md_fmt;
if (sensor->cached_md_value != md_value) {
ret = ds5_write(state, md_addr, md_value);
if (ret < 0)
return ret;
sensor->cached_md_value = md_value;
}

if (!sensor->streaming)
return ret;

if (override_addr != 0) {
ret = ds5_write(state, override_addr, fmt);
if (sensor->cached_override_value != fmt) {
ret = ds5_write(state, override_addr, fmt);
if (ret < 0)
return ret;
sensor->cached_override_value = fmt;
}
}

fps_value = sensor->config.framerate;
if (sensor->cached_fps_value != fps_value) {
ret = ds5_write(state, fps_addr, fps_value);
if (ret < 0)
return ret;
sensor->cached_fps_value = fps_value;
}

ret = ds5_write(state, fps_addr, sensor->config.framerate);
if (ret < 0)
return ret;

ret = ds5_write(state, width_addr, sensor->config.resolution->width);
if (ret < 0)
return ret;
width_value = sensor->config.resolution->width;
if (sensor->cached_width_value != width_value) {
ret = ds5_write(state, width_addr, width_value);
if (ret < 0)
return ret;
sensor->cached_width_value = width_value;
}

ret = ds5_write(state, height_addr, sensor->config.resolution->height);
if (ret < 0)
return ret;
height_value = sensor->config.resolution->height;
if (sensor->cached_height_value != height_value) {
ret = ds5_write(state, height_addr, height_value);
if (ret < 0)
return ret;
sensor->cached_height_value = height_value;
}

return 0;
}
Expand Down Expand Up @@ -4751,6 +4873,8 @@ static int ds5_mux_s_stream(struct v4l2_subdev *sd, int on)
ret = -(ENOSR);
goto restore_s_state;
}
sensor->pipe_configured = false;
sensor->last_pipe_id = sensor->pipe_id;
#endif

ret = ds5_configure(state);
Expand Down Expand Up @@ -4815,6 +4939,8 @@ static int ds5_mux_s_stream(struct v4l2_subdev *sd, int on)
if (max9296_release_pipe(state->dser_dev, sensor->pipe_id) < 0)
dev_warn(&state->client->dev, "release pipe failed\n");
sensor->pipe_id = -1;
sensor->last_pipe_id = -1;
sensor->pipe_configured = false;

Copilot AI Feb 9, 2026

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

On stream stop / pipe release you invalidate pipe_configured/last_pipe_id, but the cached register values (cached_dt_value, cached_md_value, cached_override_value, cached_fps_value, cached_width_value, cached_height_value) are left intact. If the device/firmware loses state across stop/start (or if registers are reset elsewhere), ds5_configure() may skip required writes and leave hardware misconfigured. Consider explicitly invalidating all cached_* fields here (and in the corresponding stream-on reset path) by setting them back to the sentinel/invalid state (or clearing 'valid' flags).

Suggested change
sensor->pipe_configured = false;
sensor->pipe_configured = false;
sensor->cached_dt_value = -1;
sensor->cached_md_value = -1;
sensor->cached_override_value = -1;
sensor->cached_fps_value = -1;
sensor->cached_width_value = -1;
sensor->cached_height_value = -1;

Copilot uses AI. Check for mistakes.
#else
#ifdef CONFIG_VIDEO_INTEL_IPU6
d4xx_reset_oneshot(state);
Expand All @@ -4839,6 +4965,8 @@ static int ds5_mux_s_stream(struct v4l2_subdev *sd, int on)
if (max9296_release_pipe(state->dser_dev, sensor->pipe_id) < 0)
dev_warn(&state->client->dev, "release pipe failed\n");
sensor->pipe_id = -1;
sensor->last_pipe_id = -1;
sensor->pipe_configured = false;
}
#endif

Expand Down
Loading