LXR linux/drivers/net/phy/phy_led

   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
  18static 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
  30void 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
  55out_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}
  62EXPORT_SYMBOL_GPL(phy_led_trigger_change_speed);
  63
  64static 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
  87static void phy_led_trigger_unregister(struct phy_led_trigger *plt)
  88{
  89        led_trigger_unregister(&plt->trigger);
  90}
  91
  92int 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;
 122out_unreg:
 123        while (i--)
 124                phy_led_trigger_unregister(&phy->phy_led_triggers[i]);
 125        devm_kfree(&phy->mdio.dev, phy->phy_led_triggers);
 126out_clear:
 127        phy->phy_num_led_triggers = 0;
 128        return err;
 129}
 130EXPORT_SYMBOL_GPL(phy_led_triggers_register);
 131
 132void 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}
 139EXPORT_SYMBOL_GPL(phy_led_triggers_unregister);
 140