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