diff --git a/drivers/media/i2c/vd55g1.c b/drivers/media/i2c/vd55g1.c index 37d44abd1435..c4142d771e7c 100644 --- a/drivers/media/i2c/vd55g1.c +++ b/drivers/media/i2c/vd55g1.c @@ -29,11 +29,7 @@ /* Register Map */ #define VD55G1_REG_MODEL_ID CCI_REG32_LE(0x0000) -#define VD55G1_MODEL_ID_VD55G1 0x53354731 /* Mono */ -#define VD55G1_MODEL_ID_VD65G4 0x53354733 /* RGB */ -#define VD55G1_REG_REVISION CCI_REG16_LE(0x0004) -#define VD55G1_REVISION_CCB 0x2020 -#define VD55G1_REVISION_BAYER 0x3030 +#define VD55G1_REG_COLOR_VERSION CCI_REG32_LE(0x0670) #define VD55G1_REG_FWPATCH_REVISION CCI_REG16_LE(0x0012) #define VD55G1_REG_FWPATCH_START_ADDR CCI_REG8(0x2000) #define VD55G1_REG_SYSTEM_FSM CCI_REG8(0x001c) @@ -137,8 +133,39 @@ #define VD55G1_MIPI_RATE_MIN (250 * MEGA) #define VD55G1_MIPI_RATE_MAX (1200 * MEGA) -#define VD55G1_MODEL_ID_NAME(id) \ - ((id) == VD55G1_MODEL_ID_VD55G1 ? "vd55g1" : "vd65g4") +enum vd55g1_model_id { + VD55G1_MODEL_ID_2 = 0x53354731, + VD55G1_MODEL_ID_3 = 0x53354733, +}; + +enum vd55g1_color_version { + VD55G1_COLOR_VERSION_MONO, + VD55G1_COLOR_VERSION_BAYER, +}; + +struct vd55g1_version { + char *name; + enum vd55g1_model_id id; + enum vd55g1_color_version color; +}; + +static const struct vd55g1_version vd55g1_versions[] = { + { + .name = "vd55g1", + .id = VD55G1_MODEL_ID_2, + .color = VD55G1_COLOR_VERSION_MONO, + }, + { + .name = "vd55g4", + .id = VD55G1_MODEL_ID_3, + .color = VD55G1_COLOR_VERSION_MONO, + }, + { + .name = "vd65g4", + .id = VD55G1_MODEL_ID_3, + .color = VD55G1_COLOR_VERSION_BAYER, + }, +}; static const u8 vd55g1_patch_array[] = { 0x44, 0x03, 0x09, 0x02, 0xe6, 0x01, 0x42, 0x00, 0xea, 0x01, 0x42, 0x00, @@ -534,7 +561,7 @@ struct vd55g1_vblank_limits { struct vd55g1 { struct device *dev; - unsigned int id; + const struct vd55g1_version *version; struct v4l2_subdev sd; struct media_pad pad; struct regulator_bulk_data supplies[ARRAY_SIZE(vd55g1_supply_name)]; @@ -625,20 +652,28 @@ static unsigned int vd55g1_get_fmt_data_type(u32 code) static u32 vd55g1_get_fmt_code(struct vd55g1 *sensor, u32 code) { + u32 fallback_code; unsigned int i, j; - if (sensor->id == VD55G1_MODEL_ID_VD55G1) - return code; - - for (i = 0; i < ARRAY_SIZE(vd55g1_mbus_formats_bayer); i++) { - for (j = 0; j < ARRAY_SIZE(vd55g1_mbus_formats_bayer[i]); j++) { - if (vd55g1_mbus_formats_bayer[i][j] == code) - goto adapt_bayer_pattern; + if (sensor->version->color == VD55G1_COLOR_VERSION_MONO) { + fallback_code = vd55g1_mbus_formats_mono[0]; + for (i = 0; i < ARRAY_SIZE(vd55g1_mbus_formats_mono); i++) + if (vd55g1_mbus_formats_mono[i] == code) + return code; + } else { + fallback_code = vd55g1_mbus_formats_bayer[0][0]; + for (i = 0; i < ARRAY_SIZE(vd55g1_mbus_formats_bayer); i++) { + for (j = 0; + j < ARRAY_SIZE(vd55g1_mbus_formats_bayer[i]); + j++) { + if (vd55g1_mbus_formats_bayer[i][j] == code) + goto adapt_bayer_pattern; + } } } dev_warn(sensor->dev, "Unsupported mbus format: 0x%x\n", code); - return code; + return fallback_code; adapt_bayer_pattern: j = 0; @@ -1182,8 +1217,8 @@ static int vd55g1_patch(struct vd55g1 *sensor) u64 patch; int ret = 0; - /* vd55g1 needs a patch while vd65g4 does not */ - if (sensor->id == VD55G1_MODEL_ID_VD55G1) { + /* Version 2 needs a patch while version 3 does not */ + if (sensor->version->id == VD55G1_MODEL_ID_2) { vd55g1_write_array(sensor, VD55G1_REG_FWPATCH_START_ADDR, sizeof(vd55g1_patch_array), vd55g1_patch_array, &ret); @@ -1255,7 +1290,7 @@ static int vd55g1_enum_mbus_code(struct v4l2_subdev *sd, struct vd55g1 *sensor = to_vd55g1(sd); u32 base_code; - if (sensor->id == VD55G1_MODEL_ID_VD55G1) { + if (sensor->version->color != VD55G1_COLOR_VERSION_BAYER) { if (code->index >= ARRAY_SIZE(vd55g1_mbus_formats_mono)) return -EINVAL; base_code = vd55g1_mbus_formats_mono[code->index]; @@ -1371,7 +1406,7 @@ static int vd55g1_init_state(struct v4l2_subdev *sd, if (ret) return ret; - if (sensor->id == VD55G1_MODEL_ID_VD55G1) + if (sensor->version->color != VD55G1_COLOR_VERSION_BAYER) code = vd55g1_mbus_formats_mono[0]; else code = vd55g1_mbus_formats_bayer[0][0]; @@ -1658,38 +1693,46 @@ static int vd55g1_init_ctrls(struct vd55g1 *sensor) return ret; } +static const struct vd55g1_version * +vd55g1_get_version(enum vd55g1_model_id id, + enum vd55g1_color_version color) +{ + for (unsigned int i = 0; i < ARRAY_SIZE(vd55g1_versions); i++) { + if (vd55g1_versions[i].id == id && + vd55g1_versions[i].color == color) + return &vd55g1_versions[i]; + } + + return NULL; +} + static int vd55g1_detect(struct vd55g1 *sensor) { - unsigned int dt_id = (uintptr_t)device_get_match_data(sensor->dev); - u64 rev, id; - int ret; + const struct vd55g1_version *dt_version = + device_get_match_data(sensor->dev); + const struct vd55g1_version *version; + u64 color, id; + int ret = 0; - ret = vd55g1_read(sensor, VD55G1_REG_MODEL_ID, &id, NULL); + vd55g1_read(sensor, VD55G1_REG_MODEL_ID, &id, &ret); + vd55g1_read(sensor, VD55G1_REG_COLOR_VERSION, &color, &ret); if (ret) return ret; - if (id != VD55G1_MODEL_ID_VD55G1 && id != VD55G1_MODEL_ID_VD65G4) { - dev_warn(sensor->dev, "Unsupported sensor id 0x%x\n", - (u32)id); + version = vd55g1_get_version(id, color); + if (!version) { + dev_warn(sensor->dev, "Unsupported sensor version, expected %s\n", + dt_version->name); return -ENODEV; } - if (id != dt_id) { - dev_err(sensor->dev, "Probed sensor %s and device tree definition (%s) mismatch", - VD55G1_MODEL_ID_NAME(id), VD55G1_MODEL_ID_NAME(dt_id)); + if (version->id != dt_version->id || + version->color != dt_version->color) { + dev_err(sensor->dev, "Probed sensor version %s and device tree definition %s mismatch", + version->name, dt_version->name); return -ENODEV; } - sensor->id = id; - ret = vd55g1_read(sensor, VD55G1_REG_REVISION, &rev, NULL); - if (ret) - return ret; - - if ((id == VD55G1_MODEL_ID_VD55G1 && rev != VD55G1_REVISION_CCB) && - (id == VD55G1_MODEL_ID_VD65G4 && rev != VD55G1_REVISION_BAYER)) { - dev_err(sensor->dev, "Unsupported sensor revision 0x%x for sensor %s\n", - (u16)rev, VD55G1_MODEL_ID_NAME(id)); - return -ENODEV; - } + sensor->version = version; return 0; } @@ -2047,8 +2090,9 @@ static void vd55g1_remove(struct i2c_client *client) } static const struct of_device_id vd55g1_dt_ids[] = { - { .compatible = "st,vd55g1", .data = (void *)VD55G1_MODEL_ID_VD55G1 }, - { .compatible = "st,vd65g4", .data = (void *)VD55G1_MODEL_ID_VD65G4 }, + { .compatible = "st,vd55g1", .data = (void *)&vd55g1_versions[0] }, + { .compatible = "st,vd55g4", .data = (void *)&vd55g1_versions[1] }, + { .compatible = "st,vd65g4", .data = (void *)&vd55g1_versions[2] }, { /* sentinel */ } }; MODULE_DEVICE_TABLE(of, vd55g1_dt_ids);