PCI: rcar: Finish transition to L1 state in rcar_pcie_config_access()

index 38b6e02edfa9..f0a0d560fefc 100644
@@ -41,6 +41,15 @@ struct rcar_msi {
 	int irq2;
 };
 
+/* Structure representing the PCIe interface */
+struct rcar_pcie_host {
+	struct rcar_pcie	pcie;
+	struct phy		*phy;
+	struct clk		*bus_clk;
+	struct			rcar_msi msi;
+	int			(*phy_init_fn)(struct rcar_pcie_host *host);
+};
+
 #ifdef CONFIG_ARM
 /*
  * Here we keep a static copy of the remapped PCIe controller address.

@@ -56,14 +65,34 @@ static void __iomem *pcie_base;
 static struct device *pcie_dev;
 #endif
 
-/* Structure representing the PCIe interface */
-struct rcar_pcie_host {
-	struct rcar_pcie	pcie;
-	struct phy		*phy;
-	struct clk		*bus_clk;
-	struct			rcar_msi msi;
-	int			(*phy_init_fn)(struct rcar_pcie_host *host);
-};
+static DEFINE_SPINLOCK(pmsr_lock);
+
+static int rcar_pcie_wakeup(struct device *pcie_dev, void __iomem *pcie_base)
+{
+	u32 pmsr, val;
+	int ret = 0;
+
+	if (!pcie_base || pm_runtime_suspended(pcie_dev))
+		return 1;
+
+	pmsr = readl(pcie_base + PMSR);
+
+	/*
+	 * Test if the PCIe controller received PM_ENTER_L1 DLLP and
+	 * the PCIe controller is not in L1 link state. If true, apply
+	 * fix, which will put the controller into L1 link state, from
+	 * which it can return to L0s/L0 on its own.
+	 */
+	if ((pmsr & PMEL1RX) && ((pmsr & PMSTATE) != PMSTATE_L1)) {
+		writel(L1IATN, pcie_base + PMCTLR);
+		ret = readl_poll_timeout_atomic(pcie_base + PMSR, val,
+						val & L1FAEG, 10, 1000);
+		WARN(ret, "Timeout waiting for L1 link state, ret=%d\n", ret);
+		writel(L1FAEG | PMEL1RX, pcie_base + PMSR);
+	}
+
+	return ret;
+}
 
 static struct rcar_pcie_host *msi_to_host(struct rcar_msi *msi)
 {

@@ -85,6 +114,15 @@ static int rcar_pcie_config_access(struct rcar_pcie_host *host,
 {
 	struct rcar_pcie *pcie = &host->pcie;
 	unsigned int dev, func, reg, index;
+	unsigned long flags;
+	int ret;
+
+	/* Wake the bus up in case it is in L1 state. */
+	spin_lock_irqsave(&pmsr_lock, flags);
+	ret = rcar_pcie_wakeup(pcie->dev, pcie->base);
+	spin_unlock_irqrestore(&pmsr_lock, flags);
+	if (ret)
+		return ret;
 
 	dev = PCI_SLOT(devfn);
 	func = PCI_FUNC(devfn);

@@ -1050,36 +1088,18 @@ static struct platform_driver rcar_pcie_driver = {
 };
 
 #ifdef CONFIG_ARM
-static DEFINE_SPINLOCK(pmsr_lock);
 static int rcar_pcie_aarch32_abort_handler(unsigned long addr,
 		unsigned int fsr, struct pt_regs *regs)
 {
 	unsigned long flags;
-	u32 pmsr, val;
 	int ret = 0;
 
 	spin_lock_irqsave(&pmsr_lock, flags);
 
-	if (!pcie_base || pm_runtime_suspended(pcie_dev)) {
-		ret = 1;
+	ret = rcar_pcie_wakeup(pcie_dev, pcie_base);
+	spin_unlock_irqrestore(&pmsr_lock, flags);
+	if (ret)
 		goto unlock_exit;
-	}
-
-	pmsr = readl(pcie_base + PMSR);
-
-	/*
-	 * Test if the PCIe controller received PM_ENTER_L1 DLLP and
-	 * the PCIe controller is not in L1 link state. If true, apply
-	 * fix, which will put the controller into L1 link state, from
-	 * which it can return to L0s/L0 on its own.
-	 */
-	if ((pmsr & PMEL1RX) && ((pmsr & PMSTATE) != PMSTATE_L1)) {
-		writel(L1IATN, pcie_base + PMCTLR);
-		ret = readl_poll_timeout_atomic(pcie_base + PMSR, val,
-						val & L1FAEG, 10, 1000);
-		WARN(ret, "Timeout waiting for L1 link state, ret=%d\n", ret);
-		writel(L1FAEG | PMEL1RX, pcie_base + PMSR);
-	}
 
 unlock_exit:
 	spin_unlock_irqrestore(&pmsr_lock, flags);

index f0a0d560fefc..875dd5d417ee 100644
@@ -107,6 +107,35 @@ static u32 rcar_read_conf(struct rcar_pcie *pcie, int where)
 	return val >> shift;
 }
 
+void rcar_pci_write_reg_workaround(struct rcar_pcie *pcie, u32 val, unsigned int reg)
+{
+#ifdef CONFIG_ARM
+	asm volatile(
+		"	str %0, [%1]\n"
+		"	isb\n"
+	::"r"(val), "r"(pcie->base + reg):"memory");
+#else
+	rcar_pci_write_reg(pcie, val, reg);
+#endif
+}
+
+u32 rcar_pci_read_reg_workaround(struct rcar_pcie *pcie, unsigned int reg)
+{
+#ifdef CONFIG_ARM
+	u32 val;
+
+	asm volatile(
+		"rcar_pci_read_reg_workaround_start:\n"
+		"1:	ldr %0, [%1]\n"
+		"	isb\n"
+	: "=r"(val):"r"(pcie->base + reg):"memory");
+
+	return val;
+#else
+	return rcar_pci_read_reg(pcie, reg);
+#endif
+}
+
 /* Serialization is provided by 'pci_lock' in drivers/pci/access.c */
 static int rcar_pcie_config_access(struct rcar_pcie_host *host,
 		unsigned char access_type, struct pci_bus *bus,

@@ -179,9 +208,9 @@ static int rcar_pcie_config_access(struct rcar_pcie_host *host,
 		return PCIBIOS_DEVICE_NOT_FOUND;
 
 	if (access_type == RCAR_PCI_ACCESS_READ)
-		*data = rcar_pci_read_reg(pcie, PCIECDR);
+		*data = rcar_pci_read_reg_workaround(pcie, PCIECDR);
 	else
-		rcar_pci_write_reg(pcie, *data, PCIECDR);
+		rcar_pci_write_reg_workaround(pcie, *data, PCIECDR);
 
 	/* Disable the configuration access */
 	rcar_pci_write_reg(pcie, 0, PCIECCTLR);

@@ -1091,7 +1120,11 @@ static struct platform_driver rcar_pcie_driver = {
 static int rcar_pcie_aarch32_abort_handler(unsigned long addr,
 		unsigned int fsr, struct pt_regs *regs)
 {
+	extern u32 *rcar_pci_read_reg_workaround_start;
+	unsigned long pc = instruction_pointer(regs);
+	unsigned long instr = *(unsigned long *)pc;
 	unsigned long flags;
+	u32 reg, val;
 	int ret = 0;
 
 	spin_lock_irqsave(&pmsr_lock, flags);

@@ -1101,6 +1134,36 @@ static int rcar_pcie_aarch32_abort_handler(unsigned long addr,
 	if (ret)
 		goto unlock_exit;
 
+	/*
+	 * Test whether the faulting instruction is 'isb' and if
+	 * so, test whether it is the 'isb' instruction within
+	 * rcar_pci_read_reg_workaround() asm volatile()
+	 * implementation of read access. If it is, fix it up.
+	 */
+	instr &= ~0xf;
+	if ((instr == 0xf57ff060 || instr == 0xf3bf8f60) &&
+	    (pc == (u32)&rcar_pci_read_reg_workaround_start + 4)) {
+		/*
+		 * If the instruction being executed was a read,
+		 * make it look like it read all-ones.
+		 */
+		instr = *(unsigned long *)(pc - 4);
+		reg = (instr >> 12) & 15;
+
+		if ((instr & 0x0c100000) == 0x04100000) {
+			if (instr & 0x00400000)
+				val = 255;
+			else
+				val = -1;
+
+			regs->uregs[reg] = val;
+			regs->ARM_pc += 4;
+		} else if ((instr & 0x0e100090) == 0x00100090) {
+			regs->uregs[reg] = -1;
+			regs->ARM_pc += 4;
+		}
+	}
+
 unlock_exit:
 	spin_unlock_irqrestore(&pmsr_lock, flags);
 	return ret;