From ce2edf56db8287caffe1f3f1d95e074e496a6fa5 Mon Sep 17 00:00:00 2001
From: Rodrigo Alfonso <rodri042@gmail.com>
Date: Tue, 17 Nov 2020 11:45:22 -0300
Subject: [PATCH] Adding waitCycles between transfers

---
 examples/full/src/main.cpp             |  1 +
 examples/full/src/scenes/TestScene.cpp |  2 +-
 lib/LinkConnection.h                   | 72 +++++++++++++++++++-------
 3 files changed, 56 insertions(+), 19 deletions(-)

diff --git a/examples/full/src/main.cpp b/examples/full/src/main.cpp
index 2b1c90f..d4f1054 100644
--- a/examples/full/src/main.cpp
+++ b/examples/full/src/main.cpp
@@ -60,6 +60,7 @@ inline void setUpInterrupts() {
   // LinkConnection
   irq_add(II_VBLANK, LINK_ISR_VBLANK);
   irq_add(II_TIMER3, LINK_ISR_TIMER);
+  irq_add(II_TIMER2, NULL);
   irq_add(II_SERIAL, LINK_ISR_SERIAL);
 
   // A+B+START+SELECT
diff --git a/examples/full/src/scenes/TestScene.cpp b/examples/full/src/scenes/TestScene.cpp
index 1eccfe9..8029943 100644
--- a/examples/full/src/scenes/TestScene.cpp
+++ b/examples/full/src/scenes/TestScene.cpp
@@ -69,7 +69,7 @@ void TestScene::tick(u16 keys) {
 
   // determine which value should be sent
   u16 value = LINK_NO_DATA;
-  if (!initialized && linkConnection->linkState->currentPlayerId == 1) {
+  if (!initialized && linkState->currentPlayerId == 1) {
     initialized = true;
     value = 999;
   }
diff --git a/lib/LinkConnection.h b/lib/LinkConnection.h
index 6176652..70b9f19 100644
--- a/lib/LinkConnection.h
+++ b/lib/LinkConnection.h
@@ -1,6 +1,7 @@
 #ifndef LINK_CONNECTION_H
 #define LINK_CONNECTION_H
 
+#include <tonc_bios.h>
 #include <tonc_core.h>
 #include <tonc_memdef.h>
 #include <tonc_memmap.h>
@@ -8,6 +9,8 @@
 #include <memory>
 #include <queue>
 
+#include "utils/SceneUtils.h"  // TODO: REMOVE
+
 #define LINK_MAX_PLAYERS 4
 #define LINK_DISCONNECTED 0xFFFF
 #define LINK_NO_DATA 0x0
@@ -50,6 +53,8 @@ void LINK_ISR_TIMER();
 void LINK_ISR_SERIAL();
 u16 LINK_QUEUE_POP(std::queue<u16>& q);
 void LINK_QUEUE_CLEAR(std::queue<u16>& q);
+const u16 LINK_TIMER_IRQ_IDS[] = {IRQ_TIMER0, IRQ_TIMER1, IRQ_TIMER2,
+                                  IRQ_TIMER3};
 
 struct LinkState {
   u8 playerCount;
@@ -123,50 +128,63 @@ class LinkConnection {
     push(linkState->_outgoingMessages, data);
   }
 
+  bool isReady() { return isBitHigh(LINK_BIT_READY); }
   bool hasError() { return isBitHigh(LINK_BIT_ERROR); }
 
   void _onVBlank() {
-    if (!isEnabled || resetIfNeeded())
+    if (!isEnabled)
       return;
 
-    if (!linkState->_IRQFlag) {
+    if (!linkState->_IRQFlag)
       linkState->_IRQTimeout++;
 
-      if (linkState->_IRQTimeout >= timeout)
-        reset();
-    }
-
     linkState->_IRQFlag = false;
   }
 
   void _onTimer() {
-    if (!isEnabled || resetIfNeeded())
+    if (!isEnabled || !isReady())
       return;
 
+    if (linkState->_IRQTimeout > timeout) {
+      reset();
+      return;
+    }
+
+    stopTimer();
     sendPendingData();
+    startTimer();
   }
 
   void _onSerial() {
-    if (!isEnabled || resetIfNeeded(false))
+    if (!isEnabled)
+      return;
+
+    waitCycles(1000);  // TODO: PARAMETERIZE
+    if (resetIfNeeded())
       return;
 
     linkState->_IRQFlag = true;
     linkState->_IRQTimeout = 0;
 
-    linkState->playerCount = 0;
-    linkState->currentPlayerId =
-        (REG_SIOCNT & (0b11 << LINK_BITS_PLAYER_ID)) >> LINK_BITS_PLAYER_ID;
-
+    u8 newPlayerCount = 0;
     for (u32 i = 0; i < LINK_MAX_PLAYERS; i++) {
       u16 data = REG_SIOMULTI[i];
 
       if (data != LINK_DISCONNECTED) {
         if (data != LINK_NO_DATA)
           push(linkState->_incomingMessages[i], data);
-        linkState->playerCount++;
+        newPlayerCount++;
       } else
         LINK_QUEUE_CLEAR(linkState->_incomingMessages[i]);
     }
+
+    if (linkState->playerCount == 2 && newPlayerCount == 1) {
+      DEBULOG("hey, rompió: " + asStr(REG_SIOMULTI[0]) + "-" +
+              asStr(REG_SIOMULTI[1]));
+    }
+    linkState->playerCount = newPlayerCount;
+    linkState->currentPlayerId =
+        (REG_SIOCNT & (0b11 << LINK_BITS_PLAYER_ID)) >> LINK_BITS_PLAYER_ID;
   }
 
  private:
@@ -184,12 +202,16 @@ class LinkConnection {
   void transfer(u16 data) {
     REG_SIOMLT_SEND = data;
 
+    waitCycles(1000);  // PARAMETERIZE
+
     if (isMaster())
       setBitHigh(LINK_BIT_START);
   }
 
-  bool resetIfNeeded(bool checkErrors = false) {
-    if (checkErrors && hasError()) {
+  bool resetIfNeeded() {
+    if (hasError()) {
+      DEBULOG("resetting: " + asStr(isReady()) + "-" + asStr(hasError()) + "-" +
+              asStr(linkState->_IRQTimeout));
       reset();
       return true;
     }
@@ -214,15 +236,14 @@ class LinkConnection {
   }
 
   void stop() {
-    REG_TM[timerId].cnt = REG_TM[timerId].cnt & (~TM_ENABLE);
+    stopTimer();
 
     LINK_SET_LOW(REG_RCNT, LINK_BIT_GENERAL_PURPOSE_LOW);
     LINK_SET_HIGH(REG_RCNT, LINK_BIT_GENERAL_PURPOSE_HIGH);
   }
 
   void start() {
-    REG_TM[timerId].start = -frequency;
-    REG_TM[timerId].cnt = TM_ENABLE | TM_IRQ | LINK_BASE_FREQUENCY;
+    startTimer();
 
     LINK_SET_LOW(REG_RCNT, LINK_BIT_GENERAL_PURPOSE_HIGH);
     REG_SIOCNT = baudRate;
@@ -231,6 +252,13 @@ class LinkConnection {
     setBitHigh(LINK_BIT_IRQ);
   }
 
+  void stopTimer() { REG_TM[timerId].cnt = REG_TM[timerId].cnt & (~TM_ENABLE); }
+
+  void startTimer() {
+    REG_TM[timerId].start = -frequency;
+    REG_TM[timerId].cnt = TM_ENABLE | TM_IRQ | LINK_BASE_FREQUENCY;
+  }
+
   void push(std::queue<u16>& q, u16 value) {
     if (q.size() >= bufferSize)
       LINK_QUEUE_POP(q);
@@ -238,6 +266,14 @@ class LinkConnection {
     q.push(value);
   }
 
+  void waitCycles(u16 amount) {
+    // TODO: Parameterize
+    REG_TM[2].start = -amount;
+    REG_TM[2].cnt = TM_ENABLE | TM_IRQ | TM_FREQ_1;
+    IntrWait(1, LINK_TIMER_IRQ_IDS[2]);
+    REG_TM[2].cnt = 0;
+  }
+
   bool isMaster() { return !isBitHigh(LINK_BIT_SLAVE); }
   bool isBitHigh(u8 bit) { return (REG_SIOCNT >> bit) & 1; }
   void setBitHigh(u8 bit) { LINK_SET_HIGH(REG_SIOCNT, bit); }