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/netdevice.h>
  16
  17 static struct phy_led_trigger *phy_speed_to_led_trigger(struct phy_device *phy,
  18                                                         unsigned int speed)
  19 {
  20         unsigned int i;
  21
  22         for (i = 0; i < phy->phy_num_led_triggers; i++) {
  23                 if (phy->phy_led_triggers[i].speed == speed)
  24                         return &phy->phy_led_triggers[i];
  25         }
  26         return NULL;
  27 }
  28
  29 void phy_led_trigger_change_speed(struct phy_device *phy)
  30 {
  31         struct phy_led_trigger *plt;
  32
  33         if (!phy->link)
  34                 goto out_change_speed;
  35
  36         if (phy->speed == 0)
  37                 return;
  38
  39         plt = phy_speed_to_led_trigger(phy, phy->speed);
  40         if (!plt) {
  41                 netdev_alert(phy->attached_dev,
  42                              "No phy led trigger registered for speed(%d)\n",
  43                              phy->speed);
  44                 goto out_change_speed;
  45         }
  46
  47         if (plt != phy->last_triggered) {
  48                 led_trigger_event(&phy->last_triggered->trigger, LED_OFF);
  49                 led_trigger_event(&plt->trigger, LED_FULL);
  50                 phy->last_triggered = plt;
  51         }
  52         return;
  53
  54 out_change_speed:
  55         if (phy->last_triggered) {
  56                 led_trigger_event(&phy->last_triggered->trigger,
  57                                   LED_OFF);
  58                 phy->last_triggered = NULL;
  59         }
  60 }
  61 EXPORT_SYMBOL_GPL(phy_led_trigger_change_speed);
  62
  63 static int phy_led_trigger_register(struct phy_device *phy,
  64                                     struct phy_led_trigger *plt,
  65                                     unsigned int speed)
  66 {
  67         char name_suffix[PHY_LED_TRIGGER_SPEED_SUFFIX_SIZE];
  68
  69         plt->speed = speed;
  70
  71         if (speed < SPEED_1000)
  72                 snprintf(name_suffix, sizeof(name_suffix), "%dMbps", speed);
  73         else if (speed == SPEED_2500)
  74                 snprintf(name_suffix, sizeof(name_suffix), "2.5Gbps");
  75         else
  76                 snprintf(name_suffix, sizeof(name_suffix), "%dGbps",
  77                          DIV_ROUND_CLOSEST(speed, 1000));
  78
  79         snprintf(plt->name, sizeof(plt->name), PHY_ID_FMT ":%s",
  80                  phy->mdio.bus->id, phy->mdio.addr, name_suffix);
  81         plt->trigger.name = plt->name;
  82
  83         return led_trigger_register(&plt->trigger);
  84 }
  85
  86 static void phy_led_trigger_unregister(struct phy_led_trigger *plt)
  87 {
  88         led_trigger_unregister(&plt->trigger);
  89 }
  90
  91 int phy_led_triggers_register(struct phy_device *phy)
  92 {
  93         int i, err;
  94         unsigned int speeds[50];
  95
  96         phy->phy_num_led_triggers = phy_supported_speeds(phy, speeds,
  97                                                          ARRAY_SIZE(speeds));
  98         if (!phy->phy_num_led_triggers)
  99                 return 0;
 100
 101         phy->phy_led_triggers = devm_kzalloc(&phy->mdio.dev,
 102                                             sizeof(struct phy_led_trigger) *
 103                                                    phy->phy_num_led_triggers,
 104                                             GFP_KERNEL);
 105         if (!phy->phy_led_triggers)
 106                 return -ENOMEM;
 107
 108         for (i = 0; i < phy->phy_num_led_triggers; i++) {
 109                 err = phy_led_trigger_register(phy, &phy->phy_led_triggers[i],
 110                                                speeds[i]);
 111                 if (err)
 112                         goto out_unreg;
 113         }
 114
 115         phy->last_triggered = NULL;
 116         phy_led_trigger_change_speed(phy);
 117
 118         return 0;
 119 out_unreg:
 120         while (i--)
 121                 phy_led_trigger_unregister(&phy->phy_led_triggers[i]);
 122         devm_kfree(&phy->mdio.dev, phy->phy_led_triggers);
 123         return err;
 124 }
 125 EXPORT_SYMBOL_GPL(phy_led_triggers_register);
 126
 127 void phy_led_triggers_unregister(struct phy_device *phy)
 128 {
 129         int i;
 130
 131         for (i = 0; i < phy->phy_num_led_triggers; i++)
 132                 phy_led_trigger_unregister(&phy->phy_led_triggers[i]);
 133 }
 134 EXPORT_SYMBOL_GPL(phy_led_triggers_unregister);