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

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