From e015201915b51609452c1a68cc8c57a7fb82292d Mon Sep 17 00:00:00 2001 From: zhji Date: Sat, 15 Mar 2025 17:54:38 +0800 Subject: [PATCH] [feat] add udp function --- component/CMakeLists.txt | 2 + component/eth/CMakeLists.txt | 10 + component/eth/udp.c | 258 +++++++++++++++++++++++ component/eth/udp.h | 53 +++++ component/pio_instance/inc/pio_rmii_tx.h | 6 +- 5 files changed, 326 insertions(+), 3 deletions(-) create mode 100644 component/eth/CMakeLists.txt create mode 100644 component/eth/udp.c create mode 100644 component/eth/udp.h diff --git a/component/CMakeLists.txt b/component/CMakeLists.txt index 4bb4aa3..f7647fd 100644 --- a/component/CMakeLists.txt +++ b/component/CMakeLists.txt @@ -5,5 +5,7 @@ set(TARGET component) add_library(${TARGET} STATIC ${FILELIST}) add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/pio_instance pio_instance) +add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/eth eth) target_link_libraries(${TARGET} pio_instance) +target_link_libraries(${TARGET} eth) diff --git a/component/eth/CMakeLists.txt b/component/eth/CMakeLists.txt new file mode 100644 index 0000000..66ac72f --- /dev/null +++ b/component/eth/CMakeLists.txt @@ -0,0 +1,10 @@ +file(GLOB FILELIST +udp.c +) + +set(TARGET eth) +add_library(${TARGET} STATIC ${FILELIST}) + +target_include_directories(${TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +# target_include_directories(${TARGET} PUBLIC ${BASE_DIR}/driver/inc) +# target_include_directories(${TARGET} PUBLIC ${BASE_DIR}/driver/inc/reg) diff --git a/component/eth/udp.c b/component/eth/udp.c new file mode 100644 index 0000000..bc26de1 --- /dev/null +++ b/component/eth/udp.c @@ -0,0 +1,258 @@ +#include "udp.h" + +static int udp_char2idx(char c, uint8_t *idx) +{ + if (c >= '0' && c <= '9') { + *idx = c - '0'; + } else if (c >= 'A' && c <= 'F') { + *idx = c - 'A' + 10; + } else if (c >= 'a' && c <= 'f') { + *idx = c - 'a' + 10; + } else { + return -1; + } + return 0; +} + +static int udp_str2mac(char *str, uint8_t *mac) +{ + int i, ret; + uint8_t high, low; + + for (i = 0; i < 6; i++) { + if (i >= 5) { + if (str[2] != '\0') { + return -2; + } + } else { + if (str[2] != '-' && str[2] != ':' && str[2] != ' ' && str[2] != '_') { + return -3; + } + } + ret = udp_char2idx(str[0], &high); + ret |= udp_char2idx(str[1], &low); + if (ret) { + return -1; + } + mac[i] = (high << 4) | low; + str += 3; + } + return 0; +} + +static int udp_str2ip(char *str, uint8_t *ip) +{ + char c; + int idx, len; + uint8_t num; + + idx = 0; + len = 0; + num = 0; + while (1) { + c = *str; + if (c >= '0' && c <= '9') { + c = c - '0'; + num = num * 10 + c; + len++; + if (len > 3) { + return -3; + } + } else if (c == '.') { + ip[idx++] = num; + num = 0; + len = 0; + if (idx >= 4) { + break; + } + } else if (c == '\0') { + ip[idx++] = num; + break; + } else { + return -1; + } + str++; + } + if (idx == 4) { + return 0; + } else { + return -2; + } +} + +static int udp_str2port(char *str, uint8_t *port) +{ + char c; + int len; + uint16_t num; + + len = 0; + num = 0; + while (1) { + c = *str; + if (c >= '0' && c <= '9') { + c = c - '0'; + num = num * 10 + c; + len++; + if (len > 5) { + return -2; + } + } else if (c == '\0') { + break; + } else { + return -1; + } + str++; + } + port[0] = (num >> 8) & 0xFF; + port[1] = num & 0xFF; + + return 0; +} + +static void udp_padding_length(struct udp_header_s *udp, uint16_t length) +{ + uint16_t num; + + num = length + 28; + udp->ip_total_length[0] = (num >> 8) & 0xFF; + udp->ip_total_length[1] = num & 0xFF; + num = length + 8; + udp->udp_total_length[0] = (num >> 8) & 0xFF; + udp->udp_total_length[1] = num & 0xFF; +} + +static uint16_t udp_checksum(uint32_t init, uint16_t *addr, uint16_t len) +{ + uint32_t sum = init; + + while (len > 1) { + sum += *addr++; + len -= 2; + } + if (len == 1) { + sum += *(uint8_t *)addr; + } + while (sum >> 16) { + sum = (sum >> 16) + (sum & 0xFFFF); + sum += (sum >> 16); + } + return (uint16_t)~sum; +} + +static void udp_checksum_ip(struct udp_header_s *udp) +{ + uint32_t sum; + uint16_t *p; + + udp->ip_checksum[0] = 0; + udp->ip_checksum[1] = 0; + p = (uint16_t *)(&(udp->ver_head_length)); + sum = udp_checksum(0, p, 20); + udp->ip_checksum[0] = sum & 0xFF; + udp->ip_checksum[1] = (sum >> 8) & 0xFF; +} + +static void udp_checksum_udp(struct udp_header_s *udp) +{ +#if UDP_CHECKSUM_ENABLE + uint32_t sum = 0; + uint16_t *p; + uint16_t length; + + /* pseudo_header */ + p = (uint16_t *)(&(udp->ip_src)); + sum = sum + p[0] + p[1]; + p = (uint16_t *)(&(udp->ip_dst)); + sum = sum + p[0] + p[1]; + sum = sum + (udp->prototal << 8); + p = (uint16_t *)(&(udp->udp_total_length)); + sum = sum + p[0]; + /* UDP head and data */ + udp->udp_checksum[0] = 0; + udp->udp_checksum[1] = 0; + p = (uint16_t *)(&(udp->port_src)); + length = (udp->udp_total_length[0] << 8) + udp->udp_total_length[1]; + sum = udp_checksum(sum, p, length); + udp->udp_checksum[0] = sum & 0xFF; + udp->udp_checksum[1] = (sum >> 8) & 0xFF; +#else + udp->udp_checksum[0] = 0; + udp->udp_checksum[1] = 0; +#endif +} + +int udp_create_default(struct udp_cfg_s *cfg, struct udp_header_s *udp) +{ + int ret; + + udp->preamble[0] = 0x55; // fixed 55 D5 + udp->preamble[1] = 0xD5; + /* MAC */ + ret = udp_str2mac(cfg->mac_dst, udp->mac_dst); + if (ret) { + return -1; + } + ret = udp_str2mac(cfg->mac_src, udp->mac_src); + if (ret) { + return -2; + } + udp->ip_type[0] = 0x08; // 08 00 for IPv4 type + udp->ip_type[1] = 0x00; + /* IP */ + udp->ver_head_length = 0x45; // 0x45 for IPv4 and head length is 5*4=20 bytes + udp->tos = 0; // type of service, normal 0 + udp->ip_total_length[0] = 0; // 28 + user data length, e.g. 00 20 for 32 bytes + udp->ip_total_length[1] = 0; + udp->id[0] = 0; + udp->id[1] = 0; + udp->offset[0] = 0; + udp->offset[1] = 0; + udp->ttl = 0x80; // time to live, normal 0x80 + udp->prototal = 0x11; // 0x11 for UDP prototal + udp->ip_checksum[0] = 0; + udp->ip_checksum[1] = 0; + ret = udp_str2ip(cfg->ip_src, udp->ip_src); // e.g. C0 A8 02 18 is for 192.168.2.24 + if (ret) { + return -3; + } + ret = udp_str2ip(cfg->ip_dst, udp->ip_dst); // e.g. C0 A8 02 20 is for 192.168.2.32 + if (ret) { + return -4; + } + /* UDP */ + ret = udp_str2port(cfg->port_src, udp->port_src); // e.g. 26 E5 is for 9957 + if (ret) { + return -5; + } + ret = udp_str2port(cfg->port_dst, udp->port_dst); // e.g. 0E D3 is for 3795 + if (ret) { + return -6; + } + udp->udp_total_length[0] = 0; // 8 + user data length, e.g. 00 0C for 12 bytes + udp->udp_total_length[1] = 0; + udp->udp_checksum[0] = 0; + udp->udp_checksum[1] = 0; + + return 0; +} + +uint32_t *udp_copy_header(uint32_t *dst, struct udp_header_s *udp) +{ + uint32_t *p; + int i; + + p = (uint32_t *)udp; + for (i = 0; i < sizeof(struct udp_header_s) / 4; i++) { + dst[i] = p[i]; + } + return (dst + i); +} + +uint16_t udp_pack_data(struct udp_header_s *udp, uint16_t length) +{ + udp_padding_length(udp, length); + udp_checksum_ip(udp); + udp_checksum_udp(udp); + return (sizeof(struct udp_header_s) / 4 + length); +} diff --git a/component/eth/udp.h b/component/eth/udp.h new file mode 100644 index 0000000..3b74f6f --- /dev/null +++ b/component/eth/udp.h @@ -0,0 +1,53 @@ +#ifndef __ETH_UDP_H__ +#define __ETH_UDP_H__ + +#include "stdint.h" + +#define UDP_CHECKSUM_ENABLE 1 + +struct udp_cfg_s { + char *mac_src; + char *mac_dst; + char *ip_src; + char *ip_dst; + char *port_src; + char *port_dst; +}; + +struct udp_header_s { + uint8_t preamble[2]; // fixed 55 D5 + /* MAC */ + uint8_t mac_dst[6]; + uint8_t mac_src[6]; + uint8_t ip_type[2]; // 08 00 for IPv4 type + /* IP */ + uint8_t ver_head_length; // 0x45 for IPv4 and head length is 5*4=20 bytes + uint8_t tos; // type of service, normal 0 + uint8_t ip_total_length[2]; // 28 + user data length, e.g. 00 20 for 32 bytes + uint8_t id[2]; + uint8_t offset[2]; + uint8_t ttl; // time to live, normal 0x80 + uint8_t prototal; // 0x11 for UDP prototal + uint8_t ip_checksum[2]; // normal 00 00 + uint8_t ip_src[4]; // e.g. C0 A8 02 18 is for 192.168.2.24 + uint8_t ip_dst[4]; // e.g. C0 A8 02 20 is for 192.168.2.32 + /* UDP */ + uint8_t port_src[2]; // e.g. 26 E5 is for 9957 + uint8_t port_dst[2]; // e.g. 0E D3 is for 3795 + uint8_t udp_total_length[2]; // 8 + user data length, e.g. 00 0C for 12 bytes + uint8_t udp_checksum[2]; +}; + +#ifdef __cplusplus +extern "C" { +#endif + +int udp_create_default(struct udp_cfg_s *cfg, struct udp_header_s *udp); +uint32_t *udp_copy_header(uint32_t *dst, struct udp_header_s *udp); +uint16_t udp_pack_data(struct udp_header_s *udp, uint16_t length); + +#ifdef __cplusplus +} +#endif + +#endif /* __ETH_UDP_H__ */ \ No newline at end of file diff --git a/component/pio_instance/inc/pio_rmii_tx.h b/component/pio_instance/inc/pio_rmii_tx.h index f9c0575..893dc77 100644 --- a/component/pio_instance/inc/pio_rmii_tx.h +++ b/component/pio_instance/inc/pio_rmii_tx.h @@ -1,5 +1,5 @@ -#ifndef __HARDWARE_PIO_RMII_TX_H__ -#define __HARDWARE_PIO_RMII_TX_H__ +#ifndef __PIO_RMII_TX_H__ +#define __PIO_RMII_TX_H__ #include "pio.h" @@ -22,4 +22,4 @@ void pio_rmii_write(struct pio_rmii_tx_s *rmii, uint32_t *data, uint32_t length) } #endif -#endif /* __HARDWARE_PIO_RMII_TX_H__ */ +#endif /* __PIO_RMII_TX_H__ */