259 lines
6.1 KiB
C
259 lines
6.1 KiB
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);
|
|
}
|