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 #define SWCONFIG_LED_MODE_LINK          0x01
  33 #define SWCONFIG_LED_MODE_TX            0x02
  34 #define SWCONFIG_LED_MODE_RX            0x04
  35 #define SWCONFIG_LED_MODE_TXRX          (SWCONFIG_LED_MODE_TX   | \
  36                                          SWCONFIG_LED_MODE_RX)
  37 #define SWCONFIG_LED_MODE_ALL           (SWCONFIG_LED_MODE_LINK | \
  38                                          SWCONFIG_LED_MODE_TX   | \
  39                                          SWCONFIG_LED_MODE_RX)
  40
  41 struct switch_led_trigger {
  42         struct led_trigger trig;
  43         struct switch_dev *swdev;
  44
  45         struct delayed_work sw_led_work;
  46         u32 port_mask;
  47         u32 port_link;
  48         unsigned long long port_tx_traffic[SWCONFIG_LED_NUM_PORTS];
  49         unsigned long long port_rx_traffic[SWCONFIG_LED_NUM_PORTS];
  50         u8 link_speed[SWCONFIG_LED_NUM_PORTS];
  51 };
  52
  53 struct swconfig_trig_data {
  54         struct led_classdev *led_cdev;
  55         struct switch_dev *swdev;
  56
  57         rwlock_t lock;
  58         u32 port_mask;
  59
  60         bool prev_link;
  61         unsigned long prev_traffic;
  62         enum led_brightness prev_brightness;
  63         u8 mode;
  64         u8 speed_mask;
  65 };
  66
  67 static void
  68 swconfig_trig_set_brightness(struct swconfig_trig_data *trig_data,
  69                              enum led_brightness brightness)
  70 {
  71         led_set_brightness(trig_data->led_cdev, brightness);
  72         trig_data->prev_brightness = brightness;
  73 }
  74
  75 static void
  76 swconfig_trig_update_port_mask(struct led_trigger *trigger)
  77 {
  78         struct list_head *entry;
  79         struct switch_led_trigger *sw_trig;
  80         u32 port_mask;
  81
  82         if (!trigger)
  83                 return;
  84
  85         sw_trig = (void *) trigger;
  86
  87         port_mask = 0;
  88         read_lock(&trigger->leddev_list_lock);
  89         list_for_each(entry, &trigger->led_cdevs) {
  90                 struct led_classdev *led_cdev;
  91                 struct swconfig_trig_data *trig_data;
  92
  93                 led_cdev = list_entry(entry, struct led_classdev, trig_list);
  94                 trig_data = led_cdev->trigger_data;
  95                 if (trig_data) {
  96                         read_lock(&trig_data->lock);
  97                         port_mask |= trig_data->port_mask;
  98                         read_unlock(&trig_data->lock);
  99                 }
 100         }
 101         read_unlock(&trigger->leddev_list_lock);
 102
 103         sw_trig->port_mask = port_mask;
 104
 105         if (port_mask)
 106                 schedule_delayed_work(&sw_trig->sw_led_work,
 107                                       SWCONFIG_LED_TIMER_INTERVAL);
 108         else
 109                 cancel_delayed_work_sync(&sw_trig->sw_led_work);
 110 }
 111
 112 static ssize_t
 113 swconfig_trig_port_mask_store(struct device *dev, struct device_attribute *attr,
 114                               const char *buf, size_t size)
 115 {
 116         struct led_classdev *led_cdev = dev_get_drvdata(dev);
 117         struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
 118         unsigned long port_mask;
 119         int ret;
 120         bool changed;
 121
 122         ret = kstrtoul(buf, 0, &port_mask);
 123         if (ret)
 124                 return ret;
 125
 126         write_lock(&trig_data->lock);
 127         changed = (trig_data->port_mask != port_mask);
 128         trig_data->port_mask = port_mask;
 129         write_unlock(&trig_data->lock);
 130
 131         if (changed) {
 132                 if (port_mask == 0)
 133                         swconfig_trig_set_brightness(trig_data, LED_OFF);
 134
 135                 swconfig_trig_update_port_mask(led_cdev->trigger);
 136         }
 137
 138         return size;
 139 }
 140
 141 static ssize_t
 142 swconfig_trig_port_mask_show(struct device *dev, struct device_attribute *attr,
 143                              char *buf)
 144 {
 145         struct led_classdev *led_cdev = dev_get_drvdata(dev);
 146         struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
 147         u32 port_mask;
 148
 149         read_lock(&trig_data->lock);
 150         port_mask = trig_data->port_mask;
 151         read_unlock(&trig_data->lock);
 152
 153         sprintf(buf, "%#x\n", port_mask);
 154
 155         return strlen(buf) + 1;
 156 }
 157
 158 static DEVICE_ATTR(port_mask, 0644, swconfig_trig_port_mask_show,
 159                    swconfig_trig_port_mask_store);
 160
 161 /* speed_mask file handler - display value */
 162 static ssize_t swconfig_trig_speed_mask_show(struct device *dev,
 163                                              struct device_attribute *attr,
 164                                              char *buf)
 165 {
 166         struct led_classdev *led_cdev = dev_get_drvdata(dev);
 167         struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
 168         u8 speed_mask;
 169
 170         read_lock(&trig_data->lock);
 171         speed_mask = trig_data->speed_mask;
 172         read_unlock(&trig_data->lock);
 173
 174         sprintf(buf, "%#x\n", speed_mask);
 175
 176         return strlen(buf) + 1;
 177 }
 178
 179 /* speed_mask file handler - store value */
 180 static ssize_t swconfig_trig_speed_mask_store(struct device *dev,
 181                                               struct device_attribute *attr,
 182                                               const char *buf, size_t size)
 183 {
 184         struct led_classdev *led_cdev = dev_get_drvdata(dev);
 185         struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
 186         u8 speed_mask;
 187         int ret;
 188
 189         ret = kstrtou8(buf, 0, &speed_mask);
 190         if (ret)
 191                 return ret;
 192
 193         write_lock(&trig_data->lock);
 194         trig_data->speed_mask = speed_mask & SWCONFIG_LED_PORT_SPEED_ALL;
 195         write_unlock(&trig_data->lock);
 196
 197         return size;
 198 }
 199
 200 /* speed_mask special file */
 201 static DEVICE_ATTR(speed_mask, 0644, swconfig_trig_speed_mask_show,
 202                    swconfig_trig_speed_mask_store);
 203
 204 static ssize_t swconfig_trig_mode_show(struct device *dev,
 205                 struct device_attribute *attr, char *buf)
 206 {
 207         struct led_classdev *led_cdev = dev_get_drvdata(dev);
 208         struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
 209         u8 mode;
 210
 211         read_lock(&trig_data->lock);
 212         mode = trig_data->mode;
 213         read_unlock(&trig_data->lock);
 214
 215         if (mode == 0) {
 216                 strcpy(buf, "none\n");
 217         } else {
 218                 if (mode & SWCONFIG_LED_MODE_LINK)
 219                         strcat(buf, "link ");
 220                 if (mode & SWCONFIG_LED_MODE_TX)
 221                         strcat(buf, "tx ");
 222                 if (mode & SWCONFIG_LED_MODE_RX)
 223                         strcat(buf, "rx ");
 224                 strcat(buf, "\n");
 225         }
 226
 227         return strlen(buf)+1;
 228 }
 229
 230 static ssize_t swconfig_trig_mode_store(struct device *dev,
 231                 struct device_attribute *attr, const char *buf, size_t size)
 232 {
 233         struct led_classdev *led_cdev = dev_get_drvdata(dev);
 234         struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
 235         char copybuf[128];
 236         int new_mode = -1;
 237         char *p, *token;
 238
 239         /* take a copy since we don't want to trash the inbound buffer when using strsep */
 240         strncpy(copybuf, buf, sizeof(copybuf));
 241         copybuf[sizeof(copybuf) - 1] = 0;
 242         p = copybuf;
 243
 244         while ((token = strsep(&p, " \t\n")) != NULL) {
 245                 if (!*token)
 246                         continue;
 247
 248                 if (new_mode < 0)
 249                         new_mode = 0;
 250
 251                 if (!strcmp(token, "none"))
 252                         new_mode = 0;
 253                 else if (!strcmp(token, "tx"))
 254                         new_mode |= SWCONFIG_LED_MODE_TX;
 255                 else if (!strcmp(token, "rx"))
 256                         new_mode |= SWCONFIG_LED_MODE_RX;
 257                 else if (!strcmp(token, "link"))
 258                         new_mode |= SWCONFIG_LED_MODE_LINK;
 259                 else
 260                         return -EINVAL;
 261         }
 262
 263         if (new_mode < 0)
 264                 return -EINVAL;
 265
 266         write_lock(&trig_data->lock);
 267         trig_data->mode = (u8)new_mode;
 268         write_unlock(&trig_data->lock);
 269
 270         return size;
 271 }
 272
 273 /* mode special file */
 274 static DEVICE_ATTR(mode, 0644, swconfig_trig_mode_show,
 275                    swconfig_trig_mode_store);
 276
 277 static void
 278 swconfig_trig_activate(struct led_classdev *led_cdev)
 279 {
 280         struct switch_led_trigger *sw_trig;
 281         struct swconfig_trig_data *trig_data;
 282         int err;
 283
 284         if (led_cdev->trigger->activate != swconfig_trig_activate)
 285                 return;
 286
 287         trig_data = kzalloc(sizeof(struct swconfig_trig_data), GFP_KERNEL);
 288         if (!trig_data)
 289                 return;
 290
 291         sw_trig = (void *) led_cdev->trigger;
 292
 293         rwlock_init(&trig_data->lock);
 294         trig_data->led_cdev = led_cdev;
 295         trig_data->swdev = sw_trig->swdev;
 296         trig_data->speed_mask = SWCONFIG_LED_PORT_SPEED_ALL;
 297         trig_data->mode = SWCONFIG_LED_MODE_ALL;
 298         led_cdev->trigger_data = trig_data;
 299
 300         err = device_create_file(led_cdev->dev, &dev_attr_port_mask);
 301         if (err)
 302                 goto err_free;
 303
 304         err = device_create_file(led_cdev->dev, &dev_attr_speed_mask);
 305         if (err)
 306                 goto err_dev_free;
 307
 308         err = device_create_file(led_cdev->dev, &dev_attr_mode);
 309         if (err)
 310                 goto err_mode_free;
 311
 312         return;
 313
 314 err_mode_free:
 315         device_remove_file(led_cdev->dev, &dev_attr_speed_mask);
 316
 317 err_dev_free:
 318         device_remove_file(led_cdev->dev, &dev_attr_port_mask);
 319
 320 err_free:
 321         led_cdev->trigger_data = NULL;
 322         kfree(trig_data);
 323 }
 324
 325 static void
 326 swconfig_trig_deactivate(struct led_classdev *led_cdev)
 327 {
 328         struct swconfig_trig_data *trig_data;
 329
 330         swconfig_trig_update_port_mask(led_cdev->trigger);
 331
 332         trig_data = (void *) led_cdev->trigger_data;
 333         if (trig_data) {
 334                 device_remove_file(led_cdev->dev, &dev_attr_port_mask);
 335                 device_remove_file(led_cdev->dev, &dev_attr_speed_mask);
 336                 device_remove_file(led_cdev->dev, &dev_attr_mode);
 337                 kfree(trig_data);
 338         }
 339 }
 340
 341 /*
 342  * link off -> led off (can't be any other reason to turn it on)
 343  * link on:
 344  *      mode link: led on by default only if speed matches, else off
 345  *      mode txrx: blink only if speed matches, else off
 346  */
 347 static void
 348 swconfig_trig_led_event(struct switch_led_trigger *sw_trig,
 349                         struct led_classdev *led_cdev)
 350 {
 351         struct swconfig_trig_data *trig_data;
 352         u32 port_mask;
 353         bool link;
 354         u8 speed_mask, mode;
 355         enum led_brightness led_base, led_blink;
 356
 357         trig_data = led_cdev->trigger_data;
 358         if (!trig_data)
 359                 return;
 360
 361         read_lock(&trig_data->lock);
 362         port_mask = trig_data->port_mask;
 363         speed_mask = trig_data->speed_mask;
 364         mode = trig_data->mode;
 365         read_unlock(&trig_data->lock);
 366
 367         link = !!(sw_trig->port_link & port_mask);
 368         if (!link) {
 369                 if (trig_data->prev_brightness != LED_OFF)
 370                         swconfig_trig_set_brightness(trig_data, LED_OFF); /* and stop */
 371         }
 372         else {
 373                 unsigned long traffic;
 374                 int speedok;    /* link speed flag */
 375                 int i;
 376
 377                 led_base = LED_FULL;
 378                 led_blink = LED_OFF;
 379                 traffic = 0;
 380                 speedok = 0;
 381                 for (i = 0; i < SWCONFIG_LED_NUM_PORTS; i++) {
 382                         if (port_mask & (1 << i)) {
 383                                 if (sw_trig->link_speed[i] & speed_mask) {
 384                                         traffic += ((mode & SWCONFIG_LED_MODE_TX) ?
 385                                                     sw_trig->port_tx_traffic[i] : 0) +
 386                                                 ((mode & SWCONFIG_LED_MODE_RX) ?
 387                                                  sw_trig->port_rx_traffic[i] : 0);
 388                                         speedok = 1;
 389                                 }
 390                         }
 391                 }
 392
 393                 if (speedok) {
 394                         /* At least one port speed matches speed_mask */
 395                         if (!(mode & SWCONFIG_LED_MODE_LINK)) {
 396                                 led_base = LED_OFF;
 397                                 led_blink = LED_FULL;
 398                         }
 399
 400                         if (trig_data->prev_brightness != led_base)
 401                                 swconfig_trig_set_brightness(trig_data,
 402                                                              led_base);
 403                         else if (traffic != trig_data->prev_traffic)
 404                                 swconfig_trig_set_brightness(trig_data,
 405                                                              led_blink);
 406                 } else if (trig_data->prev_brightness != LED_OFF)
 407                         swconfig_trig_set_brightness(trig_data, LED_OFF);
 408
 409                 trig_data->prev_traffic = traffic;
 410         }
 411
 412         trig_data->prev_link = link;
 413 }
 414
 415 static void
 416 swconfig_trig_update_leds(struct switch_led_trigger *sw_trig)
 417 {
 418         struct list_head *entry;
 419         struct led_trigger *trigger;
 420
 421         trigger = &sw_trig->trig;
 422         read_lock(&trigger->leddev_list_lock);
 423         list_for_each(entry, &trigger->led_cdevs) {
 424                 struct led_classdev *led_cdev;
 425
 426                 led_cdev = list_entry(entry, struct led_classdev, trig_list);
 427                 swconfig_trig_led_event(sw_trig, led_cdev);
 428         }
 429         read_unlock(&trigger->leddev_list_lock);
 430 }
 431
 432 static void
 433 swconfig_led_work_func(struct work_struct *work)
 434 {
 435         struct switch_led_trigger *sw_trig;
 436         struct switch_dev *swdev;
 437         u32 port_mask;
 438         u32 link;
 439         int i;
 440
 441         sw_trig = container_of(work, struct switch_led_trigger,
 442                                sw_led_work.work);
 443
 444         port_mask = sw_trig->port_mask;
 445         swdev = sw_trig->swdev;
 446
 447         link = 0;
 448         for (i = 0; i < SWCONFIG_LED_NUM_PORTS; i++) {
 449                 u32 port_bit;
 450
 451                 sw_trig->link_speed[i] = 0;
 452
 453                 port_bit = BIT(i);
 454                 if ((port_mask & port_bit) == 0)
 455                         continue;
 456
 457                 if (swdev->ops->get_port_link) {
 458                         struct switch_port_link port_link;
 459
 460                         memset(&port_link, '\0', sizeof(port_link));
 461                         swdev->ops->get_port_link(swdev, i, &port_link);
 462
 463                         if (port_link.link) {
 464                                 link |= port_bit;
 465                                 switch (port_link.speed) {
 466                                 case SWITCH_PORT_SPEED_UNKNOWN:
 467                                         sw_trig->link_speed[i] =
 468                                                 SWCONFIG_LED_PORT_SPEED_NA;
 469                                         break;
 470                                 case SWITCH_PORT_SPEED_10:
 471                                         sw_trig->link_speed[i] =
 472                                                 SWCONFIG_LED_PORT_SPEED_10;
 473                                         break;
 474                                 case SWITCH_PORT_SPEED_100:
 475                                         sw_trig->link_speed[i] =
 476                                                 SWCONFIG_LED_PORT_SPEED_100;
 477                                         break;
 478                                 case SWITCH_PORT_SPEED_1000:
 479                                         sw_trig->link_speed[i] =
 480                                                 SWCONFIG_LED_PORT_SPEED_1000;
 481                                         break;
 482                                 }
 483                         }
 484                 }
 485
 486                 if (swdev->ops->get_port_stats) {
 487                         struct switch_port_stats port_stats;
 488
 489                         memset(&port_stats, '\0', sizeof(port_stats));
 490                         swdev->ops->get_port_stats(swdev, i, &port_stats);
 491                         sw_trig->port_tx_traffic[i] = port_stats.tx_bytes;
 492                         sw_trig->port_rx_traffic[i] = port_stats.rx_bytes;
 493                 }
 494         }
 495
 496         sw_trig->port_link = link;
 497
 498         swconfig_trig_update_leds(sw_trig);
 499
 500         schedule_delayed_work(&sw_trig->sw_led_work,
 501                               SWCONFIG_LED_TIMER_INTERVAL);
 502 }
 503
 504 static int
 505 swconfig_create_led_trigger(struct switch_dev *swdev)
 506 {
 507         struct switch_led_trigger *sw_trig;
 508         int err;
 509
 510         if (!swdev->ops->get_port_link)
 511                 return 0;
 512
 513         sw_trig = kzalloc(sizeof(struct switch_led_trigger), GFP_KERNEL);
 514         if (!sw_trig)
 515                 return -ENOMEM;
 516
 517         sw_trig->swdev = swdev;
 518         sw_trig->trig.name = swdev->devname;
 519         sw_trig->trig.activate = swconfig_trig_activate;
 520         sw_trig->trig.deactivate = swconfig_trig_deactivate;
 521
 522         INIT_DELAYED_WORK(&sw_trig->sw_led_work, swconfig_led_work_func);
 523
 524         err = led_trigger_register(&sw_trig->trig);
 525         if (err)
 526                 goto err_free;
 527
 528         swdev->led_trigger = sw_trig;
 529
 530         return 0;
 531
 532 err_free:
 533         kfree(sw_trig);
 534         return err;
 535 }
 536
 537 static void
 538 swconfig_destroy_led_trigger(struct switch_dev *swdev)
 539 {
 540         struct switch_led_trigger *sw_trig;
 541
 542         sw_trig = swdev->led_trigger;
 543         if (sw_trig) {
 544                 cancel_delayed_work_sync(&sw_trig->sw_led_work);
 545                 led_trigger_unregister(&sw_trig->trig);
 546                 kfree(sw_trig);
 547         }
 548 }
 549
 550 #else /* SWCONFIG_LEDS */
 551 static inline int
 552 swconfig_create_led_trigger(struct switch_dev *swdev) { return 0; }
 553
 554 static inline void
 555 swconfig_destroy_led_trigger(struct switch_dev *swdev) { }
 556 #endif /* CONFIG_SWCONFIG_LEDS */