sw64: add kprobe support

/* SPDX-License-Identifier: GPL-2.0 */

/*

 *  Kernel Probes (KProbes)

 *  Based on arch/mips/include/asm/kprobes.h

 */

#ifndef _ASM_SW64_KPROBES_H

#define _ASM_SW64_KPROBES_H

#include <asm-generic/kprobes.h>

#define BREAK_KPROBE	0x40ffffff

#define BREAK_KPROBE_SS	0x40fffeff

#ifdef CONFIG_KPROBES

#include <linux/ptrace.h>

#include <linux/types.h>

#include <asm/cacheflush.h>

#include <asm/kdebug.h>

#define __ARCH_WANT_KPROBES_INSN_SLOT

struct kprobe;

struct pt_regs;

typedef u32 kprobe_opcode_t;

#define MAX_INSN_SIZE 2

#define flush_insn_slot(p)						\

do {									\

	if (p->addr)							\

		flush_icache_range((unsigned long)p->addr,		\

			(unsigned long)p->addr +			\

			(MAX_INSN_SIZE * sizeof(kprobe_opcode_t)));	\

} while (0)

#define kretprobe_blacklist_size 0

void arch_remove_kprobe(struct kprobe *p);

/* Architecture specific copy of original instruction*/

struct arch_specific_insn {

	/* copy of the original instruction */

	kprobe_opcode_t *insn;

	/*

	 * Set in kprobes code, initially to 0. If the instruction can be

	 * eumulated, this is set to 1, if not, to -1.

	 */

	int boostable;

};

struct prev_kprobe {

	struct kprobe *kp;

	unsigned long status;

};

#define SKIP_DELAYSLOT 0x0001

/* per-cpu kprobe control block */

struct kprobe_ctlblk {

	unsigned long kprobe_status;

	/* Per-thread fields, used while emulating branches */

	unsigned long flags;

	unsigned long target_pc;

	struct prev_kprobe prev_kprobe;

};

extern int kprobe_handler(struct pt_regs *regs);

extern int post_kprobe_handler(struct pt_regs *regs);

extern int kprobe_fault_handler(struct pt_regs *regs, unsigned long mmcsr);

#endif /* CONFIG_KPROBES */

#endif /* _ASM_SW64_KPROBES_H */

// SPDX-License-Identifier: GPL-2.0

/*

 * Copyright (C) 2019, serveros, linyue

 *

 * 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.

 *

 * 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 <http://www.gnu.org/licenses/>.

 */

#include <linux/spinlock.h>

#include <linux/kprobes.h>

//static DEFINE_RAW_SPINLOCK(patch_lock);

int __kprobes sw64_insn_read(void *addr, u32 *insnp)

{

	int ret;

	__le32 val;

	ret = copy_from_kernel_nofault(&val, addr, SW64_INSN_SIZE);

	if (!ret)

		*insnp = le32_to_cpu(val);

	return ret;

}

static int __kprobes __sw64_insn_write(void *addr, __le32 insn)

{

	void *waddr = addr;

	int ret;

	//raw_spin_lock_irqsave(&patch_lock, flags);

	ret = copy_to_kernel_nofault(waddr, &insn, SW64_INSN_SIZE);

	//raw_spin_unlock_irqrestore(&patch_lock, flags);

	return ret;

}

static int __kprobes __sw64_insn_double_write(void *addr, __le64 insn)

{

	void *waddr = addr;

	//unsigned long flags = 0;

	int ret;

	//raw_spin_lock_irqsave(&patch_lock, flags);

	ret = copy_to_kernel_nofault(waddr, &insn, 2 * SW64_INSN_SIZE);

	//raw_spin_unlock_irqrestore(&patch_lock, flags);

	return ret;

}

int __kprobes sw64_insn_write(void *addr, u32 insn)

{

	u32 *tp = addr;

	/* SW64 instructions must be word aligned */

	if ((uintptr_t)tp & 0x3)

		return -EINVAL;

	return __sw64_insn_write(addr, cpu_to_le32(insn));

}

int __kprobes sw64_insn_double_write(void *addr, u64 insn)

{

	u32 *tp = addr;

	/* SW64 instructions must be word aligned */

	if ((uintptr_t)tp & 0x3)

		return -EINVAL;

	return __sw64_insn_double_write(addr, cpu_to_le64(insn));

}

unsigned int __kprobes sw64_insn_nop(void)

{

	return SW64_BIS(R31, R31, R31);

}

unsigned int __kprobes sw64_insn_call(unsigned int ra, unsigned int rb)

{

	return SW64_CALL(ra, rb, 0);

}

