Fix driver headers
Improve tests and adjust to new structure
P9221R driver updated to the new file structure