Now device driver basic services are working and handshake is OK. Still a mess though, must refactor soon...