kms_vaapi: support multiple drm planes (fixes capture on some multi monitor systems)

1 files changed, 164 insertions, 92 deletions

diff --git a/kms/server/kms_server.c b/kms/server/kms_server.c
index 9c4cb36..76bf718 100644
--- a/kms/server/kms_server.c
+++ b/kms/server/kms_server.c
@@ -15,18 +15,30 @@
 #include <xf86drm.h>
 #include <xf86drmMode.h>
 
-#define DRM_CLIENT_CAP_UNIVERSAL_PLANES 2
+#define MAX_CONNECTORS 32
 
 typedef struct {
     int drmfd;
-    uint32_t plane_id;
+    uint32_t plane_ids[GSR_KMS_MAX_PLANES];
+    uint32_t connector_ids[GSR_KMS_MAX_PLANES];
+    size_t num_plane_ids;
 } gsr_drm;
 
+typedef struct {
+    uint32_t connector_id;
+    uint64_t crtc_id;
+} connector_crtc_pair;
+
+typedef struct {
+    connector_crtc_pair maps[MAX_CONNECTORS];
+    int num_maps;
+} connector_to_crtc_map;
+
 static int max_int(int a, int b) {
     return a > b ? a : b;
 }
 
-static int send_msg_to_client(int client_fd, gsr_kms_response *response, int *fds, int num_fds) {
+static int send_msg_to_client(int client_fd, gsr_kms_response *response) {
     struct iovec iov;
     iov.iov_base = response;
     iov.iov_len = sizeof(*response);
@@ -35,52 +47,95 @@ static int send_msg_to_client(int client_fd, gsr_kms_response *response, int *fd
     response_message.msg_iov = &iov;
     response_message.msg_iovlen = 1;
 
-    char cmsgbuf[CMSG_SPACE(sizeof(int)) * max_int(1, num_fds)];
+    char cmsgbuf[CMSG_SPACE(sizeof(int) * max_int(1, response->num_fds))];
     memset(cmsgbuf, 0, sizeof(cmsgbuf));
 
-    if(num_fds > 0) {
+    if(response->num_fds > 0) {
         response_message.msg_control = cmsgbuf;
         response_message.msg_controllen = sizeof(cmsgbuf);
 
-        int total_msg_len = 0;
-        struct cmsghdr *cmsg = NULL;
-        for(int i = 0; i < num_fds; ++i) {
-            if(i == 0)
-                cmsg = CMSG_FIRSTHDR(&response_message);
-            else
-                cmsg = CMSG_NXTHDR(&response_message, cmsg);
-
-            cmsg->cmsg_level = SOL_SOCKET;
-            cmsg->cmsg_type = SCM_RIGHTS;
-            cmsg->cmsg_len = CMSG_LEN(sizeof(int));
-            memcpy(CMSG_DATA(cmsg), &fds[i], sizeof(int));
-            total_msg_len += cmsg->cmsg_len;
+        struct cmsghdr *cmsg = CMSG_FIRSTHDR(&response_message);
+        cmsg->cmsg_level = SOL_SOCKET;
+        cmsg->cmsg_type = SCM_RIGHTS;
+        cmsg->cmsg_len = CMSG_LEN(sizeof(int) * response->num_fds);
+
+        int *fds = (int*)CMSG_DATA(cmsg);
+        for(int i = 0; i < response->num_fds; ++i) {
+            fds[i] = response->fds[i].fd;
         }
 
-        response_message.msg_controllen = total_msg_len;
+        response_message.msg_controllen = cmsg->cmsg_len;
     }
 
     return sendmsg(client_fd, &response_message, 0);
 }
 
-static int kms_get_plane_id(gsr_drm *drm) {
+static bool connector_get_property_by_name(int drmfd, drmModeConnectorPtr props, const char *name, uint64_t *result) {
+    for(int i = 0; i < props->count_props; ++i) {
+        drmModePropertyPtr prop = drmModeGetProperty(drmfd, props->props[i]);
+        if(prop) {
+            if(strcmp(name, prop->name) == 0) {
+                *result = props->prop_values[i];
+                drmModeFreeProperty(prop);
+                return true;
+            }
+            drmModeFreeProperty(prop);
+        }
+    }
+    return false;
+}
+
+/* Returns 0 if not found */
+static uint32_t get_connector_by_crtc_id(const connector_to_crtc_map *c2crtc_map, uint32_t crtc_id) {
+    for(int i = 0; i < c2crtc_map->num_maps; ++i) {
+        if(c2crtc_map->maps[i].crtc_id == crtc_id)
+            return c2crtc_map->maps[i].connector_id;
+    }
+    return 0;
+}
+
+static int kms_get_plane_ids(gsr_drm *drm) {
     drmModePlaneResPtr planes = NULL;
+    drmModeResPtr resources = NULL;
     int result = -1;
-    int64_t max_size = 0;
-    uint32_t best_plane_match = UINT32_MAX;
+
+    connector_to_crtc_map c2crtc_map;
+    c2crtc_map.num_maps = 0;
 
     if(drmSetClientCap(drm->drmfd, DRM_CLIENT_CAP_UNIVERSAL_PLANES, 1) != 0) {
-        fprintf(stderr, "kms server error: drmSetClientCap failed, error: %s\n", strerror(errno));
+        fprintf(stderr, "kms server error: drmSetClientCap DRM_CLIENT_CAP_UNIVERSAL_PLANES failed, error: %s\n", strerror(errno));
         goto error;
     }
 
+    if(drmSetClientCap(drm->drmfd, DRM_CLIENT_CAP_ATOMIC, 1) != 0) {
+        fprintf(stderr, "kms server warning: drmSetClientCap DRM_CLIENT_CAP_ATOMIC failed, error: %s. The wrong monitor may be captured as a result\n", strerror(errno));
+    }
+
     planes = drmModeGetPlaneResources(drm->drmfd);
     if(!planes) {
         fprintf(stderr, "kms server error: failed to access planes, error: %s\n", strerror(errno));
         goto error;
     }
 
-    for(uint32_t i = 0; i < planes->count_planes; ++i) {
+    resources = drmModeGetResources(drm->drmfd);
+    if(resources) {
+        for(int i = 0; i < resources->count_connectors && c2crtc_map.num_maps < MAX_CONNECTORS; ++i) {
+            drmModeConnectorPtr connector = drmModeGetConnectorCurrent(drm->drmfd, resources->connectors[i]);
+            if(connector) {
+                uint64_t crtc_id = 0;
+                connector_get_property_by_name(drm->drmfd, connector, "CRTC_ID", &crtc_id);
+
+                c2crtc_map.maps[c2crtc_map.num_maps].connector_id = connector->connector_id;
+                c2crtc_map.maps[c2crtc_map.num_maps].crtc_id = crtc_id;
+                ++c2crtc_map.num_maps;
+
+                drmModeFreeConnector(connector);
+            }
+        }
+        drmModeFreeResources(resources);
+    }
+
+    for(uint32_t i = 0; i < planes->count_planes && drm->num_plane_ids < GSR_KMS_MAX_PLANES; ++i) {
         drmModePlanePtr plane = drmModeGetPlane(drm->drmfd, planes->planes[i]);
         if(!plane) {
             fprintf(stderr, "kms server warning: failed to get drmModePlanePtr for plane %#x: %s (%d)\n", planes->planes[i], strerror(errno), errno);
@@ -95,22 +150,14 @@ static int kms_get_plane_id(gsr_drm *drm) {
         // TODO: Fallback to getfb(1)?
         drmModeFB2Ptr drmfb = drmModeGetFB2(drm->drmfd, plane->fb_id);
         if(drmfb) {
-            const int64_t plane_size = (int64_t)drmfb->width * (int64_t)drmfb->height;
-            if(drmfb->handles[0] && plane_size >= max_size) {
-                max_size = plane_size;
-                best_plane_match = plane->plane_id;
-            }
+            drm->plane_ids[drm->num_plane_ids] = plane->plane_id;
+            drm->connector_ids[drm->num_plane_ids] = get_connector_by_crtc_id(&c2crtc_map, plane->crtc_id);
+            ++drm->num_plane_ids;
             drmModeFreeFB2(drmfb);
         }
         drmModeFreePlane(plane);
     }
 
-    if(best_plane_match == UINT32_MAX || max_size == 0) {
-        fprintf(stderr, "kms server error: failed to find a usable plane\n");
-        goto error;
-    }
-
-    drm->plane_id = best_plane_match;
     result = 0;
 
     error:
@@ -120,65 +167,87 @@ static int kms_get_plane_id(gsr_drm *drm) {
     return result;
 }
 
+static bool drmfb_has_multiple_handles(drmModeFB2 *drmfb) {
+    int num_handles = 0;
+    for(uint32_t handle_index = 0; handle_index < 4 && drmfb->handles[handle_index]; ++handle_index) {
+        ++num_handles;
+    }
+    return num_handles > 1;
+}
+
 static int kms_get_fb(gsr_drm *drm, gsr_kms_response *response) {
-    drmModePlanePtr plane = NULL;
-    drmModeFB2 *drmfb = NULL;
     int result = -1;
 
     response->result = KMS_RESULT_OK;
-    response->data.fd.fd = 0;
-    response->data.fd.width = 0;
-    response->data.fd.height = 0;
-
-    plane = drmModeGetPlane(drm->drmfd, drm->plane_id);
-    if(!plane) {
-        response->result = KMS_RESULT_FAILED_TO_GET_PLANE;
-        snprintf(response->data.err_msg, sizeof(response->data.err_msg), "failed to get drm plane with id %u, error: %s\n", drm->plane_id, strerror(errno));
-        fprintf(stderr, "kms server error: %s\n", response->data.err_msg);
-        goto error;
-    }
+    response->err_msg[0] = '\0';
+    response->num_fds = 0;
 
-    drmfb = drmModeGetFB2(drm->drmfd, plane->fb_id);
-    if(!drmfb) {
-        response->result = KMS_RESULT_FAILED_TO_GET_PLANE;
-        snprintf(response->data.err_msg, sizeof(response->data.err_msg), "drmModeGetFB2 failed, error: %s", strerror(errno));
-        fprintf(stderr, "kms server error: %s\n", response->data.err_msg);
-        goto error;
-    }
+    for(size_t i = 0; i < drm->num_plane_ids && response->num_fds < GSR_KMS_MAX_PLANES; ++i) {
+        drmModePlanePtr plane = NULL;
+        drmModeFB2 *drmfb = NULL;
 
-    if(!drmfb->handles[0]) {
-        response->result = KMS_RESULT_FAILED_TO_GET_PLANE;
-        snprintf(response->data.err_msg, sizeof(response->data.err_msg), "drmfb handle is NULL");
-        fprintf(stderr, "kms server error: %s\n", response->data.err_msg);
-        goto error;
-    }
+        plane = drmModeGetPlane(drm->drmfd, drm->plane_ids[i]);
+        if(!plane) {
+            response->result = KMS_RESULT_FAILED_TO_GET_PLANE;
+            snprintf(response->err_msg, sizeof(response->err_msg), "failed to get drm plane with id %u, error: %s\n", drm->plane_ids[i], strerror(errno));
+            fprintf(stderr, "kms server error: %s\n", response->err_msg);
+            goto next;
+        }
 
-    // TODO: Check if dimensions have changed by comparing width and height to previous time this was called.
-    // TODO: Support other plane formats than rgb (with multiple planes, such as direct YUV420 on wayland).
+        drmfb = drmModeGetFB2(drm->drmfd, plane->fb_id);
+        if(!drmfb) {
+            // Commented out for now because we get here if the cursor is moved to another monitor and we dont care about the cursor
+            //response->result = KMS_RESULT_FAILED_TO_GET_PLANE;
+            //snprintf(response->err_msg, sizeof(response->err_msg), "drmModeGetFB2 failed, error: %s", strerror(errno));
+            //fprintf(stderr, "kms server error: %s\n", response->err_msg);
+            goto next;
+        }
 
-    int fb_fd = -1;
-    const int ret = drmPrimeHandleToFD(drm->drmfd, drmfb->handles[0], O_RDONLY, &fb_fd);
-    if(ret != 0 || fb_fd == -1) {
-        response->result = KMS_RESULT_FAILED_TO_GET_PLANE;
-        snprintf(response->data.err_msg, sizeof(response->data.err_msg), "failed to get fd from drm handle, error: %s", strerror(errno));
-        fprintf(stderr, "kms server error: %s\n", response->data.err_msg);
-        goto error;
-    }
+        if(!drmfb->handles[0]) {
+            response->result = KMS_RESULT_FAILED_TO_GET_PLANE;
+            snprintf(response->err_msg, sizeof(response->err_msg), "drmfb handle is NULL");
+            fprintf(stderr, "kms server error: %s\n", response->err_msg);
+            goto next;
+        }
 
-    response->data.fd.fd = fb_fd;
-    response->data.fd.width = drmfb->width;
-    response->data.fd.height = drmfb->height;
-    response->data.fd.pitch = drmfb->pitches[0];
-    response->data.fd.offset = drmfb->offsets[0];
-    response->data.fd.pixel_format = drmfb->pixel_format;
-    response->data.fd.modifier = drmfb->modifier;
-    result = 0;
+        // TODO: Check if dimensions have changed by comparing width and height to previous time this was called.
+        // TODO: Support other plane formats than rgb (with multiple planes, such as direct YUV420 on wayland).
 
-    error:
-    if(drmfb)
-        drmModeFreeFB2(drmfb);
-    if(plane)
-        drmModeFreePlane(plane);
+        int fb_fd = -1;
+        const int ret = drmPrimeHandleToFD(drm->drmfd, drmfb->handles[0], O_RDONLY, &fb_fd);
+        if(ret != 0 || fb_fd == -1) {
+            response->result = KMS_RESULT_FAILED_TO_GET_PLANE;
+            snprintf(response->err_msg, sizeof(response->err_msg), "failed to get fd from drm handle, error: %s", strerror(errno));
+            fprintf(stderr, "kms server error: %s\n", response->err_msg);
+            continue;
+        }
+
+        response->fds[response->num_fds].fd = fb_fd;
+        response->fds[response->num_fds].width = drmfb->width;
+        response->fds[response->num_fds].height = drmfb->height;
+        response->fds[response->num_fds].pitch = drmfb->pitches[0];
+        response->fds[response->num_fds].offset = drmfb->offsets[0];
+        response->fds[response->num_fds].pixel_format = drmfb->pixel_format;
+        response->fds[response->num_fds].modifier = drmfb->modifier;
+        response->fds[response->num_fds].connector_id = drm->connector_ids[i];
+        response->fds[response->num_fds].is_combined_plane = drmfb_has_multiple_handles(drmfb);
+        ++response->num_fds;
+
+        next:
+        if(drmfb)
+            drmModeFreeFB2(drmfb);
+        if(plane)
+            drmModeFreePlane(plane);
+    }
+
+    if(response->num_fds > 0 || response->result == KMS_RESULT_OK) {
+        result = 0;
+    } else {
+        for(int i = 0; i < response->num_fds; ++i) {
+            close(response->fds[i].fd);
+        }
+        response->num_fds = 0;
+    }
 
     return result;
 }
@@ -207,14 +276,14 @@ int main(int argc, char **argv) {
     const char *card_path = argv[2];
 
     gsr_drm drm;
-    drm.plane_id = 0;
+    drm.num_plane_ids = 0;
     drm.drmfd = open(card_path, O_RDONLY);
     if(drm.drmfd < 0) {
         fprintf(stderr, "kms server error: failed to open %s, error: %s", card_path, strerror(errno));
         return 2;
     }
 
-    if(kms_get_plane_id(&drm) != 0) {
+    if(kms_get_plane_ids(&drm) != 0) {
         close(drm.drmfd);
         return 2;
     }
@@ -284,11 +353,14 @@ int main(int argc, char **argv) {
                 gsr_kms_response response;
                 
                 if(kms_get_fb(&drm, &response) == 0) {
-                    if(send_msg_to_client(socket_fd, &response, &response.data.fd.fd, 1) == -1)
+                    if(send_msg_to_client(socket_fd, &response) == -1)
                         fprintf(stderr, "kms server error: failed to respond to client KMS_REQUEST_TYPE_GET_KMS request\n");
-                    close(response.data.fd.fd);
+
+                    for(int i = 0; i < response.num_fds; ++i) {
+                        close(response.fds[i].fd);
+                    }
                 } else {
-                    if(send_msg_to_client(socket_fd, &response, NULL, 0) == -1)
+                    if(send_msg_to_client(socket_fd, &response) == -1)
                         fprintf(stderr, "kms server error: failed to respond to client KMS_REQUEST_TYPE_GET_KMS request\n");
                 }
 
@@ -297,9 +369,9 @@ int main(int argc, char **argv) {
             default: {
                 gsr_kms_response response;
                 response.result = KMS_RESULT_INVALID_REQUEST;
-                snprintf(response.data.err_msg, sizeof(response.data.err_msg), "invalid request type %d, expected %d (%s)", request.type, KMS_REQUEST_TYPE_GET_KMS, "KMS_REQUEST_TYPE_GET_KMS");
-                fprintf(stderr, "kms server error: %s\n", response.data.err_msg);
-                if(send_msg_to_client(socket_fd, &response, NULL, 0) == -1) {
+                snprintf(response.err_msg, sizeof(response.err_msg), "invalid request type %d, expected %d (%s)", request.type, KMS_REQUEST_TYPE_GET_KMS, "KMS_REQUEST_TYPE_GET_KMS");
+                fprintf(stderr, "kms server error: %s\n", response.err_msg);
+                if(send_msg_to_client(socket_fd, &response) == -1) {
                     fprintf(stderr, "kms server error: failed to respond to client request\n");
                     break;
                 }