6 files changed, 203 insertions, 70 deletions

diff --git a/arch/arm/kernel/Makefile b/arch/arm/kernel/Makefile
index 16eed6aebfa4..43b740d0e374 100644
--- a/arch/arm/kernel/Makefile
+++ b/arch/arm/kernel/Makefile
@@ -13,7 +13,7 @@ CFLAGS_REMOVE_return_address.o = -pg
 
 # Object file lists.
 
-obj-y           := elf.o entry-armv.o entry-common.o irq.o \
+obj-y           := elf.o entry-armv.o entry-common.o irq.o opcodes.o \
                    process.o ptrace.o return_address.o setup.o signal.o \
                    sys_arm.o stacktrace.o time.o traps.o
 
diff --git a/arch/arm/kernel/kprobes-test.c b/arch/arm/kernel/kprobes-test.c
index e17cdd6d90d8..1862d8f2fd44 100644
--- a/arch/arm/kernel/kprobes-test.c
+++ b/arch/arm/kernel/kprobes-test.c
@@ -202,6 +202,8 @@
 #include <linux/slab.h>
 #include <linux/kprobes.h>
 
+#include <asm/opcodes.h>
+
 #include "kprobes.h"
 #include "kprobes-test.h"
 
@@ -1050,65 +1052,9 @@ static int test_instance;
 
 static unsigned long test_check_cc(int cc, unsigned long cpsr)
 {
-        unsigned long temp;
-
-        switch (cc) {
-        case 0x0: /* eq */
-                return cpsr & PSR_Z_BIT;
-
-        case 0x1: /* ne */
-                return (~cpsr) & PSR_Z_BIT;
-
-        case 0x2: /* cs */
-                return cpsr & PSR_C_BIT;
-
-        case 0x3: /* cc */
-                return (~cpsr) & PSR_C_BIT;
-
-        case 0x4: /* mi */
-                return cpsr & PSR_N_BIT;
-
-        case 0x5: /* pl */
-                return (~cpsr) & PSR_N_BIT;
-
-        case 0x6: /* vs */
-                return cpsr & PSR_V_BIT;
-
-        case 0x7: /* vc */
-                return (~cpsr) & PSR_V_BIT;
+        int ret = arm_check_condition(cc << 28, cpsr);
 
-        case 0x8: /* hi */
-                cpsr &= ~(cpsr >> 1); /* PSR_C_BIT &= ~PSR_Z_BIT */
-                return cpsr & PSR_C_BIT;
-
-        case 0x9: /* ls */
-                cpsr &= ~(cpsr >> 1); /* PSR_C_BIT &= ~PSR_Z_BIT */
-                return (~cpsr) & PSR_C_BIT;
-
-        case 0xa: /* ge */
-                cpsr ^= (cpsr << 3); /* PSR_N_BIT ^= PSR_V_BIT */
-                return (~cpsr) & PSR_N_BIT;
-
-        case 0xb: /* lt */
-                cpsr ^= (cpsr << 3); /* PSR_N_BIT ^= PSR_V_BIT */
-                return cpsr & PSR_N_BIT;
-
-        case 0xc: /* gt */
-                temp = cpsr ^ (cpsr << 3); /* PSR_N_BIT ^= PSR_V_BIT */
-                temp |= (cpsr << 1);       /* PSR_N_BIT |= PSR_Z_BIT */
-                return (~temp) & PSR_N_BIT;
-
-        case 0xd: /* le */
-                temp = cpsr ^ (cpsr << 3); /* PSR_N_BIT ^= PSR_V_BIT */
-                temp |= (cpsr << 1);       /* PSR_N_BIT |= PSR_Z_BIT */
-                return temp & PSR_N_BIT;
-
-        case 0xe: /* al */
-        case 0xf: /* unconditional */
-                return true;
-        }
-        BUG();
-        return false;
+        return (ret != ARM_OPCODE_CONDTEST_FAIL);
 }
 
 static int is_last_scenario;
