Refactor code largely.

Try to keep order of register read/write same as windows driver does as
it makes comparing sniffs easier.
Signed-off-by: Antti Palosaari <crope@iki.fi>
Signed-off-by: Mauro Carvalho Chehab <mchehab@osg.samsung.com>