spyderta/common/msg.c

#include <stdbool.h>
#include "msg.h"

/*
 * send async [1]
 * [10: bus][6: node][48 : address][8: tcode][8 : tlabel][16 : len][data ...]
 */
#define SEND_ASYNC_LEN (10 + 6 + 48 + 8 + 8 + 16)/8

/*
 * send iso [2]
 * [8 : channel][8 : tag][8 : synch][16 : len][data ...]
 */
#define SEND_ISO_LEN (8 + 8 + 8 + 16)/8

/*
 * allocate [3]
 * [48: start] [48: len]
 */
#define ALLOCATE_LEN (48 + 48)/8

static bool parse_send_async(struct msg_state *st) {
	const char *p = st->buf + 1;
	const size_t l = sdslen(st->buf) - 1;

	if (l < SEND_ASYNC_LEN)
		return false;
	st->msg.send_async.len = p[10] << 8 | p[11];
	if (l < SEND_ASYNC_LEN + len)
		return false;

	st->msg.send_async.bus = p[0] << 8 | p[1];
	st->msg.send_async.node = bus & 0x3F;
	st->msg.send_async.bus >>= 6;

	st->msg.send_async.addr = p[2] << 40 | p[3] << 32 | p[4] << 24 | p[5] << 16
	              | p[6] << 8 | p[7];
	st->msg.send_async.tcode = p[8];
	st->msg.send_async.tlabel = p[9];
	st->msg.send_async.buf = p + SEND_ASYNC_LEN;

	(*st->on_read[MSG_SEND_ASYNC])(&st->msg);

	sdsrange(st->buf, SEND_ASYNC_LEN + len, sdslen(st->buf));
	st->state = PARSE_VERSION_MESSAGE;
	return true;
}

static bool parse_version_message(struct msg_state *st) {
	if (sdslen(st->buf) == 0)
		return false;

	/* Unknown version: ignore byte and continue */
	if (st->buf[0] >> 6 != 0)
		return true;

	st->msgtype = st->buf[0] & 0x3F;
	switch (st->msgtype) {
	case MSG_SEND_ASYNC:
		st->state = PARSE_SEND_ASYNC;
		break;
	case MSG_SEND_ISO:
		st->state = PARSE_SEND_ISO;
		break;
	case MSG_ALLOCATE:
		st->state = PARSE_ALLOCATE;
	/* Error or noop */
	default:
		st->state = PARSE_VERSION_MESSAGE;
		break;
	}

	return true;
}

static bool parse_allocate(struct msg_state *st) {
	if (l < ALLOCATE_LEN)
		return false;
	st->msg.allocate.start = p[0] << 40 | p[1] << 32 | p[2] << 24 | p[3] << 16
	                       | p[4] << 8 | p[5];
	st->msg.allocate.len = p[6] << 40 | p[7] << 32 | p[8] << 24 | p[9] << 16
	                     | p[10] << 8 | p[11];

	(*st->on_read[MSG_SEND_ALLOCATE])(&st->msg);

	return true;
}

static bool parse_send_iso(struct msg_state *st) {
	const char *p = st->buf + 1;
	const size_t l = sdslen(st->buf) - 1;

	if (l < SEND_ISO_LEN)
		return false;
	st->msg.send_iso.len = p[3] << 8 | p[4];
	if (l < SEND_ISO_LEN + len)
		return false;

	st->msg.send_iso.ch = p[0];
	st->msg.send_iso.tag = p[1];
	st->msg.send_iso.synch = p[2];
	st->msg.send_iso.buf = p + SEND_ISO_LEN;

	(*st->on_read[MSG_SEND_ASYNC])(&st->msg);

	sdsrange(st->buf, SEND_ISO_LEN + len, sdslen(st->buf));
	st->state = PARSE_VERSION_MESSAGE;
	return true;
}

bool parse_packet(struct msg_state *st) {
	switch(st->state) {
	case PARSE_VERSION_MESSAGE:
		return parse_version_message(st);
	case PARSE_SEND_ASYNC:
		return parse_send_async(st);
	case PARSE_ALLOCATE:
		return parse_allocate(st);
	case PARSE_SEND_ISO:
		return parse_send_iso(st);
	}
}

static int sendall(SOCKET sck, unsigned char *msg, size_t msglen) {
	int bytes_sent = 0;
	do {
		int n = send(sck, msg + bytes_sent, msglen - bytes_sent, 0);
		if (n == SOCKET_ERROR) {
			return 0;
		}

		bytes_sent += n;
	} while (bytes_sent < msglen);
	return 1;
}
int msg_send_alloc(SOCKET sck, uint64_t start, uint64_t len) {
	unsigned char *msg;
	int r;

	msg = malloc(ALLOCATE_LEN);
	msg[0] = MSG_ALLOCATE;
	msg[1] = start >> 40 & 0xFF;
	msg[2] = start >> 32 & 0xFF;
	msg[3] = start >> 24 & 0xFF;
	msg[4] = start >> 16 & 0xFF;
	msg[5] = start >> 8 & 0xFF;
	msg[6] = start & 0xFF;

	msg[7] = len >> 40 & 0xFF;
	msg[8] = len >> 32 & 0xFF;
	msg[9] = len >> 24 & 0xFF;
	msg[10] = len >> 16 & 0xFF;
	msg[11] = len >> 8 & 0xFF;
	msg[12] = len & 0xFF;

	r = sendall(sck, msg, ALLOCATE_LEN);
	free(msg);
	return r;
}

int msg_send_async(SOCKET sck, uint16_t bus, uint8_t node, uint64_t addr,
                   uint8_t tcode, uint8_t tag, uint16_t len, void *data) {
	unsigned char *msg;
	size_t msglen = SEND_ASYNC_LEN + 1 + len;
	int r;

	msg = malloc(msglen);
	msg[0] = MSG_SEND_ASYNC;

	msg[1] = buf >> 8;
	msg[2] = (bus & 0x3F << 6) | node;
	msg[3] = (addr >> 40) & 0xFF;
	msg[4] = (addr >> 32) & 0xFF;
	msg[5] = (addr >> 24) & 0xFF;
	msg[6] = (addr >> 16) & 0xFF;
	msg[7] = (addr >> 8) & 0xFF;
	msg[8] = addr & 0xFF;

	msg[9] = tcode;
	msg[10] = tag;
	msg[11] = len >> 8;
	msg[12] = len & 0xFF;

	if (data)
		memcpy(msg + 13, data, len);

	r = sendall(sck, msg, msglen);
	free(msg);
	return r;
}