Encode and Decode messages for webscokets.

websocket_codec.h

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

#define HTONS(n) (((((unsigned short)(n) & 0xFF)) << 8) | (((unsigned short)(n) & 0xFF00) >> 8))
#define NTOHS(n) (((((unsigned short)(n) & 0xFF)) << 8) | (((unsigned short)(n) & 0xFF00) >> 8))

#define HTONL(n) (((((unsigned long)(n) & 0xFF)) << 24) | \
                  ((((unsigned long)(n) & 0xFF00)) << 8) | \
                  ((((unsigned long)(n) & 0xFF0000)) >> 8) | \
                  ((((unsigned long)(n) & 0xFF000000)) >> 24))

#define NTOHL(n) (((((unsigned long)(n) & 0xFF)) << 24) | \
                  ((((unsigned long)(n) & 0xFF00)) << 8) | \
                  ((((unsigned long)(n) & 0xFF0000)) >> 8) | \
                  ((((unsigned long)(n) & 0xFF000000)) >> 24))

char *websocket_encode(char *data, int data_len, char fin, char opcode, int key, int *count) {
	int i, length;
	char *packet, *back;
	const int size = (sizeof(char) * 2) + sizeof(long int) + sizeof(int) + data_len;
	
	if ((packet = calloc(size, 1 )) == NULL) return NULL;
	back = packet;
	
	if (data_len <= 125) length = data_len;
	else if (data_len < 0xFFFF) length = 126;
	else length = 127;
	
	*packet = (!!fin << 7) | 0 | opcode;
	packet += sizeof(char);
	
	*packet = ((key != 0) << 7) | length;
	packet += sizeof(char);
	
	if (length == 126) {
		length = HTONS(data_len);
		memcpy(packet, &length, sizeof(short int));
		packet += sizeof(short int);
	}
	else if (length == 127) {
		length = HTONL(data_len);
		memcpy(packet, &length, sizeof(long int));
		packet += sizeof(long int);
	}
	
	if (key != 0) {
		memcpy(packet, &key, sizeof(int));
		packet += sizeof(int);
		
		for (i=0; i<data_len; i++)
			data[i] = data[i] ^ (char)(key >> (8 * (i % 4)));
	}
	
	memcpy(packet, data, data_len);
	if (count) *count = (packet - back) + data_len;
	
	return back;
}

char *websocket_decode(char *data) {
	int i, length, key;
	char masked, *decoded;
	
	data += sizeof(char);
	
	masked = *data >> 7;
	length = *data & 0x7f;
	data += sizeof(char);
	
	if (length == 126) {
		memcpy(&length, data, sizeof(short int));
		data += sizeof(short int);
		length = NTOHS((short)length);
	}
	else if (length == 127) {
		memcpy(&length, data, sizeof(long int));
		data += sizeof(long int);
		length = NTOHL((long int)length);
	}
	
	if ((decoded = calloc(1, length + 1)) == NULL) return NULL;
	
	if (masked == 0) {
		memcpy(decoded, data, length);
		return decoded;
	}
	
	memcpy(&key, data, sizeof(int));
	data += sizeof(int);
	
	for (i=0; i<length; i++)
		decoded[i] = data[i] ^ (char)(key >> (8 * (i % 4)));
	
	decoded[i] = 0;
	
	return decoded;
}

/*///////////////////////////////////
	
		Testing:
		
///////////////////////////////////*/

int main(void) {
	char payload[] = "Hello, world!";
	char *encoded, *decoded;
	int i, count;
	
	encoded = websocket_encode(payload, strlen(payload), 1, 0x1, 6969, &count);
	
	for (i=0; i<count; i++)
		printf("0x%x ", encoded[i]);
	printf("\n");
	
	decoded = websocket_decode(encoded);
	
	printf("%s\n", decoded);
	
	free(encoded);
	free(decoded);
	
	return 0;
}