static void send_and_receive(void)
{
    uint8_t cmd[3];
    uint8_t resp[2];

    cmd[0] = TMP105_REG_TEMPERATURE;
    i2c_send(i2c, addr, cmd, 1);
    i2c_recv(i2c, addr, resp, 2);
    g_assert_cmpuint(((uint16_t)resp[0] << 8) | resp[1], ==, 0);

    cmd[0] = TMP105_REG_CONFIG;
    cmd[1] = 0x0; /* matches the reset value */
    i2c_send(i2c, addr, cmd, 2);
    i2c_recv(i2c, addr, resp, 1);
    g_assert_cmphex(resp[0], ==, cmd[1]);

    cmd[0] = TMP105_REG_T_LOW;
    cmd[1] = 0x12;
    cmd[2] = 0x34;
    i2c_send(i2c, addr, cmd, 3);
    i2c_recv(i2c, addr, resp, 2);
    g_assert_cmphex(resp[0], ==, cmd[1]);
    g_assert_cmphex(resp[1], ==, cmd[2]);

    cmd[0] = TMP105_REG_T_HIGH;
    cmd[1] = 0x42;
    cmd[2] = 0x31;
    i2c_send(i2c, addr, cmd, 3);
    i2c_recv(i2c, addr, resp, 2);
    g_assert_cmphex(resp[0], ==, cmd[1]);
    g_assert_cmphex(resp[1], ==, cmd[2]);
}

void smbus_write_byte(i2c_bus *bus, int addr, uint8_t command, uint8_t data)
{
    i2c_start_transfer(bus, addr, 0);
    i2c_send(bus, command);
    i2c_send(bus, data);
    i2c_end_transfer(bus);
}

/*
 * TODO!
 */
void RTC_WRITE(unsigned char WordAdr, unsigned char Data) {
    i2c_start();                // START condition
    i2c_send(RTC_ADR_W);        // Send a PCF8583 address to write
    i2c_send(WordAdr);          // Send on bus Word Address
    i2c_send(Data);             // Send on bus Data
    i2c_stop();                 // STOP condition
}

int eeprom_save(void)
{
        printf("Saving EEPROM data by 16 bytes pages...\n");

        // load eeprom by pages of 32 bytes each
        int counter = 0;

        for (int i = 0; i < CONFIG_EEPROM_SIZE / 16; i++) {
                // set address
                i2c_start(I2C_Direction_Transmitter, CONFIG_EEPROM_ADDRESS);

                i2c_send(counter >> 8);
                i2c_send(counter & 0xFF);

                // write data
                for (int j = 0; j < 16; j++) {
                        i2c_send(eeprom_map[counter++]);
                }
                
                I2C_GenerateSTOP(I2C1, ENABLE);

                while (!(I2C1->SR1 & I2C_SR1_BTF)) {
                        uint8_t data = I2C1->DR;
                        data++;
                }

                // wait for EEPROM to catch bytes
                delay_ms(20);
                early_putc('*');
        }

        printf("\nDone.\n");
        return 0;
}

/*! \brief i2c master read byte.
 * Read a byte from the slave with a stop at the end.
 * \param addr address of the slave.
 * \param byte pre-allocated byte.
 * \return 1 - value OK, 0 - Error.
 */
uint8_t i2c_master_read_b(const uint8_t addr, uint8_t *byte,
		const uint8_t stop)
{
	uint8_t err;

	err = i2c_send(START, 0);

	if ((err == TW_START) || (err == TW_REP_START))
		err = i2c_send(SLA, addr | READ);

	/* send an ACK to start the TX */
	if (err == TW_MR_SLA_ACK)
		err = i2c_send(ACK, 0);

	if (err == TW_MR_DATA_ACK) {
		*byte = TWDR;
		err = i2c_send(NACK, 0);
	}
	
	if (err == TW_MR_DATA_NACK)
		err = 0;

	if (stop)
		i2c_send(STOP, 0);

	return(err);
}

