|
| 1 | +#pragma once |
| 2 | + |
| 3 | +/// The hardware-free half of USB video capture: the UVC payload headers, the mode choice and the shrink onto a picture. |
| 4 | + |
| 5 | +#include "core/util/AreaSample.h" // cellSpan, areaMean: the shrink every sampler shares |
| 6 | + |
| 7 | +#include <cstddef> |
| 8 | +#include <cstdint> |
| 9 | +#include <cstring> |
| 10 | + |
| 11 | +namespace mm::usbvideo { |
| 12 | + |
| 13 | +/// One mode a grabber advertises, as its frame list reports it. |
| 14 | +struct Mode { |
| 15 | + uint16_t width = 0; ///< frame width in pixels |
| 16 | + uint16_t height = 0; ///< frame height in pixels |
| 17 | + uint8_t fps = 0; ///< frames a second at that size |
| 18 | +}; |
| 19 | + |
| 20 | +// --- UVC bulk payload headers (UVC 1.5 2.4.3.3) --------------------------------------------------- |
| 21 | + |
| 22 | +constexpr uint8_t kPts = 0x04, kScr = 0x08, kEoh = 0x80, kErr = 0x40, kReserved = 0x10; |
| 23 | +constexpr size_t kBulkMps = 512; ///< high-speed bulk packet size, the stride headers sit on |
| 24 | +constexpr size_t kConfirmStrides = 3; ///< a short header carries few bits, so one match proves nothing |
| 25 | + |
| 26 | +/// A header's own length from its flags: 2 bytes, plus 4 for a PTS and 6 for an SCR. |
| 27 | +inline size_t headerLenFor(uint8_t flags) { |
| 28 | + return 2u + ((flags & kPts) ? 4u : 0u) + ((flags & kScr) ? 6u : 0u); |
| 29 | +} |
| 30 | + |
| 31 | +/// Whether the flags are a header's: end of header set, no error, no reserved bit. |
| 32 | +inline bool headerFlagsOk(uint8_t flags) { return (flags & kEoh) && !(flags & (kErr | kReserved)); } |
| 33 | + |
| 34 | +/// Whether the SCR's reserved top 5 bits are zero, the strongest single check a header carries. |
| 35 | +inline bool headerScrOk(const uint8_t* h, uint8_t flags, uint8_t len) { |
| 36 | + return !(flags & kScr) || !(h[len - 1] & 0xF8); |
| 37 | +} |
| 38 | + |
| 39 | +/// Whether the PTS matches the frame's first header, which is constant across one frame's payloads. |
| 40 | +inline bool headerPtsOk(const uint8_t* h, uint8_t flags, const uint8_t* pts) { |
| 41 | + return !pts || !(flags & kPts) || std::memcmp(h + 2, pts, 4) == 0; |
| 42 | +} |
| 43 | + |
| 44 | +/// Whether `h` is a payload header of exactly `want` bytes; `pts` pins it to the frame's first one. |
| 45 | +inline bool isPayloadHeader(const uint8_t* h, size_t avail, size_t want, const uint8_t* pts) { |
| 46 | + if (avail < 2) return false; |
| 47 | + const uint8_t len = h[0], flags = h[1]; |
| 48 | + if (len != want || len != headerLenFor(flags) || avail < len) return false; |
| 49 | + return headerFlagsOk(flags) && headerScrOk(h, flags, len) && headerPtsOk(h, flags, pts); |
| 50 | +} |
| 51 | + |
| 52 | +/// Whether headers of length `hl` recur after `first`, which is what rules out a chance match. |
| 53 | +inline bool confirmedRun(const uint8_t* d, size_t len, size_t first, size_t hl) { |
| 54 | + const uint8_t* pts = (d[first + 1] & kPts) ? d + first + 2 : nullptr; |
| 55 | + const size_t stride = first + hl; |
| 56 | + size_t seen = 0; |
| 57 | + for (size_t at = first + stride; at + hl <= len && seen < kConfirmStrides; at += stride, seen++) |
| 58 | + if (!isPayloadHeader(d + at, len - at, hl, pts)) return false; |
| 59 | + return seen > 0 || first + stride + hl > len; // a frame too short to confirm is taken as it is |
| 60 | +} |
| 61 | + |
| 62 | +/// The offset of the first confirmed header of length `hl`, or 0 when the frame carries none. |
| 63 | +inline size_t findHeader(const uint8_t* d, size_t len, size_t hl) { |
| 64 | + for (size_t i = kBulkMps - hl; i + hl <= len; i += kBulkMps) |
| 65 | + if (isPayloadHeader(d + i, len - i, hl, nullptr) && confirmedRun(d, len, i, hl)) return i; |
| 66 | + return 0; |
| 67 | +} |
| 68 | + |
| 69 | +/// Close the gaps the headers from `hdr` on leave, answering the compacted length. |
| 70 | +inline size_t compactFrom(uint8_t* d, size_t len, size_t hdr, size_t hl) { |
| 71 | + const size_t payload = hdr + hl; |
| 72 | + uint8_t pts[4] = {}; // copied: the compaction overwrites the header it came from |
| 73 | + const bool hasPts = (d[hdr + 1] & kPts) != 0; |
| 74 | + if (hasPts) std::memcpy(pts, d + hdr + 2, sizeof pts); |
| 75 | + size_t w = hdr, r = hdr; |
| 76 | + while (isPayloadHeader(d + r, len - r, hl, hasPts ? pts : nullptr)) { |
| 77 | + r += hl; |
| 78 | + const size_t run = payload - hl < len - r ? payload - hl : len - r; |
| 79 | + std::memmove(d + w, d + r, run); |
| 80 | + w += run; |
| 81 | + r += run; |
| 82 | + } |
| 83 | + std::memmove(d + w, d + r, len - r); // whatever follows a missing header is left as delivered |
| 84 | + return w + (len - r); |
| 85 | +} |
| 86 | + |
| 87 | +/// Drops the payload headers a bulk frame still carries, answering the compacted length. |
| 88 | +inline size_t stripPayloadHeaders(uint8_t* d, size_t len) { |
| 89 | + static constexpr size_t kLens[] = {12, 8, 6, 2}; // longest first: the more fields, the surer the match |
| 90 | + for (size_t hl : kLens) { |
| 91 | + const size_t hdr = findHeader(d, len, hl); |
| 92 | + if (hdr) return compactFrom(d, len, hdr, hl); |
| 93 | + } |
| 94 | + return len; |
| 95 | +} |
| 96 | + |
| 97 | +// --- Choosing a mode ---------------------------------------------------------------------------- |
| 98 | + |
| 99 | +/// UVC states a rate as a period in 100 ns ticks. Rounded, so 59.94 fps reads as 60 rather than 59. |
| 100 | +inline uint8_t fpsFrom(uint32_t interval) { |
| 101 | + constexpr uint32_t kTicksPerSecond = 10000000; |
| 102 | + return interval ? static_cast<uint8_t>((kTicksPerSecond + interval / 2) / interval) : 0; |
| 103 | +} |
| 104 | + |
| 105 | +/// Of two advertised rates, the one to ask for: the fastest at or below `wanted`, else the slowest. |
| 106 | +inline uint8_t betterFps(uint8_t have, uint8_t row, uint8_t wanted) { |
| 107 | + if (!have) return row; |
| 108 | + const bool haveOk = have <= wanted, rowOk = row <= wanted; |
| 109 | + if (haveOk != rowOk) return haveOk ? have : row; |
| 110 | + if (haveOk) return row > have ? row : have; |
| 111 | + return row < have ? row : have; |
| 112 | +} |
| 113 | + |
| 114 | +/// Whether a mode covers the ceiling and is no bigger to decode than the best so far. |
| 115 | +inline bool modeFits(const Mode& m, uint16_t width, uint16_t height, uint32_t area, uint32_t bestArea) { |
| 116 | + if (m.width < width || m.height < height) return false; // below the ceiling it would be upscaled |
| 117 | + return !bestArea || area <= bestArea; // a bigger picture to decode every frame |
| 118 | +} |
| 119 | + |
| 120 | +/// The smallest mode at or above the ceiling, at the rate asked for, so decode and shrink stay cheap. |
| 121 | +inline bool pickMode(const Mode* modes, size_t count, uint16_t& width, uint16_t& height, uint8_t& fps) { |
| 122 | + uint32_t bestArea = 0; |
| 123 | + uint16_t bw = 0, bh = 0; |
| 124 | + uint8_t bestFps = 0; |
| 125 | + for (size_t i = 0; i < count; i++) { |
| 126 | + const Mode& m = modes[i]; |
| 127 | + const uint32_t area = static_cast<uint32_t>(m.width) * m.height; |
| 128 | + if (!modeFits(m, width, height, area, bestArea)) continue; |
| 129 | + // The rates accumulate per resolution, not per area: two shapes can share an area. |
| 130 | + if (m.width != bw || m.height != bh) bestFps = 0; |
| 131 | + bestArea = area; |
| 132 | + bw = m.width; |
| 133 | + bh = m.height; |
| 134 | + bestFps = betterFps(bestFps, m.fps, fps); |
| 135 | + } |
| 136 | + if (!bestArea) return false; // nothing listed, or nothing big enough: ask for what was wanted |
| 137 | + width = bw; |
| 138 | + height = bh; |
| 139 | + if (bestFps) fps = bestFps; |
| 140 | + return true; |
| 141 | +} |
| 142 | + |
| 143 | +// --- The decoded frame -------------------------------------------------------------------------- |
| 144 | + |
| 145 | +/// `v` rounded up to a multiple of `to`, the MCU boundary a decoder pads a frame's axis to. |
| 146 | +inline uint32_t alignUp(uint32_t v, uint8_t to) { return (v + to - 1u) / to * to; } |
| 147 | + |
| 148 | +/// The row stride in bytes the decoder wrote, or 0 when `outSize` matches no layout its MCU allows. |
| 149 | +inline size_t strideFor(uint16_t w, uint16_t h, uint8_t mcuW, uint8_t mcuH, uint32_t outSize) { |
| 150 | + if (!w || !h || !mcuW || !mcuH) return 0; |
| 151 | + if (outSize == static_cast<uint32_t>(w) * h * 3u) return static_cast<size_t>(w) * 3u; |
| 152 | + const uint32_t padW = alignUp(w, mcuW), padH = alignUp(h, mcuH); |
| 153 | + if (outSize == padW * padH * 3u) return static_cast<size_t>(padW) * 3u; |
| 154 | + return 0; |
| 155 | +} |
| 156 | + |
| 157 | +/// Each output pixel the mean of its rectangle, through the shared sampler so one fix reaches both paths. |
| 158 | +inline void areaAverage(const uint8_t* src, size_t stride, uint16_t w, uint16_t h, uint16_t ow, |
| 159 | + uint16_t oh, uint8_t* dst) { |
| 160 | + for (uint16_t y = 0; y < oh; y++) { |
| 161 | + const Span ry = cellSpan(y, oh, h); |
| 162 | + for (uint16_t x = 0; x < ow; x++) { |
| 163 | + const RGB mean = areaMean(src, stride, cellSpan(x, ow, w), ry); |
| 164 | + uint8_t* out = dst + (static_cast<size_t>(y) * ow + x) * 3; |
| 165 | + out[0] = mean.r; |
| 166 | + out[1] = mean.g; |
| 167 | + out[2] = mean.b; |
| 168 | + } |
| 169 | + } |
| 170 | +} |
| 171 | + |
| 172 | +} // namespace mm::usbvideo |
0 commit comments