diff --git a/drivers/media/platform/rcar-vin/rcar-core.c b/drivers/media/platform/rcar-vin/rcar-core.c
index 0fa330e02bd8437a..ef8a12bd63cb80d8 100644
--- a/drivers/media/platform/rcar-vin/rcar-core.c
+++ b/drivers/media/platform/rcar-vin/rcar-core.c
@@ -743,27 +743,6 @@ static int rvin_parallel_init(struct rvin_dev *vin)
  * CSI-2
  */
 
-static unsigned int rvin_csi2_get_mask(struct rvin_dev *vin,
-				       enum rvin_csi_id csi_id,
-				       unsigned char channel)
-{
-	const struct rvin_group_route *route;
-	unsigned int mask = 0;
-
-	for (route = vin->info->routes; route->mask; route++) {
-		if (route->vin == vin->id &&
-		    route->csi == csi_id &&
-		    route->channel == channel) {
-			vin_dbg(vin,
-				"Adding route: vin: %d csi: %d channel: %d\n",
-				route->vin, route->csi, route->channel);
-			mask |= route->mask;
-		}
-	}
-
-	return mask;
-}
-
 /*
  * Link setup for the links between a VIN and a CSI-2 receiver is a bit
  * complex. The reason for this is that the register controlling routing
@@ -849,9 +828,9 @@ static int rvin_csi2_link_notify(struct media_link *link, u32 flags,
 			link->source->entity->name);
 		ret = -ENODEV;
 	} else {
-		unsigned int master_id, channel, mask_new;
-		unsigned int mask = ~0;
-		struct media_pad *csi_pad;
+		const struct rvin_group_route *route;
+		unsigned int chsel = UINT_MAX;
+		unsigned int master_id;
 
 		master_id = rvin_group_id_to_master(vin->id);
 
@@ -860,8 +839,10 @@ static int rvin_csi2_link_notify(struct media_link *link, u32 flags,
 			goto out;
 		}
 
-		/* Build a mask for already enabled links. */
+		/* Make sure group is connected to same CSI-2 */
 		for (i = master_id; i < master_id + 4; i++) {
+			struct media_pad *csi_pad;
+
 			if (!group->vin[i])
 				continue;
 
@@ -871,26 +852,28 @@ static int rvin_csi2_link_notify(struct media_link *link, u32 flags,
 			if (!csi_pad)
 				continue;
 
-			csi_id = rvin_group_entity_to_remote_id(group,
-								csi_pad->entity);
-			channel = rvin_group_csi_pad_to_channel(csi_pad->index);
-
-			mask &= rvin_csi2_get_mask(group->vin[i], csi_id, channel);
+			if (csi_pad->entity != link->source->entity) {
+				vin_dbg(vin, "Already attached to %s\n",
+					csi_pad->entity->name);
+				ret = -EBUSY;
+				goto out;
+			}
 		}
 
-		channel = rvin_group_csi_pad_to_channel(link->source->index);
-		mask_new = mask & rvin_csi2_get_mask(vin, csi_id, channel);
-		vin_dbg(vin, "Try link change mask: 0x%x new: 0x%x\n", mask,
-			mask_new);
+		for (route = vin->info->routes; route->chsel; route++) {
+			if (route->master == master_id && route->csi == csi_id) {
+				chsel = route->chsel;
+				break;
+			}
+		}
 
-		if (!mask_new) {
-			ret = -EMLINK;
+		if (chsel == UINT_MAX) {
+			vin_err(vin, "No CHSEL value found\n");
+			ret = -EINVAL;
 			goto out;
 		}
 
-		/* New valid CHSEL found, set the new value. */
-		ret = rvin_set_channel_routing(group->vin[master_id],
-					       __ffs(mask_new));
+		ret = rvin_set_channel_routing(group->vin[master_id], chsel);
 		if (ret)
 			goto out;
 
@@ -906,47 +889,60 @@ static const struct media_device_ops rvin_csi2_media_ops = {
 	.link_notify = rvin_csi2_link_notify,
 };
 
-static int rvin_csi2_create_link(struct rvin_group *group,
-				 const struct rvin_group_route *route)
+static int rvin_csi2_create_link(struct rvin_group *group, unsigned int id,
+				const struct rvin_group_route *route)
 
 {
 	struct media_entity *source = &group->remotes[route->csi].subdev->entity;
-	unsigned int source_idx = rvin_group_csi_channel_to_pad(route->channel);
-	struct media_entity *sink = &group->vin[route->vin]->vdev.entity;
-	struct media_pad *source_pad = &source->pads[source_idx];
+	struct media_entity *sink = &group->vin[id]->vdev.entity;
 	struct media_pad *sink_pad = &sink->pads[0];
+	unsigned int channel;
+	int ret;
 
-	/* Skip if link already exists. */
-	if (media_entity_find_link(source_pad, sink_pad))
-		return 0;
+	for (channel = 0; channel < 4; channel++) {
+		unsigned int source_idx = rvin_group_csi_channel_to_pad(channel);
+		struct media_pad *source_pad = &source->pads[source_idx];
 
-	return media_create_pad_link(source, source_idx, sink, 0, 0);
+		/* Skip if link already exists. */
+		if (media_entity_find_link(source_pad, sink_pad))
+			continue;
+
+		ret = media_create_pad_link(source, source_idx, sink, 0, 0);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
 }
 
 static int rvin_csi2_setup_links(struct rvin_dev *vin)
 {
 	const struct rvin_group_route *route;
+	unsigned int id;
 	int ret = -EINVAL;
 
 	/* Create all media device links between VINs and CSI-2's. */
 	mutex_lock(&vin->group->lock);
-	for (route = vin->info->routes; route->mask; route++) {
-		/* Check that VIN is part of the group. */
-		if (!vin->group->vin[route->vin])
-			continue;
-
+	for (route = vin->info->routes; route->chsel; route++) {
 		/* Check that VIN' master is part of the group. */
-		if (!vin->group->vin[rvin_group_id_to_master(route->vin)])
+		if (!vin->group->vin[route->master])
 			continue;
 
 		/* Check that CSI-2 is part of the group. */
 		if (!vin->group->remotes[route->csi].subdev)
 			continue;
 
-		ret = rvin_csi2_create_link(vin->group, route);
-		if (ret)
-			break;
+		for (id = route->master; id < route->master + 4; id++) {
+			/* Check that VIN is part of the group. */
+			if (!vin->group->vin[id])
+				continue;
+
+			ret = rvin_csi2_create_link(vin->group, id, route);
+			if (ret)
+				goto out;
+		}
 	}
+out:
 	mutex_unlock(&vin->group->lock);
 
 	return ret;
@@ -1156,30 +1152,9 @@ static const struct rvin_info rcar_info_gen2 = {
 };
 
 static const struct rvin_group_route rcar_info_r8a774e1_routes[] = {
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 0, .mask = BIT(0) | BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 0, .mask = BIT(1) | BIT(4) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 0, .mask = BIT(2) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 1, .mask = BIT(0) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 1, .mask = BIT(1) | BIT(3) },
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 1, .mask = BIT(2) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 1, .mask = BIT(4) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 2, .mask = BIT(0) },
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 2, .mask = BIT(1) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 2, .mask = BIT(2) },
-	{ .csi = RVIN_CSI40, .channel = 2, .vin = 2, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 2, .vin = 2, .mask = BIT(4) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 3, .mask = BIT(0) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 3, .mask = BIT(1) | BIT(2) },
-	{ .csi = RVIN_CSI40, .channel = 3, .vin = 3, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 3, .vin = 3, .mask = BIT(4) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 4, .mask = BIT(1) | BIT(4) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 5, .mask = BIT(0) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 5, .mask = BIT(4) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 6, .mask = BIT(0) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 6, .mask = BIT(2) },
-	{ .csi = RVIN_CSI20, .channel = 2, .vin = 6, .mask = BIT(4) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 7, .mask = BIT(1) | BIT(2) },
-	{ .csi = RVIN_CSI20, .channel = 3, .vin = 7, .mask = BIT(4) },
+	{ .master = 0, .csi = RVIN_CSI20, .chsel = 0x04 },
+	{ .master = 0, .csi = RVIN_CSI40, .chsel = 0x03 },
+	{ .master = 4, .csi = RVIN_CSI20, .chsel = 0x04 },
 	{ /* Sentinel */ }
 };
 
@@ -1192,38 +1167,10 @@ static const struct rvin_info rcar_info_r8a774e1 = {
 };
 
 static const struct rvin_group_route rcar_info_r8a7795_routes[] = {
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 0, .mask = BIT(0) | BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 0, .mask = BIT(1) | BIT(4) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 0, .mask = BIT(2) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 1, .mask = BIT(0) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 1, .mask = BIT(1) | BIT(3) },
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 1, .mask = BIT(2) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 1, .mask = BIT(4) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 2, .mask = BIT(0) },
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 2, .mask = BIT(1) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 2, .mask = BIT(2) },
-	{ .csi = RVIN_CSI40, .channel = 2, .vin = 2, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 2, .vin = 2, .mask = BIT(4) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 3, .mask = BIT(0) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 3, .mask = BIT(1) | BIT(2) },
-	{ .csi = RVIN_CSI40, .channel = 3, .vin = 3, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 3, .vin = 3, .mask = BIT(4) },
-	{ .csi = RVIN_CSI41, .channel = 0, .vin = 4, .mask = BIT(0) | BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 4, .mask = BIT(1) | BIT(4) },
-	{ .csi = RVIN_CSI41, .channel = 1, .vin = 4, .mask = BIT(2) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 5, .mask = BIT(0) },
-	{ .csi = RVIN_CSI41, .channel = 1, .vin = 5, .mask = BIT(1) | BIT(3) },
-	{ .csi = RVIN_CSI41, .channel = 0, .vin = 5, .mask = BIT(2) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 5, .mask = BIT(4) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 6, .mask = BIT(0) },
-	{ .csi = RVIN_CSI41, .channel = 0, .vin = 6, .mask = BIT(1) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 6, .mask = BIT(2) },
-	{ .csi = RVIN_CSI41, .channel = 2, .vin = 6, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 2, .vin = 6, .mask = BIT(4) },
-	{ .csi = RVIN_CSI41, .channel = 1, .vin = 7, .mask = BIT(0) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 7, .mask = BIT(1) | BIT(2) },
-	{ .csi = RVIN_CSI41, .channel = 3, .vin = 7, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 3, .vin = 7, .mask = BIT(4) },
+	{ .master = 0, .csi = RVIN_CSI20, .chsel = 0x04 },
+	{ .master = 0, .csi = RVIN_CSI40, .chsel = 0x03 },
+	{ .master = 4, .csi = RVIN_CSI20, .chsel = 0x04 },
+	{ .master = 4, .csi = RVIN_CSI41, .chsel = 0x03 },
 	{ /* Sentinel */ }
 };
 
