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