Date:   Tue, 21 May 2019 15:30:04 -0400
From:   Vivien Didelot <vivien.didelot@...il.com>
To:     netdev@...r.kernel.org
Cc:     cphealy@...il.com, Vivien Didelot <vivien.didelot@...il.com>,
        Florian Fainelli <f.fainelli@...il.com>,
        Andrew Lunn <andrew@...n.ch>,
        "David S. Miller" <davem@...emloft.net>
Subject: [RFC net-next 9/9] net: dsa: mv88e6xxx: setup RMU bus

Implement the RMU register operations Read and Write and setup a
proper bus to use as an alternative for SMI.

Signed-off-by: Vivien Didelot <vivien.didelot@...il.com>
---
 drivers/net/dsa/mv88e6xxx/chip.c |  14 ++
 drivers/net/dsa/mv88e6xxx/chip.h |  10 ++
 drivers/net/dsa/mv88e6xxx/rmu.c  | 256 +++++++++++++++++++++++++++++++
 drivers/net/dsa/mv88e6xxx/rmu.h  |   1 +
 4 files changed, 281 insertions(+)

diff --git a/drivers/net/dsa/mv88e6xxx/chip.c b/drivers/net/dsa/mv88e6xxx/chip.c
index 048fdaf1335e..68c247e951aa 100644
--- a/drivers/net/dsa/mv88e6xxx/chip.c
+++ b/drivers/net/dsa/mv88e6xxx/chip.c
@@ -4573,6 +4573,19 @@ static int mv88e6xxx_port_egress_floods(struct dsa_switch *ds, int port,
 	return err;
 }
 
