From e4fb433cd685d06ddbf0ec367c19a38b75d6ac68 Mon Sep 17 00:00:00 2001
From: notaz <notasas@gmail.com>
Date: Wed, 25 Jun 2008 21:53:53 +0000
Subject: [PATCH] full ym2612 save/load for 940

git-svn-id: file:///home/notaz/opt/svn/PicoDrive@495 be3aeb3a-fb24-0410-a615-afba39da0efa
---
 Pico/Memory.c                     |  4 +-
 Pico/sound/ym2612.c               |  7 ++--
 platform/gp2x/940ctl.c            | 63 +++++++++++++++++++++++++++++--
 platform/gp2x/940ctl.h            |  2 +
 platform/gp2x/code940/940.c       | 39 ++++++++++++++++++-
 platform/gp2x/code940/940shared.h |  5 ++-
 platform/gp2x/code940/Makefile    | 26 ++++++-------
 platform/gp2x/code940/memcpy.s    | 21 +++--------
 8 files changed, 126 insertions(+), 41 deletions(-)

diff --git a/Pico/Memory.c b/Pico/Memory.c
index 06806cb0..755b5b50 100644
--- a/Pico/Memory.c
+++ b/Pico/Memory.c
@@ -925,7 +925,7 @@ void ym2612_pack_state(void)
 
 #ifdef __GP2X__
   if (PicoOpt & POPT_EXT_FM)
-    /*YM2612PicoStateSave2_940(tat, tbt)*/;
+    YM2612PicoStateSave2_940(tat, tbt);
   else
 #endif
     YM2612PicoStateSave2(tat, tbt);
@@ -960,7 +960,7 @@ void ym2612_unpack_state(void)
 
 #ifdef __GP2X__
   if (PicoOpt & POPT_EXT_FM)
-    /*ret = YM2612PicoStateLoad2_940(&tat, &tbt)*/;
+    ret = YM2612PicoStateLoad2_940(&tat, &tbt);
   else
 #endif
     ret = YM2612PicoStateLoad2(&tat, &tbt);
diff --git a/Pico/sound/ym2612.c b/Pico/sound/ym2612.c
index a619c45f..59bb0cc8 100644
--- a/Pico/sound/ym2612.c
+++ b/Pico/sound/ym2612.c
@@ -1779,11 +1779,11 @@ int YM2612Write_(unsigned int a, unsigned int v)
 				}
 				ret=0;
 				break;
+#endif
 			case 0x27:	/* mode, timer control */
 				set_timers( v );
 				ret=0;
 				break;
-#endif
 			case 0x28:	/* key on / off */
 				{
 					UINT8 c;
@@ -1797,7 +1797,6 @@ int YM2612Write_(unsigned int a, unsigned int v)
 					if(v&0x80) FM_KEYON(c,SLOT4); else FM_KEYOFF(c,SLOT4);
 					break;
 				}
-#if 0
 			case 0x2a:	/* DAC data (YM2612) */
 				ym2612.dacout = ((int)v - 0x80) << 6;	/* level unknown (notaz: 8 seems to be too much) */
 				ret=0;
@@ -1807,7 +1806,6 @@ int YM2612Write_(unsigned int a, unsigned int v)
 				ym2612.dacen = v & 0x80;
 				ret=0;
 				break;
-#endif
 			default:
 				break;
 			}
@@ -1904,13 +1902,14 @@ typedef struct
 	UINT16  unused2;
 	UINT32  keyon_field;	// 20
 	UINT32  kcode_fc_sl3_3;
+	UINT32  reserved[2];
 } ym_save_addon;
 
 typedef struct
 {
 	UINT16  block_fnum[6];
 	UINT16  block_fnum_sl3[3];
-	UINT16  unused;
+	UINT16  reserved[7];
 } ym_save_addon2;
 
 
diff --git a/platform/gp2x/940ctl.c b/platform/gp2x/940ctl.c
index 4a571ffd..b372d92e 100644
--- a/platform/gp2x/940ctl.c
+++ b/platform/gp2x/940ctl.c
@@ -33,7 +33,7 @@ _940_ctl_t *shared_ctl = 0;
 unsigned char *mp3_mem = 0;
 
 #define MP3_SIZE_MAX (0x400000 + 0x800000) // 12M
-#define CODE940_FILE "pico940_v2.bin"
+#define CODE940_FILE "pico940_v3.bin"
 
 int crashed_940 = 0;
 
