@flp1969, yes you are right there are bugs, but I guess you realized this is a pseudo code to just ilustrate how the encoder works

Anyway, I have fixed the bug, it was because sometimes GDB sends ack together with data packets itself, this is very rare case and i had there:

Code:
strcpy(packet, packet+1);
to remove the preceding + sign. I don't know why I wrote it this was as simple:

Code:
packet = packet+1
is both faster and safer.

Thanks everybody involved in the discussion