static int test_gpio_nxp_pca9535_transfer(
i2c_bus *bus,
i2c_msg *msgs,
uint32_t msg_count,
test_device *base
)
{
test_device_gpio_nxp_pca9535 *dev = (test_device_gpio_nxp_pca9535 *) base;
i2c_msg *first = &msgs[0];
i2c_msg *second = &msgs[1];
int i;
/* Get command byte */
if (
msg_count < 1
|| (first->flags & I2C_M_RD) != 0
|| first->len < 1
) {
return -EIO;
}
dev->current_reg = first->buf[0];
if (first->len > 1) {
/* Write */
if (msg_count != 1) {
return -EIO;
}
for (i = 1; i < first->len; ++i) {
dev->regs[dev->current_reg] = first->buf[i];
/* Output is input */
if (dev->current_reg == 2) {
dev->regs[0] = first->buf[i];
} else if (dev->current_reg == 3) {
dev->regs[1] = first->buf[i];
}
cyclic_inc(&dev->current_reg, 2);
}
} else {
/* Read */
if (msg_count != 2) {
return -EIO;
}
for (i = 0; i < second->len; ++i) {
second->buf[i] = dev->regs[dev->current_reg];
cyclic_inc(&dev->current_reg, 2);
}
}
return 0;
}
SharedQueue::element SharedQueue::pop_() {
element result = buf[(int)*head];
if(*head == *end_priority) {
*end_priority = cyclic_inc(*end_priority);
}
if(*head == *end_medium) {
*end_medium = cyclic_inc(*end_medium);
}
*head = cyclic_inc(*head);
return result;
}
void SharedQueue::shift_tail(char **desired_end) {
const int first_stationary = cyclic_dec(**desired_end);
int cur = cyclic_dec(*end);
while(cur != first_stationary) {
int next = cyclic_inc(cur);
buf[next] = buf[cur];
cur = cyclic_dec(cur);
}
*end = cyclic_inc(*end);
}
static int test_eeprom_transfer(
i2c_bus *bus,
i2c_msg *msgs,
uint32_t msg_count,
test_device *base
)
{
test_device_eeprom *dev = (test_device_eeprom *) base;
i2c_msg *msg = &msgs[0];
uint32_t i;
if (dev->eio) {
return -EIO;
}
if (msg_count > 0 && (msg->flags & I2C_M_RD) == 0) {
if (msg->len < 1) {
return -EIO;
}
dev->current_address = msg->buf[0] | ((msg->addr & 0x1) << 8);
--msg->len;
++msg->buf;
}
for (i = 0; i < msg_count; ++i) {
int j;
msg = &msgs[i];
if ((msg->flags & I2C_M_RD) != 0) {
for (j = 0; j < msg->len; ++j) {
msg->buf[j] = dev->data[dev->current_address];
cyclic_inc(&dev->current_address, sizeof(dev->data));
}
} else {
for (j = 0; j < msg->len; ++j) {
dev->data[dev->current_address] = msg->buf[j];
cyclic_inc(&dev->current_address, 8);
}
}
}
return 0;
}
void SharedQueue::push_(const element el, const Priority p) {
char **pos_index_ptr = &end;
if(p == priority) {
shift_tail_priority();
pos_index_ptr = &end_priority;
} else if (p == medium) {
shift_tail_medium();
pos_index_ptr = &end_medium;
}
// standard - no shifting required
buf[(int)**pos_index_ptr] = el;
**pos_index_ptr = cyclic_inc(**pos_index_ptr);
}
void SharedQueue::shift_tail_priority() {
shift_tail(&end_priority);
*end_medium = cyclic_inc(*end_medium);
}