@@ -1237,48 +1184,12 @@ static const struct rvin_info rcar_info_r8a7795 = {
 };
 
 static const struct rvin_group_route rcar_info_r8a7795es1_routes[] = {
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 0, .mask = BIT(0) | BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 0, .mask = BIT(1) | BIT(4) },
-	{ .csi = RVIN_CSI21, .channel = 0, .vin = 0, .mask = BIT(2) | BIT(5) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 1, .mask = BIT(0) },
-	{ .csi = RVIN_CSI21, .channel = 0, .vin = 1, .mask = BIT(1) },
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 1, .mask = BIT(2) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 1, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 1, .mask = BIT(4) },
-	{ .csi = RVIN_CSI21, .channel = 1, .vin = 1, .mask = BIT(5) },
-	{ .csi = RVIN_CSI21, .channel = 0, .vin = 2, .mask = BIT(0) },
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 2, .mask = BIT(1) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 2, .mask = BIT(2) },
-	{ .csi = RVIN_CSI40, .channel = 2, .vin = 2, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 2, .vin = 2, .mask = BIT(4) },
-	{ .csi = RVIN_CSI21, .channel = 2, .vin = 2, .mask = BIT(5) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 3, .mask = BIT(0) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 3, .mask = BIT(1) },
-	{ .csi = RVIN_CSI21, .channel = 1, .vin = 3, .mask = BIT(2) },
-	{ .csi = RVIN_CSI40, .channel = 3, .vin = 3, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 3, .vin = 3, .mask = BIT(4) },
-	{ .csi = RVIN_CSI21, .channel = 3, .vin = 3, .mask = BIT(5) },
-	{ .csi = RVIN_CSI41, .channel = 0, .vin = 4, .mask = BIT(0) | BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 4, .mask = BIT(1) | BIT(4) },
-	{ .csi = RVIN_CSI21, .channel = 0, .vin = 4, .mask = BIT(2) | BIT(5) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 5, .mask = BIT(0) },
-	{ .csi = RVIN_CSI21, .channel = 0, .vin = 5, .mask = BIT(1) },
-	{ .csi = RVIN_CSI41, .channel = 0, .vin = 5, .mask = BIT(2) },
-	{ .csi = RVIN_CSI41, .channel = 1, .vin = 5, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 5, .mask = BIT(4) },
-	{ .csi = RVIN_CSI21, .channel = 1, .vin = 5, .mask = BIT(5) },
-	{ .csi = RVIN_CSI21, .channel = 0, .vin = 6, .mask = BIT(0) },
-	{ .csi = RVIN_CSI41, .channel = 0, .vin = 6, .mask = BIT(1) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 6, .mask = BIT(2) },
-	{ .csi = RVIN_CSI41, .channel = 2, .vin = 6, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 2, .vin = 6, .mask = BIT(4) },
-	{ .csi = RVIN_CSI21, .channel = 2, .vin = 6, .mask = BIT(5) },
-	{ .csi = RVIN_CSI41, .channel = 1, .vin = 7, .mask = BIT(0) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 7, .mask = BIT(1) },
-	{ .csi = RVIN_CSI21, .channel = 1, .vin = 7, .mask = BIT(2) },
-	{ .csi = RVIN_CSI41, .channel = 3, .vin = 7, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 3, .vin = 7, .mask = BIT(4) },
-	{ .csi = RVIN_CSI21, .channel = 3, .vin = 7, .mask = BIT(5) },
+	{ .master = 0, .csi = RVIN_CSI20, .chsel = 0x04 },
+	{ .master = 0, .csi = RVIN_CSI21, .chsel = 0x05 },
+	{ .master = 0, .csi = RVIN_CSI40, .chsel = 0x03 },
+	{ .master = 4, .csi = RVIN_CSI20, .chsel = 0x04 },
+	{ .master = 4, .csi = RVIN_CSI21, .chsel = 0x05 },
+	{ .master = 4, .csi = RVIN_CSI41, .chsel = 0x03 },
 	{ /* Sentinel */ }
 };
 