@@ -1128,7 +1074,9 @@ static unsigned long test_context_cpsr(int scenario)
 
         if (!test_case_is_thumb) {
                 /* Testing ARM code */
-                probe_should_run = test_check_cc(current_instruction >> 28, cpsr) != 0;
+                int cc = current_instruction >> 28;
+
+                probe_should_run = test_check_cc(cc, cpsr) != 0;
                 if (scenario == 15)
                         is_last_scenario = true;
 
diff --git a/arch/arm/kernel/opcodes.c b/arch/arm/kernel/opcodes.c
new file mode 100644
index 000000000000..f8179c6a817f
--- /dev/null
+++ b/arch/arm/kernel/opcodes.c
@@ -0,0 +1,72 @@
+/*
+ *  linux/arch/arm/kernel/opcodes.c
+ *
+ *  A32 condition code lookup feature moved from nwfpe/fpopcode.c
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <asm/opcodes.h>
+
+#define ARM_OPCODE_CONDITION_UNCOND 0xf
+
+/*
+ * condition code lookup table
+ * index into the table is test code: EQ, NE, ... LT, GT, AL, NV
+ *
+ * bit position in short is condition code: NZCV
+ */
+static const unsigned short cc_map[16] = {
+        0xF0F0,                 /* EQ == Z set            */
+        0x0F0F,                 /* NE                     */
+        0xCCCC,                 /* CS == C set            */
+        0x3333,                 /* CC                     */
+        0xFF00,                 /* MI == N set            */
+        0x00FF,                 /* PL                     */
+        0xAAAA,                 /* VS == V set            */
+        0x5555,                 /* VC                     */
+        0x0C0C,                 /* HI == C set && Z clear */
+        0xF3F3,                 /* LS == C clear || Z set */
+        0xAA55,                 /* GE == (N==V)           */
+        0x55AA,                 /* LT == (N!=V)           */
+        0x0A05,                 /* GT == (!Z && (N==V))   */
+        0xF5FA,                 /* LE == (Z || (N!=V))    */
+        0xFFFF,                 /* AL always              */
+        0                       /* NV                     */
+};
+
+/*
+ * Returns:
+ * ARM_OPCODE_CONDTEST_FAIL   - if condition fails
+ * ARM_OPCODE_CONDTEST_PASS   - if condition passes (including AL)
+ * ARM_OPCODE_CONDTEST_UNCOND - if NV condition, or separate unconditional
+ *                              opcode space from v5 onwards
+ *
+ * Code that tests whether a conditional instruction would pass its condition
+ * check should check that return value == ARM_OPCODE_CONDTEST_PASS.
+ *
+ * Code that tests if a condition means that the instruction would be executed
+ * (regardless of conditional or unconditional) should instead check that the
+ * return value != ARM_OPCODE_CONDTEST_FAIL.
+ */
+asmlinkage unsigned int arm_check_condition(u32 opcode, u32 psr)
+{
+        u32 cc_bits  = opcode >> 28;
+        u32 psr_cond = psr >> 28;
+        unsigned int ret;
+
+        if (cc_bits != ARM_OPCODE_CONDITION_UNCOND) {
+                if ((cc_map[cc_bits] >> (psr_cond)) & 1)
+                        ret = ARM_OPCODE_CONDTEST_PASS;
+                else
+                        ret = ARM_OPCODE_CONDTEST_FAIL;
+        } else {
+                ret = ARM_OPCODE_CONDTEST_UNCOND;
+        }
+
+        return ret;
+}
+EXPORT_SYMBOL_GPL(arm_check_condition);
diff --git a/arch/arm/kernel/smp_twd.c b/arch/arm/kernel/smp_twd.c
index a8a6682d6b52..c8e938553d47 100644
--- a/arch/arm/kernel/smp_twd.c
+++ b/arch/arm/kernel/smp_twd.c
@@ -10,8 +10,11 @@
  */
 #include <linux/init.h>
 #include <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/cpufreq.h>
 #include <linux/delay.h>
 #include <linux/device.h>
+#include <linux/err.h>
 #include <linux/smp.h>
 #include <linux/jiffies.h>
 #include <linux/clockchips.h>
@@ -25,6 +28,7 @@
 /* set up by the platform code */
 void __iomem *twd_base;
 
+static struct clk *twd_clk;
 static unsigned long twd_timer_rate;
 
 static struct clock_event_device __percpu **twd_evt;
@@ -89,6 +93,52 @@ void twd_timer_stop(struct clock_event_device *clk)
         disable_percpu_irq(clk->irq);
 }
 
