target/linux/lantiq/patches-2.6.39/140-falcon-easy98000-cpld-led.patch

   1 --- a/arch/mips/lantiq/falcon/Makefile
   2 +++ b/arch/mips/lantiq/falcon/Makefile
   3 @@ -1,3 +1,4 @@
   4  obj-y := clk-falcon.o devices.o gpio.o prom.o sysctrl.o reset.o
   5  obj-y += softdog_vpe.o
   6  obj-$(CONFIG_LANTIQ_MACH_EASY98000) += mach-easy98000.o
   7 +obj-$(CONFIG_LANTIQ_MACH_EASY98000) += dev-leds-easy98000-cpld.o
   8 --- /dev/null
   9 +++ b/arch/mips/lantiq/falcon/dev-leds-easy98000-cpld.c
  10 @@ -0,0 +1,160 @@
  11 +/*
  12 + *  EASY98000 CPLD LED driver
  13 + *
  14 + *  Copyright (C) 2010 Ralph Hempel <ralph.hempel@lantiq.com>
  15 + *
  16 + *  This program is free software; you can redistribute it and/or modify it
  17 + *  under the terms of the GNU General Public License version 2  as published
  18 + *  by the Free Software Foundation.
  19 + *
  20 + */
  21 +
  22 +#include <linux/kernel.h>
  23 +#include <linux/version.h>
  24 +#include <linux/types.h>
  25 +#include <linux/init.h>
  26 +#include <linux/platform_device.h>
  27 +#include <linux/errno.h>
  28 +#include <linux/leds.h>
  29 +#include <linux/slab.h>
  30 +
  31 +#include "dev-leds-easy98000-cpld.h"
  32 +
  33 +const char *led_name[8] = {
  34 +       "ge0_act",
  35 +       "ge0_link",
  36 +       "ge1_act",
  37 +       "ge1_link",
  38 +       "fe2_act",
  39 +       "fe2_link",
  40 +       "fe3_act",
  41 +       "fe3_link"
  42 +};
  43 +
  44 +#define cpld_base7                     ((u16 *)(KSEG1 | 0x17c0000c))
  45 +#define cpld_base8                     ((u16 *)(KSEG1 | 0x17c00012))
  46 +
  47 +#define ltq_r16(reg)                   __raw_readw(reg)
  48 +#define ltq_w16(val, reg)              __raw_writew(val, reg)
  49 +
  50 +struct cpld_led_dev {
  51 +       struct led_classdev     cdev;
  52 +       u8                      mask;
  53 +       u16                     *base;
  54 +};
  55 +
  56 +struct cpld_led_drvdata {
  57 +       struct cpld_led_dev     *led_devs;
  58 +       int                     num_leds;
  59 +};
  60 +
  61 +void led_set(u8 mask, u16 *base)
  62 +{
  63 +       ltq_w16(ltq_r16(base) | mask, base);
  64 +}
  65 +
  66 +void led_clear(u8 mask, u16 *base)
  67 +{
  68 +       ltq_w16(ltq_r16(base) & (~mask), base);
  69 +}
  70 +
  71 +void led_blink_clear(u8 mask, u16 *base)
  72 +{
  73 +       led_clear(mask, base);
  74 +}
  75 +
  76 +static void led_brightness(struct led_classdev *led_cdev,
  77 +                              enum led_brightness value)
  78 +{
  79 +       struct cpld_led_dev *led_dev =
  80 +           container_of(led_cdev, struct cpld_led_dev, cdev);
  81 +
  82 +       if (value)
  83 +               led_set(led_dev->mask, led_dev->base);
  84 +       else
  85 +               led_clear(led_dev->mask, led_dev->base);
  86 +}
  87 +
  88 +static int led_probe(struct platform_device *pdev)
  89 +{
  90 +       int i;
  91 +       char name[32];
  92 +       struct cpld_led_drvdata *drvdata;
  93 +       int ret = 0;
  94 +
  95 +       drvdata = kzalloc(sizeof(struct cpld_led_drvdata) +
  96 +                         sizeof(struct cpld_led_dev) * MAX_LED,
  97 +                         GFP_KERNEL);
  98 +       if (!drvdata)
  99 +               return -ENOMEM;
 100 +
 101 +       drvdata->led_devs = (struct cpld_led_dev *) &drvdata[1];
 102 +
 103 +       for (i = 0; i < MAX_LED; i++) {
 104 +               struct cpld_led_dev *led_dev = &drvdata->led_devs[i];
 105 +               led_dev->cdev.brightness_set = led_brightness;
 106 +               led_dev->cdev.default_trigger = NULL;
 107 +               led_dev->mask = 1 << (i % 8);
 108 +               if(i < 8) {
 109 +                       sprintf(name, "easy98000-cpld:%s", led_name[i]);
 110 +                       led_dev->base = cpld_base8;
 111 +               } else {
 112 +                       sprintf(name, "easy98000-cpld:red:%d", i-8);
 113 +                       led_dev->base = cpld_base7;
 114 +               }
 115 +               led_dev->cdev.name = name;
 116 +               ret = led_classdev_register(&pdev->dev, &led_dev->cdev);
 117 +               if (ret)
 118 +                       goto err;
 119 +       }
 120 +       platform_set_drvdata(pdev, drvdata);
 121 +       return 0;
 122 +
 123 +err:
 124 +       printk("led_probe: 3\n");
 125 +       for (i = i - 1; i >= 0; i--)
 126 +               led_classdev_unregister(&drvdata->led_devs[i].cdev);
 127 +
 128 +       kfree(drvdata);
 129 +       return ret;
 130 +}
 131 +
 132 +static int led_remove(struct platform_device *pdev)
 133 +{
 134 +       int i;
 135 +       struct cpld_led_drvdata *drvdata = platform_get_drvdata(pdev);
 136 +       for (i = 0; i < MAX_LED; i++)
 137 +               led_classdev_unregister(&drvdata->led_devs[i].cdev);
 138 +       kfree(drvdata);
 139 +       return 0;
 140 +}
 141 +
 142 +static struct platform_driver led_driver = {
 143 +       .probe = led_probe,
 144 +       .remove = led_remove,
 145 +       .driver = {
 146 +                  .name = LED_NAME,
 147 +                  .owner = THIS_MODULE,
 148 +                  },
 149 +};
 150 +
 151 +int __init easy98000_cpld_led_init(void)
 152 +{
 153 +       pr_info(LED_DESC ", Version " LED_VERSION
 154 +               " (c) Copyright 2011, Lantiq Deutschland GmbH\n");
 155 +       return platform_driver_register(&led_driver);
 156 +}
 157 +
 158 +void __exit easy98000_cpld_led_exit(void)
 159 +{
 160 +       platform_driver_unregister(&led_driver);
 161 +}
 162 +
 163 +module_init(easy98000_cpld_led_init);
 164 +module_exit(easy98000_cpld_led_exit);
 165 +
 166 +MODULE_DESCRIPTION(LED_NAME);
 167 +MODULE_DESCRIPTION(LED_DESC);
 168 +MODULE_AUTHOR("Ralph Hempel <ralph.hempel@lantiq.com>");
 169 +MODULE_LICENSE("GPL v2");
 170 +
 171 --- /dev/null
 172 +++ b/arch/mips/lantiq/falcon/dev-leds-easy98000-cpld.h
 173 @@ -0,0 +1,20 @@
 174 +/*
 175 + *  EASY98000 CPLD LED driver
 176 + *
 177 + *  Copyright (C) 2010 Ralph Hempel <ralph.hempel@lantiq.com>
 178 + *
 179 + *  This program is free software; you can redistribute it and/or modify it
 180 + *  under the terms of the GNU General Public License version 2  as published
 181 + *  by the Free Software Foundation.
 182 + *
 183 + */
 184 +#ifndef _INCLUDE_EASY98000_CPLD_LED_H_
 185 +#define _INCLUDE_EASY98000_CPLD_LED_H_
 186 +
 187 +#define LED_NAME       "easy98000_cpld_led"
 188 +#define LED_DESC       "EASY98000 LED driver"
 189 +#define LED_VERSION    "1.0.0"
 190 +
 191 +#define MAX_LED                16
 192 +
 193 +#endif /* _INCLUDE_EASY98000_CPLD_LED_H_ */