[PATCH 6.1 0864/1191] remoteproc: use rsc_table_for_each_entry() in rproc_handle_resources()

@@ -1014,60 +1014,55 @@ static rproc_handle_resource_t rproc_loading_handlers[RSC_LAST] = {
 	[RSC_VDEV] = rproc_handle_vdev,
 };
 
-/* handle firmware resource entries before booting the remote processor */
-static int rproc_handle_resources(struct rproc *rproc,
-				  rproc_handle_resource_t handlers[RSC_LAST])
+struct rproc_rsc_cb_data {
+	struct rproc *rproc;
+	rproc_handle_resource_t *handlers;
+};
+
+static int rproc_handle_rsc_entry(u32 type, void *rsc, int offset,
+				  int avail, void *data)
 {
+	struct rproc_rsc_cb_data *d = data;
+	struct rproc *rproc = d->rproc;
 	struct device *dev = &rproc->dev;
 	rproc_handle_resource_t handler;
-	int ret = 0, i;
-
-	if (!rproc->table_ptr)
-		return 0;
+	int ret;
 
-	for (i = 0; i < rproc->table_ptr->num; i++) {
-		int offset = rproc->table_ptr->offset[i];
-		struct fw_rsc_hdr *hdr = (void *)rproc->table_ptr + offset;
-		int avail = rproc->table_sz - offset - sizeof(*hdr);
-		void *rsc = (void *)hdr + sizeof(*hdr);
+	dev_dbg(dev, "rsc: type %d\n", type);
 
-		/* make sure table isn't truncated */
-		if (avail < 0) {
-			dev_err(dev, "rsc table is truncated\n");
-			return -EINVAL;
-		}
-
-		dev_dbg(dev, "rsc: type %d\n", hdr->type);
+	if (type >= RSC_VENDOR_START && type <= RSC_VENDOR_END) {
+		ret = rproc_handle_rsc(rproc, type, rsc, offset, avail);
+		if (ret == RSC_HANDLED)
+			return 0;
+		if (ret < 0)
+			return ret;
+		dev_warn(dev, "unsupported vendor resource %d\n", type);
+		return 0;
+	}
 
-		if (hdr->type >= RSC_VENDOR_START &&
-		    hdr->type <= RSC_VENDOR_END) {
-			ret = rproc_handle_rsc(rproc, hdr->type, rsc,
-					       offset + sizeof(*hdr), avail);
-			if (ret == RSC_HANDLED)
-				continue;
-			else if (ret < 0)
-				break;
+	if (type >= RSC_LAST) {
+		dev_warn(dev, "unsupported resource %d\n", type);
+		return 0;
+	}
 
-			dev_warn(dev, "unsupported vendor resource %d\n",
-				 hdr->type);
-			continue;
-		}
+	handler = d->handlers[type];
+	if (!handler)
+		return 0;
 
-		if (hdr->type >= RSC_LAST) {
-			dev_warn(dev, "unsupported resource %d\n", hdr->type);
-			continue;
-		}
+	return handler(rproc, rsc, offset, avail);
+}
 
-		handler = handlers[hdr->type];
-		if (!handler)
-			continue;
+/* handle firmware resource entries before booting the remote processor */
+static int rproc_handle_resources(struct rproc *rproc,
+				  rproc_handle_resource_t handlers[RSC_LAST])
+{
+	struct rproc_rsc_cb_data d = { .rproc = rproc, .handlers = handlers };
 
-		ret = handler(rproc, rsc, offset + sizeof(*hdr), avail);
-		if (ret)
-			break;
-	}
+	if (!rproc->table_ptr)
+		return 0;
 
-	return ret;
+	return rsc_table_for_each_entry(rproc->table_ptr, rproc->table_sz,
+					&rproc->dev, rproc_handle_rsc_entry, &d);
 }
 
 static int rproc_prepare_subdevices(struct rproc *rproc)

@@ -303,4 +303,57 @@ struct fw_rsc_vdev {
 	struct fw_rsc_vdev_vring vring[];
 } __packed;
 
+/**
+ * rsc_table_for_each_entry() - iterate over all entries in a resource table
+ * @table:    pointer to the resource table
+ * @table_sz: total size of the table buffer in bytes
+ * @dev:      device used for error logging
+ * @cb:       callback invoked for each entry:
+ *              @type   - value from enum fw_resource_type
+ *              @rsc    - pointer to the entry payload (past struct fw_rsc_hdr)
+ *              @offset - byte offset of the payload within the table; callers
+ *                        that write back into the table (e.g. to record a
+ *                        dynamically allocated address) use this to locate the
+ *                        entry for later update
+ *              @avail  - bytes available in the payload
+ *              @data   - caller-supplied private pointer
+ *            Return 0 to continue iteration, non-zero to stop.
+ * @data:     private pointer forwarded to @cb on every call
+ *
+ * Iterates over every resource entry in @table, performing the standard
+ * truncation check, and invokes @cb for each one. Iteration stops on the
+ * first non-zero return from @cb or on a malformed table.
+ *
+ * Returns 0 after a complete iteration, -EINVAL if the table is truncated,
+ * or the first non-zero value returned by @cb.
+ */
+static inline int rsc_table_for_each_entry(struct resource_table *table,
+					   size_t table_sz,
+					   struct device *dev,
+					   int (*cb)(u32 type, void *rsc,
+						     int offset, int avail,
+						     void *data),
+					   void *data) {
+	int i, ret;
+
+	for (i = 0; i < table->num; i++) {
+		int offset = table->offset[i];
+		struct fw_rsc_hdr *hdr = (void *)table + offset;
+		int avail = table_sz - offset - sizeof(*hdr);
+		int rsc_offset = offset + sizeof(*hdr);
+		void *rsc = (void *)hdr + sizeof(*hdr);
+
+		if (avail < 0) {
+			dev_err(dev, "rsc table is truncated\n");
+			return -EINVAL;
+		}
+
+		ret = cb(hdr->type, rsc, rsc_offset, avail, data);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
 #endif /* RSC_TABLE_H */