target/linux/brcm2708/patches-4.4/0505-drm-vc4-Add-a-mode-for-using-the-closed-firmware-for.patch

   1 From 96bd31d9a82377b04c72c853f3381f34c4985587 Mon Sep 17 00:00:00 2001
   2 From: Eric Anholt <eric@anholt.net>
   3 Date: Wed, 14 Sep 2016 08:39:33 +0100
   4 Subject: [PATCH] drm/vc4: Add a mode for using the closed firmware for
   5  display.
   6
   7 Signed-off-by: Eric Anholt <eric@anholt.net>
   8 ---
   9  drivers/gpu/drm/vc4/Makefile           |   1 +
  10  drivers/gpu/drm/vc4/vc4_crtc.c         |  13 +
  11  drivers/gpu/drm/vc4/vc4_drv.c          |  10 +-
  12  drivers/gpu/drm/vc4/vc4_drv.h          |   7 +
  13  drivers/gpu/drm/vc4/vc4_firmware_kms.c | 660 +++++++++++++++++++++++++++++++++
  14  5 files changed, 689 insertions(+), 2 deletions(-)
  15  create mode 100644 drivers/gpu/drm/vc4/vc4_firmware_kms.c
  16
  17 --- a/drivers/gpu/drm/vc4/Makefile
  18 +++ b/drivers/gpu/drm/vc4/Makefile
  19 @@ -9,6 +9,7 @@ vc4-y := \
  20         vc4_drv.o \
  21         vc4_dpi.o \
  22         vc4_dsi.o \
  23 +       vc4_firmware_kms.o \
  24         vc4_kms.o \
  25         vc4_gem.o \
  26         vc4_hdmi.o \
  27 --- a/drivers/gpu/drm/vc4/vc4_crtc.c
  28 +++ b/drivers/gpu/drm/vc4/vc4_crtc.c
  29 @@ -151,6 +151,9 @@ int vc4_crtc_get_scanoutpos(struct drm_d
  30         int vblank_lines;
  31         int ret = 0;
  32
  33 +       if (vc4->firmware_kms)
  34 +               return 0;
  35 +
  36         /*
  37          * XXX Doesn't work well in interlaced mode yet, partially due
  38          * to problems in vc4 kms or drm core interlaced mode handling,
  39 @@ -639,6 +642,11 @@ int vc4_enable_vblank(struct drm_device
  40         struct vc4_dev *vc4 = to_vc4_dev(dev);
  41         struct vc4_crtc *vc4_crtc = vc4->crtc[crtc_id];
  42
  43 +       if (vc4->firmware_kms) {
  44 +               /* XXX: Can we mask the SMI interrupt? */
  45 +               return 0;
  46 +       }
  47 +
  48         CRTC_WRITE(PV_INTEN, PV_INT_VFP_START);
  49
  50         return 0;
  51 @@ -649,6 +657,11 @@ void vc4_disable_vblank(struct drm_devic
  52         struct vc4_dev *vc4 = to_vc4_dev(dev);
  53         struct vc4_crtc *vc4_crtc = vc4->crtc[crtc_id];
  54
  55 +       if (vc4->firmware_kms) {
  56 +               /* XXX: Can we mask the SMI interrupt? */
  57 +               return;
  58 +       }
  59 +
  60         CRTC_WRITE(PV_INTEN, 0);
  61  }
  62
  63 --- a/drivers/gpu/drm/vc4/vc4_drv.c
  64 +++ b/drivers/gpu/drm/vc4/vc4_drv.c
  65 @@ -47,10 +47,15 @@ void __iomem *vc4_ioremap_regs(struct pl
  66
  67  static void vc4_drm_preclose(struct drm_device *dev, struct drm_file *file)
  68  {
  69 +       struct vc4_dev *vc4 = to_vc4_dev(dev);
  70         struct drm_crtc *crtc;
  71
  72 -       list_for_each_entry(crtc, &dev->mode_config.crtc_list, head)
  73 -               vc4_cancel_page_flip(crtc, file);
  74 +       list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
  75 +               if (vc4->firmware_kms)
  76 +                       vc4_fkms_cancel_page_flip(crtc, file);
  77 +               else
  78 +                       vc4_cancel_page_flip(crtc, file);
  79 +       }
  80  }
  81
  82  void vc4_dump_regs32(const struct debugfs_reg32 *regs, unsigned int num_regs,
  83 @@ -331,6 +336,7 @@ static struct platform_driver *const com
  84         &vc4_dsi_driver,
  85         &vc4_hvs_driver,
  86         &vc4_crtc_driver,
  87 +       &vc4_firmware_kms_driver,
  88         &vc4_v3d_driver,
  89  };
  90
  91 --- a/drivers/gpu/drm/vc4/vc4_drv.h
  92 +++ b/drivers/gpu/drm/vc4/vc4_drv.h
  93 @@ -14,6 +14,9 @@ struct debugfs_reg32;
  94  struct vc4_dev {
  95         struct drm_device *dev;
  96
  97 +       bool firmware_kms;
  98 +       struct rpi_firmware *firmware;
  99 +
 100         struct vc4_hdmi *hdmi;
 101         struct vc4_hvs *hvs;
 102         struct vc4_crtc *crtc[3];
 103 @@ -461,6 +464,10 @@ int vc4_dpi_debugfs_regs(struct seq_file
 104  extern struct platform_driver vc4_dsi_driver;
 105  int vc4_dsi_debugfs_regs(struct seq_file *m, void *unused);
 106
 107 +/* vc4_firmware_kms.c */
 108 +extern struct platform_driver vc4_firmware_kms_driver;
 109 +void vc4_fkms_cancel_page_flip(struct drm_crtc *crtc, struct drm_file *file);
 110 +
 111  /* vc4_gem.c */
 112  void vc4_gem_init(struct drm_device *dev);
 113  void vc4_gem_destroy(struct drm_device *dev);
 114 --- /dev/null
 115 +++ b/drivers/gpu/drm/vc4/vc4_firmware_kms.c
 116 @@ -0,0 +1,660 @@
 117 +/*
 118 + * Copyright (C) 2016 Broadcom
 119 + *
 120 + * This program is free software; you can redistribute it and/or modify
 121 + * it under the terms of the GNU General Public License version 2 as
 122 + * published by the Free Software Foundation.
 123 + */
 124 +
 125 +/**
 126 + * DOC: VC4 firmware KMS module.
 127 + *
 128 + * As a hack to get us from the current closed source driver world
 129 + * toward a totally open stack, implement KMS on top of the Raspberry
 130 + * Pi's firmware display stack.
 131 + */
 132 +
 133 +#include "drm_atomic.h"
 134 +#include "drm_atomic_helper.h"
 135 +#include "drm_plane_helper.h"
 136 +#include "drm_crtc_helper.h"
 137 +#include "linux/clk.h"
 138 +#include "linux/debugfs.h"
 139 +#include "drm_fb_cma_helper.h"
 140 +#include "linux/component.h"
 141 +#include "linux/of_device.h"
 142 +#include "vc4_drv.h"
 143 +#include "vc4_regs.h"
 144 +#include <soc/bcm2835/raspberrypi-firmware.h>
 145 +
 146 +/* The firmware delivers a vblank interrupt to us through the SMI
 147 + * hardware, which has only this one register.
 148 + */
 149 +#define SMICS 0x0
 150 +#define SMICS_INTERRUPTS (BIT(9) | BIT(10) | BIT(11))
 151 +
 152 +struct vc4_crtc {
 153 +       struct drm_crtc base;
 154 +       struct drm_encoder *encoder;
 155 +       struct drm_connector *connector;
 156 +       void __iomem *regs;
 157 +
 158 +       struct drm_pending_vblank_event *event;
 159 +};
 160 +
 161 +static inline struct vc4_crtc *to_vc4_crtc(struct drm_crtc *crtc)
 162 +{
 163 +       return container_of(crtc, struct vc4_crtc, base);
 164 +}
 165 +
 166 +struct vc4_fkms_encoder {
 167 +       struct drm_encoder base;
 168 +};
 169 +
 170 +static inline struct vc4_fkms_encoder *
 171 +to_vc4_fkms_encoder(struct drm_encoder *encoder)
 172 +{
 173 +       return container_of(encoder, struct vc4_fkms_encoder, base);
 174 +}
 175 +
 176 +/* VC4 FKMS connector KMS struct */
 177 +struct vc4_fkms_connector {
 178 +       struct drm_connector base;
 179 +
 180 +       /* Since the connector is attached to just the one encoder,
 181 +        * this is the reference to it so we can do the best_encoder()
 182 +        * hook.
 183 +        */
 184 +       struct drm_encoder *encoder;
 185 +};
 186 +
 187 +static inline struct vc4_fkms_connector *
 188 +to_vc4_fkms_connector(struct drm_connector *connector)
 189 +{
 190 +       return container_of(connector, struct vc4_fkms_connector, base);
 191 +}
 192 +
 193 +/* Firmware's structure for making an FB mbox call. */
 194 +struct fbinfo_s {
 195 +       u32 xres, yres, xres_virtual, yres_virtual;
 196 +       u32 pitch, bpp;
 197 +       u32 xoffset, yoffset;
 198 +       u32 base;
 199 +       u32 screen_size;
 200 +       u16 cmap[256];
 201 +};
 202 +
 203 +struct vc4_fkms_plane {
 204 +       struct drm_plane base;
 205 +       struct fbinfo_s *fbinfo;
 206 +       dma_addr_t fbinfo_bus_addr;
 207 +       u32 pitch;
 208 +};
 209 +
 210 +static inline struct vc4_fkms_plane *to_vc4_fkms_plane(struct drm_plane *plane)
 211 +{
 212 +       return (struct vc4_fkms_plane *)plane;
 213 +}
 214 +
 215 +/* Turns the display on/off. */
 216 +static int vc4_plane_set_primary_blank(struct drm_plane *plane, bool blank)
 217 +{
 218 +       struct vc4_dev *vc4 = to_vc4_dev(plane->dev);
 219 +
 220 +       u32 packet = blank;
 221 +       return rpi_firmware_property(vc4->firmware,
 222 +                                    RPI_FIRMWARE_FRAMEBUFFER_BLANK,
 223 +                                    &packet, sizeof(packet));
 224 +}
 225 +
 226 +static void vc4_primary_plane_atomic_update(struct drm_plane *plane,
 227 +                                           struct drm_plane_state *old_state)
 228 +{
 229 +       struct vc4_dev *vc4 = to_vc4_dev(plane->dev);
 230 +       struct vc4_fkms_plane *vc4_plane = to_vc4_fkms_plane(plane);
 231 +       struct drm_plane_state *state = plane->state;
 232 +       struct drm_framebuffer *fb = state->fb;
 233 +       struct drm_gem_cma_object *bo = drm_fb_cma_get_gem_obj(fb, 0);
 234 +       volatile struct fbinfo_s *fbinfo = vc4_plane->fbinfo;
 235 +       u32 bpp = 32;
 236 +       int ret;
 237 +
 238 +       vc4_plane_set_primary_blank(plane, false);
 239 +
 240 +       fbinfo->xres = state->crtc_w;
 241 +       fbinfo->yres = state->crtc_h;
 242 +       fbinfo->xres_virtual = state->crtc_w;
 243 +       fbinfo->yres_virtual = state->crtc_h;
 244 +       fbinfo->bpp = bpp;
 245 +       fbinfo->xoffset = state->crtc_x;
 246 +       fbinfo->yoffset = state->crtc_y;
 247 +       fbinfo->base = bo->paddr + fb->offsets[0];
 248 +       fbinfo->pitch = fb->pitches[0];
 249 +       /* A bug in the firmware makes it so that if the fb->base is
 250 +        * set to nonzero, the configured pitch gets overwritten with
 251 +        * the previous pitch.  So, to get the configured pitch
 252 +        * recomputed, we have to make it allocate itself a new buffer
 253 +        * in VC memory, first.
 254 +        */
 255 +       if (vc4_plane->pitch != fb->pitches[0]) {
 256 +               u32 saved_base = fbinfo->base;
 257 +               fbinfo->base = 0;
 258 +
 259 +               ret = rpi_firmware_transaction(vc4->firmware,
 260 +                                              RPI_FIRMWARE_CHAN_FB,
 261 +                                              vc4_plane->fbinfo_bus_addr);
 262 +               fbinfo->base = saved_base;
 263 +
 264 +               vc4_plane->pitch = fbinfo->pitch;
 265 +               WARN_ON_ONCE(vc4_plane->pitch != fb->pitches[0]);
 266 +       }
 267 +
 268 +       ret = rpi_firmware_transaction(vc4->firmware,
 269 +                                      RPI_FIRMWARE_CHAN_FB,
 270 +                                      vc4_plane->fbinfo_bus_addr);
 271 +       WARN_ON_ONCE(fbinfo->pitch != fb->pitches[0]);
 272 +       WARN_ON_ONCE(fbinfo->base != bo->paddr + fb->offsets[0]);
 273 +}
 274 +
 275 +static void vc4_primary_plane_atomic_disable(struct drm_plane *plane,
 276 +                                            struct drm_plane_state *old_state)
 277 +{
 278 +       vc4_plane_set_primary_blank(plane, true);
 279 +}
 280 +
 281 +static void vc4_cursor_plane_atomic_update(struct drm_plane *plane,
 282 +                                          struct drm_plane_state *old_state)
 283 +{
 284 +       struct vc4_dev *vc4 = to_vc4_dev(plane->dev);
 285 +       struct drm_plane_state *state = plane->state;
 286 +       struct drm_framebuffer *fb = state->fb;
 287 +       struct drm_gem_cma_object *bo = drm_fb_cma_get_gem_obj(fb, 0);
 288 +       int ret;
 289 +       u32 packet_state[] = { true, state->crtc_x, state->crtc_y, 0 };
 290 +       u32 packet_info[] = { state->crtc_w, state->crtc_h,
 291 +                             0, /* unused */
 292 +                             bo->paddr + fb->offsets[0],
 293 +                             0, 0, /* hotx, hoty */};
 294 +       WARN_ON_ONCE(fb->pitches[0] != state->crtc_w * 4);
 295 +       WARN_ON_ONCE(fb->bits_per_pixel != 32);
 296 +
 297 +       ret = rpi_firmware_property(vc4->firmware,
 298 +                                   RPI_FIRMWARE_SET_CURSOR_STATE,
 299 +                                   &packet_state,
 300 +                                   sizeof(packet_state));
 301 +       if (ret || packet_state[0] != 0)
 302 +               DRM_ERROR("Failed to set cursor state: 0x%08x\n", packet_state[0]);
 303 +
 304 +       ret = rpi_firmware_property(vc4->firmware,
 305 +                                   RPI_FIRMWARE_SET_CURSOR_INFO,
 306 +                                   &packet_info,
 307 +                                   sizeof(packet_info));
 308 +       if (ret || packet_info[0] != 0)
 309 +               DRM_ERROR("Failed to set cursor info: 0x%08x\n", packet_info[0]);
 310 +}
 311 +
 312 +static void vc4_cursor_plane_atomic_disable(struct drm_plane *plane,
 313 +                                           struct drm_plane_state *old_state)
 314 +{
 315 +       struct vc4_dev *vc4 = to_vc4_dev(plane->dev);
 316 +       u32 packet_state[] = { false, 0, 0, 0 };
 317 +       int ret;
 318 +
 319 +       ret = rpi_firmware_property(vc4->firmware,
 320 +                                   RPI_FIRMWARE_SET_CURSOR_STATE,
 321 +                                   &packet_state,
 322 +                                   sizeof(packet_state));
 323 +       if (ret || packet_state[0] != 0)
 324 +               DRM_ERROR("Failed to set cursor state: 0x%08x\n", packet_state[0]);
 325 +}
 326 +
 327 +static int vc4_plane_atomic_check(struct drm_plane *plane,
 328 +                                 struct drm_plane_state *state)
 329 +{
 330 +       return 0;
 331 +}
 332 +
 333 +static void vc4_plane_destroy(struct drm_plane *plane)
 334 +{
 335 +       drm_plane_helper_disable(plane);
 336 +       drm_plane_cleanup(plane);
 337 +}
 338 +
 339 +static const struct drm_plane_funcs vc4_plane_funcs = {
 340 +       .update_plane = drm_atomic_helper_update_plane,
 341 +       .disable_plane = drm_atomic_helper_disable_plane,
 342 +       .destroy = vc4_plane_destroy,
 343 +       .set_property = NULL,
 344 +       .reset = drm_atomic_helper_plane_reset,
 345 +       .atomic_duplicate_state = drm_atomic_helper_plane_duplicate_state,
 346 +       .atomic_destroy_state = drm_atomic_helper_plane_destroy_state,
 347 +};
 348 +
 349 +static const struct drm_plane_helper_funcs vc4_primary_plane_helper_funcs = {
 350 +       .prepare_fb = NULL,
 351 +       .cleanup_fb = NULL,
 352 +       .atomic_check = vc4_plane_atomic_check,
 353 +       .atomic_update = vc4_primary_plane_atomic_update,
 354 +       .atomic_disable = vc4_primary_plane_atomic_disable,
 355 +};
 356 +
 357 +static const struct drm_plane_helper_funcs vc4_cursor_plane_helper_funcs = {
 358 +       .prepare_fb = NULL,
 359 +       .cleanup_fb = NULL,
 360 +       .atomic_check = vc4_plane_atomic_check,
 361 +       .atomic_update = vc4_cursor_plane_atomic_update,
 362 +       .atomic_disable = vc4_cursor_plane_atomic_disable,
 363 +};
 364 +
 365 +static struct drm_plane *vc4_fkms_plane_init(struct drm_device *dev,
 366 +                                            enum drm_plane_type type)
 367 +{
 368 +       struct drm_plane *plane = NULL;
 369 +       struct vc4_fkms_plane *vc4_plane;
 370 +       u32 xrgb8888 = DRM_FORMAT_XRGB8888;
 371 +       u32 argb8888 = DRM_FORMAT_ARGB8888;
 372 +       int ret = 0;
 373 +       bool primary = (type == DRM_PLANE_TYPE_PRIMARY);
 374 +
 375 +       vc4_plane = devm_kzalloc(dev->dev, sizeof(*vc4_plane),
 376 +                                GFP_KERNEL);
 377 +       if (!vc4_plane) {
 378 +               ret = -ENOMEM;
 379 +               goto fail;
 380 +       }
 381 +
 382 +       plane = &vc4_plane->base;
 383 +       ret = drm_universal_plane_init(dev, plane, 0xff,
 384 +                                      &vc4_plane_funcs,
 385 +                                      primary ? &xrgb8888 : &argb8888, 1,
 386 +                                      type);
 387 +
 388 +       if (type == DRM_PLANE_TYPE_PRIMARY) {
 389 +               vc4_plane->fbinfo =
 390 +                       dma_alloc_coherent(dev->dev,
 391 +                                          sizeof(*vc4_plane->fbinfo),
 392 +                                          &vc4_plane->fbinfo_bus_addr,
 393 +                                          GFP_KERNEL);
 394 +               memset(vc4_plane->fbinfo, 0, sizeof(*vc4_plane->fbinfo));
 395 +
 396 +               drm_plane_helper_add(plane, &vc4_primary_plane_helper_funcs);
 397 +       } else {
 398 +               drm_plane_helper_add(plane, &vc4_cursor_plane_helper_funcs);
 399 +       }
 400 +
 401 +       return plane;
 402 +fail:
 403 +       if (plane)
 404 +               vc4_plane_destroy(plane);
 405 +
 406 +       return ERR_PTR(ret);
 407 +}
 408 +
 409 +static void vc4_crtc_mode_set_nofb(struct drm_crtc *crtc)
 410 +{
 411 +       /* Everyting is handled in the planes. */
 412 +}
 413 +
 414 +static void vc4_crtc_disable(struct drm_crtc *crtc)
 415 +{
 416 +}
 417 +
 418 +static void vc4_crtc_enable(struct drm_crtc *crtc)
 419 +{
 420 +}
 421 +
 422 +static int vc4_crtc_atomic_check(struct drm_crtc *crtc,
 423 +                                struct drm_crtc_state *state)
 424 +{
 425 +       return 0;
 426 +}
 427 +
 428 +static void vc4_crtc_atomic_flush(struct drm_crtc *crtc,
 429 +                                 struct drm_crtc_state *old_state)
 430 +{
 431 +}
 432 +
 433 +static void vc4_crtc_handle_page_flip(struct vc4_crtc *vc4_crtc)
 434 +{
 435 +       struct drm_crtc *crtc = &vc4_crtc->base;
 436 +       struct drm_device *dev = crtc->dev;
 437 +       unsigned long flags;
 438 +
 439 +       spin_lock_irqsave(&dev->event_lock, flags);
 440 +       if (vc4_crtc->event) {
 441 +               drm_crtc_send_vblank_event(crtc, vc4_crtc->event);
 442 +               vc4_crtc->event = NULL;
 443 +               drm_crtc_vblank_put(crtc);
 444 +       }
 445 +       spin_unlock_irqrestore(&dev->event_lock, flags);
 446 +}
 447 +
 448 +static irqreturn_t vc4_crtc_irq_handler(int irq, void *data)
 449 +{
 450 +       struct vc4_crtc *vc4_crtc = data;
 451 +       u32 stat = readl(vc4_crtc->regs + SMICS);
 452 +       irqreturn_t ret = IRQ_NONE;
 453 +
 454 +       if (stat & SMICS_INTERRUPTS) {
 455 +               writel(0, vc4_crtc->regs + SMICS);
 456 +               drm_crtc_handle_vblank(&vc4_crtc->base);
 457 +               vc4_crtc_handle_page_flip(vc4_crtc);
 458 +               ret = IRQ_HANDLED;
 459 +       }
 460 +
 461 +       return ret;
 462 +}
 463 +
 464 +static int vc4_page_flip(struct drm_crtc *crtc,
 465 +                        struct drm_framebuffer *fb,
 466 +                        struct drm_pending_vblank_event *event,
 467 +                        uint32_t flags)
 468 +{
 469 +       if (flags & DRM_MODE_PAGE_FLIP_ASYNC) {
 470 +               DRM_ERROR("Async flips aren't allowed\n");
 471 +               return -EINVAL;
 472 +       }
 473 +
 474 +       return drm_atomic_helper_page_flip(crtc, fb, event, flags);
 475 +}
 476 +
 477 +static const struct drm_crtc_funcs vc4_crtc_funcs = {
 478 +       .set_config = drm_atomic_helper_set_config,
 479 +       .destroy = drm_crtc_cleanup,
 480 +       .page_flip = vc4_page_flip,
 481 +       .set_property = NULL,
 482 +       .cursor_set = NULL, /* handled by drm_mode_cursor_universal */
 483 +       .cursor_move = NULL, /* handled by drm_mode_cursor_universal */
 484 +       .reset = drm_atomic_helper_crtc_reset,
 485 +       .atomic_duplicate_state = drm_atomic_helper_crtc_duplicate_state,
 486 +       .atomic_destroy_state = drm_atomic_helper_crtc_destroy_state,
 487 +};
 488 +
 489 +static const struct drm_crtc_helper_funcs vc4_crtc_helper_funcs = {
 490 +       .mode_set_nofb = vc4_crtc_mode_set_nofb,
 491 +       .disable = vc4_crtc_disable,
 492 +       .enable = vc4_crtc_enable,
 493 +       .atomic_check = vc4_crtc_atomic_check,
 494 +       .atomic_flush = vc4_crtc_atomic_flush,
 495 +};
 496 +
 497 +/* Frees the page flip event when the DRM device is closed with the
 498 + * event still outstanding.
 499 + */
 500 +void vc4_fkms_cancel_page_flip(struct drm_crtc *crtc, struct drm_file *file)
 501 +{
 502 +       struct vc4_crtc *vc4_crtc = to_vc4_crtc(crtc);
 503 +       struct drm_device *dev = crtc->dev;
 504 +       unsigned long flags;
 505 +
 506 +       spin_lock_irqsave(&dev->event_lock, flags);
 507 +
 508 +       if (vc4_crtc->event && vc4_crtc->event->base.file_priv == file) {
 509 +               vc4_crtc->event->base.destroy(&vc4_crtc->event->base);
 510 +               drm_crtc_vblank_put(crtc);
 511 +               vc4_crtc->event = NULL;
 512 +       }
 513 +
 514 +       spin_unlock_irqrestore(&dev->event_lock, flags);
 515 +}
 516 +
 517 +static const struct of_device_id vc4_firmware_kms_dt_match[] = {
 518 +       { .compatible = "raspberrypi,rpi-firmware-kms" },
 519 +       {}
 520 +};
 521 +
 522 +static enum drm_connector_status
 523 +vc4_fkms_connector_detect(struct drm_connector *connector, bool force)
 524 +{
 525 +       return connector_status_connected;
 526 +}
 527 +
 528 +static int vc4_fkms_connector_get_modes(struct drm_connector *connector)
 529 +{
 530 +       struct drm_device *dev = connector->dev;
 531 +       struct vc4_dev *vc4 = to_vc4_dev(dev);
 532 +       u32 wh[2] = {0, 0};
 533 +       int ret;
 534 +       struct drm_display_mode *mode;
 535 +
 536 +       ret = rpi_firmware_property(vc4->firmware,
 537 +                                   RPI_FIRMWARE_FRAMEBUFFER_GET_PHYSICAL_WIDTH_HEIGHT,
 538 +                                   &wh, sizeof(wh));
 539 +       if (ret) {
 540 +               DRM_ERROR("Failed to get screen size: %d (0x%08x 0x%08x)\n",
 541 +                         ret, wh[0], wh[1]);
 542 +               return 0;
 543 +       }
 544 +
 545 +       mode = drm_cvt_mode(dev, wh[0], wh[1], 60 /* vrefresh */,
 546 +                           0, 0, false);
 547 +       drm_mode_probed_add(connector, mode);
 548 +
 549 +       return 1;
 550 +}
 551 +
 552 +static struct drm_encoder *
 553 +vc4_fkms_connector_best_encoder(struct drm_connector *connector)
 554 +{
 555 +       struct vc4_fkms_connector *fkms_connector =
 556 +               to_vc4_fkms_connector(connector);
 557 +       return fkms_connector->encoder;
 558 +}
 559 +
 560 +static void vc4_fkms_connector_destroy(struct drm_connector *connector)
 561 +{
 562 +       drm_connector_unregister(connector);
 563 +       drm_connector_cleanup(connector);
 564 +}
 565 +
 566 +static const struct drm_connector_funcs vc4_fkms_connector_funcs = {
 567 +       .dpms = drm_atomic_helper_connector_dpms,
 568 +       .detect = vc4_fkms_connector_detect,
 569 +       .fill_modes = drm_helper_probe_single_connector_modes,
 570 +       .destroy = vc4_fkms_connector_destroy,
 571 +       .reset = drm_atomic_helper_connector_reset,
 572 +       .atomic_duplicate_state = drm_atomic_helper_connector_duplicate_state,
 573 +       .atomic_destroy_state = drm_atomic_helper_connector_destroy_state,
 574 +};
 575 +
 576 +static const struct drm_connector_helper_funcs vc4_fkms_connector_helper_funcs = {
 577 +       .get_modes = vc4_fkms_connector_get_modes,
 578 +       .best_encoder = vc4_fkms_connector_best_encoder,
 579 +};
 580 +
 581 +static struct drm_connector *vc4_fkms_connector_init(struct drm_device *dev,
 582 +                                                    struct drm_encoder *encoder)
 583 +{
 584 +       struct drm_connector *connector = NULL;
 585 +       struct vc4_fkms_connector *fkms_connector;
 586 +       int ret = 0;
 587 +
 588 +       fkms_connector = devm_kzalloc(dev->dev, sizeof(*fkms_connector),
 589 +                                     GFP_KERNEL);
 590 +       if (!fkms_connector) {
 591 +               ret = -ENOMEM;
 592 +               goto fail;
 593 +       }
 594 +       connector = &fkms_connector->base;
 595 +
 596 +       fkms_connector->encoder = encoder;
 597 +
 598 +       drm_connector_init(dev, connector, &vc4_fkms_connector_funcs,
 599 +                          DRM_MODE_CONNECTOR_HDMIA);
 600 +       drm_connector_helper_add(connector, &vc4_fkms_connector_helper_funcs);
 601 +
 602 +       connector->polled = (DRM_CONNECTOR_POLL_CONNECT |
 603 +                            DRM_CONNECTOR_POLL_DISCONNECT);
 604 +
 605 +       connector->interlace_allowed = 0;
 606 +       connector->doublescan_allowed = 0;
 607 +
 608 +       drm_mode_connector_attach_encoder(connector, encoder);
 609 +
 610 +       return connector;
 611 +
 612 + fail:
 613 +       if (connector)
 614 +               vc4_fkms_connector_destroy(connector);
 615 +
 616 +       return ERR_PTR(ret);
 617 +}
 618 +
 619 +static void vc4_fkms_encoder_destroy(struct drm_encoder *encoder)
 620 +{
 621 +       drm_encoder_cleanup(encoder);
 622 +}
 623 +
 624 +static const struct drm_encoder_funcs vc4_fkms_encoder_funcs = {
 625 +       .destroy = vc4_fkms_encoder_destroy,
 626 +};
 627 +
 628 +static void vc4_fkms_encoder_enable(struct drm_encoder *encoder)
 629 +{
 630 +}
 631 +
 632 +static void vc4_fkms_encoder_disable(struct drm_encoder *encoder)
 633 +{
 634 +}
 635 +
 636 +static const struct drm_encoder_helper_funcs vc4_fkms_encoder_helper_funcs = {
 637 +       .enable = vc4_fkms_encoder_enable,
 638 +       .disable = vc4_fkms_encoder_disable,
 639 +};
 640 +
 641 +static int vc4_fkms_bind(struct device *dev, struct device *master, void *data)
 642 +{
 643 +       struct platform_device *pdev = to_platform_device(dev);
 644 +       struct drm_device *drm = dev_get_drvdata(master);
 645 +       struct vc4_dev *vc4 = to_vc4_dev(drm);
 646 +       struct vc4_crtc *vc4_crtc;
 647 +       struct vc4_fkms_encoder *vc4_encoder;
 648 +       struct drm_crtc *crtc;
 649 +       struct drm_plane *primary_plane, *cursor_plane, *destroy_plane, *temp;
 650 +       struct device_node *firmware_node;
 651 +       int ret;
 652 +
 653 +       vc4->firmware_kms = true;
 654 +
 655 +       vc4_crtc = devm_kzalloc(dev, sizeof(*vc4_crtc), GFP_KERNEL);
 656 +       if (!vc4_crtc)
 657 +               return -ENOMEM;
 658 +       crtc = &vc4_crtc->base;
 659 +
 660 +       firmware_node = of_parse_phandle(dev->of_node, "brcm,firmware", 0);
 661 +       vc4->firmware = rpi_firmware_get(firmware_node);
 662 +       if (!vc4->firmware) {
 663 +               DRM_DEBUG("Failed to get Raspberry Pi firmware reference.\n");
 664 +               return -EPROBE_DEFER;
 665 +       }
 666 +       of_node_put(firmware_node);
 667 +
 668 +       /* Map the SMI interrupt reg */
 669 +       vc4_crtc->regs = vc4_ioremap_regs(pdev, 0);
 670 +       if (IS_ERR(vc4_crtc->regs))
 671 +               return PTR_ERR(vc4_crtc->regs);
 672 +
 673 +       /* For now, we create just the primary and the legacy cursor
 674 +        * planes.  We should be able to stack more planes on easily,
 675 +        * but to do that we would need to compute the bandwidth
 676 +        * requirement of the plane configuration, and reject ones
 677 +        * that will take too much.
 678 +        */
 679 +       primary_plane = vc4_fkms_plane_init(drm, DRM_PLANE_TYPE_PRIMARY);
 680 +       if (IS_ERR(primary_plane)) {
 681 +               dev_err(dev, "failed to construct primary plane\n");
 682 +               ret = PTR_ERR(primary_plane);
 683 +               goto err;
 684 +       }
 685 +
 686 +       cursor_plane = vc4_fkms_plane_init(drm, DRM_PLANE_TYPE_CURSOR);
 687 +       if (IS_ERR(cursor_plane)) {
 688 +               dev_err(dev, "failed to construct cursor plane\n");
 689 +               ret = PTR_ERR(cursor_plane);
 690 +               goto err;
 691 +       }
 692 +
 693 +       drm_crtc_init_with_planes(drm, crtc, primary_plane, cursor_plane,
 694 +                                 &vc4_crtc_funcs);
 695 +       drm_crtc_helper_add(crtc, &vc4_crtc_helper_funcs);
 696 +       primary_plane->crtc = crtc;
 697 +       cursor_plane->crtc = crtc;
 698 +       vc4->crtc[drm_crtc_index(crtc)] = vc4_crtc;
 699 +
 700 +       vc4_encoder = devm_kzalloc(dev, sizeof(*vc4_encoder), GFP_KERNEL);
 701 +       if (!vc4_encoder)
 702 +               return -ENOMEM;
 703 +       vc4_crtc->encoder = &vc4_encoder->base;
 704 +       vc4_encoder->base.possible_crtcs |= drm_crtc_mask(crtc) ;
 705 +       drm_encoder_init(drm, &vc4_encoder->base, &vc4_fkms_encoder_funcs,
 706 +                        DRM_MODE_ENCODER_TMDS);
 707 +       drm_encoder_helper_add(&vc4_encoder->base,
 708 +                              &vc4_fkms_encoder_helper_funcs);
 709 +
 710 +       vc4_crtc->connector = vc4_fkms_connector_init(drm, &vc4_encoder->base);
 711 +       if (IS_ERR(vc4_crtc->connector)) {
 712 +               ret = PTR_ERR(vc4_crtc->connector);
 713 +               goto err_destroy_encoder;
 714 +       }
 715 +
 716 +       writel(0, vc4_crtc->regs + SMICS);
 717 +       ret = devm_request_irq(dev, platform_get_irq(pdev, 0),
 718 +                              vc4_crtc_irq_handler, 0, "vc4 firmware kms",
 719 +                              vc4_crtc);
 720 +       if (ret)
 721 +               goto err_destroy_connector;
 722 +
 723 +       platform_set_drvdata(pdev, vc4_crtc);
 724 +
 725 +       return 0;
 726 +
 727 +err_destroy_connector:
 728 +       vc4_fkms_connector_destroy(vc4_crtc->connector);
 729 +err_destroy_encoder:
 730 +       vc4_fkms_encoder_destroy(vc4_crtc->encoder);
 731 +       list_for_each_entry_safe(destroy_plane, temp,
 732 +                                &drm->mode_config.plane_list, head) {
 733 +               if (destroy_plane->possible_crtcs == 1 << drm_crtc_index(crtc))
 734 +                   destroy_plane->funcs->destroy(destroy_plane);
 735 +       }
 736 +err:
 737 +       return ret;
 738 +}
 739 +
 740 +static void vc4_fkms_unbind(struct device *dev, struct device *master,
 741 +                           void *data)
 742 +{
 743 +       struct platform_device *pdev = to_platform_device(dev);
 744 +       struct vc4_crtc *vc4_crtc = dev_get_drvdata(dev);
 745 +
 746 +       vc4_fkms_connector_destroy(vc4_crtc->connector);
 747 +       vc4_fkms_encoder_destroy(vc4_crtc->encoder);
 748 +       drm_crtc_cleanup(&vc4_crtc->base);
 749 +
 750 +       platform_set_drvdata(pdev, NULL);
 751 +}
 752 +
 753 +static const struct component_ops vc4_fkms_ops = {
 754 +       .bind   = vc4_fkms_bind,
 755 +       .unbind = vc4_fkms_unbind,
 756 +};
 757 +
 758 +static int vc4_fkms_probe(struct platform_device *pdev)
 759 +{
 760 +       return component_add(&pdev->dev, &vc4_fkms_ops);
 761 +}
 762 +
 763 +static int vc4_fkms_remove(struct platform_device *pdev)
 764 +{
 765 +       component_del(&pdev->dev, &vc4_fkms_ops);
 766 +       return 0;
 767 +}
 768 +
 769 +struct platform_driver vc4_firmware_kms_driver = {
 770 +       .probe = vc4_fkms_probe,
 771 +       .remove = vc4_fkms_remove,
 772 +       .driver = {
 773 +               .name = "vc4_firmware_kms",
 774 +               .of_match_table = vc4_firmware_kms_dt_match,
 775 +       },
 776 +};