Fix receiving of incorrect queries in write_single and mask_write_register

Thank you very much James Nutaro.