I can give you a sample. Working one, but not very good - it was just student project. I hade to implement it on 8051, and no one cares is it optimal or not :) So, mail me, and I'll send it to you :)