[NOT TESTED] frag_usb_fffaf7d8

2 files changed, 33 insertions, 69 deletions

diff --git a/src/northbridge/intel/haswell/mrc_frags.c b/src/northbridge/intel/haswell/mrc_frags.c
index 06f1e7f108..4e28fc3611 100644
--- a/src/northbridge/intel/haswell/mrc_frags.c
+++ b/src/northbridge/intel/haswell/mrc_frags.c
@@ -659,3 +659,32 @@ void frag_usb_fffaf75b(PEI_USB *upd)
 		pch_iobp_update((0xe50041 + i) << 8, 0xffff80ff, a1 | a2);
 	}
 }
+
+void frag_usb_fffaf7d8(PEI_USB *upd);
+void frag_usb_fffaf7d8(PEI_USB *upd)
+{
+	int sku = mrc_sku_type();
+
+	for (int i = 0; i < nb_usb2_ports(); i++) {
+		if (i <= 7 && (upd->xhci_resume_info[0] & 1)) {
+			if (upd->ehci_settings[i].enable & 1) {
+				pci_or_config8(PCI_DEV(0, 0x1d, 0), 0x64, (1 << i));
+			} else {
+				pci_update_config8(PCI_DEV(0, 0x1d, 0), 0x64, (~(1 << i)), 0);
+			}
+		}
+		/* low power PCH doesn't have 00:1a.0 */
+		if (sku != 1)
+			continue;
+
+		if (i > 0xd) continue;
+		if ((upd->xhci_resume_info[1] & 1) == 0)
+			continue;
+
+		if (upd->ehci_settings[i].enable & 1) {
+			pci_or_config8(PCI_DEV(0, 0x1a, 0), 0x64, 1 << (i - 8));
+		} else {
+			pci_update_config8(PCI_DEV(0, 0x1a, 0), 0x64, ~(1 << (i - 8)), 0);
+		}
+	}
+}
diff --git a/src/northbridge/intel/haswell/pei_usb.asm b/src/northbridge/intel/haswell/pei_usb.asm
index e54689e0b0..196c3b446b 100644
--- a/src/northbridge/intel/haswell/pei_usb.asm
+++ b/src/northbridge/intel/haswell/pei_usb.asm
@@ -15,6 +15,7 @@ extern frag_usb_fffaed46
 extern frag_usb_fffaeb10
 extern set_usb_overcurrent
 extern frag_usb_fffaf75b
+extern frag_usb_fffaf7d8
 
 mrc_init_usb:
 push ebp
@@ -765,77 +766,11 @@ in ax, dx
 mov word [ebp - 0x38], ax
 or eax, 2
 out dx, ax
-call mrc_sku_type
-mov esi, eax
-xor ebx, ebx
-jmp near loc_fffaf866  ; jmp 0xfffaf866
-
-loc_fffaf7d8:
-cmp ebx, 7
-ja short loc_fffaf823  ; ja 0xfffaf823
-mov ecx, dword [ebp - 0x30]
-test byte [ecx + 0x55], 1
-je short loc_fffaf823  ; je 0xfffaf823
-imul eax, ebx, 6
-test byte [ecx + eax + 1], 1
-jne short loc_fffaf806  ; jne 0xfffaf806
-mov edx, dword [ebp - 0x4c]
-mov cl, bl
-mov al, byte [edx + 0xe8064]
-mov edx, 1
-shl edx, cl
-or eax, edx
-jmp short loc_fffaf81a  ; jmp 0xfffaf81a
-
-loc_fffaf806:
-mov ecx, dword [ebp - 0x4c]
-mov edx, 0xfffffffe
-mov al, byte [ecx + 0xe8064]
-mov cl, bl
-rol edx, cl
-and eax, edx
-
-loc_fffaf81a:
-mov edx, dword [ebp - 0x4c]
-mov byte [edx + 0xe8064], al
-
-loc_fffaf823:
-cmp esi, 1
-jne short loc_fffaf865  ; jne 0xfffaf865
-cmp ebx, 0xd
-ja short loc_fffaf865  ; ja 0xfffaf865
-mov ecx, dword [ebp - 0x30]
-test byte [ecx + 0x56], 1
-je short loc_fffaf865  ; je 0xfffaf865
-imul eax, ebx, 6
-mov edx, dword [ebp - 0x50]
-test byte [ecx + eax + 1], 1
-mov al, byte [edx + 0x64]
-lea ecx, [ebx - 8]
-mov edx, esi
-jne short loc_fffaf857  ; jne 0xfffaf857
-shl edx, cl
-mov ecx, dword [ebp - 0x50]
-or eax, edx
-mov byte [ecx + 0x64], al
-jmp short loc_fffaf865  ; jmp 0xfffaf865
 
-loc_fffaf857:
-shl edx, cl
-mov ecx, edx
-not ecx
-and ecx, eax
-mov eax, dword [ebp - 0x50]
-mov byte [eax + 0x64], cl
-
-loc_fffaf865:
-inc ebx
+push dword [ebp - 0x30]
+call frag_usb_fffaf7d8
+add esp, 4
 
-loc_fffaf866:
-call nb_usb2_ports
-movzx eax, al
-cmp ebx, eax
-jb loc_fffaf7d8  ; jb 0xfffaf7d8
 call mrc_sku_type
 mov edx, dword [ebp - 0x30]
 mov dword [ebp - 0x2c], eax