301 lines
7.2 KiB
C
301 lines
7.2 KiB
C
/*
|
|
* Copyright (c) 2019, Linaro Limited
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*/
|
|
#include <zephyr/kernel.h>
|
|
|
|
#include <zephyr/drivers/video.h>
|
|
|
|
#define LOG_LEVEL CONFIG_LOG_DEFAULT_LEVEL
|
|
#include <zephyr/logging/log.h>
|
|
LOG_MODULE_REGISTER(video_sw_generator);
|
|
|
|
#define VIDEO_PATTERN_COLOR_BAR 0
|
|
#define VIDEO_PATTERN_FPS 30
|
|
|
|
struct video_sw_generator_data {
|
|
const struct device *dev;
|
|
struct video_format fmt;
|
|
struct k_fifo fifo_in;
|
|
struct k_fifo fifo_out;
|
|
struct k_work_delayable buf_work;
|
|
struct k_work_sync work_sync;
|
|
int pattern;
|
|
bool ctrl_hflip;
|
|
bool ctrl_vflip;
|
|
struct k_poll_signal *signal;
|
|
};
|
|
|
|
static const struct video_format_cap fmts[] = {{
|
|
.pixelformat = VIDEO_PIX_FMT_RGB565,
|
|
.width_min = 64,
|
|
.width_max = 1920,
|
|
.height_min = 64,
|
|
.height_max = 1080,
|
|
.width_step = 1,
|
|
.height_step = 1,
|
|
}, {
|
|
.pixelformat = VIDEO_PIX_FMT_XRGB32,
|
|
.width_min = 64,
|
|
.width_max = 1920,
|
|
.height_min = 64,
|
|
.height_max = 1080,
|
|
.width_step = 1,
|
|
.height_step = 1,
|
|
},
|
|
{0}};
|
|
|
|
static int video_sw_generator_set_fmt(const struct device *dev, enum video_endpoint_id ep,
|
|
struct video_format *fmt)
|
|
{
|
|
struct video_sw_generator_data *data = dev->data;
|
|
int i = 0;
|
|
|
|
if (ep != VIDEO_EP_OUT) {
|
|
return -EINVAL;
|
|
}
|
|
|
|
for (i = 0; i < ARRAY_SIZE(fmts); ++i) {
|
|
if (fmt->pixelformat == fmts[i].pixelformat && fmt->width >= fmts[i].width_min &&
|
|
fmt->width <= fmts[i].width_max && fmt->height >= fmts[i].height_min &&
|
|
fmt->height <= fmts[i].height_max) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (i == ARRAY_SIZE(fmts)) {
|
|
LOG_ERR("Unsupported pixel format or resolution");
|
|
return -ENOTSUP;
|
|
}
|
|
|
|
data->fmt = *fmt;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int video_sw_generator_get_fmt(const struct device *dev, enum video_endpoint_id ep,
|
|
struct video_format *fmt)
|
|
{
|
|
struct video_sw_generator_data *data = dev->data;
|
|
|
|
if (ep != VIDEO_EP_OUT) {
|
|
return -EINVAL;
|
|
}
|
|
|
|
*fmt = data->fmt;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int video_sw_generator_stream_start(const struct device *dev)
|
|
{
|
|
struct video_sw_generator_data *data = dev->data;
|
|
|
|
k_work_schedule(&data->buf_work, K_MSEC(1000 / VIDEO_PATTERN_FPS));
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int video_sw_generator_stream_stop(const struct device *dev)
|
|
{
|
|
struct video_sw_generator_data *data = dev->data;
|
|
|
|
k_work_cancel_delayable_sync(&data->buf_work, &data->work_sync);
|
|
|
|
return 0;
|
|
}
|
|
|
|
/* Black, Blue, Red, Purple, Green, Aqua, Yellow, White */
|
|
uint16_t rgb565_colorbar_value[] = {0x0000, 0x001F, 0xF800, 0xF81F, 0x07E0, 0x07FF, 0xFFE0, 0xFFFF};
|
|
|
|
uint32_t xrgb32_colorbar_value[] = {0xFF000000, 0xFF0000FF, 0xFFFF0000, 0xFFFF00FF,
|
|
0xFF00FF00, 0xFF00FFFF, 0xFFFFFF00, 0xFFFFFFFF};
|
|
|
|
static void __fill_buffer_colorbar(struct video_sw_generator_data *data, struct video_buffer *vbuf)
|
|
{
|
|
int bw = data->fmt.width / 8;
|
|
int h, w, i = 0;
|
|
|
|
for (h = 0; h < data->fmt.height; h++) {
|
|
for (w = 0; w < data->fmt.width; w++) {
|
|
int color_idx = data->ctrl_vflip ? 7 - w / bw : w / bw;
|
|
if (data->fmt.pixelformat == VIDEO_PIX_FMT_RGB565) {
|
|
uint16_t *pixel = (uint16_t *)&vbuf->buffer[i];
|
|
*pixel = rgb565_colorbar_value[color_idx];
|
|
i += 2;
|
|
} else if (data->fmt.pixelformat == VIDEO_PIX_FMT_XRGB32) {
|
|
uint32_t *pixel = (uint32_t *)&vbuf->buffer[i];
|
|
*pixel = xrgb32_colorbar_value[color_idx];
|
|
i += 4;
|
|
}
|
|
}
|
|
}
|
|
|
|
vbuf->timestamp = k_uptime_get_32();
|
|
vbuf->bytesused = i;
|
|
}
|
|
|
|
static void __buffer_work(struct k_work *work)
|
|
{
|
|
struct k_work_delayable *dwork = k_work_delayable_from_work(work);
|
|
struct video_sw_generator_data *data;
|
|
struct video_buffer *vbuf;
|
|
|
|
data = CONTAINER_OF(dwork, struct video_sw_generator_data, buf_work);
|
|
|
|
k_work_reschedule(&data->buf_work, K_MSEC(1000 / VIDEO_PATTERN_FPS));
|
|
|
|
vbuf = k_fifo_get(&data->fifo_in, K_NO_WAIT);
|
|
if (vbuf == NULL) {
|
|
return;
|
|
}
|
|
|
|
switch (data->pattern) {
|
|
case VIDEO_PATTERN_COLOR_BAR:
|
|
__fill_buffer_colorbar(data, vbuf);
|
|
break;
|
|
}
|
|
|
|
k_fifo_put(&data->fifo_out, vbuf);
|
|
|
|
if (IS_ENABLED(CONFIG_POLL) && data->signal) {
|
|
k_poll_signal_raise(data->signal, VIDEO_BUF_DONE);
|
|
}
|
|
|
|
k_yield();
|
|
}
|
|
|
|
static int video_sw_generator_enqueue(const struct device *dev, enum video_endpoint_id ep,
|
|
struct video_buffer *vbuf)
|
|
{
|
|
struct video_sw_generator_data *data = dev->data;
|
|
|
|
if (ep != VIDEO_EP_OUT) {
|
|
return -EINVAL;
|
|
}
|
|
|
|
k_fifo_put(&data->fifo_in, vbuf);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int video_sw_generator_dequeue(const struct device *dev, enum video_endpoint_id ep,
|
|
struct video_buffer **vbuf, k_timeout_t timeout)
|
|
{
|
|
struct video_sw_generator_data *data = dev->data;
|
|
|
|
if (ep != VIDEO_EP_OUT) {
|
|
return -EINVAL;
|
|
}
|
|
|
|
*vbuf = k_fifo_get(&data->fifo_out, timeout);
|
|
if (*vbuf == NULL) {
|
|
return -EAGAIN;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int video_sw_generator_flush(const struct device *dev, enum video_endpoint_id ep,
|
|
bool cancel)
|
|
{
|
|
struct video_sw_generator_data *data = dev->data;
|
|
struct video_buffer *vbuf;
|
|
|
|
if (!cancel) {
|
|
/* wait for all buffer to be processed */
|
|
do {
|
|
k_sleep(K_MSEC(1));
|
|
} while (!k_fifo_is_empty(&data->fifo_in));
|
|
} else {
|
|
while ((vbuf = k_fifo_get(&data->fifo_in, K_NO_WAIT))) {
|
|
k_fifo_put(&data->fifo_out, vbuf);
|
|
if (IS_ENABLED(CONFIG_POLL) && data->signal) {
|
|
k_poll_signal_raise(data->signal, VIDEO_BUF_ABORTED);
|
|
}
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int video_sw_generator_get_caps(const struct device *dev, enum video_endpoint_id ep,
|
|
struct video_caps *caps)
|
|
{
|
|
caps->format_caps = fmts;
|
|
caps->min_vbuf_count = 0;
|
|
|
|
return 0;
|
|
}
|
|
|
|
#ifdef CONFIG_POLL
|
|
static int video_sw_generator_set_signal(const struct device *dev, enum video_endpoint_id ep,
|
|
struct k_poll_signal *signal)
|
|
{
|
|
struct video_sw_generator_data *data = dev->data;
|
|
|
|
if (data->signal && signal != NULL) {
|
|
return -EALREADY;
|
|
}
|
|
|
|
data->signal = signal;
|
|
|
|
return 0;
|
|
}
|
|
#endif
|
|
|
|
static inline int video_sw_generator_set_ctrl(const struct device *dev, unsigned int cid,
|
|
void *value)
|
|
{
|
|
struct video_sw_generator_data *data = dev->data;
|
|
|
|
switch (cid) {
|
|
case VIDEO_CID_VFLIP:
|
|
data->ctrl_vflip = (bool)value;
|
|
break;
|
|
default:
|
|
return -ENOTSUP;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
static const struct video_driver_api video_sw_generator_driver_api = {
|
|
.set_format = video_sw_generator_set_fmt,
|
|
.get_format = video_sw_generator_get_fmt,
|
|
.stream_start = video_sw_generator_stream_start,
|
|
.stream_stop = video_sw_generator_stream_stop,
|
|
.flush = video_sw_generator_flush,
|
|
.enqueue = video_sw_generator_enqueue,
|
|
.dequeue = video_sw_generator_dequeue,
|
|
.get_caps = video_sw_generator_get_caps,
|
|
.set_ctrl = video_sw_generator_set_ctrl,
|
|
#ifdef CONFIG_POLL
|
|
.set_signal = video_sw_generator_set_signal,
|
|
#endif
|
|
};
|
|
|
|
static struct video_sw_generator_data video_sw_generator_data_0 = {
|
|
.fmt.width = 320,
|
|
.fmt.height = 160,
|
|
.fmt.pitch = 320 * 2,
|
|
.fmt.pixelformat = VIDEO_PIX_FMT_RGB565,
|
|
};
|
|
|
|
static int video_sw_generator_init(const struct device *dev)
|
|
{
|
|
struct video_sw_generator_data *data = dev->data;
|
|
|
|
data->dev = dev;
|
|
k_fifo_init(&data->fifo_in);
|
|
k_fifo_init(&data->fifo_out);
|
|
k_work_init_delayable(&data->buf_work, __buffer_work);
|
|
|
|
return 0;
|
|
}
|
|
|
|
DEVICE_DEFINE(video_sw_generator, "VIDEO_SW_GENERATOR", &video_sw_generator_init, NULL,
|
|
&video_sw_generator_data_0, NULL, POST_KERNEL, CONFIG_VIDEO_INIT_PRIORITY,
|
|
&video_sw_generator_driver_api);
|