void smbus_write_word(i2c_bus *bus, int addr, uint8_t command, uint16_t data)
{
    i2c_start_transfer(bus, addr, 0);
    i2c_send(bus, command);
    i2c_send(bus, data & 0xff);
    i2c_send(bus, data >> 8);
    i2c_end_transfer(bus);
}

/*
 * TODO!
 */
unsigned char RTC_READ(unsigned char WordAdr) {
    unsigned char byte;

    i2c_start();                // START condition
    i2c_send(RTC_ADR_W);        // Send a PCF8583 address to write
    i2c_send(WordAdr);          // Send on bus Word Address
    i2c_start();                // START condition
    i2c_send(RTC_ADR_R);        // Send a PCF8583 address to read
    byte = i2c_take();          // Read the byte from I2C-bus
    i2c_stop();                 // STOP condition

    return(byte);               // Results
}

int main(void)
{
        uart_init();
        i2c_init();

        i2c_start(0x74);
        i2c_send(0x00);
        i2c_send(0x25);
        i2c_send(0x06);
        i2c_send(0x24);
        i2c_send(0x0F);
        i2c_stop();

        uart_getchar();

        i2c_start(0x74);
        i2c_send(0x40);
        i2c_send(0xC1);
        i2c_send(0xC2);
        i2c_stop();

        while(1 > 0) {

        }

        return 0;
}

void smbus_write_block(i2c_bus *bus, int addr, uint8_t command, uint8_t *data,
                       int len)
{
    int i;

    if (len > 32)
        len = 32;

    i2c_start_transfer(bus, addr, 0);
    i2c_send(bus, command);
    i2c_send(bus, len);
    for (i = 0; i < len; i++)
        i2c_send(bus, data[i]);
    i2c_end_transfer(bus);
}

irom static app_action_t application_function_i2c_write(application_parameters_t ap)
{
	uint16_t src_current, dst_current;
	i2c_error_t error;
	uint8_t bytes[32];

	for(src_current = 1, dst_current = 0;
			(src_current < ap.nargs) && (dst_current < sizeof(bytes));
			src_current++, dst_current++)
	{
		bytes[dst_current] = (uint8_t)strtoul((*ap.args)[src_current], 0, 16);
	}

	if((error = i2c_send(i2c_address, dst_current, bytes)) != i2c_error_ok)
	{
		i2c_error_format_string("i2c-write", error, ap.size, ap.dst);
		strlcat(ap.dst, "\n", ap.size);
		i2c_reset();
		return(app_action_error);
	}

	snprintf(ap.dst, ap.size, "i2c_write: written %u bytes to %02x\n", dst_current, i2c_address);

	return(app_action_normal);
}

int eps_slave_hk2(struct command_context *ctx) {

	printf("Requesting EPS HK2 data\r\n");
	eps_hk_t * chkparam;

	i2c_frame_t * frame;
	frame = csp_buffer_get(I2C_MTU);
	frame->dest = slave_node;
	frame->data[0] = EPS_PORT_HK;
	frame->data[1] = 0;
	frame->len = 2;
	frame->len_rx = 2 + (uint8_t) sizeof(eps_hk_t);
	frame->retries = 0;

	if (i2c_send(0, frame, 0) != E_NO_ERR) {
		csp_buffer_free(frame);
		return CMD_ERROR_NOMEM;
	}

	if (i2c_receive(0, &frame, 20) != E_NO_ERR)
		return CMD_ERROR_FAIL;

	chkparam = (eps_hk_t *)&frame->data[2];
	eps_hk_unpack(chkparam);
	eps_hk_print(chkparam);

	csp_buffer_free(frame);
	return CMD_ERROR_NONE;

}

int eps_slave_ping(struct command_context *ctx) {

	i2c_frame_t * frame;
	frame = csp_buffer_get(I2C_MTU);
	frame->dest = slave_node;
	frame->data[0] = CSP_PING; // Ping port
	frame->data[1] = 0x55;
	frame->len = 2;
	frame->len_rx = 3;
	frame->retries = 0;

	if (i2c_send(0, frame, 0) != E_NO_ERR) {
		csp_buffer_free(frame);
		return CMD_ERROR_NOMEM;
	}

	if (i2c_receive(0, &frame, 20) == E_NO_ERR) {
		printf("Received a reply from EPS!\r\n");
	} else {
		printf("No reply from EPS\r\n");
	}

	csp_buffer_free(frame);
	return CMD_ERROR_NONE;

}

