1/* Copyright (c) 2013-2015 Jeffrey Pfau
  2 *
  3 * This Source Code Form is subject to the terms of the Mozilla Public
  4 * License, v. 2.0. If a copy of the MPL was not distributed with this
  5 * file, You can obtain one at http://mozilla.org/MPL/2.0/. */
  6#include "lockstep.h"
  7
  8#include "gba/gba.h"
  9#include "gba/io.h"
 10#include "gba/video.h"
 11
 12#define LOCKSTEP_INCREMENT 3000
 13
 14static bool _nodeWait(struct GBASIOLockstepNode* node, uint32_t mask) {
 15	uint32_t oldMask = 0;
 16	if (ATOMIC_CMPXCHG(node->p->waiting[node->id], oldMask, mask | 0x10)) {
 17		node->p->wait(node->p, node->id);
 18	}
 19
 20	return true;
 21}
 22
 23static bool _nodeSignal(struct GBASIOLockstepNode* node, uint32_t mask) {
 24	bool eventTriggered = false;
 25	int i;
 26	for (i = 0; i < node->p->attached; ++i) {
 27		uint32_t waiting = ATOMIC_AND(node->p->waiting[i], ~mask);
 28		if (waiting == 0x10) {
 29			if (!ATOMIC_CMPXCHG(node->p->waiting[i], waiting, 0)) {
 30				return false;
 31			}
 32			node->p->signal(node->p, i);
 33			eventTriggered = true;
 34		}
 35	}
 36	if (!eventTriggered) {
 37		ATOMIC_STORE(node->p->waitMask, mask);
 38	} else {
 39		ATOMIC_STORE(node->p->waitMask, 0);
 40	}
 41	return eventTriggered;
 42}
 43
 44
 45static bool GBASIOLockstepNodeInit(struct GBASIODriver* driver);
 46static void GBASIOLockstepNodeDeinit(struct GBASIODriver* driver);
 47static bool GBASIOLockstepNodeLoad(struct GBASIODriver* driver);
 48static bool GBASIOLockstepNodeUnload(struct GBASIODriver* driver);
 49static uint16_t GBASIOLockstepNodeMultiWriteRegister(struct GBASIODriver* driver, uint32_t address, uint16_t value);
 50static uint16_t GBASIOLockstepNodeNormalWriteRegister(struct GBASIODriver* driver, uint32_t address, uint16_t value);
 51static int32_t GBASIOLockstepNodeProcessEvents(struct GBASIODriver* driver, int32_t cycles);
 52static int32_t GBASIOLockstepNodeProcessEvents(struct GBASIODriver* driver, int32_t cycles);
 53
 54void GBASIOLockstepInit(struct GBASIOLockstep* lockstep) {
 55	lockstep->players[0] = 0;
 56	lockstep->players[1] = 0;
 57	lockstep->players[2] = 0;
 58	lockstep->players[3] = 0;
 59	lockstep->multiRecv[0] = 0xFFFF;
 60	lockstep->multiRecv[1] = 0xFFFF;
 61	lockstep->multiRecv[2] = 0xFFFF;
 62	lockstep->multiRecv[3] = 0xFFFF;
 63	lockstep->attached = 0;
 64	lockstep->attachedMulti = 0;
 65	lockstep->transferActive = 0;
 66	memset(lockstep->waiting, 0, sizeof(lockstep->waiting));
 67#ifndef NDEBUG
 68	lockstep->transferId = 0;
 69#endif
 70}
 71
 72void GBASIOLockstepDeinit(struct GBASIOLockstep* lockstep) {
 73	UNUSED(lockstep);
 74}
 75
 76void GBASIOLockstepNodeCreate(struct GBASIOLockstepNode* node) {
 77	node->d.init = GBASIOLockstepNodeInit;
 78	node->d.deinit = GBASIOLockstepNodeDeinit;
 79	node->d.load = GBASIOLockstepNodeLoad;
 80	node->d.unload = GBASIOLockstepNodeUnload;
 81	node->d.writeRegister = 0;
 82	node->d.processEvents = GBASIOLockstepNodeProcessEvents;
 83}
 84
 85bool GBASIOLockstepAttachNode(struct GBASIOLockstep* lockstep, struct GBASIOLockstepNode* node) {
 86	if (lockstep->attached == MAX_GBAS) {
 87		return false;
 88	}
 89	lockstep->players[lockstep->attached] = node;
 90	node->p = lockstep;
 91	node->id = lockstep->attached;
 92	++lockstep->attached;
 93	return true;
 94}
 95
 96void GBASIOLockstepDetachNode(struct GBASIOLockstep* lockstep, struct GBASIOLockstepNode* node) {
 97	if (lockstep->attached == 0) {
 98		return;
 99	}
100	int i;
101	for (i = 0; i < lockstep->attached; ++i) {
102		if (lockstep->players[i] != node) {
103			continue;
104		}
105		for (++i; i < lockstep->attached; ++i) {
106			lockstep->players[i - 1] = lockstep->players[i];
107			lockstep->players[i - 1]->id = i - 1;
108		}
109		--lockstep->attached;
110		break;
111	}
112}
113
114bool GBASIOLockstepNodeInit(struct GBASIODriver* driver) {
115	struct GBASIOLockstepNode* node = (struct GBASIOLockstepNode*) driver;
116	node->needsToWait = false;
117	node->d.p->multiplayerControl.slave = node->id > 0;
118	mLOG(GBA_SIO, DEBUG, "Lockstep %i: Node init", node->id);
119	return true;
120}
121
122void GBASIOLockstepNodeDeinit(struct GBASIODriver* driver) {
123	UNUSED(driver);
124}
125
126bool GBASIOLockstepNodeLoad(struct GBASIODriver* driver) {
127	struct GBASIOLockstepNode* node = (struct GBASIOLockstepNode*) driver;
128	node->nextEvent = 0;
129	node->eventDiff = 0;
130	node->mode = driver->p->mode;
131	switch (node->mode) {
132	case SIO_MULTI:
133		node->d.writeRegister = GBASIOLockstepNodeMultiWriteRegister;
134		node->d.p->rcnt |= 3;
135		++node->p->attachedMulti;
136		node->d.p->multiplayerControl.ready = node->p->attachedMulti == node->p->attached;
137		if (node->id) {
138			node->d.p->rcnt |= 4;
139			node->d.p->multiplayerControl.slave = 1;
140		}
141		break;
142	case SIO_NORMAL_32:
143		node->d.writeRegister = GBASIOLockstepNodeNormalWriteRegister;
144		break;
145	default:
146		break;
147	}
148#ifndef NDEBUG
149	node->phase = node->p->transferActive;
150	node->transferId = node->p->transferId;
151#endif
152	return true;
153}
154
155bool GBASIOLockstepNodeUnload(struct GBASIODriver* driver) {
156	struct GBASIOLockstepNode* node = (struct GBASIOLockstepNode*) driver;
157	node->mode = driver->p->mode;
158	switch (node->mode) {
159	case SIO_MULTI:
160		--node->p->attachedMulti;
161		break;
162	default:
163		break;
164	}
165	_nodeSignal(node, (1 << node->id) ^ 0xF);
166	return true;
167}
168
169static uint16_t GBASIOLockstepNodeMultiWriteRegister(struct GBASIODriver* driver, uint32_t address, uint16_t value) {
170	struct GBASIOLockstepNode* node = (struct GBASIOLockstepNode*) driver;
171	if (address == REG_SIOCNT) {
172		mLOG(GBA_SIO, DEBUG, "Lockstep %i: SIOCNT <- %04x", node->id, value);
173		if (value & 0x0080 && node->p->transferActive == TRANSFER_IDLE) {
174			if (!node->id && node->d.p->multiplayerControl.ready) {
175				mLOG(GBA_SIO, DEBUG, "Lockstep %i: Transfer initiated", node->id);
176				node->p->transferActive = TRANSFER_STARTING;
177				node->p->transferCycles = GBASIOCyclesPerTransfer[node->d.p->multiplayerControl.baud][node->p->attached - 1];
178				node->nextEvent = 0;
179			} else {
180				value &= ~0x0080;
181			}
182		}
183		value &= 0xFF83;
184		value |= driver->p->siocnt & 0x00FC;
185	} else if (address == REG_SIOMLT_SEND) {
186		mLOG(GBA_SIO, DEBUG, "Lockstep %i: SIOMLT_SEND <- %04x", node->id, value);
187	}
188	return value;
189}
190
191static void _finishTransfer(struct GBASIOLockstepNode* node) {
192	if (node->transferFinished) {
193		return;
194	}
195	struct GBASIO* sio = node->d.p;
196	switch (node->mode) {
197	case SIO_MULTI:
198		sio->p->memory.io[REG_SIOMULTI0 >> 1] = node->p->multiRecv[0];
199		sio->p->memory.io[REG_SIOMULTI1 >> 1] = node->p->multiRecv[1];
200		sio->p->memory.io[REG_SIOMULTI2 >> 1] = node->p->multiRecv[2];
201		sio->p->memory.io[REG_SIOMULTI3 >> 1] = node->p->multiRecv[3];
202		sio->rcnt |= 1;
203		sio->multiplayerControl.busy = 0;
204		sio->multiplayerControl.id = node->id;
205		if (sio->multiplayerControl.irq) {
206			GBARaiseIRQ(sio->p, IRQ_SIO);
207		}
208		break;
209	case SIO_NORMAL_8:
210	case SIO_NORMAL_32:
211		// TODO
212		sio->normalControl.start = 0;
213		if (sio->multiplayerControl.irq) {
214			GBARaiseIRQ(sio->p, IRQ_SIO);
215		}
216		break;
217	default:
218		break;
219	}
220	node->transferFinished = true;
221#ifndef NDEBUG
222	++node->transferId;
223#endif
224}
225
226static void _masterUpdate(struct GBASIOLockstepNode* node) {
227	ATOMIC_STORE(node->needsToWait, false);
228	int i;
229	switch (node->p->transferActive) {
230	case TRANSFER_IDLE:
231		// If the master hasn't initiated a transfer, it can keep going.
232		node->nextEvent += LOCKSTEP_INCREMENT;
233		node->d.p->multiplayerControl.ready = node->p->attachedMulti == node->p->attached;
234		break;
235	case TRANSFER_STARTING:
236		// Start the transfer, but wait for the other GBAs to catch up
237		node->transferFinished = false;
238		node->p->multiRecv[0] = 0xFFFF;
239		node->p->multiRecv[1] = 0xFFFF;
240		node->p->multiRecv[2] = 0xFFFF;
241		node->p->multiRecv[3] = 0xFFFF;
242		ATOMIC_STORE(node->needsToWait, true);
243		ATOMIC_STORE(node->p->transferActive, TRANSFER_STARTED);
244		node->nextEvent += 512;
245		break;
246	case TRANSFER_STARTED:
247		// All the other GBAs have caught up and are sleeping, we can all continue now
248		node->p->multiRecv[0] = node->d.p->p->memory.io[REG_SIOMLT_SEND >> 1];
249		node->nextEvent += 512;
250		ATOMIC_STORE(node->p->transferActive, TRANSFER_FINISHING);
251		break;
252	case TRANSFER_FINISHING:
253		// Finish the transfer
254		// We need to make sure the other GBAs catch up so they don't get behind
255		node->nextEvent += node->p->transferCycles - 1024; // Split the cycles to avoid waiting too long
256#ifndef NDEBUG
257		ATOMIC_ADD(node->p->transferId, 1);
258#endif
259		ATOMIC_STORE(node->needsToWait, true);
260		ATOMIC_STORE(node->p->transferActive, TRANSFER_FINISHED);
261		break;
262	case TRANSFER_FINISHED:
263		// Everything's settled. We're done.
264		_finishTransfer(node);
265		node->nextEvent += LOCKSTEP_INCREMENT;
266		ATOMIC_STORE(node->p->transferActive, TRANSFER_IDLE);
267		break;
268	}
269	if (node->needsToWait) {
270		int mask = 0;
271		for (i = 1; i < node->p->attached; ++i) {
272			if (node->p->players[i]->mode == node->mode) {
273				mask |= 1 << i;
274			}
275		}
276		if (mask) {
277			if (!_nodeWait(node, mask)) {
278#ifndef NDEBUG
279				abort();
280#endif
281			}
282		}
283	}
284	// Tell the other GBAs they can continue up to where we were
285	for (i = 1; i < node->p->attached; ++i) {
286		ATOMIC_ADD(node->p->players[i]->nextEvent, node->eventDiff);
287		ATOMIC_STORE(node->p->players[i]->needsToWait, false);
288	}
289#ifndef NDEBUG
290	node->phase = node->p->transferActive;
291#endif
292	_nodeSignal(node, 1);
293}
294
295static void _slaveUpdate(struct GBASIOLockstepNode* node) {
296	ATOMIC_STORE(node->needsToWait, true);
297	node->d.p->multiplayerControl.ready = node->p->attachedMulti == node->p->attached;
298	bool signal = false;
299	switch (node->p->transferActive) {
300	case TRANSFER_IDLE:
301		if (!node->d.p->multiplayerControl.ready) {
302			node->nextEvent += LOCKSTEP_INCREMENT;
303			ATOMIC_STORE(node->needsToWait, false);
304			return;
305		}
306		break;
307	case TRANSFER_STARTING:
308	case TRANSFER_FINISHING:
309		break;
310	case TRANSFER_STARTED:
311		node->transferFinished = false;
312		switch (node->mode) {
313		case SIO_MULTI:
314			node->d.p->rcnt &= ~1;
315			node->p->multiRecv[node->id] = node->d.p->p->memory.io[REG_SIOMLT_SEND >> 1];
316			node->d.p->p->memory.io[REG_SIOMULTI0 >> 1] = 0xFFFF;
317			node->d.p->p->memory.io[REG_SIOMULTI1 >> 1] = 0xFFFF;
318			node->d.p->p->memory.io[REG_SIOMULTI2 >> 1] = 0xFFFF;
319			node->d.p->p->memory.io[REG_SIOMULTI3 >> 1] = 0xFFFF;
320			node->d.p->multiplayerControl.busy = 1;
321			break;
322		default:
323			node->p->multiRecv[node->id]= 0xFFFF;
324			break;
325		}
326		signal = true;
327		break;
328	case TRANSFER_FINISHED:
329		_finishTransfer(node);
330		signal = true;
331		break;
332	}
333	_nodeWait(node, 1);
334#ifndef NDEBUG
335	node->phase = node->p->transferActive;
336#endif
337	if (signal) {
338		_nodeSignal(node, 1 << node->id);
339	}
340}
341
342static int32_t GBASIOLockstepNodeProcessEvents(struct GBASIODriver* driver, int32_t cycles) {
343	struct GBASIOLockstepNode* node = (struct GBASIOLockstepNode*) driver;
344	if (node->p->attached < 2) {
345		return INT_MAX;
346	}
347	node->eventDiff += cycles;
348	cycles = ATOMIC_ADD(node->nextEvent, -cycles);
349	if (cycles <= 0) {
350		if (!node->id) {
351			_masterUpdate(node);
352		} else {
353			_slaveUpdate(node);
354		}
355		node->eventDiff = 0;
356		bool needsToWait;
357		ATOMIC_LOAD(needsToWait, node->needsToWait);
358		if (needsToWait) {
359			return 0;
360		}
361		ATOMIC_LOAD(cycles, node->nextEvent);
362		if (cycles <= 0 && !needsToWait) {
363			int i;
364			// XXX: If we're multithreaded, the atomics may not be sufficient, so wait a few usecs
365			for (i = 0; i < 0x40; ++i) {
366				usleep(10);
367				ATOMIC_LOAD(cycles, node->nextEvent);
368				if (cycles > 0) {
369					return cycles;
370				}
371			}
372			abort();
373			mLOG(GBA_SIO, WARN, "Sleeping needlessly");
374		}
375		return cycles;
376	}
377	return cycles;
378}
379
380static uint16_t GBASIOLockstepNodeNormalWriteRegister(struct GBASIODriver* driver, uint32_t address, uint16_t value) {
381	struct GBASIOLockstepNode* node = (struct GBASIOLockstepNode*) driver;
382	if (address == REG_SIOCNT) {
383		mLOG(GBA_SIO, DEBUG, "Lockstep %i: SIOCNT <- %04x", node->id, value);
384		value &= 0xFF8B;
385		if (value & 0x0080) {
386			// Internal shift clock
387			if (value & 1) {
388				node->p->transferActive = TRANSFER_STARTING;
389			}
390			// Frequency
391			if (value & 2) {
392				node->p->transferCycles = GBA_ARM7TDMI_FREQUENCY / 1024;
393			} else {
394				node->p->transferCycles = GBA_ARM7TDMI_FREQUENCY / 8192;
395			}
396			node->normalSO = !!(value & 8);
397			// Opponent's SO
398			if (node->id) {
399				value |= node->p->players[node->id - 1]->normalSO << 2;
400			}
401		}
402	} else if (address == REG_SIODATA32_LO) {
403		mLOG(GBA_SIO, DEBUG, "Lockstep %i: SIODATA32_LO <- %04x", node->id, value);
404	} else if (address == REG_SIODATA32_HI) {
405		mLOG(GBA_SIO, DEBUG, "Lockstep %i: SIODATA32_HI <- %04x", node->id, value);
406	}
407	return value;
408}