target/linux/brcm2708/patches-4.19/950-0064-mfd-Add-Raspberry-Pi-Sense-HAT-core-driver.patch

   1 From 0d63b8a00c925eb02093e9ed4866c4a0d9209a00 Mon Sep 17 00:00:00 2001
   2 From: Phil Elwell <pelwell@users.noreply.github.com>
   3 Date: Tue, 14 Jul 2015 14:32:47 +0100
   4 Subject: [PATCH 064/806] mfd: Add Raspberry Pi Sense HAT core driver
   5
   6 ---
   7  drivers/input/joystick/Kconfig           |   8 +
   8  drivers/input/joystick/Makefile          |   1 +
   9  drivers/input/joystick/rpisense-js.c     | 153 ++++++++++++
  10  drivers/mfd/Kconfig                      |   8 +
  11  drivers/mfd/Makefile                     |   2 +-
  12  drivers/mfd/rpisense-core.c              | 157 ++++++++++++
  13  drivers/video/fbdev/Kconfig              |  13 +
  14  drivers/video/fbdev/Makefile             |   1 +
  15  drivers/video/fbdev/rpisense-fb.c        | 293 +++++++++++++++++++++++
  16  include/linux/mfd/rpisense/core.h        |  47 ++++
  17  include/linux/mfd/rpisense/framebuffer.h |  32 +++
  18  include/linux/mfd/rpisense/joystick.h    |  35 +++
  19  12 files changed, 749 insertions(+), 1 deletion(-)
  20  create mode 100644 drivers/input/joystick/rpisense-js.c
  21  create mode 100644 drivers/mfd/rpisense-core.c
  22  create mode 100644 drivers/video/fbdev/rpisense-fb.c
  23  create mode 100644 include/linux/mfd/rpisense/core.h
  24  create mode 100644 include/linux/mfd/rpisense/framebuffer.h
  25  create mode 100644 include/linux/mfd/rpisense/joystick.h
  26
  27 --- a/drivers/input/joystick/Kconfig
  28 +++ b/drivers/input/joystick/Kconfig
  29 @@ -361,4 +361,12 @@ config JOYSTICK_PXRC
  30           To compile this driver as a module, choose M here: the
  31           module will be called pxrc.
  32
  33 +config JOYSTICK_RPISENSE
  34 +       tristate "Raspberry Pi Sense HAT joystick"
  35 +       depends on GPIOLIB && INPUT
  36 +       select MFD_RPISENSE_CORE
  37 +
  38 +       help
  39 +         This is the joystick driver for the Raspberry Pi Sense HAT
  40 +
  41  endif
  42 --- a/drivers/input/joystick/Makefile
  43 +++ b/drivers/input/joystick/Makefile
  44 @@ -35,4 +35,5 @@ obj-$(CONFIG_JOYSTICK_WARRIOR)                += warri
  45  obj-$(CONFIG_JOYSTICK_XPAD)            += xpad.o
  46  obj-$(CONFIG_JOYSTICK_ZHENHUA)         += zhenhua.o
  47  obj-$(CONFIG_JOYSTICK_WALKERA0701)     += walkera0701.o
  48 +obj-$(CONFIG_JOYSTICK_RPISENSE)                += rpisense-js.o
  49
  50 --- /dev/null
  51 +++ b/drivers/input/joystick/rpisense-js.c
  52 @@ -0,0 +1,153 @@
  53 +/*
  54 + * Raspberry Pi Sense HAT joystick driver
  55 + * http://raspberrypi.org
  56 + *
  57 + * Copyright (C) 2015 Raspberry Pi
  58 + *
  59 + * Author: Serge Schneider
  60 + *
  61 + *  This program is free software; you can redistribute  it and/or modify it
  62 + *  under  the terms of  the GNU General  Public License as published by the
  63 + *  Free Software Foundation;  either version 2 of the  License, or (at your
  64 + *  option) any later version.
  65 + *
  66 + */
  67 +
  68 +#include <linux/module.h>
  69 +
  70 +#include <linux/mfd/rpisense/joystick.h>
  71 +#include <linux/mfd/rpisense/core.h>
  72 +
  73 +static struct rpisense *rpisense;
  74 +static unsigned char keymap[5] = {KEY_DOWN, KEY_RIGHT, KEY_UP, KEY_ENTER, KEY_LEFT,};
  75 +
  76 +static void keys_work_fn(struct work_struct *work)
  77 +{
  78 +       int i;
  79 +       static s32 prev_keys;
  80 +       struct rpisense_js *rpisense_js = &rpisense->joystick;
  81 +       s32 keys = rpisense_reg_read(rpisense, RPISENSE_KEYS);
  82 +       s32 changes = keys ^ prev_keys;
  83 +
  84 +       prev_keys = keys;
  85 +       for (i = 0; i < 5; i++) {
  86 +               if (changes & 1) {
  87 +                       input_report_key(rpisense_js->keys_dev,
  88 +                                        keymap[i], keys & 1);
  89 +               }
  90 +               changes >>= 1;
  91 +               keys >>= 1;
  92 +       }
  93 +       input_sync(rpisense_js->keys_dev);
  94 +}
  95 +
  96 +static irqreturn_t keys_irq_handler(int irq, void *pdev)
  97 +{
  98 +       struct rpisense_js *rpisense_js = &rpisense->joystick;
  99 +
 100 +       schedule_work(&rpisense_js->keys_work_s);
 101 +       return IRQ_HANDLED;
 102 +}
 103 +
 104 +static int rpisense_js_probe(struct platform_device *pdev)
 105 +{
 106 +       int ret;
 107 +       int i;
 108 +       struct rpisense_js *rpisense_js;
 109 +
 110 +       rpisense = rpisense_get_dev();
 111 +       rpisense_js = &rpisense->joystick;
 112 +
 113 +       INIT_WORK(&rpisense_js->keys_work_s, keys_work_fn);
 114 +
 115 +       rpisense_js->keys_dev = input_allocate_device();
 116 +       if (!rpisense_js->keys_dev) {
 117 +               dev_err(&pdev->dev, "Could not allocate input device.\n");
 118 +               return -ENOMEM;
 119 +       }
 120 +
 121 +       rpisense_js->keys_dev->evbit[0] = BIT_MASK(EV_KEY);
 122 +       for (i = 0; i < ARRAY_SIZE(keymap); i++) {
 123 +               set_bit(keymap[i],
 124 +                       rpisense_js->keys_dev->keybit);
 125 +       }
 126 +
 127 +       rpisense_js->keys_dev->name = "Raspberry Pi Sense HAT Joystick";
 128 +       rpisense_js->keys_dev->phys = "rpi-sense-joy/input0";
 129 +       rpisense_js->keys_dev->id.bustype = BUS_I2C;
 130 +       rpisense_js->keys_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_REP);
 131 +       rpisense_js->keys_dev->keycode = keymap;
 132 +       rpisense_js->keys_dev->keycodesize = sizeof(unsigned char);
 133 +       rpisense_js->keys_dev->keycodemax = ARRAY_SIZE(keymap);
 134 +
 135 +       ret = input_register_device(rpisense_js->keys_dev);
 136 +       if (ret) {
 137 +               dev_err(&pdev->dev, "Could not register input device.\n");
 138 +               goto err_keys_alloc;
 139 +       }
 140 +
 141 +       ret = gpiod_direction_input(rpisense_js->keys_desc);
 142 +       if (ret) {
 143 +               dev_err(&pdev->dev, "Could not set keys-int direction.\n");
 144 +               goto err_keys_reg;
 145 +       }
 146 +
 147 +       rpisense_js->keys_irq = gpiod_to_irq(rpisense_js->keys_desc);
 148 +       if (rpisense_js->keys_irq < 0) {
 149 +               dev_err(&pdev->dev, "Could not determine keys-int IRQ.\n");
 150 +               ret = rpisense_js->keys_irq;
 151 +               goto err_keys_reg;
 152 +       }
 153 +
 154 +       ret = devm_request_irq(&pdev->dev, rpisense_js->keys_irq,
 155 +                              keys_irq_handler, IRQF_TRIGGER_RISING,
 156 +                              "keys", &pdev->dev);
 157 +       if (ret) {
 158 +               dev_err(&pdev->dev, "IRQ request failed.\n");
 159 +               goto err_keys_reg;
 160 +       }
 161 +       return 0;
 162 +err_keys_reg:
 163 +       input_unregister_device(rpisense_js->keys_dev);
 164 +err_keys_alloc:
 165 +       input_free_device(rpisense_js->keys_dev);
 166 +       return ret;
 167 +}
 168 +
 169 +static int rpisense_js_remove(struct platform_device *pdev)
 170 +{
 171 +       struct rpisense_js *rpisense_js = &rpisense->joystick;
 172 +
 173 +       input_unregister_device(rpisense_js->keys_dev);
 174 +       input_free_device(rpisense_js->keys_dev);
 175 +       return 0;
 176 +}
 177 +
 178 +#ifdef CONFIG_OF
 179 +static const struct of_device_id rpisense_js_id[] = {
 180 +       { .compatible = "rpi,rpi-sense-js" },
 181 +       { },
 182 +};
 183 +MODULE_DEVICE_TABLE(of, rpisense_js_id);
 184 +#endif
 185 +
 186 +static struct platform_device_id rpisense_js_device_id[] = {
 187 +       { .name = "rpi-sense-js" },
 188 +       { },
 189 +};
 190 +MODULE_DEVICE_TABLE(platform, rpisense_js_device_id);
 191 +
 192 +static struct platform_driver rpisense_js_driver = {
 193 +       .probe = rpisense_js_probe,
 194 +       .remove = rpisense_js_remove,
 195 +       .driver = {
 196 +               .name = "rpi-sense-js",
 197 +               .owner = THIS_MODULE,
 198 +       },
 199 +};
 200 +
 201 +module_platform_driver(rpisense_js_driver);
 202 +
 203 +MODULE_DESCRIPTION("Raspberry Pi Sense HAT joystick driver");
 204 +MODULE_AUTHOR("Serge Schneider <serge@raspberrypi.org>");
 205 +MODULE_LICENSE("GPL");
 206 --- a/drivers/mfd/Kconfig
 207 +++ b/drivers/mfd/Kconfig
 208 @@ -10,6 +10,14 @@ config MFD_CORE
 209         select IRQ_DOMAIN
 210         default n
 211
 212 +config MFD_RPISENSE_CORE
 213 +       tristate "Raspberry Pi Sense HAT core functions"
 214 +       depends on I2C
 215 +       select MFD_CORE
 216 +       help
 217 +         This is the core driver for the Raspberry Pi Sense HAT. This provides
 218 +         the necessary functions to communicate with the hardware.
 219 +
 220  config MFD_CS5535
 221         tristate "AMD CS5535 and CS5536 southbridge core functions"
 222         select MFD_CORE
 223 --- a/drivers/mfd/Makefile
 224 +++ b/drivers/mfd/Makefile
 225 @@ -240,4 +240,4 @@ obj-$(CONFIG_MFD_MXS_LRADC)     += mxs-l
 226  obj-$(CONFIG_MFD_SC27XX_PMIC)  += sprd-sc27xx-spi.o
 227  obj-$(CONFIG_RAVE_SP_CORE)     += rave-sp.o
 228  obj-$(CONFIG_MFD_ROHM_BD718XX) += rohm-bd718x7.o
 229 -
 230 +obj-$(CONFIG_MFD_RPISENSE_CORE)        += rpisense-core.o
 231 --- /dev/null
 232 +++ b/drivers/mfd/rpisense-core.c
 233 @@ -0,0 +1,157 @@
 234 +/*
 235 + * Raspberry Pi Sense HAT core driver
 236 + * http://raspberrypi.org
 237 + *
 238 + * Copyright (C) 2015 Raspberry Pi
 239 + *
 240 + * Author: Serge Schneider
 241 + *
 242 + *  This program is free software; you can redistribute  it and/or modify it
 243 + *  under  the terms of  the GNU General  Public License as published by the
 244 + *  Free Software Foundation;  either version 2 of the  License, or (at your
 245 + *  option) any later version.
 246 + *
 247 + *  This driver is based on wm8350 implementation.
 248 + */
 249 +
 250 +#include <linux/module.h>
 251 +#include <linux/moduleparam.h>
 252 +#include <linux/err.h>
 253 +#include <linux/init.h>
 254 +#include <linux/i2c.h>
 255 +#include <linux/platform_device.h>
 256 +#include <linux/mfd/rpisense/core.h>
 257 +#include <linux/slab.h>
 258 +
 259 +static struct rpisense *rpisense;
 260 +
 261 +static void rpisense_client_dev_register(struct rpisense *rpisense,
 262 +                                        const char *name,
 263 +                                        struct platform_device **pdev)
 264 +{
 265 +       int ret;
 266 +
 267 +       *pdev = platform_device_alloc(name, -1);
 268 +       if (*pdev == NULL) {
 269 +               dev_err(rpisense->dev, "Failed to allocate %s\n", name);
 270 +               return;
 271 +       }
 272 +
 273 +       (*pdev)->dev.parent = rpisense->dev;
 274 +       platform_set_drvdata(*pdev, rpisense);
 275 +       ret = platform_device_add(*pdev);
 276 +       if (ret != 0) {
 277 +               dev_err(rpisense->dev, "Failed to register %s: %d\n",
 278 +                       name, ret);
 279 +               platform_device_put(*pdev);
 280 +               *pdev = NULL;
 281 +       }
 282 +}
 283 +
 284 +static int rpisense_probe(struct i2c_client *i2c,
 285 +                              const struct i2c_device_id *id)
 286 +{
 287 +       int ret;
 288 +       struct rpisense_js *rpisense_js;
 289 +
 290 +       rpisense = devm_kzalloc(&i2c->dev, sizeof(struct rpisense), GFP_KERNEL);
 291 +       if (rpisense == NULL)
 292 +               return -ENOMEM;
 293 +
 294 +       i2c_set_clientdata(i2c, rpisense);
 295 +       rpisense->dev = &i2c->dev;
 296 +       rpisense->i2c_client = i2c;
 297 +
 298 +       ret = rpisense_reg_read(rpisense, RPISENSE_WAI);
 299 +       if (ret > 0) {
 300 +               if (ret != 's')
 301 +                       return -EINVAL;
 302 +       } else {
 303 +               return ret;
 304 +       }
 305 +       ret = rpisense_reg_read(rpisense, RPISENSE_VER);
 306 +       if (ret < 0)
 307 +               return ret;
 308 +
 309 +       dev_info(rpisense->dev,
 310 +                "Raspberry Pi Sense HAT firmware version %i\n", ret);
 311 +
 312 +       rpisense_js = &rpisense->joystick;
 313 +       rpisense_js->keys_desc = devm_gpiod_get(&i2c->dev,
 314 +                                               "keys-int", GPIOD_IN);
 315 +       if (IS_ERR(rpisense_js->keys_desc)) {
 316 +               dev_warn(&i2c->dev, "Failed to get keys-int descriptor.\n");
 317 +               rpisense_js->keys_desc = gpio_to_desc(23);
 318 +               if (rpisense_js->keys_desc == NULL) {
 319 +                       dev_err(&i2c->dev, "GPIO23 fallback failed.\n");
 320 +                       return PTR_ERR(rpisense_js->keys_desc);
 321 +               }
 322 +       }
 323 +       rpisense_client_dev_register(rpisense, "rpi-sense-js",
 324 +                                    &(rpisense->joystick.pdev));
 325 +       rpisense_client_dev_register(rpisense, "rpi-sense-fb",
 326 +                                    &(rpisense->framebuffer.pdev));
 327 +
 328 +       return 0;
 329 +}
 330 +
 331 +static int rpisense_remove(struct i2c_client *i2c)
 332 +{
 333 +       struct rpisense *rpisense = i2c_get_clientdata(i2c);
 334 +
 335 +       platform_device_unregister(rpisense->joystick.pdev);
 336 +       return 0;
 337 +}
 338 +
 339 +struct rpisense *rpisense_get_dev(void)
 340 +{
 341 +       return rpisense;
 342 +}
 343 +EXPORT_SYMBOL_GPL(rpisense_get_dev);
 344 +
 345 +s32 rpisense_reg_read(struct rpisense *rpisense, int reg)
 346 +{
 347 +       int ret = i2c_smbus_read_byte_data(rpisense->i2c_client, reg);
 348 +
 349 +       if (ret < 0)
 350 +               dev_err(rpisense->dev, "Read from reg %d failed\n", reg);
 351 +       /* Due to the BCM270x I2C clock stretching bug, some values
 352 +        * may have MSB set. Clear it to avoid incorrect values.
 353 +        * */
 354 +       return ret & 0x7F;
 355 +}
 356 +EXPORT_SYMBOL_GPL(rpisense_reg_read);
 357 +
 358 +int rpisense_block_write(struct rpisense *rpisense, const char *buf, int count)
 359 +{
 360 +       int ret = i2c_master_send(rpisense->i2c_client, buf, count);
 361 +
 362 +       if (ret < 0)
 363 +               dev_err(rpisense->dev, "Block write failed\n");
 364 +       return ret;
 365 +}
 366 +EXPORT_SYMBOL_GPL(rpisense_block_write);
 367 +
 368 +static const struct i2c_device_id rpisense_i2c_id[] = {
 369 +       { "rpi-sense", 0 },
 370 +       { }
 371 +};
 372 +MODULE_DEVICE_TABLE(i2c, rpisense_i2c_id);
 373 +
 374 +
 375 +static struct i2c_driver rpisense_driver = {
 376 +       .driver = {
 377 +                  .name = "rpi-sense",
 378 +                  .owner = THIS_MODULE,
 379 +       },
 380 +       .probe = rpisense_probe,
 381 +       .remove = rpisense_remove,
 382 +       .id_table = rpisense_i2c_id,
 383 +};
 384 +
 385 +module_i2c_driver(rpisense_driver);
 386 +
 387 +MODULE_DESCRIPTION("Raspberry Pi Sense HAT core driver");
 388 +MODULE_AUTHOR("Serge Schneider <serge@raspberrypi.org>");
 389 +MODULE_LICENSE("GPL");
 390 +
 391 --- a/drivers/video/fbdev/Kconfig
 392 +++ b/drivers/video/fbdev/Kconfig
 393 @@ -2350,6 +2350,19 @@ config FB_SM712
 394           called sm712fb. If you want to compile it as a module, say M
 395           here and read <file:Documentation/kbuild/modules.txt>.
 396
 397 +config FB_RPISENSE
 398 +       tristate "Raspberry Pi Sense HAT framebuffer"
 399 +       depends on FB
 400 +       select MFD_RPISENSE_CORE
 401 +       select FB_SYS_FOPS
 402 +       select FB_SYS_FILLRECT
 403 +       select FB_SYS_COPYAREA
 404 +       select FB_SYS_IMAGEBLIT
 405 +       select FB_DEFERRED_IO
 406 +
 407 +       help
 408 +         This is the framebuffer driver for the Raspberry Pi Sense HAT
 409 +
 410  source "drivers/video/fbdev/omap/Kconfig"
 411  source "drivers/video/fbdev/omap2/Kconfig"
 412  source "drivers/video/fbdev/mmp/Kconfig"
 413 --- a/drivers/video/fbdev/Makefile
 414 +++ b/drivers/video/fbdev/Makefile
 415 @@ -138,6 +138,7 @@ obj-$(CONFIG_FB_DA8XX)                += da8xx-fb.o
 416  obj-$(CONFIG_FB_MXS)             += mxsfb.o
 417  obj-$(CONFIG_FB_SSD1307)         += ssd1307fb.o
 418  obj-$(CONFIG_FB_SIMPLE)           += simplefb.o
 419 +obj-$(CONFIG_FB_RPISENSE)        += rpisense-fb.o
 420
 421  # the test framebuffer is last
 422  obj-$(CONFIG_FB_VIRTUAL)          += vfb.o
 423 --- /dev/null
 424 +++ b/drivers/video/fbdev/rpisense-fb.c
 425 @@ -0,0 +1,293 @@
 426 +/*
 427 + * Raspberry Pi Sense HAT framebuffer driver
 428 + * http://raspberrypi.org
 429 + *
 430 + * Copyright (C) 2015 Raspberry Pi
 431 + *
 432 + * Author: Serge Schneider
 433 + *
 434 + *  This program is free software; you can redistribute  it and/or modify it
 435 + *  under  the terms of  the GNU General  Public License as published by the
 436 + *  Free Software Foundation;  either version 2 of the  License, or (at your
 437 + *  option) any later version.
 438 + *
 439 + */
 440 +
 441 +#include <linux/module.h>
 442 +#include <linux/kernel.h>
 443 +#include <linux/errno.h>
 444 +#include <linux/string.h>
 445 +#include <linux/mm.h>
 446 +#include <linux/slab.h>
 447 +#include <linux/uaccess.h>
 448 +#include <linux/delay.h>
 449 +#include <linux/fb.h>
 450 +#include <linux/init.h>
 451 +
 452 +#include <linux/mfd/rpisense/framebuffer.h>
 453 +#include <linux/mfd/rpisense/core.h>
 454 +
 455 +static bool lowlight;
 456 +module_param(lowlight, bool, 0);
 457 +MODULE_PARM_DESC(lowlight, "Reduce LED matrix brightness to one third");
 458 +
 459 +static struct rpisense *rpisense;
 460 +
 461 +struct rpisense_fb_param {
 462 +       char __iomem *vmem;
 463 +       u8 *vmem_work;
 464 +       u32 vmemsize;
 465 +       u8 *gamma;
 466 +};
 467 +
 468 +static u8 gamma_default[32] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01,
 469 +                              0x02, 0x02, 0x03, 0x03, 0x04, 0x05, 0x06, 0x07,
 470 +                              0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0E, 0x0F, 0x11,
 471 +                              0x12, 0x14, 0x15, 0x17, 0x19, 0x1B, 0x1D, 0x1F,};
 472 +
 473 +static u8 gamma_low[32] = {0x00, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
 474 +                          0x01, 0x01, 0x01, 0x01, 0x01, 0x02, 0x02, 0x02,
 475 +                          0x03, 0x03, 0x03, 0x04, 0x04, 0x05, 0x05, 0x06,
 476 +                          0x06, 0x07, 0x07, 0x08, 0x08, 0x09, 0x0A, 0x0A,};
 477 +
 478 +static u8 gamma_user[32];
 479 +
 480 +static struct rpisense_fb_param rpisense_fb_param = {
 481 +       .vmem = NULL,
 482 +       .vmemsize = 128,
 483 +       .gamma = gamma_default,
 484 +};
 485 +
 486 +static struct fb_deferred_io rpisense_fb_defio;
 487 +
 488 +static struct fb_fix_screeninfo rpisense_fb_fix = {
 489 +       .id =           "RPi-Sense FB",
 490 +       .type =         FB_TYPE_PACKED_PIXELS,
 491 +       .visual =       FB_VISUAL_TRUECOLOR,
 492 +       .xpanstep =     0,
 493 +       .ypanstep =     0,
 494 +       .ywrapstep =    0,
 495 +       .accel =        FB_ACCEL_NONE,
 496 +       .line_length =  16,
 497 +};
 498 +
 499 +static struct fb_var_screeninfo rpisense_fb_var = {
 500 +       .xres           = 8,
 501 +       .yres           = 8,
 502 +       .xres_virtual   = 8,
 503 +       .yres_virtual   = 8,
 504 +       .bits_per_pixel = 16,
 505 +       .red            = {11, 5, 0},
 506 +       .green          = {5, 6, 0},
 507 +       .blue           = {0, 5, 0},
 508 +};
 509 +
 510 +static ssize_t rpisense_fb_write(struct fb_info *info,
 511 +                                const char __user *buf, size_t count,
 512 +                                loff_t *ppos)
 513 +{
 514 +       ssize_t res = fb_sys_write(info, buf, count, ppos);
 515 +
 516 +       schedule_delayed_work(&info->deferred_work, rpisense_fb_defio.delay);
 517 +       return res;
 518 +}
 519 +
 520 +static void rpisense_fb_fillrect(struct fb_info *info,
 521 +                                const struct fb_fillrect *rect)
 522 +{
 523 +       sys_fillrect(info, rect);
 524 +       schedule_delayed_work(&info->deferred_work, rpisense_fb_defio.delay);
 525 +}
 526 +
 527 +static void rpisense_fb_copyarea(struct fb_info *info,
 528 +                                const struct fb_copyarea *area)
 529 +{
 530 +       sys_copyarea(info, area);
 531 +       schedule_delayed_work(&info->deferred_work, rpisense_fb_defio.delay);
 532 +}
 533 +
 534 +static void rpisense_fb_imageblit(struct fb_info *info,
 535 +                                 const struct fb_image *image)
 536 +{
 537 +       sys_imageblit(info, image);
 538 +       schedule_delayed_work(&info->deferred_work, rpisense_fb_defio.delay);
 539 +}
 540 +
 541 +static void rpisense_fb_deferred_io(struct fb_info *info,
 542 +                               struct list_head *pagelist)
 543 +{
 544 +       int i;
 545 +       int j;
 546 +       u8 *vmem_work = rpisense_fb_param.vmem_work;
 547 +       u16 *mem = (u16 *)rpisense_fb_param.vmem;
 548 +       u8 *gamma = rpisense_fb_param.gamma;
 549 +
 550 +       vmem_work[0] = 0;
 551 +       for (j = 0; j < 8; j++) {
 552 +               for (i = 0; i < 8; i++) {
 553 +                       vmem_work[(j * 24) + i + 1] =
 554 +                               gamma[(mem[(j * 8) + i] >> 11) & 0x1F];
 555 +                       vmem_work[(j * 24) + (i + 8) + 1] =
 556 +                               gamma[(mem[(j * 8) + i] >> 6) & 0x1F];
 557 +                       vmem_work[(j * 24) + (i + 16) + 1] =
 558 +                               gamma[(mem[(j * 8) + i]) & 0x1F];
 559 +               }
 560 +       }
 561 +       rpisense_block_write(rpisense, vmem_work, 193);
 562 +}
 563 +
 564 +static struct fb_deferred_io rpisense_fb_defio = {
 565 +       .delay          = HZ/100,
 566 +       .deferred_io    = rpisense_fb_deferred_io,
 567 +};
 568 +
 569 +static int rpisense_fb_ioctl(struct fb_info *info, unsigned int cmd,
 570 +                            unsigned long arg)
 571 +{
 572 +       switch (cmd) {
 573 +       case SENSEFB_FBIOGET_GAMMA:
 574 +               if (copy_to_user((void __user *) arg, rpisense_fb_param.gamma,
 575 +                                sizeof(u8[32])))
 576 +                       return -EFAULT;
 577 +               return 0;
 578 +       case SENSEFB_FBIOSET_GAMMA:
 579 +               if (copy_from_user(gamma_user, (void __user *)arg,
 580 +                                  sizeof(u8[32])))
 581 +                       return -EFAULT;
 582 +               rpisense_fb_param.gamma = gamma_user;
 583 +               schedule_delayed_work(&info->deferred_work,
 584 +                                     rpisense_fb_defio.delay);
 585 +               return 0;
 586 +       case SENSEFB_FBIORESET_GAMMA:
 587 +               switch (arg) {
 588 +               case 0:
 589 +                       rpisense_fb_param.gamma = gamma_default;
 590 +                       break;
 591 +               case 1:
 592 +                       rpisense_fb_param.gamma = gamma_low;
 593 +                       break;
 594 +               case 2:
 595 +                       rpisense_fb_param.gamma = gamma_user;
 596 +                       break;
 597 +               default:
 598 +                       return -EINVAL;
 599 +               }
 600 +               schedule_delayed_work(&info->deferred_work,
 601 +                                     rpisense_fb_defio.delay);
 602 +               break;
 603 +       default:
 604 +               return -EINVAL;
 605 +       }
 606 +       return 0;
 607 +}
 608 +
 609 +static struct fb_ops rpisense_fb_ops = {
 610 +       .owner          = THIS_MODULE,
 611 +       .fb_read        = fb_sys_read,
 612 +       .fb_write       = rpisense_fb_write,
 613 +       .fb_fillrect    = rpisense_fb_fillrect,
 614 +       .fb_copyarea    = rpisense_fb_copyarea,
 615 +       .fb_imageblit   = rpisense_fb_imageblit,
 616 +       .fb_ioctl       = rpisense_fb_ioctl,
 617 +};
 618 +
 619 +static int rpisense_fb_probe(struct platform_device *pdev)
 620 +{
 621 +       struct fb_info *info;
 622 +       int ret = -ENOMEM;
 623 +       struct rpisense_fb *rpisense_fb;
 624 +
 625 +       rpisense = rpisense_get_dev();
 626 +       rpisense_fb = &rpisense->framebuffer;
 627 +
 628 +       rpisense_fb_param.vmem = vzalloc(rpisense_fb_param.vmemsize);
 629 +       if (!rpisense_fb_param.vmem)
 630 +               return ret;
 631 +
 632 +       rpisense_fb_param.vmem_work = devm_kmalloc(&pdev->dev, 193, GFP_KERNEL);
 633 +       if (!rpisense_fb_param.vmem_work)
 634 +               goto err_malloc;
 635 +
 636 +       info = framebuffer_alloc(0, &pdev->dev);
 637 +       if (!info) {
 638 +               dev_err(&pdev->dev, "Could not allocate framebuffer.\n");
 639 +               goto err_malloc;
 640 +       }
 641 +       rpisense_fb->info = info;
 642 +
 643 +       rpisense_fb_fix.smem_start = (unsigned long)rpisense_fb_param.vmem;
 644 +       rpisense_fb_fix.smem_len = rpisense_fb_param.vmemsize;
 645 +
 646 +       info->fbops = &rpisense_fb_ops;
 647 +       info->fix = rpisense_fb_fix;
 648 +       info->var = rpisense_fb_var;
 649 +       info->fbdefio = &rpisense_fb_defio;
 650 +       info->flags = FBINFO_FLAG_DEFAULT | FBINFO_VIRTFB;
 651 +       info->screen_base = rpisense_fb_param.vmem;
 652 +       info->screen_size = rpisense_fb_param.vmemsize;
 653 +
 654 +       if (lowlight)
 655 +               rpisense_fb_param.gamma = gamma_low;
 656 +
 657 +       fb_deferred_io_init(info);
 658 +
 659 +       ret = register_framebuffer(info);
 660 +       if (ret < 0) {
 661 +               dev_err(&pdev->dev, "Could not register framebuffer.\n");
 662 +               goto err_fballoc;
 663 +       }
 664 +
 665 +       fb_info(info, "%s frame buffer device\n", info->fix.id);
 666 +       schedule_delayed_work(&info->deferred_work, rpisense_fb_defio.delay);
 667 +       return 0;
 668 +err_fballoc:
 669 +       framebuffer_release(info);
 670 +err_malloc:
 671 +       vfree(rpisense_fb_param.vmem);
 672 +       return ret;
 673 +}
 674 +
 675 +static int rpisense_fb_remove(struct platform_device *pdev)
 676 +{
 677 +       struct rpisense_fb *rpisense_fb = &rpisense->framebuffer;
 678 +       struct fb_info *info = rpisense_fb->info;
 679 +
 680 +       if (info) {
 681 +               unregister_framebuffer(info);
 682 +               fb_deferred_io_cleanup(info);
 683 +               framebuffer_release(info);
 684 +               vfree(rpisense_fb_param.vmem);
 685 +       }
 686 +
 687 +       return 0;
 688 +}
 689 +
 690 +#ifdef CONFIG_OF
 691 +static const struct of_device_id rpisense_fb_id[] = {
 692 +       { .compatible = "rpi,rpi-sense-fb" },
 693 +       { },
 694 +};
 695 +MODULE_DEVICE_TABLE(of, rpisense_fb_id);
 696 +#endif
 697 +
 698 +static struct platform_device_id rpisense_fb_device_id[] = {
 699 +       { .name = "rpi-sense-fb" },
 700 +       { },
 701 +};
 702 +MODULE_DEVICE_TABLE(platform, rpisense_fb_device_id);
 703 +
 704 +static struct platform_driver rpisense_fb_driver = {
 705 +       .probe = rpisense_fb_probe,
 706 +       .remove = rpisense_fb_remove,
 707 +       .driver = {
 708 +               .name = "rpi-sense-fb",
 709 +               .owner = THIS_MODULE,
 710 +       },
 711 +};
 712 +
 713 +module_platform_driver(rpisense_fb_driver);
 714 +
 715 +MODULE_DESCRIPTION("Raspberry Pi Sense HAT framebuffer driver");
 716 +MODULE_AUTHOR("Serge Schneider <serge@raspberrypi.org>");
 717 +MODULE_LICENSE("GPL");
 718 +
 719 --- /dev/null
 720 +++ b/include/linux/mfd/rpisense/core.h
 721 @@ -0,0 +1,47 @@
 722 +/*
 723 + * Raspberry Pi Sense HAT core driver
 724 + * http://raspberrypi.org
 725 + *
 726 + * Copyright (C) 2015 Raspberry Pi
 727 + *
 728 + * Author: Serge Schneider
 729 + *
 730 + *  This program is free software; you can redistribute  it and/or modify it
 731 + *  under  the terms of  the GNU General  Public License as published by the
 732 + *  Free Software Foundation;  either version 2 of the  License, or (at your
 733 + *  option) any later version.
 734 + *
 735 + */
 736 +
 737 +#ifndef __LINUX_MFD_RPISENSE_CORE_H_
 738 +#define __LINUX_MFD_RPISENSE_CORE_H_
 739 +
 740 +#include <linux/mfd/rpisense/joystick.h>
 741 +#include <linux/mfd/rpisense/framebuffer.h>
 742 +
 743 +/*
 744 + * Register values.
 745 + */
 746 +#define RPISENSE_FB                    0x00
 747 +#define RPISENSE_WAI                   0xF0
 748 +#define RPISENSE_VER                   0xF1
 749 +#define RPISENSE_KEYS                  0xF2
 750 +#define RPISENSE_EE_WP                 0xF3
 751 +
 752 +#define RPISENSE_ID                    's'
 753 +
 754 +struct rpisense {
 755 +       struct device *dev;
 756 +       struct i2c_client *i2c_client;
 757 +
 758 +       /* Client devices */
 759 +       struct rpisense_js joystick;
 760 +       struct rpisense_fb framebuffer;
 761 +};
 762 +
 763 +struct rpisense *rpisense_get_dev(void);
 764 +s32 rpisense_reg_read(struct rpisense *rpisense, int reg);
 765 +int rpisense_reg_write(struct rpisense *rpisense, int reg, u16 val);
 766 +int rpisense_block_write(struct rpisense *rpisense, const char *buf, int count);
 767 +
 768 +#endif
 769 --- /dev/null
 770 +++ b/include/linux/mfd/rpisense/framebuffer.h
 771 @@ -0,0 +1,32 @@
 772 +/*
 773 + * Raspberry Pi Sense HAT framebuffer driver
 774 + * http://raspberrypi.org
 775 + *
 776 + * Copyright (C) 2015 Raspberry Pi
 777 + *
 778 + * Author: Serge Schneider
 779 + *
 780 + *  This program is free software; you can redistribute  it and/or modify it
 781 + *  under  the terms of  the GNU General  Public License as published by the
 782 + *  Free Software Foundation;  either version 2 of the  License, or (at your
 783 + *  option) any later version.
 784 + *
 785 + */
 786 +
 787 +#ifndef __LINUX_RPISENSE_FB_H_
 788 +#define __LINUX_RPISENSE_FB_H_
 789 +
 790 +#define SENSEFB_FBIO_IOC_MAGIC 0xF1
 791 +
 792 +#define SENSEFB_FBIOGET_GAMMA _IO(SENSEFB_FBIO_IOC_MAGIC, 0)
 793 +#define SENSEFB_FBIOSET_GAMMA _IO(SENSEFB_FBIO_IOC_MAGIC, 1)
 794 +#define SENSEFB_FBIORESET_GAMMA _IO(SENSEFB_FBIO_IOC_MAGIC, 2)
 795 +
 796 +struct rpisense;
 797 +
 798 +struct rpisense_fb {
 799 +       struct platform_device *pdev;
 800 +       struct fb_info *info;
 801 +};
 802 +
 803 +#endif
 804 --- /dev/null
 805 +++ b/include/linux/mfd/rpisense/joystick.h
 806 @@ -0,0 +1,35 @@
 807 +/*
 808 + * Raspberry Pi Sense HAT joystick driver
 809 + * http://raspberrypi.org
 810 + *
 811 + * Copyright (C) 2015 Raspberry Pi
 812 + *
 813 + * Author: Serge Schneider
 814 + *
 815 + *  This program is free software; you can redistribute  it and/or modify it
 816 + *  under  the terms of  the GNU General  Public License as published by the
 817 + *  Free Software Foundation;  either version 2 of the  License, or (at your
 818 + *  option) any later version.
 819 + *
 820 + */
 821 +
 822 +#ifndef __LINUX_RPISENSE_JOYSTICK_H_
 823 +#define __LINUX_RPISENSE_JOYSTICK_H_
 824 +
 825 +#include <linux/input.h>
 826 +#include <linux/interrupt.h>
 827 +#include <linux/gpio/consumer.h>
 828 +#include <linux/platform_device.h>
 829 +
 830 +struct rpisense;
 831 +
 832 +struct rpisense_js {
 833 +       struct platform_device *pdev;
 834 +       struct input_dev *keys_dev;
 835 +       struct gpio_desc *keys_desc;
 836 +       struct work_struct keys_work_s;
 837 +       int keys_irq;
 838 +};
 839 +
 840 +
 841 +#endif