diff --git a/components/free_rtos/ethernet_ip/eth_checksum.c b/components/free_rtos/ethernet_ip/eth_checksum.c index 7be35a7..aaeeb7a 100644 --- a/components/free_rtos/ethernet_ip/eth_checksum.c +++ b/components/free_rtos/ethernet_ip/eth_checksum.c @@ -5,6 +5,7 @@ * Author: sychev */ #include "free_rtos/ethernet_ip/eth_checksum.h" +#include "free_rtos/base/swap.h" uint16_t eth_calcChksum(uint32_t sum, uint8_t * const data, uint16_t len) { @@ -52,3 +53,58 @@ uint16_t eth_calcChksum2(uint32_t sum, uint8_t * const hdata, uint16_t hlen, uin return ~sum; } + +inline uint32_t calcSum(void * buffer, uint32_t sum, void * const data, uint16_t len) { + uint32_t * buffer_start32 = (uint32_t*)buffer; + uint32_t * data_start32 = (uint32_t*)data; + uint32_t * data_end32 = data_start32 + len/sizeof(uint32_t); + uint32_t remainder = len%sizeof(uint32_t); + uint32_t value; + + while(data_start32 < data_end32) { + value = *data_start32++; + *buffer_start32++ = value; + value = __rev16(value); + sum += (value&0xFFFF) + (value>>16); + } + + uint16_t * buffer_start16 = (uint16_t*)buffer_start32; + uint16_t * data_start16 = (uint16_t*)data_start32; + + if(remainder > 1) { + value = *data_start16++; + *buffer_start16++ = value; + value = __rev16(value); + sum += value; + } + + uint8_t * buffer_start8 = (uint8_t*)buffer_start16; + uint8_t * data_start8 = (uint8_t*)data_start16; + + if(remainder == 1) { + value = *data_start8++; + *buffer_start8++ = value; + value = __rev16(value); + sum += value; + } + + return sum; +} + +uint16_t eth_calcChksum3(void * buffer, uint32_t sum, void * const hdata, uint16_t hlen, void * const data, uint16_t len) +{ + if(hdata != 0) { + sum = calcSum(buffer, sum, hdata, hlen); + + } + + if(data != 0) { + sum = calcSum((uint8_t*)buffer + hlen, sum, data, len); + } + + while (sum >> 16) { + sum = (sum & 0xFFFF) + (sum >> 16); + } + + return ~sum; +} diff --git a/components/free_rtos/ethernet_ip/eth_checksum.h b/components/free_rtos/ethernet_ip/eth_checksum.h index 869e2ca..5b2f118 100644 --- a/components/free_rtos/ethernet_ip/eth_checksum.h +++ b/components/free_rtos/ethernet_ip/eth_checksum.h @@ -16,6 +16,7 @@ extern "C" { uint16_t eth_calcChksum(uint32_t sum, uint8_t * const data, uint16_t len); uint16_t eth_calcChksum2(uint32_t sum, uint8_t * const hdata, uint16_t hlen, uint8_t * const data, uint16_t len); +uint16_t eth_calcChksum3(void * buffer, uint32_t sum, void * const hdata, uint16_t hlen, void * const data, uint16_t len); #ifdef __cplusplus } diff --git a/components/free_rtos/ethernet_ip/eth_ip.cpp b/components/free_rtos/ethernet_ip/eth_ip.cpp index 4ac4418..9629dda 100644 --- a/components/free_rtos/ethernet_ip/eth_ip.cpp +++ b/components/free_rtos/ethernet_ip/eth_ip.cpp @@ -89,43 +89,46 @@ bool free_rtos::EthIp::send(TEthMacPorts port_id, uint32_t dest_ip, uint8_t prot return eth_stack_.send_pkt(port_id, mac_dst, ETH_PROT_IP_BE, pkt); } -inline uint16_t free_rtos::EthIp::fillIpHeader(IpHandlerArgs& ipHandlerArgs) -{ - static_assert(sizeof(TIpHeader) == (IP_HEADER_WORD_SIZE * 4), "Ip header size doesn't match version !"); - - uint16_t ip_len = sizeof(TIpHeader) + ipHandlerArgs.ip_header_len + ipHandlerArgs.ip_data_len; - uint16_t cksum = 0; - - memcpy(ipHandlerArgs.ip_header.ip_src, &self_ip_, ETH_IP_ADDR_LEN); - memcpy(ipHandlerArgs.ip_header.ip_dst, &ipHandlerArgs.dest_ip, ETH_IP_ADDR_LEN); - - ipHandlerArgs.ip_header.ip_hl_v = (IP_VER << 4) | IP_HEADER_WORD_SIZE; - ipHandlerArgs.ip_header.ip_tos = 0; - ipHandlerArgs.ip_header.ip_len = BASE_SWAP16(ip_len); - ipHandlerArgs.ip_header.ip_id = BASE_SWAP16(ip_indet_); - ipHandlerArgs.ip_header.ip_off = 0; - ipHandlerArgs.ip_header.ip_ttl = IP_TIME_TO_LIVE; - ipHandlerArgs.ip_header.ip_p = ipHandlerArgs.ip_prot_id; - ipHandlerArgs.ip_header.ip_sum = 0; - - cksum = eth_calcChksum(0, (uint8_t *)&ipHandlerArgs.ip_header, sizeof(TIpHeader)); - - ipHandlerArgs.ip_header.ip_sum = BASE_SWAP16(cksum); - - ++ip_indet_; - - return sizeof(TIpHeader); -} - bool free_rtos::EthIp::send(IpHandlerArgs& handlerArgs) { handlerArgs.stack_handler = this; - fillIpHeader(handlerArgs); - return eth_stack_.send_pkt(handlerArgs); } +inline uint16_t free_rtos::EthIp::fillIpHeader(IpHandlerArgs& ipHandlerArgs) +{ + static_assert(sizeof(TIpHeader) == (IP_HEADER_WORD_SIZE * 4), "Ip header size doesn't match version !"); + + TIpHeader * ip_hdr = (TIpHeader *)(ipHandlerArgs.buffer + sizeof(TEthFrameHeader)); + uint16_t ip_len = sizeof(TIpHeader) + ipHandlerArgs.ip_header_len + ipHandlerArgs.ip_data_len; + TIpHeader ip_header; + uint16_t cksum = 0; + + memcpy(ip_header.ip_src, &self_ip_, ETH_IP_ADDR_LEN); + memcpy(ip_header.ip_dst, &ipHandlerArgs.dest_ip, ETH_IP_ADDR_LEN); + + ip_header.ip_hl_v = (IP_VER << 4) | IP_HEADER_WORD_SIZE; + ip_header.ip_tos = 0; + ip_header.ip_len = BASE_SWAP16(ip_len); + ip_header.ip_id = BASE_SWAP16(ip_indet_); + ip_header.ip_off = 0; + ip_header.ip_ttl = IP_TIME_TO_LIVE; + ip_header.ip_p = ipHandlerArgs.ip_prot_id; + ip_header.ip_sum = 0; + + //cksum = eth_calcChksum(0, (uint8_t *)&ipHandlerArgs.ip_header, sizeof(TIpHeader)); + cksum = eth_calcChksum3(ip_hdr, 0, + &ip_header, sizeof(TIpHeader), + nullptr, 0); + + ip_hdr->ip_sum = BASE_SWAP16(cksum); + + ++ip_indet_; + + return sizeof(TIpHeader); +} + inline uint16_t free_rtos::EthIp::fillEthFrameHeader(IpHandlerArgs& ipHandlerArgs) { TEthFrameHeader * p_eth_hdr = (TEthFrameHeader *)ipHandlerArgs.buffer; @@ -143,8 +146,7 @@ uint16_t free_rtos::EthIp::Sender(HandlerArgs& handlerArgs, size_t scatter_segme IpHandlerArgs& ipHandlerArgs = static_cast(handlerArgs); // downcasting back to derived type fillEthFrameHeader(ipHandlerArgs); - TIpHeader * ip_header = (TIpHeader *)(ipHandlerArgs.buffer + sizeof(TEthFrameHeader)); - *ip_header = ipHandlerArgs.ip_header; + fillIpHeader(ipHandlerArgs); ipHandlerArgs.ip_handler->Sender(ipHandlerArgs, scatter_segment); diff --git a/components/free_rtos/ethernet_ip/eth_ip.hpp b/components/free_rtos/ethernet_ip/eth_ip.hpp index f5d6d3f..cce0864 100644 --- a/components/free_rtos/ethernet_ip/eth_ip.hpp +++ b/components/free_rtos/ethernet_ip/eth_ip.hpp @@ -48,7 +48,7 @@ private: uint16_t ip_indet_; /// \u043f\u0457\u0405\u043f\u0457\u0405\u043f\u0457\u0405\u043f\u0457\u0405 ip_id - inline uint16_t fillIpHeader(IpHandlerArgs& handlerArgs); + inline uint16_t fillIpHeader(IpHandlerArgs& ipHandlerArgs); inline uint16_t fillEthFrameHeader(IpHandlerArgs& ipHandlerArgs); }; diff --git a/components/free_rtos/ethernet_ip/eth_ip_iface.hpp b/components/free_rtos/ethernet_ip/eth_ip_iface.hpp index e66d391..205c02a 100644 --- a/components/free_rtos/ethernet_ip/eth_ip_iface.hpp +++ b/components/free_rtos/ethernet_ip/eth_ip_iface.hpp @@ -21,7 +21,6 @@ struct IpHandlerArgs : public EthStackHandlerArgs { uint64_t dest_mac; uint8_t ip_prot_id; Handler* ip_handler; - TIpHeader ip_header; }; class EthIpIface { diff --git a/components/free_rtos/ethernet_ip/eth_udp_client.cpp b/components/free_rtos/ethernet_ip/eth_udp_client.cpp index a2c7bb4..b955147 100644 --- a/components/free_rtos/ethernet_ip/eth_udp_client.cpp +++ b/components/free_rtos/ethernet_ip/eth_udp_client.cpp @@ -179,36 +179,6 @@ bool free_rtos::EthUpdClient::write(uint32_t ip_dst_be, uint8_t * p_data, uint32 } */ -uint16_t free_rtos::EthUpdClient::fill_udp_header(UDPHandlerArgs& udpHandlerArgs) { - uint16_t udp_len = udpHandlerArgs.ip_header_len + udpHandlerArgs.ip_data_len; - - udpHandlerArgs.udp_header.Src_port = port_src_be_; - udpHandlerArgs.udp_header.Dst_port = port_dst_be_; - udpHandlerArgs.udp_header.Length = BASE_SWAP16(udp_len); - udpHandlerArgs.udp_header.Chk_sum = 0; - - if (use_checksum_) { - uint32_t self_ip = ip_iface_.getSelfIpAddr(); - uint32_t chk_sum32 = 0; - uint8_t * const dst_ip = (uint8_t *)&udpHandlerArgs.dest_ip; - uint8_t * const src_ip = (uint8_t *)&self_ip; - - for (int j = 0; j < ETH_IP_ADDR_LEN; j += 2) - { - chk_sum32 += ((uint16_t)dst_ip[j] << 8) + dst_ip[j+1]; - chk_sum32 += ((uint16_t)src_ip[j] << 8) + src_ip[j+1]; - } - - chk_sum32 += IP_PROT_UDP + udp_len; - - uint16_t chk_sum16 = eth_calcChksum2(chk_sum32, (uint8_t *)&udpHandlerArgs.udp_header, sizeof(TUdpHeader), (uint8_t *)udpHandlerArgs.p_data, udpHandlerArgs.ip_data_len); - - udpHandlerArgs.udp_header.Chk_sum = BASE_SWAP16(chk_sum16); - } - - return sizeof(TUdpHeader); -} - bool free_rtos::EthUpdClient::write(uint32_t ip_dst_be, uint8_t * p_data, uint32_t len) { UDPHandlerArgs udpHandlerArgs; @@ -234,19 +204,49 @@ bool free_rtos::EthUpdClient::write(uint32_t ip_dst_be, uint8_t * p_data, uint32 udpHandlerArgs.ip_handler = this; udpHandlerArgs.p_data = p_data; - fill_udp_header(udpHandlerArgs); - return ip_iface_.send(udpHandlerArgs); } +uint16_t free_rtos::EthUpdClient::fill_udp_header(UDPHandlerArgs& udpHandlerArgs) { + uint16_t udp_len = udpHandlerArgs.ip_header_len + udpHandlerArgs.ip_data_len; + TUdpHeader * udp_hdr = (TUdpHeader *)(udpHandlerArgs.buffer + sizeof(TEthFrameHeader) + sizeof(TIpHeader)); + TUdpHeader udp_header; + + udp_header.Src_port = port_src_be_; + udp_header.Dst_port = port_dst_be_; + udp_header.Length = BASE_SWAP16(udp_len); + udp_header.Chk_sum = 0; + + if (use_checksum_) { + uint32_t self_ip = ip_iface_.getSelfIpAddr(); + uint32_t chk_sum32 = 0; + uint8_t * const dst_ip = (uint8_t *)&udpHandlerArgs.dest_ip; + uint8_t * const src_ip = (uint8_t *)&self_ip; + + for (int j = 0; j < ETH_IP_ADDR_LEN; j += 2) + { + chk_sum32 += ((uint16_t)dst_ip[j] << 8) + dst_ip[j+1]; + chk_sum32 += ((uint16_t)src_ip[j] << 8) + src_ip[j+1]; + } + + chk_sum32 += IP_PROT_UDP + udp_len; + + //uint16_t chk_sum16 = eth_calcChksum2(chk_sum32, (uint8_t *)&udpHandlerArgs.udp_header, sizeof(TUdpHeader), (uint8_t *)udpHandlerArgs.p_data, udpHandlerArgs.ip_data_len); + uint16_t chk_sum16 = eth_calcChksum3(udp_hdr, chk_sum32, + &udp_header, sizeof(TUdpHeader), + udpHandlerArgs.p_data, udpHandlerArgs.ip_data_len); + + udp_hdr->Chk_sum = BASE_SWAP16(chk_sum16); + } + + return sizeof(TUdpHeader); +} + uint16_t free_rtos::EthUpdClient::Sender(HandlerArgs& handlerArgs, size_t scatter_segment) { UDPHandlerArgs& udpHandlerArgs = static_cast(handlerArgs); // downcasting back to derived type - TUdpHeader * hdr = (TUdpHeader *)(udpHandlerArgs.buffer + sizeof(TEthFrameHeader) + sizeof(TIpHeader)); - uint8_t * p_udp_data = (uint8_t *)(udpHandlerArgs.buffer + sizeof(TEthFrameHeader) + sizeof(TIpHeader) + sizeof(TUdpHeader)); - *hdr = udpHandlerArgs.udp_header; - memcpy(p_udp_data, udpHandlerArgs.p_data, udpHandlerArgs.ip_data_len); + fill_udp_header(udpHandlerArgs); return udpHandlerArgs.ip_header_len + udpHandlerArgs.ip_data_len; } diff --git a/components/free_rtos/ethernet_ip/eth_udp_client.hpp b/components/free_rtos/ethernet_ip/eth_udp_client.hpp index 884f900..2d9e158 100644 --- a/components/free_rtos/ethernet_ip/eth_udp_client.hpp +++ b/components/free_rtos/ethernet_ip/eth_udp_client.hpp @@ -29,7 +29,6 @@ namespace free_rtos { struct UDPHandlerArgs : public IpHandlerArgs { void * p_data; - TUdpHeader udp_header; }; enum UDPToggle : uint32_t {