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 #if LINUX_VERSION_CODE >= KERNEL_VERSION(5,16,0)
  89         spin_lock(&trigger->leddev_list_lock);
  90 #else
  91         read_lock(&trigger->leddev_list_lock);
  92 #endif
  93         list_for_each(entry, &trigger->led_cdevs) {
  94                 struct led_classdev *led_cdev;
  95                 struct swconfig_trig_data *trig_data;
  96
  97                 led_cdev = list_entry(entry, struct led_classdev, trig_list);
  98                 trig_data = led_cdev->trigger_data;
  99                 if (trig_data) {
 100                         read_lock(&trig_data->lock);
 101                         port_mask |= trig_data->port_mask;
 102                         read_unlock(&trig_data->lock);
 103                 }
 104         }
 105 #if LINUX_VERSION_CODE >= KERNEL_VERSION(5,16,0)
 106         spin_unlock(&trigger->leddev_list_lock);
 107 #else
 108         read_unlock(&trigger->leddev_list_lock);
 109 #endif
 110
 111         sw_trig->port_mask = port_mask;
 112
 113         if (port_mask)
 114                 schedule_delayed_work(&sw_trig->sw_led_work,
 115                                       SWCONFIG_LED_TIMER_INTERVAL);
 116         else
 117                 cancel_delayed_work_sync(&sw_trig->sw_led_work);
 118 }
 119
 120 static ssize_t
 121 swconfig_trig_port_mask_store(struct device *dev, struct device_attribute *attr,
 122                               const char *buf, size_t size)
 123 {
 124         struct led_classdev *led_cdev = dev_get_drvdata(dev);
 125         struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
 126         unsigned long port_mask;
 127         int ret;
 128         bool changed;
 129
 130         ret = kstrtoul(buf, 0, &port_mask);
 131         if (ret)
 132                 return ret;
 133
 134         write_lock(&trig_data->lock);
 135         changed = (trig_data->port_mask != port_mask);
 136         trig_data->port_mask = port_mask;
 137         write_unlock(&trig_data->lock);
 138
 139         if (changed) {
 140                 if (port_mask == 0)
 141                         swconfig_trig_set_brightness(trig_data, LED_OFF);
 142
 143                 swconfig_trig_update_port_mask(led_cdev->trigger);
 144         }
 145
 146         return size;
 147 }
 148
 149 static ssize_t
 150 swconfig_trig_port_mask_show(struct device *dev, struct device_attribute *attr,
 151                              char *buf)
 152 {
 153         struct led_classdev *led_cdev = dev_get_drvdata(dev);
 154         struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
 155         u32 port_mask;
 156
 157         read_lock(&trig_data->lock);
 158         port_mask = trig_data->port_mask;
 159         read_unlock(&trig_data->lock);
 160
 161         sprintf(buf, "%#x\n", port_mask);
 162
 163         return strlen(buf) + 1;
 164 }
 165
 166 static DEVICE_ATTR(port_mask, 0644, swconfig_trig_port_mask_show,
 167                    swconfig_trig_port_mask_store);
 168
 169 /* speed_mask file handler - display value */
 170 static ssize_t swconfig_trig_speed_mask_show(struct device *dev,
 171                                              struct device_attribute *attr,
 172                                              char *buf)
 173 {
 174         struct led_classdev *led_cdev = dev_get_drvdata(dev);
 175         struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
 176         u8 speed_mask;
 177
 178         read_lock(&trig_data->lock);
 179         speed_mask = trig_data->speed_mask;
 180         read_unlock(&trig_data->lock);
 181
 182         sprintf(buf, "%#x\n", speed_mask);
 183
 184         return strlen(buf) + 1;
 185 }
 186
 187 /* speed_mask file handler - store value */
 188 static ssize_t swconfig_trig_speed_mask_store(struct device *dev,
 189                                               struct device_attribute *attr,
 190                                               const char *buf, size_t size)
 191 {
 192         struct led_classdev *led_cdev = dev_get_drvdata(dev);
 193         struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
 194         u8 speed_mask;
 195         int ret;
 196
 197         ret = kstrtou8(buf, 0, &speed_mask);
 198         if (ret)
 199                 return ret;
 200
 201         write_lock(&trig_data->lock);
 202         trig_data->speed_mask = speed_mask & SWCONFIG_LED_PORT_SPEED_ALL;
 203         write_unlock(&trig_data->lock);
 204
 205         return size;
 206 }
 207
 208 /* speed_mask special file */
 209 static DEVICE_ATTR(speed_mask, 0644, swconfig_trig_speed_mask_show,
 210                    swconfig_trig_speed_mask_store);
 211
 212 static ssize_t swconfig_trig_mode_show(struct device *dev,
 213                 struct device_attribute *attr, char *buf)
 214 {
 215         struct led_classdev *led_cdev = dev_get_drvdata(dev);
 216         struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
 217         u8 mode;
 218
 219         read_lock(&trig_data->lock);
 220         mode = trig_data->mode;
 221         read_unlock(&trig_data->lock);
 222
 223         if (mode == 0) {
 224                 strcpy(buf, "none\n");
 225         } else {
 226                 if (mode & SWCONFIG_LED_MODE_LINK)
 227                         strcat(buf, "link ");
 228                 if (mode & SWCONFIG_LED_MODE_TX)
 229                         strcat(buf, "tx ");
 230                 if (mode & SWCONFIG_LED_MODE_RX)
 231                         strcat(buf, "rx ");
 232                 strcat(buf, "\n");
 233         }
 234
 235         return strlen(buf)+1;
 236 }
 237
 238 static ssize_t swconfig_trig_mode_store(struct device *dev,
 239                 struct device_attribute *attr, const char *buf, size_t size)
 240 {
 241         struct led_classdev *led_cdev = dev_get_drvdata(dev);
 242         struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
 243         char copybuf[128];
 244         int new_mode = -1;
 245         char *p, *token;
 246
 247         /* take a copy since we don't want to trash the inbound buffer when using strsep */
 248         strncpy(copybuf, buf, sizeof(copybuf));
 249         copybuf[sizeof(copybuf) - 1] = 0;
 250         p = copybuf;
 251
 252         while ((token = strsep(&p, " \t\n")) != NULL) {
 253                 if (!*token)
 254                         continue;
 255
 256                 if (new_mode < 0)
 257                         new_mode = 0;
 258
 259                 if (!strcmp(token, "none"))
 260                         new_mode = 0;
 261                 else if (!strcmp(token, "tx"))
 262                         new_mode |= SWCONFIG_LED_MODE_TX;
 263                 else if (!strcmp(token, "rx"))
 264                         new_mode |= SWCONFIG_LED_MODE_RX;
 265                 else if (!strcmp(token, "link"))
 266                         new_mode |= SWCONFIG_LED_MODE_LINK;
 267                 else
 268                         return -EINVAL;
 269         }
 270
 271         if (new_mode < 0)
 272                 return -EINVAL;
 273
 274         write_lock(&trig_data->lock);
 275         trig_data->mode = (u8)new_mode;
 276         write_unlock(&trig_data->lock);
 277
 278         return size;
 279 }
 280
 281 /* mode special file */
 282 static DEVICE_ATTR(mode, 0644, swconfig_trig_mode_show,
 283                    swconfig_trig_mode_store);
 284
 285 static int
 286 swconfig_trig_activate(struct led_classdev *led_cdev)
 287 {
 288         struct switch_led_trigger *sw_trig;
 289         struct swconfig_trig_data *trig_data;
 290         int err;
 291
 292         trig_data = kzalloc(sizeof(struct swconfig_trig_data), GFP_KERNEL);
 293         if (!trig_data)
 294                 return -ENOMEM;
 295
 296         sw_trig = (void *) led_cdev->trigger;
 297
 298         rwlock_init(&trig_data->lock);
 299         trig_data->led_cdev = led_cdev;
 300         trig_data->swdev = sw_trig->swdev;
 301         trig_data->speed_mask = SWCONFIG_LED_PORT_SPEED_ALL;
 302         trig_data->mode = SWCONFIG_LED_MODE_ALL;
 303         led_cdev->trigger_data = trig_data;
 304
 305         err = device_create_file(led_cdev->dev, &dev_attr_port_mask);
 306         if (err)
 307                 goto err_free;
 308
 309         err = device_create_file(led_cdev->dev, &dev_attr_speed_mask);
 310         if (err)
 311                 goto err_dev_free;
 312
 313         err = device_create_file(led_cdev->dev, &dev_attr_mode);
 314         if (err)
 315                 goto err_mode_free;
 316
 317         return 0;
 318
 319 err_mode_free:
 320         device_remove_file(led_cdev->dev, &dev_attr_speed_mask);
 321
 322 err_dev_free:
 323         device_remove_file(led_cdev->dev, &dev_attr_port_mask);
 324
 325 err_free:
 326         led_cdev->trigger_data = NULL;
 327         kfree(trig_data);
 328
 329         return err;
 330 }
 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 #if LINUX_VERSION_CODE >= KERNEL_VERSION(5,16,0)
 430         spin_lock(&trigger->leddev_list_lock);
 431 #else
 432         read_lock(&trigger->leddev_list_lock);
 433 #endif
 434         list_for_each(entry, &trigger->led_cdevs) {
 435                 struct led_classdev *led_cdev;
 436
 437                 led_cdev = list_entry(entry, struct led_classdev, trig_list);
 438                 swconfig_trig_led_event(sw_trig, led_cdev);
 439         }
 440 #if LINUX_VERSION_CODE >= KERNEL_VERSION(5,16,0)
 441         spin_unlock(&trigger->leddev_list_lock);
 442 #else
 443         read_unlock(&trigger->leddev_list_lock);
 444 #endif
 445 }
 446
 447 static void
 448 swconfig_led_work_func(struct work_struct *work)
 449 {
 450         struct switch_led_trigger *sw_trig;
 451         struct switch_dev *swdev;
 452         u32 port_mask;
 453         u32 link;
 454         int i;
 455
 456         sw_trig = container_of(work, struct switch_led_trigger,
 457                                sw_led_work.work);
 458
 459         port_mask = sw_trig->port_mask;
 460         swdev = sw_trig->swdev;
 461
 462         link = 0;
 463         for (i = 0; i < SWCONFIG_LED_NUM_PORTS; i++) {
 464                 u32 port_bit;
 465
 466                 sw_trig->link_speed[i] = 0;
 467
 468                 port_bit = BIT(i);
 469                 if ((port_mask & port_bit) == 0)
 470                         continue;
 471
 472                 if (swdev->ops->get_port_link) {
 473                         struct switch_port_link port_link;
 474
 475                         memset(&port_link, '\0', sizeof(port_link));
 476                         swdev->ops->get_port_link(swdev, i, &port_link);
 477
 478                         if (port_link.link) {
 479                                 link |= port_bit;
 480                                 switch (port_link.speed) {
 481                                 case SWITCH_PORT_SPEED_UNKNOWN:
 482                                         sw_trig->link_speed[i] =
 483                                                 SWCONFIG_LED_PORT_SPEED_NA;
 484                                         break;
 485                                 case SWITCH_PORT_SPEED_10:
 486                                         sw_trig->link_speed[i] =
 487                                                 SWCONFIG_LED_PORT_SPEED_10;
 488                                         break;
 489                                 case SWITCH_PORT_SPEED_100:
 490                                         sw_trig->link_speed[i] =
 491                                                 SWCONFIG_LED_PORT_SPEED_100;
 492                                         break;
 493                                 case SWITCH_PORT_SPEED_1000:
 494                                         sw_trig->link_speed[i] =
 495                                                 SWCONFIG_LED_PORT_SPEED_1000;
 496                                         break;
 497                                 }
 498                         }
 499                 }
 500
 501                 if (swdev->ops->get_port_stats) {
 502                         struct switch_port_stats port_stats;
 503
 504                         memset(&port_stats, '\0', sizeof(port_stats));
 505                         swdev->ops->get_port_stats(swdev, i, &port_stats);
 506                         sw_trig->port_tx_traffic[i] = port_stats.tx_bytes;
 507                         sw_trig->port_rx_traffic[i] = port_stats.rx_bytes;
 508                 }
 509         }
 510
 511         sw_trig->port_link = link;
 512
 513         swconfig_trig_update_leds(sw_trig);
 514
 515         schedule_delayed_work(&sw_trig->sw_led_work,
 516                               SWCONFIG_LED_TIMER_INTERVAL);
 517 }
 518
 519 static int
 520 swconfig_create_led_trigger(struct switch_dev *swdev)
 521 {
 522         struct switch_led_trigger *sw_trig;
 523         int err;
 524
 525         if (!swdev->ops->get_port_link)
 526                 return 0;
 527
 528         sw_trig = kzalloc(sizeof(struct switch_led_trigger), GFP_KERNEL);
 529         if (!sw_trig)
 530                 return -ENOMEM;
 531
 532         sw_trig->swdev = swdev;
 533         sw_trig->trig.name = swdev->devname;
 534         sw_trig->trig.activate = swconfig_trig_activate;
 535         sw_trig->trig.deactivate = swconfig_trig_deactivate;
 536
 537         INIT_DELAYED_WORK(&sw_trig->sw_led_work, swconfig_led_work_func);
 538
 539         err = led_trigger_register(&sw_trig->trig);
 540         if (err)
 541                 goto err_free;
 542
 543         swdev->led_trigger = sw_trig;
 544
 545         return 0;
 546
 547 err_free:
 548         kfree(sw_trig);
 549         return err;
 550 }
 551
 552 static void
 553 swconfig_destroy_led_trigger(struct switch_dev *swdev)
 554 {
 555         struct switch_led_trigger *sw_trig;
 556
 557         sw_trig = swdev->led_trigger;
 558         if (sw_trig) {
 559                 cancel_delayed_work_sync(&sw_trig->sw_led_work);
 560                 led_trigger_unregister(&sw_trig->trig);
 561                 kfree(sw_trig);
 562         }
 563 }
 564
 565 #else /* SWCONFIG_LEDS */
 566 static inline int
 567 swconfig_create_led_trigger(struct switch_dev *swdev) { return 0; }
 568
 569 static inline void
 570 swconfig_destroy_led_trigger(struct switch_dev *swdev) { }
 571 #endif /* CONFIG_SWCONFIG_LEDS */