libhdhomerun/hdhomerun_pkt.c

/*
 * hdhomerun_pkt.c
 *
 * Copyright © 2006-2014 Silicondust USA Inc. <www.silicondust.com>.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include "hdhomerun.h"

struct hdhomerun_pkt_t *hdhomerun_pkt_create(void)
{
	struct hdhomerun_pkt_t *pkt = (struct hdhomerun_pkt_t *)calloc(1, sizeof(struct hdhomerun_pkt_t));
	if (!pkt) {
		return NULL;
	}

	hdhomerun_pkt_reset(pkt);

	return pkt;
}

void hdhomerun_pkt_destroy(struct hdhomerun_pkt_t *pkt)
{
	free(pkt);
}

void hdhomerun_pkt_reset(struct hdhomerun_pkt_t *pkt)
{
	pkt->limit = pkt->buffer + sizeof(pkt->buffer) - 4;
	pkt->start = pkt->buffer + 1024;
	pkt->end = pkt->start;
	pkt->pos = pkt->start;
}

static uint32_t hdhomerun_pkt_calc_crc(uint8_t *start, uint8_t *end)
{
	uint8_t *pos = start;
	uint32_t crc = 0xFFFFFFFF;
	while (pos < end) {
		uint8_t x = (uint8_t)(crc) ^ *pos++;
		crc >>= 8;
		if (x & 0x01) crc ^= 0x77073096;
		if (x & 0x02) crc ^= 0xEE0E612C;
		if (x & 0x04) crc ^= 0x076DC419;
		if (x & 0x08) crc ^= 0x0EDB8832;
		if (x & 0x10) crc ^= 0x1DB71064;
		if (x & 0x20) crc ^= 0x3B6E20C8;
		if (x & 0x40) crc ^= 0x76DC4190;
		if (x & 0x80) crc ^= 0xEDB88320;
	}
	return crc ^ 0xFFFFFFFF;
}

uint8_t hdhomerun_pkt_read_u8(struct hdhomerun_pkt_t *pkt)
{
	uint8_t v = *pkt->pos++;
	return v;
}

uint16_t hdhomerun_pkt_read_u16(struct hdhomerun_pkt_t *pkt)
{
	uint16_t v;
	v =  (uint16_t)*pkt->pos++ << 8;
	v |= (uint16_t)*pkt->pos++ << 0;
	return v;
}

uint32_t hdhomerun_pkt_read_u32(struct hdhomerun_pkt_t *pkt)
{
	uint32_t v;
	v =  (uint32_t)*pkt->pos++ << 24;
	v |= (uint32_t)*pkt->pos++ << 16;
	v |= (uint32_t)*pkt->pos++ << 8;
	v |= (uint32_t)*pkt->pos++ << 0;
	return v;
}

size_t hdhomerun_pkt_read_var_length(struct hdhomerun_pkt_t *pkt)
{
	size_t length;
	
	if (pkt->pos + 1 > pkt->end) {
		return (size_t)-1;
	}

	length = (size_t)*pkt->pos++;
	if (length & 0x0080) {
		if (pkt->pos + 1 > pkt->end) {
			return (size_t)-1;
		}

		length &= 0x007F;
		length |= (size_t)*pkt->pos++ << 7;
	}
	
	return length; 
}

uint8_t *hdhomerun_pkt_read_tlv(struct hdhomerun_pkt_t *pkt, uint8_t *ptag, size_t *plength)
{
	if (pkt->pos + 2 > pkt->end) {
		return NULL;
	}
	
	*ptag = hdhomerun_pkt_read_u8(pkt);
	*plength = hdhomerun_pkt_read_var_length(pkt);

	if (pkt->pos + *plength > pkt->end) {
		return NULL;
	}
	
	return pkt->pos + *plength;
}

void hdhomerun_pkt_read_mem(struct hdhomerun_pkt_t *pkt, void *mem, size_t length)
{
	memcpy(mem, pkt->pos, length);
	pkt->pos += length;
}

void hdhomerun_pkt_write_u8(struct hdhomerun_pkt_t *pkt, uint8_t v)
{
	*pkt->pos++ = v;

	if (pkt->pos > pkt->end) {
		pkt->end = pkt->pos;
	}
}

void hdhomerun_pkt_write_u16(struct hdhomerun_pkt_t *pkt, uint16_t v)
{
	*pkt->pos++ = (uint8_t)(v >> 8);
	*pkt->pos++ = (uint8_t)(v >> 0);

	if (pkt->pos > pkt->end) {
		pkt->end = pkt->pos;
	}
}

void hdhomerun_pkt_write_u32(struct hdhomerun_pkt_t *pkt, uint32_t v)
{
	*pkt->pos++ = (uint8_t)(v >> 24);
	*pkt->pos++ = (uint8_t)(v >> 16);
	*pkt->pos++ = (uint8_t)(v >> 8);
	*pkt->pos++ = (uint8_t)(v >> 0);

	if (pkt->pos > pkt->end) {
		pkt->end = pkt->pos;
	}
}

void hdhomerun_pkt_write_var_length(struct hdhomerun_pkt_t *pkt, size_t v)
{
	if (v <= 127) {
		*pkt->pos++ = (uint8_t)v;
	} else {
		*pkt->pos++ = (uint8_t)(v | 0x80);
		*pkt->pos++ = (uint8_t)(v >> 7);
	}

	if (pkt->pos > pkt->end) {
		pkt->end = pkt->pos;
	}
}

void hdhomerun_pkt_write_mem(struct hdhomerun_pkt_t *pkt, const void *mem, size_t length)
{
	memcpy(pkt->pos, mem, length);
	pkt->pos += length;

	if (pkt->pos > pkt->end) {
		pkt->end = pkt->pos;
	}
}

int hdhomerun_pkt_open_frame(struct hdhomerun_pkt_t *pkt, uint16_t *ptype)
{
	pkt->pos = pkt->start;

	if (pkt->pos + 4 > pkt->end) {
		return 0;
	}

	*ptype = hdhomerun_pkt_read_u16(pkt);
	size_t length = hdhomerun_pkt_read_u16(pkt);
	pkt->pos += length;

	if (pkt->pos + 4 > pkt->end) {
		pkt->pos = pkt->start;
		return 0;
	}

	uint32_t calc_crc = hdhomerun_pkt_calc_crc(pkt->start, pkt->pos);

	uint32_t packet_crc;
	packet_crc =  (uint32_t)*pkt->pos++ << 0;
	packet_crc |= (uint32_t)*pkt->pos++ << 8;
	packet_crc |= (uint32_t)*pkt->pos++ << 16;
	packet_crc |= (uint32_t)*pkt->pos++ << 24;
	if (calc_crc != packet_crc) {
		return -1;
	}

	pkt->start += 4;
	pkt->end = pkt->start + length;
	pkt->pos = pkt->start;
	return 1;
}

void hdhomerun_pkt_seal_frame(struct hdhomerun_pkt_t *pkt, uint16_t frame_type)
{
	size_t length = pkt->end - pkt->start;

	pkt->start -= 4;
	pkt->pos = pkt->start;
	hdhomerun_pkt_write_u16(pkt, frame_type);
	hdhomerun_pkt_write_u16(pkt, (uint16_t)length);

	uint32_t crc = hdhomerun_pkt_calc_crc(pkt->start, pkt->end);
	*pkt->end++ = (uint8_t)(crc >> 0);
	*pkt->end++ = (uint8_t)(crc >> 8);
	*pkt->end++ = (uint8_t)(crc >> 16);
	*pkt->end++ = (uint8_t)(crc >> 24);

	pkt->pos = pkt->start;
}