linuxdebug/arch/csky/abiv1/alignment.c

355 lines
5.9 KiB
C

// SPDX-License-Identifier: GPL-2.0
// Copyright (C) 2018 Hangzhou C-SKY Microsystems co.,ltd.
#include <linux/kernel.h>
#include <linux/uaccess.h>
#include <linux/ptrace.h>
static int align_kern_enable = 1;
static int align_usr_enable = 1;
static int align_kern_count = 0;
static int align_usr_count = 0;
static inline uint32_t get_ptreg(struct pt_regs *regs, uint32_t rx)
{
return rx == 15 ? regs->lr : *((uint32_t *)&(regs->a0) - 2 + rx);
}
static inline void put_ptreg(struct pt_regs *regs, uint32_t rx, uint32_t val)
{
if (rx == 15)
regs->lr = val;
else
*((uint32_t *)&(regs->a0) - 2 + rx) = val;
}
/*
* Get byte-value from addr and set it to *valp.
*
* Success: return 0
* Failure: return 1
*/
static int ldb_asm(uint32_t addr, uint32_t *valp)
{
uint32_t val;
int err;
asm volatile (
"movi %0, 0\n"
"1:\n"
"ldb %1, (%2)\n"
"br 3f\n"
"2:\n"
"movi %0, 1\n"
"br 3f\n"
".section __ex_table,\"a\"\n"
".align 2\n"
".long 1b, 2b\n"
".previous\n"
"3:\n"
: "=&r"(err), "=r"(val)
: "r" (addr)
);
*valp = val;
return err;
}
/*
* Put byte-value to addr.
*
* Success: return 0
* Failure: return 1
*/
static int stb_asm(uint32_t addr, uint32_t val)
{
int err;
asm volatile (
"movi %0, 0\n"
"1:\n"
"stb %1, (%2)\n"
"br 3f\n"
"2:\n"
"movi %0, 1\n"
"br 3f\n"
".section __ex_table,\"a\"\n"
".align 2\n"
".long 1b, 2b\n"
".previous\n"
"3:\n"
: "=&r"(err)
: "r"(val), "r" (addr)
);
return err;
}
/*
* Get half-word from [rx + imm]
*
* Success: return 0
* Failure: return 1
*/
static int ldh_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
{
uint32_t byte0, byte1;
if (ldb_asm(addr, &byte0))
return 1;
addr += 1;
if (ldb_asm(addr, &byte1))
return 1;
byte0 |= byte1 << 8;
put_ptreg(regs, rz, byte0);
return 0;
}
/*
* Store half-word to [rx + imm]
*
* Success: return 0
* Failure: return 1
*/
static int sth_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
{
uint32_t byte0, byte1;
byte0 = byte1 = get_ptreg(regs, rz);
byte0 &= 0xff;
if (stb_asm(addr, byte0))
return 1;
addr += 1;
byte1 = (byte1 >> 8) & 0xff;
if (stb_asm(addr, byte1))
return 1;
return 0;
}
/*
* Get word from [rx + imm]
*
* Success: return 0
* Failure: return 1
*/
static int ldw_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
{
uint32_t byte0, byte1, byte2, byte3;
if (ldb_asm(addr, &byte0))
return 1;
addr += 1;
if (ldb_asm(addr, &byte1))
return 1;
addr += 1;
if (ldb_asm(addr, &byte2))
return 1;
addr += 1;
if (ldb_asm(addr, &byte3))
return 1;
byte0 |= byte1 << 8;
byte0 |= byte2 << 16;
byte0 |= byte3 << 24;
put_ptreg(regs, rz, byte0);
return 0;
}
/*
* Store word to [rx + imm]
*
* Success: return 0
* Failure: return 1
*/
static int stw_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
{
uint32_t byte0, byte1, byte2, byte3;
byte0 = byte1 = byte2 = byte3 = get_ptreg(regs, rz);
byte0 &= 0xff;
if (stb_asm(addr, byte0))
return 1;
addr += 1;
byte1 = (byte1 >> 8) & 0xff;
if (stb_asm(addr, byte1))
return 1;
addr += 1;
byte2 = (byte2 >> 16) & 0xff;
if (stb_asm(addr, byte2))
return 1;
addr += 1;
byte3 = (byte3 >> 24) & 0xff;
if (stb_asm(addr, byte3))
return 1;
return 0;
}
extern int fixup_exception(struct pt_regs *regs);
#define OP_LDH 0xc000
#define OP_STH 0xd000
#define OP_LDW 0x8000
#define OP_STW 0x9000
void csky_alignment(struct pt_regs *regs)
{
int ret;
uint16_t tmp;
uint32_t opcode = 0;
uint32_t rx = 0;
uint32_t rz = 0;
uint32_t imm = 0;
uint32_t addr = 0;
if (!user_mode(regs))
goto kernel_area;
if (!align_usr_enable) {
pr_err("%s user disabled.\n", __func__);
goto bad_area;
}
align_usr_count++;
ret = get_user(tmp, (uint16_t *)instruction_pointer(regs));
if (ret) {
pr_err("%s get_user failed.\n", __func__);
goto bad_area;
}
goto good_area;
kernel_area:
if (!align_kern_enable) {
pr_err("%s kernel disabled.\n", __func__);
goto bad_area;
}
align_kern_count++;
tmp = *(uint16_t *)instruction_pointer(regs);
good_area:
opcode = (uint32_t)tmp;
rx = opcode & 0xf;
imm = (opcode >> 4) & 0xf;
rz = (opcode >> 8) & 0xf;
opcode &= 0xf000;
if (rx == 0 || rx == 1 || rz == 0 || rz == 1)
goto bad_area;
switch (opcode) {
case OP_LDH:
addr = get_ptreg(regs, rx) + (imm << 1);
ret = ldh_c(regs, rz, addr);
break;
case OP_LDW:
addr = get_ptreg(regs, rx) + (imm << 2);
ret = ldw_c(regs, rz, addr);
break;
case OP_STH:
addr = get_ptreg(regs, rx) + (imm << 1);
ret = sth_c(regs, rz, addr);
break;
case OP_STW:
addr = get_ptreg(regs, rx) + (imm << 2);
ret = stw_c(regs, rz, addr);
break;
}
if (ret)
goto bad_area;
regs->pc += 2;
return;
bad_area:
if (!user_mode(regs)) {
if (fixup_exception(regs))
return;
bust_spinlocks(1);
pr_alert("%s opcode: %x, rz: %d, rx: %d, imm: %d, addr: %x.\n",
__func__, opcode, rz, rx, imm, addr);
show_regs(regs);
bust_spinlocks(0);
make_task_dead(SIGKILL);
}
force_sig_fault(SIGBUS, BUS_ADRALN, (void __user *)addr);
}
static struct ctl_table alignment_tbl[5] = {
{
.procname = "kernel_enable",
.data = &align_kern_enable,
.maxlen = sizeof(align_kern_enable),
.mode = 0666,
.proc_handler = &proc_dointvec
},
{
.procname = "user_enable",
.data = &align_usr_enable,
.maxlen = sizeof(align_usr_enable),
.mode = 0666,
.proc_handler = &proc_dointvec
},
{
.procname = "kernel_count",
.data = &align_kern_count,
.maxlen = sizeof(align_kern_count),
.mode = 0666,
.proc_handler = &proc_dointvec
},
{
.procname = "user_count",
.data = &align_usr_count,
.maxlen = sizeof(align_usr_count),
.mode = 0666,
.proc_handler = &proc_dointvec
},
{}
};
static struct ctl_table sysctl_table[2] = {
{
.procname = "csky_alignment",
.mode = 0555,
.child = alignment_tbl},
{}
};
static struct ctl_path sysctl_path[2] = {
{.procname = "csky"},
{}
};
static int __init csky_alignment_init(void)
{
register_sysctl_paths(sysctl_path, sysctl_table);
return 0;
}
arch_initcall(csky_alignment_init);