Got software I2C working again after rewrite