diff --git a/st7796s.c b/st7796s.c
new file mode 100644
index 0000000..457b707
--- /dev/null
+++ b/st7796s.c
@@ -0,0 +1,242 @@
+/**
+ * @file st7796s.c
+ *
+ */
+
+/*********************
+ *      INCLUDES
+ *********************/
+#include "st7796s.h"
+#include "disp_spi.h"
+#include "driver/gpio.h"
+#include "esp_log.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
+/*********************
+ *      DEFINES
+ *********************/
+#define TAG "ST7796S"
+
+/**********************
+ *      TYPEDEFS
+ **********************/
+
+/*The LCD needs a bunch of command/argument values to be initialized. They are stored in this struct. */
+typedef struct
+{
+	uint8_t cmd;
+	uint8_t data[16];
+	uint8_t databytes; //No of data in data; bit 7 = delay after set; 0xFF = end of cmds.
+} lcd_init_cmd_t;
+
+/**********************
+ *  STATIC PROTOTYPES
+ **********************/
+static void st7796s_set_orientation(uint8_t orientation);
+
+static void st7796s_send_cmd(uint8_t cmd);
+static void st7796s_send_data(void *data, uint16_t length);
+static void st7796s_send_color(void *data, uint16_t length);
+
+/**********************
+ *  STATIC VARIABLES
+ **********************/
+
+/**********************
+ *      MACROS
+ **********************/
+
+/**********************
+ *   GLOBAL FUNCTIONS
+ **********************/
+
+void st7796s_init(void)
+{
+	lcd_init_cmd_t init_cmds[] = {
+		{0xCF, {0x00, 0x83, 0X30}, 3},
+		{0xED, {0x64, 0x03, 0X12, 0X81}, 4},
+		{0xE8, {0x85, 0x01, 0x79}, 3},
+		{0xCB, {0x39, 0x2C, 0x00, 0x34, 0x02}, 5},
+		{0xF7, {0x20}, 1},
+		{0xEA, {0x00, 0x00}, 2},
+		{0xC0, {0x26}, 1},		 /*Power control*/
+		{0xC1, {0x11}, 1},		 /*Power control */
+		{0xC5, {0x35, 0x3E}, 2}, /*VCOM control*/
+		{0xC7, {0xBE}, 1},		 /*VCOM control*/
+		{0x36, {0x28}, 1},		 /*Memory Access Control*/
+		{0x3A, {0x55}, 1},		 /*Pixel Format Set*/
+		{0xB1, {0x00, 0x1B}, 2},
+		{0xF2, {0x08}, 1},
+		{0x26, {0x01}, 1},
+		{0xE0, {0x1F, 0x1A, 0x18, 0x0A, 0x0F, 0x06, 0x45, 0X87, 0x32, 0x0A, 0x07, 0x02, 0x07, 0x05, 0x00}, 15},
+		{0XE1, {0x00, 0x25, 0x27, 0x05, 0x10, 0x09, 0x3A, 0x78, 0x4D, 0x05, 0x18, 0x0D, 0x38, 0x3A, 0x1F}, 15},
+		{0x2A, {0x00, 0x00, 0x00, 0xEF}, 4},
+		{0x2B, {0x00, 0x00, 0x01, 0x3f}, 4},
+		{0x2C, {0}, 0},
+		{0xB7, {0x07}, 1},
+		{0xB6, {0x0A, 0x82, 0x27, 0x00}, 4},
+		{0x11, {0}, 0x80},
+		{0x29, {0}, 0x80},
+		{0, {0}, 0xff},
+	};
+
+#if ST7796S_BCKL == 15
+	gpio_config_t io_conf;
+	io_conf.intr_type = GPIO_PIN_INTR_DISABLE;
+	io_conf.mode = GPIO_MODE_OUTPUT;
+	io_conf.pin_bit_mask = GPIO_SEL_15;
+	io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
+	io_conf.pull_up_en = GPIO_PULLUP_DISABLE;
+	gpio_config(&io_conf);
+#endif
+
+	//Initialize non-SPI GPIOs
+	gpio_pad_select_gpio(ST7796S_DC);
+	gpio_set_direction(ST7796S_DC, GPIO_MODE_OUTPUT);
+	gpio_pad_select_gpio(ST7796S_RST);
+	gpio_set_direction(ST7796S_RST, GPIO_MODE_OUTPUT);
+
+#if ST7796S_ENABLE_BACKLIGHT_CONTROL
+	gpio_pad_select_gpio(ST7796S_BCKL);
+	gpio_set_direction(ST7796S_BCKL, GPIO_MODE_OUTPUT);
+#endif
+	//Reset the display
+	gpio_set_level(ST7796S_RST, 0);
+	vTaskDelay(100 / portTICK_RATE_MS);
+	gpio_set_level(ST7796S_RST, 1);
+	vTaskDelay(100 / portTICK_RATE_MS);
+
+	ESP_LOGI(TAG, "Initialization.");
+
+	//Send all the commands
+	uint16_t cmd = 0;
+	while (init_cmds[cmd].databytes != 0xff)
+	{
+		st7796s_send_cmd(init_cmds[cmd].cmd);
+		st7796s_send_data(init_cmds[cmd].data, init_cmds[cmd].databytes & 0x1F);
+		if (init_cmds[cmd].databytes & 0x80)
+		{
+			vTaskDelay(100 / portTICK_RATE_MS);
+		}
+		cmd++;
+	}
+
+	st7796s_enable_backlight(true);
+
+	st7796s_set_orientation(CONFIG_LV_DISPLAY_ORIENTATION);
+
+#if ST7796S_INVERT_COLORS == 1
+	st7796s_send_cmd(0x21);
+#else
+	st7796s_send_cmd(0x20);
+#endif
+}
+
+void st7796s_flush(lv_disp_drv_t *drv, const lv_area_t *area, lv_color_t *color_map)
+{
+	uint8_t data[4];
+
+	/*Column addresses*/
+	st7796s_send_cmd(0x2A);
+	data[0] = (area->x1 >> 8) & 0xFF;
+	data[1] = area->x1 & 0xFF;
+	data[2] = (area->x2 >> 8) & 0xFF;
+	data[3] = area->x2 & 0xFF;
+	st7796s_send_data(data, 4);
+
+	/*Page addresses*/
+	st7796s_send_cmd(0x2B);
+	data[0] = (area->y1 >> 8) & 0xFF;
+	data[1] = area->y1 & 0xFF;
+	data[2] = (area->y2 >> 8) & 0xFF;
+	data[3] = area->y2 & 0xFF;
+	st7796s_send_data(data, 4);
+
+	/*Memory write*/
+	st7796s_send_cmd(0x2C);
+
+	uint32_t size = lv_area_get_width(area) * lv_area_get_height(area);
+
+	st7796s_send_color((void *)color_map, size * 2);
+}
+
+void st7796s_enable_backlight(bool backlight)
+{
+#if ST7796S_ENABLE_BACKLIGHT_CONTROL
+	ESP_LOGI(TAG, "%s backlight.", backlight ? "Enabling" : "Disabling");
+	uint32_t tmp = 0;
+
+#if (ST7796S_BCKL_ACTIVE_LVL == 1)
+	tmp = backlight ? 1 : 0;
+#else
+	tmp = backlight ? 0 : 1;
+#endif
+
+	gpio_set_level(ST7796S_BCKL, tmp);
+#endif
+}
+
+void st7796s_sleep_in()
+{
+	uint8_t data[] = {0x08};
+	st7796s_send_cmd(0x10);
+	st7796s_send_data(&data, 1);
+}
+
+void st7796s_sleep_out()
+{
+	uint8_t data[] = {0x08};
+	st7796s_send_cmd(0x11);
+	st7796s_send_data(&data, 1);
+}
+
+/**********************
+ *   STATIC FUNCTIONS
+ **********************/
+
+static void st7796s_send_cmd(uint8_t cmd)
+{
+	disp_wait_for_pending_transactions();
+	gpio_set_level(ST7796S_DC, 0); /*Command mode*/
+	disp_spi_send_data(&cmd, 1);
+}
+
+static void st7796s_send_data(void *data, uint16_t length)
+{
+	disp_wait_for_pending_transactions();
+	gpio_set_level(ST7796S_DC, 1); /*Data mode*/
+	disp_spi_send_data(data, length);
+}
+
+static void st7796s_send_color(void *data, uint16_t length)
+{
+	disp_wait_for_pending_transactions();
+	gpio_set_level(ST7796S_DC, 1); /*Data mode*/
+	disp_spi_send_colors(data, length);
+}
+
+static void st7796s_set_orientation(uint8_t orientation)
+{
+	// ESP_ASSERT(orientation < 4);
+
+	const char *orientation_str[] = {
+		"PORTRAIT", "PORTRAIT_INVERTED", "LANDSCAPE", "LANDSCAPE_INVERTED"};
+
+	ESP_LOGI(TAG, "Display orientation: %s", orientation_str[orientation]);
+
+#if defined CONFIG_LV_PREDEFINED_DISPLAY_M5STACK
+	uint8_t data[] = {0x68, 0x68, 0x08, 0x08};
+#elif defined(CONFIG_LV_PREDEFINED_DISPLAY_WROVER4)
+	uint8_t data[] = {0x4C, 0x88, 0x28, 0xE8};
+#elif defined(CONFIG_LV_PREDEFINED_DISPLAY_WT32_SC01)
+	uint8_t data[] = {0x48, 0x88, 0x28, 0xE8};
+#elif defined(CONFIG_LV_PREDEFINED_DISPLAY_NONE)
+	uint8_t data[] = {0x48, 0x88, 0x28, 0xE8};
+#endif
+
+	ESP_LOGI(TAG, "0x36 command value: 0x%02X", data[orientation]);
+
+	st7796s_send_cmd(0x36);
+	st7796s_send_data((void *)&data[orientation], 1);
+}
diff --git a/st7796s.h b/st7796s.h
new file mode 100644
index 0000000..418ec9b
--- /dev/null
+++ b/st7796s.h
@@ -0,0 +1,131 @@
+/**
+ * @file st7796s.h
+ */
+
+#ifndef ST7796S_H
+#define ST7796S_H
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/*********************
+ *      INCLUDES
+ *********************/
+#include <stdbool.h>
+#include <stdint.h>
+
+#ifdef LV_LVGL_H_INCLUDE_SIMPLE
+#include "lvgl.h"
+#else
+#include "lvgl/lvgl.h"
+#endif
+#include "../lvgl_helpers.h"
+
+/*********************
+ *      DEFINES
+ *********************/
+#define ST7796S_DC CONFIG_LV_DISP_PIN_DC
+#define ST7796S_RST CONFIG_LV_DISP_PIN_RST
+#define ST7796S_BCKL CONFIG_LV_DISP_PIN_BCKL
+
+#define ST7796S_ENABLE_BACKLIGHT_CONTROL CONFIG_LV_ENABLE_BACKLIGHT_CONTROL
+#define ST7796S_INVERT_COLORS CONFIG_LV_INVERT_COLORS
+#define ST7796S_DISPLAY_ORIENTATION CONFIG_LV_DISPLAY_ORIENTATION
+
+#if CONFIG_LV_BACKLIGHT_ACTIVE_LVL
+#define ST7796S_BCKL_ACTIVE_LVL 1
+#else
+#define ST7796S_BCKL_ACTIVE_LVL 0
+#endif
+
+/*******************
+ * ST7796S REGS
+*********************/
+
+/* MIPI DCS Type1  */
+#define ST7796S_CMD_NOP 0x00
+#define ST7796S_CMD_SOFTWARE_RESET 0x01
+#define ST7796S_CMD_READ_DISP_POWER_MODE 0x0A
+#define ST7796S_CMD_READ_DISP_MADCTRL 0x0B // bits 7:3 only
+#define ST7796S_CMD_READ_DISP_PIXEL_FORMAT 0x0C
+#define ST7796S_CMD_READ_DISP_IMAGE_MODE 0x0D
+#define ST7796S_CMD_READ_DISP_SIGNAL_MODE 0x0E
+#define ST7796S_CMD_READ_DISP_SELF_DIAGNOSTIC 0x0F // bits 7:6 only
+#define ST7796S_CMD_ENTER_SLEEP_MODE 0x10
+#define ST7796S_CMD_SLEEP_OUT 0x11
+#define ST7796S_CMD_PARTIAL_MODE_ON 0x12
+#define ST7796S_CMD_NORMAL_DISP_MODE_ON 0x13
+#define ST7796S_CMD_DISP_INVERSION_OFF 0x20
+#define ST7796S_CMD_DISP_INVERSION_ON 0x21
+#define ST7796S_CMD_DISPLAY_OFF 0x28
+#define ST7796S_CMD_DISPLAY_ON 0x29
+#define ST7796S_CMD_COLUMN_ADDRESS_SET 0x2A
+#define ST7796S_CMD_PAGE_ADDRESS_SET 0x2B
+#define ST7796S_CMD_MEMORY_WRITE 0x2C
+#define ST7796S_CMD_MEMORY_READ 0x2E
+#define ST7796S_CMD_PARTIAL_AREA 0x30
+#define ST7796S_CMD_VERT_SCROLL_DEFINITION 0x33
+#define ST7796S_CMD_TEARING_EFFECT_LINE_OFF 0x34
+#define ST7796S_CMD_TEARING_EFFECT_LINE_ON 0x35
+#define ST7796S_CMD_MEMORY_ACCESS_CONTROL 0x36 // bits 7:3,1:0 only
+#define ST7796S_CMD_VERT_SCROLL_START_ADDRESS 0x37
+#define ST7796S_CMD_IDLE_MODE_OFF 0x38
+#define ST7796S_CMD_IDLE_MODE_ON 0x39
+#define ST7796S_CMD_COLMOD_PIXEL_FORMAT_SET 0x3A
+#define ST7796S_CMD_WRITE_MEMORY_CONTINUE 0x3C
+#define ST7796S_CMD_READ_MEMORY_CONTINUE 0x3E
+#define ST7796S_CMD_SET_TEAR_SCANLINE 0x44
+#define ST7796S_CMD_GET_SCANLINE 0x45
+
+#define ST7796S_DDB_START 0xA1
+#define ST7796S_DDB_CONTINUE 0xA8
+
+/* other */
+#define ST7796S_CMD_ACCESS_PROTECT 0xB0
+#define ST7796S_CMD_LOW_POWER_CONTROL 0xB1
+#define ST7796S_CMD_FRAME_MEMORY_ACCESS 0xB3
+#define ST7796S_CMD_DISPLAY_MODE 0xB4
+#define ST7796S_CMD_DEVICE_CODE 0xBF
+
+#define ST7796S_CMD_PANEL_DRIVE 0xC0
+#define ST7796S_CMD_DISP_TIMING_NORMAL 0xC1
+#define ST7796S_CMD_DISP_TIMING_PARTIAL 0xC2
+#define ST7796S_CMD_DISP_TIMING_IDLE 0xC3
+#define ST7796S_CMD_FRAME_RATE 0xC5
+#define ST7796S_CMD_INTERFACE_CONTROL 0xC6
+#define ST7796S_CMD_GAMMA_SETTING 0xC8
+
+#define ST7796S_CMD_POWER_SETTING 0xD0
+#define ST7796S_CMD_VCOM_CONTROL 0xD1
+#define ST7796S_CMD_POWER_CONTROL_NORMAL 0xD2
+#define ST7796S_CMD_POWER_CONTROL_IDEL 0xD3
+#define ST7796S_CMD_POWER_CONTROL_PARTIAL 0xD4
+
+#define ST7796S_CMD_NVMEM_WRITE 0xE0
+#define ST7796S_CMD_NVMEM_PROTECTION_KEY 0xE1
+#define ST7796S_CMD_NVMEM_STATUS_READ 0xE2
+#define ST7796S_CMD_NVMEM_PROTECTION 0xE3
+
+  /**********************
+ *      TYPEDEFS
+ **********************/
+
+  /**********************
+ * GLOBAL PROTOTYPES
+ **********************/
+
+  void st7796s_init(void);
+  void st7796s_flush(lv_disp_drv_t *drv, const lv_area_t *area, lv_color_t *color_map);
+  void st7796s_enable_backlight(bool backlight);
+
+  /**********************
+ *      MACROS
+ **********************/
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif /*ST7796S_H*/