diff --git a/src/sound.c b/src/sound.c
index 1c4d633..af3f5ee 100644
--- a/src/sound.c
+++ b/src/sound.c
@@ -70,6 +70,7 @@ static int32 RectDutyCount[2];
 static uint8 sweepon[2];
 static int32 curfreq[2];
 static uint8 SweepCount[2];
+static uint8 sweepReload[2];
 
 static uint16 nreg = 0;
 
@@ -183,13 +184,9 @@ static void SQReload(int x, uint8 V) {
 		lengthcount[x] = lengthtable[(V >> 3) & 0x1f];
 	}
 
-	sweepon[x] = PSG[(x << 2) | 1] & 0x80;
-	curfreq[x] = PSG[(x << 2) | 0x2] | ((V & 7) << 8);
-	SweepCount[x] = ((PSG[(x << 2) | 0x1] >> 4) & 7) + 1;
-
+	curfreq[x] = curfreq[x] & 0xff | ((V & 7) << 8);
 	RectDutyCount[x] = 7;
 	EnvUnits[x].reloaddec = 1;
-	/* reloadfreq[x]=1; */
 }
 
 static DECLFW(Write_PSG) {
@@ -204,7 +201,9 @@ static DECLFW(Write_PSG) {
 			V = (V & 0x3F) | ((V & 0x80) >> 1) | ((V & 0x40) << 1);
 		break;
 	case 0x1:
-		sweepon[0] = V & 0x80;
+		DoSQ1();
+		sweepReload[0] = 1;
+		sweepon[0] = (V & 0x80);
 		break;
 	case 0x2:
 		DoSQ1();
@@ -222,7 +221,9 @@ static DECLFW(Write_PSG) {
 			V = (V & 0x3F) | ((V & 0x80) >> 1) | ((V & 0x40) << 1);
 		break;
 	case 0x5:
-		sweepon[1] = V & 0x80;
+		DoSQ2();
+		sweepReload[1] = 1;
+		sweepon[1] = (V & 0x80);
 		break;
 	case 0x6:
 		DoSQ2();
@@ -371,34 +372,25 @@ static void FASTAPASS(1) FrameSoundStuff(int V) {
 			/* Frequency Sweep Code Here */
 			/* xxxx 0000 */
 			/* xxxx = hz.  120/(x+1)*/
-			if (sweepon[P]) {
-				int32 mod = 0;
-
-				if (SweepCount[P] > 0) SweepCount[P]--;
-				if (SweepCount[P] <= 0) {
-					SweepCount[P] = ((PSG[(P << 2) + 0x1] >> 4) & 7) + 1;	/* +1; */
+			/* http://wiki.nesdev.com/w/index.php/APU_Sweep */
+			if (SweepCount[P] > 0) SweepCount[P]--;
+			if (SweepCount[P] <= 0) {
+				uint32 sweepShift = (PSG[(P << 2) + 0x1] & 7);
+				if (sweepon[P] && sweepShift && curfreq[P] >= 8) {
+					int32 mod = (curfreq[P] >> sweepShift);
 					if (PSG[(P << 2) + 0x1] & 0x8) {
-						mod -= (P ^ 1) + ((curfreq[P]) >> (PSG[(P << 2) + 0x1] & 7));
-						if (curfreq[P] && (PSG[(P << 2) + 0x1] & 7)	/* && sweepon[P]&0x80*/) {
-							curfreq[P] += mod;
-						}
-					} else {
-						mod = curfreq[P] >> (PSG[(P << 2) + 0x1] & 7);
-						if ((mod + curfreq[P]) & 0x800) {
-							sweepon[P] = 0;
-							curfreq[P] = 0;
-						} else {
-							if (curfreq[P] && (PSG[(P << 2) + 0x1] & 7)	/* && sweepon[P]&0x80*/) {
-								curfreq[P] += mod;
-							}
-						}
+						curfreq[P] -= (mod + (P ^ 1));
+					} else if ((mod + curfreq[P]) < 0x800) {
+						curfreq[P] += mod;
 					}
 				}
-			} else {/* Sweeping is disabled: */
-#if 0
-				curfreq[P]&=0xFF00;
-				curfreq[P]|=PSG[(P<<2)|0x2]; /* |((PSG[(P<<2)|3]&7)<<8); */
-#endif
+
+				SweepCount[P] = (((PSG[(P << 2) + 0x1] >> 4) & 7) + 1);
+			}
+
+			if (sweepReload[P]) {
+				SweepCount[P] = (((PSG[(P << 2) + 0x1] >> 4) & 7) + 1);
+				sweepReload[P] = 0;
 			}
 		}
 	}