From 5cbd2e3950195f545e75d2ac76bf88cd009fa162 Mon Sep 17 00:00:00 2001 From: "Kwabena W. Agyeman" Date: Sun, 21 Mar 2021 21:12:18 -0700 Subject: [PATCH] Add debayering support to draw image --- src/omv/Makefile | 1 + src/omv/imlib/bayer.c | 1109 +++++++++++++++++++++++++ src/omv/imlib/draw.c | 120 ++- src/omv/imlib/imlib.c | 449 ---------- src/omv/imlib/imlib.h | 12 +- src/omv/modules/py_fir.c | 2 +- src/omv/modules/py_image.c | 27 +- src/omv/ports/nrf/omv_portconfig.mk | 1 + src/omv/ports/stm32/modules/py_lcd.c | 2 +- src/omv/ports/stm32/modules/py_tv.c | 6 +- src/omv/ports/stm32/omv_portconfig.mk | 1 + 11 files changed, 1258 insertions(+), 472 deletions(-) create mode 100644 src/omv/imlib/bayer.c diff --git a/src/omv/Makefile b/src/omv/Makefile index 0dcaa5401..ebf9f2642 100644 --- a/src/omv/Makefile +++ b/src/omv/Makefile @@ -53,6 +53,7 @@ SRCS += $(addprefix modules/, \ SRCS += $(addprefix imlib/, \ agast.c \ apriltag.c \ + bayer.c \ binary.c \ blob.c \ bmp.c \ diff --git a/src/omv/imlib/bayer.c b/src/omv/imlib/bayer.c new file mode 100644 index 000000000..47bbb08ab --- /dev/null +++ b/src/omv/imlib/bayer.c @@ -0,0 +1,1109 @@ +/* + * This file is part of the OpenMV project. + * + * Copyright (c) 2013-2021 Ibrahim Abdelkader + * Copyright (c) 2013-2021 Kwabena W. Agyeman + * + * This work is licensed under the MIT license, see the file LICENSE for details. + * + * Debayering Functions + */ +#include "imlib.h" + +void imlib_debayer_line_to_binary(int x_start, int x_end, int y_row, uint32_t *dst_row_ptr, image_t *src) +{ + int src_w = src->w, w_limit = src_w - 1, w_limit_m_1 = w_limit - 1; + int src_h = src->h, h_limit = src_h - 1, h_limit_m_1 = h_limit - 1; + + int y_row_odd = y_row & 1; + int y = (y_row / 2) * 2; + uint8_t *rowptr_grgr_0, *rowptr_bgbg_1, *rowptr_grgr_2, *rowptr_bgbg_3; + + // keep row pointers in bounds + if (y == 0) { + rowptr_bgbg_1 = src->data; + rowptr_grgr_2 = rowptr_bgbg_1 + ((src_h >= 2) ? src_w : 0); + rowptr_bgbg_3 = rowptr_bgbg_1 + ((src_h >= 3) ? (src_w * 2) : 0); + rowptr_grgr_0 = rowptr_grgr_2; + } else if (y == h_limit_m_1) { + rowptr_grgr_0 = src->data + ((y - 1) * src_w); + rowptr_bgbg_1 = rowptr_grgr_0 + src_w; + rowptr_grgr_2 = rowptr_bgbg_1 + src_w; + rowptr_bgbg_3 = rowptr_bgbg_1; + } else if (y >= h_limit) { + rowptr_grgr_0 = src->data + ((y - 1) * src_w); + rowptr_bgbg_1 = rowptr_grgr_0 + src_w; + rowptr_grgr_2 = rowptr_grgr_0; + rowptr_bgbg_3 = rowptr_bgbg_1; + } else { // get 4 neighboring rows + rowptr_grgr_0 = src->data + ((y - 1) * src_w); + rowptr_bgbg_1 = rowptr_grgr_0 + src_w; + rowptr_grgr_2 = rowptr_bgbg_1 + src_w; + rowptr_bgbg_3 = rowptr_grgr_2 + src_w; + } + + // If the image is an odd width this will go for the last loop and we drop the last column. + if (!y_row_odd) { // even + for (int x = x_start, i = 0; x < x_end; x += 2, i += 2) { + uint32_t row_grgr_0, row_bgbg_1, row_grgr_2; + + // keep pixels in bounds + if (x == 0) { + if (src_w >= 4) { + row_grgr_0 = *((uint32_t *) rowptr_grgr_0); + row_bgbg_1 = *((uint32_t *) rowptr_bgbg_1); + row_grgr_2 = *((uint32_t *) rowptr_grgr_2); + } else if (src_w >= 3) { + row_grgr_0 = *((uint16_t *) rowptr_grgr_0) | (*(rowptr_grgr_0 + 2) << 16); + row_bgbg_1 = *((uint16_t *) rowptr_bgbg_1) | (*(rowptr_bgbg_1 + 2) << 16); + row_grgr_2 = *((uint16_t *) rowptr_grgr_2) | (*(rowptr_grgr_2 + 2) << 16); + } else if (src_w >= 2) { + row_grgr_0 = *((uint16_t *) rowptr_grgr_0); + row_grgr_0 = (row_grgr_0 << 16) | row_grgr_0; + row_bgbg_1 = *((uint16_t *) rowptr_bgbg_1); + row_bgbg_1 = (row_bgbg_1 << 16) | row_bgbg_1; + row_grgr_2 = *((uint16_t *) rowptr_grgr_2); + row_grgr_2 = (row_grgr_2 << 16) | row_grgr_2; + } else { + row_grgr_0 = *(rowptr_grgr_0) * 0x01010101; + row_bgbg_1 = *(rowptr_bgbg_1) * 0x01010101; + row_grgr_2 = *(rowptr_grgr_2) * 0x01010101; + } + // The starting point needs to be offset by 1. The below patterns are actually + // rgrg, gbgb, rgrg, and gbgb. So, shift left and backfill the missing border pixel. + row_grgr_0 = (row_grgr_0 << 8) | __UXTB_RORn(row_grgr_0, 8); + row_bgbg_1 = (row_bgbg_1 << 8) | __UXTB_RORn(row_bgbg_1, 8); + row_grgr_2 = (row_grgr_2 << 8) | __UXTB_RORn(row_grgr_2, 8); + } else if (x == w_limit_m_1) { + row_grgr_0 = *((uint32_t *) (rowptr_grgr_0 + x - 2)); + row_grgr_0 = (row_grgr_0 >> 8) | ((row_grgr_0 << 8) & 0xff000000); + row_bgbg_1 = *((uint32_t *) (rowptr_bgbg_1 + x - 2)); + row_bgbg_1 = (row_bgbg_1 >> 8) | ((row_bgbg_1 << 8) & 0xff000000); + row_grgr_2 = *((uint32_t *) (rowptr_grgr_2 + x - 2)); + row_grgr_2 = (row_grgr_2 >> 8) | ((row_grgr_2 << 8) & 0xff000000); + } else if (x >= w_limit) { + row_grgr_0 = *((uint16_t *) (rowptr_grgr_0 + x - 1)); + row_grgr_0 = (row_grgr_0 << 16) | row_grgr_0; + row_bgbg_1 = *((uint16_t *) (rowptr_bgbg_1 + x - 1)); + row_bgbg_1 = (row_bgbg_1 << 16) | row_bgbg_1; + row_grgr_2 = *((uint16_t *) (rowptr_grgr_2 + x - 1)); + row_grgr_2 = (row_grgr_2 << 16) | row_grgr_2; + } else { // get 4 neighboring rows + row_grgr_0 = *((uint32_t *) (rowptr_grgr_0 + x - 1)); + row_bgbg_1 = *((uint32_t *) (rowptr_bgbg_1 + x - 1)); + row_grgr_2 = *((uint32_t *) (rowptr_grgr_2 + x - 1)); + } + + #if defined(MCU_SERIES_F4) || defined(MCU_SERIES_F7) || defined(MCU_SERIES_H7) + int row_01 = __UHADD8(row_grgr_0, row_grgr_2); + int row_1g = __UHADD8(row_bgbg_1, __PKHTB(row_bgbg_1, row_bgbg_1, 16)); + + int r_pixels_0 = __UXTB16(__UHADD8(row_01, __PKHTB(row_01, row_01, 16))); + int g_pixels_0 = __UXTB16(__UHADD8(row_1g, __PKHTB(row_1g, row_01, 8))); + int b_pixels_0 = __UXTB16_RORn(__UHADD8(row_bgbg_1, __PKHBT(row_bgbg_1, row_bgbg_1, 16)), 8); + #else + + int r0 = ((row_grgr_0 & 0xFF) + (row_grgr_2 & 0xFF)) >> 1; + int r2 = (((row_grgr_0 >> 16) & 0xFF) + ((row_grgr_2 >> 16) & 0xFF)) >> 1; + int r_pixels_0 = (r2 << 16) | ((r0 + r2) >> 1); + + int g0 = (row_grgr_0 >> 8) & 0xFF; + int g1 = (((row_bgbg_1 >> 16) & 0xFF) + (row_bgbg_1 & 0xFF)) >> 1; + int g2 = (row_grgr_2 >> 8) & 0xFF; + int g_pixels_0 = (row_bgbg_1 & 0xFF0000) | ((((g0 + g2) >> 1) + g1) >> 1); + + int b1 = (((row_bgbg_1 >> 24) & 0xFF) + ((row_bgbg_1 >> 8) & 0xFF)) >> 1; + int b_pixels_0 = (b1 << 16) | ((row_bgbg_1 >> 8) & 0xFF); + + #endif + + int y0 = ((r_pixels_0 * 38) + (g_pixels_0 * 75) + (b_pixels_0 * 15)) >> 7; + IMAGE_PUT_BINARY_PIXEL_FAST(dst_row_ptr, i, (y0 >> 7) & 0x1); + + if (x != w_limit) { + IMAGE_PUT_BINARY_PIXEL_FAST(dst_row_ptr, i + 1, (y0 >> 23) & 0x1); + } + } + } else { // odd + for (int x = x_start, i = 0; x < x_end; x += 2, i += 2) { + uint32_t row_bgbg_1, row_grgr_2, row_bgbg_3; + + // keep pixels in bounds + if (x == 0) { + if (src_w >= 4) { + row_bgbg_1 = *((uint32_t *) rowptr_bgbg_1); + row_grgr_2 = *((uint32_t *) rowptr_grgr_2); + row_bgbg_3 = *((uint32_t *) rowptr_bgbg_3); + } else if (src_w >= 3) { + row_bgbg_1 = *((uint16_t *) rowptr_bgbg_1) | (*(rowptr_bgbg_1 + 2) << 16); + row_grgr_2 = *((uint16_t *) rowptr_grgr_2) | (*(rowptr_grgr_2 + 2) << 16); + row_bgbg_3 = *((uint16_t *) rowptr_bgbg_3) | (*(rowptr_bgbg_3 + 2) << 16); + } else if (src_w >= 2) { + row_bgbg_1 = *((uint16_t *) rowptr_bgbg_1); + row_bgbg_1 = (row_bgbg_1 << 16) | row_bgbg_1; + row_grgr_2 = *((uint16_t *) rowptr_grgr_2); + row_grgr_2 = (row_grgr_2 << 16) | row_grgr_2; + row_bgbg_3 = *((uint16_t *) rowptr_bgbg_3); + row_bgbg_3 = (row_bgbg_3 << 16) | row_bgbg_3; + } else { + row_bgbg_1 = *(rowptr_bgbg_1) * 0x01010101; + row_grgr_2 = *(rowptr_grgr_2) * 0x01010101; + row_bgbg_3 = *(rowptr_bgbg_3) * 0x01010101; + } + // The starting point needs to be offset by 1. The below patterns are actually + // rgrg, gbgb, rgrg, and gbgb. So, shift left and backfill the missing border pixel. + row_bgbg_1 = (row_bgbg_1 << 8) | __UXTB_RORn(row_bgbg_1, 8); + row_grgr_2 = (row_grgr_2 << 8) | __UXTB_RORn(row_grgr_2, 8); + row_bgbg_3 = (row_bgbg_3 << 8) | __UXTB_RORn(row_bgbg_3, 8); + } else if (x == w_limit_m_1) { + row_bgbg_1 = *((uint32_t *) (rowptr_bgbg_1 + x - 2)); + row_bgbg_1 = (row_bgbg_1 >> 8) | ((row_bgbg_1 << 8) & 0xff000000); + row_grgr_2 = *((uint32_t *) (rowptr_grgr_2 + x - 2)); + row_grgr_2 = (row_grgr_2 >> 8) | ((row_grgr_2 << 8) & 0xff000000); + row_bgbg_3 = *((uint32_t *) (rowptr_bgbg_3 + x - 2)); + row_bgbg_3 = (row_bgbg_3 >> 8) | ((row_bgbg_1 << 8) & 0xff000000); + } else if (x >= w_limit) { + row_bgbg_1 = *((uint16_t *) (rowptr_bgbg_1 + x - 1)); + row_bgbg_1 = (row_bgbg_1 << 16) | row_bgbg_1; + row_grgr_2 = *((uint16_t *) (rowptr_grgr_2 + x - 1)); + row_grgr_2 = (row_grgr_2 << 16) | row_grgr_2; + row_bgbg_3 = *((uint16_t *) (rowptr_bgbg_3 + x - 1)); + row_bgbg_3 = (row_bgbg_3 << 16) | row_bgbg_3; + } else { // get 4 neighboring rows + row_bgbg_1 = *((uint32_t *) (rowptr_bgbg_1 + x - 1)); + row_grgr_2 = *((uint32_t *) (rowptr_grgr_2 + x - 1)); + row_bgbg_3 = *((uint32_t *) (rowptr_bgbg_3 + x - 1)); + } + + #if defined(MCU_SERIES_F4) || defined(MCU_SERIES_F7) || defined(MCU_SERIES_H7) + int row_13 = __UHADD8(row_bgbg_1, row_bgbg_3); + int row_2g = __UHADD8(row_grgr_2, __PKHBT(row_grgr_2, row_grgr_2, 16)); + + int r_pixels_1 = __UXTB16(__UHADD8(row_grgr_2, __PKHTB(row_grgr_2, row_grgr_2, 16))); + int g_pixels_1 = __UXTB16_RORn(__UHADD8(row_2g, __PKHBT(row_2g, row_13, 8)), 8); + int b_pixels_1 = __UXTB16_RORn(__UHADD8(row_13, __PKHBT(row_13, row_13, 16)), 8); + #else + + int r2 = (((row_grgr_2 >> 16) & 0xFF) + (row_grgr_2 & 0xFF)) >> 1; + int r_pixels_1 = (row_grgr_2 & 0xFF0000) | r2; + + int g1 = (row_bgbg_1 >> 16) & 0xFF; + int g2 = (((row_grgr_2 >> 24) & 0xFF) + ((row_grgr_2 >> 8) & 0xFF)) >> 1; + int g3 = (row_bgbg_3 >> 16) & 0xFF; + int g_pixels_1 = (((((g1 + g3) >> 1) + g2) >> 1) << 16) | ((row_grgr_2 >> 8) & 0xFF); + + int b1 = (((row_bgbg_1 >> 8) & 0xFF) + ((row_bgbg_3 >> 8) & 0xFF)) >> 1; + int b3 = (((row_bgbg_1 >> 24) & 0xFF) + ((row_bgbg_3 >> 24) & 0xFF)) >> 1; + int b_pixels_1 = (((b1 + b3) >> 1) << 16) | b1; + + #endif + + int y1 = ((r_pixels_1 * 38) + (g_pixels_1 * 75) + (b_pixels_1 * 15)) >> 7; + IMAGE_PUT_BINARY_PIXEL_FAST(dst_row_ptr, i, (y1 >> 7) & 0x1); + + if (x != w_limit) { + IMAGE_PUT_BINARY_PIXEL_FAST(dst_row_ptr, i + 1, (y1 >> 23) & 0x1); + } + } + } +} + +// Does no bounds checking on the destination. +void imlib_debayer_image_to_binary(image_t *dst, image_t *src) +{ + int src_w = src->w, w_limit = src_w - 1, w_limit_m_1 = w_limit - 1; + int src_h = src->h, h_limit = src_h - 1, h_limit_m_1 = h_limit - 1; + + // If the image is an odd height this will go for the last loop and we drop the last row. + for (int y = 0; y < src_h; y += 2) { + uint32_t *row_ptr_e = IMAGE_COMPUTE_BINARY_PIXEL_ROW_PTR(dst, y); + uint32_t *row_ptr_o = IMAGE_COMPUTE_BINARY_PIXEL_ROW_PTR(dst, y + 1); + uint8_t *rowptr_grgr_0, *rowptr_bgbg_1, *rowptr_grgr_2, *rowptr_bgbg_3; + + // keep row pointers in bounds + if (y == 0) { + rowptr_bgbg_1 = src->data; + rowptr_grgr_2 = rowptr_bgbg_1 + ((src_h >= 2) ? src_w : 0); + rowptr_bgbg_3 = rowptr_bgbg_1 + ((src_h >= 3) ? (src_w * 2) : 0); + rowptr_grgr_0 = rowptr_grgr_2; + } else if (y == h_limit_m_1) { + rowptr_grgr_0 = src->data + ((y - 1) * src_w); + rowptr_bgbg_1 = rowptr_grgr_0 + src_w; + rowptr_grgr_2 = rowptr_bgbg_1 + src_w; + rowptr_bgbg_3 = rowptr_bgbg_1; + } else if (y >= h_limit) { + rowptr_grgr_0 = src->data + ((y - 1) * src_w); + rowptr_bgbg_1 = rowptr_grgr_0 + src_w; + rowptr_grgr_2 = rowptr_grgr_0; + rowptr_bgbg_3 = rowptr_bgbg_1; + } else { // get 4 neighboring rows + rowptr_grgr_0 = src->data + ((y - 1) * src_w); + rowptr_bgbg_1 = rowptr_grgr_0 + src_w; + rowptr_grgr_2 = rowptr_bgbg_1 + src_w; + rowptr_bgbg_3 = rowptr_grgr_2 + src_w; + } + + // If the image is an odd width this will go for the last loop and we drop the last column. + for (int x = 0; x < src_w; x += 2) { + uint32_t row_grgr_0, row_bgbg_1, row_grgr_2, row_bgbg_3; + + // keep pixels in bounds + if (x == 0) { + if (src_w >= 4) { + row_grgr_0 = *((uint32_t *) rowptr_grgr_0); + row_bgbg_1 = *((uint32_t *) rowptr_bgbg_1); + row_grgr_2 = *((uint32_t *) rowptr_grgr_2); + row_bgbg_3 = *((uint32_t *) rowptr_bgbg_3); + } else if (src_w >= 3) { + row_grgr_0 = *((uint16_t *) rowptr_grgr_0) | (*(rowptr_grgr_0 + 2) << 16); + row_bgbg_1 = *((uint16_t *) rowptr_bgbg_1) | (*(rowptr_bgbg_1 + 2) << 16); + row_grgr_2 = *((uint16_t *) rowptr_grgr_2) | (*(rowptr_grgr_2 + 2) << 16); + row_bgbg_3 = *((uint16_t *) rowptr_bgbg_3) | (*(rowptr_bgbg_3 + 2) << 16); + } else if (src_w >= 2) { + row_grgr_0 = *((uint16_t *) rowptr_grgr_0); + row_grgr_0 = (row_grgr_0 << 16) | row_grgr_0; + row_bgbg_1 = *((uint16_t *) rowptr_bgbg_1); + row_bgbg_1 = (row_bgbg_1 << 16) | row_bgbg_1; + row_grgr_2 = *((uint16_t *) rowptr_grgr_2); + row_grgr_2 = (row_grgr_2 << 16) | row_grgr_2; + row_bgbg_3 = *((uint16_t *) rowptr_bgbg_3); + row_bgbg_3 = (row_bgbg_3 << 16) | row_bgbg_3; + } else { + row_grgr_0 = *(rowptr_grgr_0) * 0x01010101; + row_bgbg_1 = *(rowptr_bgbg_1) * 0x01010101; + row_grgr_2 = *(rowptr_grgr_2) * 0x01010101; + row_bgbg_3 = *(rowptr_bgbg_3) * 0x01010101; + } + // The starting point needs to be offset by 1. The below patterns are actually + // rgrg, gbgb, rgrg, and gbgb. So, shift left and backfill the missing border pixel. + row_grgr_0 = (row_grgr_0 << 8) | __UXTB_RORn(row_grgr_0, 8); + row_bgbg_1 = (row_bgbg_1 << 8) | __UXTB_RORn(row_bgbg_1, 8); + row_grgr_2 = (row_grgr_2 << 8) | __UXTB_RORn(row_grgr_2, 8); + row_bgbg_3 = (row_bgbg_3 << 8) | __UXTB_RORn(row_bgbg_3, 8); + } else if (x == w_limit_m_1) { + row_grgr_0 = *((uint32_t *) (rowptr_grgr_0 + x - 2)); + row_grgr_0 = (row_grgr_0 >> 8) | ((row_grgr_0 << 8) & 0xff000000); + row_bgbg_1 = *((uint32_t *) (rowptr_bgbg_1 + x - 2)); + row_bgbg_1 = (row_bgbg_1 >> 8) | ((row_bgbg_1 << 8) & 0xff000000); + row_grgr_2 = *((uint32_t *) (rowptr_grgr_2 + x - 2)); + row_grgr_2 = (row_grgr_2 >> 8) | ((row_grgr_2 << 8) & 0xff000000); + row_bgbg_3 = *((uint32_t *) (rowptr_bgbg_3 + x - 2)); + row_bgbg_3 = (row_bgbg_3 >> 8) | ((row_bgbg_1 << 8) & 0xff000000); + } else if (x >= w_limit) { + row_grgr_0 = *((uint16_t *) (rowptr_grgr_0 + x - 1)); + row_grgr_0 = (row_grgr_0 << 16) | row_grgr_0; + row_bgbg_1 = *((uint16_t *) (rowptr_bgbg_1 + x - 1)); + row_bgbg_1 = (row_bgbg_1 << 16) | row_bgbg_1; + row_grgr_2 = *((uint16_t *) (rowptr_grgr_2 + x - 1)); + row_grgr_2 = (row_grgr_2 << 16) | row_grgr_2; + row_bgbg_3 = *((uint16_t *) (rowptr_bgbg_3 + x - 1)); + row_bgbg_3 = (row_bgbg_3 << 16) | row_bgbg_3; + } else { // get 4 neighboring rows + row_grgr_0 = *((uint32_t *) (rowptr_grgr_0 + x - 1)); + row_bgbg_1 = *((uint32_t *) (rowptr_bgbg_1 + x - 1)); + row_grgr_2 = *((uint32_t *) (rowptr_grgr_2 + x - 1)); + row_bgbg_3 = *((uint32_t *) (rowptr_bgbg_3 + x - 1)); + } + + #if defined(MCU_SERIES_F4) || defined(MCU_SERIES_F7) || defined(MCU_SERIES_H7) + int row_01 = __UHADD8(row_grgr_0, row_grgr_2); + int row_1g = __UHADD8(row_bgbg_1, __PKHTB(row_bgbg_1, row_bgbg_1, 16)); + + int r_pixels_0 = __UXTB16(__UHADD8(row_01, __PKHTB(row_01, row_01, 16))); + int g_pixels_0 = __UXTB16(__UHADD8(row_1g, __PKHTB(row_1g, row_01, 8))); + int b_pixels_0 = __UXTB16_RORn(__UHADD8(row_bgbg_1, __PKHBT(row_bgbg_1, row_bgbg_1, 16)), 8); + #else + + int r0 = ((row_grgr_0 & 0xFF) + (row_grgr_2 & 0xFF)) >> 1; + int r2 = (((row_grgr_0 >> 16) & 0xFF) + ((row_grgr_2 >> 16) & 0xFF)) >> 1; + int r_pixels_0 = (r2 << 16) | ((r0 + r2) >> 1); + + int g0 = (row_grgr_0 >> 8) & 0xFF; + int g1 = (((row_bgbg_1 >> 16) & 0xFF) + (row_bgbg_1 & 0xFF)) >> 1; + int g2 = (row_grgr_2 >> 8) & 0xFF; + int g_pixels_0 = (row_bgbg_1 & 0xFF0000) | ((((g0 + g2) >> 1) + g1) >> 1); + + int b1 = (((row_bgbg_1 >> 24) & 0xFF) + ((row_bgbg_1 >> 8) & 0xFF)) >> 1; + int b_pixels_0 = (b1 << 16) | ((row_bgbg_1 >> 8) & 0xFF); + + #endif + + int y0 = ((r_pixels_0 * 38) + (g_pixels_0 * 75) + (b_pixels_0 * 15)) >> 7; + IMAGE_PUT_BINARY_PIXEL_FAST(row_ptr_e, x, (y0 >> 7) & 0x1); + + if (x != w_limit) { + IMAGE_PUT_BINARY_PIXEL_FAST(row_ptr_e, x + 1, (y0 >> 23) & 0x1); + } + + if (y == h_limit) { + break; + } + + #if defined(MCU_SERIES_F4) || defined(MCU_SERIES_F7) || defined(MCU_SERIES_H7) + int row_13 = __UHADD8(row_bgbg_1, row_bgbg_3); + int row_2g = __UHADD8(row_grgr_2, __PKHBT(row_grgr_2, row_grgr_2, 16)); + + int r_pixels_1 = __UXTB16(__UHADD8(row_grgr_2, __PKHTB(row_grgr_2, row_grgr_2, 16))); + int g_pixels_1 = __UXTB16_RORn(__UHADD8(row_2g, __PKHBT(row_2g, row_13, 8)), 8); + int b_pixels_1 = __UXTB16_RORn(__UHADD8(row_13, __PKHBT(row_13, row_13, 16)), 8); + #else + + r2 = (((row_grgr_2 >> 16) & 0xFF) + (row_grgr_2 & 0xFF)) >> 1; + int r_pixels_1 = (row_grgr_2 & 0xFF0000) | r2; + + g1 = (row_bgbg_1 >> 16) & 0xFF; + g2 = (((row_grgr_2 >> 24) & 0xFF) + ((row_grgr_2 >> 8) & 0xFF)) >> 1; + int g3 = (row_bgbg_3 >> 16) & 0xFF; + int g_pixels_1 = (((((g1 + g3) >> 1) + g2) >> 1) << 16) | ((row_grgr_2 >> 8) & 0xFF); + + b1 = (((row_bgbg_1 >> 8) & 0xFF) + ((row_bgbg_3 >> 8) & 0xFF)) >> 1; + int b3 = (((row_bgbg_1 >> 24) & 0xFF) + ((row_bgbg_3 >> 24) & 0xFF)) >> 1; + int b_pixels_1 = (((b1 + b3) >> 1) << 16) | b1; + + #endif + + int y1 = ((r_pixels_1 * 38) + (g_pixels_1 * 75) + (b_pixels_1 * 15)) >> 7; + IMAGE_PUT_BINARY_PIXEL_FAST(row_ptr_o, x, (y1 >> 7) & 0x1); + + if (x != w_limit) { + IMAGE_PUT_BINARY_PIXEL_FAST(row_ptr_o, x + 1, (y1 >> 23) & 0x1); + } + } + } +} + +void imlib_debayer_line_to_grayscale(int x_start, int x_end, int y_row, uint8_t *dst_row_ptr, image_t *src) +{ + int src_w = src->w, w_limit = src_w - 1, w_limit_m_1 = w_limit - 1; + int src_h = src->h, h_limit = src_h - 1, h_limit_m_1 = h_limit - 1; + + int y_row_odd = y_row & 1; + int y = (y_row / 2) * 2; + uint8_t *rowptr_grgr_0, *rowptr_bgbg_1, *rowptr_grgr_2, *rowptr_bgbg_3; + + // keep row pointers in bounds + if (y == 0) { + rowptr_bgbg_1 = src->data; + rowptr_grgr_2 = rowptr_bgbg_1 + ((src_h >= 2) ? src_w : 0); + rowptr_bgbg_3 = rowptr_bgbg_1 + ((src_h >= 3) ? (src_w * 2) : 0); + rowptr_grgr_0 = rowptr_grgr_2; + } else if (y == h_limit_m_1) { + rowptr_grgr_0 = src->data + ((y - 1) * src_w); + rowptr_bgbg_1 = rowptr_grgr_0 + src_w; + rowptr_grgr_2 = rowptr_bgbg_1 + src_w; + rowptr_bgbg_3 = rowptr_bgbg_1; + } else if (y >= h_limit) { + rowptr_grgr_0 = src->data + ((y - 1) * src_w); + rowptr_bgbg_1 = rowptr_grgr_0 + src_w; + rowptr_grgr_2 = rowptr_grgr_0; + rowptr_bgbg_3 = rowptr_bgbg_1; + } else { // get 4 neighboring rows + rowptr_grgr_0 = src->data + ((y - 1) * src_w); + rowptr_bgbg_1 = rowptr_grgr_0 + src_w; + rowptr_grgr_2 = rowptr_bgbg_1 + src_w; + rowptr_bgbg_3 = rowptr_grgr_2 + src_w; + } + + // If the image is an odd width this will go for the last loop and we drop the last column. + if (!y_row_odd) { // even + for (int x = x_start, i = 0; x < x_end; x += 2, i += 2) { + uint32_t row_grgr_0, row_bgbg_1, row_grgr_2; + + // keep pixels in bounds + if (x == 0) { + if (src_w >= 4) { + row_grgr_0 = *((uint32_t *) rowptr_grgr_0); + row_bgbg_1 = *((uint32_t *) rowptr_bgbg_1); + row_grgr_2 = *((uint32_t *) rowptr_grgr_2); + } else if (src_w >= 3) { + row_grgr_0 = *((uint16_t *) rowptr_grgr_0) | (*(rowptr_grgr_0 + 2) << 16); + row_bgbg_1 = *((uint16_t *) rowptr_bgbg_1) | (*(rowptr_bgbg_1 + 2) << 16); + row_grgr_2 = *((uint16_t *) rowptr_grgr_2) | (*(rowptr_grgr_2 + 2) << 16); + } else if (src_w >= 2) { + row_grgr_0 = *((uint16_t *) rowptr_grgr_0); + row_grgr_0 = (row_grgr_0 << 16) | row_grgr_0; + row_bgbg_1 = *((uint16_t *) rowptr_bgbg_1); + row_bgbg_1 = (row_bgbg_1 << 16) | row_bgbg_1; + row_grgr_2 = *((uint16_t *) rowptr_grgr_2); + row_grgr_2 = (row_grgr_2 << 16) | row_grgr_2; + } else { + row_grgr_0 = *(rowptr_grgr_0) * 0x01010101; + row_bgbg_1 = *(rowptr_bgbg_1) * 0x01010101; + row_grgr_2 = *(rowptr_grgr_2) * 0x01010101; + } + // The starting point needs to be offset by 1. The below patterns are actually + // rgrg, gbgb, rgrg, and gbgb. So, shift left and backfill the missing border pixel. + row_grgr_0 = (row_grgr_0 << 8) | __UXTB_RORn(row_grgr_0, 8); + row_bgbg_1 = (row_bgbg_1 << 8) | __UXTB_RORn(row_bgbg_1, 8); + row_grgr_2 = (row_grgr_2 << 8) | __UXTB_RORn(row_grgr_2, 8); + } else if (x == w_limit_m_1) { + row_grgr_0 = *((uint32_t *) (rowptr_grgr_0 + x - 2)); + row_grgr_0 = (row_grgr_0 >> 8) | ((row_grgr_0 << 8) & 0xff000000); + row_bgbg_1 = *((uint32_t *) (rowptr_bgbg_1 + x - 2)); + row_bgbg_1 = (row_bgbg_1 >> 8) | ((row_bgbg_1 << 8) & 0xff000000); + row_grgr_2 = *((uint32_t *) (rowptr_grgr_2 + x - 2)); + row_grgr_2 = (row_grgr_2 >> 8) | ((row_grgr_2 << 8) & 0xff000000); + } else if (x >= w_limit) { + row_grgr_0 = *((uint16_t *) (rowptr_grgr_0 + x - 1)); + row_grgr_0 = (row_grgr_0 << 16) | row_grgr_0; + row_bgbg_1 = *((uint16_t *) (rowptr_bgbg_1 + x - 1)); + row_bgbg_1 = (row_bgbg_1 << 16) | row_bgbg_1; + row_grgr_2 = *((uint16_t *) (rowptr_grgr_2 + x - 1)); + row_grgr_2 = (row_grgr_2 << 16) | row_grgr_2; + } else { // get 4 neighboring rows + row_grgr_0 = *((uint32_t *) (rowptr_grgr_0 + x - 1)); + row_bgbg_1 = *((uint32_t *) (rowptr_bgbg_1 + x - 1)); + row_grgr_2 = *((uint32_t *) (rowptr_grgr_2 + x - 1)); + } + + #if defined(MCU_SERIES_F4) || defined(MCU_SERIES_F7) || defined(MCU_SERIES_H7) + int row_01 = __UHADD8(row_grgr_0, row_grgr_2); + int row_1g = __UHADD8(row_bgbg_1, __PKHTB(row_bgbg_1, row_bgbg_1, 16)); + + int r_pixels_0 = __UXTB16(__UHADD8(row_01, __PKHTB(row_01, row_01, 16))); + int g_pixels_0 = __UXTB16(__UHADD8(row_1g, __PKHTB(row_1g, row_01, 8))); + int b_pixels_0 = __UXTB16_RORn(__UHADD8(row_bgbg_1, __PKHBT(row_bgbg_1, row_bgbg_1, 16)), 8); + #else + + int r0 = ((row_grgr_0 & 0xFF) + (row_grgr_2 & 0xFF)) >> 1; + int r2 = (((row_grgr_0 >> 16) & 0xFF) + ((row_grgr_2 >> 16) & 0xFF)) >> 1; + int r_pixels_0 = (r2 << 16) | ((r0 + r2) >> 1); + + int g0 = (row_grgr_0 >> 8) & 0xFF; + int g1 = (((row_bgbg_1 >> 16) & 0xFF) + (row_bgbg_1 & 0xFF)) >> 1; + int g2 = (row_grgr_2 >> 8) & 0xFF; + int g_pixels_0 = (row_bgbg_1 & 0xFF0000) | ((((g0 + g2) >> 1) + g1) >> 1); + + int b1 = (((row_bgbg_1 >> 24) & 0xFF) + ((row_bgbg_1 >> 8) & 0xFF)) >> 1; + int b_pixels_0 = (b1 << 16) | ((row_bgbg_1 >> 8) & 0xFF); + + #endif + + int y0 = ((r_pixels_0 * 38) + (g_pixels_0 * 75) + (b_pixels_0 * 15)) >> 7; + IMAGE_PUT_GRAYSCALE_PIXEL_FAST(dst_row_ptr, i, y0); + + if (x != w_limit) { + IMAGE_PUT_GRAYSCALE_PIXEL_FAST(dst_row_ptr, i + 1, y0 >> 16); + } + } + } else { // odd + for (int x = x_start, i = 0; x < x_end; x += 2, i += 2) { + uint32_t row_bgbg_1, row_grgr_2, row_bgbg_3; + + // keep pixels in bounds + if (x == 0) { + if (src_w >= 4) { + row_bgbg_1 = *((uint32_t *) rowptr_bgbg_1); + row_grgr_2 = *((uint32_t *) rowptr_grgr_2); + row_bgbg_3 = *((uint32_t *) rowptr_bgbg_3); + } else if (src_w >= 3) { + row_bgbg_1 = *((uint16_t *) rowptr_bgbg_1) | (*(rowptr_bgbg_1 + 2) << 16); + row_grgr_2 = *((uint16_t *) rowptr_grgr_2) | (*(rowptr_grgr_2 + 2) << 16); + row_bgbg_3 = *((uint16_t *) rowptr_bgbg_3) | (*(rowptr_bgbg_3 + 2) << 16); + } else if (src_w >= 2) { + row_bgbg_1 = *((uint16_t *) rowptr_bgbg_1); + row_bgbg_1 = (row_bgbg_1 << 16) | row_bgbg_1; + row_grgr_2 = *((uint16_t *) rowptr_grgr_2); + row_grgr_2 = (row_grgr_2 << 16) | row_grgr_2; + row_bgbg_3 = *((uint16_t *) rowptr_bgbg_3); + row_bgbg_3 = (row_bgbg_3 << 16) | row_bgbg_3; + } else { + row_bgbg_1 = *(rowptr_bgbg_1) * 0x01010101; + row_grgr_2 = *(rowptr_grgr_2) * 0x01010101; + row_bgbg_3 = *(rowptr_bgbg_3) * 0x01010101; + } + // The starting point needs to be offset by 1. The below patterns are actually + // rgrg, gbgb, rgrg, and gbgb. So, shift left and backfill the missing border pixel. + row_bgbg_1 = (row_bgbg_1 << 8) | __UXTB_RORn(row_bgbg_1, 8); + row_grgr_2 = (row_grgr_2 << 8) | __UXTB_RORn(row_grgr_2, 8); + row_bgbg_3 = (row_bgbg_3 << 8) | __UXTB_RORn(row_bgbg_3, 8); + } else if (x == w_limit_m_1) { + row_bgbg_1 = *((uint32_t *) (rowptr_bgbg_1 + x - 2)); + row_bgbg_1 = (row_bgbg_1 >> 8) | ((row_bgbg_1 << 8) & 0xff000000); + row_grgr_2 = *((uint32_t *) (rowptr_grgr_2 + x - 2)); + row_grgr_2 = (row_grgr_2 >> 8) | ((row_grgr_2 << 8) & 0xff000000); + row_bgbg_3 = *((uint32_t *) (rowptr_bgbg_3 + x - 2)); + row_bgbg_3 = (row_bgbg_3 >> 8) | ((row_bgbg_1 << 8) & 0xff000000); + } else if (x >= w_limit) { + row_bgbg_1 = *((uint16_t *) (rowptr_bgbg_1 + x - 1)); + row_bgbg_1 = (row_bgbg_1 << 16) | row_bgbg_1; + row_grgr_2 = *((uint16_t *) (rowptr_grgr_2 + x - 1)); + row_grgr_2 = (row_grgr_2 << 16) | row_grgr_2; + row_bgbg_3 = *((uint16_t *) (rowptr_bgbg_3 + x - 1)); + row_bgbg_3 = (row_bgbg_3 << 16) | row_bgbg_3; + } else { // get 4 neighboring rows + row_bgbg_1 = *((uint32_t *) (rowptr_bgbg_1 + x - 1)); + row_grgr_2 = *((uint32_t *) (rowptr_grgr_2 + x - 1)); + row_bgbg_3 = *((uint32_t *) (rowptr_bgbg_3 + x - 1)); + } + + #if defined(MCU_SERIES_F4) || defined(MCU_SERIES_F7) || defined(MCU_SERIES_H7) + int row_13 = __UHADD8(row_bgbg_1, row_bgbg_3); + int row_2g = __UHADD8(row_grgr_2, __PKHBT(row_grgr_2, row_grgr_2, 16)); + + int r_pixels_1 = __UXTB16(__UHADD8(row_grgr_2, __PKHTB(row_grgr_2, row_grgr_2, 16))); + int g_pixels_1 = __UXTB16_RORn(__UHADD8(row_2g, __PKHBT(row_2g, row_13, 8)), 8); + int b_pixels_1 = __UXTB16_RORn(__UHADD8(row_13, __PKHBT(row_13, row_13, 16)), 8); + #else + + int r2 = (((row_grgr_2 >> 16) & 0xFF) + (row_grgr_2 & 0xFF)) >> 1; + int r_pixels_1 = (row_grgr_2 & 0xFF0000) | r2; + + int g1 = (row_bgbg_1 >> 16) & 0xFF; + int g2 = (((row_grgr_2 >> 24) & 0xFF) + ((row_grgr_2 >> 8) & 0xFF)) >> 1; + int g3 = (row_bgbg_3 >> 16) & 0xFF; + int g_pixels_1 = (((((g1 + g3) >> 1) + g2) >> 1) << 16) | ((row_grgr_2 >> 8) & 0xFF); + + int b1 = (((row_bgbg_1 >> 8) & 0xFF) + ((row_bgbg_3 >> 8) & 0xFF)) >> 1; + int b3 = (((row_bgbg_1 >> 24) & 0xFF) + ((row_bgbg_3 >> 24) & 0xFF)) >> 1; + int b_pixels_1 = (((b1 + b3) >> 1) << 16) | b1; + + #endif + + int y1 = ((r_pixels_1 * 38) + (g_pixels_1 * 75) + (b_pixels_1 * 15)) >> 7; + IMAGE_PUT_GRAYSCALE_PIXEL_FAST(dst_row_ptr, i, y1); + + if (x != w_limit) { + IMAGE_PUT_GRAYSCALE_PIXEL_FAST(dst_row_ptr, i + 1, y1 >> 16); + } + } + } +} + +// Does no bounds checking on the destination. +void imlib_debayer_image_to_grayscale(image_t *dst, image_t *src) +{ + int src_w = src->w, w_limit = src_w - 1, w_limit_m_1 = w_limit - 1; + int src_h = src->h, h_limit = src_h - 1, h_limit_m_1 = h_limit - 1; + + // If the image is an odd height this will go for the last loop and we drop the last row. + for (int y = 0; y < src_h; y += 2) { + uint8_t *row_ptr_e = IMAGE_COMPUTE_GRAYSCALE_PIXEL_ROW_PTR(dst, y); + uint8_t *row_ptr_o = IMAGE_COMPUTE_GRAYSCALE_PIXEL_ROW_PTR(dst, y + 1); + uint8_t *rowptr_grgr_0, *rowptr_bgbg_1, *rowptr_grgr_2, *rowptr_bgbg_3; + + // keep row pointers in bounds + if (y == 0) { + rowptr_bgbg_1 = src->data; + rowptr_grgr_2 = rowptr_bgbg_1 + ((src_h >= 2) ? src_w : 0); + rowptr_bgbg_3 = rowptr_bgbg_1 + ((src_h >= 3) ? (src_w * 2) : 0); + rowptr_grgr_0 = rowptr_grgr_2; + } else if (y == h_limit_m_1) { + rowptr_grgr_0 = src->data + ((y - 1) * src_w); + rowptr_bgbg_1 = rowptr_grgr_0 + src_w; + rowptr_grgr_2 = rowptr_bgbg_1 + src_w; + rowptr_bgbg_3 = rowptr_bgbg_1; + } else if (y >= h_limit) { + rowptr_grgr_0 = src->data + ((y - 1) * src_w); + rowptr_bgbg_1 = rowptr_grgr_0 + src_w; + rowptr_grgr_2 = rowptr_grgr_0; + rowptr_bgbg_3 = rowptr_bgbg_1; + } else { // get 4 neighboring rows + rowptr_grgr_0 = src->data + ((y - 1) * src_w); + rowptr_bgbg_1 = rowptr_grgr_0 + src_w; + rowptr_grgr_2 = rowptr_bgbg_1 + src_w; + rowptr_bgbg_3 = rowptr_grgr_2 + src_w; + } + + // If the image is an odd width this will go for the last loop and we drop the last column. + for (int x = 0; x < src_w; x += 2) { + uint32_t row_grgr_0, row_bgbg_1, row_grgr_2, row_bgbg_3; + + // keep pixels in bounds + if (x == 0) { + if (src_w >= 4) { + row_grgr_0 = *((uint32_t *) rowptr_grgr_0); + row_bgbg_1 = *((uint32_t *) rowptr_bgbg_1); + row_grgr_2 = *((uint32_t *) rowptr_grgr_2); + row_bgbg_3 = *((uint32_t *) rowptr_bgbg_3); + } else if (src_w >= 3) { + row_grgr_0 = *((uint16_t *) rowptr_grgr_0) | (*(rowptr_grgr_0 + 2) << 16); + row_bgbg_1 = *((uint16_t *) rowptr_bgbg_1) | (*(rowptr_bgbg_1 + 2) << 16); + row_grgr_2 = *((uint16_t *) rowptr_grgr_2) | (*(rowptr_grgr_2 + 2) << 16); + row_bgbg_3 = *((uint16_t *) rowptr_bgbg_3) | (*(rowptr_bgbg_3 + 2) << 16); + } else if (src_w >= 2) { + row_grgr_0 = *((uint16_t *) rowptr_grgr_0); + row_grgr_0 = (row_grgr_0 << 16) | row_grgr_0; + row_bgbg_1 = *((uint16_t *) rowptr_bgbg_1); + row_bgbg_1 = (row_bgbg_1 << 16) | row_bgbg_1; + row_grgr_2 = *((uint16_t *) rowptr_grgr_2); + row_grgr_2 = (row_grgr_2 << 16) | row_grgr_2; + row_bgbg_3 = *((uint16_t *) rowptr_bgbg_3); + row_bgbg_3 = (row_bgbg_3 << 16) | row_bgbg_3; + } else { + row_grgr_0 = *(rowptr_grgr_0) * 0x01010101; + row_bgbg_1 = *(rowptr_bgbg_1) * 0x01010101; + row_grgr_2 = *(rowptr_grgr_2) * 0x01010101; + row_bgbg_3 = *(rowptr_bgbg_3) * 0x01010101; + } + // The starting point needs to be offset by 1. The below patterns are actually + // rgrg, gbgb, rgrg, and gbgb. So, shift left and backfill the missing border pixel. + row_grgr_0 = (row_grgr_0 << 8) | __UXTB_RORn(row_grgr_0, 8); + row_bgbg_1 = (row_bgbg_1 << 8) | __UXTB_RORn(row_bgbg_1, 8); + row_grgr_2 = (row_grgr_2 << 8) | __UXTB_RORn(row_grgr_2, 8); + row_bgbg_3 = (row_bgbg_3 << 8) | __UXTB_RORn(row_bgbg_3, 8); + } else if (x == w_limit_m_1) { + row_grgr_0 = *((uint32_t *) (rowptr_grgr_0 + x - 2)); + row_grgr_0 = (row_grgr_0 >> 8) | ((row_grgr_0 << 8) & 0xff000000); + row_bgbg_1 = *((uint32_t *) (rowptr_bgbg_1 + x - 2)); + row_bgbg_1 = (row_bgbg_1 >> 8) | ((row_bgbg_1 << 8) & 0xff000000); + row_grgr_2 = *((uint32_t *) (rowptr_grgr_2 + x - 2)); + row_grgr_2 = (row_grgr_2 >> 8) | ((row_grgr_2 << 8) & 0xff000000); + row_bgbg_3 = *((uint32_t *) (rowptr_bgbg_3 + x - 2)); + row_bgbg_3 = (row_bgbg_3 >> 8) | ((row_bgbg_1 << 8) & 0xff000000); + } else if (x >= w_limit) { + row_grgr_0 = *((uint16_t *) (rowptr_grgr_0 + x - 1)); + row_grgr_0 = (row_grgr_0 << 16) | row_grgr_0; + row_bgbg_1 = *((uint16_t *) (rowptr_bgbg_1 + x - 1)); + row_bgbg_1 = (row_bgbg_1 << 16) | row_bgbg_1; + row_grgr_2 = *((uint16_t *) (rowptr_grgr_2 + x - 1)); + row_grgr_2 = (row_grgr_2 << 16) | row_grgr_2; + row_bgbg_3 = *((uint16_t *) (rowptr_bgbg_3 + x - 1)); + row_bgbg_3 = (row_bgbg_3 << 16) | row_bgbg_3; + } else { // get 4 neighboring rows + row_grgr_0 = *((uint32_t *) (rowptr_grgr_0 + x - 1)); + row_bgbg_1 = *((uint32_t *) (rowptr_bgbg_1 + x - 1)); + row_grgr_2 = *((uint32_t *) (rowptr_grgr_2 + x - 1)); + row_bgbg_3 = *((uint32_t *) (rowptr_bgbg_3 + x - 1)); + } + + #if defined(MCU_SERIES_F4) || defined(MCU_SERIES_F7) || defined(MCU_SERIES_H7) + int row_01 = __UHADD8(row_grgr_0, row_grgr_2); + int row_1g = __UHADD8(row_bgbg_1, __PKHTB(row_bgbg_1, row_bgbg_1, 16)); + + int r_pixels_0 = __UXTB16(__UHADD8(row_01, __PKHTB(row_01, row_01, 16))); + int g_pixels_0 = __UXTB16(__UHADD8(row_1g, __PKHTB(row_1g, row_01, 8))); + int b_pixels_0 = __UXTB16_RORn(__UHADD8(row_bgbg_1, __PKHBT(row_bgbg_1, row_bgbg_1, 16)), 8); + #else + + int r0 = ((row_grgr_0 & 0xFF) + (row_grgr_2 & 0xFF)) >> 1; + int r2 = (((row_grgr_0 >> 16) & 0xFF) + ((row_grgr_2 >> 16) & 0xFF)) >> 1; + int r_pixels_0 = (r2 << 16) | ((r0 + r2) >> 1); + + int g0 = (row_grgr_0 >> 8) & 0xFF; + int g1 = (((row_bgbg_1 >> 16) & 0xFF) + (row_bgbg_1 & 0xFF)) >> 1; + int g2 = (row_grgr_2 >> 8) & 0xFF; + int g_pixels_0 = (row_bgbg_1 & 0xFF0000) | ((((g0 + g2) >> 1) + g1) >> 1); + + int b1 = (((row_bgbg_1 >> 24) & 0xFF) + ((row_bgbg_1 >> 8) & 0xFF)) >> 1; + int b_pixels_0 = (b1 << 16) | ((row_bgbg_1 >> 8) & 0xFF); + + #endif + + int y0 = ((r_pixels_0 * 38) + (g_pixels_0 * 75) + (b_pixels_0 * 15)) >> 7; + IMAGE_PUT_GRAYSCALE_PIXEL_FAST(row_ptr_e, x, y0); + + if (x != w_limit) { + IMAGE_PUT_GRAYSCALE_PIXEL_FAST(row_ptr_e, x + 1, y0 >> 16); + } + + if (y == h_limit) { + break; + } + + #if defined(MCU_SERIES_F4) || defined(MCU_SERIES_F7) || defined(MCU_SERIES_H7) + int row_13 = __UHADD8(row_bgbg_1, row_bgbg_3); + int row_2g = __UHADD8(row_grgr_2, __PKHBT(row_grgr_2, row_grgr_2, 16)); + + int r_pixels_1 = __UXTB16(__UHADD8(row_grgr_2, __PKHTB(row_grgr_2, row_grgr_2, 16))); + int g_pixels_1 = __UXTB16_RORn(__UHADD8(row_2g, __PKHBT(row_2g, row_13, 8)), 8); + int b_pixels_1 = __UXTB16_RORn(__UHADD8(row_13, __PKHBT(row_13, row_13, 16)), 8); + #else + + r2 = (((row_grgr_2 >> 16) & 0xFF) + (row_grgr_2 & 0xFF)) >> 1; + int r_pixels_1 = (row_grgr_2 & 0xFF0000) | r2; + + g1 = (row_bgbg_1 >> 16) & 0xFF; + g2 = (((row_grgr_2 >> 24) & 0xFF) + ((row_grgr_2 >> 8) & 0xFF)) >> 1; + int g3 = (row_bgbg_3 >> 16) & 0xFF; + int g_pixels_1 = (((((g1 + g3) >> 1) + g2) >> 1) << 16) | ((row_grgr_2 >> 8) & 0xFF); + + b1 = (((row_bgbg_1 >> 8) & 0xFF) + ((row_bgbg_3 >> 8) & 0xFF)) >> 1; + int b3 = (((row_bgbg_1 >> 24) & 0xFF) + ((row_bgbg_3 >> 24) & 0xFF)) >> 1; + int b_pixels_1 = (((b1 + b3) >> 1) << 16) | b1; + + #endif + + int y1 = ((r_pixels_1 * 38) + (g_pixels_1 * 75) + (b_pixels_1 * 15)) >> 7; + IMAGE_PUT_GRAYSCALE_PIXEL_FAST(row_ptr_o, x, y1); + + if (x != w_limit) { + IMAGE_PUT_GRAYSCALE_PIXEL_FAST(row_ptr_o, x + 1, y1 >> 16); + } + } + } +} + +void imlib_debayer_line_to_rgb565(int x_start, int x_end, int y_row, uint16_t *dst_row_ptr, image_t *src) +{ + int src_w = src->w, w_limit = src_w - 1, w_limit_m_1 = w_limit - 1; + int src_h = src->h, h_limit = src_h - 1, h_limit_m_1 = h_limit - 1; + + int y_row_odd = y_row & 1; + int y = (y_row / 2) * 2; + uint8_t *rowptr_grgr_0, *rowptr_bgbg_1, *rowptr_grgr_2, *rowptr_bgbg_3; + + // keep row pointers in bounds + if (y == 0) { + rowptr_bgbg_1 = src->data; + rowptr_grgr_2 = rowptr_bgbg_1 + ((src_h >= 2) ? src_w : 0); + rowptr_bgbg_3 = rowptr_bgbg_1 + ((src_h >= 3) ? (src_w * 2) : 0); + rowptr_grgr_0 = rowptr_grgr_2; + } else if (y == h_limit_m_1) { + rowptr_grgr_0 = src->data + ((y - 1) * src_w); + rowptr_bgbg_1 = rowptr_grgr_0 + src_w; + rowptr_grgr_2 = rowptr_bgbg_1 + src_w; + rowptr_bgbg_3 = rowptr_bgbg_1; + } else if (y >= h_limit) { + rowptr_grgr_0 = src->data + ((y - 1) * src_w); + rowptr_bgbg_1 = rowptr_grgr_0 + src_w; + rowptr_grgr_2 = rowptr_grgr_0; + rowptr_bgbg_3 = rowptr_bgbg_1; + } else { // get 4 neighboring rows + rowptr_grgr_0 = src->data + ((y - 1) * src_w); + rowptr_bgbg_1 = rowptr_grgr_0 + src_w; + rowptr_grgr_2 = rowptr_bgbg_1 + src_w; + rowptr_bgbg_3 = rowptr_grgr_2 + src_w; + } + + // If the image is an odd width this will go for the last loop and we drop the last column. + if (!y_row_odd) { // even + for (int x = x_start, i = 0; x < x_end; x += 2, i += 2) { + uint32_t row_grgr_0, row_bgbg_1, row_grgr_2; + + // keep pixels in bounds + if (x == 0) { + if (src_w >= 4) { + row_grgr_0 = *((uint32_t *) rowptr_grgr_0); + row_bgbg_1 = *((uint32_t *) rowptr_bgbg_1); + row_grgr_2 = *((uint32_t *) rowptr_grgr_2); + } else if (src_w >= 3) { + row_grgr_0 = *((uint16_t *) rowptr_grgr_0) | (*(rowptr_grgr_0 + 2) << 16); + row_bgbg_1 = *((uint16_t *) rowptr_bgbg_1) | (*(rowptr_bgbg_1 + 2) << 16); + row_grgr_2 = *((uint16_t *) rowptr_grgr_2) | (*(rowptr_grgr_2 + 2) << 16); + } else if (src_w >= 2) { + row_grgr_0 = *((uint16_t *) rowptr_grgr_0); + row_grgr_0 = (row_grgr_0 << 16) | row_grgr_0; + row_bgbg_1 = *((uint16_t *) rowptr_bgbg_1); + row_bgbg_1 = (row_bgbg_1 << 16) | row_bgbg_1; + row_grgr_2 = *((uint16_t *) rowptr_grgr_2); + row_grgr_2 = (row_grgr_2 << 16) | row_grgr_2; + } else { + row_grgr_0 = *(rowptr_grgr_0) * 0x01010101; + row_bgbg_1 = *(rowptr_bgbg_1) * 0x01010101; + row_grgr_2 = *(rowptr_grgr_2) * 0x01010101; + } + // The starting point needs to be offset by 1. The below patterns are actually + // rgrg, gbgb, rgrg, and gbgb. So, shift left and backfill the missing border pixel. + row_grgr_0 = (row_grgr_0 << 8) | __UXTB_RORn(row_grgr_0, 8); + row_bgbg_1 = (row_bgbg_1 << 8) | __UXTB_RORn(row_bgbg_1, 8); + row_grgr_2 = (row_grgr_2 << 8) | __UXTB_RORn(row_grgr_2, 8); + } else if (x == w_limit_m_1) { + row_grgr_0 = *((uint32_t *) (rowptr_grgr_0 + x - 2)); + row_grgr_0 = (row_grgr_0 >> 8) | ((row_grgr_0 << 8) & 0xff000000); + row_bgbg_1 = *((uint32_t *) (rowptr_bgbg_1 + x - 2)); + row_bgbg_1 = (row_bgbg_1 >> 8) | ((row_bgbg_1 << 8) & 0xff000000); + row_grgr_2 = *((uint32_t *) (rowptr_grgr_2 + x - 2)); + row_grgr_2 = (row_grgr_2 >> 8) | ((row_grgr_2 << 8) & 0xff000000); + } else if (x >= w_limit) { + row_grgr_0 = *((uint16_t *) (rowptr_grgr_0 + x - 1)); + row_grgr_0 = (row_grgr_0 << 16) | row_grgr_0; + row_bgbg_1 = *((uint16_t *) (rowptr_bgbg_1 + x - 1)); + row_bgbg_1 = (row_bgbg_1 << 16) | row_bgbg_1; + row_grgr_2 = *((uint16_t *) (rowptr_grgr_2 + x - 1)); + row_grgr_2 = (row_grgr_2 << 16) | row_grgr_2; + } else { // get 4 neighboring rows + row_grgr_0 = *((uint32_t *) (rowptr_grgr_0 + x - 1)); + row_bgbg_1 = *((uint32_t *) (rowptr_bgbg_1 + x - 1)); + row_grgr_2 = *((uint32_t *) (rowptr_grgr_2 + x - 1)); + } + + #if defined(MCU_SERIES_F4) || defined(MCU_SERIES_F7) || defined(MCU_SERIES_H7) + int row_01 = __UHADD8(row_grgr_0, row_grgr_2); + int row_1g = __UHADD8(row_bgbg_1, __PKHTB(row_bgbg_1, row_bgbg_1, 16)); + + int r_pixels_0 = __UXTB16(__UHADD8(row_01, __PKHTB(row_01, row_01, 16))); + int g_pixels_0 = __UXTB16(__UHADD8(row_1g, __PKHTB(row_1g, row_01, 8))); + int b_pixels_0 = __UXTB16_RORn(__UHADD8(row_bgbg_1, __PKHBT(row_bgbg_1, row_bgbg_1, 16)), 8); + #else + + int r0 = ((row_grgr_0 & 0xFF) + (row_grgr_2 & 0xFF)) >> 1; + int r2 = (((row_grgr_0 >> 16) & 0xFF) + ((row_grgr_2 >> 16) & 0xFF)) >> 1; + int r_pixels_0 = (r2 << 16) | ((r0 + r2) >> 1); + + int g0 = (row_grgr_0 >> 8) & 0xFF; + int g1 = (((row_bgbg_1 >> 16) & 0xFF) + (row_bgbg_1 & 0xFF)) >> 1; + int g2 = (row_grgr_2 >> 8) & 0xFF; + int g_pixels_0 = (row_bgbg_1 & 0xFF0000) | ((((g0 + g2) >> 1) + g1) >> 1); + + int b1 = (((row_bgbg_1 >> 24) & 0xFF) + ((row_bgbg_1 >> 8) & 0xFF)) >> 1; + int b_pixels_0 = (b1 << 16) | ((row_bgbg_1 >> 8) & 0xFF); + + #endif + + int rgb565_0 = ((r_pixels_0 << 8) & 0xf800f800) | + ((g_pixels_0 << 3) & 0x07e007e0) | + ((b_pixels_0 >> 3) & 0x001f001f); + + if (x == w_limit) { // just put bottom + IMAGE_PUT_RGB565_PIXEL_FAST(dst_row_ptr, i, rgb565_0); + } else { // put both + *((uint32_t *) (dst_row_ptr + i)) = rgb565_0; + } + } + } else { // odd + for (int x = x_start, i = 0; x < x_end; x += 2, i += 2) { + uint32_t row_bgbg_1, row_grgr_2, row_bgbg_3; + + // keep pixels in bounds + if (x == 0) { + if (src_w >= 4) { + row_bgbg_1 = *((uint32_t *) rowptr_bgbg_1); + row_grgr_2 = *((uint32_t *) rowptr_grgr_2); + row_bgbg_3 = *((uint32_t *) rowptr_bgbg_3); + } else if (src_w >= 3) { + row_bgbg_1 = *((uint16_t *) rowptr_bgbg_1) | (*(rowptr_bgbg_1 + 2) << 16); + row_grgr_2 = *((uint16_t *) rowptr_grgr_2) | (*(rowptr_grgr_2 + 2) << 16); + row_bgbg_3 = *((uint16_t *) rowptr_bgbg_3) | (*(rowptr_bgbg_3 + 2) << 16); + } else if (src_w >= 2) { + row_bgbg_1 = *((uint16_t *) rowptr_bgbg_1); + row_bgbg_1 = (row_bgbg_1 << 16) | row_bgbg_1; + row_grgr_2 = *((uint16_t *) rowptr_grgr_2); + row_grgr_2 = (row_grgr_2 << 16) | row_grgr_2; + row_bgbg_3 = *((uint16_t *) rowptr_bgbg_3); + row_bgbg_3 = (row_bgbg_3 << 16) | row_bgbg_3; + } else { + row_bgbg_1 = *(rowptr_bgbg_1) * 0x01010101; + row_grgr_2 = *(rowptr_grgr_2) * 0x01010101; + row_bgbg_3 = *(rowptr_bgbg_3) * 0x01010101; + } + // The starting point needs to be offset by 1. The below patterns are actually + // rgrg, gbgb, rgrg, and gbgb. So, shift left and backfill the missing border pixel. + row_bgbg_1 = (row_bgbg_1 << 8) | __UXTB_RORn(row_bgbg_1, 8); + row_grgr_2 = (row_grgr_2 << 8) | __UXTB_RORn(row_grgr_2, 8); + row_bgbg_3 = (row_bgbg_3 << 8) | __UXTB_RORn(row_bgbg_3, 8); + } else if (x == w_limit_m_1) { + row_bgbg_1 = *((uint32_t *) (rowptr_bgbg_1 + x - 2)); + row_bgbg_1 = (row_bgbg_1 >> 8) | ((row_bgbg_1 << 8) & 0xff000000); + row_grgr_2 = *((uint32_t *) (rowptr_grgr_2 + x - 2)); + row_grgr_2 = (row_grgr_2 >> 8) | ((row_grgr_2 << 8) & 0xff000000); + row_bgbg_3 = *((uint32_t *) (rowptr_bgbg_3 + x - 2)); + row_bgbg_3 = (row_bgbg_3 >> 8) | ((row_bgbg_1 << 8) & 0xff000000); + } else if (x >= w_limit) { + row_bgbg_1 = *((uint16_t *) (rowptr_bgbg_1 + x - 1)); + row_bgbg_1 = (row_bgbg_1 << 16) | row_bgbg_1; + row_grgr_2 = *((uint16_t *) (rowptr_grgr_2 + x - 1)); + row_grgr_2 = (row_grgr_2 << 16) | row_grgr_2; + row_bgbg_3 = *((uint16_t *) (rowptr_bgbg_3 + x - 1)); + row_bgbg_3 = (row_bgbg_3 << 16) | row_bgbg_3; + } else { // get 4 neighboring rows + row_bgbg_1 = *((uint32_t *) (rowptr_bgbg_1 + x - 1)); + row_grgr_2 = *((uint32_t *) (rowptr_grgr_2 + x - 1)); + row_bgbg_3 = *((uint32_t *) (rowptr_bgbg_3 + x - 1)); + } + + #if defined(MCU_SERIES_F4) || defined(MCU_SERIES_F7) || defined(MCU_SERIES_H7) + int row_13 = __UHADD8(row_bgbg_1, row_bgbg_3); + int row_2g = __UHADD8(row_grgr_2, __PKHBT(row_grgr_2, row_grgr_2, 16)); + + int r_pixels_1 = __UXTB16(__UHADD8(row_grgr_2, __PKHTB(row_grgr_2, row_grgr_2, 16))); + int g_pixels_1 = __UXTB16_RORn(__UHADD8(row_2g, __PKHBT(row_2g, row_13, 8)), 8); + int b_pixels_1 = __UXTB16_RORn(__UHADD8(row_13, __PKHBT(row_13, row_13, 16)), 8); + #else + + int r2 = (((row_grgr_2 >> 16) & 0xFF) + (row_grgr_2 & 0xFF)) >> 1; + int r_pixels_1 = (row_grgr_2 & 0xFF0000) | r2; + + int g1 = (row_bgbg_1 >> 16) & 0xFF; + int g2 = (((row_grgr_2 >> 24) & 0xFF) + ((row_grgr_2 >> 8) & 0xFF)) >> 1; + int g3 = (row_bgbg_3 >> 16) & 0xFF; + int g_pixels_1 = (((((g1 + g3) >> 1) + g2) >> 1) << 16) | ((row_grgr_2 >> 8) & 0xFF); + + int b1 = (((row_bgbg_1 >> 8) & 0xFF) + ((row_bgbg_3 >> 8) & 0xFF)) >> 1; + int b3 = (((row_bgbg_1 >> 24) & 0xFF) + ((row_bgbg_3 >> 24) & 0xFF)) >> 1; + int b_pixels_1 = (((b1 + b3) >> 1) << 16) | b1; + + #endif + + int rgb565_1 = ((r_pixels_1 << 8) & 0xf800f800) | + ((g_pixels_1 << 3) & 0x07e007e0) | + ((b_pixels_1 >> 3) & 0x001f001f); + + if (x == w_limit) { // just put bottom + IMAGE_PUT_RGB565_PIXEL_FAST(dst_row_ptr, i, rgb565_1); + } else { // put both + *((uint32_t *) (dst_row_ptr + i)) = rgb565_1; + } + } + } +} + +// Does no bounds checking on the destination. +void imlib_debayer_image_to_rgb565(image_t *dst, image_t *src) +{ + int src_w = src->w, w_limit = src_w - 1, w_limit_m_1 = w_limit - 1; + int src_h = src->h, h_limit = src_h - 1, h_limit_m_1 = h_limit - 1; + + // If the image is an odd height this will go for the last loop and we drop the last row. + for (int y = 0; y < src_h; y += 2) { + uint16_t *row_ptr_e = IMAGE_COMPUTE_RGB565_PIXEL_ROW_PTR(dst, y); + uint16_t *row_ptr_o = IMAGE_COMPUTE_RGB565_PIXEL_ROW_PTR(dst, y + 1); + uint8_t *rowptr_grgr_0, *rowptr_bgbg_1, *rowptr_grgr_2, *rowptr_bgbg_3; + + // keep row pointers in bounds + if (y == 0) { + rowptr_bgbg_1 = src->data; + rowptr_grgr_2 = rowptr_bgbg_1 + ((src_h >= 2) ? src_w : 0); + rowptr_bgbg_3 = rowptr_bgbg_1 + ((src_h >= 3) ? (src_w * 2) : 0); + rowptr_grgr_0 = rowptr_grgr_2; + } else if (y == h_limit_m_1) { + rowptr_grgr_0 = src->data + ((y - 1) * src_w); + rowptr_bgbg_1 = rowptr_grgr_0 + src_w; + rowptr_grgr_2 = rowptr_bgbg_1 + src_w; + rowptr_bgbg_3 = rowptr_bgbg_1; + } else if (y >= h_limit) { + rowptr_grgr_0 = src->data + ((y - 1) * src_w); + rowptr_bgbg_1 = rowptr_grgr_0 + src_w; + rowptr_grgr_2 = rowptr_grgr_0; + rowptr_bgbg_3 = rowptr_bgbg_1; + } else { // get 4 neighboring rows + rowptr_grgr_0 = src->data + ((y - 1) * src_w); + rowptr_bgbg_1 = rowptr_grgr_0 + src_w; + rowptr_grgr_2 = rowptr_bgbg_1 + src_w; + rowptr_bgbg_3 = rowptr_grgr_2 + src_w; + } + + // If the image is an odd width this will go for the last loop and we drop the last column. + for (int x = 0; x < src_w; x += 2) { + uint32_t row_grgr_0, row_bgbg_1, row_grgr_2, row_bgbg_3; + + // keep pixels in bounds + if (x == 0) { + if (src_w >= 4) { + row_grgr_0 = *((uint32_t *) rowptr_grgr_0); + row_bgbg_1 = *((uint32_t *) rowptr_bgbg_1); + row_grgr_2 = *((uint32_t *) rowptr_grgr_2); + row_bgbg_3 = *((uint32_t *) rowptr_bgbg_3); + } else if (src_w >= 3) { + row_grgr_0 = *((uint16_t *) rowptr_grgr_0) | (*(rowptr_grgr_0 + 2) << 16); + row_bgbg_1 = *((uint16_t *) rowptr_bgbg_1) | (*(rowptr_bgbg_1 + 2) << 16); + row_grgr_2 = *((uint16_t *) rowptr_grgr_2) | (*(rowptr_grgr_2 + 2) << 16); + row_bgbg_3 = *((uint16_t *) rowptr_bgbg_3) | (*(rowptr_bgbg_3 + 2) << 16); + } else if (src_w >= 2) { + row_grgr_0 = *((uint16_t *) rowptr_grgr_0); + row_grgr_0 = (row_grgr_0 << 16) | row_grgr_0; + row_bgbg_1 = *((uint16_t *) rowptr_bgbg_1); + row_bgbg_1 = (row_bgbg_1 << 16) | row_bgbg_1; + row_grgr_2 = *((uint16_t *) rowptr_grgr_2); + row_grgr_2 = (row_grgr_2 << 16) | row_grgr_2; + row_bgbg_3 = *((uint16_t *) rowptr_bgbg_3); + row_bgbg_3 = (row_bgbg_3 << 16) | row_bgbg_3; + } else { + row_grgr_0 = *(rowptr_grgr_0) * 0x01010101; + row_bgbg_1 = *(rowptr_bgbg_1) * 0x01010101; + row_grgr_2 = *(rowptr_grgr_2) * 0x01010101; + row_bgbg_3 = *(rowptr_bgbg_3) * 0x01010101; + } + // The starting point needs to be offset by 1. The below patterns are actually + // rgrg, gbgb, rgrg, and gbgb. So, shift left and backfill the missing border pixel. + row_grgr_0 = (row_grgr_0 << 8) | __UXTB_RORn(row_grgr_0, 8); + row_bgbg_1 = (row_bgbg_1 << 8) | __UXTB_RORn(row_bgbg_1, 8); + row_grgr_2 = (row_grgr_2 << 8) | __UXTB_RORn(row_grgr_2, 8); + row_bgbg_3 = (row_bgbg_3 << 8) | __UXTB_RORn(row_bgbg_3, 8); + } else if (x == w_limit_m_1) { + row_grgr_0 = *((uint32_t *) (rowptr_grgr_0 + x - 2)); + row_grgr_0 = (row_grgr_0 >> 8) | ((row_grgr_0 << 8) & 0xff000000); + row_bgbg_1 = *((uint32_t *) (rowptr_bgbg_1 + x - 2)); + row_bgbg_1 = (row_bgbg_1 >> 8) | ((row_bgbg_1 << 8) & 0xff000000); + row_grgr_2 = *((uint32_t *) (rowptr_grgr_2 + x - 2)); + row_grgr_2 = (row_grgr_2 >> 8) | ((row_grgr_2 << 8) & 0xff000000); + row_bgbg_3 = *((uint32_t *) (rowptr_bgbg_3 + x - 2)); + row_bgbg_3 = (row_bgbg_3 >> 8) | ((row_bgbg_1 << 8) & 0xff000000); + } else if (x >= w_limit) { + row_grgr_0 = *((uint16_t *) (rowptr_grgr_0 + x - 1)); + row_grgr_0 = (row_grgr_0 << 16) | row_grgr_0; + row_bgbg_1 = *((uint16_t *) (rowptr_bgbg_1 + x - 1)); + row_bgbg_1 = (row_bgbg_1 << 16) | row_bgbg_1; + row_grgr_2 = *((uint16_t *) (rowptr_grgr_2 + x - 1)); + row_grgr_2 = (row_grgr_2 << 16) | row_grgr_2; + row_bgbg_3 = *((uint16_t *) (rowptr_bgbg_3 + x - 1)); + row_bgbg_3 = (row_bgbg_3 << 16) | row_bgbg_3; + } else { // get 4 neighboring rows + row_grgr_0 = *((uint32_t *) (rowptr_grgr_0 + x - 1)); + row_bgbg_1 = *((uint32_t *) (rowptr_bgbg_1 + x - 1)); + row_grgr_2 = *((uint32_t *) (rowptr_grgr_2 + x - 1)); + row_bgbg_3 = *((uint32_t *) (rowptr_bgbg_3 + x - 1)); + } + + #if defined(MCU_SERIES_F4) || defined(MCU_SERIES_F7) || defined(MCU_SERIES_H7) + int row_01 = __UHADD8(row_grgr_0, row_grgr_2); + int row_1g = __UHADD8(row_bgbg_1, __PKHTB(row_bgbg_1, row_bgbg_1, 16)); + + int r_pixels_0 = __UXTB16(__UHADD8(row_01, __PKHTB(row_01, row_01, 16))); + int g_pixels_0 = __UXTB16(__UHADD8(row_1g, __PKHTB(row_1g, row_01, 8))); + int b_pixels_0 = __UXTB16_RORn(__UHADD8(row_bgbg_1, __PKHBT(row_bgbg_1, row_bgbg_1, 16)), 8); + #else + + int r0 = ((row_grgr_0 & 0xFF) + (row_grgr_2 & 0xFF)) >> 1; + int r2 = (((row_grgr_0 >> 16) & 0xFF) + ((row_grgr_2 >> 16) & 0xFF)) >> 1; + int r_pixels_0 = (r2 << 16) | ((r0 + r2) >> 1); + + int g0 = (row_grgr_0 >> 8) & 0xFF; + int g1 = (((row_bgbg_1 >> 16) & 0xFF) + (row_bgbg_1 & 0xFF)) >> 1; + int g2 = (row_grgr_2 >> 8) & 0xFF; + int g_pixels_0 = (row_bgbg_1 & 0xFF0000) | ((((g0 + g2) >> 1) + g1) >> 1); + + int b1 = (((row_bgbg_1 >> 24) & 0xFF) + ((row_bgbg_1 >> 8) & 0xFF)) >> 1; + int b_pixels_0 = (b1 << 16) | ((row_bgbg_1 >> 8) & 0xFF); + + #endif + + int rgb565_0 = ((r_pixels_0 << 8) & 0xf800f800) | + ((g_pixels_0 << 3) & 0x07e007e0) | + ((b_pixels_0 >> 3) & 0x001f001f); + + if (x == w_limit) { // just put bottom + IMAGE_PUT_RGB565_PIXEL_FAST(row_ptr_e, x, rgb565_0); + } else { // put both + *((uint32_t *) (row_ptr_e + x)) = rgb565_0; + } + + if (y == h_limit) { + break; + } + + #if defined(MCU_SERIES_F4) || defined(MCU_SERIES_F7) || defined(MCU_SERIES_H7) + int row_13 = __UHADD8(row_bgbg_1, row_bgbg_3); + int row_2g = __UHADD8(row_grgr_2, __PKHBT(row_grgr_2, row_grgr_2, 16)); + + int r_pixels_1 = __UXTB16(__UHADD8(row_grgr_2, __PKHTB(row_grgr_2, row_grgr_2, 16))); + int g_pixels_1 = __UXTB16_RORn(__UHADD8(row_2g, __PKHBT(row_2g, row_13, 8)), 8); + int b_pixels_1 = __UXTB16_RORn(__UHADD8(row_13, __PKHBT(row_13, row_13, 16)), 8); + #else + + r2 = (((row_grgr_2 >> 16) & 0xFF) + (row_grgr_2 & 0xFF)) >> 1; + int r_pixels_1 = (row_grgr_2 & 0xFF0000) | r2; + + g1 = (row_bgbg_1 >> 16) & 0xFF; + g2 = (((row_grgr_2 >> 24) & 0xFF) + ((row_grgr_2 >> 8) & 0xFF)) >> 1; + int g3 = (row_bgbg_3 >> 16) & 0xFF; + int g_pixels_1 = (((((g1 + g3) >> 1) + g2) >> 1) << 16) | ((row_grgr_2 >> 8) & 0xFF); + + b1 = (((row_bgbg_1 >> 8) & 0xFF) + ((row_bgbg_3 >> 8) & 0xFF)) >> 1; + int b3 = (((row_bgbg_1 >> 24) & 0xFF) + ((row_bgbg_3 >> 24) & 0xFF)) >> 1; + int b_pixels_1 = (((b1 + b3) >> 1) << 16) | b1; + + #endif + + int rgb565_1 = ((r_pixels_1 << 8) & 0xf800f800) | + ((g_pixels_1 << 3) & 0x07e007e0) | + ((b_pixels_1 >> 3) & 0x001f001f); + + if (x == w_limit) { // just put bottom + IMAGE_PUT_RGB565_PIXEL_FAST(row_ptr_o, x, rgb565_1); + } else { // put both + *((uint32_t *) (row_ptr_o + x)) = rgb565_1; + } + } + } +} diff --git a/src/omv/imlib/draw.c b/src/omv/imlib/draw.c index ffac0023d..78c69af91 100644 --- a/src/omv/imlib/draw.c +++ b/src/omv/imlib/draw.c @@ -2434,6 +2434,15 @@ void imlib_draw_row(int x_start, int x_end, int y_row, imlib_draw_row_data_t *da } break; } + // Only bayer copying/cropping is supported. + case IMAGE_BPP_BAYER: { + uint8_t *dst8 = (data->dst_row_override + ? ((uint8_t *) data->dst_row_override) + : IMAGE_COMPUTE_GRAYSCALE_PIXEL_ROW_PTR(data->dst_img, y_row)) + x_start; + uint8_t *src8 = ((uint8_t *) data->row_buffer[!data->toggle]) + x_start; + unaligned_memcpy(dst8, src8, (x_end - x_start) * sizeof(uint8_t)); + break; + } default: { break; } @@ -2655,10 +2664,13 @@ void imlib_draw_image(image_t *dst_img, image_t *src_img, int dst_x_start, int d src_y_accum_reset -= 0x8000; } + bool is_bayer = src_img->bpp == IMAGE_BPP_BAYER; + bool is_color = is_bayer || (src_img->bpp == IMAGE_BPP_RGB565); + // rgb_channel extracted / color_palette applied image image_t new_src_img; - if (((hint & IMAGE_HINT_EXTRACT_RGB_CHANNEL_FIRST) && (src_img->bpp == IMAGE_BPP_RGB565) && (rgb_channel != -1)) + if (((hint & IMAGE_HINT_EXTRACT_RGB_CHANNEL_FIRST) && is_color && (rgb_channel != -1)) || ((hint & IMAGE_HINT_APPLY_COLOR_PALETTE_FIRST) && color_palette)) { new_src_img.w = src_img_w; // same width as source image new_src_img.h = src_img_h; // same height as source image @@ -2670,6 +2682,9 @@ void imlib_draw_image(image_t *dst_img, image_t *src_img, int dst_x_start, int d color_palette = NULL; } + // Best bpp to convert bayer image to. + int is_bayer_bpp = (rgb_channel != -1) ? IMAGE_BPP_RGB565 : (color_palette ? IMAGE_BPP_GRAYSCALE : dst_img->bpp); + bool no_scaling_nearest_neighbor = (dst_delta_x == 1) && (dst_x_start == 0) && (src_x_start == 0) && (src_x_frac == 65536) && (src_y_frac == 65536); @@ -2682,20 +2697,60 @@ void imlib_draw_image(image_t *dst_img, image_t *src_img, int dst_x_start, int d size_t src_img_row_bytes = image_size(src_img) / src_img->h; size_t dst_img_row_bytes = image_size(dst_img) / dst_img->h; - // In-place (by definition mutually exclusive to the above)... - if ((dst_img->data == src_img->data) && (is_scaling || (src_img_row_bytes < dst_img_row_bytes))) { - size_t size = image_size(src_img); + // Do we need to convert the image? + bool is_bayer_color_conversion = is_bayer && (dst_img->bpp != IMAGE_BPP_BAYER); + + // Force a deep copy if we cannot use the image in-place. + bool need_deep_copy = (dst_img->data == src_img->data) + && (is_scaling || (src_img_row_bytes < dst_img_row_bytes) || is_bayer_color_conversion); + + // Force a deep copy if we are scaling. + bool is_bayer_scaling = is_bayer_color_conversion && is_scaling; + + // Make a deep copy of the source iamge. + if (need_deep_copy || is_bayer_scaling) { new_src_img.w = src_img->w; // same width as source image new_src_img.h = src_img->h; // same height as source image - new_src_img.bpp = src_img->bpp; - new_src_img.data = fb_alloc(size, FB_ALLOC_NO_HINT); - memcpy(new_src_img.data, src_img->data, size); + + if (is_bayer) { + new_src_img.bpp = is_bayer_bpp; + size_t size = image_size(&new_src_img); + new_src_img.data = fb_alloc(image_size(&new_src_img), FB_ALLOC_NO_HINT); + + switch (new_src_img.bpp) { + case IMAGE_BPP_BINARY: { + imlib_debayer_image_to_binary(&new_src_img, src_img); + break; + } + case IMAGE_BPP_GRAYSCALE: { + imlib_debayer_image_to_grayscale(&new_src_img, src_img); + break; + } + case IMAGE_BPP_RGB565: { + imlib_debayer_image_to_rgb565(&new_src_img, src_img); + break; + } + case IMAGE_BPP_BAYER: { + memcpy(new_src_img.data, src_img->data, size); + break; + } + default : { + break; + } + } + } else { + new_src_img.bpp = src_img->bpp; + size_t size = image_size(&new_src_img); + new_src_img.data = fb_alloc(size, FB_ALLOC_NO_HINT); + memcpy(new_src_img.data, src_img->data, size); + } + src_img = &new_src_img; } imlib_draw_row_data_t imlib_draw_row_data; imlib_draw_row_data.dst_img = dst_img; - imlib_draw_row_data.src_img_bpp = src_img->bpp; + imlib_draw_row_data.src_img_bpp = is_bayer ? is_bayer_bpp : src_img->bpp; imlib_draw_row_data.rgb_channel = rgb_channel; imlib_draw_row_data.alpha = alpha; imlib_draw_row_data.color_palette = color_palette; @@ -4427,7 +4482,9 @@ void imlib_draw_image(image_t *dst_img, image_t *src_img, int dst_x_start, int d } // while y break; } - case IMAGE_BPP_GRAYSCALE: { + case IMAGE_BPP_GRAYSCALE: + // Re-use grayscale for bayer. + case IMAGE_BPP_BAYER: { while (y_not_done) { uint8_t *src_row_ptr = IMAGE_COMPUTE_GRAYSCALE_PIXEL_ROW_PTR(src_img, next_src_y_index); // Must be called per loop to get the address of the temp buffer to blend with @@ -4543,6 +4600,47 @@ void imlib_draw_image(image_t *dst_img, image_t *src_img, int dst_x_start, int d } // while y break; } + case IMAGE_BPP_BAYER: { + while (y_not_done) { + switch (is_bayer_bpp) { + case IMAGE_BPP_BINARY: { + uint32_t *dst_row_ptr = imlib_draw_row_get_row_buffer(&imlib_draw_row_data); + imlib_debayer_line_to_binary(dst_x_start, dst_x_end, next_src_y_index, + dst_row_ptr, src_img); + break; + } + case IMAGE_BPP_GRAYSCALE: { + uint8_t *dst_row_ptr = imlib_draw_row_get_row_buffer(&imlib_draw_row_data); + imlib_debayer_line_to_grayscale(dst_x_start, dst_x_end, next_src_y_index, + dst_row_ptr, src_img); + break; + } + case IMAGE_BPP_RGB565: { + uint16_t *dst_row_ptr = imlib_draw_row_get_row_buffer(&imlib_draw_row_data); + imlib_debayer_line_to_rgb565(dst_x_start, dst_x_end, next_src_y_index, + dst_row_ptr, src_img); + break; + } + case IMAGE_BPP_BAYER: { // Bayer images have the same shape as grayscale. + uint8_t *src_row_ptr = IMAGE_COMPUTE_GRAYSCALE_PIXEL_ROW_PTR(src_img, next_src_y_index); + imlib_draw_row_put_row_buffer(&imlib_draw_row_data, src_row_ptr); + break; + } + default : { + break; + } + } + + imlib_draw_row(dst_x_start, dst_x_end, dst_y, &imlib_draw_row_data); + + // Increment offsets + dst_y += dst_delta_y; + src_y_accum += src_y_frac; + next_src_y_index = src_y_accum >> 16; + y_not_done = ++y < dst_y_end; + } // while y + break; + } default: { break; } @@ -4592,7 +4690,9 @@ void imlib_draw_image(image_t *dst_img, image_t *src_img, int dst_x_start, int d } // while y break; } - case IMAGE_BPP_GRAYSCALE: { + case IMAGE_BPP_GRAYSCALE: + // Re-use grayscale for bayer. + case IMAGE_BPP_BAYER: { while (y_not_done) { int src_y_index = next_src_y_index; uint8_t *src_row_ptr = IMAGE_COMPUTE_GRAYSCALE_PIXEL_ROW_PTR(src_img, src_y_index); diff --git a/src/omv/imlib/imlib.c b/src/omv/imlib/imlib.c index 26d0d961b..fd6272c72 100644 --- a/src/omv/imlib/imlib.c +++ b/src/omv/imlib/imlib.c @@ -483,455 +483,6 @@ uint16_t imlib_yuv_to_rgb(uint8_t y, int8_t u, int8_t v) return COLOR_R8_G8_B8_TO_RGB565(r, g, b); } -void imlib_bayer_to_rgb565(image_t *img, int w, int h, int xoffs, int yoffs, uint16_t *rgbbuf) -{ - int r, g, b; - for (int y=yoffs; y= yoffs+h-1) { - for (int x=xoffs; x> 2; - - g = (IM_GET_RAW_PIXEL_CHECK_BOUNDS_Y(img, x, y-1) + - IM_GET_RAW_PIXEL_CHECK_BOUNDS_Y(img, x, y+1) + - IM_GET_RAW_PIXEL_CHECK_BOUNDS_X(img, x-1, y) + - IM_GET_RAW_PIXEL_CHECK_BOUNDS_X(img, x+1, y)) >> 2; - - b = IM_GET_RAW_PIXEL(img, x, y); - } else { // Odd col - r = (IM_GET_RAW_PIXEL_CHECK_BOUNDS_Y(img, x, y-1) + - IM_GET_RAW_PIXEL_CHECK_BOUNDS_Y(img, x, y+1)) >> 1; - - b = (IM_GET_RAW_PIXEL_CHECK_BOUNDS_X(img, x-1, y) + - IM_GET_RAW_PIXEL_CHECK_BOUNDS_X(img, x+1, y)) >> 1; - - g = IM_GET_RAW_PIXEL(img, x, y); - } - } else { // Odd row - if ((x % 2) == 0) { // Even Col - r = (IM_GET_RAW_PIXEL_CHECK_BOUNDS_X(img, x-1, y) + - IM_GET_RAW_PIXEL_CHECK_BOUNDS_X(img, x+1, y)) >> 1; - - g = IM_GET_RAW_PIXEL(img, x, y); - - b = (IM_GET_RAW_PIXEL_CHECK_BOUNDS_Y(img, x, y-1) + - IM_GET_RAW_PIXEL_CHECK_BOUNDS_Y(img, x, y+1)) >> 1; - } else { // Odd col - r = IM_GET_RAW_PIXEL(img, x, y); - - g = (IM_GET_RAW_PIXEL_CHECK_BOUNDS_Y(img, x, y-1) + - IM_GET_RAW_PIXEL_CHECK_BOUNDS_Y(img, x, y+1) + - IM_GET_RAW_PIXEL_CHECK_BOUNDS_X(img, x-1, y) + - IM_GET_RAW_PIXEL_CHECK_BOUNDS_X(img, x+1, y)) >> 2; - - b = (IM_GET_RAW_PIXEL_CHECK_BOUNDS_XY(img, x-1, y-1) + - IM_GET_RAW_PIXEL_CHECK_BOUNDS_XY(img, x+1, y-1) + - IM_GET_RAW_PIXEL_CHECK_BOUNDS_XY(img, x-1, y+1) + - IM_GET_RAW_PIXEL_CHECK_BOUNDS_XY(img, x+1, y+1)) >> 2; - } - - } - *rgbbuf++ = COLOR_R8_G8_B8_TO_RGB565(r, g, b); - } // for x - } else { // faster way -// -// Theory of operation: -// The Bayer pattern from the sensor looks like this: -// +---+---+---+---+---+---+ -// | B | G | B | G | B | G | -// +---+---+---+---+---+---+ -// | G |*R*|*G*| R | G | R | * = Example of current pair of pixels being processed -// +---+---+---+---+---+---+ Each iteration below will advance 2 pixels to the right -// | B | G | B | G | B | G | -// +---+---+---+---+---+---+ -// | G | R | G | R | G | R | -// +---+---+---+---+---+---+ -// Each of the color stimuli above is stored as 1 byte -// The slower algorithm above reads each byte around the current pixel individually to -// average the colors together to simulate the colors not present at the current pixel -// e.g. At location 0,0, only the blue value is present; red and green must be estimated from -// neighboring pixels -// -// The optimized algorithm below minimizes memory accesses by reading 2 bytes at a time -// and re-using the last pair as it progresses from left to right. Since the ARM CPU enforces a -// memory policy of generating an exception on unaligned reads, we read 16-bits at a time and -// OR them into a 32-bit variable to hold on to the pixels left and right of the current pair. -// This way we can work on 2 pixels at a time from 3 32-bit variables containing 3 lines of 4 pixels. -// The variables l0,l1,l2 hold the 4 pixels (left, current left, current_right, right) -// in lines above the current (l0), current (l1) and below (l2) -// This algorithm assumes that the starting x is 0 (it doesn't have to be). -// The starting and ending pixel are treated like absolute sensor edges, so they are less accurate -// than the middle pixels. -// - uint32_t l0,l1,l2; // 3 lines we're working with - uint16_t *s = (uint16_t *)&img->pixels[(y * img->w) + xoffs]; - uint32_t r, g, b, w2 = img->w/2; - l0 = s[-w2]; l1 = s[0]; l2 = s[w2]; - s++; -// For each pixel, the R8 G8 B8 values are directly shifted into R5 G6 B5 -// to save a step when creating the RGB565 output - if (y & 1) { // odd lines (G R G R G R) - b = ((l0 & 0xff) + (l2 & 0xff)) >> 4; // left edge pixel - g = (l1 & 0xff) >> 2; - r = (l1 & 0xff00) >> 11; - *rgbbuf++ = COLOR_R5_G6_B5_TO_RGB565(r, g, b); // first col is treated differently - for (int x = xoffs+1; x < xoffs+w-1; x+=2) { // 'middle' pixels can use neighbors from 3x3 grid - l0 |= (s[-w2] << 16); // shift last pixel pairs into upper 16 bits - l1 |= (s[0] << 16); // of the 3 line variables - l2 |= (s[w2] << 16); - s++; // advance source pointer one pair of pixels - r = (l1 & 0xff00) >> 11; // (1, 0) red pixel at current-left - g = (((l1 >> 16) & 0xff) + (l1 & 0xff) + ((l0 >> 8) & 0xff) + ((l2 >> 8) & 0xff)) >> 4; - b = ((l0 & 0xff) + (l2 & 0xff) + ((l0 >> 16) & 0xff) + ((l2 >> 16) & 0xff)) >> 5; - *rgbbuf++ = COLOR_R5_G6_B5_TO_RGB565(r, g, b); - g = (l1 & 0xff0000) >> 18; // (0,0) green pixel at current-right - b = (((l0 >> 16) & 0xff) + ((l2 >> 16) & 0xff)) >> 4; - r = (((l1 >> 8) & 0xff) + (l1 >> 24)) >> 4; - *rgbbuf++ = COLOR_R5_G6_B5_TO_RGB565(r, g, b); - // prepare for the next set of source pixels - l0 >>= 16; l1 >>= 16; l2 >>= 16; // L-CL-CR-R becomes L-CL-0-0 - } // for x - // last col - g = ((l0 >> 8) + (l2 >> 8)) >> 3; // re-use blue from last pixel in loop - r = (l1 >> 11); - *rgbbuf++ = COLOR_R5_G6_B5_TO_RGB565(r, g, b); - } else { // even lines (B G B G B G) - b = (l1 & 0xff) >> 3; - g = ((l0 & 0xff) + (l2 & 0xff)) >> 3; - r = ((l0 & 0xff00) + (l2 & 0xff00)) >> 12; // first pixel is different - *rgbbuf++ = COLOR_R5_G6_B5_TO_RGB565(r, g, b); - for (int x = xoffs+1; x < xoffs+w-1; x+=2) { // middle part - l0 |= (s[-w2] << 16); // grab 3 more pairs of pixels and put in upper 16-bits - l1 |= (s[0] << 16); - l2 |= (s[w2] << 16); - s++; - g = (l1 & 0xff00) >> 10; // (1, 0) green pixel at current-left - b = ((l1 & 0xff) + ((l1 >> 16) & 0xff)) >> 4; - r = ((l0 & 0xff00) + (l2 & 0xff00)) >> 12; - *rgbbuf++ = COLOR_R5_G6_B5_TO_RGB565(r, g, b); - b = (l1 & 0xff0000) >> 19; // (0,0) blue pixel at current-right - g = (((l1 >> 8) & 0xff) + (l1 >> 24) + ((l0 >> 16) & 0xff) + ((l2 >> 16) & 0xff)) >> 4; - r = (((l0 >> 8) & 0xff) + (l0 >> 24) + ((l2 >> 8) & 0xff) + (l2 >> 24)) >> 5; - *rgbbuf++ = COLOR_R5_G6_B5_TO_RGB565(r, g, b); - // prepare for the next set of source pixels - l0 >>= 16; l1 >>= 16; l2 >>= 16; // L-CL-CR-R becomes L-CL-0-0 - } // for x - // last col - g = (l1 >> 10); // re-use blue pixel from last column of loop - r = ((l0 >> 8) + (l2 >> 8)) >> 4; - *rgbbuf++ = COLOR_R5_G6_B5_TO_RGB565(r, g, b); // last pixel - } // even lines - } // faster way - } // for y -} - -// -// Convert a row of Bayer pixels into grayscale output -// -void imlib_bayer_to_y(image_t *img, int x_offset, int y_offset, int width, uint8_t *Y) -{ - uint16_t *s; - uint32_t l0, l1, l2; // current, prev and next lines of current pixel(s) - int x, xx, r, g, b = 0; - int pitch = img->w; // keep in local var - int w2 = pitch/2; // pitch for a uint16_t pointer - x = x_offset; xx = x_offset+width; - if (y_offset < 1 || y_offset >= img->h-1) { // top or bottom lines - uint8_t *s8 = (uint8_t *)&img->pixels[(y_offset * pitch) + x_offset]; - if (y_offset & 1) { // odd line (y == img->h-1) - int bLastPixel = 0; - if (((x_offset+width) & 1) == 0 && x_offset+width >= img->w) { // flag to not read past the bottom-right edge - xx--; // make the loop stop on an even pixel - bLastPixel = 1; - } - if (x == 0) { // special case of starting from left edge - g = s8[0]; - b = s8[-pitch]; - r = s8[1]; - *Y++ = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - x++; - } // x == 0 - for (; x>1; - r = s8[0]; - b = (s8[-pitch-1] + s8[-pitch+1]) >> 1; - } else { // even pixels - g = s8[0]; - b = s8[-pitch]; - r = (s8[-1] + s8[1]) >> 1; - } - *Y++ = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - } // for x - if (bLastPixel) { // 1 more pixel to process - r = s8[0]; // on a red pixel - g = (s8[-pitch] + s8[-1]) >> 1; // and re-use the previous blue - *Y++ = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - } - } else { // even line (y==0) - if (x == 0) { // left edge special case - b = s8[0]; - g = s8[pitch]; - r = s8[pitch+1]; - *Y++ = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - x++; - } - for (; x>1; - g = s8[0]; - r = s8[pitch]; - } else { // even pixels - b = s8[0]; - g = (s8[1] + s8[-1]) >> 1; - r = s8[pitch]; - } - *Y++ = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - } // for x - } // even line - return; - } // edge case; need to check boundary conditions -// middle of image can be converted without checking boundary conditions on each pixel - s = (uint16_t*)&img->pixels[(y_offset * pitch) + (x_offset & 0xfffe)]; - if (x_offset == 0) { // don't read past left edge; repeat pixels instead - l0 = s[-w2] | (s[-w2] << 16); // prep current and left pixels - l1 = s[0] | (s[0] << 16); - l2 = s[w2] | (s[w2] << 16); - } else { - l0 = s[-w2-1] | (s[-w2] << 16); // prep current and left pixels - l1 = s[-1] | (s[0] << 16); - l2 = s[w2-1] | (s[w2] << 16); - } - x = x_offset; xx = x_offset+width; - s++; - if (y_offset & 1) { // odd line - if (x_offset & 1) // starting on an odd pixel, capture it differently - { - r = (l1 >> 24); // (1, 0) red pixel - g = ((l0 >> 24) + (l2 >> 24)) >> 1; - b = ((l0 & 0xff0000) + (l2 & 0xff0000)) >> 17; - *Y++ = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - x++; // advance to next pixel - } - for (; x> 16; // (0,0) green pixel - b = ((l0 & 0xff0000) + (l2 & 0xff0000)) >> 17; - r = (((l1 >> 8) & 0xff) + (l1 >> 24)) >> 1; - // faster to keep all calculations in integer math with 15-bit fractions - *Y++ = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - l0 >>= 16; l1 >>= 16; l2 >>= 16; // L-CL-CR-R becomes L-CL-0-0 - l0 |= (s[-w2] << 16); // grab 3 more pairs of pixels and put in upper 16-bits - l1 |= (s[0] << 16); - l2 |= (s[w2] << 16); - s++; - r = (l1 & 0xff00) >> 8; // (1, 0) red pixel - g = (((l1 >> 16) & 0xff) + (l1 & 0xff) + ((l0 >> 8) & 0xff) + ((l2 >> 8) & 0xff)) >> 2; - b = ((l0 & 0xff) + (l2 & 0xff) + ((l0 >> 16) & 0xff) + ((l2 >> 16) & 0xff)) >> 2; - *Y++ = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - } // for x - if (x < xx) { // one final pixel to do - g = (l1 >> 16) & 0xff; // (0, 0) green pixel - b = ((l0 & 0xff0000) + (l2 & 0xff0000)) >> 17; - r = (((l1 >> 8) & 0xff) + (l1 >> 24)) >> 1; - *Y++ = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g - } - } else { // even line - if (x_offset & 1) // starting on an odd pixel, capture it differently - { - g = (l1 >> 8) & 0xff; // (1, 0) green pixel - r = (((l0 >> 8) & 0xff) + ((l2 >> 8) && 0xff)) >> 1; - b = (((l0 >> 16) & 0xff) + ((l2 >> 16) & 0xff)) >> 1; - *Y++ = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - x++; // advance to next pixel - } - for (; x> 16; // (0,0) blue pixel - g = (((l1 >> 8) & 0xff) + (l1 >> 24) + ((l0 >> 16) & 0xff) + ((l2 >> 16) & 0xff)) >> 2; - r = (((l0 >> 8) & 0xff) + (l0 >> 24) + ((l2 >> 8) & 0xff) + (l2 >> 24)) >> 2; - *Y++ = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - // prepare for the next set of source pixels - l0 >>= 16; l1 >>= 16; l2 >>= 16; // L-CL-CR-R becomes L-CL-0-0 - l0 |= (s[-w2] << 16); // grab 3 more pairs of pixels and put in upper 16-bits - l1 |= (s[0] << 16); - l2 |= (s[w2] << 16); - s++; - g = (l1 & 0xff00) >> 8; // (1, 0) green pixel - b = ((l1 & 0xff) + ((l1 >> 16) & 0xff)) >> 1; - r = ((l0 & 0xff00) + (l2 & 0xff00)) >> 9; - *Y++ = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - } // for x - if (x < xx) { // one final pixel to do - b = (l1 & 0xff0000) >> 16; // (0,0) blue pixel - g = (((l1 >> 8) & 0xff) + (l1 >> 24) + ((l0 >> 16) & 0xff) + ((l2 >> 16) & 0xff)) >> 2; - r = (((l0 >> 8) & 0xff) + (l0 >> 24) + ((l2 >> 8) & 0xff) + (l2 >> 24)) >> 2; - *Y++ = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - } - } // even line -} /* imlib_bayer_to_y() */ -// -// Convert a row of Bayer pixels into binary output -// -void imlib_bayer_to_binary(image_t *img, int x_offset, int y_offset, int width, uint8_t *binary) -{ - uint16_t *s; - uint32_t l0, l1, l2; // current, prev and next lines of current pixel(s) - int x, xx, r, g, b = 0; - uint8_t pixel; - int pitch = img->w; // keep in local var - int w2 = pitch/2; // pitch for a uint16_t pointer - x = x_offset; xx = x_offset+width; - if (y_offset < 1 || y_offset >= img->h-1) { // top or bottom lines - uint8_t *s8 = (uint8_t *)&img->pixels[(y_offset * pitch) + x_offset]; - if (y_offset & 1) { // odd line (y == img->h-1) - int bLastPixel = 0; - if (((x_offset+width) & 1) == 0 && x_offset+width >= img->w) { // flag to not read past the bottom-right edge - xx--; // make the loop stop on an even pixel - bLastPixel = 1; - } - if (x == 0) { // special case of starting from left edge - g = s8[0]; - b = s8[-pitch]; - r = s8[1]; - pixel = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - IMAGE_PUT_BINARY_PIXEL_FAST(binary, x, (pixel >> 7)); - x++; - } - for (; x>1; - r = s8[0]; - b = (s8[-pitch-1] + s8[-pitch+1]) >> 1; - } else { // even pixels - g = s8[0]; - b = s8[-pitch]; - r = (s8[-1] + s8[1]) >> 1; - } - pixel = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - IMAGE_PUT_BINARY_PIXEL_FAST(binary, x, (pixel >> 7)); - } // for x - if (bLastPixel) { // 1 more pixel to process - r = s8[0]; // on a red pixel - g = (s8[-pitch] + s8[-1]) >> 1; // and re-use the previous blue - pixel = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - IMAGE_PUT_BINARY_PIXEL_FAST(binary, x, (pixel >> 7)); - } - } else { // even line (y==0) - if (x == 0) { // left edge special case - b = s8[0]; - g = s8[pitch]; - r = s8[pitch+1]; - pixel = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - IMAGE_PUT_BINARY_PIXEL_FAST(binary, x, (pixel >> 7)); - x++; - } - for (; x>1; - g = s8[0]; - r = s8[pitch]; - } else { // even pixels - b = s8[0]; - g = (s8[1] + s8[-1]) >> 1; - r = s8[pitch]; - } - pixel = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - IMAGE_PUT_BINARY_PIXEL_FAST(binary, x, (pixel >> 7)); - } // for x - } // even line - return; - } // edge case; need to check boundary conditions -// middle of image can be converted without checking boundary conditions on each pixel - s = (uint16_t*)&img->pixels[(y_offset * pitch) + (x_offset & 0xfffe)]; - if (x_offset == 0) { // don't read past left edge; repeat pixels instead - l0 = s[-w2] | (s[-w2] << 16); // prep current and left pixels - l1 = s[0] | (s[0] << 16); - l2 = s[w2] | (s[w2] << 16); - } else { - l0 = s[-w2-1] | (s[-w2] << 16); // prep current and left pixels - l1 = s[-1] | (s[0] << 16); - l2 = s[w2-1] | (s[w2] << 16); - } - x = x_offset; xx = x_offset+width; - s++; - if (y_offset & 1) { // odd line - if (x_offset & 1) // starting on an odd pixel, capture it differently - { - r = (l1 >> 24); // (1, 0) red pixel - g = ((l0 >> 24) + (l2 >> 24)) >> 1; - b = ((l0 & 0xff0000) + (l2 & 0xff0000)) >> 17; - pixel = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - IMAGE_PUT_BINARY_PIXEL_FAST(binary, x, (pixel >> 7)); - x++; // advance to next pixel - } - for (; x> 16; // (0,0) green pixel - b = ((l0 & 0xff0000) + (l2 & 0xff0000)) >> 17; - r = (((l1 >> 8) & 0xff) + (l1 >> 24)) >> 1; - // faster to keep all calculations in integer math with 15-bit fractions - pixel = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - IMAGE_PUT_BINARY_PIXEL_FAST(binary, x, (pixel >> 7)); - l0 >>= 16; l1 >>= 16; l2 >>= 16; // L-CL-CR-R becomes L-CL-0-0 - l0 |= (s[-w2] << 16); // grab 3 more pairs of pixels and put in upper 16-bits - l1 |= (s[0] << 16); - l2 |= (s[w2] << 16); - s++; - r = (l1 & 0xff00) >> 8; // (1, 0) red pixel - g = (((l1 >> 16) & 0xff) + (l1 & 0xff) + ((l0 >> 8) & 0xff) + ((l2 >> 8) & 0xff)) >> 2; - b = ((l0 & 0xff) + (l2 & 0xff) + ((l0 >> 16) & 0xff) + ((l2 >> 16) & 0xff)) >> 2; - pixel = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - IMAGE_PUT_BINARY_PIXEL_FAST(binary, x+1, (pixel >> 7)); - } // for x - if (x < xx) { // one final pixel to do - g = (l1 >> 16) & 0xff; // (0, 0) green pixel - b = ((l0 & 0xff0000) + (l2 & 0xff0000)) >> 17; - r = (((l1 >> 8) & 0xff) + (l1 >> 24)) >> 1; - pixel = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g - IMAGE_PUT_BINARY_PIXEL_FAST(binary, x+1, (pixel >> 7)); - } - } else { // even line - if (x_offset & 1) // starting on an odd pixel, capture it differently - { - g = (l1 >> 8) & 0xff; // (1, 0) green pixel - r = (((l0 >> 8) & 0xff) + ((l2 >> 8) && 0xff)) >> 1; - b = (((l0 >> 16) & 0xff) + ((l2 >> 16) & 0xff)) >> 1; - pixel = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - IMAGE_PUT_BINARY_PIXEL_FAST(binary, x, (pixel >> 7)); - x++; // advance to next pixel - } - for (; x> 16; // (0,0) blue pixel - g = (((l1 >> 8) & 0xff) + (l1 >> 24) + ((l0 >> 16) & 0xff) + ((l2 >> 16) & 0xff)) >> 2; - r = (((l0 >> 8) & 0xff) + (l0 >> 24) + ((l2 >> 8) & 0xff) + (l2 >> 24)) >> 2; - pixel = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - IMAGE_PUT_BINARY_PIXEL_FAST(binary, x, (pixel >> 7)); - // prepare for the next set of source pixels - l0 >>= 16; l1 >>= 16; l2 >>= 16; // L-CL-CR-R becomes L-CL-0-0 - l0 |= (s[-w2] << 16); // grab 3 more pairs of pixels and put in upper 16-bits - l1 |= (s[0] << 16); - l2 |= (s[w2] << 16); - s++; - g = (l1 & 0xff00) >> 8; // (1, 0) green pixel - b = ((l1 & 0xff) + ((l1 >> 16) & 0xff)) >> 1; - r = ((l0 & 0xff00) + (l2 & 0xff00)) >> 9; - pixel = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - IMAGE_PUT_BINARY_PIXEL_FAST(binary, x+1, (pixel >> 7)); - } // for x - if (x < xx) { // one final pixel to do - b = (l1 & 0xff0000) >> 16; // (0,0) blue pixel - g = (((l1 >> 8) & 0xff) + (l1 >> 24) + ((l0 >> 16) & 0xff) + ((l2 >> 16) & 0xff)) >> 2; - r = (((l0 >> 8) & 0xff) + (l0 >> 24) + ((l2 >> 8) & 0xff) + (l2 >> 24)) >> 2; - pixel = (uint8_t)(((r * 9770) + (g * 19182) + (b * 3736)) >> 15); // .299*r + .587*g + .114*b - IMAGE_PUT_BINARY_PIXEL_FAST(binary, x, (pixel >> 7)); - } - } // even line -} /* imlib_bayer_to_binary() */ - //////////////////////////////////////////////////////////////////////////////// #if defined(IMLIB_ENABLE_IMAGE_FILE_IO) diff --git a/src/omv/imlib/imlib.h b/src/omv/imlib/imlib.h index a0d7d719f..9d812ffd7 100644 --- a/src/omv/imlib/imlib.h +++ b/src/omv/imlib/imlib.h @@ -1051,16 +1051,20 @@ void imlib_deinit_all(); void imlib_fill_image_from_float(image_t *img, int w, int h, float *data, float min, float max, bool mirror, bool flip, bool dst_transpose, bool src_transpose); +// Bayer Image Processing +void imlib_debayer_line_to_binary(int x_start, int x_end, int y_row, uint32_t *dst_row_ptr, image_t *src); +void imlib_debayer_image_to_binary(image_t *dst, image_t *src); +void imlib_debayer_line_to_grayscale(int x_start, int x_end, int y_row, uint8_t *dst_row_ptr, image_t *src); +void imlib_debayer_image_to_grayscale(image_t *dst, image_t *src); +void imlib_debayer_line_to_rgb565(int x_start, int x_end, int y_row, uint16_t *dst_row_ptr, image_t *src); +void imlib_debayer_image_to_rgb565(image_t *dst, image_t *src); + /* Color space functions */ int8_t imlib_rgb565_to_l(uint16_t pixel); int8_t imlib_rgb565_to_a(uint16_t pixel); int8_t imlib_rgb565_to_b(uint16_t pixel); uint16_t imlib_lab_to_rgb(uint8_t l, int8_t a, int8_t b); uint16_t imlib_yuv_to_rgb(uint8_t y, int8_t u, int8_t v); -void imlib_bayer_to_rgb565(image_t *img, int w, int h, int xoffs, int yoffs, uint16_t *rgbbuf); -void imlib_bayer_to_y(image_t *img, int x_offset, int y_offset, int width, uint8_t *Y); -void imlib_bayer_to_binary(image_t *img, int x_offset, int y_offset, int width, uint8_t *binary); -bool imlib_pixel_to_binary(int bpp, uint32_t pixel); /* Image file functions */ void ppm_read_geometry(FIL *fp, image_t *img, const char *path, ppm_read_settings_t *rs); diff --git a/src/omv/modules/py_fir.c b/src/omv/modules/py_fir.c index c7b6a0116..19000b734 100644 --- a/src/omv/modules/py_fir.c +++ b/src/omv/modules/py_fir.c @@ -1083,7 +1083,7 @@ mp_obj_t py_fir_snapshot(uint n_args, const mp_obj_t *args, mp_map_t *kw_args) if (mp_obj_is_integer(copy_to_fb_obj)) { copy_to_fb = mp_obj_get_int(copy_to_fb_obj); } else { - arg_other = py_helper_arg_to_image_mutable(copy_to_fb_obj); + arg_other = py_helper_arg_to_image_mutable_bayer(copy_to_fb_obj); } } diff --git a/src/omv/modules/py_image.c b/src/omv/modules/py_image.c index 4c8fd607f..201c89db4 100644 --- a/src/omv/modules/py_image.c +++ b/src/omv/modules/py_image.c @@ -709,7 +709,7 @@ STATIC mp_obj_t py_image_get_pixel(uint n_args, const mp_obj_t *args, mp_map_t * } case IMAGE_BPP_BAYER: if (arg_rgbtuple) { - uint16_t pixel; imlib_bayer_to_rgb565(arg_img, 1, 1, arg_x, arg_y, &pixel); + uint16_t pixel; imlib_debayer_line_to_rgb565(arg_x, arg_x + 1, arg_y, &pixel, arg_img); mp_obj_t pixel_tuple[3]; pixel_tuple[0] = mp_obj_new_int(COLOR_RGB565_TO_R8(pixel)); pixel_tuple[1] = mp_obj_new_int(COLOR_RGB565_TO_G8(pixel)); @@ -870,7 +870,7 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_midpoint_pooled_obj, 3, py_image_midp static mp_obj_t py_image_to(int bpp, const uint16_t *default_color_palette, bool copy_to_fb, uint n_args, const mp_obj_t *args, mp_map_t *kw_args) { - image_t *src_img = py_helper_arg_to_image_mutable(args[0]); + image_t *src_img = py_helper_arg_to_image_mutable_bayer(args[0]); float arg_x_scale = 1.f; bool got_x_scale = py_helper_keyword_float_maybe(n_args, args, 1, kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_x_scale), &arg_x_scale); @@ -935,7 +935,7 @@ static mp_obj_t py_image_to(int bpp, const uint16_t *default_color_palette, bool if (mp_obj_is_integer(copy_obj)) { copy = mp_obj_get_int(copy_obj); } else { - arg_other = py_helper_arg_to_image_mutable(copy_obj); + arg_other = py_helper_arg_to_image_mutable_bayer(copy_obj); } } @@ -948,6 +948,23 @@ static mp_obj_t py_image_to(int bpp, const uint16_t *default_color_palette, bool dst_img.h = fast_floorf(arg_roi.h * arg_y_scale); dst_img.bpp = (bpp >= 0) ? bpp : src_img->bpp; + if (dst_img.bpp == IMAGE_BPP_BAYER) { + if (((arg_x_scale != 1) && (arg_x_scale != -1)) || + ((arg_y_scale != 1) && (arg_y_scale != -1)) || + (arg_rgb_channel != -1) || + (arg_alpha != 256) || + (color_palette != NULL) || + (alpha_palette != NULL)) { + mp_raise_msg(&mp_type_ValueError, MP_ERROR_TEXT("Only bayer copying/cropping is supported!")); + } else { + hint &= ~(IMAGE_HINT_AREA | + IMAGE_HINT_BICUBIC | + IMAGE_HINT_BILINEAR | + IMAGE_HINT_EXTRACT_RGB_CHANNEL_FIRST | + IMAGE_HINT_APPLY_COLOR_PALETTE_FIRST); + } + } + if (copy) { if (copy_to_fb) { py_helper_set_to_framebuffer(&dst_img); @@ -1437,7 +1454,7 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_draw_edges_obj, 2, py_image_draw_edge STATIC mp_obj_t py_image_draw_image(uint n_args, const mp_obj_t *args, mp_map_t *kw_args) { image_t *arg_img = py_helper_arg_to_image_mutable(args[0]); - image_t *arg_other = py_helper_arg_to_image_mutable(args[1]); + image_t *arg_other = py_helper_arg_to_image_mutable_bayer(args[1]); const mp_obj_t *arg_vec; uint offset = py_helper_consume_array(n_args, args, 2, 2, &arg_vec); @@ -5147,7 +5164,7 @@ static const mp_obj_type_t py_apriltag_type = { static mp_obj_t py_image_find_apriltags(uint n_args, const mp_obj_t *args, mp_map_t *kw_args) { - image_t *arg_img = py_helper_arg_to_image_mutable(args[0]); + image_t *arg_img = py_helper_arg_to_image_mutable_bayer(args[0]); rectangle_t roi; py_helper_keyword_rectangle_roi(arg_img, n_args, args, 1, kw_args, &roi); diff --git a/src/omv/ports/nrf/omv_portconfig.mk b/src/omv/ports/nrf/omv_portconfig.mk index 1280a64ca..3f901a47c 100644 --- a/src/omv/ports/nrf/omv_portconfig.mk +++ b/src/omv/ports/nrf/omv_portconfig.mk @@ -136,6 +136,7 @@ FIRM_OBJ += $(addprefix $(BUILD)/$(OMV_DIR)/modules/, \ FIRM_OBJ += $(addprefix $(BUILD)/$(OMV_DIR)/imlib/, \ agast.o \ apriltag.o \ + bayer.o \ binary.o \ blob.o \ bmp.o \ diff --git a/src/omv/ports/stm32/modules/py_lcd.c b/src/omv/ports/stm32/modules/py_lcd.c index b2585f1e1..b06947a60 100644 --- a/src/omv/ports/stm32/modules/py_lcd.c +++ b/src/omv/ports/stm32/modules/py_lcd.c @@ -1606,7 +1606,7 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_lcd_get_point_y_position_obj, py_lcd_get_poi STATIC mp_obj_t py_lcd_display(uint n_args, const mp_obj_t *args, mp_map_t *kw_args) { - image_t *arg_img = py_helper_arg_to_image_mutable(args[0]); + image_t *arg_img = py_helper_arg_to_image_mutable_bayer(args[0]); int arg_x_off = 0; int arg_y_off = 0; diff --git a/src/omv/ports/stm32/modules/py_tv.c b/src/omv/ports/stm32/modules/py_tv.c index aea71b30e..91f17102f 100644 --- a/src/omv/ports/stm32/modules/py_tv.c +++ b/src/omv/ports/stm32/modules/py_tv.c @@ -679,7 +679,9 @@ static void spi_tv_display(image_t *src_img, int dst_x_start, int dst_y_start, f const uint16_t *color_palette, const uint8_t *alpha_palette, image_hint_t hint) { - bool rgb565 = ((rgb_channel == -1) && (src_img->bpp == IMAGE_BPP_RGB565)) || color_palette; + bool rgb565 = ((rgb_channel == -1) && + ((src_img->bpp == IMAGE_BPP_RGB565) || src_img->bpp == IMAGE_BPP_BAYER)) + || color_palette; imlib_draw_row_callback_t cb = rgb565 ? spi_tv_draw_image_cb_rgb565 : spi_tv_draw_image_cb_grayscale; image_t dst_img; @@ -957,7 +959,7 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(py_tv_channel_obj, 0, 1, py_tv_channe STATIC mp_obj_t py_tv_display(uint n_args, const mp_obj_t *args, mp_map_t *kw_args) { - image_t *arg_img = py_helper_arg_to_image_mutable(args[0]); + image_t *arg_img = py_helper_arg_to_image_mutable_bayer(args[0]); int arg_x_off = 0; int arg_y_off = 0; diff --git a/src/omv/ports/stm32/omv_portconfig.mk b/src/omv/ports/stm32/omv_portconfig.mk index 08d20ba22..e1ad6cc16 100644 --- a/src/omv/ports/stm32/omv_portconfig.mk +++ b/src/omv/ports/stm32/omv_portconfig.mk @@ -161,6 +161,7 @@ FIRM_OBJ += $(addprefix $(BUILD)/$(OMV_DIR)/modules/, \ FIRM_OBJ += $(addprefix $(BUILD)/$(OMV_DIR)/imlib/, \ agast.o \ apriltag.o \ + bayer.o \ binary.o \ blob.o \ bmp.o \