+void i2c_send(unsigned char *buf, int len)
+{
+ uint8_t old_TWCR = TWCR;
+ uint8_t old_SREG = SREG;
+ int i;
+
+ cli();
+
+ TWCR = ((1<<TWINT) | (1<<TWSTA) | (1<<TWEN)); /* Send start */
+
+ while(!(TWCR & (1<<TWINT))) {}
+ if ((TW_STATUS & 0xf8) != TW_START)
+ goto out;
+
+ TWDR = buf[0]; /* SLA_W */
+ TWCR = ((1<<TWINT) | (1<<TWEN));
+
+ while(!(TWCR & (1<<TWINT))) {}
+ if ((TW_STATUS & 0xf8) != TW_MT_SLA_ACK)
+ goto out;
+
+ for(i = 1; i < len; i++) {
+ TWDR = buf[i]; /* Send Data */
+ TWCR = ((1<<TWINT) | (1<<TWEN));
+
+ while(!(TWCR & (1<<TWINT))) {}
+ if ((TW_STATUS & 0xf8) != TW_MT_DATA_ACK)
+ goto out;
+ }
+
+ TWCR = ((1<<TWINT) | (1<<TWEN) | (1<<TWSTO));
+ while(TWCR & (1<<TWSTO)) {}
+
+#ifdef DEBUG
+ printf("I2C Data sent\n");
+#endif
+
+out:
+ TWDR = 0x00;
+ TWCR = old_TWCR;
+ SREG = old_SREG;
+}
+