media: imx: mipi csi-2: Fix set_fmt try

csi2_set_fmt() was setting the try_fmt only on the first pad, and pad
index was ignored. Fix by introducing __csi2_get_fmt().

Signed-off-by: Steve Longerbeam <steve_longerbeam@mentor.com>
Reviewed-by: Philipp Zabel <p.zabel@pengutronix.de>
Signed-off-by: Hans Verkuil <hans.verkuil@cisco.com>
Signed-off-by: Mauro Carvalho Chehab <mchehab@s-opensource.com>