Added queue for I2C

Still need to merge with Dhiraj's code