target/linux/generic/files/drivers/net/phy/swconfig_leds.c

   1 /*
   2  * swconfig_led.c: LED trigger support for the switch configuration API
   3  *
   4  * Copyright (C) 2011 Gabor Juhos <juhosg@openwrt.org>
   5  *
   6  * This program is free software; you can redistribute it and/or
   7  * modify it under the terms of the GNU General Public License
   8  * as published by the Free Software Foundation; either version 2
   9  * of the License, or (at your option) any later version.
  10  *
  11  */
  12
  13 #ifdef CONFIG_SWCONFIG_LEDS
  14
  15 #include <linux/leds.h>
  16 #include <linux/ctype.h>
  17 #include <linux/device.h>
  18 #include <linux/workqueue.h>
  19
  20 #define SWCONFIG_LED_TIMER_INTERVAL     (HZ / 10)
  21 #define SWCONFIG_LED_NUM_PORTS          32
  22
  23 struct switch_led_trigger {
  24         struct led_trigger trig;
  25         struct switch_dev *swdev;
  26
  27         struct delayed_work sw_led_work;
  28         u32 port_mask;
  29         u32 port_link;
  30         unsigned long port_traffic[SWCONFIG_LED_NUM_PORTS];
  31 };
  32
  33 struct swconfig_trig_data {
  34         struct led_classdev *led_cdev;
  35         struct switch_dev *swdev;
  36
  37         rwlock_t lock;
  38         u32 port_mask;
  39
  40         bool prev_link;
  41         unsigned long prev_traffic;
  42         enum led_brightness prev_brightness;
  43 };
  44
  45 static void
  46 swconfig_trig_set_brightness(struct swconfig_trig_data *trig_data,
  47                              enum led_brightness brightness)
  48 {
  49         led_set_brightness(trig_data->led_cdev, brightness);
  50         trig_data->prev_brightness = brightness;
  51 }
  52
  53 static void
  54 swconfig_trig_update_port_mask(struct led_trigger *trigger)
  55 {
  56         struct list_head *entry;
  57         struct switch_led_trigger *sw_trig;
  58         u32 port_mask;
  59
  60         if (!trigger)
  61                 return;
  62
  63         sw_trig = (void *) trigger;
  64
  65         port_mask = 0;
  66         read_lock(&trigger->leddev_list_lock);
  67         list_for_each(entry, &trigger->led_cdevs) {
  68                 struct led_classdev *led_cdev;
  69                 struct swconfig_trig_data *trig_data;
  70
  71                 led_cdev = list_entry(entry, struct led_classdev, trig_list);
  72                 trig_data = led_cdev->trigger_data;
  73                 if (trig_data) {
  74                         read_lock(&trig_data->lock);
  75                         port_mask |= trig_data->port_mask;
  76                         read_unlock(&trig_data->lock);
  77                 }
  78         }
  79         read_unlock(&trigger->leddev_list_lock);
  80
  81         sw_trig->port_mask = port_mask;
  82
  83         if (port_mask)
  84                 schedule_delayed_work(&sw_trig->sw_led_work,
  85                                       SWCONFIG_LED_TIMER_INTERVAL);
  86         else
  87                 cancel_delayed_work_sync(&sw_trig->sw_led_work);
  88 }
  89
  90 static ssize_t
  91 swconfig_trig_port_mask_store(struct device *dev, struct device_attribute *attr,
  92                               const char *buf, size_t size)
  93 {
  94         struct led_classdev *led_cdev = dev_get_drvdata(dev);
  95         struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
  96         unsigned long port_mask;
  97         ssize_t ret = -EINVAL;
  98         char *after;
  99         size_t count;
 100
 101         port_mask = simple_strtoul(buf, &after, 16);
 102         count = after - buf;
 103
 104         if (*after && isspace(*after))
 105                 count++;
 106
 107         if (count == size) {
 108                 bool changed;
 109
 110                 write_lock(&trig_data->lock);
 111
 112                 changed = (trig_data->port_mask != port_mask);
 113                 if (changed) {
 114                         trig_data->port_mask = port_mask;
 115                         if (port_mask == 0)
 116                                 swconfig_trig_set_brightness(trig_data, LED_OFF);
 117                 }
 118
 119                 write_unlock(&trig_data->lock);
 120
 121                 if (changed)
 122                         swconfig_trig_update_port_mask(led_cdev->trigger);
 123
 124                 ret = count;
 125         }
 126
 127         return ret;
 128 }
 129
 130 static ssize_t
 131 swconfig_trig_port_mask_show(struct device *dev, struct device_attribute *attr,
 132                              char *buf)
 133 {
 134         struct led_classdev *led_cdev = dev_get_drvdata(dev);
 135         struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
 136
 137         read_lock(&trig_data->lock);
 138         sprintf(buf, "%#x\n", trig_data->port_mask);
 139         read_unlock(&trig_data->lock);
 140
 141         return strlen(buf) + 1;
 142 }
 143
 144 static DEVICE_ATTR(port_mask, 0644, swconfig_trig_port_mask_show,
 145                    swconfig_trig_port_mask_store);
 146
 147 static void
 148 swconfig_trig_activate(struct led_classdev *led_cdev)
 149 {
 150         struct switch_led_trigger *sw_trig;
 151         struct swconfig_trig_data *trig_data;
 152         int err;
 153
 154         if (led_cdev->trigger->activate != swconfig_trig_activate)
 155                 return;
 156
 157         trig_data = kzalloc(sizeof(struct swconfig_trig_data), GFP_KERNEL);
 158         if (!trig_data)
 159                 return;
 160
 161         sw_trig = (void *) led_cdev->trigger;
 162
 163         rwlock_init(&trig_data->lock);
 164         trig_data->led_cdev = led_cdev;
 165         trig_data->swdev = sw_trig->swdev;
 166         led_cdev->trigger_data = trig_data;
 167
 168         err = device_create_file(led_cdev->dev, &dev_attr_port_mask);
 169         if (err)
 170                 goto err_free;
 171
 172         return;
 173
 174 err_free:
 175         led_cdev->trigger_data = NULL;
 176         kfree(trig_data);
 177 }
 178
 179 static void
 180 swconfig_trig_deactivate(struct led_classdev *led_cdev)
 181 {
 182         struct swconfig_trig_data *trig_data;
 183
 184         swconfig_trig_update_port_mask(led_cdev->trigger);
 185
 186         trig_data = (void *) led_cdev->trigger_data;
 187         if (trig_data) {
 188                 device_remove_file(led_cdev->dev, &dev_attr_port_mask);
 189                 kfree(trig_data);
 190         }
 191 }
 192
 193 static void
 194 swconfig_trig_led_event(struct switch_led_trigger *sw_trig,
 195                         struct led_classdev *led_cdev)
 196 {
 197         struct swconfig_trig_data *trig_data;
 198         u32 port_mask;
 199         bool link;
 200
 201         trig_data = led_cdev->trigger_data;
 202         if (!trig_data)
 203                 return;
 204
 205         read_lock(&trig_data->lock);
 206         port_mask = trig_data->port_mask;
 207         read_unlock(&trig_data->lock);
 208
 209         link = !!(sw_trig->port_link & port_mask);
 210         if (!link) {
 211                 if (link != trig_data->prev_link)
 212                         led_set_brightness(trig_data->led_cdev, LED_OFF);
 213         } else {
 214                 unsigned long traffic;
 215                 int i;
 216
 217                 traffic = 0;
 218                 for (i = 0; i < SWCONFIG_LED_NUM_PORTS; i++) {
 219                         if (port_mask & (1 << i))
 220                                 traffic += sw_trig->port_traffic[i];
 221                 }
 222
 223                 if (trig_data->prev_brightness != LED_FULL)
 224                         swconfig_trig_set_brightness(trig_data, LED_FULL);
 225                 else if (traffic != trig_data->prev_traffic)
 226                         swconfig_trig_set_brightness(trig_data, LED_OFF);
 227
 228                 trig_data->prev_traffic = traffic;
 229         }
 230
 231         trig_data->prev_link = link;
 232 }
 233
 234 static void
 235 swconfig_trig_update_leds(struct switch_led_trigger *sw_trig)
 236 {
 237         struct list_head *entry;
 238         struct led_trigger *trigger;
 239
 240         trigger = &sw_trig->trig;
 241         read_lock(&trigger->leddev_list_lock);
 242         list_for_each(entry, &trigger->led_cdevs) {
 243                 struct led_classdev *led_cdev;
 244
 245                 led_cdev = list_entry(entry, struct led_classdev, trig_list);
 246                 swconfig_trig_led_event(sw_trig, led_cdev);
 247         }
 248         read_unlock(&trigger->leddev_list_lock);
 249 }
 250
 251 static void
 252 swconfig_led_work_func(struct work_struct *work)
 253 {
 254         struct switch_led_trigger *sw_trig;
 255         struct switch_dev *swdev;
 256         u32 port_mask;
 257         u32 link;
 258         int i;
 259
 260         sw_trig = container_of(work, struct switch_led_trigger,
 261                                sw_led_work.work);
 262
 263         port_mask = sw_trig->port_mask;
 264         swdev = sw_trig->swdev;
 265
 266         link = 0;
 267         for (i = 0; i < SWCONFIG_LED_NUM_PORTS; i++) {
 268                 u32 port_bit;
 269
 270                 port_bit = BIT(i);
 271                 if ((port_mask & port_bit) == 0)
 272                         continue;
 273
 274                 if (swdev->ops->get_port_link) {
 275                         struct switch_port_link port_link;
 276
 277                         memset(&port_link, '\0', sizeof(port_link));
 278                         swdev->ops->get_port_link(swdev, i, &port_link);
 279
 280                         if (port_link.link)
 281                                 link |= port_bit;
 282                 }
 283
 284                 if (swdev->ops->get_port_stats) {
 285                         struct switch_port_stats port_stats;
 286
 287                         memset(&port_stats, '\0', sizeof(port_stats));
 288                         swdev->ops->get_port_stats(swdev, i, &port_stats);
 289                         sw_trig->port_traffic[i] = port_stats.tx_bytes +
 290                                                    port_stats.rx_bytes;
 291                 }
 292         }
 293
 294         sw_trig->port_link = link;
 295
 296         swconfig_trig_update_leds(sw_trig);
 297
 298         schedule_delayed_work(&sw_trig->sw_led_work,
 299                               SWCONFIG_LED_TIMER_INTERVAL);
 300 }
 301
 302 static int
 303 swconfig_create_led_trigger(struct switch_dev *swdev)
 304 {
 305         struct switch_led_trigger *sw_trig;
 306         int err;
 307
 308         if (!swdev->ops->get_port_link)
 309                 return 0;
 310
 311         sw_trig = kzalloc(sizeof(struct switch_led_trigger), GFP_KERNEL);
 312         if (!sw_trig)
 313                 return -ENOMEM;
 314
 315         sw_trig->swdev = swdev;
 316         sw_trig->trig.name = swdev->devname;
 317         sw_trig->trig.activate = swconfig_trig_activate;
 318         sw_trig->trig.deactivate = swconfig_trig_deactivate;
 319
 320         INIT_DELAYED_WORK(&sw_trig->sw_led_work, swconfig_led_work_func);
 321
 322         err = led_trigger_register(&sw_trig->trig);
 323         if (err)
 324                 goto err_free;
 325
 326         swdev->led_trigger = sw_trig;
 327
 328         return 0;
 329
 330 err_free:
 331         kfree(sw_trig);
 332         return err;
 333 }
 334
 335 static void
 336 swconfig_destroy_led_trigger(struct switch_dev *swdev)
 337 {
 338         struct switch_led_trigger *sw_trig;
 339
 340         sw_trig = swdev->led_trigger;
 341         if (sw_trig) {
 342                 cancel_delayed_work_sync(&sw_trig->sw_led_work);
 343                 led_trigger_unregister(&sw_trig->trig);
 344                 kfree(sw_trig);
 345         }
 346 }
 347
 348 #else /* SWCONFIG_LEDS */
 349 static inline int
 350 swconfig_create_led_trigger(struct switch_dev *swdev) { return 0; }
 351
 352 static inline void
 353 swconfig_destroy_led_trigger(struct switch_dev *swdev) { }
 354 #endif /* CONFIG_SWCONFIG_LEDS */