static int do_command(uint16_t command, uint16_t seqnum, uint16_t varid, uint8_t *value, uint16_t length)
{
	unsigned char cp[254], rp[254];
	uint8_t cmd[10];
	uint16_t size;
	uint8_t delay, activity = 0x00;
	int timeout = 0, sent = 0;

	size = (length < 8) ? 9 : ((length + 1) / 2) + 5;

	cmd[0] = command & 0xff;
	cmd[1] = command >> 8;
	cmd[2] = size & 0xff;
	cmd[3] = size >> 8;
	cmd[4] = seqnum & 0xff;
	cmd[5] = seqnum >> 8;
	cmd[6] = varid & 0xff;
	cmd[7] = varid >> 8;
	cmd[8] = 0x00;
	cmd[9] = 0x00;

	memset(cp, 0, sizeof(cp));
	cp[0] = 0x00;
	cp[1] = 0xfc;
	cp[2] = (size * 2) + 1;
	cp[3] = 0xc2;
	memcpy(cp + 4, cmd, sizeof(cmd));
	memcpy(cp + 14, value, length);

	receive_packet.length = 512;
	ubcsp_receive_packet(&receive_packet);

	send_packet.channel  = 5;
	send_packet.reliable = 1;
	send_packet.length   = (size * 2) + 4;
	memcpy(send_packet.payload, cp, (size * 2) + 4);

	ubcsp_send_packet(&send_packet);

	while (1) {
		delay = ubcsp_poll(&activity);

		if (activity & UBCSP_PACKET_SENT) {
			switch (varid) {
			case CSR_VARID_COLD_RESET:
			case CSR_VARID_WARM_RESET:
			case CSR_VARID_COLD_HALT:
			case CSR_VARID_WARM_HALT:
				return 0;
			}

			sent = 1;
			timeout = 0;
		}

		if (activity & UBCSP_PACKET_RECEIVED) {
			if (sent && receive_packet.channel == 5 &&
					receive_packet.payload[0] == 0xff) {
				memcpy(rp, receive_packet.payload,
							receive_packet.length);
				break;
			}

			receive_packet.length = 512;
			ubcsp_receive_packet(&receive_packet);
			timeout = 0;
		}

		if (delay) {
			usleep(delay * 100);

			if (timeout++ > 100) {
				fprintf(stderr, "Operation timed out\n");
				return -1;
			}
		}
	}

	if (rp[0] != 0xff || rp[2] != 0xc2) {
		errno = EIO;
		return -1;
	}

	if ((rp[11] + (rp[12] << 8)) != 0) {
		errno = ENXIO;
		return -1;
	}

	memcpy(value, rp + 13, length);

	return 0;
}