+static bool mv88e6xxx_rcv(struct dsa_switch *ds, struct sk_buff *skb)
+{
+	struct mv88e6xxx_chip *chip = ds->priv;
+
+	/* When this operation is called after a request,
+	 * the mutex must already be held.
+	 */
+	if (mutex_is_locked(&chip->reg_lock))
+		return !!mv88e6xxx_rmu_response(chip, skb);
+
+	return false;
+}
+
 static const struct dsa_switch_ops mv88e6xxx_switch_ops = {
 	.get_tag_protocol	= mv88e6xxx_get_tag_protocol,
 	.setup			= mv88e6xxx_setup,
@@ -4617,6 +4630,7 @@ static const struct dsa_switch_ops mv88e6xxx_switch_ops = {
 	.port_txtstamp		= mv88e6xxx_port_txtstamp,
 	.port_rxtstamp		= mv88e6xxx_port_rxtstamp,
 	.get_ts_info		= mv88e6xxx_get_ts_info,
+	.rcv			= mv88e6xxx_rcv,
 };
 
 static int mv88e6xxx_register_switch(struct mv88e6xxx_chip *chip)
diff --git a/drivers/net/dsa/mv88e6xxx/chip.h b/drivers/net/dsa/mv88e6xxx/chip.h
index 2af574169e14..bf328cea67c8 100644
--- a/drivers/net/dsa/mv88e6xxx/chip.h
+++ b/drivers/net/dsa/mv88e6xxx/chip.h
@@ -12,6 +12,7 @@
 #ifndef _MV88E6XXX_CHIP_H
 #define _MV88E6XXX_CHIP_H
 
+#include <linux/completion.h>
 #include <linux/if_vlan.h>
 #include <linux/irq.h>
 #include <linux/gpio/consumer.h>
@@ -214,6 +215,15 @@ struct mv88e6xxx_chip {
 	struct mii_bus *bus;
 	int sw_addr;
 
+	/* Register access through the Remote Management Unit */
+	const struct mv88e6xxx_bus_ops *rmu_ops;
+	struct completion rmu_response_received;
+	const unsigned char *rmu_response_data;
+	size_t rmu_response_data_len;
+	struct sk_buff *rmu_response;
+	struct net_device *rmu_dev;
+	u8 rmu_sequence_num;
+
 	/* Handles automatic disabling and re-enabling of the PHY
 	 * polling unit.
 	 */
diff --git a/drivers/net/dsa/mv88e6xxx/rmu.c b/drivers/net/dsa/mv88e6xxx/rmu.c
index 71dabe6ecb46..b7dbd843753a 100644
--- a/drivers/net/dsa/mv88e6xxx/rmu.c
+++ b/drivers/net/dsa/mv88e6xxx/rmu.c
@@ -9,9 +9,255 @@
  * (at your option) any later version.
  */
 
+#include <linux/if_ether.h>
+
 #include "chip.h"
 #include "rmu.h"
 
+#define MV88E6XXX_RMU_TIMEOUT (msecs_to_jiffies(1000))
+
+static int mv88e6xxx_rmu_wait_response(struct mv88e6xxx_chip *chip)
+{
+	long timeout;
+
+	timeout = wait_for_completion_interruptible_timeout(&chip->rmu_response_received, MV88E6XXX_RMU_TIMEOUT);
+	if (timeout < 0)
+		return timeout;
+	if (timeout == 0)
+		return -ETIMEDOUT;
+
+	dev_dbg(chip->dev, "got RMU response for request %d in %d msecs\n",
+		chip->rmu_sequence_num, jiffies_to_msecs(MV88E6XXX_RMU_TIMEOUT - timeout));
+
+	return 0;
+}
+
+#define DSA_LEN		4
+
+struct edsahdr {
+	unsigned char	eth_dest_addr[ETH_ALEN];
+	unsigned char	eth_src_addr[ETH_ALEN];
+	__be16		edsa_ethertype;
+	__be16		edsa_reserved; /* 0x0000 */
+	unsigned char	dsa_tag[DSA_LEN];
+	__be16		eth_ethertype;
+} __attribute__((packed));
+
+struct dsahdr {
+	unsigned char	eth_dest_addr[ETH_ALEN];
+	unsigned char	eth_src_addr[ETH_ALEN];
+	unsigned char	dsa_tag[DSA_LEN];
+	__be16		eth_ethertype;
+} __attribute__((packed));
+
+struct mv88e6xxx_rmu_request {
+	__be16	format;
+	__be16	pad; /* 0x0000 on request, Prod Num/Rev on response */
+	__be16	code;
+} __attribute__((packed));
+
+static int mv88e6xxx_rmu_request(struct mv88e6xxx_chip *chip, u16 code, u8 *data, size_t len)
+{
+	const unsigned char dest_addr[ETH_ALEN] = { 0x01, 0x50, 0x43, 0x00, 0x00, 0x00 };
+	struct net_device *dev = chip->rmu_dev;
+	struct mv88e6xxx_rmu_request *req;
+	unsigned char *eth_dest_addr;
+	unsigned char *eth_src_addr;
+	unsigned char *dsa_tag;
+	__be16 *eth_ethertype;
+	struct edsahdr *edsa;
+	struct sk_buff *skb;
+	struct dsahdr *dsa;
+
+	if (!dev)
+		return -EOPNOTSUPP;
+
+	switch (dev->dsa_ptr->tag_ops->proto) {
+	case DSA_TAG_PROTO_DSA:
+		skb = alloc_skb(sizeof(*dsa) + sizeof(*req) + len, GFP_KERNEL);
+		if (!skb)
+			return -ENOMEM;
+
+		dsa = skb_put(skb, sizeof(*dsa));
+		eth_dest_addr = dsa->eth_dest_addr;
+		eth_src_addr = dsa->eth_src_addr;
+		dsa_tag = dsa->dsa_tag;
+		eth_ethertype = &dsa->eth_ethertype;
+		break;
+	case DSA_TAG_PROTO_EDSA:
+		skb = alloc_skb(sizeof(*edsa) + sizeof(*req) + len, GFP_KERNEL);
+		if (!skb)
+			return -ENOMEM;
+
+		edsa = skb_put(skb, sizeof(*edsa));
+		eth_dest_addr = edsa->eth_dest_addr;
+		eth_src_addr = edsa->eth_src_addr;
+		edsa->edsa_ethertype = htons(ETH_P_EDSA);
+		edsa->edsa_reserved = 0x0000;
+		dsa_tag = edsa->dsa_tag;
+		eth_ethertype = &edsa->eth_ethertype;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ether_addr_copy(eth_dest_addr, dest_addr); /* Marvell broadcast or switch MAC */
+	ether_addr_copy(eth_src_addr, dev->dev_addr);
+	dsa_tag[0] = 0x40 | (chip->ds->index & 0x1f); /* From_CPU */
+	dsa_tag[1] = 0xfa;
+	dsa_tag[2] = 0xf;
+	dsa_tag[3] = ++chip->rmu_sequence_num;
+	*eth_ethertype = htons(ETH_P_EDSA); /* User defined, useless really */
+
+	req = skb_put(skb, sizeof(*req));
+	req->format = htons(MV88E6XXX_RMU_REQUEST_FORMAT_SOHO);
+	req->pad = 0x0000;
+	req->code = htons(code);
+
+	skb_put_data(skb, data, len);
+
+	skb->dev = dev;
+
+	dsa_switch_xmit(chip->ds, skb);
+
+	return mv88e6xxx_rmu_wait_response(chip);
+}
+
+int mv88e6xxx_rmu_response(struct mv88e6xxx_chip *chip, struct sk_buff *skb)
+{
+	struct net_device *dev = chip->rmu_dev;
+	struct mv88e6xxx_rmu_request *req;
+	unsigned char *dsa_tag;
+	size_t data_offset; 
+	size_t req_offset;
+
+	/* Check if RMU is enabled */
+	if (dev != skb->dev)
+		return -EOPNOTSUPP;
+
+	if (chip->rmu_response)
+		return -EBUSY;
+
+	switch (dev->dsa_ptr->tag_ops->proto) {
+	case DSA_TAG_PROTO_DSA:
+		dsa_tag = skb->data - 2;
+		req_offset = DSA_LEN + ETH_TLEN - 2;
+		break;
+	case DSA_TAG_PROTO_EDSA:
+		/* skb->data points to the end of the (EDSA) ethertype */
+		dsa_tag = skb->data + 2;
+		req_offset = 2 + DSA_LEN + ETH_TLEN;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if ((dsa_tag[0] != chip->ds->index) ||
+            (dsa_tag[1] != 0x00) ||
+            ((dsa_tag[2] & 0x1f) != 0x1f) ||
+            (dsa_tag[3] != chip->rmu_sequence_num))
+		return -EINVAL;
+
+	data_offset = req_offset + sizeof(*req);
+	if (skb->len < data_offset)
+		return -EINVAL;
+
+	req = (struct mv88e6xxx_rmu_request *)(skb->data + req_offset);
+	if (ntohs(req->code) == 0xffff)
+		return -EINVAL;
+
+	chip->rmu_response_data_len = skb->len - data_offset;
+	if (chip->rmu_response_data_len > 0)
+		chip->rmu_response_data = skb->data + data_offset;
+	else
+		chip->rmu_response_data = NULL;
+
+	chip->rmu_response = skb_clone(skb, GFP_KERNEL);
+	if (!chip->rmu_response)
+		return -ENOMEM;
+
+	complete(&chip->rmu_response_received);
+
+	return 0;
+}
+
+static int mv88e6xxx_rmu_reg_read(struct mv88e6xxx_chip *chip,
+				  int dev, int reg, u16 *data)
+{
+	unsigned char request_data[8];
+	int err;
+
+	request_data[0] = 0x08 | ((dev >> 3) & 0x03);
+	request_data[1] = ((dev << 5) & 0xe0) | (reg & 0x1f);
+	request_data[2] = 0x00;
+	request_data[3] = 0x00;
+
+	/* End Of List Command */
+	memset(&request_data[4], 0xff, 4);
+
+	err = mv88e6xxx_rmu_request(chip, MV88E6XXX_RMU_REQUEST_CODE_READ_WRITE, request_data, sizeof(request_data));
+	if (err)
+		return err;
+
+	if (chip->rmu_response_data_len < sizeof(request_data))
+		err = -EINVAL;
+	else
+		*data = (chip->rmu_response_data[2] << 8) | chip->rmu_response_data[3];
+
+	kfree_skb(chip->rmu_response);
+	chip->rmu_response = NULL;
+
+	return err;
+}
+
+static int mv88e6xxx_rmu_reg_write(struct mv88e6xxx_chip *chip,
+				   int dev, int reg, u16 data)
+{
+	unsigned char request_data[8];
+	int err;
+
+	request_data[0] = 0x04 | ((dev >> 3) & 0x03);
+	request_data[1] = ((dev << 5) & 0xe0) | (reg & 0x1f);
+	request_data[2] = data >> 8;
+	request_data[3] = data & 0xff;
+
+	/* End Of List Command */
+	memset(&request_data[4], 0xff, 4);
+
+	err = mv88e6xxx_rmu_request(chip, MV88E6XXX_RMU_REQUEST_CODE_READ_WRITE, request_data, sizeof(request_data));
+	if (err)
+		return err;
+
+	if (chip->rmu_response_data_len < sizeof(request_data))
+		err = -EINVAL;
+
+	kfree_skb(chip->rmu_response);
+	chip->rmu_response = NULL;
+
+	return err;
+}
+
+static const struct mv88e6xxx_bus_ops mv88e6xxx_rmu_ops = {
+	.read = mv88e6xxx_rmu_reg_read,
+	.write = mv88e6xxx_rmu_reg_write,
+};
+
+static int mv88e6xxx_rmu_setup_bus(struct mv88e6xxx_chip *chip,
+				      struct net_device *dev)
+{
+	chip->rmu_ops = &mv88e6xxx_rmu_ops;
+	chip->rmu_dev = dev;
+
+	init_completion(&chip->rmu_response_received);
+
+	dev_info(chip->dev, "RMU reachable via %s\n", netdev_name(dev));
+
+	if (!chip->ops)
+		chip->ops = chip->rmu_ops;
+
+	return 0;
+}
+
 static int mv88e6xxx_rmu_setup_port(struct mv88e6xxx_chip *chip, int port)
 {
 	int err;
@@ -40,6 +286,7 @@ static int mv88e6xxx_rmu_setup_port(struct mv88e6xxx_chip *chip, int port)
 int mv88e6xxx_rmu_setup(struct mv88e6xxx_chip *chip)
 {
 	struct dsa_switch *ds = chip->ds;
+	struct net_device *dev;
 	int port;
 	int err;
 
@@ -50,6 +297,15 @@ int mv88e6xxx_rmu_setup(struct mv88e6xxx_chip *chip)
 			if (err)
 				continue;
 
+			/* When the control CPU is local, use the master interface */
+			dev = dsa_to_master(ds, port);
+			if (!dev)
+				return -ENODEV;
+
+			err = mv88e6xxx_rmu_setup_bus(chip, dev);
+			if (err)
+				return err;
+
 			return 0;
 		}
 	}
diff --git a/drivers/net/dsa/mv88e6xxx/rmu.h b/drivers/net/dsa/mv88e6xxx/rmu.h
index f7d849b169d2..099df0789bb5 100644
--- a/drivers/net/dsa/mv88e6xxx/rmu.h
+++ b/drivers/net/dsa/mv88e6xxx/rmu.h
@@ -29,5 +29,6 @@
 #include "chip.h"
 
 int mv88e6xxx_rmu_setup(struct mv88e6xxx_chip *chip);
+int mv88e6xxx_rmu_response(struct mv88e6xxx_chip *chip, struct sk_buff *skb);
 
 #endif /* _MV88E6XXX_RMU_H */
-- 
2.21.0

Powered by blists - more mailing lists