1
// SPDX-License-Identifier: GPL-2.0
2
/*
3
 * Copyright (C) 2020-2022 Loongson Technology Corporation Limited
4
 */
5
#include <linux/sizes.h>
6
#include <linux/uaccess.h>
7
#include <linux/set_memory.h>
8
#include <linux/stop_machine.h>
9

10
#include <asm/cacheflush.h>
11
#include <asm/inst.h>
12

13
static DEFINE_RAW_SPINLOCK(patch_lock);
14

15
void simu_pc(struct pt_regs *regs, union loongarch_instruction insn)
16
{
17
	unsigned long pc = regs->csr_era;
18
	unsigned int rd = insn.reg1i20_format.rd;
19
	unsigned int imm = insn.reg1i20_format.immediate;
20

21
	if (pc & 3) {
22
		pr_warn("%s: invalid pc 0x%lx\n", __func__, pc);
23
		return;
24
	}
25

26
	switch (insn.reg1i20_format.opcode) {
27
	case pcaddi_op:
28
		regs->regs[rd] = pc + sign_extend64(imm << 2, 21);
29
		break;
30
	case pcaddu12i_op:
31
		regs->regs[rd] = pc + sign_extend64(imm << 12, 31);
32
		break;
33
	case pcaddu18i_op:
34
		regs->regs[rd] = pc + sign_extend64(imm << 18, 37);
35
		break;
36
	case pcalau12i_op:
37
		regs->regs[rd] = pc + sign_extend64(imm << 12, 31);
38
		regs->regs[rd] &= ~((1 << 12) - 1);
39
		break;
40
	default:
41
		pr_info("%s: unknown opcode\n", __func__);
42
		return;
43
	}
44

45
	regs->csr_era += LOONGARCH_INSN_SIZE;
46
}
47

48
void simu_branch(struct pt_regs *regs, union loongarch_instruction insn)
49
{
50
	unsigned int imm, imm_l, imm_h, rd, rj;
51
	unsigned long pc = regs->csr_era;
52

53
	if (pc & 3) {
54
		pr_warn("%s: invalid pc 0x%lx\n", __func__, pc);
55
		return;
56
	}
57

58
	imm_l = insn.reg0i26_format.immediate_l;
59
	imm_h = insn.reg0i26_format.immediate_h;
60
	switch (insn.reg0i26_format.opcode) {
61
	case b_op:
62
		regs->csr_era = pc + sign_extend64((imm_h << 16 | imm_l) << 2, 27);
63
		return;
64
	case bl_op:
65
		regs->csr_era = pc + sign_extend64((imm_h << 16 | imm_l) << 2, 27);
66
		regs->regs[1] = pc + LOONGARCH_INSN_SIZE;
67
		return;
68
	}
69

70
	imm_l = insn.reg1i21_format.immediate_l;
71
	imm_h = insn.reg1i21_format.immediate_h;
72
	rj = insn.reg1i21_format.rj;
73
	switch (insn.reg1i21_format.opcode) {
74
	case beqz_op:
75
		if (regs->regs[rj] == 0)
76
			regs->csr_era = pc + sign_extend64((imm_h << 16 | imm_l) << 2, 22);
77
		else
78
			regs->csr_era = pc + LOONGARCH_INSN_SIZE;
79
		return;
80
	case bnez_op:
81
		if (regs->regs[rj] != 0)
82
			regs->csr_era = pc + sign_extend64((imm_h << 16 | imm_l) << 2, 22);
83
		else
84
			regs->csr_era = pc + LOONGARCH_INSN_SIZE;
85
		return;
86
	}
87

88
	imm = insn.reg2i16_format.immediate;
89
	rj = insn.reg2i16_format.rj;
90
	rd = insn.reg2i16_format.rd;
91
	switch (insn.reg2i16_format.opcode) {
92
	case beq_op:
93
		if (regs->regs[rj] == regs->regs[rd])
94
			regs->csr_era = pc + sign_extend64(imm << 2, 17);
95
		else
96
			regs->csr_era = pc + LOONGARCH_INSN_SIZE;
97
		break;
98
	case bne_op:
99
		if (regs->regs[rj] != regs->regs[rd])
100
			regs->csr_era = pc + sign_extend64(imm << 2, 17);
101
		else
102
			regs->csr_era = pc + LOONGARCH_INSN_SIZE;
103
		break;
104
	case blt_op:
105
		if ((long)regs->regs[rj] < (long)regs->regs[rd])
106
			regs->csr_era = pc + sign_extend64(imm << 2, 17);
107
		else
108
			regs->csr_era = pc + LOONGARCH_INSN_SIZE;
109
		break;
110
	case bge_op:
111
		if ((long)regs->regs[rj] >= (long)regs->regs[rd])
112
			regs->csr_era = pc + sign_extend64(imm << 2, 17);
113
		else
114
			regs->csr_era = pc + LOONGARCH_INSN_SIZE;
115
		break;
116
	case bltu_op:
117
		if (regs->regs[rj] < regs->regs[rd])
118
			regs->csr_era = pc + sign_extend64(imm << 2, 17);
119
		else
120
			regs->csr_era = pc + LOONGARCH_INSN_SIZE;
121
		break;
122
	case bgeu_op:
123
		if (regs->regs[rj] >= regs->regs[rd])
124
			regs->csr_era = pc + sign_extend64(imm << 2, 17);
125
		else
126
			regs->csr_era = pc + LOONGARCH_INSN_SIZE;
127
		break;
128
	case jirl_op:
129
		regs->csr_era = regs->regs[rj] + sign_extend64(imm << 2, 17);
130
		regs->regs[rd] = pc + LOONGARCH_INSN_SIZE;
131
		break;
132
	default:
133
		pr_info("%s: unknown opcode\n", __func__);
134
		return;
135
	}
136
}
137

