sbi_illegal_insn.c 4.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167
  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_barrier.h>
  11. #include <sbi/riscv_encoding.h>
  12. #include <sbi/sbi_bitops.h>
  13. #include <sbi/sbi_emulate_csr.h>
  14. #include <sbi/sbi_error.h>
  15. #include <sbi/sbi_illegal_insn.h>
  16. #include <sbi/sbi_pmu.h>
  17. #include <sbi/sbi_trap.h>
  18. #include <sbi/sbi_unpriv.h>
  19. #include <sbi/sbi_console.h>
  20. typedef int (*illegal_insn_func)(ulong insn, struct sbi_trap_regs *regs);
  21. static int truly_illegal_insn(ulong insn, struct sbi_trap_regs *regs)
  22. {
  23. struct sbi_trap_info trap;
  24. trap.epc = regs->mepc;
  25. trap.cause = CAUSE_ILLEGAL_INSTRUCTION;
  26. trap.tval = insn;
  27. trap.tval2 = 0;
  28. trap.tinst = 0;
  29. trap.gva = 0;
  30. return sbi_trap_redirect(regs, &trap);
  31. }
  32. static int misc_mem_opcode_insn(ulong insn, struct sbi_trap_regs *regs)
  33. {
  34. /* Errata workaround: emulate `fence.tso` as `fence rw, rw`. */
  35. if ((insn & INSN_MASK_FENCE_TSO) == INSN_MATCH_FENCE_TSO) {
  36. smp_mb();
  37. regs->mepc += 4;
  38. return 0;
  39. }
  40. return truly_illegal_insn(insn, regs);
  41. }
  42. static int system_opcode_insn(ulong insn, struct sbi_trap_regs *regs)
  43. {
  44. int do_write, rs1_num = (insn >> 15) & 0x1f;
  45. ulong rs1_val = GET_RS1(insn, regs);
  46. int csr_num = (u32)insn >> 20;
  47. ulong prev_mode = (regs->mstatus & MSTATUS_MPP) >> MSTATUS_MPP_SHIFT;
  48. ulong csr_val, new_csr_val;
  49. if (prev_mode == PRV_M) {
  50. sbi_printf("%s: Failed to access CSR %#x from M-mode",
  51. __func__, csr_num);
  52. return SBI_EFAIL;
  53. }
  54. /* TODO: Ensure that we got CSR read/write instruction */
  55. if (sbi_emulate_csr_read(csr_num, regs, &csr_val))
  56. return truly_illegal_insn(insn, regs);
  57. do_write = rs1_num;
  58. switch (GET_RM(insn)) {
  59. case 1:
  60. new_csr_val = rs1_val;
  61. do_write = 1;
  62. break;
  63. case 2:
  64. new_csr_val = csr_val | rs1_val;
  65. break;
  66. case 3:
  67. new_csr_val = csr_val & ~rs1_val;
  68. break;
  69. case 5:
  70. new_csr_val = rs1_num;
  71. do_write = 1;
  72. break;
  73. case 6:
  74. new_csr_val = csr_val | rs1_num;
  75. break;
  76. case 7:
  77. new_csr_val = csr_val & ~rs1_num;
  78. break;
  79. default:
  80. return truly_illegal_insn(insn, regs);
  81. };
  82. if (do_write && sbi_emulate_csr_write(csr_num, regs, new_csr_val))
  83. return truly_illegal_insn(insn, regs);
  84. SET_RD(insn, regs, csr_val);
  85. regs->mepc += 4;
  86. return 0;
  87. }
  88. static const illegal_insn_func illegal_insn_table[32] = {
  89. truly_illegal_insn, /* 0 */
  90. truly_illegal_insn, /* 1 */
  91. truly_illegal_insn, /* 2 */
  92. misc_mem_opcode_insn, /* 3 */
  93. truly_illegal_insn, /* 4 */
  94. truly_illegal_insn, /* 5 */
  95. truly_illegal_insn, /* 6 */
  96. truly_illegal_insn, /* 7 */
  97. truly_illegal_insn, /* 8 */
  98. truly_illegal_insn, /* 9 */
  99. truly_illegal_insn, /* 10 */
  100. truly_illegal_insn, /* 11 */
  101. truly_illegal_insn, /* 12 */
  102. truly_illegal_insn, /* 13 */
  103. truly_illegal_insn, /* 14 */
  104. truly_illegal_insn, /* 15 */
  105. truly_illegal_insn, /* 16 */
  106. truly_illegal_insn, /* 17 */
  107. truly_illegal_insn, /* 18 */
  108. truly_illegal_insn, /* 19 */
  109. truly_illegal_insn, /* 20 */
  110. truly_illegal_insn, /* 21 */
  111. truly_illegal_insn, /* 22 */
  112. truly_illegal_insn, /* 23 */
  113. truly_illegal_insn, /* 24 */
  114. truly_illegal_insn, /* 25 */
  115. truly_illegal_insn, /* 26 */
  116. truly_illegal_insn, /* 27 */
  117. system_opcode_insn, /* 28 */
  118. truly_illegal_insn, /* 29 */
  119. truly_illegal_insn, /* 30 */
  120. truly_illegal_insn /* 31 */
  121. };
  122. int sbi_illegal_insn_handler(ulong insn, struct sbi_trap_regs *regs)
  123. {
  124. struct sbi_trap_info uptrap;
  125. /*
  126. * We only deal with 32-bit (or longer) illegal instructions. If we
  127. * see instruction is zero OR instruction is 16-bit then we fetch and
  128. * check the instruction encoding using unprivilege access.
  129. *
  130. * The program counter (PC) in RISC-V world is always 2-byte aligned
  131. * so handling only 32-bit (or longer) illegal instructions also help
  132. * the case where MTVAL CSR contains instruction address for illegal
  133. * instruction trap.
  134. */
  135. sbi_pmu_ctr_incr_fw(SBI_PMU_FW_ILLEGAL_INSN);
  136. if (unlikely((insn & 3) != 3)) {
  137. insn = sbi_get_insn(regs->mepc, &uptrap);
  138. if (uptrap.cause) {
  139. uptrap.epc = regs->mepc;
  140. return sbi_trap_redirect(regs, &uptrap);
  141. }
  142. if ((insn & 3) != 3)
  143. return truly_illegal_insn(insn, regs);
  144. }
  145. return illegal_insn_table[(insn & 0x7c) >> 2](insn, regs);
  146. }