if (*p == 0x7E || *p == 0x7D) { //byte stuffing
You lost me on that line. Perhaps I should learn assembler?