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 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 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 }; 451