stuff.c

int byteStuffing(char **buf, int length){

	int newlength = length;

	int i;
	for (i = 1; i < length - 1; i++)
		if ((*buf)[i] == FLAG || (*buf)[i] == ESCAPE)
			newlength++;

	*buf = (unsigned char*) realloc(*buf, newlength);

	for(i= 1 ; i <length; i++)
		if ((*buf)[i] == FLAG || (*buf)[i] == ESCAPE){
			memmove(*buf + i + 1, *buf + i, length - i); /*void *memmove(void *str1, const void *str2, size_t n) */
			length++;

			(*buf)[i] = ESCAPE;
			(*buf)[i + 1] ^= 0x20;
		}
	}

	return newlength;
}

int byteDeStuffing(unsigned char** buf, int length) {
	int i;
	for (i = 1; i < length - 1; ++i) {
		if ((*buf)[i] == ESCAPE) {
			memmove(*buf + i, *buf + i + 1, length - i - 1);

			length--;

			(*buf)[i] ^= 0x20;
		}
	}

	*buf = (unsigned char*) realloc(*buf, length);

	return length;
}