@@ -25,6 +25,7 @@
#include <video/edid.h>
#include <video/pixel_format.h>
+#include <video/vga.h>
#include "drm_sysfb_helper.h"
@@ -33,6 +34,8 @@
#define DRIVER_MAJOR 1
#define DRIVER_MINOR 0
+#define VESADRM_GAMMA_LUT_SIZE 256
+
static int vesadrm_get_validated_int(struct drm_device *dev, const char *name,
u64 value, u32 max)
{
@@ -162,6 +165,16 @@ static const struct drm_format_info *vesadrm_get_format_si(struct drm_device *de
struct vesadrm_device {
struct drm_sysfb_device sysfb;
+#if defined(CONFIG_X86_32)
+ /* VESA Protected Mode interface */
+ struct {
+ const u8 *PrimaryPalette;
+ } pmi;
+#endif
+
+ void (*cmap_write)(struct vesadrm_device *vesa, unsigned int index,
+ u16 red, u16 green, u16 blue);
+
/* modesetting */
u32 formats[DRM_SYSFB_PLANE_NFORMATS(1)];
struct drm_plane primary_plane;
@@ -170,6 +183,151 @@ struct vesadrm_device {
struct drm_connector connector;
};
+static struct vesadrm_device *to_vesadrm_device(struct drm_device *dev)
+{
+ return container_of(to_drm_sysfb_device(dev), struct vesadrm_device, sysfb);
+}
+
+/*
+ * Palette
+ */
+
+static void vesadrm_vga_cmap_write(struct vesadrm_device *vesa, unsigned int index,
+ u16 red, u16 green, u16 blue)
+{
+ u8 i8, r8, g8, b8;
+
+ if (index > 255)
+ return;
+
+ i8 = index;
+ r8 = red >> 8;
+ g8 = green >> 8;
+ b8 = blue >> 8;
+
+ outb_p(i8, VGA_PEL_IW);
+ outb_p(r8, VGA_PEL_D);
+ outb_p(g8, VGA_PEL_D);
+ outb_p(b8, VGA_PEL_D);
+}
+
+#if defined(CONFIG_X86_32)
+static void vesadrm_pmi_cmap_write(struct vesadrm_device *vesa, unsigned int index,
+ u16 red, u16 green, u16 blue)
+{
+ u32 i32 = index;
+ struct {
+ u8 b8;
+ u8 g8;
+ u8 r8;
+ u8 x8;
+ } PaletteEntry = {
+ blue >> 8,
+ green >> 8,
+ red >> 8,
+ 0x00,
+ };
+
+ if (index > 255)
+ return;
+
+ __asm__ __volatile__(
+ "call *(%%esi)"
+ : /* no return value */
+ : "a" (0x4f09),
+ "b" (0),
+ "c" (1),
+ "d" (i32),
+ "D" (&PaletteEntry),
+ "S" (&vesa->pmi.PrimaryPalette));
+}
+#endif
+
+static void vesadrm_set_gamma_linear(struct vesadrm_device *vesa,
+ const struct drm_format_info *format)
+{
+ struct drm_device *dev = &vesa->sysfb.dev;
+ size_t i;
+ u16 r16, g16, b16;
+
+ switch (format->format) {
+ case DRM_FORMAT_XRGB1555:
+ for (i = 0; i < 32; ++i) {
+ r16 = i * 8 + i / 4;
+ r16 |= (r16 << 8) | r16;
+ vesa->cmap_write(vesa, i, r16, r16, r16);
+ }
+ break;
+ case DRM_FORMAT_RGB565:
+ for (i = 0; i < 32; ++i) {
+ r16 = i * 8 + i / 4;
+ r16 |= (r16 << 8) | r16;
+ g16 = i * 4 + i / 16;
+ g16 |= (g16 << 8) | g16;
+ b16 = r16;
+ vesa->cmap_write(vesa, i, r16, g16, b16);
+ }
+ for (i = 32; i < 64; ++i) {
+ g16 = i * 4 + i / 16;
+ g16 |= (g16 << 8) | g16;
+ vesa->cmap_write(vesa, i, 0, g16, 0);
+ }
+ break;
+ case DRM_FORMAT_RGB888:
+ case DRM_FORMAT_XRGB8888:
+ case DRM_FORMAT_BGRX8888:
+ for (i = 0; i < 256; ++i) {
+ r16 = (i << 8) | i;
+ vesa->cmap_write(vesa, i, r16, r16, r16);
+ }
+ break;
+ default:
+ drm_warn_once(dev, "Unsupported format %p4cc for gamma correction\n",
+ &format->format);
+ break;
+ }
+}
+
+static void vesadrm_set_gamma_lut(struct vesadrm_device *vesa,
+ const struct drm_format_info *format,
+ struct drm_color_lut *lut)
+{
+ struct drm_device *dev = &vesa->sysfb.dev;
+ size_t i;
+ u16 r16, g16, b16;
+
+ switch (format->format) {
+ case DRM_FORMAT_XRGB1555:
+ for (i = 0; i < 32; ++i) {
+ r16 = lut[i * 8 + i / 4].red;
+ g16 = lut[i * 8 + i / 4].green;
+ b16 = lut[i * 8 + i / 4].blue;
+ vesa->cmap_write(vesa, i, r16, g16, b16);
+ }
+ break;
+ case DRM_FORMAT_RGB565:
+ for (i = 0; i < 32; ++i) {
+ r16 = lut[i * 8 + i / 4].red;
+ g16 = lut[i * 4 + i / 16].green;
+ b16 = lut[i * 8 + i / 4].blue;
+ vesa->cmap_write(vesa, i, r16, g16, b16);
+ }
+ for (i = 32; i < 64; ++i)
+ vesa->cmap_write(vesa, i, 0, lut[i * 4 + i / 16].green, 0);
+ break;
+ case DRM_FORMAT_RGB888:
+ case DRM_FORMAT_XRGB8888:
+ case DRM_FORMAT_BGRX8888:
+ for (i = 0; i < 256; ++i)
+ vesa->cmap_write(vesa, i, lut[i].red, lut[i].green, lut[i].blue);
+ break;
+ default:
+ drm_warn_once(dev, "Unsupported format %p4cc for gamma correction\n",
+ &format->format);
+ break;
+ }
+}
+
/*
* Modesetting
*/
@@ -187,8 +345,36 @@ static const struct drm_plane_funcs vesadrm_primary_plane_funcs = {
.destroy = drm_plane_cleanup,
};
+static void vesadrm_crtc_helper_atomic_flush(struct drm_crtc *crtc,
+ struct drm_atomic_state *state)
+{
+ struct drm_device *dev = crtc->dev;
+ struct drm_sysfb_device *sysfb = to_drm_sysfb_device(dev);
+ struct vesadrm_device *vesa = to_vesadrm_device(dev);
+ struct drm_crtc_state *crtc_state = drm_atomic_get_new_crtc_state(state, crtc);
+ struct drm_sysfb_crtc_state *sysfb_crtc_state = to_drm_sysfb_crtc_state(crtc_state);
+
+ /*
+ * The gamma LUT has to be reloaded after changing the primary
+ * plane's color format.
+ */
+ if (crtc_state->enable && crtc_state->color_mgmt_changed) {
+ if (sysfb_crtc_state->format == sysfb->fb_format) {
+ if (crtc_state->gamma_lut)
+ vesadrm_set_gamma_lut(vesa,
+ sysfb_crtc_state->format,
+ crtc_state->gamma_lut->data);
+ else
+ vesadrm_set_gamma_linear(vesa, sysfb_crtc_state->format);
+ } else {
+ vesadrm_set_gamma_linear(vesa, sysfb_crtc_state->format);
+ }
+ }
+}
+
static const struct drm_crtc_helper_funcs vesadrm_crtc_helper_funcs = {
DRM_SYSFB_CRTC_HELPER_FUNCS,
+ .atomic_flush = vesadrm_crtc_helper_atomic_flush,
};
static const struct drm_crtc_funcs vesadrm_crtc_funcs = {
@@ -282,6 +468,18 @@ static struct vesadrm_device *vesadrm_device_create(struct drm_driver *drv,
drm_dbg(dev, "framebuffer format=%p4cc, size=%dx%d, stride=%d bytes\n",
&format->format, width, height, stride);
+ if (!__screen_info_vbe_mode_nonvga(si)) {
+ vesa->cmap_write = vesadrm_vga_cmap_write;
+#if defined(CONFIG_X86_32)
+ } else {
+ phys_addr_t pmi_base = __screen_info_vesapm_info_base(si);
+ const u16 *pmi_addr = phys_to_virt(pmi_base);
+
+ vesa->pmi.PrimaryPalette = (u8 *)pmi_addr + pmi_addr[2];
+ vesa->cmap_write = vesadrm_pmi_cmap_write;
+#endif
+ }
+
#ifdef CONFIG_X86
if (drm_edid_header_is_valid(edid_info.dummy) == 8)
sysfb->edid = edid_info.dummy;
@@ -289,6 +487,8 @@ static struct vesadrm_device *vesadrm_device_create(struct drm_driver *drv,
sysfb->fb_mode = drm_sysfb_mode(width, height, 0, 0);
sysfb->fb_format = format;
sysfb->fb_pitch = stride;
+ if (vesa->cmap_write)
+ sysfb->fb_gamma_lut_size = VESADRM_GAMMA_LUT_SIZE;
/*
* Memory management
@@ -360,6 +560,12 @@ static struct vesadrm_device *vesadrm_device_create(struct drm_driver *drv,
return ERR_PTR(ret);
drm_crtc_helper_add(crtc, &vesadrm_crtc_helper_funcs);
+ if (sysfb->fb_gamma_lut_size) {
+ ret = drm_mode_crtc_set_gamma_size(crtc, sysfb->fb_gamma_lut_size);
+ if (!ret)
+ drm_crtc_enable_color_mgmt(crtc, 0, false, sysfb->fb_gamma_lut_size);
+ }
+
/* Encoder */
encoder = &vesa->encoder;
@@ -126,6 +126,13 @@ static inline unsigned int screen_info_video_type(const struct screen_info *si)
return VIDEO_TYPE_CGA;
}
+static inline u32 __screen_info_vesapm_info_base(const struct screen_info *si)
+{
+ if (si->vesapm_seg < 0xc000)
+ return 0;
+ return (si->vesapm_seg << 4) + si->vesapm_off;
+}
+
ssize_t screen_info_resources(const struct screen_info *si, struct resource *r, size_t num);
u32 __screen_info_lfb_bits_per_pixel(const struct screen_info *si);