From cb1313d51e225fd1b8fe8716f0316603d3943afb Mon Sep 17 00:00:00 2001
From: Thomas Sommer <thomas@tomsaw.de>
Date: Thu, 29 Jul 2021 17:21:11 +0200
Subject: [PATCH] ili9341 Resumable Functions

---
 src/modm/driver/display/ili9341.hpp           | 346 ++++-------
 src/modm/driver/display/ili9341.lb            |   7 +-
 src/modm/driver/display/ili9341_impl.hpp      | 585 ++++++++----------
 ...lel.hpp => ili9341_interface_parallel.hpp} |  42 +-
 .../driver/display/ili9341_interface_spi.hpp  | 178 ++++++
 src/modm/driver/display/ili9341_register.hpp  | 121 ++++
 src/modm/driver/display/ili9341_spi.hpp       | 110 ----
 src/modm/ui/color.hpp                         |   2 +-
 .../color/{brightness.hpp => grayscale.hpp}   |  32 +-
 src/modm/ui/color/hsv.hpp                     |  16 +-
 src/modm/ui/color/rgb.hpp                     |  25 +-
 src/modm/ui/color/rgb565.hpp                  |  21 +-
 src/modm/ui/color/rgbhtml.hpp                 |   1 +
 src/modm/ui/display/color_graphic_display.hpp |  42 +-
 src/modm/ui/display/graphic_display.cpp       | 181 +++---
 src/modm/ui/display/graphic_display.hpp       | 217 +++----
 src/modm/ui/display/graphic_display_fill.cpp  |  55 --
 .../ui/display/monochrome_graphic_display.hpp |   4 +-
 .../monochrome_graphic_display_vertical.hpp   |   6 +-
 ...nochrome_graphic_display_vertical_impl.hpp |   6 +-
 .../ui/display/virtual_graphic_display.cpp    |  22 +-
 .../ui/display/virtual_graphic_display.hpp    |  17 +-
 22 files changed, 999 insertions(+), 1037 deletions(-)
 rename src/modm/driver/display/{ili9341_parallel.hpp => ili9341_interface_parallel.hpp} (63%)
 create mode 100644 src/modm/driver/display/ili9341_interface_spi.hpp
 create mode 100644 src/modm/driver/display/ili9341_register.hpp
 delete mode 100644 src/modm/driver/display/ili9341_spi.hpp
 rename src/modm/ui/color/{brightness.hpp => grayscale.hpp} (66%)
 delete mode 100644 src/modm/ui/display/graphic_display_fill.cpp

diff --git a/src/modm/driver/display/ili9341.hpp b/src/modm/driver/display/ili9341.hpp
index b5cbc2e4de..93c8b7cc00 100644
--- a/src/modm/driver/display/ili9341.hpp
+++ b/src/modm/driver/display/ili9341.hpp
@@ -13,295 +13,183 @@
 
 #include <modm/architecture/interface/delay.hpp>
 #include <modm/architecture/interface/register.hpp>
-#include <modm/math/utils/endianness.hpp>
 #include <modm/platform/gpio/base.hpp>
-#include <modm/ui/display/color_graphic_display.hpp>
+#include <modm/processing/resumable.hpp>
+#include <modm/ui/display.hpp>
+
+#include "ili9341_interface_parallel.hpp"
+#include "ili9341_interface_spi.hpp"
 
 namespace modm
 {
 
 /// @ingroup modm_driver_ili9341
-struct ili9341
+template<class Interface, class Reset, size_t BufferSize = 1024>
+class Ili9341 : public Interface, public ColorGraphicDisplay
 {
-protected:
-	/// @cond
-	enum class
-	Command : uint8_t
-	{
-		// common commands
-		Nop = 0x00,
-		SwReset = 0x01,
-		ReadID = 0x04,
-		ReadStatus = 0x09,
-		ReadPowerMode = 0x0a,
-		ReadMemoryAccessCtrl = 0x0b,
-		ReadPixelFormat = 0x0c,
-		ReadImageFormat = 0x0d,
-		ReadSignalMode = 0x0e,
-		ReadSelfDiagnostic = 0x0f,
-		EnterSleep = 0x10,
-		LeaveSleep = 0x11,
-		PartialMode = 0x12,
-		NormalMode = 0x13,
-		InversionOff = 0x20,
-		InversionOn = 0x21,
-		GammaSet = 0x26,
-		DisplayOff = 0x28,
-		DisplayOn = 0x29,
-		ColumnAddressSet = 0x2a,
-		PageAddressSet = 0x2b,
-		MemoryWrite = 0x2c,
-		ColorSet = 0x2d,
-		MemoryRead = 0x2e,
-		PartialArea = 0x30,
-		VerticalScrollDefinition = 0x33,
-		TearingEffectOff = 0x34,
-		TearingEffectOn = 0x35,
-		MemoryAccessCtrl = 0x36,
-		VerticalScrollStartAddr = 0x37,
-		IdleModeOff = 0x38,
-		IdleModeOn = 0x39,
-		PixelFormatSet = 0x3a,
-		WriteMemoryContinue = 0x3c,
-		ReadMemoryContinue = 0x3e,
-		SetTearScanLine = 0x44,
-		GetScanLine = 0x45,
-		WriteBrightness = 0x51,
-		ReadBrightness = 0x52,
-		WriteCtrlDisplay = 0x53,
-		ReadCtrlDisplay = 0x54,
-		WriteContentAdaptiveBrightnessCtrl = 0x55,
-		ReadContentAdaptiveBrightnessCtrl = 0x56,
-		WriteCabcMinimumBrightness = 0x5e,
-		ReadCabcMinimumBrightness = 0x5f,
-		ReadId1 = 0xda,
-		ReadId2 = 0xdb,
-		ReadId3 = 0xdc,
-		// extended commands
-		RgbInterfaceSignalCtrl = 0xb0,
-		FrameCtrlNormalMode = 0xb1,
-		FrameCtrlIdleMode = 0xb2,
-		FrameCtrlPartialMode = 0xb3,
-		InversionCtrl = 0xb4,
-		BlankingPorchCtrl = 0xb5,
-		DisplayFunctionCtrl = 0xb6,
-		EntryModeSet = 0xb7,
-		BacklightCtrl1 = 0xb8,
-		BacklightCtrl2 = 0xb9,
-		BacklightCtrl3 = 0xba,
-		BacklightCtrl4 = 0xbb,
-		BacklightCtrl5 = 0xbc,
-		BacklightCtrl7 = 0xbe,
-		BacklightCtrl8 = 0xbf,
-		PowerCtrl1 = 0xc0,
-		PowerCtrl2 = 0xc1,
-		VComCtrl1 = 0xc5,
-		VComCtrl2 = 0xc7,
-		PowerCtrlA = 0xcb,
-		PowerCtrlB = 0xcf,
-		NvMemoryWrite = 0xd0,
-		NvMemoryProtectionKey = 0xd1,
-		NvMemoryStatus = 0xd2,
-		ReadId4 = 0xd3,
-		PositiveGammaCorrection = 0xe0,
-		NegativeGammaCorrection = 0xe1,
-		DigitalGammaCtrl = 0xe2,
-		TimingCtrlA = 0xe8,
-		TimingCtrlB = 0xea,
-		PowerOnSequenceCtrl = 0xed,
-		Enable3Gamma = 0xf2,
-		InterfaceCtrl = 0xf6,
-		PumpRatioCtrl = 0xf7,
-	};
-	static constexpr uint8_t i(Command command) { return uint8_t(command); }
+	static_assert(BufferSize >= 128, "Minimum Buffer is required");
 
-	enum class
-	MemoryAccessCtrl : uint8_t
-	{
-		MY = modm::Bit7,
-		MX = modm::Bit6,
-		MV = modm::Bit5,
-		ML = modm::Bit4,
-		BGR = modm::Bit3,
-		MH = modm::Bit2
-	};
-	MODM_FLAGS8(MemoryAccessCtrl);
-	/// @endcond
+	using Toggle = ili9341_register::Toggle;
+	using ReadWrite = ili9341_register::ReadWrite;
+	using Command = ili9341_register::Command;
+	using ReadCommand = ili9341_register::ReadCommand;
 
 public:
-	enum class
-	DisplayMode : uint8_t
-	{
-		Normal = uint8_t(Command::InversionOff),
-		Inverted = uint8_t(Command::InversionOn)
-	};
-};
-
-/// @ingroup modm_driver_ili9341
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize = 320>
-class Ili9341 : public Interface, public modm::ColorGraphicDisplay
-{
-	static_assert(BufferSize >= 16, "at least a small buffer is required");
-
-	static constexpr uint16_t Width = 240;
-	static constexpr uint16_t Height = 320;
-	using BatchHandle = typename Interface::BatchHandle;
-	using Command = ili9341::Command;
-public:
-	using Orientation = glcd::Orientation;
-	using DisplayMode = ili9341::DisplayMode;
-
 	template<typename... Args>
-	Ili9341(Args&&... args): Interface(std::forward<Args>(args)...)
+	Ili9341(Args &&...args) : Interface(std::forward<Args>(args)...)
 	{
 		Reset::setOutput(modm::Gpio::High);
-		Backlight::setOutput(modm::Gpio::Low);
 	}
 
-	void
+	~Ili9341(){};
+
+	modm::ResumableResult<void>
 	initialize();
 
-	void
+	modm::ResumableResult<void>
 	reset(bool hardReset = false);
 
-	uint16_t
+	modm::ResumableResult<uint16_t>
 	getIcModel();
 
-	void
-	turnOn();
-
-	void
-	turnOff();
-
-	void
-	enableBacklight(bool enable)
-	{ Backlight::set(enable); }
-
-	void
-	setBrightness(uint8_t level);
-
-	void
-	setInvert(bool invert);
-
-	void
-	setIdle(bool enable);
-
-	void
-	enableSleep(bool enable);
-
 	uint16_t
 	getWidth() const final
-	{
-		switch (orientation)
-		{
-			case Orientation::Portrait90:
-			case Orientation::Portrait270:
-				return Height;
-			default:
-				return Width;
-		}
-	}
+	{ return (orientation & display::OrientationFlags::Portrait) ? 240 : 320; }
 
 	uint16_t
 	getHeight() const final
-	{
-		switch (orientation)
-		{
-			case Orientation::Portrait90:
-			case Orientation::Portrait270:
-				return Width;
-			default:
-				return Height;
-		}
-	}
+	{ return (orientation & display::OrientationFlags::Portrait) ? 320 : 240; }
 
-	inline std::size_t
-	getBufferWidth() const final
-	{
-		return Width;
-	}
+	/* modm::ResumableResult<uint32_t>
+	getStatus(); // 5 bytes, Datasheet P92
 
-	inline std::size_t
-	getBufferHeight() const final
-	{
-		return Height;
-	}
+	modm::ResumableResult<uint16_t>
+	getPowerMode(); // 2 bytes, Datasheet P94
 
-	void
-	setPixel(int16_t x, int16_t y) final
-	{ setColoredPixel(x, y, foregroundColor); }
+	modm::ResumableResult<uint16_t>
+	getMadCtl(); // 2 bytes, Datasheet P95
 
-	void
-	clearPixel(int16_t x, int16_t y) final
-	{ setColoredPixel(x, y, backgroundColor); }
+	modm::ResumableResult<uint16_t>
+	getPixelFormat(); // 2 bytes, Datasheet P96 */
 
-    // TODO implement getPixel for ili9341
-	color::Rgb565
-	getPixel(int16_t x, int16_t y) const final
-	{
-		(void) x;
-		(void) y;
-		return modm::color::html::White;
-	}
+	modm::ResumableResult<void>
+	set(Toggle toggle, bool state);
 
-	void
-	clear() final;
+	modm::ResumableResult<void>
+	set(ReadWrite reg, uint8_t value);
 
-	inline void
-	update() final
-	{ /* nothing to do, data is directly written to TFT RAM */ }
+	modm::ResumableResult<uint8_t>
+	get(ReadWrite reg);
 
-	inline void
-	setOrientation(glcd::Orientation orientation);
+	// After change of Orientation, wait 1ms before sending new pixels
+	modm::ResumableResult<void>
+	setOrientation(display::Orientation orientation);
 
-	void
-	fillRectangle(glcd::Point upperLeft, uint16_t width, uint16_t height);
+	modm::ResumableResult<void>
+	setScrollArea(uint16_t topFixedRows, uint16_t bottomFixedRows, uint16_t firstRow);
 
-	inline void
-	fillRectangle(int16_t x, int16_t y, uint16_t width, uint16_t height)
-	{ fillRectangle(glcd::Point(x, y), width, height); }
+	modm::ResumableResult<void>
+	scrollTo(uint16_t row);
 
+	// Some rectangular drawing functions, send directly to the display
+	// Using DMA (f.e. SpiMaster_Dma) is highly recommended!
 	void
-	fillCircle(glcd::Point center, uint16_t radius);
+	setPixel(display::Point pos);
 
 	void
-	drawImageRaw(glcd::Point upperLeft,
-				 uint16_t width, uint16_t height,
-				 modm::accessor::Flash<uint8_t> data) final;
+	clearPixel(display::Point pos);
 
 	void
-	drawRaw(glcd::Point upperLeft, uint16_t width, uint16_t height, color::Rgb565* data);
+	setPixel(display::Point pos, bool value);
 
 	void
-	setScrollArea(uint16_t topFixedRows, uint16_t bottomFixedRows, uint16_t firstRow);
+	setPixel(display::Point pos, modm::color::Rgb565<true> color);
+
+	color::Rgb565<true>
+	getPixel(display::Point) const final {
+		// TODO implement
+		return modm::color::html::Black;
+	}
 
 	void
-	scrollTo(uint16_t row);
+	drawHorizontalLine(display::Point origin, uint16_t length);
 
 	void
-	drawBitmap(glcd::Point upperLeft, uint16_t width, uint16_t height, modm::accessor::Flash<uint8_t> data);
+	drawVerticalLine(display::Point origin, uint16_t length);
 
-protected:
 	void
-	drawHorizontalLine(glcd::Point start, uint16_t length) final;
+	fillRectangle(display::Point origin, uint16_t width, uint16_t height);
+
+	modm::ResumableResult<void>
+	clear(color::Rgb565<true> color = color::html::Black);
 
 	void
-	drawVerticalLine(glcd::Point start, uint16_t length) final;
+	update() final {};
 
-private:
+	// TODO Reimplement
+	// Send monochrome image stored in flash directly to the screen
 	void
-	setColoredPixel(int16_t x, int16_t y, color::Rgb565 const &color);
+	drawImageRaw(display::Point origin, uint16_t width, uint16_t height,
+		modm::accessor::Flash<uint8_t> data);
 
+	// TODO Reimplement
+	// Send color-image stored in flash directly to the screen
 	void
-	setClipping(uint16_t x, uint16_t y, uint16_t width, uint16_t height);
+	drawBitmap(display::Point origin, uint16_t width, uint16_t height,
+		modm::accessor::Flash<uint8_t> data);
 
-	Orientation orientation{Orientation::Landscape0};
+	// TODO Reimplement
+	// Send a colored graphic-buffer directly to the screen
+	void
+	drawRaw(display::Point origin,
+	uint16_t width, uint16_t height, color::Rgb565<false>* data);
 
-	uint8_t buffer[BufferSize * 2]{0};
+protected:
+	bool
+	xOnScreen(int16_t x) const
+	{ return x >= 0 and x < int16_t(getWidth()); }
+
+	bool
+	yOnScreen(int16_t y) const
+	{ return y >= 0 and y < int16_t(getHeight()); }
+
+	bool
+	pointOnScreen(display::Point position) const
+	{ return xOnScreen(position.x) and yOnScreen(position.y); }
+
+	// setClipping for a single pixel
+	modm::ResumableResult<void>
+	setClipping(display::Point pos);
+
+	// setClipping for an area
+	modm::ResumableResult<void>
+	setClipping(display::Point origin, display::Point bottomRight);
+
+	// fill an area with buffer
+	modm::ResumableResult<void>
+	fill(display::Point origin, uint16_t width, uint16_t height);
+
+	// "Local" variables, required for resumable functions
+
+	union {
+		// Buffers for Commands & Configuration
+		uint8_t buffer8[15];
+		uint16_t buffer16[3];
+		ili9341_register::MemoryAccessCtrl_t madCtrl;
+
+		// Parallel use in resumable function
+		// No overlap permitted!
+		struct {
+			modm::color::Rgb565<true> buffer[BufferSize];
+			display::Intersection intersection;
+			uint16_t clipping_buffer[2];
+			uint32_t write_pixels;
+		} p; // p for parallel
+	};
 };
 
-} // namespace modm
+}  // namespace modm
 
 #include "ili9341_impl.hpp"
 
