You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
489 lines
15 KiB
489 lines
15 KiB
/* |
|
* Copyright (c) 2019, Linaro Limited |
|
* |
|
* SPDX-License-Identifier: Apache-2.0 |
|
*/ |
|
|
|
#define DT_DRV_COMPAT zephyr_video_sw_generator |
|
|
|
#include <zephyr/drivers/video-controls.h> |
|
#include <zephyr/drivers/video.h> |
|
#include <zephyr/kernel.h> |
|
#include <zephyr/logging/log.h> |
|
#include <zephyr/sys/byteorder.h> |
|
#include <zephyr/sys/util.h> |
|
|
|
#include "video_ctrls.h" |
|
#include "video_device.h" |
|
|
|
LOG_MODULE_REGISTER(video_sw_generator, CONFIG_VIDEO_LOG_LEVEL); |
|
|
|
#define VIDEO_PATTERN_COLOR_BAR 0 |
|
#define DEFAULT_FRAME_RATE 30 |
|
/* |
|
* The pattern generator needs about 1.5 ms to fill out a 320x160 RGB565 |
|
* buffer and 25 ms for a 720p XRGB32 buffer (tested on i.MX RT1064). So, |
|
* the max frame rate actually varies between 40 and 666 fps depending on |
|
* the buffer format. There is no way to determine this value for each |
|
* format. 60 fps is therefore chosen as a common value in practice. |
|
*/ |
|
#define MAX_FRAME_RATE 60 |
|
#define MIN_FRAME_RATE 1 |
|
|
|
struct sw_ctrls { |
|
struct video_ctrl hflip; |
|
}; |
|
|
|
struct video_sw_generator_data { |
|
const struct device *dev; |
|
struct sw_ctrls ctrls; |
|
struct video_format fmt; |
|
struct k_fifo fifo_in; |
|
struct k_fifo fifo_out; |
|
struct k_work_delayable work; |
|
int pattern; |
|
struct k_poll_signal *sig; |
|
uint32_t frame_rate; |
|
}; |
|
|
|
#define VIDEO_SW_GENERATOR_FORMAT_CAP(pixfmt) \ |
|
{ \ |
|
.pixelformat = pixfmt, \ |
|
.width_min = 64, \ |
|
.width_max = 1920, \ |
|
.height_min = 64, \ |
|
.height_max = 1080, \ |
|
.width_step = 1, \ |
|
.height_step = 1, \ |
|
} |
|
|
|
static const struct video_format_cap fmts[] = { |
|
VIDEO_SW_GENERATOR_FORMAT_CAP(VIDEO_PIX_FMT_RGB24), |
|
VIDEO_SW_GENERATOR_FORMAT_CAP(VIDEO_PIX_FMT_YUYV), |
|
VIDEO_SW_GENERATOR_FORMAT_CAP(VIDEO_PIX_FMT_RGB565), |
|
VIDEO_SW_GENERATOR_FORMAT_CAP(VIDEO_PIX_FMT_XRGB32), |
|
VIDEO_SW_GENERATOR_FORMAT_CAP(VIDEO_PIX_FMT_SRGGB8), |
|
VIDEO_SW_GENERATOR_FORMAT_CAP(VIDEO_PIX_FMT_SGRBG8), |
|
VIDEO_SW_GENERATOR_FORMAT_CAP(VIDEO_PIX_FMT_SBGGR8), |
|
VIDEO_SW_GENERATOR_FORMAT_CAP(VIDEO_PIX_FMT_SGBRG8), |
|
{0}, |
|
}; |
|
|
|
static int video_sw_generator_set_fmt(const struct device *dev, struct video_format *fmt) |
|
{ |
|
struct video_sw_generator_data *data = dev->data; |
|
size_t idx; |
|
int ret; |
|
|
|
ret = video_format_caps_index(fmts, fmt, &idx); |
|
if (ret < 0) { |
|
LOG_ERR("Unsupported pixel format or resolution"); |
|
return ret; |
|
} |
|
|
|
fmt->pitch = fmt->width * video_bits_per_pixel(fmt->pixelformat) / BITS_PER_BYTE; |
|
|
|
data->fmt = *fmt; |
|
return 0; |
|
} |
|
|
|
static int video_sw_generator_get_fmt(const struct device *dev, struct video_format *fmt) |
|
{ |
|
struct video_sw_generator_data *data = dev->data; |
|
|
|
*fmt = data->fmt; |
|
|
|
return 0; |
|
} |
|
|
|
static int video_sw_generator_set_stream(const struct device *dev, bool enable, |
|
enum video_buf_type type) |
|
{ |
|
struct video_sw_generator_data *data = dev->data; |
|
struct k_work_sync work_sync = {0}; |
|
|
|
if (enable) { |
|
k_work_schedule(&data->work, K_MSEC(1000 / data->frame_rate)); |
|
} else { |
|
k_work_cancel_delayable_sync(&data->work, &work_sync); |
|
} |
|
|
|
return 0; |
|
} |
|
|
|
static const uint8_t pattern_rggb_idx[] = {0, 1, 1, 2}; |
|
static const uint8_t pattern_bggr_idx[] = {2, 1, 1, 0}; |
|
static const uint8_t pattern_gbrg_idx[] = {1, 2, 0, 1}; |
|
static const uint8_t pattern_grbg_idx[] = {1, 0, 2, 1}; |
|
|
|
/* White, Yellow, Cyan, Green, Magenta, Red, Blue, Black */ |
|
|
|
static const uint16_t pattern_8bars_yuv_bt709[8][3] = { |
|
{0xFE, 0x80, 0x7F}, {0xEC, 0x00, 0x8B}, {0xC8, 0x9D, 0x00}, {0xB6, 0x1D, 0x0C}, |
|
{0x48, 0xE2, 0xF3}, {0x36, 0x62, 0xFF}, {0x12, 0xFF, 0x74}, {0x00, 0x80, 0x80}, |
|
}; |
|
|
|
static const uint16_t pattern_8bars_rgb[8][3] = { |
|
{0xFF, 0xFF, 0xFF}, {0xFF, 0xFF, 0x00}, {0x00, 0xFF, 0xFF}, {0x00, 0xFF, 0x00}, |
|
{0xFF, 0x00, 0xFF}, {0xFF, 0x00, 0x00}, {0x00, 0x00, 0xFF}, {0x00, 0x00, 0x00}, |
|
}; |
|
|
|
static inline int video_sw_generator_get_color_idx(uint16_t w, uint16_t width, bool hflip) |
|
{ |
|
/* If hflip is on, start from the right instead */ |
|
w = (hflip) ? (width - w - 1) : (w); |
|
|
|
/* Downscale from w/width to #/8 */ |
|
return 8 * w / width; |
|
} |
|
|
|
static uint16_t video_sw_generator_fill_yuyv(uint8_t *buffer, uint16_t width, bool hflip) |
|
{ |
|
if (width % 2 != 0) { |
|
LOG_ERR("YUYV pixels always go by pairs"); |
|
return 0; |
|
} |
|
|
|
for (size_t w = 0; w < width; w += 2) { |
|
int color_idx = video_sw_generator_get_color_idx(w, width, hflip); |
|
|
|
buffer[w * 2 + 0] = pattern_8bars_yuv_bt709[color_idx][0]; |
|
buffer[w * 2 + 1] = pattern_8bars_yuv_bt709[color_idx][1]; |
|
buffer[w * 2 + 2] = pattern_8bars_yuv_bt709[color_idx][0]; |
|
buffer[w * 2 + 3] = pattern_8bars_yuv_bt709[color_idx][2]; |
|
} |
|
return 1; |
|
} |
|
|
|
static uint16_t video_sw_generator_fill_xrgb32(uint8_t *buffer, uint16_t width, bool hflip) |
|
{ |
|
for (size_t w = 0; w < width; w++) { |
|
int color_idx = video_sw_generator_get_color_idx(w, width, hflip); |
|
|
|
buffer[w * 4 + 0] = 0xff; |
|
buffer[w * 4 + 1] = pattern_8bars_rgb[color_idx][0]; |
|
buffer[w * 4 + 2] = pattern_8bars_rgb[color_idx][1]; |
|
buffer[w * 4 + 3] = pattern_8bars_rgb[color_idx][2]; |
|
} |
|
return 1; |
|
} |
|
|
|
static uint16_t video_sw_generator_fill_rgb24(uint8_t *buffer, uint16_t width, bool hflip) |
|
{ |
|
for (size_t w = 0; w < width; w++) { |
|
int color_idx = video_sw_generator_get_color_idx(w, width, hflip); |
|
|
|
buffer[w * 3 + 0] = pattern_8bars_rgb[color_idx][0]; |
|
buffer[w * 3 + 1] = pattern_8bars_rgb[color_idx][1]; |
|
buffer[w * 3 + 2] = pattern_8bars_rgb[color_idx][2]; |
|
} |
|
return 1; |
|
} |
|
|
|
static uint16_t video_sw_generator_fill_rgb565(uint8_t *buffer, uint16_t width, bool hflip) |
|
{ |
|
for (size_t w = 0; w < width; w++) { |
|
int color_idx = video_sw_generator_get_color_idx(w, width, hflip); |
|
uint8_t r = pattern_8bars_rgb[color_idx][0] >> (8 - 5); |
|
uint8_t g = pattern_8bars_rgb[color_idx][1] >> (8 - 6); |
|
uint8_t b = pattern_8bars_rgb[color_idx][2] >> (8 - 5); |
|
|
|
((uint16_t *)buffer)[w] = sys_cpu_to_le16((r << 11) | (g << 6) | (b << 0)); |
|
} |
|
return 1; |
|
} |
|
|
|
static uint16_t video_sw_generator_fill_bayer8(uint8_t *buffer, uint16_t width, bool hflip, |
|
const uint8_t *bayer_idx) |
|
{ |
|
uint8_t *row0 = buffer + 0; |
|
uint8_t *row1 = buffer + width; |
|
|
|
if (width % 2 != 0) { |
|
LOG_ERR("Bayer pixels always go by pairs (vertically and horizontally)"); |
|
return 0; |
|
} |
|
|
|
for (size_t w = 0; w < width; w += 2) { |
|
int color_idx = video_sw_generator_get_color_idx(w, width, hflip); |
|
|
|
row0[w + 0] = pattern_8bars_rgb[color_idx][bayer_idx[0]]; |
|
row0[w + 1] = pattern_8bars_rgb[color_idx][bayer_idx[1]]; |
|
row1[w + 0] = pattern_8bars_rgb[color_idx][bayer_idx[2]]; |
|
row1[w + 1] = pattern_8bars_rgb[color_idx][bayer_idx[3]]; |
|
} |
|
return 2; |
|
} |
|
|
|
static int video_sw_generator_fill(const struct device *const dev, struct video_buffer *vbuf) |
|
{ |
|
struct video_sw_generator_data *data = dev->data; |
|
struct video_format *fmt = &data->fmt; |
|
size_t pitch = fmt->width * video_bits_per_pixel(fmt->pixelformat) / BITS_PER_BYTE; |
|
bool hflip = data->ctrls.hflip.val; |
|
uint16_t lines = 0; |
|
|
|
if (vbuf->size < pitch * 2) { |
|
LOG_ERR("At least 2 lines needed for bayer formats support"); |
|
return -EINVAL; |
|
} |
|
|
|
/* Fill the first row of the emulated framebuffer */ |
|
switch (data->fmt.pixelformat) { |
|
case VIDEO_PIX_FMT_YUYV: |
|
lines = video_sw_generator_fill_yuyv(vbuf->buffer, fmt->width, hflip); |
|
break; |
|
case VIDEO_PIX_FMT_XRGB32: |
|
lines = video_sw_generator_fill_xrgb32(vbuf->buffer, fmt->width, hflip); |
|
break; |
|
case VIDEO_PIX_FMT_RGB24: |
|
lines = video_sw_generator_fill_rgb24(vbuf->buffer, fmt->width, hflip); |
|
break; |
|
case VIDEO_PIX_FMT_RGB565: |
|
lines = video_sw_generator_fill_rgb565(vbuf->buffer, fmt->width, hflip); |
|
break; |
|
case VIDEO_PIX_FMT_SRGGB8: |
|
lines = video_sw_generator_fill_bayer8(vbuf->buffer, fmt->width, hflip, |
|
pattern_rggb_idx); |
|
break; |
|
case VIDEO_PIX_FMT_SGBRG8: |
|
lines = video_sw_generator_fill_bayer8(vbuf->buffer, fmt->width, hflip, |
|
pattern_gbrg_idx); |
|
break; |
|
case VIDEO_PIX_FMT_SBGGR8: |
|
lines = video_sw_generator_fill_bayer8(vbuf->buffer, fmt->width, hflip, |
|
pattern_bggr_idx); |
|
break; |
|
case VIDEO_PIX_FMT_SGRBG8: |
|
lines = video_sw_generator_fill_bayer8(vbuf->buffer, fmt->width, hflip, |
|
pattern_grbg_idx); |
|
break; |
|
default: |
|
CODE_UNREACHABLE; |
|
break; |
|
} |
|
|
|
if (lines == 0) { |
|
return -EINVAL; |
|
} |
|
|
|
/* How much was filled in so far */ |
|
vbuf->bytesused = data->fmt.pitch * lines; |
|
|
|
/* Duplicate the first line(s) all over the buffer */ |
|
for (int h = lines; h < data->fmt.height; h += lines) { |
|
if (vbuf->size < vbuf->bytesused + pitch * lines) { |
|
LOG_WRN("Generation stopped early: buffer too small"); |
|
break; |
|
} |
|
memcpy(vbuf->buffer + h * pitch, vbuf->buffer, pitch * lines); |
|
vbuf->bytesused += pitch * lines; |
|
} |
|
|
|
vbuf->timestamp = k_uptime_get_32(); |
|
vbuf->line_offset = 0; |
|
|
|
return 0; |
|
} |
|
|
|
static void video_sw_generator_worker(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, work); |
|
|
|
k_work_reschedule(&data->work, K_MSEC(1000 / data->frame_rate)); |
|
|
|
vbuf = k_fifo_get(&data->fifo_in, K_NO_WAIT); |
|
if (vbuf == NULL) { |
|
return; |
|
} |
|
|
|
switch (data->pattern) { |
|
case VIDEO_PATTERN_COLOR_BAR: |
|
video_sw_generator_fill(data->dev, vbuf); |
|
break; |
|
} |
|
|
|
k_fifo_put(&data->fifo_out, vbuf); |
|
|
|
if (IS_ENABLED(CONFIG_POLL) && data->sig) { |
|
k_poll_signal_raise(data->sig, VIDEO_BUF_DONE); |
|
} |
|
|
|
k_yield(); |
|
} |
|
|
|
static int video_sw_generator_enqueue(const struct device *dev, struct video_buffer *vbuf) |
|
{ |
|
struct video_sw_generator_data *data = dev->data; |
|
|
|
k_fifo_put(&data->fifo_in, vbuf); |
|
|
|
return 0; |
|
} |
|
|
|
static int video_sw_generator_dequeue(const struct device *dev, struct video_buffer **vbuf, |
|
k_timeout_t timeout) |
|
{ |
|
struct video_sw_generator_data *data = dev->data; |
|
|
|
*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, 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->sig) { |
|
k_poll_signal_raise(data->sig, VIDEO_BUF_ABORTED); |
|
} |
|
} |
|
} |
|
|
|
return 0; |
|
} |
|
|
|
static int video_sw_generator_get_caps(const struct device *dev, struct video_caps *caps) |
|
{ |
|
caps->format_caps = fmts; |
|
caps->min_vbuf_count = 0; |
|
|
|
/* SW generator produces full frames */ |
|
caps->min_line_count = caps->max_line_count = LINE_COUNT_HEIGHT; |
|
|
|
return 0; |
|
} |
|
|
|
#ifdef CONFIG_POLL |
|
static int video_sw_generator_set_signal(const struct device *dev, struct k_poll_signal *sig) |
|
{ |
|
struct video_sw_generator_data *data = dev->data; |
|
|
|
if (data->sig && sig != NULL) { |
|
return -EALREADY; |
|
} |
|
|
|
data->sig = sig; |
|
|
|
return 0; |
|
} |
|
#endif |
|
|
|
static int video_sw_generator_set_frmival(const struct device *dev, struct video_frmival *frmival) |
|
{ |
|
struct video_sw_generator_data *data = dev->data; |
|
|
|
data->frame_rate = CLAMP(DIV_ROUND_CLOSEST(frmival->denominator, frmival->numerator), |
|
MIN_FRAME_RATE, MAX_FRAME_RATE); |
|
frmival->numerator = 1; |
|
frmival->denominator = data->frame_rate; |
|
|
|
return 0; |
|
} |
|
|
|
static int video_sw_generator_get_frmival(const struct device *dev, struct video_frmival *frmival) |
|
{ |
|
struct video_sw_generator_data *data = dev->data; |
|
|
|
frmival->numerator = 1; |
|
frmival->denominator = data->frame_rate; |
|
|
|
return 0; |
|
} |
|
|
|
static int video_sw_generator_enum_frmival(const struct device *dev, struct video_frmival_enum *fie) |
|
{ |
|
size_t idx; |
|
int ret; |
|
|
|
if (fie->index >= 1) { |
|
return -ERANGE; |
|
} |
|
|
|
ret = video_format_caps_index(fmts, fie->format, &idx); |
|
if (ret < 0) { |
|
LOG_ERR("Unsupported pixel format or resolution"); |
|
return ret; |
|
} |
|
|
|
fie->type = VIDEO_FRMIVAL_TYPE_STEPWISE; |
|
fie->stepwise.min.numerator = 1; |
|
fie->stepwise.min.denominator = MAX_FRAME_RATE; |
|
fie->stepwise.max.numerator = UINT32_MAX; |
|
fie->stepwise.max.denominator = 1; |
|
/* The frame interval step size is the minimum resolution of K_MSEC(), which is 1ms */ |
|
fie->stepwise.step.numerator = 1; |
|
fie->stepwise.step.denominator = 1000; |
|
|
|
return 0; |
|
} |
|
|
|
static DEVICE_API(video, video_sw_generator_driver_api) = { |
|
.set_format = video_sw_generator_set_fmt, |
|
.get_format = video_sw_generator_get_fmt, |
|
.set_stream = video_sw_generator_set_stream, |
|
.flush = video_sw_generator_flush, |
|
.enqueue = video_sw_generator_enqueue, |
|
.dequeue = video_sw_generator_dequeue, |
|
.get_caps = video_sw_generator_get_caps, |
|
.set_frmival = video_sw_generator_set_frmival, |
|
.get_frmival = video_sw_generator_get_frmival, |
|
.enum_frmival = video_sw_generator_enum_frmival, |
|
#ifdef CONFIG_POLL |
|
.set_signal = video_sw_generator_set_signal, |
|
#endif |
|
}; |
|
|
|
static int video_sw_generator_init_controls(const struct device *dev) |
|
{ |
|
struct video_sw_generator_data *data = dev->data; |
|
|
|
return video_init_ctrl(&data->ctrls.hflip, dev, VIDEO_CID_HFLIP, |
|
(struct video_ctrl_range){.min = 0, .max = 1, .step = 1, .def = 0}); |
|
} |
|
|
|
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->work, video_sw_generator_worker); |
|
|
|
return video_sw_generator_init_controls(dev); |
|
} |
|
|
|
#define VIDEO_SW_GENERATOR_DEFINE(n) \ |
|
static struct video_sw_generator_data video_sw_generator_data_##n = { \ |
|
.fmt.width = 320, \ |
|
.fmt.height = 160, \ |
|
.fmt.pitch = 320 * 2, \ |
|
.fmt.pixelformat = VIDEO_PIX_FMT_RGB565, \ |
|
.frame_rate = DEFAULT_FRAME_RATE, \ |
|
}; \ |
|
\ |
|
DEVICE_DT_INST_DEFINE(n, &video_sw_generator_init, NULL, &video_sw_generator_data_##n, \ |
|
NULL, POST_KERNEL, CONFIG_VIDEO_INIT_PRIORITY, \ |
|
&video_sw_generator_driver_api); \ |
|
\ |
|
VIDEO_DEVICE_DEFINE(video_sw_generator_##n, DEVICE_DT_INST_GET(n), NULL); |
|
|
|
DT_INST_FOREACH_STATUS_OKAY(VIDEO_SW_GENERATOR_DEFINE)
|
|
|