csky/abiv1/alignment.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 // Copyright (C) 2018 Hangzhou C-SKY Microsystems co.,ltd.
0003
0004 #include <linux/kernel.h>
0005 #include <linux/uaccess.h>
0006 #include <linux/ptrace.h>
0007
0008 static int align_kern_enable = 1;
0009 static int align_usr_enable = 1;
0010 static int align_kern_count = 0;
0011 static int align_usr_count = 0;
0012
0013 static inline uint32_t get_ptreg(struct pt_regs *regs, uint32_t rx)
0014 {
0015     return rx == 15 ? regs->lr : *((uint32_t *)&(regs->a0) - 2 + rx);
0016 }
0017
0018 static inline void put_ptreg(struct pt_regs *regs, uint32_t rx, uint32_t val)
0019 {
0020     if (rx == 15)
0021         regs->lr = val;
0022     else
0023         *((uint32_t *)&(regs->a0) - 2 + rx) = val;
0024 }
0025
0026 /*
0027  * Get byte-value from addr and set it to *valp.
0028  *
0029  * Success: return 0
0030  * Failure: return 1
0031  */
0032 static int ldb_asm(uint32_t addr, uint32_t *valp)
0033 {
0034     uint32_t val;
0035     int err;
0036
0037     asm volatile (
0038         "movi   %0, 0\n"
0039         "1:\n"
0040         "ldb    %1, (%2)\n"
0041         "br 3f\n"
0042         "2:\n"
0043         "movi   %0, 1\n"
0044         "br 3f\n"
0045         ".section __ex_table,\"a\"\n"
0046         ".align 2\n"
0047         ".long  1b, 2b\n"
0048         ".previous\n"
0049         "3:\n"
0050         : "=&r"(err), "=r"(val)
0051         : "r" (addr)
0052     );
0053
0054     *valp = val;
0055
0056     return err;
0057 }
0058
0059 /*
0060  * Put byte-value to addr.
0061  *
0062  * Success: return 0
0063  * Failure: return 1
0064  */
0065 static int stb_asm(uint32_t addr, uint32_t val)
0066 {
0067     int err;
0068
0069     asm volatile (
0070         "movi   %0, 0\n"
0071         "1:\n"
0072         "stb    %1, (%2)\n"
0073         "br 3f\n"
0074         "2:\n"
0075         "movi   %0, 1\n"
0076         "br 3f\n"
0077         ".section __ex_table,\"a\"\n"
0078         ".align 2\n"
0079         ".long  1b, 2b\n"
0080         ".previous\n"
0081         "3:\n"
0082         : "=&r"(err)
0083         : "r"(val), "r" (addr)
0084     );
0085
0086     return err;
0087 }
0088
0089 /*
0090  * Get half-word from [rx + imm]
0091  *
0092  * Success: return 0
0093  * Failure: return 1
0094  */
0095 static int ldh_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
0096 {
0097     uint32_t byte0, byte1;
0098
0099     if (ldb_asm(addr, &byte0))
0100         return 1;
0101     addr += 1;
0102     if (ldb_asm(addr, &byte1))
0103         return 1;
0104
0105     byte0 |= byte1 << 8;
0106     put_ptreg(regs, rz, byte0);
0107
0108     return 0;
0109 }
0110
0111 /*
0112  * Store half-word to [rx + imm]
0113  *
0114  * Success: return 0
0115  * Failure: return 1
0116  */
0117 static int sth_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
0118 {
0119     uint32_t byte0, byte1;
0120
0121     byte0 = byte1 = get_ptreg(regs, rz);
0122
0123     byte0 &= 0xff;
0124
0125     if (stb_asm(addr, byte0))
0126         return 1;
0127
0128     addr += 1;
0129     byte1 = (byte1 >> 8) & 0xff;
0130     if (stb_asm(addr, byte1))
0131         return 1;
0132
0133     return 0;
0134 }
0135
0136 /*
0137  * Get word from [rx + imm]
0138  *
0139  * Success: return 0
0140  * Failure: return 1
0141  */
0142 static int ldw_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
0143 {
0144     uint32_t byte0, byte1, byte2, byte3;
0145
0146     if (ldb_asm(addr, &byte0))
0147         return 1;
0148
0149     addr += 1;
0150     if (ldb_asm(addr, &byte1))
0151         return 1;
0152
0153     addr += 1;
0154     if (ldb_asm(addr, &byte2))
0155         return 1;
0156
0157     addr += 1;
0158     if (ldb_asm(addr, &byte3))
0159         return 1;
0160
0161     byte0 |= byte1 << 8;
0162     byte0 |= byte2 << 16;
0163     byte0 |= byte3 << 24;
0164
0165     put_ptreg(regs, rz, byte0);
0166
0167     return 0;
0168 }
0169
0170 /*
0171  * Store word to [rx + imm]
0172  *
0173  * Success: return 0
0174  * Failure: return 1
0175  */
0176 static int stw_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
0177 {
0178     uint32_t byte0, byte1, byte2, byte3;
0179
0180     byte0 = byte1 = byte2 = byte3 = get_ptreg(regs, rz);
0181
0182     byte0 &= 0xff;
0183
0184     if (stb_asm(addr, byte0))
0185         return 1;
0186
0187     addr += 1;
0188     byte1 = (byte1 >> 8) & 0xff;
0189     if (stb_asm(addr, byte1))
0190         return 1;
0191
0192     addr += 1;
0193     byte2 = (byte2 >> 16) & 0xff;
0194     if (stb_asm(addr, byte2))
0195         return 1;
0196
0197     addr += 1;
0198     byte3 = (byte3 >> 24) & 0xff;
0199     if (stb_asm(addr, byte3))
0200         return 1;
0201
0202     return 0;
0203 }
0204
0205 extern int fixup_exception(struct pt_regs *regs);
0206
0207 #define OP_LDH 0xc000
0208 #define OP_STH 0xd000
0209 #define OP_LDW 0x8000
0210 #define OP_STW 0x9000
0211
0212 void csky_alignment(struct pt_regs *regs)
0213 {
0214     int ret;
0215     uint16_t tmp;
0216     uint32_t opcode = 0;
0217     uint32_t rx     = 0;
0218     uint32_t rz     = 0;
0219     uint32_t imm    = 0;
0220     uint32_t addr   = 0;
0221
0222     if (!user_mode(regs))
0223         goto kernel_area;
0224
0225     if (!align_usr_enable) {
0226         pr_err("%s user disabled.\n", __func__);
0227         goto bad_area;
0228     }
0229
0230     align_usr_count++;
0231
0232     ret = get_user(tmp, (uint16_t *)instruction_pointer(regs));
0233     if (ret) {
0234         pr_err("%s get_user failed.\n", __func__);
0235         goto bad_area;
0236     }
0237
0238     goto good_area;
0239
0240 kernel_area:
0241     if (!align_kern_enable) {
0242         pr_err("%s kernel disabled.\n", __func__);
0243         goto bad_area;
0244     }
0245
0246     align_kern_count++;
0247
0248     tmp = *(uint16_t *)instruction_pointer(regs);
0249
0250 good_area:
0251     opcode = (uint32_t)tmp;
0252
0253     rx  = opcode & 0xf;
0254     imm = (opcode >> 4) & 0xf;
0255     rz  = (opcode >> 8) & 0xf;
0256     opcode &= 0xf000;
0257
0258     if (rx == 0 || rx == 1 || rz == 0 || rz == 1)
0259         goto bad_area;
0260
0261     switch (opcode) {
0262     case OP_LDH:
0263         addr = get_ptreg(regs, rx) + (imm << 1);
0264         ret = ldh_c(regs, rz, addr);
0265         break;
0266     case OP_LDW:
0267         addr = get_ptreg(regs, rx) + (imm << 2);
0268         ret = ldw_c(regs, rz, addr);
0269         break;
0270     case OP_STH:
0271         addr = get_ptreg(regs, rx) + (imm << 1);
0272         ret = sth_c(regs, rz, addr);
0273         break;
0274     case OP_STW:
0275         addr = get_ptreg(regs, rx) + (imm << 2);
0276         ret = stw_c(regs, rz, addr);
0277         break;
0278     }
0279
0280     if (ret)
0281         goto bad_area;
0282
0283     regs->pc += 2;
0284
0285     return;
0286
0287 bad_area:
0288     if (!user_mode(regs)) {
0289         if (fixup_exception(regs))
0290             return;
0291
0292         bust_spinlocks(1);
0293         pr_alert("%s opcode: %x, rz: %d, rx: %d, imm: %d, addr: %x.\n",
0294                 __func__, opcode, rz, rx, imm, addr);
0295         show_regs(regs);
0296         bust_spinlocks(0);
0297         make_task_dead(SIGKILL);
0298     }
0299
0300     force_sig_fault(SIGBUS, BUS_ADRALN, (void __user *)addr);
0301 }
0302
0303 static struct ctl_table alignment_tbl[5] = {
0304     {
0305         .procname = "kernel_enable",
0306         .data = &align_kern_enable,
0307         .maxlen = sizeof(align_kern_enable),
0308         .mode = 0666,
0309         .proc_handler = &proc_dointvec
0310     },
0311     {
0312         .procname = "user_enable",
0313         .data = &align_usr_enable,
0314         .maxlen = sizeof(align_usr_enable),
0315         .mode = 0666,
0316         .proc_handler = &proc_dointvec
0317     },
0318     {
0319         .procname = "kernel_count",
0320         .data = &align_kern_count,
0321         .maxlen = sizeof(align_kern_count),
0322         .mode = 0666,
0323         .proc_handler = &proc_dointvec
0324     },
0325     {
0326         .procname = "user_count",
0327         .data = &align_usr_count,
0328         .maxlen = sizeof(align_usr_count),
0329         .mode = 0666,
0330         .proc_handler = &proc_dointvec
0331     },
0332     {}
0333 };
0334
0335 static struct ctl_table sysctl_table[2] = {
0336     {
0337      .procname = "csky_alignment",
0338      .mode = 0555,
0339      .child = alignment_tbl},
0340     {}
0341 };
0342
0343 static struct ctl_path sysctl_path[2] = {
0344     {.procname = "csky"},
0345     {}
0346 };
0347
0348 static int __init csky_alignment_init(void)
0349 {
0350     register_sysctl_paths(sysctl_path, sysctl_table);
0351     return 0;
0352 }
0353
0354 arch_initcall(csky_alignment_init);