builtin-programs/camera/usb.folk

# camera/usb.folk --
#
#     Hardware interface with USB webcams on Linux (v4l2).

if {$::tcl_platform(os) ne "linux"} { return }

When the image library is /imageLib/ &\
     the jpeg library is /jpegLib/ {

set camc [C]
$camc extend $imageLib
$camc include <string.h>
$camc include <math.h>

$camc include <errno.h>
$camc include <fcntl.h>
$camc include <sys/ioctl.h>
$camc include <sys/mman.h>
$camc include <asm/types.h>
$camc include <linux/videodev2.h>
$camc include <unistd.h>

$camc include <stdint.h>
$camc include <stdlib.h>

$camc struct CameraBuffer {
    uint8_t* start;
    size_t length;
}
$camc struct Camera {
    int fd;
    uint32_t width;
    uint32_t height;
    size_t buffer_count;
    CameraBuffer* buffers;
    CameraBuffer head;
}

$camc code {
    void quit(const char* msg) {
        FOLK_ERROR("camera/usb: Quitting: [%s] %d: %s\n",
                   msg, errno, strerror(errno));
    }

    int xioctl(int fd, int request, void* arg) {
        for (int i = 0; i < 100; i++) {
            int r = ioctl(fd, request, arg);
            if (r != -1 || errno != EINTR) return r;
            printf("camera/usb: Retrying: [%x][%d] %s\n",
                   request, i, strerror(errno));
        }
        return -1;
    }
}

$camc proc cameraOpen {char* device int width int height} Camera* {
    printf("camera/usb: Loading '%s'\n", device);
    // O_CLOEXEC is necessary so long-running child processes (like
    // fswatch) don't keep the camera open past the death of folk
    // itself, which causes EBUSY for the next Folk process.
    int fd = open(device, O_RDWR | O_NONBLOCK | O_CLOEXEC, 0);
    if (fd == -1) quit("open");
    Camera* camera = malloc(sizeof(Camera));
    camera->fd = fd;
    camera->width = width;
    camera->height = height;
    camera->buffer_count = 0;
    camera->buffers = NULL;
    camera->head.length = 0;
    camera->head.start = NULL;
    return camera;
}
$camc proc cameraClose {Camera* camera} void {
    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (xioctl(camera->fd, VIDIOC_STREAMOFF, &type) == -1) {
        // if ENODEV, we're already done; just return to caller.
        if (errno == ENODEV) {
            fprintf(stderr, "cameraClose (%p): ENODEV, returning.\n",
                    camera);
            close(camera->fd);
            free(camera);
            return;
        } else {
            // stop, something weird happened.
            quit("VIDIOC_STREAMOFF");
        }
    }

    struct v4l2_requestbuffers req = {0};
    // A count value of zero frees all buffers, after aborting or
    // finishing any DMA in progress, an implicit VIDIOC_STREAMOFF.
    req.count = 0;
    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;
    if (xioctl(camera->fd, VIDIOC_REQBUFS, &req) == -1) {
        quit("VIDIOC_REQBUFS");
    }
    if (close(camera->fd) != 0) {
        quit("close");
    }
    free(camera);
}

$camc proc cameraInit {Camera* camera uint32_t requested_buffer_count} void {
    struct v4l2_capability cap;
    if (xioctl(camera->fd, VIDIOC_QUERYCAP, &cap) == -1) quit("VIDIOC_QUERYCAP");
    if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) quit("no capture");
    if (!(cap.capabilities & V4L2_CAP_STREAMING)) quit("no streaming");

    struct v4l2_format format = {0};
    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    format.fmt.pix.width = camera->width;
    format.fmt.pix.height = camera->height;
    format.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
    format.fmt.pix.field = V4L2_FIELD_NONE;
    int ret;
    do {
        ret = xioctl(camera->fd, VIDIOC_S_FMT, &format);
        fprintf(stderr, "camera/usb: VIDIOC_S_FMT: ret = %d (%d) (%s)\n",
                ret, errno, strerror(errno));
        usleep(100000);
    } while (ret == -1 && errno == EBUSY);
    if (ret == -1) quit("VIDIOC_S_FMT");

    struct v4l2_requestbuffers req = {0};
    req.count = requested_buffer_count;
    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;
    if (xioctl(camera->fd, VIDIOC_REQBUFS, &req) == -1) quit("VIDIOC_REQBUFS");
    camera->buffer_count = req.count;
    camera->buffers = calloc(req.count, sizeof (CameraBuffer));

    printf("LATENCY: Camera buffer count: %d\n", req.count);
    fflush(stdout);

    struct v4l2_streamparm streamparm = {0};
    streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (xioctl(camera->fd, VIDIOC_G_PARM, &streamparm) == -1) quit("VIDIOC_G_PARM");
    if (streamparm.parm.capture.capability & V4L2_CAP_TIMEPERFRAME) {
        int req_rate_numerator = 1;
        int req_rate_denominator = 60;
        streamparm.parm.capture.timeperframe.numerator = req_rate_numerator;
        streamparm.parm.capture.timeperframe.denominator = req_rate_denominator;
        if (xioctl(camera->fd, VIDIOC_S_PARM, &streamparm) == -1) { quit("VIDIOC_S_PARM"); }

        if (streamparm.parm.capture.timeperframe.numerator != req_rate_numerator ||
            streamparm.parm.capture.timeperframe.denominator != req_rate_denominator) {
            fprintf(stderr,
                    "the driver changed the time per frame from "
                    "%d/%d to %d/%d\n",
                    req_rate_numerator, req_rate_denominator,
                    streamparm.parm.capture.timeperframe.numerator,
                    streamparm.parm.capture.timeperframe.denominator);
        }
    }

    size_t buf_max = 0;
    for (size_t i = 0; i < camera->buffer_count; i++) {
        struct v4l2_buffer buf;
        memset(&buf, 0, sizeof buf);
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = i;
        if (xioctl(camera->fd, VIDIOC_QUERYBUF, &buf) == -1) {
            quit("VIDIOC_QUERYBUF");
        }
        if (buf.length > buf_max) buf_max = buf.length;
        camera->buffers[i].length = buf.length;
        camera->buffers[i].start = 
          mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED,
             camera->fd, buf.m.offset);
        if (camera->buffers[i].start == MAP_FAILED) quit("mmap");
    }
    camera->head.start = malloc(buf_max);

    printf("camera %d; bufcount %zu\n", camera->fd, camera->buffer_count);
}