irom static io_error_t read_register(string_t *error_message, int address, int reg, int *value)
{
	uint8_t i2cbuffer[2];
	i2c_error_t error;

	i2cbuffer[0] = reg;

	if((error = i2c_send(address, 1, &i2cbuffer[0])) != i2c_error_ok)
	{
		if(error_message)
			i2c_error_format_string(error_message, error);

		return(io_error);
	}

	if((error = i2c_receive(address, 1, &i2cbuffer[1])) != i2c_error_ok)
	{
		if(error_message)
			i2c_error_format_string(error_message, error);

		return(io_error);
	}

	*value = i2cbuffer[1];

	return(io_ok);
}

int eps_slave_volt(struct command_context *ctx) {

	char * args = command_args(ctx);
	unsigned int pv1, pv2, pv3;
	if (sscanf(args, "%u %u %u", &pv1, &pv2, &pv3) != 3)
		return CMD_ERROR_SYNTAX;
	printf("PV1: %04d PV2: %04d PV3: %04d\r\n", pv1, pv2, pv3);

	uint16_t pvolt[3];
	pvolt[0] = csp_hton16(pv1);
	pvolt[1] = csp_hton16(pv2);
	pvolt[2] = csp_hton16(pv3);

	i2c_frame_t * frame;
	frame = csp_buffer_get(I2C_MTU);
	frame->dest = slave_node;
	frame->data[0] = EPS_PORT_SET_VBOOST; // Ping port
	memcpy(&frame->data[1], &pvolt, 3 * sizeof(uint16_t));
	frame->len = 1 + 3 * sizeof(uint16_t);
	frame->len_rx = 0;
	frame->retries = 0;

	if (i2c_send(0, frame, 0) != E_NO_ERR) {
		csp_buffer_free(frame);
		return CMD_ERROR_NOMEM;
	}

	return CMD_ERROR_NONE;

}

int eps_slave_ppt_mode(struct command_context *ctx) {

	char * args = command_args(ctx);
	unsigned int mode;
	printf("EPS_PPT_MODE_OFF = 0, EPS_PPT_MODE_AUTO = 1, EPS_PPT_MODE_FIXED= 2\r\n");
	if (sscanf(args, "%d", &mode) != 1)
		return CMD_ERROR_SYNTAX;
	printf("Mode %d\r\n", mode);

	i2c_frame_t * frame;
	frame = csp_buffer_get(I2C_MTU);
	frame->dest = slave_node;
	frame->data[0] = EPS_PORT_SET_PPTMODE; // Ping port
	frame->data[1] = (uint8_t)mode;
	frame->len = 2;
	frame->len_rx = 0;
	frame->retries = 0;

	if (i2c_send(0, frame, 0) != E_NO_ERR) {
		csp_buffer_free(frame);
		return CMD_ERROR_NOMEM;
	}

	return CMD_ERROR_NONE;

}

int eps_slave_output(struct command_context *ctx) {

	char * args = command_args(ctx);
	unsigned int outputs;
	printf("console args: %s\r\n", args);
	if (sscanf(args, "%X", &outputs) != 1)
		return CMD_ERROR_SYNTAX;
	printf("Outputs 0x%02X\r\n", outputs);

	i2c_frame_t * frame;
	frame = csp_buffer_get(I2C_MTU);
	frame->dest = slave_node;
	frame->data[0] = EPS_PORT_SET_OUTPUT; // Ping port
	frame->data[1] = outputs;
	frame->len = 2;
	frame->len_rx = 0;
	frame->retries = 0;

	if (i2c_send(0, frame, 0) != E_NO_ERR) {
		csp_buffer_free(frame);
		return CMD_ERROR_NOMEM;
	}

	return CMD_ERROR_NONE;

}

