| /* |
| * 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); |