1/*
  2 * Handling of a master device, switching frames via its switch fabric CPU port
  3 *
  4 * Copyright (c) 2017 Savoir-faire Linux Inc.
  5 *	Vivien Didelot <vivien.didelot@savoirfairelinux.com>
  6 *
  7 * This program is free software; you can redistribute it and/or modify
  8 * it under the terms of the GNU General Public License as published by
  9 * the Free Software Foundation; either version 2 of the License, or
 10 * (at your option) any later version.
 11 */
 12
 13#include "dsa_priv.h"
 14
 15static void dsa_master_get_ethtool_stats(struct net_device *dev,
 16					 struct ethtool_stats *stats,
 17					 uint64_t *data)
 18{
 19	struct dsa_port *cpu_dp = dev->dsa_ptr;
 20	const struct ethtool_ops *ops = cpu_dp->orig_ethtool_ops;
 21	struct dsa_switch *ds = cpu_dp->ds;
 22	int port = cpu_dp->index;
 23	int count = 0;
 24
 25	if (ops && ops->get_sset_count && ops->get_ethtool_stats) {
 26		count = ops->get_sset_count(dev, ETH_SS_STATS);
 27		ops->get_ethtool_stats(dev, stats, data);
 28	}
 29
 30	if (ds->ops->get_ethtool_stats)
 31		ds->ops->get_ethtool_stats(ds, port, data + count);
 32}
 33
 34static int dsa_master_get_sset_count(struct net_device *dev, int sset)
 35{
 36	struct dsa_port *cpu_dp = dev->dsa_ptr;
 37	const struct ethtool_ops *ops = cpu_dp->orig_ethtool_ops;
 38	struct dsa_switch *ds = cpu_dp->ds;
 39	int count = 0;
 40
 41	if (ops && ops->get_sset_count)
 42		count += ops->get_sset_count(dev, sset);
 43
 44	if (sset == ETH_SS_STATS && ds->ops->get_sset_count)
 45		count += ds->ops->get_sset_count(ds, cpu_dp->index);
 46
 47	return count;
 48}
 49
 50static void dsa_master_get_strings(struct net_device *dev, uint32_t stringset,
 51				   uint8_t *data)
 52{
 53	struct dsa_port *cpu_dp = dev->dsa_ptr;
 54	const struct ethtool_ops *ops = cpu_dp->orig_ethtool_ops;
 55	struct dsa_switch *ds = cpu_dp->ds;
 56	int port = cpu_dp->index;
 57	int len = ETH_GSTRING_LEN;
 58	int mcount = 0, count;
 59	unsigned int i;
 60	uint8_t pfx[4];
 61	uint8_t *ndata;
 62
 63	snprintf(pfx, sizeof(pfx), "p%.2d", port);
 64	/* We do not want to be NULL-terminated, since this is a prefix */
 65	pfx[sizeof(pfx) - 1] = '_';
 66
 67	if (ops && ops->get_sset_count && ops->get_strings) {
 68		mcount = ops->get_sset_count(dev, ETH_SS_STATS);
 69		ops->get_strings(dev, stringset, data);
 70	}
 71
 72	if (stringset == ETH_SS_STATS && ds->ops->get_strings) {
 73		ndata = data + mcount * len;
 74		/* This function copies ETH_GSTRINGS_LEN bytes, we will mangle
 75		 * the output after to prepend our CPU port prefix we
 76		 * constructed earlier
 77		 */
 78		ds->ops->get_strings(ds, port, ndata);
 79		count = ds->ops->get_sset_count(ds, port);
 80		for (i = 0; i < count; i++) {
 81			memmove(ndata + (i * len + sizeof(pfx)),
 82				ndata + i * len, len - sizeof(pfx));
 83			memcpy(ndata + i * len, pfx, sizeof(pfx));
 84		}
 85	}
 86}
 87
 88static int dsa_master_ethtool_setup(struct net_device *dev)
 89{
 90	struct dsa_port *cpu_dp = dev->dsa_ptr;
 91	struct dsa_switch *ds = cpu_dp->ds;
 92	struct ethtool_ops *ops;
 93
 94	ops = devm_kzalloc(ds->dev, sizeof(*ops), GFP_KERNEL);
 95	if (!ops)
 96		return -ENOMEM;
 97
 98	cpu_dp->orig_ethtool_ops = dev->ethtool_ops;
 99	if (cpu_dp->orig_ethtool_ops)
100		memcpy(ops, cpu_dp->orig_ethtool_ops, sizeof(*ops));
101
102	ops->get_sset_count = dsa_master_get_sset_count;
103	ops->get_ethtool_stats = dsa_master_get_ethtool_stats;
104	ops->get_strings = dsa_master_get_strings;
105
106	dev->ethtool_ops = ops;
107
108	return 0;
109}
110
111static void dsa_master_ethtool_teardown(struct net_device *dev)
112{
113	struct dsa_port *cpu_dp = dev->dsa_ptr;
114
115	dev->ethtool_ops = cpu_dp->orig_ethtool_ops;
116	cpu_dp->orig_ethtool_ops = NULL;
117}
118
119int dsa_master_setup(struct net_device *dev, struct dsa_port *cpu_dp)
120{
121	/* If we use a tagging format that doesn't have an ethertype
122	 * field, make sure that all packets from this point on get
123	 * sent to the tag format's receive function.
124	 */
125	wmb();
126
127	dev->dsa_ptr = cpu_dp;
128
129	return dsa_master_ethtool_setup(dev);
130}
131
132void dsa_master_teardown(struct net_device *dev)
133{
134	dsa_master_ethtool_teardown(dev);
135
136	dev->dsa_ptr = NULL;
137
138	/* If we used a tagging format that doesn't have an ethertype
139	 * field, make sure that all packets from this point get sent
140	 * without the tag and go through the regular receive path.
141	 */
142	wmb();
143}