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