-#endif //  MODM_ILI9341_HPP
+#endif  //  MODM_ILI9341_HPP
diff --git a/src/modm/driver/display/ili9341.lb b/src/modm/driver/display/ili9341.lb
index 63333894e3..3fdc37498d 100644
--- a/src/modm/driver/display/ili9341.lb
+++ b/src/modm/driver/display/ili9341.lb
@@ -15,6 +15,7 @@ def init(module):
     module.name = ":driver:ili9341"
     module.description = "ILI9341 Display with parallel and SPI bus transports"
 
+
 def prepare(module, options):
     module.depends(
         ":architecture:delay",
@@ -22,9 +23,11 @@ def prepare(module, options):
         ":ui:display")
     return True
 
+
 def build(env):
     env.outbasepath = "modm/src/modm/driver/display"
     env.copy("ili9341.hpp")
+    env.copy("ili9341_register.hpp")
     env.copy("ili9341_impl.hpp")
-    env.copy("ili9341_spi.hpp")
-    env.copy("ili9341_parallel.hpp")
+    env.copy("ili9341_interface_spi.hpp")
+    env.copy("ili9341_interface_parallel.hpp")
diff --git a/src/modm/driver/display/ili9341_impl.hpp b/src/modm/driver/display/ili9341_impl.hpp
index d44ce85c69..4f51e879fc 100644
--- a/src/modm/driver/display/ili9341_impl.hpp
+++ b/src/modm/driver/display/ili9341_impl.hpp
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2019, Mike Wolfram
+ * Copyright (c) 2021, Thomas Sommer
  *
  * This file is part of the modm project.
  *
@@ -9,322 +10,333 @@
  */
 
 #ifndef MODM_ILI9341_HPP
-#	error	"Don't include this file directly, use 'ili9341.hpp' instead!"
+#error "Don't include this file directly, use 'ili9341.hpp' instead!"
 #endif
 
 namespace modm
 {
 
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
-void
-Ili9341<Interface, Reset, Backlight, BufferSize>::initialize()
+#define MODM_INIT_BUFFER(...) {\
+	constexpr uint8_t data[]__VA_ARGS__;\
+	std::copy(std::begin(data), std::end(data), std::begin(buffer8));\
+	}
+
+template<class Interface, class Reset, size_t BufferSize>
+modm::ResumableResult<void>
+Ili9341<Interface, Reset, BufferSize>::initialize()
 {
-	constexpr uint8_t pwrCtrlA[] { 0x39, 0x2c, 0x00, 0x34, 0x02 };
-	constexpr uint8_t pwrCtrlB[] { 0x00, 0xc1, 0x30 };
-	constexpr uint8_t timCtrlA[] { 0x85, 0x10, 0x7a };
-	constexpr uint8_t timCtrlB[] { 0x00, 0x00 };
-	constexpr uint8_t pwrOnSeqCtrl[] { 0x64, 0x03, 0x12, 0x81 };
-	constexpr uint8_t pwrCtrl1[] { 0x1b };
-	constexpr uint8_t pwrCtrl2[] { 0x12 };
-	constexpr uint8_t vcomCtrl1[] { 0x08, 0x26 };//0x3e, 0x28 };
-	constexpr uint8_t vcomCtrl2[] { 0xb7 };
+	RF_BEGIN();
+	RF_CALL(reset());
+	RF_CALL(set(Toggle::Enable, false));
+
+	MODM_INIT_BUFFER({0x39, 0x2c, 0x00, 0x34, 0x02})
+	RF_CALL(this->writeCommand(Command::PowerCtrlA, buffer8, 5));
+
+ 	MODM_INIT_BUFFER({0x00, 0xc1, 0x30})
+	RF_CALL(this->writeCommand(Command::PowerCtrlB, buffer8, 3));
+
+	MODM_INIT_BUFFER({0x85, 0x10, 0x7a})
+	RF_CALL(this->writeCommand(Command::TimingCtrlA, buffer8, 3));
+
+	MODM_INIT_BUFFER({0x00, 0x00})
+	RF_CALL(this->writeCommand(Command::TimingCtrlB, buffer8, 2));
+
+	MODM_INIT_BUFFER({0x64, 0x03, 0x12, 0x81})
+	RF_CALL(this->writeCommand(Command::PowerOnSequenceCtrl, buffer8, 4));
+
+	RF_CALL(this->writeCommand(Command::PowerCtrl1, 0x1b));
+	RF_CALL(this->writeCommand(Command::PowerCtrl2, 0x12));
+
+	MODM_INIT_BUFFER({0x08, 0x26})  // 0x3e, 0x28 };
+	RF_CALL(this->writeCommand(Command::VComCtrl1, buffer8, 2));
+
+	RF_CALL(this->writeCommand(Command::VComCtrl2, 0xb7));
 	// constexpr uint8_t pumpRatioCtrl[] { 0x20 };
-	constexpr uint8_t pixelFormat[] { 0x55 };
-	constexpr uint8_t frameCtrl[] { 0x00, 0x1a };
-	constexpr uint8_t dispFuncCtrl[] { 0x0a, 0xa2, 0x27, 00 };//{ 0x08, 0x82, 0x27, 0x00 };
-	constexpr uint8_t enable3G[] { 0x00 };
-	constexpr uint8_t gammaSet[] { 0x01 };
-//	constexpr uint8_t positiveGammaCorr[] { 0x0f, 0x31, 0x2b, 0x0c, 0x0e, 0x08,
-//			0xe4, 0xf1, 0x37, 0x07, 0x10, 0x03, 0x0e, 0x09, 0x00 };
-//	constexpr uint8_t negativeGammaCorr[] { 0x00, 0x0e, 0x14, 0x03, 0x11, 0x07,
-//			0x31, 0xc1, 0x48, 0x08, 0x0f, 0x0c, 0x31, 0x36, 0x0f };
-	constexpr uint8_t positiveGammaCorr[] { 0x0f, 0x1d, 0x1a, 0x0a, 0x0d, 0x07,
-			0x49, 0x66, 0x3b, 0x07, 0x11, 0x01, 0x09, 0x05, 0x04 };
-	constexpr uint8_t negativeGammaCorr[] { 0x00, 0x18, 0x1d, 0x02, 0x0f, 0x04,
-			0x36, 0x13, 0x4c, 0x07, 0x13, 0x0f, 0x2e, 0x2f, 0x05 };
-
-	reset();
+	RF_CALL(this->writeCommand(Command::PixelFormatSet, 0x55));
 
-	{
-		BatchHandle h(*this);
-
-		this->writeCommand(Command::DisplayOff);
-
-		this->writeCommand(Command::PowerCtrlA, pwrCtrlA, sizeof(pwrCtrlA));
-		this->writeCommand(Command::PowerCtrlB, pwrCtrlB, sizeof(pwrCtrlB));
-		this->writeCommand(Command::TimingCtrlA, timCtrlA, sizeof(timCtrlA));
-		this->writeCommand(Command::TimingCtrlB, timCtrlB, sizeof(timCtrlB));
-		this->writeCommand(Command::PowerOnSequenceCtrl, pwrOnSeqCtrl, sizeof(pwrOnSeqCtrl));
-		this->writeCommand(Command::PowerCtrl1, pwrCtrl1, sizeof(pwrCtrl1));
-		this->writeCommand(Command::PowerCtrl2, pwrCtrl2, sizeof(pwrCtrl2));
-		this->writeCommand(Command::VComCtrl1, vcomCtrl1, sizeof(vcomCtrl1));
-		this->writeCommand(Command::VComCtrl2, vcomCtrl2, sizeof(vcomCtrl2));
-		this->writeCommand(Command::PixelFormatSet, pixelFormat, sizeof(pixelFormat));
-		this->writeCommand(Command::FrameCtrlNormalMode, frameCtrl, sizeof(frameCtrl));
-		this->writeCommand(Command::DisplayFunctionCtrl, dispFuncCtrl, sizeof(dispFuncCtrl));
-		this->writeCommand(Command::Enable3Gamma, enable3G, sizeof(enable3G));
-		this->writeCommand(Command::GammaSet, gammaSet, sizeof(gammaSet));
-		this->writeCommand(Command::PositiveGammaCorrection, positiveGammaCorr,
-				sizeof(positiveGammaCorr));
-		this->writeCommand(Command::NegativeGammaCorrection, negativeGammaCorr,
-				sizeof(negativeGammaCorr));
-
-		this->writeCommand(Command::LeaveSleep);
-		modm::delay_ms(120);
-		this->writeCommand(Command::InversionOff);
-		this->writeCommand(Command::DisplayOn);
-
-		setOrientation(orientation);
-	}
+	MODM_INIT_BUFFER({0x00, 0x1a})
+	RF_CALL(this->writeCommand(Command::FrameCtrlNormalMode, buffer8, 2));
+
+	MODM_INIT_BUFFER({0x0a, 0xa2, 0x27, 0x00})  //{ 0x08, 0x82, 0x27, 0x00 };
+	RF_CALL(this->writeCommand(Command::DisplayFunctionCtrl, buffer8, 4));
+
+	RF_CALL(this->writeCommand(Command::Enable3Gamma, 0x00));
+	RF_CALL(this->writeCommand(Command::GammaSet, 0x01));
+
+	// MODM_INIT_BUFFER({ 0x0f, 0x31, 0x2b, 0x0c, 0x0e, 0x08, 0xe4, 0xf1, 0x37, 0x07, 0x10, 0x03, 0x0e, 0x09, 0x00});
+	MODM_INIT_BUFFER({0x0f, 0x1d, 0x1a, 0x0a, 0x0d, 0x07, 0x49, 0x66, 0x3b, 0x07, 0x11, 0x01, 0x09, 0x05, 0x04})
+	RF_CALL(this->writeCommand(Command::PositiveGammaCorrection, buffer8, 15));
+
+	// MODM_INIT_BUFFER({ 0x00, 0x0e, 0x14, 0x03, 0x11, 0x07, 0x31, 0xc1, 0x48, 0x08, 0x0f, 0x0c, 0x31, 0x36, 0x0f});
+	MODM_INIT_BUFFER({0x00, 0x18, 0x1d, 0x02, 0x0f, 0x04, 0x36, 0x13, 0x4c, 0x07, 0x13, 0x0f, 0x2e, 0x2f, 0x05})
+	RF_CALL(this->writeCommand(Command::NegativeGammaCorrection, buffer8, 15));
+
+	RF_CALL(set(Toggle::SleepDisable, true));
+	modm::delay(120ms);
+	RF_CALL(set(Toggle::Inversion, false));
+	RF_CALL(setOrientation(orientation));
+	RF_CALL(set(Toggle::Enable, true));
+
+	RF_END();
 }
 
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
-void
-Ili9341<Interface, Reset, Backlight, BufferSize>::reset(bool hardReset /* = false */)
+#undef MODM_INIT_BUFFER
+
+template<class Interface, class Reset, size_t BufferSize>
+modm::ResumableResult<void>
+Ili9341<Interface, Reset, BufferSize>::reset(bool hardReset /* = false */)
 {
+	RF_BEGIN();
+
+	// TODO use modm::ShortTimeout;
 	if (hardReset)
 	{
 		Reset::set();
-		modm::delay_ms(5);
+		modm::delay(5ms);
 		Reset::reset();
-		modm::delay_ms(5);
+		modm::delay(5ms);
 		Reset::set();
-		modm::delay_ms(5);
-	}
-	else {
-		BatchHandle h(*this);
-		this->writeCommand(Command::SwReset);
-		modm::delay_ms(5);
+		modm::delay(5ms);
+	} else
+	{
+		RF_CALL(this->writeCommand(Command::SwReset));
+		modm::delay(5ms);
 	}
+	RF_END();
 }
 
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
-uint16_t
-Ili9341<Interface, Reset, Backlight, BufferSize>::getIcModel()
+template<class Interface, class Reset, size_t BufferSize>
+modm::ResumableResult<uint16_t>
+Ili9341<Interface, Reset, BufferSize>::getIcModel()
 {
-	BatchHandle h(*this);
-
-	uint8_t buffer[4] { 0 };
-	this->readData(Command::ReadId4, buffer, 4);
-	return (buffer[2] << 8) | buffer[3];
+	RF_BEGIN();
+	RF_CALL(this->readData(ReadCommand::Id, buffer8, 4));
+	RF_END_RETURN(buffer8[2] << 8 | buffer8[3]);
 }
 
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
-inline void
-Ili9341<Interface, Reset, Backlight, BufferSize>::setOrientation(glcd::Orientation orientation)
+template<class Interface, class Reset, size_t BufferSize>
+modm::ResumableResult<void>
+Ili9341<Interface, Reset, BufferSize>::set(Toggle toggle, bool state)
 {
-	using MemoryAccessCtrl_t = ili9341::MemoryAccessCtrl_t;
-	using MemoryAccessCtrl = ili9341::MemoryAccessCtrl;
-	MemoryAccessCtrl_t madCtrl { MemoryAccessCtrl::BGR };
-
-	switch (orientation)
-	{
-		case glcd::Orientation::Portrait90:
-			madCtrl |= MemoryAccessCtrl::MV | MemoryAccessCtrl::MX;
-			break;
-		case glcd::Orientation::Landscape180:
-			madCtrl |= MemoryAccessCtrl::MX | MemoryAccessCtrl::MY;
-			break;
-		case glcd::Orientation::Portrait270:
-			madCtrl |= MemoryAccessCtrl::MV | MemoryAccessCtrl::MY;
-			break;
-		default:
-//			madCtrl |= MemoryAccessCtrl::ML;
-			break;
-	}
-
-	this->orientation = orientation;
-
-	BatchHandle h(*this);
-	this->writeCommandValue8(Command::MemoryAccessCtrl, madCtrl.value);
+	RF_BEGIN();
+	RF_CALL(this->writeCommand(Command(uint8_t(toggle) | uint8_t(state))));
+	RF_END();
 }
 
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
-void
-Ili9341<Interface, Reset, Backlight, BufferSize>::turnOn()
+template<class Interface, class Reset, size_t BufferSize>
+modm::ResumableResult<void>
+Ili9341<Interface, Reset, BufferSize>::set(ReadWrite reg, uint8_t value)
 {
-	BatchHandle h(*this);
-	this->writeCommand(Command::DisplayOn);
+	RF_BEGIN();
+	RF_CALL(this->writeCommand(Command(reg), value));
+	RF_END();
 }
 
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
-void
-Ili9341<Interface, Reset, Backlight, BufferSize>::turnOff()
+template<class Interface, class Reset, size_t BufferSize>
+modm::ResumableResult<uint8_t>
+Ili9341<Interface, Reset, BufferSize>::get(ReadWrite reg)
 {
-	BatchHandle h(*this);
-	this->writeCommand(Command::DisplayOff);
+	RF_BEGIN();
+	RF_END_RETURN_CALL(this->readData(Command(uint8_t(reg) | uint8_t(Bit0))));
 }
 
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
-void
-Ili9341<Interface, Reset, Backlight, BufferSize>::setIdle(bool enable)
+template<class Interface, class Reset, size_t BufferSize>
+modm::ResumableResult<void>
+Ili9341<Interface, Reset, BufferSize>::setOrientation(
+	display::Orientation orientation)
 {
-	BatchHandle h(*this);
-	this->writeCommand(enable ? Command::IdleModeOn : Command::IdleModeOff);
+	RF_BEGIN();
+	this->orientation = orientation;
+
+	using MemoryAccessCtrl = ili9341_register::MemoryAccessCtrl;
+	using OrientationFlags = modm::display::OrientationFlags;
+
+	madCtrl = MemoryAccessCtrl::PIXEL_DIR;
+
+	if (orientation & OrientationFlags::Portrait)
+		madCtrl |= MemoryAccessCtrl::MEMORY_MODE;
+
+	if (orientation & OrientationFlags::Rotate180)
+		madCtrl |= MemoryAccessCtrl::MEMORY_DIR_Y;
+
+	if (bool(orientation & OrientationFlags::Portrait) == bool(orientation & OrientationFlags::Rotate180))
+		madCtrl |= MemoryAccessCtrl::MEMORY_DIR_X;
+
+	RF_CALL(this->writeCommand(Command::MemoryAccessCtrl, madCtrl.value));
+
+	RF_END();
 }
 
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
-void
-Ili9341<Interface, Reset, Backlight, BufferSize>::enableSleep(bool enable)
+template<class Interface, class Reset, size_t BufferSize>
+modm::ResumableResult<void>
+Ili9341<Interface, Reset, BufferSize>::setScrollArea(uint16_t topFixedRows,
+																	uint16_t bottomFixedRows,
+																	uint16_t firstRow)
 {
-	BatchHandle h(*this);
-	this->writeCommand(enable ? Command::EnterSleep : Command::LeaveSleep);
+	RF_BEGIN();
+
+	buffer16[0] = modm::fromBigEndian(topFixedRows);
+	buffer16[1] = modm::fromBigEndian(firstRow);
+	buffer16[2] = modm::fromBigEndian(bottomFixedRows);
+
+	RF_CALL(this->writeCommand(Command::VerticalScrollDefinition, buffer8, 6));
+	RF_END();
 }
 
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
-void
-Ili9341<Interface, Reset, Backlight, BufferSize>::setBrightness(uint8_t level)
+template<class Interface, class Reset, size_t BufferSize>
+modm::ResumableResult<void>
+Ili9341<Interface, Reset, BufferSize>::scrollTo(uint16_t row)
 {
-	BatchHandle h(*this);
-	this->writeCommand(Command::WriteBrightness, &level, 1);
+	RF_BEGIN();
+
+	buffer16[0] = modm::fromBigEndian(row);
+
+	RF_CALL(this->writeCommand(Command::VerticalScrollStartAddr, buffer8, 2));
+	RF_END();
 }
 
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
-void
-Ili9341<Interface, Reset, Backlight, BufferSize>::setInvert(bool invert)
+template<class Interface, class Reset, size_t BufferSize>
+modm::ResumableResult<void>
+Ili9341<Interface, Reset, BufferSize>::setClipping(display::Point pos)
 {
-	BatchHandle h(*this);
-	this->writeCommand(invert ? Command::InversionOn : Command::InversionOff);
+	RF_BEGIN();
+
+	p.clipping_buffer[0] = modm::fromBigEndian(pos.x);
+	p.clipping_buffer[1] = p.clipping_buffer[0];
+	RF_CALL(this->writeCommand(Command::PageAddressSet, (uint8_t *)(p.clipping_buffer), 4));
+
+	p.clipping_buffer[0] = modm::fromBigEndian(pos.y);
+	p.clipping_buffer[1] = p.clipping_buffer[0];
+	RF_CALL(this->writeCommand(Command::ColumnAddressSet, (uint8_t *)(p.clipping_buffer), 4));
+
+	RF_CALL(this->writeCommand(Command::MemoryWrite));
+
+	RF_END();
 }
 
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
-void
-Ili9341<Interface, Reset, Backlight, BufferSize>::clear()
+template<class Interface, class Reset, size_t BufferSize>
+modm::ResumableResult<void>
+Ili9341<Interface, Reset, BufferSize>::setClipping(display::Point origin,
+																  display::Point bottomRight)
 {
-	auto const saveForegroundColor { foregroundColor };
-	foregroundColor = backgroundColor;
-	fillRectangle(glcd::Point(0, 0), Width, Height);
-	foregroundColor = saveForegroundColor;
+	RF_BEGIN();
+
+	p.clipping_buffer[0] = modm::fromBigEndian(origin.x);
+	p.clipping_buffer[1] = modm::fromBigEndian(int16_t(bottomRight.x - 1));
+	RF_CALL(this->writeCommand(Command::PageAddressSet, (uint8_t *)(p.clipping_buffer), 4));
+
+	p.clipping_buffer[0] = modm::fromBigEndian(origin.y);
+	p.clipping_buffer[1] = modm::fromBigEndian(int16_t(bottomRight.y - 1));
+	RF_CALL(this->writeCommand(Command::ColumnAddressSet, (uint8_t *)(p.clipping_buffer), 4));
+
+	RF_CALL(this->writeCommand(Command::MemoryWrite));
+
+	RF_END();
 }
 
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
+template<class Interface, class Reset, size_t BufferSize>
 void
