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         int ret;
  98         bool changed;
  99
 100         ret = kstrtoul(buf, 0, &port_mask);
 101         if (ret)
 102                 return ret;
 103
 104         write_lock(&trig_data->lock);
 105
 106         changed = (trig_data->port_mask != port_mask);
 107         if (changed) {
 108                 trig_data->port_mask = port_mask;
 109                 if (port_mask == 0)
 110                         swconfig_trig_set_brightness(trig_data, LED_OFF);
 111         }
 112
 113         write_unlock(&trig_data->lock);
 114
 115         if (changed)
 116                 swconfig_trig_update_port_mask(led_cdev->trigger);
 117
 118         return size;
 119 }
 120
 121 static ssize_t
 122 swconfig_trig_port_mask_show(struct device *dev, struct device_attribute *attr,
 123                              char *buf)
 124 {
 125         struct led_classdev *led_cdev = dev_get_drvdata(dev);
 126         struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
 127
 128         read_lock(&trig_data->lock);
 129         sprintf(buf, "%#x\n", trig_data->port_mask);
 130         read_unlock(&trig_data->lock);
 131
 132         return strlen(buf) + 1;
 133 }
 134
 135 static DEVICE_ATTR(port_mask, 0644, swconfig_trig_port_mask_show,
 136                    swconfig_trig_port_mask_store);
 137
 138 static void
 139 swconfig_trig_activate(struct led_classdev *led_cdev)
 140 {
 141         struct switch_led_trigger *sw_trig;
 142         struct swconfig_trig_data *trig_data;
 143         int err;
 144
 145         if (led_cdev->trigger->activate != swconfig_trig_activate)
 146                 return;
 147
 148         trig_data = kzalloc(sizeof(struct swconfig_trig_data), GFP_KERNEL);
 149         if (!trig_data)
 150                 return;
 151
 152         sw_trig = (void *) led_cdev->trigger;
 153
 154         rwlock_init(&trig_data->lock);
 155         trig_data->led_cdev = led_cdev;
 156         trig_data->swdev = sw_trig->swdev;
 157         led_cdev->trigger_data = trig_data;
 158
 159         err = device_create_file(led_cdev->dev, &dev_attr_port_mask);
 160         if (err)
 161                 goto err_free;
 162
 163         return;
 164
 165 err_free:
 166         led_cdev->trigger_data = NULL;
 167         kfree(trig_data);
 168 }
 169
 170 static void
 171 swconfig_trig_deactivate(struct led_classdev *led_cdev)
 172 {
 173         struct swconfig_trig_data *trig_data;
 174
 175         swconfig_trig_update_port_mask(led_cdev->trigger);
 176
 177         trig_data = (void *) led_cdev->trigger_data;
 178         if (trig_data) {
 179                 device_remove_file(led_cdev->dev, &dev_attr_port_mask);
 180                 kfree(trig_data);
 181         }
 182 }
 183
 184 static void
 185 swconfig_trig_led_event(struct switch_led_trigger *sw_trig,
 186                         struct led_classdev *led_cdev)
 187 {
 188         struct swconfig_trig_data *trig_data;
 189         u32 port_mask;
 190         bool link;
 191
 192         trig_data = led_cdev->trigger_data;
 193         if (!trig_data)
 194                 return;
 195
 196         read_lock(&trig_data->lock);
 197         port_mask = trig_data->port_mask;
 198         read_unlock(&trig_data->lock);
 199
 200         link = !!(sw_trig->port_link & port_mask);
 201         if (!link) {
 202                 if (link != trig_data->prev_link)
 203                         swconfig_trig_set_brightness(trig_data, LED_OFF);
 204         } else {
 205                 unsigned long traffic;
 206                 int i;
 207
 208                 traffic = 0;
 209                 for (i = 0; i < SWCONFIG_LED_NUM_PORTS; i++) {
 210                         if (port_mask & (1 << i))
 211                                 traffic += sw_trig->port_traffic[i];
 212                 }
 213
 214                 if (trig_data->prev_brightness != LED_FULL)
 215                         swconfig_trig_set_brightness(trig_data, LED_FULL);
 216                 else if (traffic != trig_data->prev_traffic)
 217                         swconfig_trig_set_brightness(trig_data, LED_OFF);
 218
 219                 trig_data->prev_traffic = traffic;
 220         }
 221
 222         trig_data->prev_link = link;
 223 }
 224
 225 static void
 226 swconfig_trig_update_leds(struct switch_led_trigger *sw_trig)
 227 {
 228         struct list_head *entry;
 229         struct led_trigger *trigger;
 230
 231         trigger = &sw_trig->trig;
 232         read_lock(&trigger->leddev_list_lock);
 233         list_for_each(entry, &trigger->led_cdevs) {
 234                 struct led_classdev *led_cdev;
 235
 236                 led_cdev = list_entry(entry, struct led_classdev, trig_list);
 237                 swconfig_trig_led_event(sw_trig, led_cdev);
 238         }
 239         read_unlock(&trigger->leddev_list_lock);
 240 }
 241
 242 static void
 243 swconfig_led_work_func(struct work_struct *work)
 244 {
 245         struct switch_led_trigger *sw_trig;
 246         struct switch_dev *swdev;
 247         u32 port_mask;
 248         u32 link;
 249         int i;
 250
 251         sw_trig = container_of(work, struct switch_led_trigger,
 252                                sw_led_work.work);
 253
 254         port_mask = sw_trig->port_mask;
 255         swdev = sw_trig->swdev;
 256
 257         link = 0;
 258         for (i = 0; i < SWCONFIG_LED_NUM_PORTS; i++) {
 259                 u32 port_bit;
 260
 261                 port_bit = BIT(i);
 262                 if ((port_mask & port_bit) == 0)
 263                         continue;
 264
 265                 if (swdev->ops->get_port_link) {
 266                         struct switch_port_link port_link;
 267
 268                         memset(&port_link, '\0', sizeof(port_link));
 269                         swdev->ops->get_port_link(swdev, i, &port_link);
 270
 271                         if (port_link.link)
 272                                 link |= port_bit;
 273                 }
 274
 275                 if (swdev->ops->get_port_stats) {
 276                         struct switch_port_stats port_stats;
 277
 278                         memset(&port_stats, '\0', sizeof(port_stats));
 279                         swdev->ops->get_port_stats(swdev, i, &port_stats);
 280                         sw_trig->port_traffic[i] = port_stats.tx_bytes +
 281                                                    port_stats.rx_bytes;
 282                 }
 283         }
 284
 285         sw_trig->port_link = link;
 286
 287         swconfig_trig_update_leds(sw_trig);
 288
 289         schedule_delayed_work(&sw_trig->sw_led_work,
 290                               SWCONFIG_LED_TIMER_INTERVAL);
 291 }
 292
 293 static int
 294 swconfig_create_led_trigger(struct switch_dev *swdev)
 295 {
 296         struct switch_led_trigger *sw_trig;
 297         int err;
 298
 299         if (!swdev->ops->get_port_link)
 300                 return 0;
 301
 302         sw_trig = kzalloc(sizeof(struct switch_led_trigger), GFP_KERNEL);
 303         if (!sw_trig)
 304                 return -ENOMEM;
 305
 306         sw_trig->swdev = swdev;
 307         sw_trig->trig.name = swdev->devname;
 308         sw_trig->trig.activate = swconfig_trig_activate;
 309         sw_trig->trig.deactivate = swconfig_trig_deactivate;
 310
 311         INIT_DELAYED_WORK(&sw_trig->sw_led_work, swconfig_led_work_func);
 312
 313         err = led_trigger_register(&sw_trig->trig);
 314         if (err)
 315                 goto err_free;
 316
 317         swdev->led_trigger = sw_trig;
 318
 319         return 0;
 320
 321 err_free:
 322         kfree(sw_trig);
 323         return err;
 324 }
 325
 326 static void
 327 swconfig_destroy_led_trigger(struct switch_dev *swdev)
 328 {
 329         struct switch_led_trigger *sw_trig;
 330
 331         sw_trig = swdev->led_trigger;
 332         if (sw_trig) {
 333                 cancel_delayed_work_sync(&sw_trig->sw_led_work);
 334                 led_trigger_unregister(&sw_trig->trig);
 335                 kfree(sw_trig);
 336         }
 337 }
 338
 339 #else /* SWCONFIG_LEDS */
 340 static inline int
 341 swconfig_create_led_trigger(struct switch_dev *swdev) { return 0; }
 342
 343 static inline void
 344 swconfig_destroy_led_trigger(struct switch_dev *swdev) { }
 345 #endif /* CONFIG_SWCONFIG_LEDS */