$camc proc cameraStart {Camera* camera} void {
    for (size_t i = 0; i < camera->buffer_count; i++) {
        struct v4l2_buffer buf;
        memset(&buf, 0, sizeof buf);
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = i;
        if (xioctl(camera->fd, VIDIOC_QBUF, &buf) == -1) quit("VIDIOC_QBUF");
        printf("camera_start(%zu): %s\n", i, strerror(errno));
    }

    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (xioctl(camera->fd, VIDIOC_STREAMON, &type) == -1) {
        quit("VIDIOC_STREAMON");
    }
}

$camc code {
    int camera_capture(Camera* camera) {
        struct v4l2_buffer buf;
        memset(&buf, 0, sizeof buf);
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        if (xioctl(camera->fd, VIDIOC_DQBUF, &buf) == -1) {
            fprintf(stderr, "camera_capture: VIDIOC_DQBUF failed: %d: %s\n", errno, strerror(errno));
            return 0;
        }
        memcpy(camera->head.start, camera->buffers[buf.index].start, buf.bytesused);
        camera->head.length = buf.bytesused;
        if (xioctl(camera->fd, VIDIOC_QBUF, &buf) == -1) {
            fprintf(stderr, "camera_capture: VIDIOC_QBUF failed: %d: %s\n", errno, strerror(errno));
            return 0;
        }
        return 1;
    }
}

$camc proc cameraFrameJpeg {Camera* camera} CameraBuffer {
    // Retry select() up to 5 times with 2 second timeout each
    // Some cameras need time to start streaming
    int r = 0;
    for (int attempt = 0; attempt < 5 && r == 0; attempt++) {
        struct timeval timeout;
        timeout.tv_sec = 2;
        timeout.tv_usec = 0;
        fd_set fds;
        FD_ZERO(&fds);
        FD_SET(camera->fd, &fds);
        r = select(camera->fd + 1, &fds, 0, 0, &timeout);
        if (r == -1) quit("select");
        if (r == 0 && attempt < 4) {
            fprintf(stderr, "camera/usb: select timeout on fd %d, retry %d/4\n",
                    camera->fd, attempt + 1);
        }
    }
    if (r == 0) {
        FOLK_ERROR("selection failed of fd %d after 5 attempts\n", camera->fd);
    }

    FOLK_ENSURE(camera_capture(camera) != 0);

    // Clone the head buffer into a new independent buffer that can be
    // owned by the caller.
    CameraBuffer buf = {
        .start = malloc(camera->head.length),
        .length = camera->head.length
    };
    memcpy(buf.start, camera->head.start, buf.length);
    return buf;
}
$camc proc jpegFree {CameraBuffer jpeg} void {
    free(jpeg.start);
}

$camc proc setExposure {Camera* camera int value} void {
    struct v4l2_control c;

    fprintf(stderr, "setExposure %d\n", value);
    if (value == 0) {
        c.id = V4L2_CID_EXPOSURE_AUTO;
        c.value = V4L2_EXPOSURE_APERTURE_PRIORITY;
        FOLK_ENSURE(xioctl(camera->fd, VIDIOC_S_CTRL, &c) == 0);

    } else {
        c.id = V4L2_CID_EXPOSURE_AUTO;
        c.value = V4L2_EXPOSURE_MANUAL;
        FOLK_ENSURE(xioctl(camera->fd, VIDIOC_S_CTRL, &c) == 0);

        c.id = V4L2_CID_EXPOSURE_ABSOLUTE;
        c.value = value;
        FOLK_ENSURE(xioctl(camera->fd, VIDIOC_S_CTRL, &c) == 0);
    }
}

