[PATCH 11/15] cxl/core: Add support for HDM-D cache id programming

[Ben Cheatham <Benjamin.Cheatham@amd.com>]

Add cache id allocation and programming to the pre-existing cache id
programming routines.

Signed-off-by: Ben Cheatham <Benjamin.Cheatham@amd.com>
---
 drivers/cxl/cache.c      |  6 -----
 drivers/cxl/core/cache.c | 52 +++++++++++++++++++++++++++++++++++-----
 drivers/cxl/cxl.h        |  3 +++
 3 files changed, 49 insertions(+), 12 deletions(-)

diff --git a/drivers/cxl/cache.c b/drivers/cxl/cache.c
index 0fc8f413c638..40d1e8330df7 100644
--- a/drivers/cxl/cache.c
+++ b/drivers/cxl/cache.c
@@ -156,12 +156,6 @@ static int program_cache_id(struct cxl_cachedev *cxlcd)
 	if (!rc)
 		return cxl_allocate_cache_id(cxlcd);
 
-	if (cxlcd->cxlds->hdmd) {
-		dev_err(dev,
-			"Cache id programming not supported for HDM-D devices\n");
-		return -ENXIO;
-	}
-
 	rc = cxl_allocate_cache_id(cxlcd);
 	if (rc)
 		return rc;
diff --git a/drivers/cxl/core/cache.c b/drivers/cxl/core/cache.c
index 9c6c8ea713ef..7810772326e9 100644
--- a/drivers/cxl/core/cache.c
+++ b/drivers/cxl/core/cache.c
@@ -204,7 +204,7 @@ static int cxl_cid_commit_decoder(struct cxl_dport *dport)
 }
 
 static int cxl_cid_program_decoder(struct cxl_dport *dport, int cid,
-				   bool endpoint)
+				   bool endpoint, bool hdmd)
 {
 	void __iomem *ciddc = dport->regs.ciddc;
 	u32 ctrl;
@@ -225,7 +225,14 @@ static int cxl_cid_program_decoder(struct cxl_dport *dport, int cid,
 		ctrl |= CXL_CACHE_ID_DC_CTRL_FWD_ID;
 	}
 
-	FIELD_MODIFY(CXL_CACHE_ID_DC_CTRL_LOCAL_ID, &ctrl, cid);
+	if (hdmd) {
+		ctrl |= CXL_CACHE_ID_DC_CTRL_HDMD_PRESENT;
+		FIELD_MODIFY(CXL_CACHE_ID_DC_CTRL_HDMD_ID, &ctrl, cid);
+	} else {
+		ctrl &= ~CXL_CACHE_ID_DC_CTRL_HDMD_PRESENT;
+		FIELD_MODIFY(CXL_CACHE_ID_DC_CTRL_LOCAL_ID, &ctrl, cid);
+	}
+
 	writel(ctrl, ciddc + CXL_CACHE_ID_DC_CTRL_OFFSET);
 
 	return cxl_cid_commit_decoder(dport);
@@ -269,10 +276,10 @@ static int cxl_cid_commit_table(struct cxl_port *port)
 }
 
 static int cxl_cid_program_table_entry(struct cxl_port *port, int cid,
-				       unsigned int dport_id)
+				       unsigned int dport_id, bool hdmd)
 {
 	void __iomem *cidrt = port->regs.cidrt;
-	u8 target_cnt, portn;
+	u8 target_cnt, hdmd_max, portn;
 	u16 target_n;
 	u32 cap;
 
@@ -290,6 +297,16 @@ static int cxl_cid_program_table_entry(struct cxl_port *port, int cid,
 		return -EINVAL;
 	}
 
+	if (is_cxl_root(parent_port_of(port)) && hdmd) {
+		hdmd_max = FIELD_GET(CXL_CACHE_ID_RT_CAP_HDMD_MAX, cap);
+
+		if (port->num_hdmd > hdmd_max) {
+			dev_err(&port->dev,
+				"Maximum number of devices using HDM-D reached\n");
+			return -EINVAL;
+		}
+	}
+
 	target_n = readw(cidrt + CXL_CACHE_ID_RT_TARGETN_OFFSET(cid));
 	if (FIELD_GET(CXL_CACHE_ID_RT_TARGETN_VALID, target_n)) {
 		portn = FIELD_GET(CXL_CACHE_ID_RT_TARGETN_PORTN, target_n);
@@ -385,6 +402,22 @@ static struct ida *find_cache_id_ida(struct cxl_port *port)
 	return NULL;
 }
 
+static void cxl_port_add_hdmd(struct cxl_port *endpoint, int val)
+{
+	struct cxl_port *parent = parent_port_of(endpoint);
+	struct cxl_port *port = endpoint;
+
+	if (!parent || !is_cxl_cachedev(endpoint->uport_dev))
+		return;
+
+	while (parent && !is_cxl_root(parent)) {
+		port = parent;
+		parent = parent_port_of(port);
+	}
+
+	port->num_hdmd += val;
+}
+
 int cxl_allocate_cache_id(struct cxl_cachedev *cxlcd)
 {
 	struct ida *ida = find_cache_id_ida(cxlcd->endpoint);
@@ -407,6 +440,9 @@ int cxl_allocate_cache_id(struct cxl_cachedev *cxlcd)
 	if (id < 0)
 		return id;
 
+	if (cxlcd->cxlds->hdmd)
+		cxl_port_add_hdmd(cxlcd->endpoint, 1);
+
 	cxlcd->cache_id = id;
 	return 0;
 }
@@ -418,6 +454,9 @@ void cxl_free_cache_id(struct cxl_cachedev *cxlcd)
 
 	if (ida)
 		ida_free(ida, cxlcd->cache_id);
+
+	if (cxlcd->cxlds->hdmd)
+		cxl_port_add_hdmd(cxlcd->endpoint, -1);
 }
 EXPORT_SYMBOL_FOR_MODULES(cxl_free_cache_id, "cxl_cache");
 
