mirror of
https://github.com/DeeJanuz/frametop.git
synced 2026-10-06 04:04:09 +02:00
Bring in frame-hands' hand tracking under hands/
The history of frame-hands (~/Desktop/Projects/frame-hands on the Frame), filtered to what moves: the camera broker (camd/), the tracker and its offline tools (trackd/), the shared file layouts (include/), the ncnn models, the analysis tools, and the calibration and model helpers they import from the Python prototype. The reverse-engineering notes, probes, camprobe, and the rest of the prototype stay in frame-hands. Unchanged here: the renames to ft- names and Frametop paths follow. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
DeeJanuzandClaude Opus 5.5 committed
commit
1a76d1560b
49 files changed
+7650
No files matched your search
@@ -0,0 +1,21 @@
|
||||
MIT License
|
||||
|
||||
Copyright (c) 2026 Curtis English
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in all
|
||||
copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
SOFTWARE.
|
||||
@@ -0,0 +1,15 @@
|
||||
CFLAGS ?= -O2 -g -Wall -Wextra -Wno-unused-parameter
|
||||
LDLIBS = -lm
|
||||
|
||||
all: fh-camprobe fh-camd
|
||||
|
||||
fh-camprobe: camprobe.c tp.c xrcams.c tp.h xrcams.h
|
||||
$(CC) $(CFLAGS) -o $@ camprobe.c tp.c xrcams.c $(LDLIBS)
|
||||
|
||||
fh-camd: camd.c tp.c xrcams.c tp.h xrcams.h fhring.h
|
||||
$(CC) $(CFLAGS) -o $@ camd.c tp.c xrcams.c $(LDLIBS)
|
||||
|
||||
clean:
|
||||
rm -f fh-camprobe fh-camd
|
||||
|
||||
.PHONY: all clean
|
||||
@@ -0,0 +1,63 @@
|
||||
# camd
|
||||
|
||||
Root-side camera access for frame-hands:
|
||||
|
||||
- `fh-camd`: the frame broker. It publishes the four IR tracking cameras, and optionally the Arcturus color pair, to a shared-memory ring that the unprivileged tracker reads.
|
||||
- `fh-camprobe`: a test tool that records timestamped frames from every camera, including the Arcturus color pair, with CSV logs.
|
||||
|
||||
## How it gets frames
|
||||
|
||||
XRService owns the headset cameras. Both tools borrow its DMA-BUFs read-only with `pidfd_getfd`, the same way FrameEyeCameraFeed does. They never touch XRService's V4L2 descriptors.
|
||||
|
||||
Polling buffers for changes can catch a frame while the camera is still writing it. Instead, they listen to the `v4l2:v4l2_dqbuf` tracepoint, which fires when XRService takes a buffer. It gives the buffer index, the sequence number and the capture timestamp.
|
||||
|
||||
The tools learn which DMA-BUF holds each V4L2 index by watching which buffer changes at each dequeue.
|
||||
|
||||
- Right after XRService allocates its buffers, the mapping is allocation order.
|
||||
- After XRService restarts streaming, the order is shuffled, and the mapping is learned index by index.
|
||||
- The two upper cameras share one run of buffers. For them, only allocation order can tell the cameras apart.
|
||||
- `fh-camd` also re-maps an index on the fly when its buffer holds no new frame.
|
||||
|
||||
## fh-camd
|
||||
|
||||
```
|
||||
make
|
||||
sudo ./fh-camd # runs until stopped or XRService exits
|
||||
```
|
||||
|
||||
It needs root only to set up: to borrow the buffers (`ptrace_scope=1` blocks `pidfd_getfd`) and to open the root-only tracepoints. Then it drops to the invoking user for good; XRService itself runs as that user. It reads nothing from the ring's readers.
|
||||
|
||||
Frames go to `/run/frame-hands/ir-ring`. The file is mode 0600 and owned by the user. It sits in a root-owned directory, so no other account can plant a file or link there. The layout is in `fhring.h`, and `tracker/ring.py` reads it.
|
||||
|
||||
- Only complete, bright frames are published. The cameras alternate a normal exposure with a near-black one, so each camera gets 30 of its 60 fps.
|
||||
- A copy torn by the camera overwriting the buffer is dropped.
|
||||
- Each copy takes about 0.1 ms, and a cache sync about 0.15 ms.
|
||||
|
||||
Options:
|
||||
|
||||
- `--with-dark`: also publish the near-black frames, as extra ring cameras flagged `FH_CAM_DARK`. They show only light sources, so they're no use for hands.
|
||||
- `--with-color`: also publish the two Arcturus color cameras, flagged `FH_CAM_COLOR`. Each is the luma of the 10-bit frame's valid 1972x2464 (the top 8 bits), at half size (`--color-scale 2`: 986x1232) and at most 30 fps (`--color-fps`; the cameras run at 60). Frames that carry the module's warped half-size copy are dropped. Their `capture_ns` is on the color module's clock (2.2 s off the mono cameras' on 2026-09-29), so line them up with the mono cameras by `dqbuf_ns`. Each frame costs about 0.65 ms of cache sync and 1.1 ms of decoding, so both cameras at 30 fps take about 11% of a core.
|
||||
- The ring holds 8 cameras: 4 mono, plus 4 dark twins or 2 color cameras.
|
||||
- `--sensor S`: only the mono cameras whose sensor name contains S.
|
||||
|
||||
It exits when XRService exits, or when a camera's buffers keep going stale, which means XRService has reallocated them. Start it again to re-attach.
|
||||
|
||||
## fh-camprobe
|
||||
|
||||
Wear the headset (cameras only stream while it's worn), then run:
|
||||
|
||||
```
|
||||
sudo ./fh-camprobe # records 15 s
|
||||
sudo ./fh-camprobe --list # discovery and tracepoints only
|
||||
```
|
||||
|
||||
Output goes to `~/Pictures/framecap/stereo-<time>/`:
|
||||
|
||||
- `summary.txt`: delays, buffer mapping, exposure pattern, stereo sync, and torn or stale copies.
|
||||
- `frames.csv`: one row per frame. `events.csv`: every tracepoint sample.
|
||||
- `<model>_NNNN_<camera>.pgm`: saved bright pairs. The color cameras are saved raw as `.yuv420_10p` (`tools/decode.py` reads them).
|
||||
- `plane1_*.bin`: raw plane 1 of a few frames, which may hold sensor metadata.
|
||||
|
||||
Which camera is which: video9 is `slam_left`, video13 is `slam_right`, video6 is `upper_left` and video7 is `upper_right`. This was checked by rendering the same view from each camera with the factory calibration. But fh-camd tells the side cameras' buffers apart only by XRService's allocation order, and after some XRService restarts it gets them backwards: check with `tools/check_sides.py --ring` and run the tracker with `--swap-sides` when it says swapped. The color cameras are video3 (`arcimx616 0-0010`) and video0 (`0-001a`); which of them is `passthrough_left` in the module's calibration is for `tools/check_color.py` to settle on a recording with texture in view. `tracker/live.py` maps them by capture pipe (`/sys/class/video4linux/videoN/name`).
|
||||
|
||||
`discovery` in `xrcams.c` is adapted from FrameEyeCameraFeed (MIT, see `LICENSE.FrameEyeCameraFeed`).
|
||||
+1066
File diff suppressed because it is too large.
Load diff
@@ -0,0 +1,87 @@
|
||||
/*
|
||||
* fhring - the shared-memory frame ring fh-camd writes and trackers read.
|
||||
*
|
||||
* One file (FH_RING_PATH) holds a header, then for each camera
|
||||
* a few slots, each a slot header followed by the image rows packed tightly
|
||||
* (stride == width for 8-bit mono). Only complete, bright frames are published.
|
||||
*
|
||||
* Writer, for frame n of a camera: slot = n % nslots
|
||||
* slot.seq = 2n+1; write slot fields and pixels; slot.seq = 2n+2; cam.latest = n
|
||||
* Reader:
|
||||
* n = cam.latest; read slot.seq, expect 2n+2; copy; re-read slot.seq; if it
|
||||
* changed the copy is torn, retry with the new latest.
|
||||
*
|
||||
* All multi-byte fields are little-endian; offsets are fixed so Python can read
|
||||
* them with struct (tracker/ring.py mirrors this file).
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <assert.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#define FH_RING_MAGIC "FHRING01"
|
||||
#define FH_RING_VERSION 1
|
||||
#define FH_RING_MAX_CAMS 8
|
||||
#define FH_RING_SLOTS 4
|
||||
#define FH_RING_DIR "/run/frame-hands"
|
||||
#define FH_RING_PATH FH_RING_DIR "/ir-ring"
|
||||
|
||||
enum {
|
||||
FH_FMT_GREY8 = 0,
|
||||
};
|
||||
|
||||
enum {
|
||||
FH_CAM_DARK = 1u << 0, /* the near-black exposures between this node's */
|
||||
/* normal frames (fh-camd --with-dark) */
|
||||
FH_CAM_COLOR = 1u << 1, /* an Arcturus color camera's luma, downscaled */
|
||||
/* (fh-camd --with-color). Not synced with the */
|
||||
/* mono cameras, and capture_ns is on its own */
|
||||
/* clock: line it up with them by dqbuf_ns */
|
||||
};
|
||||
|
||||
typedef struct {
|
||||
char sensor[32]; /* media entity, e.g. "og01a1b 4-0060" */
|
||||
char name[32]; /* calibration name if known, else sensor slug */
|
||||
int32_t node; /* N of /dev/videoN */
|
||||
uint32_t format; /* FH_FMT_* */
|
||||
uint32_t width;
|
||||
uint32_t height;
|
||||
uint32_t stride; /* bytes per row in the ring */
|
||||
uint32_t nslots;
|
||||
uint64_t slot_offset; /* file offset of slot 0 */
|
||||
uint64_t slot_bytes; /* slot header + image, 64-byte aligned */
|
||||
volatile uint64_t latest; /* newest published frame number, 0 = none yet */
|
||||
uint64_t published; /* frames published */
|
||||
uint64_t dropped; /* dark, stale or torn frames not published */
|
||||
uint32_t flags; /* FH_CAM_* */
|
||||
uint8_t reserved[28];
|
||||
} fh_ring_cam_t; /* 160 bytes */
|
||||
|
||||
typedef struct {
|
||||
volatile uint64_t seq; /* 2n+1 while frame n is written, 2n+2 when done */
|
||||
uint64_t frame; /* n */
|
||||
uint64_t capture_ns; /* V4L2 timestamp (camera clock) */
|
||||
uint64_t dqbuf_ns; /* CLOCK_MONOTONIC when XRService dequeued it */
|
||||
uint64_t publish_ns; /* CLOCK_MONOTONIC when the copy finished */
|
||||
uint32_t v4l2_seq; /* V4L2 sequence number */
|
||||
float mean; /* mean luma on a sparse grid */
|
||||
uint8_t reserved[16];
|
||||
} fh_ring_slot_t; /* 64 bytes, image follows */
|
||||
|
||||
typedef struct {
|
||||
char magic[8]; /* FH_RING_MAGIC */
|
||||
uint32_t version;
|
||||
uint32_t header_bytes; /* sizeof(fh_ring_hdr_t) */
|
||||
uint32_t ncams;
|
||||
uint32_t reserved0;
|
||||
uint64_t file_bytes;
|
||||
int64_t writer_pid;
|
||||
volatile uint64_t heartbeat_ns; /* CLOCK_MONOTONIC, refreshed at least every 0.2 s */
|
||||
uint8_t reserved[16];
|
||||
fh_ring_cam_t cams[FH_RING_MAX_CAMS];
|
||||
} fh_ring_hdr_t;
|
||||
|
||||
static_assert(sizeof(fh_ring_cam_t) == 160, "fh_ring_cam_t layout");
|
||||
static_assert(sizeof(fh_ring_slot_t) == 64, "fh_ring_slot_t layout");
|
||||
static_assert(sizeof(fh_ring_hdr_t) == 64 + 160 * FH_RING_MAX_CAMS, "fh_ring_hdr_t layout");
|
||||
+427
@@ -0,0 +1,427 @@
|
||||
/*
|
||||
* tp - read kernel tracepoints system-wide through perf_event_open.
|
||||
*/
|
||||
|
||||
#define _GNU_SOURCE
|
||||
|
||||
#include "tp.h"
|
||||
|
||||
#include <errno.h>
|
||||
#include <stdarg.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/epoll.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/mman.h>
|
||||
#include <sys/syscall.h>
|
||||
#include <time.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <linux/perf_event.h>
|
||||
|
||||
#ifndef TRACEFS
|
||||
#define TRACEFS "/sys/kernel/tracing/events"
|
||||
#endif
|
||||
#define RING_DATA_PAGES 16
|
||||
|
||||
static void set_err(char *err, size_t n, const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
|
||||
va_start(ap, fmt);
|
||||
vsnprintf(err, n, fmt, ap);
|
||||
va_end(ap);
|
||||
}
|
||||
|
||||
bool tp_event_load(tp_event_t *ev, const char *system, const char *name, char *err, size_t errn)
|
||||
{
|
||||
memset(ev, 0, sizeof(*ev));
|
||||
snprintf(ev->system, sizeof(ev->system), "%s", system);
|
||||
snprintf(ev->name, sizeof(ev->name), "%s", name);
|
||||
ev->id = -1;
|
||||
|
||||
char path[256];
|
||||
snprintf(path, sizeof(path), TRACEFS "/%s/%s/format", system, name);
|
||||
|
||||
FILE *f = fopen(path, "r");
|
||||
|
||||
if (!f) {
|
||||
set_err(err, errn, "%s: %s", path, strerror(errno));
|
||||
return false;
|
||||
}
|
||||
|
||||
char line[512];
|
||||
|
||||
while (fgets(line, sizeof(line), f)) {
|
||||
|
||||
int id;
|
||||
|
||||
if (sscanf(line, "ID: %d", &id) == 1) {
|
||||
ev->id = id;
|
||||
continue;
|
||||
}
|
||||
|
||||
char *fp = line;
|
||||
|
||||
while (*fp == ' ' || *fp == '\t')
|
||||
fp++;
|
||||
|
||||
if (strncmp(fp, "field:", 6) || ev->nfields >= TP_MAX_FIELDS)
|
||||
continue;
|
||||
|
||||
char *semi = strchr(fp, ';');
|
||||
|
||||
if (!semi)
|
||||
continue;
|
||||
|
||||
/* the field name is the last identifier in the declaration */
|
||||
char decl[256];
|
||||
size_t dl = (size_t)(semi - (fp + 6));
|
||||
|
||||
if (dl >= sizeof(decl))
|
||||
dl = sizeof(decl) - 1;
|
||||
|
||||
memcpy(decl, fp + 6, dl);
|
||||
decl[dl] = 0;
|
||||
|
||||
char *br = strchr(decl, '[');
|
||||
|
||||
if (br)
|
||||
*br = 0;
|
||||
|
||||
char *end = decl + strlen(decl);
|
||||
|
||||
while (end > decl && (end[-1] == ' ' || end[-1] == '\t'))
|
||||
*--end = 0;
|
||||
|
||||
char *start = end;
|
||||
|
||||
while (start > decl && start[-1] != ' ' && start[-1] != '\t' && start[-1] != '*')
|
||||
start--;
|
||||
|
||||
tp_field_t *fd = &ev->fields[ev->nfields];
|
||||
const char *o = strstr(semi, "offset:");
|
||||
const char *s = strstr(semi, "size:");
|
||||
const char *g = strstr(semi, "signed:");
|
||||
|
||||
if (!o || !s)
|
||||
continue;
|
||||
|
||||
snprintf(fd->name, sizeof(fd->name), "%s", start);
|
||||
fd->offset = atoi(o + 7);
|
||||
fd->size = atoi(s + 5);
|
||||
fd->is_signed = g ? atoi(g + 7) != 0 : false;
|
||||
ev->nfields++;
|
||||
}
|
||||
|
||||
fclose(f);
|
||||
|
||||
if (ev->id < 0) {
|
||||
set_err(err, errn, "%s: no ID line", path);
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
int tp_field(const tp_event_t *ev, const char *name)
|
||||
{
|
||||
for (int i = 0; i < ev->nfields; i++)
|
||||
if (!strcmp(ev->fields[i].name, name))
|
||||
return i;
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
int64_t tp_get(const tp_event_t *ev, int field, const uint8_t *raw, uint32_t rawlen)
|
||||
{
|
||||
if (field < 0 || field >= ev->nfields)
|
||||
return 0;
|
||||
|
||||
const tp_field_t *f = &ev->fields[field];
|
||||
|
||||
if (f->offset < 0 || (uint32_t)(f->offset + f->size) > rawlen)
|
||||
return 0;
|
||||
|
||||
const uint8_t *p = raw + f->offset;
|
||||
|
||||
switch (f->size) {
|
||||
case 1: { uint8_t v; memcpy(&v, p, 1); return f->is_signed ? (int64_t)(int8_t)v : (int64_t)v; }
|
||||
case 2: { uint16_t v; memcpy(&v, p, 2); return f->is_signed ? (int64_t)(int16_t)v : (int64_t)v; }
|
||||
case 4: { uint32_t v; memcpy(&v, p, 4); return f->is_signed ? (int64_t)(int32_t)v : (int64_t)v; }
|
||||
case 8: { uint64_t v; memcpy(&v, p, 8); return (int64_t)v; }
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
static int online_cpus(int *cpus, int max)
|
||||
{
|
||||
FILE *f = fopen("/sys/devices/system/cpu/online", "r");
|
||||
int n = 0;
|
||||
|
||||
if (!f)
|
||||
return 0;
|
||||
|
||||
char buf[256] = {0};
|
||||
|
||||
if (!fgets(buf, sizeof(buf), f))
|
||||
buf[0] = 0;
|
||||
|
||||
fclose(f);
|
||||
|
||||
for (char *tok = strtok(buf, ",\n"); tok && n < max; tok = strtok(NULL, ",\n")) {
|
||||
|
||||
int a, b;
|
||||
|
||||
if (sscanf(tok, "%d-%d", &a, &b) == 2) {
|
||||
for (int c = a; c <= b && n < max; c++)
|
||||
cpus[n++] = c;
|
||||
} else if (sscanf(tok, "%d", &a) == 1) {
|
||||
cpus[n++] = a;
|
||||
}
|
||||
}
|
||||
|
||||
return n;
|
||||
}
|
||||
|
||||
bool tp_open(tp_t *tp, tp_event_t **events, int nevents, char *err, size_t errn)
|
||||
{
|
||||
memset(tp, 0, sizeof(*tp));
|
||||
tp->epfd = -1;
|
||||
|
||||
if (nevents <= 0 || nevents > TP_MAX_EVENTS) {
|
||||
set_err(err, errn, "bad event count %d", nevents);
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int i = 0; i < nevents; i++)
|
||||
tp->events[i] = events[i];
|
||||
|
||||
tp->nevents = nevents;
|
||||
|
||||
int cpus[TP_MAX_CPUS];
|
||||
tp->ncpu = online_cpus(cpus, TP_MAX_CPUS);
|
||||
|
||||
if (tp->ncpu <= 0) {
|
||||
set_err(err, errn, "no online CPUs found");
|
||||
return false;
|
||||
}
|
||||
|
||||
long page = sysconf(_SC_PAGESIZE);
|
||||
tp->map_len = (size_t)page * (1 + RING_DATA_PAGES);
|
||||
|
||||
tp->epfd = epoll_create1(EPOLL_CLOEXEC);
|
||||
|
||||
if (tp->epfd < 0) {
|
||||
set_err(err, errn, "epoll_create1: %s", strerror(errno));
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int c = 0; c < tp->ncpu; c++) {
|
||||
|
||||
tp->ring_fd[c] = -1;
|
||||
|
||||
for (int e = 0; e < nevents; e++) {
|
||||
|
||||
struct perf_event_attr a;
|
||||
memset(&a, 0, sizeof(a));
|
||||
|
||||
a.size = sizeof(a);
|
||||
a.type = PERF_TYPE_TRACEPOINT;
|
||||
a.config = (uint64_t)events[e]->id;
|
||||
a.sample_period = 1;
|
||||
a.sample_type = PERF_SAMPLE_TID | PERF_SAMPLE_TIME | PERF_SAMPLE_CPU | PERF_SAMPLE_RAW;
|
||||
a.wakeup_events = 1;
|
||||
a.use_clockid = 1;
|
||||
a.clockid = CLOCK_MONOTONIC;
|
||||
a.disabled = 1;
|
||||
|
||||
int fd = (int)syscall(SYS_perf_event_open, &a, -1, cpus[c], -1, PERF_FLAG_FD_CLOEXEC);
|
||||
|
||||
if (fd < 0) {
|
||||
set_err(err, errn, "perf_event_open(%s:%s, cpu %d): %s",
|
||||
events[e]->system, events[e]->name, cpus[c], strerror(errno));
|
||||
tp_close(tp);
|
||||
return false;
|
||||
}
|
||||
|
||||
tp->fds[tp->nfds++] = fd;
|
||||
|
||||
if (tp->ring_fd[c] < 0) {
|
||||
|
||||
void *m = mmap(NULL, tp->map_len, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
|
||||
|
||||
if (m == MAP_FAILED) {
|
||||
set_err(err, errn, "mmap perf ring (cpu %d): %s", cpus[c], strerror(errno));
|
||||
tp_close(tp);
|
||||
return false;
|
||||
}
|
||||
|
||||
tp->ring[c] = m;
|
||||
tp->ring_fd[c] = fd;
|
||||
|
||||
struct epoll_event ee = { .events = EPOLLIN, .data.u32 = (uint32_t)c };
|
||||
epoll_ctl(tp->epfd, EPOLL_CTL_ADD, fd, &ee);
|
||||
|
||||
} else if (ioctl(fd, PERF_EVENT_IOC_SET_OUTPUT, tp->ring_fd[c]) < 0) {
|
||||
set_err(err, errn, "PERF_EVENT_IOC_SET_OUTPUT: %s", strerror(errno));
|
||||
tp_close(tp);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < tp->nfds; i++)
|
||||
ioctl(tp->fds[i], PERF_EVENT_IOC_ENABLE, 0);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static void ring_copy(uint8_t *dst, const uint8_t *base, uint64_t size, uint64_t pos, size_t len)
|
||||
{
|
||||
uint64_t off = pos % size;
|
||||
size_t first = (size_t)(size - off);
|
||||
|
||||
if (first >= len) {
|
||||
memcpy(dst, base + off, len);
|
||||
} else {
|
||||
memcpy(dst, base + off, first);
|
||||
memcpy(dst + first, base, len - first);
|
||||
}
|
||||
}
|
||||
|
||||
static int cmp_sample(const void *a, const void *b)
|
||||
{
|
||||
const tp_sample_t *x = a, *y = b;
|
||||
|
||||
return (x->time > y->time) - (x->time < y->time);
|
||||
}
|
||||
|
||||
static void dispatch(tp_t *tp, tp_cb cb, void *ctx)
|
||||
{
|
||||
qsort(tp->pend, tp->npend, sizeof(tp->pend[0]), cmp_sample);
|
||||
|
||||
for (int i = 0; i < tp->npend; i++)
|
||||
cb(ctx, &tp->pend[i]);
|
||||
|
||||
tp->npend = 0;
|
||||
}
|
||||
|
||||
static int drain_ring(tp_t *tp, int c, tp_cb cb, void *ctx)
|
||||
{
|
||||
struct perf_event_mmap_page *pg = tp->ring[c];
|
||||
long page = sysconf(_SC_PAGESIZE);
|
||||
uint64_t off = pg->data_offset ? pg->data_offset : (uint64_t)page;
|
||||
uint64_t size = pg->data_size ? pg->data_size : (uint64_t)page * RING_DATA_PAGES;
|
||||
const uint8_t *base = (const uint8_t *)pg + off;
|
||||
|
||||
uint64_t head = __atomic_load_n(&pg->data_head, __ATOMIC_ACQUIRE);
|
||||
uint64_t tail = pg->data_tail;
|
||||
int n = 0;
|
||||
|
||||
while (tail < head) {
|
||||
|
||||
struct perf_event_header hdr;
|
||||
ring_copy((uint8_t *)&hdr, base, size, tail, sizeof(hdr));
|
||||
|
||||
if (hdr.size < sizeof(hdr))
|
||||
break;
|
||||
|
||||
ring_copy(tp->scratch, base, size, tail, hdr.size);
|
||||
|
||||
const uint8_t *p = tp->scratch + sizeof(hdr);
|
||||
const uint8_t *end = tp->scratch + hdr.size;
|
||||
|
||||
if (hdr.type == PERF_RECORD_LOST && end - p >= 16) {
|
||||
|
||||
uint64_t lost;
|
||||
memcpy(&lost, p + 8, 8);
|
||||
tp->lost += lost;
|
||||
|
||||
} else if (hdr.type == PERF_RECORD_SAMPLE && end - p >= 28) {
|
||||
|
||||
tp_sample_t s;
|
||||
uint32_t v32[2];
|
||||
|
||||
memcpy(v32, p, 8); p += 8;
|
||||
s.pid = v32[0];
|
||||
s.tid = v32[1];
|
||||
memcpy(&s.time, p, 8); p += 8;
|
||||
memcpy(v32, p, 8); p += 8;
|
||||
s.cpu = v32[0];
|
||||
memcpy(&s.rawlen, p, 4); p += 4;
|
||||
s.raw = p;
|
||||
|
||||
if (s.rawlen >= 2 && p + s.rawlen <= end) {
|
||||
|
||||
uint16_t type;
|
||||
memcpy(&type, s.raw, 2);
|
||||
s.ev = NULL;
|
||||
|
||||
for (int e = 0; e < tp->nevents; e++)
|
||||
if (tp->events[e]->id == type)
|
||||
s.ev = tp->events[e];
|
||||
|
||||
if (s.ev && s.rawlen <= TP_MAX_RAW) {
|
||||
|
||||
if (tp->npend == TP_MAX_PENDING)
|
||||
dispatch(tp, cb, ctx);
|
||||
|
||||
memcpy(tp->pend_raw[tp->npend], s.raw, s.rawlen);
|
||||
s.raw = tp->pend_raw[tp->npend];
|
||||
tp->pend[tp->npend++] = s;
|
||||
n++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
tail += hdr.size;
|
||||
}
|
||||
|
||||
__atomic_store_n(&pg->data_tail, tail, __ATOMIC_RELEASE);
|
||||
|
||||
return n;
|
||||
}
|
||||
|
||||
int tp_poll(tp_t *tp, int timeout_ms, tp_cb cb, void *ctx)
|
||||
{
|
||||
struct epoll_event ev[TP_MAX_CPUS];
|
||||
|
||||
if (epoll_wait(tp->epfd, ev, TP_MAX_CPUS, timeout_ms) < 0 && errno != EINTR)
|
||||
return -1;
|
||||
|
||||
/*
|
||||
* Drain every ring, not just the ones that woke us: samples from several
|
||||
* CPUs need to be handled together to keep per-camera order sane.
|
||||
*/
|
||||
int n = 0;
|
||||
|
||||
for (int c = 0; c < tp->ncpu; c++)
|
||||
if (tp->ring[c])
|
||||
n += drain_ring(tp, c, cb, ctx);
|
||||
|
||||
dispatch(tp, cb, ctx);
|
||||
|
||||
return n;
|
||||
}
|
||||
|
||||
void tp_close(tp_t *tp)
|
||||
{
|
||||
for (int i = 0; i < tp->nfds; i++) {
|
||||
ioctl(tp->fds[i], PERF_EVENT_IOC_DISABLE, 0);
|
||||
}
|
||||
|
||||
for (int c = 0; c < tp->ncpu; c++)
|
||||
if (tp->ring[c])
|
||||
munmap(tp->ring[c], tp->map_len);
|
||||
|
||||
for (int i = 0; i < tp->nfds; i++)
|
||||
close(tp->fds[i]);
|
||||
|
||||
if (tp->epfd >= 0)
|
||||
close(tp->epfd);
|
||||
|
||||
tp->nfds = 0;
|
||||
tp->epfd = -1;
|
||||
}
|
||||
@@ -0,0 +1,76 @@
|
||||
/*
|
||||
* tp - read kernel tracepoints system-wide through perf_event_open.
|
||||
*
|
||||
* One perf ring per CPU; every event on that CPU writes into it. Field
|
||||
* offsets come from the tracefs format files, so kernel layout changes don't
|
||||
* silently break parsing. Needs root (or CAP_PERFMON plus tracefs access).
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#define TP_MAX_FIELDS 40
|
||||
#define TP_MAX_EVENTS 8
|
||||
#define TP_MAX_CPUS 64
|
||||
#define TP_MAX_PENDING 2048
|
||||
#define TP_MAX_RAW 256
|
||||
|
||||
typedef struct {
|
||||
char name[48];
|
||||
int offset;
|
||||
int size;
|
||||
bool is_signed;
|
||||
} tp_field_t;
|
||||
|
||||
typedef struct {
|
||||
char system[32];
|
||||
char name[48];
|
||||
int id;
|
||||
tp_field_t fields[TP_MAX_FIELDS];
|
||||
int nfields;
|
||||
} tp_event_t;
|
||||
|
||||
typedef struct {
|
||||
const tp_event_t *ev;
|
||||
const uint8_t *raw;
|
||||
uint32_t rawlen;
|
||||
uint64_t time; /* CLOCK_MONOTONIC ns */
|
||||
uint32_t cpu;
|
||||
uint32_t pid;
|
||||
uint32_t tid;
|
||||
} tp_sample_t;
|
||||
|
||||
typedef void (*tp_cb)(void *ctx, const tp_sample_t *s);
|
||||
|
||||
typedef struct {
|
||||
int ncpu;
|
||||
int ring_fd[TP_MAX_CPUS];
|
||||
void *ring[TP_MAX_CPUS];
|
||||
size_t map_len;
|
||||
int fds[TP_MAX_CPUS * TP_MAX_EVENTS];
|
||||
int nfds;
|
||||
int epfd;
|
||||
tp_event_t *events[TP_MAX_EVENTS];
|
||||
int nevents;
|
||||
uint64_t lost;
|
||||
uint8_t scratch[65536];
|
||||
/* samples drained from all rings, sorted by time before dispatch */
|
||||
tp_sample_t pend[TP_MAX_PENDING];
|
||||
uint8_t pend_raw[TP_MAX_PENDING][TP_MAX_RAW];
|
||||
int npend;
|
||||
} tp_t;
|
||||
|
||||
bool tp_event_load(tp_event_t *ev, const char *system, const char *name, char *err, size_t errn);
|
||||
int tp_field(const tp_event_t *ev, const char *name);
|
||||
int64_t tp_get(const tp_event_t *ev, int field, const uint8_t *raw, uint32_t rawlen);
|
||||
|
||||
bool tp_open(tp_t *tp, tp_event_t **events, int nevents, char *err, size_t errn);
|
||||
/*
|
||||
* Wait up to timeout_ms, then hand every pending sample to cb in time order,
|
||||
* across all CPUs. Returns samples read, -1 on error.
|
||||
*/
|
||||
int tp_poll(tp_t *tp, int timeout_ms, tp_cb cb, void *ctx);
|
||||
void tp_close(tp_t *tp);
|
||||
@@ -0,0 +1,783 @@
|
||||
/*
|
||||
* xrcams - find the headset cameras and the DMA-BUF queues XRService feeds them.
|
||||
*
|
||||
* Adapted from framecap.c in FrameEyeCameraFeed (vendor/FrameEyeCameraFeed),
|
||||
* MIT License, Copyright (c) 2026 Curtis English. See LICENSE.FrameEyeCameraFeed.
|
||||
*
|
||||
* Everything is discovered rather than hardcoded:
|
||||
* - XRService is found by scanning /proc for its cmdline.
|
||||
* - The V4L2 nodes and sensor subdevs it holds open come from /proc/<pid>/fd.
|
||||
* - Each node's geometry comes from VIDIOC_G_FMT on our own handle.
|
||||
* - Each node is traced back to its sensor through MEDIA_IOC_G_TOPOLOGY.
|
||||
* - Buffers are split into queues by allocation order: XRService opens a
|
||||
* sensor subdev, then allocates that camera's buffers.
|
||||
*/
|
||||
|
||||
#define _GNU_SOURCE
|
||||
|
||||
#include "xrcams.h"
|
||||
|
||||
#include <dirent.h>
|
||||
#include <errno.h>
|
||||
#include <fcntl.h>
|
||||
#include <stdarg.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/sysmacros.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <linux/media.h>
|
||||
|
||||
#ifndef MEDIA_ENT_F_CAM_SENSOR
|
||||
#define MEDIA_ENT_F_CAM_SENSOR 0x00020001
|
||||
#endif
|
||||
|
||||
#define MAX_FDENTS 4096
|
||||
#define MAX_TOPOS 8
|
||||
|
||||
enum fdkind { FD_DMABUF, FD_SUBDEV_SENSOR, FD_VIDEO };
|
||||
|
||||
typedef struct {
|
||||
int xfd;
|
||||
enum fdkind kind;
|
||||
size_t size;
|
||||
unsigned long ino;
|
||||
char sensor[XR_SENSOR_LEN];
|
||||
char path[64];
|
||||
} fdent_t;
|
||||
|
||||
typedef struct {
|
||||
struct media_v2_entity *ents;
|
||||
struct media_v2_interface *intfs;
|
||||
struct media_v2_pad *pads;
|
||||
struct media_v2_link *links;
|
||||
__u32 nents, nintfs, npads, nlinks;
|
||||
} topo_t;
|
||||
|
||||
static fdent_t fdents[MAX_FDENTS];
|
||||
static int nfdents;
|
||||
static topo_t topos[MAX_TOPOS];
|
||||
static int ntopos;
|
||||
|
||||
static void set_err(char *err, size_t n, const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
|
||||
va_start(ap, fmt);
|
||||
vsnprintf(err, n, fmt, ap);
|
||||
va_end(ap);
|
||||
}
|
||||
|
||||
void xr_slugify(const char *in, char *out, size_t n)
|
||||
{
|
||||
size_t i = 0;
|
||||
|
||||
for (; in[i] && i + 1 < n; i++)
|
||||
out[i] = (in[i] == ' ' || in[i] == '/') ? '_' : in[i];
|
||||
|
||||
out[i] = 0;
|
||||
}
|
||||
|
||||
/* --------------------------------------------------- media graph handling */
|
||||
|
||||
static void topo_free_all(void)
|
||||
{
|
||||
for (int i = 0; i < ntopos; i++) {
|
||||
free(topos[i].ents);
|
||||
free(topos[i].intfs);
|
||||
free(topos[i].pads);
|
||||
free(topos[i].links);
|
||||
}
|
||||
|
||||
ntopos = 0;
|
||||
}
|
||||
|
||||
static void topo_load_all(void)
|
||||
{
|
||||
for (int mi = 0; mi < MAX_TOPOS; mi++) {
|
||||
|
||||
char mpath[32];
|
||||
snprintf(mpath, sizeof(mpath), "/dev/media%d", mi);
|
||||
|
||||
int mfd = open(mpath, O_RDWR | O_CLOEXEC);
|
||||
|
||||
if (mfd < 0)
|
||||
continue;
|
||||
|
||||
struct media_v2_topology t;
|
||||
memset(&t, 0, sizeof(t));
|
||||
|
||||
if (ioctl(mfd, MEDIA_IOC_G_TOPOLOGY, &t) < 0) {
|
||||
close(mfd);
|
||||
continue;
|
||||
}
|
||||
|
||||
topo_t *o = &topos[ntopos];
|
||||
memset(o, 0, sizeof(*o));
|
||||
|
||||
o->nents = t.num_entities;
|
||||
o->nintfs = t.num_interfaces;
|
||||
o->npads = t.num_pads;
|
||||
o->nlinks = t.num_links;
|
||||
|
||||
o->ents = calloc(o->nents ? o->nents : 1, sizeof(*o->ents));
|
||||
o->intfs = calloc(o->nintfs ? o->nintfs : 1, sizeof(*o->intfs));
|
||||
o->pads = calloc(o->npads ? o->npads : 1, sizeof(*o->pads));
|
||||
o->links = calloc(o->nlinks ? o->nlinks : 1, sizeof(*o->links));
|
||||
|
||||
t.ptr_entities = (__u64)(uintptr_t)o->ents;
|
||||
t.ptr_interfaces = (__u64)(uintptr_t)o->intfs;
|
||||
t.ptr_pads = (__u64)(uintptr_t)o->pads;
|
||||
t.ptr_links = (__u64)(uintptr_t)o->links;
|
||||
|
||||
bool ok = o->ents && o->intfs && o->pads && o->links &&
|
||||
ioctl(mfd, MEDIA_IOC_G_TOPOLOGY, &t) == 0;
|
||||
close(mfd);
|
||||
|
||||
if (!ok) {
|
||||
free(o->ents); free(o->intfs); free(o->pads); free(o->links);
|
||||
continue;
|
||||
}
|
||||
|
||||
ntopos++;
|
||||
}
|
||||
}
|
||||
|
||||
static struct media_v2_entity *topo_entity(topo_t *t, __u32 id)
|
||||
{
|
||||
for (__u32 i = 0; i < t->nents; i++)
|
||||
if (t->ents[i].id == id)
|
||||
return &t->ents[i];
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static struct media_v2_pad *topo_pad(topo_t *t, __u32 id)
|
||||
{
|
||||
for (__u32 i = 0; i < t->npads; i++)
|
||||
if (t->pads[i].id == id)
|
||||
return &t->pads[i];
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static __u32 topo_entity_for_devnode(topo_t *t, dev_t rdev)
|
||||
{
|
||||
__u32 intf_id = 0;
|
||||
|
||||
for (__u32 i = 0; i < t->nintfs; i++)
|
||||
if (t->intfs[i].devnode.major == major(rdev) &&
|
||||
t->intfs[i].devnode.minor == minor(rdev)) {
|
||||
intf_id = t->intfs[i].id;
|
||||
break;
|
||||
}
|
||||
|
||||
if (!intf_id)
|
||||
return 0;
|
||||
|
||||
for (__u32 i = 0; i < t->nlinks; i++)
|
||||
if ((t->links[i].flags & MEDIA_LNK_FL_LINK_TYPE) == MEDIA_LNK_FL_INTERFACE_LINK &&
|
||||
t->links[i].source_id == intf_id)
|
||||
return t->links[i].sink_id;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
* Walk upstream across enabled data links until a sensor is reached. A CSIPHY
|
||||
* carries two sensors on separate (sink, source) pad pairs, so re-enter on the
|
||||
* sink pad paired with the source pad we left through.
|
||||
*/
|
||||
static bool topo_walk_to_sensor(topo_t *t, __u32 ent_id, char *out, size_t outn)
|
||||
{
|
||||
int exit_pad_index = -1;
|
||||
|
||||
for (int hop = 0; hop < 32 && ent_id; hop++) {
|
||||
|
||||
struct media_v2_entity *e = topo_entity(t, ent_id);
|
||||
|
||||
if (!e)
|
||||
return false;
|
||||
|
||||
if (e->function == MEDIA_ENT_F_CAM_SENSOR) {
|
||||
snprintf(out, outn, "%s", e->name);
|
||||
return true;
|
||||
}
|
||||
|
||||
__u32 first_sink = 0, paired = 0;
|
||||
int nsinks = 0;
|
||||
|
||||
for (__u32 p = 0; p < t->npads; p++) {
|
||||
|
||||
if (t->pads[p].entity_id != ent_id || !(t->pads[p].flags & MEDIA_PAD_FL_SINK))
|
||||
continue;
|
||||
|
||||
nsinks++;
|
||||
|
||||
if (!first_sink)
|
||||
first_sink = t->pads[p].id;
|
||||
|
||||
if (exit_pad_index >= 1 && (int)t->pads[p].index == exit_pad_index - 1)
|
||||
paired = t->pads[p].id;
|
||||
}
|
||||
|
||||
__u32 sink_pad = (nsinks == 1) ? first_sink : (paired ? paired : first_sink);
|
||||
|
||||
if (!sink_pad)
|
||||
return false;
|
||||
|
||||
__u32 src_pad = 0;
|
||||
|
||||
for (__u32 i = 0; i < t->nlinks; i++) {
|
||||
|
||||
if ((t->links[i].flags & MEDIA_LNK_FL_LINK_TYPE) != MEDIA_LNK_FL_DATA_LINK)
|
||||
continue;
|
||||
|
||||
if (!(t->links[i].flags & MEDIA_LNK_FL_ENABLED))
|
||||
continue;
|
||||
|
||||
if (t->links[i].sink_id == sink_pad) {
|
||||
src_pad = t->links[i].source_id;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
struct media_v2_pad *sp = src_pad ? topo_pad(t, src_pad) : NULL;
|
||||
|
||||
if (!sp)
|
||||
return false;
|
||||
|
||||
ent_id = sp->entity_id;
|
||||
exit_pad_index = (int)sp->index;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool sensor_for_video(dev_t rdev, char *out, size_t outn)
|
||||
{
|
||||
for (int i = 0; i < ntopos; i++) {
|
||||
|
||||
__u32 ent = topo_entity_for_devnode(&topos[i], rdev);
|
||||
|
||||
if (ent && topo_walk_to_sensor(&topos[i], ent, out, outn))
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool sensor_for_subdev(dev_t rdev, char *out, size_t outn)
|
||||
{
|
||||
for (int i = 0; i < ntopos; i++) {
|
||||
|
||||
__u32 id = topo_entity_for_devnode(&topos[i], rdev);
|
||||
struct media_v2_entity *e = id ? topo_entity(&topos[i], id) : NULL;
|
||||
|
||||
if (e && e->function == MEDIA_ENT_F_CAM_SENSOR) {
|
||||
snprintf(out, outn, "%s", e->name);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
static const char *role_for_sensor(const char *sensor)
|
||||
{
|
||||
if (strstr(sensor, "og01a1b"))
|
||||
return "tracking"; /* 1056x1024 side fisheye */
|
||||
|
||||
if (strstr(sensor, "og0ve10"))
|
||||
return "tracking"; /* 640x480 upper */
|
||||
|
||||
if (strstr(sensor, "imx616"))
|
||||
return "passthrough"; /* 2464x2464 Arcturus color */
|
||||
|
||||
return "unknown";
|
||||
}
|
||||
|
||||
/* ------------------------------------------------- XRService / proc scan */
|
||||
|
||||
static pid_t find_process(const char *needle)
|
||||
{
|
||||
DIR *d = opendir("/proc");
|
||||
|
||||
if (!d)
|
||||
return 0;
|
||||
|
||||
struct dirent *e;
|
||||
pid_t found = 0;
|
||||
|
||||
while ((e = readdir(d))) {
|
||||
|
||||
if (e->d_name[0] < '0' || e->d_name[0] > '9')
|
||||
continue;
|
||||
|
||||
char path[288];
|
||||
snprintf(path, sizeof(path), "/proc/%s/cmdline", e->d_name);
|
||||
|
||||
FILE *f = fopen(path, "rb");
|
||||
|
||||
if (!f)
|
||||
continue;
|
||||
|
||||
char buf[512] = {0};
|
||||
size_t got = fread(buf, 1, sizeof(buf) - 1, f);
|
||||
fclose(f);
|
||||
|
||||
if (got == 0)
|
||||
continue;
|
||||
|
||||
const char *base = strrchr(buf, '/');
|
||||
base = base ? base + 1 : buf;
|
||||
|
||||
if (strstr(base, needle)) {
|
||||
found = (pid_t)atoi(e->d_name);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
closedir(d);
|
||||
|
||||
return found;
|
||||
}
|
||||
|
||||
static bool read_dmabuf_size(pid_t pid, int fd, size_t *size, unsigned long *ino)
|
||||
{
|
||||
char path[64];
|
||||
snprintf(path, sizeof(path), "/proc/%d/fdinfo/%d", pid, fd);
|
||||
|
||||
FILE *f = fopen(path, "r");
|
||||
|
||||
if (!f)
|
||||
return false;
|
||||
|
||||
bool have = false;
|
||||
char line[256];
|
||||
|
||||
*ino = 0;
|
||||
|
||||
while (fgets(line, sizeof(line), f)) {
|
||||
|
||||
unsigned long long v;
|
||||
|
||||
if (sscanf(line, "size: %llu", &v) == 1) {
|
||||
*size = (size_t)v;
|
||||
have = true;
|
||||
} else if (sscanf(line, "ino: %llu", &v) == 1) {
|
||||
*ino = (unsigned long)v;
|
||||
}
|
||||
}
|
||||
|
||||
fclose(f);
|
||||
|
||||
return have;
|
||||
}
|
||||
|
||||
static int cmp_int(const void *a, const void *b)
|
||||
{
|
||||
return *(const int *)a - *(const int *)b;
|
||||
}
|
||||
|
||||
static bool scan_xr_fds(pid_t pid, char *err, size_t errn)
|
||||
{
|
||||
char dirpath[64];
|
||||
snprintf(dirpath, sizeof(dirpath), "/proc/%d/fd", pid);
|
||||
|
||||
DIR *d = opendir(dirpath);
|
||||
|
||||
if (!d) {
|
||||
set_err(err, errn, "opendir(%s): %s (are you root?)", dirpath, strerror(errno));
|
||||
return false;
|
||||
}
|
||||
|
||||
static int fds[8192];
|
||||
int nfds = 0;
|
||||
struct dirent *e;
|
||||
|
||||
while ((e = readdir(d)) && nfds < (int)(sizeof(fds) / sizeof(fds[0])))
|
||||
if (e->d_name[0] >= '0' && e->d_name[0] <= '9')
|
||||
fds[nfds++] = atoi(e->d_name);
|
||||
|
||||
closedir(d);
|
||||
|
||||
qsort(fds, nfds, sizeof(int), cmp_int);
|
||||
|
||||
nfdents = 0;
|
||||
|
||||
for (int i = 0; i < nfds && nfdents < MAX_FDENTS; i++) {
|
||||
|
||||
char link[64], target[256];
|
||||
snprintf(link, sizeof(link), "/proc/%d/fd/%d", pid, fds[i]);
|
||||
|
||||
ssize_t n = readlink(link, target, sizeof(target) - 1);
|
||||
|
||||
if (n < 0)
|
||||
continue;
|
||||
|
||||
target[n] = 0;
|
||||
|
||||
fdent_t ent;
|
||||
memset(&ent, 0, sizeof(ent));
|
||||
ent.xfd = fds[i];
|
||||
|
||||
if (strstr(target, "dmabuf")) {
|
||||
|
||||
if (!read_dmabuf_size(pid, fds[i], &ent.size, &ent.ino))
|
||||
continue;
|
||||
|
||||
ent.kind = FD_DMABUF;
|
||||
|
||||
} else if (strncmp(target, "/dev/video", 10) == 0) {
|
||||
|
||||
ent.kind = FD_VIDEO;
|
||||
snprintf(ent.path, sizeof(ent.path), "%s", target);
|
||||
|
||||
} else if (strncmp(target, "/dev/v4l-subdev", 15) == 0) {
|
||||
|
||||
struct stat st;
|
||||
|
||||
if (stat(target, &st) < 0 || !sensor_for_subdev(st.st_rdev, ent.sensor, sizeof(ent.sensor)))
|
||||
continue;
|
||||
|
||||
ent.kind = FD_SUBDEV_SENSOR;
|
||||
|
||||
} else {
|
||||
continue;
|
||||
}
|
||||
|
||||
fdents[nfdents++] = ent;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ------------------------------------------------------ camera discovery */
|
||||
|
||||
static void probe_cameras(xr_state_t *st)
|
||||
{
|
||||
int seen[64];
|
||||
int nseen = 0;
|
||||
|
||||
for (int i = 0; i < nfdents; i++) {
|
||||
|
||||
if (fdents[i].kind != FD_VIDEO)
|
||||
continue;
|
||||
|
||||
const char *path = fdents[i].path;
|
||||
int node = atoi(path + 10);
|
||||
bool dup = false;
|
||||
|
||||
for (int k = 0; k < nseen; k++)
|
||||
if (seen[k] == node)
|
||||
dup = true;
|
||||
|
||||
if (dup || st->ncameras >= XR_MAX_CAMERAS || nseen >= 64)
|
||||
continue;
|
||||
|
||||
seen[nseen++] = node;
|
||||
|
||||
int fd = open(path, O_RDWR | O_CLOEXEC);
|
||||
|
||||
if (fd < 0)
|
||||
continue;
|
||||
|
||||
struct v4l2_format fmt;
|
||||
memset(&fmt, 0, sizeof(fmt));
|
||||
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
|
||||
|
||||
xr_camera_t *c = &st->cameras[st->ncameras];
|
||||
memset(c, 0, sizeof(*c));
|
||||
|
||||
if (ioctl(fd, VIDIOC_G_FMT, &fmt) == 0) {
|
||||
|
||||
c->width = fmt.fmt.pix_mp.width;
|
||||
c->height = fmt.fmt.pix_mp.height;
|
||||
c->pixfmt = fmt.fmt.pix_mp.pixelformat;
|
||||
c->nplanes = fmt.fmt.pix_mp.num_planes;
|
||||
c->bytesperline = fmt.fmt.pix_mp.plane_fmt[0].bytesperline;
|
||||
|
||||
for (unsigned p = 0; p < c->nplanes && p < VIDEO_MAX_PLANES; p++)
|
||||
c->planesize[p] = fmt.fmt.pix_mp.plane_fmt[p].sizeimage;
|
||||
|
||||
} else {
|
||||
|
||||
memset(&fmt, 0, sizeof(fmt));
|
||||
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
||||
|
||||
if (ioctl(fd, VIDIOC_G_FMT, &fmt) < 0) {
|
||||
close(fd);
|
||||
continue;
|
||||
}
|
||||
|
||||
c->width = fmt.fmt.pix.width;
|
||||
c->height = fmt.fmt.pix.height;
|
||||
c->pixfmt = fmt.fmt.pix.pixelformat;
|
||||
c->nplanes = 1;
|
||||
c->bytesperline = fmt.fmt.pix.bytesperline;
|
||||
c->planesize[0] = fmt.fmt.pix.sizeimage;
|
||||
}
|
||||
|
||||
struct stat sb;
|
||||
|
||||
if (fstat(fd, &sb) == 0) {
|
||||
c->minor = minor(sb.st_rdev);
|
||||
sensor_for_video(sb.st_rdev, c->sensor, sizeof(c->sensor));
|
||||
}
|
||||
|
||||
close(fd);
|
||||
|
||||
if (!c->sensor[0])
|
||||
snprintf(c->sensor, sizeof(c->sensor), "unknown");
|
||||
|
||||
c->node = node;
|
||||
snprintf(c->path, sizeof(c->path), "%s", path);
|
||||
c->role = role_for_sensor(c->sensor);
|
||||
|
||||
st->ncameras++;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* qcom-camss can report bytesperline as the visible width while the VFE
|
||||
* writes a larger aligned pitch. sizeimage is right, so derive the pitch.
|
||||
*/
|
||||
unsigned xr_camera_stride(const xr_camera_t *c)
|
||||
{
|
||||
if (!c->height || !c->planesize[0])
|
||||
return c->bytesperline ? c->bytesperline : c->width;
|
||||
|
||||
double bpp = 1.0;
|
||||
|
||||
if (c->pixfmt == V4L2_PIX_FMT_NV12 || c->pixfmt == V4L2_PIX_FMT_NV21)
|
||||
bpp = 1.5;
|
||||
|
||||
unsigned s = (unsigned)((double)c->planesize[0] / ((double)c->height * bpp));
|
||||
|
||||
if (s >= c->width && s <= c->width * 4)
|
||||
return s;
|
||||
|
||||
return c->bytesperline ? c->bytesperline : c->width;
|
||||
}
|
||||
|
||||
/*
|
||||
* The Arcturus color cameras (arcimx616) claim 2464x2464 NV12, but measured on
|
||||
* 2026-09-28 their plane 0 holds 10-bit MIPI-packed YUV 4:2:0: 2464 luma rows
|
||||
* then 1232 rows of interleaved UV, each row 2464 packed pixels (3080 bytes)
|
||||
* padded to a 256-byte pitch (3328). Only the first 1972 pixels of a row carry
|
||||
* image; the rest are zero.
|
||||
*/
|
||||
#define IMX616_VALID_WIDTH 1972
|
||||
|
||||
void xr_camera_layout(const xr_camera_t *c, xr_layout_t *l)
|
||||
{
|
||||
memset(l, 0, sizeof(*l));
|
||||
l->height = c->height;
|
||||
|
||||
if (c->pixfmt == V4L2_PIX_FMT_NV12 && strstr(c->sensor, "imx616")) {
|
||||
|
||||
unsigned packed = (c->width * 5 + 3) / 4;
|
||||
|
||||
l->fmt = XR_FMT_YUV420_10P;
|
||||
l->pitch = (packed + 255) & ~255u;
|
||||
l->rows = c->height + c->height / 2;
|
||||
l->width = IMX616_VALID_WIDTH < c->width ? IMX616_VALID_WIDTH : c->width;
|
||||
return;
|
||||
}
|
||||
|
||||
l->pitch = xr_camera_stride(c);
|
||||
l->width = c->width < l->pitch ? c->width : l->pitch;
|
||||
|
||||
if (c->pixfmt == V4L2_PIX_FMT_NV12 || c->pixfmt == V4L2_PIX_FMT_NV21) {
|
||||
l->fmt = XR_FMT_NV12;
|
||||
l->rows = c->height + c->height / 2;
|
||||
} else {
|
||||
l->fmt = XR_FMT_GREY8;
|
||||
l->rows = c->height;
|
||||
}
|
||||
}
|
||||
|
||||
const char *xr_fmt_name(xr_fmt_t f)
|
||||
{
|
||||
switch (f) {
|
||||
case XR_FMT_GREY8: return "grey8";
|
||||
case XR_FMT_NV12: return "nv12";
|
||||
case XR_FMT_YUV420_10P: return "yuv420_10p";
|
||||
}
|
||||
|
||||
return "?";
|
||||
}
|
||||
|
||||
/* ------------------------------------------------------- buffer grouping */
|
||||
|
||||
/*
|
||||
* XRService allocates one udmabuf per plane, plane 0 then plane 1, a whole
|
||||
* queue at a time right after opening the sensor's subdev. Plane 1 matches
|
||||
* VIDIOC_G_FMT exactly; plane 0 has slack, so it is matched with >=.
|
||||
*/
|
||||
static void build_groups(xr_state_t *st)
|
||||
{
|
||||
char current_sensor[XR_SENSOR_LEN] = "";
|
||||
|
||||
for (int i = 0; i < nfdents; i++) {
|
||||
|
||||
if (fdents[i].kind == FD_SUBDEV_SENSOR) {
|
||||
snprintf(current_sensor, sizeof(current_sensor), "%s", fdents[i].sensor);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (fdents[i].kind != FD_DMABUF)
|
||||
continue;
|
||||
|
||||
if (i + 1 >= nfdents || fdents[i + 1].kind != FD_DMABUF)
|
||||
continue;
|
||||
|
||||
size_t s0 = fdents[i].size;
|
||||
size_t s1 = fdents[i + 1].size;
|
||||
bool match = false;
|
||||
|
||||
for (int c = 0; c < st->ncameras; c++) {
|
||||
|
||||
xr_camera_t *cam = &st->cameras[c];
|
||||
|
||||
if (cam->nplanes >= 2 && s1 == cam->planesize[1] && s0 >= cam->planesize[0]) {
|
||||
match = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!match)
|
||||
continue;
|
||||
|
||||
xr_group_t *g = NULL;
|
||||
|
||||
if (st->ngroups > 0) {
|
||||
|
||||
xr_group_t *last = &st->groups[st->ngroups - 1];
|
||||
|
||||
if (last->planesize[0] == s0 && last->planesize[1] == s1 &&
|
||||
!strcmp(last->sensor, current_sensor))
|
||||
g = last;
|
||||
}
|
||||
|
||||
if (!g) {
|
||||
|
||||
if (st->ngroups >= XR_MAX_GROUPS)
|
||||
break;
|
||||
|
||||
g = &st->groups[st->ngroups++];
|
||||
memset(g, 0, sizeof(*g));
|
||||
g->planesize[0] = s0;
|
||||
g->planesize[1] = s1;
|
||||
snprintf(g->sensor, sizeof(g->sensor), "%s", current_sensor);
|
||||
}
|
||||
|
||||
if (g->nbufs < XR_MAX_RUNBUFS) {
|
||||
g->buf[g->nbufs].xfd = fdents[i].xfd;
|
||||
g->buf[g->nbufs].xfd1 = fdents[i + 1].xfd;
|
||||
g->buf[g->nbufs].size = s0;
|
||||
g->buf[g->nbufs].size1 = s1;
|
||||
g->nbufs++;
|
||||
}
|
||||
|
||||
i++; /* consume the plane 1 descriptor */
|
||||
}
|
||||
|
||||
int keep = 0;
|
||||
|
||||
for (int i = 0; i < st->ngroups; i++)
|
||||
if (st->groups[i].nbufs >= 4)
|
||||
st->groups[keep++] = st->groups[i];
|
||||
|
||||
st->ngroups = keep;
|
||||
|
||||
/*
|
||||
* Bind each run to a camera. The sensor marker alone can be wrong: XRService
|
||||
* sometimes opens another sensor's subdev (e.g. the idle color camera)
|
||||
* between an upper camera's subdev and its buffers, and two upper cameras
|
||||
* can resolve to the same sensor name. So a marker match must also fit the
|
||||
* camera's plane sizes, and each camera takes at most one run.
|
||||
*/
|
||||
for (int pass = 0; pass < 2; pass++)
|
||||
for (int i = 0; i < st->ngroups; i++) {
|
||||
|
||||
xr_group_t *g = &st->groups[i];
|
||||
|
||||
for (int c = 0; c < st->ncameras && !g->cam; c++) {
|
||||
|
||||
xr_camera_t *cam = &st->cameras[c];
|
||||
|
||||
if (pass == 0 && (!g->sensor[0] || strcmp(cam->sensor, g->sensor)))
|
||||
continue;
|
||||
|
||||
if (cam->nplanes < 2 || g->planesize[1] != cam->planesize[1] ||
|
||||
g->planesize[0] < cam->planesize[0])
|
||||
continue;
|
||||
|
||||
bool taken = false;
|
||||
|
||||
for (int k = 0; k < st->ngroups; k++)
|
||||
if (k != i && st->groups[k].cam == cam)
|
||||
taken = true;
|
||||
|
||||
if (!taken)
|
||||
g->cam = cam;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool xr_discover(xr_state_t *st, const char *process, char *err, size_t errn)
|
||||
{
|
||||
memset(st, 0, sizeof(*st));
|
||||
|
||||
st->pid = find_process(process);
|
||||
|
||||
if (!st->pid) {
|
||||
set_err(err, errn, "%s is not running; start SteamVR on the headset first", process);
|
||||
return false;
|
||||
}
|
||||
|
||||
topo_load_all();
|
||||
|
||||
bool ok = scan_xr_fds(st->pid, err, errn);
|
||||
|
||||
if (ok) {
|
||||
probe_cameras(st);
|
||||
build_groups(st);
|
||||
}
|
||||
|
||||
topo_free_all();
|
||||
|
||||
return ok;
|
||||
}
|
||||
|
||||
void xr_print(const xr_state_t *st, FILE *f)
|
||||
{
|
||||
fprintf(f, "XRService pid %d\n", st->pid);
|
||||
|
||||
for (int i = 0; i < st->ncameras; i++) {
|
||||
|
||||
const xr_camera_t *c = &st->cameras[i];
|
||||
char fcc[5] = {
|
||||
(char)(c->pixfmt & 0xff), (char)((c->pixfmt >> 8) & 0xff),
|
||||
(char)((c->pixfmt >> 16) & 0xff), (char)((c->pixfmt >> 24) & 0xff), 0
|
||||
};
|
||||
|
||||
fprintf(f, " camera %-12s minor %-3u %-16s %ux%u %s pitch %u planes %zu %zu role=%s\n",
|
||||
c->path, c->minor, c->sensor, c->width, c->height, fcc,
|
||||
xr_camera_stride(c), c->planesize[0], c->planesize[1], c->role);
|
||||
}
|
||||
|
||||
for (int i = 0; i < st->ngroups; i++) {
|
||||
|
||||
const xr_group_t *g = &st->groups[i];
|
||||
|
||||
fprintf(f, " queue %d: %d buffers plane0=%zu plane1=%zu fds %d..%d sensor '%s' -> %s\n",
|
||||
i, g->nbufs, g->planesize[0], g->planesize[1],
|
||||
g->buf[0].xfd, g->buf[g->nbufs - 1].xfd1, g->sensor,
|
||||
g->cam ? g->cam->path : "(unbound)");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,82 @@
|
||||
/*
|
||||
* xrcams - find the headset cameras and the DMA-BUF queues XRService feeds them.
|
||||
*
|
||||
* Adapted from framecap.c in FrameEyeCameraFeed (vendor/FrameEyeCameraFeed),
|
||||
* MIT License, Copyright (c) 2026 Curtis English. See LICENSE.FrameEyeCameraFeed.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <sys/types.h>
|
||||
|
||||
#include <linux/videodev2.h>
|
||||
|
||||
#define XR_MAX_CAMERAS 16
|
||||
#define XR_MAX_GROUPS 32
|
||||
#define XR_MAX_RUNBUFS 128
|
||||
#define XR_SENSOR_LEN 64
|
||||
|
||||
typedef struct {
|
||||
int node; /* N from /dev/videoN */
|
||||
unsigned minor; /* char device minor, as tracepoints report it */
|
||||
char path[64];
|
||||
unsigned width;
|
||||
unsigned height;
|
||||
unsigned bytesperline;
|
||||
unsigned nplanes;
|
||||
size_t planesize[VIDEO_MAX_PLANES];
|
||||
uint32_t pixfmt;
|
||||
char sensor[XR_SENSOR_LEN]; /* media entity name of the sensor */
|
||||
const char *role;
|
||||
} xr_camera_t;
|
||||
|
||||
typedef struct {
|
||||
int xfd; /* plane 0 descriptor in XRService */
|
||||
int xfd1; /* plane 1 descriptor in XRService */
|
||||
size_t size;
|
||||
size_t size1;
|
||||
} xr_bufref_t;
|
||||
|
||||
/* One run of buffers XRService allocated for a camera queue, in allocation order. */
|
||||
typedef struct {
|
||||
size_t planesize[2];
|
||||
int nbufs;
|
||||
xr_bufref_t buf[XR_MAX_RUNBUFS];
|
||||
char sensor[XR_SENSOR_LEN]; /* from the preceding sensor subdev */
|
||||
xr_camera_t *cam;
|
||||
} xr_group_t;
|
||||
|
||||
typedef struct {
|
||||
pid_t pid;
|
||||
xr_camera_t cameras[XR_MAX_CAMERAS];
|
||||
int ncameras;
|
||||
xr_group_t groups[XR_MAX_GROUPS];
|
||||
int ngroups;
|
||||
} xr_state_t;
|
||||
|
||||
typedef enum {
|
||||
XR_FMT_GREY8, /* 8-bit mono */
|
||||
XR_FMT_NV12, /* 8-bit Y plane then interleaved UV, same pitch */
|
||||
XR_FMT_YUV420_10P /* like NV12, but 10-bit MIPI-packed (4 px in 5 bytes) */
|
||||
} xr_fmt_t;
|
||||
|
||||
/* Where the image really sits in plane 0; V4L2's numbers can be misleading. */
|
||||
typedef struct {
|
||||
xr_fmt_t fmt;
|
||||
unsigned pitch; /* bytes per row */
|
||||
unsigned rows; /* rows in plane 0: luma, plus chroma for YUV */
|
||||
unsigned width; /* valid pixels per row */
|
||||
unsigned height; /* luma rows */
|
||||
} xr_layout_t;
|
||||
|
||||
/* Scan XRService's descriptors and the media graph. Needs root. */
|
||||
bool xr_discover(xr_state_t *st, const char *process, char *err, size_t errn);
|
||||
unsigned xr_camera_stride(const xr_camera_t *c);
|
||||
void xr_camera_layout(const xr_camera_t *c, xr_layout_t *l);
|
||||
const char *xr_fmt_name(xr_fmt_t f);
|
||||
void xr_print(const xr_state_t *st, FILE *f);
|
||||
void xr_slugify(const char *in, char *out, size_t n);
|
||||
Reference in new issue
Block a user