13 files changed, 597 insertions, 162 deletions

diff --git a/arch/blackfin/kernel/Makefile b/arch/blackfin/kernel/Makefile
index 6140cd69c782..606adc78aa85 100644
--- a/arch/blackfin/kernel/Makefile
+++ b/arch/blackfin/kernel/Makefile
@@ -18,6 +18,5 @@ endif
 obj-$(CONFIG_BFIN_GPTIMERS)          += gptimers.o
 obj-$(CONFIG_MODULES)                += module.o
 obj-$(CONFIG_BFIN_DMA_5XX)           += bfin_dma_5xx.o
-obj-$(CONFIG_DUAL_CORE_TEST_MODULE)  += dualcore_test.o
 obj-$(CONFIG_KGDB)                   += kgdb.o
 obj-$(CONFIG_EARLY_PRINTK)           += early_printk.o
diff --git a/arch/blackfin/kernel/bfin_dma_5xx.c b/arch/blackfin/kernel/bfin_dma_5xx.c
index d54f19085f37..93229b3d6e3e 100644
--- a/arch/blackfin/kernel/bfin_dma_5xx.c
+++ b/arch/blackfin/kernel/bfin_dma_5xx.c
@@ -472,6 +472,40 @@ unsigned long get_dma_curr_addr(unsigned int channel)
 }
 EXPORT_SYMBOL(get_dma_curr_addr);
 
