rp2040/component/eth/udp.c

#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);
}