[media] V4L: mt9m111: rewrite set_pixfmt

added new bit offset defines,
more supported BE colour formats
and also support BGR565 swapped pixel formats

removed pixfmt helper functions and option flags
setting the configuration register directly in set_pixfmt

added reg_mask function

reg_mask is basically the same as clearing & setting registers,
but it is more convenient and faster (saves one rw cycle).

Signed-off-by: Michael Grzeschik <m.grzeschik@pengutronix.de>
Signed-off-by: Philipp Wiesner <p.wiesner@phytec.de>
Acked-by: Robert Jarzmik <robert.jarzmik@free.fr>
[g.liakhovetski@gmx.de: remove Bayer swap, forward-port, rename macros]
Signed-off-by: Guennadi Liakhovetski <g.liakhovetski@gmx.de>
Signed-off-by: Mauro Carvalho Chehab <mchehab@redhat.com>

drivers/media/video/mt9m111.c

@@ -63,6 +63,12 @@
 #define MT9M111_RESET_RESTART_FRAME	(1 << 1)
 #define MT9M111_RESET_RESET_MODE	(1 << 0)
 
+#define MT9M111_RM_FULL_POWER_RD	(0 << 10)
+#define MT9M111_RM_LOW_POWER_RD		(1 << 10)
+#define MT9M111_RM_COL_SKIP_4X		(1 << 5)
+#define MT9M111_RM_ROW_SKIP_4X		(1 << 4)
+#define MT9M111_RM_COL_SKIP_2X		(1 << 3)
+#define MT9M111_RM_ROW_SKIP_2X		(1 << 2)
 #define MT9M111_RMB_MIRROR_COLS		(1 << 1)
 #define MT9M111_RMB_MIRROR_ROWS		(1 << 0)
 #define MT9M111_CTXT_CTRL_RESTART	(1 << 15)
@@ -95,7 +101,8 @@
 
 #define MT9M111_OPMODE_AUTOEXPO_EN	(1 << 14)
 #define MT9M111_OPMODE_AUTOWHITEBAL_EN	(1 << 1)
-
+#define MT9M111_OUTFMT_FLIP_BAYER_COL	(1 << 9)
+#define MT9M111_OUTFMT_FLIP_BAYER_ROW	(1 << 8)
 #define MT9M111_OUTFMT_PROCESSED_BAYER	(1 << 14)
 #define MT9M111_OUTFMT_BYPASS_IFP	(1 << 10)
 #define MT9M111_OUTFMT_INV_PIX_CLOCK	(1 << 9)
@@ -110,9 +117,8 @@
 #define MT9M111_OUTFMT_TST_RAMP_FRAME	(3 << 4)
 #define MT9M111_OUTFMT_SHIFT_3_UP	(1 << 3)
 #define MT9M111_OUTFMT_AVG_CHROMA	(1 << 2)
