/*
* Send the given nv structure via conn.
* We keep headers in nv structure and pass data in separate argument.
* There can be no data at all (data is NULL then).
*/
int
hast_proto_send(const struct hast_resource *res, struct proto_conn *conn,
struct nv *nv, const void *data, size_t size)
{
struct hast_main_header hdr;
struct ebuf *eb;
bool freedata;
void *dptr, *hptr;
size_t hsize;
int ret;
dptr = (void *)(uintptr_t)data;
freedata = false;
ret = -1;
if (data != NULL) {
unsigned int ii;
for (ii = 0; ii < sizeof(pipeline) / sizeof(pipeline[0]);
ii++) {
(void)pipeline[ii].hps_send(res, nv, &dptr, &size,
&freedata);
}
nv_add_uint32(nv, size, "size");
if (nv_error(nv) != 0) {
errno = nv_error(nv);
goto end;
}
}
eb = nv_hton(nv);
if (eb == NULL)
goto end;
hdr.version = res != NULL ? res->hr_version : HAST_PROTO_VERSION;
hdr.size = htole32((uint32_t)ebuf_size(eb));
if (ebuf_add_head(eb, &hdr, sizeof(hdr)) == -1)
goto end;
hptr = ebuf_data(eb, &hsize);
if (proto_send(conn, hptr, hsize) == -1)
goto end;
if (data != NULL && proto_send(conn, dptr, size) == -1)
goto end;
ret = 0;
end:
if (freedata)
free(dptr);
return (ret);
}
void
proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
{
/* TODO: command to select a slave. */
#define c(cmd, size) (cmd << 8 | size)
switch (c (cmd, size))
{
case c ('z', 0):
utils_reset ();
break;
case c ('s', 1):
spi_send (args[0]);
break;
case c ('r', 0):
proto_send1b ('R', spi_recv ());
break;
case c ('r', 1):
proto_send1b ('R', spi_send_and_recv (args[0]));
break;
default:
proto_send0 ('?');
return;
}
proto_send (cmd, size, args);
#undef c
}
void
proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
{
/* May be unused. */
uint8_t status;
#define c(cmd, size) (cmd << 8 | size)
switch (c (cmd, size))
{
case c ('z', 0):
/* Reset */
utils_reset ();
break;
default:
if (cmd == 'l')
{
status = flash_log (size, args);
if (!status)
{
/* Error */
proto_send0('?');
return;
}
}
else
{
/* Error */
proto_send0 ('?');
return;
}
}
/* Acknowledge what has been done */
proto_send (cmd, size, args);
}
int proxy_sendmsg(unsigned char msg_id, nethost_t* host,
const objlist_t* list, unsigned long flags)
{
if(net_send(host->s, (char*)&msg_id, 1) != NETOK)
return PROXY_ERROR;
if(proto_send(host, list, flags) != 0)
return PROXY_ERROR;
return PROXY_OK;
}
/** Handle incoming messages. */
void
proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
{
#define c(cmd, size) (cmd << 8 | size)
switch (c (cmd, size))
{
case c ('z', 0):
/* Reset. */
utils_reset ();
break;
case c ('r', 1):
/* Output encoders raw value after every read. */
read_cpt = read = args[0];
read_mode = 0;
break;
case c ('R', 1):
/* Output encoders raw value only if last encoder changed. */
read_cpt = read = args[0];
read_mode = 1;
break;
case c ('Z', 1):
/* Output encoders raw value if any encoder is null. */
read_cpt = read = args[0];
read_mode = 2;
break;
case c ('i', 1):
/* Index checking mode. Require counter_index_test. */
ind_cpt = ind = args[0];
ind_init = 1;
break;
case c ('C', 1):
/* Regular encoder output, use encoder module code. */
count_cpt = count = args[0];
break;
case c ('c', 4):
/* Set correction. */
encoder_corrector_set_correction (&encoder_corrector_right,
v8_to_v32 (args[0], args[1],
args[2], args[3]));
break;
default:
proto_send0 ('?');
return;
}
proto_send (cmd, size, args);
#undef c
}
static int
tls_send(void *ctx, const unsigned char *data, size_t size, int fd)
{
struct tls_ctx *tlsctx = ctx;
PJDLOG_ASSERT(tlsctx != NULL);
PJDLOG_ASSERT(tlsctx->tls_magic == TLS_CTX_MAGIC);
PJDLOG_ASSERT(tlsctx->tls_side == TLS_SIDE_CLIENT ||
tlsctx->tls_side == TLS_SIDE_SERVER_WORK);
PJDLOG_ASSERT(tlsctx->tls_sock != NULL);
PJDLOG_ASSERT(tlsctx->tls_wait_called);
PJDLOG_ASSERT(fd == -1);
if (proto_send(tlsctx->tls_sock, data, size) == -1)
return (errno);
return (0);
}
void
hastd_secondary(struct hast_resource *res, struct nv *nvin)
{
sigset_t mask;
pthread_t td;
pid_t pid;
int error, mode, debuglevel;
/*
* Create communication channel between parent and child.
*/
if (proto_client(NULL, "socketpair://", &res->hr_ctrl) < 0) {
KEEP_ERRNO((void)pidfile_remove(pfh));
pjdlog_exit(EX_OSERR,
"Unable to create control sockets between parent and child");
}
/*
* Create communication channel between child and parent.
*/
if (proto_client(NULL, "socketpair://", &res->hr_event) < 0) {
KEEP_ERRNO((void)pidfile_remove(pfh));
pjdlog_exit(EX_OSERR,
"Unable to create event sockets between child and parent");
}
pid = fork();
if (pid < 0) {
KEEP_ERRNO((void)pidfile_remove(pfh));
pjdlog_exit(EX_OSERR, "Unable to fork");
}
if (pid > 0) {
/* This is parent. */
proto_close(res->hr_remotein);
res->hr_remotein = NULL;
proto_close(res->hr_remoteout);
res->hr_remoteout = NULL;
/* Declare that we are receiver. */
proto_recv(res->hr_event, NULL, 0);
/* Declare that we are sender. */
proto_send(res->hr_ctrl, NULL, 0);
res->hr_workerpid = pid;
return;
}
gres = res;
mode = pjdlog_mode_get();
debuglevel = pjdlog_debug_get();
/* Declare that we are sender. */
proto_send(res->hr_event, NULL, 0);
/* Declare that we are receiver. */
proto_recv(res->hr_ctrl, NULL, 0);
descriptors_cleanup(res);
descriptors_assert(res, mode);
pjdlog_init(mode);
pjdlog_debug_set(debuglevel);
pjdlog_prefix_set("[%s] (%s) ", res->hr_name, role2str(res->hr_role));
setproctitle("%s (%s)", res->hr_name, role2str(res->hr_role));
PJDLOG_VERIFY(sigemptyset(&mask) == 0);
PJDLOG_VERIFY(sigprocmask(SIG_SETMASK, &mask, NULL) == 0);
/* Error in setting timeout is not critical, but why should it fail? */
if (proto_timeout(res->hr_remotein, 2 * HAST_KEEPALIVE) < 0)
pjdlog_errno(LOG_WARNING, "Unable to set connection timeout");
if (proto_timeout(res->hr_remoteout, res->hr_timeout) < 0)
pjdlog_errno(LOG_WARNING, "Unable to set connection timeout");
init_local(res);
init_environment();
if (drop_privs(res) != 0)
exit(EX_CONFIG);
pjdlog_info("Privileges successfully dropped.");
/*
* Create the control thread before sending any event to the parent,
* as we can deadlock when parent sends control request to worker,
* but worker has no control thread started yet, so parent waits.
* In the meantime worker sends an event to the parent, but parent
* is unable to handle the event, because it waits for control
* request response.
*/
error = pthread_create(&td, NULL, ctrl_thread, res);
PJDLOG_ASSERT(error == 0);
init_remote(res, nvin);
event_send(res, EVENT_CONNECT);
error = pthread_create(&td, NULL, recv_thread, res);
PJDLOG_ASSERT(error == 0);
error = pthread_create(&td, NULL, disk_thread, res);
PJDLOG_ASSERT(error == 0);
(void)send_thread(res);
}
static void
init_remote(struct hast_resource *res, struct nv *nvin)
{
uint64_t resuid;
struct nv *nvout;
unsigned char *map;
size_t mapsize;
#ifdef notyet
/* Setup direction. */
if (proto_send(res->hr_remoteout, NULL, 0) == -1)
pjdlog_errno(LOG_WARNING, "Unable to set connection direction");
#endif
map = NULL;
mapsize = 0;
nvout = nv_alloc();
nv_add_int64(nvout, (int64_t)res->hr_datasize, "datasize");
nv_add_int32(nvout, (int32_t)res->hr_extentsize, "extentsize");
resuid = nv_get_uint64(nvin, "resuid");
res->hr_primary_localcnt = nv_get_uint64(nvin, "localcnt");
res->hr_primary_remotecnt = nv_get_uint64(nvin, "remotecnt");
nv_add_uint64(nvout, res->hr_secondary_localcnt, "localcnt");
nv_add_uint64(nvout, res->hr_secondary_remotecnt, "remotecnt");
mapsize = activemap_calc_ondisk_size(res->hr_local_mediasize -
METADATA_SIZE, res->hr_extentsize, res->hr_local_sectorsize);
map = malloc(mapsize);
if (map == NULL) {
pjdlog_exitx(EX_TEMPFAIL,
"Unable to allocate memory (%zu bytes) for activemap.",
mapsize);
}
/*
* When we work as primary and secondary is missing we will increase
* localcnt in our metadata. When secondary is connected and synced
* we make localcnt be equal to remotecnt, which means nodes are more
* or less in sync.
* Split-brain condition is when both nodes are not able to communicate
* and are both configured as primary nodes. In turn, they can both
* make incompatible changes to the data and we have to detect that.
* Under split-brain condition we will increase our localcnt on first
* write and remote node will increase its localcnt on first write.
* When we connect we can see that primary's localcnt is greater than
* our remotecnt (primary was modified while we weren't watching) and
* our localcnt is greater than primary's remotecnt (we were modified
* while primary wasn't watching).
* There are many possible combinations which are all gathered below.
* Don't pay too much attention to exact numbers, the more important
* is to compare them. We compare secondary's local with primary's
* remote and secondary's remote with primary's local.
* Note that every case where primary's localcnt is smaller than
* secondary's remotecnt and where secondary's localcnt is smaller than
* primary's remotecnt should be impossible in practise. We will perform
* full synchronization then. Those cases are marked with an asterisk.
* Regular synchronization means that only extents marked as dirty are
* synchronized (regular synchronization).
*
* SECONDARY METADATA PRIMARY METADATA
* local=3 remote=3 local=2 remote=2* ?! Full sync from secondary.
* local=3 remote=3 local=2 remote=3* ?! Full sync from primary.
* local=3 remote=3 local=2 remote=4* ?! Full sync from primary.
* local=3 remote=3 local=3 remote=2 Primary is out-of-date,
* regular sync from secondary.
* local=3 remote=3 local=3 remote=3 Regular sync just in case.
* local=3 remote=3 local=3 remote=4* ?! Full sync from primary.
* local=3 remote=3 local=4 remote=2 Split-brain condition.
* local=3 remote=3 local=4 remote=3 Secondary out-of-date,
* regular sync from primary.
* local=3 remote=3 local=4 remote=4* ?! Full sync from primary.
*/
if (res->hr_resuid == 0) {
/*
* Provider is used for the first time. If primary node done no
* writes yet as well (we will find "virgin" argument) then
* there is no need to synchronize anything. If primary node
* done any writes already we have to synchronize everything.
*/
PJDLOG_ASSERT(res->hr_secondary_localcnt == 0);
res->hr_resuid = resuid;
if (metadata_write(res) < 0)
exit(EX_NOINPUT);
if (nv_exists(nvin, "virgin")) {
free(map);
map = NULL;
mapsize = 0;
} else {
memset(map, 0xff, mapsize);
}
nv_add_int8(nvout, 1, "virgin");
nv_add_uint8(nvout, HAST_SYNCSRC_PRIMARY, "syncsrc");
} else if (res->hr_resuid != resuid) {
char errmsg[256];
free(map);
(void)snprintf(errmsg, sizeof(errmsg),
"Resource unique ID mismatch (primary=%ju, secondary=%ju).",
(uintmax_t)resuid, (uintmax_t)res->hr_resuid);
pjdlog_error("%s", errmsg);
nv_add_string(nvout, errmsg, "errmsg");
if (hast_proto_send(res, res->hr_remotein, nvout, NULL, 0) < 0) {
pjdlog_exit(EX_TEMPFAIL, "Unable to send response to %s",
res->hr_remoteaddr);
}
nv_free(nvout);
exit(EX_CONFIG);
} else if (
/* Is primary out-of-date? */
(res->hr_secondary_localcnt > res->hr_primary_remotecnt &&
res->hr_secondary_remotecnt == res->hr_primary_localcnt) ||
/* Are the nodes more or less in sync? */
(res->hr_secondary_localcnt == res->hr_primary_remotecnt &&
res->hr_secondary_remotecnt == res->hr_primary_localcnt) ||
/* Is secondary out-of-date? */
(res->hr_secondary_localcnt == res->hr_primary_remotecnt &&
res->hr_secondary_remotecnt < res->hr_primary_localcnt)) {
/*
* Nodes are more or less in sync or one of the nodes is
* out-of-date.
* It doesn't matter at this point which one, we just have to
* send out local bitmap to the remote node.
*/
if (pread(res->hr_localfd, map, mapsize, METADATA_SIZE) !=
(ssize_t)mapsize) {
pjdlog_exit(LOG_ERR, "Unable to read activemap");
}
if (res->hr_secondary_localcnt > res->hr_primary_remotecnt &&
res->hr_secondary_remotecnt == res->hr_primary_localcnt) {
/* Primary is out-of-date, sync from secondary. */
nv_add_uint8(nvout, HAST_SYNCSRC_SECONDARY, "syncsrc");
} else {
/*
* Secondary is out-of-date or counts match.
* Sync from primary.
*/
nv_add_uint8(nvout, HAST_SYNCSRC_PRIMARY, "syncsrc");
}
} else if (res->hr_secondary_localcnt > res->hr_primary_remotecnt &&
res->hr_primary_localcnt > res->hr_secondary_remotecnt) {
/*
* Not good, we have split-brain condition.
*/
free(map);
pjdlog_error("Split-brain detected, exiting.");
nv_add_string(nvout, "Split-brain condition!", "errmsg");
if (hast_proto_send(res, res->hr_remotein, nvout, NULL, 0) < 0) {
pjdlog_exit(EX_TEMPFAIL, "Unable to send response to %s",
res->hr_remoteaddr);
}
nv_free(nvout);
/* Exit on split-brain. */
event_send(res, EVENT_SPLITBRAIN);
exit(EX_CONFIG);
} else /* if (res->hr_secondary_localcnt < res->hr_primary_remotecnt ||
res->hr_primary_localcnt < res->hr_secondary_remotecnt) */ {
/*
* This should never happen in practise, but we will perform
* full synchronization.
*/
PJDLOG_ASSERT(res->hr_secondary_localcnt < res->hr_primary_remotecnt ||
res->hr_primary_localcnt < res->hr_secondary_remotecnt);
mapsize = activemap_calc_ondisk_size(res->hr_local_mediasize -
METADATA_SIZE, res->hr_extentsize,
res->hr_local_sectorsize);
memset(map, 0xff, mapsize);
if (res->hr_secondary_localcnt > res->hr_primary_remotecnt) {
/* In this one of five cases sync from secondary. */
nv_add_uint8(nvout, HAST_SYNCSRC_SECONDARY, "syncsrc");
} else {
/* For the rest four cases sync from primary. */
nv_add_uint8(nvout, HAST_SYNCSRC_PRIMARY, "syncsrc");
}
pjdlog_warning("This should never happen, asking for full synchronization (primary(local=%ju, remote=%ju), secondary(local=%ju, remote=%ju)).",
(uintmax_t)res->hr_primary_localcnt,
(uintmax_t)res->hr_primary_remotecnt,
(uintmax_t)res->hr_secondary_localcnt,
(uintmax_t)res->hr_secondary_remotecnt);
}
nv_add_uint32(nvout, (uint32_t)mapsize, "mapsize");
if (hast_proto_send(res, res->hr_remotein, nvout, map, mapsize) < 0) {
pjdlog_exit(EX_TEMPFAIL, "Unable to send activemap to %s",
res->hr_remoteaddr);
}
if (map != NULL)
free(map);
nv_free(nvout);
#ifdef notyet
/* Setup direction. */
if (proto_recv(res->hr_remotein, NULL, 0) == -1)
pjdlog_errno(LOG_WARNING, "Unable to set connection direction");
#endif
}
static void
tls_call_exec_server(struct proto_conn *sock, struct proto_conn *tcp)
{
int startfd, sockfd, tcpfd, safefd;
char *startfdstr, *debugstr;
if (pjdlog_mode_get() == PJDLOG_MODE_STD)
startfd = 3;
else /* if (pjdlog_mode_get() == PJDLOG_MODE_SYSLOG) */
startfd = 0;
/* Declare that we are receiver. */
proto_send(sock, NULL, 0);
sockfd = proto_descriptor(sock);
tcpfd = proto_descriptor(tcp);
safefd = MAX(sockfd, tcpfd);
safefd = MAX(safefd, startfd);
safefd++;
/* Move sockfd and tcpfd to safe numbers first. */
if (dup2(sockfd, safefd) == -1)
pjdlog_exit(EX_OSERR, "dup2() failed");
proto_close(sock);
sockfd = safefd;
if (dup2(tcpfd, safefd + 1) == -1)
pjdlog_exit(EX_OSERR, "dup2() failed");
proto_close(tcp);
tcpfd = safefd + 1;
/* Move socketpair descriptor to descriptor number startfd. */
if (dup2(sockfd, startfd) == -1)
pjdlog_exit(EX_OSERR, "dup2() failed");
(void)close(sockfd);
/* Move tcp descriptor to descriptor number startfd + 1. */
if (dup2(tcpfd, startfd + 1) == -1)
pjdlog_exit(EX_OSERR, "dup2() failed");
(void)close(tcpfd);
closefrom(startfd + 2);
/*
* Even if FD_CLOEXEC was set on descriptors before dup2(), it should
* have been cleared on dup2(), but better be safe than sorry.
*/
if (fcntl(startfd, F_SETFD, 0) == -1)
pjdlog_exit(EX_OSERR, "fcntl() failed");
if (fcntl(startfd + 1, F_SETFD, 0) == -1)
pjdlog_exit(EX_OSERR, "fcntl() failed");
if (asprintf(&startfdstr, "%d", startfd) == -1)
pjdlog_exit(EX_TEMPFAIL, "asprintf() failed");
if (asprintf(&debugstr, "%d", pjdlog_debug_get()) == -1)
pjdlog_exit(EX_TEMPFAIL, "asprintf() failed");
execl(proto_get("execpath"), proto_get("execpath"), "proto", "tls",
proto_get("user"), "server", startfdstr, proto_get("tls:keyfile"),
proto_get("tls:certfile"), debugstr, NULL);
pjdlog_exit(EX_SOFTWARE, "execl() failed");
}
static int
tls_connect(const char *srcaddr, const char *dstaddr, int timeout, void **ctxp)
{
struct tls_ctx *tlsctx;
struct proto_conn *sock;
pid_t pid;
int error;
PJDLOG_ASSERT(srcaddr == NULL || srcaddr[0] != '\0');
PJDLOG_ASSERT(dstaddr != NULL);
PJDLOG_ASSERT(timeout >= -1);
PJDLOG_ASSERT(ctxp != NULL);
if (strncmp(dstaddr, "tls://", 6) != 0)
return (-1);
if (srcaddr != NULL && strncmp(srcaddr, "tls://", 6) != 0)
return (-1);
if (proto_connect(NULL, "socketpair://", -1, &sock) == -1)
return (errno);
#if 0
/*
* We use rfork() with the following flags to disable SIGCHLD
* delivery upon the sandbox process exit.
*/
pid = rfork(RFFDG | RFPROC | RFTSIGZMB | RFTSIGFLAGS(0));
#else
/*
* We don't use rfork() to be able to log information about sandbox
* process exiting.
*/
pid = fork();
#endif
switch (pid) {
case -1:
/* Failure. */
error = errno;
proto_close(sock);
return (error);
case 0:
/* Child. */
pjdlog_prefix_set("[TLS sandbox] (client) ");
#ifdef HAVE_SETPROCTITLE
setproctitle("[TLS sandbox] (client) ");
#endif
tls_call_exec_client(sock, srcaddr, dstaddr, timeout);
/* NOTREACHED */
default:
/* Parent. */
tlsctx = calloc(1, sizeof(*tlsctx));
if (tlsctx == NULL) {
error = errno;
proto_close(sock);
(void)kill(pid, SIGKILL);
return (error);
}
proto_send(sock, NULL, 0);
tlsctx->tls_sock = sock;
tlsctx->tls_tcp = NULL;
tlsctx->tls_side = TLS_SIDE_CLIENT;
tlsctx->tls_wait_called = false;
tlsctx->tls_magic = TLS_CTX_MAGIC;
if (timeout >= 0) {
error = tls_connect_wait(tlsctx, timeout);
if (error != 0) {
(void)kill(pid, SIGKILL);
tls_close(tlsctx);
return (error);
}
}
*ctxp = tlsctx;
return (0);
}
}
void
proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
{
uint8_t ap, bp, as, bs;
uint16_t i;
int32_t al, bl, rl[4];
uint32_t patl[] = { 0xa66a6aa6, 0x5a5affff, 0xffcdffcd, 0xffffffff };
#define patn (sizeof (patl) / sizeof (patl[0]))
#define c(cmd, size) (cmd << 8 | size)
switch (c (cmd, size))
{
case c ('z', 0):
utils_reset ();
break;
case c ('m', 0):
for (ap = 0; ap < patn; ap++)
for (bp = 0; bp < patn; bp++)
for (as = 0; as < 32; as++)
for (bs = 0; bs < 32; bs++)
{
al = patl[ap] >> as;
bl = patl[bp] >> bs;
proto_send2d ('a', al, bl);
rl[0] = fixed_mul_f824 (al, bl);
check_mul (al, bl, rl[0]);
rl[1] = fixed_mul_f824 (-al, bl);
check_mul (-al, bl, rl[1]);
rl[2] = fixed_mul_f824 (al, -bl);
check_mul (al, -bl, rl[2]);
rl[3] = fixed_mul_f824 (-al, -bl);
check_mul (-al, -bl, rl[3]);
proto_send4d ('r', rl[0], rl[1], rl[2], rl[3]);
}
for (i = 0; i < 64000; i++)
{
al = random_u32 ();
bl = random_u32 ();
proto_send2d ('a', al, bl);
rl[0] = fixed_mul_f824 (al, bl);
check_mul (al, bl, rl[0]);
rl[1] = fixed_mul_f824 (-al, bl);
check_mul (-al, bl, rl[1]);
rl[2] = fixed_mul_f824 (al, -bl);
check_mul (al, -bl, rl[2]);
rl[3] = fixed_mul_f824 (-al, -bl);
check_mul (-al, -bl, rl[3]);
proto_send4d ('r', rl[0], rl[1], rl[2], rl[3]);
}
break;
case c ('d', 0):
for (ap = 0; ap < patn; ap++)
for (bp = 0; bp < patn; bp++)
for (as = 0; as < 32; as++)
for (bs = 0; bs < 31; bs++)
{
al = patl[ap] >> as;
bl = patl[bp] >> bs;
proto_send2d ('a', al, bl);
rl[0] = fixed_div_f824 (al, bl);
check_div (al, bl, rl[0]);
rl[1] = fixed_div_f824 (-al, bl);
check_div (-al, bl, rl[1]);
rl[2] = fixed_div_f824 (al, -bl);
check_div (al, -bl, rl[2]);
rl[3] = fixed_div_f824 (-al, -bl);
check_div (-al, -bl, rl[3]);
proto_send4d ('r', rl[0], rl[1], rl[2], rl[3]);
}
for (i = 0; i < 64000; i++)
{
al = random_u32 ();
bl = random_u32 ();
if (bl != 0)
{
proto_send2d ('a', al, bl);
rl[0] = fixed_div_f824 (al, bl);
check_div (al, bl, rl[0]);
rl[1] = fixed_div_f824 (-al, bl);
check_div (-al, bl, rl[1]);
rl[2] = fixed_div_f824 (al, -bl);
check_div (al, -bl, rl[2]);
rl[3] = fixed_div_f824 (-al, -bl);
check_div (-al, -bl, rl[3]);
proto_send4d ('r', rl[0], rl[1], rl[2], rl[3]);
}
}
break;
case c ('c', 0):
for (al = 0; al < (1L << 24); al += 257)
{
proto_send1d ('a', al);
rl[0] = fixed_cos_f824 (al);
rl[1] = fixed_sin_f824 (al);
check_cos (al, rl[0], rl[1]);
proto_send2d ('r', rl[0], rl[1]);
}
for (i = 0; i < 64000; i++)
{
al = random_u32 () & 0xffffff;
proto_send1d ('a', al);
rl[0] = fixed_cos_f824 (al);
rl[1] = fixed_sin_f824 (al);
check_cos (al, rl[0], rl[1]);
proto_send2d ('r', rl[0], rl[1]);
}
break;
case c ('s', 0):
for (ap = 0; ap < patn; ap++)
for (as = 0; as < 32; as++)
{
al = patl[ap] >> as;
proto_send1d ('a', al);
rl[0] = fixed_sqrt_uf248 (al);
rl[1] = fixed_sqrt_ui32 (al);
check_sqrt (al, rl[0], rl[1]);
proto_send2d ('r', rl[0], rl[1]);
}
for (i = 0; i < 64000; i++)
{
al = random_u32 ();
proto_send1d ('a', al);
rl[0] = fixed_sqrt_uf248 (al);
rl[1] = fixed_sqrt_ui32 (al);
check_sqrt (al, rl[0], rl[1]);
proto_send2d ('r', rl[0], rl[1]);
}
break;
default:
proto_send0 ('?');
return;
}
/* When no error acknoledge. */
proto_send (cmd, size, args);
#undef c
}
static void send_u8(uint8_t v)
{
proto_send(v);
}
/** Handle received commands. */
void
proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
{
#define c(cmd, size) (cmd << 8 | size)
switch (c (cmd, size))
{
case c ('z', 0):
/* Reset */
utils_reset ();
break;
case c ('j', 0):
/* Simulate jack insertion. */
fsm_queue_post_event (FSM_EVENT (jack_inserted));
break;
case c ('w', 3):
/* Set PWM.
* - 1b: index.
* - 1w: value. */
pwm_set (args[0], v8_to_v16 (args[1], args[2]));
break;
case c ('w', 7):
/* Set timed PWM.
* - 1b: index.
* - 1w: value.
* - 1w: time.
* - 1w: rest value. */
pwm_set_timed (args[0], v8_to_v16 (args[1], args[2]),
v8_to_v16 (args[3], args[4]),
v8_to_v16 (args[5], args[6]));
break;
case c ('m', 5):
/* Go to position.
* - 2w: x, y.
* - 1b: backward. */
{
vect_t position = { v8_to_v16 (args[0], args[1]),
v8_to_v16 (args[2], args[3]) };
move_stop ();
move_start_noangle (position, args[4], 0);
}
break;
case c ('c', 1):
/* Move clamp.
* - 1b: position. */
clamp_move (args[0]);
break;
case c ('c', 2):
/* Move element using clamp.
* - 1b: source.
* - 1b: destination. */
clamp_move_element (args[0], args[1]);
break;
case c ('n', 2):
/* Simulate the presence of a new element.
* - 1b: position.
* - 1b: type. */
clamp_new_element (args[0], args[1]);
break;
case c ('d', 0):
/* Open all doors. */
pwm_set_timed (BOT_PWM_DOOR_FRONT_BOTTOM,
BOT_PWM_DOOR_OPEN (CLAMP_SLOT_FRONT_BOTTOM));
pwm_set_timed (BOT_PWM_DOOR_FRONT_TOP,
BOT_PWM_DOOR_OPEN (CLAMP_SLOT_FRONT_TOP));
pwm_set_timed (BOT_PWM_DOOR_BACK_BOTTOM,
BOT_PWM_DOOR_OPEN (CLAMP_SLOT_BACK_BOTTOM));
pwm_set_timed (BOT_PWM_DOOR_BACK_TOP,
BOT_PWM_DOOR_OPEN (CLAMP_SLOT_BACK_TOP));
break;
case c ('d', 1):
/* Drop elements.
* - 1b: 00: drop clear, 01: drop forward, 02: drop backward. */
if (args[0] == 0x00)
clamp_drop_clear ();
else
clamp_drop (args[0]);
break;
case c ('d', 2):
/* Open or close door or clamp.
* - 1b: pos, or 0xff for clamp.
* - 1b: non zero to open. */
clamp_door (args[0], args[1]);
break;
case c ('w', 0):
/* Disable all motor control. */
mimot_motor_free (0, 0);
mimot_motor_free (1, 0);
asserv_free_motor ();
break;
/* Stats commands.
* - b: interval between stats. */
case c ('A', 1):
/* Position stats. */
main_stats_asserv_ = main_stats_asserv_cpt_ = args[0];
break;
case c ('P', 1):
/* Contact stats. */
main_stats_contact_ = main_stats_contact_cpt_ = args[0];
break;
case c ('B', 1):
/* Codebar stats. */
main_stats_codebar_ = main_stats_codebar_cpt_ = args[0];
break;
case c ('U', 1):
/* US sensors stats. */
main_stats_usdist_ = main_stats_usdist_cpt_ = args[0];
break;
default:
/* Unknown commands */
proto_send0 ('?');
return;
}
/* When no error, acknowledge commands */
proto_send (cmd, size, args);
#undef c
}
static int
sender_connect(void)
{
unsigned char rnd[32], hash[32], resp[32];
struct proto_conn *conn;
char welcome[8];
int16_t val;
val = 1;
if (proto_send(adhost->adh_conn, &val, sizeof(val)) < 0) {
pjdlog_exit(EX_TEMPFAIL,
"Unable to send connection request to parent");
}
if (proto_recv(adhost->adh_conn, &val, sizeof(val)) < 0) {
pjdlog_exit(EX_TEMPFAIL,
"Unable to receive reply to connection request from parent");
}
if (val != 0) {
errno = val;
pjdlog_errno(LOG_WARNING, "Unable to connect to %s",
adhost->adh_remoteaddr);
return (-1);
}
if (proto_connection_recv(adhost->adh_conn, true, &conn) < 0) {
pjdlog_exit(EX_TEMPFAIL,
"Unable to receive connection from parent");
}
if (proto_connect_wait(conn, adcfg->adc_timeout) < 0) {
pjdlog_errno(LOG_WARNING, "Unable to connect to %s",
adhost->adh_remoteaddr);
proto_close(conn);
return (-1);
}
pjdlog_debug(1, "Connected to %s.", adhost->adh_remoteaddr);
/* Error in setting timeout is not critical, but why should it fail? */
if (proto_timeout(conn, adcfg->adc_timeout) < 0)
pjdlog_errno(LOG_WARNING, "Unable to set connection timeout");
else
pjdlog_debug(1, "Timeout set to %d.", adcfg->adc_timeout);
/* Exchange welcome message, which includes version number. */
(void)snprintf(welcome, sizeof(welcome), "ADIST%02d", ADIST_VERSION);
if (proto_send(conn, welcome, sizeof(welcome)) < 0) {
pjdlog_errno(LOG_WARNING,
"Unable to send welcome message to %s",
adhost->adh_remoteaddr);
proto_close(conn);
return (-1);
}
pjdlog_debug(1, "Welcome message sent (%s).", welcome);
bzero(welcome, sizeof(welcome));
if (proto_recv(conn, welcome, sizeof(welcome)) < 0) {
pjdlog_errno(LOG_WARNING,
"Unable to receive welcome message from %s",
adhost->adh_remoteaddr);
proto_close(conn);
return (-1);
}
if (strncmp(welcome, "ADIST", 5) != 0 || !isdigit(welcome[5]) ||
!isdigit(welcome[6]) || welcome[7] != '\0') {
pjdlog_warning("Invalid welcome message from %s.",
adhost->adh_remoteaddr);
proto_close(conn);
return (-1);
}
pjdlog_debug(1, "Welcome message received (%s).", welcome);
/*
* Receiver can only reply with version number lower or equal to
* the one we sent.
*/
adhost->adh_version = atoi(welcome + 5);
if (adhost->adh_version > ADIST_VERSION) {
pjdlog_warning("Invalid version number from %s (%d received, up to %d supported).",
adhost->adh_remoteaddr, adhost->adh_version, ADIST_VERSION);
proto_close(conn);
return (-1);
}
pjdlog_debug(1, "Version %d negotiated with %s.", adhost->adh_version,
adhost->adh_remoteaddr);
if (proto_send(conn, adcfg->adc_name, sizeof(adcfg->adc_name)) == -1) {
pjdlog_errno(LOG_WARNING, "Unable to send name to %s",
adhost->adh_remoteaddr);
proto_close(conn);
return (-1);
}
pjdlog_debug(1, "Name (%s) sent.", adcfg->adc_name);
if (proto_recv(conn, rnd, sizeof(rnd)) == -1) {
pjdlog_errno(LOG_WARNING, "Unable to receive challenge from %s",
adhost->adh_remoteaddr);
proto_close(conn);
return (-1);
}
pjdlog_debug(1, "Challenge received.");
if (HMAC(EVP_sha256(), adhost->adh_password,
(int)strlen(adhost->adh_password), rnd, (int)sizeof(rnd), hash,
NULL) == NULL) {
pjdlog_warning("Unable to generate response.");
proto_close(conn);
return (-1);
}
pjdlog_debug(1, "Response generated.");
if (proto_send(conn, hash, sizeof(hash)) == -1) {
pjdlog_errno(LOG_WARNING, "Unable to send response to %s",
adhost->adh_remoteaddr);
proto_close(conn);
return (-1);
}
pjdlog_debug(1, "Response sent.");
if (adist_random(rnd, sizeof(rnd)) == -1) {
pjdlog_warning("Unable to generate challenge.");
proto_close(conn);
return (-1);
}
pjdlog_debug(1, "Challenge generated.");
if (proto_send(conn, rnd, sizeof(rnd)) == -1) {
pjdlog_errno(LOG_WARNING, "Unable to send challenge to %s",
adhost->adh_remoteaddr);
proto_close(conn);
return (-1);
}
pjdlog_debug(1, "Challenge sent.");
if (proto_recv(conn, resp, sizeof(resp)) == -1) {
pjdlog_errno(LOG_WARNING, "Unable to receive response from %s",
adhost->adh_remoteaddr);
proto_close(conn);
return (-1);
}
pjdlog_debug(1, "Response received.");
if (HMAC(EVP_sha256(), adhost->adh_password,
(int)strlen(adhost->adh_password), rnd, (int)sizeof(rnd), hash,
NULL) == NULL) {
pjdlog_warning("Unable to generate hash.");
proto_close(conn);
return (-1);
}
pjdlog_debug(1, "Hash generated.");
if (memcmp(resp, hash, sizeof(hash)) != 0) {
pjdlog_warning("Invalid response from %s (wrong password?).",
adhost->adh_remoteaddr);
proto_close(conn);
return (-1);
}
pjdlog_info("Receiver authenticated.");
if (proto_recv(conn, &adhost->adh_trail_offset,
sizeof(adhost->adh_trail_offset)) == -1) {
pjdlog_errno(LOG_WARNING,
"Unable to receive size of the most recent trail file from %s",
adhost->adh_remoteaddr);
proto_close(conn);
return (-1);
}
adhost->adh_trail_offset = le64toh(adhost->adh_trail_offset);
if (proto_recv(conn, &adhost->adh_trail_name,
sizeof(adhost->adh_trail_name)) == -1) {
pjdlog_errno(LOG_WARNING,
"Unable to receive name of the most recent trail file from %s",
adhost->adh_remoteaddr);
proto_close(conn);
return (-1);
}
pjdlog_debug(1, "Trail name (%s) and offset (%ju) received.",
adhost->adh_trail_name, (uintmax_t)adhost->adh_trail_offset);
rw_wlock(&adist_remote_lock);
mtx_lock(&adist_remote_mtx);
PJDLOG_ASSERT(adhost->adh_remote == NULL);
PJDLOG_ASSERT(conn != NULL);
adhost->adh_remote = conn;
mtx_unlock(&adist_remote_mtx);
rw_unlock(&adist_remote_lock);
cv_signal(&adist_remote_cond);
return (0);
}
