dispmanx: fix grabber issues with certain widths and support cropping (#634)

* dispmanx: fix grabber issues with certain widths and support cropping

The dispmanx grabber will produce garbage output if the destination
pitch is not set to a multiple of 64 bytes (16 RGBA pixels).

It can also fail when retrieving only a part of the image
(eg in 3DSBS or TAB mode).

Handle these cases by capturing the full image into a separate
buffer with the pitch set to an appropriate value and manually
handle 3D SBS/TAB left/top half copying.

At this point supporting cropping like in the V4L2 grabber is
rather easy and added as well. This'll help handling overscan
setups (old TVs) and removing (possibly asymmetric) overscan
borders.

Cropping is disabled in video capture mode (when the
DISPMANX_SNAPSHOT_FILL flag is set).

Signed-off-by: Matthias Reichl <hias@horus.com>

* hyperion-dispmanx: add optional crop values and 3D mode options

Signed-off-by: Matthias Reichl <hias@horus.com>

* hyperiond: support cropping on the dispmanx grabber

Honor cropLeft, cropRight, cropTop and cropBottom settings in
the framegrabber section of the conf file to control cropping.

Signed-off-by: Matthias Reichl <hias@horus.com>

Former-commit-id: bbb55f6621b90384e417f37da4f2543d112ef57a

include/grabber/DispmanxFrameGrabber.h

@@ -41,6 +41,9 @@ public:
 	///
 	void setVideoMode(const VideoMode videoMode);
 
+	void setCropping(const unsigned cropLeft, const unsigned cropRight,
+			 const unsigned cropTop, const unsigned cropBottom);
+
 	///
 	/// Captures a single snapshot of the display and writes the data to the given image. The
 	/// provided image should have the same dimensions as the configured values (_width and
@@ -68,4 +71,17 @@ private:
 	const unsigned _width;
 	/// Height of the captured snapshot [pixels]
 	const unsigned _height;
+
+	// the selected VideoMode
+	VideoMode _videoMode;
+
+	// number of pixels to crop after capturing
+	unsigned _cropLeft, _cropRight, _cropTop, _cropBottom;
+
+	// temp buffer when capturing with unsupported pitch size or
+	// when we need to crop the image
+	ColorRgba* _captureBuffer;
+
+	// size of the capture buffer in Pixels
+	unsigned _captureBufferSize;
 };

include/grabber/DispmanxWrapper.h

@@ -56,6 +56,9 @@ public slots:
 	///
 	void stop();
 
+	void setCropping(const unsigned cropLeft, const unsigned cropRight,
+		const unsigned cropTop, const unsigned cropBottom);
+
 	///
 	/// Set the grabbing mode
 	/// @param[in] mode The new grabbing mode

libsrc/grabber/dispmanx/DispmanxFrameGrabber.cpp

