arch/nds32/mm/alignment.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 // Copyright (C) 2005-2017 Andes Technology Corporation
   3
   4 #include <linux/proc_fs.h>
   5 #include <linux/uaccess.h>
   6 #include <linux/sysctl.h>
   7 #include <asm/unaligned.h>
   8
   9 #define DEBUG(enable, tagged, ...)                              \
  10         do{                                                     \
  11                 if (enable) {                                   \
  12                         if (tagged)                             \
  13                         pr_warn("[ %30s() ] ", __func__);       \
  14                         pr_warn(__VA_ARGS__);                   \
  15                 }                                               \
  16         } while (0)
  17
  18 #define RT(inst)        (((inst) >> 20) & 0x1FUL)
  19 #define RA(inst)        (((inst) >> 15) & 0x1FUL)
  20 #define RB(inst)        (((inst) >> 10) & 0x1FUL)
  21 #define SV(inst)        (((inst) >> 8) & 0x3UL)
  22 #define IMM(inst)       (((inst) >> 0) & 0x3FFFUL)
  23
  24 #define RA3(inst)       (((inst) >> 3) & 0x7UL)
  25 #define RT3(inst)       (((inst) >> 6) & 0x7UL)
  26 #define IMM3U(inst)     (((inst) >> 0) & 0x7UL)
  27
  28 #define RA5(inst)       (((inst) >> 0) & 0x1FUL)
  29 #define RT4(inst)       (((inst) >> 5) & 0xFUL)
  30
  31 #define __get8_data(val,addr,err)       \
  32         __asm__(                                        \
  33         "1:     lbi.bi  %1, [%2], #1\n"                 \
  34         "2:\n"                                          \
  35         "       .pushsection .text.fixup,\"ax\"\n"      \
  36         "       .align  2\n"                            \
  37         "3:     movi    %0, #1\n"                       \
  38         "       j       2b\n"                           \
  39         "       .popsection\n"                          \
  40         "       .pushsection __ex_table,\"a\"\n"        \
  41         "       .align  3\n"                            \
  42         "       .long   1b, 3b\n"                       \
  43         "       .popsection\n"                          \
  44         : "=r" (err), "=&r" (val), "=r" (addr)          \
  45         : "0" (err), "2" (addr))
  46
  47 #define get16_data(addr, val_ptr)                               \
  48         do {                                                    \
  49                 unsigned int err = 0, v, a = addr;              \
  50                 __get8_data(v,a,err);                           \
  51                 *val_ptr =  v << 0;                             \
  52                 __get8_data(v,a,err);                           \
  53                 *val_ptr |= v << 8;                             \
  54                 if (err)                                        \
  55                         goto fault;                             \
  56                 *val_ptr = le16_to_cpu(*val_ptr);               \
  57         } while(0)
  58
  59 #define get32_data(addr, val_ptr)                               \
  60         do {                                                    \
  61                 unsigned int err = 0, v, a = addr;              \
  62                 __get8_data(v,a,err);                           \
  63                 *val_ptr =  v << 0;                             \
  64                 __get8_data(v,a,err);                           \
  65                 *val_ptr |= v << 8;                             \
  66                 __get8_data(v,a,err);                           \
  67                 *val_ptr |= v << 16;                            \
  68                 __get8_data(v,a,err);                           \
  69                 *val_ptr |= v << 24;                            \
  70                 if (err)                                        \
  71                         goto fault;                             \
  72                 *val_ptr = le32_to_cpu(*val_ptr);               \
  73         } while(0)
  74
  75 #define get_data(addr, val_ptr, len)                            \
  76         if (len == 2)                                           \
  77                 get16_data(addr, val_ptr);                      \
  78         else                                                    \
  79                 get32_data(addr, val_ptr);
  80
  81 #define set16_data(addr, val)                                   \
  82         do {                                                    \
  83                 unsigned int err = 0, *ptr = addr ;             \
  84                 val = le32_to_cpu(val);                         \
  85                 __asm__(                                        \
  86                 "1:     sbi.bi  %2, [%1], #1\n"                 \
  87                 "       srli    %2, %2, #8\n"                   \
  88                 "2:     sbi     %2, [%1]\n"                     \
  89                 "3:\n"                                          \
  90                 "       .pushsection .text.fixup,\"ax\"\n"      \
  91                 "       .align  2\n"                            \
  92                 "4:     movi    %0, #1\n"                       \
  93                 "       j       3b\n"                           \
  94                 "       .popsection\n"                          \
  95                 "       .pushsection __ex_table,\"a\"\n"        \
  96                 "       .align  3\n"                            \
  97                 "       .long   1b, 4b\n"                       \
  98                 "       .long   2b, 4b\n"                       \
  99                 "       .popsection\n"                          \
 100                 : "=r" (err), "+r" (ptr), "+r" (val)            \
 101                 : "0" (err)                                     \
 102                 );                                              \
 103                 if (err)                                        \
 104                         goto fault;                             \
 105         } while(0)
 106
 107 #define set32_data(addr, val)                                   \
 108         do {                                                    \
 109                 unsigned int err = 0, *ptr = addr ;             \
 110                 val = le32_to_cpu(val);                         \
 111                 __asm__(                                        \
 112                 "1:     sbi.bi  %2, [%1], #1\n"                 \
 113                 "       srli    %2, %2, #8\n"                   \
 114                 "2:     sbi.bi  %2, [%1], #1\n"                 \
 115                 "       srli    %2, %2, #8\n"                   \
 116                 "3:     sbi.bi  %2, [%1], #1\n"                 \
 117                 "       srli    %2, %2, #8\n"                   \
 118                 "4:     sbi     %2, [%1]\n"                     \
 119                 "5:\n"                                          \
 120                 "       .pushsection .text.fixup,\"ax\"\n"      \
 121                 "       .align  2\n"                            \
 122                 "6:     movi    %0, #1\n"                       \
 123                 "       j       5b\n"                           \
 124                 "       .popsection\n"                          \
 125                 "       .pushsection __ex_table,\"a\"\n"        \
 126                 "       .align  3\n"                            \
 127                 "       .long   1b, 6b\n"                       \
 128                 "       .long   2b, 6b\n"                       \
 129                 "       .long   3b, 6b\n"                       \
 130                 "       .long   4b, 6b\n"                       \
 131                 "       .popsection\n"                          \
 132                 : "=r" (err), "+r" (ptr), "+r" (val)            \
 133                 : "0" (err)                                     \
 134                 );                                              \
 135                 if (err)                                        \
 136                         goto fault;                             \
 137         } while(0)
 138 #define set_data(addr, val, len)                                \
 139         if (len == 2)                                           \
 140                 set16_data(addr, val);                          \
 141         else                                                    \
 142                 set32_data(addr, val);
 143 #define NDS32_16BIT_INSTRUCTION 0x80000000
 144
 145 extern pte_t va_present(struct mm_struct *mm, unsigned long addr);
 146 extern pte_t va_kernel_present(unsigned long addr);
 147 extern int va_readable(struct pt_regs *regs, unsigned long addr);
 148 extern int va_writable(struct pt_regs *regs, unsigned long addr);
 149
 150 int unalign_access_mode = 0, unalign_access_debug = 0;
 151
 152 static inline unsigned long *idx_to_addr(struct pt_regs *regs, int idx)
 153 {
 154         /* this should be consistent with ptrace.h */
 155         if (idx >= 0 && idx <= 25)      /* R0-R25 */
 156                 return &regs->uregs[0] + idx;
 157         else if (idx >= 28 && idx <= 30)        /* FP, GP, LP */
 158                 return &regs->fp + (idx - 28);
 159         else if (idx == 31)     /* SP */
 160                 return &regs->sp;
 161         else
 162                 return NULL;    /* cause a segfault */
 163 }
 164
 165 static inline unsigned long get_inst(unsigned long addr)
 166 {
 167         return be32_to_cpu(get_unaligned((u32 *) addr));
 168 }
 169
 170 static inline unsigned long sign_extend(unsigned long val, int len)
 171 {
 172         unsigned long ret = 0;
 173         unsigned char *s, *t;
 174         int i = 0;
 175
 176         val = cpu_to_le32(val);
 177
 178         s = (void *)&val;
 179         t = (void *)&ret;
 180
 181         while (i++ < len)
 182                 *t++ = *s++;
 183
 184         if (((*(t - 1)) & 0x80) && (i < 4)) {
 185
 186                 while (i++ <= 4)
 187                         *t++ = 0xff;
 188         }
 189
 190         return le32_to_cpu(ret);
 191 }
 192
 193 static inline int do_16(unsigned long inst, struct pt_regs *regs)
 194 {
 195         int imm, regular, load, len, addr_mode, idx_mode;
 196         unsigned long unaligned_addr, target_val, source_idx, target_idx,
 197             shift = 0;
 198         switch ((inst >> 9) & 0x3F) {
 199
 200         case 0x12:              /* LHI333    */
 201                 imm = 1;
 202                 regular = 1;
 203                 load = 1;
 204                 len = 2;
 205                 addr_mode = 3;
 206                 idx_mode = 3;
 207                 break;
 208         case 0x10:              /* LWI333    */
 209                 imm = 1;
 210                 regular = 1;
 211                 load = 1;
 212                 len = 4;
 213                 addr_mode = 3;
 214                 idx_mode = 3;
 215                 break;
 216         case 0x11:              /* LWI333.bi */
 217                 imm = 1;
 218                 regular = 0;
 219                 load = 1;
 220                 len = 4;
 221                 addr_mode = 3;
 222                 idx_mode = 3;
 223                 break;
 224         case 0x1A:              /* LWI450    */
 225                 imm = 0;
 226                 regular = 1;
 227                 load = 1;
 228                 len = 4;
 229                 addr_mode = 5;
 230                 idx_mode = 4;
 231                 break;
 232         case 0x16:              /* SHI333    */
 233                 imm = 1;
 234                 regular = 1;
 235                 load = 0;
 236                 len = 2;
 237                 addr_mode = 3;
 238                 idx_mode = 3;
 239                 break;
 240         case 0x14:              /* SWI333    */
 241                 imm = 1;
 242                 regular = 1;
 243                 load = 0;
 244                 len = 4;
 245                 addr_mode = 3;
 246                 idx_mode = 3;
 247                 break;
 248         case 0x15:              /* SWI333.bi */
 249                 imm = 1;
 250                 regular = 0;
 251                 load = 0;
 252                 len = 4;
 253                 addr_mode = 3;
 254                 idx_mode = 3;
 255                 break;
 256         case 0x1B:              /* SWI450    */
 257                 imm = 0;
 258                 regular = 1;
 259                 load = 0;
 260                 len = 4;
 261                 addr_mode = 5;
 262                 idx_mode = 4;
 263                 break;
 264
 265         default:
 266                 return -EFAULT;
 267         }
 268
 269         if (addr_mode == 3) {
 270                 unaligned_addr = *idx_to_addr(regs, RA3(inst));
 271                 source_idx = RA3(inst);
 272         } else {
 273                 unaligned_addr = *idx_to_addr(regs, RA5(inst));
 274                 source_idx = RA5(inst);
 275         }
 276
 277         if (idx_mode == 3)
 278                 target_idx = RT3(inst);
 279         else
 280                 target_idx = RT4(inst);
 281
 282         if (imm)
 283                 shift = IMM3U(inst) * len;
 284
 285         if (regular)
 286                 unaligned_addr += shift;
 287
 288         if (load) {
 289                 if (!access_ok(VERIFY_READ, (void *)unaligned_addr, len))
 290                         return -EACCES;
 291
 292                 get_data(unaligned_addr, &target_val, len);
 293                 *idx_to_addr(regs, target_idx) = target_val;
 294         } else {
 295                 if (!access_ok(VERIFY_WRITE, (void *)unaligned_addr, len))
 296                         return -EACCES;
 297                 target_val = *idx_to_addr(regs, target_idx);
 298                 set_data((void *)unaligned_addr, target_val, len);
 299         }
 300
 301         if (!regular)
 302                 *idx_to_addr(regs, source_idx) = unaligned_addr + shift;
 303         regs->ipc += 2;
 304
 305         return 0;
 306 fault:
 307         return -EACCES;
 308 }
 309
 310 static inline int do_32(unsigned long inst, struct pt_regs *regs)
 311 {
 312         int imm, regular, load, len, sign_ext;
 313         unsigned long unaligned_addr, target_val, shift;
 314
 315         unaligned_addr = *idx_to_addr(regs, RA(inst));
 316
 317         switch ((inst >> 25) << 1) {
 318
 319         case 0x02:              /* LHI       */
 320                 imm = 1;
 321                 regular = 1;
 322                 load = 1;
 323                 len = 2;
 324                 sign_ext = 0;
 325                 break;
 326         case 0x0A:              /* LHI.bi    */
 327                 imm = 1;
 328                 regular = 0;
 329                 load = 1;
 330                 len = 2;
 331                 sign_ext = 0;
 332                 break;
 333         case 0x22:              /* LHSI      */
 334                 imm = 1;
 335                 regular = 1;
 336                 load = 1;
 337                 len = 2;
 338                 sign_ext = 1;
 339                 break;
 340         case 0x2A:              /* LHSI.bi   */
 341                 imm = 1;
 342                 regular = 0;
 343                 load = 1;
 344                 len = 2;
 345                 sign_ext = 1;
 346                 break;
 347         case 0x04:              /* LWI       */
 348                 imm = 1;
 349                 regular = 1;
 350                 load = 1;
 351                 len = 4;
 352                 sign_ext = 0;
 353                 break;
 354         case 0x0C:              /* LWI.bi    */
 355                 imm = 1;
 356                 regular = 0;
 357                 load = 1;
 358                 len = 4;
 359                 sign_ext = 0;
 360                 break;
 361         case 0x12:              /* SHI       */
 362                 imm = 1;
 363                 regular = 1;
 364                 load = 0;
 365                 len = 2;
 366                 sign_ext = 0;
 367                 break;
 368         case 0x1A:              /* SHI.bi    */
 369                 imm = 1;
 370                 regular = 0;
 371                 load = 0;
 372                 len = 2;
 373                 sign_ext = 0;
 374                 break;
 375         case 0x14:              /* SWI       */
 376                 imm = 1;
 377                 regular = 1;
 378                 load = 0;
 379                 len = 4;
 380                 sign_ext = 0;
 381                 break;
 382         case 0x1C:              /* SWI.bi    */
 383                 imm = 1;
 384                 regular = 0;
 385                 load = 0;
 386                 len = 4;
 387                 sign_ext = 0;
 388                 break;
 389
 390         default:
 391                 switch (inst & 0xff) {
 392
 393                 case 0x01:      /* LH        */
 394                         imm = 0;
 395                         regular = 1;
 396                         load = 1;
 397                         len = 2;
 398                         sign_ext = 0;
 399                         break;
 400                 case 0x05:      /* LH.bi     */
 401                         imm = 0;
 402                         regular = 0;
 403                         load = 1;
 404                         len = 2;
 405                         sign_ext = 0;
 406                         break;
 407                 case 0x11:      /* LHS       */
 408                         imm = 0;
 409                         regular = 1;
 410                         load = 1;
 411                         len = 2;
 412                         sign_ext = 1;
 413                         break;
 414                 case 0x15:      /* LHS.bi    */
 415                         imm = 0;
 416                         regular = 0;
 417                         load = 1;
 418                         len = 2;
 419                         sign_ext = 1;
 420                         break;
 421                 case 0x02:      /* LW        */
 422                         imm = 0;
 423                         regular = 1;
 424                         load = 1;
 425                         len = 4;
 426                         sign_ext = 0;
 427                         break;
 428                 case 0x06:      /* LW.bi     */
 429                         imm = 0;
 430                         regular = 0;
 431                         load = 1;
 432                         len = 4;
 433                         sign_ext = 0;
 434                         break;
 435                 case 0x09:      /* SH        */
 436                         imm = 0;
 437                         regular = 1;
 438                         load = 0;
 439                         len = 2;
 440                         sign_ext = 0;
 441                         break;
 442                 case 0x0D:      /* SH.bi     */
 443                         imm = 0;
 444                         regular = 0;
 445                         load = 0;
 446                         len = 2;
 447                         sign_ext = 0;
 448                         break;
 449                 case 0x0A:      /* SW        */
 450                         imm = 0;
 451                         regular = 1;
 452                         load = 0;
 453                         len = 4;
 454                         sign_ext = 0;
 455                         break;
 456                 case 0x0E:      /* SW.bi     */
 457                         imm = 0;
 458                         regular = 0;
 459                         load = 0;
 460                         len = 4;
 461                         sign_ext = 0;
 462                         break;
 463
 464                 default:
 465                         return -EFAULT;
 466                 }
 467         }
 468
 469         if (imm)
 470                 shift = IMM(inst) * len;
 471         else
 472                 shift = *idx_to_addr(regs, RB(inst)) << SV(inst);
 473
 474         if (regular)
 475                 unaligned_addr += shift;
 476
 477         if (load) {
 478
 479                 if (!access_ok(VERIFY_READ, (void *)unaligned_addr, len))
 480                         return -EACCES;
 481
 482                 get_data(unaligned_addr, &target_val, len);
 483
 484                 if (sign_ext)
 485                         *idx_to_addr(regs, RT(inst)) =
 486                             sign_extend(target_val, len);
 487                 else
 488                         *idx_to_addr(regs, RT(inst)) = target_val;
 489         } else {
 490
 491                 if (!access_ok(VERIFY_WRITE, (void *)unaligned_addr, len))
 492                         return -EACCES;
 493
 494                 target_val = *idx_to_addr(regs, RT(inst));
 495                 set_data((void *)unaligned_addr, target_val, len);
 496         }
 497
 498         if (!regular)
 499                 *idx_to_addr(regs, RA(inst)) = unaligned_addr + shift;
 500
 501         regs->ipc += 4;
 502
 503         return 0;
 504 fault:
 505         return -EACCES;
 506 }
 507
 508 int do_unaligned_access(unsigned long addr, struct pt_regs *regs)
 509 {
 510         unsigned long inst;
 511         int ret = -EFAULT;
 512         mm_segment_t seg = get_fs();
 513
 514         inst = get_inst(regs->ipc);
 515
 516         DEBUG((unalign_access_debug > 0), 1,
 517               "Faulting addr: 0x%08lx, pc: 0x%08lx [inst: 0x%08lx ]\n", addr,
 518               regs->ipc, inst);
 519
 520         set_fs(USER_DS);
 521
 522         if (inst & NDS32_16BIT_INSTRUCTION)
 523                 ret = do_16((inst >> 16) & 0xffff, regs);
 524         else
 525                 ret = do_32(inst, regs);
 526         set_fs(seg);
 527
 528         return ret;
 529 }
 530
 531 #ifdef CONFIG_PROC_FS
 532
 533 static struct ctl_table alignment_tbl[3] = {
 534         {
 535          .procname = "enable",
 536          .data = &unalign_access_mode,
 537          .maxlen = sizeof(unalign_access_mode),
 538          .mode = 0666,
 539          .proc_handler = &proc_dointvec
 540         }
 541         ,
 542         {
 543          .procname = "debug_info",
 544          .data = &unalign_access_debug,
 545          .maxlen = sizeof(unalign_access_debug),
 546          .mode = 0644,
 547          .proc_handler = &proc_dointvec
 548         }
 549         ,
 550         {}
 551 };
 552
 553 static struct ctl_table nds32_sysctl_table[2] = {
 554         {
 555          .procname = "unaligned_acess",
 556          .mode = 0555,
 557          .child = alignment_tbl},
 558         {}
 559 };
 560
 561 static struct ctl_path nds32_path[2] = {
 562         {.procname = "nds32"},
 563         {}
 564 };
 565
 566 /*
 567  * Initialize nds32 alignment-correction interface
 568  */
 569 static int __init nds32_sysctl_init(void)
 570 {
 571         register_sysctl_paths(nds32_path, nds32_sysctl_table);
 572         return 0;
 573 }
 574
 575 __initcall(nds32_sysctl_init);
 576 #endif /* CONFIG_PROC_FS */