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