3 files changed, 57 insertions, 9 deletions

diff --git a/cpu/ppc4xx/440spe_pcie.c b/cpu/ppc4xx/440spe_pcie.c
index 7b27e87..bf68cc1 100644
--- a/cpu/ppc4xx/440spe_pcie.c
+++ b/cpu/ppc4xx/440spe_pcie.c
@@ -1,5 +1,5 @@
 /*
- * (C) Copyright 2006
+ * (C) Copyright 2006 - 2007
  * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
  *
  * Copyright (c) 2005 Cisco Systems.  All rights reserved.
@@ -40,6 +40,34 @@ enum {
 	LNKW_X8			= 0x8
 };
 
+static inline int pcie_in_8(const volatile unsigned char __iomem *addr)
+{
+	int ret;
+
+	PCIE_IN(lbzx, ret, addr);
+
+	return ret;
+}
+
+static inline int pcie_in_le16(const volatile unsigned short __iomem *addr)
+{
+	int ret;
+
+	PCIE_IN(lhbrx, ret, addr)
+
+	return ret;
+}
+
+static inline unsigned pcie_in_le32(const volatile unsigned __iomem *addr)
+{
+	unsigned ret;
+
+	PCIE_IN(lwbrx, ret, addr);
+
+	return ret;
+}
+
+
 static int pcie_read_config(struct pci_controller *hose, unsigned int devfn,
 	int offset, int len, u32 *val) {
 
@@ -55,13 +83,13 @@ static int pcie_read_config(struct pci_controller *hose, unsigned int devfn,
 
 	switch (len) {
 	case 1:
-		*val = in_8(hose->cfg_data + offset);
+		*val = pcie_in_8(hose->cfg_data + offset);
 		break;
 	case 2:
-		*val = in_le16((u16 *)(hose->cfg_data + offset));
+		*val = pcie_in_le16((u16 *)(hose->cfg_data + offset));
 		break;
 	default:
-		*val = in_le32((u32 *)(hose->cfg_data + offset));
+		*val = pcie_in_le32((u32*)(hose->cfg_data + offset));
 		break;
 	}
 	return 0;
diff --git a/cpu/ppc4xx/440spe_pcie.h b/cpu/ppc4xx/440spe_pcie.h
index 2becc77..eb7cecf 100644
--- a/cpu/ppc4xx/440spe_pcie.h
+++ b/cpu/ppc4xx/440spe_pcie.h
@@ -145,8 +145,8 @@
 #define PECFG_PIMEN		0x33c
 #define PECFG_PIM0LAL		0x340
 #define PECFG_PIM0LAH		0x344
-#define PECFG_PIM1LAL     	0x348
-#define PECFG_PIM1LAH     	0x34c
+#define PECFG_PIM1LAL		0x348
+#define PECFG_PIM1LAH		0x34c
 #define PECFG_PIM01SAL		0x350
 #define PECFG_PIM01SAH		0x354
 
@@ -161,6 +161,21 @@
 	mtdcr(DCRN_SDR0_CFGADDR, offset); \
 	mtdcr(DCRN_SDR0_CFGDATA,data);})
 
+#define PCIE_IN(opcode, ret, addr) \
+	__asm__ __volatile__(			\
+		"sync\n"			\
+		#opcode " %0,0,%1\n"		\
+		"1: twi 0,%0,0\n"		\
+		"isync\n"			\
+		"b 3f\n"			\
+		"2: li %0,-1\n"			\
+		"3:\n"				\
+		".section __ex_table,\"a\"\n"	\
+		".balign 4\n"			\
+		".long 1b,2b\n"			\
+		".previous\n"			\
+		: "=r" (ret) : "r" (addr), "m" (*addr));
+
 int ppc440spe_init_pcie(void);
 int ppc440spe_init_pcie_rootport(int port);
 void yucca_setup_pcie_fpga_rootpoint(int port);
diff --git a/cpu/ppc4xx/traps.c b/cpu/ppc4xx/traps.c
index 2fcce3d..6b15a9e 100755
--- a/cpu/ppc4xx/traps.c
+++ b/cpu/ppc4xx/traps.c
@@ -151,12 +151,17 @@ MachineCheckException(struct pt_regs *regs)
 	int uncorr_ecc = 0;
 #endif
 
-	/* Probing PCI using config cycles cause this exception
-	 * when a device is not present.  Catch it and return to
-	 * the PCI exception handler.
+	/* Probing PCI(E) using config cycles may cause this exception
+	 * when a device is not present. To gracefully recover in such
+	 * scenarios config read/write routines need to be instrumented in
+	 * order to return via fixup handler. For examples refer to
+	 * pcie_in_8(), pcie_in_le16() and pcie_in_le32()
 	 */
 	if ((fixup = search_exception_table(regs->nip)) != 0) {
 		regs->nip = fixup;
+		val = mfspr(MCSR);
+		/* Clear MCSR */
+                mtspr(SPRN_MCSR, val);
 		return;
 	}