+#ifdef CONFIG_PM
+int blackfin_dma_suspend(void)
+{
+        int i;
+
+#ifdef CONFIG_BF561     /* IMDMA channels doesn't have a PERIPHERAL_MAP */
+        for (i = 0; i <= CH_MEM_STREAM3_SRC; i++) {
+#else
+        for (i = 0; i < MAX_BLACKFIN_DMA_CHANNEL; i++) {
+#endif
+                if (dma_ch[i].chan_status == DMA_CHANNEL_ENABLED) {
+                        printk(KERN_ERR "DMA Channel %d failed to suspend\n", i);
+                        return -EBUSY;
+                }
+
+                dma_ch[i].saved_peripheral_map = dma_ch[i].regs->peripheral_map;
+        }
+
+        return 0;
+}
+
+void blackfin_dma_resume(void)
+{
+        int i;
+
+#ifdef CONFIG_BF561     /* IMDMA channels doesn't have a PERIPHERAL_MAP */
+        for (i = 0; i <= CH_MEM_STREAM3_SRC; i++)
+#else
+        for (i = 0; i < MAX_BLACKFIN_DMA_CHANNEL; i++)
+#endif
+                dma_ch[i].regs->peripheral_map = dma_ch[i].saved_peripheral_map;
+}
+#endif
+
 static void *__dma_memcpy(void *dest, const void *src, size_t size)
 {
         int direction;  /* 1 - address decrease, 0 - address increase */
diff --git a/arch/blackfin/kernel/bfin_gpio.c b/arch/blackfin/kernel/bfin_gpio.c
index b6d89d1644cc..ecbd141e0ef2 100644
--- a/arch/blackfin/kernel/bfin_gpio.c
+++ b/arch/blackfin/kernel/bfin_gpio.c
@@ -186,7 +186,10 @@ static struct str_ident {
         char name[RESOURCE_LABEL_SIZE];
 } str_ident[MAX_RESOURCES];
 
-#if defined(CONFIG_PM) && !defined(CONFIG_BF54x)
+#if defined(CONFIG_PM)
+#if defined(CONFIG_BF54x)
+static struct gpio_port_s gpio_bank_saved[gpio_bank(MAX_BLACKFIN_GPIOS)];
+#else
 static unsigned short wakeup_map[gpio_bank(MAX_BLACKFIN_GPIOS)];
 static unsigned char wakeup_flags_map[MAX_BLACKFIN_GPIOS];
 static struct gpio_port_s gpio_bank_saved[gpio_bank(MAX_BLACKFIN_GPIOS)];
@@ -206,7 +209,7 @@ static unsigned int sic_iwr_irqs[gpio_bank(MAX_BLACKFIN_GPIOS)] = {IRQ_PORTF_INT
 #ifdef BF561_FAMILY
 static unsigned int sic_iwr_irqs[gpio_bank(MAX_BLACKFIN_GPIOS)] = {IRQ_PROG0_INTB, IRQ_PROG1_INTB, IRQ_PROG2_INTB};
 #endif
-
+#endif
 #endif /* CONFIG_PM */
 
 #if defined(BF548_FAMILY)
@@ -667,7 +670,7 @@ static int bfin_gpio_wakeup_type(unsigned gpio, unsigned char type)
         return 0;
 }
 
-u32 bfin_pm_setup(void)
+u32 bfin_pm_standby_setup(void)
 {
         u16 bank, mask, i, gpio;
 
@@ -679,7 +682,7 @@ u32 bfin_pm_setup(void)
                 gpio_bankb[bank]->maskb = 0;
 
                 if (mask) {
-#ifdef BF537_FAMILY
+#if defined(BF527_FAMILY) || defined(BF537_FAMILY)
                         gpio_bank_saved[bank].fer   = *port_fer[bank];
 #endif
                         gpio_bank_saved[bank].inen  = gpio_bankb[bank]->inen;
@@ -715,7 +718,7 @@ u32 bfin_pm_setup(void)
         return 0;
 }
 
-void bfin_pm_restore(void)
+void bfin_pm_standby_restore(void)
 {
         u16 bank, mask, i;
 
@@ -724,7 +727,7 @@ void bfin_pm_restore(void)
                 bank = gpio_bank(i);
 
                 if (mask) {
-#ifdef BF537_FAMILY
+#if defined(BF527_FAMILY) || defined(BF537_FAMILY)
                         *port_fer[bank]         = gpio_bank_saved[bank].fer;
 #endif
                         gpio_bankb[bank]->inen  = gpio_bank_saved[bank].inen;
@@ -743,8 +746,111 @@ void bfin_pm_restore(void)
         AWA_DUMMY_READ(maskb);
 }
 
+void bfin_gpio_pm_hibernate_suspend(void)
+{
+        int i, bank;
+
+        for (i = 0; i < MAX_BLACKFIN_GPIOS; i += GPIO_BANKSIZE) {
+                bank = gpio_bank(i);
+
+#if defined(BF527_FAMILY) || defined(BF537_FAMILY)
+                        gpio_bank_saved[bank].fer   = *port_fer[bank];
+#ifdef BF527_FAMILY
+                        gpio_bank_saved[bank].mux   = *port_mux[bank];
+#else
+                        if (bank == 0)
+                                gpio_bank_saved[bank].mux   = bfin_read_PORT_MUX();
+#endif
+#endif
+                        gpio_bank_saved[bank].data  = gpio_bankb[bank]->data;
+                        gpio_bank_saved[bank].inen  = gpio_bankb[bank]->inen;
+                        gpio_bank_saved[bank].polar = gpio_bankb[bank]->polar;
+                        gpio_bank_saved[bank].dir   = gpio_bankb[bank]->dir;
+                        gpio_bank_saved[bank].edge  = gpio_bankb[bank]->edge;
+                        gpio_bank_saved[bank].both  = gpio_bankb[bank]->both;
+                        gpio_bank_saved[bank].maska  = gpio_bankb[bank]->maska;
+        }
+
+        AWA_DUMMY_READ(maska);
+}
+
+void bfin_gpio_pm_hibernate_restore(void)
+{
+        int i, bank;
+
+        for (i = 0; i < MAX_BLACKFIN_GPIOS; i += GPIO_BANKSIZE) {
+                        bank = gpio_bank(i);
+
+#if defined(BF527_FAMILY) || defined(BF537_FAMILY)
+#ifdef BF527_FAMILY
+                        *port_mux[bank] = gpio_bank_saved[bank].mux;
+#else
+                        if (bank == 0)
+                                bfin_write_PORT_MUX(gpio_bank_saved[bank].mux);
+#endif
+                        *port_fer[bank]         = gpio_bank_saved[bank].fer;
+#endif
+                        gpio_bankb[bank]->inen  = gpio_bank_saved[bank].inen;
+                        gpio_bankb[bank]->dir   = gpio_bank_saved[bank].dir;
+                        gpio_bankb[bank]->polar = gpio_bank_saved[bank].polar;
+                        gpio_bankb[bank]->edge  = gpio_bank_saved[bank].edge;
+                        gpio_bankb[bank]->both  = gpio_bank_saved[bank].both;
+
+                        gpio_bankb[bank]->data_set = gpio_bank_saved[bank].data
+                                                        | gpio_bank_saved[bank].dir;
+
+                        gpio_bankb[bank]->maska = gpio_bank_saved[bank].maska;
+        }
+        AWA_DUMMY_READ(maska);
+}
+
+
 #endif
 #else /* BF548_FAMILY */
+#ifdef CONFIG_PM
+
+u32 bfin_pm_standby_setup(void)
+{
+        return 0;
+}
+
+void bfin_pm_standby_restore(void)
+{
+
+}
+
+void bfin_gpio_pm_hibernate_suspend(void)
+{
+        int i, bank;
+
+        for (i = 0; i < MAX_BLACKFIN_GPIOS; i += GPIO_BANKSIZE) {
+                bank = gpio_bank(i);
+
+                        gpio_bank_saved[bank].fer  = gpio_array[bank]->port_fer;
+                        gpio_bank_saved[bank].mux  = gpio_array[bank]->port_mux;
+                        gpio_bank_saved[bank].data  = gpio_array[bank]->port_data;
+                        gpio_bank_saved[bank].data  = gpio_array[bank]->port_data;
+                        gpio_bank_saved[bank].inen  = gpio_array[bank]->port_inen;
+                        gpio_bank_saved[bank].dir   = gpio_array[bank]->port_dir_set;
+        }
+}
+
+void bfin_gpio_pm_hibernate_restore(void)
+{
+        int i, bank;
+
+        for (i = 0; i < MAX_BLACKFIN_GPIOS; i += GPIO_BANKSIZE) {
+                        bank = gpio_bank(i);
+
+                        gpio_array[bank]->port_mux  = gpio_bank_saved[bank].mux;
+                        gpio_array[bank]->port_fer  = gpio_bank_saved[bank].fer;
+                        gpio_array[bank]->port_inen  = gpio_bank_saved[bank].inen;
+                        gpio_array[bank]->port_dir_set   = gpio_bank_saved[bank].dir;
+                        gpio_array[bank]->port_set = gpio_bank_saved[bank].data
+                                                        | gpio_bank_saved[bank].dir;
+        }
+}
+#endif
 
 unsigned short get_gpio_dir(unsigned gpio)
 {
diff --git a/arch/blackfin/kernel/cplb-nompu/cplbhdlr.S b/arch/blackfin/kernel/cplb-nompu/cplbhdlr.S
index 2788532de72b..ecbabc0a1fed 100644
--- a/arch/blackfin/kernel/cplb-nompu/cplbhdlr.S
+++ b/arch/blackfin/kernel/cplb-nompu/cplbhdlr.S
@@ -125,6 +125,6 @@ ENTRY(__cplb_hdr)
         SP += -12;
         call _panic_cplb_error;
         SP += 12;
-        JUMP _handle_bad_cplb;
+        JUMP.L _handle_bad_cplb;
 
 ENDPROC(__cplb_hdr)
diff --git a/arch/blackfin/kernel/cplb-nompu/cplbinit.c b/arch/blackfin/kernel/cplb-nompu/cplbinit.c
index 6be0c50122e8..224e7cc30bc5 100644
--- a/arch/blackfin/kernel/cplb-nompu/cplbinit.c
+++ b/arch/blackfin/kernel/cplb-nompu/cplbinit.c
@@ -26,11 +26,7 @@
 #include <asm/cplb.h>
 #include <asm/cplbinit.h>
 
-#ifdef CONFIG_MAX_MEM_SIZE
-# define CPLB_MEM CONFIG_MAX_MEM_SIZE
-#else
-# define CPLB_MEM CONFIG_MEM_SIZE
-#endif
+#define CPLB_MEM CONFIG_MAX_MEM_SIZE
 
 /*
 * Number of required data CPLB switchtable entries
diff --git a/arch/blackfin/kernel/dualcore_test.c b/arch/blackfin/kernel/dualcore_test.c
deleted file mode 100644
index 0fcba74840b7..000000000000
--- a/arch/blackfin/kernel/dualcore_test.c
+++ /dev/null
@@ -1,49 +0,0 @@
-/*
- * File:         arch/blackfin/kernel/dualcore_test.c
- * Based on:
- * Author:
- *
- * Created:
- * Description:  Small test code for CoreB on a BF561
- *
- * Modified:
- *               Copyright 2004-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
- */
-
-#include <linux/init.h>
-#include <linux/module.h>
-
-static int *testarg = (int *)0xfeb00000;
-
-static int test_init(void)
-{
-        *testarg = 1;
-        printk(KERN_INFO "Dual core test module inserted: set testarg = [%d]\n @ [%p]\n",
-               *testarg, testarg);
-        return 0;
-}
-
-static void test_exit(void)
-{
-        printk(KERN_INFO "Dual core test module removed: testarg = [%d]\n", *testarg);
-}
-
-module_init(test_init);
-module_exit(test_exit);
diff --git a/arch/blackfin/kernel/entry.S b/arch/blackfin/kernel/entry.S
index 65f4e67a65c4..31bd9bf3efae 100644
--- a/arch/blackfin/kernel/entry.S
+++ b/arch/blackfin/kernel/entry.S
@@ -64,6 +64,11 @@ ENDPROC(_ret_from_fork)
 
 ENTRY(_sys_fork)
         r0 = -EINVAL;
+#if (ANOMALY_05000371)
+        nop;
+        nop;
+        nop;
+#endif
         rts;
 ENDPROC(_sys_fork)
 
diff --git a/arch/blackfin/kernel/kgdb.c b/arch/blackfin/kernel/kgdb.c
index a9c15515bfd7..a1f9641a6425 100644
--- a/arch/blackfin/kernel/kgdb.c
+++ b/arch/blackfin/kernel/kgdb.c
@@ -203,6 +203,8 @@ struct hw_breakpoint {
 
 int kgdb_arch_init(void)
 {
+        debugger_step = 0;
+
         kgdb_remove_all_hw_break();
         return 0;
 }
@@ -368,6 +370,7 @@ int kgdb_arch_handle_exception(int exceptionVector, int signo,
         char *ptr;
         int newPC;
         int wp_status;
+        int i;
 
         switch (remcom_in_buffer[0]) {
         case 'c':
@@ -392,7 +395,18 @@ int kgdb_arch_handle_exception(int exceptionVector, int signo,
                 /* set the trace bit if we're stepping */
                 if (remcom_in_buffer[0] == 's') {
                         linux_regs->syscfg |= 0x1;
-                        debugger_step = 1;
+                        debugger_step = linux_regs->ipend;
+                        debugger_step >>= 6;
+                        for (i = 10; i > 0; i--, debugger_step >>= 1)
+                                if (debugger_step & 1)
+                                        break;
+                        /* i indicate event priority of current stopped instruction
+                         * user space instruction is 0, IVG15 is 1, IVTMR is 10.
+                         * debugger_step > 0 means in single step mode
+                         */
+                        debugger_step = i + 1;
+                } else {
+                        debugger_step = 0;
                 }
 
                 wp_status = bfin_read_WPSTAT();
diff --git a/arch/blackfin/kernel/module.c b/arch/blackfin/kernel/module.c
index 14a42848f37f..e1bebc80a5bf 100644
--- a/arch/blackfin/kernel/module.c
+++ b/arch/blackfin/kernel/module.c
@@ -173,7 +173,7 @@ module_frob_arch_sections(Elf_Ehdr * hdr, Elf_Shdr * sechdrs,
         for (s = sechdrs; s < sechdrs_end; ++s) {
                 if ((strcmp(".l1.text", secstrings + s->sh_name) == 0) ||
                     ((strcmp(".text", secstrings + s->sh_name) == 0) &&
-                     (hdr->e_flags & FLG_CODE_IN_L1) && (s->sh_size > 0))) {
+                     (hdr->e_flags & EF_BFIN_CODE_IN_L1) && (s->sh_size > 0))) {
                         dest = l1_inst_sram_alloc(s->sh_size);
                         mod->arch.text_l1 = dest;
                         if (dest == NULL) {
@@ -188,7 +188,7 @@ module_frob_arch_sections(Elf_Ehdr * hdr, Elf_Shdr * sechdrs,
                 }
                 if ((strcmp(".l1.data", secstrings + s->sh_name) == 0) ||
                     ((strcmp(".data", secstrings + s->sh_name) == 0) &&
-                     (hdr->e_flags & FLG_DATA_IN_L1) && (s->sh_size > 0))) {
+                     (hdr->e_flags & EF_BFIN_DATA_IN_L1) && (s->sh_size > 0))) {
                         dest = l1_data_sram_alloc(s->sh_size);
                         mod->arch.data_a_l1 = dest;
                         if (dest == NULL) {
@@ -203,7 +203,7 @@ module_frob_arch_sections(Elf_Ehdr * hdr, Elf_Shdr * sechdrs,
                 }
                 if (strcmp(".l1.bss", secstrings + s->sh_name) == 0 ||
                     ((strcmp(".bss", secstrings + s->sh_name) == 0) &&
-                     (hdr->e_flags & FLG_DATA_IN_L1) && (s->sh_size > 0))) {
+                     (hdr->e_flags & EF_BFIN_DATA_IN_L1) && (s->sh_size > 0))) {
                         dest = l1_data_sram_alloc(s->sh_size);
                         mod->arch.bss_a_l1 = dest;
                         if (dest == NULL) {
@@ -242,6 +242,51 @@ module_frob_arch_sections(Elf_Ehdr * hdr, Elf_Shdr * sechdrs,
                         s->sh_flags &= ~SHF_ALLOC;
                         s->sh_addr = (unsigned long)dest;
                 }
+                if ((strcmp(".l2.text", secstrings + s->sh_name) == 0) ||
+                    ((strcmp(".text", secstrings + s->sh_name) == 0) &&
+                     (hdr->e_flags & EF_BFIN_CODE_IN_L2) && (s->sh_size > 0))) {
+                        dest = l2_sram_alloc(s->sh_size);
+                        mod->arch.text_l2 = dest;
+                        if (dest == NULL) {
+                                printk(KERN_ERR
+                                       "module %s: L2 SRAM allocation failed\n",
+                                       mod->name);
+                                return -1;
+                        }
+                        memcpy(dest, (void *)s->sh_addr, s->sh_size);
+                        s->sh_flags &= ~SHF_ALLOC;
+                        s->sh_addr = (unsigned long)dest;
+                }
+                if ((strcmp(".l2.data", secstrings + s->sh_name) == 0) ||
+                    ((strcmp(".data", secstrings + s->sh_name) == 0) &&
+                     (hdr->e_flags & EF_BFIN_DATA_IN_L2) && (s->sh_size > 0))) {
+                        dest = l2_sram_alloc(s->sh_size);
+                        mod->arch.data_l2 = dest;
+                        if (dest == NULL) {
+                                printk(KERN_ERR
+                                        "module %s: L2 SRAM allocation failed\n",
+                                        mod->name);
+                                return -1;
+                        }
+                        memcpy(dest, (void *)s->sh_addr, s->sh_size);
+                        s->sh_flags &= ~SHF_ALLOC;
+                        s->sh_addr = (unsigned long)dest;
+                }
+                if (strcmp(".l2.bss", secstrings + s->sh_name) == 0 ||
+                    ((strcmp(".bss", secstrings + s->sh_name) == 0) &&
+                     (hdr->e_flags & EF_BFIN_DATA_IN_L2) && (s->sh_size > 0))) {
+                        dest = l2_sram_alloc(s->sh_size);
+                        mod->arch.bss_l2 = dest;
+                        if (dest == NULL) {
+                                printk(KERN_ERR
+                                        "module %s: L2 SRAM allocation failed\n",
+                                        mod->name);
+                                return -1;
+                        }
+                        memset(dest, 0, s->sh_size);
+                        s->sh_flags &= ~SHF_ALLOC;
+                        s->sh_addr = (unsigned long)dest;
+                }
         }
         return 0;
 }
@@ -411,9 +456,10 @@ module_finalize(const Elf_Ehdr * hdr,
                         continue;
 
                 if ((sechdrs[i].sh_type == SHT_RELA) &&
-                    ((strcmp(".rela.l1.text", secstrings + sechdrs[i].sh_name) == 0) ||
+                    ((strcmp(".rela.l2.text", secstrings + sechdrs[i].sh_name) == 0) ||
+                    (strcmp(".rela.l1.text", secstrings + sechdrs[i].sh_name) == 0) ||
                     ((strcmp(".rela.text", secstrings + sechdrs[i].sh_name) == 0) &&
-                         (hdr->e_flags & FLG_CODE_IN_L1)))) {
+                        (hdr->e_flags & (EF_BFIN_CODE_IN_L1|EF_BFIN_CODE_IN_L2))))) {
                         apply_relocate_add((Elf_Shdr *) sechdrs, strtab,
                                            symindex, i, mod);
                 }
@@ -423,14 +469,12 @@ module_finalize(const Elf_Ehdr * hdr,
 
 void module_arch_cleanup(struct module *mod)
 {
-        if (mod->arch.text_l1)
-                l1_inst_sram_free((void *)mod->arch.text_l1);
-        if (mod->arch.data_a_l1)
-                l1_data_sram_free((void *)mod->arch.data_a_l1);
-        if (mod->arch.bss_a_l1)
-                l1_data_sram_free((void *)mod->arch.bss_a_l1);
-        if (mod->arch.data_b_l1)
-                l1_data_B_sram_free((void *)mod->arch.data_b_l1);
-        if (mod->arch.bss_b_l1)
-                l1_data_B_sram_free((void *)mod->arch.bss_b_l1);
+        l1_inst_sram_free(mod->arch.text_l1);
+        l1_data_A_sram_free(mod->arch.data_a_l1);
+        l1_data_A_sram_free(mod->arch.bss_a_l1);
+        l1_data_B_sram_free(mod->arch.data_b_l1);
+        l1_data_B_sram_free(mod->arch.bss_b_l1);
+        l2_sram_free(mod->arch.text_l2);
+        l2_sram_free(mod->arch.data_l2);
+        l2_sram_free(mod->arch.bss_l2);
 }
diff --git a/arch/blackfin/kernel/ptrace.c b/arch/blackfin/kernel/ptrace.c
index f51ab088098e..bf1a51d8e608 100644
--- a/arch/blackfin/kernel/ptrace.c
+++ b/arch/blackfin/kernel/ptrace.c
@@ -220,6 +220,20 @@ long arch_ptrace(struct task_struct *child, long request, long addr, long data)
                                 copied = sizeof(tmp);
                         } else
 #endif
+#if L1_DATA_A_LENGTH != 0
+                        if (addr + add >= L1_DATA_A_START
+                            && addr + add + sizeof(tmp) <= L1_DATA_A_START + L1_DATA_A_LENGTH) {
+                                memcpy(&tmp, (const void *)(addr + add), sizeof(tmp));
+                                copied = sizeof(tmp);
+                        } else
+#endif
+#if L1_DATA_B_LENGTH != 0
+                        if (addr + add >= L1_DATA_B_START
+                            && addr + add + sizeof(tmp) <= L1_DATA_B_START + L1_DATA_B_LENGTH) {
+                                memcpy(&tmp, (const void *)(addr + add), sizeof(tmp));
+                                copied = sizeof(tmp);
+                        } else
+#endif
                         if (addr + add >= FIXED_CODE_START
                             && addr + add + sizeof(tmp) <= FIXED_CODE_END) {
                                 memcpy(&tmp, (const void *)(addr + add), sizeof(tmp));
@@ -290,6 +304,20 @@ long arch_ptrace(struct task_struct *child, long request, long addr, long data)
                                 copied = sizeof(data);
                         } else
 #endif
+#if L1_DATA_A_LENGTH != 0
+                        if (addr + add >= L1_DATA_A_START
+                            && addr + add + sizeof(data) <= L1_DATA_A_START + L1_DATA_A_LENGTH) {
+                                memcpy((void *)(addr + add), &data, sizeof(data));
+                                copied = sizeof(data);
+                        } else
+#endif
+#if L1_DATA_B_LENGTH != 0
+                        if (addr + add >= L1_DATA_B_START
+                            && addr + add + sizeof(data) <= L1_DATA_B_START + L1_DATA_B_LENGTH) {
+                                memcpy((void *)(addr + add), &data, sizeof(data));
+                                copied = sizeof(data);
+                        } else
+#endif
                         if (addr + add >= FIXED_CODE_START
                             && addr + add + sizeof(data) <= FIXED_CODE_END) {
                                 memcpy((void *)(addr + add), &data, sizeof(data));
diff --git a/arch/blackfin/kernel/setup.c b/arch/blackfin/kernel/setup.c
index 8efea004aecb..23e637eb78da 100644
--- a/arch/blackfin/kernel/setup.c
+++ b/arch/blackfin/kernel/setup.c
@@ -104,6 +104,7 @@ void __init bf53x_relocate_l1_mem(void)
         unsigned long l1_code_length;
         unsigned long l1_data_a_length;
         unsigned long l1_data_b_length;
+        unsigned long l2_length;
 
         l1_code_length = _etext_l1 - _stext_l1;
         if (l1_code_length > L1_CODE_LENGTH)
@@ -129,6 +130,15 @@ void __init bf53x_relocate_l1_mem(void)
         /* Copy _sdata_b_l1 to _ebss_b_l1 to L1 data bank B SRAM */
         dma_memcpy(_sdata_b_l1, _l1_lma_start + l1_code_length +
                         l1_data_a_length, l1_data_b_length);
+
+#ifdef L2_LENGTH
+        l2_length = _ebss_l2 - _stext_l2;
+        if (l2_length > L2_LENGTH)
+                panic("L2 SRAM Overflow\n");
+
+        /* Copy _stext_l2 to _edata_l2 to L2 SRAM */
+        dma_memcpy(_stext_l2, _l2_lma_start, l2_length);
+#endif
 }
 
 /* add_memory_region to memmap */
@@ -664,11 +674,8 @@ static __init void setup_bootmem_allocator(void)
 })
 static inline int __init get_mem_size(void)
 {
-#ifdef CONFIG_MEM_SIZE
-        return CONFIG_MEM_SIZE;
-#else
-# if defined(EBIU_SDBCTL)
-#  if defined(BF561_FAMILY)
+#if defined(EBIU_SDBCTL)
+# if defined(BF561_FAMILY)
         int ret = 0;
         u32 sdbctl = bfin_read_EBIU_SDBCTL();
         ret += EBSZ_TO_MEG(sdbctl >>  0);
@@ -676,10 +683,10 @@ static inline int __init get_mem_size(void)
         ret += EBSZ_TO_MEG(sdbctl >> 16);
         ret += EBSZ_TO_MEG(sdbctl >> 24);
         return ret;
-#  else
+# else
         return EBSZ_TO_MEG(bfin_read_EBIU_SDBCTL());
-#  endif
-# elif defined(EBIU_DDRCTL1)
+# endif
+#elif defined(EBIU_DDRCTL1)
         u32 ddrctl = bfin_read_EBIU_DDRCTL1();
         int ret = 0;
         switch (ddrctl & 0xc0000) {
@@ -693,8 +700,9 @@ static inline int __init get_mem_size(void)
                 case DEVWD_8:  ret *= 2;
                 case DEVWD_16: break;
         }
+        if ((ddrctl & 0xc000) == 0x4000)
+                ret *= 2;
         return ret;
-# endif
 #endif
         BUG();
 }
@@ -763,6 +771,9 @@ void __init setup_arch(char **cmdline_p)
 
         _bfin_swrst = bfin_read_SWRST();
 
+        /* If we double fault, reset the system - otherwise we hang forever */
+        bfin_write_SWRST(DOUBLE_FAULT);
+
         if (_bfin_swrst & RESET_DOUBLE)
                 printk(KERN_INFO "Recovering from Double Fault event\n");
         else if (_bfin_swrst & RESET_WDOG)
@@ -842,38 +853,55 @@ static int __init topology_init(void)
 
 subsys_initcall(topology_init);
 
+/* Get the voltage input multiplier */
+static u_long cached_vco_pll_ctl, cached_vco;
 static u_long get_vco(void)
 {
         u_long msel;
-        u_long vco;
 
-        msel = (bfin_read_PLL_CTL() >> 9) & 0x3F;
+        u_long pll_ctl = bfin_read_PLL_CTL();
+        if (pll_ctl == cached_vco_pll_ctl)
+                return cached_vco;
+        else
+                cached_vco_pll_ctl = pll_ctl;
+
+        msel = (pll_ctl >> 9) & 0x3F;
         if (0 == msel)
                 msel = 64;
 
-        vco = CONFIG_CLKIN_HZ;
-        vco >>= (1 & bfin_read_PLL_CTL());      /* DF bit */
-        vco = msel * vco;
-        return vco;
+        cached_vco = CONFIG_CLKIN_HZ;
+        cached_vco >>= (1 & pll_ctl);   /* DF bit */
+        cached_vco *= msel;
+        return cached_vco;
 }
 
 /* Get the Core clock */
+static u_long cached_cclk_pll_div, cached_cclk;
 u_long get_cclk(void)
 {
         u_long csel, ssel;
+
         if (bfin_read_PLL_STAT() & 0x1)
                 return CONFIG_CLKIN_HZ;
 
         ssel = bfin_read_PLL_DIV();
+        if (ssel == cached_cclk_pll_div)
+                return cached_cclk;
+        else
+                cached_cclk_pll_div = ssel;
+
         csel = ((ssel >> 4) & 0x03);
         ssel &= 0xf;
         if (ssel && ssel < (1 << csel)) /* SCLK > CCLK */
-                return get_vco() / ssel;
-        return get_vco() >> csel;
+                cached_cclk = get_vco() / ssel;
+        else
+                cached_cclk = get_vco() >> csel;
+        return cached_cclk;
 }
 EXPORT_SYMBOL(get_cclk);
 
 /* Get the System clock */
+static u_long cached_sclk_pll_div, cached_sclk;
 u_long get_sclk(void)
 {
         u_long ssel;
@@ -881,13 +909,20 @@ u_long get_sclk(void)
         if (bfin_read_PLL_STAT() & 0x1)
                 return CONFIG_CLKIN_HZ;
 
-        ssel = (bfin_read_PLL_DIV() & 0xf);
+        ssel = bfin_read_PLL_DIV();
+        if (ssel == cached_sclk_pll_div)
+                return cached_sclk;
+        else
+                cached_sclk_pll_div = ssel;
+
+        ssel &= 0xf;
         if (0 == ssel) {
                 printk(KERN_WARNING "Invalid System Clock\n");
                 ssel = 1;
         }
 
-        return get_vco() / ssel;
+        cached_sclk = get_vco() / ssel;
+        return cached_sclk;
 }
 EXPORT_SYMBOL(get_sclk);
 
@@ -916,7 +951,7 @@ static int show_cpuinfo(struct seq_file *m, void *v)
         uint32_t revid;
 
         u_long cclk = 0, sclk = 0;
-        u_int dcache_size = 0, dsup_banks = 0;
+        u_int icache_size = BFIN_ICACHESIZE / 1024, dcache_size = 0, dsup_banks = 0;
 
         cpu = CPU;
         mmu = "none";
@@ -985,12 +1020,15 @@ static int show_cpuinfo(struct seq_file *m, void *v)
         }
 
         /* Is it turned on? */
-        if (!((bfin_read_DMEM_CONTROL()) & (ENDCPLB | DMC_ENABLE)))
+        if ((bfin_read_DMEM_CONTROL() & (ENDCPLB | DMC_ENABLE)) != (ENDCPLB | DMC_ENABLE))
                 dcache_size = 0;
 
+        if ((bfin_read_IMEM_CONTROL() & (IMC | ENICPLB)) == (IMC | ENICPLB))
+                icache_size = 0;
+
         seq_printf(m, "cache size\t: %d KB(L1 icache) "
                 "%d KB(L1 dcache-%s) %d KB(L2 cache)\n",
-                BFIN_ICACHESIZE / 1024, dcache_size,
+                icache_size, dcache_size,
 #if defined CONFIG_BFIN_WB
                 "wb"
 #elif defined CONFIG_BFIN_WT
@@ -1000,8 +1038,12 @@ static int show_cpuinfo(struct seq_file *m, void *v)
 
         seq_printf(m, "%s\n", cache);
 
-        seq_printf(m, "icache setup\t: %d Sub-banks/%d Ways, %d Lines/Way\n",
-                   BFIN_ISUBBANKS, BFIN_IWAYS, BFIN_ILINES);
+        if (icache_size)
+                seq_printf(m, "icache setup\t: %d Sub-banks/%d Ways, %d Lines/Way\n",
+                           BFIN_ISUBBANKS, BFIN_IWAYS, BFIN_ILINES);
+        else
+                seq_printf(m, "icache setup\t: off\n");
+
         seq_printf(m,
                    "dcache setup\t: %d Super-banks/%d Sub-banks/%d Ways, %d Lines/Way\n",
                    dsup_banks, BFIN_DSUBBANKS, BFIN_DWAYS,
diff --git a/arch/blackfin/kernel/traps.c b/arch/blackfin/kernel/traps.c
index f061f5181623..ad922ab91543 100644
--- a/arch/blackfin/kernel/traps.c
+++ b/arch/blackfin/kernel/traps.c
@@ -69,8 +69,6 @@ void __init trap_init(void)
 
 unsigned long saved_icplb_fault_addr, saved_dcplb_fault_addr;
 
-int kstack_depth_to_print = 48;
-
 static void decode_address(char *buf, unsigned long address)
 {
         struct vm_list_struct *vml;
@@ -163,6 +161,9 @@ static void decode_address(char *buf, unsigned long address)
                                 if (!in_atomic)
                                         mmput(mm);
 
+                                if (!strlen(buf))
+                                        sprintf(buf, "<0x%p> [ %s ] dynamic memory", (void *)address, name);
+
                                 goto done;
                         }
 
@@ -173,7 +174,7 @@ static void decode_address(char *buf, unsigned long address)
         }
 
         /* we were unable to find this address anywhere */
-        sprintf(buf, "<0x%p> /* unknown address */", (void *)address);
+        sprintf(buf, "<0x%p> /* kernel dynamic memory */", (void *)address);
 
 done:
         write_unlock_irqrestore(&tasklist_lock, flags);
@@ -494,7 +495,7 @@ asmlinkage void trap_c(struct pt_regs *fp)
         BUG_ON(sig == 0);
 
         if (sig != SIGTRAP) {
-                unsigned long stack;
+                unsigned long *stack;
                 dump_bfin_process(fp);
                 dump_bfin_mem(fp);
                 show_regs(fp);
@@ -508,14 +509,23 @@ asmlinkage void trap_c(struct pt_regs *fp)
                 else
 #endif
                         dump_bfin_trace_buffer();
-                show_stack(current, &stack);
+
                 if (oops_in_progress) {
+                        /* Dump the current kernel stack */
+                        printk(KERN_NOTICE "\n" KERN_NOTICE "Kernel Stack\n");
+                        show_stack(current, NULL);
+
                         print_modules();
 #ifndef CONFIG_ACCESS_CHECK
                         printk(KERN_EMERG "Please turn on "
                                "CONFIG_ACCESS_CHECK\n");
 #endif
                         panic("Kernel exception");
+                } else {
+                        /* Dump the user space stack */
+                        stack = (unsigned long *)rdusp();
+                        printk(KERN_NOTICE "Userspace Stack\n");
+                        show_stack(NULL, stack);
                 }
         }
 
@@ -532,11 +542,71 @@ asmlinkage void trap_c(struct pt_regs *fp)
 
 #define EXPAND_LEN ((1 << CONFIG_DEBUG_BFIN_HWTRACE_EXPAND_LEN) * 256 - 1)
 
+/*
+ * Similar to get_user, do some address checking, then dereference
+ * Return true on sucess, false on bad address
+ */
+bool get_instruction(unsigned short *val, unsigned short *address)
+{
+
+        unsigned long addr;
+
+        addr = (unsigned long)address;
+
+        /* Check for odd addresses */
+        if (addr & 0x1)
+                return false;
+
+        /* Check that things do not wrap around */
+        if (addr > (addr + 2))
+                return false;
+
+        /*
+         * Since we are in exception context, we need to do a little address checking
+         * We need to make sure we are only accessing valid memory, and
+         * we don't read something in the async space that can hang forever
+         */
+        if ((addr >= FIXED_CODE_START && (addr + 2) <= physical_mem_end) ||
+#ifdef L2_START
+            (addr >= L2_START && (addr + 2) <= (L2_START + L2_LENGTH)) ||
+#endif
+            (addr >= BOOT_ROM_START && (addr + 2) <= (BOOT_ROM_START + BOOT_ROM_LENGTH)) ||
+#if L1_DATA_A_LENGTH != 0
+            (addr >= L1_DATA_A_START && (addr + 2) <= (L1_DATA_A_START + L1_DATA_A_LENGTH)) ||
+#endif
+#if L1_DATA_B_LENGTH != 0
+            (addr >= L1_DATA_B_START && (addr + 2) <= (L1_DATA_B_START + L1_DATA_B_LENGTH)) ||
+#endif
+            (addr >= L1_SCRATCH_START && (addr + 2) <= (L1_SCRATCH_START + L1_SCRATCH_LENGTH)) ||
+            (!(bfin_read_EBIU_AMBCTL0() & B0RDYEN) &&
+               addr >= ASYNC_BANK0_BASE && (addr + 2) <= (ASYNC_BANK0_BASE + ASYNC_BANK0_SIZE)) ||
+            (!(bfin_read_EBIU_AMBCTL0() & B1RDYEN) &&
+               addr >= ASYNC_BANK1_BASE && (addr + 2) <= (ASYNC_BANK1_BASE + ASYNC_BANK1_SIZE)) ||
+            (!(bfin_read_EBIU_AMBCTL1() & B2RDYEN) &&
+               addr >= ASYNC_BANK2_BASE && (addr + 2) <= (ASYNC_BANK2_BASE + ASYNC_BANK1_SIZE)) ||
+            (!(bfin_read_EBIU_AMBCTL1() & B3RDYEN) &&
+              addr >= ASYNC_BANK3_BASE && (addr + 2) <= (ASYNC_BANK3_BASE + ASYNC_BANK1_SIZE))) {
+                *val = *address;
+                return true;
+        }
+
+#if L1_CODE_LENGTH != 0
+        if (addr >= L1_CODE_START && (addr + 2) <= (L1_CODE_START + L1_CODE_LENGTH)) {
+                dma_memcpy(val, address, 2);
+                return true;
+        }
+#endif
+
+
+        return false;
+}
+
 void dump_bfin_trace_buffer(void)
 {
 #ifdef CONFIG_DEBUG_BFIN_HWTRACE_ON
         int tflags, i = 0;
         char buf[150];
+        unsigned short val = 0, *addr;
 #ifdef CONFIG_DEBUG_BFIN_HWTRACE_EXPAND
         int j, index;
 #endif
@@ -549,8 +619,42 @@ void dump_bfin_trace_buffer(void)
                 for (; bfin_read_TBUFSTAT() & TBUFCNT; i++) {
                         decode_address(buf, (unsigned long)bfin_read_TBUF());
                         printk(KERN_NOTICE "%4i Target : %s\n", i, buf);
-                        decode_address(buf, (unsigned long)bfin_read_TBUF());
-                        printk(KERN_NOTICE "     Source : %s\n", buf);
+                        addr = (unsigned short *)bfin_read_TBUF();
+                        decode_address(buf, (unsigned long)addr);
+                        printk(KERN_NOTICE "     Source : %s ", buf);
+                        if (get_instruction(&val, addr)) {
+                                if (val == 0x0010)
+                                        printk("RTS");
+                                else if (val == 0x0011)
+                                        printk("RTI");
+                                else if (val == 0x0012)
+                                        printk("RTX");
+                                else if (val >= 0x0050 && val <= 0x0057)
+                                        printk("JUMP (P%i)", val & 7);
+                                else if (val >= 0x0060 && val <= 0x0067)
+                                        printk("CALL (P%i)", val & 7);
+                                else if (val >= 0x0070 && val <= 0x0077)
+                                        printk("CALL (PC+P%i)", val & 7);
+                                else if (val >= 0x0080 && val <= 0x0087)
+                                        printk("JUMP (PC+P%i)", val & 7);
+                                else if ((val >= 0x1000 && val <= 0x13FF) ||
+                                    (val >= 0x1800 && val <= 0x1BFF))
+                                        printk("IF !CC JUMP");
+                                else if ((val >= 0x1400 && val <= 0x17ff) ||
+                                    (val >= 0x1c00 && val <= 0x1fff))
+                                        printk("IF CC JUMP");
+                                else if (val >= 0x2000 && val <= 0x2fff)
+                                        printk("JUMP.S");
+                                else if (val >= 0xe080 && val <= 0xe0ff)
+                                        printk("LSETUP");
+                                else if (val >= 0xe200 && val <= 0xe2ff)
+                                        printk("JUMP.L");
+                                else if (val >= 0xe300 && val <= 0xe3ff)
+                                        printk("CALL pcrel");
+                                else
+                                        printk("0x%04x", val);
+                        }
+                        printk("\n");
                 }
         }
 
@@ -582,59 +686,151 @@ void dump_bfin_trace_buffer(void)
 }
 EXPORT_SYMBOL(dump_bfin_trace_buffer);
 
-static void show_trace(struct task_struct *tsk, unsigned long *sp)
+/*
+ * Checks to see if the address pointed to is either a
+ * 16-bit CALL instruction, or a 32-bit CALL instruction
+ */
+bool is_bfin_call(unsigned short *addr)
 {
-        unsigned long addr;
+        unsigned short opcode = 0, *ins_addr;
+        ins_addr = (unsigned short *)addr;
 
-        printk(KERN_NOTICE "\n" KERN_NOTICE "Call Trace:\n");
-
-        while (!kstack_end(sp)) {
-                addr = *sp++;
-                /*
-                 * If the address is either in the text segment of the
-                 * kernel, or in the region which contains vmalloc'ed
-                 * memory, it *may* be the address of a calling
-                 * routine; if so, print it so that someone tracing
-                 * down the cause of the crash will be able to figure
-                 * out the call path that was taken.
-                 */
-                if (kernel_text_address(addr))
-                        print_ip_sym(addr);
-        }
+        if (!get_instruction(&opcode, ins_addr))
+                return false;
 
-        printk(KERN_NOTICE "\n");
-}
+        if ((opcode >= 0x0060 && opcode <= 0x0067) ||
+            (opcode >= 0x0070 && opcode <= 0x0077))
+                return true;
+
+        ins_addr--;
+        if (!get_instruction(&opcode, ins_addr))
+                return false;
 
+        if (opcode >= 0xE300 && opcode <= 0xE3FF)
+                return true;
+
+        return false;
+
+}
 void show_stack(struct task_struct *task, unsigned long *stack)
 {
-        unsigned long *endstack, addr;
-        int i;
+        unsigned int *addr, *endstack, *fp = 0, *frame;
+        unsigned short *ins_addr;
+        char buf[150];
+        unsigned int i, j, ret_addr, frame_no = 0;
 
-        /* Cannot call dump_bfin_trace_buffer() here as show_stack() is
-         * called externally in some places in the kernel.
+        /*
+         * If we have been passed a specific stack, use that one otherwise
+         *    if we have been passed a task structure, use that, otherwise
+         *    use the stack of where the variable "stack" exists
          */
 
-        if (!stack) {
-                if (task)
+        if (stack == NULL) {
+                if (task) {
+                        /* We know this is a kernel stack, so this is the start/end */
                         stack = (unsigned long *)task->thread.ksp;
-                else
+                        endstack = (unsigned int *)(((unsigned int)(stack) & ~(THREAD_SIZE - 1)) + THREAD_SIZE);
+                } else {
+                        /* print out the existing stack info */
                         stack = (unsigned long *)&stack;
+                        endstack = (unsigned int *)PAGE_ALIGN((unsigned int)stack);
+                }
+        } else
+                endstack = (unsigned int *)PAGE_ALIGN((unsigned int)stack);
+
+        decode_address(buf, (unsigned int)stack);
+        printk(KERN_NOTICE "Stack info:\n" KERN_NOTICE " SP: [0x%p] %s\n", stack, buf);
+        addr = (unsigned int *)((unsigned int)stack & ~0x3F);
+
+        /* First thing is to look for a frame pointer */
+        for (addr = (unsigned int *)((unsigned int)stack & ~0xF), i = 0;
+                addr < endstack; addr++, i++) {
+                if (*addr & 0x1)
+                        continue;
+                ins_addr = (unsigned short *)*addr;
+                ins_addr--;
+                if (is_bfin_call(ins_addr))
+                        fp = addr - 1;
+
+                if (fp) {
+                        /* Let's check to see if it is a frame pointer */
+                        while (fp >= (addr - 1) && fp < endstack && fp)
+                                fp = (unsigned int *)*fp;
+                        if (fp == 0 || fp == endstack) {
+                                fp = addr - 1;
+                                break;
+                        }
+                        fp = 0;
+                }
         }
+        if (fp) {
+                frame = fp;
+                printk(" FP: (0x%p)\n", fp);
+        } else
+                frame = 0;
 
-        addr = (unsigned long)stack;
-        endstack = (unsigned long *)PAGE_ALIGN(addr);
+        /*
+         * Now that we think we know where things are, we
+         * walk the stack again, this time printing things out
+         * incase there is no frame pointer, we still look for
+         * valid return addresses
+         */
 
-        printk(KERN_NOTICE "Stack from %08lx:", (unsigned long)stack);
-        for (i = 0; i < kstack_depth_to_print; i++) {
-                if (stack + 1 > endstack)
-                        break;
-                if (i % 8 == 0)
-                        printk("\n" KERN_NOTICE "       ");
-                printk(" %08lx", *stack++);
+        /* First time print out data, next time, print out symbols */
+        for (j = 0; j <= 1; j++) {
+                if (j)
+                        printk(KERN_NOTICE "Return addresses in stack:\n");
+                else
+                        printk(KERN_NOTICE " Memory from 0x%08lx to %p", ((long unsigned int)stack & ~0xF), endstack);
+
+                fp = frame;
+                frame_no = 0;
+
+                for (addr = (unsigned int *)((unsigned int)stack & ~0xF), i = 0;
+                     addr <= endstack; addr++, i++) {
+
+                        ret_addr = 0;
+                        if (!j && i % 8 == 0)
+                                printk("\n" KERN_NOTICE "%p:",addr);
+
+                        /* if it is an odd address, or zero, just skip it */
+                        if (*addr & 0x1 || !*addr)
+                                goto print;
+
+                        ins_addr = (unsigned short *)*addr;
+
+                        /* Go back one instruction, and see if it is a CALL */
+                        ins_addr--;
+                        ret_addr = is_bfin_call(ins_addr);
+ print:
+                        if (!j && stack == (unsigned long *)addr)
+                                printk("[%08x]", *addr);
+                        else if (ret_addr)
+                                if (j) {
+                                        decode_address(buf, (unsigned int)*addr);
+                                        if (frame == addr) {
+                                                printk(KERN_NOTICE "   frame %2i : %s\n", frame_no, buf);
+                                                continue;
+                                        }
+                                        printk(KERN_NOTICE "    address : %s\n", buf);
+                                } else
+                                        printk("<%08x>", *addr);
+                        else if (fp == addr) {
+                                if (j)
+                                        frame = addr+1;
+                                else
+                                        printk("(%08x)", *addr);
+
+                                fp = (unsigned int *)*addr;
+                                frame_no++;
+
+                        } else if (!j)
+                                printk(" %08x ", *addr);
+                }
+                if (!j)
+                        printk("\n");
         }
-        printk("\n");
 
-        show_trace(task, stack);
 }
 
 void dump_stack(void)
@@ -715,19 +911,9 @@ void dump_bfin_mem(struct pt_regs *fp)
                 if (!((unsigned long)addr & 0xF))
                         printk("\n" KERN_NOTICE "0x%p: ", addr);
 
-                if (get_user(val, addr)) {
-                        if (addr >= (unsigned short *)L1_CODE_START &&
-                            addr < (unsigned short *)(L1_CODE_START + L1_CODE_LENGTH)) {
-                                dma_memcpy(&val, addr, sizeof(val));
-                                sprintf(buf, "%04x", val);
-                        } else if (addr >= (unsigned short *)FIXED_CODE_START &&
-                                addr <= (unsigned short *)memory_start) {
-                                val = bfin_read16(addr);
-                                sprintf(buf, "%04x", val);
-                        } else {
+                if (get_instruction(&val, addr)) {
                                 val = 0;
                                 sprintf(buf, "????");
-                        }
                 } else
                         sprintf(buf, "%04x", val);
 
diff --git a/arch/blackfin/kernel/vmlinux.lds.S b/arch/blackfin/kernel/vmlinux.lds.S
index 3ecc64cab3be..0896e38d6108 100644
--- a/arch/blackfin/kernel/vmlinux.lds.S
+++ b/arch/blackfin/kernel/vmlinux.lds.S
@@ -101,6 +101,11 @@ SECTIONS
 #if !L1_DATA_B_LENGTH
                 *(.l1.data.B)
 #endif
+#ifndef L2_LENGTH
+                . = ALIGN(32);
+                *(.data_l2.cacheline_aligned)
+                *(.l2.data)
+#endif
 
                 DATA_DATA
                 *(.data.*)
@@ -182,14 +187,13 @@ SECTIONS
                 *(.l1.data)
                 __edata_l1 = .;
 
-                . = ALIGN(4);
-                __sbss_l1 = .;
-                *(.l1.bss)
-
                 . = ALIGN(32);
                 *(.data_l1.cacheline_aligned)
 
                 . = ALIGN(4);
+                __sbss_l1 = .;
+                *(.l1.bss)
+                . = ALIGN(4);
                 __ebss_l1 = .;
         }
 
@@ -203,11 +207,37 @@ SECTIONS
                 . = ALIGN(4);
                 __sbss_b_l1 = .;
                 *(.l1.bss.B)
-
                 . = ALIGN(4);
                 __ebss_b_l1 = .;
         }
 
+#ifdef L2_LENGTH
+        __l2_lma_start = .;
+
+        .text_data_l2 L2_START : AT(LOADADDR(.data_b_l1) + SIZEOF(.data_b_l1))
+        {
+                . = ALIGN(4);
+                __stext_l2 = .;
+                *(.l1.text)
+                . = ALIGN(4);
+                __etext_l2 = .;
+
+                . = ALIGN(4);
+                __sdata_l2 = .;
+                *(.l1.data)
+                __edata_l2 = .;
+
+                . = ALIGN(32);
+                *(.data_l2.cacheline_aligned)
+
+                . = ALIGN(4);
+                __sbss_l2 = .;
+                *(.l1.bss)
+                . = ALIGN(4);
+                __ebss_l2 = .;
+        }
+#endif
+
         /* Force trailing alignment of our init section so that when we
          * free our init memory, we don't leave behind a partial page.
          */