arch/csky/abiv1/alignment.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 // Copyright (C) 2018 Hangzhou C-SKY Microsystems co.,ltd.
   3
   4 #include <linux/kernel.h>
   5 #include <linux/uaccess.h>
   6 #include <linux/ptrace.h>
   7
   8 static int align_kern_enable = 1;
   9 static int align_usr_enable = 1;
  10 static int align_kern_count = 0;
  11 static int align_usr_count = 0;
  12
  13 static inline uint32_t get_ptreg(struct pt_regs *regs, uint32_t rx)
  14 {
  15         return rx == 15 ? regs->lr : *((uint32_t *)&(regs->a0) - 2 + rx);
  16 }
  17
  18 static inline void put_ptreg(struct pt_regs *regs, uint32_t rx, uint32_t val)
  19 {
  20         if (rx == 15)
  21                 regs->lr = val;
  22         else
  23                 *((uint32_t *)&(regs->a0) - 2 + rx) = val;
  24 }
  25
  26 /*
  27  * Get byte-value from addr and set it to *valp.
  28  *
  29  * Success: return 0
  30  * Failure: return 1
  31  */
  32 static int ldb_asm(uint32_t addr, uint32_t *valp)
  33 {
  34         uint32_t val;
  35         int err;
  36
  37         asm volatile (
  38                 "movi   %0, 0\n"
  39                 "1:\n"
  40                 "ldb    %1, (%2)\n"
  41                 "br     3f\n"
  42                 "2:\n"
  43                 "movi   %0, 1\n"
  44                 "br     3f\n"
  45                 ".section __ex_table,\"a\"\n"
  46                 ".align 2\n"
  47                 ".long  1b, 2b\n"
  48                 ".previous\n"
  49                 "3:\n"
  50                 : "=&r"(err), "=r"(val)
  51                 : "r" (addr)
  52         );
  53
  54         *valp = val;
  55
  56         return err;
  57 }
  58
  59 /*
  60  * Put byte-value to addr.
  61  *
  62  * Success: return 0
  63  * Failure: return 1
  64  */
  65 static int stb_asm(uint32_t addr, uint32_t val)
  66 {
  67         int err;
  68
  69         asm volatile (
  70                 "movi   %0, 0\n"
  71                 "1:\n"
  72                 "stb    %1, (%2)\n"
  73                 "br     3f\n"
  74                 "2:\n"
  75                 "movi   %0, 1\n"
  76                 "br     3f\n"
  77                 ".section __ex_table,\"a\"\n"
  78                 ".align 2\n"
  79                 ".long  1b, 2b\n"
  80                 ".previous\n"
  81                 "3:\n"
  82                 : "=&r"(err)
  83                 : "r"(val), "r" (addr)
  84         );
  85
  86         return err;
  87 }
  88
  89 /*
  90  * Get half-word from [rx + imm]
  91  *
  92  * Success: return 0
  93  * Failure: return 1
  94  */
  95 static int ldh_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
  96 {
  97         uint32_t byte0, byte1;
  98
  99         if (ldb_asm(addr, &byte0))
 100                 return 1;
 101         addr += 1;
 102         if (ldb_asm(addr, &byte1))
 103                 return 1;
 104
 105         byte0 |= byte1 << 8;
 106         put_ptreg(regs, rz, byte0);
 107
 108         return 0;
 109 }
 110
 111 /*
 112  * Store half-word to [rx + imm]
 113  *
 114  * Success: return 0
 115  * Failure: return 1
 116  */
 117 static int sth_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
 118 {
 119         uint32_t byte0, byte1;
 120
 121         byte0 = byte1 = get_ptreg(regs, rz);
 122
 123         byte0 &= 0xff;
 124
 125         if (stb_asm(addr, byte0))
 126                 return 1;
 127
 128         addr += 1;
 129         byte1 = (byte1 >> 8) & 0xff;
 130         if (stb_asm(addr, byte1))
 131                 return 1;
 132
 133         return 0;
 134 }
 135
 136 /*
 137  * Get word from [rx + imm]
 138  *
 139  * Success: return 0
 140  * Failure: return 1
 141  */
 142 static int ldw_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
 143 {
 144         uint32_t byte0, byte1, byte2, byte3;
 145
 146         if (ldb_asm(addr, &byte0))
 147                 return 1;
 148
 149         addr += 1;
 150         if (ldb_asm(addr, &byte1))
 151                 return 1;
 152
 153         addr += 1;
 154         if (ldb_asm(addr, &byte2))
 155                 return 1;
 156
 157         addr += 1;
 158         if (ldb_asm(addr, &byte3))
 159                 return 1;
 160
 161         byte0 |= byte1 << 8;
 162         byte0 |= byte2 << 16;
 163         byte0 |= byte3 << 24;
 164
 165         put_ptreg(regs, rz, byte0);
 166
 167         return 0;
 168 }
 169
 170 /*
 171  * Store word to [rx + imm]
 172  *
 173  * Success: return 0
 174  * Failure: return 1
 175  */
 176 static int stw_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
 177 {
 178         uint32_t byte0, byte1, byte2, byte3;
 179
 180         byte0 = byte1 = byte2 = byte3 = get_ptreg(regs, rz);
 181
 182         byte0 &= 0xff;
 183
 184         if (stb_asm(addr, byte0))
 185                 return 1;
 186
 187         addr += 1;
 188         byte1 = (byte1 >> 8) & 0xff;
 189         if (stb_asm(addr, byte1))
 190                 return 1;
 191
 192         addr += 1;
 193         byte2 = (byte2 >> 16) & 0xff;
 194         if (stb_asm(addr, byte2))
 195                 return 1;
 196
 197         addr += 1;
 198         byte3 = (byte3 >> 24) & 0xff;
 199         if (stb_asm(addr, byte3))
 200                 return 1;
 201
 202         return 0;
 203 }
 204
 205 extern int fixup_exception(struct pt_regs *regs);
 206
 207 #define OP_LDH 0xc000
 208 #define OP_STH 0xd000
 209 #define OP_LDW 0x8000
 210 #define OP_STW 0x9000
 211
 212 void csky_alignment(struct pt_regs *regs)
 213 {
 214         int ret;
 215         uint16_t tmp;
 216         uint32_t opcode = 0;
 217         uint32_t rx     = 0;
 218         uint32_t rz     = 0;
 219         uint32_t imm    = 0;
 220         uint32_t addr   = 0;
 221
 222         if (!user_mode(regs))
 223                 goto kernel_area;
 224
 225         if (!align_usr_enable) {
 226                 pr_err("%s user disabled.\n", __func__);
 227                 goto bad_area;
 228         }
 229
 230         align_usr_count++;
 231
 232         ret = get_user(tmp, (uint16_t *)instruction_pointer(regs));
 233         if (ret) {
 234                 pr_err("%s get_user failed.\n", __func__);
 235                 goto bad_area;
 236         }
 237
 238         goto good_area;
 239
 240 kernel_area:
 241         if (!align_kern_enable) {
 242                 pr_err("%s kernel disabled.\n", __func__);
 243                 goto bad_area;
 244         }
 245
 246         align_kern_count++;
 247
 248         tmp = *(uint16_t *)instruction_pointer(regs);
 249
 250 good_area:
 251         opcode = (uint32_t)tmp;
 252
 253         rx  = opcode & 0xf;
 254         imm = (opcode >> 4) & 0xf;
 255         rz  = (opcode >> 8) & 0xf;
 256         opcode &= 0xf000;
 257
 258         if (rx == 0 || rx == 1 || rz == 0 || rz == 1)
 259                 goto bad_area;
 260
 261         switch (opcode) {
 262         case OP_LDH:
 263                 addr = get_ptreg(regs, rx) + (imm << 1);
 264                 ret = ldh_c(regs, rz, addr);
 265                 break;
 266         case OP_LDW:
 267                 addr = get_ptreg(regs, rx) + (imm << 2);
 268                 ret = ldw_c(regs, rz, addr);
 269                 break;
 270         case OP_STH:
 271                 addr = get_ptreg(regs, rx) + (imm << 1);
 272                 ret = sth_c(regs, rz, addr);
 273                 break;
 274         case OP_STW:
 275                 addr = get_ptreg(regs, rx) + (imm << 2);
 276                 ret = stw_c(regs, rz, addr);
 277                 break;
 278         }
 279
 280         if (ret)
 281                 goto bad_area;
 282
 283         regs->pc += 2;
 284
 285         return;
 286
 287 bad_area:
 288         if (!user_mode(regs)) {
 289                 if (fixup_exception(regs))
 290                         return;
 291
 292                 bust_spinlocks(1);
 293                 pr_alert("%s opcode: %x, rz: %d, rx: %d, imm: %d, addr: %x.\n",
 294                                 __func__, opcode, rz, rx, imm, addr);
 295                 show_regs(regs);
 296                 bust_spinlocks(0);
 297                 do_exit(SIGKILL);
 298         }
 299
 300         force_sig_fault(SIGBUS, BUS_ADRALN, (void __user *)addr);
 301 }
 302
 303 static struct ctl_table alignment_tbl[5] = {
 304         {
 305                 .procname = "kernel_enable",
 306                 .data = &align_kern_enable,
 307                 .maxlen = sizeof(align_kern_enable),
 308                 .mode = 0666,
 309                 .proc_handler = &proc_dointvec
 310         },
 311         {
 312                 .procname = "user_enable",
 313                 .data = &align_usr_enable,
 314                 .maxlen = sizeof(align_usr_enable),
 315                 .mode = 0666,
 316                 .proc_handler = &proc_dointvec
 317         },
 318         {
 319                 .procname = "kernel_count",
 320                 .data = &align_kern_count,
 321                 .maxlen = sizeof(align_kern_count),
 322                 .mode = 0666,
 323                 .proc_handler = &proc_dointvec
 324         },
 325         {
 326                 .procname = "user_count",
 327                 .data = &align_usr_count,
 328                 .maxlen = sizeof(align_usr_count),
 329                 .mode = 0666,
 330                 .proc_handler = &proc_dointvec
 331         },
 332         {}
 333 };
 334
 335 static struct ctl_table sysctl_table[2] = {
 336         {
 337          .procname = "csky_alignment",
 338          .mode = 0555,
 339          .child = alignment_tbl},
 340         {}
 341 };
 342
 343 static struct ctl_path sysctl_path[2] = {
 344         {.procname = "csky"},
 345         {}
 346 };
 347
 348 static int __init csky_alignment_init(void)
 349 {
 350         register_sysctl_paths(sysctl_path, sysctl_table);
 351         return 0;
 352 }
 353
 354 arch_initcall(csky_alignment_init);