cdrom: implement attenuator/volume properly

3 files changed, 71 insertions, 31 deletions

diff --git a/libpcsxcore/cdriso.c b/libpcsxcore/cdriso.c
index 8f18f4f..94bd3eb 100644
--- a/libpcsxcore/cdriso.c
+++ b/libpcsxcore/cdriso.c
@@ -212,6 +212,8 @@ static void *playthread(void *param)
 				}
 			}
 
+			// can't do it yet due to readahead..
+			//cdrAttenuate((short *)sndbuffer, s / 4, 1);
 			do {
 				ret = SPU_playCDDAchannel((short *)sndbuffer, s);
 				if (ret == 0x7761)
diff --git a/libpcsxcore/cdrom.c b/libpcsxcore/cdrom.c
index f227b1b..2a1ce61 100644
--- a/libpcsxcore/cdrom.c
+++ b/libpcsxcore/cdrom.c
@@ -1076,6 +1076,54 @@ void cdrInterrupt() {
 #endif
 }
 
+#ifdef __ARM_ARCH_7A__
+ #define ssat32_to_16(v) \
+  asm("ssat %0,#16,%1" : "=r" (v) : "r" (v))
+#else
+ #define ssat32_to_16(v) do { \
+  if (v < -32768) v = -32768; \
+  else if (v > 32767) v = 32767; \
+ } while (0)
+#endif
+
+void cdrAttenuate(s16 *buf, int samples, int stereo)
+{
+	int i, l, r;
+	int ll = cdr.AttenuatorLeftToLeft;
+	int lr = cdr.AttenuatorLeftToRight;
+	int rl = cdr.AttenuatorRightToLeft;
+	int rr = cdr.AttenuatorRightToRight;
+
+	if (lr == 0 && rl == 0 && 0x78 <= ll && ll <= 0x88 && 0x78 <= rr && rr <= 0x88)
+		return;
+
+	if (!stereo && ll == 0x40 && lr == 0x40 && rl == 0x40 && rr == 0x40)
+		return;
+
+	if (stereo) {
+		for (i = 0; i < samples; i++) {
+			l = buf[i * 2];
+			r = buf[i * 2 + 1];
+			l = (l * ll + r * rl) >> 7;
+			r = (r * rr + l * lr) >> 7;
+			ssat32_to_16(l);
+			ssat32_to_16(r);
+			buf[i * 2] = l;
+			buf[i * 2 + 1] = r;
+		}
+	}
+	else {
+		for (i = 0; i < samples; i++) {
+			l = buf[i];
+			l = l * (ll + rl) >> 7;
+			//r = r * (rr + lr) >> 7;
+			ssat32_to_16(l);
+			//ssat32_to_16(r);
+			buf[i] = l;
+		}
+	}
+}
+
 void cdrReadInterrupt() {
 	u8 *buf;
 
@@ -1127,25 +1175,9 @@ void cdrReadInterrupt() {
 			 (cdr.Transfer[4 + 1] == cdr.Channel) &&
 			(cdr.Transfer[4 + 0] == cdr.File)) {
 			int ret = xa_decode_sector(&cdr.Xa, cdr.Transfer+4, cdr.FirstSector);
-
 			if (!ret) {
-				// only handle attenuator basic channel switch for now
-				if (cdr.Xa.stereo) {
-					int i;
-					if ((cdr.AttenuatorLeft[0] | cdr.AttenuatorLeft[1])
-					    && !(cdr.AttenuatorRight[0] | cdr.AttenuatorRight[1]))
-					{
-						for (i = 0; i < cdr.Xa.nsamples; i++)
-							cdr.Xa.pcm[i*2 + 1] = cdr.Xa.pcm[i*2];
-					}
-					else if (!(cdr.AttenuatorLeft[0] | cdr.AttenuatorLeft[1])
-					    && (cdr.AttenuatorRight[0] | cdr.AttenuatorRight[1]))
-					{
-						for (i = 0; i < cdr.Xa.nsamples; i++)
-							cdr.Xa.pcm[i*2] = cdr.Xa.pcm[i*2 + 1];
-					}
-				}
 
+				cdrAttenuate(cdr.Xa.pcm, cdr.Xa.nsamples, cdr.Xa.stereo);
 				SPU_playADPCMchannel(&cdr.Xa);
 				cdr.FirstSector = 0;
 
@@ -1253,7 +1285,7 @@ void cdrWrite1(unsigned char rt) {
 	case 0:
 		break;
 	case 3:
-		cdr.AttenuatorRight[1] = rt;
+		cdr.AttenuatorRightToRightT = rt;
 		return;
 	default:
 		return;
@@ -1474,10 +1506,10 @@ void cdrWrite2(unsigned char rt) {
 		setIrq();
 		return;
 	case 2:
-		cdr.AttenuatorLeft[0] = rt;
+		cdr.AttenuatorLeftToLeftT = rt;
 		return;
 	case 3:
-		cdr.AttenuatorRight[0] = rt;
+		cdr.AttenuatorRightToLeftT = rt;
 		return;
 	}
 }
@@ -1505,13 +1537,15 @@ void cdrWrite3(unsigned char rt) {
 			cdr.ParamC = 0;
 		return;
 	case 2:
-		cdr.AttenuatorLeft[1] = rt;
+		cdr.AttenuatorLeftToRightT = rt;
 		return;
 	case 3:
-		if (rt == 0x20)
-		CDR_LOG( "CD-XA Volume: %X %X | %X %X\n",
-			cdr.AttenuatorLeft[0], cdr.AttenuatorLeft[1],
-			cdr.AttenuatorRight[0], cdr.AttenuatorRight[1] );
+		if (rt & 0x20) {
+			memcpy(&cdr.AttenuatorLeftToLeft, &cdr.AttenuatorLeftToLeftT, 4);
+			CDR_LOG_I("CD-XA Volume: %02x %02x | %02x %02x\n",
+				cdr.AttenuatorLeftToLeft, cdr.AttenuatorLeftToRight,
+				cdr.AttenuatorRightToLeft, cdr.AttenuatorRightToRight);
+		}
 		return;
 	}
 
@@ -1634,10 +1668,10 @@ void cdrReset() {
 	pTransfer = cdr.Transfer;
 
 	// BIOS player - default values
-	cdr.AttenuatorLeft[0] = 0x80;
-	cdr.AttenuatorLeft[1] = 0x00;
-	cdr.AttenuatorRight[0] = 0x00;
-	cdr.AttenuatorRight[1] = 0x80;
+	cdr.AttenuatorLeftToLeft = 0x80;
+	cdr.AttenuatorLeftToRight = 0x00;
+	cdr.AttenuatorRightToLeft = 0x00;
+	cdr.AttenuatorRightToRight = 0x80;
 
 	getCdInfo();
 }
diff --git a/libpcsxcore/cdrom.h b/libpcsxcore/cdrom.h
index 541a12b..fc1df46 100644
--- a/libpcsxcore/cdrom.h
+++ b/libpcsxcore/cdrom.h
@@ -102,13 +102,17 @@ typedef struct {
 	u8 FastBackward;
 	u8 pad;
 
-	u8 AttenuatorLeft[2], AttenuatorRight[2];
-	u32 pad2;
+	u8 AttenuatorLeftToLeft, AttenuatorLeftToRight;
+	u8 AttenuatorRightToRight, AttenuatorRightToLeft;
+	u8 AttenuatorLeftToLeftT, AttenuatorLeftToRightT;
+	u8 AttenuatorRightToRightT, AttenuatorRightToLeftT;
 } cdrStruct;
 
 extern cdrStruct cdr;
 
 void cdrReset();
+void cdrAttenuate(s16 *buf, int samples, int stereo);
+
 void cdrInterrupt();
 void cdrReadInterrupt();
 void cdrRepplayInterrupt();