int eps_slave_single_output(struct command_context *ctx) {

	char * args = command_args(ctx);
	unsigned int channel;
	unsigned int mode;
	int delay;
	printf("Input channel, mode (0=off, 1=on), and delay\r\n");
	if (sscanf(args, "%u %u %d", &channel, &mode, &delay) != 3)
		return CMD_ERROR_SYNTAX;
	printf("Channel %d is set to %d with delay %d\r\n", channel, mode, delay);

	eps_output_set_single_req eps_switch;
	eps_switch.channel = (uint8_t)channel;
	eps_switch.mode = (uint8_t)mode;
	eps_switch.delay = csp_hton16((int16_t)delay);

	i2c_frame_t * frame;
	frame = csp_buffer_get(I2C_MTU);
	frame->dest = slave_node;
	frame->data[0] = EPS_PORT_SET_SINGLE_OUTPUT; // Ping port
	memcpy(&frame->data[1], &eps_switch, sizeof(eps_switch));
	frame->len = 1 + sizeof(eps_switch);
	frame->len_rx = 0;
	frame->retries = 0;

	if (i2c_send(0, frame, 0) != E_NO_ERR) {
		csp_buffer_free(frame);
		return CMD_ERROR_NOMEM;
	}

	return CMD_ERROR_NONE;

}

void initialize_i2c(void)
{
	i2c_str i2c_h;
	i2c_msg_str msg;
	uint8_t tab[2];

	i2c_init_str i2c;
	i2c.i2c_clock_val = 15;
	i2c.op_mode = I2C_MASTER;
	i2c.own_address = 0;
	i2c.rx_buffer_size = 6;
	i2c.tx_buffer_size = 2;

	i2c_h = i2c_init(&i2c);


	msg.destination_address = 0b1101000;
	msg.generate_stop_after_transmission = 1;
	msg.i2c_buffer = (uint8_t*)&tab;
	msg.i2c_buffer_length = 2;
	msg.i2c_op_mode = I2C_MASTER;

	tab[0] = 0x68;
	tab[1] = 0x10;

	i2c_enable(i2c_h);

	i2c_send(i2c_h, &msg);

}

irom static app_action_t application_function_i2c_write(const string_t *src, string_t *dst)
{
	i2c_error_t error;
	static uint8_t bytes[32];
	int current, out;

	for(current = 0; current < (int)sizeof(bytes); current++)
	{
		if(parse_int(current + 1, src, &out, 16) != parse_ok)
			break;

		bytes[current] = (uint8_t)(out & 0xff);
	}

	if((error = i2c_send(i2c_address, current, bytes)) != i2c_error_ok)
	{
		string_cat(dst, "i2c_write");
		i2c_error_format_string(dst, error);
		string_cat(dst, "\n");
		i2c_reset();
		return(app_action_error);
	}

	string_format(dst, "i2c_write: written %d bytes to %02x\n", current, i2c_address);

	return(app_action_normal);
}

int csp_i2c_tx(csp_packet_t * packet, uint32_t timeout) {

	/* Cast the CSP packet buffer into an i2c frame */
	i2c_frame_t * frame = (i2c_frame_t *) packet;

	/* Insert destination node into the i2c destination field */
	if (csp_route_get_nexthop_mac(packet->id.dst) == CSP_NODE_MAC) {
		frame->dest = packet->id.dst;
	} else {
		frame->dest = csp_route_get_nexthop_mac(packet->id.dst);
	}

	/* Save the outgoing id in the buffer */
	packet->id.ext = csp_hton32(packet->id.ext);

	/* Add the CSP header to the I2C length field */
	frame->len += sizeof(packet->id);
	frame->len_rx = 0;

	/* Some I2C drivers support X number of retries
	 * CSP don't care about this. If it doesn't work the first
	 * time, don'y use time on it.
	 */
	frame->retries = 0;

	/* enqueue the frame */
	if (i2c_send(csp_i2c_handle, frame, timeout) != E_NO_ERR)
		return CSP_ERR_DRIVER;

	return CSP_ERR_NONE;

}