Save & restore CIA state correctly (backported from 0adb6e4)

cahirwpz · Dec 16, 2023 · c3c03ea · c3c03ea
1 parent 2161ddf
commit c3c03ea
Showing 1 changed file with 119 additions and 19 deletions.
diff --git a/system/amigaos.c b/system/amigaos.c
@@ -5,14 +5,18 @@
 #include <hardware/custom.h>
 #include <hardware/dmabits.h>
 #include <hardware/intbits.h>
+#include <resources/cia.h>
 #include <proto/alib.h>
+#define LP2NB LP2UB
+#include <proto/cia.h>
 #include <proto/exec.h>
 #undef Debug
 #include <proto/graphics.h>
 
 #include <system/boot.h>
 #include <system/cpu.h>
 #include <system/exception.h>
+#include <system/cia.h>
 
 #include <debug.h>
 #include <string.h>
@@ -32,12 +36,76 @@ static void WaitVBlank(void) {
     continue;
 }
 
+typedef struct TimerState {
+  u_char cr; /* control register */
+  u_char tlo, thi; /* timer values */
+  u_char llo, lhi; /* latch values */
+} TimerStateT;
+
+typedef volatile u_char *CiaRegT;
+
+static inline void nop(void) {
+  asm volatile("nop;" ::: "memory");
+}
+
+static inline void bclr(CiaRegT reg, char bit) {
+  asm volatile("bclr %1,%0" :: "m" (*reg), "dI" (bit) : "memory");
+  nop();
+}
+
+static inline void bset(CiaRegT reg, char bit) {
+  asm volatile("bset %1,%0" :: "m" (*reg), "dI" (bit) : "memory");
+  nop();
+}
+
+/* Based on jst_cus.asm from https://github.com/jotd666/jst */
+static void GetTimerState(CiaRegT cr asm("a0"), CiaRegT tlo asm("a1"),
+                          CiaRegT thi asm("a2"), TimerStateT *ts asm("a3"))
+{
+  ts->cr = *cr;
+
+  bclr(cr, CIACRB_START);
+
+  ts->tlo = *tlo;
+  ts->thi = *thi;
+
+  bclr(cr, CIACRB_RUNMODE);
+  bclr(cr, CIACRB_PBON);
+  bset(cr, CIACRB_LOAD);
+
+  ts->llo = *tlo;
+  ts->lhi = *thi;
+}
+
+static void SetTimerState(CiaRegT cr asm("a0"), CiaRegT tlo asm("a1"),
+                          CiaRegT thi asm("a2"), TimerStateT *ts asm("a3"))
+{
+  *cr = 0;
+  nop();
+
+  *tlo = ts->tlo;
+  *thi = ts->thi;
+  nop();
+
+  bset(cr, CIACRB_LOAD);
+  *tlo = ts->llo;
+  *thi = ts->thi;
+  nop();
+
+  *cr = ts->cr;
+}
+
 /* AmigaOS state that we want to preserve. */
-static struct View *oldView;
-static u_short oldDmacon, oldIntena, oldAdkcon;
-static u_int oldCacheBits;
-static ExcVecT oldExcVec;
-static void *oldSysStack;
+static __code struct {
+  struct View *view;
+  u_short dmacon, intena, intreq, adkcon;
+  u_int cacheBits;
+  ExcVecT excVec;
+  void *sysStack;
+  TimerStateT timer[4];
+  struct Library *resCiaA, *resCiaB;
+  u_char icrCiaA, icrCiaB;
+} old;
 
 /* Memory for framework allocator. */
 static __aligned(4) __bss_chip char ChipMem[CHIPMEM_KB * 1024];
