get wifi beacons going

src/HLE_Retail/Sound_Nitro.cpp

@@ -395,6 +395,8 @@ void Reset()
             Version = 1;
         else if (gamecode == 0x595241) // ARYx / Rayman DS
             Version = 2;
+        else if (gamecode == 0x475541) // AUGx / Need for Speed Underground
+            Version = 2;
     }
 
     NDS::ScheduleEvent(NDS::Event_HLE_SoundCmd, true, 174592, Process, 1);

src/HLE_Retail/Wifi.cpp

@@ -21,6 +21,7 @@
 #include "../HLE.h"
 #include "../FIFO.h"
 #include "../SPI.h"
+#include "../GPU.h"
 
 #include "IPC.h"
 #include "Wifi.h"
@@ -43,13 +44,26 @@ struct IPCReply
 
 FIFO<IPCReply, 16> IPCReplyQueue;
 
+s32 TimerError;
+
 u32 SharedMem[2];
 
 u8 MAC[6];
+u16 TXSeqNo;
+u64 Timestamp;
+u16 Channel;
 
+u8 RXBuffer[2048];
+
+u16 BeaconCount;
 u16 BeaconInterval;
 u8 BeaconFrame[1024];
+u8* BeaconTagDD;
 
+u16 HostScanCount;
+u32 HostScanBuffer;
+
+void ScheduleTimer(bool first);
 void WifiIPCReply(u16 cmd, u16 status, int numextra=0, u16* extra=nullptr);
 
 
@@ -57,20 +71,271 @@ void Reset()
 {
     IPCReplyQueue.Clear();
 
+    TimerError = 0;
+
     SharedMem[0] = 0;
     SharedMem[1] = 0;
 
     u8* mac = SPI_Firmware::GetWifiMAC();
     memcpy(MAC, mac, 6);
+    TXSeqNo = 0;
+    Timestamp = 0;
+    Channel = 0;
 
+    memset(RXBuffer, 0, sizeof(RXBuffer));
+
+    BeaconCount = 0;
     BeaconInterval = 0;
     memset(BeaconFrame, 0, sizeof(BeaconFrame));
+    BeaconTagDD = nullptr;
+
+    HostScanCount = 0;
+    HostScanBuffer = 0;
 
     u16 chanmask = 0x2082;
     NDS::ARM7Write16(0x027FFCFA, chanmask);
 }
 
 
