[PATCH 12/18] dbtr: Move hardware trigger probing to a function

[Nicholas Piggin]

This makes the code a bit neater. HW trigger probing will be expanded
with subsequent changes.

Signed-off-by: Nicholas Piggin <npiggin at gmail.com>
---
 lib/sbi/sbi_dbtr.c | 89 +++++++++++++++++++++++++---------------------
 1 file changed, 48 insertions(+), 41 deletions(-)

diff --git a/lib/sbi/sbi_dbtr.c b/lib/sbi/sbi_dbtr.c
index 5735bdc6..cfcd728f 100644
--- a/lib/sbi/sbi_dbtr.c
+++ b/lib/sbi/sbi_dbtr.c
@@ -134,11 +134,57 @@ static inline void sbi_free_trigger(struct sbi_dbtr_trigger *trig)
 	hart_state->available_trigs++;
 }
 
-int sbi_dbtr_init(struct sbi_scratch *scratch, bool coldboot)
+static bool sbi_hw_trigger_probe(int i)
 {
 	struct sbi_trap_info trap = {0};
 	unsigned long tdata1;
 	unsigned long val;
+
+	csr_write_allowed(CSR_TSELECT, &trap, i);
+	if (trap.cause)
+		return false;
+
+	val = csr_read_allowed(CSR_TSELECT, &trap);
+	if (trap.cause)
+		return false;
+
+	/*
+	 * Read back tselect and check that it contains the
+	 * written value
+	 */
+	if (val != i)
+		return false;
+
+	val = csr_read_allowed(CSR_TINFO, &trap);
+	if (trap.cause) {
+		unsigned long type;
+
+		/*
+		 * If reading tinfo caused an exception, the
+		 * debugger must read tdata1 to discover the
+		 * type.
+		 */
+		tdata1 = csr_read_allowed(CSR_TDATA1, &trap);
+		if (trap.cause)
+			return false;
+
+		type = TDATA1_GET_TYPE(tdata1);
+		if (type == 0)
+			return false;
+
+		sbi_trigger_init(INDEX_TO_TRIGGER(i), BIT(type), i);
+	} else {
+		if (val == 1)
+			return false;
+
+		sbi_trigger_init(INDEX_TO_TRIGGER(i), val, i);
+	}
+
+	return true;
+}
+
+int sbi_dbtr_init(struct sbi_scratch *scratch, bool coldboot)
+{
 	int i;
 	struct sbi_dbtr_hart_triggers_state *hart_state = NULL;
 
@@ -168,47 +214,8 @@ int sbi_dbtr_init(struct sbi_scratch *scratch, bool coldboot)
 		goto _probed;
 
 	for (i = 0; i < RV_MAX_TRIGGERS; i++) {
-		csr_write_allowed(CSR_TSELECT, &trap, i);
-		if (trap.cause)
-			break;
-
-		val = csr_read_allowed(CSR_TSELECT, &trap);
-		if (trap.cause)
-			break;
-
-		/*
-		 * Read back tselect and check that it contains the
-		 * written value
-		 */
-		if (val != i)
-			break;
-
-		val = csr_read_allowed(CSR_TINFO, &trap);
-		if (trap.cause) {
-			/*
-			 * If reading tinfo caused an exception, the
-			 * debugger must read tdata1 to discover the
-			 * type.
-			 */
-			tdata1 = csr_read_allowed(CSR_TDATA1,
-						  &trap);
-			if (trap.cause)
-				break;
-
-			if (TDATA1_GET_TYPE(tdata1) == 0)
-				break;
-
-			sbi_trigger_init(INDEX_TO_TRIGGER(i),
-					 BIT(TDATA1_GET_TYPE(tdata1)),
-					 i);
+		if (sbi_hw_trigger_probe(i))
 			hart_state->total_trigs++;
-		} else {
-			if (val == 1)
-				break;
-
-			sbi_trigger_init(INDEX_TO_TRIGGER(i), val, i);
-			hart_state->total_trigs++;
-		}
 	}
 
 	hart_state->probed = 1;
-- 
2.51.0