@@ -1291,34 +1202,10 @@ static const struct rvin_info rcar_info_r8a7795es1 = {
 };
 
 static const struct rvin_group_route rcar_info_r8a7796_routes[] = {
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 0, .mask = BIT(0) | BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 0, .mask = BIT(1) | BIT(4) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 1, .mask = BIT(0) },
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 1, .mask = BIT(2) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 1, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 1, .mask = BIT(4) },
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 2, .mask = BIT(1) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 2, .mask = BIT(2) },
-	{ .csi = RVIN_CSI40, .channel = 2, .vin = 2, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 2, .vin = 2, .mask = BIT(4) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 3, .mask = BIT(0) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 3, .mask = BIT(1) },
-	{ .csi = RVIN_CSI40, .channel = 3, .vin = 3, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 3, .vin = 3, .mask = BIT(4) },
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 4, .mask = BIT(0) | BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 4, .mask = BIT(1) | BIT(4) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 5, .mask = BIT(0) },
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 5, .mask = BIT(2) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 5, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 5, .mask = BIT(4) },
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 6, .mask = BIT(1) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 6, .mask = BIT(2) },
-	{ .csi = RVIN_CSI40, .channel = 2, .vin = 6, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 2, .vin = 6, .mask = BIT(4) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 7, .mask = BIT(0) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 7, .mask = BIT(1) },
-	{ .csi = RVIN_CSI40, .channel = 3, .vin = 7, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 3, .vin = 7, .mask = BIT(4) },
+	{ .master = 0, .csi = RVIN_CSI20, .chsel = 0x04 },
+	{ .master = 0, .csi = RVIN_CSI40, .chsel = 0x03 },
+	{ .master = 4, .csi = RVIN_CSI20, .chsel = 0x04 },
+	{ .master = 4, .csi = RVIN_CSI40, .chsel = 0x03 },
 	{ /* Sentinel */ }
 };
 
