From 421909a6a8d6f445b93373912927c8c3a55e60a4 Mon Sep 17 00:00:00 2001 From: ScottW514 Date: Mon, 3 Aug 2026 13:46:08 -0400 Subject: [PATCH] forgectrl: NEON demosaic - 15 fps, sensor-limited NEON kernel for the YUV420 superpixel convert (vld2q deinterleave, rounding-halving greens, mlal/rshrn luma, pairwise-add chroma block sums), bit-identical to the scalar path and proven so on a live frame via the FORGECTRL_NEON_CHECK one-shot memcmp. Convert 75 to 18 ms; the stream now runs at the OV5648 frame rate. Motion coexistence re-proven at 15 fps (clamped 0). --- docs/BRINGUP.md | 20 ++- .../forgectrl/forgectrl/cam.c | 27 ++++ .../forgectrl/forgectrl/debayer.c | 142 +++++++++++++++++- .../forgectrl/forgectrl/debayer.h | 12 +- 4 files changed, 192 insertions(+), 9 deletions(-) diff --git a/docs/BRINGUP.md b/docs/BRINGUP.md index 53a86e0..0a4a587 100644 --- a/docs/BRINGUP.md +++ b/docs/BRINGUP.md @@ -156,8 +156,9 @@ The cameras share the hardware video-mux; the NEWEST request wins it The per-camera lamp (`pic/lid_led` / `head/white_led`) is raised to `FORGECTRL_LAMP` (default 132) while capturing and restored on idle. -Bench (2026-08-03, on the board): stream **7.9 fps** sustained at -1296×972 (VPU encode; 3.2 fps on the software fallback); +Bench (2026-08-03, on the board): stream **15.0 fps** sustained at +1296×972 (NEON demosaic + VPU encode; 3.2 fps on the full software +fallback); full-res snapshot 2.4 s warm / 2.7 s cold (cold includes the pipeline bring-up); two parallel same-camera clients share the frame rate; idle teardown observed. Borrow verified: head snapshot 200 during a lid @@ -190,9 +191,18 @@ encode 7 ms**. Two hard-won facts: needed) with quality via V4L2_CID_JPEG_COMPRESSION_QUALITY. libjpeg remains the automatic fallback (`FORGECTRL_NO_VPU=1` forces it) and the snapshot path; `/cam/status` reports `"encoder"`. -Remaining headroom: the scalar convert dominates — NEON would push -toward the sensor/CSI limit. If motion contention ever shows clamps -during streaming, a stream-fps cap knob is the easy relief valve. + +**NEON demosaic: DONE 2026-08-03 — 15.0 fps, sensor-limited.** The +YUV420 superpixel convert has a NEON kernel (vld2q deinterleave, +vrhaddq greens, vmlal/vrshrn luma, vpaddlq block sums for chroma; +`FORGECTRL_NO_NEON=1` forces scalar): convert 75 → 18 ms, per-frame +copy 34 + convert 18 + encode 7 ≈ 59 ms against the sensor's 66 ms +frame period. The NEON and scalar paths are bit-identical — proven on +a live frame via `FORGECTRL_NEON_CHECK=1` (one-shot memcmp, logs +IDENTICAL). Motion coexistence re-proven at 15 fps: jogs with an +active stream show clamped 0, max behind 7.2 ms (~4 % of the 200 ms +queue) — the worst-case contention signature so far; if real jobs +ever clamp, a stream-fps cap knob is the relief valve. The IPU cannot help with demosaic (its IC is CSC/scale only — the `imx-csc-scaler` at /dev/video8 matters only for a future full-res stream). Not yet done: lens calibration / bed alignment (the diff --git a/meta-forgefirm/recipes-forgefirm/forgectrl/forgectrl/cam.c b/meta-forgefirm/recipes-forgefirm/forgectrl/forgectrl/cam.c index 491b7ab..9dde7db 100644 --- a/meta-forgefirm/recipes-forgefirm/forgectrl/forgectrl/cam.c +++ b/meta-forgefirm/recipes-forgefirm/forgectrl/forgectrl/cam.c @@ -704,6 +704,33 @@ static void *worker(void *arg) vpu_jpeg_planes(vpu, &yp, &up, &vp, &ys, &uvs); debayer_bggr_half_yuv420(raw, CAM_W, CAM_H, HFLIP, yp, ys, up, vp, uvs); +#ifdef __ARM_NEON + /* One-shot NEON-vs-scalar equivalence check on a live + * frame (the paths are constructed to be bit-identical; + * this proves it on real data). Stride == width here. */ + static int neon_checked; + if (!neon_checked && getenv("FORGECTRL_NEON_CHECK")) { + neon_checked = 1; + size_t ysz = (size_t)ys * HALF_H; + size_t usz = (size_t)uvs * (HALF_H / 2); + uint8_t *ry = malloc(ysz); + uint8_t *ru = malloc(usz); + uint8_t *rv = malloc(usz); + if (ry && ru && rv) { + debayer_bggr_half_yuv420_scalar(raw, CAM_W, CAM_H, + HFLIP, ry, ys, + ru, rv, uvs); + fprintf(stderr, "cam: NEON/scalar compare: %s\n", + (!memcmp(ry, yp, ysz) && + !memcmp(ru, up, usz) && + !memcmp(rv, vp, usz)) + ? "IDENTICAL" : "MISMATCH"); + } + free(ry); + free(ru); + free(rv); + } +#endif now_ts(&e1); if (vpu_jpeg_encode(vpu, &jpg, &len) == 0) { via_vpu = 1; diff --git a/meta-forgefirm/recipes-forgefirm/forgectrl/forgectrl/debayer.c b/meta-forgefirm/recipes-forgefirm/forgectrl/forgectrl/debayer.c index 095ace6..8ebbd3f 100644 --- a/meta-forgefirm/recipes-forgefirm/forgectrl/forgectrl/debayer.c +++ b/meta-forgefirm/recipes-forgefirm/forgectrl/forgectrl/debayer.c @@ -64,9 +64,9 @@ void debayer_bggr_bilinear(const uint8_t *raw, uint8_t *rgb, } } -void debayer_bggr_half_yuv420(const uint8_t *raw, int w, int h, int hflip, - uint8_t *yp, int y_stride, - uint8_t *up, uint8_t *vp, int uv_stride) +void debayer_bggr_half_yuv420_scalar(const uint8_t *raw, int w, int h, + int hflip, uint8_t *yp, int y_stride, + uint8_t *up, uint8_t *vp, int uv_stride) { const int ow = w / 2; /* luma dimensions */ const int oh = h / 2; @@ -127,3 +127,139 @@ void debayer_bggr_half(const uint8_t *raw, uint8_t *rgb, } } } + +#ifdef __ARM_NEON +#include + +/* Store 16 luma bytes at column x, mirrored when hflip (the vector is + * byte-reversed and lands at the mirrored block position). */ +static inline void store16_flip(uint8_t *row, int x, int ow, int hflip, + uint8x16_t v) +{ + if (!hflip) { + vst1q_u8(row + x, v); + } else { + uint8x16_t r = vrev64q_u8(v); + r = vextq_u8(r, r, 8); /* swap halves: full 16-byte reverse */ + vst1q_u8(row + (ow - 16 - x), r); + } +} + +static inline void store8_flip(uint8_t *row, int x, int n, int hflip, + uint8x8_t v) +{ + if (!hflip) + vst1_u8(row + x, v); + else + vst1_u8(row + (n - 8 - x), vrev64_u8(v)); +} + +/* Y = (77R + 150G + 29B + 128) >> 8 for 16 pixels. */ +static inline uint8x16_t luma16(uint8x16_t R, uint8x16_t G, uint8x16_t B) +{ + const uint8x8_t cR = vdup_n_u8(77), cG = vdup_n_u8(150), + cB = vdup_n_u8(29); + uint16x8_t lo = vmull_u8(vget_low_u8(R), cR); + lo = vmlal_u8(lo, vget_low_u8(G), cG); + lo = vmlal_u8(lo, vget_low_u8(B), cB); + uint16x8_t hi = vmull_u8(vget_high_u8(R), cR); + hi = vmlal_u8(hi, vget_high_u8(G), cG); + hi = vmlal_u8(hi, vget_high_u8(B), cB); + return vcombine_u8(vrshrn_n_u16(lo, 8), vrshrn_n_u16(hi, 8)); +} + +/* 4-superpixel block sums (vertical add then horizontal pair-add) for one + * 16-column pair of rows -> 8 lanes of u32 split across two quads. */ +static inline void block_sums(uint8x16_t a, uint8x16_t b, + int32x4_t *q0, int32x4_t *q1) +{ + uint16x8_t lo = vaddl_u8(vget_low_u8(a), vget_low_u8(b)); + uint16x8_t hi = vaddl_u8(vget_high_u8(a), vget_high_u8(b)); + *q0 = vreinterpretq_s32_u32(vpaddlq_u16(lo)); + *q1 = vreinterpretq_s32_u32(vpaddlq_u16(hi)); +} + +/* ((cr*rs + cg*gs + cb*bs + 512) >> 10) + 128, clamped to 0..255. */ +static inline uint16x4_t chroma4(int32x4_t rs, int32x4_t gs, int32x4_t bs, + int cr, int cg, int cb) +{ + int32x4_t acc = vmulq_n_s32(rs, cr); + acc = vmlaq_n_s32(acc, gs, cg); + acc = vmlaq_n_s32(acc, bs, cb); + acc = vaddq_s32(acc, vdupq_n_s32(512)); + acc = vshrq_n_s32(acc, 10); + acc = vaddq_s32(acc, vdupq_n_s32(128)); + return vqmovun_s32(acc); /* clamps < 0 */ +} + +static void debayer_bggr_half_yuv420_neon(const uint8_t *raw, int w, int h, + int hflip, + uint8_t *yp, int y_stride, + uint8_t *up, uint8_t *vp, + int uv_stride) +{ + const int ow = w / 2; + const int oh = h / 2; + const int uvw = ow / 2; + + for (int y2 = 0; y2 < oh / 2; y2++) { + const uint8_t *r0 = raw + (size_t)(4 * y2) * w; /* B G ... */ + const uint8_t *r1 = r0 + w; /* G R ... */ + const uint8_t *r2 = r1 + w; + const uint8_t *r3 = r2 + w; + uint8_t *ya = yp + (size_t)(2 * y2) * y_stride; + uint8_t *yb = ya + y_stride; + uint8_t *ur = up + (size_t)y2 * uv_stride; + uint8_t *vr = vp + (size_t)y2 * uv_stride; + + for (int x = 0; x < ow; x += 16) { + /* superpixel row A: raw rows r0/r1 */ + uint8x16x2_t ea = vld2q_u8(r0 + 2 * x); /* [0]=B [1]=G1 */ + uint8x16x2_t oa = vld2q_u8(r1 + 2 * x); /* [0]=G2 [1]=R */ + uint8x16_t Ba = ea.val[0]; + uint8x16_t Ga = vrhaddq_u8(ea.val[1], oa.val[0]); + uint8x16_t Ra = oa.val[1]; + store16_flip(ya, x, ow, hflip, luma16(Ra, Ga, Ba)); + + /* superpixel row B: raw rows r2/r3 */ + uint8x16x2_t eb = vld2q_u8(r2 + 2 * x); + uint8x16x2_t ob = vld2q_u8(r3 + 2 * x); + uint8x16_t Bb = eb.val[0]; + uint8x16_t Gb = vrhaddq_u8(eb.val[1], ob.val[0]); + uint8x16_t Rb = ob.val[1]; + store16_flip(yb, x, ow, hflip, luma16(Rb, Gb, Bb)); + + /* chroma from the 2x2 superpixel blocks of both rows */ + int32x4_t rs0, rs1, gs0, gs1, bs0, bs1; + block_sums(Ra, Rb, &rs0, &rs1); + block_sums(Ga, Gb, &gs0, &gs1); + block_sums(Ba, Bb, &bs0, &bs1); + + uint16x8_t cb = vcombine_u16(chroma4(rs0, gs0, bs0, -43, -85, 128), + chroma4(rs1, gs1, bs1, -43, -85, 128)); + uint16x8_t cr = vcombine_u16(chroma4(rs0, gs0, bs0, 128, -107, -21), + chroma4(rs1, gs1, bs1, 128, -107, -21)); + store8_flip(ur, x / 2, uvw, hflip, vqmovn_u16(cb)); + store8_flip(vr, x / 2, uvw, hflip, vqmovn_u16(cr)); + } + } +} +#endif /* __ARM_NEON */ + +void debayer_bggr_half_yuv420(const uint8_t *raw, int w, int h, int hflip, + uint8_t *yp, int y_stride, + uint8_t *up, uint8_t *vp, int uv_stride) +{ +#ifdef __ARM_NEON + static int no_neon = -1; + if (no_neon < 0) + no_neon = getenv("FORGECTRL_NO_NEON") != NULL; + if (!no_neon && w % 32 == 0 && h % 4 == 0) { + debayer_bggr_half_yuv420_neon(raw, w, h, hflip, yp, y_stride, + up, vp, uv_stride); + return; + } +#endif + debayer_bggr_half_yuv420_scalar(raw, w, h, hflip, yp, y_stride, + up, vp, uv_stride); +} diff --git a/meta-forgefirm/recipes-forgefirm/forgectrl/forgectrl/debayer.h b/meta-forgefirm/recipes-forgefirm/forgectrl/forgectrl/debayer.h index a6f841c..1d55732 100644 --- a/meta-forgefirm/recipes-forgefirm/forgectrl/forgectrl/debayer.h +++ b/meta-forgefirm/recipes-forgefirm/forgectrl/forgectrl/debayer.h @@ -24,9 +24,19 @@ void debayer_bggr_half(const uint8_t *raw, uint8_t *rgb, /* Half-resolution demosaic straight to planar YUV420 (JFIF full-range, * ITU-R 601) for the VPU JPEG encoder: luma per 2x2 BGGR quad at * (w/2)x(h/2), chroma averaged per 2x2 luma block at (w/4)x(h/4). - * w/2 and h/2 must be even. Strides are in bytes. */ + * w/2 and h/2 must be even. Strides are in bytes. + * + * Dispatches to a NEON kernel when compiled for NEON and the geometry + * allows (w%32==0, h%4==0); FORGECTRL_NO_NEON=1 forces the scalar path. + * Both paths produce bit-identical output. */ void debayer_bggr_half_yuv420(const uint8_t *raw, int w, int h, int hflip, uint8_t *yp, int y_stride, uint8_t *up, uint8_t *vp, int uv_stride); +/* The scalar reference path (used by the NEON self-check). */ +void debayer_bggr_half_yuv420_scalar(const uint8_t *raw, int w, int h, + int hflip, uint8_t *yp, int y_stride, + uint8_t *up, uint8_t *vp, + int uv_stride); + #endif