-#define MT9M111_OUTFMT_SWAP_YCbCr_C_Y	(1 << 1)
-#define MT9M111_OUTFMT_SWAP_RGB_EVEN	(1 << 1)
-#define MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr	(1 << 0)
+#define MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN	(1 << 1)
+#define MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B	(1 << 0)
 
 /*
  * Camera control register addresses (0x200..0x2ff not implemented)
@@ -122,6 +128,8 @@
 #define reg_write(reg, val) mt9m111_reg_write(client, MT9M111_##reg, (val))
 #define reg_set(reg, val) mt9m111_reg_set(client, MT9M111_##reg, (val))
 #define reg_clear(reg, val) mt9m111_reg_clear(client, MT9M111_##reg, (val))
+#define reg_mask(reg, val, mask) mt9m111_reg_mask(client, MT9M111_##reg, \
+		(val), (mask))
 
 #define MT9M111_MIN_DARK_ROWS	8
 #define MT9M111_MIN_DARK_COLS	26
@@ -153,7 +161,11 @@ static const struct mt9m111_datafmt mt9m111_colour_fmts[] = {
 	{V4L2_MBUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_JPEG},
 	{V4L2_MBUS_FMT_VYUY8_2X8, V4L2_COLORSPACE_JPEG},
 	{V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
+	{V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE, V4L2_COLORSPACE_SRGB},
 	{V4L2_MBUS_FMT_RGB565_2X8_LE, V4L2_COLORSPACE_SRGB},
+	{V4L2_MBUS_FMT_RGB565_2X8_BE, V4L2_COLORSPACE_SRGB},
+	{V4L2_MBUS_FMT_BGR565_2X8_LE, V4L2_COLORSPACE_SRGB},
+	{V4L2_MBUS_FMT_BGR565_2X8_BE, V4L2_COLORSPACE_SRGB},
 	{V4L2_MBUS_FMT_SBGGR8_1X8, V4L2_COLORSPACE_SRGB},
 	{V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
 };
@@ -178,10 +190,6 @@ struct mt9m111 {
 	unsigned int powered:1;
 	unsigned int hflip:1;
 	unsigned int vflip:1;
-	unsigned int swap_rgb_even_odd:1;
-	unsigned int swap_rgb_red_blue:1;
-	unsigned int swap_yuv_y_chromas:1;
-	unsigned int swap_yuv_cb_cr:1;
 	unsigned int autowhitebalance:1;
 };
 
@@ -255,6 +263,17 @@ static int mt9m111_reg_clear(struct i2c_client *client, const u16 reg,
 	return ret;
 }
 
+static int mt9m111_reg_mask(struct i2c_client *client, const u16 reg,
+			    const u16 data, const u16 mask)
+{
+	int ret;
+
+	ret = mt9m111_reg_read(client, reg);
+	if (ret >= 0)
+		ret = mt9m111_reg_write(client, reg, (ret & ~mask) | data);
+	return ret;
+}
+
 static int mt9m111_set_context(struct mt9m111 *mt9m111,
 			       enum mt9m111_context ctxt)
 {
@@ -317,80 +336,6 @@ static int mt9m111_setup_rect(struct mt9m111 *mt9m111,
 	return ret;
 }
 
-static int mt9m111_setup_pixfmt(struct i2c_client *client, u16 outfmt)
-{
-	int ret;
-	u16 mask = MT9M111_OUTFMT_PROCESSED_BAYER | MT9M111_OUTFMT_RGB |
-		MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_SWAP_RGB_EVEN |
-		MT9M111_OUTFMT_RGB565 | MT9M111_OUTFMT_RGB555 |
-		MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr |
-		MT9M111_OUTFMT_SWAP_YCbCr_C_Y;
-
-	ret = reg_read(OUTPUT_FORMAT_CTRL2_A);
-	if (ret >= 0)
-		ret = reg_write(OUTPUT_FORMAT_CTRL2_A, (ret & ~mask) | outfmt);
-	if (!ret)
-		ret = reg_read(OUTPUT_FORMAT_CTRL2_B);
-	if (ret >= 0)
-		ret = reg_write(OUTPUT_FORMAT_CTRL2_B, (ret & ~mask) | outfmt);
-
-	return ret;
-}
-
-static int mt9m111_setfmt_bayer8(struct mt9m111 *mt9m111)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-
-	return mt9m111_setup_pixfmt(client, MT9M111_OUTFMT_PROCESSED_BAYER |
-				    MT9M111_OUTFMT_RGB);
-}
-
-static int mt9m111_setfmt_bayer10(struct mt9m111 *mt9m111)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-
-	return mt9m111_setup_pixfmt(client, MT9M111_OUTFMT_BYPASS_IFP);
-}
-
-static int mt9m111_setfmt_rgb565(struct mt9m111 *mt9m111)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-	int val = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565;
-
-	if (mt9m111->swap_rgb_red_blue)
-		val |= MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr;
-	if (mt9m111->swap_rgb_even_odd)
-		val |= MT9M111_OUTFMT_SWAP_RGB_EVEN;
-
-	return mt9m111_setup_pixfmt(client, val);
-}
-
-static int mt9m111_setfmt_rgb555(struct mt9m111 *mt9m111)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-	int val = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555;
-
-	if (mt9m111->swap_rgb_red_blue)
-		val |= MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr;
-	if (mt9m111->swap_rgb_even_odd)
-		val |= MT9M111_OUTFMT_SWAP_RGB_EVEN;
-
-	return mt9m111_setup_pixfmt(client, val);
-}
-
-static int mt9m111_setfmt_yuv(struct mt9m111 *mt9m111)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-	int val = 0;
-
-	if (mt9m111->swap_yuv_cb_cr)
-		val |= MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr;
-	if (mt9m111->swap_yuv_y_chromas)
-		val |= MT9M111_OUTFMT_SWAP_YCbCr_C_Y;
-
-	return mt9m111_setup_pixfmt(client, val);
-}
-
 static int mt9m111_enable(struct mt9m111 *mt9m111)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
@@ -515,49 +460,70 @@ static int mt9m111_g_fmt(struct v4l2_subdev *sd,
 static int mt9m111_set_pixfmt(struct mt9m111 *mt9m111,
 			      enum v4l2_mbus_pixelcode code)
 {
-	struct i2c_client *client;
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	u16 data_outfmt2, mask_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
+		MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB |
+		MT9M111_OUTFMT_RGB565 | MT9M111_OUTFMT_RGB555 |
+		MT9M111_OUTFMT_RGB444x | MT9M111_OUTFMT_RGBx444 |
+		MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
+		MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
 	int ret;
 
 	switch (code) {
 	case V4L2_MBUS_FMT_SBGGR8_1X8:
-		ret = mt9m111_setfmt_bayer8(mt9m111);
+		data_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
+			MT9M111_OUTFMT_RGB;
 		break;
 	case V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE:
-		ret = mt9m111_setfmt_bayer10(mt9m111);
+		data_outfmt2 = MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB;
 		break;
 	case V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE:
-		ret = mt9m111_setfmt_rgb555(mt9m111);
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555 |
+			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
+		break;
+	case V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE:
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555;
 		break;
 	case V4L2_MBUS_FMT_RGB565_2X8_LE:
-		ret = mt9m111_setfmt_rgb565(mt9m111);
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
+			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
+		break;
+	case V4L2_MBUS_FMT_RGB565_2X8_BE:
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565;
+		break;
+	case V4L2_MBUS_FMT_BGR565_2X8_BE:
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
+			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
+		break;
+	case V4L2_MBUS_FMT_BGR565_2X8_LE:
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
+			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
+			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
 		break;
 	case V4L2_MBUS_FMT_UYVY8_2X8:
-		mt9m111->swap_yuv_y_chromas = 0;
-		mt9m111->swap_yuv_cb_cr = 0;
-		ret = mt9m111_setfmt_yuv(mt9m111);
+		data_outfmt2 = 0;
 		break;
 	case V4L2_MBUS_FMT_VYUY8_2X8:
-		mt9m111->swap_yuv_y_chromas = 0;
-		mt9m111->swap_yuv_cb_cr = 1;
-		ret = mt9m111_setfmt_yuv(mt9m111);
+		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
 		break;
 	case V4L2_MBUS_FMT_YUYV8_2X8:
-		mt9m111->swap_yuv_y_chromas = 1;
-		mt9m111->swap_yuv_cb_cr = 0;
-		ret = mt9m111_setfmt_yuv(mt9m111);
+		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
 		break;
 	case V4L2_MBUS_FMT_YVYU8_2X8:
-		mt9m111->swap_yuv_y_chromas = 1;
-		mt9m111->swap_yuv_cb_cr = 1;
-		ret = mt9m111_setfmt_yuv(mt9m111);
+		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
+			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
 		break;
 	default:
-		client = v4l2_get_subdevdata(&mt9m111->subdev);
-		dev_err(&client->dev, "Pixel format not handled : %x\n",
-			code);
-		ret = -EINVAL;
+		dev_err(&client->dev, "Pixel format not handled: %x\n", code);
+		return -EINVAL;
 	}
 
+	ret = reg_mask(OUTPUT_FORMAT_CTRL2_A, data_outfmt2,
+		       mask_outfmt2);
+	if (!ret)
+		ret = reg_mask(OUTPUT_FORMAT_CTRL2_B, data_outfmt2,
+			       mask_outfmt2);
+
 	return ret;
 }
 
@@ -969,9 +935,6 @@ static int mt9m111_video_probe(struct soc_camera_device *icd,
 	mt9m111->autoexposure = 1;
 	mt9m111->autowhitebalance = 1;
 
-	mt9m111->swap_rgb_even_odd = 1;
-	mt9m111->swap_rgb_red_blue = 1;
-
 	data = reg_read(CHIP_VERSION);
 
 	switch (data) {