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