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