void read_and_echo_data(struct device *dev)
{
uart_buf_t *rx_buf;
while(true) {
rx_buf = k_fifo_get(&rx_fifo, K_FOREVER);
if (rx_buf != NULL) {
uint16_t len = sys_be16_to_cpu(*(uint16_t *)rx_buf->data);
printf("FIFO get: %d bytes [hdr=%d]\n", rx_buf->len, len);
/* if len is 0, echo bach the same data */
/* if len is !0, echo bach len bytes of data w/ filled content so receiver can verify */
if (len != 0) {
fill_data(rx_buf->data, len);
}
else {
len = rx_buf->len;
}
send_data(dev, rx_buf->data, len);
}
}
}
static int hmc5883l_sample_fetch(struct device *dev, enum sensor_channel chan)
{
struct hmc5883l_data *drv_data = dev->driver_data;
s16_t buf[3];
__ASSERT_NO_MSG(chan == SENSOR_CHAN_ALL);
/* fetch magnetometer sample */
if (i2c_burst_read(drv_data->i2c, HMC5883L_I2C_ADDR,
HMC5883L_REG_DATA_START, (u8_t *)buf, 6) < 0) {
LOG_ERR("Failed to fetch megnetometer sample.");
return -EIO;
}
drv_data->x_sample = sys_be16_to_cpu(buf[0]);
drv_data->z_sample = sys_be16_to_cpu(buf[1]);
drv_data->y_sample = sys_be16_to_cpu(buf[2]);
return 0;
}
static int adt7420_temp_reg_read(struct device *dev, u8_t reg, s16_t *val)
{
struct adt7420_data *drv_data = dev->driver_data;
const struct adt7420_dev_config *cfg = dev->config->config_info;
if (i2c_burst_read(drv_data->i2c, cfg->i2c_addr,
reg, (u8_t *) val, 2) < 0) {
return -EIO;
}
*val = sys_be16_to_cpu(*val);
return 0;
}
static int mpu6050_sample_fetch(struct device *dev, enum sensor_channel chan)
{
struct mpu6050_data *drv_data = dev->driver_data;
s16_t buf[7];
if (i2c_burst_read(drv_data->i2c, CONFIG_MPU6050_I2C_ADDR,
MPU6050_REG_DATA_START, (u8_t *)buf, 14) < 0) {
SYS_LOG_ERR("Failed to read data sample.");
return -EIO;
}
drv_data->accel_x = sys_be16_to_cpu(buf[0]);
drv_data->accel_y = sys_be16_to_cpu(buf[1]);
drv_data->accel_z = sys_be16_to_cpu(buf[2]);
drv_data->temp = sys_be16_to_cpu(buf[3]);
drv_data->gyro_x = sys_be16_to_cpu(buf[4]);
drv_data->gyro_y = sys_be16_to_cpu(buf[5]);
drv_data->gyro_z = sys_be16_to_cpu(buf[6]);
return 0;
}
char *net_addr_ntop(sa_family_t family, const void *src,
char *dst, size_t size)
{
struct in_addr *addr;
struct in6_addr *addr6;
u16_t *w;
u8_t i, bl, bh, longest = 1;
s8_t pos = -1;
char delim = ':';
unsigned char zeros[8] = { 0 };
char *ptr = dst;
int len = -1;
u16_t value;
bool needcolon = false;
if (family == AF_INET6) {
addr6 = (struct in6_addr *)src;
w = (u16_t *)addr6->s6_addr16;
len = 8;
for (i = 0; i < 8; i++) {
u8_t j;
for (j = i; j < 8; j++) {
if (UNALIGNED_GET(&w[j]) != 0) {
break;
}
zeros[i]++;
}
}
for (i = 0; i < 8; i++) {
if (zeros[i] > longest) {
longest = zeros[i];
pos = i;
}
}
if (longest == 1) {
pos = -1;
}
} else if (family == AF_INET) {
addr = (struct in_addr *)src;
len = 4;
delim = '.';
} else {
return NULL;
}
for (i = 0; i < len; i++) {
/* IPv4 address a.b.c.d */
if (len == 4) {
u8_t l;
value = (u32_t)addr->s4_addr[i];
/* net_byte_to_udec() eats 0 */
if (value == 0) {
*ptr++ = '0';
*ptr++ = delim;
continue;
}
l = net_value_to_udec(ptr, value, 0);
ptr += l;
*ptr++ = delim;
continue;
}
/* IPv6 address */
if (i == pos) {
if (needcolon || i == 0) {
*ptr++ = ':';
}
*ptr++ = ':';
needcolon = false;
i += longest - 1;
continue;
}
if (needcolon) {
*ptr++ = ':';
needcolon = false;
}
value = (u32_t)sys_be16_to_cpu(UNALIGNED_GET(&w[i]));
bh = value >> 8;
bl = value & 0xff;
if (bh) {
if (bh > 0x0f) {
ptr = net_byte_to_hex(ptr, bh, 'a', false);
} else {
if (bh < 10) {
*ptr++ = (char)(bh + '0');
} else {
*ptr++ = (char) (bh - 10 + 'a');
}
}
ptr = net_byte_to_hex(ptr, bl, 'a', true);
} else if (bl > 0x0f) {
ptr = net_byte_to_hex(ptr, bl, 'a', false);
} else {
if (bl < 10) {
*ptr++ = (char)(bl + '0');
} else {
*ptr++ = (char) (bl - 10 + 'a');
}
}
needcolon = true;
}
if (!(ptr - dst)) {
return NULL;
}
if (family == AF_INET) {
*(ptr - 1) = '\0';
} else {
*ptr = '\0';
}
return dst;
}