138
bool insns_not_supported(union loongarch_instruction insn)
139
{
140
	switch (insn.reg3_format.opcode) {
141
	case amswapw_op ... ammindbdu_op:
142
		pr_notice("atomic memory access instructions are not supported\n");
143
		return true;
144
	case scq_op:
145
		pr_notice("sc.q instruction is not supported\n");
146
		return true;
147
	}
148

149
	switch (insn.reg2i14_format.opcode) {
150
	case llw_op:
151
	case lld_op:
152
	case scw_op:
153
	case scd_op:
154
		pr_notice("ll and sc instructions are not supported\n");
155
		return true;
156
	}
157

158
	switch (insn.reg2_format.opcode) {
159
	case llacqw_op:
160
	case llacqd_op:
161
	case screlw_op:
162
	case screld_op:
163
		pr_notice("llacq and screl instructions are not supported\n");
164
		return true;
165
	}
166

167
	switch (insn.reg1i21_format.opcode) {
168
	case bceqz_op:
169
		pr_notice("bceqz and bcnez instructions are not supported\n");
170
		return true;
171
	}
172

173
	return false;
174
}
175

176
bool insns_need_simulation(union loongarch_instruction insn)
177
{
178
	if (is_pc_ins(&insn))
179
		return true;
180

181
	if (is_branch_ins(&insn))
182
		return true;
183

184
	return false;
185
}
186

187
void arch_simulate_insn(union loongarch_instruction insn, struct pt_regs *regs)
188
{
189
	if (is_pc_ins(&insn))
190
		simu_pc(regs, insn);
191
	else if (is_branch_ins(&insn))
192
		simu_branch(regs, insn);
193
}
194

195
int larch_insn_read(void *addr, u32 *insnp)
196
{
197
	int ret;
198
	u32 val;
199

200
	ret = copy_from_kernel_nofault(&val, addr, LOONGARCH_INSN_SIZE);
201
	if (!ret)
202
		*insnp = val;
203

204
	return ret;
205
}
206

207
int larch_insn_write(void *addr, u32 insn)
208
{
209
	int ret;
210
	unsigned long flags = 0;
211

212
	raw_spin_lock_irqsave(&patch_lock, flags);
213
	ret = copy_to_kernel_nofault(addr, &insn, LOONGARCH_INSN_SIZE);
214
	raw_spin_unlock_irqrestore(&patch_lock, flags);
215

216
	return ret;
217
}
218

219
int larch_insn_patch_text(void *addr, u32 insn)
220
{
221
	int ret;
222
	u32 *tp = addr;
223

224
	if ((unsigned long)tp & 3)
225
		return -EINVAL;
226

227
	ret = larch_insn_write(tp, insn);
228
	if (!ret)
229
		flush_icache_range((unsigned long)tp,
230
				   (unsigned long)tp + LOONGARCH_INSN_SIZE);
231

232
	return ret;
233
}
234

235
struct insn_copy {
236
	void *dst;
237
	void *src;
238
	size_t len;
239
	unsigned int cpu;
240
};
241

242
static int text_copy_cb(void *data)
243
{
244
	int ret = 0;
245
	struct insn_copy *copy = data;
246

247
	if (smp_processor_id() == copy->cpu) {
248
		ret = copy_to_kernel_nofault(copy->dst, copy->src, copy->len);
249
		if (ret)
250
			pr_err("%s: operation failed\n", __func__);
251
	}
252

253
	flush_icache_range((unsigned long)copy->dst, (unsigned long)copy->dst + copy->len);
254

255
	return ret;
256
}
257

258
int larch_insn_text_copy(void *dst, void *src, size_t len)
259
{
260
	int ret = 0;
261
	size_t start, end;
262
	struct insn_copy copy = {
263
		.dst = dst,
264
		.src = src,
265
		.len = len,
266
		.cpu = smp_processor_id(),
267
	};
268

269
	start = round_down((size_t)dst, PAGE_SIZE);
270
	end   = round_up((size_t)dst + len, PAGE_SIZE);
271

272
	set_memory_rw(start, (end - start) / PAGE_SIZE);
273
	ret = stop_machine(text_copy_cb, &copy, cpu_online_mask);
274
	set_memory_rox(start, (end - start) / PAGE_SIZE);
275

276
	return ret;
277
}
278