@@ -1332,38 +1219,10 @@ static const struct rvin_info rcar_info_r8a7796 = {
 };
 
 static const struct rvin_group_route rcar_info_r8a77965_routes[] = {
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 0, .mask = BIT(0) | BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 0, .mask = BIT(1) | BIT(4) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 0, .mask = BIT(2) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 1, .mask = BIT(0) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 1, .mask = BIT(1) | BIT(3) },
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 1, .mask = BIT(2) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 1, .mask = BIT(4) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 2, .mask = BIT(0) },
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 2, .mask = BIT(1) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 2, .mask = BIT(2) },
-	{ .csi = RVIN_CSI40, .channel = 2, .vin = 2, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 2, .vin = 2, .mask = BIT(4) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 3, .mask = BIT(0) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 3, .mask = BIT(1) | BIT(2) },
-	{ .csi = RVIN_CSI40, .channel = 3, .vin = 3, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 3, .vin = 3, .mask = BIT(4) },
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 4, .mask = BIT(0) | BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 4, .mask = BIT(1) | BIT(4) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 4, .mask = BIT(2) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 5, .mask = BIT(0) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 5, .mask = BIT(1) | BIT(3) },
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 5, .mask = BIT(2) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 5, .mask = BIT(4) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 6, .mask = BIT(0) },
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 6, .mask = BIT(1) },
-	{ .csi = RVIN_CSI20, .channel = 0, .vin = 6, .mask = BIT(2) },
-	{ .csi = RVIN_CSI40, .channel = 2, .vin = 6, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 2, .vin = 6, .mask = BIT(4) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 7, .mask = BIT(0) },
-	{ .csi = RVIN_CSI20, .channel = 1, .vin = 7, .mask = BIT(1) | BIT(2) },
-	{ .csi = RVIN_CSI40, .channel = 3, .vin = 7, .mask = BIT(3) },
-	{ .csi = RVIN_CSI20, .channel = 3, .vin = 7, .mask = BIT(4) },
+	{ .master = 0, .csi = RVIN_CSI20, .chsel = 0x04 },
+	{ .master = 0, .csi = RVIN_CSI40, .chsel = 0x03 },
+	{ .master = 4, .csi = RVIN_CSI20, .chsel = 0x04 },
+	{ .master = 4, .csi = RVIN_CSI40, .chsel = 0x03 },
 	{ /* Sentinel */ }
 };
 
