diff options
Diffstat (limited to 'usr.sbin/ldomd/ds.c')
| -rw-r--r-- | usr.sbin/ldomd/ds.c | 725 |
1 files changed, 725 insertions, 0 deletions
diff --git a/usr.sbin/ldomd/ds.c b/usr.sbin/ldomd/ds.c new file mode 100644 index 00000000000..c287bf66098 --- /dev/null +++ b/usr.sbin/ldomd/ds.c @@ -0,0 +1,725 @@ +/* $OpenBSD: ds.c,v 1.1 2012/10/27 18:34:03 kettenis Exp $ */ + +/* + * Copyright (c) 2012 Mark Kettenis + * + * Permission to use, copy, modify, and distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + */ + +#include <sys/types.h> +#include <sys/poll.h> +#include <sys/queue.h> +#include <err.h> +#include <fcntl.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> + +#include "ds.h" +#include "util.h" + +void ldc_rx_ctrl_vers(struct ldc_conn *, struct ldc_pkt *); +void ldc_rx_ctrl_rtr(struct ldc_conn *, struct ldc_pkt *); +void ldc_rx_ctrl_rts(struct ldc_conn *, struct ldc_pkt *); +void ldc_rx_ctrl_rdx(struct ldc_conn *, struct ldc_pkt *); + +void ldc_send_ack(struct ldc_conn *); +void ldc_send_rtr(struct ldc_conn *); +void ldc_send_rts(struct ldc_conn *); +void ldc_send_rdx(struct ldc_conn *); + +void +ldc_rx_ctrl(struct ldc_conn *lc, struct ldc_pkt *lp) +{ + switch (lp->ctrl) { + case LDC_VERS: + ldc_rx_ctrl_vers(lc, lp); + break; + + case LDC_RTS: + ldc_rx_ctrl_rts(lc, lp); + break; + + case LDC_RTR: + ldc_rx_ctrl_rtr(lc, lp); + break; + + case LDC_RDX: + ldc_rx_ctrl_rdx(lc, lp); + break; + + default: + DPRINTF(("CTRL/0x%02x/0x%02x\n", lp->stype, lp->ctrl)); + ldc_reset(lc); + break; + } +} + +void +ldc_rx_ctrl_vers(struct ldc_conn *lc, struct ldc_pkt *lp) +{ + struct ldc_pkt *lvp = (struct ldc_pkt *)lp; + + switch (lp->stype) { + case LDC_INFO: + if (lc->lc_state == LDC_RCV_VERS) { + DPRINTF(("Spurious CTRL/INFO/VERS: state %d\n", + lc->lc_state)); + return; + } + DPRINTF(("CTRL/INFO/VERS\n")); + if (lvp->major == LDC_VERSION_MAJOR && + lvp->minor == LDC_VERSION_MINOR) + ldc_send_ack(lc); + else + /* XXX do nothing for now. */ + ; + break; + + case LDC_ACK: + if (lc->lc_state != LDC_SND_VERS) { + DPRINTF(("Spurious CTRL/ACK/VERS: state %d\n", + lc->lc_state)); + ldc_reset(lc); + return; + } + DPRINTF(("CTRL/ACK/VERS\n")); + ldc_send_rts(lc); + break; + + case LDC_NACK: + DPRINTF(("CTRL/NACK/VERS\n")); + ldc_reset(lc); + break; + + default: + DPRINTF(("CTRL/0x%02x/VERS\n", lp->stype)); + ldc_reset(lc); + break; + } +} + +void +ldc_rx_ctrl_rts(struct ldc_conn *lc, struct ldc_pkt *lp) +{ + switch (lp->stype) { + case LDC_INFO: + if (lc->lc_state != LDC_RCV_VERS) { + DPRINTF(("Spurious CTRL/INFO/RTS: state %d\n", + lc->lc_state)); + ldc_reset(lc); + return; + } + DPRINTF(("CTRL/INFO/RTS\n")); + if (lp->env != LDC_MODE_RELIABLE) { + ldc_reset(lc); + return; + } + ldc_send_rtr(lc); + break; + + case LDC_ACK: + DPRINTF(("CTRL/ACK/RTS\n")); + ldc_reset(lc); + break; + + case LDC_NACK: + DPRINTF(("CTRL/NACK/RTS\n")); + ldc_reset(lc); + break; + + default: + DPRINTF(("CTRL/0x%02x/RTS\n", lp->stype)); + ldc_reset(lc); + break; + } +} + +void +ldc_rx_ctrl_rtr(struct ldc_conn *lc, struct ldc_pkt *lp) +{ + switch (lp->stype) { + case LDC_INFO: + if (lc->lc_state != LDC_SND_RTS) { + DPRINTF(("Spurious CTRL/INFO/RTR: state %d\n", + lc->lc_state)); + ldc_reset(lc); + return; + } + DPRINTF(("CTRL/INFO/RTR\n")); + if (lp->env != LDC_MODE_RELIABLE) { + ldc_reset(lc); + return; + } + ldc_send_rdx(lc); +#if 0 + lc->lc_start(lc); +#endif + break; + + case LDC_ACK: + DPRINTF(("CTRL/ACK/RTR\n")); + ldc_reset(lc); + break; + + case LDC_NACK: + DPRINTF(("CTRL/NACK/RTR\n")); + ldc_reset(lc); + break; + + default: + DPRINTF(("CTRL/0x%02x/RTR\n", lp->stype)); + ldc_reset(lc); + break; + } +} + +void +ldc_rx_ctrl_rdx(struct ldc_conn *lc, struct ldc_pkt *lp) +{ + switch (lp->stype) { + case LDC_INFO: + if (lc->lc_state != LDC_SND_RTR) { + DPRINTF(("Spurious CTRL/INFO/RTR: state %d\n", + lc->lc_state)); + ldc_reset(lc); + return; + } + DPRINTF(("CTRL/INFO/RDX\n")); +#if 0 + lc->lc_start(lc); +#endif + break; + + case LDC_ACK: + DPRINTF(("CTRL/ACK/RDX\n")); + ldc_reset(lc); + break; + + case LDC_NACK: + DPRINTF(("CTRL/NACK/RDX\n")); + ldc_reset(lc); + break; + + default: + DPRINTF(("CTRL/0x%02x/RDX\n", lp->stype)); + ldc_reset(lc); + break; + } +} + +void +ldc_rx_data(struct ldc_conn *lc, struct ldc_pkt *lp) +{ + size_t len; + + if (lp->stype != LDC_INFO && lp->stype != LDC_ACK) { + DPRINTF(("DATA/0x%02x\n", lp->stype)); + ldc_reset(lc); + return; + } + + if (lc->lc_state != LDC_SND_RTR && + lc->lc_state != LDC_SND_RDX) { + DPRINTF(("Spurious DATA/INFO: state %d\n", lc->lc_state)); + ldc_reset(lc); + return; + } + +#if 0 + if (lp->ackid) { + int i; + + for (i = 0; ds_service[i].ds_svc_id; i++) { + if (ds_service[i].ds_ackid && + lp->ackid >= ds_service[i].ds_ackid) { + ds_service[i].ds_ackid = 0; + ds_service[i].ds_start(lc, ds_service[i].ds_svc_handle); + } + } + } +#endif + if (lp->stype == LDC_ACK) + return; + + if (lp->env & LDC_FRAG_START) { + lc->lc_len = (lp->env & LDC_LEN_MASK); + memcpy((uint8_t *)lc->lc_msg, &lp->data, lc->lc_len); + } else { + len = (lp->env & LDC_LEN_MASK); + if (lc->lc_len + len > sizeof(lc->lc_msg)) { + DPRINTF(("Buffer overrun\n")); + ldc_reset(lc); + return; + } + memcpy((uint8_t *)lc->lc_msg + lc->lc_len, &lp->data, len); + lc->lc_len += len; + } + + if (lp->env & LDC_FRAG_STOP) { + ldc_ack(lc, lp->seqid); + lc->lc_rx_data(lc, lc->lc_msg, lc->lc_len); + } +} + +void +ldc_send_vers(struct ldc_conn *lc) +{ + struct ldc_pkt lp; + ssize_t nbytes; + + bzero(&lp, sizeof(lp)); + lp.type = LDC_CTRL; + lp.stype = LDC_INFO; + lp.ctrl = LDC_VERS; + lp.major = 1; + lp.minor = 0; + + nbytes = write(lc->lc_fd, &lp, sizeof(lp)); + if (nbytes != sizeof(lp)) + err(1, "write"); + + lc->lc_state = LDC_SND_VERS; +} + +void +ldc_send_ack(struct ldc_conn *lc) +{ + struct ldc_pkt lp; + ssize_t nbytes; + + bzero(&lp, sizeof(lp)); + lp.type = LDC_CTRL; + lp.stype = LDC_ACK; + lp.ctrl = LDC_VERS; + lp.major = 1; + lp.minor = 0; + + nbytes = write(lc->lc_fd, &lp, sizeof(lp)); + if (nbytes != sizeof(lp)) + err(1, "write"); + + lc->lc_state = LDC_RCV_VERS; +} + +void +ldc_send_rts(struct ldc_conn *lc) +{ + struct ldc_pkt lp; + ssize_t nbytes; + + bzero(&lp, sizeof(lp)); + lp.type = LDC_CTRL; + lp.stype = LDC_INFO; + lp.ctrl = LDC_RTS; + lp.env = LDC_MODE_RELIABLE; + lp.seqid = lc->lc_tx_seqid++; + + nbytes = write(lc->lc_fd, &lp, sizeof(lp)); + if (nbytes != sizeof(lp)) + err(1, "write"); + + lc->lc_state = LDC_SND_RTS; +} + +void +ldc_send_rtr(struct ldc_conn *lc) +{ + struct ldc_pkt lp; + ssize_t nbytes; + + bzero(&lp, sizeof(lp)); + lp.type = LDC_CTRL; + lp.stype = LDC_INFO; + lp.ctrl = LDC_RTR; + lp.env = LDC_MODE_RELIABLE; + lp.seqid = lc->lc_tx_seqid++; + + nbytes = write(lc->lc_fd, &lp, sizeof(lp)); + if (nbytes != sizeof(lp)) + err(1, "write"); + + lc->lc_state = LDC_SND_RTR; +} + +void +ldc_send_rdx(struct ldc_conn *lc) +{ + struct ldc_pkt lp; + ssize_t nbytes; + + bzero(&lp, sizeof(lp)); + lp.type = LDC_CTRL; + lp.stype = LDC_INFO; + lp.ctrl = LDC_RDX; + lp.env = LDC_MODE_RELIABLE; + lp.seqid = lc->lc_tx_seqid++; + + nbytes = write(lc->lc_fd, &lp, sizeof(lp)); + if (nbytes != sizeof(lp)) + err(1, "write"); + + lc->lc_state = LDC_SND_RDX; +} + +void +ldc_reset(struct ldc_conn *lc) +{ + lc->lc_tx_seqid = 0; + lc->lc_state = 0; +#if 0 + lc->lc_reset(lc); +#endif +} + +void +ldc_ack(struct ldc_conn *lc, uint32_t ackid) +{ + struct ldc_pkt lp; + ssize_t nbytes; + + bzero(&lp, sizeof(lp)); + lp.type = LDC_DATA; + lp.stype = LDC_ACK; + lp.seqid = lc->lc_tx_seqid++; + lp.ackid = ackid; + nbytes = write(lc->lc_fd, &lp, sizeof(lp)); + if (nbytes != sizeof(lp)) + err(1, "write"); +} + +void +ds_rx_msg(struct ldc_conn *lc, void *data, size_t len) +{ + struct ds_conn *dc = lc->lc_cookie; + struct ds_msg *dm = data; + + switch(dm->msg_type) { + case DS_INIT_REQ: + { + struct ds_init_req *dr = data; + + DPRINTF(("DS_INIT_REQ %d.%d\n", dr->major_vers, + dr->minor_vers)); + if (dr->major_vers != 1 || dr->minor_vers != 0){ + ldc_reset(lc); + return; + } + ds_init_ack(lc); + break; + } + + case DS_REG_REQ: + { + struct ds_reg_req *dr = data; + struct ds_conn *dc = lc->lc_cookie; + struct ds_conn_svc *dcs; + + DPRINTF(("DS_REG_REQ %s %d.%d 0x%016llx\n", dr->svc_id, + dr->major_vers, dr->minor_vers, dr->svc_handle)); + TAILQ_FOREACH(dcs, &dc->services, link) { + if (strcmp(dr->svc_id, dcs->service->ds_svc_id) == 0) { + dcs->svc_handle = dr->svc_handle; + dcs->ackid = lc->lc_tx_seqid; + ds_reg_ack(lc, dr->svc_handle); + return; + } + } + + ds_reg_nack(lc, dr->svc_handle); + break; + } + + case DS_UNREG: + { + struct ds_unreg *du = data; + + DPRINTF(("DS_UNREG 0x%016llx\n", du->svc_handle)); + ds_unreg_ack(lc, du->svc_handle); + break; + } + + case DS_DATA: + { + struct ds_data *dd = data; + struct ds_conn *dc = lc->lc_cookie; + struct ds_conn_svc *dcs; + + DPRINTF(("DS_DATA 0x%016llx\n", dd->svc_handle)); + TAILQ_FOREACH(dcs, &dc->services, link) { + if (dcs->svc_handle == dd->svc_handle) + dcs->service->ds_rx_data(lc, dd->svc_handle, + data, len); + } + break; + } + + default: + DPRINTF(("Unknown DS message type 0x%x\n", dm->msg_type)); + ldc_reset(lc); + break; + } +} + +void +ds_init_ack(struct ldc_conn *lc) +{ + struct ds_init_ack da; + + DPRINTF((" DS_INIT_ACK\n")); + bzero(&da, sizeof(da)); + da.msg_type = DS_INIT_ACK; + da.payload_len = sizeof(da) - 8; + da.minor_vers = 0; + ds_send_msg(lc, &da, sizeof(da)); +} + +void +ds_reg_ack(struct ldc_conn *lc, uint64_t svc_handle) +{ + struct ds_reg_ack da; + + DPRINTF((" DS_REG_ACK 0x%016llx\n", svc_handle)); + bzero(&da, sizeof(da)); + da.msg_type = DS_REG_ACK; + da.payload_len = sizeof(da) - 8; + da.svc_handle = svc_handle; + da.minor_vers = 0; + ds_send_msg(lc, &da, sizeof(da)); +} + +void +ds_reg_nack(struct ldc_conn *lc, uint64_t svc_handle) +{ + struct ds_reg_nack dn; + + DPRINTF((" DS_REG_NACK 0x%016llx\n", svc_handle)); + bzero(&dn, sizeof(dn)); + dn.msg_type = DS_REG_NACK; + dn.payload_len = sizeof(dn) - 8; + dn.svc_handle = svc_handle; + dn.result = DS_REG_VER_NACK; + dn.major_vers = 0; + ds_send_msg(lc, &dn, sizeof(dn)); +} + +void +ds_unreg_ack(struct ldc_conn *lc, uint64_t svc_handle) +{ + struct ds_unreg du; + + DPRINTF((" DS_UNREG_ACK 0x%016llx\n", svc_handle)); + bzero(&du, sizeof(du)); + du.msg_type = DS_UNREG_ACK; + du.payload_len = sizeof(du) - 8; + du.svc_handle = svc_handle; + ds_send_msg(lc, &du, sizeof(du)); +} + +void +ds_unreg_nack(struct ldc_conn *lc, uint64_t svc_handle) +{ + struct ds_unreg du; + + DPRINTF((" DS_UNREG_NACK 0x%016llx\n", svc_handle)); + bzero(&du, sizeof(du)); + du.msg_type = DS_UNREG_NACK; + du.payload_len = sizeof(du) - 8; + du.svc_handle = svc_handle; + ds_send_msg(lc, &du, sizeof(du)); +} + +void +ds_receive_msg(struct ldc_conn *lc, void *buf, size_t len) +{ + int env = LDC_FRAG_START; + struct ldc_pkt lp; + uint8_t *p = buf; + ssize_t nbytes; + + while (len > 0) { + nbytes = read(lc->lc_fd, &lp, sizeof(lp)); + if (nbytes != sizeof(lp)) + err(1, "read"); + + if (lp.type != LDC_DATA && + lp.stype != LDC_INFO) { + ldc_reset(lc); + return; + } + + if ((lp.env & LDC_FRAG_START) != env) { + ldc_reset(lc); + return; + } + + bcopy(&lp.data, p, (lp.env & LDC_LEN_MASK)); + p += (lp.env & LDC_LEN_MASK); + len -= (lp.env & LDC_LEN_MASK); + + if (lp.env & LDC_FRAG_STOP) + ldc_ack(lc, lp.seqid); + + env = (lp.env & LDC_FRAG_STOP) ? LDC_FRAG_START : 0; + } +} + +void +ldc_send_msg(struct ldc_conn *lc, void *buf, size_t len) +{ + struct ldc_pkt lp; + uint8_t *p = buf; + ssize_t nbytes; + + while (len > 0) { + bzero(&lp, sizeof(lp)); + lp.type = LDC_DATA; + lp.stype = LDC_INFO; + lp.env = min(len, LDC_PKT_PAYLOAD); + if (p == buf) + lp.env |= LDC_FRAG_START; + if (len <= LDC_PKT_PAYLOAD) + lp.env |= LDC_FRAG_STOP; + lp.seqid = lc->lc_tx_seqid++; + bcopy(p, &lp.data, min(len, LDC_PKT_PAYLOAD)); + + nbytes = write(lc->lc_fd, &lp, sizeof(lp)); + if (nbytes != sizeof(lp)) + err(1, "write"); + p += min(len, LDC_PKT_PAYLOAD); + len -= min(len, LDC_PKT_PAYLOAD); + } +} + +void +ds_send_msg(struct ldc_conn *lc, void *buf, size_t len) +{ + uint8_t *p = buf; +#if 0 + struct ldc_pkt lp; + ssize_t nbytes; +#endif + + while (len > 0) { + ldc_send_msg(lc, p, min(len, LDC_MSG_MAX)); + p += min(len, LDC_MSG_MAX); + len -= min(len, LDC_MSG_MAX); + +#if 0 + /* Consume ACK. */ + nbytes = read(lc->lc_fd, &lp, sizeof(lp)); + if (nbytes != sizeof(lp)) + err(1, "read"); + + { + uint64_t *msg = (uint64_t *)&lp; + int i; + + for (i = 0; i < 8; i++) + printf("%02x: %016llx\n", i, msg[i]); + } +#endif + } +} + +TAILQ_HEAD(ds_conn_head, ds_conn) ds_conns = + TAILQ_HEAD_INITIALIZER(ds_conns); +int num_ds_conns; + +struct ds_conn * +ds_conn_open(const char *path, void *cookie) +{ + struct ds_conn *dc; + + dc = xmalloc(sizeof(*dc)); + dc->path = xstrdup(path); + dc->cookie = cookie; + + dc->fd = open(path, O_RDWR, 0); + if (dc->fd == -1) + err(1, "open"); + + memset(&dc->lc, 0, sizeof(dc->lc)); + dc->lc.lc_fd = dc->fd; + dc->lc.lc_cookie = dc; + dc->lc.lc_rx_data = ds_rx_msg; + + TAILQ_INIT(&dc->services); + TAILQ_INSERT_TAIL(&ds_conns, dc, link); + dc->id = num_ds_conns++; + return dc; +} + +void +ds_conn_register_service(struct ds_conn *dc, struct ds_service *ds) +{ + struct ds_conn_svc *dcs; + + dcs = xzalloc(sizeof(*dcs)); + dcs->service = ds; + + TAILQ_INSERT_TAIL(&dc->services, dcs, link); +} + +void +ds_conn_handle(struct ds_conn *dc) +{ + struct ldc_pkt lp; + ssize_t nbytes; + + nbytes = read(dc->fd, &lp, sizeof(lp)); + if (nbytes != sizeof(lp)) { + ldc_reset(&dc->lc); + return; + } + + switch (lp.type) { + case LDC_CTRL: + ldc_rx_ctrl(&dc->lc, &lp); + break; + case LDC_DATA: + ldc_rx_data(&dc->lc, &lp); + break; + default: + DPRINTF(("0x%02x/0x%02x/0x%02x\n", lp.type, lp.stype, + lp.ctrl)); + ldc_reset(&dc->lc); + break; + } +} + +void +ds_conn_serve(void) +{ + struct ds_conn *dc; + struct pollfd *pfd; + int nfds;; + + pfd = xmalloc(num_ds_conns * sizeof(*pfd)); + TAILQ_FOREACH(dc, &ds_conns, link) { + pfd[dc->id].fd = dc->fd; + pfd[dc->id].events = POLLIN; + } + + while (1) { + nfds = poll(pfd, num_ds_conns, -1); + if (nfds == -1 || nfds == 0) + errx(1, "poll"); + + TAILQ_FOREACH(dc, &ds_conns, link) { + if (pfd[dc->id].revents) + ds_conn_handle(dc); + } + } +} |