int mdp5_pipe_assign(struct drm_atomic_state *s, struct drm_plane *plane,
                     uint32_t caps, uint32_t blkcfg,
-                    struct mdp5_hw_pipe **hwpipe)
+                    struct mdp5_hw_pipe **hwpipe,
+                    struct mdp5_hw_pipe **r_hwpipe)
 {
        struct msm_drm_private *priv = s->dev->dev_private;
        struct mdp5_kms *mdp5_kms = to_mdp5_kms(to_mdp_kms(priv->kms));
        struct mdp5_state *state;
        struct mdp5_hw_pipe_state *old_state, *new_state;
-       int i;
+       int i, j;
 
        state = mdp5_get_state(s);
        if (IS_ERR(state))
                 * fewest unneeded caps bits set:
                 */
                if (!(*hwpipe) || (hweight_long(cur->caps & ~caps) <
-                                  hweight_long((*hwpipe)->caps & ~caps)))
-                       *hwpipe = cur;
+                                  hweight_long((*hwpipe)->caps & ~caps))) {
+                       bool r_found = false;
+
+                       if (r_hwpipe) {
+                               for (j = i + 1; j < mdp5_kms->num_hwpipes;
+                                    j++) {
+                                       struct mdp5_hw_pipe *r_cur =
+                                                       mdp5_kms->hwpipes[j];
+
+                                       /* reject different types of hwpipes */
+                                       if (r_cur->caps != cur->caps)
+                                               continue;
+
+                                       /* respect priority, eg. VIG0 > VIG1 */
+                                       if (cur->pipe > r_cur->pipe)
+                                               continue;
+
+                                       *r_hwpipe = r_cur;
+                                       r_found = true;
+                                       break;
+                               }
+                       }
+
+                       if (!r_hwpipe || r_found)
+                               *hwpipe = cur;
+               }
        }
 
        if (!(*hwpipe))
                return -ENOMEM;
 
+       if (r_hwpipe && !(*r_hwpipe))
+               return -ENOMEM;
+
        if (mdp5_kms->smp) {
                int ret;
 
+               /* We don't support SMP and 2 hwpipes/plane together */
+               WARN_ON(r_hwpipe);
+
                DBG("%s: alloc SMP blocks", (*hwpipe)->name);
                ret = mdp5_smp_assign(mdp5_kms->smp, &state->smp,
                                (*hwpipe)->pipe, blkcfg);
                        (*hwpipe)->name, plane->name, caps);
        new_state->hwpipe_to_plane[(*hwpipe)->idx] = plane;
 
+       if (r_hwpipe) {
+               DBG("%s: assign to right of plane %s for caps %x",
+                   (*r_hwpipe)->name, plane->name, caps);
+               new_state->hwpipe_to_plane[(*r_hwpipe)->idx] = plane;
+       }
+
        return 0;
 }
 
 
        struct drm_plane *hwpipe_to_plane[SSPP_MAX];
 };
 
-int
-mdp5_pipe_assign(struct drm_atomic_state *s, struct drm_plane *plane,
-                uint32_t caps, uint32_t blkcfg, struct mdp5_hw_pipe **hwpipe);
+int mdp5_pipe_assign(struct drm_atomic_state *s, struct drm_plane *plane,
+                    uint32_t caps, uint32_t blkcfg,
+                    struct mdp5_hw_pipe **hwpipe,
+                    struct mdp5_hw_pipe **r_hwpipe);
 void mdp5_pipe_release(struct drm_atomic_state *s, struct mdp5_hw_pipe *hwpipe);
 
 struct mdp5_hw_pipe *mdp5_pipe_init(enum mdp5_pipe pipe,
 
                        struct mdp5_hw_pipe *old_hwpipe = mdp5_state->hwpipe;
                        struct mdp5_hw_pipe *old_right_hwpipe =
                                                          mdp5_state->r_hwpipe;
+                       struct mdp5_hw_pipe *new_hwpipe = NULL;
+                       struct mdp5_hw_pipe *new_right_hwpipe = NULL;
 
                        ret = mdp5_pipe_assign(state->state, plane, caps,
-                                              blkcfg, &mdp5_state->hwpipe);
+                                              blkcfg, &new_hwpipe,
+                                              need_right_hwpipe ?
+                                              &new_right_hwpipe : NULL);
                        if (ret) {
-                               DBG("%s: failed to assign hwpipe!", plane->name);
+                               DBG("%s: failed to assign hwpipe(s)!",
+                                   plane->name);
                                return ret;
                        }
 
-                       if (need_right_hwpipe) {
-                               ret = mdp5_pipe_assign(state->state, plane,
-                                                      caps, blkcfg,
-                                                      &mdp5_state->r_hwpipe);
-                               if (ret) {
-                                       DBG("%s: failed to assign right hwpipe",
-                                           plane->name);
-                                       return ret;
-                               }
-                       } else {
+                       mdp5_state->hwpipe = new_hwpipe;
+                       if (need_right_hwpipe)
+                               mdp5_state->r_hwpipe = new_right_hwpipe;
+                       else
                                /*
                                 * set it to NULL so that the driver knows we
                                 * don't have a right hwpipe when committing a
                                 * new state
                                 */
                                mdp5_state->r_hwpipe = NULL;
-                       }
+
 
                        mdp5_pipe_release(state->state, old_hwpipe);
                        mdp5_pipe_release(state->state, old_right_hwpipe);