@@ -1377,13 +1236,7 @@ static const struct rvin_info rcar_info_r8a77965 = {
 };
 
 static const struct rvin_group_route rcar_info_r8a77970_routes[] = {
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 0, .mask = BIT(0) | BIT(3) },
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 1, .mask = BIT(2) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 1, .mask = BIT(3) },
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 2, .mask = BIT(1) },
-	{ .csi = RVIN_CSI40, .channel = 2, .vin = 2, .mask = BIT(3) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 3, .mask = BIT(0) },
-	{ .csi = RVIN_CSI40, .channel = 3, .vin = 3, .mask = BIT(3) },
+	{ .master = 0, .csi = RVIN_CSI40, .chsel = 0x03 },
 	{ /* Sentinel */ }
 };
 
@@ -1396,22 +1249,8 @@ static const struct rvin_info rcar_info_r8a77970 = {
 };
 
 static const struct rvin_group_route rcar_info_r8a77980_routes[] = {
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 0, .mask = BIT(0) | BIT(3) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 0, .mask = BIT(2) },
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 1, .mask = BIT(2) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 1, .mask = BIT(1) | BIT(3) },
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 2, .mask = BIT(1) },
-	{ .csi = RVIN_CSI40, .channel = 2, .vin = 2, .mask = BIT(3) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 3, .mask = BIT(0) },
-	{ .csi = RVIN_CSI40, .channel = 3, .vin = 3, .mask = BIT(3) },
-	{ .csi = RVIN_CSI41, .channel = 0, .vin = 4, .mask = BIT(0) | BIT(3) },
-	{ .csi = RVIN_CSI41, .channel = 1, .vin = 4, .mask = BIT(2) },
-	{ .csi = RVIN_CSI41, .channel = 0, .vin = 5, .mask = BIT(2) },
-	{ .csi = RVIN_CSI41, .channel = 1, .vin = 5, .mask = BIT(1) | BIT(3) },
-	{ .csi = RVIN_CSI41, .channel = 0, .vin = 6, .mask = BIT(1) },
-	{ .csi = RVIN_CSI41, .channel = 2, .vin = 6, .mask = BIT(3) },
-	{ .csi = RVIN_CSI41, .channel = 1, .vin = 7, .mask = BIT(0) },
-	{ .csi = RVIN_CSI41, .channel = 3, .vin = 7, .mask = BIT(3) },
+	{ .master = 0, .csi = RVIN_CSI40, .chsel = 0x03 },
+	{ .master = 4, .csi = RVIN_CSI41, .chsel = 0x03 },
 	{ /* Sentinel */ }
 };
 
