libpdbg/cfam.c: Enable hmfsi ports

Errors on the hmfsi port can cause them to be disabled. So rather than relying
on the host to enable the corresponding FSI ports enable them during probing of
the downstream FSI link.

Signed-off-by: Alistair Popple <alistair@popple.id.au>

1 files changed, 11 insertions, 1 deletions

diff --git a/libpdbg/cfam.c b/libpdbg/cfam.c
index 69bab80..54eb4b5 100644
--- a/libpdbg/cfam.c
+++ b/libpdbg/cfam.c
@@ -305,9 +305,19 @@ static int cfam_hmfsi_write(struct fsi *fsi, uint32_t addr, uint32_t data)
 static int cfam_hmfsi_probe(struct target *target)
 {
 	struct fsi *fsi = target_to_fsi(target);
-	uint32_t value;
+	struct target *fsi_parent = target->dn->parent->target;
+	uint32_t value, port;
 	int rc;
 
+	/* Enable the port in the upstream control register */
+	port = dt_prop_get_u32(target->dn, "port");
+	fsi_read(fsi_parent, 0x3404, &value);
+	value |= 1 << (31 - port);
+	if ((rc = fsi_write(fsi_parent, 0x3404, value))) {
+		PR_ERROR("Unale to enable HMFSI port %d\n", port);
+		return rc;
+	}
+
 	if ((rc = fsi_read(&fsi->target, 0xc09, &value)))
 		return rc;