|
4 | 4 | #include <stdlib.h> |
5 | 5 | #include <stdint.h> |
6 | 6 | #include <math.h> |
| 7 | +#include <fenv.h> |
7 | 8 | #include <assert.h> |
8 | 9 |
|
9 | 10 | #if defined(_WIN32) |
@@ -135,6 +136,124 @@ static inline void do_swr(uint8_t* rdram, gpr offset, gpr reg, gpr val) { |
135 | 136 | MEM_W(0, word_address) = masked_initial_value | shifted_input_value; |
136 | 137 | } |
137 | 138 |
|
| 139 | +static inline gpr do_ldl(uint8_t* rdram, gpr initial_value, gpr offset, gpr reg) { |
| 140 | + // Calculate the overall address |
| 141 | + gpr address = (offset + reg); |
| 142 | + |
| 143 | + // Load the aligned dword |
| 144 | + gpr dword_address = address & ~0x7; |
| 145 | + uint64_t loaded_value = load_doubleword(rdram, 0, dword_address); |
| 146 | + |
| 147 | + // Mask the existing value and shift the loaded value appropriately |
| 148 | + gpr misalignment = address & 0x7; |
| 149 | + gpr masked_value = initial_value & ~(0xFFFFFFFFFFFFFFFFu << (misalignment * 8)); |
| 150 | + loaded_value <<= (misalignment * 8); |
| 151 | + |
| 152 | + return masked_value | loaded_value; |
| 153 | +} |
| 154 | + |
| 155 | +static inline gpr do_ldr(uint8_t* rdram, gpr initial_value, gpr offset, gpr reg) { |
| 156 | + // Calculate the overall address |
| 157 | + gpr address = (offset + reg); |
| 158 | + |
| 159 | + // Load the aligned dword |
| 160 | + gpr dword_address = address & ~0x7; |
| 161 | + uint64_t loaded_value = load_doubleword(rdram, 0, dword_address); |
| 162 | + |
| 163 | + // Mask the existing value and shift the loaded value appropriately |
| 164 | + gpr misalignment = address & 0x7; |
| 165 | + gpr masked_value = initial_value & ~(0xFFFFFFFFFFFFFFFFu >> (56 - misalignment * 8)); |
| 166 | + loaded_value >>= (56 - misalignment * 8); |
| 167 | + |
| 168 | + return masked_value | loaded_value; |
| 169 | +} |
| 170 | + |
| 171 | +static inline void do_sdl(uint8_t* rdram, gpr offset, gpr reg, gpr val) { |
| 172 | + // Calculate the overall address |
| 173 | + gpr address = (offset + reg); |
| 174 | + |
| 175 | + // Get the initial value of the aligned dword |
| 176 | + gpr dword_address = address & ~0x7; |
| 177 | + uint64_t initial_value = load_doubleword(rdram, 0, dword_address); |
| 178 | + |
| 179 | + // Mask the initial value and shift the input value appropriately |
| 180 | + gpr misalignment = address & 0x7; |
| 181 | + uint64_t masked_initial_value = initial_value & ~(0xFFFFFFFFFFFFFFFFu >> (misalignment * 8)); |
| 182 | + uint64_t shifted_input_value = val >> (misalignment * 8); |
| 183 | + |
| 184 | + uint64_t ret = masked_initial_value | shifted_input_value; |
| 185 | + uint32_t lo = (uint32_t)ret; |
| 186 | + uint32_t hi = (uint32_t)(ret >> 32); |
| 187 | + |
| 188 | + MEM_W(0, dword_address + 4) = lo; |
| 189 | + MEM_W(0, dword_address + 0) = hi; |
| 190 | +} |
| 191 | + |
| 192 | +static inline void do_sdr(uint8_t* rdram, gpr offset, gpr reg, gpr val) { |
| 193 | + // Calculate the overall address |
| 194 | + gpr address = (offset + reg); |
| 195 | + |
| 196 | + // Get the initial value of the aligned dword |
| 197 | + gpr dword_address = address & ~0x7; |
| 198 | + uint64_t initial_value = load_doubleword(rdram, 0, dword_address); |
| 199 | + |
| 200 | + // Mask the initial value and shift the input value appropriately |
| 201 | + gpr misalignment = address & 0x7; |
| 202 | + uint64_t masked_initial_value = initial_value & ~(0xFFFFFFFFFFFFFFFFu << (56 - misalignment * 8)); |
| 203 | + uint64_t shifted_input_value = val << (56 - misalignment * 8); |
| 204 | + |
| 205 | + uint64_t ret = masked_initial_value | shifted_input_value; |
| 206 | + uint32_t lo = (uint32_t)ret; |
| 207 | + uint32_t hi = (uint32_t)(ret >> 32); |
| 208 | + |
| 209 | + MEM_W(0, dword_address + 4) = lo; |
| 210 | + MEM_W(0, dword_address + 0) = hi; |
| 211 | +} |
| 212 | + |
| 213 | +static inline uint32_t get_cop1_cs() { |
| 214 | + uint32_t rounding_mode = 0; |
| 215 | + switch (fegetround()) { |
| 216 | + // round to nearest value |
| 217 | + case FE_TONEAREST: |
| 218 | + default: |
| 219 | + rounding_mode = 0; |
| 220 | + break; |
| 221 | + // round to zero (truncate) |
| 222 | + case FE_TOWARDZERO: |
| 223 | + rounding_mode = 1; |
| 224 | + break; |
| 225 | + // round to positive infinity (ceil) |
| 226 | + case FE_UPWARD: |
| 227 | + rounding_mode = 2; |
| 228 | + break; |
| 229 | + // round to negative infinity (floor) |
| 230 | + case FE_DOWNWARD: |
| 231 | + rounding_mode = 3; |
| 232 | + break; |
| 233 | + } |
| 234 | + return rounding_mode; |
| 235 | +} |
| 236 | + |
| 237 | +static inline void set_cop1_cs(uint32_t val) { |
| 238 | + uint32_t rounding_mode = val & 0x3; |
| 239 | + int round = FE_TONEAREST; |
| 240 | + switch (rounding_mode) { |
| 241 | + case 0: // round to nearest value |
| 242 | + round = FE_TONEAREST; |
| 243 | + break; |
| 244 | + case 1: // round to zero (truncate) |
| 245 | + round = FE_TOWARDZERO; |
| 246 | + break; |
| 247 | + case 2: // round to positive infinity (ceil) |
| 248 | + round = FE_UPWARD; |
| 249 | + break; |
| 250 | + case 3: // round to negative infinity (floor) |
| 251 | + round = FE_DOWNWARD; |
| 252 | + break; |
| 253 | + } |
| 254 | + fesetround(round); |
| 255 | +} |
| 256 | + |
138 | 257 | #define S32(val) \ |
139 | 258 | ((int32_t)(val)) |
140 | 259 |
|
@@ -185,41 +304,37 @@ static inline void do_swr(uint8_t* rdram, gpr offset, gpr reg, gpr val) { |
185 | 304 |
|
186 | 305 | #define DEFAULT_ROUNDING_MODE 0 |
187 | 306 |
|
188 | | -static inline int32_t do_cvt_w_s(float val, unsigned int rounding_mode) { |
189 | | - switch (rounding_mode) { |
190 | | - case 0: // round to nearest value |
191 | | - return (int32_t)lroundf(val); |
192 | | - case 1: // round to zero (truncate) |
193 | | - return (int32_t)val; |
194 | | - case 2: // round to positive infinity (ceil) |
195 | | - return (int32_t)ceilf(val); |
196 | | - case 3: // round to negative infinity (floor) |
197 | | - return (int32_t)floorf(val); |
198 | | - } |
199 | | - assert(0); |
200 | | - return 0; |
| 307 | +static inline int32_t do_cvt_w_s(float val) { |
| 308 | + // Rounding mode aware float to 32-bit int conversion. |
| 309 | + return (int32_t)lrintf(val); |
201 | 310 | } |
202 | 311 |
|
203 | 312 | #define CVT_W_S(val) \ |
204 | | - do_cvt_w_s(val, rounding_mode) |
| 313 | + do_cvt_w_s(val) |
205 | 314 |
|
206 | | -static inline int32_t do_cvt_w_d(double val, unsigned int rounding_mode) { |
207 | | - switch (rounding_mode) { |
208 | | - case 0: // round to nearest value |
209 | | - return (int32_t)lround(val); |
210 | | - case 1: // round to zero (truncate) |
211 | | - return (int32_t)val; |
212 | | - case 2: // round to positive infinity (ceil) |
213 | | - return (int32_t)ceil(val); |
214 | | - case 3: // round to negative infinity (floor) |
215 | | - return (int32_t)floor(val); |
216 | | - } |
217 | | - assert(0); |
218 | | - return 0; |
| 315 | +static inline int64_t do_cvt_l_s(float val) { |
| 316 | + // Rounding mode aware float to 64-bit int conversion. |
| 317 | + return (int64_t)llrintf(val); |
| 318 | +} |
| 319 | + |
| 320 | +#define CVT_L_S(val) \ |
| 321 | + do_cvt_l_s(val); |
| 322 | + |
| 323 | +static inline int32_t do_cvt_w_d(double val) { |
| 324 | + // Rounding mode aware double to 32-bit int conversion. |
| 325 | + return (int32_t)lrint(val); |
219 | 326 | } |
220 | 327 |
|
221 | 328 | #define CVT_W_D(val) \ |
222 | | - do_cvt_w_d(val, rounding_mode) |
| 329 | + do_cvt_w_d(val) |
| 330 | + |
| 331 | +static inline int64_t do_cvt_l_d(double val) { |
| 332 | + // Rounding mode aware double to 64-bit int conversion. |
| 333 | + return (int64_t)llrint(val); |
| 334 | +} |
| 335 | + |
| 336 | +#define CVT_L_D(val) \ |
| 337 | + do_cvt_l_d(val) |
223 | 338 |
|
224 | 339 | #define NAN_CHECK(val) \ |
225 | 340 | assert(val == val) |
|
0 commit comments