drivers/net/phy/phy_led_triggers.c

   1 /* Copyright (C) 2016 National Instruments Corp.
   2  *
   3  * This program is free software; you can redistribute it and/or modify
   4  * it under the terms of the GNU General Public License as published by
   5   * the Free Software Foundation; either version 2 of the License, or
   6  * (at your option) any later version.
   7  *
   8  * This program is distributed in the hope that it will be useful,
   9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  * GNU General Public License for more details.
  12  */
  13 #include <linux/leds.h>
  14 #include <linux/phy.h>
  15 #include <linux/phy_led_triggers.h>
  16 #include <linux/netdevice.h>
  17
  18 static struct phy_led_trigger *phy_speed_to_led_trigger(struct phy_device *phy,
  19                                                         unsigned int speed)
  20 {
  21         unsigned int i;
  22
  23         for (i = 0; i < phy->phy_num_led_triggers; i++) {
  24                 if (phy->phy_led_triggers[i].speed == speed)
  25                         return &phy->phy_led_triggers[i];
  26         }
  27         return NULL;
  28 }
  29
  30 void phy_led_trigger_change_speed(struct phy_device *phy)
  31 {
  32         struct phy_led_trigger *plt;
  33
  34         if (!phy->link)
  35                 goto out_change_speed;
  36
  37         if (phy->speed == 0)
  38                 return;
  39
  40         plt = phy_speed_to_led_trigger(phy, phy->speed);
  41         if (!plt) {
  42                 netdev_alert(phy->attached_dev,
  43                              "No phy led trigger registered for speed(%d)\n",
  44                              phy->speed);
  45                 goto out_change_speed;
  46         }
  47
  48         if (plt != phy->last_triggered) {
  49                 led_trigger_event(&phy->last_triggered->trigger, LED_OFF);
  50                 led_trigger_event(&plt->trigger, LED_FULL);
  51                 phy->last_triggered = plt;
  52         }
  53         return;
  54
  55 out_change_speed:
  56         if (phy->last_triggered) {
  57                 led_trigger_event(&phy->last_triggered->trigger,
  58                                   LED_OFF);
  59                 phy->last_triggered = NULL;
  60         }
  61 }
  62 EXPORT_SYMBOL_GPL(phy_led_trigger_change_speed);
  63
  64 static int phy_led_trigger_register(struct phy_device *phy,
  65                                     struct phy_led_trigger *plt,
  66                                     unsigned int speed)
  67 {
  68         char name_suffix[PHY_LED_TRIGGER_SPEED_SUFFIX_SIZE];
  69
  70         plt->speed = speed;
  71
  72         if (speed < SPEED_1000)
  73                 snprintf(name_suffix, sizeof(name_suffix), "%dMbps", speed);
  74         else if (speed == SPEED_2500)
  75                 snprintf(name_suffix, sizeof(name_suffix), "2.5Gbps");
  76         else
  77                 snprintf(name_suffix, sizeof(name_suffix), "%dGbps",
  78                          DIV_ROUND_CLOSEST(speed, 1000));
  79
  80         snprintf(plt->name, sizeof(plt->name), PHY_ID_FMT ":%s",
  81                  phy->mdio.bus->id, phy->mdio.addr, name_suffix);
  82         plt->trigger.name = plt->name;
  83
  84         return led_trigger_register(&plt->trigger);
  85 }
  86
  87 static void phy_led_trigger_unregister(struct phy_led_trigger *plt)
  88 {
  89         led_trigger_unregister(&plt->trigger);
  90 }
  91
  92 int phy_led_triggers_register(struct phy_device *phy)
  93 {
  94         int i, err;
  95         unsigned int speeds[50];
  96
  97         phy->phy_num_led_triggers = phy_supported_speeds(phy, speeds,
  98                                                          ARRAY_SIZE(speeds));
  99         if (!phy->phy_num_led_triggers)
 100                 return 0;
 101
 102         phy->phy_led_triggers = devm_kzalloc(&phy->mdio.dev,
 103                                             sizeof(struct phy_led_trigger) *
 104                                                    phy->phy_num_led_triggers,
 105                                             GFP_KERNEL);
 106         if (!phy->phy_led_triggers) {
 107                 err = -ENOMEM;
 108                 goto out_clear;
 109         }
 110
 111         for (i = 0; i < phy->phy_num_led_triggers; i++) {
 112                 err = phy_led_trigger_register(phy, &phy->phy_led_triggers[i],
 113                                                speeds[i]);
 114                 if (err)
 115                         goto out_unreg;
 116         }
 117
 118         phy->last_triggered = NULL;
 119         phy_led_trigger_change_speed(phy);
 120
 121         return 0;
 122 out_unreg:
 123         while (i--)
 124                 phy_led_trigger_unregister(&phy->phy_led_triggers[i]);
 125         devm_kfree(&phy->mdio.dev, phy->phy_led_triggers);
 126 out_clear:
 127         phy->phy_num_led_triggers = 0;
 128         return err;
 129 }
 130 EXPORT_SYMBOL_GPL(phy_led_triggers_register);
 131
 132 void phy_led_triggers_unregister(struct phy_device *phy)
 133 {
 134         int i;
 135
 136         for (i = 0; i < phy->phy_num_led_triggers; i++)
 137                 phy_led_trigger_unregister(&phy->phy_led_triggers[i]);
 138 }
 139 EXPORT_SYMBOL_GPL(phy_led_triggers_unregister);