  };
  
  #define RCAR_CSI_80MBPS		0
-@@ -245,6 +246,7 @@ struct rcar_csi2 {
+@@ -245,6 +248,89 @@ struct rcar_csi2 {
  #define RCAR_CSI_1400MBPS	40
  #define RCAR_CSI_1450MBPS	41
  #define RCAR_CSI_1500MBPS	42
 +#define RCAR_CSI_NUMRATES	43
++
++#define RCAR_CSI2_PHxM0(i)		(0xf0 + i * 0x08)
++#define RCAR_CSI2_PHxM1(i)		(0xf4 + i * 0x08)
++#define RCAR_CSI2_PHRM(i)		(0x110 + i * 0x04)
++#define RCAR_CSI2_PHCM(i)		(0x120 + i * 0x04)
++#define RCAR_CSI2_SERCCNT		0x140
++#define RCAR_CSI2_SSERCCNT		0x144
++#define RCAR_CSI2_ECCCM			0x148
++#define RCAR_CSI2_ECECM			0x14c
++#define RCAR_CSI2_CRCECM		0x150
++#define RCAR_CSI2_LCNT(i)		(0x160 + i * 0x04)
++#define RCAR_CSI2_LCNTM(i)		(0x168 + i * 0x04)
++#define RCAR_CSI2_FCNTM			0x170
++#define RCAR_CSI2_FCNTM2		0x174
++#define RCAR_CSI2_VINSM(i)		(0x190 + i * 0x04)
++#define RCAR_CSI2_PHM(i)		(0x1C0 + i * 0x04)
++
++#define RCAR_CSI2_INTSTATE_ALL		0x3FFFFCDD
++
++#ifdef RCAR_CSI2_DUMP
++static void rcar_sci2_debug_show(struct rcar_csi2 *priv)
++{
++	int i;
++	u32 reg0, reg1;
++
++	printk("Debug registers:\n");
++	printk("FCNTM : 0x%08x\n", ioread32(priv->base + RCAR_CSI2_FCNTM));
++	printk("FCNTM2: 0x%08x\n", ioread32(priv->base + RCAR_CSI2_FCNTM2));
++
++	for (i = 0; i < 4; i++) {
++		reg0 = ioread32(priv->base + RCAR_CSI2_PHxM0(i));
++		reg1 = ioread32(priv->base + RCAR_CSI2_PHxM1(i));
++
++		printk("Packet header %d: dt: 0x%02x, vc: %d, wc: %d, cnt: %d\n",
++			i,
++			reg0 & 0x3F, (reg0 >> 6) & 0x03, (reg0 >> 8) & 0xffff,
++			reg1 & 0xffff);
++	}
++	for (i = 0; i < 3; i++) {
++		reg0 = ioread32(priv->base + RCAR_CSI2_PHRM(i));
++
++		printk("Packet header R %d dt: 0x%02x, vc: %d, wc: %d, ecc: 0x%02x\n",
++			i,
++			reg0 & 0x3F, (reg0 >> 6) & 0x03, (reg0 >> 8) & 0xffff,
++			(reg0 >> 24) & 0xff);
++	}
++	for (i = 0; i < 2; i++) {
++		reg0 = ioread32(priv->base + RCAR_CSI2_PHCM(i));
++
++		printk("Packet header C %d: dt: 0x%02x, vc: %d, wc: %d, cal_parity: 0x%02x\n",
++			i,
++			reg0 & 0x3F, (reg0 >> 6) & 0x03, (reg0 >> 8) & 0xffff,
++			(reg0 >> 24) & 0xff);
++	}
++	for (i = 0; i < 8; i++) {
++		reg0 = ioread32(priv->base + RCAR_CSI2_PHM(i));
++
++		printk("Packet header Monitor %d: dt: 0x%02x, vc: %d, wc: %d, ecc: 0x%02x\n",
++			i + 1,
++			reg0 & 0x3F, (reg0 >> 6) & 0x03, (reg0 >> 8) & 0xffff,
++			(reg0 >> 24) & 0xff);
++	}
++	for (i = 0; i < 3; i++)
++		printk("VINSM%d: 0x%08x\n", i, ioread32(priv->base + RCAR_CSI2_VINSM(i)));
++	printk("SERCCNT: %d\n",
++		ioread32(priv->base + RCAR_CSI2_SERCCNT));
++	printk("SSERCCNT: %d\n",
++		ioread32(priv->base + RCAR_CSI2_SSERCCNT));
++	printk("ECCCM: %d\n",
++		ioread32(priv->base + RCAR_CSI2_ECCCM));
++	printk("ECECM: %d\n",
++		ioread32(priv->base + RCAR_CSI2_ECECM));
++	printk("CRCECM: %d\n",
++		ioread32(priv->base + RCAR_CSI2_CRCECM));
++	for (i = 0; i < 2; i++)
++		printk("LCNT%d: 0x%08x\n", i, ioread32(priv->base + RCAR_CSI2_LCNT(i)));
++	for (i = 0; i < 2; i++)
++		printk("LCNTM%d: 0x%08x\n", i, ioread32(priv->base + RCAR_CSI2_LCNTM(i)));
++}
++#else
++#define rcar_sci2_debug_show(args)
++#endif /* RCAR_CSI2_DUMP */
  
  static int rcar_csi2_set_phy_freq(struct rcar_csi2 *priv)
  {
-@@ -259,47 +261,22 @@ static int rcar_csi2_set_phy_freq(struct rcar_csi2 *priv)
+@@ -259,47 +345,22 @@ static int rcar_csi2_set_phy_freq(struct rcar_csi2 *priv)
  		0x0B, 0x1B, 0x2B, 0x3B, 0x0C,  /* 35-39 */
  		0x1C, 0x2C, 0x3C               /* 40-42 */
  	};

 
 	match = of_match_device(of_match_ptr(rcar_vin_of_table), &pdev->dev);
 
@@ -2974,13 +3034,22 @@ static int rcar_vin_probe(struct platform_device *pdev)
 		dev_dbg(&pdev->dev, "node name:%s\n",
 			of_node_full_name(ren->parent));
 

 static int rcar_vin_add_device(struct soc_camera_device *icd)
 {
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
@@ -1328,7 +1364,8 @@ static int rcar_vin_add_device(struct soc_camera_device *icd)
 
 	if (priv->chip == RCAR_H3 || priv->chip == RCAR_M3) {
 		struct v4l2_subdev *csi2_sd = find_csi2(priv);
