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