@@ -1425,10 +1264,7 @@ static const struct rvin_info rcar_info_r8a77980 = {
 };
 
 static const struct rvin_group_route rcar_info_r8a77990_routes[] = {
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 4, .mask = BIT(0) | BIT(3) },
-	{ .csi = RVIN_CSI40, .channel = 0, .vin = 5, .mask = BIT(2) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 4, .mask = BIT(2) },
-	{ .csi = RVIN_CSI40, .channel = 1, .vin = 5, .mask = BIT(1) | BIT(3) },
+	{ .master = 0, .csi = RVIN_CSI40, .chsel = 0x03 },
 	{ /* Sentinel */ }
 };
 
diff --git a/drivers/media/platform/rcar-vin/rcar-csi2.c b/drivers/media/platform/rcar-vin/rcar-csi2.c
index 8c939cb3073d79ec..909419963188afc5 100644
--- a/drivers/media/platform/rcar-vin/rcar-csi2.c
+++ b/drivers/media/platform/rcar-vin/rcar-csi2.c
@@ -468,6 +468,8 @@ struct rcar_csi2 {
 	struct v4l2_subdev *remote;
 	unsigned int remote_pad;
 
+	int channel_vc[4];
+
 	struct mutex lock; /* Protects mf and stream_count. */
 	struct v4l2_mbus_framefmt mf;
 	int stream_count;
@@ -675,8 +677,11 @@ static int rcsi2_start_receiver(struct rcar_csi2 *priv)
 	for (i = 0; i < priv->info->num_channels; i++) {
 		u32 vcdt_part;
 
-		vcdt_part = VCDT_SEL_VC(i) | VCDT_VCDTN_EN | VCDT_SEL_DTN_ON |
-			VCDT_SEL_DT(format->datatype);
+		if (priv->channel_vc[i] < 0)
+			continue;
+
+		vcdt_part = VCDT_SEL_VC(priv->channel_vc[i]) | VCDT_VCDTN_EN |
+			VCDT_SEL_DTN_ON | VCDT_SEL_DT(format->datatype);
 
 		/* Store in correct reg and offset. */
 		if (i < 2)
@@ -1258,7 +1263,53 @@ static int rcsi2_init_phtw_v3u(struct rcar_csi2 *priv,
  * Platform Device Driver.
  */
 
+static int rcsi2_link_setup(struct media_entity *entity,
+			    const struct media_pad *local,
+			    const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct rcar_csi2 *priv = sd_to_csi2(sd);
+	struct video_device *vdev;
+	int channel, vc;
+	u32 id;
+
+	if (!is_media_entity_v4l2_video_device(remote->entity)) {
+		dev_err(priv->dev, "Remote is not a video device\n");
+		return -EINVAL;
+	}
+
+	vdev = media_entity_to_video_device(remote->entity);
+
+	if (of_property_read_u32(vdev->dev_parent->of_node, "renesas,id", &id)) {
+		dev_err(priv->dev, "No renesas,id, can't configure routing\n");
+		return -EINVAL;
+	}
+
+	channel = id % 4;
+
+	if (flags & MEDIA_LNK_FL_ENABLED) {
+		if (media_entity_remote_pad(local)) {
+			dev_dbg(priv->dev,
+				"Each VC can only be routed to one output channel\n");
+			return -EINVAL;
+		}
+
+		vc = local->index - 1;
+
+		dev_dbg(priv->dev, "Route VC%d to VIN%u on output channel %d\n",
+			vc, id, channel);
+	} else {
+		vc = -1;
+	}
+
+	priv->channel_vc[channel] = vc;
+
+	return 0;
+}
+
+
 static const struct media_entity_operations rcar_csi2_entity_ops = {
+	.link_setup = rcsi2_link_setup,
 	.link_validate = v4l2_subdev_link_validate,
 };
 
@@ -1477,6 +1528,9 @@ static int rcsi2_probe(struct platform_device *pdev)
 	if (ret)
 		goto error_async;
 
+	for (i = 0; i < ARRAY_SIZE(priv->channel_vc); i++)
+		priv->channel_vc[i] = -1;
+
 	pm_runtime_enable(&pdev->dev);
 
 	ret = v4l2_async_register_subdev(&priv->subdev);
diff --git a/drivers/media/platform/rcar-vin/rcar-dma.c b/drivers/media/platform/rcar-vin/rcar-dma.c
index 8136bc75e7c47cdd..2272f1c96aafd72a 100644
--- a/drivers/media/platform/rcar-vin/rcar-dma.c
+++ b/drivers/media/platform/rcar-vin/rcar-dma.c
@@ -1507,7 +1507,7 @@ int rvin_set_channel_routing(struct rvin_dev *vin, u8 chsel)
 	 * register. IFMD_DES1 controls data expansion mode for CSI20/21,
 	 * IFMD_DES0 controls data expansion mode for CSI40/41.
 	 */
-	for (route = vin->info->routes; route->mask; route++) {
+	for (route = vin->info->routes; route->chsel; route++) {
 		if (route->csi == RVIN_CSI20 || route->csi == RVIN_CSI21)
 			ifmd |= VNCSI_IFMD_DES1;
 		else
diff --git a/drivers/media/platform/rcar-vin/rcar-vin.h b/drivers/media/platform/rcar-vin/rcar-vin.h
index 6c06320174a2ed96..ddc1830be9a5d6c3 100644
--- a/drivers/media/platform/rcar-vin/rcar-vin.h
+++ b/drivers/media/platform/rcar-vin/rcar-vin.h
@@ -128,11 +128,9 @@ struct rvin_parallel_entity {
  * struct rvin_group_route - describes a route from a channel of a
  *	CSI-2 receiver to a VIN
  *
+ * @master:	VIN group master ID.
  * @csi:	CSI-2 receiver ID.
- * @channel:	Output channel of the CSI-2 receiver.
- * @vin:	VIN ID.
- * @mask:	Bitmask of the different CHSEL register values that
- *		allow for a route from @csi + @chan to @vin.
+ * @chsel:	CHSEL register values that connects VIN group to CSI-2.
  *
  * .. note::
  *	Each R-Car CSI-2 receiver has four output channels facing the VIN
@@ -140,19 +138,11 @@ struct rvin_parallel_entity {
  *	There is no correlation between channel number and CSI-2 VC. It's
  *	up to the CSI-2 receiver driver to configure which VC is output
  *	on which channel, the VIN devices only care about output channels.
- *
- *	There are in some cases multiple CHSEL register settings which would
- *	allow for the same route from @csi + @channel to @vin. For example
- *	on R-Car H3 both the CHSEL values 0 and 3 allow for a route from
- *	CSI40/VC0 to VIN0. All possible CHSEL values for a route need to be
- *	recorded as a bitmask in @mask, in this example bit 0 and 3 should
- *	be set.
  */
 struct rvin_group_route {
+	unsigned int master;
 	enum rvin_csi_id csi;
-	unsigned int channel;
-	unsigned int vin;
-	unsigned int mask;
+	unsigned int chsel;
 };
 
 /**