-Ili9341<Interface, Reset, Backlight, BufferSize>::drawHorizontalLine(
-		glcd::Point start, uint16_t length)
+Ili9341<Interface, Reset, BufferSize>::setPixel(display::Point pos)
 {
-	uint16_t const pixelValue { modm::toBigEndian(foregroundColor.color) };
-	auto minLength { std::min(std::size_t(length), BufferSize) };
-	uint16_t *buffer16 { reinterpret_cast<uint16_t *>(buffer) };
-	std::fill(buffer16, buffer16+minLength, pixelValue);
-
-	BatchHandle h(*this);
-
-	setClipping(start.getX(), start.getY(), length, 1);
-	while (length > BufferSize)
+	if (pointOnScreen(pos))
 	{
-		this->writeData(buffer, BufferSize * 2);
-		length -= BufferSize;
+		RF_CALL_BLOCKING(setClipping(pos));
+		RF_CALL_BLOCKING(this->writeData(color));
 	}
-	this->writeData(buffer, length * 2);
 }
 
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
+template<class Interface, class Reset, size_t BufferSize>
 void
-Ili9341<Interface, Reset, Backlight, BufferSize>::drawVerticalLine(
-		glcd::Point start, uint16_t length)
+Ili9341<Interface, Reset, BufferSize>::setPixel(display::Point pos, modm::color::Rgb565<true> color)
 {
-	uint16_t const pixelValue { modm::toBigEndian(foregroundColor.color) };
-	auto minLength { std::min(std::size_t(length), BufferSize) };
-	uint16_t *buffer16 { reinterpret_cast<uint16_t *>(buffer) };
-	std::fill(buffer16, buffer16+minLength, pixelValue);
-
-	BatchHandle h(*this);
-
-	setClipping(start.getX(), start.getY(), 1, length);
-	while (length > BufferSize)
+	if (pointOnScreen(pos))
 	{
-		this->writeData(buffer, BufferSize * 2);
-		length -= BufferSize;
+		RF_CALL_BLOCKING(setClipping(pos));
+		RF_CALL_BLOCKING(this->writeData(color));
 	}
-	this->writeData(buffer, length * 2);
 }
 
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
+template<class Interface, class Reset, size_t BufferSize>
 void
-Ili9341<Interface, Reset, Backlight, BufferSize>::fillRectangle(
-		glcd::Point upperLeft, uint16_t width, uint16_t height)
+Ili9341<Interface, Reset, BufferSize>::drawHorizontalLine(
+	display::Point start, uint16_t length)
 {
-	auto const x { upperLeft.getX() };
-	auto const y { upperLeft.getY() };
-	std::size_t pixelCount { std::size_t(width) * std::size_t(height) };
-
-	uint16_t const pixelValue { modm::toBigEndian(foregroundColor.color) };
-	auto minLength { std::min(std::size_t(pixelCount), BufferSize) };
-	uint16_t *buffer16 { reinterpret_cast<uint16_t *>(buffer) };
-	std::fill(buffer16, buffer16+minLength, pixelValue);
-
-	BatchHandle h(*this);
+	fillRectangle(start, length, 1);
+}
 
-	setClipping(x, y, width, height);
-	while (pixelCount > BufferSize)
-	{
-		this->writeData(buffer, BufferSize * 2);
-		pixelCount -= BufferSize;
-	}
-	if (pixelCount)
-		this->writeData(buffer, pixelCount * 2);
+template<class Interface, class Reset, size_t BufferSize>
+void
+Ili9341<Interface, Reset, BufferSize>::drawVerticalLine(
+	display::Point start, uint16_t length)
+{
+	if(xOnScreen(start.x))
+		fillRectangle(start, 1, length);
 }
 
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
+template<class Interface, class Reset, size_t BufferSize>
 void
-Ili9341<Interface, Reset, Backlight, BufferSize>::fillCircle(
-		glcd::Point center, uint16_t radius)
+Ili9341<Interface, Reset, BufferSize>::fillRectangle(
+	display::Point origin, uint16_t width, uint16_t height)
 {
-	uint8_t const setColor[] { uint8_t((foregroundColor.color >> 8) & 0xff),
-			uint8_t(foregroundColor.color & 0xff) };
+	std::fill(&p.buffer[0], &p.buffer[BufferSize], color);
+	RF_CALL_BLOCKING(fill(origin, width, height));
+}
 
-	int16_t f = 1 - radius;
-	int16_t ddF_x = 0;
-	int16_t ddF_y = -2 * radius;
-	uint16_t x = 0;
-	uint16_t y = radius;
+template<class Interface, class Reset, size_t BufferSize>
+modm::ResumableResult<void>
+Ili9341<Interface, Reset, BufferSize>::clear(color::Rgb565<true> color)
+{
+	RF_BEGIN();
+	std::fill(&p.buffer[0], &p.buffer[BufferSize], color);
+	RF_CALL(fill({0, 0}, this->getWidth(), this->getHeight()));
+	RF_END();
+}
 
-	BatchHandle h(*this);
+// -------------------------------------------------------------
 
-	setClipping(center.getX() - radius, center.getY(), 2 * radius, 1);
-	for (std::size_t i = 0; i < 2 * radius; ++i)
-		this->writeData(setColor, 2);
+template<class Interface, class Reset, size_t BufferSize>
+modm::ResumableResult<void>
+Ili9341<Interface, Reset, BufferSize>::fill(
+	display::Point origin, uint16_t width, uint16_t height)
+{
+	RF_BEGIN();
 
-	while(x < y)
+	p.intersection = display::Intersection(origin, width, height, this->getWidth(), this->getHeight());
+	RF_CALL(setClipping(p.intersection.topLeft, p.intersection.bottomRight));
+
+	p.write_pixels = p.intersection.getPixelCount();
+
+	while (true)
 	{
-		if (f >= 0)
+		if (p.write_pixels > BufferSize)
+		{
+			RF_CALL(this->writeData((uint8_t *)(p.buffer), 2 * BufferSize));
+			p.write_pixels -= BufferSize;
+		} else
 		{
-			y--;
-			ddF_y += 2;
-			f += ddF_y;
+			RF_CALL(this->writeData((uint8_t *)(p.buffer), 2 * p.write_pixels));
+			break;
 		}
-		x++;
-		ddF_x += 2;
-		f += ddF_x + 1;
-
-		setClipping(center.getX() - x, center.getY() - y, 2 * x, 1);
-		for (std::size_t i = 0; i < 2 * x; ++i)
-			this->writeData(setColor, 2);
-		setClipping(center.getX() - y, center.getY() - x, 2 * y, 1);
-		for (std::size_t i = 0; i < 2 * y; ++i)
-			this->writeData(setColor, 2);
-		setClipping(center.getX() - x, center.getY() + y, 2 * x, 1);
-		for (std::size_t i = 0; i < 2 * x; ++i)
-			this->writeData(setColor, 2);
-		setClipping(center.getX() - y, center.getY() + x, 2 * y, 1);
-		for (std::size_t i = 0; i < 2 * y; ++i)
-			this->writeData(setColor, 2);
 	}
+	RF_END();
 }
 
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
+template <class Interface, class Reset, std::size_t BufferSize>
 void
