From 8040911a25d7459b5c007eb35c65cb4abe05eaaa Mon Sep 17 00:00:00 2001
From: Marcus Overhagen <marcus@overhagen.de>
Date: Mon, 10 Sep 2012 22:32:20 +0200
Subject: [PATCH] Use implemented ports mask to check if maximum port count
 needs to be extended.

 * This should fix #8953
 * Also fix some harmless off-by-one errors
---
 .../kernel/busses/scsi/ahci/ahci_controller.cpp   | 15 ++++++++++++---
 src/add-ons/kernel/busses/scsi/ahci/util.cpp      | 14 ++++++++++++++
 src/add-ons/kernel/busses/scsi/ahci/util.h        |  2 ++
 3 files changed, 28 insertions(+), 3 deletions(-)

diff --git a/src/add-ons/kernel/busses/scsi/ahci/ahci_controller.cpp b/src/add-ons/kernel/busses/scsi/ahci/ahci_controller.cpp
index e7bc95affd..24ded54f21 100644
--- a/src/add-ons/kernel/busses/scsi/ahci/ahci_controller.cpp
+++ b/src/add-ons/kernel/busses/scsi/ahci/ahci_controller.cpp
@@ -131,12 +131,21 @@ AHCIController::Init()
 	fPortCountMax = 1 + ((fRegs->cap >> CAP_NP_SHIFT) & CAP_NP_MASK);
 
 	fPortImplementedMask = fRegs->pi;
+	// reported mask of implemented ports is sometimes empty
 	if (fPortImplementedMask == 0) {
 		fPortImplementedMask = 0xffffffff >> (32 - fPortCountMax);
 		TRACE("ports-implemented mask is zero, using 0x%" B_PRIx32 " instead.\n",
 			fPortImplementedMask);
 	}
 
+	// reported number of ports is sometimes too small
+	int maxPortIndex;
+	maxPortIndex = fls(fPortImplementedMask);
+	if (fPortCountMax < maxPortIndex) {
+		TRACE("reported number of ports is wrong, using %d instead.\n", maxPortIndex);
+		fPortCountMax = maxPortIndex;
+	}
+
 	fPortCountAvail = count_bits_set(fPortImplementedMask);
 
 	TRACE("cap: Interface Speed Support: generation %" B_PRIu32 "\n",	(fRegs->cap >> CAP_ISS_SHIFT) & CAP_ISS_MASK);
@@ -169,7 +178,7 @@ AHCIController::Init()
 		goto err;
 	}
 
-	for (int i = 0; i <= fPortCountMax; i++) {
+	for (int i = 0; i < fPortCountMax; i++) {
 		if (fPortImplementedMask & (1 << i)) {
 			fPort[i] = new (std::nothrow)AHCIPort(this, i);
 			if (!fPort[i]) {
@@ -189,7 +198,7 @@ AHCIController::Init()
 	fRegs->ghc |= GHC_IE;
 	FlushPostedWrites();
 
-	for (int i = 0; i <= fPortCountMax; i++) {
+	for (int i = 0; i < fPortCountMax; i++) {
 		if (fPort[i]) {
 			status_t status = fPort[i]->Init2();
 			if (status < B_OK) {
@@ -215,7 +224,7 @@ AHCIController::Uninit()
 {
 	TRACE("AHCIController::Uninit\n");
 
-	for (int i = 0; i <= fPortCountMax; i++) {
+	for (int i = 0; i < fPortCountMax; i++) {
 		if (fPort[i]) {
 			fPort[i]->Uninit();
 			delete fPort[i];
diff --git a/src/add-ons/kernel/busses/scsi/ahci/util.cpp b/src/add-ons/kernel/busses/scsi/ahci/util.cpp
index d630d9f814..ff58f823e8 100644
--- a/src/add-ons/kernel/busses/scsi/ahci/util.cpp
+++ b/src/add-ons/kernel/busses/scsi/ahci/util.cpp
@@ -121,3 +121,17 @@ swap_words(void *data, size_t size)
 		word++;
 	}
 }
+
+
+int
+fls(unsigned mask)
+{
+	if (mask == 0)
+		return 0;
+	int pos = 1;
+	while (mask != 1) {
+		mask >>= 1;
+		pos++;
+	}
+	return pos;
+}
diff --git a/src/add-ons/kernel/busses/scsi/ahci/util.h b/src/add-ons/kernel/busses/scsi/ahci/util.h
index 94fc1ac94c..75b4f9dd45 100644
--- a/src/add-ons/kernel/busses/scsi/ahci/util.h
+++ b/src/add-ons/kernel/busses/scsi/ahci/util.h
@@ -20,6 +20,8 @@ status_t sg_memcpy(const physical_entry *sgTable, int sgCount, const void *data,
 
 void swap_words(void *data, size_t size);
 
+int fls(unsigned mask);
+
 #ifdef __cplusplus
 }
 #endif