279
u32 larch_insn_gen_nop(void)
280
{
281
	return INSN_NOP;
282
}
283

284
u32 larch_insn_gen_b(unsigned long pc, unsigned long dest)
285
{
286
	long offset = dest - pc;
287
	union loongarch_instruction insn;
288

289
	if ((offset & 3) || offset < -SZ_128M || offset >= SZ_128M) {
290
		pr_warn("The generated b instruction is out of range.\n");
291
		return INSN_BREAK;
292
	}
293

294
	emit_b(&insn, offset >> 2);
295

296
	return insn.word;
297
}
298

299
u32 larch_insn_gen_bl(unsigned long pc, unsigned long dest)
300
{
301
	long offset = dest - pc;
302
	union loongarch_instruction insn;
303

304
	if ((offset & 3) || offset < -SZ_128M || offset >= SZ_128M) {
305
		pr_warn("The generated bl instruction is out of range.\n");
306
		return INSN_BREAK;
307
	}
308

309
	emit_bl(&insn, offset >> 2);
310

311
	return insn.word;
312
}
313

314
u32 larch_insn_gen_break(int imm)
315
{
316
	union loongarch_instruction insn;
317

318
	if (imm < 0 || imm >= SZ_32K) {
319
		pr_warn("The generated break instruction is out of range.\n");
320
		return INSN_BREAK;
321
	}
322

323
	emit_break(&insn, imm);
324

325
	return insn.word;
326
}
327

328
u32 larch_insn_gen_or(enum loongarch_gpr rd, enum loongarch_gpr rj, enum loongarch_gpr rk)
329
{
330
	union loongarch_instruction insn;
331

332
	emit_or(&insn, rd, rj, rk);
333

334
	return insn.word;
335
}
336

337
u32 larch_insn_gen_move(enum loongarch_gpr rd, enum loongarch_gpr rj)
338
{
339
	return larch_insn_gen_or(rd, rj, 0);
340
}
341

342
u32 larch_insn_gen_lu12iw(enum loongarch_gpr rd, int imm)
343
{
344
	union loongarch_instruction insn;
345

346
	if (imm < -SZ_512K || imm >= SZ_512K) {
347
		pr_warn("The generated lu12i.w instruction is out of range.\n");
348
		return INSN_BREAK;
349
	}
350

351
	emit_lu12iw(&insn, rd, imm);
352

353
	return insn.word;
354
}
355

356
u32 larch_insn_gen_lu32id(enum loongarch_gpr rd, int imm)
357
{
358
	union loongarch_instruction insn;
359

360
	if (imm < -SZ_512K || imm >= SZ_512K) {
361
		pr_warn("The generated lu32i.d instruction is out of range.\n");
362
		return INSN_BREAK;
363
	}
364

365
	emit_lu32id(&insn, rd, imm);
366

367
	return insn.word;
368
}
369

370
u32 larch_insn_gen_lu52id(enum loongarch_gpr rd, enum loongarch_gpr rj, int imm)
371
{
372
	union loongarch_instruction insn;
373

374
	if (imm < -SZ_2K || imm >= SZ_2K) {
375
		pr_warn("The generated lu52i.d instruction is out of range.\n");
376
		return INSN_BREAK;
377
	}
378

379
	emit_lu52id(&insn, rd, rj, imm);
380

381
	return insn.word;
382
}
383

384
u32 larch_insn_gen_beq(enum loongarch_gpr rd, enum loongarch_gpr rj, int imm)
385
{
386
	union loongarch_instruction insn;
387

388
	if ((imm & 3) || imm < -SZ_128K || imm >= SZ_128K) {
389
		pr_warn("The generated beq instruction is out of range.\n");
390
		return INSN_BREAK;
391
	}
392

393
	emit_beq(&insn, rj, rd, imm >> 2);
394

395
	return insn.word;
396
}
397

398
u32 larch_insn_gen_bne(enum loongarch_gpr rd, enum loongarch_gpr rj, int imm)
399
{
400
	union loongarch_instruction insn;
401

402
	if ((imm & 3) || imm < -SZ_128K || imm >= SZ_128K) {
403
		pr_warn("The generated bne instruction is out of range.\n");
404
		return INSN_BREAK;
405
	}
406

407
	emit_bne(&insn, rj, rd, imm >> 2);
408

409
	return insn.word;
410
}
411

412
u32 larch_insn_gen_jirl(enum loongarch_gpr rd, enum loongarch_gpr rj, int imm)
413
{
414
	union loongarch_instruction insn;
415

416
	if ((imm & 3) || imm < -SZ_128K || imm >= SZ_128K) {
417
		pr_warn("The generated jirl instruction is out of range.\n");
418
		return INSN_BREAK;
419
	}
420

421
	emit_jirl(&insn, rd, rj, imm >> 2);
422

423
	return insn.word;
424
}
425

426