6 files changed, 53 insertions, 15 deletions
diff --git a/kms++/src/atomicreq.cpp b/kms++/src/atomicreq.cpp
index f2809af..28128f2 100644
--- a/kms++/src/atomicreq.cpp
+++ b/kms++/src/atomicreq.cpp
@@ -60,6 +60,31 @@ void AtomicReq::add(kms::DrmPropObject* ob, const map<string, uint64_t>& values)
 		add(ob, kvp.first, kvp.second);
 }
 
+void AtomicReq::add_display(Connector* conn, Crtc* crtc, Blob* videomode, Plane* primary, Framebuffer* fb)
+{
+	add(conn, {
+		    { "CRTC_ID", crtc->id() },
+	    });
+
+	add(crtc, {
+		    { "ACTIVE", 1 },
+		    { "MODE_ID", videomode->id() },
+	    });
+
+	add(primary, {
+		    { "FB_ID", fb->id() },
+		    { "CRTC_ID", crtc->id() },
+		    { "SRC_X", 0 << 16 },
+		    { "SRC_Y", 0 << 16 },
+		    { "SRC_W", fb->width() << 16 },
+		    { "SRC_H", fb->height() << 16 },
+		    { "CRTC_X", 0 },
+		    { "CRTC_Y", 0 },
+		    { "CRTC_W", fb->width() },
+		    { "CRTC_H", fb->height() },
+	    });
+}
+
 int AtomicReq::test(bool allow_modeset)
 {
 	uint32_t flags = DRM_MODE_ATOMIC_TEST_ONLY;
diff --git a/kms++/src/connector.cpp b/kms++/src/connector.cpp
index ec37d5d..92700af 100644
--- a/kms++/src/connector.cpp
+++ b/kms++/src/connector.cpp
@@ -3,6 +3,7 @@
 #include <unistd.h>
 #include <fcntl.h>
 #include <cassert>
+#include <cmath>
 
 #include <kms++/kms++.h>
 #include "helpers.h"
@@ -118,23 +119,23 @@ Videomode Connector::get_mode(const string& mode) const
         throw invalid_argument(mode + ": mode not found");
 }
 
