2 files changed, 26 insertions, 60 deletions

diff --git a/asm/librfu.s b/asm/librfu.s
index 491b4fb66..10db8da19 100644
--- a/asm/librfu.s
+++ b/asm/librfu.s
@@ -4,62 +4,6 @@
 
 	.text
 
-	thumb_func_start STWI_start_Command
-STWI_start_Command: @ 82E3428
-	push {r4,r5,lr}
-	ldr r5, _082E3480
-	ldr r0, [r5]
-	ldr r3, [r0, 0x24]
-	ldrb r1, [r0, 0x4]
-	lsls r1, 8
-	ldrb r0, [r0, 0x6]
-	ldr r2, _082E3484
-	orrs r0, r2
-	orrs r1, r0
-	str r1, [r3]
-	ldr r2, _082E3488
-	ldr r1, [r5]
-	ldr r0, [r1, 0x24]
-	ldr r0, [r0]
-	str r0, [r2]
-	movs r2, 0
-	str r2, [r1]
-	movs r0, 0x1
-	strb r0, [r1, 0x5]
-	ldr r4, _082E348C
-	ldrh r3, [r4]
-	strh r2, [r4]
-	ldr r2, _082E3490
-	ldr r1, [r5]
-	movs r0, 0x8
-	ldrb r1, [r1, 0xA]
-	lsls r0, r1
-	ldrh r1, [r2]
-	orrs r0, r1
-	strh r0, [r2]
-	ldrh r0, [r2]
-	movs r1, 0x80
-	orrs r0, r1
-	strh r0, [r2]
-	strh r3, [r4]
-	ldr r1, _082E3494
-	ldr r2, _082E3498
-	adds r0, r2, 0
-	strh r0, [r1]
-	movs r0, 0
-	pop {r4,r5}
-	pop {r1}
-	bx r1
-	.align 2, 0
-_082E3480: .4byte gRfuState
-_082E3484: .4byte 0x99660000
-_082E3488: .4byte 0x04000120
-_082E348C: .4byte 0x04000208
-_082E3490: .4byte 0x04000200
-_082E3494: .4byte 0x04000128
-_082E3498: .4byte 0x00005083
-	thumb_func_end STWI_start_Command
-
 	thumb_func_start STWI_restart_Command
 STWI_restart_Command: @ 82E349C
 	push {r4,lr}
diff --git a/src/librfu.c b/src/librfu.c
index f1c35b1ec..37b76ec3e 100644
--- a/src/librfu.c
+++ b/src/librfu.c
@@ -73,7 +73,7 @@ struct RfuStruct
     vs32 unk_0;
     u8 txParams;
     u8 unk_5;
-    u8 unk_6;
+    u8 activeCommand;
     u8 unk_7;
     u8 unk_8;
     u8 unk_9;
@@ -187,7 +187,7 @@ void AgbRFU_SoftReset(void)
     gRfuState->unk_0 = 0;
     gRfuState->txParams = 0;
     gRfuState->unk_5 = 0;
-    gRfuState->unk_6 = 0;
+    gRfuState->activeCommand = 0;
     gRfuState->unk_7 = 0;
     gRfuState->unk_8 = 0;
     gRfuState->unk_9 = 0;
@@ -215,7 +215,7 @@ u16 STWI_read_status(u8 index)
     case 2:
         return gRfuState->unk_0;
     case 3:
-        return gRfuState->unk_6;
+        return gRfuState->activeCommand;
     default:
         return 0xFFFF;
     }
@@ -702,7 +702,7 @@ u16 STWI_init(u8 request)
     else
     {
         gRfuState->unk_2c = TRUE;
-        gRfuState->unk_6 = request;
+        gRfuState->activeCommand = request;
         gRfuState->unk_0 = 0;
         gRfuState->txParams = 0;
         gRfuState->unk_5 = 0;
@@ -719,3 +719,25 @@ u16 STWI_init(u8 request)
         return FALSE;
     }
 }
+
+int STWI_start_Command()
+{
+    u16 imeTemp;
+    
+    // Yes, it matters that it's casted to a u32...
+    *(u32*)gRfuState->txPacket->rfuPacket8.data = 0x99660000 | (gRfuState->txParams << 8) | gRfuState->activeCommand;
+    REG_SIODATA32 = gRfuState->txPacket->rfuPacket32.command;
+    
+    gRfuState->unk_0 = 0;
+    gRfuState->unk_5 = 1;
+    
+    imeTemp = REG_IME;
+    REG_IME = 0;
+    REG_IE |= (INTR_FLAG_TIMER0 << gRfuState->timerSelect);
+    REG_IE |= INTR_FLAG_SERIAL;
+    REG_IME = imeTemp;
+    
+    REG_SIOCNT = SIO_INTR_ENABLE | SIO_32BIT_MODE | SIO_MULTI_BUSY | SIO_115200_BPS;
+    
+    return 0;
+}