hlswmaster-ng/netpkt.cpp

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>

// needed for string.h / memmem()
#ifndef __USE_GNU
#define __USE_GNU
#endif

#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <ctype.h>

#include "netpkt.h"
#include "logging.h"

NetPkt::NetPkt(const char* data, int size)
: data(data), size(size), alloc(0)
{
}

NetPkt::NetPkt(int alloc)
: size(0), alloc(alloc)
{
	data = new char[alloc];
}

NetPkt::~NetPkt()
{
	if (alloc > 0)
		delete [] data;
}

int NetPkt::show(char* buf, int size)
{
	return snprintf(buf, size, "(%s:%d) (%d/%d) bytes",
		inet_ntoa(this->addr.sin_addr),
		ntohs(this->addr.sin_port),
		this->size, this->alloc);
}

bool NetPkt::compare(unsigned int offset, const char* buf, unsigned int len)
{
	if (offset + len >= this->size)
		return false;

	return (memcmp(this->data + offset, buf, len) == 0);
}

int NetPkt::find(unsigned int offset, const char *buf, unsigned int len)
{
	if (offset >= this->size)
		return -1;

	void* found = memmem(this->data + offset, this->size, buf, len);

	return (found == NULL) ? -1 : ((char*)found - this->data);
}

void NetPkt::setAddress(struct sockaddr_in *tmp)
{
	memcpy(&addr, tmp, sizeof(addr));
}

struct sockaddr_in * NetPkt::getAddress()
{
	return &addr;
}

int NetPkt::parse_int(unsigned int offset, int *val)
{
	const char *max = this->data + this->size;
	const char *c = this->data + offset;

	/* untere grenze abtesten */
	if (this->data > c || c > max)
		return -1;

	*val = 0;

	/* ziffern einlesen */
	while (isdigit(*c) && c < max)
		*val = (*val * 10) + (*c++ - 0x30);

	return (c - (this->data + offset));
}

int NetPkt::parse_ip(unsigned int offset, struct in_addr *ip)
{
	int i, tmp, count, pos = offset;
	ip->s_addr = 0;

	for (i = 0; i < 4; i++) {
		count = this->parse_int(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 && this->data[pos++] != '.')
			return 0;
	}
	return pos - offset;
}

int NetPkt::getPort()
{
	return ntohs(addr.sin_port);
}

char* NetPkt::showfull()
{
	unsigned int pos = 0, i = 0, j;
	char *buf = new char[this->size * 4 + 64];

	while (pos < this->size) {
		i += sprintf(buf + i, "%04X: ", pos);
		for (j = 0; j < 16; j++) {
			if (pos + j < this->size)
				i += sprintf(buf + i, "%02X", (unsigned char)this->data[pos + j]);
			else
				i += sprintf(buf + i, "  ");

			if (j % 2)
				buf[i++] = ' ';
		}

		for (j = 0; j < 16; j++) {
			if (pos + j < this->size) {
				unsigned char val = this->data[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;
}

int NetPkt::getSize()
{
	return size;
}

bool NetPkt::sameAddress(NetPkt* pkt)
{
	return (this->addr.sin_addr.s_addr == pkt->addr.sin_addr.s_addr) &&
		(this->addr.sin_port == pkt->addr.sin_port);
}

void NetPkt::merge(NetPkt* pkt)
{
	unsigned int new_alloc = size + pkt->size;

	char* new_data = new char[new_alloc];
	memcpy(new_data, data, size);

	if (alloc)
		delete [] data;

	data = new_data;
	alloc = new_alloc;

	memcpy((void*)(data + size), pkt->data, pkt->size);
	size += pkt->size;
}

bool NetPkt::append(const char* buf, unsigned int len)
{
	if (alloc < size + len)
		return false;

	memcpy((void*)(data + size), buf, len);
	size += len;

	return true;
}