dominic/Tactility
1
0
Fork 0
mirror of https://github.com/ByteWelder/Tactility.git synced 2026-10-17 19:05:07 +00:00
Code Issues Packages Projects Releases Wiki Activity
Tactility/Drivers/gc0308-module/source/gc0308.cpp
Shadowtrance 7c508bbaa0
Tab5, CoreS3, StackChan and app icons (#662)
2026-09-26 00:35:33 +02:00

553 lines
20 KiB
C++
Raw Blame History

// SPDX-License-Identifier: Apache-2.0
#include <drivers/gc0308.h>
#include <gc0308_module.h>
#include <tactility/device.h>
#include <tactility/drivers/esp32_i2c_master.h>
#include <tactility/drivers/gpio_controller.h>
#include <tactility/drivers/i2c_controller.h>
#include <tactility/log.h>
#include <esp_heap_caps.h>
#include <esp_video_device.h>
#include <esp_video_init.h>
#include <freertos/task.h>
#include <fcntl.h>
#include <linux/videodev2.h>
#include <sys/errno.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/poll.h>
#include <esp_jpeg_enc.h>
#include <cstring>
#include <freertos/semphr.h>
#define TAG "GC0308"
static constexpr int VIDEO_BUFFER_COUNT = 2;
static bool s_video_initialized = false;
static SemaphoreHandle_t s_video_init_mutex = nullptr;
static uint32_t s_video_handle_count = 0;
struct Gc0308State : CameraHandleData {
int fd;
// Native sensor output dimensions
uint32_t native_width;
uint32_t native_height;
// Frames are returned in the requested orientation.
uint32_t width;
uint32_t height;
uint8_t* buffers[VIDEO_BUFFER_COUNT];
size_t buf_lengths[VIDEO_BUFFER_COUNT];
int last_dqbuf_index;
uint8_t* frame_buf;
size_t frame_buf_size;
jpeg_enc_handle_t encoder;
i2c_master_bus_handle_t sccb_bus;
CameraRotation rotation;
SemaphoreHandle_t rotation_mutex; // protects rotation, width, height
uint32_t encoder_width;
uint32_t encoder_height;
};
#define GET_CONFIG(device) (static_cast<const Gc0308Config*>((device)->config))
static gpio_num_t pin_or_nc(const GpioPinSpec& pin) {
return pin.gpio_controller == nullptr ? GPIO_NUM_NC : static_cast<gpio_num_t>(pin.pin);
}
static CameraRotation add_rotation(CameraRotation rotation, uint16_t offset) {
return static_cast<CameraRotation>((static_cast<uint16_t>(rotation) + offset) % 360);
}
static error_t reset_pulse(GpioDescriptor* descriptor) {
// Release is attempted unconditionally, even if assert failed - leaving the expander output
// asserted on an assert failure would hold the sensor in reset for the rest of boot.
const bool assert_ok = gpio_descriptor_set_level(descriptor, true) == ERROR_NONE; // assert
vTaskDelay(pdMS_TO_TICKS(10));
const bool release_ok = gpio_descriptor_set_level(descriptor, false) == ERROR_NONE; // release
vTaskDelay(pdMS_TO_TICKS(10));
return (assert_ok && release_ok) ? ERROR_NONE : ERROR_RESOURCE;
}
static error_t start(Device* device) {
auto* i2c = device_get_parent(device);
if (device_get_type(i2c) != &I2C_CONTROLLER_TYPE) {
LOG_E(TAG, "Parent is not an I2C controller");
return ERROR_RESOURCE;
}
const auto* config = GET_CONFIG(device);
//auto address = config->address;
// Reset is pulsed (not just held released) so the sensor reaches a known state regardless of
// whatever it inherited from a previous boot
if (config->pin_reset.gpio_controller != nullptr) {
// Merge with the devicetree-supplied flags (e.g. GPIO_FLAG_ACTIVE_LOW) rather than
// overwriting them, so a board that wires reset active-high isn't silently forced to
// active-low polarity.
auto* reset_descriptor = gpio_descriptor_acquire(config->pin_reset.gpio_controller, config->pin_reset.pin, config->pin_reset.flags | GPIO_FLAG_DIRECTION_OUTPUT, GPIO_OWNER_GPIO);
if (reset_descriptor == nullptr) {
LOG_E(TAG, "Failed to acquire reset pin");
return ERROR_RESOURCE;
}
error_t reset_error = reset_pulse(reset_descriptor);
gpio_descriptor_release(reset_descriptor);
if (reset_error != ERROR_NONE) {
LOG_E(TAG, "Failed to pulse reset pin");
return reset_error;
}
}
return ERROR_NONE;
}
static error_t stop([[maybe_unused]] Device* device) {
return ERROR_NONE;
}
static SemaphoreHandle_t get_video_init_mutex() {
static portMUX_TYPE creation_lock = portMUX_INITIALIZER_UNLOCKED;
portENTER_CRITICAL(&creation_lock);
if (!s_video_init_mutex) {
s_video_init_mutex = xSemaphoreCreateMutex();
if (!s_video_init_mutex) {
LOG_E(TAG, "Failed to create video init mutex");
}
}
portEXIT_CRITICAL(&creation_lock);
return s_video_init_mutex;
}
extern "C" {
error_t gc0308_open(Device* device, Gc0308Handle* out_handle) {
if (!device || !out_handle) return ERROR_INVALID_ARGUMENT;
auto* state = static_cast<Gc0308State*>(heap_caps_calloc(1, sizeof(Gc0308State), MALLOC_CAP_DEFAULT));
if (!state) return ERROR_OUT_OF_MEMORY;
state->device = device;
state->fd = -1;
state->last_dqbuf_index = -1;
state->rotation_mutex = xSemaphoreCreateMutex();
if (!state->rotation_mutex) {
heap_caps_free(state);
return ERROR_OUT_OF_MEMORY;
}
state->rotation = CAMERA_ROTATION_0;
struct v4l2_format fmt = {};
// Retrieve bus handle from the parent I2C controller
auto* i2c = device_get_parent(device);
state->sccb_bus = esp32_i2c_master_get_bus_handle(i2c);
if (!state->sccb_bus) {
LOG_E(TAG, "Failed to get i2c_master bus handle from parent device");
vSemaphoreDelete(state->rotation_mutex);
heap_caps_free(state);
return ERROR_RESOURCE;
}
const auto* config = GET_CONFIG(device);
bool video_handle_acquired = false;
// esp_video_init only needs to run once across all gc0308_open calls.
{
SemaphoreHandle_t init_mutex = get_video_init_mutex();
if (!init_mutex) {
vSemaphoreDelete(state->rotation_mutex);
heap_caps_free(state);
return ERROR_OUT_OF_MEMORY;
}
xSemaphoreTake(init_mutex, portMAX_DELAY);
if (!s_video_initialized) {
esp_video_init_dvp_config_t dvp_config = {
.sccb_config = {
.init_sccb = false,
.i2c_handle = state->sccb_bus,
.freq = 100000,
},
.reset_pin = GPIO_NUM_NC,
.pwdn_pin = GPIO_NUM_NC,
.dvp_pin = {
.data_width = CAM_CTLR_DATA_WIDTH_8,
.data_io = {
static_cast<gpio_num_t>(config->pin_d0.pin),
static_cast<gpio_num_t>(config->pin_d1.pin),
static_cast<gpio_num_t>(config->pin_d2.pin),
static_cast<gpio_num_t>(config->pin_d3.pin),
static_cast<gpio_num_t>(config->pin_d4.pin),
static_cast<gpio_num_t>(config->pin_d5.pin),
static_cast<gpio_num_t>(config->pin_d6.pin),
static_cast<gpio_num_t>(config->pin_d7.pin),
},
.vsync_io = static_cast<gpio_num_t>(config->pin_vsync.pin),
.de_io = static_cast<gpio_num_t>(config->pin_de.pin),
.pclk_io = static_cast<gpio_num_t>(config->pin_pclk.pin),
.xclk_io = pin_or_nc(config->pin_xclk),
},
.xclk_freq = config->xclk_frequency_hz,
};
esp_video_init_config_t video_config = { .dvp = &dvp_config };
esp_err_t esp_err = esp_video_init_with_flags(&video_config, ESP_VIDEO_INIT_FLAGS_DVP);
if (esp_err != ESP_OK) {
LOG_E(TAG, "esp_video_init DVP failed: %s", esp_err_to_name(esp_err));
xSemaphoreGive(init_mutex);
vSemaphoreDelete(state->rotation_mutex);
heap_caps_free(state);
return ERROR_RESOURCE;
}
s_video_initialized = true;
}
++s_video_handle_count;
video_handle_acquired = true;
xSemaphoreGive(init_mutex);
}
state->fd = open(ESP_VIDEO_DVP_DEVICE_NAME, O_RDONLY);
if (state->fd < 0) {
LOG_E(TAG, "Failed to open %s", ESP_VIDEO_DVP_DEVICE_NAME);
goto err_close;
}
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (ioctl(state->fd, VIDIOC_G_FMT, &fmt) != 0) {
LOG_E(TAG, "VIDIOC_G_FMT failed");
goto err_close;
}
state->native_width = fmt.fmt.pix.width;
state->native_height = fmt.fmt.pix.height;
state->frame_buf_size = (size_t)state->native_width * state->native_height * 2;
state->rotation = add_rotation(CAMERA_ROTATION_0, config->rotation_offset);
if (state->rotation == CAMERA_ROTATION_90 || state->rotation == CAMERA_ROTATION_270) {
state->width = state->native_height;
state->height = state->native_width;
} else {
state->width = state->native_width;
state->height = state->native_height;
}
state->frame_buf = static_cast<uint8_t*>(heap_caps_aligned_alloc(16, state->frame_buf_size, MALLOC_CAP_SPIRAM));
if (!state->frame_buf) goto err_close;
{
struct v4l2_requestbuffers request = {};
request.count = VIDEO_BUFFER_COUNT;
request.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
request.memory = V4L2_MEMORY_MMAP;
if (ioctl(state->fd, VIDIOC_REQBUFS, &request) != 0) goto err_close;
}
for (int i = 0; i < VIDEO_BUFFER_COUNT; i++) {
struct v4l2_buffer buffer = {};
buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buffer.memory = V4L2_MEMORY_MMAP;
buffer.index = i;
if (ioctl(state->fd, VIDIOC_QUERYBUF, &buffer) != 0) goto err_unmap;
state->buf_lengths[i] = buffer.length;
state->buffers[i] = static_cast<uint8_t*>(mmap(nullptr, buffer.length, PROT_READ | PROT_WRITE, MAP_SHARED, state->fd, buffer.m.offset));
if (state->buffers[i] == MAP_FAILED) {
state->buffers[i] = nullptr;
goto err_unmap;
}
if (ioctl(state->fd, VIDIOC_QBUF, &buffer) != 0) goto err_unmap;
}
{
int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (ioctl(state->fd, VIDIOC_STREAMON, &type) != 0) goto err_unmap;
}
{
jpeg_enc_config_t encoder_config = {
.width = static_cast<int>(state->width),
.height = static_cast<int>(state->height),
.src_type = JPEG_PIXEL_FORMAT_RGB565_LE,
.subsampling = JPEG_SUBSAMPLE_420,
.quality = 80,
.rotate = JPEG_ROTATE_0D,
.task_enable = false,
.hfm_task_priority = 13,
.hfm_task_core = 1,
};
if (jpeg_enc_open(&encoder_config, &state->encoder) != JPEG_ERR_OK) goto err_unmap;
state->encoder_width = state->width;
state->encoder_height = state->height;
}
LOG_I(TAG, "Video format: %lux%lu native", (unsigned long)state->native_width, (unsigned long)state->native_height);
*out_handle = state;
return ERROR_NONE;
err_unmap:
if (state->encoder) {
jpeg_enc_close(state->encoder);
state->encoder = nullptr;
}
{
int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
ioctl(state->fd, VIDIOC_STREAMOFF, &type);
}
for (int i = 0; i < VIDEO_BUFFER_COUNT; i++) {
if (state->buffers[i]) munmap(state->buffers[i], state->buf_lengths[i]);
}
err_close:
if (state->fd >= 0) close(state->fd);
if (video_handle_acquired) {
SemaphoreHandle_t init_mutex = get_video_init_mutex();
xSemaphoreTake(init_mutex, portMAX_DELAY);
--s_video_handle_count;
if (s_video_handle_count == 0 && s_video_initialized) {
esp_video_deinit_with_flags(ESP_VIDEO_INIT_FLAGS_DVP);
s_video_initialized = false;
}
xSemaphoreGive(init_mutex);
}
vSemaphoreDelete(state->rotation_mutex);
heap_caps_free(state->frame_buf);
heap_caps_free(state);
return ERROR_RESOURCE;
}
error_t gc0308_close(Gc0308Handle handle) {
if (!handle) return ERROR_INVALID_ARGUMENT;
auto* state = static_cast<Gc0308State*>(handle);
if (state->encoder) jpeg_enc_close(state->encoder);
int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
ioctl(state->fd, VIDIOC_STREAMOFF, &type);
for (int i = 0; i < VIDEO_BUFFER_COUNT; i++) {
if (state->buffers[i]) munmap(state->buffers[i], state->buf_lengths[i]);
}
heap_caps_free(state->frame_buf);
close(state->fd);
SemaphoreHandle_t init_mutex = get_video_init_mutex();
xSemaphoreTake(init_mutex, portMAX_DELAY);
if (s_video_handle_count > 0) --s_video_handle_count;
if (s_video_handle_count == 0 && s_video_initialized) {
esp_video_deinit_with_flags(ESP_VIDEO_INIT_FLAGS_DVP);
s_video_initialized = false;
}
xSemaphoreGive(init_mutex);
if (state->rotation_mutex) vSemaphoreDelete(state->rotation_mutex);
// Bus handle is owned by esp32_i2c_master driver - do not delete it
heap_caps_free(state);
return ERROR_NONE;
}
error_t gc0308_set_rotation(Gc0308Handle handle, CameraRotation rotation) {
if (!handle) return ERROR_INVALID_ARGUMENT;
auto* state = static_cast<Gc0308State*>(handle);
CameraRotation effective_rotation = add_rotation(rotation, GET_CONFIG(state->device)->rotation_offset);
xSemaphoreTake(state->rotation_mutex, portMAX_DELAY);
if (state->rotation == effective_rotation) {
xSemaphoreGive(state->rotation_mutex);
return ERROR_NONE;
}
bool needs_swap = (effective_rotation == CAMERA_ROTATION_90 || effective_rotation == CAMERA_ROTATION_270);
state->rotation = effective_rotation;
if (needs_swap) {
state->width = state->native_height;
state->height = state->native_width;
} else {
state->width = state->native_width;
state->height = state->native_height;
}
xSemaphoreGive(state->rotation_mutex);
return ERROR_NONE;
}
error_t gc0308_get_frame(Gc0308Handle handle, uint8_t** buf, size_t* len, uint32_t timeout_ms, uint32_t* out_width, uint32_t* out_height) {
if (!handle || !buf || !len) return ERROR_INVALID_ARGUMENT;
auto* state = static_cast<Gc0308State*>(handle);
struct pollfd poll_descriptor = { .fd = state->fd, .events = POLLIN };
int poll_result = poll(&poll_descriptor, 1, static_cast<int>(timeout_ms));
if (poll_result == 0) return ERROR_TIMEOUT;
if (poll_result < 0) return ERROR_RESOURCE;
struct v4l2_buffer buffer = {};
buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buffer.memory = V4L2_MEMORY_MMAP;
if (ioctl(state->fd, VIDIOC_DQBUF, &buffer) != 0) return ERROR_RESOURCE;
state->last_dqbuf_index = static_cast<int>(buffer.index);
if (buffer.index >= VIDEO_BUFFER_COUNT || buffer.bytesused != state->frame_buf_size ||
(buffer.flags & V4L2_BUF_FLAG_ERROR) != 0) {
gc0308_release_frame(handle);
return ERROR_RESOURCE;
}
xSemaphoreTake(state->rotation_mutex, portMAX_DELAY);
CameraRotation rotation = state->rotation;
uint32_t frame_width = state->width;
uint32_t frame_height = state->height;
size_t copy_length = state->frame_buf_size;
const uint8_t* source = state->buffers[buffer.index];
for (uint32_t y = 0; y < frame_height; y++) {
for (uint32_t x = 0; x < frame_width; x++) {
uint32_t source_x;
uint32_t source_y;
switch (rotation) {
case CAMERA_ROTATION_90:
source_x = y;
source_y = state->native_height - 1 - x;
break;
case CAMERA_ROTATION_180:
source_x = state->native_width - 1 - x;
source_y = state->native_height - 1 - y;
break;
case CAMERA_ROTATION_270:
source_x = state->native_width - 1 - y;
source_y = x;
break;
default:
source_x = x;
source_y = y;
break;
}
size_t source_offset = ((size_t)source_y * state->native_width + source_x) * 2;
size_t destination_offset = ((size_t)y * frame_width + x) * 2;
if (source_offset + 1 >= copy_length) continue;
state->frame_buf[destination_offset] = source[source_offset + 1];
state->frame_buf[destination_offset + 1] = source[source_offset];
}
}
xSemaphoreGive(state->rotation_mutex);
*buf = state->frame_buf;
*len = (size_t)frame_width * frame_height * 2;
if (out_width != nullptr) {
*out_width = frame_width;
}
if (out_height != nullptr) {
*out_height = frame_height;
}
return ERROR_NONE;
}
error_t gc0308_release_frame(Gc0308Handle handle) {
if (!handle) return ERROR_INVALID_ARGUMENT;
auto* state = static_cast<Gc0308State*>(handle);
if (state->last_dqbuf_index < 0) return ERROR_INVALID_STATE;
struct v4l2_buffer buffer = {};
buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buffer.memory = V4L2_MEMORY_MMAP;
buffer.index = static_cast<uint32_t>(state->last_dqbuf_index);
state->last_dqbuf_index = -1;
return ioctl(state->fd, VIDIOC_QBUF, &buffer) == 0 ? ERROR_NONE : ERROR_RESOURCE;
}
uint32_t gc0308_get_width(Gc0308Handle handle) {
if (!handle) return 0;
auto* state = static_cast<Gc0308State*>(handle);
xSemaphoreTake(state->rotation_mutex, portMAX_DELAY);
uint32_t width = state->width;
xSemaphoreGive(state->rotation_mutex);
return width;
}
uint32_t gc0308_get_height(Gc0308Handle handle) {
if (!handle) return 0;
auto* state = static_cast<Gc0308State*>(handle);
xSemaphoreTake(state->rotation_mutex, portMAX_DELAY);
uint32_t height = state->height;
xSemaphoreGive(state->rotation_mutex);
return height;
}
error_t gc0308_capture_jpeg(Gc0308Handle handle, uint8_t** out_buf, size_t* out_len, uint8_t quality) {
if (!handle || !out_buf || !out_len) return ERROR_INVALID_ARGUMENT;
auto* state = static_cast<Gc0308State*>(handle);
// Dequeue one frame. Dimensions come back atomically with the frame data itself,
// so they can't drift if a concurrent gc0308_set_rotation() changes state->width/height.
uint8_t* frame = nullptr;
size_t frame_len = 0;
uint32_t frame_width = 0;
uint32_t frame_height = 0;
error_t err = gc0308_get_frame(handle, &frame, &frame_len, 500, &frame_width, &frame_height);
if (err != ERROR_NONE) return err;
if (frame == nullptr || frame_len == 0) {
gc0308_release_frame(handle);
return ERROR_RESOURCE;
}
size_t image_size = (size_t)frame_width * frame_height * 2;
uint8_t* aligned_input = static_cast<uint8_t*>(heap_caps_aligned_alloc(16, image_size, MALLOC_CAP_SPIRAM));
uint8_t* jpeg_output = static_cast<uint8_t*>(heap_caps_aligned_alloc(16, image_size, MALLOC_CAP_SPIRAM));
if (!aligned_input || !jpeg_output) {
heap_caps_free(aligned_input);
heap_caps_free(jpeg_output);
gc0308_release_frame(handle);
return ERROR_OUT_OF_MEMORY;
}
memcpy(aligned_input, frame, frame_len);
if (state->encoder_width != frame_width || state->encoder_height != frame_height) {
if (state->encoder) jpeg_enc_close(state->encoder);
state->encoder = nullptr;
jpeg_enc_config_t cfg = DEFAULT_JPEG_ENC_CONFIG();
cfg.width = static_cast<int>(frame_width);
cfg.height = static_cast<int>(frame_height);
cfg.src_type = JPEG_PIXEL_FORMAT_RGB565_LE;
cfg.subsampling = JPEG_SUBSAMPLE_420;
cfg.quality = quality;
if (jpeg_enc_open(&cfg, &state->encoder) != JPEG_ERR_OK) {
heap_caps_free(aligned_input);
heap_caps_free(jpeg_output);
gc0308_release_frame(handle);
return ERROR_RESOURCE;
}
state->encoder_width = frame_width;
state->encoder_height = frame_height;
}
jpeg_enc_set_quality(state->encoder, quality);
int encoded_size = 0;
jpeg_error_t encode_error = jpeg_enc_process(state->encoder, aligned_input, static_cast<int>(frame_len), jpeg_output, static_cast<int>(image_size), &encoded_size);
heap_caps_free(aligned_input);
gc0308_release_frame(handle);
if (encode_error != JPEG_ERR_OK || encoded_size <= 0) {
LOG_E(TAG, "JPEG conversion failed: %d", static_cast<int>(encode_error));
heap_caps_free(jpeg_output);
return ERROR_RESOURCE;
}
*out_buf = jpeg_output;
*out_len = encoded_size;
return ERROR_NONE;
}
CameraApi gc0308_camera_api = {
.open = gc0308_open,
.close = gc0308_close,
.get_frame = gc0308_get_frame,
.release_frame = gc0308_release_frame,
.get_width = gc0308_get_width,
.get_height = gc0308_get_height,
.set_rotation = gc0308_set_rotation,
.capture_jpeg = gc0308_capture_jpeg
};
Driver gc0308_driver = {
.name = "gc0308",
.compatible = (const char*[]) { "galaxycore,gc0308", nullptr },
.start_device = start,
.stop_device = stop,
.api = &gc0308_camera_api,
.device_type = &CAMERA_TYPE,
.owner = &gc0308_module,
.internal = nullptr
};
} // extern "C"
Reference in New Issue View Git Blame Copy Permalink