This repository has been archived on 2022-09-26. You can view files and clone it, but cannot push or open issues or pull requests.
spyderta/common/msg.c

191 lines
4.3 KiB
C
Raw Normal View History

2022-09-26 13:17:04 -04:00
#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;
}