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

   1 From 157f59328885549a0f2c1fe35e9ec3aff0f0b52e 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] 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                     |   1 +
  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(+)
  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 @@ -211,3 +211,4 @@ obj-$(CONFIG_INTEL_SOC_PMIC)        += intel-so
 226  obj-$(CONFIG_MFD_MT6397)       += mt6397-core.o
 227
 228  obj-$(CONFIG_MFD_ALTERA_A10SR) += altera-a10sr.o
 229 +obj-$(CONFIG_MFD_RPISENSE_CORE)        += rpisense-core.o
 230 --- /dev/null
 231 +++ b/drivers/mfd/rpisense-core.c
 232 @@ -0,0 +1,157 @@
 233 +/*
 234 + * Raspberry Pi Sense HAT core driver
 235 + * http://raspberrypi.org
 236 + *
 237 + * Copyright (C) 2015 Raspberry Pi
 238 + *
 239 + * Author: Serge Schneider
 240 + *
 241 + *  This program is free software; you can redistribute  it and/or modify it
 242 + *  under  the terms of  the GNU General  Public License as published by the
 243 + *  Free Software Foundation;  either version 2 of the  License, or (at your
 244 + *  option) any later version.
 245 + *
 246 + *  This driver is based on wm8350 implementation.
 247 + */
 248 +
 249 +#include <linux/module.h>
 250 +#include <linux/moduleparam.h>
 251 +#include <linux/err.h>
 252 +#include <linux/init.h>
 253 +#include <linux/i2c.h>
 254 +#include <linux/platform_device.h>
 255 +#include <linux/mfd/rpisense/core.h>
 256 +#include <linux/slab.h>
 257 +
 258 +static struct rpisense *rpisense;
 259 +
 260 +static void rpisense_client_dev_register(struct rpisense *rpisense,
 261 +                                        const char *name,
 262 +                                        struct platform_device **pdev)
 263 +{
 264 +       int ret;
 265 +
 266 +       *pdev = platform_device_alloc(name, -1);
 267 +       if (*pdev == NULL) {
 268 +               dev_err(rpisense->dev, "Failed to allocate %s\n", name);
 269 +               return;
 270 +       }
 271 +
 272 +       (*pdev)->dev.parent = rpisense->dev;
 273 +       platform_set_drvdata(*pdev, rpisense);
 274 +       ret = platform_device_add(*pdev);
 275 +       if (ret != 0) {
 276 +               dev_err(rpisense->dev, "Failed to register %s: %d\n",
 277 +                       name, ret);
 278 +               platform_device_put(*pdev);
 279 +               *pdev = NULL;
 280 +       }
 281 +}
 282 +
 283 +static int rpisense_probe(struct i2c_client *i2c,
 284 +                              const struct i2c_device_id *id)
 285 +{
 286 +       int ret;
 287 +       struct rpisense_js *rpisense_js;
 288 +
 289 +       rpisense = devm_kzalloc(&i2c->dev, sizeof(struct rpisense), GFP_KERNEL);
 290 +       if (rpisense == NULL)
 291 +               return -ENOMEM;
 292 +
 293 +       i2c_set_clientdata(i2c, rpisense);
 294 +       rpisense->dev = &i2c->dev;
 295 +       rpisense->i2c_client = i2c;
 296 +
 297 +       ret = rpisense_reg_read(rpisense, RPISENSE_WAI);
 298 +       if (ret > 0) {
 299 +               if (ret != 's')
 300 +                       return -EINVAL;
 301 +       } else {
 302 +               return ret;
 303 +       }
 304 +       ret = rpisense_reg_read(rpisense, RPISENSE_VER);
 305 +       if (ret < 0)
 306 +               return ret;
 307 +
 308 +       dev_info(rpisense->dev,
 309 +                "Raspberry Pi Sense HAT firmware version %i\n", ret);
 310 +
 311 +       rpisense_js = &rpisense->joystick;
 312 +       rpisense_js->keys_desc = devm_gpiod_get(&i2c->dev,
 313 +                                               "keys-int", GPIOD_IN);
 314 +       if (IS_ERR(rpisense_js->keys_desc)) {
 315 +               dev_warn(&i2c->dev, "Failed to get keys-int descriptor.\n");
 316 +               rpisense_js->keys_desc = gpio_to_desc(23);
 317 +               if (rpisense_js->keys_desc == NULL) {
 318 +                       dev_err(&i2c->dev, "GPIO23 fallback failed.\n");
 319 +                       return PTR_ERR(rpisense_js->keys_desc);
 320 +               }
 321 +       }
 322 +       rpisense_client_dev_register(rpisense, "rpi-sense-js",
 323 +                                    &(rpisense->joystick.pdev));
 324 +       rpisense_client_dev_register(rpisense, "rpi-sense-fb",
 325 +                                    &(rpisense->framebuffer.pdev));
 326 +
 327 +       return 0;
 328 +}
 329 +
 330 +static int rpisense_remove(struct i2c_client *i2c)
 331 +{
 332 +       struct rpisense *rpisense = i2c_get_clientdata(i2c);
 333 +
 334 +       platform_device_unregister(rpisense->joystick.pdev);
 335 +       return 0;
 336 +}
 337 +
 338 +struct rpisense *rpisense_get_dev(void)
 339 +{
 340 +       return rpisense;
 341 +}
 342 +EXPORT_SYMBOL_GPL(rpisense_get_dev);
 343 +
 344 +s32 rpisense_reg_read(struct rpisense *rpisense, int reg)
 345 +{
 346 +       int ret = i2c_smbus_read_byte_data(rpisense->i2c_client, reg);
 347 +
 348 +       if (ret < 0)
 349 +               dev_err(rpisense->dev, "Read from reg %d failed\n", reg);
 350 +       /* Due to the BCM270x I2C clock stretching bug, some values
 351 +        * may have MSB set. Clear it to avoid incorrect values.
 352 +        * */
 353 +       return ret & 0x7F;
 354 +}
 355 +EXPORT_SYMBOL_GPL(rpisense_reg_read);
 356 +
 357 +int rpisense_block_write(struct rpisense *rpisense, const char *buf, int count)
 358 +{
 359 +       int ret = i2c_master_send(rpisense->i2c_client, buf, count);
 360 +
 361 +       if (ret < 0)
 362 +               dev_err(rpisense->dev, "Block write failed\n");
 363 +       return ret;
 364 +}
 365 +EXPORT_SYMBOL_GPL(rpisense_block_write);
 366 +
 367 +static const struct i2c_device_id rpisense_i2c_id[] = {
 368 +       { "rpi-sense", 0 },
 369 +       { }
 370 +};
 371 +MODULE_DEVICE_TABLE(i2c, rpisense_i2c_id);
 372 +
 373 +
 374 +static struct i2c_driver rpisense_driver = {
 375 +       .driver = {
 376 +                  .name = "rpi-sense",
 377 +                  .owner = THIS_MODULE,
 378 +       },
 379 +       .probe = rpisense_probe,
 380 +       .remove = rpisense_remove,
 381 +       .id_table = rpisense_i2c_id,
 382 +};
 383 +
 384 +module_i2c_driver(rpisense_driver);
 385 +
 386 +MODULE_DESCRIPTION("Raspberry Pi Sense HAT core driver");
 387 +MODULE_AUTHOR("Serge Schneider <serge@raspberrypi.org>");
 388 +MODULE_LICENSE("GPL");
 389 +
 390 --- a/drivers/video/fbdev/Kconfig
 391 +++ b/drivers/video/fbdev/Kconfig
 392 @@ -2504,3 +2504,16 @@ config FB_SM712
 393           This driver is also available as a module. The module will be
 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 --- a/drivers/video/fbdev/Makefile
 410 +++ b/drivers/video/fbdev/Makefile
 411 @@ -147,6 +147,7 @@ obj-$(CONFIG_FB_DA8XX)                += da8xx-fb.o
 412  obj-$(CONFIG_FB_MXS)             += mxsfb.o
 413  obj-$(CONFIG_FB_SSD1307)         += ssd1307fb.o
 414  obj-$(CONFIG_FB_SIMPLE)           += simplefb.o
 415 +obj-$(CONFIG_FB_RPISENSE)        += rpisense-fb.o
 416
 417  # the test framebuffer is last
 418  obj-$(CONFIG_FB_VIRTUAL)          += vfb.o
 419 --- /dev/null
 420 +++ b/drivers/video/fbdev/rpisense-fb.c
 421 @@ -0,0 +1,293 @@
 422 +/*
 423 + * Raspberry Pi Sense HAT framebuffer driver
 424 + * http://raspberrypi.org
 425 + *
 426 + * Copyright (C) 2015 Raspberry Pi
 427 + *
 428 + * Author: Serge Schneider
 429 + *
 430 + *  This program is free software; you can redistribute  it and/or modify it
 431 + *  under  the terms of  the GNU General  Public License as published by the
 432 + *  Free Software Foundation;  either version 2 of the  License, or (at your
 433 + *  option) any later version.
 434 + *
 435 + */
 436 +
 437 +#include <linux/module.h>
 438 +#include <linux/kernel.h>
 439 +#include <linux/errno.h>
 440 +#include <linux/string.h>
 441 +#include <linux/mm.h>
 442 +#include <linux/slab.h>
 443 +#include <linux/uaccess.h>
 444 +#include <linux/delay.h>
 445 +#include <linux/fb.h>
 446 +#include <linux/init.h>
 447 +
 448 +#include <linux/mfd/rpisense/framebuffer.h>
 449 +#include <linux/mfd/rpisense/core.h>
 450 +
 451 +static bool lowlight;
 452 +module_param(lowlight, bool, 0);
 453 +MODULE_PARM_DESC(lowlight, "Reduce LED matrix brightness to one third");
 454 +
 455 +static struct rpisense *rpisense;
 456 +
 457 +struct rpisense_fb_param {
 458 +       char __iomem *vmem;
 459 +       u8 *vmem_work;
 460 +       u32 vmemsize;
 461 +       u8 *gamma;
 462 +};
 463 +
 464 +static u8 gamma_default[32] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01,
 465 +                              0x02, 0x02, 0x03, 0x03, 0x04, 0x05, 0x06, 0x07,
 466 +                              0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0E, 0x0F, 0x11,
 467 +                              0x12, 0x14, 0x15, 0x17, 0x19, 0x1B, 0x1D, 0x1F,};
 468 +
 469 +static u8 gamma_low[32] = {0x00, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
 470 +                          0x01, 0x01, 0x01, 0x01, 0x01, 0x02, 0x02, 0x02,
 471 +                          0x03, 0x03, 0x03, 0x04, 0x04, 0x05, 0x05, 0x06,
 472 +                          0x06, 0x07, 0x07, 0x08, 0x08, 0x09, 0x0A, 0x0A,};
 473 +
 474 +static u8 gamma_user[32];
 475 +
 476 +static struct rpisense_fb_param rpisense_fb_param = {
 477 +       .vmem = NULL,
 478 +       .vmemsize = 128,
 479 +       .gamma = gamma_default,
 480 +};
 481 +
 482 +static struct fb_deferred_io rpisense_fb_defio;
 483 +
 484 +static struct fb_fix_screeninfo rpisense_fb_fix = {
 485 +       .id =           "RPi-Sense FB",
 486 +       .type =         FB_TYPE_PACKED_PIXELS,
 487 +       .visual =       FB_VISUAL_TRUECOLOR,
 488 +       .xpanstep =     0,
 489 +       .ypanstep =     0,
 490 +       .ywrapstep =    0,
 491 +       .accel =        FB_ACCEL_NONE,
 492 +       .line_length =  16,
 493 +};
 494 +
 495 +static struct fb_var_screeninfo rpisense_fb_var = {
 496 +       .xres           = 8,
 497 +       .yres           = 8,
 498 +       .xres_virtual   = 8,
 499 +       .yres_virtual   = 8,
 500 +       .bits_per_pixel = 16,
 501 +       .red            = {11, 5, 0},
 502 +       .green          = {5, 6, 0},
 503 +       .blue           = {0, 5, 0},
 504 +};
 505 +
 506 +static ssize_t rpisense_fb_write(struct fb_info *info,
 507 +                                const char __user *buf, size_t count,
 508 +                                loff_t *ppos)
 509 +{
 510 +       ssize_t res = fb_sys_write(info, buf, count, ppos);
 511 +
 512 +       schedule_delayed_work(&info->deferred_work, rpisense_fb_defio.delay);
 513 +       return res;
 514 +}
 515 +
 516 +static void rpisense_fb_fillrect(struct fb_info *info,
 517 +                                const struct fb_fillrect *rect)
 518 +{
 519 +       sys_fillrect(info, rect);
 520 +       schedule_delayed_work(&info->deferred_work, rpisense_fb_defio.delay);
 521 +}
 522 +
 523 +static void rpisense_fb_copyarea(struct fb_info *info,
 524 +                                const struct fb_copyarea *area)
 525 +{
 526 +       sys_copyarea(info, area);
 527 +       schedule_delayed_work(&info->deferred_work, rpisense_fb_defio.delay);
 528 +}
 529 +
 530 +static void rpisense_fb_imageblit(struct fb_info *info,
 531 +                                 const struct fb_image *image)
 532 +{
 533 +       sys_imageblit(info, image);
 534 +       schedule_delayed_work(&info->deferred_work, rpisense_fb_defio.delay);
 535 +}
 536 +
 537 +static void rpisense_fb_deferred_io(struct fb_info *info,
 538 +                               struct list_head *pagelist)
 539 +{
 540 +       int i;
 541 +       int j;
 542 +       u8 *vmem_work = rpisense_fb_param.vmem_work;
 543 +       u16 *mem = (u16 *)rpisense_fb_param.vmem;
 544 +       u8 *gamma = rpisense_fb_param.gamma;
 545 +
 546 +       vmem_work[0] = 0;
 547 +       for (j = 0; j < 8; j++) {
 548 +               for (i = 0; i < 8; i++) {
 549 +                       vmem_work[(j * 24) + i + 1] =
 550 +                               gamma[(mem[(j * 8) + i] >> 11) & 0x1F];
 551 +                       vmem_work[(j * 24) + (i + 8) + 1] =
 552 +                               gamma[(mem[(j * 8) + i] >> 6) & 0x1F];
 553 +                       vmem_work[(j * 24) + (i + 16) + 1] =
 554 +                               gamma[(mem[(j * 8) + i]) & 0x1F];
 555 +               }
 556 +       }
 557 +       rpisense_block_write(rpisense, vmem_work, 193);
 558 +}
 559 +
 560 +static struct fb_deferred_io rpisense_fb_defio = {
 561 +       .delay          = HZ/100,
 562 +       .deferred_io    = rpisense_fb_deferred_io,
 563 +};
 564 +
 565 +static int rpisense_fb_ioctl(struct fb_info *info, unsigned int cmd,
 566 +                            unsigned long arg)
 567 +{
 568 +       switch (cmd) {
 569 +       case SENSEFB_FBIOGET_GAMMA:
 570 +               if (copy_to_user((void __user *) arg, rpisense_fb_param.gamma,
 571 +                                sizeof(u8[32])))
 572 +                       return -EFAULT;
 573 +               return 0;
 574 +       case SENSEFB_FBIOSET_GAMMA:
 575 +               if (copy_from_user(gamma_user, (void __user *)arg,
 576 +                                  sizeof(u8[32])))
 577 +                       return -EFAULT;
 578 +               rpisense_fb_param.gamma = gamma_user;
 579 +               schedule_delayed_work(&info->deferred_work,
 580 +                                     rpisense_fb_defio.delay);
 581 +               return 0;
 582 +       case SENSEFB_FBIORESET_GAMMA:
 583 +               switch (arg) {
 584 +               case 0:
 585 +                       rpisense_fb_param.gamma = gamma_default;
 586 +                       break;
 587 +               case 1:
 588 +                       rpisense_fb_param.gamma = gamma_low;
 589 +                       break;
 590 +               case 2:
 591 +                       rpisense_fb_param.gamma = gamma_user;
 592 +                       break;
 593 +               default:
 594 +                       return -EINVAL;
 595 +               }
 596 +               schedule_delayed_work(&info->deferred_work,
 597 +                                     rpisense_fb_defio.delay);
 598 +               break;
 599 +       default:
 600 +               return -EINVAL;
 601 +       }
 602 +       return 0;
 603 +}
 604 +
 605 +static struct fb_ops rpisense_fb_ops = {
 606 +       .owner          = THIS_MODULE,
 607 +       .fb_read        = fb_sys_read,
 608 +       .fb_write       = rpisense_fb_write,
 609 +       .fb_fillrect    = rpisense_fb_fillrect,
 610 +       .fb_copyarea    = rpisense_fb_copyarea,
 611 +       .fb_imageblit   = rpisense_fb_imageblit,
 612 +       .fb_ioctl       = rpisense_fb_ioctl,
 613 +};
 614 +
 615 +static int rpisense_fb_probe(struct platform_device *pdev)
 616 +{
 617 +       struct fb_info *info;
 618 +       int ret = -ENOMEM;
 619 +       struct rpisense_fb *rpisense_fb;
 620 +
 621 +       rpisense = rpisense_get_dev();
 622 +       rpisense_fb = &rpisense->framebuffer;
 623 +
 624 +       rpisense_fb_param.vmem = vzalloc(rpisense_fb_param.vmemsize);
 625 +       if (!rpisense_fb_param.vmem)
 626 +               return ret;
 627 +
 628 +       rpisense_fb_param.vmem_work = devm_kmalloc(&pdev->dev, 193, GFP_KERNEL);
 629 +       if (!rpisense_fb_param.vmem_work)
 630 +               goto err_malloc;
 631 +
 632 +       info = framebuffer_alloc(0, &pdev->dev);
 633 +       if (!info) {
 634 +               dev_err(&pdev->dev, "Could not allocate framebuffer.\n");
 635 +               goto err_malloc;
 636 +       }
 637 +       rpisense_fb->info = info;
 638 +
 639 +       rpisense_fb_fix.smem_start = (unsigned long)rpisense_fb_param.vmem;
 640 +       rpisense_fb_fix.smem_len = rpisense_fb_param.vmemsize;
 641 +
 642 +       info->fbops = &rpisense_fb_ops;
 643 +       info->fix = rpisense_fb_fix;
 644 +       info->var = rpisense_fb_var;
 645 +       info->fbdefio = &rpisense_fb_defio;
 646 +       info->flags = FBINFO_FLAG_DEFAULT | FBINFO_VIRTFB;
 647 +       info->screen_base = rpisense_fb_param.vmem;
 648 +       info->screen_size = rpisense_fb_param.vmemsize;
 649 +
 650 +       if (lowlight)
 651 +               rpisense_fb_param.gamma = gamma_low;
 652 +
 653 +       fb_deferred_io_init(info);
 654 +
 655 +       ret = register_framebuffer(info);
 656 +       if (ret < 0) {
 657 +               dev_err(&pdev->dev, "Could not register framebuffer.\n");
 658 +               goto err_fballoc;
 659 +       }
 660 +
 661 +       fb_info(info, "%s frame buffer device\n", info->fix.id);
 662 +       schedule_delayed_work(&info->deferred_work, rpisense_fb_defio.delay);
 663 +       return 0;
 664 +err_fballoc:
 665 +       framebuffer_release(info);
 666 +err_malloc:
 667 +       vfree(rpisense_fb_param.vmem);
 668 +       return ret;
 669 +}
 670 +
 671 +static int rpisense_fb_remove(struct platform_device *pdev)
 672 +{
 673 +       struct rpisense_fb *rpisense_fb = &rpisense->framebuffer;
 674 +       struct fb_info *info = rpisense_fb->info;
 675 +
 676 +       if (info) {
 677 +               unregister_framebuffer(info);
 678 +               fb_deferred_io_cleanup(info);
 679 +               framebuffer_release(info);
 680 +               vfree(rpisense_fb_param.vmem);
 681 +       }
 682 +
 683 +       return 0;
 684 +}
 685 +
 686 +#ifdef CONFIG_OF
 687 +static const struct of_device_id rpisense_fb_id[] = {
 688 +       { .compatible = "rpi,rpi-sense-fb" },
 689 +       { },
 690 +};
 691 +MODULE_DEVICE_TABLE(of, rpisense_fb_id);
 692 +#endif
 693 +
 694 +static struct platform_device_id rpisense_fb_device_id[] = {
 695 +       { .name = "rpi-sense-fb" },
 696 +       { },
 697 +};
 698 +MODULE_DEVICE_TABLE(platform, rpisense_fb_device_id);
 699 +
 700 +static struct platform_driver rpisense_fb_driver = {
 701 +       .probe = rpisense_fb_probe,
 702 +       .remove = rpisense_fb_remove,
 703 +       .driver = {
 704 +               .name = "rpi-sense-fb",
 705 +               .owner = THIS_MODULE,
 706 +       },
 707 +};
 708 +
 709 +module_platform_driver(rpisense_fb_driver);
 710 +
 711 +MODULE_DESCRIPTION("Raspberry Pi Sense HAT framebuffer driver");
 712 +MODULE_AUTHOR("Serge Schneider <serge@raspberrypi.org>");
 713 +MODULE_LICENSE("GPL");
 714 +
 715 --- /dev/null
 716 +++ b/include/linux/mfd/rpisense/core.h
 717 @@ -0,0 +1,47 @@
 718 +/*
 719 + * Raspberry Pi Sense HAT core driver
 720 + * http://raspberrypi.org
 721 + *
 722 + * Copyright (C) 2015 Raspberry Pi
 723 + *
 724 + * Author: Serge Schneider
 725 + *
 726 + *  This program is free software; you can redistribute  it and/or modify it
 727 + *  under  the terms of  the GNU General  Public License as published by the
 728 + *  Free Software Foundation;  either version 2 of the  License, or (at your
 729 + *  option) any later version.
 730 + *
 731 + */
 732 +
 733 +#ifndef __LINUX_MFD_RPISENSE_CORE_H_
 734 +#define __LINUX_MFD_RPISENSE_CORE_H_
 735 +
 736 +#include <linux/mfd/rpisense/joystick.h>
 737 +#include <linux/mfd/rpisense/framebuffer.h>
 738 +
 739 +/*
 740 + * Register values.
 741 + */
 742 +#define RPISENSE_FB                    0x00
 743 +#define RPISENSE_WAI                   0xF0
 744 +#define RPISENSE_VER                   0xF1
 745 +#define RPISENSE_KEYS                  0xF2
 746 +#define RPISENSE_EE_WP                 0xF3
 747 +
 748 +#define RPISENSE_ID                    's'
 749 +
 750 +struct rpisense {
 751 +       struct device *dev;
 752 +       struct i2c_client *i2c_client;
 753 +
 754 +       /* Client devices */
 755 +       struct rpisense_js joystick;
 756 +       struct rpisense_fb framebuffer;
 757 +};
 758 +
 759 +struct rpisense *rpisense_get_dev(void);
 760 +s32 rpisense_reg_read(struct rpisense *rpisense, int reg);
 761 +int rpisense_reg_write(struct rpisense *rpisense, int reg, u16 val);
 762 +int rpisense_block_write(struct rpisense *rpisense, const char *buf, int count);
 763 +
 764 +#endif
 765 --- /dev/null
 766 +++ b/include/linux/mfd/rpisense/framebuffer.h
 767 @@ -0,0 +1,32 @@
 768 +/*
 769 + * Raspberry Pi Sense HAT framebuffer driver
 770 + * http://raspberrypi.org
 771 + *
 772 + * Copyright (C) 2015 Raspberry Pi
 773 + *
 774 + * Author: Serge Schneider
 775 + *
 776 + *  This program is free software; you can redistribute  it and/or modify it
 777 + *  under  the terms of  the GNU General  Public License as published by the
 778 + *  Free Software Foundation;  either version 2 of the  License, or (at your
 779 + *  option) any later version.
 780 + *
 781 + */
 782 +
 783 +#ifndef __LINUX_RPISENSE_FB_H_
 784 +#define __LINUX_RPISENSE_FB_H_
 785 +
 786 +#define SENSEFB_FBIO_IOC_MAGIC 0xF1
 787 +
 788 +#define SENSEFB_FBIOGET_GAMMA _IO(SENSEFB_FBIO_IOC_MAGIC, 0)
 789 +#define SENSEFB_FBIOSET_GAMMA _IO(SENSEFB_FBIO_IOC_MAGIC, 1)
 790 +#define SENSEFB_FBIORESET_GAMMA _IO(SENSEFB_FBIO_IOC_MAGIC, 2)
 791 +
 792 +struct rpisense;
 793 +
 794 +struct rpisense_fb {
 795 +       struct platform_device *pdev;
 796 +       struct fb_info *info;
 797 +};
 798 +
 799 +#endif
 800 --- /dev/null
 801 +++ b/include/linux/mfd/rpisense/joystick.h
 802 @@ -0,0 +1,35 @@
 803 +/*
 804 + * Raspberry Pi Sense HAT joystick driver
 805 + * http://raspberrypi.org
 806 + *
 807 + * Copyright (C) 2015 Raspberry Pi
 808 + *
 809 + * Author: Serge Schneider
 810 + *
 811 + *  This program is free software; you can redistribute  it and/or modify it
 812 + *  under  the terms of  the GNU General  Public License as published by the
 813 + *  Free Software Foundation;  either version 2 of the  License, or (at your
 814 + *  option) any later version.
 815 + *
 816 + */
 817 +
 818 +#ifndef __LINUX_RPISENSE_JOYSTICK_H_
 819 +#define __LINUX_RPISENSE_JOYSTICK_H_
 820 +
 821 +#include <linux/input.h>
 822 +#include <linux/interrupt.h>
 823 +#include <linux/gpio/consumer.h>
 824 +#include <linux/platform_device.h>
 825 +
 826 +struct rpisense;
 827 +
 828 +struct rpisense_js {
 829 +       struct platform_device *pdev;
 830 +       struct input_dev *keys_dev;
 831 +       struct gpio_desc *keys_desc;
 832 +       struct work_struct keys_work_s;
 833 +       int keys_irq;
 834 +};
 835 +
 836 +
 837 +#endif