-Ili9341<Interface, Reset, Backlight, BufferSize>::drawImageRaw(glcd::Point upperLeft,
+Ili9341<Interface, Reset, BufferSize>::drawImageRaw(display::Point origin,
 		uint16_t width, uint16_t height, modm::accessor::Flash<uint8_t> data)
 {
-	uint8_t const setColor[] { uint8_t((foregroundColor.color >> 8) & 0xff),
-			uint8_t(foregroundColor.color & 0xff) };
+	(void)origin;
+	(void)width;
+	(void)height;
+	(void)data;
+/* 	uint8_t const setColor[] { uint8_t((color.color >> 8) & 0xff),
+			uint8_t(color.color & 0xff) };
 	uint8_t const clearColor[] { uint8_t((backgroundColor.color >> 8) & 0xff),
 			uint8_t(backgroundColor.color & 0xff) };
 
-	BatchHandle h(*this);
 
-	setClipping(upperLeft.getX(), upperLeft.getY(), width, height);
+	RF_CALL(setClipping(origin, origin + display::Point(width, height)));
 
 	uint8_t bit = 0x01;
 	for (uint16_t r = 0; r < height; ++r)
@@ -341,100 +353,35 @@ Ili9341<Interface, Reset, Backlight, BufferSize>::drawImageRaw(glcd::Point upper
 		bit <<= 1;
 		if (bit == 0)
 			bit = 0x01;
-	}
+	} */
 }
 
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
+template <class Interface, class Reset, size_t BufferSize>
 void
-Ili9341<Interface, Reset, Backlight, BufferSize>::drawRaw(glcd::Point upperLeft,
-		uint16_t width, uint16_t height, color::Rgb565* data)
+Ili9341<Interface, Reset, BufferSize>::drawBitmap(display::Point origin,
+		uint16_t width, uint16_t height, modm::accessor::Flash<uint8_t> data)
 {
-	BatchHandle h(*this);
-
-	uint16_t* buffer = (uint16_t*)data;
-	for(size_t i = 0; i < size_t(width*height); i++) {
-		buffer[i] = modm::fromBigEndian(buffer[i]);
+	RF_CALL_BLOCKING(setClipping(origin, origin + display::Point(width, height)));
+	for (int i = 0; i < width * height; ++i) {
+		p.buffer[0] = data[i*2+1];
+		p.buffer[1] = data[i*2];
+		this->writeData(p.buffer, 2);
 	}
-
-	setClipping(upperLeft.getX(), upperLeft.getY(), width, height);
-	this->writeData((uint8_t*)buffer, width * height * 2);
-}
-
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
-void
-Ili9341<Interface, Reset, Backlight, BufferSize>::setScrollArea(
-		uint16_t topFixedRows, uint16_t bottomFixedRows, uint16_t firstRow)
-{
-	BatchHandle h(*this);
-
-	uint8_t arg[]
-	{
-		uint8_t((topFixedRows >> 8) & 0xff), uint8_t(topFixedRows & 0xff),
-		uint8_t((firstRow >> 8) & 0xff), uint8_t(firstRow & 0xff),
-		uint8_t((bottomFixedRows >> 8) & 0xff), uint8_t(bottomFixedRows & 0xff)
-	};
-	this->writeCommand(Command::VerticalScrollDefinition, arg, 6);
-}
-
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
-void
-Ili9341<Interface, Reset, Backlight, BufferSize>::scrollTo(uint16_t row)
-{
-	BatchHandle h(*this);
-
-	uint8_t arg[] { uint8_t((row >> 8) & 0xff), uint8_t(row & 0xff) };
-	this->writeCommand(Command::VerticalScrollStartAddr, arg, 2);
-}
-
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
-void
-Ili9341<Interface, Reset, Backlight, BufferSize>::setColoredPixel(
-		int16_t x, int16_t y, color::Rgb565 const &color)
-{
-	auto const pixelColor { color };
-	uint8_t const setColor[] { uint8_t((pixelColor.color >> 8) & 0xff), uint8_t(pixelColor.color & 0xff) };
-
-	BatchHandle h(*this);
-
-	this->setClipping(x, y, 1, 1);
-	this->writeData(setColor, 2);
-}
-
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
-void
-Ili9341<Interface, Reset, Backlight, BufferSize>::setClipping(
-		uint16_t x, uint16_t y, uint16_t width, uint16_t height)
-{
-	uint8_t buffer[4];
-
-	buffer[0] = uint8_t((x >> 8) & 0xff);
-	buffer[1] = uint8_t(x & 0xff);
-	buffer[2] = uint8_t(((x + width - 1) >> 8) & 0xff);
-	buffer[3] = uint8_t((x + width - 1) & 0xff);
-	this->writeCommand(Command::ColumnAddressSet, buffer, 4);
-
-	buffer[0] = uint8_t((y >> 8) & 0xff);
-	buffer[1] = uint8_t(y & 0xff);
-	buffer[2] = uint8_t(((y + height - 1) >> 8) & 0xff);
-	buffer[3] = uint8_t((y + height - 1) & 0xff);
-	this->writeCommand(Command::PageAddressSet, buffer, 4);
-	this->writeCommand(Command::MemoryWrite);
+//	this->writeData(data.getPointer(), width * height * 2);
 }
 
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize>
+template<class Interface, class Reset, size_t BufferSize>
 void
-Ili9341<Interface, Reset, Backlight, BufferSize>::drawBitmap(glcd::Point upperLeft,
-		uint16_t width, uint16_t height, modm::accessor::Flash<uint8_t> data)
+Ili9341<Interface, Reset, BufferSize>::drawRaw(display::Point origin,
+		uint16_t width, uint16_t height, color::Rgb565<false>* data)
 {
-	BatchHandle h(*this);
-
-	setClipping(upperLeft.getX(), upperLeft.getY(), width, height);
-	for (int i = 0; i < width * height; ++i) {
-		buffer[0] = data[i*2+1];
-		buffer[1] = data[i*2];
-		this->writeData(buffer, 2);
+/* 	uint16_t* p.buffer = (uint16_t*)data;
+	for(size_t i = 0; i < size_t(width*height); i++) {
+		p.buffer[i] = modm::fromBigEndian(p.buffer[i]);
 	}
-//	this->writeData(data.getPointer(), width * height * 2);
+
+	RF_CALL_BLOCKING(setClipping(origin, origin + display::Point(width, height)));
+	RF_CALL_BLOCKING(this->writeData((uint8_t*)p.buffer, width * height * 2)); */
 }
 
-} // namespace modm
+}  // namespace modm
\ No newline at end of file
diff --git a/src/modm/driver/display/ili9341_parallel.hpp b/src/modm/driver/display/ili9341_interface_parallel.hpp
similarity index 63%
rename from src/modm/driver/display/ili9341_parallel.hpp
rename to src/modm/driver/display/ili9341_interface_parallel.hpp
index 4a282e32a3..d773ba0847 100644
--- a/src/modm/driver/display/ili9341_parallel.hpp
+++ b/src/modm/driver/display/ili9341_interface_parallel.hpp
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2020, Pavel Pletenev
+ * Copyright (c) 2021, Thomas Sommer
  *
  * This file is part of the modm project.
  *
@@ -7,32 +8,37 @@
  * License, v. 2.0. If a copy of the MPL was not distributed with this
  * file, You can obtain one at http://mozilla.org/MPL/2.0/.
  */
-#ifndef MODM_ILI9341_PARALLEL_HPP
-#define MODM_ILI9341_PARALLEL_HPP
+#pragma once
 
-#include "ili9341.hpp"
+#ifndef MODM_ILI9341_HPP
+#error "Don't include this file directly, use 'ili9341.hpp' instead!"
+#endif
+
+#include <modm/math/utils/endianness.hpp>
+
+#include "ili9341_register.hpp"
 
 namespace modm
 {
 
 /// @ingroup modm_driver_ili9341
-template<class INTERFACE>
-class Ili9341ParallelInterface: public ili9341
+template<class Interface>
+class Ili9341InterfaceParallel: public ili9341_register
 {
-	INTERFACE& interface;
+	Interface& interface;
 public:
-	Ili9341ParallelInterface(INTERFACE& interface)
+	Ili9341InterfaceParallel(Interface& interface)
 	: interface(interface) {}
 
-	__attribute__((noinline)) void
+	void
 	writeCommand(Command command)
 	{
-		interface.writeIndex(i(command));
+		interface.writeIndex(uint8_t(command));
 	}
-	__attribute__((noinline)) void
+	void
 	writeCommand(Command command, uint8_t const *args, std::size_t length)
 	{
-		interface.writeIndex(i(command));
+		interface.writeIndex(uint8_t(command));
 		for(std::size_t i=0; i<length; ++i)
 			interface.writeData(args[i]);
 	}
@@ -53,7 +59,7 @@ class Ili9341ParallelInterface: public ili9341
 	void
 	readData(Command command, uint8_t *buffer, std::size_t length)
 	{
-		interface.writeIndex(i(command));
+		interface.writeIndex(uint8_t(command));
 		interface.readData();
 		for(std::size_t i=0; i<length; ++i)
 			buffer[i] = interface.readData();
@@ -69,17 +75,9 @@ class Ili9341ParallelInterface: public ili9341
 public:
 	struct BatchHandle
 	{
-		BatchHandle(Ili9341ParallelInterface&) {}
+		BatchHandle(Ili9341InterfaceParallel&) {}
 		~BatchHandle() {}
 	};
 };
 
-/// @ingroup modm_driver_ili9341
-template <class Interface, class Reset, class Backlight, std::size_t BufferSize = 320>
-using Ili9341Parallel = Ili9341<
-	Ili9341ParallelInterface<Interface>,
-	Reset, Backlight, BufferSize>;
-
-} // namespace modm
-
-#endif // MODM_ILI9341_PARALLEL_HPP
+} // namespace modm
\ No newline at end of file
diff --git a/src/modm/driver/display/ili9341_interface_spi.hpp b/src/modm/driver/display/ili9341_interface_spi.hpp
new file mode 100644
index 0000000000..9039a151eb
--- /dev/null
+++ b/src/modm/driver/display/ili9341_interface_spi.hpp
@@ -0,0 +1,178 @@
+/*
+ * Copyright (c) 2019, Mike Wolfram
+ * Copyright (c) 2021, Thomas Sommer
+ *
+ * This file is part of the modm project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+#pragma once
+
+#ifndef MODM_ILI9341_HPP
+#error "Don't include this file directly, use 'ili9341.hpp' instead!"
+#endif
+
+#include <modm/architecture/interface/spi_device.hpp>
+#include <modm/platform/spi/spi_base.hpp>
+#include <modm/processing/resumable.hpp>
+
+#include "ili9341_register.hpp"
+
+namespace modm
+{
+
+/// @ingroup modm_driver_ili9341
+template<class SpiMaster, class Cs, class Dc>
+class Ili9341InterfaceSpi : public ili9341_register, public modm::SpiDevice< SpiMaster >, protected modm::NestedResumable<4>
+{
+	uint8_t read;
+public:
+	Ili9341InterfaceSpi()
+	{
+		this->attachConfigurationHandler([]() {
+			SpiMaster::setDataMode(SpiMaster::DataMode::Mode0);
+			SpiMaster::setDataOrder(SpiMaster::DataOrder::MsbFirst);
+		});
+		Cs::setOutput(modm::Gpio::High);
+		Dc::setOutput();
+	}
+
+	modm::ResumableResult<void>
+	writeCommand(Command command)
+	{
+		RF_BEGIN();
+
+		RF_WAIT_UNTIL(this->acquireMaster());
+		Cs::reset();
+
+		Dc::reset();
+		RF_CALL(SpiMaster::transfer(uint8_t(command)));
+		Dc::set();
+
+		if (this->releaseMaster())
+			Cs::set();
+
+		RF_END();
+	}
+
+	modm::ResumableResult<void>
+	writeCommand(Command command, const uint8_t data)
+	{
+		RF_BEGIN();
+
+		RF_WAIT_UNTIL(this->acquireMaster());
+		Cs::reset();
+
+		Dc::reset();
+		RF_CALL(SpiMaster::transfer(uint8_t(command)));
+		Dc::set();
+
+		RF_CALL(SpiMaster::transfer(data));
+
+		if (this->releaseMaster())
+			Cs::set();
+
+		RF_END();
+	}
+
+	modm::ResumableResult<void>
+	writeCommand(Command command, const uint8_t *data, std::size_t length)
+	{
+		RF_BEGIN();
+
+		RF_WAIT_UNTIL(this->acquireMaster());
+		Cs::reset();
+
+		Dc::reset();
+		RF_CALL(SpiMaster::transfer(uint8_t(command)));
+		Dc::set();
+
+		RF_CALL(SpiMaster::transfer(data, nullptr, length));
+
+		if (this->releaseMaster())
+			Cs::set();
+
+		RF_END();
+	}
+
+	modm::ResumableResult<void>
+	writeData(const uint8_t *data, size_t length)
+	{
+		RF_BEGIN();
+
+		RF_WAIT_UNTIL(this->acquireMaster());
+		Cs::reset();
+
+		RF_CALL(SpiMaster::transfer(data, nullptr, length));
+
+		if (this->releaseMaster())
+			Cs::set();
+
+		RF_END();
+	}
+
+	// OPTIMIZE determine if implementation is optimal
+	modm::ResumableResult<void>
+	writeData(color::Rgb565<true> rgb565)
+	{
+		RF_BEGIN();
+
+		RF_WAIT_UNTIL(this->acquireMaster());
+		Cs::reset();
+
+		RF_CALL(SpiMaster::transfer((uint8_t*)(&rgb565.color), nullptr, 2));
+
+		// Alternative
+		// RF_CALL(SpiMaster::transfer(rgb565.color >> 8));
+		// RF_CALL(SpiMaster::transfer(rgb565.color & 0xFF));
+
+		if (this->releaseMaster())
+			Cs::set();
+
+		RF_END();
+	}
+
+	modm::ResumableResult<uint8_t>
+	readData(Command command)
+	{
+		RF_BEGIN();
+
+		RF_WAIT_UNTIL(this->acquireMaster());
+		Cs::reset();
+
+		Dc::reset();
+		RF_CALL(SpiMaster::transfer(uint8_t(command)));
+		Dc::set();
+
+		read = RF_CALL(SpiMaster::transfer(0x00)).getResult();
+
+		if (this->releaseMaster())
+			Cs::set();
+
+		RF_END_RETURN(read);
+	}
+
+	modm::ResumableResult<void>
+	readData(Command command, uint8_t *buffer, size_t length)
+	{
+		RF_BEGIN();
+
+		RF_WAIT_UNTIL(this->acquireMaster());
+		Cs::reset();
+
+		Dc::reset();
+		RF_CALL(SpiMaster::transfer(uint8_t(command)));
+		Dc::set();
+
+		RF_CALL(SpiMaster::transfer(nullptr, buffer, length));
+
+		if (this->releaseMaster())
+			Cs::set();
+
+		RF_END();
+	}
+};
+
+}  // namespace modm
\ No newline at end of file
diff --git a/src/modm/driver/display/ili9341_register.hpp b/src/modm/driver/display/ili9341_register.hpp
new file mode 100644
index 0000000000..e5c3725d88
--- /dev/null
+++ b/src/modm/driver/display/ili9341_register.hpp
@@ -0,0 +1,121 @@
+#pragma once
+
+#include <modm/math/utils/bit_constants.hpp>
+
+namespace modm {
+
+/// @ingroup modm_driver_ili9341
+struct ili9341_register
+{
+public:
+	// Bit0 => false: Off, true: On
+	enum Toggle : uint8_t {
+		SleepDisable = 0x10,
+		Inversion = 0x20,
+		Enable = 0x28,
+		TearingEffect = 0x34,
+		Idle = 0x38,
+	};
+
+	// Bit0 => false: Write, true: Read
+	enum class ReadWrite : uint8_t {
+		Brightness = 0x51,
+		CtrlDisplay = 0x53,
+		ContentAdaptiveBrightnessCtrl = 0x55,
+		CabcMinimumBrightness = 0x5E,
+	};
+protected:
+	enum class Command : uint8_t
+	{
+		Nop = 0x00,
+		SwReset = 0x01,
+
+		GammaSet = 0x26,
+		ColumnAddressSet = 0x2A,
+		PageAddressSet = 0x2B,
+		MemoryWrite = 0x2C,
+		ColorSet = 0x2D,
+		MemoryRead = 0x2E,
+
+		PartialMode = 0x12,
+		NormalMode = 0x13,
+
+		PartialArea = 0x30,
+		VerticalScrollDefinition = 0x33,
+		MemoryAccessCtrl = 0x36,
+		VerticalScrollStartAddr = 0x37,
+
+		PixelFormatSet = 0x3A,
+		WriteMemoryContinue = 0x3C,
+		ReadMemoryContinue = 0x3E,
+		SetTearScanLine = 0x44,
+		GetScanLine = 0x45,
+
+		// Extended
+		RgbInterfaceSignalCtrl = 0xB0,
+		FrameCtrlNormalMode = 0xB1,
+		FrameCtrlIdleMode = 0xB2,
+		FrameCtrlPartialMode = 0xB3,
+		InversionCtrl = 0xB4,
+		BlankingPorchCtrl = 0xB5,
+		DisplayFunctionCtrl = 0xB6,
+		EntryModeSet = 0xB7,
+		BacklightCtrl1 = 0xB8,
+		BacklightCtrl2 = 0xB9,
+		BacklightCtrl3 = 0xBA,
+		BacklightCtrl4 = 0xBB,
+		BacklightCtrl5 = 0xBC,
+		BacklightCtrl7 = 0xBE,
+		BacklightCtrl8 = 0xBF,
+		PowerCtrl1 = 0xC0,
+		PowerCtrl2 = 0xC1,
+		VComCtrl1 = 0xC5,
+		VComCtrl2 = 0xC7,
+		PowerCtrlA = 0xCB,
+		PowerCtrlB = 0xCF,
+		NvMemoryWrite = 0xD0,
+		NvMemoryProtectionKey = 0xD1,
+		NvMemoryStatus = 0xD2,
+		PositiveGammaCorrection = 0xE0,
+		NegativeGammaCorrection = 0xE1,
+		DigitalGammaCtrl = 0xE2,
+		TimingCtrlA = 0xE8,
+		TimingCtrlB = 0xEA,
+		PowerOnSequenceCtrl = 0xED,
+		Enable3Gamma = 0xF2,
+		InterfaceCtrl = 0xF6,
+		PumpRatioCtrl = 0xF7,
+	};
+
+	enum class ReadCommand : uint8_t {
+		// TODO got 0s only for each of ReadId*
+		Id = 0x04,				// returns 4 bytes
+		Id1 = 0xDA,
+		Id2 = 0xDB,
+		Id3 = 0xDC,
+		Id4 = 0xD3,
+
+		Status = 0x09,			// returns 5 bytes
+		PowerMode = 0x0A,		// returns 2 bytes
+		MemoryAccessCtrl = 0x0B,// returns 2 bytes
+		PixelFormat = 0x0C,		// returns 2 bytes
+		ImageFormat = 0x0D,		// returns 2 bytes
+		SignalMode = 0x0E,		// returns 2 bytes
+		SelfDiagnostic = 0x0F,	// returns 2 bytes
+	};
+
+	enum class MemoryAccessCtrl : uint8_t
+	{
+		// Rgb565 bits-order
+		PIXEL_DIR = Bit3,		// 0: RGB, 1: BGR
+		// LCD Refresh
+		REFRESH_DIR_X = Bit2,	// 0: Left->Right, 1: Right->Left
+		REFRESH_DIR_Y = Bit4,	// 0: Top->Bottom, 1: Bottom->Top
+		// Memory Access
+		MEMORY_MODE = Bit5,		// 0: Normal, 1: Reverse
+		MEMORY_DIR_X = Bit6,	// 0: Left->Right, 1: Right->Left
+		MEMORY_DIR_Y = Bit7		// 0: Top->Bottom, 1: Bottom->Top
+	};
+	MODM_FLAGS8(MemoryAccessCtrl);
+};
+}
\ No newline at end of file
diff --git a/src/modm/driver/display/ili9341_spi.hpp b/src/modm/driver/display/ili9341_spi.hpp
deleted file mode 100644
index 7f145dde49..0000000000
--- a/src/modm/driver/display/ili9341_spi.hpp
+++ /dev/null
@@ -1,110 +0,0 @@
-/*
- * Copyright (c) 2019, Mike Wolfram
- *
- * This file is part of the modm project.
- *
- * This Source Code Form is subject to the terms of the Mozilla Public
- * License, v. 2.0. If a copy of the MPL was not distributed with this
- * file, You can obtain one at http://mozilla.org/MPL/2.0/.
- */
-#ifndef MODM_ILI9341_SPI_HPP
-#define MODM_ILI9341_SPI_HPP
-
-#include "ili9341.hpp"
-#include <modm/architecture/interface/spi_device.hpp>
-#include <modm/platform/spi/spi_base.hpp>
-
-namespace modm
-{
-
-/// @ingroup modm_driver_ili9341
-template<class SPI, class Cs, class Dc>
-class Ili9341SPIInterface: public ili9341, public modm::SpiDevice<SPI>
-{
-public:
-	Ili9341SPIInterface()
-	{
-		this->attachConfigurationHandler([]() {
-			SPI::setDataMode(SPI::DataMode::Mode0);
-			SPI::setDataOrder(SPI::DataOrder::MsbFirst);
-		});
-		Cs::setOutput(modm::Gpio::High);
-		Dc::setOutput();
-	}
-
-	__attribute__((noinline)) void
-	writeCommand(Command command)
-	{
-		Dc::reset(); // enable command
-		SPI::transferBlocking(i(command));
-		Dc::set(); // reset to data
-	}
-	__attribute__((noinline)) void
-	writeCommand(Command command, uint8_t const *args, std::size_t length)
-	{
-		Dc::reset(); // enable command
-		SPI::transferBlocking(i(command));
-		Dc::set(); // reset to data
-		if (length != 0)
-		{
-			SPI::transferBlocking(const_cast<unsigned char *>(args), nullptr, length);
-		}
-	}
-	void
-	writeData(uint8_t const *data, std::size_t length)
-	{
-		SPI::transferBlocking(const_cast<unsigned char *>(data), nullptr, length);
-	}
-	void
-	writeCommandValue8(Command command, uint8_t value)
-	{
-		writeCommand(command, &value, 1);
-	}
-
-	void
-	readData(Command command, uint8_t *buffer, std::size_t length)
-	{
-		using modm::platform::SpiBase;
-		uint8_t b[4];
-
-		Dc::reset(); // enable command
-		// SPI::Hal::setDataSize(SpiBase::DataSize::Bit9);
-		SPI::transferBlocking(i(command) << 1);
-		SPI::Hal::setDataSize(SpiBase::DataSize::Bit8);
-		Dc::set(); // reset to data
-		SPI::transferBlocking(b /*nullptr*/, buffer, length);
-	}
-	uint8_t
-	readData(Command command)
-	{
-		writeCommand(command);
-		return SPI::transferBlocking(0x00);
-	}
-
-public:
-	struct BatchHandle
-	{
-		Ili9341SPIInterface& i;
-		BatchHandle(Ili9341SPIInterface& iface)
-		: i(iface)
-		{
-			i.acquireMaster();
-			Cs::reset();
-		}
-		~BatchHandle()
-		{
-			if (i.releaseMaster())
-				Cs::set();
-		}
-	};
-};
-
-/// @ingroup modm_driver_ili9341
-template <class SPI, class Cs, class Dc, class Reset, class Backlight, std::size_t BufferSize = 320>
-using Ili9341Spi = Ili9341<
-	Ili9341SPIInterface<SPI, Cs, Dc>,
-	Reset, Backlight, BufferSize>;
-
-} // namespace modm
-
-#endif // MODM_ILI9341_SPI_HPP
diff --git a/src/modm/ui/color.hpp b/src/modm/ui/color.hpp
index 64e4ef53ce..8fe3d1b110 100644
--- a/src/modm/ui/color.hpp
+++ b/src/modm/ui/color.hpp
@@ -15,7 +15,7 @@
 
 #include "color/rgb.hpp"
 #include "color/hsv.hpp"
-#include "color/brightness.hpp"
+#include "color/grayscale.hpp"
 
 #include "color/rgb565.hpp"
 #include "color/rgbhtml.hpp"
diff --git a/src/modm/ui/color/brightness.hpp b/src/modm/ui/color/grayscale.hpp
similarity index 66%
rename from src/modm/ui/color/brightness.hpp
rename to src/modm/ui/color/grayscale.hpp
index bc5e961cb5..da6404ba0f 100644
--- a/src/modm/ui/color/brightness.hpp
+++ b/src/modm/ui/color/grayscale.hpp
@@ -29,34 +29,35 @@ class RgbT;
 template<std::unsigned_integral T>
 class HsvT;
 
+template<bool BigEndian>
 class Rgb565;
 
 /**
- * @brief		Brightness as unsigned integral. Add's the math for conversion to and from
- *				Color-Types. Use with: Grayscale Buffers, Dimmed LEDs, Brightness sensors
+ * @brief		Grayscale as unsigned integral. Add's the math for conversion to and from
+ *				Color-Types. Use with: Grayscale Buffers, Dimmed LEDs, Grayscale sensors
  *
  * @author		Thomas Sommer
  *
- * @tparam	T	Unsigned integral for the brightness-value
+ * @tparam	T	Unsigned integral for the grayscale-value
  * @ingroup		modm_ui_color
  */
 template<std::unsigned_integral T>
-class BrightnessT
+class GrayscaleT
 {
 public:
 	T value{0};
 
-	constexpr BrightnessT() = default;
+	constexpr GrayscaleT() = default;
 
-	constexpr BrightnessT(T value) : value(value) {}
+	constexpr GrayscaleT(T value) : value(value) {}
 
 	/**
 	 * Copy Constructor 8bit->16bit
 	 */
 	template<std::unsigned_integral U>
 		requires std::is_same_v<T, uint16_t> && std::is_same_v<U, uint8_t>
-	constexpr BrightnessT(const BrightnessT<U> &brightness_other)
-		: value(brightness_other.value << 8)
+	constexpr GrayscaleT(const GrayscaleT<U> &other)
+		: value(other.value << 8)
 	{}
 
 	/**
@@ -64,8 +65,8 @@ class BrightnessT
 	 */
 	template<std::unsigned_integral U>
 		requires std::is_same_v<T, uint8_t> && std::is_same_v<U, uint16_t>
-	constexpr BrightnessT(const BrightnessT<U> &brightness_other)
-		: value(brightness_other.value >> 8)
+	constexpr GrayscaleT(const GrayscaleT<U> &other)
+		: value(other.value >> 8)
 	{}
 
 	/**
@@ -74,7 +75,7 @@ class BrightnessT
 	 * @param rgb	RGB Color
 	 */
 	template<std::unsigned_integral U>
-	constexpr BrightnessT(RgbT<U> rgb)
+	constexpr GrayscaleT(RgbT<U> rgb)
 		: value((0.2125 * float(rgb.red)) + (0.7154 * float(rgb.green)) +
 				(0.0721 * float(rgb.blue)))
 	{}
@@ -85,7 +86,7 @@ class BrightnessT
 	 * @param hsv	HSV Color
 	 */
 	template<std::unsigned_integral U>
-	constexpr BrightnessT(HsvT<U> hsv) : value(hsv.value)
+	constexpr GrayscaleT(HsvT<U> hsv) : value(hsv.value)
 	{}
 
 	/**
@@ -93,13 +94,14 @@ class BrightnessT
 	 *
 	 * @param rgb565	RGB565 Color
 	 */
-	constexpr BrightnessT(Rgb565 rgb565) : BrightnessT<T>(RgbT<uint8_t>(rgb565)) {}
+	template<bool BigEndian>
+	constexpr GrayscaleT(Rgb565<BigEndian> rgb565) : GrayscaleT<T>(RgbT<uint8_t>(rgb565)) {}
 
 	constexpr bool
-	operator==(const BrightnessT<T> &other) const = default;
+	operator==(const GrayscaleT<T> &other) const = default;
 };
 
 /// @ingroup modm_ui_color
-using Brightness = BrightnessT<uint8_t>;
+using Grayscale = GrayscaleT<uint8_t>;
 
 }  // namespace modm::color