@@ -69,7 +69,8 @@ int YM2612Write_940(unsigned int a, unsigned int v, int scanline)
 
 	//printf("%05i:%03i: ym w ([%i] %02x)\n", Pico.m.frame_count, Pico.m.scanline, a, v);
 
-	switch (a) {
+	switch (a)
+	{
 		case 0:	/* address port 0 */
 			if (addr_A1 == 0 && ST_address == v)
 				return 0; /* address already selected, don't send this command to 940 */
@@ -186,6 +187,61 @@ void YM2612PicoStateLoad_940(void)
 	addr_A1 = *(INT32 *) (REGS + 0x200);
 }
 
+void YM2612PicoStateSave2_940(int tat, int tbt)
+{
+	UINT8 *ym_remote_regs, *ym_local_regs;
+	add_job_940(JOB940_PICOSTATESAVE2);
+	if (CHECK_BUSY(JOB940_PICOSTATESAVE2)) wait_busy_940(JOB940_PICOSTATESAVE2);
+
+	ym_remote_regs = (UINT8 *) shared_ctl->writebuff0;
+	ym_local_regs  = YM2612GetRegs();
+	if (*(UINT32 *)(ym_remote_regs + 0x100) != 0x41534d59) {
+		printf("code940 didn't return valid save data\n");
+		return;
+	}
+
+	/* copy addin data only */
+	memcpy(ym_local_regs,         ym_remote_regs,         0x20);
+	memcpy(ym_local_regs + 0x100, ym_remote_regs + 0x100, 0x30);
+	memcpy(ym_local_regs + 0x0b8, ym_remote_regs + 0x0b8, 0x48);
+	memcpy(ym_local_regs + 0x1b8, ym_remote_regs + 0x1b8, 0x48);
+	*(INT32 *)(ym_local_regs + 0x108) = tat;
+	*(INT32 *)(ym_local_regs + 0x10c) = tbt;
+}
+
+int YM2612PicoStateLoad2_940(int *tat, int *tbt)
+{
+	UINT8 *ym_remote_regs, *ym_local_regs;
+	ym_local_regs  = YM2612GetRegs();
+	ym_remote_regs = (UINT8 *) shared_ctl->writebuff0;
+
+	if (*(UINT32 *)(ym_local_regs + 0x100) != 0x41534d59)
+		return -1;
+
+	*tat = *(INT32 *)(ym_local_regs + 0x108);
+	*tbt = *(INT32 *)(ym_local_regs + 0x10c);
+
+	if (CHECK_BUSY(JOB940_YM2612UPDATEONE)) wait_busy_940(JOB940_YM2612UPDATEONE);
+
+	/* flush writes */
+	if (shared_ctl->writebuffsel == 1) {
+		shared_ctl->writebuff0[writebuff_ptr & 0xffff] = 0xffff;
+	} else {
+		shared_ctl->writebuff1[writebuff_ptr & 0xffff] = 0xffff;
+	}
+	shared_ctl->writebuffsel ^= 1;
+	writebuff_ptr = 0;
+	add_job_940(JOB940_PICOSTATELOAD2_PREP);
+	if (CHECK_BUSY(JOB940_PICOSTATELOAD2_PREP)) wait_busy_940(JOB940_PICOSTATELOAD2_PREP);
+
+	memcpy(ym_remote_regs, ym_local_regs, 0x200);
+
+	add_job_940(JOB940_PICOSTATELOAD2);
+	if (CHECK_BUSY(JOB940_PICOSTATELOAD2)) wait_busy_940(JOB940_PICOSTATELOAD2);
+
+	return 0;
+}
+
 
 static void internal_reset(void)
 {
@@ -200,7 +256,7 @@ void sharedmem_init(void)
 	if (shared_mem != NULL) return;
 
 	shared_mem = (unsigned char *) mmap(0, 0x210000, PROT_READ|PROT_WRITE, MAP_SHARED, memdev, 0x2000000);
-	if(shared_mem == MAP_FAILED)
+	if (shared_mem == MAP_FAILED)
 	{
 		printf("mmap(shared_data) failed with %i\n", errno);
 		exit(1);
@@ -230,7 +286,6 @@ void sharedmem_deinit(void)
 
 extern char **g_argv;
 
-/* none of the functions in this file should be called before this one */
 void YM2612Init_940(int baseclock, int rate)
 {
 	printf("YM2612Init_940()\n");
diff --git a/platform/gp2x/940ctl.h b/platform/gp2x/940ctl.h
index 32512820..f845819b 100644
--- a/platform/gp2x/940ctl.h
+++ b/platform/gp2x/940ctl.h
@@ -9,3 +9,5 @@ int  YM2612Write_940(unsigned int a, unsigned int v, int scanline);
 
 int  YM2612PicoTick_940(int n);
 void YM2612PicoStateLoad_940(void);
+void YM2612PicoStateSave2_940(int tat, int tbt);
+int  YM2612PicoStateLoad2_940(int *tat, int *tbt);
diff --git a/platform/gp2x/code940/940.c b/platform/gp2x/code940/940.c
index 97a3b5c9..0df19453 100644
--- a/platform/gp2x/code940/940.c
+++ b/platform/gp2x/code940/940.c
@@ -18,6 +18,8 @@ void drain_wb(void);
 // is changed by other core just before we update it
 void set_if_not_changed(int *val, int oldval, int newval);
 
+void _memcpy(void *dst, const void *src, int count);
+
 //	asm volatile ("mov r0, #0" ::: "r0");
 //	asm volatile ("mcr p15, 0, r0, c7, c6,  0" ::: "r0"); /* flush dcache */
 //	asm volatile ("mcr p15, 0, r0, c7, c10, 4" ::: "r0"); /* drain write buffer */
@@ -58,6 +60,26 @@ static void mp3_decode(void)
 	set_if_not_changed(&shared_ctl->mp3_offs, mp3_offs, readPtr - mp3_data);
 }
 
+static void ym_flush_writes(void)
+{
+	UINT16 *wbuff;
+	int i;
+
+	if (shared_ctl->writebuffsel == 1) {
+		wbuff = shared_ctl->writebuff1;
+	} else {
+		wbuff = shared_ctl->writebuff0;
+	}
+
+	/* playback all writes */
+	for (i = 2048; i > 0; i--) {
+		UINT16 d = *wbuff++;
+		if (d == 0xffff) break;
+		if (d == 0xfffe) continue;
+		YM2612Write_(d >> 8, d);
+	}
+}
+
 static void ym_update(int *ym_buffer)
 {
 	int i, dw;
@@ -86,7 +108,8 @@ static void ym_update(int *ym_buffer)
 		YM2612Write_(d >> 8, d);
 	}
 
-	if (two_upds) {
+	if (two_upds)
+	{
 		int len1 = shared_ctl->length / 2;
 		shared_ctl->ym_active_chs =
 			YM2612UpdateOne_(ym_buffer, len1, shared_ctl->stereo, 1);
@@ -145,6 +168,20 @@ void Main940(void)
 				YM2612PicoStateLoad_();
 				break;
 
+			case JOB940_PICOSTATESAVE2:
+				YM2612PicoStateSave2(0, 0);
+				_memcpy(shared_ctl->writebuff0, ym2612_940->REGS, 0x200);
+				break;
+
+			case JOB940_PICOSTATELOAD2_PREP:
+				ym_flush_writes();
+				break;
+
+			case JOB940_PICOSTATELOAD2:
+				_memcpy(ym2612_940->REGS, shared_ctl->writebuff0, 0x200);
+				YM2612PicoStateLoad2(0, 0);
+				break;
+
 			case JOB940_YM2612UPDATEONE:
 				ym_update(ym_buffer);
 				break;
diff --git a/platform/gp2x/code940/940shared.h b/platform/gp2x/code940/940shared.h
index a55020b7..7e578abe 100644
--- a/platform/gp2x/code940/940shared.h
+++ b/platform/gp2x/code940/940shared.h
@@ -1,7 +1,7 @@
 #include "../../../Pico/sound/ym2612.h"
 #include "../../common/helix/pub/mp3dec.h"
 
-// max 16 jobs
+// max 16 jobs, lower num means higher prio
 enum _940_job_t {
 	JOB940_INITALL = 1,
 	JOB940_INVALIDATE_DCACHE,
@@ -9,6 +9,9 @@ enum _940_job_t {
 	JOB940_YM2612UPDATEONE,
 	JOB940_MP3DECODE,
 	JOB940_PICOSTATELOAD,
+	JOB940_PICOSTATESAVE2,
+	JOB940_PICOSTATELOAD2_PREP,
+	JOB940_PICOSTATELOAD2,
 };
 
 //#define MAX_940JOBS	2
diff --git a/platform/gp2x/code940/Makefile b/platform/gp2x/code940/Makefile
index 6b3f5f8c..ab88bf11 100644
--- a/platform/gp2x/code940/Makefile
+++ b/platform/gp2x/code940/Makefile
@@ -18,16 +18,16 @@ AS = $(CROSS)as
 LD = $(CROSS)ld
 OBJCOPY = $(CROSS)objcopy
 
-BIN = pico940_v2.bin
+BIN = pico940_v3.bin
 
 all: $(BIN)
 
 
 .c.o:
-	@echo $<
+	@echo ">>>" $<
 	$(GCC) $(COPT) $(DEFINC) -c $< -o $@
 .s.o:
-	@echo $<
+	@echo ">>>" $<
 	$(GCC) $(COPT) $(DEFINC) -c $< -o $@
 
 
@@ -44,23 +44,23 @@ OBJS940 += uClibc/s_scalbn.o uClibc/s_copysign.o uClibc/k_sin.o uClibc/k_cos.o u
 OBJS940 += uClibc/e_rem_pio2.o uClibc/k_rem_pio2.o uClibc/e_log.o uClibc/wrappers.o
 
 $(BIN) : code940.gpe
-	@echo $@
-	@$(OBJCOPY) -O binary $< $@
+	@echo ">>>" $@
+	$(OBJCOPY) -O binary $< $@
 
 code940.gpe : $(OBJS940) ../../common/helix/helix_mp3.a
-	@echo $@
-	@$(LD) -static -e code940 -Ttext 0x0 $^ -L$(lgcc_path) -lgcc -o $@ -Map code940.map
+	@echo ">>>" $@
+	$(LD) -static -e code940 -Ttext 0x0 $^ -L$(lgcc_path) -lgcc -o $@ -Map code940.map
 
 940ym2612.o : ../../../Pico/sound/ym2612.c
-	@echo $@
-	@$(GCC) $(COPT) -Os $(DEFINC) -DEXTERNAL_YM2612 -c $< -o $@
+	@echo ">>>" $@
+	$(GCC) $(COPT) -Os $(DEFINC) -DEXTERNAL_YM2612 -c $< -o $@
 
 mix.o : ../../../Pico/sound/mix.s
-	@echo $@
-	@$(GCC) $(COPT) $(DEFINC) -DEXTERNAL_YM2612 -c $< -o $@
+	@echo ">>>" $@
+	$(GCC) $(COPT) $(DEFINC) -DEXTERNAL_YM2612 -c $< -o $@
 misc.o : ../../../Pico/Misc.s
-	@echo $@
-	@$(GCC) $(COPT) $(DEFINC) -DEXTERNAL_YM2612 -c $< -o $@
+	@echo ">>>" $@
+	$(GCC) $(COPT) $(DEFINC) -DEXTERNAL_YM2612 -c $< -o $@
 
 ../../common/helix/helix_mp3.a:
 	@make -C ../../common/helix/
diff --git a/platform/gp2x/code940/memcpy.s b/platform/gp2x/code940/memcpy.s
index bb4cdaab..282762fd 100644
--- a/platform/gp2x/code940/memcpy.s
+++ b/platform/gp2x/code940/memcpy.s
@@ -36,33 +36,23 @@
 * POSSIBILITY OF SUCH DAMAGE.
 */
 
-/* This was modified by Jay Monkman <jmonkman@smoothsmoothie.com> to
-*   save and restore r12. This is necessary for RTEMS.
-*/
 /* #include <machine/asm.h>*/
 
-#define ENTRY(_LABEL) \
- .global _LABEL; _LABEL:
-
 .globl memcpy
+.globl _memcpy
 memcpy:
 
-@ ENTRY(gp2x_memcpy)
-stmfd sp!, {r0, r12, lr}
-@ bl _gp2x_memcpy
+stmfd sp!, {r0, lr}
 bl _memcpy
-ldmfd sp!, {r0, r12, pc}
-
+ldmfd sp!, {r0, pc}
 
 
 .globl memmove
 memmove:
 
-@ ENTRY(gp2x_memmove)
-stmfd sp!, {r0, r12, lr}
-@ bl _gp2x_memcpy
+stmfd sp!, {r0, lr}
 bl _memcpy
-ldmfd sp!, {r0, r12, pc}
+ldmfd sp!, {r0, pc}
 
 
 
@@ -101,7 +91,6 @@ ldmfd sp!, {r0, r12, pc}
 
 _memcpy:
 
-@ ENTRY(_gp2x_memcpy)
 /* Determine copy direction */
 cmp r1, r0
 bcc Lmemcpy_backwards
-- 
2.39.5