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 int
 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         trig_data = kzalloc(sizeof(struct swconfig_trig_data), GFP_KERNEL);
 285         if (!trig_data)
 286                 return -ENOMEM;
 287
 288         sw_trig = (void *) led_cdev->trigger;
 289
 290         rwlock_init(&trig_data->lock);
 291         trig_data->led_cdev = led_cdev;
 292         trig_data->swdev = sw_trig->swdev;
 293         trig_data->speed_mask = SWCONFIG_LED_PORT_SPEED_ALL;
 294         trig_data->mode = SWCONFIG_LED_MODE_ALL;
 295         led_cdev->trigger_data = trig_data;
 296
 297         err = device_create_file(led_cdev->dev, &dev_attr_port_mask);
 298         if (err)
 299                 goto err_free;
 300
 301         err = device_create_file(led_cdev->dev, &dev_attr_speed_mask);
 302         if (err)
 303                 goto err_dev_free;
 304
 305         err = device_create_file(led_cdev->dev, &dev_attr_mode);
 306         if (err)
 307                 goto err_mode_free;
 308
 309         return 0;
 310
 311 err_mode_free:
 312         device_remove_file(led_cdev->dev, &dev_attr_speed_mask);
 313
 314 err_dev_free:
 315         device_remove_file(led_cdev->dev, &dev_attr_port_mask);
 316
 317 err_free:
 318         led_cdev->trigger_data = NULL;
 319         kfree(trig_data);
 320
 321         return err;
 322 }
 323
 324 #if LINUX_VERSION_CODE < KERNEL_VERSION(4,19,0)
 325 static void
 326 swconfig_trig_activate_void(struct led_classdev *led_cdev)
 327 {
 328         swconfig_trig_activate(led_cdev);
 329 }
 330 #endif
 331
 332 static void
 333 swconfig_trig_deactivate(struct led_classdev *led_cdev)
 334 {
 335         struct swconfig_trig_data *trig_data;
 336
 337         swconfig_trig_update_port_mask(led_cdev->trigger);
 338
 339         trig_data = (void *) led_cdev->trigger_data;
 340         if (trig_data) {
 341                 device_remove_file(led_cdev->dev, &dev_attr_port_mask);
 342                 device_remove_file(led_cdev->dev, &dev_attr_speed_mask);
 343                 device_remove_file(led_cdev->dev, &dev_attr_mode);
 344                 kfree(trig_data);
 345         }
 346 }
 347
 348 /*
 349  * link off -> led off (can't be any other reason to turn it on)
 350  * link on:
 351  *      mode link: led on by default only if speed matches, else off
 352  *      mode txrx: blink only if speed matches, else off
 353  */
 354 static void
 355 swconfig_trig_led_event(struct switch_led_trigger *sw_trig,
 356                         struct led_classdev *led_cdev)
 357 {
 358         struct swconfig_trig_data *trig_data;
 359         u32 port_mask;
 360         bool link;
 361         u8 speed_mask, mode;
 362         enum led_brightness led_base, led_blink;
 363
 364         trig_data = led_cdev->trigger_data;
 365         if (!trig_data)
 366                 return;
 367
 368         read_lock(&trig_data->lock);
 369         port_mask = trig_data->port_mask;
 370         speed_mask = trig_data->speed_mask;
 371         mode = trig_data->mode;
 372         read_unlock(&trig_data->lock);
 373
 374         link = !!(sw_trig->port_link & port_mask);
 375         if (!link) {
 376                 if (trig_data->prev_brightness != LED_OFF)
 377                         swconfig_trig_set_brightness(trig_data, LED_OFF); /* and stop */
 378         }
 379         else {
 380                 unsigned long traffic;
 381                 int speedok;    /* link speed flag */
 382                 int i;
 383
 384                 led_base = LED_FULL;
 385                 led_blink = LED_OFF;
 386                 traffic = 0;
 387                 speedok = 0;
 388                 for (i = 0; i < SWCONFIG_LED_NUM_PORTS; i++) {
 389                         if (port_mask & (1 << i)) {
 390                                 if (sw_trig->link_speed[i] & speed_mask) {
 391                                         traffic += ((mode & SWCONFIG_LED_MODE_TX) ?
 392                                                     sw_trig->port_tx_traffic[i] : 0) +
 393                                                 ((mode & SWCONFIG_LED_MODE_RX) ?
 394                                                  sw_trig->port_rx_traffic[i] : 0);
 395                                         speedok = 1;
 396                                 }
 397                         }
 398                 }
 399
 400                 if (speedok) {
 401                         /* At least one port speed matches speed_mask */
 402                         if (!(mode & SWCONFIG_LED_MODE_LINK)) {
 403                                 led_base = LED_OFF;
 404                                 led_blink = LED_FULL;
 405                         }
 406
 407                         if (trig_data->prev_brightness != led_base)
 408                                 swconfig_trig_set_brightness(trig_data,
 409                                                              led_base);
 410                         else if (traffic != trig_data->prev_traffic)
 411                                 swconfig_trig_set_brightness(trig_data,
 412                                                              led_blink);
 413                 } else if (trig_data->prev_brightness != LED_OFF)
 414                         swconfig_trig_set_brightness(trig_data, LED_OFF);
 415
 416                 trig_data->prev_traffic = traffic;
 417         }
 418
 419         trig_data->prev_link = link;
 420 }
 421
 422 static void
 423 swconfig_trig_update_leds(struct switch_led_trigger *sw_trig)
 424 {
 425         struct list_head *entry;
 426         struct led_trigger *trigger;
 427
 428         trigger = &sw_trig->trig;
 429         read_lock(&trigger->leddev_list_lock);
 430         list_for_each(entry, &trigger->led_cdevs) {
 431                 struct led_classdev *led_cdev;
 432
 433                 led_cdev = list_entry(entry, struct led_classdev, trig_list);
 434                 swconfig_trig_led_event(sw_trig, led_cdev);
 435         }
 436         read_unlock(&trigger->leddev_list_lock);
 437 }
 438
 439 static void
 440 swconfig_led_work_func(struct work_struct *work)
 441 {
 442         struct switch_led_trigger *sw_trig;
 443         struct switch_dev *swdev;
 444         u32 port_mask;
 445         u32 link;
 446         int i;
 447
 448         sw_trig = container_of(work, struct switch_led_trigger,
 449                                sw_led_work.work);
 450
 451         port_mask = sw_trig->port_mask;
 452         swdev = sw_trig->swdev;
 453
 454         link = 0;
 455         for (i = 0; i < SWCONFIG_LED_NUM_PORTS; i++) {
 456                 u32 port_bit;
 457
 458                 sw_trig->link_speed[i] = 0;
 459
 460                 port_bit = BIT(i);
 461                 if ((port_mask & port_bit) == 0)
 462                         continue;
 463
 464                 if (swdev->ops->get_port_link) {
 465                         struct switch_port_link port_link;
 466
 467                         memset(&port_link, '\0', sizeof(port_link));
 468                         swdev->ops->get_port_link(swdev, i, &port_link);
 469
 470                         if (port_link.link) {
 471                                 link |= port_bit;
 472                                 switch (port_link.speed) {
 473                                 case SWITCH_PORT_SPEED_UNKNOWN:
 474                                         sw_trig->link_speed[i] =
 475                                                 SWCONFIG_LED_PORT_SPEED_NA;
 476                                         break;
 477                                 case SWITCH_PORT_SPEED_10:
 478                                         sw_trig->link_speed[i] =
 479                                                 SWCONFIG_LED_PORT_SPEED_10;
 480                                         break;
 481                                 case SWITCH_PORT_SPEED_100:
 482                                         sw_trig->link_speed[i] =
 483                                                 SWCONFIG_LED_PORT_SPEED_100;
 484                                         break;
 485                                 case SWITCH_PORT_SPEED_1000:
 486                                         sw_trig->link_speed[i] =
 487                                                 SWCONFIG_LED_PORT_SPEED_1000;
 488                                         break;
 489                                 }
 490                         }
 491                 }
 492
 493                 if (swdev->ops->get_port_stats) {
 494                         struct switch_port_stats port_stats;
 495
 496                         memset(&port_stats, '\0', sizeof(port_stats));
 497                         swdev->ops->get_port_stats(swdev, i, &port_stats);
 498                         sw_trig->port_tx_traffic[i] = port_stats.tx_bytes;
 499                         sw_trig->port_rx_traffic[i] = port_stats.rx_bytes;
 500                 }
 501         }
 502
 503         sw_trig->port_link = link;
 504
 505         swconfig_trig_update_leds(sw_trig);
 506
 507         schedule_delayed_work(&sw_trig->sw_led_work,
 508                               SWCONFIG_LED_TIMER_INTERVAL);
 509 }
 510
 511 static int
 512 swconfig_create_led_trigger(struct switch_dev *swdev)
 513 {
 514         struct switch_led_trigger *sw_trig;
 515         int err;
 516
 517         if (!swdev->ops->get_port_link)
 518                 return 0;
 519
 520         sw_trig = kzalloc(sizeof(struct switch_led_trigger), GFP_KERNEL);
 521         if (!sw_trig)
 522                 return -ENOMEM;
 523
 524         sw_trig->swdev = swdev;
 525         sw_trig->trig.name = swdev->devname;
 526 #if LINUX_VERSION_CODE < KERNEL_VERSION(4,19,0)
 527         sw_trig->trig.activate = swconfig_trig_activate_void;
 528 #else
 529         sw_trig->trig.activate = swconfig_trig_activate;
 530 #endif
 531         sw_trig->trig.deactivate = swconfig_trig_deactivate;
 532
 533         INIT_DELAYED_WORK(&sw_trig->sw_led_work, swconfig_led_work_func);
 534
 535         err = led_trigger_register(&sw_trig->trig);
 536         if (err)
 537                 goto err_free;
 538
 539         swdev->led_trigger = sw_trig;
 540
 541         return 0;
 542
 543 err_free:
 544         kfree(sw_trig);
 545         return err;
 546 }
 547
 548 static void
 549 swconfig_destroy_led_trigger(struct switch_dev *swdev)
 550 {
 551         struct switch_led_trigger *sw_trig;
 552
 553         sw_trig = swdev->led_trigger;
 554         if (sw_trig) {
 555                 cancel_delayed_work_sync(&sw_trig->sw_led_work);
 556                 led_trigger_unregister(&sw_trig->trig);
 557                 kfree(sw_trig);
 558         }
 559 }
 560
 561 #else /* SWCONFIG_LEDS */
 562 static inline int
 563 swconfig_create_led_trigger(struct switch_dev *swdev) { return 0; }
 564
 565 static inline void
 566 swconfig_destroy_led_trigger(struct switch_dev *swdev) { }
 567 #endif /* CONFIG_SWCONFIG_LEDS */