diff --git a/src/modm/ui/color/hsv.hpp b/src/modm/ui/color/hsv.hpp
index 57fada273b..1ab1d87be7 100644
--- a/src/modm/ui/color/hsv.hpp
+++ b/src/modm/ui/color/hsv.hpp
@@ -19,7 +19,7 @@
 #include <stdint.h>
 #include <concepts>
 
-#include "brightness.hpp"
+#include "grayscale.hpp"
 #include "rgb.hpp"
 #include "rgb565.hpp"
 
@@ -31,8 +31,9 @@ template<std::unsigned_integral T>
 class RgbT;
 
 template<std::unsigned_integral T>
-class BrightnessT;
+class GrayscaleT;
 
+template<bool BigEndian>
 class Rgb565;
 
 /**
@@ -45,6 +46,8 @@ template<std::unsigned_integral T>
 class HsvT
 {
 public:
+	static constexpr bool isColor = true;
+
 	T hue{0};
 	T saturation{0};
 	T value{0};
@@ -80,12 +83,12 @@ class HsvT
 	constexpr HsvT(const RgbT<U>& rgb);
 
 	/**
-	 * Convertion Constructor for Brightness
+	 * Convertion Constructor for Grayscale
 	 *
-	 * @param brightness	Brightness 'Color'-object
+	 * @param grayscale	Grayscale 'Color'-object
 	 */
 	template<std::unsigned_integral U>
