drivers/net/sandbox.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright (c) 2015 National Instruments
   4  *
   5  * (C) Copyright 2015
   6  * Joe Hershberger <joe.hershberger@ni.com>
   7  */
   8
   9 #include <common.h>
  10 #include <dm.h>
  11 #include <log.h>
  12 #include <malloc.h>
  13 #include <net.h>
  14 #include <asm/eth.h>
  15 #include <asm/global_data.h>
  16 #include <asm/test.h>
  17
  18 DECLARE_GLOBAL_DATA_PTR;
  19
  20 static bool skip_timeout;
  21
  22 /*
  23  * sandbox_eth_disable_response()
  24  *
  25  * index - The alias index (also DM seq number)
  26  * disable - If non-zero, ignore sent packets and don't send mock response
  27  */
  28 void sandbox_eth_disable_response(int index, bool disable)
  29 {
  30         struct udevice *dev;
  31         struct eth_sandbox_priv *priv;
  32         int ret;
  33
  34         ret = uclass_get_device(UCLASS_ETH, index, &dev);
  35         if (ret)
  36                 return;
  37
  38         priv = dev_get_priv(dev);
  39         priv->disabled = disable;
  40 }
  41
  42 /*
  43  * sandbox_eth_skip_timeout()
  44  *
  45  * When the first packet read is attempted, fast-forward time
  46  */
  47 void sandbox_eth_skip_timeout(void)
  48 {
  49         skip_timeout = true;
  50 }
  51
  52 /*
  53  * sandbox_eth_arp_req_to_reply()
  54  *
  55  * Check for an arp request to be sent. If so, inject a reply
  56  *
  57  * returns 0 if injected, -EAGAIN if not
  58  */
  59 int sandbox_eth_arp_req_to_reply(struct udevice *dev, void *packet,
  60                                  unsigned int len)
  61 {
  62         struct eth_sandbox_priv *priv = dev_get_priv(dev);
  63         struct ethernet_hdr *eth = packet;
  64         struct arp_hdr *arp;
  65         struct ethernet_hdr *eth_recv;
  66         struct arp_hdr *arp_recv;
  67
  68         if (ntohs(eth->et_protlen) != PROT_ARP)
  69                 return -EAGAIN;
  70
  71         arp = packet + ETHER_HDR_SIZE;
  72
  73         if (ntohs(arp->ar_op) != ARPOP_REQUEST)
  74                 return -EAGAIN;
  75
  76         /* Don't allow the buffer to overrun */
  77         if (priv->recv_packets >= PKTBUFSRX)
  78                 return 0;
  79
  80         /* store this as the assumed IP of the fake host */
  81         priv->fake_host_ipaddr = net_read_ip(&arp->ar_tpa);
  82
  83         /* Formulate a fake response */
  84         eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];
  85         memcpy(eth_recv->et_dest, eth->et_src, ARP_HLEN);
  86         memcpy(eth_recv->et_src, priv->fake_host_hwaddr, ARP_HLEN);
  87         eth_recv->et_protlen = htons(PROT_ARP);
  88
  89         arp_recv = (void *)eth_recv + ETHER_HDR_SIZE;
  90         arp_recv->ar_hrd = htons(ARP_ETHER);
  91         arp_recv->ar_pro = htons(PROT_IP);
  92         arp_recv->ar_hln = ARP_HLEN;
  93         arp_recv->ar_pln = ARP_PLEN;
  94         arp_recv->ar_op = htons(ARPOP_REPLY);
  95         memcpy(&arp_recv->ar_sha, priv->fake_host_hwaddr, ARP_HLEN);
  96         net_write_ip(&arp_recv->ar_spa, priv->fake_host_ipaddr);
  97         memcpy(&arp_recv->ar_tha, &arp->ar_sha, ARP_HLEN);
  98         net_copy_ip(&arp_recv->ar_tpa, &arp->ar_spa);
  99
 100         priv->recv_packet_length[priv->recv_packets] =
 101                 ETHER_HDR_SIZE + ARP_HDR_SIZE;
 102         ++priv->recv_packets;
 103
 104         return 0;
 105 }
 106
 107 /*
 108  * sandbox_eth_ping_req_to_reply()
 109  *
 110  * Check for a ping request to be sent. If so, inject a reply
 111  *
 112  * returns 0 if injected, -EAGAIN if not
 113  */
 114 int sandbox_eth_ping_req_to_reply(struct udevice *dev, void *packet,
 115                                   unsigned int len)
 116 {
 117         struct eth_sandbox_priv *priv = dev_get_priv(dev);
 118         struct ethernet_hdr *eth = packet;
 119         struct ip_udp_hdr *ip;
 120         struct icmp_hdr *icmp;
 121         struct ethernet_hdr *eth_recv;
 122         struct ip_udp_hdr *ipr;
 123         struct icmp_hdr *icmpr;
 124
 125         if (ntohs(eth->et_protlen) != PROT_IP)
 126                 return -EAGAIN;
 127
 128         ip = packet + ETHER_HDR_SIZE;
 129
 130         if (ip->ip_p != IPPROTO_ICMP)
 131                 return -EAGAIN;
 132
 133         icmp = (struct icmp_hdr *)&ip->udp_src;
 134
 135         if (icmp->type != ICMP_ECHO_REQUEST)
 136                 return -EAGAIN;
 137
 138         /* Don't allow the buffer to overrun */
 139         if (priv->recv_packets >= PKTBUFSRX)
 140                 return 0;
 141
 142         /* reply to the ping */
 143         eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];
 144         memcpy(eth_recv, packet, len);
 145         ipr = (void *)eth_recv + ETHER_HDR_SIZE;
 146         icmpr = (struct icmp_hdr *)&ipr->udp_src;
 147         memcpy(eth_recv->et_dest, eth->et_src, ARP_HLEN);
 148         memcpy(eth_recv->et_src, priv->fake_host_hwaddr, ARP_HLEN);
 149         ipr->ip_sum = 0;
 150         ipr->ip_off = 0;
 151         net_copy_ip((void *)&ipr->ip_dst, &ip->ip_src);
 152         net_write_ip((void *)&ipr->ip_src, priv->fake_host_ipaddr);
 153         ipr->ip_sum = compute_ip_checksum(ipr, IP_HDR_SIZE);
 154
 155         icmpr->type = ICMP_ECHO_REPLY;
 156         icmpr->checksum = 0;
 157         icmpr->checksum = compute_ip_checksum(icmpr, ICMP_HDR_SIZE);
 158
 159         priv->recv_packet_length[priv->recv_packets] = len;
 160         ++priv->recv_packets;
 161
 162         return 0;
 163 }
 164
 165 /*
 166  * sandbox_eth_recv_arp_req()
 167  *
 168  * Inject an ARP request for this target
 169  *
 170  * returns 0 if injected, -EOVERFLOW if not
 171  */
 172 int sandbox_eth_recv_arp_req(struct udevice *dev)
 173 {
 174         struct eth_sandbox_priv *priv = dev_get_priv(dev);
 175         struct ethernet_hdr *eth_recv;
 176         struct arp_hdr *arp_recv;
 177
 178         /* Don't allow the buffer to overrun */
 179         if (priv->recv_packets >= PKTBUFSRX)
 180                 return -EOVERFLOW;
 181
 182         /* Formulate a fake request */
 183         eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];
 184         memcpy(eth_recv->et_dest, net_bcast_ethaddr, ARP_HLEN);
 185         memcpy(eth_recv->et_src, priv->fake_host_hwaddr, ARP_HLEN);
 186         eth_recv->et_protlen = htons(PROT_ARP);
 187
 188         arp_recv = (void *)eth_recv + ETHER_HDR_SIZE;
 189         arp_recv->ar_hrd = htons(ARP_ETHER);
 190         arp_recv->ar_pro = htons(PROT_IP);
 191         arp_recv->ar_hln = ARP_HLEN;
 192         arp_recv->ar_pln = ARP_PLEN;
 193         arp_recv->ar_op = htons(ARPOP_REQUEST);
 194         memcpy(&arp_recv->ar_sha, priv->fake_host_hwaddr, ARP_HLEN);
 195         net_write_ip(&arp_recv->ar_spa, priv->fake_host_ipaddr);
 196         memcpy(&arp_recv->ar_tha, net_null_ethaddr, ARP_HLEN);
 197         net_write_ip(&arp_recv->ar_tpa, net_ip);
 198
 199         priv->recv_packet_length[priv->recv_packets] =
 200                 ETHER_HDR_SIZE + ARP_HDR_SIZE;
 201         ++priv->recv_packets;
 202
 203         return 0;
 204 }
 205
 206 /*
 207  * sandbox_eth_recv_ping_req()
 208  *
 209  * Inject a ping request for this target
 210  *
 211  * returns 0 if injected, -EOVERFLOW if not
 212  */
 213 int sandbox_eth_recv_ping_req(struct udevice *dev)
 214 {
 215         struct eth_sandbox_priv *priv = dev_get_priv(dev);
 216         struct ethernet_hdr *eth_recv;
 217         struct ip_udp_hdr *ipr;
 218         struct icmp_hdr *icmpr;
 219
 220         /* Don't allow the buffer to overrun */
 221         if (priv->recv_packets >= PKTBUFSRX)
 222                 return -EOVERFLOW;
 223
 224         /* Formulate a fake ping */
 225         eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];
 226
 227         memcpy(eth_recv->et_dest, net_ethaddr, ARP_HLEN);
 228         memcpy(eth_recv->et_src, priv->fake_host_hwaddr, ARP_HLEN);
 229         eth_recv->et_protlen = htons(PROT_IP);
 230
 231         ipr = (void *)eth_recv + ETHER_HDR_SIZE;
 232         ipr->ip_hl_v = 0x45;
 233         ipr->ip_len = htons(IP_ICMP_HDR_SIZE);
 234         ipr->ip_off = htons(IP_FLAGS_DFRAG);
 235         ipr->ip_p = IPPROTO_ICMP;
 236         ipr->ip_sum = 0;
 237         net_write_ip(&ipr->ip_src, priv->fake_host_ipaddr);
 238         net_write_ip(&ipr->ip_dst, net_ip);
 239         ipr->ip_sum = compute_ip_checksum(ipr, IP_HDR_SIZE);
 240
 241         icmpr = (struct icmp_hdr *)&ipr->udp_src;
 242
 243         icmpr->type = ICMP_ECHO_REQUEST;
 244         icmpr->code = 0;
 245         icmpr->checksum = 0;
 246         icmpr->un.echo.id = 0;
 247         icmpr->un.echo.sequence = htons(1);
 248         icmpr->checksum = compute_ip_checksum(icmpr, ICMP_HDR_SIZE);
 249
 250         priv->recv_packet_length[priv->recv_packets] =
 251                 ETHER_HDR_SIZE + IP_ICMP_HDR_SIZE;
 252         ++priv->recv_packets;
 253
 254         return 0;
 255 }
 256
 257 /*
 258  * sb_default_handler()
 259  *
 260  * perform typical responses to simple ping
 261  *
 262  * dev - device pointer
 263  * pkt - "sent" packet buffer
 264  * len - length of packet
 265  */
 266 static int sb_default_handler(struct udevice *dev, void *packet,
 267                               unsigned int len)
 268 {
 269         if (!sandbox_eth_arp_req_to_reply(dev, packet, len))
 270                 return 0;
 271         if (!sandbox_eth_ping_req_to_reply(dev, packet, len))
 272                 return 0;
 273
 274         return 0;
 275 }
 276
 277 /*
 278  * sandbox_eth_set_tx_handler()
 279  *
 280  * Set a custom response to a packet being sent through the sandbox eth test
 281  *      driver
 282  *
 283  * index - interface to set the handler for
 284  * handler - The func ptr to call on send. If NULL, set to default handler
 285  */
 286 void sandbox_eth_set_tx_handler(int index, sandbox_eth_tx_hand_f *handler)
 287 {
 288         struct udevice *dev;
 289         struct eth_sandbox_priv *priv;
 290         int ret;
 291
 292         ret = uclass_get_device(UCLASS_ETH, index, &dev);
 293         if (ret)
 294                 return;
 295
 296         priv = dev_get_priv(dev);
 297         if (handler)
 298                 priv->tx_handler = handler;
 299         else
 300                 priv->tx_handler = sb_default_handler;
 301 }
 302
 303 /*
 304  * Set priv ptr
 305  *
 306  * priv - priv void ptr to store in the device
 307  */
 308 void sandbox_eth_set_priv(int index, void *priv)
 309 {
 310         struct udevice *dev;
 311         struct eth_sandbox_priv *dev_priv;
 312         int ret;
 313
 314         ret = uclass_get_device(UCLASS_ETH, index, &dev);
 315         if (ret)
 316                 return;
 317
 318         dev_priv = dev_get_priv(dev);
 319
 320         dev_priv->priv = priv;
 321 }
 322
 323 static int sb_eth_start(struct udevice *dev)
 324 {
 325         struct eth_sandbox_priv *priv = dev_get_priv(dev);
 326
 327         debug("eth_sandbox: Start\n");
 328
 329         priv->recv_packets = 0;
 330         for (int i = 0; i < PKTBUFSRX; i++) {
 331                 priv->recv_packet_buffer[i] = net_rx_packets[i];
 332                 priv->recv_packet_length[i] = 0;
 333         }
 334
 335         return 0;
 336 }
 337
 338 static int sb_eth_send(struct udevice *dev, void *packet, int length)
 339 {
 340         struct eth_sandbox_priv *priv = dev_get_priv(dev);
 341
 342         debug("eth_sandbox: Send packet %d\n", length);
 343
 344         if (priv->disabled)
 345                 return 0;
 346
 347         return priv->tx_handler(dev, packet, length);
 348 }
 349
 350 static int sb_eth_recv(struct udevice *dev, int flags, uchar **packetp)
 351 {
 352         struct eth_sandbox_priv *priv = dev_get_priv(dev);
 353
 354         if (skip_timeout) {
 355                 timer_test_add_offset(11000UL);
 356                 skip_timeout = false;
 357         }
 358
 359         if (priv->recv_packets) {
 360                 int lcl_recv_packet_length = priv->recv_packet_length[0];
 361
 362                 debug("eth_sandbox: received packet[%d], %d waiting\n",
 363                       lcl_recv_packet_length, priv->recv_packets - 1);
 364                 *packetp = priv->recv_packet_buffer[0];
 365                 return lcl_recv_packet_length;
 366         }
 367         return 0;
 368 }
 369
 370 static int sb_eth_free_pkt(struct udevice *dev, uchar *packet, int length)
 371 {
 372         struct eth_sandbox_priv *priv = dev_get_priv(dev);
 373         int i;
 374
 375         if (!priv->recv_packets)
 376                 return 0;
 377
 378         --priv->recv_packets;
 379         for (i = 0; i < priv->recv_packets; i++) {
 380                 priv->recv_packet_length[i] = priv->recv_packet_length[i + 1];
 381                 memcpy(priv->recv_packet_buffer[i],
 382                        priv->recv_packet_buffer[i + 1],
 383                        priv->recv_packet_length[i + 1]);
 384         }
 385         priv->recv_packet_length[priv->recv_packets] = 0;
 386
 387         return 0;
 388 }
 389
 390 static void sb_eth_stop(struct udevice *dev)
 391 {
 392         debug("eth_sandbox: Stop\n");
 393 }
 394
 395 static int sb_eth_write_hwaddr(struct udevice *dev)
 396 {
 397         struct eth_pdata *pdata = dev_get_plat(dev);
 398         struct eth_sandbox_priv *priv = dev_get_priv(dev);
 399
 400         debug("eth_sandbox %s: Write HW ADDR - %pM\n", dev->name,
 401               pdata->enetaddr);
 402         memcpy(priv->fake_host_hwaddr, pdata->enetaddr, ARP_HLEN);
 403         return 0;
 404 }
 405
 406 static const struct eth_ops sb_eth_ops = {
 407         .start                  = sb_eth_start,
 408         .send                   = sb_eth_send,
 409         .recv                   = sb_eth_recv,
 410         .free_pkt               = sb_eth_free_pkt,
 411         .stop                   = sb_eth_stop,
 412         .write_hwaddr           = sb_eth_write_hwaddr,
 413 };
 414
 415 static int sb_eth_remove(struct udevice *dev)
 416 {
 417         return 0;
 418 }
 419
 420 static int sb_eth_of_to_plat(struct udevice *dev)
 421 {
 422         struct eth_pdata *pdata = dev_get_plat(dev);
 423         struct eth_sandbox_priv *priv = dev_get_priv(dev);
 424
 425         pdata->iobase = dev_read_addr(dev);
 426         priv->disabled = false;
 427         priv->tx_handler = sb_default_handler;
 428
 429         return 0;
 430 }
 431
 432 static const struct udevice_id sb_eth_ids[] = {
 433         { .compatible = "sandbox,eth" },
 434         { }
 435 };
 436
 437 U_BOOT_DRIVER(eth_sandbox) = {
 438         .name   = "eth_sandbox",
 439         .id     = UCLASS_ETH,
 440         .of_match = sb_eth_ids,
 441         .of_to_plat = sb_eth_of_to_plat,
 442         .remove = sb_eth_remove,
 443         .ops    = &sb_eth_ops,
 444         .priv_auto      = sizeof(struct eth_sandbox_priv),
 445         .plat_auto      = sizeof(struct eth_pdata),
 446 };