target/linux/bcm27xx/patches-5.15/950-0083-mfd-Add-Raspberry-Pi-Sense-HAT-core-driver.patch

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