+#ifdef CONFIG_CPU_FREQ
+
+/*
+ * Updates clockevent frequency when the cpu frequency changes.
+ * Called on the cpu that is changing frequency with interrupts disabled.
+ */
+static void twd_update_frequency(void *data)
+{
+        twd_timer_rate = clk_get_rate(twd_clk);
+
+        clockevents_update_freq(*__this_cpu_ptr(twd_evt), twd_timer_rate);
+}
+
+static int twd_cpufreq_transition(struct notifier_block *nb,
+        unsigned long state, void *data)
+{
+        struct cpufreq_freqs *freqs = data;
+
+        /*
+         * The twd clock events must be reprogrammed to account for the new
+         * frequency.  The timer is local to a cpu, so cross-call to the
+         * changing cpu.
+         */
+        if (state == CPUFREQ_POSTCHANGE || state == CPUFREQ_RESUMECHANGE)
+                smp_call_function_single(freqs->cpu, twd_update_frequency,
+                        NULL, 1);
+
+        return NOTIFY_OK;
+}
+
+static struct notifier_block twd_cpufreq_nb = {
+        .notifier_call = twd_cpufreq_transition,
+};
+
+static int twd_cpufreq_init(void)
+{
+        if (!IS_ERR(twd_clk))
+                return cpufreq_register_notifier(&twd_cpufreq_nb,
+                        CPUFREQ_TRANSITION_NOTIFIER);
+
+        return 0;
+}
+core_initcall(twd_cpufreq_init);
+
+#endif
+
 static void __cpuinit twd_calibrate_rate(void)
 {
         unsigned long count;
@@ -140,6 +190,35 @@ static irqreturn_t twd_handler(int irq, void *dev_id)
         return IRQ_NONE;
 }
 
+static struct clk *twd_get_clock(void)
+{
+        struct clk *clk;
+        int err;
+
+        clk = clk_get_sys("smp_twd", NULL);
+        if (IS_ERR(clk)) {
+                pr_err("smp_twd: clock not found: %d\n", (int)PTR_ERR(clk));
+                return clk;
+        }
+
+        err = clk_prepare(clk);
+        if (err) {
+                pr_err("smp_twd: clock failed to prepare: %d\n", err);
+                clk_put(clk);
+                return ERR_PTR(err);
+        }
+
+        err = clk_enable(clk);
+        if (err) {
+                pr_err("smp_twd: clock failed to enable: %d\n", err);
+                clk_unprepare(clk);
+                clk_put(clk);
+                return ERR_PTR(err);
+        }
+
+        return clk;
+}
+
 /*
  * Setup the local clock events for a CPU.
  */
@@ -165,7 +244,13 @@ void __cpuinit twd_timer_setup(struct clock_event_device *clk)
                 }
         }
 
-        twd_calibrate_rate();
+        if (!twd_clk)
+                twd_clk = twd_get_clock();
+
+        if (!IS_ERR_OR_NULL(twd_clk))
+                twd_timer_rate = clk_get_rate(twd_clk);
+        else
+                twd_calibrate_rate();
 
         clk->name = "local_timer";
         clk->features = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT |
@@ -173,15 +258,11 @@ void __cpuinit twd_timer_setup(struct clock_event_device *clk)
         clk->rating = 350;
         clk->set_mode = twd_set_mode;
         clk->set_next_event = twd_set_next_event;
