Implement custom display resolution

Updated:
- removed ununsed yoffset
- removed VIDEO_MODE_NO_OFF_SCREEN usage
- commented why 2 modes are needed

3 files changed, 147 insertions, 4 deletions

diff --git a/display/driver.c b/display/driver.c
index 5c4578c..ef011ad 100644
--- a/display/driver.c
+++ b/display/driver.c
@@ -45,6 +45,7 @@
 
 static DRVFN drv_calls[] = {
     {INDEX_DrvDisableDriver, (PFN)DrvDisableDriver},
+    {INDEX_DrvEscape, (PFN)DrvEscape},
     {INDEX_DrvEnablePDEV, (PFN)DrvEnablePDEV},
     {INDEX_DrvDisablePDEV, (PFN)DrvDisablePDEV},
     {INDEX_DrvCompletePDEV, (PFN)DrvCompletePDEV},
@@ -252,6 +253,37 @@ BOOL DrvEnableDriver(ULONG engine_version, ULONG enable_data_size, PDRVENABLEDAT
     return TRUE;
 }
 
+ULONG DrvEscape(SURFOBJ *pso, ULONG iEsc, ULONG cjIn, PVOID pvIn,
+                ULONG cjOut, PVOID pvOut)
+{
+    PDev* pdev = pso ? (PDev*)pso->dhpdev : NULL;
+    int RetVal = -1;
+
+    switch (iEsc) {
+    case QXL_ESCAPE_SET_CUSTOM_DISPLAY: {
+        ULONG length;
+
+        DEBUG_PRINT((pdev, 1, "set custom display %p\n", pdev));
+        if (pdev == NULL)
+            break;
+
+        if (EngDeviceIoControl(pdev->driver, IOCTL_QXL_SET_CUSTOM_DISPLAY,
+                               pvIn, cjIn, NULL, 0, &length)) {
+            DEBUG_PRINT((pdev, 0, "%s: IOCTL_QXL_SET_CUSTOM_DISPLAY failed\n", __FUNCTION__));
+            break;
+        }
+        RetVal = 1;
+        break;
+    }
+    default:
+        DEBUG_PRINT((NULL, 1, "%s: unhandled escape code %d\n", __FUNCTION__, iEsc));
+        RetVal = 0;
+    }
+
+    DEBUG_PRINT((NULL, 1, "%s: end\n", __FUNCTION__));
+    return RetVal;
+}
+
 VOID DrvDisableDriver(VOID)
 {
     DEBUG_PRINT((NULL, 1, "%s\n", __FUNCTION__));
diff --git a/include/qxl_driver.h b/include/qxl_driver.h
index af65916..eac6f5f 100644
--- a/include/qxl_driver.h
+++ b/include/qxl_driver.h
@@ -23,6 +23,7 @@
 #define _H_QXL_DRIVER
 
 #include <spice\qxl_dev.h>
+#include <spice\qxl_windows.h>
 
 #if (WINVER < 0x0501)
 #include "wdmhelper.h"
@@ -30,12 +31,16 @@
 
 enum {
     FIRST_AVIL_IOCTL_FUNC = 0x800,
-    QXL_GET_INFO_FUNC = FIRST_AVIL_IOCTL_FUNC
+    QXL_GET_INFO_FUNC = FIRST_AVIL_IOCTL_FUNC,
+    QXL_SET_CUSTOM_DISPLAY
 };
 
 #define IOCTL_QXL_GET_INFO \
     CTL_CODE(FILE_DEVICE_VIDEO, QXL_GET_INFO_FUNC, METHOD_BUFFERED, FILE_ANY_ACCESS)
 
+#define IOCTL_QXL_SET_CUSTOM_DISPLAY \
+    CTL_CODE(FILE_DEVICE_VIDEO, QXL_SET_CUSTOM_DISPLAY, METHOD_BUFFERED, FILE_ANY_ACCESS)
+
 #define QXL_DRIVER_INFO_VERSION 3
 
 typedef struct MemSlot {
@@ -116,6 +121,5 @@ typedef struct QXLDriverInfo {
     UINT64 fb_phys;
 } QXLDriverInfo;
 
-
 #endif
 
diff --git a/miniport/qxl.c b/miniport/qxl.c
index aefd780..44c2a40 100644
--- a/miniport/qxl.c
+++ b/miniport/qxl.c
@@ -83,6 +83,7 @@ typedef struct QXLExtension {
 
     ULONG current_mode;
     ULONG n_modes;
+    ULONG custom_mode;
     PVIDEO_MODE_INFORMATION modes;
 
     PEVENT display_event;
@@ -442,6 +443,69 @@ VP_STATUS Prob(QXLExtension *dev, VIDEO_PORT_CONFIG_INFO *conf_info,
 }
 
 #if defined(ALLOC_PRAGMA)
+void FillVidModeBPP(VIDEO_MODE_INFORMATION *pMode, ULONG bitsR, ULONG bitsG, ULONG bitsB,
+                    ULONG maskR, ULONG maskG, ULONG maskB);
+#pragma alloc_text(PAGE, FillVidModeBPP)
+#endif
+
+/* Fills given video mode BPP related fields */
+void FillVidModeBPP(VIDEO_MODE_INFORMATION *pMode, ULONG bitsR, ULONG bitsG, ULONG bitsB,
+                    ULONG maskR, ULONG maskG, ULONG maskB)
+{
+    pMode->NumberRedBits   = bitsR;
+    pMode->NumberGreenBits = bitsG;
+    pMode->NumberBlueBits  = bitsB;
+    pMode->RedMask         = maskR;
+    pMode->GreenMask       = maskG;
+    pMode->BlueMask        = maskB;
+}
+
+#if defined(ALLOC_PRAGMA)
+VP_STATUS FillVidModeInfo(VIDEO_MODE_INFORMATION *pMode, ULONG xres, ULONG yres, ULONG bpp, ULONG index);
+#pragma alloc_text(PAGE, FillVidModeInfo)
+#endif
+/* Fills given video mode structure */
+VP_STATUS FillVidModeInfo(VIDEO_MODE_INFORMATION *pMode, ULONG xres, ULONG yres, ULONG bpp, ULONG index)
+{
+    if (xres <= 0 || yres <= 0)
+        return ERROR_INVALID_DATA;
+
+    VideoPortZeroMemory(pMode, sizeof(VIDEO_MODE_INFORMATION));
+
+    /*Common entries*/
+    pMode->Length                       = sizeof(VIDEO_MODE_INFORMATION);
+    pMode->ModeIndex                    = index;
+    pMode->VisScreenWidth               = xres;
+    pMode->VisScreenHeight              = yres;
+    pMode->ScreenStride                 = xres * ((bpp + 7) / 8);
+    pMode->NumberOfPlanes               = 1;
+    pMode->BitsPerPlane                 = bpp;
+    pMode->Frequency                    = 60;
+    pMode->XMillimeter                  = 320;
+    pMode->YMillimeter                  = 240;
+    pMode->VideoMemoryBitmapWidth       = xres;
+    pMode->VideoMemoryBitmapHeight      = yres;
+    pMode->DriverSpecificAttributeFlags = 0;
+    pMode->AttributeFlags               = VIDEO_MODE_GRAPHICS | VIDEO_MODE_COLOR;
+
+    /*BPP related entries*/
+    switch (bpp)
+    {
+        case 16:
+            FillVidModeBPP(pMode, 5, 6, 5, 0xF800, 0x7E0, 0x1F);
+            break;
+        case 24:
+        case 32:
+            FillVidModeBPP(pMode, 8, 8, 8, 0xFF0000, 0xFF00, 0xFF);
+            break;
+        default:
+            return ERROR_INVALID_DATA;
+    }
+
+    return NO_ERROR;
+}
+
+#if defined(ALLOC_PRAGMA)
 VP_STATUS SetVideoModeInfo(QXLExtension *dev, PVIDEO_MODE_INFORMATION video_mode, QXLMode *qxl_mode);
 #pragma alloc_text(PAGE, SetVideoModeInfo)
 #endif
@@ -550,7 +614,7 @@ VP_STATUS InitModes(QXLExtension *dev)
         return ERROR_NOT_ENOUGH_MEMORY;
     }
 #endif
-    VideoPortZeroMemory(modes_info, sizeof(VIDEO_MODE_INFORMATION) * n_modes);
+    VideoPortZeroMemory(modes_info, sizeof(VIDEO_MODE_INFORMATION) * n_modes + 2);
     for (i = 0; i < n_modes; i++) {
         error = SetVideoModeInfo(dev, &modes_info[i], &modes->modes[i]);
         if (error != NO_ERROR) {
@@ -559,7 +623,17 @@ VP_STATUS InitModes(QXLExtension *dev)
             return error;
         }
     }
-    dev->n_modes = n_modes;
+
+    /* 2 dummy modes for custom display resolution */
+    /* This is necessary to bypass Windows mode index check, that
+       would prevent reusing the same index */
+    dev->custom_mode = n_modes;
+    memcpy(&modes_info[n_modes], &modes_info[0], sizeof(VIDEO_MODE_INFORMATION));
+    modes_info[n_modes].ModeIndex = n_modes;
+    memcpy(&modes_info[n_modes + 1], &modes_info[0], sizeof(VIDEO_MODE_INFORMATION));
+    modes_info[n_modes + 1].ModeIndex = n_modes + 1;
+
+    dev->n_modes = n_modes + 2;
     dev->modes = modes_info;
     DEBUG_PRINT((dev, 0, "%s OK\n", __FUNCTION__));
     return NO_ERROR;
@@ -912,6 +986,20 @@ PVIDEO_MODE_INFORMATION FindMode(QXLExtension *dev_ext, ULONG mode)
     return NULL;
 }
 
+static VP_STATUS SetCustomDisplay(QXLExtension *dev_ext, QXLEscapeSetCustomDisplay *custom_display)
+{
+    /* alternate custom mode index */
+    if (dev_ext->custom_mode == (dev_ext->n_modes - 1))
+        dev_ext->custom_mode = dev_ext->n_modes - 2;
+    else
+        dev_ext->custom_mode = dev_ext->n_modes - 1;
+
+    return FillVidModeInfo(&dev_ext->modes[dev_ext->custom_mode],
+                           custom_display->xres, custom_display->yres,
+                           custom_display->bpp,
+                           dev_ext->custom_mode);
+}
+
 BOOLEAN StartIO(PVOID dev_extension, PVIDEO_REQUEST_PACKET packet)
 {
     QXLExtension *dev_ext = dev_extension;
@@ -1102,6 +1190,25 @@ BOOLEAN StartIO(PVOID dev_extension, PVIDEO_REQUEST_PACKET packet)
             driver_info->dev_id = dev_ext->rom->id;
         }
         break;
+
+    case IOCTL_QXL_SET_CUSTOM_DISPLAY: {
+            QXLEscapeSetCustomDisplay *custom_display;
+            DEBUG_PRINT((dev_ext, 0, "%s: IOCTL_QXL_SET_CUSTOM_DISPLAY\n", __FUNCTION__));
+
+            if (packet->InputBufferLength < (packet->StatusBlock->Information =
+                                             sizeof(QXLEscapeSetCustomDisplay))) {
+                error = ERROR_INSUFFICIENT_BUFFER;
+                goto err;
+            }
+
+            custom_display = packet->InputBuffer;
+            DEBUG_PRINT((dev_ext, 0, "%s: %dx%d@%d\n", __FUNCTION__,
+                         custom_display->xres, custom_display->yres,
+                         custom_display->bpp));
+            SetCustomDisplay(dev_ext, custom_display);
+        }
+        break;
+
     default:
         DEBUG_PRINT((dev_ext, 0, "%s: invalid command 0x%lx\n", __FUNCTION__, packet->IoControlCode));
         error = ERROR_INVALID_FUNCTION;