Стаффинг:

берётся байт, скажем в аккум. 16 бит, в младшие 8 вставляются нули(где надо), с учётом того, сколько единиц было до этого в конце байта в предыдущей раз (берём, допустим из B)
При этом в B сохраняется новое значение количества единиц в конце младшего байта.
И ещё нужно гдето сохранять количество "лишних" бит, добавленных при операции, просуммировав это с предыдущим значением.

т.е. три регистра
a) 16 бит накопитель
b) 8 бит - количество единичных бит в конце байта в прошлый раз
c) 8 бит - количество лишних бит

При этом потребуется потребуются такие операции:
1) проверить с), если == 8 - перейти на 3)
2) считать байт из памяти, сдвинуть на величину с), сложить с а)
3) стаффинг
4) прочитать младший байт а) и отправить на периферию
5) сдвинуть а) на 8 бит


Дестаффинг правда сложнее, там мля ошибки нужно обнаруживать, и флаги :-(