Sowas in der Art hab ich mal für nen Tiny15 in Assembler geschrieben.
Wurde in einem Polizeiautomodell eingesetzt.

Ist zwar wie gesagt Assembler, aber wenn Du willst kann ich es posten.

Gruß Sebastian