RSDK-6367 - Fix v4l2m2m encoder padding (viamrobotics#3453)

Author: seanavery

gostream/codec/h264/encoder.go

@@ -25,6 +25,8 @@ const (
 	pixelFormat = avcodec.AvPixFmtYuv420p
 	// V4l2m2m Is a V4L2 memory-to-memory H.264 hardware encoder.
 	V4l2m2m = "h264_v4l2m2m"
+	// macroBlock is the encoder boundary block size in bytes.
+	macroBlock = 64
 )
 
 type encoder struct {
@@ -96,7 +98,7 @@ func (h *encoder) Encode(ctx context.Context, img image.Image) ([]byte, error) {
 		return nil, errors.Wrap(err, "cannot read image")
 	}
 
-	h.frame.SetFrameFromImg(yuvImg.(*image.YCbCr))
+	h.frame.SetFrameFromImgMacroAlign(yuvImg.(*image.YCbCr), macroBlock)
 	h.frame.SetFramePTS(h.pts)
 	h.pts++
 

gostream/ffmpeg/avcodec/avcodec.go

@@ -21,8 +21,15 @@ import (
 	"go.viam.com/rdk/gostream/ffmpeg/avlog"
 )
 
-// AvPixFmtYuv420p the pixel format AV_PIX_FMT_YUV420P
-const AvPixFmtYuv420p = C.AV_PIX_FMT_YUV420P
+const (
+	// AvPixFmtYuv420p the pixel format AV_PIX_FMT_YUV420P
+	AvPixFmtYuv420p = C.AV_PIX_FMT_YUV420P
+	// Target bitrate in bits per second.
+	bitrate = 1_200_000
+	// Target bitrate tolerance factor.
+	// E.g., 2.0 gives 100% deviation from the target bitrate.
+	bitrateDeviation = 2
+)
 
 type (
 	// Codec an AVCodec
@@ -88,14 +95,15 @@ func (ctxt *Context) FreeContext() {
 func (ctxt *Context) SetEncodeParams(width, height int, pxlFmt PixelFormat, hasBFrames bool, gopSize int) {
 	ctxt.width = C.int(width)
 	ctxt.height = C.int(height)
-	ctxt.bit_rate = 2000000
+	ctxt.bit_rate = bitrate
 	ctxt.gop_size = C.int(gopSize)
 	if hasBFrames {
 		ctxt.has_b_frames = 1
 	} else {
 		ctxt.has_b_frames = 0
 	}
 	ctxt.pix_fmt = int32(pxlFmt)
+	ctxt.rc_buffer_size = bitrate * bitrateDeviation
 }
 
 // SetFramerate sets the context's framerate

gostream/ffmpeg/avutil/avutil.go

@@ -73,6 +73,42 @@ func ptr(buf []byte) *C.uint8_t {
 	return (*C.uint8_t)(unsafe.Pointer(h.Data))
 }
 
+// SetFrameFromImgMacroAlign sets the frame from the given image.YCbCr adding
+// line padding to the image to ensure that the data is aligned to the given boundary.
+// For example see alignment requirements for the Raspberry Pi GPU codec:
+// https://github.com/raspberrypi/linux/blob/rpi-6.1.y/drivers/staging/vc04_services/bcm2835-codec/bcm2835-v4l2-codec.c#L174
+func (f *Frame) SetFrameFromImgMacroAlign(img *image.YCbCr, boundary int) {
+	// Calculating padded strides
+	// Rounding up to next multiple of boundary value
+	paddedYStride := ((img.YStride + boundary - 1) / boundary) * boundary
+	// UV half the Y stride for 4:2:0
+	paddedCbCrStride := paddedYStride / 2
+
+	// Allocate new buffers with padding
+	// These will be freed by the GC
+	paddedY := make([]byte, paddedYStride*img.Rect.Dy())
+	paddedCb := make([]byte, paddedCbCrStride*img.Rect.Dy()/2)
+	paddedCr := make([]byte, paddedCbCrStride*img.Rect.Dy()/2)
+
+	// Copy data from img to padded buffers line by line
+	for i := 0; i < img.Rect.Dy(); i++ {
+		copy(paddedY[i*paddedYStride:(i+1)*paddedYStride], img.Y[i*img.YStride:])
+	}
+	for i := 0; i < img.Rect.Dy()/2; i++ {
+		copy(paddedCb[i*paddedCbCrStride:(i+1)*paddedCbCrStride], img.Cb[i*img.CStride:])
+		copy(paddedCr[i*paddedCbCrStride:(i+1)*paddedCbCrStride], img.Cr[i*img.CStride:])
+	}
+
+	// Update AVFrame data pointers and linesize
+	// Casting from go slice to C array without changing memory
+	f.data[0] = (*C.uchar)(unsafe.Pointer(&paddedY[0]))
+	f.data[1] = (*C.uchar)(unsafe.Pointer(&paddedCb[0]))
+	f.data[2] = (*C.uchar)(unsafe.Pointer(&paddedCr[0]))
+	f.linesize[0] = C.int(paddedYStride)
+	f.linesize[1] = C.int(paddedCbCrStride)
+	f.linesize[2] = C.int(paddedCbCrStride)
+}
+
 // SetFrameFromImg sets the frame from the given image.YCbCr
 func (f *Frame) SetFrameFromImg(img *image.YCbCr) {
 	f.data[0] = ptr(img.Y)