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

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);
+}