-	constexpr HsvT(const BrightnessT<U> gray) : hue(0), saturation(0), value(gray.value)
+	constexpr HsvT(const GrayscaleT<U> gray) : hue(0), saturation(0), value(gray.value)
 	{}
 
 	/**
@@ -93,7 +96,8 @@ class HsvT
 	 *
 	 * @param rgb565	RGB565 Color
 	 */
-	constexpr HsvT(const Rgb565& rgb565) : HsvT(RgbT<uint8_t>(rgb565)) {}
+	template<bool BigEndian>
+	constexpr HsvT(const Rgb565<BigEndian>& rgb565) : HsvT(RgbT<uint8_t>(rgb565)) {}
 
 	constexpr bool
 	operator==(const HsvT<T>& other) const = default;
diff --git a/src/modm/ui/color/rgb.hpp b/src/modm/ui/color/rgb.hpp
index cb5df9f00a..a7f856a6eb 100644
--- a/src/modm/ui/color/rgb.hpp
+++ b/src/modm/ui/color/rgb.hpp
@@ -24,7 +24,7 @@
 #include <modm/math/utils/arithmetic_traits.hpp>
 #include <type_traits>
 
-#include "brightness.hpp"
+#include "grayscale.hpp"
 #include "hsv.hpp"
 #include "rgb565.hpp"
 
@@ -36,8 +36,9 @@ template<std::unsigned_integral T>
 class HsvT;
 
 template<std::unsigned_integral T>
-class BrightnessT;
+class GrayscaleT;
 
+template <bool BigEndian>
 class Rgb565;
 
 /**
@@ -50,6 +51,8 @@ template<std::unsigned_integral T>
 class RgbT
 {
 public:
+	static constexpr bool isColor = true;
+
 	T red{0};
 	T green{0};
 	T blue{0};
@@ -87,14 +90,14 @@ class RgbT
 	constexpr RgbT(const HsvT<U>& hsv);
 
 	/**
-	 * Convertion Constructor for Brightness
+	 * Convertion Constructor for Grayscale
 	 *
-	 * @param brightness	Brightness 'Color'-object
+	 * @param grayscale	Grayscale 'Color'-object
 	 */
 	// TODO Plump conversion, implement the right way
 	template<std::unsigned_integral U>
-	constexpr RgbT(const BrightnessT<U> brightness)
-		: red(brightness), green(brightness), blue(brightness)
+	constexpr RgbT(const GrayscaleT<U> grayscale)
+		: red(grayscale.value), green(grayscale.value), blue(grayscale.value)
 	{}
 
 	/**
@@ -102,12 +105,20 @@ class RgbT
 	 *
 	 * @param rgb565	RGB565 Color
 	 */
-	constexpr RgbT(const Rgb565& rgb565)
+	constexpr RgbT(const Rgb565<false>& rgb565)
 		: red((rgb565.color >> 8) & 0xF8),
 		  green((rgb565.color >> 3) & 0xFC),
 		  blue(rgb565.color << 3)
 	{}
 
+	constexpr RgbT(const Rgb565<true>& rgb565)
+	{
+		uint16_t color = std::rotr(rgb565.color, 8);
+		red = (rgb565.color >> 8) & 0xF8;
+		green = (rgb565.color >> 3) & 0xFC;
+		blue = rgb565.color << 3;
+	}
+
 	constexpr bool
 	operator==(const RgbT<T>& other) const = default;
 
diff --git a/src/modm/ui/color/rgb565.hpp b/src/modm/ui/color/rgb565.hpp
index dbb19aba56..4e4b33b169 100644
--- a/src/modm/ui/color/rgb565.hpp
+++ b/src/modm/ui/color/rgb565.hpp
@@ -12,8 +12,9 @@
 #pragma once
 
 #include <algorithm>
+#include <bit>
 
-#include "brightness.hpp"
+#include "grayscale.hpp"
 #include "hsv.hpp"
 #include "rgb.hpp"
 
@@ -28,7 +29,7 @@ template<std::unsigned_integral T>
 class HsvT;
 
 template<std::unsigned_integral T>
-class BrightnessT;
+class GrayscaleT;
 
 /**
  * Color in RGB Colorspace, 16 bits: RRRR RGGG GGGB BBBB
@@ -36,9 +37,13 @@ class BrightnessT;
  * @author		Fabian Greif, Thomas Sommer
  * @ingroup		modm_ui_color
  */
