LXR uboot/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
  18DECLARE_GLOBAL_DATA_PTR;
  19
  20static 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 */
  28void 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 */
  47void 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 */
  59int 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 */
 114int 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 */
 172int 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 */
 213int 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 */
 266static 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 */
 286void 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 */
 308void 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
 323static 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
 338static 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
 350static 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
 370static 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
 390static void sb_eth_stop(struct udevice *dev)
 391{
 392        debug("eth_sandbox: Stop\n");
 393}
 394
 395static 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
 406static 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
 415static int sb_eth_remove(struct udevice *dev)
 416{
 417        return 0;
 418}
 419
 420static 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
 432static const struct udevice_id sb_eth_ids[] = {
 433        { .compatible = "sandbox,eth" },
 434        { }
 435};
 436
 437U_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};
 447