void /*PORT_NAME*/_forwardMessage(byte * msg, uint8_t size) {
/*TRACE_LEVEL_2*/printf("[/*PORT_NAME*/] forwarding message\n");
/*PORT_NAME*/_send_byte(/*PORT_NAME*/_device_id, /*PORT_NAME*/_START_BYTE);
uint8_t i;
for(i = 0; i < size; i++) {
if((msg[i] == /*PORT_NAME*/_START_BYTE) || (msg[i] == /*PORT_NAME*/_STOP_BYTE) || (msg[i] == /*PORT_NAME*/_ESCAPE_BYTE)) {
/*PORT_NAME*/_send_byte(/*PORT_NAME*/_device_id, /*PORT_NAME*/_ESCAPE_BYTE);
}
/*PORT_NAME*/_send_byte(/*PORT_NAME*/_device_id, msg[i]);
}
/*PORT_NAME*/_send_byte(/*PORT_NAME*/_device_id, /*PORT_NAME*/_STOP_BYTE);
}
uint8_t I2C_Write_Bytes(I2C_t device, uint8_t address, uint8_t *data, uint8_t length) {
_send_start(device.port);
_send_address(device.port, address, 0);
int i = 0;
for (; i < length; i++) {
_send_byte(device.port, data[i]);
}
return i;
}
bool write_registers(uint8_t addr, uint8_t reg, const uint8_t* data, uint8_t size)
{
QASSERT(s_is_initialized);
QASSERT(s_is_locked);
QASSERT(data && size > 0);
if (!_start()) goto error;
if (!_send_address(SLA_W(addr))) goto error;
if (!_send_byte(reg)) goto error;
for (int8_t i = size; i > 0; i--)
{
if (!_send_byte(*data++)) goto error;
}
if (!_stop()) goto error;
return true;
error:
s_lockup_count++;
return false;
}
bool read_registers_uint16_le(uint8_t addr, uint8_t reg, uint16_t* data, uint8_t size)
{
QASSERT(s_is_initialized);
QASSERT(s_is_locked);
QASSERT(data && size > 0);
if (!data || size == 0)
{
return 0;
}
uint8_t* data_ptr8 = reinterpret_cast<uint8_t*>(data) + 1;
//the data_ptr8 will advance like this
//1, 0, 3, 2, 5, 4, 7, 6, 9, 8
if (!_start()) goto error;
if (!_send_address(SLA_W(addr))) goto error;
if (!_send_byte(reg)) goto error;
if (!_start()) goto error;
if (!_send_address(SLA_R(addr))) goto error;
{
uint8_t i = size - 1;
while (i > 0)
{
if (!_receive_byte_ack()) goto error;
*data_ptr8 = TWDR;
data_ptr8 -= 1;
if (!_receive_byte_ack()) goto error;
*data_ptr8 = TWDR;
data_ptr8 += 3;
--i;
}
}
//last bytes
{
if (!_receive_byte_ack()) goto error;
*data_ptr8 = TWDR;
data_ptr8 -= 1;
if (!_receive_byte()) goto error;
*data_ptr8 = TWDR;
//data_ptr8 += 3;
}
if (!_stop()) goto error;
return true;
error:
s_lockup_count++;
return false;
}
bool read_registers(uint8_t addr, uint8_t reg, uint8_t* data, uint8_t size)
{
QASSERT(s_is_initialized);
QASSERT(s_is_locked);
QASSERT(size > 0);
if (size == 0)
{
return 0;
}
if (!_start()) goto error;
if (!_send_address(SLA_W(addr))) goto error;
if (!_send_byte(reg)) goto error;
if (!_start()) goto error;
if (!_send_address(SLA_R(addr))) goto error;
if (data)
{
uint8_t i = size - 1;
while (i > 0)
{
if (!_receive_byte_ack()) goto error;
*data++ = TWDR;
--i;
}
//last item
{
if (!_receive_byte()) goto error;
*data = TWDR;
}
}
else
{
uint8_t i = size - 1;
while (i > 0)
{
if (!_receive_byte_ack()) goto error;
s_i2c_null_sink = TWDR;
--i;
}
//last item
{
if (!_receive_byte()) goto error;
s_i2c_null_sink = TWDR;
}
}
if (!_stop()) goto error;
return true;
error:
s_lockup_count++;
return false;
}
static inline void _send_address(uint8_t port, uint8_t address, uint8_t read) {
if (address) {
//_send_byte(port, (address << 1) | read);
_send_byte(port, address | read);
}
}
uint8_t I2C_Write_Byte(I2C_t device, uint8_t address, uint8_t data) {
_send_start(device.port);
_send_address(device.port, address, 0);
return _send_byte(device.port, data);
}