@@ -489,17 +528,18 @@ int cxl_cachedev_program_cache_id(struct cxl_cachedev *cxlcd)
 {
 	struct cxl_dport *dport = cxlcd->endpoint->parent_dport;
 	struct cxl_port *port = dport->port;
+	bool hdmd = cxlcd->cxlds->hdmd;
 	bool endpoint = true;
 	int rc;
 
 	while (!is_cxl_root(port)) {
 		rc = cxl_cid_program_decoder(dport, cxlcd->cache_id,
-					     endpoint);
+					     endpoint, hdmd);
 		if (rc)
 			goto err;
 
 		rc = cxl_cid_program_table_entry(port, cxlcd->cache_id,
-						 dport->port_id);
+						 dport->port_id, hdmd);
 		if (rc)
 			goto err;
 
diff --git a/drivers/cxl/cxl.h b/drivers/cxl/cxl.h
index 4cdc26dbdb6a..63efe98d1c89 100644
--- a/drivers/cxl/cxl.h
+++ b/drivers/cxl/cxl.h
@@ -229,6 +229,7 @@ static inline int ways_to_eiw(unsigned int ways, u8 *eiw)
 /* CXL 4.0 8.2.4.28.1 CXL Cache ID Route Table Capability Structure */
 #define CXL_CACHE_ID_RT_CAP_OFFSET 0x0
 #define   CXL_CACHE_ID_RT_CAP_TARGET_CNT GENMASK(4, 0)
+#define   CXL_CACHE_ID_RT_CAP_HDMD_MAX GENMASK(11, 8)
 #define   CXL_CACHE_ID_RT_CAP_COMMIT_REQ BIT(16)
 #define CXL_CACHE_ID_RT_CTRL_OFFSET 0x4
 #define   CXL_CACHE_ID_RT_CTRL_COMMIT BIT(0)
@@ -603,6 +604,7 @@ struct cxl_dax_region {
  * @pci_latency: Upstream latency in picoseconds
  * @component_reg_phys: Physical address of component register
  * @cache_ida: ida used for cache id allocations on host bridges
+ * @num_hdmd: Number of devices using HDM-D flows below this port
  */
 struct cxl_port {
 	struct device dev;
@@ -629,6 +631,7 @@ struct cxl_port {
 	long pci_latency;
 	resource_size_t component_reg_phys;
 	struct ida cache_ida;
+	u32 num_hdmd;
 };
 
 struct cxl_root;
-- 
2.53.0