2 files changed, 48 insertions, 15 deletions

diff --git a/arch/blackfin/kernel/cplb-mpu/cplbinit.c b/arch/blackfin/kernel/cplb-mpu/cplbinit.c
index 8d42b9e50dfa..30fd6417f069 100644
--- a/arch/blackfin/kernel/cplb-mpu/cplbinit.c
+++ b/arch/blackfin/kernel/cplb-mpu/cplbinit.c
@@ -64,6 +64,15 @@ void __init generate_cplb_tables_cpu(unsigned int cpu)
 		icplb_tbl[cpu][i_i++].data = i_data | (addr == 0 ? CPLB_USER_RD : 0);
 	}
 
+#ifdef CONFIG_ROMKERNEL
+	/* Cover kernel XIP flash area */
+	addr = CONFIG_ROM_BASE & ~(4 * 1024 * 1024 - 1);
+	dcplb_tbl[cpu][i_d].addr = addr;
+	dcplb_tbl[cpu][i_d++].data = d_data | CPLB_USER_RD;
+	icplb_tbl[cpu][i_i].addr = addr;
+	icplb_tbl[cpu][i_i++].data = i_data | CPLB_USER_RD;
+#endif
+
 	/* Cover L1 memory.  One 4M area for code and data each is enough.  */
 #if L1_DATA_A_LENGTH > 0 || L1_DATA_B_LENGTH > 0
 	dcplb_tbl[cpu][i_d].addr = get_l1_data_a_start_cpu(cpu);
diff --git a/arch/blackfin/kernel/cplb-mpu/cplbmgr.c b/arch/blackfin/kernel/cplb-mpu/cplbmgr.c
index 930c01c06813..87b25b1b30ed 100644
--- a/arch/blackfin/kernel/cplb-mpu/cplbmgr.c
+++ b/arch/blackfin/kernel/cplb-mpu/cplbmgr.c
@@ -31,6 +31,12 @@ int nr_dcplb_miss[NR_CPUS], nr_icplb_miss[NR_CPUS];
 int nr_icplb_supv_miss[NR_CPUS], nr_dcplb_prot[NR_CPUS];
 int nr_cplb_flush[NR_CPUS];
 
+#ifdef CONFIG_EXCPT_IRQ_SYSC_L1
+#define MGR_ATTR __attribute__((l1_text))
+#else
+#define MGR_ATTR
+#endif
+
 /*
  * Given the contents of the status register, return the index of the
  * CPLB that caused the fault.
@@ -59,7 +65,7 @@ static int icplb_rr_index[NR_CPUS], dcplb_rr_index[NR_CPUS];
 /*
  * Find an ICPLB entry to be evicted and return its index.
  */
-static int evict_one_icplb(unsigned int cpu)
+MGR_ATTR static int evict_one_icplb(unsigned int cpu)
 {
 	int i;
 	for (i = first_switched_icplb; i < MAX_CPLBS; i++)
@@ -74,7 +80,7 @@ static int evict_one_icplb(unsigned int cpu)
 	return i;
 }
 
-static int evict_one_dcplb(unsigned int cpu)
+MGR_ATTR static int evict_one_dcplb(unsigned int cpu)
 {
 	int i;
 	for (i = first_switched_dcplb; i < MAX_CPLBS; i++)
@@ -89,7 +95,7 @@ static int evict_one_dcplb(unsigned int cpu)
 	return i;
 }
 
