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 <malloc.h>
  12#include <net.h>
  13#include <asm/eth.h>
  14#include <asm/test.h>
  15
  16DECLARE_GLOBAL_DATA_PTR;
  17
  18static 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 */
  26void 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 */
  45void 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 */
  57int 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 */
 112int 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 */
 170int 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 */
 211int 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 */
 264static 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 */
 284void 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 */
 306void 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
 321static 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
 336static 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
 348static 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                sandbox_timer_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
 368static 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
 388static void sb_eth_stop(struct udevice *dev)
 389{
 390        debug("eth_sandbox: Stop\n");
 391}
 392
 393static 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
 402static 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
 411static int sb_eth_remove(struct udevice *dev)
 412{
 413        return 0;
 414}
 415
 416static 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
 436static const struct udevice_id sb_eth_ids[] = {
 437        { .compatible = "sandbox,eth" },
 438        { }
 439};
 440
 441U_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};
 451