diff --git a/bcwc_hw.c b/bcwc_hw.c
index 08156f9..315332f 100644
--- a/bcwc_hw.c
+++ b/bcwc_hw.c
@@ -26,9 +26,25 @@ static int bcwc_hw_set_core_clk(struct bcwc_private *dev_priv)
 
 static int bcwc_hw_s2_pll_reset(struct bcwc_private *dev_priv)
 {
-	BCWC_ISP_REG_WRITE(0x40, DDR_PHY_2C);
+	BCWC_S2_REG_WRITE(0x40, S2_PLL_CTRL_2C);
 	bcwc_hw_pci_post(dev_priv);
 
+	BCWC_S2_REG_WRITE(0x0, S2_PLL_CTRL_2C);
+	bcwc_hw_pci_post(dev_priv);
+
+	BCWC_S2_REG_WRITE(0xBCBC1500, S2_PLL_CTRL_100);
+	bcwc_hw_pci_post(dev_priv);
+
+	BCWC_S2_REG_WRITE(0x0, S2_PLL_CTRL_14);
+	bcwc_hw_pci_post(dev_priv);
+
+	udelay(10000);
+
+	BCWC_S2_REG_WRITE(0x3, S2_PLL_CTRL_14);
+	bcwc_hw_pci_post(dev_priv);
+
+	dev_info(&dev_priv->pdev->dev, "PLL reset finished\n");
+
 	return 0;
 }
 
