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