+
+template<bool BigEndian = false>
 class Rgb565
 {
 public:
+	static constexpr bool isColor = true;
+
 	uint16_t color{0x0000};
 
 	using RgbCalcType = RgbT<uint8_t>;
@@ -61,7 +66,9 @@ class Rgb565
 	 */
 	constexpr Rgb565(uint8_t red, uint8_t green, uint8_t blue)
 		: color(uint16_t(red & 0xF8) << 8 | uint16_t(green & 0xFC) << 3 | uint16_t(blue >> 3))
-	{}
+	{
+		if(BigEndian) color = std::rotr(color, 8);
+	}
 
 	/**
 	 * Convertion Constructor for RGB Color
@@ -82,12 +89,12 @@ class Rgb565
 	{}
 
 	/**
-	 * Convertion Constructor for Brightness
+	 * Convertion Constructor for Grayscale
 	 *
-	 * @param brightness	Brightness 'Color'-object
+	 * @param grayscale	Grayscale 'Color'-object
 	 */
 	template<std::unsigned_integral U>
-	constexpr Rgb565(const BrightnessT<U> brightness) : Rgb565(RgbCalcType(brightness))
+	constexpr Rgb565(const GrayscaleT<U> grayscale) : Rgb565(RgbCalcType(grayscale))
 	{}
 
 	constexpr bool
@@ -97,6 +104,7 @@ class Rgb565
 	Rgb565
 	operator+(const Rgb565 other) const
 	{
+		// FIXME implement for BigEndian == true
 		const int8_t red_raw = (color >> 11) + (other.color >> 11);
 		const uint16_t red = std::clamp<int8_t>(red_raw, 0, 31) << 11;
 
@@ -113,6 +121,7 @@ class Rgb565
 	Rgb565
 	operator-(const Rgb565 other) const
 	{
+		// FIXME implement for BigEndian == true
 		const int8_t red_raw = (color >> 11) - (other.color >> 11);
 		const uint16_t red = std::clamp<int8_t>(red_raw, 0, 0x1F) << 11;
 
diff --git a/src/modm/ui/color/rgbhtml.hpp b/src/modm/ui/color/rgbhtml.hpp
index 71d6c9dccd..51abcaca86 100644
--- a/src/modm/ui/color/rgbhtml.hpp
+++ b/src/modm/ui/color/rgbhtml.hpp
@@ -21,6 +21,7 @@ namespace modm::color::html
  *
  * @see 		https://htmlcolorcodes.com/color-names/
  * @see 		modm:color:RgbT
+ * @author      Thomas Sommer
  * @ingroup 	modm_ui_color
  * @{
  */
diff --git a/src/modm/ui/display/color_graphic_display.hpp b/src/modm/ui/display/color_graphic_display.hpp
index c9a6cbf5ef..750cc9bac0 100644
--- a/src/modm/ui/display/color_graphic_display.hpp
+++ b/src/modm/ui/display/color_graphic_display.hpp
@@ -13,47 +13,13 @@ class ColorGraphicDisplay : public GraphicDisplay
 {
 public:
 	ColorGraphicDisplay()
-		: foregroundColor(color::html::White), backgroundColor(color::html::Black)
+		: color(color::html::White)
 	{}
 
-	virtual color::Rgb565
-	getPixel(int16_t x, int16_t y) const = 0;
+	virtual color::Rgb565<true>
+	getPixel(display::Point pos) const = 0;
 
-	/**
-	 * Set a new foreground color.
-	 * Used for drawing operations. Default is white.
-	 */
-	inline void
-	setColor(const color::Rgb565 color)
-	{
-		foregroundColor = color;
-	}
-
-	inline color::Rgb565
-	getColor() const
-	{
-		return foregroundColor;
-	}
-
-	/**
-	 * Set background color.
-	 * Used when clearing the screen. Default is black.
-	 */
-	inline void
-	setBackgroundColor(const color::Rgb565 color)
-	{
-		backgroundColor = color;
-	}
-
-	inline color::Rgb565
-	getBackgroundColor() const
-	{
-		return backgroundColor;
-	}
-
-protected:
-	color::Rgb565 foregroundColor;
-	color::Rgb565 backgroundColor;
+	color::Rgb565<true> color;
 };
 }  // namespace modm
 
diff --git a/src/modm/ui/display/graphic_display.cpp b/src/modm/ui/display/graphic_display.cpp
index 1c5727e66e..d790357c1f 100644
--- a/src/modm/ui/display/graphic_display.cpp
+++ b/src/modm/ui/display/graphic_display.cpp
@@ -23,59 +23,63 @@
 
 #include "font/fixed_width_5x8.hpp"
 
+using namespace modm::display;
+
 // ----------------------------------------------------------------------------
 modm::GraphicDisplay::GraphicDisplay()
 	: IOStream(writer), writer(this), font(modm::accessor::asFlash(modm::font::FixedWidth5x8))
 {}
 
+#include <modm/debug/logger.hpp>
+
 // ----------------------------------------------------------------------------
 void
-modm::GraphicDisplay::drawLine(int16_t x1, int16_t y1, int16_t x2, int16_t y2)
+modm::GraphicDisplay::drawLine(Point start, Point end)
 {
-	if (x1 == x2)
+	if (start.x == end.x)
 	{
-		// x1|y1 must be the upper point
-		if (y1 > y2) { modm::swap(y1, y2); }
-		this->drawVerticalLine(glcd::Point(x1, y1), y2 - y1 + 1);
-	} else if (y1 == y2)
+		// start.x|start.y must be the upper point
+		if (start.y > end.y) { modm::swap(start.y, end.y); }
+		this->drawVerticalLine(start, end.y - start.y + 1);
+	} else if (start.y == end.y)
 	{
-		// x1|y1 must be the left point
-		if (x1 > x2) { modm::swap(x1, x2); }
-		this->drawHorizontalLine(glcd::Point(x1, y1), x2 - x1 + 1);
+		// start.x|start.y must be the left point
+		if (start.x > end.x) { modm::swap(start.x, end.x); }
+		this->drawHorizontalLine(start, end.x - start.x + 1);
 	} else
 	{
 		// bresenham algorithm
-		bool steep = abs(y2 - y1) > abs(x2 - x1);
+		bool steep = abs(end.y - start.y) > abs(end.x - start.x);
 		if (steep)
 		{
-			modm::swap(x1, y1);
-			modm::swap(x2, y2);
+			modm::swap(start.x, start.y);
+			modm::swap(end.x, end.y);
 		}
 
-		if (x1 > x2)
+		if (start.x > end.x)
 		{
-			modm::swap(x1, x2);
-			modm::swap(y1, y2);
+			modm::swap(start.x, end.x);
+			modm::swap(start.y, end.y);
 		}
 
-		int16_t deltaX = x2 - x1;
-		int16_t deltaY = abs(y2 - y1);
+		int16_t deltaX = end.x - start.x;
+		int16_t deltaY = abs(end.y - start.y);
 		int16_t error = deltaX / 2;
 
 		int16_t yStep;
-		int16_t y = y1;
+		int16_t y = start.y;
 
-		if (y1 < y2)
+		if (start.y < end.y)
 			yStep = 1;
 		else
 			yStep = -1;
 
-		for (int_fast16_t x = x1; x <= x2; ++x)
+		for (int_fast16_t x = start.x; x <= end.x; ++x)
 		{
 			if (steep)
-				this->setPixel(y, x);
+				this->setPixel({y, x});
 			else
-				this->setPixel(x, y);
+				this->setPixel({x, y});
 
 			error = error - deltaY;
 
@@ -89,37 +93,43 @@ modm::GraphicDisplay::drawLine(int16_t x1, int16_t y1, int16_t x2, int16_t y2)
 }
 
 void
-modm::GraphicDisplay::drawHorizontalLine(glcd::Point start, uint16_t length)
+modm::GraphicDisplay::drawHorizontalLine(Point start, uint16_t length)
 {
-	for (int_fast16_t i = start.x; i < static_cast<int16_t>(start.x + length); ++i)
-	{
-		this->setPixel(i, start.y);
-	}
+	uint16_t x_end = start.x + length;
+	while(start.x < x_end)
+		this->setPixel(start);
 }
 
 void
-modm::GraphicDisplay::drawVerticalLine(glcd::Point start, uint16_t length)
+modm::GraphicDisplay::drawVerticalLine(Point start, uint16_t length)
 {
-	for (int_fast16_t i = start.y; i < static_cast<int16_t>(start.y + length); ++i)
-	{
-		this->setPixel(start.x, i);
-	}
+	uint16_t y_end = start.y + length;
+	while(start.y < y_end)
+		this->setPixel(start);
 }
 
 void
-modm::GraphicDisplay::drawRectangle(glcd::Point start, uint16_t width, uint16_t height)
+modm::GraphicDisplay::drawRectangle(Point origin, uint16_t width, uint16_t height)
 {
-	uint16_t x2 = start.x + width - 1;
-	uint16_t y2 = start.y + height - 1;
+	uint16_t end_x = origin.x + width - 1;
+	uint16_t end_y = origin.y + height - 1;
 
-	this->drawHorizontalLine(start, width);
-	this->drawHorizontalLine(glcd::Point(start.x, y2), width);
-	this->drawVerticalLine(start, height);
-	this->drawVerticalLine(glcd::Point(x2, start.y), height);
+	this->drawHorizontalLine(origin, width);
+	this->drawHorizontalLine({origin.x, end_y}, width);
+	this->drawVerticalLine(origin, height);
+	this->drawVerticalLine({end_x, origin.y}, height);
 }
 
 void
-modm::GraphicDisplay::drawRoundedRectangle(glcd::Point start, uint16_t width, uint16_t height,
+modm::GraphicDisplay::fillRectangle(Point start, uint16_t width, uint16_t height)
+{
+	for (uint16_t i = start.x; (i < start.x + width) && (i < getWidth()); ++i)
+		for (uint16_t k = start.y; (k < start.y + height) && (k < getHeight()); ++k)
+			this->setPixel({i, k});
+}
+
+void
+modm::GraphicDisplay::drawRoundedRectangle(Point start, uint16_t width, uint16_t height,
 										   uint16_t radius)
 {
 	if (radius == 0) { this->drawRectangle(start, width, height); }
@@ -133,37 +143,37 @@ modm::GraphicDisplay::drawRoundedRectangle(glcd::Point start, uint16_t width, ui
 
 	while (x1 <= y1)
 	{
-		this->setPixel(x + radius - x1, y + radius - y1);
-		this->setPixel(x + radius - x1, y + height - radius + y1);
-		this->setPixel(x + radius - y1, y + radius - x1);
-		this->setPixel(x + radius - y1, y + height - radius + x1);
+		this->setPixel({x + radius - start.x, y + radius - start.y});
+		this->setPixel({x + radius - start.x, y + height - radius + start.y});
+		this->setPixel({x + radius - start.y, y + radius - start.x});
+		this->setPixel({x + radius - start.y, y + height - radius + start.x});
 
-		this->setPixel(x + width - radius + x1, y + radius - y1);
-		this->setPixel(x + width - radius + x1, y + height - radius + y1);
-		this->setPixel(x + width - radius + y1, y + radius - x1);
-		this->setPixel(x + width - radius + y1, y + height - radius + x1);
+		this->setPixel({x + width - radius + start.x, y + radius - start.y});
+		this->setPixel({x + width - radius + start.x, y + height - radius + start.y});
+		this->setPixel({x + width - radius + start.y, y + radius - start.x});
+		this->setPixel({x + width - radius + start.y, y + height - radius + start.x});
 
 		if (f < 0)
 		{
-			f += (4 * x1 + 6);
+			f += (4 * start.x + 6);
 		} else
 		{
-			f += (4 * (x1 - y1) + 10);
-			y1--;
+			f += (4 * (start.x - start.y) + 10);
+			start.y--;
 		}
-		x1++;
+		start.x++;
 	}
 
-	this->drawHorizontalLine(glcd::Point(x + radius, y), width - (2 * radius));
-	this->drawHorizontalLine(glcd::Point(x + radius, y + height), width - (2 * radius));
-	this->drawVerticalLine(glcd::Point(x, y + radius), height - (2 * radius));
-	this->drawVerticalLine(glcd::Point(x + width, y + radius), height - (2 * radius));
+	this->drawHorizontalLine(Point(x + radius, y), width - (2 * radius));
+	this->drawHorizontalLine(Point(x + radius, y + height), width - (2 * radius));
+	this->drawVerticalLine(Point(x, y + radius), height - (2 * radius));
+	this->drawVerticalLine(Point(x + width, y + radius), height - (2 * radius));
 }
 
 void
-modm::GraphicDisplay::drawCircle(glcd::Point center, uint16_t radius)
+modm::GraphicDisplay::drawCircle(Point center, uint16_t radius)
 {
-	if (radius == 0) { return; }
+	if (radius == 0) return;
 
 	int16_t error = -radius;
 	int16_t x = radius;
@@ -189,19 +199,39 @@ modm::GraphicDisplay::drawCircle(glcd::Point center, uint16_t radius)
 }
 
 void
-modm::GraphicDisplay::drawCircle4(glcd::Point center, int16_t x, int16_t y)
+modm::GraphicDisplay::fillCircle(Point center, uint16_t radius)
 {
-	const int16_t cx = center.x;
-	const int16_t cy = center.y;
+	if (radius < 2) return;
+
+	int16_t f = 1 - radius;
+	int16_t ddF_x = 0;
+	int16_t ddF_y = -2 * radius;
+	uint16_t x = 0;
+	uint16_t y = radius;
 
-	this->setPixel(cx + x, cy + y);
-	this->setPixel(cx - x, cy - y);
-	if (x != 0) { this->setPixel(cx - x, cy + y); }
-	if (y != 0) { this->setPixel(cx + x, cy - y); }
+	this->drawVerticalLine(Point(center.x, center.y - radius), 2 * radius);
+
+	while(x < y)
+	{
+		if (f >= 0)
+		{
+			y--;
+			ddF_y += 2;
+			f += ddF_y;
+		}
+		x++;
+		ddF_x += 2;
+		f += ddF_x + 1;
+
+		this->drawVerticalLine(center + Point(x, -y), 2 * y);
+		this->drawVerticalLine(center + Point(y, -x), 2 * x);
+		this->drawVerticalLine(center + Point(-x, -y), 2 * y);
+		this->drawVerticalLine(center + Point(-y, -x), 2 * x);
+	}
 }
 
 void
-modm::GraphicDisplay::drawEllipse(glcd::Point center, int16_t rx, int16_t ry)
+modm::GraphicDisplay::drawEllipse(Point center, int16_t rx, int16_t ry)
 {
 	int32_t rx_2 = rx * rx;
 	int32_t ry_2 = ry * ry;
@@ -258,7 +288,7 @@ modm::GraphicDisplay::drawEllipse(glcd::Point center, int16_t rx, int16_t ry)
 
 // ----------------------------------------------------------------------------
 void
-modm::GraphicDisplay::drawImage(glcd::Point start, modm::accessor::Flash<uint8_t> image)
+modm::GraphicDisplay::drawImage(Point start, modm::accessor::Flash<uint8_t> image)
 {
 	uint8_t width = image[0];
 	uint8_t height = image[1];
@@ -267,8 +297,8 @@ modm::GraphicDisplay::drawImage(glcd::Point start, modm::accessor::Flash<uint8_t
 }
 
 void
-modm::GraphicDisplay::drawImageRaw(glcd::Point start, uint16_t width, uint16_t height,
-								   modm::accessor::Flash<uint8_t> data)
+modm::GraphicDisplay::drawImageRaw(Point start,
+	uint16_t width, uint16_t height, modm::accessor::Flash<uint8_t> data)
 {
 	uint16_t rows = (height + 7) / 8;
 	for (uint16_t i = 0; i < width; i++)
@@ -281,12 +311,19 @@ modm::GraphicDisplay::drawImageRaw(glcd::Point start, uint16_t width, uint16_t h
 			for (uint16_t j = 0; j < rowHeight; j++)
 			{
 				if (byte & 0x01)
-					this->setPixel(start.x + i, start.y + k * 8 + j);
-				else
-					this->clearPixel(start.x + i, start.y + k * 8 + j);
+					this->setPixel({start.x + i, start.y + k * 8 + j});
 
 				byte >>= 1;
 			}
 		}
 	}
 }
+
+void
+modm::GraphicDisplay::drawCircle4(Point center, int16_t x, int16_t y)
+{
+	this->setPixel(center + Point(x, y));
+	this->setPixel(center - Point(x, y));
+	if (x != 0) this->setPixel(center + Point(-x, y));
+	if (y != 0) this->setPixel(center + Point(x, -y));
+}
\ No newline at end of file
diff --git a/src/modm/ui/display/graphic_display.hpp b/src/modm/ui/display/graphic_display.hpp
index a1c708f21c..0a666632db 100644
--- a/src/modm/ui/display/graphic_display.hpp
+++ b/src/modm/ui/display/graphic_display.hpp
@@ -23,6 +23,7 @@
 #include <modm/io/iodevice.hpp>
 #include <modm/io/iostream.hpp>
 #include <modm/math/geometry.hpp>
+#include <modm/math/utils/bit_constants.hpp>
 
 #include <modm/ui/color.hpp>
 #include "font.hpp"
@@ -31,21 +32,84 @@ namespace modm
 {
 
 /// @ingroup	modm_ui_display
-namespace glcd
+namespace display
 {
 
 /// @ingroup modm_ui_display
-using Point = Vector<int16_t, 2>;
+class Point : public Vector<int16_t, 2>
+{
+public:
+	Point() = default;
+
+	// Allow arbitray arithmetic Types for Point-construction
+	// This suspresses "narrowing conversion" compiler warnings
+	template<typename T, typename U>
+	requires std::is_arithmetic_v<T> and std::is_arithmetic_v<U>
+	Point(T x, U y) : Vector<int16_t, 2>(x, y){};
+
+	template<typename T>
+	requires std::is_arithmetic_v<T>
+	Point(Vector<T, 2> vector) : Vector<int16_t, 2>(vector){};
+};
+
+/**
+ * Helper class for 2d-drawing functions and buffer-conversion
+ */
+class Intersection
+{
+public:
+	Point pos, topLeft, bottomRight;
+
+	Intersection() = default;
+
+	Intersection(Point pos, int16_t width_source, int16_t height_source, int16_t width_target, int16_t height_target)
+		: pos(pos),
+		topLeft({
+			std::clamp<int16_t>(pos.x, 0, width_target),
+			std::clamp<int16_t>(pos.y, 0, height_target)
+		}),
+		bottomRight({
+			std::clamp<int16_t>(pos.x + width_source, 0, width_target),
+			std::clamp<int16_t>(pos.y + height_source, 0, height_target)
+		})
+	{}
+
+	// Optional getters
+
+	Point
+	getSourceTopLeft() const
+	{ return {pos.x < 0 ? -pos.x : 0, pos.y < 0 ? -pos.y : 0}; }
+
+	uint32_t getPixelCount() const
+	{
+		const Point diagonal = bottomRight - topLeft;
+		return diagonal.x * diagonal.y;
+	}
+
+	bool noIntersection() const
+	{
+		return topLeft == bottomRight;
+	}
+};
+
+/// @ingroup modm_ui_display
+enum OrientationFlags : uint8_t
+{
+	Portrait = Bit0,
+	Rotate180 = Bit1
+};
 
+/// @ingroup modm_ui_display
 enum Orientation : uint8_t
 {
-	Landscape0,
-	Portrait90,
-	Landscape180,
-	Portrait270,
+	Landscape0 = 0,
+	Portrait90 = Portrait,
+	Landscape180 = Rotate180,
+	Portrait270 = Portrait | Rotate180
 };
+}  // namespace display
 
-}  // namespace glcd
+using namespace display;
 
 /**
  * Base class for graphical displays.
@@ -84,67 +148,13 @@ class GraphicDisplay : public IOStream
 	virtual uint16_t
 	getHeight() const = 0;
 
-	// TODO Requires all inherited drivers work with resumable functions
-	// virtual modm::ResumableResult<bool>
-	// setOrientation() = 0;
-
-	/**
-	 * Buffer-array size of first dimension
-	 */
-	virtual std::size_t
-	getBufferWidth() const = 0;
-
-	/**
-	 * Buffer-array size of second dimension
-	 */
-	virtual std::size_t
-	getBufferHeight() const = 0;
-
-	/**
-	 * Set a pixel to foregroundColor
-	 *
-	 * \param x		x-position
-	 * \param y		y-position
-	 */
-	virtual void
-	setPixel(int16_t x, int16_t y) = 0;
-
 	/**
-	 * Set a pixel to foregroundColor
+	 * Set a pixel to color
 	 *
 	 * \param p		point
 	 */
-	inline void
-	setPixel(glcd::Point p)
-	{
-		this->setPixel(p.x, p.y);
-	}
-
-	/**
-	 * Set a pixel to backgroundColor
-	 *
-	 * \param x		x-position
-	 * \param y		y-position
-	 */
-	virtual void
-	clearPixel(int16_t x, int16_t y) = 0;
-
-	/**
-	 * Set a pixel to backgroundColor
-	 *
-	 * \param p		point
-	 */
-	inline void
-	clearPixel(glcd::Point p)
-	{
-		this->setPixel(p.x, p.y);
-	}
-
-	/**
-	 * Set whole screen to backgroundColor
-	 */
 	virtual void
-	clear() = 0;
+	setPixel(Point p) = 0;
 
 	/**
 	 * Transfer the content of the RAM buffer to the real display.
@@ -159,7 +169,7 @@ class GraphicDisplay : public IOStream
 	// TODO Set a clipping area
 	// Everything drawn outside this area will be discarded.
 	// inline void
-	// setClippingWindow(glcd::Point start, glcd::Point end);
+	// setClippingWindow(Point start, Point end);
 
 	/**
 	 * Draw a line.
@@ -171,22 +181,8 @@ class GraphicDisplay : public IOStream
 	 * \param start	first point
 	 * \param end	second point
 	 */
-	inline void
-	drawLine(glcd::Point start, glcd::Point end)
-	{
-		this->drawLine(start.x, start.y, end.x, end.y);
-	}
-
-	/**
-	 * Draw a line
-	 *
-	 * \param x1	Start x-position
-	 * \param y1	Start y-position
-	 * \param x2	End x-position
-	 * \param y3	End y-position
-	 */
 	void
-	drawLine(int16_t x1, int16_t y1, int16_t x2, int16_t y2);
+	drawLine(Point start, Point end);
 
 	/**
 	 * Draw a rectangle.
@@ -196,21 +192,7 @@ class GraphicDisplay : public IOStream
 	 * \param height	Height of rectangle
 	 */
 	void
-	drawRectangle(glcd::Point start, uint16_t width, uint16_t height);
-
-	/**
-	 * Draw a rectangle.
-	 *
-	 * \param x 		Upper left corner x-position
-	 * \param y 		Upper left corner y-position
-	 * \param width		Width of rectangle
-	 * \param height	Height of rectangle
-	 */
-	inline void
-	drawRectangle(int16_t x, int16_t y, uint16_t width, uint16_t height)
-	{
-		drawRectangle(glcd::Point(x, y), width, height);
-	}
+	drawRectangle(Point start, uint16_t width, uint16_t height);
 
 	/**
 	 * Draw a filled rectangle.
@@ -220,21 +202,7 @@ class GraphicDisplay : public IOStream
 	 * \param height	Height of rectangle
 	 */
 	void
-	fillRectangle(glcd::Point start, uint16_t width, uint16_t height);
-
-	/**
-	 * Draw a rectangle.
-	 *
-	 * \param x 		Upper left corner x-position
-	 * \param y 		Upper left corner y-position
-	 * \param width		Width of rectangle
-	 * \param height	Height of rectangle
-	 */
-	inline void
-	fillRectangle(int16_t x, int16_t y, uint16_t width, uint16_t height)
-	{
-		fillRectangle(glcd::Point(x, y), width, height);
-	}
+	fillRectangle(Point start, uint16_t width, uint16_t height);
 
 	/**
 	 * Draw a rectangle with rounded corners
@@ -245,7 +213,7 @@ class GraphicDisplay : public IOStream
 	 * \param radius	Rounding radius
 	 */
 	void
-	drawRoundedRectangle(glcd::Point start, uint16_t width, uint16_t height, uint16_t radius);
+	drawRoundedRectangle(Point start, uint16_t width, uint16_t height, uint16_t radius);
 
 	/**
 	 * Draw a filled rectangle with rounded corners
@@ -257,7 +225,7 @@ class GraphicDisplay : public IOStream
 	 */
 	// TODO Not yet implemented
 	// void
-	// fillRoundedRectangle(glcd::Point start, uint16_t width, uint16_t height, uint16_t radius);
+	// fillRoundedRectangle(Point start, uint16_t width, uint16_t height, uint16_t radius);
 
 	/**
 	 * Draw a circle
@@ -268,7 +236,7 @@ class GraphicDisplay : public IOStream
 	 * \param radius	Radius of the circle
 	 */
 	void
-	drawCircle(glcd::Point center, uint16_t radius);
+	drawCircle(Point center, uint16_t radius);
 
 	/**
 	 * Draw a filled circle.
@@ -277,7 +245,7 @@ class GraphicDisplay : public IOStream
 	 * \param radius	Radius of the circle
 	 */
 	virtual void
-	fillCircle(glcd::Point center, uint16_t radius);
+	fillCircle(Point center, uint16_t radius);
 
 	/**
 	 * Draw an ellipse.
@@ -290,7 +258,7 @@ class GraphicDisplay : public IOStream
 	 * \param ry		Radius in y-direction
 	 */
 	void
-	drawEllipse(glcd::Point center, int16_t rx, int16_t ry);
+	drawEllipse(Point center, int16_t rx, int16_t ry);
 
 	/**
 	 * Draw an image.
@@ -304,7 +272,7 @@ class GraphicDisplay : public IOStream
 	 * \see	drawImage()
 	 */
 	void
-	drawImage(glcd::Point start, modm::accessor::Flash<uint8_t> image);
+	drawImage(Point start, modm::accessor::Flash<uint8_t> image);
 
 	/**
 	 * Draw an image.
@@ -315,7 +283,7 @@ class GraphicDisplay : public IOStream
 	 * \param data		Image data in Flash without any size information.
 	 */
 	virtual void
-	drawImageRaw(glcd::Point start, uint16_t width, uint16_t height,
+	drawImageRaw(Point start, uint16_t width, uint16_t height,
 				 modm::accessor::Flash<uint8_t> data);
 
 	/**
@@ -324,7 +292,7 @@ class GraphicDisplay : public IOStream
 	 * \param position	Cursor position
 	 */
 	inline void
-	setCursor(glcd::Point position)
+	setCursor(Point position)
 	{
 		this->cursor = position;
 	}
@@ -338,7 +306,7 @@ class GraphicDisplay : public IOStream
 	inline void
 	setCursor(int16_t x, int16_t y)
 	{
-		this->cursor = glcd::Point(x, y);
+		this->cursor = Point(x, y);
 	}
 
 	/**
@@ -363,7 +331,7 @@ class GraphicDisplay : public IOStream
 		this->cursor.y = y;
 	}
 
-	inline glcd::Point
+	inline Point
 	getCursor() const
 	{
 		return this->cursor;
@@ -416,13 +384,13 @@ class GraphicDisplay : public IOStream
 protected:
 	/// helper method for drawCircle() and drawEllipse()
 	void
-	drawCircle4(glcd::Point center, int16_t x, int16_t y);
+	drawCircle4(Point center, int16_t x, int16_t y);
 
 	virtual void
-	drawHorizontalLine(glcd::Point start, uint16_t length);
+	drawHorizontalLine(Point start, uint16_t length);
 
 	virtual void
-	drawVerticalLine(glcd::Point start, uint16_t length);
+	drawVerticalLine(Point start, uint16_t length);
 
 protected:
 	// Interface class for the IOStream
@@ -452,7 +420,10 @@ class GraphicDisplay : public IOStream
 protected:
 	Writer writer;
 	modm::accessor::Flash<uint8_t> font;
-	glcd::Point cursor;
+
+	Point cursor;
+
+	Orientation orientation = Orientation::Landscape0;
 };
 }  // namespace modm
 
diff --git a/src/modm/ui/display/graphic_display_fill.cpp b/src/modm/ui/display/graphic_display_fill.cpp
deleted file mode 100644
index 389f8e8564..0000000000
--- a/src/modm/ui/display/graphic_display_fill.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-/*
- * Copyright (c) 2010-2011, 2013, Fabian Greif
- * Copyright (c) 2012-2013, Niklas Hauser
- * Copyright (c) 2013, Hans Schily
- * Copyright (c) 2013, Thorsten Lajewski
- *
- * This file is part of the modm project.
- *
- * This Source Code Form is subject to the terms of the Mozilla Public
- * License, v. 2.0. If a copy of the MPL was not distributed with this
- * file, You can obtain one at http://mozilla.org/MPL/2.0/.
- */
-// ----------------------------------------------------------------------------
-
-#include "graphic_display.hpp"
-
-// ----------------------------------------------------------------------------
-void
-modm::GraphicDisplay::fillRectangle(glcd::Point start,
-		uint16_t width, uint16_t height)
-{
-	for (uint16_t i = start.x; (i < start.x + width) && (i < getWidth()); ++i)
-		for (uint16_t k = start.y; (k < start.y + height) && (k < getHeight()); ++k)
-			this->setPixel(i, k);
-}
-
-void
-modm::GraphicDisplay::fillCircle(glcd::Point center, uint16_t radius)
-{
-	int16_t f = 1 - radius;
-	int16_t ddF_x = 0;
-	int16_t ddF_y = -2 * radius;
-	uint16_t x = 0;
-	uint16_t y = radius;
-
-	this->drawVerticalLine(glcd::Point(center.x, center.y - radius), 2 * radius);
-
-	while(x < y)
-	{
-		if (f >= 0)
-		{
-			y--;
-			ddF_y += 2;
-			f += ddF_y;
-		}
-		x++;
-		ddF_x += 2;
-		f += ddF_x + 1;
-
-		this->drawVerticalLine(glcd::Point(center.x + x, center.y - y), 2 * y);
-		this->drawVerticalLine(glcd::Point(center.x + y, center.y - x), 2 * x);
-		this->drawVerticalLine(glcd::Point(center.x - x, center.y - y), 2 * y);
-		this->drawVerticalLine(glcd::Point(center.x - y, center.y - x), 2 * x);
-	}
-}
diff --git a/src/modm/ui/display/monochrome_graphic_display.hpp b/src/modm/ui/display/monochrome_graphic_display.hpp
index 599827de52..bddaa2b372 100644
--- a/src/modm/ui/display/monochrome_graphic_display.hpp
+++ b/src/modm/ui/display/monochrome_graphic_display.hpp
@@ -58,13 +58,13 @@ class MonochromeGraphicDisplay : public GraphicDisplay
 	}
 
 	inline std::size_t
-	getBufferWidth() const final
+	getBufferWidth() const
 	{
 		return BufferWidth;
 	}
 
 	inline std::size_t
-	getBufferHeight() const final
+	getBufferHeight() const
 	{
 		return BufferHeight;
 	}
diff --git a/src/modm/ui/display/monochrome_graphic_display_vertical.hpp b/src/modm/ui/display/monochrome_graphic_display_vertical.hpp
index 3377d476b9..c49d849b99 100644
--- a/src/modm/ui/display/monochrome_graphic_display_vertical.hpp
+++ b/src/modm/ui/display/monochrome_graphic_display_vertical.hpp
@@ -45,7 +45,7 @@ class MonochromeGraphicDisplayVertical
 
 	// Faster version adapted for the RAM buffer
 	void
-	drawImageRaw(glcd::Point start, uint16_t width, uint16_t height,
+	drawImageRaw(display::Point start, uint16_t width, uint16_t height,
 				 modm::accessor::Flash<uint8_t> data) final;
 
 	void
@@ -60,11 +60,11 @@ class MonochromeGraphicDisplayVertical
 protected:
 	// Faster version adapted for the RAM buffer
 	void
-	drawHorizontalLine(glcd::Point start, uint16_t length) final;
+	drawHorizontalLine(display::Point start, uint16_t length) final;
 
 	// Faster version adapted for the RAM buffer
 	void
-	drawVerticalLine(glcd::Point start, uint16_t length) final;
+	drawVerticalLine(display::Point start, uint16_t length) final;
 };
 }  // namespace modm
 
diff --git a/src/modm/ui/display/monochrome_graphic_display_vertical_impl.hpp b/src/modm/ui/display/monochrome_graphic_display_vertical_impl.hpp
index aefaeaec81..51a4e9c4d8 100644
--- a/src/modm/ui/display/monochrome_graphic_display_vertical_impl.hpp
+++ b/src/modm/ui/display/monochrome_graphic_display_vertical_impl.hpp
@@ -18,7 +18,7 @@
 
 template<int16_t Width, int16_t Height>
 void
-modm::MonochromeGraphicDisplayVertical<Width, Height>::drawHorizontalLine(glcd::Point start,
+modm::MonochromeGraphicDisplayVertical<Width, Height>::drawHorizontalLine(display::Point start,
 																		  uint16_t length)
 {
 	if (start.y >= 0 and start.y < Height)
@@ -35,7 +35,7 @@ modm::MonochromeGraphicDisplayVertical<Width, Height>::drawHorizontalLine(glcd::
 
 template<int16_t Width, int16_t Height>
 void
-modm::MonochromeGraphicDisplayVertical<Width, Height>::drawVerticalLine(glcd::Point start,
+modm::MonochromeGraphicDisplayVertical<Width, Height>::drawVerticalLine(display::Point start,
 																		uint16_t length)
 {
 	if (start.x >= 0 and start.x < Width)
@@ -67,7 +67,7 @@ modm::MonochromeGraphicDisplayVertical<Width, Height>::drawVerticalLine(glcd::Po
 template<int16_t Width, int16_t Height>
 void
 modm::MonochromeGraphicDisplayVertical<Width, Height>::drawImageRaw(
-	glcd::Point start, uint16_t width, uint16_t height, modm::accessor::Flash<uint8_t> data)
+	display::Point start, uint16_t width, uint16_t height, modm::accessor::Flash<uint8_t> data)
 {
 	if ((start.y % 8) == 0)
 	{
diff --git a/src/modm/ui/display/virtual_graphic_display.cpp b/src/modm/ui/display/virtual_graphic_display.cpp
index 936629c168..02dc5c2040 100644
--- a/src/modm/ui/display/virtual_graphic_display.cpp
+++ b/src/modm/ui/display/virtual_graphic_display.cpp
@@ -13,7 +13,7 @@
 #include "virtual_graphic_display.hpp"
 
 modm::VirtualGraphicDisplay::VirtualGraphicDisplay(modm::ColorGraphicDisplay* display,
-		modm::glcd::Point leftUpper, modm::glcd::Point rightLower):
+		modm::display::Point leftUpper, modm::display::Point rightLower):
 		display(display), leftUpper(leftUpper), rightLower(rightLower),
 		width(static_cast<uint16_t>(this->rightLower[0] - this->leftUpper[0])),
 		height(static_cast<uint16_t>(this->rightLower[1] - this->leftUpper[1]))
@@ -31,9 +31,9 @@ void
 modm::VirtualGraphicDisplay::clear()
 {
 	//TODO switch black , white
-	this->display->setColor(color::Rgb(0, 0, 0));
+	this->display->color = color::html::Black;
 	this->display->fillRectangle(this->leftUpper, width, height);
-	this->display->setColor(color::Rgb(255, 255, 255));
+	this->display->color = color::html::White;
 }
 
 void
@@ -44,19 +44,13 @@ modm::VirtualGraphicDisplay::update()
 }
 
 void
-modm::VirtualGraphicDisplay::setPixel(int16_t x, int16_t y)
+modm::VirtualGraphicDisplay::setPixel(display::Point pos)
 {
-	this->display->setPixel(x + this->leftUpper[0], y + this->leftUpper[1]);
+	this->display->setPixel(pos + display::Point(this->leftUpper[0], this->leftUpper[1]));
 }
 
-void
-modm::VirtualGraphicDisplay::clearPixel(int16_t x, int16_t y)
-{
-	this->display->clearPixel(x + this->leftUpper[0], y + this->leftUpper[1] );
-}
-
-modm::color::Rgb565
-modm::VirtualGraphicDisplay::getPixel(int16_t x, int16_t y) const
+modm::color::Rgb565<true>
+modm::VirtualGraphicDisplay::getPixel(display::Point pos) const
 {
-	return this->display->getPixel(x + this->leftUpper[0], y + this->leftUpper[1] );
+	return this->display->getPixel(pos + display::Point(this->leftUpper[0], this->leftUpper[1]));
 }
\ No newline at end of file
diff --git a/src/modm/ui/display/virtual_graphic_display.hpp b/src/modm/ui/display/virtual_graphic_display.hpp
index 5d8b9d4b2d..81be89ac9f 100644
--- a/src/modm/ui/display/virtual_graphic_display.hpp
+++ b/src/modm/ui/display/virtual_graphic_display.hpp
@@ -24,8 +24,8 @@ namespace modm
 class VirtualGraphicDisplay : public modm::ColorGraphicDisplay
 {
 public:
-	VirtualGraphicDisplay(modm::ColorGraphicDisplay* display, modm::glcd::Point leftUpper,
-						  modm::glcd::Point rightLower);
+	VirtualGraphicDisplay(modm::ColorGraphicDisplay* display, modm::display::Point leftUpper,
+						  modm::display::Point rightLower);
 
 	void
 	setDisplay(modm::ColorGraphicDisplay* display);
@@ -50,18 +50,15 @@ class VirtualGraphicDisplay : public modm::ColorGraphicDisplay
 
 protected:
 	void
-	setPixel(int16_t x, int16_t y) final;
+	setPixel(display::Point pos) final;
 
-	void
-	clearPixel(int16_t x, int16_t y) final;
-
-	color::Rgb565
-	getPixel(int16_t x, int16_t y) const final;
+	color::Rgb565<true>
+	getPixel(display::Point pos) const final;
 
 private:
 	modm::ColorGraphicDisplay* display;
-	modm::glcd::Point leftUpper;
-	modm::glcd::Point rightLower;
+	modm::display::Point leftUpper;
+	modm::display::Point rightLower;
 	const uint16_t width;
 	const uint16_t height;
 };