@@ -91,6 +159,9 @@ BootDataT *SaveOS(void) {
   *(struct GfxBase **)&GfxBase =
     (struct GfxBase *)OpenLibrary("graphics.library", 33);
 
+  old.resCiaA = OpenResource(CIAANAME);
+  old.resCiaB = OpenResource(CIABNAME);
+
   /* Allocate blitter. */
   WaitBlit();
   OwnBlitter();
@@ -100,18 +171,19 @@ BootDataT *SaveOS(void) {
 
   /* Disable CPU caches. */
   if (ExecVer >= 36)
-    oldCacheBits = CacheControl(0, -1);
+    old.cacheBits = CacheControl(0, -1);
 
   /* Intercept the view of AmigaOS. */
-  oldView = GfxBase->ActiView;
+  old.view = GfxBase->ActiView;
   LoadView(NULL);
   WaitTOF();
   WaitTOF();
 
   /* DMA & interrupts take-over. */
-  oldAdkcon = custom->adkconr;
-  oldDmacon = custom->dmaconr;
-  oldIntena = custom->intenar;
+  old.adkcon = custom->adkconr;
+  old.dmacon = custom->dmaconr;
+  old.intena = custom->intenar;
+  old.intreq = custom->intreqr;
 
   /* Prohibit dma & interrupts. */
   custom->adkcon = (UWORD)~ADKF_SETCLR;
@@ -123,6 +195,22 @@ BootDataT *SaveOS(void) {
   custom->intreq = (UWORD)~INTF_SETCLR;
   custom->intreq = (UWORD)~INTF_SETCLR;
 
+  /* CIA-A & CIA-B: Mask all interrupts, get old masks. */
+  old.icrCiaA = AbleICR(old.resCiaA, CIAICRF_ALL);
+  old.icrCiaB = AbleICR(old.resCiaB, CIAICRF_ALL);
+
+  /* CIA-A & CIA-B: Save state of all timers. */
+  GetTimerState(&ciaa->ciacra, &ciaa->ciatalo, &ciaa->ciatahi, &old.timer[0]);
+  GetTimerState(&ciaa->ciacrb, &ciaa->ciatblo, &ciaa->ciatbhi, &old.timer[1]);
+  GetTimerState(&ciab->ciacra, &ciab->ciatalo, &ciab->ciatahi, &old.timer[2]);
+  GetTimerState(&ciab->ciacrb, &ciab->ciatblo, &ciab->ciatbhi, &old.timer[3]);
+
+  /* CIA-A & CIA-B: Stop timers and return to default settings. */
+  ciaa->ciacra = 0;
+  ciaa->ciacrb = 0;
+  ciab->ciacra = 0;
+  ciab->ciacrb = 0;
+
   {
     struct Task *self = FindTask(NULL);
     bd->bd_stkbot = self->tc_SPLower;
@@ -131,7 +219,7 @@ BootDataT *SaveOS(void) {
 
   /* Enter supervisor mode and save exception vector
    * since the framework takes full control over it. */
-  oldSysStack = SuperState();
+  old.sysStack = SuperState();
 
   /* Detect CPU model and fetch VBR on 68010 and later. */
   {
@@ -157,7 +245,7 @@ BootDataT *SaveOS(void) {
     bd->bd_cpumodel = cpu;
   }
 
-  memcpy(oldExcVec, bd->bd_vbr, sizeof(oldExcVec));
+  memcpy(old.excVec, bd->bd_vbr, sizeof(old.excVec));
 
   return &BootData;
 }
@@ -177,26 +265,38 @@ void RestoreOS(void) {
   custom->intreq = (UWORD)~INTF_SETCLR;
 
   /* Restore exception vector and leave supervisor mode. */
-  memcpy(bd->bd_vbr, oldExcVec, sizeof(oldExcVec));
-  UserState(oldSysStack);
+  memcpy(bd->bd_vbr, old.excVec, sizeof(old.excVec));
+  /* TODO: This function is broken in V33/34 Kickstart, hangs on 68010. */
+  UserState(old.sysStack);
+
+  /* CIA-A & CIA-B: Restore state of all timers. */
+  SetTimerState(&ciaa->ciacra, &ciaa->ciatalo, &ciaa->ciatahi, &old.timer[0]);
+  SetTimerState(&ciaa->ciacrb, &ciaa->ciatblo, &ciaa->ciatbhi, &old.timer[1]);
+  SetTimerState(&ciab->ciacra, &ciab->ciatalo, &ciab->ciatahi, &old.timer[2]);
+  SetTimerState(&ciab->ciacrb, &ciab->ciatblo, &ciab->ciatbhi, &old.timer[3]);
+
+  /* CIA-A & CIA-B: Restore old interrupts masks. */
+  AbleICR(old.resCiaA, old.icrCiaA | CIAICRF_SETCLR);
+  AbleICR(old.resCiaB, old.icrCiaB | CIAICRF_SETCLR);
 
   /* Restore AmigaOS state of dma & interrupts. */
-  custom->dmacon = oldDmacon | DMAF_SETCLR;
-  custom->intena = oldIntena | INTF_SETCLR;
-  custom->adkcon = oldAdkcon | ADKF_SETCLR;
+  custom->intreq = old.intreq | INTF_SETCLR;
+  custom->intena = old.intena | INTF_SETCLR;
+  custom->dmacon = old.dmacon | DMAF_SETCLR;
+  custom->adkcon = old.adkcon | ADKF_SETCLR;
 
   /* Restore old copper list... */
   custom->cop1lc = (ULONG)GfxBase->copinit;
   WaitTOF();
 
   /* ... and original view. */
-  LoadView(oldView);
+  LoadView(old.view);
   WaitTOF();
   WaitTOF();
 
   /* Enable CPU caches. */
   if (ExecVer >= 36)
-    CacheControl(oldCacheBits, -1);
+    CacheControl(old.cacheBits, -1);
 
   /* Restore multitasking. */
   Permit();