2 files changed, 34 insertions, 79 deletions

diff --git a/src/northbridge/intel/haswell/me_uma.asm b/src/northbridge/intel/haswell/me_uma.asm
index a21c1b210a..1829974b9a 100644
--- a/src/northbridge/intel/haswell/me_uma.asm
+++ b/src/northbridge/intel/haswell/me_uma.asm
@@ -5,6 +5,7 @@ extern gEfiPeiStallPpiGuid
 extern mrc_sku_type
 extern gWdtPpiGuid
 extern get_uma_size
+extern fcn_fffbe070
 
 global fcn_fffbdf70
 
@@ -122,85 +123,6 @@ pop esi
 pop ebp
 ret
 
-fcn_fffbe070:
-push ebp
-mov ebp, esp
-push edi
-push esi
-lea edx, [ebp - 0x1c]
-push ebx
-lea esp, [esp - 0x38]
-mov ebx, dword [ebp + 8]
-mov esi, dword [0xf00b0048]
-mov eax, dword [ebx]
-push edx
-push 0
-push 0
-push gEfiPeiStallPpiGuid
-push ebx
-call dword [eax + 0x20]  ; ucall
-mov edi, eax
-add esp, 0x20
-xor edx, edx
-jmp short loc_fffbe0bd  ; jmp 0xfffbe0bd
-
-loc_fffbe09f:  ; not directly referenced
-push ecx
-mov eax, dword [ebp - 0x1c]
-mov dword [ebp - 0x2c], edx
-push 0x3e8
-push eax
-push ebx
-call dword [eax + 4]  ; ucall
-mov edx, dword [ebp - 0x2c]
-mov esi, dword [0xf00b0048]
-inc edx
-add esp, 0x10
-
-loc_fffbe0bd:  ; not directly referenced
-test esi, 0x100
-jne short loc_fffbe0ff  ; jne 0xfffbe0ff
-cmp edx, 0x32
-jne short loc_fffbe09f  ; jne 0xfffbe09f
-jmp short loc_fffbe0f7  ; jmp 0xfffbe0f7
-
-loc_fffbe0cc:  ; not directly referenced
-mov eax, esi
-test al, al
-jns short loc_fffbe0ee  ; jns 0xfffbe0ee
-mov ax, word [0xf00f80a2]
-and al, 0x7f
-mov word [0xf00f80a2], ax
-mov edx, 1
-mov eax, ebx
-call fcn_fffbdf70  ; call 0xfffbdf70
-mov edi, eax
-
-loc_fffbe0ee:  ; not directly referenced
-mov eax, esi
-and eax, 0xffffff90
-cmp al, 0x10
-jne short loc_fffbe106  ; jne 0xfffbe106
-
-loc_fffbe0f7:  ; not directly referenced
-mov eax, dword [ebp + 0x10]
-mov byte [eax], 1
-jmp short loc_fffbe106  ; jmp 0xfffbe106
-
-loc_fffbe0ff:  ; not directly referenced
-cmp edx, 0x32
-jne short loc_fffbe0cc  ; jne 0xfffbe0cc
-jmp short loc_fffbe0f7  ; jmp 0xfffbe0f7
-
-loc_fffbe106:  ; not directly referenced
-lea esp, [ebp - 0xc]
-mov eax, edi
-pop ebx
-pop esi
-pop edi
-pop ebp
-ret
-
 fcn_fffbe110:  ; not directly referenced
 push ebp
 mov ebp, esp
diff --git a/src/northbridge/intel/haswell/me_uma.c b/src/northbridge/intel/haswell/me_uma.c
index 7093e71f8d..5a23ff714c 100644
--- a/src/northbridge/intel/haswell/me_uma.c
+++ b/src/northbridge/intel/haswell/me_uma.c
@@ -38,3 +38,36 @@ u32 get_uma_size(EFI_PEI_SERVICES **pps, void *me)
 		return 0;
 	}
 }
+
+int __attribute((regparm(2)))
+fcn_fffbdf70(EFI_PEI_SERVICES **pps, int v);
+
+int fcn_fffbe070(EFI_PEI_SERVICES **pps, void *me, u8 *a2);
+int fcn_fffbe070(EFI_PEI_SERVICES **pps, void *me, u8 *a2)
+{
+	int i = 0;
+	int ret = 0;
+
+	u32 gmes = pci_read_config32(PCH_ME_DEV, 0x48);
+
+	while ((gmes & 0x100) == 0) {
+		if (i == 50) {
+			*a2 = 1;
+			return ret;
+		}
+		usleep(1000);
+		i++;
+		gmes = pci_read_config32(PCH_ME_DEV, 0x48);
+	}
+	if (i != 50) {
+		if (gmes & 0x80) {
+			/* clear GEN_PMCON_2 DRAM initialization bit */
+			pci_update_config16(PCH_LPC_DEV, 0xa2, 0xff7f, 0);
+			ret = fcn_fffbdf70(pps, 1);
+		}
+		if ((gmes & 0x90) != 0x10)
+			return ret;
+	}
+	*a2 = 1;
+	return ret;
+}