dsa_sandbox.c 3.8 KB

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