-static noinline int dcplb_miss(unsigned int cpu)
+MGR_ATTR static noinline int dcplb_miss(unsigned int cpu)
 {
 	unsigned long addr = bfin_read_DCPLB_FAULT_ADDR();
 	int status = bfin_read_DCPLB_STATUS();
@@ -114,10 +120,15 @@ static noinline int dcplb_miss(unsigned int cpu)
 		d_data = L2_DMEMORY;
 	} else if (addr >= physical_mem_end) {
 		if (addr >= ASYNC_BANK0_BASE && addr < ASYNC_BANK3_BASE + ASYNC_BANK3_SIZE) {
-			addr &= ~(4 * 1024 * 1024 - 1);
-			d_data &= ~PAGE_SIZE_4KB;
-			d_data |= PAGE_SIZE_4MB;
-			d_data |= CPLB_USER_RD | CPLB_USER_WR;
+			mask = current_rwx_mask[cpu];
+			if (mask) {
+				int page = (addr - (ASYNC_BANK0_BASE - _ramend)) >> PAGE_SHIFT;
+				int idx = page >> 5;
+				int bit = 1 << (page & 31);
+
+				if (mask[idx] & bit)
+					d_data |= CPLB_USER_RD;
+			}
 		} else if (addr >= BOOT_ROM_START && addr < BOOT_ROM_START + BOOT_ROM_LENGTH
 		    && (status & (FAULT_RW | FAULT_USERSUPV)) == FAULT_USERSUPV) {
 			addr &= ~(1 * 1024 * 1024 - 1);
@@ -126,7 +137,9 @@ static noinline int dcplb_miss(unsigned int cpu)
 		} else
 			return CPLB_PROT_VIOL;
 	} else if (addr >= _ramend) {
-	    d_data |= CPLB_USER_RD | CPLB_USER_WR;
+		d_data |= CPLB_USER_RD | CPLB_USER_WR;
+		if (reserved_mem_dcache_on)
+			d_data |= CPLB_L1_CHBL;
 	} else {
 		mask = current_rwx_mask[cpu];
 		if (mask) {
@@ -156,7 +169,7 @@ static noinline int dcplb_miss(unsigned int cpu)
 	return 0;
 }
 
-static noinline int icplb_miss(unsigned int cpu)
+MGR_ATTR static noinline int icplb_miss(unsigned int cpu)
 {
 	unsigned long addr = bfin_read_ICPLB_FAULT_ADDR();
 	int status = bfin_read_ICPLB_STATUS();
@@ -204,10 +217,19 @@ static noinline int icplb_miss(unsigned int cpu)
 		i_data = L2_IMEMORY;
 	} else if (addr >= physical_mem_end) {
 		if (addr >= ASYNC_BANK0_BASE && addr < ASYNC_BANK3_BASE + ASYNC_BANK3_SIZE) {
-			addr &= ~(4 * 1024 * 1024 - 1);
-			i_data &= ~PAGE_SIZE_4KB;
-			i_data |= PAGE_SIZE_4MB;
-			i_data |= CPLB_USER_RD;
+			if (!(status & FAULT_USERSUPV)) {
+				unsigned long *mask = current_rwx_mask[cpu];
+
+				if (mask) {
+					int page = (addr - (ASYNC_BANK0_BASE - _ramend)) >> PAGE_SHIFT;
+					int idx = page >> 5;
+					int bit = 1 << (page & 31);
+
+					mask += 2 * page_mask_nelts;
+					if (mask[idx] & bit)
+						i_data |= CPLB_USER_RD;
+				}
+			}
 		} else if (addr >= BOOT_ROM_START && addr < BOOT_ROM_START + BOOT_ROM_LENGTH
 		    && (status & FAULT_USERSUPV)) {
 			addr &= ~(1 * 1024 * 1024 - 1);
@@ -217,6 +239,8 @@ static noinline int icplb_miss(unsigned int cpu)
 		    return CPLB_PROT_VIOL;
 	} else if (addr >= _ramend) {
 		i_data |= CPLB_USER_RD;
+		if (reserved_mem_icache_on)
+			i_data |= CPLB_L1_CHBL;
 	} else {
 		/*
 		 * Two cases to distinguish - a supervisor access must
@@ -251,7 +275,7 @@ static noinline int icplb_miss(unsigned int cpu)
 	return 0;
 }
 
-static noinline int dcplb_protection_fault(unsigned int cpu)
+MGR_ATTR static noinline int dcplb_protection_fault(unsigned int cpu)
 {
 	int status = bfin_read_DCPLB_STATUS();
 
@@ -271,7 +295,7 @@ static noinline int dcplb_protection_fault(unsigned int cpu)
 	return CPLB_PROT_VIOL;
 }
 
-int cplb_hdr(int seqstat, struct pt_regs *regs)
+MGR_ATTR int cplb_hdr(int seqstat, struct pt_regs *regs)
 {
 	int cause = seqstat & 0x3f;
 	unsigned int cpu = raw_smp_processor_id();