drivers/net/dsa_sandbox.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * Copyright 2019-2021 NXP Semiconductors
   4  */
   5
   6 #include <asm/eth.h>
   7 #include <net/dsa.h>
   8 #include <net.h>
   9
  10 #define DSA_SANDBOX_MAGIC       0x00415344
  11 #define DSA_SANDBOX_TAG_LEN     sizeof(struct dsa_sandbox_tag)
  12
  13 struct dsa_sandbox_priv {
  14         struct eth_sandbox_priv *master_priv;
  15         int port_en_mask;
  16 };
  17
  18 struct dsa_sandbox_tag {
  19         u32 magic;
  20         u32 port;
  21 };
  22
  23 static bool sb_dsa_port_enabled(struct udevice *dev, int port)
  24 {
  25         struct dsa_sandbox_priv *priv = dev_get_priv(dev);
  26
  27         return priv->port_en_mask & BIT(port);
  28 }
  29
  30 static bool sb_dsa_master_enabled(struct udevice *dev)
  31 {
  32         struct dsa_sandbox_priv *priv = dev_get_priv(dev);
  33
  34         return !priv->master_priv->disabled;
  35 }
  36
  37 static int dsa_sandbox_port_enable(struct udevice *dev, int port,
  38                                    struct phy_device *phy)
  39 {
  40         struct dsa_sandbox_priv *priv = dev_get_priv(dev);
  41
  42         if (!sb_dsa_master_enabled(dev))
  43                 return -EFAULT;
  44
  45         priv->port_en_mask |= BIT(port);
  46
  47         return 0;
  48 }
  49
  50 static void dsa_sandbox_port_disable(struct udevice *dev, int port,
  51                                      struct phy_device *phy)
  52 {
  53         struct dsa_sandbox_priv *priv = dev_get_priv(dev);
  54
  55         priv->port_en_mask &= ~BIT(port);
  56 }
  57
  58 static int dsa_sandbox_xmit(struct udevice *dev, int port, void *packet,
  59                             int length)
  60 {
  61         struct dsa_sandbox_tag *tag = packet;
  62
  63         if (!sb_dsa_master_enabled(dev))
  64                 return -EFAULT;
  65
  66         if (!sb_dsa_port_enabled(dev, port))
  67                 return -EFAULT;
  68
  69         tag->magic = DSA_SANDBOX_MAGIC;
  70         tag->port = port;
  71
  72         return 0;
  73 }
  74
  75 static int dsa_sandbox_rcv(struct udevice *dev, int *port, void *packet,
  76                            int length)
  77 {
  78         struct dsa_sandbox_tag *tag = packet;
  79
  80         if (!sb_dsa_master_enabled(dev))
  81                 return -EFAULT;
  82
  83         if (tag->magic != DSA_SANDBOX_MAGIC)
  84                 return -EFAULT;
  85
  86         *port = tag->port;
  87         if (!sb_dsa_port_enabled(dev, tag->port))
  88                 return -EFAULT;
  89
  90         return 0;
  91 }
  92
  93 static const struct dsa_ops dsa_sandbox_ops = {
  94         .port_enable = dsa_sandbox_port_enable,
  95         .port_disable = dsa_sandbox_port_disable,
  96         .xmit = dsa_sandbox_xmit,
  97         .rcv = dsa_sandbox_rcv,
  98 };
  99
 100 static int sb_dsa_handler(struct udevice *dev, void *packet,
 101                           unsigned int len)
 102 {
 103         struct eth_sandbox_priv *master_priv;
 104         struct dsa_sandbox_tag *tag = packet;
 105         struct udevice *dsa_dev;
 106         u32 port_index;
 107         void *rx_buf;
 108         int i;
 109
 110         /* this emulates the switch hw and the network side */
 111         if (tag->magic != DSA_SANDBOX_MAGIC)
 112                 return -EFAULT;
 113
 114         port_index = tag->port;
 115         master_priv = dev_get_priv(dev);
 116         dsa_dev = master_priv->priv;
 117         if (!sb_dsa_port_enabled(dsa_dev, port_index))
 118                 return -EFAULT;
 119
 120         packet += DSA_SANDBOX_TAG_LEN;
 121         len -= DSA_SANDBOX_TAG_LEN;
 122
 123         if (!sandbox_eth_arp_req_to_reply(dev, packet, len))
 124                 goto dsa_tagging;
 125         if (!sandbox_eth_ping_req_to_reply(dev, packet, len))
 126                 goto dsa_tagging;
 127
 128         return 0;
 129
 130 dsa_tagging:
 131         master_priv->recv_packets--;
 132         i = master_priv->recv_packets;
 133         rx_buf = master_priv->recv_packet_buffer[i];
 134         len = master_priv->recv_packet_length[i];
 135         memmove(rx_buf + DSA_SANDBOX_TAG_LEN, rx_buf, len);
 136
 137         tag = rx_buf;
 138         tag->magic = DSA_SANDBOX_MAGIC;
 139         tag->port = port_index;
 140         len += DSA_SANDBOX_TAG_LEN;
 141         master_priv->recv_packet_length[i] = len;
 142         master_priv->recv_packets++;
 143
 144         return 0;
 145 }
 146
 147 static int dsa_sandbox_probe(struct udevice *dev)
 148 {
 149         struct dsa_sandbox_priv *priv = dev_get_priv(dev);
 150         struct udevice *master = dsa_get_master(dev);
 151         struct eth_sandbox_priv *master_priv;
 152
 153         if (!master)
 154                 return -ENODEV;
 155
 156         dsa_set_tagging(dev, DSA_SANDBOX_TAG_LEN, 0);
 157
 158         master_priv = dev_get_priv(master);
 159         master_priv->priv = dev;
 160         master_priv->tx_handler = sb_dsa_handler;
 161
 162         priv->master_priv = master_priv;
 163
 164         return 0;
 165 }
 166
 167 static const struct udevice_id dsa_sandbox_ids[] = {
 168         { .compatible = "sandbox,dsa" },
 169         { }
 170 };
 171
 172 U_BOOT_DRIVER(dsa_sandbox) = {
 173         .name           = "dsa_sandbox",
 174         .id             = UCLASS_DSA,
 175         .of_match       = dsa_sandbox_ids,
 176         .probe          = dsa_sandbox_probe,
 177         .ops            = &dsa_sandbox_ops,
 178         .priv_auto      = sizeof(struct dsa_sandbox_priv),
 179 };