/*
* Encode the relevant fields into a sun disklabel, compute new checksum.
*/
void
sunlabel_enc(void *pp, struct sun_disklabel *sl)
{
uint8_t *p;
size_t i;
u_int u;
p = pp;
for (i = 0; i < SL_TEXT_SIZEOF; i++)
p[SL_TEXT + i] = sl->sl_text[i];
be16enc(p + SL_RPM, sl->sl_rpm);
be16enc(p + SL_PCYLINDERS, sl->sl_pcylinders);
be16enc(p + SL_SPARESPERCYL, sl->sl_sparespercyl);
be16enc(p + SL_INTERLEAVE, sl->sl_interleave);
be16enc(p + SL_NCYLINDERS, sl->sl_ncylinders);
be16enc(p + SL_ACYLINDERS, sl->sl_acylinders);
be16enc(p + SL_NTRACKS, sl->sl_ntracks);
be16enc(p + SL_NSECTORS, sl->sl_nsectors);
for (i = 0; i < SUN_NPART; i++) {
be32enc(p + SL_PART + (i * SDKP_SIZEOF) + SDKP_CYLOFFSET,
sl->sl_part[i].sdkp_cyloffset);
be32enc(p + SL_PART + (i * SDKP_SIZEOF) + SDKP_NSECTORS,
sl->sl_part[i].sdkp_nsectors);
}
be16enc(p + SL_MAGIC, sl->sl_magic);
for (i = u = 0; i < SUN_SIZE; i += 2)
u ^= be16dec(p + i);
be16enc(p + SL_CKSUM, u);
}
void
uuid_enc_be(void *buf, const struct uuid *uuid)
{
uint8_t *p = buf;
int i;
be32enc(p, uuid->time_low);
be16enc(p + 4, uuid->time_mid);
be16enc(p + 6, uuid->time_hi_and_version);
p[8] = uuid->clock_seq_hi_and_reserved;
p[9] = uuid->clock_seq_low;
for (i = 0; i < _UUID_NODE_LEN; i++)
p[10 + i] = uuid->node[i];
}
struct isns_req *
isns_req_create(uint16_t func, uint16_t flags)
{
struct isns_req *req;
struct isns_hdr *hdr;
req = isns_req_alloc();
req->ir_usedlen = sizeof(struct isns_hdr);
hdr = (struct isns_hdr *)req->ir_buf;
be16enc(hdr->ih_version, ISNS_VERSION);
be16enc(hdr->ih_function, func);
be16enc(hdr->ih_flags, flags);
return (req);
}
static const char *tencode(int t, uint64_t val)
{
static char res[64];
uint8_t buf[16];
bool be = t > 0;
int i;
if (t < 0) t = -t;
memset(buf, 0xFC, sizeof(buf));
if (be) {
switch (t) {
case 2: be16enc(buf, val); break;
case 4: be32enc(buf, val); break;
case 8: be64enc(buf, val); break;
}
} else {
switch (t) {
case 2: le16enc(buf, val); break;
case 4: le32enc(buf, val); break;
case 8: le64enc(buf, val); break;
}
}
for (i = t; i < (int)sizeof(buf); i++) {
if (buf[i] != 0xFC)
return "OVER";
}
snprintf(res, sizeof(res), "%02X %02X %02X %02X %02X %02X %02X %02X ",
buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7]);
res[t*3 - 1] = 0;
return res;
}
int
isns_req_send(int s, struct isns_req *req)
{
struct isns_hdr *hdr;
int res;
hdr = (struct isns_hdr *)req->ir_buf;
be16enc(hdr->ih_length, req->ir_usedlen - sizeof(*hdr));
be16enc(hdr->ih_flags, be16dec(hdr->ih_flags) |
ISNS_FLAG_LAST | ISNS_FLAG_FIRST);
be16enc(hdr->ih_transaction, 0);
be16enc(hdr->ih_sequence, 0);
res = write(s, req->ir_buf, req->ir_usedlen);
return ((res < 0) ? -1 : 0);
}
static struct servent *
_servent_getbyport(struct servent_data *sd, struct servent *sp, int port,
const char *proto)
{
if ((sd->flags & (_SV_CDB | _SV_PLAINFILE)) == 0)
return NULL;
if (sd->flags & _SV_CDB) {
uint8_t buf[255 + 4];
size_t protolen;
const uint8_t *data;
const void *data_ptr;
size_t datalen;
port = be16toh(port);
if (proto != NULL && *proto == '\0')
return NULL;
if (proto != NULL)
protolen = strlen(proto);
else
protolen = 0;
if (port < 0 || port > 65536)
return NULL;
buf[0] = 0;
buf[1] = protolen;
be16enc(buf + 2, port);
memcpy(buf + 4, proto, protolen);
if (cdbr_find(sd->cdb, buf, 4 + protolen,
&data_ptr, &datalen))
return NULL;
if (datalen < protolen + 4)
return NULL;
data = data_ptr;
if (be16dec(data) != port)
return NULL;
if (protolen) {
if (data[2] != protolen)
return NULL;
if (memcmp(data + 3, proto, protolen + 1))
return NULL;
}
return _servent_parsedb(sd, sp, data, datalen);
} else {
while (_servent_getline(sd) != -1) {
if (_servent_parseline(sd, sp) == NULL)
continue;
if (sp->s_port != port)
continue;
if (proto == NULL || strcmp(sp->s_proto, proto) == 0)
return sp;
}
return NULL;
}
}
static inline void
udl_cmd_add_2(struct udl_softc *sc, uint16_t val)
{
be16enc(sc->sc_cmd_buf, val);
sc->sc_cmd_buf += 2;
}
static void
vhd_geometry(struct vhd_footer *footer, uint64_t image_size)
{
lba_t imgsz;
long cth;
/* Respect command line options if possible. */
if (nheads > 1 && nheads < 256 &&
nsecs > 1 && nsecs < 256 &&
ncyls < 65536) {
be16enc(&footer->cylinders, ncyls);
footer->heads = nheads;
footer->sectors = nsecs;
return;
}
imgsz = image_size / VHD_SECTOR_SIZE;
if (imgsz > 65536 * 16 * 255)
imgsz = 65536 * 16 * 255;
if (imgsz >= 65535 * 16 * 63) {
be16enc(&footer->cylinders, imgsz / (16 * 255));
footer->heads = 16;
footer->sectors = 255;
return;
}
footer->sectors = 17;
cth = imgsz / 17;
footer->heads = (cth + 1023) / 1024;
if (footer->heads < 4)
footer->heads = 4;
if (cth >= (footer->heads * 1024) || footer->heads > 16) {
footer->heads = 16;
footer->sectors = 31;
cth = imgsz / 31;
}
if (cth >= (footer->heads * 1024)) {
footer->heads = 16;
footer->sectors = 63;
cth = imgsz / 63;
}
be16enc(&footer->cylinders, cth / footer->heads);
}
static void
le_lebuffer_copyfromdesc(struct lance_softc *sc, void *tov, int off, int len)
{
struct le_lebuffer_softc *lesc = (struct le_lebuffer_softc *)sc;
caddr_t to = tov;
for (; len >= 8; len -= 8, off += 8, to += 8)
be64enc(to,
bus_read_8(lesc->sc_bres, off));
for (; len >= 4; len -= 4, off += 4, to += 4)
be32enc(to,
bus_read_4(lesc->sc_bres, off));
for (; len >= 2; len -= 2, off += 2, to += 2)
be16enc(to,
bus_read_2(lesc->sc_bres, off));
if (len == 1)
*to = bus_read_1(lesc->sc_bres, off);
}
static void
vhd_make_footer(struct vhd_footer *footer, uint64_t image_size,
uint32_t disk_type, uint64_t data_offset)
{
mkimg_uuid_t id;
memset(footer, 0, sizeof(*footer));
be64enc(&footer->cookie, VHD_FOOTER_COOKIE);
be32enc(&footer->features, VHD_FEATURES_RESERVED);
be32enc(&footer->version, VHD_VERSION);
be64enc(&footer->data_offset, data_offset);
be32enc(&footer->timestamp, vhd_timestamp());
be32enc(&footer->creator_tool, VHD_CREATOR_TOOL);
be32enc(&footer->creator_version, VHD_CREATOR_VERSION);
be32enc(&footer->creator_os, VHD_CREATOR_OS);
be64enc(&footer->original_size, image_size);
be64enc(&footer->current_size, image_size);
vhd_geometry(image_size, &footer->geometry);
be16enc(&footer->geometry.cylinders, footer->geometry.cylinders);
be32enc(&footer->disk_type, disk_type);
mkimg_uuid(&id);
mkimg_uuid_enc(&footer->id, &id);
be32enc(&footer->checksum, vhd_checksum(footer, sizeof(*footer)));
}
/*
* Encode the relevant fields into a sun disklabel, compute new checksum.
*/
void
sunlabel_enc(void *pp, struct sun_disklabel *sl)
{
uint8_t *p;
size_t i;
u_int u;
p = pp;
for (i = 0; i < SL_TEXT_SIZEOF; i++)
p[SL_TEXT + i] = sl->sl_text[i];
be16enc(p + SL_RPM, sl->sl_rpm);
be16enc(p + SL_PCYLINDERS, sl->sl_pcylinders);
be16enc(p + SL_SPARESPERCYL, sl->sl_sparespercyl);
be16enc(p + SL_INTERLEAVE, sl->sl_interleave);
be16enc(p + SL_NCYLINDERS, sl->sl_ncylinders);
be16enc(p + SL_ACYLINDERS, sl->sl_acylinders);
be16enc(p + SL_NTRACKS, sl->sl_ntracks);
be16enc(p + SL_NSECTORS, sl->sl_nsectors);
for (i = 0; i < SUN_NPART; i++) {
be32enc(p + SL_PART + (i * SDKP_SIZEOF) + SDKP_CYLOFFSET,
sl->sl_part[i].sdkp_cyloffset);
be32enc(p + SL_PART + (i * SDKP_SIZEOF) + SDKP_NSECTORS,
sl->sl_part[i].sdkp_nsectors);
}
be16enc(p + SL_MAGIC, sl->sl_magic);
if (sl->sl_vtoc_sane == SUN_VTOC_SANE
&& sl->sl_vtoc_nparts == SUN_NPART) {
/*
* Write SVR4-compatible VTOC elements.
*/
be32enc(p + SL_VTOC_VERS, sl->sl_vtoc_vers);
be32enc(p + SL_VTOC_SANITY, SUN_VTOC_SANE);
memcpy(p + SL_VTOC_VOLNAME, sl->sl_vtoc_volname,
SUN_VOLNAME_LEN);
be16enc(p + SL_VTOC_NPART, SUN_NPART);
for (i = 0; i < SUN_NPART; i++) {
be16enc(p + SL_VTOC_MAP + (i * SVTOC_SIZEOF)
+ SVTOC_TAG,
sl->sl_vtoc_map[i].svtoc_tag);
be16enc(p + SL_VTOC_MAP + (i * SVTOC_SIZEOF)
+ SVTOC_FLAG,
sl->sl_vtoc_map[i].svtoc_flag);
}
}
for (i = u = 0; i < SUN_SIZE; i += 2)
u ^= be16dec(p + i);
be16enc(p + SL_CKSUM, u);
}
/*
* Process request from the client
*/
static bool
server_process_request(server_t *srv, int fd)
{
struct msghdr msg;
struct iovec iov[2];
struct cmsghdr *cmsg;
ssize_t len;
uint16_t error;
assert(FD_ISSET(fd, &srv->fdset));
assert(srv->fdidx[fd].valid);
assert(!srv->fdidx[fd].server);
iov[0].iov_base = &srv->pdu;
iov[0].iov_len = sizeof(srv->pdu);
iov[1].iov_base = srv->ibuf;
iov[1].iov_len = srv->imtu;
msg.msg_name = NULL;
msg.msg_namelen = 0;
msg.msg_iov = iov;
msg.msg_iovlen = __arraycount(iov);
msg.msg_control = srv->ctlbuf;
msg.msg_controllen = srv->ctllen;
msg.msg_flags = 0;
do {
len = recvmsg(fd, &msg, 0);
} while (len == -1 && errno == EINTR);
if (len == -1) {
log_err("Could not receive SDP request on %s socket. %s (%d)",
srv->fdidx[fd].control ? "control" : "L2CAP",
strerror(errno), errno);
return false;
}
if (len == 0) {
log_info("Client on %s socket has disconnected",
srv->fdidx[fd].control ? "control" : "L2CAP");
return false;
}
if (msg.msg_flags & MSG_TRUNC)
log_info("Truncated message on %s socket",
srv->fdidx[fd].control ? "control" : "L2CAP");
if ((cmsg = CMSG_FIRSTHDR(&msg)) != NULL
&& cmsg->cmsg_level == SOL_SOCKET
&& cmsg->cmsg_type == SCM_CREDS
&& cmsg->cmsg_len >= CMSG_LEN(SOCKCREDSIZE(0)))
srv->fdidx[fd].priv = server_auth_check(srv, CMSG_DATA(cmsg));
srv->pdu.len = be16toh(srv->pdu.len);
if ((uint32_t)len < sizeof(srv->pdu)
|| (uint32_t)len != sizeof(srv->pdu) + srv->pdu.len) {
error = SDP_ERROR_CODE_INVALID_PDU_SIZE;
} else {
switch (srv->pdu.pid) {
case SDP_PDU_SERVICE_SEARCH_REQUEST:
error = service_search_request(srv, fd);
break;
case SDP_PDU_SERVICE_ATTRIBUTE_REQUEST:
error = service_attribute_request(srv, fd);
break;
case SDP_PDU_SERVICE_SEARCH_ATTRIBUTE_REQUEST:
error = service_search_attribute_request(srv, fd);
break;
#ifdef SDP_COMPAT
case SDP_PDU_SERVICE_REGISTER_REQUEST:
error = compat_register_request(srv, fd);
break;
case SDP_PDU_SERVICE_CHANGE_REQUEST:
error = compat_change_request(srv, fd);
break;
#endif
case SDP_PDU_RECORD_INSERT_REQUEST:
error = record_insert_request(srv, fd);
break;
case SDP_PDU_RECORD_UPDATE_REQUEST:
error = record_update_request(srv, fd);
break;
case SDP_PDU_RECORD_REMOVE_REQUEST:
error = record_remove_request(srv, fd);
break;
default:
error = SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX;
break;
}
}
if (error != 0) {
srv->fdidx[fd].offset = 0;
db_unselect(srv, fd);
srv->pdu.pid = SDP_PDU_ERROR_RESPONSE;
srv->pdu.len = sizeof(error);
be16enc(srv->obuf, error);
log_debug("sending ErrorResponse (error=0x%04x)", error);
}
iov[0].iov_base = &srv->pdu;
iov[0].iov_len = sizeof(srv->pdu);
iov[1].iov_base = srv->obuf;
iov[1].iov_len = srv->pdu.len;
srv->pdu.len = htobe16(srv->pdu.len);
msg.msg_name = NULL;
msg.msg_namelen = 0;
msg.msg_iov = iov;
msg.msg_iovlen = __arraycount(iov);
msg.msg_control = NULL;
msg.msg_controllen = 0;
msg.msg_flags = 0;
do {
len = sendmsg(fd, &msg, 0);
} while (len == -1 && errno == EINTR);
if (len == -1) {
log_err("Could not send SDP response on %s socket. %s (%d)",
srv->fdidx[fd].control ? "control" : "L2CAP",
strerror(errno), errno);
return false;
}
return true;
}