From da4acab0bdcc13b29f145a0ac7fbe83172d217bc Mon Sep 17 00:00:00 2001 From: Arisotura Date: Thu, 25 Aug 2022 00:36:31 +0200 Subject: [PATCH] somewhat better exchange/sync method --- src/Platform.h | 8 +- src/Wifi.cpp | 337 ++++++++++++++++++++------ src/frontend/qt_sdl/LocalMP.cpp | 389 +++++++++++++++---------------- src/frontend/qt_sdl/LocalMP.h | 7 +- src/frontend/qt_sdl/Platform.cpp | 17 +- 5 files changed, 475 insertions(+), 283 deletions(-) diff --git a/src/Platform.h b/src/Platform.h index c05d31cc..ea50f53b 100644 --- a/src/Platform.h +++ b/src/Platform.h @@ -158,9 +158,11 @@ bool MP_Init(); void MP_DeInit(); int MP_SendPacket(u8* data, int len, u64 timestamp); int MP_RecvPacket(u8* data, bool block, u64* timestamp); -bool MP_SendSync(u16 clientmask, u16 type, u64 val); -bool MP_WaitSync(u16 clientmask, u16* type, u64* val); -u16 MP_WaitMultipleSyncs(u16 type, u16 clientmask, u64 curval); +int MP_SendCmd(u8* data, int len, u64 timestamp); +int MP_SendReply(u8* data, int len, u64 timestamp, u16 aid); +int MP_SendAck(u8* data, int len, u64 timestamp); +u16 MP_RecvReplies(u8* data, u64 timestamp, u16 aidmask); + // LAN comm interface // packet type: Ethernet (802.3) diff --git a/src/Wifi.cpp b/src/Wifi.cpp index 97e71c12..27d3932c 100644 --- a/src/Wifi.cpp +++ b/src/Wifi.cpp @@ -41,6 +41,11 @@ u16 IO[0x1000>>1]; #define IOPORT(x) IO[(x)>>1] #define IOPORT8(x) ((u8*)IO)[x] +// destination MACs for MP frames +const u8 MPCmdMAC[6] = {0x03, 0x09, 0xBF, 0x00, 0x00, 0x00}; +const u8 MPReplyMAC[6] = {0x03, 0x09, 0xBF, 0x00, 0x00, 0x10}; +const u8 MPAckMAC[6] = {0x03, 0x09, 0xBF, 0x00, 0x00, 0x03}; + u16 Random; // general, always-on microsecond counter @@ -91,6 +96,8 @@ int MPReplyTimer; u16 MPCurClient; u16 MPClientMask, MPClientFail; +u8 MPClientReplies[15*1024]; + bool MPInited; bool LANInited; @@ -101,6 +108,7 @@ bool ForcePowerOn; bool IsMPClient; u64 TimeOffsetToHost; // clienttime - hosttime u64 NextSync; // for clients: timestamp for next forced sync +u64 RXCutoff; u32 NextSyncType; bool SyncBack; // for clients: whether to send the host a sync once the sync is reached const u64 kMaxRunahead = 4096; @@ -219,6 +227,8 @@ void Reset() memset(&IOPORT(0x020), 0xFF, 6); USTimestamp = 0; + RXTimestamp = 0; + RXCutoff = 0; USCounter = 0; USCompare = 0; @@ -421,6 +431,7 @@ void StartTX_Cmd() // TODO: cancel the transfer if there isn't enough time left (check CMDCOUNT) if (IOPORT(W_TXSlotCmd) & 0x7000) printf("wifi: !! unusual TXSLOT_CMD bits set %04X %08X\n", IOPORT(W_TXSlotCmd), NDS::GetPC(1)); + //if (IOPORT(W_TXSlotCmd) & 0x7000) getc(stdin); slot->Addr = (IOPORT(W_TXSlotCmd) & 0x0FFF) << 1; slot->Length = *(u16*)&RAM[slot->Addr + 0xA] & 0x3FFF; @@ -493,6 +504,38 @@ void FireTX() } u16 vogon = 0x1312; +void SendMPDefaultReply() +{ + u8 reply[12 + 32]; + + *(u16*)&reply[0xA] = 28; // length + + // rate + //if (TXSlots[1].Rate == 2) reply[0x8] = 0x14; + //else reply[0x8] = 0xA; + // TODO + reply[0x8] = 0x14; + + *(u16*)&reply[0x6] = vogon; + + *(u16*)&reply[0xC + 0x00] = 0x0158; + *(u16*)&reply[0xC + 0x02] = 0x00F0;//0; // TODO?? + *(u16*)&reply[0xC + 0x04] = IOPORT(W_BSSID0); + *(u16*)&reply[0xC + 0x06] = IOPORT(W_BSSID1); + *(u16*)&reply[0xC + 0x08] = IOPORT(W_BSSID2); + *(u16*)&reply[0xC + 0x0A] = IOPORT(W_MACAddr0); + *(u16*)&reply[0xC + 0x0C] = IOPORT(W_MACAddr1); + *(u16*)&reply[0xC + 0x0E] = IOPORT(W_MACAddr2); + *(u16*)&reply[0xC + 0x10] = 0x0903; + *(u16*)&reply[0xC + 0x12] = 0x00BF; + *(u16*)&reply[0xC + 0x14] = 0x1000; + *(u16*)&reply[0xC + 0x16] = IOPORT(W_TXSeqNo) << 4; + *(u32*)&reply[0xC + 0x18] = 0; + + int txlen = Platform::MP_SendReply(reply, 12+28, USTimestamp, IOPORT(W_AIDLow)); + WIFI_LOG("wifi: sent %d/40 bytes of MP default reply\n", txlen); +} + void SendMPReply(u16 clienttime, u16 clientmask) { TXSlot* slot = &TXSlots[5]; @@ -541,10 +584,15 @@ void SendMPReply(u16 clienttime, u16 clientmask) IncrementTXCount(slot); slot->CurPhase = 0; + + int txlen = Platform::MP_SendReply(&RAM[slot->Addr], 12 + slot->Length, USTimestamp, IOPORT(W_AIDLow)); + WIFI_LOG("wifi: sent %d/40 bytes of MP reply\n", txlen); } else { slot->CurPhase = 10; + + SendMPDefaultReply(); } u16 clientnum = 0; @@ -559,38 +607,6 @@ void SendMPReply(u16 clienttime, u16 clientmask) IOPORT(W_TXBusy) |= 0x0080; } -void SendMPDefaultReply() -{ - u8 reply[12 + 32]; - - *(u16*)&reply[0xA] = 28; // length - - // rate - //if (TXSlots[1].Rate == 2) reply[0x8] = 0x14; - //else reply[0x8] = 0xA; - // TODO - reply[0x8] = 0x14; - - *(u16*)&reply[0x6] = vogon; - - *(u16*)&reply[0xC + 0x00] = 0x0158; - *(u16*)&reply[0xC + 0x02] = 0x00F0;//0; // TODO?? - *(u16*)&reply[0xC + 0x04] = IOPORT(W_BSSID0); - *(u16*)&reply[0xC + 0x06] = IOPORT(W_BSSID1); - *(u16*)&reply[0xC + 0x08] = IOPORT(W_BSSID2); - *(u16*)&reply[0xC + 0x0A] = IOPORT(W_MACAddr0); - *(u16*)&reply[0xC + 0x0C] = IOPORT(W_MACAddr1); - *(u16*)&reply[0xC + 0x0E] = IOPORT(W_MACAddr2); - *(u16*)&reply[0xC + 0x10] = 0x0903; - *(u16*)&reply[0xC + 0x12] = 0x00BF; - *(u16*)&reply[0xC + 0x14] = 0x1000; - *(u16*)&reply[0xC + 0x16] = IOPORT(W_TXSeqNo) << 4; - *(u32*)&reply[0xC + 0x18] = 0; - - int txlen = Platform::MP_SendPacket(reply, 12+28, USTimestamp); - WIFI_LOG("wifi: sent %d/40 bytes of MP default reply\n", txlen); -} - void SendMPAck(u16 clientfail) { u8 ack[12 + 32]; @@ -601,27 +617,28 @@ void SendMPAck(u16 clientfail) if (TXSlots[1].Rate == 2) ack[0x8] = 0x14; else ack[0x8] = 0xA; - *(u16*)&ack[0xC + 0x00] = 0x0218; - *(u16*)&ack[0xC + 0x02] = 0; - *(u16*)&ack[0xC + 0x04] = 0x0903; - *(u16*)&ack[0xC + 0x06] = 0x00BF; - *(u16*)&ack[0xC + 0x08] = 0x0300; - *(u16*)&ack[0xC + 0x0A] = IOPORT(W_BSSID0); - *(u16*)&ack[0xC + 0x0C] = IOPORT(W_BSSID1); - *(u16*)&ack[0xC + 0x0E] = IOPORT(W_BSSID2); - *(u16*)&ack[0xC + 0x10] = IOPORT(W_MACAddr0); - *(u16*)&ack[0xC + 0x12] = IOPORT(W_MACAddr1); - *(u16*)&ack[0xC + 0x14] = IOPORT(W_MACAddr2); - *(u16*)&ack[0xC + 0x16] = IOPORT(W_TXSeqNo) << 4; - *(u16*)&ack[0xC + 0x18] = 0x0033; // ??? - *(u16*)&ack[0xC + 0x1A] = clientfail; - *(u32*)&ack[0xC + 0x1C] = 0; + *(u16*)&ack[0xC + 0x00] = 0x0218; + *(u16*)&ack[0xC + 0x02] = 0; + *(u16*)&ack[0xC + 0x04] = 0x0903; + *(u16*)&ack[0xC + 0x06] = 0x00BF; + *(u16*)&ack[0xC + 0x08] = 0x0300; + *(u16*)&ack[0xC + 0x0A] = IOPORT(W_BSSID0); + *(u16*)&ack[0xC + 0x0C] = IOPORT(W_BSSID1); + *(u16*)&ack[0xC + 0x0E] = IOPORT(W_BSSID2); + *(u16*)&ack[0xC + 0x10] = IOPORT(W_MACAddr0); + *(u16*)&ack[0xC + 0x12] = IOPORT(W_MACAddr1); + *(u16*)&ack[0xC + 0x14] = IOPORT(W_MACAddr2); + *(u16*)&ack[0xC + 0x16] = IOPORT(W_TXSeqNo) << 4; + *(u16*)&ack[0xC + 0x18] = 0x0033; // ??? + *(u16*)&ack[0xC + 0x1A] = clientfail; + *(u32*)&ack[0xC + 0x1C] = 0; - int txlen = Platform::MP_SendPacket(ack, 12+32, USTimestamp); - WIFI_LOG("wifi: sent %d/44 bytes of MP ack, %d %d\n", txlen, ComStatus, RXTime); + int txlen = Platform::MP_SendAck(ack, 12+32, USTimestamp); + WIFI_LOG("wifi: sent %d/44 bytes of MP ack, %d %d\n", txlen, ComStatus, RXTime); } -int baurf; -bool CheckRX(bool local); + +bool CheckRX(int type); +void MPClientReplyRX(int client); bool ProcessTX(TXSlot* slot, int num) { @@ -638,10 +655,12 @@ bool ProcessTX(TXSlot* slot, int num) MPReplyTimer--; if (MPReplyTimer == 0 && MPClientMask != 0) { - u32 curclient = 0x0002; - while (!(MPClientMask & curclient)) curclient <<= 1; -baurf=1; - if (CheckRX(true)) + int nclient = 1; + while (!(MPClientMask & (1 << nclient))) nclient++; + + u32 curclient = 1 << nclient; + + /*if (CheckRX(1)) { // we received a reply, mark it as such // TODO: is any received packet considered a good reply? @@ -649,6 +668,9 @@ baurf=1; MPClientFail &= ~curclient; } + else printf("REPLY %04X NOT RECEIVED\n");*/ + if (!(MPClientFail & curclient)) + MPClientReplyRX(nclient); MPReplyTimer = 10 + IOPORT(W_CmdReplyTime); MPClientMask &= ~curclient; @@ -749,7 +771,7 @@ baurf=1; //Platform::MP_SendSync(0xFFFF, 2, USTimestamp); // send - int txlen = Platform::MP_SendPacket(&RAM[slot->Addr], 12 + slot->Length, USTimestamp); + int txlen = Platform::MP_SendCmd(&RAM[slot->Addr], 12 + slot->Length, USTimestamp); WIFI_LOG("wifi: sent %d/%d bytes of slot%d packet, addr=%04X, framectl=%04X, %04X %04X\n", txlen, slot->Length+12, num, slot->Addr, *(u16*)&RAM[slot->Addr + 0xC], *(u16*)&RAM[slot->Addr + 0x24], *(u16*)&RAM[slot->Addr + 0x26]); @@ -765,16 +787,17 @@ baurf=1; } else if (num == 5) { - u16 barfo = *(u16*)&RAM[slot->Addr+6]; + /*u16 barfo = *(u16*)&RAM[slot->Addr+6]; *(u16*)&RAM[slot->Addr+6] = vogon; // send - int txlen = Platform::MP_SendPacket(&RAM[slot->Addr], 12 + slot->Length, USTimestamp); + int txlen = Platform::MP_SendReply(&RAM[slot->Addr], 12 + slot->Length, USTimestamp, IOPORT(W_AIDLow)); WIFI_LOG("wifi: sent %d/%d bytes of slot%d packet, addr=%04X, framectl=%04X, %04X %04X\n", txlen, slot->Length+12, num, slot->Addr, *(u16*)&RAM[slot->Addr + 0xC], *(u16*)&RAM[slot->Addr + 0x24], *(u16*)&RAM[slot->Addr + 0x26]); - *(u16*)&RAM[slot->Addr+6] = barfo; + *(u16*)&RAM[slot->Addr+6] = barfo;*/ + // frame was } else //if (num != 5) { @@ -819,7 +842,7 @@ baurf=1; SetIRQ(7); SetStatus(8); - SendMPDefaultReply(); + //SendMPDefaultReply(); //slot->Addr = 0; //slot->Length = 28; @@ -851,6 +874,9 @@ baurf=1; MPClientMask = clientmask; MPClientFail = clientmask; + u16 res = Platform::MP_RecvReplies(MPClientReplies, USTimestamp, clientmask); + MPClientFail &= ~res; + // TODO: 112 likely includes the ack preamble, which needs adjusted // for long-preamble settings slot->CurPhase = 2; @@ -970,6 +996,7 @@ baurf=1; u16 clientmask = *(u16*)&RAM[slot->Addr + 12 + 24 + 2]; //Platform::MP_SendSync(clientmask, 1, kMaxRunahead); + RXCutoff = USTimestamp; FireTX(); } return true; @@ -1032,8 +1059,150 @@ void StartRX() LocalMP::_logstring2(USTimestamp, "STARTING RX", ((*(u16*)&RXBuffer[12])<<16)|(*(u16*)&RXBuffer[12+26]), RXTimestamp); } -u16 zarp = 0; -bool CheckRX(bool local) +/*void FinishRX() +{ + u16 addr = IOPORT(W_RXTXAddr) << 1; + if (addr & 0x2) IncrementRXAddr(addr); + + // copy the RX header + u16 headeraddr = IOPORT(W_RXBufWriteCursor) << 1; + *(u16*)&RAM[headeraddr] = *(u16*)&RXBuffer[0]; IncrementRXAddr(headeraddr); + *(u16*)&RAM[headeraddr] = *(u16*)&RXBuffer[2]; IncrementRXAddr(headeraddr, 4); + *(u16*)&RAM[headeraddr] = *(u16*)&RXBuffer[6]; IncrementRXAddr(headeraddr); + *(u16*)&RAM[headeraddr] = *(u16*)&RXBuffer[8]; IncrementRXAddr(headeraddr); + *(u16*)&RAM[headeraddr] = *(u16*)&RXBuffer[10]; + + IOPORT(W_RXBufWriteCursor) = (addr & ~0x3) >> 1; + + SetIRQ(0); + SetStatus(1); +//printf("%016llX: finished receiving a frame, aid=%04X, FC=%04X, client=%04X\n", USTimestamp, IOPORT(W_AIDLow), *(u16*)&RXBuffer[0xC], *(u16*)&RXBuffer[0xC + 26]); + WIFI_LOG("wifi: finished receiving packet %04X\n", *(u16*)&RXBuffer[12]); + LocalMP::_logstring2(USTimestamp, "FINISH RX", ((IOPORT(W_AIDLow))<<16)|(*(u16*)&RXBuffer[12+26]), RXTimestamp); + + ComStatus &= ~0x1; + RXCounter = 0; + + if ((RXBuffer[0] & 0x0F) == 0x0C) + {bidon=USTimestamp-bazar;bazar=USTimestamp; + u16 clientmask = *(u16*)&RXBuffer[0xC + 26]; + //printf("RECEIVED REPLY!!! CLIENT=%04X AID=%04X\n", clientmask, IOPORT(W_AIDLow)); + if (IOPORT(W_AIDLow) && (RXBuffer[0xC + 4] & 0x01) && (clientmask & (1 << IOPORT(W_AIDLow)))) + {//printf("%016llX: sending MP reply\n", USTimestamp); + LocalMP::_logstring(USTimestamp, "SENDING MP REPLY"); + SendMPReply(*(u16*)&RXBuffer[0xC + 24], clientmask); + } + } +}*/ + +void MPClientReplyRX(int client) +{ + if (!(IOPORT(W_RXCnt) & 0x8000)) + return; + + if (IOPORT(W_RXBufBegin) == IOPORT(W_RXBufEnd)) + return; + + u16 framelen; + u16 framectl; + u8 txrate; + bool bssidmatch; + u16 rxflags; + + u8* reply = &MPClientReplies[(client-1)*1024]; + framelen = *(u16*)&reply[10]; + framelen -= 4; + + // TODO rework RX system so we don't need this (by reading directly into MPClientReplies) + memcpy(RXBuffer, reply, 12+framelen); + + framectl = *(u16*)&RXBuffer[12+0]; + txrate = RXBuffer[8]; + + u32 a_src, a_dst, a_bss; + rxflags = 0x0010; + switch (framectl & 0x000C) + { + case 0x0000: // management + a_src = 10; + a_dst = 4; + a_bss = 16; + if ((framectl & 0x00F0) == 0x0080) + rxflags |= 0x0001; + break; + + case 0x0004: // control + printf("blarg\n"); + return; + + case 0x0008: // data + switch (framectl & 0x0300) + { + case 0x0000: // STA to STA + a_src = 10; + a_dst = 4; + a_bss = 16; + break; + case 0x0100: // STA to DS + a_src = 10; + a_dst = 16; + a_bss = 4; + break; + case 0x0200: // DS to STA + a_src = 16; + a_dst = 4; + a_bss = 10; + break; + case 0x0300: // DS to DS + printf("blarg\n"); + return; + } + // TODO: those also trigger on other framectl values + // like 0208 -> C + framectl &= 0xE7FF; + if (framectl == 0x0228) rxflags |= 0x000C; // MP host frame + else if (framectl == 0x0218) rxflags |= 0x000D; // MP ack frame + else if (framectl == 0x0118) rxflags |= 0x000E; // MP reply frame + else if (framectl == 0x0158) rxflags |= 0x000F; // empty MP reply frame + else rxflags |= 0x0008; + break; + } + + // TODO get rid of this cruft!!! + + if (MACEqual(&RXBuffer[12 + a_src], (u8*)&IOPORT(W_MACAddr0))) + { + printf("MAC equal??\n"); + return; // oops. we received a packet we just sent. + } + + bssidmatch = MACEqual(&RXBuffer[12 + a_bss], (u8*)&IOPORT(W_BSSID0)); + //if (!(IOPORT(W_BSSID0) & 0x0001) && !(RXBuffer[12 + a_bss] & 0x01) && + if (!MACEqual(&RXBuffer[12 + a_dst], (u8*)&IOPORT(W_MACAddr0)) && + !(RXBuffer[12 + a_dst] & 0x01)) + { + printf("dst MAC bad\n"); + PRINT_MAC("frame: ", &RXBuffer[12+a_dst]); + PRINT_MAC("mac: ", (u8*)&IOPORT(W_MACAddr0)); + return; + } + + // make RX header + + if (bssidmatch) rxflags |= 0x8000; + + *(u16*)&RXBuffer[0] = rxflags; + *(u16*)&RXBuffer[2] = 0x0040; // ??? + *(u16*)&RXBuffer[6] = txrate; + *(u16*)&RXBuffer[8] = framelen; + *(u16*)&RXBuffer[10] = 0x4080; // min/max RSSI. dunno + + RXTimestamp = 0; + StartRX(); +} + +u16 zarp = 0; u64 bazar=0, bidon=0; +bool CheckRX(int type) // 0=regular 1=MP replies 2=MP host frames { if (!(IOPORT(W_RXCnt) & 0x8000)) return false; @@ -1041,7 +1210,7 @@ bool CheckRX(bool local) if (IOPORT(W_RXBufBegin) == IOPORT(W_RXBufEnd)) return false; //printf("CheckRX(%d) %016llX\n", local, USTimestamp); -if (ComStatus & 1) printf("WHAT?????? CHECKRX(%d,%d) WHILE ALREADY RECEIVING %04X\n", local, baurf, *(u16*)&RXBuffer[12]); +if (ComStatus & 1) printf("WHAT?????? CHECKRX(%d) WHILE ALREADY RECEIVING %04X\n", type, *(u16*)&RXBuffer[12]); u16 framelen; u16 framectl; u8 txrate; @@ -1052,11 +1221,14 @@ if (ComStatus & 1) printf("WHAT?????? CHECKRX(%d,%d) WHILE ALREADY RECEIVING %04 for (;;) { timestamp = 0; - int rxlen = Platform::MP_RecvPacket(RXBuffer, local, ×tamp); - if ((rxlen == 0) && (!local)) rxlen = WifiAP::RecvPacket(RXBuffer); + int rxlen = Platform::MP_RecvPacket(RXBuffer, (type != 0), ×tamp); + if ((rxlen == 0) && (type == 0)) rxlen = WifiAP::RecvPacket(RXBuffer); if (rxlen == 0) return false; if (rxlen < 12+24) continue; + if (timestamp < RXCutoff) + continue; + framelen = *(u16*)&RXBuffer[10]; if (framelen != rxlen-12) { @@ -1065,6 +1237,13 @@ if (ComStatus & 1) printf("WHAT?????? CHECKRX(%d,%d) WHILE ALREADY RECEIVING %04 } framelen -= 4; + /*if (type == 1) + { + // reject stale frames, so we don't run too far ahead of the clients and cause a mess + if (timestamp < (USTimestamp - 50)) + continue; + }*/ + framectl = *(u16*)&RXBuffer[12+0]; txrate = RXBuffer[8]; @@ -1154,12 +1333,12 @@ if (ComStatus & 1) printf("WHAT?????? CHECKRX(%d,%d) WHILE ALREADY RECEIVING %04 //if ((framectl&0xFF00)==0) printf("RECEIVED: %04X\n", framectl); zarp = framectl; - if (local && (framectl == 0x0118 || framectl == 0x0158)) + /*if (local && (framectl == 0x0118 || framectl == 0x0158)) {//printf("received reply: %016llX, %016llX\n", timestamp, USTimestamp); u16 bourf = (IOPORT(W_TXSeqNo) - 0x1) << 4; if (bourf != *(u16*)&RXBuffer[6]) printf("BAD REPLY SEQNO!!! SENT %04X, RECV %04X\n", bourf, *(u16*)&RXBuffer[6]); - } + }*/ // make RX header @@ -1237,7 +1416,8 @@ if (RXTimestamp < USTimestamp) printf("!!! RECEIVED FRAME IN THE PAST\n"); else { // otherwise, just start receiving this frame now - +if ((rxflags&0xF)>=0xE && TXSlots[1].CurPhase!=2) + printf("!!! RECEIVING MP REPLY OUTSIDE OF WINDOW (%016llX %016lX, type=%d phase=%d)\n", timestamp, USTimestamp, type, TXSlots[1].CurPhase); RXTimestamp = 0; StartRX(); } @@ -1296,8 +1476,8 @@ void USTimer(u32 param) }*/ //if (NextSyncType == 2) if (!(ComStatus & 1)) - {baurf=2; - CheckRX(true); + { + CheckRX(2); } //u64 sync = Platform::MP_WaitSync(1, 1<<(IOPORT(W_AIDLow)&0xF), USCounter - TimeOffsetToHost); @@ -1405,8 +1585,8 @@ void USTimer(u32 param) if ((!IsMPClient) || (USTimestamp > NextSync)) { if ((!(RXCounter & 0x1FF))) - {baurf=3; - CheckRX(false); + { + CheckRX(0); } } @@ -1472,13 +1652,22 @@ void USTimer(u32 param) RXCounter = 0; if ((RXBuffer[0] & 0x0F) == 0x0C) - { + {bidon=USTimestamp-bazar;bazar=USTimestamp; u16 clientmask = *(u16*)&RXBuffer[0xC + 26]; + //printf("RECEIVED REPLY!!! CLIENT=%04X AID=%04X\n", clientmask, IOPORT(W_AIDLow)); if (IOPORT(W_AIDLow) && (RXBuffer[0xC + 4] & 0x01) && (clientmask & (1 << IOPORT(W_AIDLow)))) {//printf("%016llX: sending MP reply\n", USTimestamp); LocalMP::_logstring(USTimestamp, "SENDING MP REPLY"); SendMPReply(*(u16*)&RXBuffer[0xC + 24], clientmask); } + else + { + // send a blank + // this is just so the host can have something to receive, instead of hitting a timeout + // in the case this client wasn't ready to send a reply + + Platform::MP_SendReply(nullptr, 0, USTimestamp, 0); + } } } @@ -1751,7 +1940,7 @@ void Write(u32 addr, u16 val) return; case W_AIDLow: - //printf("[%016llX] AIDLOW=%04X (%08X)\n", USTimestamp, val, NDS::GetPC(1)); + //printf("[%016llX] AIDLOW=%04X (%08X), frame=%016llX (intv %016llX) frameintv=%016llX\n", USTimestamp, val, NDS::GetPC(1), bazar, USTimestamp-bazar, bidon); IOPORT(W_AIDLow) = val & 0x000F; return; case W_AIDFull: diff --git a/src/frontend/qt_sdl/LocalMP.cpp b/src/frontend/qt_sdl/LocalMP.cpp index 06da6e0c..a676853d 100644 --- a/src/frontend/qt_sdl/LocalMP.cpp +++ b/src/frontend/qt_sdl/LocalMP.cpp @@ -69,14 +69,17 @@ struct MPQueueHeader { u16 NumInstances; u16 InstanceBitmask; - u32 SyncWriteOffset; u32 PacketWriteOffset; + u32 ReplyWriteOffset; + u16 MPHostInstanceID; // instance ID from which the last CMD frame was sent + u16 MPReplyBitmask; // bitmask of which clients replied in time }; struct MPPacketHeader { u32 Magic; u32 SenderID; + u32 Type; // 0=regular 1=CMD 2=reply 3=ack u32 Length; u64 Timestamp; }; @@ -93,13 +96,17 @@ struct MPSync QSharedMemory* MPQueue; //QSystemSemaphore* MPQueueSem[16]; int InstanceID; -u32 SyncReadOffset; u32 PacketReadOffset; +u32 ReplyReadOffset; -const u32 kSyncStart = 0x0010; -const u32 kSyncEnd = 0x0100; -const u32 kPacketStart = 0x0100; -const u32 kPacketEnd = 0x10000; +const u32 kQueueSize = 0x20000; +const u32 kMaxFrameSize = 0x800; +const u32 kPacketStart = 0x00010; +const u32 kReplyStart = kQueueSize / 2; +const u32 kPacketEnd = kReplyStart; +const u32 kReplyEnd = kQueueSize; + +const int RecvTimeout = 500; #define NIFI_VER 2 @@ -123,6 +130,14 @@ void SemPoolInit() } } +void SemDeinit(int num); + +void SemPoolDeinit() +{ + for (int i = 0; i < 32; i++) + SemDeinit(i); +} + bool SemInit(int num) { if (SemInited[num]) @@ -143,8 +158,9 @@ void SemDeinit(int num) { CloseHandle(SemPool[num]); SemPool[num] = INVALID_HANDLE_VALUE; - SemInited[num] = false; } + + SemInited[num] = false; } bool SemPost(int num) @@ -158,6 +174,25 @@ bool SemWait(int num, int timeout) return WaitForSingleObject(SemPool[num], timeout) == WAIT_OBJECT_0; } +/*bool SemWaitMultiple(int start, u16 bitmask, int timeout) +{ + HANDLE semlist[16]; + int numsem = 0; + + for (int i = 0; i < 16; i++) + { + if (bitmask & (1<= WAIT_OBJECT_0) && (res < (WAIT_OBJECT_0+numsem)); +}*/ + #else // TODO: code semaphore shit for other platforms! @@ -166,7 +201,7 @@ bool SemWait(int num, int timeout) void _logpacket(bool tx, u8* data, int len, u64 ts) -{return; +{//return; char path[256]; sprintf(path, "framelog_%08X.log", InstanceID); static FILE* f = nullptr; @@ -194,7 +229,7 @@ void _logpacket(bool tx, u8* data, int len, u64 ts) } void _logstring(u64 ts, char* str) -{return; +{//return; char path[256]; sprintf(path, "framelog_%08X.log", InstanceID); static FILE* f = nullptr; @@ -205,7 +240,7 @@ void _logstring(u64 ts, char* str) } void _logstring2(u64 ts, char* str, u32 arg, u64 arg2) -{return; +{//return; char path[256]; sprintf(path, "framelog_%08X.log", InstanceID); static FILE* f = nullptr; @@ -318,7 +353,7 @@ bool Init() if (!MPQueue->attach()) { printf("MP sharedmem doesn't exist. creating\n"); - if (!MPQueue->create(65536)) + if (!MPQueue->create(kQueueSize)) { printf("MP sharedmem create failed :(\n"); return false; @@ -327,8 +362,8 @@ bool Init() MPQueue->lock(); memset(MPQueue->data(), 0, MPQueue->size()); MPQueueHeader* header = (MPQueueHeader*)MPQueue->data(); - header->SyncWriteOffset = kSyncStart; header->PacketWriteOffset = kPacketStart; + header->ReplyWriteOffset = kReplyStart; MPQueue->unlock(); } @@ -347,8 +382,8 @@ bool Init() } header->NumInstances++; - SyncReadOffset = header->SyncWriteOffset; PacketReadOffset = header->PacketWriteOffset; + ReplyReadOffset = header->ReplyWriteOffset; MPQueue->unlock(); @@ -358,6 +393,10 @@ bool Init() MPQueueSem[i] = new QSystemSemaphore(key, 0, (i==InstanceID) ? QSystemSemaphore::Create : QSystemSemaphore::Open) }*/ + // prepare semaphores + // semaphores 0-15: regular frames; semaphore I is posted when instance I needs to process a new frame + // semaphores 16-31: MP replies; semaphore I is posted by instance I when it sends a MP reply + SemPoolInit(); SemInit(InstanceID); SemInit(16+InstanceID); @@ -377,24 +416,43 @@ void DeInit() #ifdef __WIN32__ WSACleanup(); #endif // __WIN32__*/ - SemDeinit(InstanceID); - SemDeinit(16+InstanceID); + //SemDeinit(InstanceID); + //SemDeinit(16+InstanceID); + MPQueue->lock(); + MPQueueHeader* header = (MPQueueHeader*)MPQueue->data(); + header->InstanceBitmask &= ~(1 << InstanceID); + MPQueue->unlock(); + + SemPoolDeinit(); MPQueue->detach(); delete MPQueue; } -void PacketFIFORead(void* buf, int len) +void FIFORead(int fifo, void* buf, int len) { u8* data = (u8*)MPQueue->data(); - u32 offset = PacketReadOffset; - if ((offset + len) >= kPacketEnd) + u32 offset, start, end; + if (fifo == 0) { - u32 part1 = kPacketEnd - offset; + offset = PacketReadOffset; + start = kPacketStart; + end = kPacketEnd; + } + else + { + offset = ReplyReadOffset; + start = kReplyStart; + end = kReplyEnd; + } + + if ((offset + len) >= end) + { + u32 part1 = end - offset; memcpy(buf, &data[offset], part1); - memcpy(&((u8*)buf)[part1], &data[kPacketStart], len - part1); - offset = kPacketStart + len - part1; + memcpy(&((u8*)buf)[part1], &data[start], len - part1); + offset = start + len - part1; } else { @@ -402,21 +460,35 @@ void PacketFIFORead(void* buf, int len) offset += len; } - PacketReadOffset = offset; + if (fifo == 0) PacketReadOffset = offset; + else ReplyReadOffset = offset; } -void PacketFIFOWrite(void* buf, int len) +void FIFOWrite(int fifo, void* buf, int len) { u8* data = (u8*)MPQueue->data(); MPQueueHeader* header = (MPQueueHeader*)&data[0]; - u32 offset = header->PacketWriteOffset; - if ((offset + len) >= kPacketEnd) + u32 offset, start, end; + if (fifo == 0) { - u32 part1 = kPacketEnd - offset; + offset = header->PacketWriteOffset; + start = kPacketStart; + end = kPacketEnd; + } + else + { + offset = header->ReplyWriteOffset; + start = kReplyStart; + end = kReplyEnd; + } + + if ((offset + len) >= end) + { + u32 part1 = end - offset; memcpy(&data[offset], buf, part1); - memcpy(&data[kPacketStart], &((u8*)buf)[part1], len - part1); - offset = kPacketStart + len - part1; + memcpy(&data[start], &((u8*)buf)[part1], len - part1); + offset = start + len - part1; } else { @@ -424,12 +496,12 @@ void PacketFIFOWrite(void* buf, int len) offset += len; } - header->PacketWriteOffset = offset; + if (fifo == 0) header->PacketWriteOffset = offset; + else header->ReplyWriteOffset = offset; } -int SendPacket(u8* packet, int len, u64 timestamp) +int SendPacketGeneric(u32 type, u8* packet, int len, u64 timestamp) { - _logpacket(true, packet, len, timestamp); MPQueue->lock(); u8* data = (u8*)MPQueue->data(); MPQueueHeader* header = (MPQueueHeader*)&data[0]; @@ -441,28 +513,58 @@ int SendPacket(u8* packet, int len, u64 timestamp) MPPacketHeader pktheader; pktheader.Magic = 0x4946494E; pktheader.SenderID = InstanceID; + pktheader.Type = type; pktheader.Length = len; pktheader.Timestamp = timestamp; - PacketFIFOWrite(&pktheader, sizeof(pktheader)); - PacketFIFOWrite(packet, len); + type &= 0xFFFF; + int nfifo = (type == 2) ? 1 : 0; + FIFOWrite(nfifo, &pktheader, sizeof(pktheader)); + if (len) + FIFOWrite(nfifo, packet, len); + + if (type == 1) + { + // NOTE: this is not guarded against, say, multiple multiplay games happening on the same machine + // we would need to pass the packet's SenderID through the wifi module for that + header->MPHostInstanceID = InstanceID; + header->MPReplyBitmask = 0; + ReplyReadOffset = header->ReplyWriteOffset; + } + else if (type == 2) + { + header->MPReplyBitmask |= (1 << InstanceID); + } MPQueue->unlock(); - for (int i = 0; i < 16; i++) + if (type == 2) { - if (mask & (1<MPHostInstanceID); + } + else + { + for (int i = 0; i < 16; i++) + { + if (mask & (1<data(); MPPacketHeader pktheader; - PacketFIFORead(&pktheader, sizeof(pktheader)); + FIFORead(0, &pktheader, sizeof(pktheader)); if (pktheader.Magic != 0x4946494E) { @@ -491,8 +593,9 @@ int RecvPacket(u8* packet, bool block, u64* timestamp) continue; } - PacketFIFORead(packet, pktheader.Length); - _logpacket(false, packet, pktheader.Length, pktheader.Timestamp); + if (pktheader.Length) + FIFORead(0, packet, pktheader.Length); + //_logpacket(false, packet, pktheader.Length, pktheader.Timestamp); if (timestamp) *timestamp = pktheader.Timestamp; MPQueue->unlock(); return pktheader.Length; @@ -500,186 +603,78 @@ int RecvPacket(u8* packet, bool block, u64* timestamp) } -void SyncFIFORead(MPSync* sync) +int SendCmd(u8* packet, int len, u64 timestamp) { - u8* data = (u8*)MPQueue->data(); - - int len = sizeof(MPSync); - u32 offset = SyncReadOffset; - if ((offset + len) >= kSyncEnd) - { - u32 part1 = kSyncEnd - offset; - memcpy(sync, &data[offset], part1); - memcpy(&((u8*)sync)[part1], &data[kSyncStart], len - part1); - offset = kSyncStart + len - part1; - } - else - { - memcpy(sync, &data[offset], len); - offset += len; - } - - SyncReadOffset = offset; + return SendPacketGeneric(1, packet, len, timestamp); } -void SyncFIFOWrite(MPSync* sync) +int SendReply(u8* packet, int len, u64 timestamp, u16 aid) { - u8* data = (u8*)MPQueue->data(); - MPQueueHeader* header = (MPQueueHeader*)&data[0]; - - int len = sizeof(MPSync); - u32 offset = header->SyncWriteOffset; - if ((offset + len) >= kSyncEnd) - { - u32 part1 = kSyncEnd - offset; - memcpy(&data[offset], sync, part1); - memcpy(&data[kSyncStart], &((u8*)sync)[part1], len - part1); - offset = kSyncStart + len - part1; - } - else - { - memcpy(&data[offset], sync, len); - offset += len; - } - - header->SyncWriteOffset = offset; + return SendPacketGeneric(2 | (aid<<16), packet, len, timestamp); } -bool SendSync(u16 clientmask, u16 type, u64 timestamp) +int SendAck(u8* packet, int len, u64 timestamp) { - MPQueue->lock(); - u8* data = (u8*)MPQueue->data(); - MPQueueHeader* header = (MPQueueHeader*)&data[0]; - - u16 mask = header->InstanceBitmask; - - // TODO: check if the FIFO is full! - - MPSync sync; - sync.Magic = 0x434E5953; - sync.SenderID = InstanceID; - sync.ClientMask = clientmask; - sync.Type = type; - sync.Timestamp = timestamp; - - SyncFIFOWrite(&sync); - - MPQueue->unlock(); - - for (int i = 0; i < 16; i++) - { - if (mask & (1<lock(); u8* data = (u8*)MPQueue->data(); + MPQueueHeader* header = (MPQueueHeader*)&data[0]; - MPSync sync; - SyncFIFORead(&sync); + MPPacketHeader pktheader; + FIFORead(1, &pktheader, sizeof(pktheader)); - if (sync.Magic != 0x434E5953) + if (pktheader.Magic != 0x4946494E) { - printf("MP: !!!! SYNC FIFO IS CRAPOED\n"); + printf("MP: !!!! REPLY FIFO IS CRAPOED\n"); MPQueue->unlock(); - return false; - } - - if (sync.SenderID == InstanceID) - { - MPQueue->unlock(); - continue; - } -printf("received sync: ID=%08X type=%04X mask=%04X (wanted=%04X) ts=%016llX\n", sync.SenderID, sync.Type, sync.ClientMask, clientmask, sync.Timestamp); - if (!(sync.ClientMask & clientmask)) - { - MPQueue->unlock(); - continue; - } - - if (type) *type = sync.Type; - if (timestamp) *timestamp = sync.Timestamp; - MPQueue->unlock(); - return true; - } -} - -u16 WaitMultipleSyncs(u16 type, u16 clientmask, u64 curval) -{ - /*u8 syncbuf[32]; - - if (!clientmask) - return 0; - - if (MPSocket[1] < 0) - return 0; - - fd_set fd; - struct timeval tv; - - for (;;) - { - FD_ZERO(&fd); - FD_SET(MPSocket[1], &fd); - tv.tv_sec = 0; - tv.tv_usec = 500*1000; - - if (!select(MPSocket[1]+1, &fd, 0, 0, &tv)) - {printf("[sync3] nope :(\n"); - return clientmask; - } - - sockaddr_t fromAddr; - socklen_t fromLen = sizeof(sockaddr_t); - int rlen = recvfrom(MPSocket[1], (char*)syncbuf, 32, 0, &fromAddr, &fromLen); - if (rlen != 8+16) - continue; - rlen -= 8; - - if (ntohl(*(u32*)&syncbuf[0]) != 0x4946494E) - continue; - - if (syncbuf[4] != NIFI_VER || syncbuf[5] != 1) - continue; -//printf("[sync3] atleast header is good\n"); - if (ntohs(*(u16*)&syncbuf[6]) != rlen) - continue; - - if (*(u32*)&syncbuf[12] == MPUniqueID) - continue; - - if (ntohs(*(u16*)&syncbuf[8]) != type) - continue; - - u16 clientval = ntohs(*(u16*)&syncbuf[10]); - //printf("[sync3] good rlen/type %04X %04X, clientmask=%04X \n", ntohs(*(u16*)&syncbuf[6]), ntohs(*(u16*)&syncbuf[8]), clientval); - if (!(clientmask & clientval)) - continue; - - // check the sync val, it should be ahead of the current sync val - u64 syncval = ntohl(*(u32*)&syncbuf[12]) | (((u64)ntohl(*(u32*)&syncbuf[16])) << 32); - //if (syncval <= curval) - // continue; -//printf("[sync3] good\n"); - clientmask &= ~clientval; - //if (!clientmask) return 0; + } + + if ((pktheader.SenderID == InstanceID) || // packet we sent out (shouldn't happen, but hey) + (pktheader.Timestamp < (timestamp - 32))) // stale packet + { + // skip this packet + ReplyReadOffset += pktheader.Length; + if (ReplyReadOffset >= kReplyEnd) + ReplyReadOffset += kReplyStart - kReplyEnd; + + MPQueue->unlock(); + continue; + } + + if (pktheader.Length) + { + u32 aid = (pktheader.Type >> 16); + FIFORead(1, &packets[(aid-1)*1024], pktheader.Length); + ret |= (1 << aid); + } + + instmask |= (1 << pktheader.SenderID); + if ((instmask & header->InstanceBitmask) == header->InstanceBitmask) + { + // all the clients have sent their reply + + MPQueue->unlock(); + return ret; + } + + MPQueue->unlock(); } - - return clientmask;*/ - return 0; } } diff --git a/src/frontend/qt_sdl/LocalMP.h b/src/frontend/qt_sdl/LocalMP.h index 609ed1e9..b2a44288 100644 --- a/src/frontend/qt_sdl/LocalMP.h +++ b/src/frontend/qt_sdl/LocalMP.h @@ -28,9 +28,10 @@ bool Init(); void DeInit(); int SendPacket(u8* data, int len, u64 timestamp); int RecvPacket(u8* data, bool block, u64* timestamp); -bool SendSync(u16 clientmask, u16 type, u64 val); -bool WaitSync(u16 clientmask, u16* type, u64* val); -u16 WaitMultipleSyncs(u16 type, u16 clientmask, u64 curval); +int SendCmd(u8* data, int len, u64 timestamp); +int SendReply(u8* data, int len, u64 timestamp, u16 aid); +int SendAck(u8* data, int len, u64 timestamp); +u16 RecvReplies(u8* data, u64 timestamp, u16 aidmask); } diff --git a/src/frontend/qt_sdl/Platform.cpp b/src/frontend/qt_sdl/Platform.cpp index 4a201110..62bd21db 100644 --- a/src/frontend/qt_sdl/Platform.cpp +++ b/src/frontend/qt_sdl/Platform.cpp @@ -379,19 +379,24 @@ int MP_RecvPacket(u8* data, bool block, u64* timestamp) return LocalMP::RecvPacket(data, block, timestamp); } -bool MP_SendSync(u16 clientmask, u16 type, u64 val) +int MP_SendCmd(u8* data, int len, u64 timestamp) { - return LocalMP::SendSync(clientmask, type, val); + return LocalMP::SendCmd(data, len, timestamp); } -bool MP_WaitSync(u16 clientmask, u16* type, u64* val) +int MP_SendReply(u8* data, int len, u64 timestamp, u16 aid) { - return LocalMP::WaitSync(clientmask, type, val); + return LocalMP::SendReply(data, len, timestamp, aid); } -u16 MP_WaitMultipleSyncs(u16 type, u16 clientmask, u64 curval) +int MP_SendAck(u8* data, int len, u64 timestamp) { - return LocalMP::WaitMultipleSyncs(type, clientmask, curval); + return LocalMP::SendAck(data, len, timestamp); +} + +u16 MP_RecvReplies(u8* data, u64 timestamp, u16 aidmask) +{ + return LocalMP::RecvReplies(data, timestamp, aidmask); }