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