Посоветую взять готовый драйвер, коих сейчас навалом всяких.