1 files changed, 39 insertions, 15 deletions

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();