1 files changed, 144 insertions, 0 deletions
diff --git a/drivers/pinctrl/devicetree.c b/drivers/pinctrl/devicetree.c
index 54dad89fc9bf..260908480075 100644
--- a/drivers/pinctrl/devicetree.c
+++ b/drivers/pinctrl/devicetree.c
@@ -253,3 +253,147 @@ err:
 	pinctrl_dt_free_maps(p);
 	return ret;
 }
+
+/*
+ * For pinctrl binding, typically #pinctrl-cells is for the pin controller
+ * device, so either parent or grandparent. See pinctrl-bindings.txt.
+ */
+static int pinctrl_find_cells_size(const struct device_node *np)
+{
+	const char *cells_name = "#pinctrl-cells";
+	int cells_size, error;
+
+	error = of_property_read_u32(np->parent, cells_name, &cells_size);
+	if (error) {
+		error = of_property_read_u32(np->parent->parent,
+					     cells_name, &cells_size);
+		if (error)
+			return -ENOENT;
+	}
+
+	return cells_size;
+}
+
+/**
+ * pinctrl_get_list_and_count - Gets the list and it's cell size and number
+ * @np: pointer to device node with the property
+ * @list_name: property that contains the list
+ * @list: pointer for the list found
+ * @cells_size: pointer for the cell size found
+ * @nr_elements: pointer for the number of elements found
+ *
+ * Typically np is a single pinctrl entry containing the list.
+ */
+static int pinctrl_get_list_and_count(const struct device_node *np,
+				      const char *list_name,
+				      const __be32 **list,
+				      int *cells_size,
+				      int *nr_elements)
+{
+	int size;
+
+	*cells_size = 0;
+	*nr_elements = 0;
+
+	*list = of_get_property(np, list_name, &size);
+	if (!*list)
+		return -ENOENT;
+
+	*cells_size = pinctrl_find_cells_size(np);
+	if (*cells_size < 0)
+		return -ENOENT;
+
+	/* First element is always the index within the pinctrl device */
+	*nr_elements = (size / sizeof(**list)) / (*cells_size + 1);
+
+	return 0;
+}
+
+/**
+ * pinctrl_count_index_with_args - Count number of elements in a pinctrl entry
+ * @np: pointer to device node with the property
+ * @list_name: property that contains the list
+ *
+ * Counts the number of elements in a pinctrl array consisting of an index
+ * within the controller and a number of u32 entries specified for each
+ * entry. Note that device_node is always for the parent pin controller device.
+ */
+int pinctrl_count_index_with_args(const struct device_node *np,
+				  const char *list_name)
+{
+	const __be32 *list;
+	int size, nr_cells, error;
+
+	error = pinctrl_get_list_and_count(np, list_name, &list,
+					   &nr_cells, &size);
+	if (error)
+		return error;
+
+	return size;
+}
+EXPORT_SYMBOL_GPL(pinctrl_count_index_with_args);
+
+/**
+ * pinctrl_copy_args - Populates of_phandle_args based on index
+ * @np: pointer to device node with the property
+ * @list: pointer to a list with the elements
+ * @index: entry within the list of elements
+ * @nr_cells: number of cells in the list
+ * @nr_elem: number of elements for each entry in the list
+ * @out_args: returned values
+ *
+ * Populates the of_phandle_args based on the index in the list.
+ */
+static int pinctrl_copy_args(const struct device_node *np,
+			     const __be32 *list,
+			     int index, int nr_cells, int nr_elem,
+			     struct of_phandle_args *out_args)
+{
+	int i;
+
+	memset(out_args, 0, sizeof(*out_args));
+	out_args->np = (struct device_node *)np;
+	out_args->args_count = nr_cells + 1;
+
+	if (index >= nr_elem)
+		return -EINVAL;
+
+	list += index * (nr_cells + 1);
+
+	for (i = 0; i < nr_cells + 1; i++)
+		out_args->args[i] = be32_to_cpup(list++);
+
+	return 0;
+}
+
+/**
+ * pinctrl_parse_index_with_args - Find a node pointed by index in a list
+ * @np: pointer to device node with the property
+ * @list_name: property that contains the list
+ * @index: index within the list
+ * @out_arts: entries in the list pointed by index
+ *
+ * Finds the selected element in a pinctrl array consisting of an index
+ * within the controller and a number of u32 entries specified for each
+ * entry. Note that device_node is always for the parent pin controller device.
+ */
+int pinctrl_parse_index_with_args(const struct device_node *np,
+				  const char *list_name, int index,
+				  struct of_phandle_args *out_args)
+{
+	const __be32 *list;
+	int nr_elem, nr_cells, error;
+
+	error = pinctrl_get_list_and_count(np, list_name, &list,
+					   &nr_cells, &nr_elem);
+	if (error || !nr_cells)
+		return error;
+
+	error = pinctrl_copy_args(np, list, index, nr_cells, nr_elem,
+				  out_args);
+	if (error)
+		return error;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(pinctrl_parse_index_with_args);