sbi_misaligned_ldst.c 6.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248
  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/riscv_fp.h>
  12. #include <sbi/sbi_error.h>
  13. #include <sbi/sbi_misaligned_ldst.h>
  14. #include <sbi/sbi_pmu.h>
  15. #include <sbi/sbi_trap.h>
  16. #include <sbi/sbi_unpriv.h>
  17. union reg_data {
  18. u8 data_bytes[8];
  19. ulong data_ulong;
  20. u64 data_u64;
  21. };
  22. int sbi_misaligned_load_handler(ulong addr, ulong tval2, ulong tinst,
  23. struct sbi_trap_regs *regs)
  24. {
  25. ulong insn, insn_len;
  26. union reg_data val;
  27. struct sbi_trap_info uptrap;
  28. int i, fp = 0, shift = 0, len = 0;
  29. sbi_pmu_ctr_incr_fw(SBI_PMU_FW_MISALIGNED_LOAD);
  30. if (tinst & 0x1) {
  31. /*
  32. * Bit[0] == 1 implies trapped instruction value is
  33. * transformed instruction or custom instruction.
  34. */
  35. insn = tinst | INSN_16BIT_MASK;
  36. insn_len = (tinst & 0x2) ? INSN_LEN(insn) : 2;
  37. } else {
  38. /*
  39. * Bit[0] == 0 implies trapped instruction value is
  40. * zero or special value.
  41. */
  42. insn = sbi_get_insn(regs->mepc, &uptrap);
  43. if (uptrap.cause) {
  44. uptrap.epc = regs->mepc;
  45. return sbi_trap_redirect(regs, &uptrap);
  46. }
  47. insn_len = INSN_LEN(insn);
  48. }
  49. if ((insn & INSN_MASK_LW) == INSN_MATCH_LW) {
  50. len = 4;
  51. shift = 8 * (sizeof(ulong) - len);
  52. #if __riscv_xlen == 64
  53. } else if ((insn & INSN_MASK_LD) == INSN_MATCH_LD) {
  54. len = 8;
  55. shift = 8 * (sizeof(ulong) - len);
  56. } else if ((insn & INSN_MASK_LWU) == INSN_MATCH_LWU) {
  57. len = 4;
  58. #endif
  59. #ifdef __riscv_flen
  60. } else if ((insn & INSN_MASK_FLD) == INSN_MATCH_FLD) {
  61. fp = 1;
  62. len = 8;
  63. } else if ((insn & INSN_MASK_FLW) == INSN_MATCH_FLW) {
  64. fp = 1;
  65. len = 4;
  66. #endif
  67. } else if ((insn & INSN_MASK_LH) == INSN_MATCH_LH) {
  68. len = 2;
  69. shift = 8 * (sizeof(ulong) - len);
  70. } else if ((insn & INSN_MASK_LHU) == INSN_MATCH_LHU) {
  71. len = 2;
  72. #if __riscv_xlen >= 64
  73. } else if ((insn & INSN_MASK_C_LD) == INSN_MATCH_C_LD) {
  74. len = 8;
  75. shift = 8 * (sizeof(ulong) - len);
  76. insn = RVC_RS2S(insn) << SH_RD;
  77. } else if ((insn & INSN_MASK_C_LDSP) == INSN_MATCH_C_LDSP &&
  78. ((insn >> SH_RD) & 0x1f)) {
  79. len = 8;
  80. shift = 8 * (sizeof(ulong) - len);
  81. #endif
  82. } else if ((insn & INSN_MASK_C_LW) == INSN_MATCH_C_LW) {
  83. len = 4;
  84. shift = 8 * (sizeof(ulong) - len);
  85. insn = RVC_RS2S(insn) << SH_RD;
  86. } else if ((insn & INSN_MASK_C_LWSP) == INSN_MATCH_C_LWSP &&
  87. ((insn >> SH_RD) & 0x1f)) {
  88. len = 4;
  89. shift = 8 * (sizeof(ulong) - len);
  90. #ifdef __riscv_flen
  91. } else if ((insn & INSN_MASK_C_FLD) == INSN_MATCH_C_FLD) {
  92. fp = 1;
  93. len = 8;
  94. insn = RVC_RS2S(insn) << SH_RD;
  95. } else if ((insn & INSN_MASK_C_FLDSP) == INSN_MATCH_C_FLDSP) {
  96. fp = 1;
  97. len = 8;
  98. #if __riscv_xlen == 32
  99. } else if ((insn & INSN_MASK_C_FLW) == INSN_MATCH_C_FLW) {
  100. fp = 1;
  101. len = 4;
  102. insn = RVC_RS2S(insn) << SH_RD;
  103. } else if ((insn & INSN_MASK_C_FLWSP) == INSN_MATCH_C_FLWSP) {
  104. fp = 1;
  105. len = 4;
  106. #endif
  107. #endif
  108. } else {
  109. uptrap.epc = regs->mepc;
  110. uptrap.cause = CAUSE_MISALIGNED_LOAD;
  111. uptrap.tval = addr;
  112. uptrap.tval2 = tval2;
  113. uptrap.tinst = tinst;
  114. return sbi_trap_redirect(regs, &uptrap);
  115. }
  116. val.data_u64 = 0;
  117. for (i = 0; i < len; i++) {
  118. val.data_bytes[i] = sbi_load_u8((void *)(addr + i),
  119. &uptrap);
  120. if (uptrap.cause) {
  121. uptrap.epc = regs->mepc;
  122. return sbi_trap_redirect(regs, &uptrap);
  123. }
  124. }
  125. if (!fp)
  126. SET_RD(insn, regs, ((long)(val.data_ulong << shift)) >> shift);
  127. #ifdef __riscv_flen
  128. else if (len == 8)
  129. SET_F64_RD(insn, regs, val.data_u64);
  130. else
  131. SET_F32_RD(insn, regs, val.data_ulong);
  132. #endif
  133. regs->mepc += insn_len;
  134. return 0;
  135. }
  136. int sbi_misaligned_store_handler(ulong addr, ulong tval2, ulong tinst,
  137. struct sbi_trap_regs *regs)
  138. {
  139. ulong insn, insn_len;
  140. union reg_data val;
  141. struct sbi_trap_info uptrap;
  142. int i, len = 0;
  143. sbi_pmu_ctr_incr_fw(SBI_PMU_FW_MISALIGNED_STORE);
  144. if (tinst & 0x1) {
  145. /*
  146. * Bit[0] == 1 implies trapped instruction value is
  147. * transformed instruction or custom instruction.
  148. */
  149. insn = tinst | INSN_16BIT_MASK;
  150. insn_len = (tinst & 0x2) ? INSN_LEN(insn) : 2;
  151. } else {
  152. /*
  153. * Bit[0] == 0 implies trapped instruction value is
  154. * zero or special value.
  155. */
  156. insn = sbi_get_insn(regs->mepc, &uptrap);
  157. if (uptrap.cause) {
  158. uptrap.epc = regs->mepc;
  159. return sbi_trap_redirect(regs, &uptrap);
  160. }
  161. insn_len = INSN_LEN(insn);
  162. }
  163. val.data_ulong = GET_RS2(insn, regs);
  164. if ((insn & INSN_MASK_SW) == INSN_MATCH_SW) {
  165. len = 4;
  166. #if __riscv_xlen == 64
  167. } else if ((insn & INSN_MASK_SD) == INSN_MATCH_SD) {
  168. len = 8;
  169. #endif
  170. #ifdef __riscv_flen
  171. } else if ((insn & INSN_MASK_FSD) == INSN_MATCH_FSD) {
  172. len = 8;
  173. val.data_u64 = GET_F64_RS2(insn, regs);
  174. } else if ((insn & INSN_MASK_FSW) == INSN_MATCH_FSW) {
  175. len = 4;
  176. val.data_ulong = GET_F32_RS2(insn, regs);
  177. #endif
  178. } else if ((insn & INSN_MASK_SH) == INSN_MATCH_SH) {
  179. len = 2;
  180. #if __riscv_xlen >= 64
  181. } else if ((insn & INSN_MASK_C_SD) == INSN_MATCH_C_SD) {
  182. len = 8;
  183. val.data_ulong = GET_RS2S(insn, regs);
  184. } else if ((insn & INSN_MASK_C_SDSP) == INSN_MATCH_C_SDSP &&
  185. ((insn >> SH_RD) & 0x1f)) {
  186. len = 8;
  187. val.data_ulong = GET_RS2C(insn, regs);
  188. #endif
  189. } else if ((insn & INSN_MASK_C_SW) == INSN_MATCH_C_SW) {
  190. len = 4;
  191. val.data_ulong = GET_RS2S(insn, regs);
  192. } else if ((insn & INSN_MASK_C_SWSP) == INSN_MATCH_C_SWSP &&
  193. ((insn >> SH_RD) & 0x1f)) {
  194. len = 4;
  195. val.data_ulong = GET_RS2C(insn, regs);
  196. #ifdef __riscv_flen
  197. } else if ((insn & INSN_MASK_C_FSD) == INSN_MATCH_C_FSD) {
  198. len = 8;
  199. val.data_u64 = GET_F64_RS2S(insn, regs);
  200. } else if ((insn & INSN_MASK_C_FSDSP) == INSN_MATCH_C_FSDSP) {
  201. len = 8;
  202. val.data_u64 = GET_F64_RS2C(insn, regs);
  203. #if __riscv_xlen == 32
  204. } else if ((insn & INSN_MASK_C_FSW) == INSN_MATCH_C_FSW) {
  205. len = 4;
  206. val.data_ulong = GET_F32_RS2S(insn, regs);
  207. } else if ((insn & INSN_MASK_C_FSWSP) == INSN_MATCH_C_FSWSP) {
  208. len = 4;
  209. val.data_ulong = GET_F32_RS2C(insn, regs);
  210. #endif
  211. #endif
  212. } else {
  213. uptrap.epc = regs->mepc;
  214. uptrap.cause = CAUSE_MISALIGNED_STORE;
  215. uptrap.tval = addr;
  216. uptrap.tval2 = tval2;
  217. uptrap.tinst = tinst;
  218. return sbi_trap_redirect(regs, &uptrap);
  219. }
  220. for (i = 0; i < len; i++) {
  221. sbi_store_u8((void *)(addr + i), val.data_bytes[i],
  222. &uptrap);
  223. if (uptrap.cause) {
  224. uptrap.epc = regs->mepc;
  225. return sbi_trap_redirect(regs, &uptrap);
  226. }
  227. }
  228. regs->mepc += insn_len;
  229. return 0;
  230. }