2 files changed, 28 insertions, 0 deletions
diff --git a/tests/libqos/ahci.c b/tests/libqos/ahci.c
index 5164d42450..7ea55f90b7 100644
--- a/tests/libqos/ahci.c
+++ b/tests/libqos/ahci.c
@@ -267,3 +267,30 @@ void ahci_hba_enable(AHCIQState *ahci)
      * In the future, a small test-case to inspect the Register D2H FIS
      * and clear the initial interrupts might be good. */
 }
+
+/**
+ * Pick the first implemented and running port
+ */
+unsigned ahci_port_select(AHCIQState *ahci)
+{
+    uint32_t ports, reg;
+    unsigned i;
+
+    ports = ahci_rreg(ahci, AHCI_PI);
+    for (i = 0; i < 32; ports >>= 1, ++i) {
+        if (ports == 0) {
+            i = 32;
+        }
+
+        if (!(ports & 0x01)) {
+            continue;
+        }
+
+        reg = ahci_px_rreg(ahci, i, AHCI_PX_CMD);
+        if (BITSET(reg, AHCI_PX_CMD_ST)) {
+            break;
+        }
+    }
+    g_assert(i < 32);
+    return i;
+}
diff --git a/tests/libqos/ahci.h b/tests/libqos/ahci.h
index 77f205590e..b3992e19e5 100644
--- a/tests/libqos/ahci.h
+++ b/tests/libqos/ahci.h
@@ -431,5 +431,6 @@ void free_ahci_device(QPCIDevice *dev);
 void ahci_pci_enable(AHCIQState *ahci);
 void start_ahci_device(AHCIQState *ahci);
 void ahci_hba_enable(AHCIQState *ahci);
+unsigned ahci_port_select(AHCIQState *ahci);
 
 #endif