diff --git a/drivers/pinctrl/pinctrl-imx.c b/drivers/pinctrl/pinctrl-imx.c
index 4cebb9c..8c6abc0 100644
--- a/drivers/pinctrl/pinctrl-imx.c
+++ b/drivers/pinctrl/pinctrl-imx.c
@@ -56,11 +56,15 @@ struct imx_pinctrl {
 
 static const struct imx_pin_reg *imx_find_pin_reg(
 				const struct imx_pinctrl_soc_info *info,
-				unsigned pin, bool is_mux, unsigned mux)
+				unsigned pin, bool is_mux, unsigned mux,
+				int pin_func_id)
 {
 	const struct imx_pin_reg *pin_reg = NULL;
 	int i;
 
+	if (pin_func_id > -1 && pin_func_id < info->npin_regs)
+		return info->pin_regs + pin_func_id;
+
 	for (i = 0; i < info->npin_regs; i++) {
 		pin_reg = &info->pin_regs[i];
 		if (pin_reg->pid != pin)
@@ -223,8 +227,10 @@ static int imx_pmx_enable(struct pinctrl_dev *pctldev, unsigned selector,
 	struct imx_pinctrl *ipctl = pinctrl_dev_get_drvdata(pctldev);
 	const struct imx_pinctrl_soc_info *info = ipctl->info;
 	const struct imx_pin_reg *pin_reg;
-	const unsigned *pins, *mux;
+	const unsigned int *pins, *mux;
+	int *pin_func_ids;
 	unsigned int npins, pin_id;
+	int pin_func_id;
 	int i;
 
 	/*
@@ -234,6 +240,7 @@ static int imx_pmx_enable(struct pinctrl_dev *pctldev, unsigned selector,
 	pins = info->groups[group].pins;
 	npins = info->groups[group].npins;
 	mux = info->groups[group].mux_mode;
+	pin_func_ids = info->groups[group].pin_func_ids;
 
 	WARN_ON(!pins || !npins || !mux);
 
@@ -242,8 +249,10 @@ static int imx_pmx_enable(struct pinctrl_dev *pctldev, unsigned selector,
 
 	for (i = 0; i < npins; i++) {
 		pin_id = pins[i];
+		pin_func_id = pin_func_ids[i];
 
-		pin_reg = imx_find_pin_reg(info, pin_id, 1, mux[i]);
+		pin_reg = imx_find_pin_reg(info, pin_id, 1, mux[i],
+					pin_func_id);
 		if (!pin_reg)
 			return -EINVAL;
 
@@ -254,8 +263,9 @@ static int imx_pmx_enable(struct pinctrl_dev *pctldev, unsigned selector,
 		}
 
 		writel(mux[i], ipctl->base + pin_reg->mux_reg);
-		dev_dbg(ipctl->dev, "write: offset 0x%x val 0x%x\n",
-			pin_reg->mux_reg, mux[i]);
+		dev_dbg(ipctl->dev,
+			"Pinid(%d), func-id(%d), write: offset 0x%x val 0x%x\n",
+			pin_id, pin_func_id, pin_reg->mux_reg, mux[i]);
 
 		/* some pins also need select input setting, set it if found */
 		if (pin_reg->input_reg) {
@@ -313,7 +323,7 @@ static int imx_pinconf_get(struct pinctrl_dev *pctldev,
 	const struct imx_pinctrl_soc_info *info = ipctl->info;
 	const struct imx_pin_reg *pin_reg;
 
-	pin_reg = imx_find_pin_reg(info, pin_id, 0, 0);
+	pin_reg = imx_find_pin_reg(info, pin_id, 0, 0, -1);
 	if (!pin_reg)
 		return -EINVAL;
 
@@ -335,7 +345,7 @@ static int imx_pinconf_set(struct pinctrl_dev *pctldev,
 	const struct imx_pinctrl_soc_info *info = ipctl->info;
 	const struct imx_pin_reg *pin_reg;
 
-	pin_reg = imx_find_pin_reg(info, pin_id, 0, 0);
+	pin_reg = imx_find_pin_reg(info, pin_id, 0, 0, -1);
 	if (!pin_reg)
 		return -EINVAL;
 
@@ -363,7 +373,7 @@ static void imx_pinconf_dbg_show(struct pinctrl_dev *pctldev,
 	const struct imx_pin_reg *pin_reg;
 	unsigned long config;
 
-	pin_reg = imx_find_pin_reg(info, pin_id, 0, 0);
+	pin_reg = imx_find_pin_reg(info, pin_id, 0, 0, -1);
 	if (!pin_reg || !pin_reg->conf_reg) {
 		seq_printf(s, "N/A");
 		return;
@@ -460,6 +470,9 @@ static int imx_pinctrl_parse_groups(struct device_node *np,
 				GFP_KERNEL);
 	grp->configs = devm_kzalloc(info->dev, grp->npins * sizeof(unsigned long),
 				GFP_KERNEL);
+	grp->pin_func_ids = devm_kzalloc(info->dev,
+				grp->npins * sizeof(unsigned long), GFP_KERNEL);
+
 	for (i = 0, j = 0; i < size; i += 2, j++) {
 		pin_func_id = be32_to_cpu(*list++);
 		ret = imx_pinctrl_get_pin_id_and_mux(info, pin_func_id,
@@ -473,6 +486,7 @@ static int imx_pinctrl_parse_groups(struct device_node *np,
 		if (config & IMX_PAD_SION)
 			grp->mux_mode[j] |= IOMUXC_CONFIG_SION;
 		grp->configs[j] = config & ~IMX_PAD_SION;
+		grp->pin_func_ids[j] = pin_func_id;
 	}
 
 #ifdef DEBUG
diff --git a/drivers/pinctrl/pinctrl-imx.h b/drivers/pinctrl/pinctrl-imx.h
index 9b65e78..2c11b2f 100644
--- a/drivers/pinctrl/pinctrl-imx.h
+++ b/drivers/pinctrl/pinctrl-imx.h
@@ -35,6 +35,7 @@ struct imx_pin_group {
 	unsigned npins;
 	unsigned int *mux_mode;
 	unsigned long *configs;
+	int *pin_func_ids;
 };
 
 /**
