media: rp1: Add lane-polarity support to CFE driver

The PHY supports swapping the pairs within a lane, so expose this.

Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.com>

Author: 6by9

drivers/media/platform/raspberrypi/rp1_cfe/cfe.c

@@ -2206,12 +2206,18 @@ static int of_cfe_connect_subdevs(struct cfe_device *cfe)
 		return -EINVAL;
 	}
 
+	cfe->csi2.dphy.lane_polarities[0] =
+					ep.bus.mipi_csi2.lane_polarities[0];
+
 	for (lane = 0; lane < ep.bus.mipi_csi2.num_data_lanes; lane++) {
 		if (ep.bus.mipi_csi2.data_lanes[lane] != lane + 1) {
 			cfe_err("subdevice %pOF: data lanes reordering not supported\n",
 				sensor_node);
 			goto cleanup_exit;
 		}
+
+		cfe->csi2.dphy.lane_polarities[lane + 1] =
+					ep.bus.mipi_csi2.lane_polarities[lane + 1];
 	}
 
 	cfe->csi2.dphy.max_lanes = ep.bus.mipi_csi2.num_data_lanes;

drivers/media/platform/raspberrypi/rp1_cfe/dphy.c

@@ -34,6 +34,11 @@
 #define DPHY_PLL_INPUT_DIV_OFFSET	0x17
 #define DPHY_PLL_LOOP_DIV_OFFSET	0x18
 #define DPHY_PLL_DIV_CTRL_OFFSET	0x19
+#define DPHY_CLK_PN_SWAP		0x35
+#define DPHY_D0_PN_SWAP			0x45
+#define DPHY_D1_PN_SWAP			0x55
+#define DPHY_D2_PN_SWAP			0x85
+#define DPHY_D3_PN_SWAP			0x95
 
 static u32 dw_csi2_host_read(struct dphy_data *dphy, u32 offset)
 {
@@ -141,6 +146,12 @@ static void dphy_init(struct dphy_data *dphy)
 
 	dphy_set_hsfreqrange(dphy, dphy->dphy_rate);
 
+	dphy_transaction(dphy, DPHY_CLK_PN_SWAP, dphy->lane_polarities[0]);
+	dphy_transaction(dphy, DPHY_D0_PN_SWAP, dphy->lane_polarities[1]);
+	dphy_transaction(dphy, DPHY_D1_PN_SWAP, dphy->lane_polarities[2]);
+	dphy_transaction(dphy, DPHY_D2_PN_SWAP, dphy->lane_polarities[3]);
+	dphy_transaction(dphy, DPHY_D3_PN_SWAP, dphy->lane_polarities[4]);
+
 	usleep_range(5, 10);
 	dw_csi2_host_write(dphy, PHY_SHUTDOWNZ, 1);
 	usleep_range(5, 10);

drivers/media/platform/raspberrypi/rp1_cfe/dphy.h

@@ -9,6 +9,7 @@
 
 #include <linux/io.h>
 #include <linux/types.h>
+#include <media/v4l2-mediabus.h>
 
 struct dphy_data {
 	struct device *dev;
@@ -18,6 +19,7 @@ struct dphy_data {
 	u32 dphy_rate;
 	u32 max_lanes;
 	u32 active_lanes;
+	bool lane_polarities[1 + V4L2_MBUS_CSI2_MAX_DATA_LANES];
 };
 
 void dphy_probe(struct dphy_data *dphy);