diff options
Diffstat (limited to 'drivers/net/dsa_sandbox.c')
| -rw-r--r-- | drivers/net/dsa_sandbox.c | 179 |
1 files changed, 179 insertions, 0 deletions
diff --git a/drivers/net/dsa_sandbox.c b/drivers/net/dsa_sandbox.c new file mode 100644 index 0000000000..4b62670e5d --- /dev/null +++ b/drivers/net/dsa_sandbox.c @@ -0,0 +1,179 @@ +// SPDX-License-Identifier: GPL-2.0+ +/* + * Copyright 2019-2021 NXP Semiconductors + */ + +#include <asm/eth.h> +#include <net/dsa.h> +#include <net.h> + +#define DSA_SANDBOX_MAGIC 0x00415344 +#define DSA_SANDBOX_TAG_LEN sizeof(struct dsa_sandbox_tag) + +struct dsa_sandbox_priv { + struct eth_sandbox_priv *master_priv; + int port_en_mask; +}; + +struct dsa_sandbox_tag { + u32 magic; + u32 port; +}; + +static bool sb_dsa_port_enabled(struct udevice *dev, int port) +{ + struct dsa_sandbox_priv *priv = dev_get_priv(dev); + + return priv->port_en_mask & BIT(port); +} + +static bool sb_dsa_master_enabled(struct udevice *dev) +{ + struct dsa_sandbox_priv *priv = dev_get_priv(dev); + + return !priv->master_priv->disabled; +} + +static int dsa_sandbox_port_enable(struct udevice *dev, int port, + struct phy_device *phy) +{ + struct dsa_sandbox_priv *priv = dev_get_priv(dev); + + if (!sb_dsa_master_enabled(dev)) + return -EFAULT; + + priv->port_en_mask |= BIT(port); + + return 0; +} + +static void dsa_sandbox_port_disable(struct udevice *dev, int port, + struct phy_device *phy) +{ + struct dsa_sandbox_priv *priv = dev_get_priv(dev); + + priv->port_en_mask &= ~BIT(port); +} + +static int dsa_sandbox_xmit(struct udevice *dev, int port, void *packet, + int length) +{ + struct dsa_sandbox_tag *tag = packet; + + if (!sb_dsa_master_enabled(dev)) + return -EFAULT; + + if (!sb_dsa_port_enabled(dev, port)) + return -EFAULT; + + tag->magic = DSA_SANDBOX_MAGIC; + tag->port = port; + + return 0; +} + +static int dsa_sandbox_rcv(struct udevice *dev, int *port, void *packet, + int length) +{ + struct dsa_sandbox_tag *tag = packet; + + if (!sb_dsa_master_enabled(dev)) + return -EFAULT; + + if (tag->magic != DSA_SANDBOX_MAGIC) + return -EFAULT; + + *port = tag->port; + if (!sb_dsa_port_enabled(dev, tag->port)) + return -EFAULT; + + return 0; +} + +static const struct dsa_ops dsa_sandbox_ops = { + .port_enable = dsa_sandbox_port_enable, + .port_disable = dsa_sandbox_port_disable, + .xmit = dsa_sandbox_xmit, + .rcv = dsa_sandbox_rcv, +}; + +static int sb_dsa_handler(struct udevice *dev, void *packet, + unsigned int len) +{ + struct eth_sandbox_priv *master_priv; + struct dsa_sandbox_tag *tag = packet; + struct udevice *dsa_dev; + u32 port_index; + void *rx_buf; + int i; + + /* this emulates the switch hw and the network side */ + if (tag->magic != DSA_SANDBOX_MAGIC) + return -EFAULT; + + port_index = tag->port; + master_priv = dev_get_priv(dev); + dsa_dev = master_priv->priv; + if (!sb_dsa_port_enabled(dsa_dev, port_index)) + return -EFAULT; + + packet += DSA_SANDBOX_TAG_LEN; + len -= DSA_SANDBOX_TAG_LEN; + + if (!sandbox_eth_arp_req_to_reply(dev, packet, len)) + goto dsa_tagging; + if (!sandbox_eth_ping_req_to_reply(dev, packet, len)) + goto dsa_tagging; + + return 0; + +dsa_tagging: + master_priv->recv_packets--; + i = master_priv->recv_packets; + rx_buf = master_priv->recv_packet_buffer[i]; + len = master_priv->recv_packet_length[i]; + memmove(rx_buf + DSA_SANDBOX_TAG_LEN, rx_buf, len); + + tag = rx_buf; + tag->magic = DSA_SANDBOX_MAGIC; + tag->port = port_index; + len += DSA_SANDBOX_TAG_LEN; + master_priv->recv_packet_length[i] = len; + master_priv->recv_packets++; + + return 0; +} + +static int dsa_sandbox_probe(struct udevice *dev) +{ + struct dsa_sandbox_priv *priv = dev_get_priv(dev); + struct udevice *master = dsa_get_master(dev); + struct eth_sandbox_priv *master_priv; + + if (!master) + return -ENODEV; + + dsa_set_tagging(dev, DSA_SANDBOX_TAG_LEN, 0); + + master_priv = dev_get_priv(master); + master_priv->priv = dev; + master_priv->tx_handler = sb_dsa_handler; + + priv->master_priv = master_priv; + + return 0; +} + +static const struct udevice_id dsa_sandbox_ids[] = { + { .compatible = "sandbox,dsa" }, + { } +}; + +U_BOOT_DRIVER(dsa_sandbox) = { + .name = "dsa_sandbox", + .id = UCLASS_DSA, + .of_match = dsa_sandbox_ids, + .probe = dsa_sandbox_probe, + .ops = &dsa_sandbox_ops, + .priv_auto = sizeof(struct dsa_sandbox_priv), +}; |