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