target/linux/brcm2708/patches-4.4/0084-mfd-Add-Raspberry-Pi-Sense-HAT-core-driver.patch

   1 From 933c347e34871491afd9d53c7a9d0bf3c283c41f 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 084/127] 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, 750 insertions(+)
  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 @@ -330,4 +330,12 @@ config JOYSTICK_MAPLE
  30           To compile this as a module choose M here: the module will be called
  31           maplecontrol.
  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 @@ -32,4 +32,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 @@ -194,3 +194,5 @@ intel-soc-pmic-objs         := intel_soc_pmic_c
 226  intel-soc-pmic-$(CONFIG_INTEL_PMC_IPC) += intel_soc_pmic_bxtwc.o
 227  obj-$(CONFIG_INTEL_SOC_PMIC)   += intel-soc-pmic.o
 228  obj-$(CONFIG_MFD_MT6397)       += mt6397-core.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 @@ -2506,3 +2506,16 @@ config FB_SM712
 394           This driver is also available as a module. The module will be
 395           called sm712fb. If you want to compile it as a module, say M
 396           here and read <file:Documentation/kbuild/modules.txt>.
 397 +
 398 +config FB_RPISENSE
 399 +       tristate "Raspberry Pi Sense HAT framebuffer"
 400 +       depends on FB
 401 +       select MFD_RPISENSE_CORE
 402 +       select FB_SYS_FOPS
 403 +       select FB_SYS_FILLRECT
 404 +       select FB_SYS_COPYAREA
 405 +       select FB_SYS_IMAGEBLIT
 406 +       select FB_DEFERRED_IO
 407 +
 408 +       help
 409 +         This is the framebuffer driver for the Raspberry Pi Sense HAT
 410 --- a/drivers/video/fbdev/Makefile
 411 +++ b/drivers/video/fbdev/Makefile
 412 @@ -150,6 +150,7 @@ obj-$(CONFIG_FB_DA8XX)                += da8xx-fb.o
 413  obj-$(CONFIG_FB_MXS)             += mxsfb.o
 414  obj-$(CONFIG_FB_SSD1307)         += ssd1307fb.o
 415  obj-$(CONFIG_FB_SIMPLE)           += simplefb.o
 416 +obj-$(CONFIG_FB_RPISENSE)        += rpisense-fb.o
 417
 418  # the test framebuffer is last
 419  obj-$(CONFIG_FB_VIRTUAL)          += vfb.o
 420 --- /dev/null
 421 +++ b/drivers/video/fbdev/rpisense-fb.c
 422 @@ -0,0 +1,293 @@
 423 +/*
 424 + * Raspberry Pi Sense HAT framebuffer driver
 425 + * http://raspberrypi.org
 426 + *
 427 + * Copyright (C) 2015 Raspberry Pi
 428 + *
 429 + * Author: Serge Schneider
 430 + *
 431 + *  This program is free software; you can redistribute  it and/or modify it
 432 + *  under  the terms of  the GNU General  Public License as published by the
 433 + *  Free Software Foundation;  either version 2 of the  License, or (at your
 434 + *  option) any later version.
 435 + *
 436 + */
 437 +
 438 +#include <linux/module.h>
 439 +#include <linux/kernel.h>
 440 +#include <linux/errno.h>
 441 +#include <linux/string.h>
 442 +#include <linux/mm.h>
 443 +#include <linux/slab.h>
 444 +#include <linux/uaccess.h>
 445 +#include <linux/delay.h>
 446 +#include <linux/fb.h>
 447 +#include <linux/init.h>
 448 +
 449 +#include <linux/mfd/rpisense/framebuffer.h>
 450 +#include <linux/mfd/rpisense/core.h>
 451 +
 452 +static bool lowlight;
 453 +module_param(lowlight, bool, 0);
 454 +MODULE_PARM_DESC(lowlight, "Reduce LED matrix brightness to one third");
 455 +
 456 +static struct rpisense *rpisense;
 457 +
 458 +struct rpisense_fb_param {
 459 +       char __iomem *vmem;
 460 +       u8 *vmem_work;
 461 +       u32 vmemsize;
 462 +       u8 *gamma;
 463 +};
 464 +
 465 +static u8 gamma_default[32] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01,
 466 +                              0x02, 0x02, 0x03, 0x03, 0x04, 0x05, 0x06, 0x07,
 467 +                              0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0E, 0x0F, 0x11,
 468 +                              0x12, 0x14, 0x15, 0x17, 0x19, 0x1B, 0x1D, 0x1F,};
 469 +
 470 +static u8 gamma_low[32] = {0x00, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
 471 +                          0x01, 0x01, 0x01, 0x01, 0x01, 0x02, 0x02, 0x02,
 472 +                          0x03, 0x03, 0x03, 0x04, 0x04, 0x05, 0x05, 0x06,
 473 +                          0x06, 0x07, 0x07, 0x08, 0x08, 0x09, 0x0A, 0x0A,};
 474 +
 475 +static u8 gamma_user[32];
 476 +
 477 +static struct rpisense_fb_param rpisense_fb_param = {
 478 +       .vmem = NULL,
 479 +       .vmemsize = 128,
 480 +       .gamma = gamma_default,
 481 +};
 482 +
 483 +static struct fb_deferred_io rpisense_fb_defio;
 484 +
 485 +static struct fb_fix_screeninfo rpisense_fb_fix = {
 486 +       .id =           "RPi-Sense FB",
 487 +       .type =         FB_TYPE_PACKED_PIXELS,
 488 +       .visual =       FB_VISUAL_TRUECOLOR,
 489 +       .xpanstep =     0,
 490 +       .ypanstep =     0,
 491 +       .ywrapstep =    0,
 492 +       .accel =        FB_ACCEL_NONE,
 493 +       .line_length =  16,
 494 +};
 495 +
 496 +static struct fb_var_screeninfo rpisense_fb_var = {
 497 +       .xres           = 8,
 498 +       .yres           = 8,
 499 +       .xres_virtual   = 8,
 500 +       .yres_virtual   = 8,
 501 +       .bits_per_pixel = 16,
 502 +       .red            = {11, 5, 0},
 503 +       .green          = {5, 6, 0},
 504 +       .blue           = {0, 5, 0},
 505 +};
 506 +
 507 +static ssize_t rpisense_fb_write(struct fb_info *info,
 508 +                                const char __user *buf, size_t count,
 509 +                                loff_t *ppos)
 510 +{
 511 +       ssize_t res = fb_sys_write(info, buf, count, ppos);
 512 +
 513 +       schedule_delayed_work(&info->deferred_work, rpisense_fb_defio.delay);
 514 +       return res;
 515 +}
 516 +
 517 +static void rpisense_fb_fillrect(struct fb_info *info,
 518 +                                const struct fb_fillrect *rect)
 519 +{
 520 +       sys_fillrect(info, rect);
 521 +       schedule_delayed_work(&info->deferred_work, rpisense_fb_defio.delay);
 522 +}
 523 +
 524 +static void rpisense_fb_copyarea(struct fb_info *info,
 525 +                                const struct fb_copyarea *area)
 526 +{
 527 +       sys_copyarea(info, area);
 528 +       schedule_delayed_work(&info->deferred_work, rpisense_fb_defio.delay);
 529 +}
 530 +
 531 +static void rpisense_fb_imageblit(struct fb_info *info,
 532 +                                 const struct fb_image *image)
 533 +{
 534 +       sys_imageblit(info, image);
 535 +       schedule_delayed_work(&info->deferred_work, rpisense_fb_defio.delay);
 536 +}
 537 +
 538 +static void rpisense_fb_deferred_io(struct fb_info *info,
 539 +                               struct list_head *pagelist)
 540 +{
 541 +       int i;
 542 +       int j;
 543 +       u8 *vmem_work = rpisense_fb_param.vmem_work;
 544 +       u16 *mem = (u16 *)rpisense_fb_param.vmem;
 545 +       u8 *gamma = rpisense_fb_param.gamma;
 546 +
 547 +       vmem_work[0] = 0;
 548 +       for (j = 0; j < 8; j++) {
 549 +               for (i = 0; i < 8; i++) {
 550 +                       vmem_work[(j * 24) + i + 1] =
 551 +                               gamma[(mem[(j * 8) + i] >> 11) & 0x1F];
 552 +                       vmem_work[(j * 24) + (i + 8) + 1] =
 553 +                               gamma[(mem[(j * 8) + i] >> 6) & 0x1F];
 554 +                       vmem_work[(j * 24) + (i + 16) + 1] =
 555 +                               gamma[(mem[(j * 8) + i]) & 0x1F];
 556 +               }
 557 +       }
 558 +       rpisense_block_write(rpisense, vmem_work, 193);
 559 +}
 560 +
 561 +static struct fb_deferred_io rpisense_fb_defio = {
 562 +       .delay          = HZ/100,
 563 +       .deferred_io    = rpisense_fb_deferred_io,
 564 +};
 565 +
 566 +static int rpisense_fb_ioctl(struct fb_info *info, unsigned int cmd,
 567 +                            unsigned long arg)
 568 +{
 569 +       switch (cmd) {
 570 +       case SENSEFB_FBIOGET_GAMMA:
 571 +               if (copy_to_user((void __user *) arg, rpisense_fb_param.gamma,
 572 +                                sizeof(u8[32])))
 573 +                       return -EFAULT;
 574 +               return 0;
 575 +       case SENSEFB_FBIOSET_GAMMA:
 576 +               if (copy_from_user(gamma_user, (void __user *)arg,
 577 +                                  sizeof(u8[32])))
 578 +                       return -EFAULT;
 579 +               rpisense_fb_param.gamma = gamma_user;
 580 +               schedule_delayed_work(&info->deferred_work,
 581 +                                     rpisense_fb_defio.delay);
 582 +               return 0;
 583 +       case SENSEFB_FBIORESET_GAMMA:
 584 +               switch (arg) {
 585 +               case 0:
 586 +                       rpisense_fb_param.gamma = gamma_default;
 587 +                       break;
 588 +               case 1:
 589 +                       rpisense_fb_param.gamma = gamma_low;
 590 +                       break;
 591 +               case 2:
 592 +                       rpisense_fb_param.gamma = gamma_user;
 593 +                       break;
 594 +               default:
 595 +                       return -EINVAL;
 596 +               }
 597 +               schedule_delayed_work(&info->deferred_work,
 598 +                                     rpisense_fb_defio.delay);
 599 +               break;
 600 +       default:
 601 +               return -EINVAL;
 602 +       }
 603 +       return 0;
 604 +}
 605 +
 606 +static struct fb_ops rpisense_fb_ops = {
 607 +       .owner          = THIS_MODULE,
 608 +       .fb_read        = fb_sys_read,
 609 +       .fb_write       = rpisense_fb_write,
 610 +       .fb_fillrect    = rpisense_fb_fillrect,
 611 +       .fb_copyarea    = rpisense_fb_copyarea,
 612 +       .fb_imageblit   = rpisense_fb_imageblit,
 613 +       .fb_ioctl       = rpisense_fb_ioctl,
 614 +};
 615 +
 616 +static int rpisense_fb_probe(struct platform_device *pdev)
 617 +{
 618 +       struct fb_info *info;
 619 +       int ret = -ENOMEM;
 620 +       struct rpisense_fb *rpisense_fb;
 621 +
 622 +       rpisense = rpisense_get_dev();
 623 +       rpisense_fb = &rpisense->framebuffer;
 624 +
 625 +       rpisense_fb_param.vmem = vzalloc(rpisense_fb_param.vmemsize);
 626 +       if (!rpisense_fb_param.vmem)
 627 +               return ret;
 628 +
 629 +       rpisense_fb_param.vmem_work = devm_kmalloc(&pdev->dev, 193, GFP_KERNEL);
 630 +       if (!rpisense_fb_param.vmem_work)
 631 +               goto err_malloc;
 632 +
 633 +       info = framebuffer_alloc(0, &pdev->dev);
 634 +       if (!info) {
 635 +               dev_err(&pdev->dev, "Could not allocate framebuffer.\n");
 636 +               goto err_malloc;
 637 +       }
 638 +       rpisense_fb->info = info;
 639 +
 640 +       rpisense_fb_fix.smem_start = (unsigned long)rpisense_fb_param.vmem;
 641 +       rpisense_fb_fix.smem_len = rpisense_fb_param.vmemsize;
 642 +
 643 +       info->fbops = &rpisense_fb_ops;
 644 +       info->fix = rpisense_fb_fix;
 645 +       info->var = rpisense_fb_var;
 646 +       info->fbdefio = &rpisense_fb_defio;
 647 +       info->flags = FBINFO_FLAG_DEFAULT | FBINFO_VIRTFB;
 648 +       info->screen_base = rpisense_fb_param.vmem;
 649 +       info->screen_size = rpisense_fb_param.vmemsize;
 650 +
 651 +       if (lowlight)
 652 +               rpisense_fb_param.gamma = gamma_low;
 653 +
 654 +       fb_deferred_io_init(info);
 655 +
 656 +       ret = register_framebuffer(info);
 657 +       if (ret < 0) {
 658 +               dev_err(&pdev->dev, "Could not register framebuffer.\n");
 659 +               goto err_fballoc;
 660 +       }
 661 +
 662 +       fb_info(info, "%s frame buffer device\n", info->fix.id);
 663 +       schedule_delayed_work(&info->deferred_work, rpisense_fb_defio.delay);
 664 +       return 0;
 665 +err_fballoc:
 666 +       framebuffer_release(info);
 667 +err_malloc:
 668 +       vfree(rpisense_fb_param.vmem);
 669 +       return ret;
 670 +}
 671 +
 672 +static int rpisense_fb_remove(struct platform_device *pdev)
 673 +{
 674 +       struct rpisense_fb *rpisense_fb = &rpisense->framebuffer;
 675 +       struct fb_info *info = rpisense_fb->info;
 676 +
 677 +       if (info) {
 678 +               unregister_framebuffer(info);
 679 +               fb_deferred_io_cleanup(info);
 680 +               framebuffer_release(info);
 681 +               vfree(rpisense_fb_param.vmem);
 682 +       }
 683 +
 684 +       return 0;
 685 +}
 686 +
 687 +#ifdef CONFIG_OF
 688 +static const struct of_device_id rpisense_fb_id[] = {
 689 +       { .compatible = "rpi,rpi-sense-fb" },
 690 +       { },
 691 +};
 692 +MODULE_DEVICE_TABLE(of, rpisense_fb_id);
 693 +#endif
 694 +
 695 +static struct platform_device_id rpisense_fb_device_id[] = {
 696 +       { .name = "rpi-sense-fb" },
 697 +       { },
 698 +};
 699 +MODULE_DEVICE_TABLE(platform, rpisense_fb_device_id);
 700 +
 701 +static struct platform_driver rpisense_fb_driver = {
 702 +       .probe = rpisense_fb_probe,
 703 +       .remove = rpisense_fb_remove,
 704 +       .driver = {
 705 +               .name = "rpi-sense-fb",
 706 +               .owner = THIS_MODULE,
 707 +       },
 708 +};
 709 +
 710 +module_platform_driver(rpisense_fb_driver);
 711 +
 712 +MODULE_DESCRIPTION("Raspberry Pi Sense HAT framebuffer driver");
 713 +MODULE_AUTHOR("Serge Schneider <serge@raspberrypi.org>");
 714 +MODULE_LICENSE("GPL");
 715 +
 716 --- /dev/null
 717 +++ b/include/linux/mfd/rpisense/core.h
 718 @@ -0,0 +1,47 @@
 719 +/*
 720 + * Raspberry Pi Sense HAT core driver
 721 + * http://raspberrypi.org
 722 + *
 723 + * Copyright (C) 2015 Raspberry Pi
 724 + *
 725 + * Author: Serge Schneider
 726 + *
 727 + *  This program is free software; you can redistribute  it and/or modify it
 728 + *  under  the terms of  the GNU General  Public License as published by the
 729 + *  Free Software Foundation;  either version 2 of the  License, or (at your
 730 + *  option) any later version.
 731 + *
 732 + */
 733 +
 734 +#ifndef __LINUX_MFD_RPISENSE_CORE_H_
 735 +#define __LINUX_MFD_RPISENSE_CORE_H_
 736 +
 737 +#include <linux/mfd/rpisense/joystick.h>
 738 +#include <linux/mfd/rpisense/framebuffer.h>
 739 +
 740 +/*
 741 + * Register values.
 742 + */
 743 +#define RPISENSE_FB                    0x00
 744 +#define RPISENSE_WAI                   0xF0
 745 +#define RPISENSE_VER                   0xF1
 746 +#define RPISENSE_KEYS                  0xF2
 747 +#define RPISENSE_EE_WP                 0xF3
 748 +
 749 +#define RPISENSE_ID                    's'
 750 +
 751 +struct rpisense {
 752 +       struct device *dev;
 753 +       struct i2c_client *i2c_client;
 754 +
 755 +       /* Client devices */
 756 +       struct rpisense_js joystick;
 757 +       struct rpisense_fb framebuffer;
 758 +};
 759 +
 760 +struct rpisense *rpisense_get_dev(void);
 761 +s32 rpisense_reg_read(struct rpisense *rpisense, int reg);
 762 +int rpisense_reg_write(struct rpisense *rpisense, int reg, u16 val);
 763 +int rpisense_block_write(struct rpisense *rpisense, const char *buf, int count);
 764 +
 765 +#endif
 766 --- /dev/null
 767 +++ b/include/linux/mfd/rpisense/framebuffer.h
 768 @@ -0,0 +1,32 @@
 769 +/*
 770 + * Raspberry Pi Sense HAT framebuffer driver
 771 + * http://raspberrypi.org
 772 + *
 773 + * Copyright (C) 2015 Raspberry Pi
 774 + *
 775 + * Author: Serge Schneider
 776 + *
 777 + *  This program is free software; you can redistribute  it and/or modify it
 778 + *  under  the terms of  the GNU General  Public License as published by the
 779 + *  Free Software Foundation;  either version 2 of the  License, or (at your
 780 + *  option) any later version.
 781 + *
 782 + */
 783 +
 784 +#ifndef __LINUX_RPISENSE_FB_H_
 785 +#define __LINUX_RPISENSE_FB_H_
 786 +
 787 +#define SENSEFB_FBIO_IOC_MAGIC 0xF1
 788 +
 789 +#define SENSEFB_FBIOGET_GAMMA _IO(SENSEFB_FBIO_IOC_MAGIC, 0)
 790 +#define SENSEFB_FBIOSET_GAMMA _IO(SENSEFB_FBIO_IOC_MAGIC, 1)
 791 +#define SENSEFB_FBIORESET_GAMMA _IO(SENSEFB_FBIO_IOC_MAGIC, 2)
 792 +
 793 +struct rpisense;
 794 +
 795 +struct rpisense_fb {
 796 +       struct platform_device *pdev;
 797 +       struct fb_info *info;
 798 +};
 799 +
 800 +#endif
 801 --- /dev/null
 802 +++ b/include/linux/mfd/rpisense/joystick.h
 803 @@ -0,0 +1,35 @@
 804 +/*
 805 + * Raspberry Pi Sense HAT joystick driver
 806 + * http://raspberrypi.org
 807 + *
 808 + * Copyright (C) 2015 Raspberry Pi
 809 + *
 810 + * Author: Serge Schneider
 811 + *
 812 + *  This program is free software; you can redistribute  it and/or modify it
 813 + *  under  the terms of  the GNU General  Public License as published by the
 814 + *  Free Software Foundation;  either version 2 of the  License, or (at your
 815 + *  option) any later version.
 816 + *
 817 + */
 818 +
 819 +#ifndef __LINUX_RPISENSE_JOYSTICK_H_
 820 +#define __LINUX_RPISENSE_JOYSTICK_H_
 821 +
 822 +#include <linux/input.h>
 823 +#include <linux/interrupt.h>
 824 +#include <linux/gpio/consumer.h>
 825 +#include <linux/platform_device.h>
 826 +
 827 +struct rpisense;
 828 +
 829 +struct rpisense_js {
 830 +       struct platform_device *pdev;
 831 +       struct input_dev *keys_dev;
 832 +       struct gpio_desc *keys_desc;
 833 +       struct work_struct keys_work_s;
 834 +       int keys_irq;
 835 +};
 836 +
 837 +
 838 +#endif