1#include <linux/kernel.h>
 2#include <linux/kprobes.h>
 3#include <linux/stop_machine.h>
 4
 5#include <asm/cacheflush.h>
 6#include <asm/smp_plat.h>
 7#include <asm/opcodes.h>
 8
 9#include "patch.h"
10
11struct patch {
12	void *addr;
13	unsigned int insn;
14};
15
16void __kprobes __patch_text(void *addr, unsigned int insn)
17{
18	bool thumb2 = IS_ENABLED(CONFIG_THUMB2_KERNEL);
19	int size;
20
21	if (thumb2 && __opcode_is_thumb16(insn)) {
22		*(u16 *)addr = __opcode_to_mem_thumb16(insn);
23		size = sizeof(u16);
24	} else if (thumb2 && ((uintptr_t)addr & 2)) {
25		u16 first = __opcode_thumb32_first(insn);
26		u16 second = __opcode_thumb32_second(insn);
27		u16 *addrh = addr;
28
29		addrh[0] = __opcode_to_mem_thumb16(first);
30		addrh[1] = __opcode_to_mem_thumb16(second);
31
32		size = sizeof(u32);
33	} else {
34		if (thumb2)
35			insn = __opcode_to_mem_thumb32(insn);
36		else
37			insn = __opcode_to_mem_arm(insn);
38
39		*(u32 *)addr = insn;
40		size = sizeof(u32);
41	}
42
43	flush_icache_range((uintptr_t)(addr),
44			   (uintptr_t)(addr) + size);
45}
46
47static int __kprobes patch_text_stop_machine(void *data)
48{
49	struct patch *patch = data;
50
51	__patch_text(patch->addr, patch->insn);
52
53	return 0;
54}
55
56void __kprobes patch_text(void *addr, unsigned int insn)
57{
58	struct patch patch = {
59		.addr = addr,
60		.insn = insn,
61	};
62
63	if (cache_ops_need_broadcast()) {
64		stop_machine(patch_text_stop_machine, &patch, cpu_online_mask);
65	} else {
66		bool straddles_word = IS_ENABLED(CONFIG_THUMB2_KERNEL)
67				      && __opcode_is_thumb32(insn)
68				      && ((uintptr_t)addr & 2);
69
70		if (straddles_word)
71			stop_machine(patch_text_stop_machine, &patch, NULL);
72		else
73			__patch_text(addr, insn);
74	}
75}