diff --git a/drmconnector.cpp b/drmconnector.cpp
index 756791f..f272024 100644
--- a/drmconnector.cpp
+++ b/drmconnector.cpp
@@ -122,6 +122,7 @@
 
   state_ = c->connection;
 
+  bool preferred_mode_found = false;
   std::vector<DrmMode> new_modes;
   for (int i = 0; i < c->count_modes; ++i) {
     bool exists = false;
@@ -132,14 +133,20 @@
         break;
       }
     }
-    if (exists)
-      continue;
-
-    DrmMode m(&c->modes[i]);
-    m.set_id(drm_->next_mode_id());
-    new_modes.push_back(m);
+    if (!exists) {
+      DrmMode m(&c->modes[i]);
+      m.set_id(drm_->next_mode_id());
+      new_modes.push_back(m);
+    }
+    if (new_modes.back().type() & DRM_MODE_TYPE_PREFERRED) {
+      preferred_mode_id_ = new_modes.back().id();
+      preferred_mode_found = true;
+    }
   }
   modes_.swap(new_modes);
+  if ((!preferred_mode_found) && (modes_.size() != 0)) {
+    preferred_mode_id_ = modes_[0].id();
+  }
   return 0;
 }
 
diff --git a/drmconnector.h b/drmconnector.h
index 4f7f6a8..9c526c8 100644
--- a/drmconnector.h
+++ b/drmconnector.h
@@ -74,6 +74,10 @@
   uint32_t mm_width() const;
   uint32_t mm_height() const;
 
+  uint32_t get_preferred_mode_id() const {
+    return preferred_mode_id_;
+  }
+
  private:
   DrmDevice *drm_;
 
@@ -97,6 +101,8 @@
   DrmProperty writeback_out_fence_;
 
   std::vector<DrmEncoder *> possible_encoders_;
+
+  uint32_t preferred_mode_id_;
 };
 }  // namespace android
 
diff --git a/drmhwctwo.cpp b/drmhwctwo.cpp
index f506bfc..3123b29 100644
--- a/drmhwctwo.cpp
+++ b/drmhwctwo.cpp
@@ -245,14 +245,7 @@
   if (err != HWC2::Error::None || !num_configs)
     return err;
 
-  // Grab the first mode, we'll choose this as the active mode
-  // TODO: Should choose the preferred mode here
-  hwc2_config_t default_config;
-  num_configs = 1;
-  err = GetDisplayConfigs(&num_configs, &default_config);
-  if (err != HWC2::Error::None)
-    return err;
-  return SetActiveConfig(default_config);
+  return SetActiveConfig(connector_->get_preferred_mode_id());
 }
 
 HWC2::Error DrmHwcTwo::HwcDisplay::RegisterVsyncCallback(