[6319] | 1 | /** @file wlan_exp_ip_udp_ip_udp.c |
---|
| 2 | * @brief Mango wlan_exp IP/UDP Library (IP/UDP/ARP/ICMP Processing) |
---|
| 3 | * |
---|
| 4 | * @copyright Copyright 2014-2019, Mango Communications. All rights reserved. |
---|
| 5 | * Distributed under the Mango Reference Design license (https://mangocomm.com/802.11/license) |
---|
| 6 | */ |
---|
| 7 | |
---|
| 8 | /***************************** Include Files *********************************/ |
---|
| 9 | #include "wlan_mac_high_sw_config.h" |
---|
| 10 | |
---|
| 11 | #if WLAN_SW_CONFIG_ENABLE_WLAN_EXP |
---|
| 12 | |
---|
| 13 | // Xilinx / Standard library includes |
---|
| 14 | #include <xparameters.h> |
---|
| 15 | #include <xstatus.h> |
---|
| 16 | #include <xil_io.h> |
---|
| 17 | #include <stdlib.h> |
---|
| 18 | #include <stdio.h> |
---|
| 19 | |
---|
| 20 | // Mango wlan_exp IP/UDP Library includes |
---|
| 21 | #include "wlan_exp_ip_udp.h" |
---|
| 22 | #include "wlan_exp_ip_udp_socket.h" |
---|
| 23 | #include "wlan_exp_ip_udp_arp.h" |
---|
| 24 | |
---|
| 25 | // Mango Framework includes |
---|
| 26 | #include "wlan_high_types.h" |
---|
| 27 | #include "wlan_mac_queue.h" |
---|
| 28 | #include "wlan_mac_common.h" |
---|
| 29 | #include "wlan_exp_transport.h" |
---|
| 30 | |
---|
| 31 | /*********************** Global Variable Definitions *************************/ |
---|
| 32 | |
---|
| 33 | u16 ipv4_id_counter = 0; |
---|
| 34 | static u8 gl_ip_addr[IP_ADDR_LEN]; // Ethernet device IP address |
---|
| 35 | static u8 gl_hw_addr[ETH_ADDR_LEN]; // Ethernet device MAC address |
---|
| 36 | |
---|
| 37 | /*************************** Function Prototypes *****************************/ |
---|
| 38 | |
---|
| 39 | void icmp_echo_reply(eth_rx_queue_buffer_t* eth_rx_queue_buffer, eth_tx_queue_buffer_t* eth_tx_queue_buffer); |
---|
| 40 | int _set_ip_length(u8* pkt, u32 total_length); |
---|
| 41 | int _set_udp_length(u8* pkt, u32 total_length); |
---|
| 42 | |
---|
| 43 | |
---|
| 44 | /******************************** Functions **********************************/ |
---|
| 45 | |
---|
| 46 | /*****************************************************************************/ |
---|
| 47 | /** |
---|
| 48 | * Initialize the IP/UDP Library |
---|
| 49 | * |
---|
| 50 | * This function will initialize all subsystems within the library: |
---|
| 51 | * - Global Ethernet structures |
---|
| 52 | * - Socket data structures |
---|
| 53 | * - ARP cache |
---|
| 54 | * - IP V4 global structures (ie ID counter) |
---|
| 55 | * |
---|
| 56 | * @param None |
---|
| 57 | * |
---|
| 58 | * @return int - WLAN_SUCCESS (cannot fail) |
---|
| 59 | * |
---|
| 60 | ******************************************************************************/ |
---|
| 61 | int ip_udp_init( u8* hw_addr, u8* ip_addr) { |
---|
| 62 | |
---|
| 63 | // Initialize the sockets |
---|
| 64 | socket_init(); |
---|
| 65 | |
---|
| 66 | // Initialize the ARP cache |
---|
| 67 | arp_init_cache(); |
---|
| 68 | |
---|
| 69 | // Initialize the IP v4 global structures |
---|
| 70 | ipv4_id_counter = 0; |
---|
| 71 | |
---|
| 72 | eth_set_hw_addr(hw_addr); |
---|
| 73 | eth_set_ip_addr(ip_addr); |
---|
| 74 | |
---|
| 75 | // Return success |
---|
| 76 | return WLAN_SUCCESS; |
---|
| 77 | } |
---|
| 78 | |
---|
| 79 | |
---|
| 80 | /*****************************************************************************/ |
---|
| 81 | /** |
---|
| 82 | * Create Ethernet / IP / UDP Headers |
---|
| 83 | * |
---|
| 84 | *****************************************************************************/ |
---|
| 85 | |
---|
| 86 | int ip_udp_template_init(u8* pkt, u32 port, sockaddr_in_t* to) { |
---|
| 87 | |
---|
| 88 | int status; |
---|
| 89 | sockaddr_in_t from; |
---|
| 90 | |
---|
| 91 | // Get current open socket details |
---|
| 92 | int socket_index = socket_find_index_by_port(port); |
---|
| 93 | |
---|
| 94 | if (socket_index != SOCKET_INVALID_SOCKET) { |
---|
| 95 | from = socket_get(socket_index); |
---|
| 96 | } else { |
---|
| 97 | return WLAN_FAILURE; |
---|
| 98 | } |
---|
| 99 | |
---|
| 100 | u8 dest_hw_addr[ETH_ADDR_LEN]; |
---|
| 101 | u8 src_hw_addr[ETH_ADDR_LEN]; |
---|
| 102 | u8* src_ip_addr = from.sin_addr.s_addr; |
---|
| 103 | u32 src_port = from.sin_port; |
---|
| 104 | u8* dest_ip_addr = to->sin_addr.s_addr; |
---|
| 105 | u16 dest_port = to->sin_port; |
---|
| 106 | |
---|
| 107 | ethernet_header_t* eth_hdr = (ethernet_header_t*)pkt; |
---|
| 108 | ipv4_header_t* ip_hdr = (ipv4_header_t*)( pkt + sizeof(ethernet_header_t) ); |
---|
| 109 | udp_header_t* udp_hdr = (udp_header_t*)( pkt + sizeof(ethernet_header_t) + sizeof(ipv4_header_t) ); |
---|
| 110 | |
---|
| 111 | eth_get_hw_addr(src_hw_addr); |
---|
| 112 | |
---|
| 113 | // Look up destination HW address in ARP table |
---|
| 114 | status = arp_get_hw_addr(dest_hw_addr, dest_ip_addr); |
---|
| 115 | |
---|
| 116 | if (status != WLAN_SUCCESS) { |
---|
| 117 | return WLAN_FAILURE; |
---|
| 118 | } |
---|
| 119 | |
---|
| 120 | // Update the Ethernet header |
---|
| 121 | eth_hdr_init(eth_hdr, |
---|
| 122 | dest_hw_addr, |
---|
| 123 | src_hw_addr); |
---|
| 124 | |
---|
| 125 | // Update the IPv4 header |
---|
| 126 | ip_hdr_init(ip_hdr, |
---|
| 127 | src_ip_addr, |
---|
| 128 | dest_ip_addr, |
---|
| 129 | IP_PROTOCOL_UDP); |
---|
| 130 | |
---|
| 131 | |
---|
| 132 | // Update the UDP header |
---|
| 133 | udp_hdr_init(udp_hdr, |
---|
| 134 | src_port, |
---|
| 135 | dest_port); |
---|
| 136 | |
---|
| 137 | |
---|
| 138 | // Send the Ethernet frame using the socket header |
---|
| 139 | return (sizeof(ethernet_header_t) + sizeof(ipv4_header_t) + sizeof(udp_header_t)); |
---|
| 140 | } |
---|
| 141 | |
---|
| 142 | int wlan_exp_ip_udp_set_length(u8* pkt, u32 total_length){ |
---|
| 143 | // total_length refers to the number of bytes that will |
---|
| 144 | // go over the wire starting with the Ethernet header |
---|
| 145 | |
---|
| 146 | if(pkt==NULL) return WLAN_FAILURE; |
---|
| 147 | |
---|
| 148 | _set_ip_length(pkt, total_length); |
---|
| 149 | _set_udp_length(pkt, total_length); |
---|
| 150 | |
---|
| 151 | return WLAN_SUCCESS; |
---|
| 152 | } |
---|
| 153 | |
---|
| 154 | int _set_ip_length(u8* pkt, u32 total_length){ |
---|
| 155 | // total_length refers to the number of bytes that will |
---|
| 156 | // go over the wire starting with the Ethernet header |
---|
| 157 | |
---|
| 158 | if(pkt==NULL) return WLAN_FAILURE; |
---|
| 159 | |
---|
| 160 | ipv4_header_t* ip_hdr = (ipv4_header_t*)( pkt + sizeof(ethernet_header_t) ); |
---|
| 161 | |
---|
| 162 | // Set IP length |
---|
| 163 | ip_hdr->total_length = Xil_Htons(total_length - sizeof(ethernet_header_t) ); |
---|
| 164 | |
---|
| 165 | // Set IP checksum |
---|
| 166 | ip_hdr->header_checksum = Xil_Htons(ipv4_compute_checksum((u8*)ip_hdr, sizeof(ipv4_header_t))); |
---|
| 167 | |
---|
| 168 | return WLAN_SUCCESS; |
---|
| 169 | |
---|
| 170 | } |
---|
| 171 | int _set_udp_length(u8* pkt, u32 total_length){ |
---|
| 172 | // total_length refers to the number of bytes that will |
---|
| 173 | // go over the wire starting with the Ethernet header |
---|
| 174 | |
---|
| 175 | if(pkt==NULL) return WLAN_FAILURE; |
---|
| 176 | |
---|
| 177 | udp_header_t* udp_hdr = (udp_header_t*)( pkt + sizeof(ethernet_header_t) + sizeof(ipv4_header_t) ); |
---|
| 178 | |
---|
| 179 | // Set UDP length |
---|
| 180 | udp_hdr->length = Xil_Htons(total_length - sizeof(ethernet_header_t) - sizeof(ipv4_header_t)); |
---|
| 181 | |
---|
| 182 | // If we wanted to use UDP checksums, here is where we would set it |
---|
| 183 | |
---|
| 184 | return WLAN_SUCCESS; |
---|
| 185 | } |
---|
| 186 | |
---|
| 187 | /**********************************************************************************************************************/ |
---|
| 188 | /** |
---|
| 189 | * @brief IP Functions |
---|
| 190 | * |
---|
| 191 | **********************************************************************************************************************/ |
---|
| 192 | |
---|
| 193 | |
---|
| 194 | /*****************************************************************************/ |
---|
| 195 | /** |
---|
| 196 | * Process the IP packet |
---|
| 197 | * |
---|
| 198 | * @param eth_rx_queue_buffer - Rx Ethernet packet |
---|
| 199 | * @param eth_tx_queue_buffer - Buffer for Tx Ethernet packet to be filled in (if necessary) |
---|
| 200 | * |
---|
| 201 | * @return int - Number of bytes of data in the processed packet; 0 if the packet could not be processed |
---|
| 202 | * |
---|
| 203 | * @note This function assumes that both Ethernet device and buffer are valid. |
---|
| 204 | * |
---|
| 205 | *****************************************************************************/ |
---|
| 206 | int ipv4_process_packet(eth_rx_queue_buffer_t* eth_rx_queue_buffer, eth_tx_queue_buffer_t* eth_tx_queue_buffer) { |
---|
| 207 | |
---|
| 208 | ipv4_header_t* header; |
---|
| 209 | ethernet_header_t* eth_header; |
---|
| 210 | u8 my_ip_addr[IP_ADDR_LEN]; |
---|
| 211 | |
---|
| 212 | header = (ipv4_header_t*)((void*)(eth_rx_queue_buffer->pkt) + sizeof(ethernet_header_t)); |
---|
| 213 | |
---|
| 214 | u32 addr_check = 0; |
---|
| 215 | u8* src_ip_addr = header->src_ip_addr; |
---|
| 216 | u8* dest_ip_addr = header->dest_ip_addr; |
---|
| 217 | |
---|
| 218 | eth_get_ip_addr(my_ip_addr); |
---|
| 219 | |
---|
| 220 | // Get the Ethernet header so we have the source MAC address for the ARP cache |
---|
| 221 | eth_header = (ethernet_header_t*)eth_rx_queue_buffer->pkt; |
---|
| 222 | |
---|
| 223 | // Check the address of the IP packet |
---|
| 224 | // - If the node has not been initialized (eg the node address is 10.0.0.0), then accept broadcast packets from 10.0.X.255 |
---|
| 225 | // - If the node has been initialized, then accept unicast packets and broadcast packets on the given subnet |
---|
| 226 | // |
---|
| 227 | // |
---|
| 228 | // !!! TBD !!! - Future addition: The address check should really be more configurable (ie it should be a callback |
---|
| 229 | // that can be set by the application that uses the library). |
---|
| 230 | // |
---|
| 231 | // |
---|
| 232 | if (my_ip_addr[3] == 0) { |
---|
| 233 | |
---|
| 234 | // Accept broadcast packets from 10.0.X.255 |
---|
| 235 | if ((my_ip_addr[0] == dest_ip_addr[0]) && (my_ip_addr[1] == dest_ip_addr[1]) && (dest_ip_addr[3] == 255)) { |
---|
| 236 | addr_check = 1; |
---|
| 237 | } |
---|
| 238 | } else { |
---|
| 239 | // Accept unicast packets and broadcast packets on the given subnet |
---|
| 240 | if ((my_ip_addr[0] == dest_ip_addr[0]) && |
---|
| 241 | (my_ip_addr[1] == dest_ip_addr[1]) && |
---|
| 242 | (my_ip_addr[2] == dest_ip_addr[2]) && |
---|
| 243 | ((my_ip_addr[3] == dest_ip_addr[3]) || (dest_ip_addr[3] == 255))) { |
---|
| 244 | addr_check = 1; |
---|
| 245 | } |
---|
| 246 | } |
---|
| 247 | |
---|
| 248 | // |
---|
| 249 | // |
---|
| 250 | // !!! TBD !!! - Future consideration: Additional packet checks - Length & Checksum |
---|
| 251 | // u16 packet_length = packet->length; |
---|
| 252 | // u16 ip_length = Xil_Ntohs(header->total_length); |
---|
| 253 | // |
---|
| 254 | // |
---|
| 255 | |
---|
| 256 | if (addr_check == 1) { |
---|
| 257 | |
---|
| 258 | // The Xilinx Ethernet / DMA hardware does not support fragmented Ethernet packets. However, the |
---|
| 259 | // library will still pass the first fragment of a packet up to the higher level transport for |
---|
| 260 | // processing so that the host that sent the fragmented packet does not have a transport timeout |
---|
| 261 | // (This is important when trying to determine the maximum packet size supported by the transport). |
---|
| 262 | // If this behavior needs to change the below code will cause the Mango wlan_exp IP/UDP Library to discard |
---|
| 263 | // packet fragments. |
---|
| 264 | // |
---|
| 265 | // The 'fragment offset field' is 16 bits (see http://en.wikipedia.org/wiki/IPv4#Packet_structure |
---|
| 266 | // for more information). The 'DF' flag can be legitimately set to '1' so we need to mask that |
---|
| 267 | // bit before we decide if we can discard the packet. This means that the frag_off field can |
---|
| 268 | // have valid values of either 0x0000 or 0x4000 (big endian). However, we have to convert to |
---|
| 269 | // little endian so the valid values are 0x0000 and 0x0040 (ie byte swapped). |
---|
| 270 | // |
---|
| 271 | // if ((header->fragment_offset & 0xFFBF) != 0x0000) { |
---|
| 272 | // xil_printf("ERROR: Library does not support fragmented packets.\n"); |
---|
| 273 | // return 0; |
---|
| 274 | // } |
---|
| 275 | |
---|
| 276 | // Update ARP table (Maps IP address to MAC address) |
---|
| 277 | arp_update_cache(eth_header->src_mac_addr, src_ip_addr); |
---|
| 278 | |
---|
| 279 | // Process the IP packet |
---|
| 280 | switch (header->protocol) { |
---|
| 281 | // UDP packets |
---|
| 282 | case IP_PROTOCOL_UDP: |
---|
| 283 | return udp_process_packet(eth_rx_queue_buffer); |
---|
| 284 | break; |
---|
| 285 | |
---|
| 286 | // ICMP packets |
---|
| 287 | case IP_PROTOCOL_ICMP: |
---|
| 288 | return icmp_process_packet(eth_rx_queue_buffer, eth_tx_queue_buffer); |
---|
| 289 | break; |
---|
| 290 | |
---|
| 291 | // If a packet has made it here, the it is destined for the node but cannot be processed |
---|
| 292 | // by the library. Therefore, we need to print an error message. |
---|
| 293 | default: |
---|
| 294 | xil_printf("ERROR: Unknown IP protocol: %d\n", header->protocol); |
---|
| 295 | break; |
---|
| 296 | } |
---|
| 297 | } |
---|
| 298 | |
---|
| 299 | return 0; |
---|
| 300 | } |
---|
| 301 | |
---|
| 302 | void eth_hdr_init(ethernet_header_t* eth_hdr, |
---|
| 303 | u8* dest_hw_addr, |
---|
| 304 | u8* src_hw_addr){ |
---|
| 305 | |
---|
| 306 | memcpy(eth_hdr->dest_mac_addr, dest_hw_addr, ETH_ADDR_LEN); |
---|
| 307 | memcpy(eth_hdr->src_mac_addr, src_hw_addr, ETH_ADDR_LEN); |
---|
| 308 | eth_hdr->ethertype = Xil_Htons(ETHERTYPE_IP_V4); |
---|
| 309 | } |
---|
| 310 | |
---|
| 311 | void ip_hdr_init(ipv4_header_t* ip_hdr, |
---|
| 312 | u8* src_ip_addr, |
---|
| 313 | u8* dest_ip_addr, |
---|
| 314 | u8 protocol){ |
---|
| 315 | |
---|
| 316 | memcpy(ip_hdr->src_ip_addr, src_ip_addr, IP_ADDR_LEN); |
---|
| 317 | ip_hdr->version_ihl = (IP_VERSION_4 << 4) + IP_HEADER_LEN; |
---|
| 318 | ip_hdr->dscp_ecn = (IP_DSCP_CS0 << 2) + IP_ECN_NON_ECT; |
---|
| 319 | ip_hdr->fragment_offset = IP_NO_FRAGMENTATION; |
---|
| 320 | ip_hdr->ttl = IP_DEFAULT_TTL; |
---|
| 321 | ip_hdr->total_length = 0; // Lengths are set after the full packet contents are completed |
---|
| 322 | ip_hdr->identification = Xil_Htons(ipv4_id_counter++); |
---|
| 323 | ip_hdr->protocol = protocol; |
---|
| 324 | ip_hdr->header_checksum = 0; // Checksum is set after the full packet contents are completed |
---|
| 325 | memcpy(ip_hdr->dest_ip_addr, dest_ip_addr, IP_ADDR_LEN); |
---|
| 326 | } |
---|
| 327 | |
---|
| 328 | // Update the UDP header |
---|
| 329 | void udp_hdr_init(udp_header_t* udp_hdr, |
---|
| 330 | u16 src_port, |
---|
| 331 | u16 dest_port){ |
---|
| 332 | |
---|
| 333 | udp_hdr->src_port = Xil_Htons(src_port); |
---|
| 334 | udp_hdr->dest_port = Xil_Htons(dest_port); |
---|
| 335 | udp_hdr->length = 0; // Lengths are set after the full packet contents are completed |
---|
| 336 | udp_hdr->checksum = UDP_NO_CHECKSUM; |
---|
| 337 | } |
---|
| 338 | |
---|
| 339 | |
---|
| 340 | /*****************************************************************************/ |
---|
| 341 | /** |
---|
| 342 | * Compute IP Checksum |
---|
| 343 | * |
---|
| 344 | * The ones' complement of the ones' complement sum of the data's 16-bit words |
---|
| 345 | * |
---|
| 346 | * @param data - Pointer to the data words |
---|
| 347 | * @param size - Size of the data to use |
---|
| 348 | * |
---|
| 349 | * @return u16 - Checksum value |
---|
| 350 | * |
---|
| 351 | * @note IP Checksum Algorithm: http://en.wikipedia.org/wiki/ipv4_header_t_checksum |
---|
| 352 | * |
---|
| 353 | *****************************************************************************/ |
---|
| 354 | u16 ipv4_compute_checksum(u8 * data, u32 size) { |
---|
| 355 | |
---|
| 356 | u32 i; |
---|
| 357 | u32 sum = 0; |
---|
| 358 | u16 word = 0; |
---|
| 359 | |
---|
| 360 | // Sum all 16-bit words in the header (big-endian) |
---|
| 361 | for (i = 0; i < size; i = i + 2) { |
---|
| 362 | word = ((data[i] << 8) & 0xFF00) + (data[i+1] & 0x00FF); |
---|
| 363 | sum = sum + ((u32) word); |
---|
| 364 | } |
---|
| 365 | |
---|
| 366 | // 1's complement 16-bit sum, formed by "end around carry" of 32-bit 2's complement sum |
---|
| 367 | sum = ((sum & 0xFFFF0000) >> 16) + (sum & 0x0000FFFF); |
---|
| 368 | |
---|
| 369 | // Return the 1's complement of 1's complement 16-bit sum |
---|
| 370 | return (~sum); |
---|
| 371 | } |
---|
| 372 | |
---|
| 373 | |
---|
| 374 | |
---|
| 375 | |
---|
| 376 | /**********************************************************************************************************************/ |
---|
| 377 | /** |
---|
| 378 | * @brief UDP Functions |
---|
| 379 | * |
---|
| 380 | **********************************************************************************************************************/ |
---|
| 381 | |
---|
| 382 | |
---|
| 383 | /*****************************************************************************/ |
---|
| 384 | /** |
---|
| 385 | * Process the UDP packet |
---|
| 386 | * |
---|
| 387 | * @param eth_rx_queue_buffer - Ethernet packet |
---|
| 388 | * |
---|
| 389 | * @return int - Number of bytes of data in the UDP packet; 0 if the packet could not be processed |
---|
| 390 | * |
---|
| 391 | * @note This function assumes that both Ethernet device and buffer are valid. |
---|
| 392 | * |
---|
| 393 | *****************************************************************************/ |
---|
| 394 | int udp_process_packet(eth_rx_queue_buffer_t* eth_rx_queue_buffer) { |
---|
| 395 | |
---|
| 396 | ipv4_header_t* ip_hdr; |
---|
| 397 | udp_header_t* udp_hdr; |
---|
| 398 | int socket_index; |
---|
| 399 | wlan_exp_eth_rx_queue_buffer_t* wlan_exp_eth_rx_queue_buffer; |
---|
| 400 | |
---|
| 401 | ip_hdr = (ipv4_header_t*)((void*)(eth_rx_queue_buffer->pkt) + sizeof(ethernet_header_t)); |
---|
| 402 | udp_hdr = (udp_header_t*)((void*)ip_hdr + 4*((u8)(ip_hdr->version_ihl) & 0xF)); |
---|
| 403 | |
---|
| 404 | |
---|
| 405 | u32 port_check = 0; |
---|
| 406 | u16 dest_port = Xil_Ntohs(udp_hdr->dest_port); |
---|
| 407 | |
---|
| 408 | // See if there is a socket that corresponds to the UDP packet |
---|
| 409 | // - Check all open sockets to see if one matches the port / eth_dev_num |
---|
| 410 | // |
---|
| 411 | socket_index = socket_find_index_by_port(dest_port); |
---|
| 412 | if (socket_index != SOCKET_INVALID_SOCKET) { |
---|
| 413 | port_check = 1; |
---|
| 414 | |
---|
| 415 | // Copy socket details into eth_rx_queue_buffer for future wlan_exp processing |
---|
| 416 | wlan_exp_eth_rx_queue_buffer = (wlan_exp_eth_rx_queue_buffer_t*)eth_rx_queue_buffer; |
---|
| 417 | wlan_exp_eth_rx_queue_buffer->rx_from.sin_family = AF_INET; |
---|
| 418 | wlan_exp_eth_rx_queue_buffer->rx_from.sin_port = Xil_Ntohs(udp_hdr->src_port); |
---|
| 419 | memcpy(wlan_exp_eth_rx_queue_buffer->rx_from.sin_addr.s_addr, ip_hdr->src_ip_addr, IP_ADDR_LEN); |
---|
| 420 | } |
---|
| 421 | |
---|
| 422 | // |
---|
| 423 | // |
---|
| 424 | // !!! TBD !!! - Future consideration: Additional packet checks - Length & Checksum |
---|
| 425 | // u16 packet_length = packet->length; |
---|
| 426 | // u16 udp_length = Xil_Ntohs(header->length); |
---|
| 427 | // |
---|
| 428 | // |
---|
| 429 | |
---|
| 430 | if (port_check == 1) { |
---|
| 431 | // Return the length of the UDP data bytes |
---|
| 432 | return (eth_rx_queue_buffer->length - sizeof(udp_header_t) - sizeof(ipv4_header_t) - sizeof(ethernet_header_t)); |
---|
| 433 | } |
---|
| 434 | |
---|
| 435 | return 0; |
---|
| 436 | } |
---|
| 437 | |
---|
| 438 | |
---|
| 439 | |
---|
| 440 | /*****************************************************************************/ |
---|
| 441 | /** |
---|
| 442 | * Initialize the UDP Header |
---|
| 443 | * |
---|
| 444 | * @param header - Pointer to the UDP header |
---|
| 445 | * @param src_port - Source port for UDP packet |
---|
| 446 | * |
---|
| 447 | * @return None |
---|
| 448 | * |
---|
| 449 | *****************************************************************************/ |
---|
| 450 | void udp_init_header(udp_header_t * header, u16 src_port) { |
---|
| 451 | |
---|
| 452 | // Update the following fields that are static for the socket: |
---|
| 453 | // - Source port |
---|
| 454 | // |
---|
| 455 | header->src_port = Xil_Htons(src_port); |
---|
| 456 | } |
---|
| 457 | |
---|
| 458 | |
---|
| 459 | |
---|
| 460 | /*****************************************************************************/ |
---|
| 461 | /** |
---|
| 462 | * Update the UDP Header |
---|
| 463 | * |
---|
| 464 | * @param header - Pointer to the UDP header |
---|
| 465 | * @param dest_port - Destination port for UDP packet (big endian) |
---|
| 466 | * @param udp_length - Length of the UDP packet (includes UDP header) |
---|
| 467 | * |
---|
| 468 | * @return None |
---|
| 469 | * |
---|
| 470 | *****************************************************************************/ |
---|
| 471 | void udp_update_header(udp_header_t* header, u16 dest_port, u16 udp_length) { |
---|
| 472 | |
---|
| 473 | // Update the following fields: |
---|
| 474 | // - Destination port |
---|
| 475 | // - Length |
---|
| 476 | // |
---|
| 477 | // NOTE: We do not need to update the following fields because they are static for the socket: |
---|
| 478 | // - Source port |
---|
| 479 | // |
---|
| 480 | header->dest_port = dest_port; |
---|
| 481 | header->length = Xil_Htons(udp_length); |
---|
| 482 | |
---|
| 483 | // Currently, the Mango wlan_exp IP/UDP Library does not use the UDP checksum capabilities. This is primarily |
---|
| 484 | // due to the amount of time required to compute the checksum. Also, given that communication |
---|
| 485 | // between hosts and nodes is, in general, fairly localized, there is not as much of a need for |
---|
| 486 | // the data integrity check that the UDP checksum provides. |
---|
| 487 | // |
---|
| 488 | header->checksum = UDP_NO_CHECKSUM; |
---|
| 489 | } |
---|
| 490 | |
---|
| 491 | /**********************************************************************************************************************/ |
---|
| 492 | /** |
---|
| 493 | * @brief ICMP Functions |
---|
| 494 | * |
---|
| 495 | **********************************************************************************************************************/ |
---|
| 496 | |
---|
| 497 | |
---|
| 498 | /*****************************************************************************/ |
---|
| 499 | /** |
---|
| 500 | * Process the ICMP packet |
---|
| 501 | * |
---|
| 502 | * @param eth_rx_queue_buffer - Rx Ethernet packet containing ICMP |
---|
| 503 | * @param eth_tx_queue_buffer - Buffer with space to construct Tx Ethernet packet |
---|
| 504 | * |
---|
| 505 | * @return int - Always returns 0 since we don't want higher level transports |
---|
| 506 | * to process this packet |
---|
| 507 | * |
---|
| 508 | * @note The library only support Echo Request ICMP packets |
---|
| 509 | * @note This function assumes that both Ethernet device and buffer are valid. |
---|
| 510 | * |
---|
| 511 | *****************************************************************************/ |
---|
| 512 | int icmp_process_packet(eth_rx_queue_buffer_t* eth_rx_queue_buffer, eth_tx_queue_buffer_t* eth_tx_queue_buffer) { |
---|
| 513 | |
---|
| 514 | icmp_header_t* icmp; |
---|
| 515 | ipv4_header_t* ip_hdr; |
---|
| 516 | |
---|
| 517 | ip_hdr = (ipv4_header_t*)(eth_rx_queue_buffer->pkt + sizeof(ethernet_header_t)); |
---|
| 518 | |
---|
| 519 | icmp = (icmp_header_t*)((u8*)ip_hdr + sizeof(ipv4_header_t)); |
---|
| 520 | |
---|
| 521 | // Check if this is an ICMP Echo Request to the node |
---|
| 522 | if ((icmp->type == ICMP_ECHO_REQUEST_TYPE) && (icmp->code == ICMP_ECHO_CODE) ) { |
---|
| 523 | // Send an ICMP Echo Reply |
---|
| 524 | icmp_echo_reply(eth_rx_queue_buffer, eth_tx_queue_buffer); |
---|
| 525 | } |
---|
| 526 | |
---|
| 527 | return 0; // Upper layer stacks should not process this packet so return zero bytes |
---|
| 528 | } |
---|
| 529 | |
---|
| 530 | |
---|
| 531 | |
---|
| 532 | /*****************************************************************************/ |
---|
| 533 | /** |
---|
| 534 | * Send an ICMP Echo Reply |
---|
| 535 | * |
---|
| 536 | * @param eth_rx_queue_buffer - Rx Ethernet packing containing ICMP Echo Request |
---|
| 537 | * @param eth_tx_queue_buffer - Buffer containing space to construct Tx Ethernet packet |
---|
| 538 | * |
---|
| 539 | * @return None |
---|
| 540 | * |
---|
| 541 | * @note This function assumes that both socket and buffer are valid. |
---|
| 542 | * |
---|
| 543 | *****************************************************************************/ |
---|
| 544 | void icmp_echo_reply(eth_rx_queue_buffer_t* eth_rx_queue_buffer, eth_tx_queue_buffer_t* eth_tx_queue_buffer) { |
---|
| 545 | |
---|
| 546 | icmp_echo_header_t* recv_icmp_hdr; |
---|
| 547 | icmp_echo_header_t* send_icmp_hdr; |
---|
| 548 | ipv4_header_t* recv_ip_hdr; |
---|
| 549 | ethernet_header_t* recv_eth_hdr; |
---|
| 550 | u8* recv_icmp_data; |
---|
| 551 | u32 recv_icmp_data_len; |
---|
| 552 | ipv4_header_t* ip_hdr; |
---|
| 553 | |
---|
| 554 | u8 my_ip_addr[IP_ADDR_LEN]; |
---|
| 555 | eth_get_ip_addr(my_ip_addr); |
---|
| 556 | |
---|
| 557 | u8 eth_hw_addr[ETH_ADDR_LEN]; |
---|
| 558 | |
---|
| 559 | eth_get_hw_addr(eth_hw_addr); |
---|
| 560 | |
---|
| 561 | recv_ip_hdr = (ipv4_header_t*)((eth_rx_queue_buffer->pkt) + sizeof(ethernet_header_t)); |
---|
| 562 | recv_eth_hdr = (ethernet_header_t*)(eth_rx_queue_buffer->pkt); |
---|
| 563 | |
---|
| 564 | recv_icmp_hdr = (icmp_echo_header_t*)((void*)recv_ip_hdr + sizeof(ipv4_header_t)); |
---|
| 565 | recv_icmp_data = (u8*)recv_icmp_hdr + sizeof(icmp_echo_header_t); |
---|
| 566 | recv_icmp_data_len = eth_rx_queue_buffer->length - sizeof(ethernet_header_t) - sizeof(ipv4_header_t) - sizeof(icmp_echo_header_t); |
---|
| 567 | |
---|
| 568 | if (eth_tx_queue_buffer != NULL) { |
---|
| 569 | |
---|
| 570 | // Initialize the Ethernet header |
---|
| 571 | eth_hdr_init((ethernet_header_t*)(eth_tx_queue_buffer->seg0), |
---|
| 572 | recv_eth_hdr->src_mac_addr, |
---|
| 573 | eth_hw_addr); |
---|
| 574 | |
---|
| 575 | ip_hdr = (ipv4_header_t*)((eth_tx_queue_buffer->seg0) + sizeof(ethernet_header_t)); |
---|
| 576 | |
---|
| 577 | // Initialize the IP header |
---|
| 578 | ip_hdr_init(ip_hdr, |
---|
| 579 | my_ip_addr, |
---|
| 580 | recv_ip_hdr->src_ip_addr, |
---|
| 581 | IP_PROTOCOL_ICMP); |
---|
| 582 | |
---|
| 583 | // Get the pointer to the ICMP reply header |
---|
| 584 | send_icmp_hdr = (icmp_echo_header_t*)((u8*)ip_hdr + sizeof(ipv4_header_t)); |
---|
| 585 | |
---|
| 586 | // Populate the ICMP reply |
---|
| 587 | send_icmp_hdr->type = ICMP_ECHO_REPLY_TYPE; |
---|
| 588 | send_icmp_hdr->code = ICMP_ECHO_CODE; |
---|
| 589 | send_icmp_hdr->checksum = 0; |
---|
| 590 | send_icmp_hdr->identifier = recv_icmp_hdr->identifier; |
---|
| 591 | send_icmp_hdr->seq_num = recv_icmp_hdr->seq_num; |
---|
| 592 | |
---|
| 593 | // Copy all the data from the request packet |
---|
| 594 | memcpy( (u8*)send_icmp_hdr + sizeof(icmp_echo_header_t), recv_icmp_data, recv_icmp_data_len); |
---|
| 595 | |
---|
| 596 | // Calculate the ICMP checksum |
---|
| 597 | send_icmp_hdr->checksum = Xil_Htons(ipv4_compute_checksum((u8*)send_icmp_hdr, |
---|
| 598 | eth_rx_queue_buffer->length - sizeof(ethernet_header_t) - sizeof(ipv4_header_t))); |
---|
| 599 | |
---|
| 600 | |
---|
| 601 | // By setting the length of segment 0, we inform the calling context we have filled |
---|
| 602 | // in an Ethernet frame that needs transmission |
---|
| 603 | eth_tx_queue_buffer->seg0_len = eth_rx_queue_buffer->length; |
---|
| 604 | |
---|
| 605 | // Set the length in the IP header and update checksum |
---|
| 606 | _set_ip_length(eth_tx_queue_buffer->seg0, eth_tx_queue_buffer->seg0_len); |
---|
| 607 | |
---|
| 608 | } else { |
---|
| 609 | xil_printf("Error: Provided with NULL eth_tx_queue_buffer_t*\n"); |
---|
| 610 | } |
---|
| 611 | } |
---|
| 612 | |
---|
| 613 | |
---|
| 614 | /*****************************************************************************/ |
---|
| 615 | /** |
---|
| 616 | * Set MAC Address |
---|
| 617 | * |
---|
| 618 | * @param hw_addr - u8 pointer of MAC address to set |
---|
| 619 | * |
---|
| 620 | * @return None |
---|
| 621 | * |
---|
| 622 | ******************************************************************************/ |
---|
| 623 | void eth_set_hw_addr(u8* hw_addr) { |
---|
| 624 | |
---|
| 625 | memcpy(gl_hw_addr, hw_addr, ETH_ADDR_LEN); |
---|
| 626 | |
---|
| 627 | return; |
---|
| 628 | } |
---|
| 629 | |
---|
| 630 | |
---|
| 631 | /*****************************************************************************/ |
---|
| 632 | /** |
---|
| 633 | * Get MAC Address |
---|
| 634 | * |
---|
| 635 | * @param hw_addr - u8 pointer to where MAC address will be copied to |
---|
| 636 | * |
---|
| 637 | * @return None |
---|
| 638 | * |
---|
| 639 | ******************************************************************************/ |
---|
| 640 | void eth_get_hw_addr(u8* hw_addr) { |
---|
| 641 | |
---|
| 642 | memcpy(hw_addr, gl_hw_addr, ETH_ADDR_LEN); |
---|
| 643 | |
---|
| 644 | return; |
---|
| 645 | } |
---|
| 646 | |
---|
| 647 | |
---|
| 648 | |
---|
| 649 | /*****************************************************************************/ |
---|
| 650 | /** |
---|
| 651 | * Set IP Address |
---|
| 652 | * |
---|
| 653 | * @param ip_addr - u8 pointer of IP address to set |
---|
| 654 | * |
---|
| 655 | * @return None |
---|
| 656 | * |
---|
| 657 | ******************************************************************************/ |
---|
| 658 | void eth_set_ip_addr(u8* ip_addr) { |
---|
| 659 | |
---|
| 660 | memcpy(gl_ip_addr, ip_addr, IP_ADDR_LEN); |
---|
| 661 | |
---|
| 662 | return; |
---|
| 663 | } |
---|
| 664 | |
---|
| 665 | /*****************************************************************************/ |
---|
| 666 | /** |
---|
| 667 | * Het IP Address |
---|
| 668 | * |
---|
| 669 | * @param ip_addr - u8 pointer to where IP address will be copied to |
---|
| 670 | * |
---|
| 671 | * @return None |
---|
| 672 | * |
---|
| 673 | ******************************************************************************/ |
---|
| 674 | void eth_get_ip_addr(u8* ip_addr) { |
---|
| 675 | |
---|
| 676 | memcpy(ip_addr, gl_ip_addr, IP_ADDR_LEN); |
---|
| 677 | |
---|
| 678 | return; |
---|
| 679 | } |
---|
| 680 | |
---|
| 681 | #endif // #if WLAN_SW_CONFIG_ENABLE_WLAN_EXP |
---|
| 682 | |
---|