-        clk->shift = 20;
-        clk->mult = div_sc(twd_timer_rate, NSEC_PER_SEC, clk->shift);
-        clk->max_delta_ns = clockevent_delta2ns(0xffffffff, clk);
-        clk->min_delta_ns = clockevent_delta2ns(0xf, clk);
 
         this_cpu_clk = __this_cpu_ptr(twd_evt);
         *this_cpu_clk = clk;
 
-        clockevents_register_device(clk);
-
+        clockevents_config_and_register(clk, twd_timer_rate,
+                                        0xf, 0xffffffff);
         enable_percpu_irq(clk->irq, 0);
 }
diff --git a/arch/arm/kernel/swp_emulate.c b/arch/arm/kernel/swp_emulate.c
index 5f452f8fde05..df745188f5de 100644
--- a/arch/arm/kernel/swp_emulate.c
+++ b/arch/arm/kernel/swp_emulate.c
@@ -25,6 +25,7 @@
 #include <linux/syscalls.h>
 #include <linux/perf_event.h>
 
+#include <asm/opcodes.h>
 #include <asm/traps.h>
 #include <asm/uaccess.h>
 
@@ -185,6 +186,21 @@ static int swp_handler(struct pt_regs *regs, unsigned int instr)
 
         perf_sw_event(PERF_COUNT_SW_EMULATION_FAULTS, 1, regs, regs->ARM_pc);
 
+        res = arm_check_condition(instr, regs->ARM_cpsr);
+        switch (res) {
+        case ARM_OPCODE_CONDTEST_PASS:
+                break;
+        case ARM_OPCODE_CONDTEST_FAIL:
+                /* Condition failed - return to next instruction */
+                regs->ARM_pc += 4;
+                return 0;
+        case ARM_OPCODE_CONDTEST_UNCOND:
+                /* If unconditional encoding - not a SWP, undef */
+                return -EFAULT;
+        default:
+                return -EINVAL;
+        }
+
         if (current->pid != previous_pid) {
                 pr_debug("\"%s\" (%ld) uses deprecated SWP{B} instruction\n",
                          current->comm, (unsigned long)current->pid);
diff --git a/arch/arm/kernel/tcm.c b/arch/arm/kernel/tcm.c
index 30e302d33e0a..01ec453bb924 100644
--- a/arch/arm/kernel/tcm.c
+++ b/arch/arm/kernel/tcm.c
@@ -180,9 +180,9 @@ static int __init setup_tcm_bank(u8 type, u8 bank, u8 banks,
  */
 void __init tcm_init(void)
 {
-        u32 tcm_status = read_cpuid_tcmstatus();
-        u8 dtcm_banks = (tcm_status >> 16) & 0x03;
-        u8 itcm_banks = (tcm_status & 0x03);
+        u32 tcm_status;
+        u8 dtcm_banks;
+        u8 itcm_banks;
         size_t dtcm_code_sz = &__edtcm_data - &__sdtcm_data;
         size_t itcm_code_sz = &__eitcm_text - &__sitcm_text;
         char *start;
@@ -191,6 +191,22 @@ void __init tcm_init(void)
         int ret;
         int i;
 
+        /*
+         * Prior to ARMv5 there is no TCM, and trying to read the status
+         * register will hang the processor.
+         */
+        if (cpu_architecture() < CPU_ARCH_ARMv5) {
+                if (dtcm_code_sz || itcm_code_sz)
+                        pr_info("CPU TCM: %u bytes of DTCM and %u bytes of "
+                                "ITCM code compiled in, but no TCM present "
+                                "in pre-v5 CPU\n", dtcm_code_sz, itcm_code_sz);
+                return;
+        }
+
+        tcm_status = read_cpuid_tcmstatus();
+        dtcm_banks = (tcm_status >> 16) & 0x03;
+        itcm_banks = (tcm_status & 0x03);
+
         /* Values greater than 2 for D/ITCM banks are "reserved" */
         if (dtcm_banks > 2)
                 dtcm_banks = 0;