sbi_illegal_insn.c 3.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143
  1. /*
  2. * SPDX-License-Identifier: BSD-2-Clause
  3. *
  4. * Copyright (c) 2019 Western Digital Corporation or its affiliates.
  5. *
  6. * Authors:
  7. * Anup Patel <anup.patel@wdc.com>
  8. */
  9. #include <sbi/riscv_asm.h>
  10. #include <sbi/riscv_encoding.h>
  11. #include <sbi/sbi_bitops.h>
  12. #include <sbi/sbi_emulate_csr.h>
  13. #include <sbi/sbi_error.h>
  14. #include <sbi/sbi_illegal_insn.h>
  15. #include <sbi/sbi_trap.h>
  16. #include <sbi/sbi_unpriv.h>
  17. typedef int (*illegal_insn_func)(ulong insn, struct sbi_trap_regs *regs);
  18. static int truly_illegal_insn(ulong insn, struct sbi_trap_regs *regs)
  19. {
  20. struct sbi_trap_info trap;
  21. trap.epc = regs->mepc;
  22. trap.cause = CAUSE_ILLEGAL_INSTRUCTION;
  23. trap.tval = insn;
  24. trap.tval2 = 0;
  25. trap.tinst = 0;
  26. return sbi_trap_redirect(regs, &trap);
  27. }
  28. static int system_opcode_insn(ulong insn, struct sbi_trap_regs *regs)
  29. {
  30. int do_write, rs1_num = (insn >> 15) & 0x1f;
  31. ulong rs1_val = GET_RS1(insn, regs);
  32. int csr_num = (u32)insn >> 20;
  33. ulong csr_val, new_csr_val;
  34. /* TODO: Ensure that we got CSR read/write instruction */
  35. if (sbi_emulate_csr_read(csr_num, regs, &csr_val))
  36. return truly_illegal_insn(insn, regs);
  37. do_write = rs1_num;
  38. switch (GET_RM(insn)) {
  39. case 1:
  40. new_csr_val = rs1_val;
  41. do_write = 1;
  42. break;
  43. case 2:
  44. new_csr_val = csr_val | rs1_val;
  45. break;
  46. case 3:
  47. new_csr_val = csr_val & ~rs1_val;
  48. break;
  49. case 5:
  50. new_csr_val = rs1_num;
  51. do_write = 1;
  52. break;
  53. case 6:
  54. new_csr_val = csr_val | rs1_num;
  55. break;
  56. case 7:
  57. new_csr_val = csr_val & ~rs1_num;
  58. break;
  59. default:
  60. return truly_illegal_insn(insn, regs);
  61. };
  62. if (do_write && sbi_emulate_csr_write(csr_num, regs, new_csr_val))
  63. return truly_illegal_insn(insn, regs);
  64. SET_RD(insn, regs, csr_val);
  65. regs->mepc += 4;
  66. return 0;
  67. }
  68. static illegal_insn_func illegal_insn_table[32] = {
  69. truly_illegal_insn, /* 0 */
  70. truly_illegal_insn, /* 1 */
  71. truly_illegal_insn, /* 2 */
  72. truly_illegal_insn, /* 3 */
  73. truly_illegal_insn, /* 4 */
  74. truly_illegal_insn, /* 5 */
  75. truly_illegal_insn, /* 6 */
  76. truly_illegal_insn, /* 7 */
  77. truly_illegal_insn, /* 8 */
  78. truly_illegal_insn, /* 9 */
  79. truly_illegal_insn, /* 10 */
  80. truly_illegal_insn, /* 11 */
  81. truly_illegal_insn, /* 12 */
  82. truly_illegal_insn, /* 13 */
  83. truly_illegal_insn, /* 14 */
  84. truly_illegal_insn, /* 15 */
  85. truly_illegal_insn, /* 16 */
  86. truly_illegal_insn, /* 17 */
  87. truly_illegal_insn, /* 18 */
  88. truly_illegal_insn, /* 19 */
  89. truly_illegal_insn, /* 20 */
  90. truly_illegal_insn, /* 21 */
  91. truly_illegal_insn, /* 22 */
  92. truly_illegal_insn, /* 23 */
  93. truly_illegal_insn, /* 24 */
  94. truly_illegal_insn, /* 25 */
  95. truly_illegal_insn, /* 26 */
  96. truly_illegal_insn, /* 27 */
  97. system_opcode_insn, /* 28 */
  98. truly_illegal_insn, /* 29 */
  99. truly_illegal_insn, /* 30 */
  100. truly_illegal_insn /* 31 */
  101. };
  102. int sbi_illegal_insn_handler(ulong insn, struct sbi_trap_regs *regs)
  103. {
  104. struct sbi_trap_info uptrap;
  105. /*
  106. * We only deal with 32-bit (or longer) illegal instructions. If we
  107. * see instruction is zero OR instruction is 16-bit then we fetch and
  108. * check the instruction encoding using unprivilege access.
  109. *
  110. * The program counter (PC) in RISC-V world is always 2-byte aligned
  111. * so handling only 32-bit (or longer) illegal instructions also help
  112. * the case where MTVAL CSR contains instruction address for illegal
  113. * instruction trap.
  114. */
  115. if (unlikely((insn & 3) != 3)) {
  116. insn = sbi_get_insn(regs->mepc, &uptrap);
  117. if (uptrap.cause) {
  118. uptrap.epc = regs->mepc;
  119. return sbi_trap_redirect(regs, &uptrap);
  120. }
  121. if ((insn & 3) != 3)
  122. return truly_illegal_insn(insn, regs);
  123. }
  124. return illegal_insn_table[(insn & 0x7c) >> 2](insn, regs);
  125. }