diff options
| -rw-r--r-- | drivers/media/platform/qcom/camss/camss-csid.c | 69 |
1 files changed, 56 insertions, 13 deletions
diff --git a/drivers/media/platform/qcom/camss/camss-csid.c b/drivers/media/platform/qcom/camss/camss-csid.c index db960da560c9..cf543fa2c497 100644 --- a/drivers/media/platform/qcom/camss/camss-csid.c +++ b/drivers/media/platform/qcom/camss/camss-csid.c @@ -300,6 +300,47 @@ static const struct csid_format csid_formats_8x96[] = { } }; +static u32 csid_find_code(u32 *code, unsigned int n_code, + unsigned int index, u32 req_code) +{ + int i; + + if (!req_code && (index >= n_code)) + return 0; + + for (i = 0; i < n_code; i++) + if (req_code) { + if (req_code == code[i]) + return req_code; + } else { + if (i == index) + return code[i]; + } + + return code[0]; +} + +static u32 csid_src_pad_code(struct csid_device *csid, u32 sink_code, + unsigned int index, u32 src_req_code) +{ + if (csid->camss->version == CAMSS_8x16) { + if (index > 0) + return 0; + + return sink_code; + } else if (csid->camss->version == CAMSS_8x96) { + switch (sink_code) { + default: + if (index > 0) + return 0; + + return sink_code; + } + } else { + return 0; + } +} + static const struct csid_format *csid_get_fmt_entry( const struct csid_format *formats, unsigned int nformat, @@ -674,15 +715,15 @@ static void csid_try_format(struct csid_device *csid, case MSM_CSID_PAD_SRC: if (csid->testgen_mode->cur.val == 0) { - /* Test generator is disabled, keep pad formats */ - /* in sync - set and return a format same as sink pad */ - struct v4l2_mbus_framefmt format; + /* Test generator is disabled, */ + /* keep pad formats in sync */ + u32 code = fmt->code; - format = *__csid_get_format(csid, cfg, - MSM_CSID_PAD_SINK, which); - *fmt = format; + *fmt = *__csid_get_format(csid, cfg, + MSM_CSID_PAD_SINK, which); + fmt->code = csid_src_pad_code(csid, fmt->code, 0, code); } else { - /* Test generator is enabled, set format on source*/ + /* Test generator is enabled, set format on source */ /* pad to allow test generator usage */ for (i = 0; i < csid->nformats; i++) @@ -716,7 +757,6 @@ static int csid_enum_mbus_code(struct v4l2_subdev *sd, struct v4l2_subdev_mbus_code_enum *code) { struct csid_device *csid = v4l2_get_subdevdata(sd); - struct v4l2_mbus_framefmt *format; if (code->pad == MSM_CSID_PAD_SINK) { if (code->index >= csid->nformats) @@ -725,13 +765,16 @@ static int csid_enum_mbus_code(struct v4l2_subdev *sd, code->code = csid->formats[code->index].code; } else { if (csid->testgen_mode->cur.val == 0) { - if (code->index > 0) - return -EINVAL; + struct v4l2_mbus_framefmt *sink_fmt; - format = __csid_get_format(csid, cfg, MSM_CSID_PAD_SINK, - code->which); + sink_fmt = __csid_get_format(csid, cfg, + MSM_CSID_PAD_SINK, + code->which); - code->code = format->code; + code->code = csid_src_pad_code(csid, sink_fmt->code, + code->index, 0); + if (!code->code) + return -EINVAL; } else { if (code->index >= csid->nformats) return -EINVAL; |