Have a look at this minimalistic firmware by DrJones. It is well commented, so you should be able to get started: MiniDrv - simple firmware (source code)
I have to agree with the others… Writing firmware with semi-complex features takes time. People here are very helpful and share knowledge, but to be frank: you are asking for too much.