set_usb_pdo

3 files changed, 84 insertions, 99 deletions

diff --git a/src/northbridge/intel/haswell/mrc.asm b/src/northbridge/intel/haswell/mrc.asm
index 29956c0fcf..fcc7e61778 100644
--- a/src/northbridge/intel/haswell/mrc.asm
+++ b/src/northbridge/intel/haswell/mrc.asm
@@ -128,7 +128,6 @@ global fcn_fffc7720
 
 extern mrc_init_usb
 global ref_fffcb998
-global ref_fffcb99c
 global ref_fffcc988
 
 ;; mrc_end_of_pei
@@ -47233,24 +47232,6 @@ dd 0x00000001
 ref_fffcb998:
 dd 0x00000102
 
-ref_fffcb99c:
-dd 0x00000000
-dd 0x00000001
-dd 0x00000002
-dd 0x00000003
-dd 0x00000008
-dd 0x00000009
-dd 0x0000000c
-dd 0x0000000d
-dd 0x00000004
-dd 0x00000005
-dd 0x00000006
-dd 0x00000007
-dd 0x0000000a
-dd 0x0000000b
-dd 0x0000000c
-dd 0x0000000d
-
 ref_fffcba38:
 dd loc_fffafaad
 dd loc_fffafc42
diff --git a/src/northbridge/intel/haswell/mrc_frags.c b/src/northbridge/intel/haswell/mrc_frags.c
index 1b37673275..2dc8e69236 100644
--- a/src/northbridge/intel/haswell/mrc_frags.c
+++ b/src/northbridge/intel/haswell/mrc_frags.c
@@ -897,3 +897,81 @@ void frag_usb_fffaf7d8(PEI_USB *upd)
 		}
 	}
 }
+
+static const u32 shift_tab[16] = {
+	0x00000000,
+	0x00000001,
+	0x00000002,
+	0x00000003,
+	0x00000008,
+	0x00000009,
+	0x0000000c,
+	0x0000000d,
+	0x00000004,
+	0x00000005,
+	0x00000006,
+	0x00000007,
+	0x0000000a,
+	0x0000000b,
+	0x0000000c,
+	0x0000000d
+};
+
+void set_usb_pdo(PEI_USB *upd, u8 ppiv);
+void set_usb_pdo(PEI_USB *upd, u8 ppiv)
+{
+	int sku = mrc_sku_type();
+	int nusb = nb_usb2_ports();
+
+	u32 usb2pdo = pci_read_config32(PCI_DEV(0, 0x14, 0), 0xe4) & 0x7fff;
+
+	for (int i = 0; i < nusb; i++) {
+		u32 shifts;
+		if (sku == 1) {
+			shifts = shift_tab[i];
+		} else {
+			shifts = i;
+		}
+		/* if usb2 port is enabled, prevents it from
+		 * reporting a device connection to the xHC,
+		 * otherwise allows the reporting
+		 */
+		if (upd->ehci_settings[i].enable & 1) {
+			usb2pdo |= (1 << shifts);
+		} else {
+			usb2pdo &= ~(1 << shifts);
+		}
+	}
+
+	u32 usb3pdo = pci_read_config32(PCI_DEV(0, 0x14, 0), 0xe8) & 0x3f;
+	int nb_xxx;
+	if (sku == 1)
+		nb_xxx = 6;
+	else if (sku == 2)
+		nb_xxx = 4;
+	else
+		nb_xxx = 0;
+
+	/* ppiv is 4
+	printk(BIOS_DEBUG, "%s: ppiv is %d.\n", __func__, ppiv);
+	*/
+
+	for (int i = 0; i < nb_xxx; i++) {
+		if (ppiv > 2) {
+			if ((upd->xhci_en[i] & 1) == 0) {
+				usb3pdo |= (1 << i);
+			} else {
+				usb3pdo &= ~(1 << i);
+			}
+		} else {
+			if ((upd->ehci_settings[i].enable & 1) == 0) {
+				usb3pdo |= (1 << i);
+			} else {
+				usb3pdo &= ~(1 << i);
+			}
+		}
+	}
+
+	pci_write_config32(PCI_DEV(0, 0x14, 0), 0xe4, usb2pdo);
+	pci_write_config32(PCI_DEV(0, 0x14, 0), 0xe8, usb3pdo);
+}
diff --git a/src/northbridge/intel/haswell/pei_usb.asm b/src/northbridge/intel/haswell/pei_usb.asm
index 58c2c38837..d5522993ab 100644
--- a/src/northbridge/intel/haswell/pei_usb.asm
+++ b/src/northbridge/intel/haswell/pei_usb.asm
@@ -22,6 +22,7 @@ extern frag_usb_loop4
 extern frag_usb_fffaf4b1
 extern frag_usb_fffaf555
 extern set_ehci_oc_map
