1 /* 2 * Copyright (c) 2015 National Instruments 3 * 4 * (C) Copyright 2015 5 * Joe Hershberger <joe.hershberger@ni.com> 6 * 7 * SPDX-License-Identifier: GPL-2.0 8 */ 9 10 #include <common.h> 11 #include <dm.h> 12 #include <malloc.h> 13 #include <net.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 * 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 uchar *recv_packet_buffer; 30 int recv_packet_length; 31 }; 32 33 static bool disabled[8] = {false}; 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 disabled[index] = disable; 45 } 46 47 /* 48 * sandbox_eth_skip_timeout() 49 * 50 * When the first packet read is attempted, fast-forward time 51 */ 52 void sandbox_eth_skip_timeout(void) 53 { 54 skip_timeout = true; 55 } 56 57 static int sb_eth_start(struct udevice *dev) 58 { 59 struct eth_sandbox_priv *priv = dev_get_priv(dev); 60 61 debug("eth_sandbox: Start\n"); 62 63 fdtdec_get_byte_array(gd->fdt_blob, dev_of_offset(dev), 64 "fake-host-hwaddr", priv->fake_host_hwaddr, 65 ARP_HLEN); 66 priv->recv_packet_buffer = net_rx_packets[0]; 67 return 0; 68 } 69 70 static int sb_eth_send(struct udevice *dev, void *packet, int length) 71 { 72 struct eth_sandbox_priv *priv = dev_get_priv(dev); 73 struct ethernet_hdr *eth = packet; 74 75 debug("eth_sandbox: Send packet %d\n", length); 76 77 if (dev->seq >= 0 && dev->seq < ARRAY_SIZE(disabled) && 78 disabled[dev->seq]) 79 return 0; 80 81 if (ntohs(eth->et_protlen) == PROT_ARP) { 82 struct arp_hdr *arp = packet + ETHER_HDR_SIZE; 83 84 if (ntohs(arp->ar_op) == ARPOP_REQUEST) { 85 struct ethernet_hdr *eth_recv; 86 struct arp_hdr *arp_recv; 87 88 /* store this as the assumed IP of the fake host */ 89 priv->fake_host_ipaddr = net_read_ip(&arp->ar_tpa); 90 /* Formulate a fake response */ 91 eth_recv = (void *)priv->recv_packet_buffer; 92 memcpy(eth_recv->et_dest, eth->et_src, ARP_HLEN); 93 memcpy(eth_recv->et_src, priv->fake_host_hwaddr, 94 ARP_HLEN); 95 eth_recv->et_protlen = htons(PROT_ARP); 96 97 arp_recv = (void *)priv->recv_packet_buffer + 98 ETHER_HDR_SIZE; 99 arp_recv->ar_hrd = htons(ARP_ETHER); 100 arp_recv->ar_pro = htons(PROT_IP); 101 arp_recv->ar_hln = ARP_HLEN; 102 arp_recv->ar_pln = ARP_PLEN; 103 arp_recv->ar_op = htons(ARPOP_REPLY); 104 memcpy(&arp_recv->ar_sha, priv->fake_host_hwaddr, 105 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 + 111 ARP_HDR_SIZE; 112 } 113 } else if (ntohs(eth->et_protlen) == PROT_IP) { 114 struct ip_udp_hdr *ip = packet + ETHER_HDR_SIZE; 115 116 if (ip->ip_p == IPPROTO_ICMP) { 117 struct icmp_hdr *icmp = (struct icmp_hdr *)&ip->udp_src; 118 119 if (icmp->type == ICMP_ECHO_REQUEST) { 120 struct ethernet_hdr *eth_recv; 121 struct ip_udp_hdr *ipr; 122 struct icmp_hdr *icmpr; 123 124 /* reply to the ping */ 125 memcpy(priv->recv_packet_buffer, packet, 126 length); 127 eth_recv = (void *)priv->recv_packet_buffer; 128 ipr = (void *)priv->recv_packet_buffer + 129 ETHER_HDR_SIZE; 130 icmpr = (struct icmp_hdr *)&ipr->udp_src; 131 memcpy(eth_recv->et_dest, eth->et_src, 132 ARP_HLEN); 133 memcpy(eth_recv->et_src, priv->fake_host_hwaddr, 134 ARP_HLEN); 135 ipr->ip_sum = 0; 136 ipr->ip_off = 0; 137 net_copy_ip((void *)&ipr->ip_dst, &ip->ip_src); 138 net_write_ip((void *)&ipr->ip_src, 139 priv->fake_host_ipaddr); 140 ipr->ip_sum = compute_ip_checksum(ipr, 141 IP_HDR_SIZE); 142 143 icmpr->type = ICMP_ECHO_REPLY; 144 icmpr->checksum = 0; 145 icmpr->checksum = compute_ip_checksum(icmpr, 146 ICMP_HDR_SIZE); 147 148 priv->recv_packet_length = length; 149 } 150 } 151 } 152 153 return 0; 154 } 155 156 static int sb_eth_recv(struct udevice *dev, int flags, uchar **packetp) 157 { 158 struct eth_sandbox_priv *priv = dev_get_priv(dev); 159 160 if (skip_timeout) { 161 sandbox_timer_add_offset(11000UL); 162 skip_timeout = false; 163 } 164 165 if (priv->recv_packet_length) { 166 int lcl_recv_packet_length = priv->recv_packet_length; 167 168 debug("eth_sandbox: received packet %d\n", 169 priv->recv_packet_length); 170 priv->recv_packet_length = 0; 171 *packetp = priv->recv_packet_buffer; 172 return lcl_recv_packet_length; 173 } 174 return 0; 175 } 176 177 static void sb_eth_stop(struct udevice *dev) 178 { 179 debug("eth_sandbox: Stop\n"); 180 } 181 182 static int sb_eth_write_hwaddr(struct udevice *dev) 183 { 184 struct eth_pdata *pdata = dev_get_platdata(dev); 185 186 debug("eth_sandbox %s: Write HW ADDR - %pM\n", dev->name, 187 pdata->enetaddr); 188 return 0; 189 } 190 191 static const struct eth_ops sb_eth_ops = { 192 .start = sb_eth_start, 193 .send = sb_eth_send, 194 .recv = sb_eth_recv, 195 .stop = sb_eth_stop, 196 .write_hwaddr = sb_eth_write_hwaddr, 197 }; 198 199 static int sb_eth_remove(struct udevice *dev) 200 { 201 return 0; 202 } 203 204 static int sb_eth_ofdata_to_platdata(struct udevice *dev) 205 { 206 struct eth_pdata *pdata = dev_get_platdata(dev); 207 208 pdata->iobase = devfdt_get_addr(dev); 209 return 0; 210 } 211 212 static const struct udevice_id sb_eth_ids[] = { 213 { .compatible = "sandbox,eth" }, 214 { } 215 }; 216 217 U_BOOT_DRIVER(eth_sandbox) = { 218 .name = "eth_sandbox", 219 .id = UCLASS_ETH, 220 .of_match = sb_eth_ids, 221 .ofdata_to_platdata = sb_eth_ofdata_to_platdata, 222 .remove = sb_eth_remove, 223 .ops = &sb_eth_ops, 224 .priv_auto_alloc_size = sizeof(struct eth_sandbox_priv), 225 .platdata_auto_alloc_size = sizeof(struct eth_pdata), 226 }; 227