From 10d46b483eb2855255796b7fce598a6213bf7fa2 Mon Sep 17 00:00:00 2001 From: Drath Date: Thu, 20 Aug 2026 08:06:55 +0200 Subject: [PATCH] spi ver. --- CMakeLists.txt | 4 +- idf_component.yml | 2 +- st7567-spi/st7567.c | 165 ++++++++++++++++++++++++++++++++++++++++++++ st7567/st7567-i2c.h | 129 ++++++++++++++++++++++++++++++++++ st7567/st7567-spi.h | 130 ++++++++++++++++++++++++++++++++++ st7567/st7567.c | 127 ++++++++++++++++++++++++++++++++++ 6 files changed, 554 insertions(+), 3 deletions(-) create mode 100644 st7567-spi/st7567.c create mode 100644 st7567/st7567-i2c.h create mode 100644 st7567/st7567-spi.h create mode 100644 st7567/st7567.c diff --git a/CMakeLists.txt b/CMakeLists.txt index c7402e0..cb02afc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ idf_component_register( - SRCS "st7567.c" + SRCS "st7567/st7567.c" INCLUDE_DIRS "." - REQUIRES esp_driver_i2c + REQUIRES esp_driver_i2ci esp_driver_spi esp_driver_gpio ) diff --git a/idf_component.yml b/idf_component.yml index 7695f8c..77d1495 100644 --- a/idf_component.yml +++ b/idf_component.yml @@ -1,5 +1,5 @@ version: "1.1.0" -description: "ST7567 I2C 128x64 display driver" +description: "ST7567 128x64 display driver" url: "https://git.drath.cc/you/esp-st7567" dependencies: idf: ">=5.2" diff --git a/st7567-spi/st7567.c b/st7567-spi/st7567.c new file mode 100644 index 0000000..5dcfbb4 --- /dev/null +++ b/st7567-spi/st7567.c @@ -0,0 +1,165 @@ +#include "st7567.h" +#include +#include "esp_log.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" + +static const char *TAG = "ST7567"; + +#define ST7567_SPI_TIMEOUT_MS 100 +#define ST7567_DEFAULT_CLOCK_HZ 8000000 +#define ST7567_DEFAULT_CONTRAST 0x20 + +// DC pin levels: LOW = command, HIGH = data (matches most ST7567 SPI breakouts; +// check your panel's datasheet if the display comes up garbled/inverted) +#define ST7567_DC_CMD 0 +#define ST7567_DC_DATA 1 + +static esp_err_t st7567_spi_write(const st7567_device_t *dev, int dc_level, const uint8_t *data, size_t len) { + if (len == 0) + return ESP_OK; + + gpio_set_level(dev->dc_gpio, dc_level); + + spi_transaction_t t = { + .length = len * 8, // bits, not bytes + .tx_buffer = data, + }; + + // blocking polling transmit: this driver is only ever called from one task + // (ui_task), so there's no need for the queued/async SPI API here + return spi_device_polling_transmit(dev->spi_dev, &t); +} + +esp_err_t st7567_send_cmd(const st7567_device_t *dev, const uint8_t cmd) { + return st7567_spi_write(dev, ST7567_DC_CMD, &cmd, 1); +} + +esp_err_t st7567_send_data(const st7567_device_t *dev, const uint8_t *data, size_t len) { + return st7567_spi_write(dev, ST7567_DC_DATA, data, len); +} + +esp_err_t st7567_set_contrast(const st7567_device_t *dev, uint8_t contrast) { + if (contrast > 63) + contrast = 63; + + esp_err_t ret = st7567_send_cmd(dev, ST7567_CMD_CONTRAST_MODE); + if (ret != ESP_OK) + return ret; + + return st7567_send_cmd(dev, contrast); +} + +esp_err_t st7567_init(const st7567_config_t *config, st7567_device_t *device) { + memset(device, 0, sizeof(st7567_device_t)); // clears framebuffer too + device->dc_gpio = config->dc_gpio; + + // DC is toggled every single command/data write, so drive it directly + // rather than routing it through the SPI peripheral + gpio_config_t dc_cfg = { + .pin_bit_mask = 1ULL << config->dc_gpio, + .mode = GPIO_MODE_OUTPUT, + }; + esp_err_t ret = gpio_config(&dc_cfg); + if (ret != ESP_OK) { + ESP_LOGE(TAG, "Failed to configure DC gpio: %s", esp_err_to_name(ret)); + return ret; + } + + if (config->rst_gpio != GPIO_NUM_NC) { + gpio_config_t rst_cfg = { + .pin_bit_mask = 1ULL << config->rst_gpio, + .mode = GPIO_MODE_OUTPUT, + }; + ret = gpio_config(&rst_cfg); + if (ret != ESP_OK) { + ESP_LOGE(TAG, "Failed to configure RST gpio: %s", esp_err_to_name(ret)); + return ret; + } + + // hardware reset pulse, active low - panel datasheets typically want + // >=1us low, we hold well past that + gpio_set_level(config->rst_gpio, 0); + vTaskDelay(pdMS_TO_TICKS(10)); + gpio_set_level(config->rst_gpio, 1); + vTaskDelay(pdMS_TO_TICKS(10)); + } + + // Add device on the (already-initialized) SPI bus + const spi_device_interface_config_t dev_cfg = { + .clock_speed_hz = config->spi_clock_speed_hz ? config->spi_clock_speed_hz : ST7567_DEFAULT_CLOCK_HZ, + .mode = 0, // CPOL=0, CPHA=0 - standard for ST7567, check your panel if init garbles + .spics_io_num = config->cs_gpio, + .queue_size = 1, + }; + + ret = spi_bus_add_device(config->spi_host, &dev_cfg, &device->spi_dev); + if (ret != ESP_OK) { + ESP_LOGE(TAG, "Failed to add SPI device: %s", esp_err_to_name(ret)); + return ret; + } + + // internal software reset + init sequence (standard ST7567 sequence) + st7567_send_cmd(device, ST7567_CMD_RESET); + vTaskDelay(pdMS_TO_TICKS(10)); + st7567_send_cmd(device, ST7567_CMD_BIAS_1_9); + st7567_send_cmd(device, ST7567_CMD_ADC_NORMAL); + st7567_send_cmd(device, ST7567_CMD_COM_REVERSE); + st7567_send_cmd(device, ST7567_CMD_RESISTOR_RATIO | 0x03); + st7567_send_cmd(device, ST7567_CMD_POWER_CTRL | 0x07); // booster, regulator, follower all on + vTaskDelay(pdMS_TO_TICKS(10)); // let the charge pump stabilize before touching contrast + + st7567_set_contrast(device, config->contrast ? config->contrast : ST7567_DEFAULT_CONTRAST); + + st7567_send_cmd(device, ST7567_CMD_START_LINE | 0x00); + st7567_send_cmd(device, ST7567_CMD_ALL_PIXEL_NORMAL); + st7567_send_cmd(device, ST7567_CMD_DISPLAY_NORMAL); + st7567_send_cmd(device, ST7567_CMD_DISPLAY_ON); + + ESP_LOGI(TAG, "display init done"); + return ESP_OK; +} + +void st7567_clear_buffer(st7567_device_t *dev) { + memset(dev->framebuffer, 0, sizeof(dev->framebuffer)); +} + +esp_err_t st7567_clear(st7567_device_t *dev) { + st7567_clear_buffer(dev); + return st7567_update_screen(dev); +} + +esp_err_t st7567_update_screen(const st7567_device_t *dev) { + // ST7567 is page-addressed: 8 pages of 8 vertical px each, byte = one column + // within a page (LSB = top row of the page). Same layout as SSD1306. + for (uint8_t page = 0; page < ST7567_PAGES; page++) { + esp_err_t ret; + if ((ret = st7567_send_cmd(dev, ST7567_CMD_PAGE_ADDR | page)) != ESP_OK) return ret; + if ((ret = st7567_send_cmd(dev, ST7567_CMD_COL_ADDR_HIGH | 0x00)) != ESP_OK) return ret; + if ((ret = st7567_send_cmd(dev, ST7567_CMD_COL_ADDR_LOW | 0x00)) != ESP_OK) return ret; + if ((ret = st7567_send_data(dev, &dev->framebuffer[page * ST7567_WIDTH], ST7567_WIDTH)) != ESP_OK) return ret; + } + + return ESP_OK; +} + +static void st7567_set_pixel_color(st7567_device_t *dev, const uint8_t x, const uint8_t y, const uint8_t color) { + if (x >= ST7567_WIDTH || y >= ST7567_HEIGHT) + return; + + const uint16_t byte_index = (y / 8) * ST7567_WIDTH + x; + const uint8_t bit_index = y % 8; + + if (color) + dev->framebuffer[byte_index] |= (1 << bit_index); + else + dev->framebuffer[byte_index] &= ~(1 << bit_index); +} + +void st7567_draw_pixel(void *dev, const uint8_t x, const uint8_t y) { + st7567_set_pixel_color(dev, x, y, 1); +} + +void st7567_clear_pixel(void *dev, const uint8_t x, const uint8_t y) { + st7567_set_pixel_color(dev, x, y, 0); +} diff --git a/st7567/st7567-i2c.h b/st7567/st7567-i2c.h new file mode 100644 index 0000000..27bbf4d --- /dev/null +++ b/st7567/st7567-i2c.h @@ -0,0 +1,129 @@ +#ifndef ST7567_H +#define ST7567_H + +#include "esp_err.h" +#include "driver/i2c_master.h" + +// Display dimensions +#define ST7567_WIDTH 128 +#define ST7567_HEIGHT 64 +#define ST7567_PAGES (ST7567_HEIGHT / 8) + +// Default I2C 7-bit address for most ST7567 breakout boards (some use 0x3F) +#define ST7567_I2C_ADDR_DEFAULT 0x3F + +// ST7567 command set +#define ST7567_CMD_DISPLAY_OFF 0xAE +#define ST7567_CMD_DISPLAY_ON 0xAF +#define ST7567_CMD_START_LINE 0x40 // OR with 0-63 +#define ST7567_CMD_PAGE_ADDR 0xB0 // OR with 0-8 +#define ST7567_CMD_COL_ADDR_HIGH 0x10 // OR with high nibble +#define ST7567_CMD_COL_ADDR_LOW 0x00 // OR with low nibble +#define ST7567_CMD_ADC_NORMAL 0xA0 // column addr 0 = SEG0 +#define ST7567_CMD_ADC_REVERSE 0xA1 // column addr 131 = SEG0 +#define ST7567_CMD_ALL_PIXEL_NORMAL 0xA4 +#define ST7567_CMD_ALL_PIXEL_ON 0xA5 +#define ST7567_CMD_DISPLAY_NORMAL 0xA6 +#define ST7567_CMD_DISPLAY_INVERSE 0xA7 +#define ST7567_CMD_BIAS_1_9 0xA2 +#define ST7567_CMD_BIAS_1_7 0xA3 +#define ST7567_CMD_RESET 0xE2 +#define ST7567_CMD_COM_NORMAL 0xC0 +#define ST7567_CMD_COM_REVERSE 0xC8 +#define ST7567_CMD_POWER_CTRL 0x28 // OR with 0-7 (booster/regulator/follower bits) +#define ST7567_CMD_RESISTOR_RATIO 0x20 // OR with 0-7 +#define ST7567_CMD_CONTRAST_MODE 0x81 // followed by a second byte, 0-63 +#define ST7567_CMD_STATIC_INDICATOR_OFF 0xAC +#define ST7567_CMD_NOP 0xE3 + +// Configuration structure +typedef struct { + i2c_master_bus_handle_t i2c_bus; // already-initialized I2C bus + uint16_t i2c_addr; // 7-bit device address, 0 = use ST7567_I2C_ADDR_DEFAULT + uint32_t i2c_clock_speed_hz; // 0 = use 400kHz default + uint8_t contrast; // 0-63, 0 = use a sane default (0x20) +} st7567_config_t; + +// Handle for ST7567 device +typedef struct { + i2c_master_dev_handle_t i2c_dev; + uint8_t framebuffer[ST7567_WIDTH * ST7567_PAGES]; +} st7567_device_t; + +/** + * @brief Initialize ST7567 display + * + * @param config Configuration structure + * @param device Pointer to store device state (framebuffer etc, caller-owned) + * @return esp_err_t ESP_OK on success + */ +esp_err_t st7567_init(const st7567_config_t *config, st7567_device_t *device); + +/** + * @brief Send a single command byte to the ST7567 + * + * @param dev Device handle + * @param cmd Command byte + * @return esp_err_t ESP_OK on success + */ +esp_err_t st7567_send_cmd(const st7567_device_t *dev, uint8_t cmd); + +/** + * @brief Send a data buffer to the ST7567 + * + * @param dev Device handle + * @param data Pointer to data buffer + * @param len Length of data buffer + * @return esp_err_t ESP_OK on success + */ +esp_err_t st7567_send_data(const st7567_device_t *dev, const uint8_t *data, size_t len); + +/** + * @brief Clear the display (just wipe framebuffer, don't send any commands) + * + * @param dev Device handle + */ +void st7567_clear_buffer(st7567_device_t *dev); + +/** + * @brief Clear the display (both GDRAM and local framebuffer) + * + * @param dev Device handle + * @return esp_err_t ESP_OK on success + */ +esp_err_t st7567_clear(st7567_device_t *dev); + +/** + * @brief Set contrast (electronic volume) + * + * @param dev Device handle + * @param contrast 0-63 + * @return esp_err_t ESP_OK on success + */ +esp_err_t st7567_set_contrast(const st7567_device_t *dev, uint8_t contrast); + +/** + * @brief Push the local framebuffer contents to the display + * + * @param dev Device handle + * @return esp_err_t ESP_OK on success + */ +esp_err_t st7567_update_screen(const st7567_device_t *dev); + +/** + * @brief Turn a pixel ON at given coordinates + * @param dev Device handle + * @param x x coordinate + * @param y y coordinate + */ +void st7567_draw_pixel(void *dev, uint8_t x, uint8_t y); + +/** + * @brief Turn a pixel OFF at given coordinates + * @param dev Device handle + * @param x x coordinate + * @param y y coordinate + */ +void st7567_clear_pixel(void *dev, uint8_t x, uint8_t y); + +#endif // ST7567_H diff --git a/st7567/st7567-spi.h b/st7567/st7567-spi.h new file mode 100644 index 0000000..a033f45 --- /dev/null +++ b/st7567/st7567-spi.h @@ -0,0 +1,130 @@ +#ifndef ST7567_H +#define ST7567_H + +#include "esp_err.h" +#include "driver/spi_master.h" +#include "driver/gpio.h" + +// Display dimensions +#define ST7567_WIDTH 128 +#define ST7567_HEIGHT 64 +#define ST7567_PAGES (ST7567_HEIGHT / 8) + +// ST7567 command set +#define ST7567_CMD_DISPLAY_OFF 0xAE +#define ST7567_CMD_DISPLAY_ON 0xAF +#define ST7567_CMD_START_LINE 0x40 // OR with 0-63 +#define ST7567_CMD_PAGE_ADDR 0xB0 // OR with 0-8 +#define ST7567_CMD_COL_ADDR_HIGH 0x10 // OR with high nibble +#define ST7567_CMD_COL_ADDR_LOW 0x00 // OR with low nibble +#define ST7567_CMD_ADC_NORMAL 0xA0 // column addr 0 = SEG0 +#define ST7567_CMD_ADC_REVERSE 0xA1 // column addr 131 = SEG0 +#define ST7567_CMD_ALL_PIXEL_NORMAL 0xA4 +#define ST7567_CMD_ALL_PIXEL_ON 0xA5 +#define ST7567_CMD_DISPLAY_NORMAL 0xA6 +#define ST7567_CMD_DISPLAY_INVERSE 0xA7 +#define ST7567_CMD_BIAS_1_9 0xA2 +#define ST7567_CMD_BIAS_1_7 0xA3 +#define ST7567_CMD_RESET 0xE2 +#define ST7567_CMD_COM_NORMAL 0xC0 +#define ST7567_CMD_COM_REVERSE 0xC8 +#define ST7567_CMD_POWER_CTRL 0x28 // OR with 0-7 (booster/regulator/follower bits) +#define ST7567_CMD_RESISTOR_RATIO 0x20 // OR with 0-7 +#define ST7567_CMD_CONTRAST_MODE 0x81 // followed by a second byte, 0-63 +#define ST7567_CMD_STATIC_INDICATOR_OFF 0xAC +#define ST7567_CMD_NOP 0xE3 + +// Configuration structure +typedef struct { + spi_host_device_t spi_host; // already-initialized SPI bus (spi_bus_initialize called by caller) + gpio_num_t cs_gpio; // chip select + gpio_num_t dc_gpio; // data/command select (required, no in-band control byte over SPI) + gpio_num_t rst_gpio; // hardware reset, GPIO_NUM_NC if not wired + uint32_t spi_clock_speed_hz; // 0 = use 8MHz default + uint8_t contrast; // 0-63, 0 = use a sane default (0x20) +} st7567_config_t; + +// Handle for ST7567 device +typedef struct { + spi_device_handle_t spi_dev; + gpio_num_t dc_gpio; + uint8_t framebuffer[ST7567_WIDTH * ST7567_PAGES]; +} st7567_device_t; + +/** + * @brief Initialize ST7567 display + * + * @param config Configuration structure + * @param device Pointer to store device state (framebuffer etc, caller-owned) + * @return esp_err_t ESP_OK on success + */ +esp_err_t st7567_init(const st7567_config_t *config, st7567_device_t *device); + +/** + * @brief Send a single command byte to the ST7567 + * + * @param dev Device handle + * @param cmd Command byte + * @return esp_err_t ESP_OK on success + */ +esp_err_t st7567_send_cmd(const st7567_device_t *dev, uint8_t cmd); + +/** + * @brief Send a data buffer to the ST7567 + * + * @param dev Device handle + * @param data Pointer to data buffer + * @param len Length of data buffer + * @return esp_err_t ESP_OK on success + */ +esp_err_t st7567_send_data(const st7567_device_t *dev, const uint8_t *data, size_t len); + +/** + * @brief Clear the display (just wipe framebuffer, don't send any commands) + * + * @param dev Device handle + */ +void st7567_clear_buffer(st7567_device_t *dev); + +/** + * @brief Clear the display (both GDRAM and local framebuffer) + * + * @param dev Device handle + * @return esp_err_t ESP_OK on success + */ +esp_err_t st7567_clear(st7567_device_t *dev); + +/** + * @brief Set contrast (electronic volume) + * + * @param dev Device handle + * @param contrast 0-63 + * @return esp_err_t ESP_OK on success + */ +esp_err_t st7567_set_contrast(const st7567_device_t *dev, uint8_t contrast); + +/** + * @brief Push the local framebuffer contents to the display + * + * @param dev Device handle + * @return esp_err_t ESP_OK on success + */ +esp_err_t st7567_update_screen(const st7567_device_t *dev); + +/** + * @brief Turn a pixel ON at given coordinates + * @param dev Device handle + * @param x x coordinate + * @param y y coordinate + */ +void st7567_draw_pixel(void *dev, uint8_t x, uint8_t y); + +/** + * @brief Turn a pixel OFF at given coordinates + * @param dev Device handle + * @param x x coordinate + * @param y y coordinate + */ +void st7567_clear_pixel(void *dev, uint8_t x, uint8_t y); + +#endif // ST7567_H diff --git a/st7567/st7567.c b/st7567/st7567.c new file mode 100644 index 0000000..8edfe2c --- /dev/null +++ b/st7567/st7567.c @@ -0,0 +1,127 @@ +#include "st7567.h" +#include +#include "esp_log.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" + +static const char *TAG = "ST7567"; + +// I2C control byte: bit7 = Co (0 = stream, no further control bytes needed), +// bit6 = D/C# (0 = command, 1 = data). Same convention as SSD1306-family displays. +#define ST7567_I2C_CTRL_CMD 0x00 +#define ST7567_I2C_CTRL_DATA 0x40 + +#define ST7567_I2C_TIMEOUT_MS 100 +#define ST7567_DEFAULT_CLOCK_HZ 400000 +#define ST7567_DEFAULT_CONTRAST 0x20 + +static esp_err_t st7567_i2c_write(const st7567_device_t *dev, uint8_t ctrl, const uint8_t *data, size_t len) { + // small stack buffer, framebuffer flush is chunked by page (128B) so this is plenty + uint8_t buf[ST7567_WIDTH + 1]; + if (len + 1 > sizeof(buf)) + return ESP_ERR_INVALID_SIZE; + + buf[0] = ctrl; + memcpy(&buf[1], data, len); + return i2c_master_transmit(dev->i2c_dev, buf, len + 1, ST7567_I2C_TIMEOUT_MS); +} + +esp_err_t st7567_send_cmd(const st7567_device_t *dev, const uint8_t cmd) { + return st7567_i2c_write(dev, ST7567_I2C_CTRL_CMD, &cmd, 1); +} + +esp_err_t st7567_send_data(const st7567_device_t *dev, const uint8_t *data, size_t len) { + return st7567_i2c_write(dev, ST7567_I2C_CTRL_DATA, data, len); +} + +esp_err_t st7567_set_contrast(const st7567_device_t *dev, uint8_t contrast) { + if (contrast > 63) + contrast = 63; + + esp_err_t ret = st7567_send_cmd(dev, ST7567_CMD_CONTRAST_MODE); + if (ret != ESP_OK) + return ret; + + return st7567_send_cmd(dev, contrast); +} + +esp_err_t st7567_init(const st7567_config_t *config, st7567_device_t *device) { + memset(device, 0, sizeof(st7567_device_t)); // clears framebuffer too + + // Add device on the (already-initialized) I2C bus + const i2c_device_config_t dev_cfg = { + .dev_addr_length = I2C_ADDR_BIT_LEN_7, + .device_address = config->i2c_addr ? config->i2c_addr : ST7567_I2C_ADDR_DEFAULT, + .scl_speed_hz = config->i2c_clock_speed_hz ? config->i2c_clock_speed_hz : ST7567_DEFAULT_CLOCK_HZ, + }; + + const esp_err_t ret = i2c_master_bus_add_device(config->i2c_bus, &dev_cfg, &device->i2c_dev); + if (ret != ESP_OK) { + ESP_LOGE(TAG, "Failed to add I2C device: %s", esp_err_to_name(ret)); + return ret; + } + + // internal software reset + init sequence (standard ST7567 sequence) + st7567_send_cmd(device, ST7567_CMD_RESET); + vTaskDelay(pdMS_TO_TICKS(10)); + st7567_send_cmd(device, ST7567_CMD_BIAS_1_9); + st7567_send_cmd(device, ST7567_CMD_ADC_NORMAL); + st7567_send_cmd(device, ST7567_CMD_COM_REVERSE); + st7567_send_cmd(device, ST7567_CMD_RESISTOR_RATIO | 0x03); + st7567_send_cmd(device, ST7567_CMD_POWER_CTRL | 0x07); // booster, regulator, follower all on + vTaskDelay(pdMS_TO_TICKS(10)); // let the charge pump stabilize before touching contrast + + st7567_set_contrast(device, config->contrast ? config->contrast : ST7567_DEFAULT_CONTRAST); + + st7567_send_cmd(device, ST7567_CMD_START_LINE | 0x00); + st7567_send_cmd(device, ST7567_CMD_ALL_PIXEL_NORMAL); + st7567_send_cmd(device, ST7567_CMD_DISPLAY_NORMAL); + st7567_send_cmd(device, ST7567_CMD_DISPLAY_ON); + + ESP_LOGI(TAG, "display init done"); + return ESP_OK; +} + +void st7567_clear_buffer(st7567_device_t *dev) { + memset(dev->framebuffer, 0, sizeof(dev->framebuffer)); +} + +esp_err_t st7567_clear(st7567_device_t *dev) { + st7567_clear_buffer(dev); + return st7567_update_screen(dev); +} + +esp_err_t st7567_update_screen(const st7567_device_t *dev) { + // ST7567 is page-addressed: 8 pages of 8 vertical px each, byte = one column + // within a page (LSB = top row of the page). Same layout as SSD1306. + for (uint8_t page = 0; page < ST7567_PAGES; page++) { + esp_err_t ret; + if ((ret = st7567_send_cmd(dev, ST7567_CMD_PAGE_ADDR | page)) != ESP_OK) return ret; + if ((ret = st7567_send_cmd(dev, ST7567_CMD_COL_ADDR_HIGH | 0x00)) != ESP_OK) return ret; + if ((ret = st7567_send_cmd(dev, ST7567_CMD_COL_ADDR_LOW | 0x00)) != ESP_OK) return ret; + if ((ret = st7567_send_data(dev, &dev->framebuffer[page * ST7567_WIDTH], ST7567_WIDTH)) != ESP_OK) return ret; + } + + return ESP_OK; +} + +static void st7567_set_pixel_color(st7567_device_t *dev, const uint8_t x, const uint8_t y, const uint8_t color) { + if (x >= ST7567_WIDTH || y >= ST7567_HEIGHT) + return; + + const uint16_t byte_index = (y / 8) * ST7567_WIDTH + x; + const uint8_t bit_index = y % 8; + + if (color) + dev->framebuffer[byte_index] |= (1 << bit_index); + else + dev->framebuffer[byte_index] &= ~(1 << bit_index); +} + +void st7567_draw_pixel(void *dev, const uint8_t x, const uint8_t y) { + st7567_set_pixel_color(dev, x, y, 1); +} + +void st7567_clear_pixel(void *dev, const uint8_t x, const uint8_t y) { + st7567_set_pixel_color(dev, x, y, 0); +}