-Videomode Connector::get_mode(unsigned xres, unsigned yres, unsigned refresh, bool ilace) const
+Videomode Connector::get_mode(unsigned xres, unsigned yres, float vrefresh, bool ilace) const
 {
 	auto c = m_priv->drm_connector;
 
 	for (int i = 0; i < c->count_modes; i++) {
-		drmModeModeInfo& m = c->modes[i];
+		Videomode m = drm_mode_to_video_mode(c->modes[i]);
 
 		if (m.hdisplay != xres || m.vdisplay != yres)
 			continue;
 
-		if (refresh && m.vrefresh != refresh)
+		if (ilace != m.interlace())
 			continue;
 
-		if (ilace != !!(m.flags & DRM_MODE_FLAG_INTERLACE))
+		if (vrefresh && std::abs(m.calculated_vrefresh() - vrefresh) >= 0.001)
 			continue;
 
-		return drm_mode_to_video_mode(c->modes[i]);
+		return m;
 	}
 
 	throw invalid_argument("mode not found");
diff --git a/kms++/src/modedb.cpp b/kms++/src/modedb.cpp
index 24d6f63..858c3d0 100644
--- a/kms++/src/modedb.cpp
+++ b/kms++/src/modedb.cpp
@@ -1,5 +1,6 @@
 #include <xf86drm.h>
 #include <stdexcept>
+#include <cmath>
 
 #include <kms++/modedb.h>
 
@@ -8,7 +9,7 @@ using namespace std;
 namespace kms
 {
 
-static const Videomode& find_from_table(const Videomode* modes, uint32_t width, uint32_t height, uint32_t refresh, bool ilace)
+static const Videomode& find_from_table(const Videomode* modes, uint32_t width, uint32_t height, float vrefresh, bool ilace)
 {
 	for (unsigned i = 0; modes[i].clock; ++i) {
 		const Videomode& m = modes[i];
@@ -16,10 +17,10 @@ static const Videomode& find_from_table(const Videomode* modes, uint32_t width,
 		if (m.hdisplay != width || m.vdisplay != height)
 			continue;
 
-		if (refresh && m.vrefresh != refresh)
+		if (ilace != m.interlace())
 			continue;
 
-		if (ilace != !!(m.flags & DRM_MODE_FLAG_INTERLACE))
+		if (vrefresh && std::abs(m.calculated_vrefresh() - vrefresh) >= 0.001)
 			continue;
 
 		return m;
@@ -28,14 +29,14 @@ static const Videomode& find_from_table(const Videomode* modes, uint32_t width,
 	throw invalid_argument("mode not found");
 }
 
-const Videomode& find_dmt(uint32_t width, uint32_t height, uint32_t refresh, bool ilace)
+const Videomode& find_dmt(uint32_t width, uint32_t height, float vrefresh, bool ilace)
 {
-	return find_from_table(dmt_modes, width, height, refresh, ilace);
+	return find_from_table(dmt_modes, width, height, vrefresh, ilace);
 }
 
-const Videomode& find_cea(uint32_t width, uint32_t height, uint32_t refresh, bool ilace)
+const Videomode& find_cea(uint32_t width, uint32_t height, float vrefresh, bool ilace)
 {
-	return find_from_table(cea_modes, width, height, refresh, ilace);
+	return find_from_table(cea_modes, width, height, vrefresh, ilace);
 }
 
 }
diff --git a/kms++/src/modedb_cea.cpp b/kms++/src/modedb_cea.cpp
index a105dd8..a99a612 100644
--- a/kms++/src/modedb_cea.cpp
+++ b/kms++/src/modedb_cea.cpp
@@ -42,7 +42,8 @@ namespace kms
 	.name = nm, .clock = (c), \
 	.hdisplay = (hd), .hsync_start = (hss), .hsync_end = (hse), .htotal = (ht), .hskew = (hsk), \
 	.vdisplay = (vd), .vsync_start = (vss), .vsync_end = (vse), .vtotal = (vt), .vscan = (vs), \
-	.vrefresh = DIV_ROUND(c * 1000, ht * vt), .flags = (f), .type = 0
+	.vrefresh = DIV_ROUND(c * 1000, ht * vt) * (((f) & DRM_MODE_FLAG_INTERLACE) ? 2 : 1), \
+	.flags = (f), .type = 0
 
 /*
  * Probably taken from CEA-861 spec.
diff --git a/kms++/src/modedb_dmt.cpp b/kms++/src/modedb_dmt.cpp
index e3f2ad5..c3d5426 100644
--- a/kms++/src/modedb_dmt.cpp
+++ b/kms++/src/modedb_dmt.cpp
@@ -42,8 +42,8 @@ namespace kms
 	.name = nm, .clock = (c), \
 	.hdisplay = (hd), .hsync_start = (hss), .hsync_end = (hse), .htotal = (ht), .hskew = (hsk), \
 	.vdisplay = (vd), .vsync_start = (vss), .vsync_end = (vse), .vtotal = (vt), .vscan = (vs), \
-	.vrefresh = DIV_ROUND(c * 1000, ht * vt), .flags = (f), .type = 0
-
+	.vrefresh = DIV_ROUND(c * 1000, ht * vt) * (((f) & DRM_MODE_FLAG_INTERLACE) ? 2 : 1), \
+	.flags = (f), .type = 0
 
 /*
  * Autogenerated from the DMT spec.
diff --git a/kms++/src/videomode.cpp b/kms++/src/videomode.cpp
index 30d47f8..16330bb 100644
--- a/kms++/src/videomode.cpp
+++ b/kms++/src/videomode.cpp
@@ -16,4 +16,14 @@ unique_ptr<Blob> Videomode::to_blob(Card& card) const
 	return unique_ptr<Blob>(new Blob(card, &drm_mode, sizeof(drm_mode)));
 }
 
+bool Videomode::interlace() const
+{
+	return flags & DRM_MODE_FLAG_INTERLACE;
+}
+
+float Videomode::calculated_vrefresh() const
+{
+	return (clock * 1000.0) / (htotal * vtotal) * (interlace() ? 2 : 1);
+}
+
 }