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);