+void SendHostBeacon()
+{printf("HOST BEACON\n");
+    *(u16*)&BeaconFrame[12 + 22] = (TXSeqNo << 4);
+    TXSeqNo++;
+
+    *(u64*)&BeaconFrame[12 + 24] = Timestamp;
+
+    u16 syncval = ((GPU::VCount * 0x7F) - (Timestamp * 2)) >> 7;
+    *(u16*)&BeaconTagDD[8] = syncval;
+
+    Platform::MP_SendPacket(BeaconFrame, 12+*(u16*)&BeaconFrame[10], Timestamp);
+}
+
+bool ReceiveHostBeacon()
+{
+    int rxlen = Platform::MP_RecvPacket(RXBuffer, nullptr);
+    if (rxlen <= (12+24)) return false;
+
+    u16 framelen = *(u16*)&RXBuffer[10];
+    if (framelen != rxlen-12)
+    {
+        //Log(LogLevel::Error, "bad frame length %d/%d\n", framelen, rxlen-12);
+        return false;
+    }
+
+    u16 framectl = *(u16*)&RXBuffer[12+0];
+    if ((framectl & 0xE7FF) != 0x0080) return false;
+
+    u8* tagpos = &RXBuffer[12+24+8+2+2];
+    u8* frameend = &RXBuffer[rxlen];
+    while (tagpos < frameend)
+    {
+        u8 tag = *tagpos++;
+        u8 len = *tagpos++;
+
+        if (tag == 0xDD)
+        {
+            u32 oui = *(u32*)tagpos;
+            if (oui == 0x00BF0900)
+                return true;
+        }
+
+        tagpos += len;
+    }
+
+    return false;
+}
+
+void HostScan()
+{
+    bool res = ReceiveHostBeacon();
+    if (res)
+    {
+        // fill beacon info buffer
+
+        u16 buf_len = 0x41;
+
+        u8* dd_offset = nullptr;
+        u16 dd_len = 0;
+
+        NDS::ARM7Write16(HostScanBuffer+0x02, RXBuffer[12]); // CHECKME
+        NDS::ARM7Write16(HostScanBuffer+0x04, RXBuffer[12+10]);
+        NDS::ARM7Write16(HostScanBuffer+0x06, RXBuffer[12+12]);
+        NDS::ARM7Write16(HostScanBuffer+0x08, RXBuffer[12+14]);
+        NDS::ARM7Write16(HostScanBuffer+0x2C, RXBuffer[12+24+8+2]);
+        NDS::ARM7Write16(HostScanBuffer+0x32, RXBuffer[12+24+8]);
+
+        NDS::ARM7Write16(HostScanBuffer+0x0A, 0);
+        NDS::ARM7Write16(HostScanBuffer+0x34, 0);
+        NDS::ARM7Write16(HostScanBuffer+0x38, 0);
+        NDS::ARM7Write16(HostScanBuffer+0x3C, 0);
+        NDS::ARM7Write16(HostScanBuffer+0x3E, 0);
+
+        u16 rxlen = *(u16*)&RXBuffer[10] + 12;
+        u8* tagpos = &RXBuffer[12+24+8+2+2];
+        u8* frameend = &RXBuffer[rxlen];
+        while (tagpos < frameend)
+        {
+            u8 tag = *tagpos++;
+            u8 len = *tagpos++;
+
+            switch (tag)
+            {
+            case 0x00: // SSID
+                {
+                    u8 ssidlen = len;
+                    if (ssidlen > 0x20) ssidlen = 0x20;
+
+                    NDS::ARM7Write16(HostScanBuffer+0x0A, ssidlen);
+                    for (int i = 0; i < ssidlen; i++)
+                        NDS::ARM7Write8(HostScanBuffer+0xC+i, tagpos[i]);
+                }
+                break;
+
+            case 0x01: // supported rates
+                {
+                    u16 mask1 = 0;
+                    u16 mask2 = 0;
+
+                    for (int i = 0; i < len; i++)
+                    {
+                        u8 val = tagpos[i];
+                        u16 bit;
+                        switch (val & 0x7F)
+                        {
+                        case 0x02: bit = (1<<0); break;
+                        case 0x04: bit = (1<<1); break;
+                        case 0x0B: bit = (1<<2); break;
+                        case 0x0C: bit = (1<<3); break;
+                        case 0x12: bit = (1<<4); break;
+                        case 0x16: bit = (1<<5); break;
+                        case 0x30: bit = (1<<6); break;
+                        case 0x48: bit = (1<<7); break;
+                        case 0x60: bit = (1<<8); break;
+                        case 0x6C: bit = (1<<9); break;
+                        default: bit = (1<<15); break;
+                        }
+
+                        if (val & 0x80) mask1 |= bit;
+                        mask2 |= bit;
+                    }
+
+                    NDS::ARM7Write16(HostScanBuffer+0x2E, mask1);
+                    NDS::ARM7Write16(HostScanBuffer+0x30, mask2);
+                }
+                break;
+
+            case 0x03: // channel
+                {
+                    if (len != 1) break;
+
+                    NDS::ARM7Write16(HostScanBuffer+0x36, tagpos[0]);
+                }
+                break;
+
+            case 0x04: // CF parameters
+                {
+                    if (len != 6) break;
+
+                    NDS::ARM7Write16(HostScanBuffer+0x38, tagpos[1]);
+                }
+                break;
+
+            case 0x05: // TIM
+                {
+                    if (len < 4) break;
+
+                    NDS::ARM7Write16(HostScanBuffer+0x34, tagpos[1]);
+                }
+                break;
+
+            case 0xDD: // tag DD
+                {
+                    // TODO count bad tag DD's
+                    if (len < 8) break;
+                    if (*(u32*)&tagpos[0] != 0x00BF0900) break;
+
+                    dd_offset = &tagpos[8];
+                    dd_len = len - 8;
+
+                    NDS::ARM7Write16(HostScanBuffer+0x3C, dd_len);
+                    for (int i = 0; i < dd_len; i++)
+                        NDS::ARM7Write8(HostScanBuffer+0x40+i, tagpos[8+i]);
+
+                    buf_len += dd_len;
+                }
+                break;
+            }
+
+            tagpos += len;
+        }
+
+        NDS::ARM7Write16(HostScanBuffer+0x00, (buf_len >> 1));
+
+        // fill reply buffer
+
+        WifiIPCReply(0xA, 0, dd_len, (u16*)dd_offset);
+    }
+    else if (HostScanCount == 0)
+    {
+        WifiIPCReply(0xA, 0);
+    }
+}
+
+
+void MSTimer(u32 param)
+{
+    u16 status = NDS::ARM7Read16(SharedMem[1]);
+
+    Timestamp += 1024;
+
+    switch (status)
+    {
+    case 5: // scanning for host beacons
+        {
+            if (HostScanCount > 0)
+            {
+                HostScanCount--;
+                HostScan();
+            }
+
+            if (HostScanCount > 0)
+                ScheduleTimer(false);
+        }
+        break;
+
+    case 7:
+    case 9: // host comm
+        {
+            BeaconCount++;
+            if (BeaconCount >= BeaconInterval)
+            {
+                BeaconCount = 0;
+                SendHostBeacon();
+            }
+
+            ScheduleTimer(false);
+        }
+        break;
+    }
+}
+
+void ScheduleTimer(bool first)
+{
+    if (first)
+    {
+        NDS::CancelEvent(NDS::Event_Wifi);
+        TimerError = 0;
+    }
+
+    s64 cycles = 33513982LL * 1024LL;
+    cycles -= TimerError;
+    s64 delay = (cycles + 999999LL) / 1000000LL;
+    TimerError = (s32)((delay * 1000000LL) - cycles);
+
+    NDS::ScheduleEvent(NDS::Event_Wifi, !first, (s32)delay, MSTimer, 0);
+}
+
+
 void StartHostComm()
 {
     memset(BeaconFrame, 0xFF, sizeof(BeaconFrame));
@@ -84,6 +349,7 @@ void StartHostComm()
     u32 gameid = NDS::ARM7Read32(paramblock + 0x08);
     u16 streamcode = NDS::ARM7Read16(paramblock + 0x0C);
 
+    BeaconCount = 0;
     BeaconInterval = NDS::ARM7Read16(paramblock + 0x18);
     u16 channel = NDS::ARM7Read16(paramblock + 0x32);
 
@@ -142,6 +408,7 @@ void StartHostComm()
     *ptr++ = 0x00;
 
     // tag DD
+    BeaconTagDD = ptr;
     *ptr++ = 0xDD; *ptr++ = (0x18 + dd_len);
     *(u32*)ptr = 0x00BF0900;     ptr += 4;
     *(u16*)ptr = 0x000A;         ptr += 2; // stepping (checkme)
@@ -167,7 +434,7 @@ void StartHostComm()
     len += 4;
 
     // frame length
-    *(u16*&)BeaconFrame[0xA] = len;
+    *(u16*)&BeaconFrame[0xA] = len;
 }
 
 
