diff --git a/src/Wifi.cpp b/src/Wifi.cpp index 9df3a32c..c8e3cd1e 100644 --- a/src/Wifi.cpp +++ b/src/Wifi.cpp @@ -41,6 +41,9 @@ u16 IO[0x1000>>1]; u16 Random; +// general, always-on microsecond counter +u64 USTimestamp; + u64 USCounter; u64 USCompare; bool BlockBeaconIRQ14; @@ -214,6 +217,8 @@ void Reset() memset(&IOPORT(0x018), 0xFF, 6); memset(&IOPORT(0x020), 0xFF, 6); + USTimestamp = 0; + USCounter = 0; USCompare = 0; BlockBeaconIRQ14 = false; @@ -1207,6 +1212,8 @@ void MSTimer() void USTimer(u32 param) { + USTimestamp++; + WifiAP::USTimer(); bool switchOffPowerSaving = false; @@ -1389,10 +1396,6 @@ void USTimer(u32 param) { SendMPReply(*(u16*)&RXBuffer[0xC + 24], *(u16*)&RXBuffer[0xC + 26]); } - /*else if (RXBuffer[0xC + 4] & 0x01) - { - DummyMPReplyFrame(*(u16*)&RXBuffer[0xC + 24], *(u16*)&RXBuffer[0xC + 26]); - }*/ } } @@ -1454,26 +1457,15 @@ void RFTransfer_Type3() } -// TODO: wifi waitstates - u16 Read(u32 addr) -{//printf("WIFI READ %08X\n", addr); +{ if (addr >= 0x04810000) return 0; addr &= 0x7FFE; - //printf("WIFI: read %08X\n", addr); + if (addr >= 0x4000 && addr < 0x6000) { - //if (addr>=0x4000 && addr<0x400C) printf("READ TX HEADER 0480%04X, %08X\n", addr, NDS::GetPC(1)); - //if (addr>=0x4234 && addr<0x4240) printf("READ TX HEADER 0480%04X, %08X\n", addr, NDS::GetPC(1)); - //if (RXBuffer[12] == 0x0228) - { - //u16 headeraddr = IOPORT(W_RXBufReadCursor) << 1; - //if ((addr & 0x1FFE) == (headeraddr + 12 + 28)) - //if (*(u16*)&RAM[(addr - 0x1C) & 0x1FFE] == 0x0228) - // printf("READ SHITTY CMD HEADER %08X\n", NDS::GetPC(1)); - } return *(u16*)&RAM[addr & 0x1FFE]; } if (addr >= 0x2000 && addr < 0x4000) @@ -1558,12 +1550,12 @@ u16 Read(u32 addr) } void Write(u32 addr, u16 val) -{//printf("WIFI WRITE %08X %04X\n", addr, val); +{ if (addr >= 0x04810000) return; addr &= 0x7FFE; - //printf("WIFI: write %08X %04X\n", addr, val); + if (addr >= 0x4000 && addr < 0x6000) { *(u16*)&RAM[addr & 0x1FFE] = val; @@ -1762,7 +1754,7 @@ void Write(u32 addr, u16 val) case W_USCompare2: USCompare = (USCompare & 0xFFFF0000FFFFFFFF) | ((u64)val << 32); return; case W_USCompare3: USCompare = (USCompare & 0x0000FFFFFFFFFFFF) | ((u64)val << 48); return; - case W_CmdCount: /*printf("CMDCOUNT=%d (%04X)\n", val*10, val);*/ CmdCounter = val * 10; return; + case W_CmdCount: CmdCounter = val * 10; return; case W_BBCnt: IOPORT(W_BBCnt) = val; @@ -1907,16 +1899,11 @@ void Write(u32 addr, u16 val) case 0x268: return; - case W_RXFilter: - //printf("wifi: stupid RX filter=%04X\n", val); - break; - default: //printf("WIFI unk: write %08X %04X\n", addr, val); break; } - //printf("WIFI: write %08X %04X\n", addr, val); IOPORT(addr&0xFFF) = val; } diff --git a/src/frontend/qt_sdl/LocalMP.cpp b/src/frontend/qt_sdl/LocalMP.cpp index 0a439bbb..e6280219 100644 --- a/src/frontend/qt_sdl/LocalMP.cpp +++ b/src/frontend/qt_sdl/LocalMP.cpp @@ -73,14 +73,82 @@ struct MPQueueHeader u32 PacketWriteOffset; }; +struct MPPacketHeader +{ + u32 Magic; + u32 Length; + u32 SenderID; +}; + QSharedMemory* MPQueue; -QSystemSemaphore* MPQueueSem[16]; +//QSystemSemaphore* MPQueueSem[16]; +int InstanceID; u32 SyncReadOffset; u32 PacketReadOffset; +const u32 kSyncStart = 0x0010; +const u32 kSyncEnd = 0x0100; +const u32 kPacketStart = 0x0100; +const u32 kPacketEnd = 0x10000; + #define NIFI_VER 2 +// we need to come up with our own abstraction layer for named semaphores +// because QSystemSemaphore doesn't support waiting with a timeout +// and, as such, is unsuitable to our needs + +//#ifdef _WIN32 +#if 1 + +HANDLE SemPool[32]; + +void SemPoolInit() +{ + for (int i = 0; i < 32; i++) + SemPool[i] = INVALID_HANDLE_VALUE; +} + +bool SemInit(int num) +{ + if (SemPool[num] != INVALID_HANDLE_VALUE) + return true; + + char semname[64]; + sprintf(semname, "Local\\melonNIFI_Sem%d", num); + + HANDLE sem = CreateSemaphore(nullptr, 0, 64, semname); + SemPool[num] = sem; + + return sem != INVALID_HANDLE_VALUE; +} + +void SemDeinit(int num) +{ + if (SemPool[num] != INVALID_HANDLE_VALUE) + { + CloseHandle(SemPool[num]); + SemPool[num] = INVALID_HANDLE_VALUE; + } +} + +bool SemPost(int num) +{ + SemInit(num); + return ReleaseSemaphore(SemPool[num], 1, nullptr) != 0; +} + +bool SemWait(int num, int timeout) +{ + return WaitForSingleObject(SemPool[num], timeout) == WAIT_OBJECT_0; +} + +#else + +// TODO: code semaphore shit for other platforms! + +#endif // _WIN32 + void _logpacket(bool tx, u8* data, int len) {return; @@ -112,7 +180,7 @@ void _logpacket(bool tx, u8* data, int len) bool Init() { - int opt_true = 1; + /*int opt_true = 1; int res0, res1; #ifdef __WIN32__ @@ -201,9 +269,14 @@ bool Init() MPUniqueID ^= *(u32*)&mac[2]; printf("local MP unique ID: %08X\n", MPUniqueID); - return true; + return true;*/ - /*MPQueue = new QSharedMemory("melonNIFI"); + /*u8* mac = SPI_Firmware::GetWifiMAC(); + MPUniqueID = *(u32*)&mac[0]; + MPUniqueID ^= *(u32*)&mac[2]; + printf("local MP unique ID: %08X\n", MPUniqueID);*/ + + MPQueue = new QSharedMemory("melonNIFI_FIFO"); if (!MPQueue->attach()) { @@ -217,15 +290,42 @@ bool Init() MPQueue->lock(); memset(MPQueue->data(), 0, MPQueue->size()); MPQueueHeader* header = (MPQueueHeader*)MPQueue->data(); - header->NumInstances = 1; - header->InstanceBitmask = 0x0001; - header->SyncWriteOffset = 0x0010; - header->PacketWriteOffset = 0x0100; + header->SyncWriteOffset = kSyncStart; + header->PacketWriteOffset = kPacketStart; MPQueue->unlock(); } - printf("sharedmem good\n");*/ + MPQueue->lock(); + MPQueueHeader* header = (MPQueueHeader*)MPQueue->data(); + u16 mask = header->InstanceBitmask; + for (int i = 0; i < 16; i++) + { + if (!(mask & (1<InstanceBitmask |= (1<NumInstances++; + + SyncReadOffset = header->SyncWriteOffset; + PacketReadOffset = header->PacketWriteOffset; + + MPQueue->unlock(); + + /*for (int i = 0; i < 16; i++) + { + QString key = QString("melonSEMA%1").arg(i); + MPQueueSem[i] = new QSystemSemaphore(key, 0, (i==InstanceID) ? QSystemSemaphore::Create : QSystemSemaphore::Open) + }*/ + + SemPoolInit(); + SemInit(InstanceID); + SemInit(16+InstanceID); + + printf("MP comm init OK, instance ID %d\n", InstanceID); return true; } @@ -242,86 +342,115 @@ void DeInit() #endif // __WIN32__ } -int SendPacket(u8* data, int len) +void PacketFIFORead(void* buf, int len) { - if (MPSocket[0] < 0) - return 0; + u8* data = (u8*)MPQueue->data(); - if (len > 2048-12) + u32 offset = PacketReadOffset; + if ((offset + len) >= kPacketEnd) { - printf("MP_SendPacket: error: packet too long (%d)\n", len); - return 0; + u32 part1 = kPacketEnd - offset; + memcpy(buf, &data[offset], part1); + memcpy(&((u8*)buf)[part1], &data[kPacketStart], len - part1); + offset = kPacketStart + len - part1; + } + else + { + memcpy(buf, &data[offset], len); + offset += len; } - *(u32*)&PacketBuffer[0] = htonl(0x4946494E); // NIFI - PacketBuffer[4] = NIFI_VER; - PacketBuffer[5] = 0; - *(u16*)&PacketBuffer[6] = htons(len); - *(u32*)&PacketBuffer[8] = MPUniqueID; - memcpy(&PacketBuffer[12], data, len); - - _logpacket(true, data, len); - - int slen = sendto(MPSocket[0], (const char*)PacketBuffer, len+12, 0, &MPSendAddr[0], sizeof(sockaddr_t)); - if (slen < 12) return 0; - return slen - 12; + PacketReadOffset = offset; } -int RecvPacket(u8* data, bool block) +void PacketFIFOWrite(void* buf, int len) { - if (MPSocket[0] < 0) - return 0; + u8* data = (u8*)MPQueue->data(); + MPQueueHeader* header = (MPQueueHeader*)&data[0]; - fd_set fd; - struct timeval tv; + u32 offset = header->PacketWriteOffset; + if ((offset + len) >= kPacketEnd) + { + u32 part1 = kPacketEnd - offset; + memcpy(&data[offset], buf, part1); + memcpy(&data[kPacketStart], &((u8*)buf)[part1], len - part1); + offset = kPacketStart + len - part1; + } + else + { + memcpy(&data[offset], buf, len); + offset += len; + } + + header->PacketWriteOffset = offset; +} + +int SendPacket(u8* packet, int len) +{ + MPQueue->lock(); + u8* data = (u8*)MPQueue->data(); + MPQueueHeader* header = (MPQueueHeader*)&data[0]; + + u16 mask = header->InstanceBitmask; + u32 offset = header->PacketWriteOffset; + + // TODO: check if the FIFO is full! + + MPPacketHeader pktheader; + pktheader.Magic = 0x4946494E; + pktheader.Length = len; + pktheader.SenderID = InstanceID; + + PacketFIFOWrite(&pktheader, sizeof(pktheader)); + PacketFIFOWrite(packet, len); + + for (int i = 0; i < 16; i++) + { + if (mask & (1<unlock(); + return len; +} + +int RecvPacket(u8* packet, bool block) +{ + MPQueue->lock(); + u8* data = (u8*)MPQueue->data(); + MPQueueHeader* header = (MPQueueHeader*)&data[0]; for (;;) { - FD_ZERO(&fd); - FD_SET(MPSocket[0], &fd); - tv.tv_sec = 0; - //tv.tv_usec = block ? 5000 : 0; - tv.tv_usec = block ? 500*1000 : 0; - - if (!select(MPSocket[0]+1, &fd, 0, 0, &tv)) + if (!SemWait(InstanceID, block ? 500 : 0)) { + MPQueue->unlock(); return 0; } - sockaddr_t fromAddr; - socklen_t fromLen = sizeof(sockaddr_t); - int rlen = recvfrom(MPSocket[0], (char*)PacketBuffer, 2048, 0, &fromAddr, &fromLen); - if (rlen < 12+24) - { - continue; - } - rlen -= 12; + MPPacketHeader pktheader; + PacketFIFORead(&pktheader, sizeof(pktheader)); - if (ntohl(*(u32*)&PacketBuffer[0]) != 0x4946494E) + if (pktheader.Magic != 0x4946494E) { + printf("MP: !!!! PACKET FIFO IS CRAPOED\n"); + MPQueue->unlock(); + return 0; + } + + if (pktheader.SenderID == InstanceID) + { + // skip this packet + PacketReadOffset += pktheader.Length; + if (PacketReadOffset >= kPacketEnd) + PacketReadOffset += kPacketStart - kPacketEnd; + continue; } - if (PacketBuffer[4] != NIFI_VER || PacketBuffer[5] != 0) - { - continue; - } - - if (ntohs(*(u16*)&PacketBuffer[6]) != rlen) - { - continue; - } - - if (*(u32*)&PacketBuffer[8] == MPUniqueID) - { - continue; - } - - zanf = *(u16*)&PacketBuffer[12+6]; - _logpacket(false, &PacketBuffer[12], rlen); - - memcpy(data, &PacketBuffer[12], rlen); - return rlen; + PacketFIFORead(packet, pktheader.Length); + MPQueue->unlock(); + return pktheader.Length; } }