[NOT TESTED] set_usb_overcurrent

2 files changed, 63 insertions, 84 deletions

diff --git a/src/northbridge/intel/haswell/mrc_frags.c b/src/northbridge/intel/haswell/mrc_frags.c
index 10748c8f5a..d20058b595 100644
--- a/src/northbridge/intel/haswell/mrc_frags.c
+++ b/src/northbridge/intel/haswell/mrc_frags.c
@@ -590,3 +590,62 @@ void frag_usb_fffaeb10(PEI_USB *upd, void *ebar)
 		}
 	}
 }
+
+/* from loc_fffaf684 to loc_fffaf75b */
+void set_usb_overcurrent(PEI_USB *upd);
+void set_usb_overcurrent(PEI_USB *upd)
+{
+	device_t dev = PCI_DEV(0, 0x14, 0);
+
+	u32 u2ocm1 = 0;
+	u32 u2ocm2 = 0;
+	u32 u3ocm2 = 0;
+	u32 u3ocm1 = 0;
+
+	for (int i = 0; i < nb_usb2_ports(); i++) {
+		u32 oc = upd->ehci_oc[i];
+		if (oc == 8) continue;
+		if (i > 7) {
+			if (oc >= 4 && oc <= 7) {
+				u2ocm2 |= (1 << ((oc - 4) * 8 + (i - 8)));
+			}
+		} else {
+			if (oc <= 3) {
+				u2ocm1 |= (1 << (oc * 8 + i));
+			}
+		}
+	}
+
+	int nb_xxx;
+	int sku = mrc_sku_type();
+	if (sku == 1)
+		nb_xxx = 6;
+	else if (sku == 2)
+		nb_xxx = 4;
+	else
+		nb_xxx = 0;
+
+	for (int i = 0; i < nb_xxx; i++) {
+		u32 oc = upd->xhci_oc[i];
+		if (oc == 8) continue;
+		if (oc <= 3) {
+			u3ocm1 |= (1 << (oc * 8 + i));
+		} else {
+			u3ocm2 |= (1 << ((oc - 4) * 8 + i));
+		}
+	}
+
+	/* U2OCM1: XHCI USB2 Overcurrent Mapping Register1
+	 * OC 0~3 */
+	pci_write_config32(dev, 0xc0, u2ocm1);
+	/* U3OCM1: XHCI USB3 Overcurrent Mapping Register1
+	 * OC 0~3 */
+	pci_write_config32(dev, 0xc8, u3ocm1);
+
+	if (sku == 1) {
+		/* U2OCM2: OC 4~7 */
+		pci_write_config32(dev, 0xc4, u2ocm2);
+		/* U3OCM2: OC 4~7 */
+		pci_write_config32(dev, 0xcc, u3ocm2);
+	}
+}
diff --git a/src/northbridge/intel/haswell/pei_usb.asm b/src/northbridge/intel/haswell/pei_usb.asm
index 097b2e0ba1..975c600532 100644
--- a/src/northbridge/intel/haswell/pei_usb.asm
+++ b/src/northbridge/intel/haswell/pei_usb.asm
@@ -13,6 +13,7 @@ extern xhci_setup_ss_route
 extern frag_usb_fffaecbe
 extern frag_usb_fffaed46
 extern frag_usb_fffaeb10
+extern set_usb_overcurrent
 
 mrc_init_usb:
 push ebp
@@ -739,91 +740,10 @@ test byte [eax + 0x57], 3
 jne short loc_fffaf6c2  ; jne 0xfffaf6c2
 jmp near loc_fffaf75b  ; jmp 0xfffaf75b
 
-loc_fffaf684:
-mov edx, dword [ebp - 0x30]
-mov eax, dword [edx + ebx*4 + 0x6c]
-cmp eax, 8
-je short loc_fffaf6bf  ; je 0xfffaf6bf
-cmp ebx, 7
-ja short loc_fffaf6a9  ; ja 0xfffaf6a9
-cmp eax, 3
-ja short loc_fffaf6bf  ; ja 0xfffaf6bf
-lea ecx, [ebx + eax*8]
-mov eax, 1
-shl eax, cl
-or dword [ebp - 0x34], eax
-jmp short loc_fffaf6bf  ; jmp 0xfffaf6bf
-
-loc_fffaf6a9:
-lea edx, [eax - 4]
-cmp edx, 3
-ja short loc_fffaf6bf  ; ja 0xfffaf6bf
-lea ecx, [ebx + eax*8 - 0x28]
-mov eax, 1
-shl eax, cl
-or dword [ebp - 0x38], eax
-
-loc_fffaf6bf:
-inc ebx
-jmp short loc_fffaf6d2  ; jmp 0xfffaf6d2
-
 loc_fffaf6c2:
-mov dword [ebp - 0x38], 0
-mov dword [ebp - 0x34], 0
-xor ebx, ebx
-
-loc_fffaf6d2:
-call nb_usb2_ports
-movzx eax, al
-cmp ebx, eax
-jb short loc_fffaf684  ; jb 0xfffaf684
-xor esi, esi
-mov dword [ebp - 0x2c], 0
-xor ebx, ebx
-jmp short loc_fffaf71c  ; jmp 0xfffaf71c
-
-loc_fffaf6eb:
-mov ecx, dword [ebp - 0x30]
-mov eax, dword [ecx + ebx*4 + 0xa4]
-cmp eax, 8
-je short loc_fffaf71b  ; je 0xfffaf71b
-cmp eax, 3
-ja short loc_fffaf70e  ; ja 0xfffaf70e
-lea ecx, [ebx + eax*8]
-mov eax, 1
-shl eax, cl
-or dword [ebp - 0x2c], eax
-jmp short loc_fffaf71b  ; jmp 0xfffaf71b
-
-loc_fffaf70e:
-lea ecx, [ebx + eax*8 - 0x20]
-mov eax, 1
-shl eax, cl
-or esi, eax
-
-loc_fffaf71b:
-inc ebx
-
-loc_fffaf71c:
-call mrc_sku_type
-dec eax
-xor edx, edx
-cmp eax, 1
-ja short loc_fffaf730  ; ja 0xfffaf730
-movzx edx, byte [eax + ref_fffcb9dc]  ; movzx edx, byte [eax - 0x34624]
-
-loc_fffaf730:
-cmp ebx, edx
-jb short loc_fffaf6eb  ; jb 0xfffaf6eb
-mov edx, dword [ebp - 0x34]
-mov dword [edi + 0xa00c0], edx
-mov edx, dword [ebp - 0x2c]
-mov dword [edi + 0xa00c8], edx
-cmp dword [ebp - 0x3c], 1
-jne short loc_fffaf75b  ; jne 0xfffaf75b
-mov edx, dword [ebp - 0x38]
-mov dword [edi + 0xa00c4], edx
-mov dword [edi + 0xa00cc], esi
+push dword [ebp - 0x30]
+call set_usb_overcurrent
+add esp, 4
 
 loc_fffaf75b:
 mov esi, dword [ebp - 0x30]