unsigned int __kprobes sw64_insn_sys_call(unsigned int num)

{

	return  SW64_SYS_CALL(num);

}

/* 'pc' is the address of br instruction, not the +4 PC. 'new_pc' is the target address. */

unsigned int __kprobes sw64_insn_br(unsigned int ra, unsigned long pc, unsigned long new_pc)

{

	int offset = new_pc - pc;

	unsigned int disp, minus = 0x1fffff;

	if (!(offset <= BR_MAX_DISP && offset >= -BR_MAX_DISP))

		return -1;

	if (offset > 0)

		disp = (offset - 4) / 4;

	else

		disp = ~(-offset / 4) & minus;

	return SW64_BR(ra, disp);

}

# SPDX-License-Identifier: GPL-2.0

obj-$(CONFIG_KPROBES)           += kprobes.o decode-insn.o

obj-$(CONFIG_KPROBES_ON_FTRACE) += kprobes-ftrace.o

/* SPDX-License-Identifier: GPL-2.0 */

#ifndef _SW64_KERNEL_KPROBES_COMMON_H

#define _SW64_KERNEL_KPROBES_COMMON_H

extern bool sw64_insn_can_kprobe(kprobe_opcode_t *addr);

#endif /* _SW64_KERNEL_KPROBES_COMMON_H */

// SPDX-License-Identifier: GPL-2.0

/*

 * Based on arch/arm64/kernel/probes/decode-insn.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.

 *

 * 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.

 */

#include <linux/kprobes.h>

#include "common.h"

static bool __kprobes sw64_insn_is_steppable(u32 insn)

{

	/*

	 * Branch instructions will write a new value into the PC which is

	 * likely to be relative to the XOL address and therefore invalid.

	 * Deliberate generation of an exception during stepping is also not

	 * currently safe. Lastly, MSR instructions can do any number of nasty

	 * things we can't handle during single-stepping.

	 */

	if (sw64_insn_is_sys_call_b(insn) ||

		sw64_insn_is_sys_call(insn) ||

		sw64_insn_is_call(insn) ||

		sw64_insn_is_ret(insn) ||

		sw64_insn_is_jmp(insn) ||

		sw64_insn_is_br(insn) ||

		sw64_insn_is_bsr(insn) ||

		sw64_insn_is_memb(insn) ||

		sw64_insn_is_imemb(insn) ||

		sw64_insn_is_rtc(insn) ||

		sw64_insn_is_lldl(insn) ||

		sw64_insn_is_lldw(insn) ||

		sw64_insn_is_beq(insn) ||

		sw64_insn_is_bne(insn) ||

		sw64_insn_is_blt(insn) ||

		sw64_insn_is_ble(insn) ||

		sw64_insn_is_bgt(insn) ||

		sw64_insn_is_bge(insn) ||

		sw64_insn_is_blbc(insn) ||

		sw64_insn_is_blbs(insn) ||

		sw64_insn_is_fbeq(insn) ||

		sw64_insn_is_fbne(insn) ||

		sw64_insn_is_fblt(insn) ||

		sw64_insn_is_fble(insn) ||

		sw64_insn_is_fbgt(insn) ||

		sw64_insn_is_fbge(insn))

		return false;

	return true;

}

#ifdef CONFIG_KPROBES

//  lldl  rd_f

static bool __kprobes is_probed_between_atomic(kprobe_opcode_t *addr)

{

	int count = 0;

	unsigned long size = 0, offset = 0;

	kprobe_opcode_t *scan_start = NULL;

	if (kallsyms_lookup_size_offset((unsigned long)addr, &size, &offset))

		scan_start = addr - (offset / sizeof(kprobe_opcode_t));

	while (scan_start < addr) {

		if (sw64_insn_is_lldl(le32_to_cpu(*scan_start)) ||

				sw64_insn_is_lldw(le32_to_cpu(*scan_start)))

			count++;

		if (sw64_insn_is_rd_f(le32_to_cpu(*scan_start)))

			count--;

		scan_start++;

	}

	if (count)

		return false;

	return true;

}

bool __kprobes sw64_insn_can_kprobe(kprobe_opcode_t *addr)

{

	u32 insn = le32_to_cpu(*addr);

	if (!sw64_insn_is_steppable(insn)) {

		pr_warn("addr is not steppable\n");

		return false;

	}

#ifdef CONFIG_SUBARCH_C3B

	if (!is_probed_between_atomic(addr)) {

		pr_warn("addr between atomic can't probe\n");

		return false;

	}

#endif

	return true;

}

#endif