bartoszek/zephyr
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
35d94de056
zephyr/drivers/video/video_emul_imager.c
Phi Bang Nguyen b341d9d6a4 video: Merge video_stream_start/stop driver APIs 
The video_stream_start/stop() APIs are counter-symetric and have
the same function signature. Also, the implementation logic for
those driver APIs is generally the same. Merge them to save memory
and code lines.

For the sake of simplicity, still keep the user APIs to preserve
backward compatibility with downstream applications.

Signed-off-by: Phi Bang Nguyen <phibang.nguyen@nxp.com>
2025-02-11 07:43:58 +01:00

492 lines
15 KiB
C
Raw Blame History

/*
* Copyright (c) 2024 tinyVision.ai Inc.
*
* SPDX-License-Identifier: Apache-2.0
*/
#define DT_DRV_COMPAT zephyr_video_emul_imager
#include <string.h>
#include <zephyr/kernel.h>
#include <zephyr/device.h>
#include <zephyr/sys/util.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/drivers/video.h>
#include <zephyr/drivers/video-controls.h>
#include <zephyr/drivers/i2c.h>
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(video_emul_imager, CONFIG_VIDEO_LOG_LEVEL);
#define EMUL_IMAGER_REG_SENSOR_ID 0x0000
#define EMUL_IMAGER_SENSOR_ID 0x99
#define EMUL_IMAGER_REG_CTRL 0x0001
#define EMUL_IMAGER_REG_INIT1 0x0002
#define EMUL_IMAGER_REG_INIT2 0x0003
#define EMUL_IMAGER_REG_TIMING1 0x0004
#define EMUL_IMAGER_REG_TIMING2 0x0005
#define EMUL_IMAGER_REG_TIMING3 0x0006
#define EMUL_IMAGER_REG_EXPOSURE 0x0007
#define EMUL_IMAGER_REG_GAIN 0x0008
#define EMUL_IMAGER_REG_PATTERN 0x0009
#define EMUL_IMAGER_PATTERN_OFF 0x00
#define EMUL_IMAGER_PATTERN_BARS1 0x01
#define EMUL_IMAGER_PATTERN_BARS2 0x02
/* Emulated register bank */
uint8_t emul_imager_fake_regs[10];
enum emul_imager_fmt_id {
RGB565_64x20,
YUYV_64x20,
};
struct emul_imager_reg {
uint16_t addr;
uint8_t value;
};
struct emul_imager_mode {
uint8_t fps;
/* List of registers lists to configure the various properties of the sensor.
* This permits to deduplicate the list of registers in case some lare sections
* are repeated across modes, such as the resolution for different FPS.
*/
const struct emul_imager_reg *regs[2];
/* More fields can be added according to the needs of the sensor driver */
};
struct emul_imager_config {
struct i2c_dt_spec i2c;
};
struct emul_imager_data {
/* First field is a framebuffer for I/O emulation purpose */
uint8_t framebuffer[CONFIG_VIDEO_EMUL_IMAGER_FRAMEBUFFER_SIZE];
/* Other fields are shared with real hardware drivers */
const struct emul_imager_mode *mode;
enum emul_imager_fmt_id fmt_id;
struct video_format fmt;
};
/* Initial parameters of the sensors common to all modes. */
static const struct emul_imager_reg emul_imager_init_regs[] = {
{EMUL_IMAGER_REG_CTRL, 0x00},
/* Example comment about REG_INIT1 */
{EMUL_IMAGER_REG_INIT1, 0x10},
{EMUL_IMAGER_REG_INIT2, 0x00},
{0},
};
/* List of registers aggregated together in "modes" that can be applied
* to set the timing parameters and other mode-dependent configuration.
*/
static const struct emul_imager_reg emul_imager_rgb565_64x20[] = {
{EMUL_IMAGER_REG_TIMING1, 0x64},
{EMUL_IMAGER_REG_TIMING2, 0x20},
{0},
};
static const struct emul_imager_reg emul_imager_rgb565_64x20_15fps[] = {
{EMUL_IMAGER_REG_TIMING3, 15},
{0},
};
static const struct emul_imager_reg emul_imager_rgb565_64x20_30fps[] = {
{EMUL_IMAGER_REG_TIMING3, 30},
{0},
};
static const struct emul_imager_reg emul_imager_rgb565_64x20_60fps[] = {
{EMUL_IMAGER_REG_TIMING3, 60},
{0},
};
struct emul_imager_mode emul_imager_rgb565_64x20_modes[] = {
{.fps = 15, .regs = {emul_imager_rgb565_64x20, emul_imager_rgb565_64x20_15fps}},
{.fps = 30, .regs = {emul_imager_rgb565_64x20, emul_imager_rgb565_64x20_30fps}},
{.fps = 60, .regs = {emul_imager_rgb565_64x20, emul_imager_rgb565_64x20_60fps}},
{0},
};
static const struct emul_imager_reg emul_imager_yuyv_64x20[] = {
{EMUL_IMAGER_REG_TIMING1, 0x64},
{EMUL_IMAGER_REG_TIMING2, 0x20},
{0},
};
static const struct emul_imager_reg emul_imager_yuyv_64x20_15fps[] = {
{EMUL_IMAGER_REG_TIMING3, 15},
{0},
};
static const struct emul_imager_reg emul_imager_yuyv_64x20_30fps[] = {
{EMUL_IMAGER_REG_TIMING3, 30},
{0},
};
struct emul_imager_mode emul_imager_yuyv_64x20_modes[] = {
{.fps = 15, .regs = {emul_imager_yuyv_64x20, emul_imager_yuyv_64x20_15fps}},
{.fps = 30, .regs = {emul_imager_yuyv_64x20, emul_imager_yuyv_64x20_30fps}},
{0},
};
/* Summary of all the modes of all the frame formats, with the format ID as
* index, matching fmts[].
*/
static const struct emul_imager_mode *emul_imager_modes[] = {
[RGB565_64x20] = emul_imager_rgb565_64x20_modes,
[YUYV_64x20] = emul_imager_yuyv_64x20_modes,
};
/* Video device capabilities where the supported resolutions and pixel formats are listed.
* The format ID is used as index to fetch the matching mode from the list above.
*/
#define EMUL_IMAGER_VIDEO_FORMAT_CAP(width, height, format) \
{ \
.pixelformat = (format), \
.width_min = (width), \
.width_max = (width), \
.height_min = (height), \
.height_max = (height), \
.width_step = 0, \
.height_step = 0, \
}
static const struct video_format_cap fmts[] = {
[RGB565_64x20] = EMUL_IMAGER_VIDEO_FORMAT_CAP(64, 20, VIDEO_PIX_FMT_RGB565),
[YUYV_64x20] = EMUL_IMAGER_VIDEO_FORMAT_CAP(64, 20, VIDEO_PIX_FMT_YUYV),
{0},
};
/* Emulated I2C register interface, to replace with actual I2C calls for real hardware */
static int emul_imager_read_reg(const struct device *const dev, uint8_t reg_addr, uint8_t *value)
{
LOG_DBG("%s placeholder for I2C read from 0x%02x", dev->name, reg_addr);
switch (reg_addr) {
case EMUL_IMAGER_REG_SENSOR_ID:
*value = EMUL_IMAGER_SENSOR_ID;
break;
default:
*value = emul_imager_fake_regs[reg_addr];
}
return 0;
}
/* Helper to read a full integer directly from a register */
static int emul_imager_read_int(const struct device *const dev, uint8_t reg_addr, int *value)
{
uint8_t val8;
int ret;
ret = emul_imager_read_reg(dev, reg_addr, &val8);
*value = val8;
return ret;
}
/* Some sensors will need reg8 or reg16 variants. */
static int emul_imager_write_reg(const struct device *const dev, uint8_t reg_addr, uint8_t value)
{
LOG_DBG("%s placeholder for I2C write 0x%08x to 0x%02x", dev->name, value, reg_addr);
emul_imager_fake_regs[reg_addr] = value;
return 0;
}
static int emul_imager_write_multi(const struct device *const dev,
const struct emul_imager_reg *regs)
{
int ret;
for (int i = 0; regs[i].addr != 0; i++) {
ret = emul_imager_write_reg(dev, regs[i].addr, regs[i].value);
if (ret < 0) {
return ret;
}
}
return 0;
}
static int emul_imager_set_ctrl(const struct device *dev, unsigned int cid, void *value)
{
switch (cid) {
case VIDEO_CID_EXPOSURE:
return emul_imager_write_reg(dev, EMUL_IMAGER_REG_EXPOSURE, (int)value);
case VIDEO_CID_GAIN:
return emul_imager_write_reg(dev, EMUL_IMAGER_REG_GAIN, (int)value);
case VIDEO_CID_TEST_PATTERN:
return emul_imager_write_reg(dev, EMUL_IMAGER_REG_PATTERN, (int)value);
default:
return -ENOTSUP;
}
}
static int emul_imager_get_ctrl(const struct device *dev, unsigned int cid, void *value)
{
struct emul_imager_data *data = dev->data;
switch (cid) {
case VIDEO_CID_EXPOSURE:
return emul_imager_read_int(dev, EMUL_IMAGER_REG_EXPOSURE, value);
case VIDEO_CID_GAIN:
return emul_imager_read_int(dev, EMUL_IMAGER_REG_GAIN, value);
case VIDEO_CID_TEST_PATTERN:
return emul_imager_read_int(dev, EMUL_IMAGER_REG_PATTERN, value);
case VIDEO_CID_PIXEL_RATE:
*(int64_t *)value = (int64_t)data->fmt.width * data->fmt.pitch * data->mode->fps;
return 0;
default:
return -ENOTSUP;
}
}
/* Customize this function according to your "struct emul_imager_mode". */
static int emul_imager_set_mode(const struct device *dev, const struct emul_imager_mode *mode)
{
struct emul_imager_data *data = dev->data;
int ret;
if (data->mode == mode) {
return 0;
}
LOG_DBG("Applying mode %p at %d FPS", mode, mode->fps);
/* Apply all the configuration registers for that mode */
for (int i = 0; i < 2; i++) {
ret = emul_imager_write_multi(dev, mode->regs[i]);
if (ret < 0) {
goto err;
}
}
data->mode = mode;
return 0;
err:
LOG_ERR("Could not apply %s mode %p (%u FPS)", dev->name, mode, mode->fps);
return ret;
}
static int emul_imager_set_frmival(const struct device *dev, enum video_endpoint_id ep,
struct video_frmival *frmival)
{
struct emul_imager_data *data = dev->data;
struct video_frmival_enum fie = {.format = &data->fmt, .discrete = *frmival};
if (ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
return -EINVAL;
}
video_closest_frmival(dev, ep, &fie);
LOG_DBG("Applying frame interval number %u", fie.index);
return emul_imager_set_mode(dev, &emul_imager_modes[data->fmt_id][fie.index]);
}
static int emul_imager_get_frmival(const struct device *dev, enum video_endpoint_id ep,
struct video_frmival *frmival)
{
struct emul_imager_data *data = dev->data;
if (ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
return -EINVAL;
}
frmival->numerator = 1;
frmival->denominator = data->mode->fps;
return 0;
}
static int emul_imager_enum_frmival(const struct device *dev, enum video_endpoint_id ep,
struct video_frmival_enum *fie)
{
const struct emul_imager_mode *mode;
size_t fmt_id;
int ret;
if (ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
return -EINVAL;
}
ret = video_format_caps_index(fmts, fie->format, &fmt_id);
if (ret < 0) {
return ret;
}
mode = &emul_imager_modes[fmt_id][fie->index];
fie->type = VIDEO_FRMIVAL_TYPE_DISCRETE;
fie->discrete.numerator = 1;
fie->discrete.denominator = mode->fps;
fie->index++;
return mode->fps == 0;
}
/* White, Yellow, Cyan, Green, Magenta, Red, Blue, Black */
static const uint16_t pattern_8bars_yuv[8][3] = {
{0xFF, 0x7F, 0x7F}, {0xFF, 0x00, 0xFF}, {0xFF, 0xFF, 0x00}, {0x7F, 0x00, 0x00},
{0x00, 0xFF, 0xFF}, {0x00, 0x00, 0xFF}, {0x00, 0xFF, 0x00}, {0x00, 0x7F, 0x7F}};
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 void emul_imager_fill_framebuffer(const struct device *const dev, struct video_format *fmt)
{
struct emul_imager_data *data = dev->data;
uint16_t *fb16 = (uint16_t *)data->framebuffer;
uint16_t r, g, b, y, uv;
/* Fill the first row of the emulated framebuffer */
switch (fmt->pixelformat) {
case VIDEO_PIX_FMT_YUYV:
for (size_t i = 0; i < fmt->width; i++) {
y = pattern_8bars_yuv[i * 8 / fmt->width][0];
uv = pattern_8bars_yuv[i * 8 / fmt->width][1 + i % 2];
fb16[i] = sys_cpu_to_be16(y << 8 | uv << 0);
}
break;
case VIDEO_PIX_FMT_RGB565:
for (size_t i = 0; i < fmt->width; i++) {
r = pattern_8bars_rgb[i * 8 / fmt->width][0] >> (8 - 5);
g = pattern_8bars_rgb[i * 8 / fmt->width][1] >> (8 - 6);
b = pattern_8bars_rgb[i * 8 / fmt->width][2] >> (8 - 5);
fb16[i] = sys_cpu_to_le16((r << 11) | (g << 6) | (b << 0));
}
break;
default:
LOG_WRN("Unsupported pixel format %x, supported: %x, %x", fmt->pixelformat,
VIDEO_PIX_FMT_YUYV, VIDEO_PIX_FMT_RGB565);
memset(fb16, 0, fmt->pitch);
}
/* Duplicate the first row over the whole frame */
for (size_t i = 1; i < fmt->height; i++) {
memcpy(data->framebuffer + fmt->pitch * i, data->framebuffer, fmt->pitch);
}
}
static int emul_imager_set_fmt(const struct device *const dev, enum video_endpoint_id ep,
struct video_format *fmt)
{
struct emul_imager_data *data = dev->data;
size_t fmt_id;
int ret;
if (ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
return -EINVAL;
}
if (fmt->pitch * fmt->height > CONFIG_VIDEO_EMUL_IMAGER_FRAMEBUFFER_SIZE) {
LOG_ERR("%s has %u bytes of memory, unable to support %x %ux%u (%u bytes)",
dev->name, CONFIG_VIDEO_EMUL_IMAGER_FRAMEBUFFER_SIZE, fmt->pixelformat,
fmt->width, fmt->height, fmt->pitch * fmt->height);
return -ENOMEM;
}
if (memcmp(&data->fmt, fmt, sizeof(data->fmt)) == 0) {
return 0;
}
ret = video_format_caps_index(fmts, fmt, &fmt_id);
if (ret < 0) {
LOG_ERR("Format %x %ux%u not found for %s", fmt->pixelformat, fmt->width,
fmt->height, dev->name);
return ret;
}
ret = emul_imager_set_mode(dev, &emul_imager_modes[fmt_id][0]);
if (ret < 0) {
return ret;
}
/* Change the image pattern on the framebuffer */
emul_imager_fill_framebuffer(dev, fmt);
data->fmt_id = fmt_id;
data->fmt = *fmt;
return 0;
}
static int emul_imager_get_fmt(const struct device *dev, enum video_endpoint_id ep,
struct video_format *fmt)
{
struct emul_imager_data *data = dev->data;
if (ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
return -EINVAL;
}
*fmt = data->fmt;
return 0;
}
static int emul_imager_get_caps(const struct device *dev, enum video_endpoint_id ep,
struct video_caps *caps)
{
if (ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
return -EINVAL;
}
caps->format_caps = fmts;
return 0;
}
static int emul_imager_set_stream(const struct device *dev, bool enable)
{
return emul_imager_write_reg(dev, EMUL_IMAGER_REG_CTRL, enable ? 1 : 0);
}
static DEVICE_API(video, emul_imager_driver_api) = {
.set_ctrl = emul_imager_set_ctrl,
.get_ctrl = emul_imager_get_ctrl,
.set_frmival = emul_imager_set_frmival,
.get_frmival = emul_imager_get_frmival,
.enum_frmival = emul_imager_enum_frmival,
.set_format = emul_imager_set_fmt,
.get_format = emul_imager_get_fmt,
.get_caps = emul_imager_get_caps,
.set_stream = emul_imager_set_stream,
};
int emul_imager_init(const struct device *dev)
{
struct video_format fmt;
uint8_t sensor_id;
int ret;
if (/* !i2c_is_ready_dt(&cfg->i2c) */ false) {
/* LOG_ERR("Bus %s is not ready", cfg->i2c.bus->name); */
return -ENODEV;
}
ret = emul_imager_read_reg(dev, EMUL_IMAGER_REG_SENSOR_ID, &sensor_id);
if (ret < 0 || sensor_id != EMUL_IMAGER_SENSOR_ID) {
LOG_ERR("Failed to get %s correct sensor ID (0x%x", dev->name, sensor_id);
return ret;
}
ret = emul_imager_write_multi(dev, emul_imager_init_regs);
if (ret < 0) {
LOG_ERR("Could not set %s initial registers", dev->name);
return ret;
}
fmt.pixelformat = fmts[0].pixelformat;
fmt.width = fmts[0].width_min;
fmt.height = fmts[0].height_min;
fmt.pitch = fmt.width * 2;
ret = emul_imager_set_fmt(dev, VIDEO_EP_OUT, &fmt);
if (ret < 0) {
LOG_ERR("Failed to set %s to default format %x %ux%u", dev->name, fmt.pixelformat,
fmt.width, fmt.height);
}
return 0;
}
#define EMUL_IMAGER_DEFINE(inst) \
static struct emul_imager_data emul_imager_data_##inst; \
\
static const struct emul_imager_config emul_imager_cfg_##inst = { \
.i2c = /* I2C_DT_SPEC_INST_GET(inst) */ {0}, \
}; \
\
DEVICE_DT_INST_DEFINE(inst, &emul_imager_init, NULL, &emul_imager_data_##inst, \
&emul_imager_cfg_##inst, POST_KERNEL, CONFIG_VIDEO_INIT_PRIORITY, \
&emul_imager_driver_api);
DT_INST_FOREACH_STATUS_OKAY(EMUL_IMAGER_DEFINE)
Reference in New Issue View Git Blame Copy Permalink