rpicamera: use exact frame timestamps

internal/core/rpicamera_source.go

@@ -44,9 +44,8 @@ func (s *rpiCameraSource) run(ctx context.Context) error {
 	enc := &rtph264.Encoder{PayloadType: 96}
 	enc.Init()
 	var stream *stream
-	var start time.Time
 
-	onData := func(nalus [][]byte) {
+	onData := func(dts time.Duration, nalus [][]byte) {
 		if stream == nil {
 			res := s.parent.sourceStaticImplSetReady(pathSourceStaticSetReadyReq{
 				tracks:             tracks,
@@ -58,15 +57,12 @@ func (s *rpiCameraSource) run(ctx context.Context) error {
 
 			s.Log(logger.Info, "ready: %s", sourceTrackInfo(tracks))
 			stream = res.stream
-			start = time.Now()
 		}
 
-		pts := time.Since(start)
-
 		stream.writeData(&data{
 			trackID:      0,
 			ptsEqualsDTS: h264.IDRPresent(nalus),
-			pts:          pts,
+			pts:          dts,
 			h264NALUs:    nalus,
 		})
 	}

internal/rpicamera/exe/encoder.c

@@ -18,6 +18,7 @@
 #include "encoder.h"
 
 #define DEVICE              "/dev/video11"
+#define POLL_TIMEOUT_MS     200
 
 char errbuf[256];
 
@@ -38,6 +39,8 @@ typedef struct {
     int cur_buffer;
     encoder_output_cb output_cb;
     pthread_t output_thread;
+    bool ts_initialized;
+    uint64_t start_ts;
 } encoder_priv_t;
 
 static void *output_thread(void *userdata) {
@@ -45,7 +48,7 @@ static void *output_thread(void *userdata) {
 
     while (true) {
         struct pollfd p = { encp->fd, POLLIN, 0 };
-        int res = poll(&p, 1, 200);
+        int res = poll(&p, 1, POLL_TIMEOUT_MS);
         if (res == -1) {
             fprintf(stderr, "output_thread(): poll() failed\n");
             exit(1);
@@ -72,9 +75,18 @@ static void *output_thread(void *userdata) {
             buf.m.planes = planes;
             res = ioctl(encp->fd, VIDIOC_DQBUF, &buf);
             if (res == 0) {
+                uint64_t ts = ((uint64_t)buf.timestamp.tv_sec * (uint64_t)1000000) + (uint64_t)buf.timestamp.tv_usec;
+
+                if (!encp->ts_initialized) {
+                    encp->ts_initialized = true;
+                    encp->start_ts = ts;
+                }
+
+                ts -= encp->start_ts;
+
                 const uint8_t *bufmem = (const uint8_t *)encp->capture_buffers[buf.index];
                 int bufsize = buf.m.planes[0].bytesused;
-                encp->output_cb(bufmem, bufsize);
+                encp->output_cb(ts, bufmem, bufsize);
 
                 int index = buf.index;
                 int length = buf.m.planes[0].length;
@@ -284,6 +296,7 @@ bool encoder_create(parameters_t *params, int stride, int colorspace, encoder_ou
     encp->params = params;
     encp->cur_buffer = 0;
     encp->output_cb = output_cb;
+    encp->ts_initialized = false;
 
     pthread_create(&encp->output_thread, NULL, output_thread, encp);
 

internal/rpicamera/exe/encoder.h

@@ -3,7 +3,7 @@
 
 typedef void encoder_t;
 
-typedef void (*encoder_output_cb)(const uint8_t *buf, uint64_t size);
+typedef void (*encoder_output_cb)(uint64_t ts, const uint8_t *buf, uint64_t size);
 
 const char *encoder_get_error();
 bool encoder_create(parameters_t *params, int stride, int colorspace, encoder_output_cb output_cb, encoder_t **enc);

internal/rpicamera/exe/main.c

@@ -37,21 +37,22 @@ static void pipe_write_ready(int fd) {
     write(fd, buf, n);
 }
 
-static void pipe_write_buf(int fd, const uint8_t *buf, int n) {
+static void pipe_write_buf(int fd, uint64_t ts, const uint8_t *buf, int n) {
     char head[] = {'b'};
-    n++;
+    n += 1 + sizeof(uint64_t);
     write(fd, &n, 4);
     write(fd, head, 1);
-    write(fd, buf, n-1);
+    write(fd, &ts, sizeof(uint64_t));
+    write(fd, buf, n - 1 - sizeof(uint64_t));
 }
 
 static void on_frame(int buffer_fd, uint64_t size, uint64_t timestamp) {
     encoder_encode(enc, buffer_fd, size, timestamp);
 }
 
-static void on_encoder_output(const uint8_t *buf, uint64_t size) {
+static void on_encoder_output(uint64_t ts, const uint8_t *buf, uint64_t size) {
     pthread_mutex_lock(&pipe_mutex);
-    pipe_write_buf(pipe_fd, buf, size);
+    pipe_write_buf(pipe_fd, ts, buf, size);
     pthread_mutex_unlock(&pipe_mutex);
 }
 

internal/rpicamera/rpicamera.go

@@ -7,6 +7,7 @@ import (
 	_ "embed"
 	"fmt"
 	"strconv"
+	"time"
 
 	"github.com/aler9/gortsplib/pkg/h264"
 )
@@ -22,7 +23,7 @@ func bool2env(v bool) string {
 }
 
 type RPICamera struct {
-	onData func([][]byte)
+	onData func(time.Duration, [][]byte)
 
 	exe  *embeddedExe
 	pipe *pipe
@@ -33,7 +34,7 @@ type RPICamera struct {
 
 func New(
 	params Params,
-	onData func([][]byte),
+	onData func(time.Duration, [][]byte),
 ) (*RPICamera, error) {
 	pipe, err := newPipe()
 	if err != nil {
@@ -128,14 +129,17 @@ func New(
 				if buf[0] != 'b' {
 					return fmt.Errorf("unexpected output from pipe (%c)", buf[0])
 				}
-				buf = buf[1:]
 
-				nalus, err := h264.AnnexBUnmarshal(buf)
+				tmp := uint64(buf[8])<<56 | uint64(buf[7])<<48 | uint64(buf[6])<<40 | uint64(buf[5])<<32 |
+					uint64(buf[4])<<24 | uint64(buf[3])<<16 | uint64(buf[2])<<8 | uint64(buf[1])
+				dts := time.Duration(tmp) * time.Microsecond
+
+				nalus, err := h264.AnnexBUnmarshal(buf[9:])
 				if err != nil {
 					return err
 				}
 
-				onData(nalus)
+				onData(dts, nalus)
 			}
 		}()
 	}()

internal/rpicamera/rpicamera_disabled.go

@@ -6,6 +6,7 @@ package rpicamera
 
 import (
 	"fmt"
+	"time"
 )
 
 // RPICamera is a RPI Camera reader.
@@ -14,7 +15,7 @@ type RPICamera struct{}
 // New allocates a RPICamera.
 func New(
 	params Params,
-	onData func([][]byte),
+	onData func(time.Duration, [][]byte),
 ) (*RPICamera, error) {
 	return nil, fmt.Errorf("server was compiled without support for the Raspberry Pi Camera")
 }