hello guys...i try to make an application, i have to make to communicate 2 devices together and send packets to each other...the problem is i cant manage to develop the bit stuffing problem...can anyone help me with that?

bit stuffing: one counter will count the bytes and if check that 5 bits are "1" in row then it puts one 0 after those 5 "1" bits...

i want that program work in c, thanks a lot