Bug fixing and cleanup. Driver working.

Signed-off-by: Carlo Caione <carlo.caione@gmail.com>