@@ -36,22 +52,47 @@ static int bcwc_hw_s2_init_pcie_link(struct bcwc_private *dev_priv)
 {
 	u32 reg;
 
-	BCWC_ISP_REG_WRITE(0x10, S2_PCIE_LINK_D000);
+	reg = BCWC_S2_REG_READ(S2_PCIE_LINK_D000);
+	BCWC_S2_REG_WRITE(reg | 0x10, S2_PCIE_LINK_D000);
 	bcwc_hw_pci_post(dev_priv);
 
-	BCWC_ISP_REG_WRITE(0x1804, S2_PCIE_LINK_D120);
+	BCWC_S2_REG_WRITE(0x1804, S2_PCIE_LINK_D120);
 	bcwc_hw_pci_post(dev_priv);
 
-	BCWC_ISP_REG_WRITE(0xac5800, S2_PCIE_LINK_D124);
+	BCWC_S2_REG_WRITE(0xac5800, S2_PCIE_LINK_D124);
 	bcwc_hw_pci_post(dev_priv);
 
-	BCWC_ISP_REG_WRITE(0x1804, S2_PCIE_LINK_D120);
+	BCWC_S2_REG_WRITE(0x1804, S2_PCIE_LINK_D120);
 	bcwc_hw_pci_post(dev_priv);
 
-	reg = BCWC_ISP_REG_READ(S2_PCIE_LINK_D124);
-	if (reg == 0xac5800) {
+	/* Check if PLL is powered down when S2 PCIe link is in L1 state */
+	reg = BCWC_S2_REG_READ(S2_PCIE_LINK_D124);
+	if (reg != 0xac5800) {
+		dev_err(&dev_priv->pdev->dev,
+			"Failed to init S2 PCIe link: %u\n", reg);
+		return -EIO;
 	}
 
+	dev_info(&dev_priv->pdev->dev, "S2 PCIe link init succeded\n");
+
+	BCWC_S2_REG_WRITE(0xf108, S2_PCIE_LINK_D128);
+	bcwc_hw_pci_post(dev_priv);
+
+	BCWC_S2_REG_WRITE(0x8000fc00, S2_PCIE_LINK_D12C);
+	bcwc_hw_pci_post(dev_priv);
+
+	BCWC_S2_REG_WRITE(0xf108, S2_PCIE_LINK_D128);
+	bcwc_hw_pci_post(dev_priv);
+
+	BCWC_S2_REG_WRITE(0x80008610, S2_PCIE_LINK_D12C);
+	bcwc_hw_pci_post(dev_priv);
+
+	BCWC_S2_REG_WRITE(0x1708, S2_PCIE_LINK_D128);
+	bcwc_hw_pci_post(dev_priv);
+
+	BCWC_S2_REG_WRITE(0x800005bf, S2_PCIE_LINK_D12C);
+	bcwc_hw_pci_post(dev_priv);
+
 	return 0;
 }
 
@@ -61,7 +102,7 @@ static int bcwc_hw_s2_pll_init(struct bcwc_private *dev_priv, u32 ddr_speed)
 	u32 reg;
 	int retries = 0;
 
-	ref_clk_25 = (BCWC_ISP_REG_READ(S2_PLL_STATUS_04) && 0x8);
+	ref_clk_25 = BCWC_S2_REG_READ(S2_PLL_STATUS_04) & S2_PLL_REFCLK;
 
 	if (ref_clk_25)
 		dev_info(&dev_priv->pdev->dev, "Refclk: 25MHz\n");
@@ -71,38 +112,38 @@ static int bcwc_hw_s2_pll_init(struct bcwc_private *dev_priv, u32 ddr_speed)
 	if (ddr_speed == 400) {
 		if (ref_clk_25) {
 			/* Ref clk 25 */
-			BCWC_ISP_REG_WRITE(0x00400078, S2_PLL_CTRL_510);
+			BCWC_S2_REG_WRITE(0x00400078, S2_PLL_CTRL_510);
 			bcwc_hw_pci_post(dev_priv);
-			BCWC_ISP_REG_WRITE(0x19280804, S2_PLL_CTRL_24);
+			BCWC_S2_REG_WRITE(0x19280804, S2_PLL_CTRL_24);
 		} else {
 			/* Ref clk 24 */
-			BCWC_ISP_REG_WRITE(0x03200000, S2_PLL_CTRL_20);
+			BCWC_S2_REG_WRITE(0x03200000, S2_PLL_CTRL_20);
 			bcwc_hw_pci_post(dev_priv);
-			BCWC_ISP_REG_WRITE(0x14280603, S2_PLL_CTRL_24);
+			BCWC_S2_REG_WRITE(0x14280603, S2_PLL_CTRL_24);
 		}
 	} else if (ddr_speed == 300) {
 		if (ref_clk_25) {
 			/* Ref clk 25 */
-			BCWC_ISP_REG_WRITE(0x03200000, S2_PLL_CTRL_20);
+			BCWC_S2_REG_WRITE(0x00480078, S2_PLL_CTRL_510);
 			bcwc_hw_pci_post(dev_priv);
-			BCWC_ISP_REG_WRITE(0x14280804, S2_PLL_CTRL_24);
+			BCWC_S2_REG_WRITE(0x19280c06, S2_PLL_CTRL_24);
 		} else {
 			/* Ref clk 24 */
-			BCWC_ISP_REG_WRITE(0x00480078, S2_PLL_CTRL_510);
+			BCWC_S2_REG_WRITE(0x03200000, S2_PLL_CTRL_20);
 			bcwc_hw_pci_post(dev_priv);
-			BCWC_ISP_REG_WRITE(0x19280c06, S2_PLL_CTRL_24);
+			BCWC_S2_REG_WRITE(0x14280804, S2_PLL_CTRL_24);
 		}
 	} else if (ddr_speed == 200) {
 		if (ref_clk_25) {
 			/* Ref clk 25 */
-			BCWC_ISP_REG_WRITE(0x03200000, S2_PLL_CTRL_20);
+			BCWC_S2_REG_WRITE(0x00400078, S2_PLL_CTRL_510);
 			bcwc_hw_pci_post(dev_priv);
-			BCWC_ISP_REG_WRITE(0x14280c06, S2_PLL_CTRL_24);
+			BCWC_S2_REG_WRITE(0x19281008, S2_PLL_CTRL_24);
 		} else {
 			/* Ref clk 24 */
-			BCWC_ISP_REG_WRITE(0x00400078, S2_PLL_CTRL_510);
+			BCWC_S2_REG_WRITE(0x03200000, S2_PLL_CTRL_20);
 			bcwc_hw_pci_post(dev_priv);
-			BCWC_ISP_REG_WRITE(0x19281008, S2_PLL_CTRL_24);
+			BCWC_S2_REG_WRITE(0x14280c06, S2_PLL_CTRL_24);
 		}
 	} else {
 		if (ddr_speed != 450) {
@@ -114,41 +155,44 @@ static int bcwc_hw_s2_pll_init(struct bcwc_private *dev_priv, u32 ddr_speed)
 
 		if (ref_clk_25) {
 			/* Ref clk 25 */
-			BCWC_ISP_REG_WRITE(0x04b00000, S2_PLL_CTRL_20);
+			BCWC_S2_REG_WRITE(0x0048007d, S2_PLL_CTRL_510);
 			bcwc_hw_pci_post(dev_priv);
-			BCWC_ISP_REG_WRITE(0x14280904, S2_PLL_CTRL_24);
+			BCWC_S2_REG_WRITE(0x19280904, S2_PLL_CTRL_24);
 		} else {
 			/* Ref clk 24 */
-			BCWC_ISP_REG_WRITE(0x0048007d, S2_PLL_CTRL_510);
+			BCWC_S2_REG_WRITE(0x04b00000, S2_PLL_CTRL_20);
 			bcwc_hw_pci_post(dev_priv);
-			BCWC_ISP_REG_WRITE(0x19280904, S2_PLL_CTRL_24);
+			BCWC_S2_REG_WRITE(0x14280904, S2_PLL_CTRL_24);
+
 		}
 	}
 
 	bcwc_hw_pci_post(dev_priv);
 	bcwc_hw_s2_pll_reset(dev_priv);
 
-	dev_info(&dev_priv->pdev->dev, "Waiting for PLL to lock\n");
+	dev_info(&dev_priv->pdev->dev, "Waiting for PLL to lock for %d MHz\n",
+		 ddr_speed);
 
 	do {
-		reg = BCWC_ISP_REG_READ(S2_PLL_STATUS_0C) & 0x80;
+		reg = BCWC_S2_REG_READ(S2_PLL_STATUS_0C);
 		udelay(10);
 		retries++;
-	} while (!reg && retries <= 10000);
+	} while ((reg & 0x80) == 0 && retries <= 10000);
 
 	if (retries > 10000) {
-		dev_info(&dev_priv->pdev->dev, "Failed to lock PLL\n");
+		dev_info(&dev_priv->pdev->dev, "Failed to lock PLL: 0x%x\n",
+			 reg);
 		return -EINVAL;
 	} else {
 		dev_info(&dev_priv->pdev->dev, "PLL is locked\n");
 	}
 
-	reg = BCWC_ISP_REG_READ(S2_PLL_STATUS_A8);
-	BCWC_ISP_REG_WRITE(reg | 0x1, S2_PLL_STATUS_A8);
+	reg = BCWC_S2_REG_READ(S2_PLL_STATUS_A8);
+	BCWC_S2_REG_WRITE(reg | S2_PLL_BYPASS, S2_PLL_STATUS_A8);
 	bcwc_hw_pci_post(dev_priv);
 	udelay(10000);
 
-	reg = BCWC_ISP_REG_READ(S2_PLL_STATUS_A8);
+	reg = BCWC_S2_REG_READ(S2_PLL_STATUS_A8);
 	if (reg & 0x1) {
 		dev_info(&dev_priv->pdev->dev, "PLL is in bypass mode\n");
 	} else {
@@ -158,6 +202,29 @@ static int bcwc_hw_s2_pll_init(struct bcwc_private *dev_priv, u32 ddr_speed)
 	return 0;
 }
 
+static int bcwc_hw_s2_preinit_ddr_controller_soc(struct bcwc_private *dev_priv)
+{
+	/* Wingardium leviosa */
+	BCWC_S2_REG_WRITE(0x203, S2_DDR_REG_1100);
+	bcwc_hw_pci_post(dev_priv);
+	BCWC_S2_REG_WRITE(0x203, S2_DDR_REG_1104);
+	bcwc_hw_pci_post(dev_priv);
+	BCWC_S2_REG_WRITE(0x203, S2_DDR_REG_1108);
+	bcwc_hw_pci_post(dev_priv);
+	BCWC_S2_REG_WRITE(0x203, S2_DDR_REG_110c);
+	bcwc_hw_pci_post(dev_priv);
+	BCWC_S2_REG_WRITE(0x203, S2_DDR_REG_1110);
+	bcwc_hw_pci_post(dev_priv);
+	BCWC_S2_REG_WRITE(0x203, S2_DDR_REG_1114);
+	bcwc_hw_pci_post(dev_priv);
+	BCWC_S2_REG_WRITE(0x203, S2_DDR_REG_1118);
+	bcwc_hw_pci_post(dev_priv);
+	BCWC_S2_REG_WRITE(0x203, S2_DDR_REG_111C);
+	bcwc_hw_pci_post(dev_priv);
+
+	return 0;
+}
+
 static int bcwc_hw_s2_init_ddr_controller_soc(struct bcwc_private *dev_priv)
 {
 	u32 cmd;
@@ -166,7 +233,7 @@ static int bcwc_hw_s2_init_ddr_controller_soc(struct bcwc_private *dev_priv)
 
 	/* Read PCI config command register */
 	ret = pci_read_config_dword(dev_priv->pdev, 4, &cmd);
-	if (!ret) {
+	if (ret) {
 		dev_err(&dev_priv->pdev->dev, "Failed to read PCI config\n");
 		return -EIO;
 	}
@@ -177,20 +244,19 @@ static int bcwc_hw_s2_init_ddr_controller_soc(struct bcwc_private *dev_priv)
 		return -EIO;
 	}
 
-	reg = BCWC_ISP_REG_READ(DDR_PHY_9C);
+	reg = BCWC_S2_REG_READ(S2_PLL_CTRL_9C);
 	reg &= 0xfffffcff;
 
-	BCWC_ISP_REG_WRITE(reg, DDR_PHY_9C);
+	BCWC_S2_REG_WRITE(reg, S2_PLL_CTRL_9C);
 	bcwc_hw_pci_post(dev_priv);
 
-	BCWC_ISP_REG_WRITE(reg | 0x300, DDR_PHY_9C);
+	BCWC_S2_REG_WRITE(reg | 0x300, S2_PLL_CTRL_9C);
 	bcwc_hw_pci_post(dev_priv);
 
-	/*
-	 *  FIXME: Need to find out the correct DDR speed.
-	 * Just using 200 MHz for now
-	 */
-	bcwc_hw_s2_pll_init(dev_priv, 200);
+	/* Default to 450 MHz DDR speed for now */
+	bcwc_hw_s2_pll_init(dev_priv, 450);
+
+	/* DDR PHY soft reset */
 
 	/* FIXME: Unfinished */
 
@@ -361,7 +427,17 @@ static int bcwc_hw_power_off(struct bcwc_private *dev_priv)
 
 int bcwc_hw_init(struct bcwc_private *dev_priv)
 {
+	int ret;
+
+	ret = bcwc_hw_s2_init_pcie_link(dev_priv);
+	if (ret)
+		goto out;
+
+	/* Set DDR speed to 450 MHz */
+	bcwc_hw_s2_preinit_ddr_controller_soc(dev_priv);
+	bcwc_hw_s2_init_ddr_controller_soc(dev_priv);
 	// bcwc_hw_isp_init(dev_priv);
 
-	return 0;
+out:
+	return ret;
 }
diff --git a/bcwc_reg.h b/bcwc_reg.h
index 3d6df48..26d25ed 100644
--- a/bcwc_reg.h
+++ b/bcwc_reg.h
@@ -13,29 +13,44 @@
 #ifndef _BCWC_REG_H
 #define _BCWC_REG_H
 
-/* On iomem with pointer at 0x0fd0 */
-#define DDR_PHY_2C	0x2c
-#define DDR_PHY_9C	0x9c
+/* S2 IO reg */
 
 #define S2_PCIE_LINK_D000	0xd000
 #define S2_PCIE_LINK_D120	0xd120
 #define S2_PCIE_LINK_D124	0xd124
+#define S2_PCIE_LINK_D128	0xd128
+#define S2_PCIE_LINK_D12C	0xd12c
+
+/* These are written to 0x203 before DDR soc init */
+#define S2_DDR_REG_1100		0x1100
+#define S2_DDR_REG_1104		0x1104
+#define S2_DDR_REG_1108		0x1108
+#define S2_DDR_REG_110c		0x110c
+#define S2_DDR_REG_1110		0x1110
+#define S2_DDR_REG_1114		0x1114
+#define S2_DDR_REG_1118		0x1118
+#define S2_DDR_REG_111C		0x111c
 
 #define DDR_PHY_REG_BASE	0x2800
 #define DDR_PHY_NUM_REGS	127 /* Found in AppleCamIn::Start() */
 
 /* On iomem with pointer at ...fill me in... */
 #define S2_PLL_STATUS_04	0x04
-#define S2_PLL_STATUS_REFCLK	(1 << 3) /* 1 = 25MHz, 0 = 24MHz */
+#define S2_PLL_REFCLK		(1 << 3) /* 1 = 25MHz, 0 = 24MHz */
 
 #define S2_PLL_STATUS_0C	0x0c	/* Register is called CMU_R_PLL_STS_MEMADDR */
 #define S2_PLL_STATUS_LOCKED	(1 << 7) /* 1 = PLL locked, 0 = PLL not locked */
 
-#define S2_PLL_STATUS_A8	0xa8	/* Bit 0 is PLL bypass mode (1 = bypass, 0 = non-bypass mode */
+#define S2_PLL_STATUS_A8	0xa8
+#define S2_PLL_BYPASS		(1 << 0) /* 1 = bypass, 0 = non-bypass */
 
-#define S2_PLL_CTRL_20		0x20	/* S2 PLL CLK REG 1 */
-#define S2_PLL_CTRL_24		0x24	/* S2 PLL CLK REG 2 */
-#define S2_PLL_CTRL_510		0x510	/* S2 PLL CLK REG 4 */
+#define S2_PLL_CTRL_14		0x0014
+#define S2_PLL_CTRL_20		0x0020
+#define S2_PLL_CTRL_24		0x0024
+#define S2_PLL_CTRL_2C		0x002c
+#define S2_PLL_CTRL_9C		0x009c
+#define S2_PLL_CTRL_100		0x0100
+#define S2_PLL_CTRL_510		0x0510
 
 /* On iomem with pointer at 0x0ff0 (Bar 4: 1MB) */
 #define IRQ_IPC_NUM_CHAN	0xc3000