$camc cflags -Wall -Werror
set camLib [$camc compile]

When camera /cameraPath/ has width /decompWidth/ height /decompHeight/ {
    When camera $cameraPath has jpeg frame /jpeg/ at timestamp /ts/ {
        set grayImage [$jpegLib jpegDecompressGray $jpeg \
                           $decompWidth $decompHeight \
                           [expr {int($ts * 1000)}]]
        Hold! -key [list camera $cameraPath gray-frame] \
            Claim camera $cameraPath has gray frame $grayImage at timestamp $ts \
            -destructor [list $imageLib imageFree $grayImage]
    }
    When camera $cameraPath has jpeg frame /jpeg/ at timestamp /ts/ {
        set frameImage [$jpegLib jpegDecompressRGB $jpeg \
                            $decompWidth $decompHeight \
                            [expr {int($ts * 1000)}]]
        Hold! -key [list camera $cameraPath frame] \
            Claim camera $cameraPath has frame $frameImage at timestamp $ts \
            -destructor [list $imageLib imageFree $frameImage]
    }
}

When /someone/ wishes $::thisNode uses camera /camera/ with /...options/ {
    if {![string match "/dev/*" $camera]} { return }

    set width [dict get $options width]
    set height [dict get $options height]
    set bufferCount [dict getdef $options bufferCount 2]

    if {[dict exists $options crops]} {
        set crops [dict get $options crops]
    }

    fn runCamera {camObjVar} {
        puts "camera/usb: Will try to open $camera"

        upvar $camObjVar camObj_
        set camObj_ [$camLib cameraOpen $camera $width $height]
        set camObj $camObj_ ;# HACK: we don't capture upvars yet.
        puts "camera/usb: Loaded $camera -> $camObj"

        $camLib cameraInit $camObj $bufferCount
        $camLib cameraStart $camObj
        # skip 5 frames for booting a cam
        for {set i 0} {$i < 5} {incr i} {
            $camLib cameraFrameJpeg $camObj
        }

        if {[info exists crops]} {
            for {set i 0} {$i < [llength $crops]} {incr i} {
                set crop [lindex $crops $i]
                Claim camera [list $camera $i] has width [dict get $crop width] \
                    height [dict get $crop height]
            }
        } else {
            Claim camera $camera has width $width height $height
        }

        When /someone/ wishes camera $camera uses exposure time /exposureTimeUs/ us {
            $camLib setExposure $camObj [expr {int($exposureTimeUs / 100)}]
        }

        # Inner, frame loop.
        while true {
            tracy zoneBegin

            set ms [clock milliseconds]
            set jpeg [$camLib cameraFrameJpeg $camObj]
            set timestamp [expr {$ms / 1000.0}]
            tracy zoneName "camera/usb: $timestamp"

            if {[info exists crops]} {
                for {set i 0} {$i < [llength $crops]} {incr i} {
                    set crop [lindex $crops $i]
                    set cropPath [list $camera $i]
                    set croppedJpeg [$jpegLib jpegSubimage $jpeg \
                                         [dict get $crop x] \
                                         [dict get $crop y] \
                                         [dict get $crop width] \
                                         [dict get $crop height]]
                    Hold! -key [list camera $cropPath jpeg] \
                        Claim camera $cropPath has jpeg frame $croppedJpeg at timestamp $timestamp \
                        -destructor [list $camLib jpegFree $croppedJpeg]
                }
            }
            # Always publish the source jpeg so the preview keeps working
            # even when virtual crops are configured. Gray/RGB decompressors
            # on the source camera are gated off in crops mode to avoid
            # full-resolution decoding.
            Hold! -key [list camera $camera jpeg] \
                Claim camera $camera has jpeg frame $jpeg at timestamp $timestamp \
                -destructor [list $camLib jpegFree $jpeg]

            tracy zoneEnd
        }
    }

    # Outer, retry loop.
    while true {
        set camObj {}
        try -signal {
            runCamera camObj
        } on error e {
            puts stderr "camera/usb: Error $e"
        } on signal sig {
            puts stderr "camera/usb: Signal $sig"
            # This is actually a termination condition.
            break
        } finally {
            if {$camObj ne {}} {
                puts "camera/usb: Close $camObj"
                $camLib cameraClose $camObj
            }
        }

        # We only get down here if something goes wrong. Wait before
        # trying to restart the camera.
        sleep 1
    }
}

}