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 #define SWCONFIG_LED_PORT_SPEED_NA      0x01    /* unknown speed */
  24 #define SWCONFIG_LED_PORT_SPEED_10      0x02    /* 10 Mbps */
  25 #define SWCONFIG_LED_PORT_SPEED_100     0x04    /* 100 Mbps */
  26 #define SWCONFIG_LED_PORT_SPEED_1000    0x08    /* 1000 Mbps */
  27 #define SWCONFIG_LED_PORT_SPEED_ALL     (SWCONFIG_LED_PORT_SPEED_NA | \
  28                                          SWCONFIG_LED_PORT_SPEED_10 | \
  29                                          SWCONFIG_LED_PORT_SPEED_100 | \
  30                                          SWCONFIG_LED_PORT_SPEED_1000)
  31
  32 struct switch_led_trigger {
  33         struct led_trigger trig;
  34         struct switch_dev *swdev;
  35
  36         struct delayed_work sw_led_work;
  37         u32 port_mask;
  38         u32 port_link;
  39         unsigned long long port_traffic[SWCONFIG_LED_NUM_PORTS];
  40         u8 link_speed[SWCONFIG_LED_NUM_PORTS];
  41 };
  42
  43 struct swconfig_trig_data {
  44         struct led_classdev *led_cdev;
  45         struct switch_dev *swdev;
  46
  47         rwlock_t lock;
  48         u32 port_mask;
  49
  50         bool prev_link;
  51         unsigned long prev_traffic;
  52         enum led_brightness prev_brightness;
  53         u8 speed_mask;
  54 };
  55
  56 static void
  57 swconfig_trig_set_brightness(struct swconfig_trig_data *trig_data,
  58                              enum led_brightness brightness)
  59 {
  60         led_set_brightness(trig_data->led_cdev, brightness);
  61         trig_data->prev_brightness = brightness;
  62 }
  63
  64 static void
  65 swconfig_trig_update_port_mask(struct led_trigger *trigger)
  66 {
  67         struct list_head *entry;
  68         struct switch_led_trigger *sw_trig;
  69         u32 port_mask;
  70
  71         if (!trigger)
  72                 return;
  73
  74         sw_trig = (void *) trigger;
  75
  76         port_mask = 0;
  77         read_lock(&trigger->leddev_list_lock);
  78         list_for_each(entry, &trigger->led_cdevs) {
  79                 struct led_classdev *led_cdev;
  80                 struct swconfig_trig_data *trig_data;
  81
  82                 led_cdev = list_entry(entry, struct led_classdev, trig_list);
  83                 trig_data = led_cdev->trigger_data;
  84                 if (trig_data) {
  85                         read_lock(&trig_data->lock);
  86                         port_mask |= trig_data->port_mask;
  87                         read_unlock(&trig_data->lock);
  88                 }
  89         }
  90         read_unlock(&trigger->leddev_list_lock);
  91
  92         sw_trig->port_mask = port_mask;
  93
  94         if (port_mask)
  95                 schedule_delayed_work(&sw_trig->sw_led_work,
  96                                       SWCONFIG_LED_TIMER_INTERVAL);
  97         else
  98                 cancel_delayed_work_sync(&sw_trig->sw_led_work);
  99 }
 100
 101 static ssize_t
 102 swconfig_trig_port_mask_store(struct device *dev, struct device_attribute *attr,
 103                               const char *buf, size_t size)
 104 {
 105         struct led_classdev *led_cdev = dev_get_drvdata(dev);
 106         struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
 107         unsigned long port_mask;
 108         int ret;
 109         bool changed;
 110
 111         ret = kstrtoul(buf, 0, &port_mask);
 112         if (ret)
 113                 return ret;
 114
 115         write_lock(&trig_data->lock);
 116
 117         changed = (trig_data->port_mask != port_mask);
 118         if (changed) {
 119                 trig_data->port_mask = port_mask;
 120                 if (port_mask == 0)
 121                         swconfig_trig_set_brightness(trig_data, LED_OFF);
 122         }
 123
 124         write_unlock(&trig_data->lock);
 125
 126         if (changed)
 127                 swconfig_trig_update_port_mask(led_cdev->trigger);
 128
 129         return size;
 130 }
 131
 132 static ssize_t
 133 swconfig_trig_port_mask_show(struct device *dev, struct device_attribute *attr,
 134                              char *buf)
 135 {
 136         struct led_classdev *led_cdev = dev_get_drvdata(dev);
 137         struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
 138
 139         read_lock(&trig_data->lock);
 140         sprintf(buf, "%#x\n", trig_data->port_mask);
 141         read_unlock(&trig_data->lock);
 142
 143         return strlen(buf) + 1;
 144 }
 145
 146 static DEVICE_ATTR(port_mask, 0644, swconfig_trig_port_mask_show,
 147                    swconfig_trig_port_mask_store);
 148
 149 /* speed_mask file handler - display value */
 150 static ssize_t swconfig_trig_speed_mask_show(struct device *dev,
 151                                              struct device_attribute *attr,
 152                                              char *buf)
 153 {
 154         struct led_classdev *led_cdev = dev_get_drvdata(dev);
 155         struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
 156
 157         read_lock(&trig_data->lock);
 158         sprintf(buf, "%#x\n", trig_data->speed_mask);
 159         read_unlock(&trig_data->lock);
 160
 161         return strlen(buf) + 1;
 162 }
 163
 164 /* speed_mask file handler - store value */
 165 static ssize_t swconfig_trig_speed_mask_store(struct device *dev,
 166                                               struct device_attribute *attr,
 167                                               const char *buf, size_t size)
 168 {
 169         struct led_classdev *led_cdev = dev_get_drvdata(dev);
 170         struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
 171         u8 speed_mask;
 172         int ret;
 173
 174         ret = kstrtou8(buf, 0, &speed_mask);
 175         if (ret)
 176                 return ret;
 177
 178         write_lock(&trig_data->lock);
 179         trig_data->speed_mask = speed_mask & SWCONFIG_LED_PORT_SPEED_ALL;
 180         write_unlock(&trig_data->lock);
 181
 182         return size;
 183 }
 184
 185 /* speed_mask special file */
 186 static DEVICE_ATTR(speed_mask, 0644, swconfig_trig_speed_mask_show,
 187                    swconfig_trig_speed_mask_store);
 188
 189 static void
 190 swconfig_trig_activate(struct led_classdev *led_cdev)
 191 {
 192         struct switch_led_trigger *sw_trig;
 193         struct swconfig_trig_data *trig_data;
 194         int err;
 195
 196         if (led_cdev->trigger->activate != swconfig_trig_activate)
 197                 return;
 198
 199         trig_data = kzalloc(sizeof(struct swconfig_trig_data), GFP_KERNEL);
 200         if (!trig_data)
 201                 return;
 202
 203         sw_trig = (void *) led_cdev->trigger;
 204
 205         rwlock_init(&trig_data->lock);
 206         trig_data->led_cdev = led_cdev;
 207         trig_data->swdev = sw_trig->swdev;
 208         trig_data->speed_mask = SWCONFIG_LED_PORT_SPEED_ALL;
 209         led_cdev->trigger_data = trig_data;
 210
 211         err = device_create_file(led_cdev->dev, &dev_attr_port_mask);
 212         if (err)
 213                 goto err_free;
 214
 215         err = device_create_file(led_cdev->dev, &dev_attr_speed_mask);
 216         if (err)
 217                 goto err_dev_free;
 218
 219         return;
 220
 221 err_dev_free:
 222         device_remove_file(led_cdev->dev, &dev_attr_port_mask);
 223
 224 err_free:
 225         led_cdev->trigger_data = NULL;
 226         kfree(trig_data);
 227 }
 228
 229 static void
 230 swconfig_trig_deactivate(struct led_classdev *led_cdev)
 231 {
 232         struct swconfig_trig_data *trig_data;
 233
 234         swconfig_trig_update_port_mask(led_cdev->trigger);
 235
 236         trig_data = (void *) led_cdev->trigger_data;
 237         if (trig_data) {
 238                 device_remove_file(led_cdev->dev, &dev_attr_port_mask);
 239                 device_remove_file(led_cdev->dev, &dev_attr_speed_mask);
 240                 kfree(trig_data);
 241         }
 242 }
 243
 244 static void
 245 swconfig_trig_led_event(struct switch_led_trigger *sw_trig,
 246                         struct led_classdev *led_cdev)
 247 {
 248         struct swconfig_trig_data *trig_data;
 249         u32 port_mask;
 250         bool link;
 251         u8 speed_mask;
 252
 253         trig_data = led_cdev->trigger_data;
 254         if (!trig_data)
 255                 return;
 256
 257         read_lock(&trig_data->lock);
 258         port_mask = trig_data->port_mask;
 259         speed_mask = trig_data->speed_mask;
 260         read_unlock(&trig_data->lock);
 261
 262         link = !!(sw_trig->port_link & port_mask);
 263         if (!link) {
 264                 if (link != trig_data->prev_link)
 265                         swconfig_trig_set_brightness(trig_data, LED_OFF);
 266         } else {
 267                 unsigned long traffic;
 268                 int speedok;    /* link speed flag */
 269                 int i;
 270
 271                 traffic = 0;
 272                 speedok = 0;
 273                 for (i = 0; i < SWCONFIG_LED_NUM_PORTS; i++) {
 274                         if (port_mask & (1 << i))
 275                                 if (sw_trig->link_speed[i] & speed_mask) {
 276                                         traffic += sw_trig->port_traffic[i];
 277                                         speedok = 1;
 278                                 }
 279                 }
 280
 281                 if (speedok) {
 282                         /* At least one port speed matches speed_mask */
 283                         if (trig_data->prev_brightness != LED_FULL)
 284                                 swconfig_trig_set_brightness(trig_data,
 285                                                              LED_FULL);
 286                         else if (traffic != trig_data->prev_traffic)
 287                                 swconfig_trig_set_brightness(trig_data,
 288                                                              LED_OFF);
 289                 } else if (trig_data->prev_brightness != LED_OFF)
 290                         swconfig_trig_set_brightness(trig_data, LED_OFF);
 291
 292                 trig_data->prev_traffic = traffic;
 293         }
 294
 295         trig_data->prev_link = link;
 296 }
 297
 298 static void
 299 swconfig_trig_update_leds(struct switch_led_trigger *sw_trig)
 300 {
 301         struct list_head *entry;
 302         struct led_trigger *trigger;
 303
 304         trigger = &sw_trig->trig;
 305         read_lock(&trigger->leddev_list_lock);
 306         list_for_each(entry, &trigger->led_cdevs) {
 307                 struct led_classdev *led_cdev;
 308
 309                 led_cdev = list_entry(entry, struct led_classdev, trig_list);
 310                 swconfig_trig_led_event(sw_trig, led_cdev);
 311         }
 312         read_unlock(&trigger->leddev_list_lock);
 313 }
 314
 315 static void
 316 swconfig_led_work_func(struct work_struct *work)
 317 {
 318         struct switch_led_trigger *sw_trig;
 319         struct switch_dev *swdev;
 320         u32 port_mask;
 321         u32 link;
 322         int i;
 323
 324         sw_trig = container_of(work, struct switch_led_trigger,
 325                                sw_led_work.work);
 326
 327         port_mask = sw_trig->port_mask;
 328         swdev = sw_trig->swdev;
 329
 330         link = 0;
 331         for (i = 0; i < SWCONFIG_LED_NUM_PORTS; i++) {
 332                 u32 port_bit;
 333
 334                 sw_trig->link_speed[i] = 0;
 335
 336                 port_bit = BIT(i);
 337                 if ((port_mask & port_bit) == 0)
 338                         continue;
 339
 340                 if (swdev->ops->get_port_link) {
 341                         struct switch_port_link port_link;
 342
 343                         memset(&port_link, '\0', sizeof(port_link));
 344                         swdev->ops->get_port_link(swdev, i, &port_link);
 345
 346                         if (port_link.link) {
 347                                 link |= port_bit;
 348                                 switch (port_link.speed) {
 349                                 case SWITCH_PORT_SPEED_UNKNOWN:
 350                                         sw_trig->link_speed[i] =
 351                                                 SWCONFIG_LED_PORT_SPEED_NA;
 352                                         break;
 353                                 case SWITCH_PORT_SPEED_10:
 354                                         sw_trig->link_speed[i] =
 355                                                 SWCONFIG_LED_PORT_SPEED_10;
 356                                         break;
 357                                 case SWITCH_PORT_SPEED_100:
 358                                         sw_trig->link_speed[i] =
 359                                                 SWCONFIG_LED_PORT_SPEED_100;
 360                                         break;
 361                                 case SWITCH_PORT_SPEED_1000:
 362                                         sw_trig->link_speed[i] =
 363                                                 SWCONFIG_LED_PORT_SPEED_1000;
 364                                         break;
 365                                 }
 366                         }
 367                 }
 368
 369                 if (swdev->ops->get_port_stats) {
 370                         struct switch_port_stats port_stats;
 371
 372                         memset(&port_stats, '\0', sizeof(port_stats));
 373                         swdev->ops->get_port_stats(swdev, i, &port_stats);
 374                         sw_trig->port_traffic[i] = port_stats.tx_bytes +
 375                                                    port_stats.rx_bytes;
 376                 }
 377         }
 378
 379         sw_trig->port_link = link;
 380
 381         swconfig_trig_update_leds(sw_trig);
 382
 383         schedule_delayed_work(&sw_trig->sw_led_work,
 384                               SWCONFIG_LED_TIMER_INTERVAL);
 385 }
 386
 387 static int
 388 swconfig_create_led_trigger(struct switch_dev *swdev)
 389 {
 390         struct switch_led_trigger *sw_trig;
 391         int err;
 392
 393         if (!swdev->ops->get_port_link)
 394                 return 0;
 395
 396         sw_trig = kzalloc(sizeof(struct switch_led_trigger), GFP_KERNEL);
 397         if (!sw_trig)
 398                 return -ENOMEM;
 399
 400         sw_trig->swdev = swdev;
 401         sw_trig->trig.name = swdev->devname;
 402         sw_trig->trig.activate = swconfig_trig_activate;
 403         sw_trig->trig.deactivate = swconfig_trig_deactivate;
 404
 405         INIT_DELAYED_WORK(&sw_trig->sw_led_work, swconfig_led_work_func);
 406
 407         err = led_trigger_register(&sw_trig->trig);
 408         if (err)
 409                 goto err_free;
 410
 411         swdev->led_trigger = sw_trig;
 412
 413         return 0;
 414
 415 err_free:
 416         kfree(sw_trig);
 417         return err;
 418 }
 419
 420 static void
 421 swconfig_destroy_led_trigger(struct switch_dev *swdev)
 422 {
 423         struct switch_led_trigger *sw_trig;
 424
 425         sw_trig = swdev->led_trigger;
 426         if (sw_trig) {
 427                 cancel_delayed_work_sync(&sw_trig->sw_led_work);
 428                 led_trigger_unregister(&sw_trig->trig);
 429                 kfree(sw_trig);
 430         }
 431 }
 432
 433 #else /* SWCONFIG_LEDS */
 434 static inline int
 435 swconfig_create_led_trigger(struct switch_dev *swdev) { return 0; }
 436
 437 static inline void
 438 swconfig_destroy_led_trigger(struct switch_dev *swdev) { }
 439 #endif /* CONFIG_SWCONFIG_LEDS */