diff --git a/drmhwctwo.cpp b/drmhwctwo.cpp
index b96eb31..9268cdc 100644
--- a/drmhwctwo.cpp
+++ b/drmhwctwo.cpp
@@ -399,17 +399,37 @@
     }
   }
 
-  uint32_t idx = 0;
+  // Since the upper layers only look at vactive/hactive/refresh, height and
+  // width, it doesn't differentiate interlaced from progressive and other
+  // similar modes. Depending on the order of modes we return to SF, it could
+  // end up choosing a suboptimal configuration and dropping the preferred
+  // mode. To workaround this, don't offer interlaced modes to SF if there is
+  // at least one non-interlaced alternative and only offer a single WxH@R
+  // mode with at least the prefered mode from in DrmConnector::UpdateModes()
+
+  // TODO: Remove the following block of code until AOSP handles all modes
+  std::vector<DrmMode> sel_modes;
+
+  // Add the preferred mode first to be sure it's not dropped
+  auto mode = std::find_if(connector_->modes().begin(),
+                           connector_->modes().end(), [&](DrmMode const &m) {
+                             return m.id() ==
+                                    connector_->get_preferred_mode_id();
+                           });
+  if (mode != connector_->modes().end())
+    sel_modes.push_back(*mode);
+
+  // Add the active mode if different from preferred mode
+  if (connector_->active_mode().id() != connector_->get_preferred_mode_id())
+    sel_modes.push_back(connector_->active_mode());
+
+  // Cycle over the modes and filter out "similar" modes, keeping only the
+  // first ones in the order given by DRM (from CEA ids and timings order)
   for (const DrmMode &mode : connector_->modes()) {
-    if (configs && idx >= *num_configs)
-      break;
-    // Since the upper layers only look at vactive/hactive/refresh, it doesn't
-    // differentiate interlaced from progressive modes. Depending on the order
-    // of modes we return to SF, it could end up choosing a suboptimal
-    // configuration.
-    // To workaround this, don't offer interlaced modes to SF if there is at
-    // least one non-interlaced alternative.
-    //
+    // TODO: Remove this when 3D Attributes are in AOSP
+    if (mode.flags() & DRM_MODE_FLAG_3D_MASK)
+      continue;
+
     // TODO: Remove this when the Interlaced attribute is in AOSP
     if (mode.flags() & DRM_MODE_FLAG_INTERLACE) {
       auto m = std::find_if(connector_->modes().begin(),
@@ -419,14 +439,36 @@
                                      m.h_display() == mode.h_display() &&
                                      m.v_display() == mode.v_display();
                             });
-      if (m != connector_->modes().end())
-        continue;
+      if (m == connector_->modes().end())
+        sel_modes.push_back(mode);
+
+      continue;
     }
-    if (configs) {
-      configs[idx++] = mode.id();
-    } else {
-      idx++;
-    }
+
+    // Search for a similar WxH@R mode in the filtered list and drop it if
+    // another mode with the same WxH@R has already been selected
+    // TODO: Remove this when AOSP handles duplicates modes
+    auto m = std::find_if(sel_modes.begin(), sel_modes.end(),
+                          [&mode](DrmMode const &m) {
+                            return m.h_display() == mode.h_display() &&
+                                   m.v_display() == mode.v_display() &&
+                                   m.v_refresh() == mode.v_refresh();
+                          });
+    if (m == sel_modes.end())
+      sel_modes.push_back(mode);
+  }
+
+  auto num_modes = static_cast<uint32_t>(sel_modes.size());
+  if (!configs) {
+    *num_configs = num_modes;
+    return HWC2::Error::None;
+  }
+
+  uint32_t idx = 0;
+  for (const DrmMode &mode : sel_modes) {
+    if (idx >= *num_configs)
+      break;
+    configs[idx++] = mode.id();
   }
   *num_configs = idx;
   return HWC2::Error::None;