@@ -11,7 +11,14 @@ DispmanxFrameGrabber::DispmanxFrameGrabber(const unsigned width, const unsigned
 	_vc_resource(0),
 	_vc_flags(0),
 	_width(width),
-	_height(height)
+	_height(height),
+	_videoMode(VIDEO_2D),
+	_cropLeft(0),
+	_cropRight(0),
+	_cropTop(0),
+	_cropBottom(0),
+	_captureBuffer(new ColorRgba[0]),
+	_captureBufferSize(0)
 {
 	// Initiase BCM
 	bcm_host_init();
@@ -49,6 +56,8 @@ DispmanxFrameGrabber::DispmanxFrameGrabber(const unsigned width, const unsigned
 
 DispmanxFrameGrabber::~DispmanxFrameGrabber()
 {
+	delete[] _captureBuffer;
+
 	// Clean up resources
 	vc_dispmanx_resource_delete(_vc_resource);
 
@@ -63,38 +72,147 @@ void DispmanxFrameGrabber::setFlags(const int vc_flags)
 
 void DispmanxFrameGrabber::setVideoMode(const VideoMode videoMode)
 {
-	switch (videoMode) {
-	case VIDEO_3DSBS:
-		vc_dispmanx_rect_set(&_rectangle, 0, 0, _width/2, _height);
-		break;
-	case VIDEO_3DTAB:
-		vc_dispmanx_rect_set(&_rectangle, 0, 0, _width, _height/2);
-		break;
-	case VIDEO_2D:
-	default:
-		vc_dispmanx_rect_set(&_rectangle, 0, 0, _width, _height);
-		break;
+	_videoMode = videoMode;
+}
+
+void DispmanxFrameGrabber::setCropping(unsigned cropLeft, unsigned cropRight, unsigned cropTop, unsigned cropBottom)
+{
+	if (cropLeft + cropRight >= _width || cropTop + cropBottom >= _height)
+	{
+		std::cout
+			<< "DISPMANXGRABBER ERROR: Rejecting invalid crop values"
+			<< " left: " << cropLeft
+			<< " right: " << cropRight
+			<< " top: " << cropTop
+			<< " bottom: " << cropBottom
+			<< std::endl;
+		return;
+	}
+	_cropLeft = cropLeft;
+	_cropRight = cropRight;
+	_cropTop = cropTop;
+	_cropBottom = cropBottom;
+
+	if (cropLeft > 0 || cropRight > 0 || cropTop > 0 || cropBottom > 0)
+	{
+		std::cout
+			<< "DISPMANXGRABBER INFO: Cropping " << _width << "x" << _height << " image"
+			<< " left: " << cropLeft
+			<< " right: " << cropRight
+			<< " top: " << cropTop
+			<< " bottom: " << cropBottom
+			<< std::endl;
 	}
 }
 
 void DispmanxFrameGrabber::grabFrame(Image<ColorRgba> & image)
 {
-	// resize the given image if needed
-	if (image.width() != unsigned(_rectangle.width) || image.height() != unsigned(_rectangle.height))
+	int ret;
+
+	// vc_dispmanx_resource_read_data doesn't seem to work well
+	// with arbitrary positions so we have to handle cropping by ourselves
+	unsigned cropLeft = _cropLeft;
+	unsigned cropRight = _cropRight;
+	unsigned cropTop = _cropTop;
+	unsigned cropBottom = _cropBottom;
+
+	if (_vc_flags & DISPMANX_SNAPSHOT_FILL)
 	{
-		image.resize(_rectangle.width, _rectangle.height);
+		// disable cropping, we are capturing the video overlay window
+		cropLeft = cropRight = cropTop = cropBottom = 0;
+	}
+
+	unsigned imageWidth = _width - cropLeft - cropRight;
+	unsigned imageHeight = _height - cropTop - cropBottom;
+
+	// calculate final image dimensions and adjust top/left cropping in 3D modes
+	switch (_videoMode)
+	{
+	case VIDEO_3DSBS:
+		imageWidth = imageWidth / 2;
+		cropLeft = cropLeft / 2;
+		break;
+	case VIDEO_3DTAB:
+		imageHeight = imageHeight / 2;
+		cropTop = cropTop / 2;
+		break;
+	case VIDEO_2D:
+	default:
+		break;
+	}
+
+	// resize the given image if needed
+	if (image.width() != imageWidth || image.height() != imageHeight)
+	{
+		image.resize(imageWidth, imageHeight);
 	}
 
 	// Open the connection to the display
 	_vc_display = vc_dispmanx_display_open(0);
+	if (_vc_display < 0)
+	{
+		std::cout << "DISPMANXGRABBER ERROR: Cannot open display: " << _vc_display << std::endl;
+		return;
+	}
 
 	// Create the snapshot (incl down-scaling)
-	vc_dispmanx_snapshot(_vc_display, _vc_resource, (DISPMANX_TRANSFORM_T) _vc_flags);
+	ret = vc_dispmanx_snapshot(_vc_display, _vc_resource, (DISPMANX_TRANSFORM_T) _vc_flags);
+	if (ret < 0)
+	{
+		std::cout << "DISPMANXGRABBER ERROR: Snapshot failed: " << ret << std::endl;
+		vc_dispmanx_display_close(_vc_display);
+		return;
+	}
 
 	// Read the snapshot into the memory
-	void* image_ptr = image.memptr();
-	const unsigned destPitch = _rectangle.width * sizeof(ColorRgba);
-	vc_dispmanx_resource_read_data(_vc_resource, &_rectangle, image_ptr, destPitch);
+	void* imagePtr = image.memptr();
+	void* capturePtr = imagePtr;
+
+	unsigned imagePitch = imageWidth * sizeof(ColorRgba);
+
+	// dispmanx seems to require the pitch to be a multiple of 64
+	unsigned capturePitch = (_rectangle.width * sizeof(ColorRgba) + 63) & (~63);
+
+	// grab to temp buffer if image pitch isn't valid or if we are cropping
+	if (imagePitch != capturePitch 
+	    || (unsigned)_rectangle.width != imageWidth 
+	    || (unsigned)_rectangle.height != imageHeight)
+	{
+		// check if we need to resize the capture buffer
+		unsigned captureSize = capturePitch * _rectangle.height / sizeof(ColorRgba);
+		if (_captureBufferSize != captureSize)
+		{
+			delete[] _captureBuffer;
+			_captureBuffer = new ColorRgba[captureSize];
+			_captureBufferSize = captureSize;
+		}
+
+		capturePtr = &_captureBuffer[0];
+	}
+
+	ret = vc_dispmanx_resource_read_data(_vc_resource, &_rectangle, capturePtr, capturePitch);
+	if (ret < 0)
+	{
+		std::cout << "DISPMANXGRABBER ERROR: vc_dispmanx_resource_read_data failed: " << ret << std::endl;
+		vc_dispmanx_display_close(_vc_display);
+		return;
+	}
+
+	// copy capture data to image if we captured to temp buffer
+	if (imagePtr != capturePtr)
+	{
+		// adjust source pointer to top/left cropping
+		uint8_t* src_ptr = (uint8_t*) capturePtr
+			+ cropLeft * sizeof(ColorRgba)
+			+ cropTop * capturePitch;
+
+		for (unsigned y = 0; y < imageHeight; y++)
+		{
+			memcpy((uint8_t*)imagePtr + y * imagePitch,
+				src_ptr + y * capturePitch,
+				imagePitch);
+		}
+	}
 
 	// Close the displaye
 	vc_dispmanx_display_close(_vc_display);

libsrc/grabber/dispmanx/DispmanxWrapper.cpp

@@ -94,3 +94,9 @@ void DispmanxWrapper::setVideoMode(const VideoMode mode)
 {
 	_frameGrabber->setVideoMode(mode);
 }
+
+void DispmanxWrapper::setCropping(const unsigned cropLeft, const unsigned cropRight,
+	const unsigned cropTop, const unsigned cropBottom)
+{
+	_frameGrabber->setCropping(cropLeft, cropRight, cropTop, cropBottom);
+}

src/hyperion-dispmanx/DispmanxWrapper.cpp

@@ -2,10 +2,16 @@
 // Hyperion-Dispmanx includes
 #include "DispmanxWrapper.h"
 
-DispmanxWrapper::DispmanxWrapper(const unsigned grabWidth, const unsigned grabHeight, const unsigned updateRate_Hz) :
+DispmanxWrapper::DispmanxWrapper(const unsigned grabWidth, const unsigned grabHeight,
+	const VideoMode& videoMode,
+	const unsigned cropLeft, const unsigned cropRight,
+	const unsigned cropTop, const unsigned cropBottom,
+	const unsigned updateRate_Hz) :
     _timer(this),
     _grabber(grabWidth, grabHeight)
 {
+    _grabber.setVideoMode(videoMode);
+    _grabber.setCropping(cropLeft, cropRight, cropTop, cropBottom);
     _timer.setSingleShot(false);
     _timer.setInterval(updateRate_Hz);
 

src/hyperion-dispmanx/DispmanxWrapper.h

@@ -9,7 +9,11 @@ class DispmanxWrapper : public QObject
 {
 	Q_OBJECT
 public:
-	DispmanxWrapper(const unsigned grabWidth, const unsigned grabHeight, const unsigned updateRate_Hz);
+	DispmanxWrapper(const unsigned grabWidth, const unsigned grabHeight,
+		const VideoMode& videoMode,
+		const unsigned cropLeft, const unsigned cropRight,
+		const unsigned cropTop, const unsigned cropBottom,
+		const unsigned updateRate_Hz);
 
 	const Image<ColorRgb> & getScreenshot();
 

src/hyperion-dispmanx/hyperion-dispmanx.cpp

@@ -46,6 +46,14 @@ int main(int argc, char ** argv)
 		SwitchParameter<>      & argSkipReply  = parameters.add<SwitchParameter<>>     (0x0, "skip-reply",  "Do not receive and check reply messages from Hyperion");
 		SwitchParameter<>      & argHelp       = parameters.add<SwitchParameter<>>     ('h', "help",        "Show this help message and exit");
 
+		IntParameter           & argCropLeft   = parameters.add<IntParameter>          (0x0, "crop-left",       "pixels to remove on left after grabbing");
+		IntParameter           & argCropRight  = parameters.add<IntParameter>          (0x0, "crop-right",       "pixels to remove on right after grabbing");
+		IntParameter           & argCropTop    = parameters.add<IntParameter>          (0x0, "crop-top",       "pixels to remove on top after grabbing");
+		IntParameter           & argCropBottom = parameters.add<IntParameter>          (0x0, "crop-bottom",       "pixels to remove on bottom after grabbing");
+
+		SwitchParameter<>      & arg3DSBS           = parameters.add<SwitchParameter<>>     (0x0, "3DSBS",            "Interpret the incoming video stream as 3D side-by-side");
+		SwitchParameter<>      & arg3DTAB           = parameters.add<SwitchParameter<>>     (0x0, "3DTAB",            "Interpret the incoming video stream as 3D top-and-bottom");
+
 		// set defaults
 		argFps.setDefault(10);
 		argWidth.setDefault(64);
@@ -53,9 +61,25 @@ int main(int argc, char ** argv)
 		argAddress.setDefault("127.0.0.1:19445");
 		argPriority.setDefault(800);
 
+		argCropLeft.setDefault(0);
+		argCropRight.setDefault(0);
+		argCropTop.setDefault(0);
+		argCropBottom.setDefault(0);
+
 		// parse all options
 		optionParser.parse(argc, const_cast<const char **>(argv));
 
+		VideoMode videoMode = VIDEO_2D;
+
+		if (arg3DSBS.isSet())
+		{
+			videoMode = VIDEO_3DSBS;
+		}
+		else if (arg3DTAB.isSet())
+		{
+			videoMode = VIDEO_3DTAB;
+		}
+
 		// check if we need to display the usage. exit if we do.
 		if (argHelp.isSet())
 		{
@@ -65,7 +89,13 @@ int main(int argc, char ** argv)
 
 		// Create the dispmanx grabbing stuff
 		int grabInterval = 1000 / argFps.getValue();
-		DispmanxWrapper dispmanxWrapper(argWidth.getValue(),argHeight.getValue(),grabInterval);
+		DispmanxWrapper dispmanxWrapper(argWidth.getValue(),argHeight.getValue(),
+			videoMode,
+			std::max(0, argCropLeft.getValue()),
+			std::max(0, argCropRight.getValue()),
+			std::max(0, argCropTop.getValue()),
+			std::max(0, argCropBottom.getValue()),
+			grabInterval);
 
 		if (argScreenshot.isSet())
 		{

src/hyperiond/hyperiond.cpp

@@ -231,6 +231,11 @@ int main(int argc, char** argv)
 			frameGrabberConfig["frequency_Hz"].asUInt(),
 			frameGrabberConfig.get("priority",900).asInt(),
 			&hyperion);
+		dispmanx->setCropping(
+					frameGrabberConfig.get("cropLeft", 0).asInt(),
+					frameGrabberConfig.get("cropRight", 0).asInt(),
+					frameGrabberConfig.get("cropTop", 0).asInt(),
+					frameGrabberConfig.get("cropBottom", 0).asInt());
 
 		if (xbmcVideoChecker != nullptr)
 		{