cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

phy_led_triggers.c (3873B)

      1// SPDX-License-Identifier: GPL-2.0+
      2/* Copyright (C) 2016 National Instruments Corp. */
      3#include <linux/leds.h>
      4#include <linux/phy.h>
      5#include <linux/phy_led_triggers.h>
      6#include <linux/netdevice.h>
      7
      8static struct phy_led_trigger *phy_speed_to_led_trigger(struct phy_device *phy,
      9							unsigned int speed)
     10{
     11	unsigned int i;
     12
     13	for (i = 0; i < phy->phy_num_led_triggers; i++) {
     14		if (phy->phy_led_triggers[i].speed == speed)
     15			return &phy->phy_led_triggers[i];
     16	}
     17	return NULL;
     18}
     19
     20static void phy_led_trigger_no_link(struct phy_device *phy)
     21{
     22	if (phy->last_triggered) {
     23		led_trigger_event(&phy->last_triggered->trigger, LED_OFF);
     24		led_trigger_event(&phy->led_link_trigger->trigger, LED_OFF);
     25		phy->last_triggered = NULL;
     26	}
     27}
     28
     29void phy_led_trigger_change_speed(struct phy_device *phy)
     30{
     31	struct phy_led_trigger *plt;
     32
     33	if (!phy->link)
     34		return phy_led_trigger_no_link(phy);
     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		return phy_led_trigger_no_link(phy);
     45	}
     46
     47	if (plt != phy->last_triggered) {
     48		if (!phy->last_triggered)
     49			led_trigger_event(&phy->led_link_trigger->trigger,
     50					  LED_FULL);
     51		else
     52			led_trigger_event(&phy->last_triggered->trigger, LED_OFF);
     53
     54		led_trigger_event(&plt->trigger, LED_FULL);
     55		phy->last_triggered = plt;
     56	}
     57}
     58EXPORT_SYMBOL_GPL(phy_led_trigger_change_speed);
     59
     60static void phy_led_trigger_format_name(struct phy_device *phy, char *buf,
     61					size_t size, const char *suffix)
     62{
     63	snprintf(buf, size, PHY_ID_FMT ":%s",
     64		 phy->mdio.bus->id, phy->mdio.addr, suffix);
     65}
     66
     67static int phy_led_trigger_register(struct phy_device *phy,
     68				    struct phy_led_trigger *plt,
     69				    unsigned int speed,
     70				    const char *suffix)
     71{
     72	plt->speed = speed;
     73	phy_led_trigger_format_name(phy, plt->name, sizeof(plt->name), suffix);
     74	plt->trigger.name = plt->name;
     75
     76	return led_trigger_register(&plt->trigger);
     77}
     78
     79static void phy_led_trigger_unregister(struct phy_led_trigger *plt)
     80{
     81	led_trigger_unregister(&plt->trigger);
     82}
     83
     84int phy_led_triggers_register(struct phy_device *phy)
     85{
     86	int i, err;
     87	unsigned int speeds[50];
     88
     89	phy->phy_num_led_triggers = phy_supported_speeds(phy, speeds,
     90							 ARRAY_SIZE(speeds));
     91	if (!phy->phy_num_led_triggers)
     92		return 0;
     93
     94	phy->led_link_trigger = devm_kzalloc(&phy->mdio.dev,
     95					     sizeof(*phy->led_link_trigger),
     96					     GFP_KERNEL);
     97	if (!phy->led_link_trigger) {
     98		err = -ENOMEM;
     99		goto out_clear;
    100	}
    101
    102	err = phy_led_trigger_register(phy, phy->led_link_trigger, 0, "link");
    103	if (err)
    104		goto out_free_link;
    105
    106	phy->phy_led_triggers = devm_kcalloc(&phy->mdio.dev,
    107					    phy->phy_num_led_triggers,
    108					    sizeof(struct phy_led_trigger),
    109					    GFP_KERNEL);
    110	if (!phy->phy_led_triggers) {
    111		err = -ENOMEM;
    112		goto out_unreg_link;
    113	}
    114
    115	for (i = 0; i < phy->phy_num_led_triggers; i++) {
    116		err = phy_led_trigger_register(phy, &phy->phy_led_triggers[i],
    117					       speeds[i],
    118					       phy_speed_to_str(speeds[i]));
    119		if (err)
    120			goto out_unreg;
    121	}
    122
    123	phy->last_triggered = NULL;
    124	phy_led_trigger_change_speed(phy);
    125
    126	return 0;
    127out_unreg:
    128	while (i--)
    129		phy_led_trigger_unregister(&phy->phy_led_triggers[i]);
    130	devm_kfree(&phy->mdio.dev, phy->phy_led_triggers);
    131out_unreg_link:
    132	phy_led_trigger_unregister(phy->led_link_trigger);
    133out_free_link:
    134	devm_kfree(&phy->mdio.dev, phy->led_link_trigger);
    135	phy->led_link_trigger = NULL;
    136out_clear:
    137	phy->phy_num_led_triggers = 0;
    138	return err;
    139}
    140EXPORT_SYMBOL_GPL(phy_led_triggers_register);
    141
    142void phy_led_triggers_unregister(struct phy_device *phy)
    143{
    144	int i;
    145
    146	for (i = 0; i < phy->phy_num_led_triggers; i++)
    147		phy_led_trigger_unregister(&phy->phy_led_triggers[i]);
    148
    149	if (phy->led_link_trigger)
    150		phy_led_trigger_unregister(phy->led_link_trigger);
    151}
    152EXPORT_SYMBOL_GPL(phy_led_triggers_unregister);