fcn_fffbe14d

2 files changed, 40 insertions, 96 deletions

diff --git a/src/northbridge/intel/haswell/me_uma.asm b/src/northbridge/intel/haswell/me_uma.asm
index 1829974b9a..dbdc05ab96 100644
--- a/src/northbridge/intel/haswell/me_uma.asm
+++ b/src/northbridge/intel/haswell/me_uma.asm
@@ -6,7 +6,9 @@ extern mrc_sku_type
 extern gWdtPpiGuid
 extern get_uma_size
 extern fcn_fffbe070
+extern fcn_fffbe14d
 
+global fcn_fffbe110
 global fcn_fffbdf70
 
 fcn_fffbdf70:  ; not directly referenced
@@ -154,102 +156,6 @@ xor eax, eax
 leave
 ret
 
-fcn_fffbe14d:  ; not directly referenced
-push ebp
-mov ebp, esp
-push esi
-push ebx
-lea edx, [ebp - 0xc]
-lea esp, [esp - 0x2c]
-mov ebx, dword [ebp + 8]
-mov cl, byte [ebp + 0x14]
-mov eax, dword [ebx]
-mov byte [ebp - 0x1c], cl
-push edx
-push 0
-push 0
-push gEfiPeiStallPpiGuid
-push ebx
-call dword [eax + 0x20]  ; ucall
-mov eax, dword [0xf00b0040]
-add esp, 0x20
-mov edx, eax
-mov cl, byte [ebp - 0x1c]
-shr edx, 0x10
-and edx, 0xf
-cmp dl, 2
-je loc_fffbe22f  ; je 0xfffbe22f
-movzx edx, ah
-xor eax, eax
-and dl, 0xf0
-jne loc_fffbe231  ; jne 0xfffbe231
-mov edx, dword [0xf0000070]
-mov eax, dword [0xf0000074]
-shl eax, 0x1c
-shr edx, 4
-add edx, eax
-shr edx, 0x10
-mov eax, edx
-or edx, 0x10800000
-or eax, 0x10000000
-test cl, cl
-cmovs eax, edx
-mov edx, ecx
-and edx, 0x7f
-shl edx, 0x18
-or eax, edx
-mov edx, dword [0xf00b004c]
-mov dword [0xf00b004c], eax
-push esi
-mov eax, dword [ebp - 0xc]
-mov esi, 0x1389
-push 0x44c
-push eax
-push ebx
-call dword [eax + 4]  ; ucall
-mov eax, dword [0xf00b0040]
-add esp, 0x10
-jmp short loc_fffbe20c  ; jmp 0xfffbe20c
-
-loc_fffbe1f6:  ; not directly referenced
-push ecx
-mov eax, dword [ebp - 0xc]
-push 0x3e8
-push eax
-push ebx
-call dword [eax + 4]  ; ucall
-mov eax, dword [0xf00b0040]
-add esp, 0x10
-
-loc_fffbe20c:  ; not directly referenced
-mov edx, eax
-shr edx, 0x18
-and dl, 0xf0
-jne short loc_fffbe219  ; jne 0xfffbe219
-dec esi
-jne short loc_fffbe1f6  ; jne 0xfffbe1f6
-
-loc_fffbe219:  ; not directly referenced
-shr eax, 0x19
-push edx
-and eax, 7
-push eax
-push dword [ebp + 0x10]
-push ebx
-call fcn_fffbe110  ; call 0xfffbe110
-add esp, 0x10
-jmp short loc_fffbe231  ; jmp 0xfffbe231
-
-loc_fffbe22f:  ; not directly referenced
-xor eax, eax
-
-loc_fffbe231:  ; not directly referenced
-lea esp, [ebp - 8]
-pop ebx
-pop esi
-pop ebp
-ret
-
 PchMeUmaDesc:
 dd 0x80000010
 dd gPchMeUmaPpiGuid
diff --git a/src/northbridge/intel/haswell/me_uma.c b/src/northbridge/intel/haswell/me_uma.c
index 5a23ff714c..0528e5d4be 100644
--- a/src/northbridge/intel/haswell/me_uma.c
+++ b/src/northbridge/intel/haswell/me_uma.c
@@ -71,3 +71,41 @@ int fcn_fffbe070(EFI_PEI_SERVICES **pps, void *me, u8 *a2)
 	*a2 = 1;
 	return ret;
 }
+
+int fcn_fffbe110(EFI_PEI_SERVICES **pps, u32, u32);
+
+int fcn_fffbe14d(EFI_PEI_SERVICES **pps, void *me, int a3, u32 a4);
+int fcn_fffbe14d(EFI_PEI_SERVICES **pps, void *me, int a3, u32 a4)
+{
+	u32 hfs = pci_read_config32(PCH_ME_DEV, 0x40);
+
+	if (((hfs >> 16) & 0xf) == 2)
+		return 0;
+
+	if (((hfs >> 8) & 0xf0) != 0)
+		return 0;
+
+	u32 meseg_lo = pci_read_config32(PCI_DEV(0, 0, 0), 0x70);
+	u32 meseg_hi = pci_read_config32(PCI_DEV(0, 0, 0), 0x74);
+	/* from intel_early_me_init_done in sb/intel/lynxpoint/early_me.c */
+	/* low 4 bits of meseg_hi << 12, meseg_lo >> 20 */
+	u32 uma_base = ((meseg_hi << 28) | (meseg_lo >> 4)) >> 16;
+	u32 meDID = uma_base | (1 << 28);
+	if (a4 & 0x80)
+		meDID |= (1 << 23);
+	meDID |= ((a4 & 0x7f) << 24);
+	pci_read_config32(PCH_ME_DEV, 0x4c);
+	pci_write_config32(PCH_ME_DEV, 0x4c, meDID);
+
+	usleep(1100);
+	pci_read_config32(PCH_ME_DEV, 0x40);
+
+	for (int i = 0; i < 5001; i++) {
+		usleep(1000);
+		hfs = pci_read_config32(PCH_ME_DEV, 0x40);
+		if (((hfs >> 24) & 0xf0) != 0) {
+			break;
+		}
+	}
+	return fcn_fffbe110(pps, a3, ((hfs >> 25) & 7));
+}