Implemented SMU service requests

Signed-off-by: Damien Zammit <damien@zamaudio.com>

1 files changed, 137 insertions, 24 deletions

diff --git a/firmware/servicereq.c b/firmware/servicereq.c
index 4e924d4..21eedf9 100644
--- a/firmware/servicereq.c
+++ b/firmware/servicereq.c
@@ -1569,6 +1569,37 @@ x180ac:
 	return;
 }
 
+static void x18420(u32 r1, u32 r2, u32 r3)
+{
+	u32 r11, r12, r13, r14;
+
+	r14 = r1;
+	r13 = r2;
+	r11 = r3 & 0xff;
+	r12 = 0;
+	if (r11 == r12)
+		goto end;
+x1844c:
+	r3 = r11 & 1;
+	r1 = r12 << 7;
+	r11 = r11 >> 1;
+	r3 = (r3 == 0);
+	r12++;
+	r1 = r14 | r1;
+	r2 = r13;
+	if (r3 != 0)
+		goto x18470;
+	r3 = 0x80080000;
+	write32(r3+160, r1);
+	write32(r3+164, r2);
+
+x18470:
+	if (r11 != 0)
+		goto x1844c;
+end:
+	return;
+}
+
 static void x18d04(phy_t *phy, u32 rr2, u32 rr3)
 {
 	u32 r1, r2, r3, r4, r5, r6, r7, r8, r9, r10;
@@ -1955,12 +1986,16 @@ x18538:
 	r2 = read32(r14);
 	r1 = r11 + 11;
 	r2 &= r15;
-	x16fb0();
+	r3 = 0x80080000;
+	write32(r3+160, r1);
+	write32(r3+164, r2);
 
 	r2 = read32(r14+20);
 	r1 = r11 + 12;
 	r2 &= r15;
-	x16fb0();
+	r3 = 0x80080000;
+	write32(r3+160, r1);
+	write32(r3+164, r2);
 
 x18590:
 	r1 = (r12 > 4);
@@ -1971,7 +2006,11 @@ x18590:
 		goto x185b0;
 	r2 = read8(r13);
 	r1 = 0x01308023;
-	x16fd8();
+	r3 = 0x80080000;
+	write32(r3+160, r1);
+	r1 = read32(r3+164);
+	r1 |= r2;
+	write32(r3+164, r1);
 
 x185b0:
 	r7 = (phy->p1 & 0xff00) >> 8;
@@ -2020,7 +2059,13 @@ x18630:
 x18638:
 	r1 = 0x01318025;
 	r2 = r4;
-	x17010();
+	r4 = 0x80080000;
+	write32(r4+160, r1);
+	r1 = read32(r4+164);
+	r3 = ~r3;
+	r1 &= r3;
+	r1 |= r2;
+	write32(r4+164, r1);
 
 x18648:
 	r1 = (phy->p1 & 0xff000000) >> 24;
@@ -2043,7 +2088,13 @@ x1866c:
 x1867c:
 	r1 = 0x01328025;
 	r2 = r4;
-	x17010();
+	r4 = 0x80080000;
+	write32(r4+160, r1);
+	r1 = read32(r4+164);
+	r3 = ~r3;
+	r1 &= r3;
+	r1 |= r2;
+	write32(r4+164, r1);
 
 x1868c:
 	r1 = phy->p2 & 0xff;
@@ -2066,7 +2117,13 @@ x186b0:
 x186c0:
 	r1 = 0x01338025;
 	r2 = r4;
-	x17010();
+	r4 = 0x80080000;
+	write32(r4+160, r1);
+	r1 = read32(r4+164);
+	r3 = ~r3;
+	r1 &= r3;
+	r1 |= r2;
+	write32(r4+164, r1);
 x186d0:
 	r12 = 0;
 x186d4:
@@ -2081,8 +2138,13 @@ x186d4:
 		goto x18704;
 	r1 = read32(r3);
 	r1 += 21;
-	x17034();
-
+	r3 = 0x80080000;
+	write32(r3+160, r1);
+back1:
+	r1 = read32(r3+164);
+	r1 = r2 & r1;
+	if (r1 != r2)
+		goto back1;
 x18704:
 	r1 = (r12 > 4);
 	if (r1 == 0)
@@ -2126,7 +2188,11 @@ x18768:
 x18770:
 	r1 = 0x01318025;
 	r2 = ~r4;
-	x16ff4();
+	r3 = 0x80080000;
+	write32(r3+160, r1);
+	r1 = read32(r3+164);
+	r1 &= r2;
+	write32(r3+164, r1);
 
 x18780:
 	r1 = (phy->p1 & 0xff000000) >> 24;
@@ -2146,7 +2212,11 @@ x1879c:
 x187a8:
 	r1 = 0x01328025;
 	r2 = ~r4;
-	x16ff4();
+	r3 = 0x80080000;
+	write32(r3+160, r1);
+	r1 = read32(r3+164);
+	r1 &= r2;
+	write32(r3+164, r1);
 
 x187b8:
 	r1 = phy->p2 & 0xff;
@@ -2167,7 +2237,11 @@ x187d4:
 x187e0:
 	r1 = 0x01338025;
 	r2 = ~r4;
-	x16ff4();
+	r3 = 0x80080000;
+	write32(r3+160, r1);
+	r1 = read32(r3+164);
+	r1 &= r2;
+	write32(r3+164, r1);
 
 x187f0:
 	r12 = 0;
@@ -2185,7 +2259,8 @@ x187f4:
 	r3 = read8(r3);
 	r1 = 0xc00b;
 	r1 = r4 + r1;
-	x18420();
+	x18420(r1, r2, r3);
+
 
 x18830:
 	r1 = (r12 > 4);
@@ -2210,7 +2285,7 @@ x18844:
 	r3 = read8(r3);
 	r1 = 0xc00b;
 	r1 = r4 + r1;
-	x18420();
+	x18420(r1, r2, r3);
 
 x18880:
 	r1 = (r12 > 4);
@@ -2255,7 +2330,11 @@ x188e4:
 x188ec:
 	r1 = 0x01318025;
 	r2 = r4;
-	x16fd8();
+	r3 = 0x80080000;
+	write32(r3+160, r1);
+	r1 = read32(r3+164);
+	r1 |= r2;
+	write32(r3+164, r1);
 
 x188fc:
 	r1 = (phy->p1 & 0xff000000) >> 24;
@@ -2275,7 +2354,11 @@ x18918:
 x18924:
 	r1 = 0x01328025;
 	r2 = r4;
-	x16fd8();
+	r3 = 0x80080000;
+	write32(r3+160, r1);
+	r1 = read32(r3+164);
+	r1 |= r2;
+	write32(r3+164, r1);
 
 x18934:
 	r1 = phy->p2 & 0xff;
@@ -2295,16 +2378,24 @@ x18950:
 x1895c:
 	r1 = 0x01338025;
 	r2 = r4;
-	x16fd8();
+	r3 = 0x80080000;
+	write32(r3+160, r1);
+	r1 = read32(r3+164);
+	r1 |= r2;
+	write32(r3+164, r1);
 
 x1896c:
 	r1 = phy->p1 & 0xff;
-	if (r1 = 0)
+	if (r1 == 0)
 		goto x18988;
 	r2 = read8(r13);
 	r1 = 0x01308023;
 	r2 = ~r2;
-	x16ff4();
+	r3 = 0x80080000;
+	write32(r3+160, r1);
+	r1 = read32(r3+164);
+	r1 &= r2;
+	write32(r3+164, r1);
 
 x18988:
 	r5 = (phy->p1 & 0xff00) >> 8;
@@ -2345,7 +2436,11 @@ x189e4:
 x189ec:
 	r1 = 0x01318025;
 	r2 = r4;
-	x16fd8();
+	r3 = 0x80080000;
+	write32(r3+160, r1);
+	r1 = read32(r3+164);
+	r1 |= r2;
+	write32(r3+164, r1);
 
 x189fc:
 	r1 = (phy->p1 & 0xff000000) >> 24;
@@ -2365,7 +2460,11 @@ x18a18:
 x18a24:
 	r1 = 0x01328025;
 	r2 = r4;
-	x16fd8();
+	r3 = 0x80080000;
+	write32(r3+160, r1);
+	r1 = read32(r3+164);
+	r1 |= r2;
+	write32(r3+164, r1);
 
 x18a34:
 	r1 = phy->p2 & 0xff;
@@ -2385,7 +2484,11 @@ x18a50:
 x18a5c:
 	r1 = 0x01338025;
 	r2 = r4;
-	x16fd8();
+	r3 = 0x80080000;
+	write32(r3+160, r1);
+	r1 = read32(r3+164);
+	r1 |= r2;
+	write32(r3+164, r1);
 
 x18a6c:
 	r12 = 0;
@@ -2401,7 +2504,13 @@ x18a70:
 		goto x18aa0;
 	r1 = read32(r3);
 	r1 += 21;
-	x17034();
+	r3 = 0x80080000;
+	write32(r3+160, r1);
+back2:
+	r1 = read32(r3+164);
+	r1 = r2 & r1;
+	if (r1 != r2)
+		goto back2;
 
 x18aa0:
 	r1 = (r12 > 4);
@@ -2423,11 +2532,15 @@ x18aac:
 	r11 = read32(r11);
 	r2 = read32(r13);
 	r1 = r11 + 11;
-	x16fb0();
+	r3 = 0x80080000;
+	write32(r3+160, r1);
+	write32(r3+164, r2);
 
 	r2 = read32(r13+20);
 	r1 = r11 + 12;
-	x16fb0();
+	r3 = 0x80080000;
+	write32(r3+160, r1);
+	write32(r3+164, r2);
 
 x18af4:
 	r1 = (r12 > 4);