int gp_io_writef(gp_io *io, uint16_t *types, ...)
{
va_list va;
uint8_t *ptr, t;
int32_t i4;
int16_t i2;
va_start(va, types);
while (*types != GP_IO_END) {
switch (TYPE(*types)) {
case GP_IO_CONST:
t = VAL(*types);
if (gp_io_write(io, &t, 1) != 1)
goto err;
break;
case GP_IO_L2:
case GP_IO_B2:
i2 = va_arg(va, int);
ptr = (void*)&i2;
if (needs_swap(*types))
GP_SWAP(ptr[0], ptr[1]);
if (gp_io_write(io, ptr, 2) != 2)
goto err;
break;
case GP_IO_L4:
case GP_IO_B4:
i4 = va_arg(va, int);
ptr = (void*)&i4;
if (needs_swap(*types)) {
GP_SWAP(ptr[0], ptr[3]);
GP_SWAP(ptr[1], ptr[2]);
}
if (gp_io_write(io, ptr, 4) != 4)
goto err;
break;
default:
GP_WARN("Invalid type %"PRIu16"\n", *types);
goto err;
}
types++;
}
va_end(va);
return 0;
err:
va_end(va);
return -1;
}
void sort_events(void) {
for (size_t i = 0; i < events_.size() - 1;) {
if (needs_swap(events_[i], events_[i + 1])) {
std::swap(events_[i], events_[i + 1]);
if (i > 0) {
--i;
}
continue;
}
++i;
}
}
int gp_io_readf(gp_io *self, uint16_t *types, ...)
{
unsigned int read_size, buf_size, size;
int ret;
va_list va;
uint8_t *ptr;
readf_size(types, &read_size, &buf_size);
if (!read_size)
return 0;
uint8_t buffer[buf_size], *buf = buffer;
if (gp_io_fill(self, buf, read_size))
return -1;
ret = 0;
va_start(va, types);
while (*types != GP_IO_END) {
switch (TYPE(*types)) {
case GP_IO_CONST:
//TODO: Seek back?
if (VAL(*types) != *buf) {
errno = EINVAL;
goto end;
}
buf++;
break;
case GP_IO_BYTE:
ptr = va_arg(va, uint8_t*);
*ptr = *buf;
buf++;
break;
case GP_IO_L2:
case GP_IO_B2:
ptr = va_arg(va, uint8_t*);
if (needs_swap(*types)) {
ptr[0] = buf[1];
ptr[1] = buf[0];
} else {
ptr[0] = buf[0];
ptr[1] = buf[1];
}
buf += 2;
break;
case GP_IO_L4:
case GP_IO_B4:
ptr = va_arg(va, uint8_t*);
if (needs_swap(*types)) {
ptr[0] = buf[3];
ptr[1] = buf[2];
ptr[2] = buf[1];
ptr[3] = buf[0];
} else {
ptr[0] = buf[0];
ptr[1] = buf[1];
ptr[2] = buf[2];
ptr[3] = buf[3];
}
buf += 4;
break;
case GP_IO_ARRAY:
ptr = va_arg(va, void*);
memcpy(ptr, buf, VAL(*types));
case GP_IO_IGN:
buf += VAL(*types);
break;
case GP_IO_PPSTR:
ptr = va_arg(va, void*);
size = *buf;
/* empty string */
if (!size) {
write_str(*types, ptr, NULL, 0);
buf += 2;
} else {
/* fill up another part of the buffer */
if (gp_io_fill(self, buf + read_size, size))
return -1;
read_size += size;
write_str(*types, ptr, buf + 1, size);
buf += GP_ALIGN2(size + 1);
}
break;
}
types++;
ret++;
}
end:
va_end(va);
return ret;
}
