Library bugfixes and additions, _sdk_log_inner() removal

9 files changed, 131 insertions, 141 deletions

diff --git a/libpsn00b/include/assert.h b/libpsn00b/include/assert.h
index e93c983..12212af 100644
--- a/libpsn00b/include/assert.h
+++ b/libpsn00b/include/assert.h
@@ -9,8 +9,9 @@
 #ifndef __ASSERT_H
 #define __ASSERT_H
 
+#include <stdio.h>
+
 void _assert_abort(const char *file, int line, const char *expr);
-void _sdk_log_inner(const char *fmt, ...);
 
 #ifdef NDEBUG
 
@@ -24,9 +25,9 @@ void _sdk_log_inner(const char *fmt, ...);
 }
 
 #ifdef SDK_LIBRARY_NAME
-#define _sdk_log(fmt, ...) _sdk_log_inner(SDK_LIBRARY_NAME ": " fmt, ##__VA_ARGS__)
+#define _sdk_log(fmt, ...) printf(SDK_LIBRARY_NAME ": " fmt, ##__VA_ARGS__)
 #else
-#define _sdk_log(fmt, ...) _sdk_log_inner(fmt, ##__VA_ARGS__)
+#define _sdk_log(fmt, ...) printf(fmt, ##__VA_ARGS__)
 #endif
 
 #endif
diff --git a/libpsn00b/include/psxsio.h b/libpsn00b/include/psxsio.h
index e0cc49b..d5f7d9a 100644
--- a/libpsn00b/include/psxsio.h
+++ b/libpsn00b/include/psxsio.h
@@ -145,15 +145,19 @@ int SIO_ReadSync(int mode);
 
 /**
  * @brief Registers a function to be called whenever a byte is received. The
- * received byte is appended to the RX buffer and passed as an argument to the
- * callback. The callback will run in the exception handler's context, so it
- * should be as fast as possible and not use any function that relies on
- * interrupts in order to work.
+ * received byte is passed as an argument to the callback, which shall then
+ * return a zero value to also store the byte in the RX buffer or a non-zero
+ * value to drop it. This can be used to e.g. filter or validate incoming data,
+ * or to bypass the library's RX buffer for custom buffering purposes.
+ *
+ * The callback will run in the exception handler's context, so it should be as
+ * fast as possible and shall not call any function that relies on interrupts
+ * being enabled.
  *
  * @param func
  * @return Previously set callback or NULL
  */
-void *SIO_ReadCallback(void (*func)(uint8_t));
+void *SIO_ReadCallback(int (*func)(uint8_t));
 
 /**
  * @brief Sends the given byte, or appends it to the TX buffer if the serial
diff --git a/libpsn00b/include/psxspu.h b/libpsn00b/include/psxspu.h
index cf78e3d..7858e88 100644
--- a/libpsn00b/include/psxspu.h
+++ b/libpsn00b/include/psxspu.h
@@ -73,6 +73,20 @@ typedef struct _SpuCommonAttr {
 	SpuExtAttr	cd, ext;
 } SpuCommonAttr;
 
+/* Macros */
+
+#define getSPUAddr(addr)		((uint16_t) (((addr) + 7) / 8))
+#define getSPUSampleRate(rate)	((uint16_t) (((rate) * (1 << 12)) / 44100))
+
+#define getSPUADSR(ar, dr, sr, rr, sl) ( \
+	(sl) | \
+	((dr) <<  4) | \
+	((ar) <<  8) | \
+	((rr) << 16) | \
+	((sr) << 22) | \
+	(1    << 30) \
+)
+
 /* "Useless" macros for official SDK compatibility */
 
 #define SpuSetCommonMasterVolume(left, right) \
