144 lines
2.9 KiB
C
144 lines
2.9 KiB
C
#include <stdio.h>
|
|
|
|
#define __USE_GNU
|
|
#include <string.h>
|
|
|
|
#include <ctype.h>
|
|
|
|
#include <sys/socket.h>
|
|
#include <netinet/in.h>
|
|
#include <netinet/ip.h>
|
|
#include <arpa/inet.h>
|
|
|
|
#include "netpkt.h"
|
|
|
|
int pkt_memcmp(struct net_pkt *pkt, unsigned int offset, char *search, unsigned int size)
|
|
{
|
|
|
|
if (offset >= pkt->size)
|
|
return 1;
|
|
|
|
/* nicht ueber das paket hinaus vergleichen */
|
|
if (offset + size >= pkt->size)
|
|
size = pkt->size - offset;
|
|
|
|
return memcmp(pkt->buf + offset, search, size);
|
|
}
|
|
|
|
int pkt_memmem(struct net_pkt *pkt, unsigned int offset, char *search, unsigned int size)
|
|
{
|
|
void *found;
|
|
|
|
if (offset >= pkt->size)
|
|
return -1;
|
|
|
|
found = memmem(pkt->buf + offset, pkt->size, search, size);
|
|
|
|
return (found == NULL) ? -1 : (found - (void *)pkt->buf);
|
|
}
|
|
|
|
struct net_pkt * pkt_merge(struct net_pkt *pkt1, struct net_pkt *pkt2)
|
|
{
|
|
struct net_pkt *ret;
|
|
ret = malloc(sizeof(struct net_pkt) + pkt1->size + pkt2->size);
|
|
|
|
memcpy(&ret->addr, &pkt1->addr, sizeof(ret->addr));
|
|
ret->size = pkt1->size + pkt2->size;
|
|
|
|
memcpy(ret->buf, pkt1->buf, pkt1->size);
|
|
memcpy(ret->buf + pkt1->size, pkt2->buf, pkt2->size);
|
|
|
|
return ret;
|
|
}
|
|
|
|
char * pkt_ntoa(struct net_pkt *pkt)
|
|
{
|
|
return inet_ntoa(pkt->addr.sin_addr);
|
|
}
|
|
|
|
unsigned short pkt_getport(struct net_pkt *pkt)
|
|
{
|
|
return ntohs(pkt->addr.sin_port);
|
|
}
|
|
|
|
int pkt_sameaddr(struct net_pkt *pkt1, struct net_pkt *pkt2)
|
|
{
|
|
return (pkt1->addr.sin_addr.s_addr == pkt2->addr.sin_addr.s_addr) &&
|
|
(pkt1->addr.sin_port == pkt2->addr.sin_port);
|
|
}
|
|
|
|
int pkt_parse_int(struct net_pkt *pkt, unsigned int offset, int *val)
|
|
{
|
|
unsigned char *max = pkt->buf + pkt->size;
|
|
unsigned char *c = pkt->buf + offset;
|
|
|
|
/* untere grenze abtesten */
|
|
if (pkt->buf > c || c > max)
|
|
return -1;
|
|
|
|
*val = 0;
|
|
|
|
/* ziffern einlesen */
|
|
while (isdigit(*c) && c < max)
|
|
*val = (*val * 10) + (*c++ - 0x30);
|
|
|
|
return (c - (pkt->buf + offset));
|
|
}
|
|
|
|
int pkt_parse_ip(struct net_pkt *pkt, int offset, struct in_addr *ip)
|
|
{
|
|
int i, tmp, count, pos = offset;
|
|
ip->s_addr = 0;
|
|
|
|
for (i = 0; i < 4; i++) {
|
|
count = pkt_parse_int(pkt, pos, &tmp);
|
|
pos += count;
|
|
if (count == 0 || tmp < 0 || tmp > 255)
|
|
return 0;
|
|
|
|
ip->s_addr = ip->s_addr>>8 | tmp<<24;
|
|
|
|
if (i != 3 && pkt->buf[pos++] != '.')
|
|
return 0;
|
|
}
|
|
return pos - offset;
|
|
}
|
|
|
|
char * pkt_print(struct net_pkt *pkt)
|
|
{
|
|
int pos = 0, i = 0, j;
|
|
char *buf = malloc(pkt->size * 4 + 64);
|
|
|
|
while (pos < pkt->size) {
|
|
i += sprintf(buf + i, "%04X: ", pos);
|
|
for (j = 0; j < 16; j++) {
|
|
if (pos + j < pkt->size)
|
|
i += sprintf(buf + i, "%02X", pkt->buf[pos + j]);
|
|
else
|
|
i += sprintf(buf + i, " ");
|
|
|
|
if (j % 2)
|
|
buf[i++] = ' ';
|
|
}
|
|
|
|
for (j = 0; j < 16; j++) {
|
|
if (pos + j < pkt->size) {
|
|
unsigned char val = pkt->buf[pos + j];
|
|
if (val >= 0x20 && val < 0x80)
|
|
buf[i++] = val;
|
|
else
|
|
buf[i++] = '.';
|
|
} else {
|
|
buf[i++] = ' ';
|
|
}
|
|
}
|
|
|
|
pos += 16;
|
|
buf[i++] = '\r';
|
|
buf[i++] = '\n';
|
|
}
|
|
buf[i] = 0;
|
|
return buf;
|
|
}
|
|
|