+/*
+ * Framework for instruction set test cases
+ */
+
+void __naked __kprobes_test_case_start(void)
+{
+ __asm__ __volatile__ (
+ "stmdb sp!, {r4-r11} \n\t"
+ "sub sp, sp, #"__stringify(TEST_MEMORY_SIZE)"\n\t"
+ "bic r0, lr, #1 @ r0 = inline title string \n\t"
+ "mov r1, sp \n\t"
+ "bl kprobes_test_case_start \n\t"
+ "bx r0 \n\t"
+ );
+}
+
+#ifndef CONFIG_THUMB2_KERNEL
+
+void __naked __kprobes_test_case_end_32(void)
+{
+ __asm__ __volatile__ (
+ "mov r4, lr \n\t"
+ "bl kprobes_test_case_end \n\t"
+ "cmp r0, #0 \n\t"
+ "movne pc, r0 \n\t"
+ "mov r0, r4 \n\t"
+ "add sp, sp, #"__stringify(TEST_MEMORY_SIZE)"\n\t"
+ "ldmia sp!, {r4-r11} \n\t"
+ "mov pc, r0 \n\t"
+ );
+}
+
+#else /* CONFIG_THUMB2_KERNEL */
+
+void __naked __kprobes_test_case_end_16(void)
+{
+ __asm__ __volatile__ (
+ "mov r4, lr \n\t"
+ "bl kprobes_test_case_end \n\t"
+ "cmp r0, #0 \n\t"
+ "bxne r0 \n\t"
+ "mov r0, r4 \n\t"
+ "add sp, sp, #"__stringify(TEST_MEMORY_SIZE)"\n\t"
+ "ldmia sp!, {r4-r11} \n\t"
+ "bx r0 \n\t"
+ );
+}
+
+void __naked __kprobes_test_case_end_32(void)
+{
+ __asm__ __volatile__ (
+ ".arm \n\t"
+ "orr lr, lr, #1 @ will return to Thumb code \n\t"
+ "ldr pc, 1f \n\t"
+ "1: \n\t"
+ ".word __kprobes_test_case_end_16 \n\t"
+ );
+}
+
+#endif
+
+
+int kprobe_test_flags;
+int kprobe_test_cc_position;
+
+static int test_try_count;
+static int test_pass_count;
+static int test_fail_count;
+
+static struct pt_regs initial_regs;
+static struct pt_regs expected_regs;
+static struct pt_regs result_regs;
+
+static u32 expected_memory[TEST_MEMORY_SIZE/sizeof(u32)];
+
+static const char *current_title;
+static struct test_arg *current_args;
+static u32 *current_stack;
+static uintptr_t current_branch_target;
+
+static uintptr_t current_code_start;
+static kprobe_opcode_t current_instruction;
+
+
+#define TEST_CASE_PASSED -1
+#define TEST_CASE_FAILED -2
+
+static int test_case_run_count;
+static bool test_case_is_thumb;
+static int test_instance;
+
+/*
+ * We ignore the state of the imprecise abort disable flag (CPSR.A) because this
+ * can change randomly as the kernel doesn't take care to preserve or initialise
+ * this across context switches. Also, with Security Extentions, the flag may
+ * not be under control of the kernel; for this reason we ignore the state of
+ * the FIQ disable flag CPSR.F as well.
+ */
+#define PSR_IGNORE_BITS (PSR_A_BIT | PSR_F_BIT)
+
+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;
+
+ 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;
+}
+
+static int is_last_scenario;
+static int probe_should_run; /* 0 = no, 1 = yes, -1 = unknown */
+static int memory_needs_checking;
+
+static unsigned long test_context_cpsr(int scenario)
+{
+ unsigned long cpsr;
+
+ probe_should_run = 1;
+
+ /* Default case is that we cycle through 16 combinations of flags */
+ cpsr = (scenario & 0xf) << 28; /* N,Z,C,V flags */
+ cpsr |= (scenario & 0xf) << 16; /* GE flags */
+ cpsr |= (scenario & 0x1) << 27; /* Toggle Q flag */
+
+ if (!test_case_is_thumb) {
+ /* Testing ARM code */
+ probe_should_run = test_check_cc(current_instruction >> 28, cpsr) != 0;
+ if (scenario == 15)
+ is_last_scenario = true;
+
+ } else if (kprobe_test_flags & TEST_FLAG_NO_ITBLOCK) {
+ /* Testing Thumb code without setting ITSTATE */
+ if (kprobe_test_cc_position) {
+ int cc = (current_instruction >> kprobe_test_cc_position) & 0xf;
+ probe_should_run = test_check_cc(cc, cpsr) != 0;
+ }
+
+ if (scenario == 15)
+ is_last_scenario = true;
+
+ } else if (kprobe_test_flags & TEST_FLAG_FULL_ITBLOCK) {
+ /* Testing Thumb code with all combinations of ITSTATE */
+ unsigned x = (scenario >> 4);
+ unsigned cond_base = x % 7; /* ITSTATE<7:5> */
+ unsigned mask = x / 7 + 2; /* ITSTATE<4:0>, bits reversed */
+
+ if (mask > 0x1f) {
+ /* Finish by testing state from instruction 'itt al' */
+ cond_base = 7;
+ mask = 0x4;
+ if ((scenario & 0xf) == 0xf)
+ is_last_scenario = true;
+ }
+
+ cpsr |= cond_base << 13; /* ITSTATE<7:5> */
+ cpsr |= (mask & 0x1) << 12; /* ITSTATE<4> */
+ cpsr |= (mask & 0x2) << 10; /* ITSTATE<3> */
+ cpsr |= (mask & 0x4) << 8; /* ITSTATE<2> */
+ cpsr |= (mask & 0x8) << 23; /* ITSTATE<1> */
+ cpsr |= (mask & 0x10) << 21; /* ITSTATE<0> */
+
+ probe_should_run = test_check_cc((cpsr >> 12) & 0xf, cpsr) != 0;
+
+ } else {
+ /* Testing Thumb code with several combinations of ITSTATE */
+ switch (scenario) {
+ case 16: /* Clear NZCV flags and 'it eq' state (false as Z=0) */
+ cpsr = 0x00000800;
+ probe_should_run = 0;
+ break;
+ case 17: /* Set NZCV flags and 'it vc' state (false as V=1) */
+ cpsr = 0xf0007800;
+ probe_should_run = 0;
+ break;
+ case 18: /* Clear NZCV flags and 'it ls' state (true as C=0) */
+ cpsr = 0x00009800;
+ break;
+ case 19: /* Set NZCV flags and 'it cs' state (true as C=1) */
+ cpsr = 0xf0002800;
+ is_last_scenario = true;
+ break;
+ }
+ }
+
+ return cpsr;
+}
+
+static void setup_test_context(struct pt_regs *regs)
+{
+ int scenario = test_case_run_count>>1;
+ unsigned long val;
+ struct test_arg *args;
+ int i;
+
+ is_last_scenario = false;
+ memory_needs_checking = false;
+
+ /* Initialise test memory on stack */
+ val = (scenario & 1) ? VALM : ~VALM;
+ for (i = 0; i < TEST_MEMORY_SIZE / sizeof(current_stack[0]); ++i)
+ current_stack[i] = val + (i << 8);
+ /* Put target of branch on stack for tests which load PC from memory */
+ if (current_branch_target)
+ current_stack[15] = current_branch_target;
+ /* Put a value for SP on stack for tests which load SP from memory */
+ current_stack[13] = (u32)current_stack + 120;
+
+ /* Initialise register values to their default state */
+ val = (scenario & 2) ? VALR : ~VALR;
+ for (i = 0; i < 13; ++i)
+ regs->uregs[i] = val ^ (i << 8);
+ regs->ARM_lr = val ^ (14 << 8);
+ regs->ARM_cpsr &= ~(APSR_MASK | PSR_IT_MASK);
+ regs->ARM_cpsr |= test_context_cpsr(scenario);
+
+ /* Perform testcase specific register setup */
+ args = current_args;
+ for (; args[0].type != ARG_TYPE_END; ++args)
+ switch (args[0].type) {
+ case ARG_TYPE_REG: {
+ struct test_arg_regptr *arg =
+ (struct test_arg_regptr *)args;
+ regs->uregs[arg->reg] = arg->val;
+ break;
+ }
+ case ARG_TYPE_PTR: {
+ struct test_arg_regptr *arg =
+ (struct test_arg_regptr *)args;
+ regs->uregs[arg->reg] =
+ (unsigned long)current_stack + arg->val;
+ memory_needs_checking = true;
+ break;
+ }
+ case ARG_TYPE_MEM: {
+ struct test_arg_mem *arg = (struct test_arg_mem *)args;
+ current_stack[arg->index] = arg->val;
+ break;
+ }
+ default:
+ break;
+ }
+}
+
+struct test_probe {
+ struct kprobe kprobe;
+ bool registered;
+ int hit;
+};
+
+static void unregister_test_probe(struct test_probe *probe)
+{
+ if (probe->registered) {
+ unregister_kprobe(&probe->kprobe);
+ probe->kprobe.flags = 0; /* Clear disable flag to allow reuse */
+ }
+ probe->registered = false;
+}
+
+static int register_test_probe(struct test_probe *probe)
+{
+ int ret;
+
+ if (probe->registered)
+ BUG();
+
+ ret = register_kprobe(&probe->kprobe);
+ if (ret >= 0) {
+ probe->registered = true;
+ probe->hit = -1;
+ }
+ return ret;
+}
+
+static int __kprobes
+test_before_pre_handler(struct kprobe *p, struct pt_regs *regs)
+{
+ container_of(p, struct test_probe, kprobe)->hit = test_instance;
+ return 0;
+}
+
+static void __kprobes
+test_before_post_handler(struct kprobe *p, struct pt_regs *regs,
+ unsigned long flags)
+{
+ setup_test_context(regs);
+ initial_regs = *regs;
+ initial_regs.ARM_cpsr &= ~PSR_IGNORE_BITS;
+}
+
+static int __kprobes
+test_case_pre_handler(struct kprobe *p, struct pt_regs *regs)
+{
+ container_of(p, struct test_probe, kprobe)->hit = test_instance;
+ return 0;
+}
+
+static int __kprobes
+test_after_pre_handler(struct kprobe *p, struct pt_regs *regs)
+{
+ if (container_of(p, struct test_probe, kprobe)->hit == test_instance)
+ return 0; /* Already run for this test instance */
+
+ result_regs = *regs;
+ result_regs.ARM_cpsr &= ~PSR_IGNORE_BITS;
+
+ /* Undo any changes done to SP by the test case */
+ regs->ARM_sp = (unsigned long)current_stack;
+
+ container_of(p, struct test_probe, kprobe)->hit = test_instance;
+ return 0;
+}
+
+static struct test_probe test_before_probe = {
+ .kprobe.pre_handler = test_before_pre_handler,
+ .kprobe.post_handler = test_before_post_handler,
+};
+
+static struct test_probe test_case_probe = {
+ .kprobe.pre_handler = test_case_pre_handler,
+};
+
+static struct test_probe test_after_probe = {
+ .kprobe.pre_handler = test_after_pre_handler,
+};
+
+static struct test_probe test_after2_probe = {
+ .kprobe.pre_handler = test_after_pre_handler,
+};
+
+static void test_case_cleanup(void)
+{
+ unregister_test_probe(&test_before_probe);
+ unregister_test_probe(&test_case_probe);
+ unregister_test_probe(&test_after_probe);
+ unregister_test_probe(&test_after2_probe);
+}
+
+static void print_registers(struct pt_regs *regs)
+{
+ pr_err("r0 %08lx | r1 %08lx | r2 %08lx | r3 %08lx\n",
+ regs->ARM_r0, regs->ARM_r1, regs->ARM_r2, regs->ARM_r3);
+ pr_err("r4 %08lx | r5 %08lx | r6 %08lx | r7 %08lx\n",
+ regs->ARM_r4, regs->ARM_r5, regs->ARM_r6, regs->ARM_r7);
+ pr_err("r8 %08lx | r9 %08lx | r10 %08lx | r11 %08lx\n",
+ regs->ARM_r8, regs->ARM_r9, regs->ARM_r10, regs->ARM_fp);
+ pr_err("r12 %08lx | sp %08lx | lr %08lx | pc %08lx\n",
+ regs->ARM_ip, regs->ARM_sp, regs->ARM_lr, regs->ARM_pc);
+ pr_err("cpsr %08lx\n", regs->ARM_cpsr);
+}
+
+static void print_memory(u32 *mem, size_t size)
+{
+ int i;
+ for (i = 0; i < size / sizeof(u32); i += 4)
+ pr_err("%08x %08x %08x %08x\n", mem[i], mem[i+1],
+ mem[i+2], mem[i+3]);
+}
+
+static size_t expected_memory_size(u32 *sp)
+{
+ size_t size = sizeof(expected_memory);
+ int offset = (uintptr_t)sp - (uintptr_t)current_stack;
+ if (offset > 0)
+ size -= offset;
+ return size;
+}
+
+static void test_case_failed(const char *message)
+{
+ test_case_cleanup();
+
+ pr_err("FAIL: %s\n", message);
+ pr_err("FAIL: Test %s\n", current_title);
+ pr_err("FAIL: Scenario %d\n", test_case_run_count >> 1);
+}
+
+static unsigned long next_instruction(unsigned long pc)
+{
+#ifdef CONFIG_THUMB2_KERNEL
+ if ((pc & 1) && !is_wide_instruction(*(u16 *)(pc - 1)))
+ return pc + 2;
+ else
+#endif
+ return pc + 4;
+}
+
+static uintptr_t __used kprobes_test_case_start(const char *title, void *stack)
+{
+ struct test_arg *args;
+ struct test_arg_end *end_arg;
+ unsigned long test_code;
+
+ args = (struct test_arg *)PTR_ALIGN(title + strlen(title) + 1, 4);
+
+ current_title = title;
+ current_args = args;
+ current_stack = stack;
+
+ ++test_try_count;
+
+ while (args->type != ARG_TYPE_END)
+ ++args;
+ end_arg = (struct test_arg_end *)args;
+
+ test_code = (unsigned long)(args + 1); /* Code starts after args */
+
+ test_case_is_thumb = end_arg->flags & ARG_FLAG_THUMB;
+ if (test_case_is_thumb)
+ test_code |= 1;
+
+ current_code_start = test_code;
+
+ current_branch_target = 0;
+ if (end_arg->branch_offset != end_arg->end_offset)
+ current_branch_target = test_code + end_arg->branch_offset;
+
+ test_code += end_arg->code_offset;
+ test_before_probe.kprobe.addr = (kprobe_opcode_t *)test_code;
+
+ test_code = next_instruction(test_code);
+ test_case_probe.kprobe.addr = (kprobe_opcode_t *)test_code;
+
+ if (test_case_is_thumb) {
+ u16 *p = (u16 *)(test_code & ~1);
+ current_instruction = p[0];
+ if (is_wide_instruction(current_instruction)) {
+ current_instruction <<= 16;
+ current_instruction |= p[1];
+ }
+ } else {
+ current_instruction = *(u32 *)test_code;
+ }
+
+ if (current_title[0] == '.')
+ verbose("%s\n", current_title);
+ else
+ verbose("%s\t@ %0*x\n", current_title,
+ test_case_is_thumb ? 4 : 8,
+ current_instruction);
+
+ test_code = next_instruction(test_code);
+ test_after_probe.kprobe.addr = (kprobe_opcode_t *)test_code;
+
+ if (kprobe_test_flags & TEST_FLAG_NARROW_INSTR) {
+ if (!test_case_is_thumb ||
+ is_wide_instruction(current_instruction)) {
+ test_case_failed("expected 16-bit instruction");
+ goto fail;
+ }
+ } else {
+ if (test_case_is_thumb &&
+ !is_wide_instruction(current_instruction)) {
+ test_case_failed("expected 32-bit instruction");
+ goto fail;
+ }
+ }
+
+ if (end_arg->flags & ARG_FLAG_UNSUPPORTED) {
+ if (register_test_probe(&test_case_probe) < 0)
+ goto pass;
+ test_case_failed("registered probe for unsupported instruction");
+ goto fail;
+ }
+
+ if (end_arg->flags & ARG_FLAG_SUPPORTED) {
+ if (register_test_probe(&test_case_probe) >= 0)
+ goto pass;
+ test_case_failed("couldn't register probe for supported instruction");
+ goto fail;
+ }
+
+ if (register_test_probe(&test_before_probe) < 0) {
+ test_case_failed("register test_before_probe failed");
+ goto fail;
+ }
+ if (register_test_probe(&test_after_probe) < 0) {
+ test_case_failed("register test_after_probe failed");
+ goto fail;
+ }
+ if (current_branch_target) {
+ test_after2_probe.kprobe.addr =
+ (kprobe_opcode_t *)current_branch_target;
+ if (register_test_probe(&test_after2_probe) < 0) {
+ test_case_failed("register test_after2_probe failed");
+ goto fail;
+ }
+ }
+
+ /* Start first run of test case */
+ test_case_run_count = 0;
+ ++test_instance;
+ return current_code_start;
+pass:
+ test_case_run_count = TEST_CASE_PASSED;
+ return (uintptr_t)test_after_probe.kprobe.addr;
+fail:
+ test_case_run_count = TEST_CASE_FAILED;
+ return (uintptr_t)test_after_probe.kprobe.addr;
+}
+
+static bool check_test_results(void)
+{
+ size_t mem_size = 0;
+ u32 *mem = 0;
+
+ if (memcmp(&expected_regs, &result_regs, sizeof(expected_regs))) {
+ test_case_failed("registers differ");
+ goto fail;
+ }
+
+ if (memory_needs_checking) {
+ mem = (u32 *)result_regs.ARM_sp;
+ mem_size = expected_memory_size(mem);
+ if (memcmp(expected_memory, mem, mem_size)) {
+ test_case_failed("test memory differs");
+ goto fail;
+ }
+ }
+
+ return true;
+
+fail:
+ pr_err("initial_regs:\n");
+ print_registers(&initial_regs);
+ pr_err("expected_regs:\n");
+ print_registers(&expected_regs);
+ pr_err("result_regs:\n");
+ print_registers(&result_regs);
+
+ if (mem) {
+ pr_err("current_stack=%p\n", current_stack);
+ pr_err("expected_memory:\n");
+ print_memory(expected_memory, mem_size);
+ pr_err("result_memory:\n");
+ print_memory(mem, mem_size);
+ }
+
+ return false;
+}
+
+static uintptr_t __used kprobes_test_case_end(void)
+{
+ if (test_case_run_count < 0) {
+ if (test_case_run_count == TEST_CASE_PASSED)
+ /* kprobes_test_case_start did all the needed testing */
+ goto pass;
+ else
+ /* kprobes_test_case_start failed */
+ goto fail;
+ }
+
+ if (test_before_probe.hit != test_instance) {
+ test_case_failed("test_before_handler not run");
+ goto fail;
+ }
+
+ if (test_after_probe.hit != test_instance &&
+ test_after2_probe.hit != test_instance) {
+ test_case_failed("test_after_handler not run");
+ goto fail;
+ }
+
+ /*
+ * Even numbered test runs ran without a probe on the test case so
+ * we can gather reference results. The subsequent odd numbered run
+ * will have the probe inserted.
+ */
+ if ((test_case_run_count & 1) == 0) {
+ /* Save results from run without probe */
+ u32 *mem = (u32 *)result_regs.ARM_sp;
+ expected_regs = result_regs;
+ memcpy(expected_memory, mem, expected_memory_size(mem));
+
+ /* Insert probe onto test case instruction */
+ if (register_test_probe(&test_case_probe) < 0) {
+ test_case_failed("register test_case_probe failed");
+ goto fail;
+ }
+ } else {
+ /* Check probe ran as expected */
+ if (probe_should_run == 1) {
+ if (test_case_probe.hit != test_instance) {
+ test_case_failed("test_case_handler not run");
+ goto fail;
+ }
+ } else if (probe_should_run == 0) {
+ if (test_case_probe.hit == test_instance) {
+ test_case_failed("test_case_handler ran");
+ goto fail;
+ }
+ }
+
+ /* Remove probe for any subsequent reference run */
+ unregister_test_probe(&test_case_probe);
+
+ if (!check_test_results())
+ goto fail;
+
+ if (is_last_scenario)
+ goto pass;
+ }
+
+ /* Do next test run */
+ ++test_case_run_count;
+ ++test_instance;
+ return current_code_start;
+fail:
+ ++test_fail_count;
+ goto end;
+pass:
+ ++test_pass_count;
+end:
+ test_case_cleanup();
+ return 0;
+}
+
+