@@ -87,18 +101,18 @@ typedef struct _SpuCommonAttr {
 	((enable) ? (SPU_CTRL |= 0x0002) : (SPU_CTRL &= 0xfffd))
 
 #define SpuSetReverbAddr(addr) \
-	(SPU_REVERB_ADDR = ((addr) + 7) / 8)
+	(SPU_REVERB_ADDR = getSPUAddr(addr))
 #define SpuSetIRQAddr(addr) \
-	(SPU_IRQ_ADDR = ((addr) + 7) / 8)
+	(SPU_IRQ_ADDR = getSPUAddr(addr))
 
 #define SpuSetVoiceVolume(ch, left, right) \
 	(SPU_CH_VOL_L(ch) = (left), SPU_CH_VOL_R(ch) = (right))
 #define SpuSetVoicePitch(ch, pitch) \
 	(SPU_CH_FREQ(ch) = (pitch))
 #define SpuSetVoiceStartAddr(ch, addr) \
-	(SPU_CH_ADDR(ch) = ((addr) + 7) / 8)
+	(SPU_CH_ADDR(ch) = getSPUAddr(addr))
 #define SpuSetVoiceADSR(ch, ar, dr, sr, rr, sl) \
-	(SPU_CH_ADSR(ch) = ((sl)) | ((dr) << 4) | ((ar) << 8) | ((rr) << 16) | ((sr) << 22) | (1 << 30))
+	(SPU_CH_ADSR(ch) = getSPUADSR(ar, dr, sr, rr, sl))
 
 #define SpuSetKey(enable, voice_bit) \
 	((enable) ? (SPU_KEY_ON = (voice_bit)) : (SPU_KEY_OFF = (voice_bit)))
diff --git a/libpsn00b/libc/_start.s b/libpsn00b/libc/_start.s
new file mode 100644
index 0000000..fcd4c4c
--- /dev/null
+++ b/libpsn00b/libc/_start.s
@@ -0,0 +1,17 @@
+# PSn00bSDK _start() trampoline
+# (C) 2022 spicyjpeg - MPL licensed
+#
+# This file provides a weak function that can be easily overridden to e.g. set
+# $sp or perform additional initialization before the "real" _start() function
+# (_start_inner()) is called.
+
+.set noreorder
+
+.section .text._start
+.global _start
+.type _start, @function
+.weak _start
+_start:
+	la $gp, _gp
+	j  _start_inner
+	nop
diff --git a/libpsn00b/libc/_stubs.s b/libpsn00b/libc/_stubs.s
deleted file mode 100644
index aa7bfbe..0000000
--- a/libpsn00b/libc/_stubs.s
+++ /dev/null
@@ -1,27 +0,0 @@
-# PSn00bSDK _start() trampoline and logging endpoint
-# (C) 2022 spicyjpeg - MPL licensed
-#
-# This file provides a weak function that can be easily overridden to e.g. set
-# $sp or perform additional initialization before the "real" _start() function
-# (_start_inner()) is called. The _sdk_log_inner() function called by other
-# libraries to log debug messages can also be overridden in a similar way.
-
-.set noreorder
-
-.section .text._start
-.global _start
-.type _start, @function
-.weak _start
-_start:
-	la $gp, _gp
-	j  _start_inner
-	nop
-
-.section .text._sdk_log_inner
-.global _sdk_log_inner
-.type _sdk_log_inner, @function
-.weak _sdk_log_inner
-_sdk_log_inner:
-	li $t2, 0xa0
-	jr $t2
-	li $t1, 0x3f
diff --git a/libpsn00b/libc/abort.c b/libpsn00b/libc/abort.c
index 9a4661a..0a3c325 100644
--- a/libpsn00b/libc/abort.c
+++ b/libpsn00b/libc/abort.c
@@ -3,13 +3,15 @@
  * (C) 2022 spicyjpeg - MPL licensed
  */
 
+#undef SDK_LIBRARY_NAME
+
 #include <assert.h>
 #include <psxapi.h>
 
 /* Internal function used by assert() macro */
 
 void _assert_abort(const char *file, int line, const char *expr) {
-	_sdk_log_inner("%s:%d: assert(%s)\n", file, line, expr);
+	_sdk_log("%s:%d: assert(%s)\n", file, line, expr);
 
 	for (;;)
 		__asm__ volatile("");
@@ -18,7 +20,7 @@ void _assert_abort(const char *file, int line, const char *expr) {
 /* Standard abort */
 
 void abort(void) {
-	_sdk_log_inner("abort()\n");
+	_sdk_log("abort()\n");
 
 	for (;;)
 		__asm__ volatile("");
@@ -27,7 +29,7 @@ void abort(void) {
 /* Pure virtual function call (C++) */
 
 void __cxa_pure_virtual(void) {
-	_sdk_log_inner("__cxa_pure_virtual()\n");
+	_sdk_log("__cxa_pure_virtual()\n");
 
 	for (;;)
 		__asm__ volatile("");
diff --git a/libpsn00b/psxpress/vlc.s b/libpsn00b/psxpress/vlc.s
index 885a3f7..75e33d3 100644
--- a/libpsn00b/psxpress/vlc.s
+++ b/libpsn00b/psxpress/vlc.s
@@ -29,17 +29,17 @@
 .set VLC_Context_block_index,	20
 .set VLC_Context_coeff_index,	21
 
-.set DECDCTSMALLTAB_lut0,	0
-.set DECDCTSMALLTAB_lut2,	4
-.set DECDCTSMALLTAB_lut3,	36
-.set DECDCTSMALLTAB_lut4,	292
-.set DECDCTSMALLTAB_lut5,	308
-.set DECDCTSMALLTAB_lut7,	324
-.set DECDCTSMALLTAB_lut8,	356
-.set DECDCTSMALLTAB_lut9,	420
-.set DECDCTSMALLTAB_lut10,	484
-.set DECDCTSMALLTAB_lut11,	548
-.set DECDCTSMALLTAB_lut12,	612
+.set DECDCTTAB_lut0,	0
+.set DECDCTTAB_lut2,	4
+.set DECDCTTAB_lut3,	36
+.set DECDCTTAB_lut4,	292
+.set DECDCTTAB_lut5,	308
+.set DECDCTTAB_lut7,	324
+.set DECDCTTAB_lut8,	356
+.set DECDCTTAB_lut9,	420
+.set DECDCTTAB_lut10,	484
+.set DECDCTTAB_lut11,	548
+.set DECDCTTAB_lut12,	612
 
 .section .text.DecDCTvlcStart
 .global DecDCTvlcStart
@@ -115,7 +115,7 @@ _vlc_skip_context_load:
 	# Obtain the addresses of the lookup table and jump area in advance so that
 	# they don't have to be retrieved for each coefficient decoded.
 	lw    $t8, _vlc_huffman_table
-	la    $t9, .Lac_jump_area
+	la    $t9, .Lac_prefix_10
 
 	beqz  $a2, .Lstop_processing
 	addiu $a1, 4 # output = (uint16_t *) &output[1]
@@ -123,32 +123,32 @@ _vlc_skip_context_load:
 .Lprocess_next_code_loop: # while (max_size)
 	# This is the "hot" part of the decoder, executed for each code in the
 	# bitstream. The first step is to determine if the next code is a DC or AC
-	# coefficient. The GTE is also given the task of counting the number of
-	# leading zeroes/ones, which takes 2 more cycles.
+	# coefficient.
 	bnez  $t7, .Lprocess_ac_coefficient
-	mtc2  $t0, $30
+	addiu $t7, 1 # coeff_index++
 	bnez  $t4, .Lprocess_dc_v3_coefficient
-	#nop
+	li    $v1, 0x01ff
 
 .Lprocess_dc_v2_coefficient: # if (!coeff_index && !is_v3)
 	# The DC coefficient in version 2 frames is not compressed. Value 0x1ff is
 	# used to signal the end of the bitstream.
 	srl   $v0, $t0, 22 # prefix = (window >> (32 - 10))
-	li    $v1, 0x01ff
 	beq   $v0, $v1, .Lstop_processing # if (prefix == 0x1ff) break
 	or    $v0, $t3 # *output = prefix | quant_scale
 	sll   $t0, 10 # window <<= 10
-	addiu $t5, -10 # bit_offset -= 10
 	b     .Lwrite_value
-	addiu $t7, 1 # coeff_index++
+	addiu $t5, -10 # bit_offset -= 10
 
 .Lprocess_dc_v3_coefficient: # if (!coeff_index && is_v3)
 	# TODO: version 3 is currently not supported.
 	jr    $ra
 	li    $v0, -1
-	#b     .Lwrite_value
 
 .Lprocess_ac_coefficient: # if (coeff_index)
+	# Start counting the number of leading zeroes/ones using the GTE. This
+	# takes 2 more cycles.
+	mtc2  $t0, $30
+
 	# Check whether the prefix code is one of the shorter, more common ones.
 	srl   $v0, $t0, 30
 	li    $v1, 3
@@ -157,33 +157,34 @@ _vlc_skip_context_load:
 	beq   $v0, $v1, .Lac_prefix_10
 	li    $v1, 1
 	beq   $v0, $v1, .Lac_prefix_01
-	#srl   $v0, $t0, 29
-	#beq   $v0, $v1, .Lac_prefix_001
-	#nop
+	nop
 
 	# If the code is longer, retrieve the number of leading zeroes from the GTE
 	# and use it as an index into the jump area. Each block in the area is 8
 	# instructions long and handles decoding a specific prefix.
 	mfc2  $v0, $31
-	nop
-	andi  $v0, 15 # jump_addr = &ac_jump_area[(prefix % 16) * 8 * sizeof(u32)]
-	sll   $v0, 5
+	li    $v1, 11
+	bgt   $v0, $v1, .Lreturn_error # if (prefix > 11) return -1
+	sll   $v0, 5 # jump_addr = &ac_jump_area[prefix * 8 * sizeof(u32)]
 	addu  $v0, $t9
 	jr    $v0
 	nop
 
+.Lreturn_error:
+	jr    $ra
+	li    $v0, -1
+
 .Lac_prefix_11:
 	# Prefix 11 is followed by a single bit.
 	srl   $v0, $t0, 28 # index = ((window >> (32 - 2 - 1)) & 1) * sizeof(u16)
 	andi  $v0, 2
 	addu  $v0, $t8 # value = table->lut0[index]
-	lhu   $v0, DECDCTSMALLTAB_lut0($v0)
+	lhu   $v0, DECDCTTAB_lut0($v0)
 	sll   $t0, 3 # window <<= 3
-	addiu $t5, -3 # bit_offset -= 3
 	b     .Lwrite_value
-	addiu $t7, 1 # coeff_index++
+	addiu $t5, -3 # bit_offset -= 3
+	#.word 0
 
-.Lac_jump_area:
 .Lac_prefix_10:
 	# Prefix 10 marks the end of a block.
 	li    $v0, 0xfe00 # value = 0xfe00
@@ -202,11 +203,10 @@ _vlc_skip_context_load:
 	srl   $v0, $t0, 25 # index = ((window >> (32 - 2 - 3)) & 7) * sizeof(u32)
 	andi  $v0, 28
 	addu  $v0, $t8 # value = table->lut2[index]
-	lw    $v0, DECDCTSMALLTAB_lut2($v0)
-	addiu $t7, 1 # coeff_index++
+	lw    $v0, DECDCTTAB_lut2($v0)
 	b     .Lupdate_window_and_write
 	srl   $v1, $v0, 16 # length = value >> 16
-	.word 0
+	.word 0, 0
 
 .Lac_prefix_001:
 	# Prefix 001 can be followed by a 6-bit lookup index starting with 00, or a
@@ -214,136 +214,106 @@ _vlc_skip_context_load:
 	srl   $v0, $t0, 21 # index = ((window >> (32 - 3 - 6)) & 63) * sizeof(u32)
 	andi  $v0, 252
 	addu  $v0, $t8 # value = table->lut3[index]
-	lw    $v0, DECDCTSMALLTAB_lut3($v0)
-	addiu $t7, 1 # coeff_index++
+	lw    $v0, DECDCTTAB_lut3($v0)
 	b     .Lupdate_window_and_write
 	srl   $v1, $v0, 16 # length = value >> 16
-	.word 0
+	.word 0, 0
 
 .Lac_prefix_0001:
 	# Prefix 0001 is followed by a 3-bit lookup index.
 	srl   $v0, $t0, 24 # index = ((window >> (32 - 4 - 3)) & 7) * sizeof(u16)
 	andi  $v0, 14
 	addu  $v0, $t8 # value = table->lut4[index]
-	lhu   $v0, DECDCTSMALLTAB_lut4($v0)
+	lhu   $v0, DECDCTTAB_lut4($v0)
 	sll   $t0, 7 # window <<= 4 + 3
-	addiu $t5, -7 # bit_offset -= 4 + 3
 	b     .Lwrite_value
-	addiu $t7, 1 # coeff_index++
+	addiu $t5, -7 # bit_offset -= 4 + 3
+	.word 0
 
 .Lac_prefix_00001:
 	# Prefix 00001 is followed by a 3-bit lookup index.
 	srl   $v0, $t0, 23 # index = ((window >> (32 - 5 - 3)) & 7) * sizeof(u16)
 	andi  $v0, 14
 	addu  $v0, $t8 # value = table->lut5[index]
-	lhu   $v0, DECDCTSMALLTAB_lut5($v0)
+	lhu   $v0, DECDCTTAB_lut5($v0)
 	sll   $t0, 8 # window <<= 5 + 3
-	addiu $t5, -8 # bit_offset -= 5 + 3
 	b     .Lwrite_value
-	addiu $t7, 1 # coeff_index++
+	addiu $t5, -8 # bit_offset -= 5 + 3
+	.word 0
 
 .Lac_prefix_000001:
 	# Prefix 000001 is an escape code followed by a full 16-bit MDEC value.
 	srl   $v0, $t0, 10 # value = window >> (32 - 6 - 16)
 	sll   $t0, 22 # window <<= 6 + 16
-	addiu $t5, -22 # bit_offset -= 6 + 16
 	b     .Lwrite_value
-	addiu $t7, 1 # coeff_index++
-	.word 0, 0, 0
+	addiu $t5, -22 # bit_offset -= 6 + 16
+	.word 0, 0, 0, 0
 
 .Lac_prefix_0000001:
 	# Prefix 0000001 is followed by a 4-bit lookup index.
 	srl   $v0, $t0, 20 # index = ((window >> (32 - 7 - 4)) & 15) * sizeof(u16)
 	andi  $v0, 30
 	addu  $v0, $t8 # value = table->lut7[index]
-	lhu   $v0, DECDCTSMALLTAB_lut7($v0)
+	lhu   $v0, DECDCTTAB_lut7($v0)
 	sll   $t0, 11 # window <<= 7 + 4
-	addiu $t5, -11 # bit_offset -= 7 + 4
 	b     .Lwrite_value
-	addiu $t7, 1 # coeff_index++
+	addiu $t5, -11 # bit_offset -= 7 + 4
+	.word 0
 
 .Lac_prefix_00000001:
 	# Prefix 00000001 is followed by a 5-bit lookup index.
 	srl   $v0, $t0, 18 # index = ((window >> (32 - 8 - 5)) & 31) * sizeof(u16)
 	andi  $v0, 62
 	addu  $v0, $t8 # value = table->lut8[index]
-	lhu   $v0, DECDCTSMALLTAB_lut8($v0)
+	lhu   $v0, DECDCTTAB_lut8($v0)
 	sll   $t0, 13 # window <<= 8 + 5
-	addiu $t5, -13 # bit_offset -= 8 + 5
 	b     .Lwrite_value
-	addiu $t7, 1 # coeff_index++
+	addiu $t5, -13 # bit_offset -= 8 + 5
+	.word 0
 
 .Lac_prefix_000000001:
 	# Prefix 000000001 is followed by a 5-bit lookup index.
 	srl   $v0, $t0, 17 # index = ((window >> (32 - 9 - 5)) & 31) * sizeof(u16)
 	andi  $v0, 62
 	addu  $v0, $t8 # value = table->lut9[index]
-	lhu   $v0, DECDCTSMALLTAB_lut9($v0)
+	lhu   $v0, DECDCTTAB_lut9($v0)
 	sll   $t0, 14 # window <<= 9 + 5
-	addiu $t5, -14 # bit_offset -= 9 + 5
 	b     .Lwrite_value
-	addiu $t7, 1 # coeff_index++
+	addiu $t5, -14 # bit_offset -= 9 + 5
+	.word 0
 
 .Lac_prefix_0000000001:
 	# Prefix 0000000001 is followed by a 5-bit lookup index.
 	srl   $v0, $t0, 16 # index = ((window >> (32 - 10 - 5)) & 31) * sizeof(u16)
 	andi  $v0, 62
 	addu  $v0, $t8 # value = table->lut10[index]
-	lhu   $v0, DECDCTSMALLTAB_lut10($v0)
+	lhu   $v0, DECDCTTAB_lut10($v0)
 	sll   $t0, 15 # window <<= 10 + 5
-	addiu $t5, -15 # bit_offset -= 10 + 5
 	b     .Lwrite_value
-	addiu $t7, 1 # coeff_index++
+	addiu $t5, -15 # bit_offset -= 10 + 5
+	.word 0
 
 .Lac_prefix_00000000001:
 	# Prefix 00000000001 is followed by a 5-bit lookup index.
 	srl   $v0, $t0, 15 # index = ((window >> (32 - 11 - 5)) & 31) * sizeof(u16)
 	andi  $v0, 62
 	addu  $v0, $t8 # value = table->lut11[index]
-	lhu   $v0, DECDCTSMALLTAB_lut11($v0)
+	lhu   $v0, DECDCTTAB_lut11($v0)
 	sll   $t0, 16 # window <<= 11 + 5
-	addiu $t5, -16 # bit_offset -= 11 + 5
 	b     .Lwrite_value
-	addiu $t7, 1 # coeff_index++
+	addiu $t5, -16 # bit_offset -= 11 + 5
+	.word 0
 
 .Lac_prefix_000000000001:
 	# Prefix 000000000001 is followed by a 5-bit lookup index.
 	srl   $v0, $t0, 14 # index = ((window >> (32 - 12 - 5)) & 31) * sizeof(u16)
 	andi  $v0, 62
 	addu  $v0, $t8 # value = table->lut12[index]
-	lhu   $v0, DECDCTSMALLTAB_lut12($v0)
+	lhu   $v0, DECDCTTAB_lut12($v0)
 	sll   $t0, 17 # window <<= 12 + 5
-	addiu $t5, -17 # bit_offset -= 12 + 5
 	b     .Lwrite_value
-	addiu $t7, 1 # coeff_index++
-
-	# Prefix 0000000000001 is not valid.
-	beqz  $t0, .Lstop_processing
-	nop
-	jr    $ra
-	li    $v0, -1
-	.word 0, 0, 0, 0
-
-	# Prefix 00000000000001 is not valid.
-	beqz  $t0, .Lstop_processing
-	nop
-	jr    $ra
-	li    $v0, -1
-	.word 0, 0, 0, 0
-
-	# Prefix 000000000000001 is not valid.
-	beqz  $t0, .Lstop_processing
-	nop
-	jr    $ra
-	li    $v0, -1
-	.word 0, 0, 0, 0
-
-	# Prefix 0000000000000001 is not valid.
-	beqz  $t0, .Lstop_processing
-	nop
-	jr    $ra
-	li    $v0, -1
-	#.word 0, 0, 0, 0
+	addiu $t5, -17 # bit_offset -= 12 + 5
+	.word 0
 
 .Lupdate_window_and_write:
 	sllv  $t0, $t0, $v1 # window <<= length
diff --git a/libpsn00b/psxsio/sio.c b/libpsn00b/psxsio/sio.c
index 3fb3caf..6b80352 100644
--- a/libpsn00b/psxsio/sio.c
+++ b/libpsn00b/psxsio/sio.c
@@ -27,7 +27,7 @@ typedef struct {
 static SIO_FlowControl _flow_control;
 static uint16_t _ctrl_reg_flag;
 
-static void (*_read_callback)(uint8_t) = (void *) 0;
+static int  (*_read_callback)(uint8_t) = (void *) 0;
 static void (*_old_sio_handler)(void)  = (void *) 0;
 
 static volatile RingBuffer _tx_buffer, _rx_buffer;
@@ -41,7 +41,15 @@ static void _sio_handler(void) {
 	// Handle any incoming bytes.
 	while (SIO_STAT & SR_RXRDY) {
 		uint8_t value  = SIO_TXRX;
-		int     length = _rx_buffer.length;
+
+		// Skip storing this byte into the RX buffer if the callback returns a
+		// non-zero value.
+		if (_read_callback) {
+			if (_read_callback(value))
+				continue;
+		}
+
+		int length = _rx_buffer.length;
 
 		if (length >= BUFFER_LENGTH) {
 			//_sdk_log("RX overrun, dropping bytes\n");
@@ -53,8 +61,6 @@ static void _sio_handler(void) {
 		_rx_buffer.length = length + 1;
 
 		_rx_buffer.data[tail] = value;
-		if (_read_callback)
-			_read_callback(value);
 	}
 
 	// Send the next byte in the buffer if the TX unit is ready. Note that
@@ -185,7 +191,7 @@ int SIO_ReadSync(int mode) {
 	return 0;
 }
 
-void *SIO_ReadCallback(void (*func)(uint8_t)) {
+void *SIO_ReadCallback(int (*func)(uint8_t)) {
 	EnterCriticalSection();
 
 	void *old_callback  = _read_callback;
diff --git a/libpsn00b/psxspu/common.c b/libpsn00b/psxspu/common.c
index 42f7cb7..1613ca9 100644
--- a/libpsn00b/psxspu/common.c
+++ b/libpsn00b/psxspu/common.c
@@ -38,13 +38,16 @@ static void _dma_transfer(uint32_t *data, size_t length, int write) {
 		length += DMA_CHUNK_LENGTH - 1;
 	}
 
-	SPU_CTRL &= 0xffcf; // Disable DMA request
+	SPU_DMA_CTRL = 0x0004; // Reset transfer mode
+	SPU_CTRL    &= 0xffcf; // Disable DMA request
 	_wait_status(0x0030, 0x0000);
 
 	// Enable DMA request for writing (2) or reading (3)
+	uint16_t ctrl = write ? 0x0020 : 0x0030;
+
 	SPU_ADDR  = _transfer_addr;
-	SPU_CTRL |= write ? 0x0020 : 0x0030;
-	_wait_status(0x0400, 0x0000);
+	SPU_CTRL |= ctrl;
+	_wait_status(0x0430, ctrl);
 
 	DMA_MADR(4) = (uint32_t) data;
 	if (length < DMA_CHUNK_LENGTH)