diff --git a/include/mgba/internal/gba/sio/lockstep.h b/include/mgba/internal/gba/sio/lockstep.h index 13c181039..313285a44 100644 --- a/include/mgba/internal/gba/sio/lockstep.h +++ b/include/mgba/internal/gba/sio/lockstep.h @@ -1,4 +1,4 @@ -/* Copyright (c) 2013-2015 Jeffrey Pfau +/* Copyright (c) 2013-2024 Jeffrey Pfau * * This Source Code Form is subject to the terms of the Mozilla Public * License, v. 2.0. If a copy of the MPL was not distributed with this @@ -13,6 +13,9 @@ CXX_GUARD_START #include #include #include +#include +#include +#include struct GBASIOLockstep { struct mLockstep d; @@ -48,6 +51,78 @@ void GBASIOLockstepNodeCreate(struct GBASIOLockstepNode*); bool GBASIOLockstepAttachNode(struct GBASIOLockstep*, struct GBASIOLockstepNode*); void GBASIOLockstepDetachNode(struct GBASIOLockstep*, struct GBASIOLockstepNode*); +#define MAX_LOCKSTEP_EVENTS 8 + +enum GBASIOLockstepEventType { + SIO_EV_ATTACH, + SIO_EV_DETACH, + SIO_EV_HARD_SYNC, + SIO_EV_MODE_SET, + SIO_EV_TRANSFER_START, +}; + +struct GBASIOLockstepCoordinator { + struct Table players; + Mutex mutex; + + unsigned nextId; + + unsigned attachedPlayers[MAX_GBAS]; + int nAttached; + uint32_t waiting; + + bool transferActive; + enum GBASIOMode transferMode; + + int32_t cycle; + + uint16_t multiData[4]; + uint32_t normalData[4]; +}; + +struct GBASIOLockstepEvent { + enum GBASIOLockstepEventType type; + int32_t timestamp; + struct GBASIOLockstepEvent* next; + int playerId; + union { + enum GBASIOMode mode; + int32_t finishCycle; + }; +}; + +struct GBASIOLockstepPlayer { + struct GBASIOLockstepDriver* driver; + int playerId; + enum GBASIOMode mode; + enum GBASIOMode otherModes[MAX_GBAS]; + bool asleep; + int32_t cycleOffset; + struct GBASIOLockstepEvent* queue; + bool dataReceived; + + struct GBASIOLockstepEvent buffer[MAX_LOCKSTEP_EVENTS]; + struct GBASIOLockstepEvent* freeList; +}; + +struct GBASIOLockstepDriver { + struct GBASIODriver d; + struct GBASIOLockstepCoordinator* coordinator; + struct mTimingEvent event; + unsigned lockstepId; + + struct mLockstepUser* user; +}; + +void GBASIOLockstepCoordinatorInit(struct GBASIOLockstepCoordinator*); +void GBASIOLockstepCoordinatorDeinit(struct GBASIOLockstepCoordinator*); + +void GBASIOLockstepCoordinatorAttach(struct GBASIOLockstepCoordinator*, struct GBASIOLockstepDriver*); +void GBASIOLockstepCoordinatorDetach(struct GBASIOLockstepCoordinator*, struct GBASIOLockstepDriver*); +size_t GBASIOLockstepCoordinatorAttached(struct GBASIOLockstepCoordinator*); + +void GBASIOLockstepDriverCreate(struct GBASIOLockstepDriver*, struct mLockstepUser*); + CXX_GUARD_END #endif diff --git a/src/gba/sio/lockstep.c b/src/gba/sio/lockstep.c index 2008638bd..18603e054 100644 --- a/src/gba/sio/lockstep.c +++ b/src/gba/sio/lockstep.c @@ -1,4 +1,4 @@ -/* Copyright (c) 2013-2015 Jeffrey Pfau +/* Copyright (c) 2013-2024 Jeffrey Pfau * * This Source Code Form is subject to the terms of the Mozilla Public * License, v. 2.0. If a copy of the MPL was not distributed with this @@ -10,6 +10,7 @@ #define LOCKSTEP_INCREMENT 2000 #define LOCKSTEP_TRANSFER 512 +#define QUEUE_SIZE 16 static bool GBASIOLockstepNodeInit(struct GBASIODriver* driver); static void GBASIOLockstepNodeDeinit(struct GBASIODriver* driver); @@ -568,3 +569,769 @@ static uint16_t GBASIOLockstepNodeNormalWriteSIOCNT(struct GBASIODriver* driver, return value; } + +#define TARGET(P) (1 << (P)) +#define TARGET_ALL 0xF +#define TARGET_PRIMARY 0x1 +#define TARGET_SECONDARY ((TARGET_ALL) & ~(TARGET_PRIMARY)) + +static bool GBASIOLockstepDriverInit(struct GBASIODriver* driver); +static void GBASIOLockstepDriverDeinit(struct GBASIODriver* driver); +static void GBASIOLockstepDriverReset(struct GBASIODriver* driver); +static bool GBASIOLockstepDriverLoad(struct GBASIODriver* driver); +static bool GBASIOLockstepDriverUnload(struct GBASIODriver* driver); +static void GBASIOLockstepDriverSetMode(struct GBASIODriver* driver, enum GBASIOMode mode); +static bool GBASIOLockstepDriverHandlesMode(struct GBASIODriver* driver, enum GBASIOMode mode); +static int GBASIOLockstepDriverConnectedDevices(struct GBASIODriver* driver); +static int GBASIOLockstepDriverDeviceId(struct GBASIODriver* driver); +static uint16_t GBASIOLockstepDriverWriteSIOCNT(struct GBASIODriver* driver, uint16_t value); +static uint16_t GBASIOLockstepDriverWriteRCNT(struct GBASIODriver* driver, uint16_t value); +static bool GBASIOLockstepDriverStart(struct GBASIODriver* driver); +static void GBASIOLockstepDriverFinishMultiplayer(struct GBASIODriver* driver, uint16_t data[4]); +static uint8_t GBASIOLockstepDriverFinishNormal8(struct GBASIODriver* driver); +static uint32_t GBASIOLockstepDriverFinishNormal32(struct GBASIODriver* driver); + +static void GBASIOLockstepCoordinatorWaitOnPlayers(struct GBASIOLockstepCoordinator*, struct GBASIOLockstepPlayer*); +static void GBASIOLockstepCoordinatorAckPlayer(struct GBASIOLockstepCoordinator*, struct GBASIOLockstepPlayer*); +static void GBASIOLockstepCoordinatorWakePlayers(struct GBASIOLockstepCoordinator*); + +static int32_t GBASIOLockstepTime(struct GBASIOLockstepPlayer*); +static void GBASIOLockstepPlayerWake(struct GBASIOLockstepPlayer*); +static void GBASIOLockstepPlayerSleep(struct GBASIOLockstepPlayer*); + +static void _advanceCycle(struct GBASIOLockstepCoordinator*, struct GBASIOLockstepPlayer*); +static void _removePlayer(struct GBASIOLockstepCoordinator*, struct GBASIOLockstepPlayer*); +static void _reconfigPlayers(struct GBASIOLockstepCoordinator*); +static int32_t _untilNextSync(struct GBASIOLockstepCoordinator*, struct GBASIOLockstepPlayer*); +static void _enqueueEvent(struct GBASIOLockstepCoordinator*, const struct GBASIOLockstepEvent*, uint32_t target); +static void _setData(struct GBASIOLockstepCoordinator*, uint32_t id, struct GBASIO* sio); +static void _setReady(struct GBASIOLockstepCoordinator*, struct GBASIOLockstepPlayer* activePlayer, int playerId, enum GBASIOMode mode); +static void _hardSync(struct GBASIOLockstepCoordinator*, struct GBASIOLockstepPlayer*); + +static void _lockstepEvent(struct mTiming*, void* context, uint32_t cyclesLate); + +void GBASIOLockstepDriverCreate(struct GBASIOLockstepDriver* driver, struct mLockstepUser* user) { + memset(driver, 0, sizeof(*driver)); + driver->d.init = GBASIOLockstepDriverInit; + driver->d.deinit = GBASIOLockstepDriverDeinit; + driver->d.reset = GBASIOLockstepDriverReset; + driver->d.load = GBASIOLockstepDriverLoad; + driver->d.unload = GBASIOLockstepDriverUnload; + driver->d.setMode = GBASIOLockstepDriverSetMode; + driver->d.handlesMode = GBASIOLockstepDriverHandlesMode; + driver->d.deviceId = GBASIOLockstepDriverDeviceId; + driver->d.connectedDevices = GBASIOLockstepDriverConnectedDevices; + driver->d.writeSIOCNT = GBASIOLockstepDriverWriteSIOCNT; + driver->d.writeRCNT = GBASIOLockstepDriverWriteRCNT; + driver->d.start = GBASIOLockstepDriverStart; + driver->d.finishMultiplayer = GBASIOLockstepDriverFinishMultiplayer; + driver->d.finishNormal8 = GBASIOLockstepDriverFinishNormal8; + driver->d.finishNormal32 = GBASIOLockstepDriverFinishNormal32; + driver->event.context = driver; + driver->event.callback = _lockstepEvent; + driver->event.name = "GBA SIO Lockstep"; + driver->event.priority = 0x80; + driver->user = user; +} + +static bool GBASIOLockstepDriverInit(struct GBASIODriver* driver) { + GBASIOLockstepDriverReset(driver); + return true; +} + +static void GBASIOLockstepDriverDeinit(struct GBASIODriver* driver) { + struct GBASIOLockstepDriver* lockstep = (struct GBASIOLockstepDriver*) driver; + struct GBASIOLockstepCoordinator* coordinator = lockstep->coordinator; + MutexLock(&coordinator->mutex); + struct GBASIOLockstepPlayer* player = TableLookup(&coordinator->players, lockstep->lockstepId); + if (player) { + _removePlayer(coordinator, player); + } + MutexUnlock(&coordinator->mutex); + mTimingDeschedule(&lockstep->d.p->p->timing, &lockstep->event); + lockstep->lockstepId = 0; +} + +static void GBASIOLockstepDriverReset(struct GBASIODriver* driver) { + struct GBASIOLockstepDriver* lockstep = (struct GBASIOLockstepDriver*) driver; + struct GBASIOLockstepCoordinator* coordinator = lockstep->coordinator; + if (!lockstep->lockstepId) { + struct GBASIOLockstepPlayer* player = calloc(1, sizeof(*player)); + unsigned id; + player->driver = lockstep; + player->mode = driver->p->mode; + player->playerId = -1; + + int i; + for (i = 0; i < MAX_LOCKSTEP_EVENTS - 1; ++i) { + player->buffer[i].next = &player->buffer[i + 1]; + } + player->freeList = &player->buffer[0]; + + MutexLock(&coordinator->mutex); + while (true) { + if (coordinator->nextId == UINT_MAX) { + coordinator->nextId = 0; + } + ++coordinator->nextId; + id = coordinator->nextId; + if (!TableLookup(&coordinator->players, id)) { + TableInsert(&coordinator->players, id, player); + lockstep->lockstepId = id; + break; + } + } + _reconfigPlayers(coordinator); + if (player->playerId != 0) { + player->cycleOffset = mTimingCurrentTime(&driver->p->p->timing) - coordinator->cycle + LOCKSTEP_INCREMENT; + struct GBASIOLockstepEvent event = { + .type = SIO_EV_ATTACH, + .playerId = player->playerId, + .timestamp = GBASIOLockstepTime(player), + }; + _enqueueEvent(coordinator, &event, TARGET_ALL & ~TARGET(player->playerId)); + } + MutexUnlock(&coordinator->mutex); + } + + if (mTimingIsScheduled(&lockstep->d.p->p->timing, &lockstep->event)) { + return; + } + + int32_t nextEvent; + MutexLock(&coordinator->mutex); + struct GBASIOLockstepPlayer* player = TableLookup(&coordinator->players, lockstep->lockstepId); + _setReady(coordinator, player, player->playerId, player->mode); + if (TableSize(&coordinator->players) == 1) { + coordinator->cycle = mTimingCurrentTime(&lockstep->d.p->p->timing); + nextEvent = LOCKSTEP_INCREMENT; + } else { + _setReady(coordinator, player, 0, coordinator->transferMode); + nextEvent = _untilNextSync(lockstep->coordinator, player); + } + MutexUnlock(&coordinator->mutex); + mTimingSchedule(&lockstep->d.p->p->timing, &lockstep->event, nextEvent); +} + +static bool GBASIOLockstepDriverLoad(struct GBASIODriver* driver) { + struct GBASIOLockstepDriver* lockstep = (struct GBASIOLockstepDriver*) driver; + if (lockstep->lockstepId) { + struct GBASIOLockstepCoordinator* coordinator = lockstep->coordinator; + MutexLock(&coordinator->mutex); + struct GBASIOLockstepPlayer* player = TableLookup(&coordinator->players, lockstep->lockstepId); + _setReady(coordinator, player, 0, coordinator->transferMode); + MutexUnlock(&coordinator->mutex); + GBASIOLockstepDriverSetMode(driver, driver->p->mode); + } + return true; +} + +static bool GBASIOLockstepDriverUnload(struct GBASIODriver* driver) { + struct GBASIOLockstepDriver* lockstep = (struct GBASIOLockstepDriver*) driver; + if (lockstep->lockstepId) { + GBASIOLockstepDriverSetMode(driver, -1); + } + return true; +} + +static void GBASIOLockstepDriverSetMode(struct GBASIODriver* driver, enum GBASIOMode mode) { + struct GBASIOLockstepDriver* lockstep = (struct GBASIOLockstepDriver*) driver; + struct GBASIOLockstepCoordinator* coordinator = lockstep->coordinator; + MutexLock(&coordinator->mutex); + struct GBASIOLockstepPlayer* player = TableLookup(&coordinator->players, lockstep->lockstepId); + if (mode != player->mode) { + player->mode = mode; + struct GBASIOLockstepEvent event = { + .type = SIO_EV_MODE_SET, + .playerId = player->playerId, + .timestamp = GBASIOLockstepTime(player), + .mode = mode, + }; + if (player->playerId == 0) { + mASSERT(!coordinator->transferActive); // TODO + coordinator->transferMode = mode; + GBASIOLockstepCoordinatorWaitOnPlayers(coordinator, player); + } + _setReady(coordinator, player, player->playerId, mode); + _enqueueEvent(coordinator, &event, TARGET_ALL & ~TARGET(player->playerId)); + } + MutexUnlock(&coordinator->mutex); +} + +static bool GBASIOLockstepDriverHandlesMode(struct GBASIODriver* driver, enum GBASIOMode mode) { + UNUSED(driver); + UNUSED(mode); + return true; +} + +static int GBASIOLockstepDriverConnectedDevices(struct GBASIODriver* driver) { + struct GBASIOLockstepDriver* lockstep = (struct GBASIOLockstepDriver*) driver; + struct GBASIOLockstepCoordinator* coordinator = lockstep->coordinator; + if (!lockstep->lockstepId) { + return 0; + } + MutexLock(&coordinator->mutex); + int attached = coordinator->nAttached - 1; + MutexUnlock(&coordinator->mutex); + return attached; +} + +static int GBASIOLockstepDriverDeviceId(struct GBASIODriver* driver) { + struct GBASIOLockstepDriver* lockstep = (struct GBASIOLockstepDriver*) driver; + struct GBASIOLockstepCoordinator* coordinator = lockstep->coordinator; + int playerId = 0; + MutexLock(&coordinator->mutex); + struct GBASIOLockstepPlayer* player = TableLookup(&coordinator->players, lockstep->lockstepId); + if (player && player->playerId >= 0) { + playerId = player->playerId; + } + MutexUnlock(&coordinator->mutex); + return playerId; +} + +static uint16_t GBASIOLockstepDriverWriteSIOCNT(struct GBASIODriver* driver, uint16_t value) { + UNUSED(driver); + mLOG(GBA_SIO, DEBUG, "Lockstep: SIOCNT <- %04X", value); + return value; +} + +static uint16_t GBASIOLockstepDriverWriteRCNT(struct GBASIODriver* driver, uint16_t value) { + UNUSED(driver); + mLOG(GBA_SIO, DEBUG, "Lockstep: RCNT <- %04X", value); + return value; +} + +static bool GBASIOLockstepDriverStart(struct GBASIODriver* driver) { + struct GBASIOLockstepDriver* lockstep = (struct GBASIOLockstepDriver*) driver; + struct GBASIOLockstepCoordinator* coordinator = lockstep->coordinator; + bool ret = false; + MutexLock(&coordinator->mutex); + if (coordinator->transferActive) { + mLOG(GBA_SIO, ERROR, "Transfer restarted unexpectedly"); + goto out; + } + struct GBASIOLockstepPlayer* player = TableLookup(&coordinator->players, lockstep->lockstepId); + if (player->playerId != 0) { + mLOG(GBA_SIO, DEBUG, "Secondary player attempted to start transfer"); + goto out; + } + mLOG(GBA_SIO, DEBUG, "Transfer starting at %08X", coordinator->cycle); + memset(coordinator->multiData, 0xFF, sizeof(coordinator->multiData)); + _setData(coordinator, 0, player->driver->d.p); + + int32_t timestamp = GBASIOLockstepTime(player); + struct GBASIOLockstepEvent event = { + .type = SIO_EV_TRANSFER_START, + .timestamp = timestamp, + .finishCycle = timestamp + GBASIOTransferCycles(player->mode, player->driver->d.p->siocnt, coordinator->nAttached - 1), + }; + _enqueueEvent(coordinator, &event, TARGET_SECONDARY); + GBASIOLockstepCoordinatorWaitOnPlayers(coordinator, player); + coordinator->transferActive = true; + ret = true; +out: + MutexUnlock(&coordinator->mutex); + return ret; +} + +static void GBASIOLockstepDriverFinishMultiplayer(struct GBASIODriver* driver, uint16_t data[4]) { + struct GBASIOLockstepDriver* lockstep = (struct GBASIOLockstepDriver*) driver; + struct GBASIOLockstepCoordinator* coordinator = lockstep->coordinator; + MutexLock(&coordinator->mutex); + if (coordinator->transferMode == GBA_SIO_MULTI) { + struct GBASIOLockstepPlayer* player = TableLookup(&coordinator->players, lockstep->lockstepId); + if (!player->dataReceived) { + mLOG(GBA_SIO, WARN, "MULTI did not receive data. Are we running behind?"); + memset(data, 0xFF, sizeof(uint16_t) * 4); + } else { + mLOG(GBA_SIO, INFO, "MULTI transfer finished: %04X %04X %04X %04X", + coordinator->multiData[0], + coordinator->multiData[1], + coordinator->multiData[2], + coordinator->multiData[3]); + memcpy(data, coordinator->multiData, sizeof(uint16_t) * 4); + } + player->dataReceived = false; + if (player->playerId == 0) { + _hardSync(coordinator, player); + } + } + MutexUnlock(&coordinator->mutex); +} + +static uint8_t GBASIOLockstepDriverFinishNormal8(struct GBASIODriver* driver) { + struct GBASIOLockstepDriver* lockstep = (struct GBASIOLockstepDriver*) driver; + struct GBASIOLockstepCoordinator* coordinator = lockstep->coordinator; + uint8_t data = 0xFF; + MutexLock(&coordinator->mutex); + if (coordinator->transferMode == GBA_SIO_NORMAL_8) { + struct GBASIOLockstepPlayer* player = TableLookup(&coordinator->players, lockstep->lockstepId); + if (player->playerId > 0) { + if (!player->dataReceived) { + mLOG(GBA_SIO, WARN, "NORMAL did not receive data. Are we running behind?"); + } else { + data = coordinator->normalData[player->playerId - 1]; + mLOG(GBA_SIO, INFO, "NORMAL8 transfer finished: %02X", data); + } + } + player->dataReceived = false; + if (player->playerId == 0) { + _hardSync(coordinator, player); + } + } + MutexUnlock(&coordinator->mutex); + return data; +} + +static uint32_t GBASIOLockstepDriverFinishNormal32(struct GBASIODriver* driver) { + struct GBASIOLockstepDriver* lockstep = (struct GBASIOLockstepDriver*) driver; + struct GBASIOLockstepCoordinator* coordinator = lockstep->coordinator; + uint32_t data = 0xFFFFFFFF; + MutexLock(&coordinator->mutex); + if (coordinator->transferMode == GBA_SIO_NORMAL_32) { + struct GBASIOLockstepPlayer* player = TableLookup(&coordinator->players, lockstep->lockstepId); + if (player->playerId > 0) { + if (!player->dataReceived) { + mLOG(GBA_SIO, WARN, "Did not receive data. Are we running behind?"); + } else { + data = coordinator->normalData[player->playerId - 1]; + mLOG(GBA_SIO, INFO, "NORMAL32 transfer finished: %08X", data); + } + } + player->dataReceived = false; + if (player->playerId == 0) { + _hardSync(coordinator, player); + } + } + MutexUnlock(&coordinator->mutex); + return data; +} + +void GBASIOLockstepCoordinatorInit(struct GBASIOLockstepCoordinator* coordinator) { + memset(coordinator, 0, sizeof(*coordinator)); + MutexInit(&coordinator->mutex); + TableInit(&coordinator->players, 8, free); +} + +void GBASIOLockstepCoordinatorDeinit(struct GBASIOLockstepCoordinator* coordinator) { + MutexDeinit(&coordinator->mutex); + TableDeinit(&coordinator->players); +} + +void GBASIOLockstepCoordinatorAttach(struct GBASIOLockstepCoordinator* coordinator, struct GBASIOLockstepDriver* driver) { + if (driver->coordinator && driver->coordinator != coordinator) { + // TODO + abort(); + } + driver->coordinator = coordinator; +} + +void GBASIOLockstepCoordinatorDetach(struct GBASIOLockstepCoordinator* coordinator, struct GBASIOLockstepDriver* driver) { + if (driver->coordinator != coordinator) { + // TODO + abort(); + return; + } + MutexLock(&coordinator->mutex); + struct GBASIOLockstepPlayer* player = TableLookup(&coordinator->players, driver->lockstepId); + if (player) { + _removePlayer(coordinator, player); + } + MutexUnlock(&coordinator->mutex); + driver->coordinator = NULL; +} + +int32_t _untilNextSync(struct GBASIOLockstepCoordinator* coordinator, struct GBASIOLockstepPlayer* player) { + int32_t cycle = coordinator->cycle - GBASIOLockstepTime(player); + if (player->playerId == 0) { + cycle += LOCKSTEP_INCREMENT; + } + return cycle; +} + +void _advanceCycle(struct GBASIOLockstepCoordinator* coordinator, struct GBASIOLockstepPlayer* player) { + int32_t newCycle = GBASIOLockstepTime(player); + mASSERT(newCycle - coordinator->cycle >= 0); + //mLOG(GBA_SIO, DEBUG, "Advancing from cycle %08X to %08X (%i cycles)", coordinator->cycle, newCycle, newCycle - coordinator->cycle); + coordinator->cycle = newCycle; +} + +void _removePlayer(struct GBASIOLockstepCoordinator* coordinator, struct GBASIOLockstepPlayer* player) { + struct GBASIOLockstepEvent event = { + .type = SIO_EV_DETACH, + .playerId = player->playerId, + .timestamp = GBASIOLockstepTime(player), + }; + _enqueueEvent(coordinator, &event, TARGET_ALL & ~TARGET(player->playerId)); + GBASIOLockstepCoordinatorWakePlayers(coordinator); + if (player->playerId != 0) { + GBASIOLockstepCoordinatorAckPlayer(coordinator, player); + } + TableRemove(&coordinator->players, player->driver->lockstepId); + _reconfigPlayers(coordinator); +} + +void _reconfigPlayers(struct GBASIOLockstepCoordinator* coordinator) { + size_t players = TableSize(&coordinator->players); + memset(coordinator->attachedPlayers, 0, sizeof(coordinator->attachedPlayers)); + if (players == 0) { + mLOG(GBA_SIO, WARN, "Reconfiguring player IDs with no players attached somehow?"); + } else if (players == 1) { + struct TableIterator iter; + mASSERT(TableIteratorStart(&coordinator->players, &iter)); + unsigned p0 = TableIteratorGetKey(&coordinator->players, &iter); + coordinator->attachedPlayers[0] = p0; + + struct GBASIOLockstepPlayer* player = TableIteratorGetValue(&coordinator->players, &iter); + coordinator->cycle = mTimingCurrentTime(&player->driver->d.p->p->timing); + + if (player->playerId != 0) { + player->playerId = 0; + if (player->driver->user->playerIdChanged) { + player->driver->user->playerIdChanged(player->driver->user, player->playerId); + } + } + + if (!coordinator->transferActive) { + coordinator->transferMode = player->mode; + } + } else { + struct UIntList playerPreferences[MAX_GBAS]; + + int i; + for (i = 0; i < MAX_GBAS; ++i) { + UIntListInit(&playerPreferences[i], 4); + } + + // Collect the first four players' requested player IDs so we can sort through them later + int seen = 0; + struct TableIterator iter; + mASSERT(TableIteratorStart(&coordinator->players, &iter)); + do { + unsigned pid = TableIteratorGetKey(&coordinator->players, &iter); + struct GBASIOLockstepPlayer* player = TableIteratorGetValue(&coordinator->players, &iter); + int requested = MAX_GBAS - 1; + if (player->driver->user->requestedId) { + requested = player->driver->user->requestedId(player->driver->user); + } + if (requested < 0) { + continue; + } + if (requested >= MAX_GBAS) { + requested = MAX_GBAS - 1; + } + + *UIntListAppend(&playerPreferences[requested]) = pid; + ++seen; + } while (TableIteratorNext(&coordinator->players, &iter) && seen < MAX_GBAS); + + // Now sort each requested player ID to figure out who gets which ID + seen = 0; + for (i = 0; i < MAX_GBAS; ++i) { + int j; + for (j = 0; j <= i; ++j) { + while (UIntListSize(&playerPreferences[j]) && seen < MAX_GBAS) { + unsigned pid = *UIntListGetPointer(&playerPreferences[j], 0); + UIntListShift(&playerPreferences[j], 0, 1); + struct GBASIOLockstepPlayer* player = TableLookup(&coordinator->players, pid); + if (!player) { + mLOG(GBA_SIO, ERROR, "Player list appears to have changed unexpectedly. PID %u missing.", pid); + continue; + } + coordinator->attachedPlayers[seen] = pid; + if (player->playerId != seen) { + player->playerId = seen; + if (player->driver->user->playerIdChanged) { + player->driver->user->playerIdChanged(player->driver->user, player->playerId); + } + } + ++seen; + } + } + } + + for (i = 0; i < MAX_GBAS; ++i) { + UIntListDeinit(&playerPreferences[i]); + } + } + + int nAttached = 0; + size_t i; + for (i = 0; i < MAX_GBAS; ++i) { + unsigned pid = coordinator->attachedPlayers[i]; + if (!pid) { + continue; + } + struct GBASIOLockstepPlayer* player = TableLookup(&coordinator->players, pid); + if (!player) { + coordinator->attachedPlayers[i] = 0; + } else { + ++nAttached; + } + } + coordinator->nAttached = nAttached; +} + +static void _setData(struct GBASIOLockstepCoordinator* coordinator, uint32_t id, struct GBASIO* sio) { + switch (coordinator->transferMode) { + case GBA_SIO_MULTI: + coordinator->multiData[id] = sio->p->memory.io[GBA_REG(SIOMLT_SEND)]; + break; + case GBA_SIO_NORMAL_8: + coordinator->normalData[id] = sio->p->memory.io[GBA_REG(SIODATA8)]; + break; + case GBA_SIO_NORMAL_32: + coordinator->normalData[id] = sio->p->memory.io[GBA_REG(SIODATA32_LO)]; + coordinator->normalData[id] |= sio->p->memory.io[GBA_REG(SIODATA32_HI)] << 16; + break; + case GBA_SIO_UART: + case GBA_SIO_GPIO: + case GBA_SIO_JOYBUS: + mLOG(GBA_SIO, ERROR, "Unsupported mode %i in lockstep", coordinator->transferMode); + // TODO: Should we handle this or just abort? + abort(); + break; + } +} + +void _setReady(struct GBASIOLockstepCoordinator* coordinator, struct GBASIOLockstepPlayer* activePlayer, int playerId, enum GBASIOMode mode) { + activePlayer->otherModes[playerId] = mode; + bool ready = true; + int i; + for (i = 0; ready && i < coordinator->nAttached; ++i) { + ready = activePlayer->otherModes[i] == activePlayer->mode; + } + if (activePlayer->mode == GBA_SIO_MULTI) { + struct GBASIO* sio = activePlayer->driver->d.p; + sio->siocnt = GBASIOMultiplayerSetReady(sio->siocnt, ready); + sio->rcnt = GBASIORegisterRCNTSetSd(sio->rcnt, ready); + } +} + +void _hardSync(struct GBASIOLockstepCoordinator* coordinator, struct GBASIOLockstepPlayer* player) { + mASSERT_DEBUG(player->playerId == 0); + struct GBASIOLockstepEvent event = { + .type = SIO_EV_HARD_SYNC, + .playerId = 0, + .timestamp = GBASIOLockstepTime(player), + }; + _enqueueEvent(coordinator, &event, TARGET_SECONDARY); + GBASIOLockstepCoordinatorWaitOnPlayers(coordinator, player); +} + +void _enqueueEvent(struct GBASIOLockstepCoordinator* coordinator, const struct GBASIOLockstepEvent* event, uint32_t target) { + mLOG(GBA_SIO, DEBUG, "Enqueuing event of type %X from %i for target %X at timestamp %X", + event->type, event->playerId, target, event->timestamp); + + int i; + for (i = 0; i < coordinator->nAttached; ++i) { + if (!(target & TARGET(i))) { + continue; + } + struct GBASIOLockstepPlayer* player = TableLookup(&coordinator->players, coordinator->attachedPlayers[i]); + mASSERT_LOG(GBA_SIO, player->freeList, "No free events"); + struct GBASIOLockstepEvent* newEvent = player->freeList; + player->freeList = newEvent->next; + + memcpy(newEvent, event, sizeof(*event)); + struct GBASIOLockstepEvent** previous = &player->queue; + struct GBASIOLockstepEvent* next = player->queue; + while (next) { + int32_t until = newEvent->timestamp - next->timestamp; + if (until < 0) { + break; + } + previous = &next->next; + next = next->next; + } + newEvent->next = next; + *previous = newEvent; + } +} + +void _lockstepEvent(struct mTiming* timing, void* context, uint32_t cyclesLate) { + struct GBASIOLockstepDriver* lockstep = context; + struct GBASIOLockstepCoordinator* coordinator = lockstep->coordinator; + MutexLock(&coordinator->mutex); + struct GBASIOLockstepPlayer* player = TableLookup(&coordinator->players, lockstep->lockstepId); + struct GBASIO* sio = player->driver->d.p; + mASSERT(player->playerId >= 0 && player->playerId < 4); + + bool wasDetach = false; + if (player->queue && player->queue->type == SIO_EV_DETACH) { + mLOG(GBA_SIO, DEBUG, "Player %i detached at timestamp %X, picking up the pieces", + player->queue->playerId, player->queue->timestamp); + wasDetach = true; + } + if (player->playerId == 0) { + // We are the clock owner; advance the shared clock + _advanceCycle(coordinator, player); + if (!coordinator->transferActive) { + GBASIOLockstepCoordinatorWakePlayers(coordinator); + } + } + + int32_t nextEvent = _untilNextSync(coordinator, player); + //mASSERT_DEBUG(nextEvent + cyclesLate > 0); + while (true) { + struct GBASIOLockstepEvent* event = player->queue; + if (!event) { + break; + } + if (event->timestamp > GBASIOLockstepTime(player)) { + break; + } + player->queue = event->next; + struct GBASIOLockstepEvent reply = { + .playerId = player->playerId, + .timestamp = GBASIOLockstepTime(player), + }; + mLOG(GBA_SIO, DEBUG, "Got event of type %X from %i at timestamp %X", + event->type, event->playerId, event->timestamp); + switch (event->type) { + case SIO_EV_ATTACH: + _setReady(coordinator, player, event->playerId, -1); + if (player->playerId == 0) { + struct GBASIO* sio = player->driver->d.p; + sio->siocnt = GBASIOMultiplayerClearSlave(sio->siocnt); + } + reply.mode = player->mode; + reply.type = SIO_EV_MODE_SET; + _enqueueEvent(coordinator, &reply, TARGET(event->playerId)); + break; + case SIO_EV_HARD_SYNC: + GBASIOLockstepCoordinatorAckPlayer(coordinator, player); + break; + case SIO_EV_TRANSFER_START: + _setData(coordinator, player->playerId, sio); + nextEvent = event->finishCycle - GBASIOLockstepTime(player) - cyclesLate; + player->driver->d.p->siocnt |= 0x80; + mTimingDeschedule(&sio->p->timing, &sio->completeEvent); + mTimingSchedule(&sio->p->timing, &sio->completeEvent, nextEvent); + GBASIOLockstepCoordinatorAckPlayer(coordinator, player); + break; + case SIO_EV_MODE_SET: + _setReady(coordinator, player, event->playerId, event->mode); + if (event->playerId == 0) { + GBASIOLockstepCoordinatorAckPlayer(coordinator, player); + } + break; + case SIO_EV_DETACH: + _setReady(coordinator, player, event->playerId, -1); + _setReady(coordinator, player, player->playerId, player->mode); + reply.mode = player->mode; + reply.type = SIO_EV_MODE_SET; + _enqueueEvent(coordinator, &reply, ~TARGET(event->playerId)); + if (player->mode == GBA_SIO_MULTI) { + sio->siocnt = GBASIOMultiplayerSetId(sio->siocnt, player->playerId); + sio->siocnt = GBASIOMultiplayerSetSlave(sio->siocnt, player->playerId || coordinator->nAttached < 2); + } + wasDetach = true; + break; + } + event->next = player->freeList; + player->freeList = event; + } + if (player->queue && player->queue->timestamp - GBASIOLockstepTime(player) < nextEvent) { + nextEvent = player->queue->timestamp - GBASIOLockstepTime(player); + } + + if (player->playerId != 0 && nextEvent <= LOCKSTEP_INCREMENT) { + if (!player->queue || wasDetach) { + GBASIOLockstepPlayerSleep(player); + // XXX: Is there a better way to gain sync lock at the beginning? + if (nextEvent < 4) { + nextEvent = 4; + } + } + } + MutexUnlock(&coordinator->mutex); + + mASSERT_DEBUG(nextEvent > 0); + mTimingSchedule(timing, &lockstep->event, nextEvent); +} + +int32_t GBASIOLockstepTime(struct GBASIOLockstepPlayer* player) { + return mTimingCurrentTime(&player->driver->d.p->p->timing) - player->cycleOffset; +} + +void GBASIOLockstepCoordinatorWaitOnPlayers(struct GBASIOLockstepCoordinator* coordinator, struct GBASIOLockstepPlayer* player) { + mASSERT(!coordinator->waiting); + mASSERT(!player->asleep); + mASSERT(player->playerId == 0); + if (coordinator->nAttached < 2) { + return; + } + + _advanceCycle(coordinator, player); + mLOG(GBA_SIO, DEBUG, "Primary waiting for players to ack"); + coordinator->waiting = ((1 << coordinator->nAttached) - 1) & ~TARGET(player->playerId); + GBASIOLockstepPlayerSleep(player); + GBASIOLockstepCoordinatorWakePlayers(coordinator); +} + +void GBASIOLockstepCoordinatorWakePlayers(struct GBASIOLockstepCoordinator* coordinator) { + //mLOG(GBA_SIO, DEBUG, "Waking all secondary players"); + int i; + for (i = 1; i < coordinator->nAttached; ++i) { + if (!coordinator->attachedPlayers[i]) { + continue; + } + struct GBASIOLockstepPlayer* player = TableLookup(&coordinator->players, coordinator->attachedPlayers[i]); + GBASIOLockstepPlayerWake(player); + } +} + +void GBASIOLockstepPlayerWake(struct GBASIOLockstepPlayer* player) { + if (!player->asleep) { + return; + } + player->asleep = false; + player->driver->user->wake(player->driver->user); +} + +void GBASIOLockstepCoordinatorAckPlayer(struct GBASIOLockstepCoordinator* coordinator, struct GBASIOLockstepPlayer* player) { + if (player->playerId == 0) { + return; + } + mLOG(GBA_SIO, DEBUG, "Player %i acking primary", player->playerId); + coordinator->waiting &= ~TARGET(player->playerId); + if (!coordinator->waiting) { + mLOG(GBA_SIO, DEBUG, "All players acked, waking primary"); + if (coordinator->transferActive) { + int i; + for (i = 0; i < coordinator->nAttached; ++i) { + if (!coordinator->attachedPlayers[i]) { + continue; + } + struct GBASIOLockstepPlayer* player = TableLookup(&coordinator->players, coordinator->attachedPlayers[i]); + player->dataReceived = true; + } + + coordinator->transferActive = false; + } + + struct GBASIOLockstepPlayer* runner = TableLookup(&coordinator->players, coordinator->attachedPlayers[0]); + GBASIOLockstepPlayerWake(runner); + } + GBASIOLockstepPlayerSleep(player); +} + +void GBASIOLockstepPlayerSleep(struct GBASIOLockstepPlayer* player) { + if (player->asleep) { + return; + } + //mLOG(GBA_SIO, DEBUG, "Player %i going to sleep with %i cycles until sync", player->playerId, _untilNextSync(coordinator, player)); + player->asleep = true; + player->driver->user->sleep(player->driver->user); + player->driver->d.p->p->cpu->nextEvent = 0; + player->driver->d.p->p->earlyExit = true; +} + +size_t GBASIOLockstepCoordinatorAttached(struct GBASIOLockstepCoordinator* coordinator) { + size_t count; + MutexLock(&coordinator->mutex); + count = TableSize(&coordinator->players); + MutexUnlock(&coordinator->mutex); + return count; +}