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 /* store this as the assumed IP of the fake host */ 75 priv->fake_host_ipaddr = net_read_ip(&arp->ar_tpa); 76 77 /* Formulate a fake response */ 78 eth_recv = (void *)priv->recv_packet_buffer; 79 memcpy(eth_recv->et_dest, eth->et_src, ARP_HLEN); 80 memcpy(eth_recv->et_src, priv->fake_host_hwaddr, ARP_HLEN); 81 eth_recv->et_protlen = htons(PROT_ARP); 82 83 arp_recv = (void *)eth_recv + ETHER_HDR_SIZE; 84 arp_recv->ar_hrd = htons(ARP_ETHER); 85 arp_recv->ar_pro = htons(PROT_IP); 86 arp_recv->ar_hln = ARP_HLEN; 87 arp_recv->ar_pln = ARP_PLEN; 88 arp_recv->ar_op = htons(ARPOP_REPLY); 89 memcpy(&arp_recv->ar_sha, priv->fake_host_hwaddr, ARP_HLEN); 90 net_write_ip(&arp_recv->ar_spa, priv->fake_host_ipaddr); 91 memcpy(&arp_recv->ar_tha, &arp->ar_sha, ARP_HLEN); 92 net_copy_ip(&arp_recv->ar_tpa, &arp->ar_spa); 93 94 priv->recv_packet_length = ETHER_HDR_SIZE + ARP_HDR_SIZE; 95 96 return 0; 97 } 98 99 /* 100 * sandbox_eth_ping_req_to_reply() 101 * 102 * Check for a ping request to be sent. If so, inject a reply 103 * 104 * returns 0 if injected, -EAGAIN if not 105 */ 106 int sandbox_eth_ping_req_to_reply(struct udevice *dev, void *packet, 107 unsigned int len) 108 { 109 struct eth_sandbox_priv *priv = dev_get_priv(dev); 110 struct ethernet_hdr *eth = packet; 111 struct ip_udp_hdr *ip; 112 struct icmp_hdr *icmp; 113 struct ethernet_hdr *eth_recv; 114 struct ip_udp_hdr *ipr; 115 struct icmp_hdr *icmpr; 116 117 if (ntohs(eth->et_protlen) != PROT_IP) 118 return -EAGAIN; 119 120 ip = packet + ETHER_HDR_SIZE; 121 122 if (ip->ip_p != IPPROTO_ICMP) 123 return -EAGAIN; 124 125 icmp = (struct icmp_hdr *)&ip->udp_src; 126 127 if (icmp->type != ICMP_ECHO_REQUEST) 128 return -EAGAIN; 129 130 /* reply to the ping */ 131 eth_recv = (void *)priv->recv_packet_buffer; 132 memcpy(eth_recv, packet, len); 133 ipr = (void *)eth_recv + ETHER_HDR_SIZE; 134 icmpr = (struct icmp_hdr *)&ipr->udp_src; 135 memcpy(eth_recv->et_dest, eth->et_src, ARP_HLEN); 136 memcpy(eth_recv->et_src, priv->fake_host_hwaddr, ARP_HLEN); 137 ipr->ip_sum = 0; 138 ipr->ip_off = 0; 139 net_copy_ip((void *)&ipr->ip_dst, &ip->ip_src); 140 net_write_ip((void *)&ipr->ip_src, priv->fake_host_ipaddr); 141 ipr->ip_sum = compute_ip_checksum(ipr, IP_HDR_SIZE); 142 143 icmpr->type = ICMP_ECHO_REPLY; 144 icmpr->checksum = 0; 145 icmpr->checksum = compute_ip_checksum(icmpr, ICMP_HDR_SIZE); 146 147 priv->recv_packet_length = len; 148 149 return 0; 150 } 151 152 /* 153 * sb_default_handler() 154 * 155 * perform typical responses to simple ping 156 * 157 * dev - device pointer 158 * pkt - "sent" packet buffer 159 * len - length of packet 160 */ 161 static int sb_default_handler(struct udevice *dev, void *packet, 162 unsigned int len) 163 { 164 if (!sandbox_eth_arp_req_to_reply(dev, packet, len)) 165 return 0; 166 if (!sandbox_eth_ping_req_to_reply(dev, packet, len)) 167 return 0; 168 169 return 0; 170 } 171 172 /* 173 * sandbox_eth_set_tx_handler() 174 * 175 * Set a custom response to a packet being sent through the sandbox eth test 176 * driver 177 * 178 * index - interface to set the handler for 179 * handler - The func ptr to call on send. If NULL, set to default handler 180 */ 181 void sandbox_eth_set_tx_handler(int index, sandbox_eth_tx_hand_f *handler) 182 { 183 struct udevice *dev; 184 struct eth_sandbox_priv *priv; 185 int ret; 186 187 ret = uclass_get_device(UCLASS_ETH, index, &dev); 188 if (ret) 189 return; 190 191 priv = dev_get_priv(dev); 192 if (handler) 193 priv->tx_handler = handler; 194 else 195 priv->tx_handler = sb_default_handler; 196 } 197 198 static int sb_eth_start(struct udevice *dev) 199 { 200 struct eth_sandbox_priv *priv = dev_get_priv(dev); 201 202 debug("eth_sandbox: Start\n"); 203 204 priv->recv_packet_buffer = net_rx_packets[0]; 205 206 return 0; 207 } 208 209 static int sb_eth_send(struct udevice *dev, void *packet, int length) 210 { 211 struct eth_sandbox_priv *priv = dev_get_priv(dev); 212 213 debug("eth_sandbox: Send packet %d\n", length); 214 215 if (priv->disabled) 216 return 0; 217 218 return priv->tx_handler(dev, packet, length); 219 } 220 221 static int sb_eth_recv(struct udevice *dev, int flags, uchar **packetp) 222 { 223 struct eth_sandbox_priv *priv = dev_get_priv(dev); 224 225 if (skip_timeout) { 226 sandbox_timer_add_offset(11000UL); 227 skip_timeout = false; 228 } 229 230 if (priv->recv_packet_length) { 231 int lcl_recv_packet_length = priv->recv_packet_length; 232 233 debug("eth_sandbox: received packet %d\n", 234 priv->recv_packet_length); 235 priv->recv_packet_length = 0; 236 *packetp = priv->recv_packet_buffer; 237 return lcl_recv_packet_length; 238 } 239 return 0; 240 } 241 242 static void sb_eth_stop(struct udevice *dev) 243 { 244 debug("eth_sandbox: Stop\n"); 245 } 246 247 static int sb_eth_write_hwaddr(struct udevice *dev) 248 { 249 struct eth_pdata *pdata = dev_get_platdata(dev); 250 251 debug("eth_sandbox %s: Write HW ADDR - %pM\n", dev->name, 252 pdata->enetaddr); 253 return 0; 254 } 255 256 static const struct eth_ops sb_eth_ops = { 257 .start = sb_eth_start, 258 .send = sb_eth_send, 259 .recv = sb_eth_recv, 260 .stop = sb_eth_stop, 261 .write_hwaddr = sb_eth_write_hwaddr, 262 }; 263 264 static int sb_eth_remove(struct udevice *dev) 265 { 266 return 0; 267 } 268 269 static int sb_eth_ofdata_to_platdata(struct udevice *dev) 270 { 271 struct eth_pdata *pdata = dev_get_platdata(dev); 272 struct eth_sandbox_priv *priv = dev_get_priv(dev); 273 const u8 *mac; 274 275 pdata->iobase = dev_read_addr(dev); 276 277 mac = dev_read_u8_array_ptr(dev, "fake-host-hwaddr", ARP_HLEN); 278 if (!mac) { 279 printf("'fake-host-hwaddr' is missing from the DT\n"); 280 return -EINVAL; 281 } 282 memcpy(priv->fake_host_hwaddr, mac, ARP_HLEN); 283 priv->disabled = false; 284 priv->tx_handler = sb_default_handler; 285 286 return 0; 287 } 288 289 static const struct udevice_id sb_eth_ids[] = { 290 { .compatible = "sandbox,eth" }, 291 { } 292 }; 293 294 U_BOOT_DRIVER(eth_sandbox) = { 295 .name = "eth_sandbox", 296 .id = UCLASS_ETH, 297 .of_match = sb_eth_ids, 298 .ofdata_to_platdata = sb_eth_ofdata_to_platdata, 299 .remove = sb_eth_remove, 300 .ops = &sb_eth_ops, 301 .priv_auto_alloc_size = sizeof(struct eth_sandbox_priv), 302 .platdata_auto_alloc_size = sizeof(struct eth_pdata), 303 }; 304