@@ -215,6 +482,38 @@ void WifiIPCReply(u16 cmd, u16 status, int numextra, u16* extra)
             NDS::ARM7Write16(replybuf+0x2E, extra[2]);
         }
     }
+    else if (cmd == 0xA)
+    {
+        if (numextra > 0)
+        {
+            NDS::ARM7Write16(replybuf+0x8, 5);
+            NDS::ARM7Write16(replybuf+0x10, Channel);
+            NDS::ARM7Write16(replybuf+0x12, 0);
+
+            // source MAC
+            NDS::ARM7Write16(replybuf+0xA, *(u16*)&RXBuffer[12+10]);
+            NDS::ARM7Write16(replybuf+0xC, *(u16*)&RXBuffer[12+12]);
+            NDS::ARM7Write16(replybuf+0xE, *(u16*)&RXBuffer[12+14]);
+
+            NDS::ARM7Write16(replybuf+0x14, 0); // ???
+
+            NDS::ARM7Write16(replybuf+0x36, numextra);
+            for (int i = 0; i < numextra; i+=2)
+            {
+                NDS::ARM7Write16(replybuf+0x38+i, extra[i>>1]);
+            }
+        }
+        else
+        {
+            NDS::ARM7Write16(replybuf+0x8, 4);
+            NDS::ARM7Write16(replybuf+0x10, Channel);
+            NDS::ARM7Write16(replybuf+0x12, 0);
+        }
+    }
+    else if (cmd == 0xE && status == 0)
+    {
+        NDS::ARM7Write16(replybuf+0x04, 0xA);
+    }
     else
     {
         for (int i = 0; i < numextra; i++)
@@ -232,17 +531,12 @@ void WifiIPCReply(u16 cmd, u16 status, int numextra, u16* extra)
 void OnIPCRequest(u32 addr)
 {
     u16 cmd = NDS::ARM7Read16(addr);
+    cmd &= ~0x8000;
 
-    /*printf("WIFI HLE: cmd %04X\n", cmd);
+    if (cmd < 0x2E)
-    for (u32 i = 0; i < 16; i++)
     {
-        for (u32 j = 0; j < 16; j+=4)
+        NDS::ARM7Write32(SharedMem[1]+0x4, 1);
-        {
+        NDS::ARM7Write16(SharedMem[1]+0x2, cmd);
-            u32 varp = NDS::ARM7Read32(addr+(i*16)+j);
-            printf("%08X ", varp);
-        }
-        printf("\n");
-    }*/
 
         switch (cmd)
         {
@@ -259,6 +553,7 @@ void OnIPCRequest(u32 addr)
                 // TODO other shito too!!
 
                 NDS::ARM7Write16(SharedMem[1], 2);
+                Platform::MP_Begin();
 
                 WifiIPCReply(0x0, 0);
             }
@@ -270,6 +565,7 @@ void OnIPCRequest(u32 addr)
                 if (status == 2)
                 {
                     NDS::ARM7Write16(SharedMem[1], 0);
+                    Platform::MP_End();
                     WifiIPCReply(0x2, 0);
                 }
                 else
@@ -365,6 +661,7 @@ void OnIPCRequest(u32 addr)
                 WifiIPCReply(0x8, 0, 3, extra);
 
                 NDS::ARM7Write16(SharedMem[1]+0xC2, 1);
+                ScheduleTimer(true);
             }
             break;
 
@@ -375,13 +672,14 @@ void OnIPCRequest(u32 addr)
                 //
                 // COMMAND BUFFER
                 // offset  size  desc.
+                // 02      2     channel
                 // 04      4     pointer to beacon data buffer
                 // 08      2     timeout in milliseconds
                 // 0A      6     desired source MAC? seen set to FFFFFFFFFFFF
                 //
                 // REPLY BUFFER
                 // offset  size  desc.
-            // 08      2     ??? seen set to 4
+                // 08      2     5 to indicate a new beacon, 4 otherwise
                 // 0A      6     source MAC from beacon (zero if none)
                 // 10      ?     channel
                 // 36      2     length of tag DD data (minus first 8 bytes) (zero if none)
@@ -427,6 +725,39 @@ void OnIPCRequest(u32 addr)
                 // 3C      2     length of tag DD data (minus first 8 bytes)
                 // 3E      2     number of bad tag DD's (when first 4 bytes are =/= 00:09:BF:00)
                 // 40      X     tag DD data (minus first 8 bytes)
+
+                u16 status = NDS::ARM7Read16(SharedMem[1]);
+                if (status != 2 && status != 3 && status != 5)
+                {
+                    u16 ext = 4;
+                    WifiIPCReply(0xA, 3, 1, &ext);
+                    break;
+                }
+
+                // TODO: check incoming parameters
+
+                Channel = NDS::ARM7Read16(addr+0x2);
+                HostScanCount = NDS::ARM7Read16(addr+0x8);
+                HostScanBuffer = NDS::ARM7Read32(addr+0x4);
+
+                NDS::ARM7Write16(SharedMem[1], 5);
+                ScheduleTimer(true);
+            }
+            break;
+
+        case 0xE: // start local MP
+            {
+                // TODO!!
+
+                u16 status = NDS::ARM7Read16(SharedMem[1]);
+                if (status == 8)
+                    NDS::ARM7Write16(SharedMem[1], 10);
+                else if (status == 7)
+                    NDS::ARM7Write16(SharedMem[1], 9);
+
+                printf("WIFI HLE: START MP\n");
+
+                WifiIPCReply(0xE, 0);
             }
             break;
 
@@ -450,6 +781,12 @@ void OnIPCRequest(u32 addr)
             printf("WIFI HLE: unknown command %04X\n", cmd);
             break;
         }
+
+        NDS::ARM7Write32(SharedMem[1]+0x4, 0);
+    }
+
+    cmd |= 0x8000;
+    NDS::ARM7Write16(addr, cmd);
 }
 
 }