/***************************************************************************
Function: sWriteTxPrioByte
Purpose: Write a byte of priority transmit data to a channel
Call: sWriteTxPrioByte(ChP,Data)
CHANNEL_T *ChP; Ptr to channel structure
Byte_t Data; The transmit data byte
Return: int: 1 if the bytes is successfully written, otherwise 0.
Comments: The priority byte is transmitted before any data in the Tx FIFO.
Warnings: No context switches are allowed while executing this function.
*/
int sWriteTxPrioByte(CHANNEL_T *ChP, Byte_t Data)
{
Byte_t DWBuf[4]; /* buffer for double word writes */
if(sGetTxCnt(ChP) > 1) /* write it to Tx priority buffer */
{
rp_writech2(ChP,_INDX_ADDR,ChP->TxPrioCnt); /* get priority buffer status */
if(rp_readch1(ChP,_INDX_DATA) & PRI_PEND) /* priority buffer busy */
return(0); /* nothing sent */
le16enc(DWBuf,ChP->TxPrioBuf); /* data byte address */
DWBuf[2] = Data; /* data byte value */
DWBuf[3] = 0; /* priority buffer pointer */
rp_writech4(ChP,_INDX_ADDR,le32dec(DWBuf)); /* write it out */
le16enc(DWBuf,ChP->TxPrioCnt); /* Tx priority count address */
DWBuf[2] = PRI_PEND + 1; /* indicate 1 byte pending */
DWBuf[3] = 0; /* priority buffer pointer */
rp_writech4(ChP,_INDX_ADDR,le32dec(DWBuf)); /* write it out */
}
else /* write it to Tx FIFO */
{
sWriteTxByte(ChP,sGetTxRxDataIO(ChP),Data);
}
return(1); /* 1 byte sent */
}
size_t
_ti_store_extra(TIC *tic, int wrn, char *id, char type, char flag, short num,
char *str, size_t strl, int flags)
{
size_t l;
_DIAGASSERT(tic != NULL);
if (strcmp(id, "use") != 0) {
if (_ti_find_extra(&tic->extras, id) != NULL)
return 0;
if (!(flags & TIC_EXTRA)) {
if (wrn != 0)
dowarn(flags, "%s: %s: unknown capability",
tic->name, id);
return 0;
}
}
l = strlen(id) + 1;
if (l > UINT16_T_MAX) {
dowarn(flags, "%s: %s: cap name is too long", tic->name, id);
return 0;
}
if (!_ti_grow_tbuf(&tic->extras,
l + strl + (sizeof(uint16_t) * 2) + 1))
return 0;
le16enc(tic->extras.buf + tic->extras.bufpos, l);
tic->extras.bufpos += sizeof(uint16_t);
memcpy(tic->extras.buf + tic->extras.bufpos, id, l);
tic->extras.bufpos += l;
tic->extras.buf[tic->extras.bufpos++] = type;
switch (type) {
case 'f':
tic->extras.buf[tic->extras.bufpos++] = flag;
break;
case 'n':
le16enc(tic->extras.buf + tic->extras.bufpos, num);
tic->extras.bufpos += sizeof(uint16_t);
break;
case 's':
le16enc(tic->extras.buf + tic->extras.bufpos, strl);
tic->extras.bufpos += sizeof(uint16_t);
memcpy(tic->extras.buf + tic->extras.bufpos, str, strl);
tic->extras.bufpos += strl;
break;
}
tic->extras.entries++;
return 1;
}
void
uuid_enc_le(void *buf, const struct uuid *uuid)
{
uint8_t *p = buf;
int i;
le32enc(p, uuid->time_low);
le16enc(p + 4, uuid->time_mid);
le16enc(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];
}
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;
}
void
bsd_partition_le_enc(u_char *ptr, struct partition *d)
{
le32enc(ptr + 0, d->p_size);
le32enc(ptr + 4, d->p_offset);
le32enc(ptr + 8, d->p_fsize);
ptr[12] = d->p_fstype;
ptr[13] = d->p_frag;
le16enc(ptr + 14, d->p_cpg);
}
static void
le_lebuffer_copyfrombuf(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 >= 2; len -= 2, off += 2, to += 2)
le16enc(to,
bus_read_2(lesc->sc_bres, off));
if (len == 1)
*to = bus_read_1(lesc->sc_bres, off + 1);
}
void
pc98_partition_enc(void *pp, struct pc98_partition *d)
{
unsigned char *ptr = pp;
int i;
ptr[0] = d->dp_mid;
ptr[1] = d->dp_sid;
ptr[2] = d->dp_dum1;
ptr[3] = d->dp_dum2;
ptr[4] = d->dp_ipl_sct;
ptr[5] = d->dp_ipl_head;
le16enc(ptr + 6, d->dp_ipl_cyl);
ptr[8] = d->dp_ssect;
ptr[9] = d->dp_shd;
le16enc(ptr + 10, d->dp_scyl);
ptr[12] = d->dp_esect;
ptr[13] = d->dp_ehd;
le16enc(ptr + 14, d->dp_ecyl);
for (i = 0; i < sizeof (d->dp_name); i++)
ptr[16 + i] = d->dp_name[i];
}
void
smb_rq_bend(struct smb_rq *rqp)
{
int bcnt;
if (rqp->sr_bcount == NULL) {
SMBERROR("no bcount\n"); /* actually panic */
return;
}
bcnt = rqp->sr_rq.mb_count;
if (bcnt > 0xffff)
SMBERROR("byte count too large (%d)\n", bcnt);
le16enc(rqp->sr_bcount, bcnt);
}
static void
x86bios_emu_wrw(struct x86emu *emu, uint32_t addr, uint16_t val)
{
uint16_t *va;
va = x86bios_get_pages(addr, sizeof(*va));
if (va == NULL)
x86bios_set_fault(emu, addr);
#ifndef __NO_STRICT_ALIGNMENT
if ((addr & 1) != 0)
le16enc(va, val);
else
#endif
*va = htole16(val);
}
static int
kue_send(struct kue_softc *sc, struct mbuf *m, int idx)
{
int total_len;
struct kue_chain *c;
usbd_status err;
DPRINTFN(10,("%s: %s: enter\n", device_xname(sc->kue_dev),__func__));
c = &sc->kue_cdata.kue_tx_chain[idx];
/* Frame length is specified in the first 2 bytes of the buffer. */
le16enc(c->kue_buf, (uint16_t)m->m_pkthdr.len);
/*
* Copy the mbuf data into a contiguous buffer, leaving two
* bytes at the beginning to hold the frame length.
*/
m_copydata(m, 0, m->m_pkthdr.len, c->kue_buf + 2);
total_len = 2 + m->m_pkthdr.len;
total_len = roundup2(total_len, 64);
usbd_setup_xfer(c->kue_xfer, sc->kue_ep[KUE_ENDPT_TX],
c, c->kue_buf, total_len, USBD_NO_COPY, USBD_DEFAULT_TIMEOUT,
kue_txeof);
/* Transmit */
err = usbd_transfer(c->kue_xfer);
if (err != USBD_IN_PROGRESS) {
printf("%s: kue_send error=%s\n", device_xname(sc->kue_dev),
usbd_errstr(err));
kue_stop(sc);
return (EIO);
}
sc->kue_cdata.kue_tx_cnt++;
return (0);
}
TIC *
_ti_compile(char *cap, int flags)
{
char *token, *p, *e, *name, *desc, *alias;
signed char flag;
long cnum;
short num;
ssize_t ind;
size_t len;
TBUF buf;
TIC *tic;
_DIAGASSERT(cap != NULL);
name = _ti_get_token(&cap, ',');
if (name == NULL) {
dowarn(flags, "no seperator found: %s", cap);
return NULL;
}
desc = strrchr(name, '|');
if (desc != NULL)
*desc++ = '\0';
alias = strchr(name, '|');
if (alias != NULL)
*alias++ = '\0';
tic = calloc(sizeof(*tic), 1);
if (tic == NULL)
return NULL;
buf.buf = NULL;
buf.buflen = 0;
tic->name = strdup(name);
if (tic->name == NULL)
goto error;
if (alias != NULL && flags & TIC_ALIAS) {
tic->alias = strdup(alias);
if (tic->alias == NULL)
goto error;
}
if (desc != NULL && flags & TIC_DESCRIPTION) {
tic->desc = strdup(desc);
if (tic->desc == NULL)
goto error;
}
for (token = _ti_get_token(&cap, ',');
token != NULL && *token != '\0';
token = _ti_get_token(&cap, ','))
{
/* Skip commented caps */
if (!(flags & TIC_COMMENT) && token[0] == '.')
continue;
/* Obsolete entries */
if (token[0] == 'O' && token[1] == 'T') {
if (!(flags & TIC_EXTRA))
continue;
token += 2;
}
/* str cap */
p = strchr(token, '=');
if (p != NULL) {
*p++ = '\0';
/* Don't use the string if we already have it */
ind = _ti_strindex(token);
if (ind != -1 &&
_ti_find_cap(&tic->strs, 's', ind) != NULL)
continue;
/* Encode the string to our scratch buffer */
buf.bufpos = 0;
if (encode_string(tic->name, token,
&buf, p, flags) == -1)
goto error;
if (buf.bufpos > UINT16_T_MAX) {
dowarn(flags, "%s: %s: string is too long",
tic->name, token);
continue;
}
if (!VALID_STRING(buf.buf)) {
dowarn(flags, "%s: %s: invalid string",
tic->name, token);
continue;
}
if (ind == -1)
_ti_store_extra(tic, 1, token, 's', -1, -2,
buf.buf, buf.bufpos, flags);
else {
if (!_ti_grow_tbuf(&tic->strs,
(sizeof(uint16_t) * 2) + buf.bufpos))
goto error;
le16enc(tic->strs.buf + tic->strs.bufpos, ind);
tic->strs.bufpos += sizeof(uint16_t);
le16enc(tic->strs.buf + tic->strs.bufpos,
buf.bufpos);
tic->strs.bufpos += sizeof(uint16_t);
memcpy(tic->strs.buf + tic->strs.bufpos,
buf.buf, buf.bufpos);
tic->strs.bufpos += buf.bufpos;
tic->strs.entries++;
}
continue;
}
/* num cap */
p = strchr(token, '#');
if (p != NULL) {
*p++ = '\0';
/* Don't use the number if we already have it */
ind = _ti_numindex(token);
if (ind != -1 &&
_ti_find_cap(&tic->nums, 'n', ind) != NULL)
continue;
cnum = strtol(p, &e, 0);
if (*e != '\0') {
dowarn(flags, "%s: %s: not a number",
tic->name, token);
continue;
}
if (!VALID_NUMERIC(cnum)) {
dowarn(flags, "%s: %s: number out of range",
tic->name, token);
continue;
}
num = (short)cnum;
if (ind == -1)
_ti_store_extra(tic, 1, token, 'n', -1,
num, NULL, 0, flags);
else {
if (_ti_grow_tbuf(&tic->nums,
sizeof(uint16_t) * 2) == NULL)
goto error;
le16enc(tic->nums.buf + tic->nums.bufpos, ind);
tic->nums.bufpos += sizeof(uint16_t);
le16enc(tic->nums.buf + tic->nums.bufpos, num);
tic->nums.bufpos += sizeof(uint16_t);
tic->nums.entries++;
}
continue;
}
flag = 1;
len = strlen(token) - 1;
if (token[len] == '@') {
flag = CANCELLED_BOOLEAN;
token[len] = '\0';
}
ind = _ti_flagindex(token);
if (ind == -1 && flag == CANCELLED_BOOLEAN) {
if ((ind = _ti_numindex(token)) != -1) {
if (_ti_find_cap(&tic->nums, 'n', ind) != NULL)
continue;
if (_ti_grow_tbuf(&tic->nums,
sizeof(uint16_t) * 2) == NULL)
goto error;
le16enc(tic->nums.buf + tic->nums.bufpos, ind);
tic->nums.bufpos += sizeof(uint16_t);
le16enc(tic->nums.buf + tic->nums.bufpos,
(uint16_t)CANCELLED_NUMERIC);
tic->nums.bufpos += sizeof(uint16_t);
tic->nums.entries++;
continue;
} else if ((ind = _ti_strindex(token)) != -1) {
if (_ti_find_cap(&tic->strs, 's', ind) != NULL)
continue;
if (_ti_grow_tbuf(&tic->strs,
(sizeof(uint16_t) * 2) + 1) == NULL)
goto error;
le16enc(tic->strs.buf + tic->strs.bufpos, ind);
tic->strs.bufpos += sizeof(uint16_t);
le16enc(tic->strs.buf + tic->strs.bufpos, 0);
tic->strs.bufpos += sizeof(uint16_t);
tic->strs.entries++;
continue;
}
}
if (ind == -1)
_ti_store_extra(tic, 1, token, 'f', flag, 0, NULL, 0,
flags);
else if (_ti_find_cap(&tic->flags, 'f', ind) == NULL) {
if (_ti_grow_tbuf(&tic->flags, sizeof(uint16_t) + 1)
== NULL)
goto error;
le16enc(tic->flags.buf + tic->flags.bufpos, ind);
tic->flags.bufpos += sizeof(uint16_t);
tic->flags.buf[tic->flags.bufpos++] = flag;
tic->flags.entries++;
}
}
free(buf.buf);
return tic;
error:
free(buf.buf);
_ti_freetic(tic);
return NULL;
}
ssize_t
_ti_flatten(uint8_t **buf, const TIC *tic)
{
size_t buflen, len, alen, dlen;
uint8_t *cap;
_DIAGASSERT(buf != NULL);
_DIAGASSERT(tic != NULL);
len = strlen(tic->name) + 1;
if (tic->alias == NULL)
alen = 0;
else
alen = strlen(tic->alias) + 1;
if (tic->desc == NULL)
dlen = 0;
else
dlen = strlen(tic->desc) + 1;
buflen = sizeof(char) +
sizeof(uint16_t) + len +
sizeof(uint16_t) + alen +
sizeof(uint16_t) + dlen +
(sizeof(uint16_t) * 2) + tic->flags.bufpos +
(sizeof(uint16_t) * 2) + tic->nums.bufpos +
(sizeof(uint16_t) * 2) + tic->strs.bufpos +
(sizeof(uint16_t) * 2) + tic->extras.bufpos;
*buf = malloc(buflen);
if (*buf == NULL)
return -1;
cap = *buf;
*cap++ = 1;
le16enc(cap, len);
cap += sizeof(uint16_t);
memcpy(cap, tic->name, len);
cap += len;
le16enc(cap, alen);
cap += sizeof(uint16_t);
if (tic->alias != NULL) {
memcpy(cap, tic->alias, alen);
cap += alen;
}
le16enc(cap, dlen);
cap += sizeof(uint16_t);
if (tic->desc != NULL) {
memcpy(cap, tic->desc, dlen);
cap += dlen;
}
if (tic->flags.entries == 0) {
le16enc(cap, 0);
cap += sizeof(uint16_t);
} else {
le16enc(cap, (tic->flags.bufpos + sizeof(uint16_t)));
cap += sizeof(uint16_t);
le16enc(cap, tic->flags.entries);
cap += sizeof(uint16_t);
memcpy(cap, tic->flags.buf, tic->flags.bufpos);
cap += tic->flags.bufpos;
}
if (tic->nums.entries == 0) {
le16enc(cap, 0);
cap += sizeof(uint16_t);
} else {
le16enc(cap, (tic->nums.bufpos + sizeof(uint16_t)));
cap += sizeof(uint16_t);
le16enc(cap, tic->nums.entries);
cap += sizeof(uint16_t);
memcpy(cap, tic->nums.buf, tic->nums.bufpos);
cap += tic->nums.bufpos;
}
if (tic->strs.entries == 0) {
le16enc(cap, 0);
cap += sizeof(uint16_t);
} else {
le16enc(cap, (tic->strs.bufpos + sizeof(uint16_t)));
cap += sizeof(uint16_t);
le16enc(cap, tic->strs.entries);
cap += sizeof(uint16_t);
memcpy(cap, tic->strs.buf, tic->strs.bufpos);
cap += tic->strs.bufpos;
}
if (tic->extras.entries == 0) {
le16enc(cap, 0);
cap += sizeof(uint16_t);
} else {
le16enc(cap, (tic->extras.bufpos + sizeof(uint16_t)));
cap += sizeof(uint16_t);
le16enc(cap, tic->extras.entries);
cap += sizeof(uint16_t);
memcpy(cap, tic->extras.buf, tic->extras.bufpos);
cap += tic->extras.bufpos;
}
return cap - *buf;
}
/* Write a 16bit uint; return next position. */
unsigned
bs_write_u16(bin_stream_t * bs, uint16_t data)
{
le16enc(bs->data + bs->pos, data);
return (bs->pos += 2);
}
void
bsd_disklabel_le_enc(u_char *ptr, struct disklabel *d)
{
int i;
u_char *p, *pe;
uint16_t sum;
le32enc(ptr + 0, d->d_magic);
le16enc(ptr + 4, d->d_type);
le16enc(ptr + 6, d->d_subtype);
bcopy(d->d_typename, ptr + 8, 16);
bcopy(d->d_packname, ptr + 24, 16);
le32enc(ptr + 40, d->d_secsize);
le32enc(ptr + 44, d->d_nsectors);
le32enc(ptr + 48, d->d_ntracks);
le32enc(ptr + 52, d->d_ncylinders);
le32enc(ptr + 56, d->d_secpercyl);
le32enc(ptr + 60, d->d_secperunit);
le16enc(ptr + 64, d->d_sparespertrack);
le16enc(ptr + 66, d->d_sparespercyl);
le32enc(ptr + 68, d->d_acylinders);
le16enc(ptr + 72, d->d_rpm);
le16enc(ptr + 74, d->d_interleave);
le16enc(ptr + 76, d->d_trackskew);
le16enc(ptr + 78, d->d_cylskew);
le32enc(ptr + 80, d->d_headswitch);
le32enc(ptr + 84, d->d_trkseek);
le32enc(ptr + 88, d->d_flags);
le32enc(ptr + 92, d->d_drivedata[0]);
le32enc(ptr + 96, d->d_drivedata[1]);
le32enc(ptr + 100, d->d_drivedata[2]);
le32enc(ptr + 104, d->d_drivedata[3]);
le32enc(ptr + 108, d->d_drivedata[4]);
le32enc(ptr + 112, d->d_spare[0]);
le32enc(ptr + 116, d->d_spare[1]);
le32enc(ptr + 120, d->d_spare[2]);
le32enc(ptr + 124, d->d_spare[3]);
le32enc(ptr + 128, d->d_spare[4]);
le32enc(ptr + 132, d->d_magic2);
le16enc(ptr + 136, 0);
le16enc(ptr + 138, d->d_npartitions);
le32enc(ptr + 140, d->d_bbsize);
le32enc(ptr + 144, d->d_sbsize);
for (i = 0; i < d->d_npartitions; i++)
bsd_partition_le_enc(ptr + 148 + 16 * i, &d->d_partitions[i]);
pe = ptr + 148 + 16 * d->d_npartitions;
sum = 0;
for (p = ptr; p < pe; p += 2)
sum ^= le16dec(p);
le16enc(ptr + 136, sum);
}
