sbi_misaligned_ldst.c 6.2 KB

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