diff --git a/drivers/video/Kconfig.sw_generator b/drivers/video/Kconfig.sw_generator index f258a00e1a2f..5ad686a37325 100644 --- a/drivers/video/Kconfig.sw_generator +++ b/drivers/video/Kconfig.sw_generator @@ -1,4 +1,4 @@ -# MT9m114 +# Software-based pattern generator # Copyright (c) 2016 Linaro Limited # SPDX-License-Identifier: Apache-2.0 diff --git a/drivers/video/video_sw_generator.c b/drivers/video/video_sw_generator.c index 988811db1c32..7600a8c72bfd 100644 --- a/drivers/video/video_sw_generator.c +++ b/drivers/video/video_sw_generator.c @@ -1,5 +1,6 @@ /* * Copyright (c) 2019, Linaro Limited + * Copyright (c) 2024, tinyVision.ai Inc. * * SPDX-License-Identifier: Apache-2.0 */ @@ -9,6 +10,11 @@ #include #include #include +#include +#include +#include + +#include LOG_MODULE_REGISTER(video_sw_generator, CONFIG_VIDEO_LOG_LEVEL); @@ -22,6 +28,15 @@ LOG_MODULE_REGISTER(video_sw_generator, CONFIG_VIDEO_LOG_LEVEL); * format. 60 fps is therefore chosen as a common value in practice. */ #define MAX_FRAME_RATE 60 +#define HUE(a) Q15f((double)(a) / 360.) +#define SMPTE_NUM 7 + +static const q15_t smpte_colorbar_hsv[3][SMPTE_NUM] = { + /* white, yellow, cyan, green, magenta, red, blue */ + {HUE(0), HUE(60), HUE(180), HUE(120), HUE(300), HUE(0), HUE(240)}, + {Q15f(0.), Q15f(1.), Q15f(1.), Q15f(1.), Q15f(1.), Q15f(1.), Q15f(1.)}, + {Q15f(1.), Q15f(1.), Q15f(1.), Q15f(1.), Q15f(1.), Q15f(1.), Q15f(1.)}, +}; struct video_sw_generator_data { const struct device *dev; @@ -31,30 +46,27 @@ struct video_sw_generator_data { struct k_work_delayable buf_work; struct k_work_sync work_sync; int pattern; + uint8_t colorbar_rgb565[SMPTE_NUM][2]; + uint8_t colorbar_xrgb32[SMPTE_NUM][4]; + uint8_t colorbar_yuyv[SMPTE_NUM][4]; + q7_t ctrl_hsv_q7[3]; bool ctrl_hflip; - bool ctrl_vflip; struct k_poll_signal *signal; uint32_t frame_rate; }; -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}}; +#define VIDEO_SW_GENERATOR_FORMAT_CAP(fourcc) \ + { \ + .pixelformat = (fourcc), .width_min = 64, .width_max = 1920, .height_min = 1, \ + .height_max = 1080, .width_step = 1, .height_step = 1, \ + } + +static const struct video_format_cap fmts[] = { + 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_YUYV), + {0}, +}; static int video_sw_generator_set_fmt(const struct device *dev, enum video_endpoint_id ep, struct video_format *fmt) @@ -116,28 +128,145 @@ static int video_sw_generator_stream_stop(const struct device *dev) return 0; } -/* Black, Blue, Red, Purple, Green, Aqua, Yellow, White */ -uint16_t rgb565_colorbar_value[] = {0x0000, 0x001F, 0xF800, 0xF81F, 0x07E0, 0x07FF, 0xFFE0, 0xFFFF}; +static void hsv_to_rgb(q15_t h, q15_t s, q15_t v, q15_t *r, q15_t *g, q15_t *b) +{ + q15_t chroma = MULq15(s, v); + q15_t x; + + if (h < Q15f(1. / 6.)) { + x = SUBq15(h, Q15f(0. / 6.)) * 6; + *r = chroma; + *g = MULq15(chroma, x); + *b = Q15f(0.); + } else if (h < Q15f(2. / 6.)) { + x = SUBq15(h, Q15f(1. / 6.)) * 6; + *r = MULq15(chroma, SUBq15(Q15f(1.), x)); + *g = chroma; + *b = Q15f(0.); + } else if (h < Q15f(3. / 6.)) { + x = SUBq15(h, Q15f(2. / 6.)) * 6; + *r = Q15f(0.); + *g = chroma; + *b = MULq15(chroma, x); + } else if (h < Q15f(4. / 6.)) { + x = SUBq15(h, Q15f(3. / 6.)) * 6; + *r = Q15f(0.); + *g = MULq15(chroma, SUBq15(Q15f(1), x)); + *b = chroma; + } else if (h < Q15f(5. / 6.)) { + x = SUBq15(h, Q15f(4. / 6.)) * 6; + *r = MULq15(chroma, x); + *g = Q15f(0.); + *b = chroma; + } else { + x = SUBq15(h, Q15f(5. / 6.)) * 6; + *r = chroma; + *g = Q15f(0.); + *b = MULq15(chroma, SUBq15(Q15f(1), x)); + } + + *r = ADDq15(SUBq15(v, chroma), *r); + *g = ADDq15(SUBq15(v, chroma), *g); + *b = ADDq15(SUBq15(v, chroma), *b); +} + +#define BT709_WR 0.2126 +#define BT709_WG 0.7152 +#define BT709_WB 0.0722 +#define BT709_UMAX 0.436 +#define BT709_VMAX 0.615 + +static void rgb_to_yuv(q15_t r, q15_t g, q15_t b, q15_t *y, q15_t *u, q15_t *v) +{ + q15_t ux = Q15f(BT709_UMAX / (1. - BT709_WB)); + q15_t vx = Q15f(BT709_VMAX / (1. - BT709_WR)); + + /* Using BT.709 coefficients */ + *y = 0; + *y = ADDq15(*y, MULq15(Q15f(BT709_WR), r)); + *y = ADDq15(*y, MULq15(Q15f(BT709_WG), g)); + *y = ADDq15(*y, MULq15(Q15f(BT709_WB), b)); + *u = MULq15(ux, SUBq15(b, *y)); + *v = MULq15(vx, SUBq15(r, *y)); +} + +static void hsv_adjust(q15_t *hue, q15_t *sat, q15_t *val, q15_t h, q15_t s, q15_t v) +{ + *hue = MODq15((q31_t)MAXq15 + (q31_t)*hue + (q31_t)h, MAXq15); + *sat = CLAMP((q31_t)*sat + (q31_t)s, 0, MAXq15); + *val = CLAMP((q31_t)*val + (q31_t)v, 0, MAXq15); +} + +static void init_colors(const struct device *dev) +{ + struct video_sw_generator_data *data = dev->data; -uint32_t xrgb32_colorbar_value[] = {0xFF000000, 0xFF0000FF, 0xFFFF0000, 0xFFFF00FF, - 0xFF00FF00, 0xFF00FFFF, 0xFFFFFF00, 0xFFFFFFFF}; + for (int i = 0; i < SMPTE_NUM; i++) { + q15_t r, g, b; + q15_t y, u, v; + q15_t hue = smpte_colorbar_hsv[0][i]; + q15_t sat = smpte_colorbar_hsv[1][i]; + q15_t val = smpte_colorbar_hsv[2][i]; + uint16_t u16; + + hsv_adjust(&hue, &sat, &val, Q15q7(data->ctrl_hsv_q7[0]), + Q15q7(data->ctrl_hsv_q7[1]), Q15q7(data->ctrl_hsv_q7[2])); + hsv_to_rgb(hue, sat, val, &r, &g, &b); + rgb_to_yuv(r, g, b, &y, &u, &v); + + LOG_DBG("H%1"PRIq(3)" S%1"PRIq(3)" V%1"PRIq(3)", " + "R%1"PRIq(3)" G%1"PRIq(3)" B%1"PRIq(3)", " + "Y%1"PRIq(3)" U%1"PRIq(3)" V%1"PRIq(3), + PRIq_arg(hue, 3, 0), PRIq_arg(sat, 3, 0), PRIq_arg(val, 3, 0), + PRIq_arg(r, 3, 0), PRIq_arg(g, 3, 0), PRIq_arg(b, 3, 0), + PRIq_arg(y, 3, 0), PRIq_arg(u, 3, 0), PRIq_arg(v, 3, 0)); + + u16 = BITSq15(r, 5) | (BITSq15(g, 6) << 5) | (BITSq15(b, 5) << 11); + data->colorbar_rgb565[i][0] = u16 >> 0; + data->colorbar_rgb565[i][1] = u16 >> 8; + + data->colorbar_xrgb32[i][0] = 0x00; + data->colorbar_xrgb32[i][1] = BITSq15(r, 8); + data->colorbar_xrgb32[i][2] = BITSq15(g, 8); + data->colorbar_xrgb32[i][3] = BITSq15(g, 8); + + data->colorbar_yuyv[i][0] = BITSq15(y, 8); + data->colorbar_yuyv[i][1] = BITSq15(ADDq15(Q15f(0.5), u), 8); + data->colorbar_yuyv[i][2] = BITSq15(y, 8); + data->colorbar_yuyv[i][3] = BITSq15(ADDq15(Q15f(0.5), v), 8); + } +} 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]; + for (w = 0; w < data->fmt.width;) { + int color_idx = w * SMPTE_NUM / data->fmt.width; + + if (data->ctrl_hflip) { + color_idx = SMPTE_NUM - 1 - color_idx; + } + + switch (data->fmt.pixelformat) { + case VIDEO_PIX_FMT_RGB565: + memcpy(&vbuf->buffer[i], data->colorbar_rgb565[color_idx], 2); 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]; + w += 1; + break; + case VIDEO_PIX_FMT_XRGB32: + memcpy(&vbuf->buffer[i], data->colorbar_xrgb32[color_idx], 4); + i += 4; + w += 1; + break; + case VIDEO_PIX_FMT_YUYV: + memcpy(&vbuf->buffer[i], data->colorbar_yuyv[color_idx], 4); i += 4; + w += 2; + break; + default: + __ASSERT_NO_MSG(false); } } } @@ -259,10 +388,30 @@ static inline int video_sw_generator_set_ctrl(const struct device *dev, unsigned void *value) { struct video_sw_generator_data *data = dev->data; + uint32_t u32 = (uint32_t)value; + int ret; + + ret = video_check_range_u32(dev, cid, u32); + if (ret < 0) { + LOG_ERR("value %u not in range", u32); + return ret; + } switch (cid) { - case VIDEO_CID_VFLIP: - data->ctrl_vflip = (bool)value; + case VIDEO_CID_HFLIP: + data->ctrl_hflip = (bool)value; + break; + case VIDEO_CID_HUE: + data->ctrl_hsv_q7[0] = u32 - MINq7; + init_colors(dev); + break; + case VIDEO_CID_SATURATION: + data->ctrl_hsv_q7[1] = u32 - MINq7; + init_colors(dev); + break; + case VIDEO_CID_BRIGHTNESS: + data->ctrl_hsv_q7[2] = u32 - MINq7; + init_colors(dev); break; default: return -ENOTSUP; @@ -350,18 +499,12 @@ static const struct video_driver_api video_sw_generator_driver_api = { #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, - .frame_rate = DEFAULT_FRAME_RATE, -}; - static int video_sw_generator_init(const struct device *dev) { struct video_sw_generator_data *data = dev->data; + init_colors(dev); + data->dev = dev; k_fifo_init(&data->fifo_in); k_fifo_init(&data->fifo_out); @@ -370,6 +513,16 @@ static int video_sw_generator_init(const struct device *dev) 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); +#define VIDEO_SW_GENERATOR_DEFINE(inst) \ + static struct video_sw_generator_data video_sw_generator_data_##inst = { \ + .fmt.width = 320, \ + .fmt.height = 160, \ + .fmt.pitch = 320 * 2, \ + .fmt.pixelformat = VIDEO_PIX_FMT_RGB565, \ + }; \ + \ + DEVICE_DT_INST_DEFINE(inst, &video_sw_generator_init, NULL, \ + &video_sw_generator_data_##inst, NULL, POST_KERNEL, \ + CONFIG_VIDEO_INIT_PRIORITY, &video_sw_generator_driver_api); + +DT_INST_FOREACH_STATUS_OKAY(VIDEO_SW_GENERATOR_DEFINE) diff --git a/dts/bindings/video/zephyr,sw-generator.yaml b/dts/bindings/video/zephyr,sw-generator.yaml new file mode 100644 index 000000000000..9a3a2eeac86c --- /dev/null +++ b/dts/bindings/video/zephyr,sw-generator.yaml @@ -0,0 +1,11 @@ +# Copyright 2024 tinyVision.ai Inc. +# SPDX-License-Identifier: Apache-2.0 + +compatible: "zephyr,sw-generator" + +description: | + Software-based video pattern generator for testing purpose. + This generates an image pattern in the RAM buffer passed to it, + at the specified dimension. + +include: base.yaml diff --git a/include/zephyr/dsp/macros.h b/include/zephyr/dsp/macros.h new file mode 100644 index 000000000000..34f860ddb721 --- /dev/null +++ b/include/zephyr/dsp/macros.h @@ -0,0 +1,337 @@ +/* Copyright (c) 2024 tinyVision.ai Inc. + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef INCLUDE_ZEPHYR_DSP_MACROS_H_ +#define INCLUDE_ZEPHYR_DSP_MACROS_H_ + +#include + +#include +#include + +/** + * @brief Definition of the minimum and maximum values of the internal representation + * + * @note The value represented is always between -1.0 and 1.0 for any storage size. + * Scaling variables imported before using them in fixed-points is required. + * + * @{ + */ + +/** Minimum internal value for Q.7. */ +#define MINq7 INT8_MIN + +/** Minimum internal value for Q.15. */ +#define MINq15 INT16_MIN + +/** Minimum internal value for Q.31. */ +#define MINq31 INT32_MIN + +/** Maximum internal value for Q.7. */ +#define MAXq7 INT8_MAX + +/** Maximum internal value for Q.15. */ +#define MAXq15 INT16_MAX + +/** Maximum internal value for Q.31. */ +#define MAXq31 INT32_MAX + +/* @} + */ + +/** + * @brief Clamp a fixed-point value to -1.0 to 1.0. + * + * @note Useful for internal use, or when breaking the abstraction barrier. + * + * @{ + */ + +/** Enforce the a Q.7 fixed-point to be between -1.0 and 1.0 */ +#define CLAMPq7(q7) ((q7_t)CLAMP((q7), MINq7, MAXq7)) + +/** Enforce the a Q.15 fixed-point to be between -1.0 and 1.0 */ +#define CLAMPq15(q15) ((q15_t)CLAMP((q15), MINq15, MAXq15)) + +/** Enforce the a Q.31 fixed-point to be between -1.0 and 1.0 */ +#define CLAMPq31(q31) ((q31_t)CLAMP((q31), MINq31, MAXq31)) + +/* @} + */ + +/** + * @brief Construct a fixed-point number out of a floating point number. + * + * The input float can be an expression or a single number. + * If it is below -1.0 or above 1.0, it is adjusted to fit the -1.0 to 1.0 range. + * + * @example "Q15(-1.0) // Minimum value" + * @example "Q31(1.0) // Maximum value" + * @example "Q15(1.3) // Will become 1.0" + * @example "Q7(f - 1.0)" + * @example "Q15(2.0 / 3.1415)" + * + * @param f Floating-point number to convert into a fixed-point number. + * + * @{ + */ + +/** Construct a Q.7 fixed-point: 1 bit for the sign, 7 bits of fractional part */ +#define Q7f(f) CLAMPq7((f) * (1ll << 7)) + +/** Construct a Q.15 fixed-point: 1 bit for the sign, 15 bits of fractional part */ +#define Q15f(f) CLAMPq15((f) * (1ll << 15)) + +/** Construct a Q.31 fixed-point: 1 bit for the sign, 31 bits of fractional part */ +#define Q31f(f) CLAMPq31((f) * (1ll << 31)) + +/* @} + */ + +/** + * @brief Construct a fixed-point out of an integer and a scale. + * + * This permits to work with numbers above the -1.0 to 1.0 range: + * The minimum and maximum possible value of the input number are specified. + * The input number is then scaled so that the minimum value becomes -1.0 and + * maximum value becomes 1.0. + * Once the computation is done in fixed piotn, it is popssible to get an + * unscaled value with @ref INTq7, @ref INtq15 and @rev INTq31. + * + * @example "Q15i(temperature, -20, 200)" + * @example "Q15i(angle, 0, 360)" + * @example "Q15i(voltage, V_MIN, V_MAX)" + * + * @param i Input number to convert to a fixed-point representation + * @param min Minimum value that @p i can take. + * The same minimum value is used to convert the numbers back. + * @param max Maximum value that @p i can take. + * The same maximum value is used to convert the numbers back. + * + * @{ + */ + +/** Build a Q.7 number by scaling @p i range from [min, max] (at most [ , ]) to [-1.0, 1.0] */ +#define Q7i(i, min, max) CLAMPq7(SCALE((i), (min), (max), MINq7, MAXq7)) + +/** Build a Q.15 number by scaling @p i range from [min, max] to [-1.0, 1.0] */ +#define Q15i(i, min, max) CLAMPq15(SCALE((i), (min), (max), MINq15, MAXq15)) + +/** Build a Q.31 number by scaling @p i range from [min, max] to [-1.0, 1.0] */ +#define Q31i(i, min, max) CLAMPq31(SCALE((i), (min), (max), MINq31, MAXq31)) + +/* @} + */ + +/** + * @brief Convert fixed-points from one size to another + * + * This permits to increase or decrease the precision by switching between + * smaller and larger representation. + * The values represented does not change except due to loss of precision. + * The minimum value remains -1.0 and maximum value remains 1.0. + * Only the internal representation is scaled. + * + * @{ + */ + +/** Build a Q.7 number out of a Q.15 number, losing 8 bits of precision */ +#define Q7q15(q15) (q7_t)((q15) / (1 << 8)) + +/** Build a Q.7 number out of a Q.31 number, losing 24 bits of precision */ +#define Q7q31(q31) (q7_t)((q31) / (1 << 24)) + +/** Build a Q.15 number out of a Q.7 number, gaining 8 bits of precision */ +#define Q15q7(q7) CLAMPq15((q15_t)(q7) * (1 << 8)) + +/** Build a Q.15 number out of a Q.31 number, losing 16 bits of precision */ +#define Q15q31(q31) (q15_t)((q31) / (1 << 16)) + +/** Build a Q.31 number out of a Q.7 number, gaining 24 bits of precision */ +#define Q31q7(q7) CLAMPq31((q31_t)(q7) * (1 << 24)) + +/** Build a Q.31 number out of a Q.15 number, gaining 16 bits of precision */ +#define Q31q15(q15) CLAMPq31((q31_t)(q15) * (1 << 16)) + +/* @} + */ + +/** + * @brief Convert a fixed-point number back to an natural representation. + * + * This permits to extract a result value out of a fixed-point number, + * to reverse the effect of @ref Q7i, @ref Q15i and @ref Q31i. + * + * @param i The fixed-point number to convert to a natural number. + * @param min The minimum value specified to create the fixed-point. + * @param max The maximum value specified to create the fixed-point. + * + * @{ + */ + +/** Convert a Q.7 fixed-point number to a natural integer */ +#define INTq7(i, min, max) SCALE((i), MINq7, MAXq7, (min), (max)) + +/** Convert a Q.15 fixed-point number to a natural integer */ +#define INTq15(i, min, max) SCALE((i), MINq15, MAXq15, (min), (max)) + +/** Convert a Q.31 fixed-point number to a natural integer */ +#define INTq31(i, min, max) SCALE((i), MINq31, MAXq31, (min), (max)) + +/* @} + */ + +/** + * @brief Add two fixed-point numbers together. + * + * Saturation logic is applied, so number out of ranges will be converted + * to the minimum -1.0 or maximum 1.0 value instead of overflowing. + * + * @note If two fixed-point numbers are to be subtracted into a larger type + * such as Q.7 + Q.7 = Q.15, the C operator @c + can be used instead. + * + * @param a First number to add. + * @param b Second number to add. + * + * @{ + */ + +/** Sum two Q.7 numbers and produce a Q.7 result */ +#define ADDq7(a, b) CLAMPq7((int16_t)(a) + (int16_t)(b)) + +/** Sum two Q.15 numbers and produce a Q.15 result */ +#define ADDq15(a, b) CLAMPq15((int32_t)(a) + (int32_t)(b)) + +/** Sum two Q.31 numbers and produce a Q.31 result */ +#define ADDq31(a, b) CLAMPq31((int64_t)(a) + (int64_t)(b)) + +/* @} + */ + +/** + * @brief Subtract a fixed-point number to another. + * + * Saturation logic is applied, so number out of ranges will be converted + * to the minimum 1.0 or maximum -1.0 value instead of overflowing. + * + * @note If two fixed-point numbers are to be subtracted into a larger type + * such as Q.7 - Q.7 = Q.15, the C operator @c - can be used instead. + * + * @param a First number to add. + * @param a Second number to add. + * + * @{ + */ + +/** Subtract two Q.7 numbers and produce a Q.7 result */ +#define SUBq7(a, b) CLAMPq7((int16_t)(a) - (int16_t)(b)) + +/** Subtract two Q.15 numbers and produce a Q.15 result */ +#define SUBq15(a, b) CLAMPq15((int32_t)(a) - (int32_t)(b)) + +/** Subtract two Q.31 numbers and produce a Q.31 result */ +#define SUBq31(a, b) CLAMPq31((int64_t)(a) - (int64_t)(b)) + +/* @} + */ + +/** + * @brief Multiply two fixed-point numbers together. + * + * Saturation logic is applied, so number out of range will be converted + * to handle the edge case Q#f(-1.0) * Q#f(-1.0) = Q#f(1.0) + * + * @note This implementation does not perform rounding. + * + * @param a First number to add. + * @param b Second number to add. + * + * @{ + */ + +/** Multiply two Q.7 numbers and produce a Q.7 result */ +#define MULq7(a, b) CLAMPq7(((int16_t)(a) * (int16_t)(b)) / (1 << 7)) + +/** Multiply two Q.15 numbers and produce a Q.15 result */ +#define MULq15(a, b) CLAMPq15(((int32_t)(a) * (int32_t)(b)) / (1 << 15)) + +/** Multiply two Q.31 numbers and produce a Q.31 result */ +#define MULq31(a, b) CLAMPq31(((int64_t)(a) * (int64_t)(b)) / (1 << 31)) + +/* @} + */ + +/** + * @brief Divide two fixed-point numbers together. + * + * Saturation logic is applied, so number out of ranges will be converted + * to the minimum -1.0 or maximum 1.0 value instead of overflowing. + * + * @note This implementation does not perform rounding. + * + * @param a Numerator of the division. + * @param b Denominator of the division. + * + * @{ + */ + +/** Divide a Q.7 number and produce a Q.7 result */ +#define DIVq7(a, b) CLAMPq7((a) * (1 << 7) / (b)) + +/** Divide a Q.15 number and produce a Q.15 result */ +#define DIVq15(a, b) CLAMPq15((a) * (1 << 15) / (b)) + +/** Divide a Q.31 number and produce a Q.31 result */ +#define DIVq31(a, b) CLAMPq31((a) * (1 << 31) / (b)) + +/* @} + */ + +/** + * @brief Apply the opposite value of the fixed-point number. + * + * Saturation logic is applied, as the most negative number could + * overflow internally if converted to positive. + * + * @param a Number to get the opposite value for + * + * @{ + */ + +/** Get the negation of a Q.7 number and produce a Q.7 result */ +#define NEGq7(a) (q7_t)((a) == MINq7 ? MAXq7 : -(a)) + +/** Get the negation of a Q.15 number and produce a Q.15 result */ +#define NEGq15(a) (q15_t)((a) == MINq15 ? MAXq15 : -(a)) + +/** Get the negation of a Q.15 number and produce a Q.15 result */ +#define NEGq31(a) (q31_t)((a) == MINq31 ? MAXq31 : -(a)) + +/* @} + */ + +/** + * @brief Get the absolute value of a fixed-point number. + * + * Saturation logic is applied, as the most negative number overflows if + * converted to positive. + * + * @param a Number to get the absolute value for. + * + * @{ + */ + +/** Get the absolute value of a Q.7 number and produce a Q.7 result */ +#define ABSq7(a) (q7_t)((a) < 0 ? NEGq7(a) : (a)) + +/** Get the absolute value of a Q.15 number and produce a Q.15 result */ +#define ABSq15(a) (q15_t)((a) < 0 ? NEGq15(a) : (a)) + +/** Get the absolute value of a Q.31 number and produce a Q.31 result */ +#define ABSq31(a) (q31_t)((a) < 0 ? NEGq31(a) : (a)) + +/* @} + */ + +#endif /* INCLUDE_ZEPHYR_DSP_MACROS_H_ */ diff --git a/include/zephyr/sys/util.h b/include/zephyr/sys/util.h index 6fba21dc16ac..f35d06eff04a 100644 --- a/include/zephyr/sys/util.h +++ b/include/zephyr/sys/util.h @@ -407,6 +407,30 @@ extern "C" { #define CLAMP(val, low, high) (((val) <= (low)) ? (low) : MIN(val, high)) #endif +/** + * @brief Linearly maps a value from one scale to another. + * + * A value is considered to fit on an input range, and is scaled to + * the same proportion of the output range. + * The ranges are described by their minimum and maximum values. + * + * @note To avoid an overflow, the number of bits of the input and output range + * must be equal or lower to 61. For instance, an input range with min and max + * values between INT8_MIN and INT8_MAX, then the output range must have + * values within INT53_MIN and INT53_MAX. + * + * @note The size of the output range can be [0, 0]. + * The size of the input range must be greater than 0 + * + * @param i The value to convert from the input range to the output range. + * @param imin The minimum value of the input range. + * @param imax The maximum value of the input range. + * @param omin The minimum value of the output range. + * @param omax The maximum value of the output range. + */ +#define SCALE(i, imin, imax, omin, omax) \ + (((int64_t)(i) - (imin)) * ((int64_t)(omax) - (omin)) / ((int64_t)(imax) - (imin)) + (omin)) + /** * @brief Checks if a value is within range. * @@ -421,6 +445,21 @@ extern "C" { */ #define IN_RANGE(val, min, max) ((val) >= (min) && (val) <= (max)) +/** + * @brief Map a value from one scale to another. + * + * A value of an input range is mappedto a value of an output range. + * The ranges are described by their min and max values. + * + * @param i The value to convert from the input range to the output range. + * @param imin The minimum value of the input range. + * @param imax The maximum value of the input range. + * @param omin The minimum value of the output range. + * @param omax The maximum value of the output range. + */ +#define MAP_VALUE(i, imin, imax, omin, omax) \ + (((i) - (imin)) * (omin - omax) / ((imax) - (imin)) + (omin)) + /** * @brief Is @p x a power of two? * @param x value to check diff --git a/tests/lib/sys_util/src/main.c b/tests/lib/sys_util/src/main.c index 02e6465e4478..4dcdeb8947ac 100644 --- a/tests/lib/sys_util/src/main.c +++ b/tests/lib/sys_util/src/main.c @@ -13,6 +13,73 @@ * @{ */ +ZTEST(sys_util, test_SCALE) +{ + /* Test for a few arbitrary values */ + zassert_equal(SCALE(3, 0, 10, 0, 100), 30); + zassert_equal(SCALE(-3, -10, 0, -100, 0), -30); + zassert_equal(SCALE(10, -100, 100, -10, 10), 1); + zassert_equal(SCALE(0, -10, 40, -50, 0), -40); + zassert_equal(SCALE(0, -128, 127, 0, 2), 1); + zassert_equal(SCALE(5, -50, 5000, -1000, 10), -989); + + /* Test for i = 1..60 and o = 60..1 */ + for (int i = 1; i < 61; i++) { + int o = 61 - i; + int64_t imin = -(1ll << i); + int64_t imax = (1ll << i); + int64_t omin = -(1ll << o); + int64_t omax = (1ll << o); + + /* Special case: the output range can be [0, 0] */ + + zassert_equal(SCALE(imin, imin, imax, 0, 0), 0); + zassert_equal(SCALE(0, imin, imax, 0, 0), 0); + zassert_equal(SCALE(imax, imin, imax, 0, 0), 0); + + zassert_equal(SCALE(0, 0, imax, 0, 0), 0); + zassert_equal(SCALE(imax, 0, imax, 0, 0), 0); + + zassert_equal(SCALE(imin, imin, 0, 0, 0), 0); + zassert_equal(SCALE(0, imin, 0, 0, 0), 0); + + /* Test the extreme cases */ + + zassert_equal(SCALE(imin, imin, imax, omin, omax), omin); + zassert_equal(SCALE(0, imin, imax, omin, omax), 0); + zassert_equal(SCALE(imax, imin, imax, omin, omax), omax); + + zassert_equal(SCALE(0, 0, imax, omin, omax), omin); + zassert_equal(SCALE(imax, 0, imax, omin, omax), omax); + + zassert_equal(SCALE(imin, imin, 0, omin, omax), omin); + zassert_equal(SCALE(0, imin, 0, omin, omax), omax); + + zassert_equal(SCALE(imin, imin, imax, 0, omax), 0); + zassert_equal(SCALE(0, imin, imax, 0, omax), omax / 2); + zassert_equal(SCALE(imax, imin, imax, 0, omax), omax); + + zassert_equal(SCALE(imin, imin, imax, omin, 0), omin); + zassert_equal(SCALE(0, imin, imax, omin, 0), omin / 2); + zassert_equal(SCALE(imax, imin, imax, omin, 0), 0); + + zassert_equal(SCALE(0, 0, imax, 0, omax), 0); + zassert_equal(SCALE(imax, 0, imax, 0, omax), omax); + + zassert_equal(SCALE(0, 0, imax, omin, 0), omin); + zassert_equal(SCALE(imax, 0, imax, omin, 0), 0); + + zassert_equal(SCALE(imin, imin, 0, 0, omax), 0); + zassert_equal(SCALE(0, imin, 0, 0, omax), omax); + + zassert_equal(SCALE(imin, imin, 0, omin, 0), omin); + zassert_equal(SCALE(0, imin, 0, omin, 0), 0); + + zassert_equal(SCALE(0, 0, imax, 0, omax), 0); + zassert_equal(SCALE(imax, 0, imax, 0, omax), omax); + } +} + /** * @brief Test wait_for works as expected with typical use cases * @@ -71,7 +138,6 @@ ZTEST(sys_util, test_NUM_VA_ARGS_LESS_1) * @} */ - /** * @defgroup sys_util_tests Sys Util Tests * @ingroup all_tests diff --git a/tests/subsys/dsp/macros/CMakeLists.txt b/tests/subsys/dsp/macros/CMakeLists.txt new file mode 100644 index 000000000000..a71f0b95b0ff --- /dev/null +++ b/tests/subsys/dsp/macros/CMakeLists.txt @@ -0,0 +1,9 @@ +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20.0) +find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) +project(cmsis_dsp_macros) + +target_sources(app PRIVATE + src/q7.c + ) diff --git a/tests/subsys/dsp/macros/prj.conf b/tests/subsys/dsp/macros/prj.conf new file mode 100644 index 000000000000..a4dcdd10ccb4 --- /dev/null +++ b/tests/subsys/dsp/macros/prj.conf @@ -0,0 +1,2 @@ +CONFIG_ZTEST=y +CONFIG_DSP=y diff --git a/tests/subsys/dsp/macros/src/q15.c b/tests/subsys/dsp/macros/src/q15.c new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/tests/subsys/dsp/macros/src/q31.c b/tests/subsys/dsp/macros/src/q31.c new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/tests/subsys/dsp/macros/src/q7.c b/tests/subsys/dsp/macros/src/q7.c new file mode 100644 index 000000000000..811891a894c1 --- /dev/null +++ b/tests/subsys/dsp/macros/src/q7.c @@ -0,0 +1,255 @@ +/* + * Copyright (c) 2021 Stephanos Ioannidis + * Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include + +#include + +ZTEST(zdsp_macros_q7, test_zdsp_macros_int_to_q7) +{ + q7_t e = 1; + + /* Test a few arbitrary values */ + zassert_within(Q7i(12, 10, 100), -122, e); + zassert_within(Q7i(120, 10, 234), -3, e); + zassert_within(Q7i(-1232, -91875010, 129308519), -23, e); + zassert_within(Q7i(-100000000, -91875010, -129308519), -73, e); + + /* Test the 1:1 conversion scale factor */ + for (int i = INT8_MIN; i <= INT8_MAX; i++) { + zassert_equal(Q7i(i, INT8_MIN, INT8_MAX), i); + } + + /* Test edge cases on the output range */ + zassert_equal(Q7i(0, 0, 1), MINq7); + zassert_equal(Q7i(1, 0, 1), MAXq7); + zassert_within(Q7i(1, 0, 2), 0, e); + zassert_equal(Q7i(1000, 1000, 1001), MINq7); + zassert_equal(Q7i(1001, 1000, 1001), MAXq7); + zassert_within(Q7i(1001, 1000, 1002), 0, e); + zassert_equal(Q7i(-1001, -1001, -1000), MINq7); + zassert_equal(Q7i(-1000, -1001, -1000), MAXq7); + zassert_within(Q7i(-1001, -1002, -1000), 0, e); + + /* Test boundary cases on the input range */ + zassert_equal(Q7i(0, INT8_MIN, 0), MAXq7); + zassert_equal(Q7i(0, 0, INT8_MAX), MINq7); + zassert_equal(Q7i(0, 0, UINT8_MAX), MINq7); + zassert_equal(Q7i(0, INT16_MIN, 0), MAXq7); + zassert_equal(Q7i(0, 0, INT16_MAX), MINq7); + zassert_equal(Q7i(0, 0, UINT16_MAX), MINq7); + zassert_equal(Q7i(0, INT32_MIN, 0), MAXq7); + zassert_equal(Q7i(0, 0, INT32_MAX), MINq7); + zassert_equal(Q7i(0, 0, UINT32_MAX), MINq7); + zassert_equal(Q7i(0, -(1LL << 54), 0), MAXq7); + zassert_equal(Q7i(0, 0, (1LL << 54) - 1), MINq7); + + /* Test that saturation happens if aiming above the output range */ + zassert_equal(Q7i(-1, 10, 20), MINq7); + zassert_equal(Q7i(200, 10, 20), MAXq7); +} + +ZTEST(zdsp_macros_q7, test_zdsp_macros_float_to_q7) +{ + q7_t e = 1; + + /* Test floatability for well-known values */ + zassert_within(Q7f(1.0), MAXq7, e); + zassert_within(Q7f(0.5), MAXq7 / 2, e); + zassert_within(Q7f(0.25), MAXq7 / 4, e); + zassert_within(Q7f(0.125), MAXq7 / 8, e); + zassert_within(Q7f(0.0), 0, e); + zassert_within(Q7f(-0.125), MINq7 / 8, e); + zassert_within(Q7f(-0.25), MINq7 / 4, e); + zassert_within(Q7f(-0.5), MINq7 / 2, e); + zassert_within(Q7f(-1.0), MINq7, e); + + /* Test saturation */ + zassert_within(Q7f(-1.1), MINq7, e); + zassert_within(Q7f(-1000000), MINq7, e); + zassert_within(Q7f(1.1), MAXq7, e); + zassert_within(Q7f(1000000), MAXq7, e); +} + +ZTEST(zdsp_macros_q7, test_zdsp_macros_int_from_q7) +{ + q7_t e = 1; + + /* Test edge cases for selected output ranges */ + zassert_within(INTq7(MINq7, 10, 100), 10, e); + zassert_within(INTq7(MAXq7, 10, 100), 100, e); + zassert_within(INTq7(MINq7, -100, -10), -100, e); + zassert_within(INTq7(MAXq7, -100, -10), -10, e); + + /* Test the 1:1 conversion scale factor */ + for (int i = MINq7; i <= MAXq7; i++) { + zassert_within(INTq7(i, INT8_MIN, INT8_MAX), i, e); + } +} + +ZTEST(zdsp_macros_q7, test_zdsp_macros_equiv_q7) +{ + /* Acceptable precision related to the change of range */ + int64_t e = ((int64_t)INT32_MAX - (int64_t)INT32_MIN) / (MAXq7 - MINq7); + + /* Test a few selected values within [INT32_MIN, INT32_MAX] */ + zassert_within(INTq7(Q7i(INT32_MIN, INT32_MIN, INT32_MAX), INT32_MIN, INT32_MAX), INT32_MIN, e); + zassert_within(INTq7(Q7i(-1032, INT32_MIN, INT32_MAX), INT32_MIN, INT32_MAX), -1032, e); + zassert_within(INTq7(Q7i(0, INT32_MIN, INT32_MAX), INT32_MIN, INT32_MAX), 0, e); + zassert_within(INTq7(Q7i(1032, INT32_MIN, INT32_MAX), INT32_MIN, INT32_MAX), 1032, e); + zassert_within(INTq7(Q7i(INT32_MAX, INT32_MIN, INT32_MAX), INT32_MIN, INT32_MAX), INT32_MAX, e); + + /* Test a few selected values within arbitrary ranges */ + zassert_within(INTq7(Q7i(132, 0, 1000), 0, 1000), 132, e); + zassert_within(INTq7(Q7i(-132, -1000, 0), -1000, 0), -132, e); + zassert_within(INTq7(Q7i(132, -1000, 1000), -1000, 1000), 132, e); + + /* Test a much larger range */ + for (int64_t i = INT32_MIN; i <= INT32_MAX; i += 1000009) { + zassert_within(INTq7(Q7i(i, INT32_MIN, INT32_MAX), INT32_MIN, INT32_MAX), i, e); + zassert_within(INTq7(Q7i(i, i, INT32_MAX), i, INT32_MAX), i, e); + zassert_within(INTq7(Q7i(i, INT32_MIN, i), INT32_MIN, i), i, e); + } +} + +ZTEST(zdsp_macros_q7, test_zdsp_macros_q15_from_q7) +{ + q15_t e = 1 << 8; + + /* Test through selected values through a whole range for Q.15 */ + zassert_within(Q15q7(Q7f(-1.0)), Q15f(-1.0), e); + zassert_within(Q15q7(Q7f(-0.3)), Q15f(-0.3), e); + zassert_within(Q15q7(Q7f(0.0)), Q15f(0.0), e); + zassert_within(Q15q7(Q7f(0.7)), Q15f(0.7), e); + zassert_within(Q15q7(Q7f(1.0)), Q15f(1.0), e); +} + +ZTEST(zdsp_macros_q7, test_zdsp_macros_q31_from_q7) +{ + q31_t e = 1 << 24; + + /* Test through selected values through a whole range for Q.31 */ + zassert_within(Q31q7(Q7f(-1.0)), Q31f(-1.0), e); + zassert_within(Q31q7(Q7f(-0.3)), Q31f(-0.3), e); + zassert_within(Q31q7(Q7f(0.0)), Q31f(0.0), e); + zassert_within(Q31q7(Q7f(0.7)), Q31f(0.7), e); + zassert_within(Q31q7(Q7f(1.0)), Q31f(1.0), e); +} + +ZTEST(zdsp_macros_q7, test_zdsp_macros_add_q7) +{ + q7_t e = 1; + + /* Test arbitrary values in range */ + zassert_equal(ADDq7(Q7f(0.0), Q7f(0.0)), Q7f(0.0)); + zassert_within(ADDq7(Q7f(0.3), Q7f(0.3)), Q7f(0.6), e); + zassert_within(ADDq7(Q7f(0.3), Q7f(-0.3)), Q7f(0.0), e); + zassert_within(ADDq7(Q7f(0.1), Q7f(0.9)), Q7f(1.0), e); + zassert_within(ADDq7(Q7f(0.1), Q7f(0.2)), Q7f(0.3), e); + zassert_within(ADDq7(Q7f(0.3123), Q7f(0.4123)), Q7f(0.7246), e); + + /* Test saturation */ + zassert_equal(ADDq7(Q7f(0.9), Q7f(0.5)), Q7f(1.0)); + zassert_equal(ADDq7(Q7f(-0.9), Q7f(-0.5)), Q7f(-1.0)); + zassert_equal(ADDq7(Q7f(1.1), Q7f(1.2)), Q7f(1.0)); + zassert_equal(ADDq7(Q7f(-1.1), Q7f(-1.2)), Q7f(-1.0)); + zassert_within(ADDq7(Q7f(1.1), Q7f(-1.2)), Q7f(0.0), e); +} + +ZTEST(zdsp_macros_q7, test_zdsp_macros_sub_q7) +{ + q7_t e = 1; + + /* Test arbitrary values in range */ + zassert_equal(SUBq7(Q7f(0.0), Q7f(0.0)), Q7f(0.0)); + zassert_equal(SUBq7(Q7f(0.3), Q7f(0.3)), Q7f(0.0)); + zassert_within(SUBq7(Q7f(0.1), Q7f(0.9)), Q7f(-0.8), e); + zassert_within(SUBq7(Q7f(0.1), Q7f(0.2)), Q7f(-0.1), e); + zassert_within(SUBq7(Q7f(0.3123), Q7f(0.4123)), Q7f(-0.1), e); + + /* Test saturation */ + zassert_within(SUBq7(Q7f(-0.1), Q7f(1.5)), Q7f(-1.0), e); + zassert_within(SUBq7(Q7f(-1.0), Q7f(0.3)), Q7f(-1.0), e); + zassert_equal(SUBq7(Q7f(0.2), Q7f(-1.6)), Q7f(1.0)); + zassert_equal(SUBq7(Q7f(-1.0), Q7f(0.4)), Q7f(-1.0)); +} + +ZTEST(zdsp_macros_q7, test_zdsp_macros_mul_q7) +{ + q7_t e = 1; + + /* Test arbitrary values */ + zassert_within(MULq7(Q7f(0.1), Q7f(0.2)), Q7f(0.02), e); + zassert_within(MULq7(Q7f(0.2), Q7f(0.2)), Q7f(0.04), e); + zassert_within(MULq7(Q7f(-0.1), Q7f(-0.2)), Q7f(0.02), e); + zassert_within(MULq7(Q7f(-0.1), Q7f(-0.2)), Q7f(0.02), e); + + /* Test identity. Note that with fixed-points 1.0 * 1.0 is slightly smaller than 1.0 */ + for (double f = -1.0; f <= 1.0; f += 0.001) { + zassert_within(MULq7(Q7f(f), Q7f(1.0)), Q7f(f), e); + zassert_within(MULq7(Q7f(f), Q7f(-1.0)), Q7f(-f), e); + zassert_within(MULq7(Q7f(-f), Q7f(1.0)), Q7f(-f), e); + zassert_within(MULq7(Q7f(-f), Q7f(-1.0)), Q7f(f), e); + } +} + +ZTEST(zdsp_macros_q7, test_zdsp_macros_div_q7) +{ + q7_t e = 3; + + /* Test arbitrary values */ + zassert_within(DIVq7(Q7f(0.1), Q7f(0.1)), Q7f(1.0), e); + zassert_within(DIVq7(Q7f(0.1), Q7f(0.2)), Q7f(0.5), e); + zassert_within(DIVq7(Q7f(0.1), Q7f(0.4)), Q7f(0.25), e); + zassert_within(DIVq7(Q7f(0.4), Q7f(0.5)), Q7f(0.8), e); + + /* Test saturation */ + zassert_within(DIVq7(Q7f(1.0), Q7f(0.2)), Q7f(1.0), e); + zassert_within(DIVq7(Q7f(1.0), Q7f(-0.9)), Q7f(-1.0), e); + zassert_within(DIVq7(Q7f(-1.0), Q7f(0.6)), Q7f(-1.0), e); + zassert_within(DIVq7(Q7f(-0.9), Q7f(-0.6)), Q7f(1.0), e); + + /* Test identity */ + for (double f = -1.0; f <= 1.0; f += 0.001) { + zassert_within(DIVq7(Q7f(f), Q7f(1.0)), Q7f(f), e); + zassert_within(DIVq7(Q7f(f), Q7f(-1.0)), Q7f(-f), e); + zassert_within(DIVq7(Q7f(-f), Q7f(1.0)), Q7f(-f), e); + zassert_within(DIVq7(Q7f(-f), Q7f(-1.0)), Q7f(f), e); + } +} + +ZTEST(zdsp_macros_q7, test_zdsp_macros_neg_q7) +{ + q7_t e = 1; + + /* Test arbitrary values */ + zassert_equal(NEGq7(Q7f(-1.0)), Q7f(1.0)); + zassert_equal(NEGq7(Q7f(-0.3)), Q7f(0.3)); + zassert_equal(NEGq7(Q7f(0.0)), Q7f(0.0)); + zassert_equal(NEGq7(Q7f(0.5)), Q7f(-0.5)); + zassert_within(NEGq7(Q7f(1.0)), Q7f(-1.0), e); +} + +ZTEST(zdsp_macros_q7, test_zdsp_macros_abs_q7) +{ + /* Test arbitrary values */ + zassert_equal(ABSq7(Q7f(-1.0)), Q7f(1.0)); + zassert_equal(ABSq7(Q7f(-0.4)), Q7f(0.4)); + zassert_equal(ABSq7(Q7f(0.0)), Q7f(0.0)); + zassert_equal(ABSq7(Q7f(0.4)), Q7f(0.4)); + zassert_equal(ABSq7(Q7f(1.0)), Q7f(1.0)); +} + +ZTEST(zdsp_macros_q7, test_zdsp_macros_complex_q7) +{ + /* */ + zassert_equal(ABSq7(Q7f(-1.0)), Q7f(1.0)); +} + +ZTEST_SUITE(zdsp_macros_q7, NULL, NULL, NULL, NULL, NULL); diff --git a/tests/subsys/dsp/macros/testcase.yaml b/tests/subsys/dsp/macros/testcase.yaml new file mode 100644 index 000000000000..af877788739e --- /dev/null +++ b/tests/subsys/dsp/macros/testcase.yaml @@ -0,0 +1,7 @@ +tests: + zdsp.macros: + integration_platforms: + - native_sim + tags: zdsp + min_flash: 64 + min_ram: 32