+extern set_usb_pdo
 
 mrc_init_usb:
 push ebp
@@ -482,87 +483,12 @@ mov edx, dword [ebp - 0x30]
 mov dword [ebp - 0x2c], eax
 test byte [edx + 0x57], 3
 je loc_fffaf944  ; je 0xfffaf944
-mov esi, dword [edi + 0xa00e4]
-xor ebx, ebx
-and esi, 0x7fff
-jmp short loc_fffaf8cc  ; jmp 0xfffaf8cc
-
-loc_fffaf89b:
-cmp dword [ebp - 0x2c], 1
-mov ecx, ebx
-jne short loc_fffaf8aa  ; jne 0xfffaf8aa
-mov ecx, dword [ebx*4 + ref_fffcb99c]  ; mov ecx, dword [ebx*4 - 0x34664]
-
-loc_fffaf8aa:
-imul eax, ebx, 6
-mov edx, dword [ebp - 0x30]
-test byte [edx + eax + 1], 1
-jne short loc_fffaf8c2  ; jne 0xfffaf8c2
-mov eax, 1
-shl eax, cl
-or esi, eax
-jmp short loc_fffaf8cb  ; jmp 0xfffaf8cb
-
-loc_fffaf8c2:
-mov eax, 0xfffffffe
-rol eax, cl
-and esi, eax
-
-loc_fffaf8cb:
-inc ebx
-
-loc_fffaf8cc:
-call nb_usb2_ports
-movzx eax, al
-cmp ebx, eax
-jb short loc_fffaf89b  ; jb 0xfffaf89b
-mov ebx, dword [edi + 0xa00e8]
-xor ecx, ecx
-and ebx, 0x3f
-jmp short loc_fffaf91a  ; jmp 0xfffaf91a
-
-loc_fffaf8e5:
-cmp byte [ebp - 0x67], 2
-jbe short loc_fffaf8f8  ; jbe 0xfffaf8f8
-mov eax, dword [ebp - 0x30]
-test byte [eax + ecx + 0xbd], 1
-jmp short loc_fffaf903  ; jmp 0xfffaf903
-
-loc_fffaf8f8:
-imul eax, ecx, 6
-mov edx, dword [ebp - 0x30]
-test byte [edx + eax + 1], 1
-
-loc_fffaf903:
-jne short loc_fffaf910  ; jne 0xfffaf910
-mov eax, 1
-shl eax, cl
-or ebx, eax
-jmp short loc_fffaf919  ; jmp 0xfffaf919
-
-loc_fffaf910:
-mov eax, 0xfffffffe
-rol eax, cl
-and ebx, eax
 
-loc_fffaf919:
-inc ecx
-
-loc_fffaf91a:
-mov dword [ebp - 0x70], ecx
-call mrc_sku_type
-dec eax
-xor edx, edx
-cmp eax, 1
-mov ecx, dword [ebp - 0x70]
-ja short loc_fffaf934  ; ja 0xfffaf934
-movzx edx, byte [eax + ref_fffcb9dc]  ; movzx edx, byte [eax - 0x34624]
-
-loc_fffaf934:
-cmp ecx, edx
-jb short loc_fffaf8e5  ; jb 0xfffaf8e5
-mov dword [edi + 0xa00e4], esi
-mov dword [edi + 0xa00e8], ebx
+movzx eax, byte [ebp - 0x67]
+push eax
+push edx
+call set_usb_pdo
+add esp, 8
 
 loc_fffaf944:
 mov eax, dword [ebp - 0x38]