int
ikev2_validate_xform(struct iked_message *msg, size_t offset, size_t total,
struct ikev2_transform *xfrm)
{
u_int8_t *msgbuf = ibuf_data(msg->msg_data);
size_t xfrm_length;
if (total < sizeof(*xfrm)) {
log_debug("%s: payload malformed: too short for header "
"(%zu < %zu)", __func__, total, sizeof(*xfrm));
return (-1);
}
memcpy(xfrm, msgbuf + offset, sizeof(*xfrm));
xfrm_length = betoh16(xfrm->xfrm_length);
if (xfrm_length < sizeof(*xfrm)) {
log_debug("%s: payload malformed: shorter than minimal header "
"(%zu < %zu)", __func__, xfrm_length, sizeof(*xfrm));
return (-1);
}
if (total < xfrm_length) {
log_debug("%s: malformed payload: too long for payload size "
"(%zu < %zu)", __func__, total, xfrm_length);
return (-1);
}
return (0);
}
int
ikev2_validate_id(struct iked_message *msg, size_t offset, size_t left,
struct ikev2_payload *pld, struct ikev2_id *id)
{
u_int8_t *msgbuf = ibuf_data(msg->msg_data);
size_t pld_length;
pld_length = betoh16(pld->pld_length);
if (pld_length < sizeof(*pld) + sizeof(*id)) {
log_debug("%s: malformed payload: specified length smaller "
"than minimum size (%zu < %zu)", __func__, pld_length,
sizeof(*pld) + sizeof(*id));
return (-1);
}
/* This will actually be caught by earlier checks. */
if (left < sizeof(*id)) {
log_debug("%s: malformed payload: too short for header "
"(%zu < %zu)", __func__, left, sizeof(*id));
return (-1);
}
memcpy(id, msgbuf + offset, sizeof(*id));
return (0);
}
void
sdtemp_refresh(void *arg)
{
struct sdtemp_softc *sc = arg;
u_int8_t cmd;
int16_t data, sdata;
iic_acquire_bus(sc->sc_tag, 0);
cmd = JC_TEMP;
if (iic_exec(sc->sc_tag, I2C_OP_READ_WITH_STOP, sc->sc_addr,
&cmd, sizeof cmd, &data, sizeof data, 0) == 0) {
sdata = betoh16(data) & 0x1fff;
if (sdata & 0x1000)
sdata -= 0x2000;
sc->sc_sensor[JCTEMP_TEMP].value =
273150000 + 62500 * sdata;
sc->sc_sensor[JCTEMP_TEMP].flags &= ~SENSOR_FINVALID;
#if 0
printf("sdtemp %04x %04x %d\n", data & 0xffff,
(u_int)sdata & 0xffff,
sc->sc_sensor[JCTEMP_TEMP].value);
#endif
}
iic_release_bus(sc->sc_tag, 0);
}
/* Read an unsigned 16-bit integer from a big-endian file. */
int read_uint16be(int fd, uint16_t* buf)
{
size_t n;
n = read(fd, (char*) buf, 2);
*buf = betoh16(*buf);
return n;
}
UInt16 ByteOrder::ntoh16(UInt16 data)
{
#ifndef ANDROID
return be16toh(data);
#else
return betoh16(data);
#endif //ANDROID
}
int main(int cgc_argc, char *cgc_argv[])
{
uint8_t *data = NULL;
cgc_init_vault();
cgc_store_in_vault(0, (void *)cgc_handlers, sizeof(cgc_handlers));
while (1)
{
uint8_t msg[6], *newdata;
uint16_t id;
uint32_t len;
if (cgc_read_bytes(&msg[0], 4) == 0)
break;
id = betoh16(*(uint16_t *)&msg[0]);
len = betoh16(*(uint16_t *)&msg[2]);
if (len & 0x8000)
{
if (cgc_read_bytes(&msg[4], 2) == 0)
break;
len = betoh32(*(uint32_t *)&msg[2]) & 0x7fffffff;
}
if (!is_supported(id))
{
ignore:
if (consume_bytes(len) == 0)
break;
continue;
}
newdata = cgc_realloc(data, len);
if (newdata == NULL)
goto ignore;
data = newdata;
if (cgc_read_bytes(data, len) == 0)
break;
handle_msg(id, data, len);
}
cgc_free(data);
return 0;
}
static short readShort() {
short ret = *(short*)(s_data + s_pos);
if(s_bigEndian)
ret = betoh16(ret);
else
ret = letoh16(ret);
s_pos += sizeof(short);
return ret;
}
int
ikev2_pld_id(struct iked *env, struct ikev2_payload *pld,
struct iked_message *msg, size_t offset, size_t left, u_int payload)
{
u_int8_t *ptr;
struct ikev2_id id;
size_t len;
struct iked_id *idp, idb;
struct iked_sa *sa = msg->msg_sa;
u_int8_t *msgbuf = ibuf_data(msg->msg_data);
char idstr[IKED_ID_SIZE];
if (ikev2_validate_id(msg, offset, left, pld, &id))
return (-1);
bzero(&idb, sizeof(idb));
/* Don't strip the Id payload header */
ptr = msgbuf + offset;
len = betoh16(pld->pld_length) - sizeof(*pld);
idb.id_type = id.id_type;
idb.id_offset = sizeof(id);
if ((idb.id_buf = ibuf_new(ptr, len)) == NULL)
return (-1);
if (ikev2_print_id(&idb, idstr, sizeof(idstr)) == -1) {
log_debug("%s: malformed id", __func__);
return (-1);
}
log_debug("%s: id %s length %zu", __func__, idstr, len);
if (!ikev2_msg_frompeer(msg)) {
ibuf_release(idb.id_buf);
return (0);
}
if (!((sa->sa_hdr.sh_initiator && payload == IKEV2_PAYLOAD_IDr) ||
(!sa->sa_hdr.sh_initiator && payload == IKEV2_PAYLOAD_IDi))) {
log_debug("%s: unexpected id payload", __func__);
return (0);
}
idp = &msg->msg_parent->msg_id;
if (idp->id_type) {
log_debug("%s: duplicate id payload", __func__);
return (-1);
}
idp->id_buf = idb.id_buf;
idp->id_offset = idb.id_offset;
idp->id_type = idb.id_type;
return (0);
}
uint32_t
print_vdmphys_instance(uint8_t *buf, uint32_t size)
{
struct vdit_vdmphys_instance entry;
if (size < sizeof entry) {
printf("\tTRUNCATED ENTRY (%02x bytes, expected %02zx)\n",
size, sizeof entry);
return 0;
}
memcpy(&entry, buf, sizeof entry);
entry.version = betoh16(entry.version);
entry.mode = betoh16(entry.mode);
printf("\tvdmphys: version %02x mode %02x\n",
entry.version, entry.mode);
return size - sizeof entry;
}
int
ikev2_pld_ke(struct iked *env, struct ikev2_payload *pld,
struct iked_message *msg, size_t offset, size_t left)
{
struct ikev2_keyexchange kex;
u_int8_t *buf;
size_t len;
u_int8_t *msgbuf = ibuf_data(msg->msg_data);
if (ikev2_validate_ke(msg, offset, left, pld, &kex))
return (-1);
log_debug("%s: dh group %s reserved %d", __func__,
print_map(betoh16(kex.kex_dhgroup), ikev2_xformdh_map),
betoh16(kex.kex_reserved));
buf = msgbuf + offset + sizeof(kex);
len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(kex);
if (len == 0) {
log_debug("%s: malformed payload: no KE data given", __func__);
return (-1);
}
/* This will actually be caught by earlier checks. */
if (left < len) {
log_debug("%s: malformed payload: smaller than specified "
"(%zu < %zu)", __func__, left, len);
return (-1);
}
print_hex(buf, 0, len);
if (ikev2_msg_frompeer(msg)) {
ibuf_release(msg->msg_parent->msg_ke);
if ((msg->msg_parent->msg_ke = ibuf_new(buf, len)) == NULL) {
log_debug("%s: failed to get exchange", __func__);
return (-1);
}
}
return (0);
}
int
ikev2_pld_cp(struct iked *env, struct ikev2_payload *pld,
struct iked_message *msg, size_t offset, size_t left)
{
struct ikev2_cp cp;
struct ikev2_cfg *cfg;
u_int8_t *buf;
size_t len, i;
u_int8_t *msgbuf = ibuf_data(msg->msg_data);
struct iked_sa *sa = msg->msg_sa;
if (ikev2_validate_cp(msg, offset, left, pld, &cp))
return (-1);
offset += sizeof(cp);
buf = msgbuf + offset;
len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(cp);
log_debug("%s: type %s length %zu",
__func__, print_map(cp.cp_type, ikev2_cp_map), len);
print_hex(buf, 0, len);
for (i = 0; i < len;) {
cfg = (struct ikev2_cfg *)(buf + i);
log_debug("%s: %s 0x%04x length %d", __func__,
print_map(betoh16(cfg->cfg_type), ikev2_cfg_map),
betoh16(cfg->cfg_type),
betoh16(cfg->cfg_length));
i += betoh16(cfg->cfg_length) + sizeof(*cfg);
}
if (!ikev2_msg_frompeer(msg))
return (0);
if (sa)
sa->sa_cp = cp.cp_type;
return (0);
}
int
ikev2_pld_certreq(struct iked *env, struct ikev2_payload *pld,
struct iked_message *msg, size_t offset, size_t left)
{
struct iked_sa *sa = msg->msg_sa;
struct ikev2_cert cert;
u_int8_t *buf;
ssize_t len;
u_int8_t *msgbuf = ibuf_data(msg->msg_data);
if (ikev2_validate_certreq(msg, offset, left, pld, &cert))
return (-1);
offset += sizeof(cert);
buf = msgbuf + offset;
len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(cert);
log_debug("%s: type %s length %zd",
__func__, print_map(cert.cert_type, ikev2_cert_map), len);
/* This will actually be caught by earlier checks. */
if (len < 0) {
log_debug("%s: invalid certificate request length", __func__);
return (-1);
}
print_hex(buf, 0, len);
if (!ikev2_msg_frompeer(msg))
return (0);
if (cert.cert_type == IKEV2_CERT_X509_CERT) {
if (!len || (len % SHA_DIGEST_LENGTH) != 0) {
log_debug("%s: invalid certificate request", __func__);
return (-1);
}
}
if (msg->msg_sa == NULL)
return (-1);
/* Optional certreq for PSK */
if (sa->sa_hdr.sh_initiator)
sa->sa_stateinit |= IKED_REQ_CERT;
else
sa->sa_statevalid |= IKED_REQ_CERT;
ca_setreq(env, &sa->sa_hdr, &sa->sa_policy->pol_localid,
cert.cert_type, buf, len, PROC_CERT);
return (0);
}
uint32_t
print_vdmaggr_instance(uint8_t *buf, uint32_t size)
{
struct vdit_vdmaggr_instance entry;
struct vdit_instance_id *aggr;
uint32_t aggrsize;
uint stripe;
if (size < sizeof entry) {
printf("\tTRUNCATED ENTRY (%02x bytes, expected %02zx)\n",
size, sizeof entry);
return 0;
}
memcpy(&entry, buf, sizeof entry);
entry.version = betoh16(entry.version);
entry.aggr_count = betoh16(entry.aggr_count);
entry.stripe_size = betoh32(entry.stripe_size);
printf("\tvdmaggr: version %02x count %02x stripe size %08x\n",
entry.version, entry.aggr_count, entry.stripe_size);
aggrsize = entry.aggr_count * sizeof(struct vdit_instance_id) +
sizeof entry;
if (size < aggrsize) {
printf("\tTRUNCATED ENTRY (%02x bytes, expected %02x)\n",
size, aggrsize);
return 0;
}
aggr = (struct vdit_instance_id *)(buf + sizeof entry);
for (stripe = 0; stripe < entry.aggr_count; stripe++) {
printf("\t\tstripe %u", stripe);
print_vdit_instance_id(aggr++, "");
printf("\n");
}
return size - aggrsize;
}
int
ikev2_pld_eap(struct iked *env, struct ikev2_payload *pld,
struct iked_message *msg, off_t offset)
{
struct eap_header *hdr;
struct eap_message *eap = NULL;
struct iked_sa *sa = msg->msg_sa;
size_t len;
if ((hdr = ibuf_seek(msg->msg_data, offset, sizeof(*hdr))) == NULL) {
log_debug("%s: failed to get EAP header", __func__);
return (-1);
}
len = betoh16(hdr->eap_length);
if (len < sizeof(*eap)) {
log_info("%s: %s id %d length %d", __func__,
print_map(hdr->eap_code, eap_code_map),
hdr->eap_id, betoh16(hdr->eap_length));
} else {
/* Now try to get the indicated length */
if ((eap = ibuf_seek(msg->msg_data, offset, len)) == NULL) {
log_debug("%s: invalid EAP length", __func__);
return (-1);
}
log_info("%s: %s id %d length %d EAP-%s", __func__,
print_map(eap->eap_code, eap_code_map),
eap->eap_id, betoh16(eap->eap_length),
print_map(eap->eap_type, eap_type_map));
}
if (eap_parse(env, sa, hdr, msg->msg_response) == -1)
return (-1);
return (0);
}
int
ikev2_pld_e(struct iked *env, struct ikev2_payload *pld,
struct iked_message *msg, off_t offset)
{
struct iked_sa *sa = msg->msg_sa;
struct ibuf *e = NULL;
u_int8_t *msgbuf = ibuf_data(msg->msg_data);
struct iked_message emsg;
u_int8_t *buf;
size_t len;
int ret = -1;
buf = msgbuf + offset;
len = betoh16(pld->pld_length) - sizeof(*pld);
if ((e = ibuf_new(buf, len)) == NULL)
goto done;
if (ikev2_msg_frompeer(msg)) {
e = ikev2_msg_decrypt(env, msg->msg_sa, msg->msg_data, e);
} else {
sa->sa_hdr.sh_initiator = sa->sa_hdr.sh_initiator ? 0 : 1;
e = ikev2_msg_decrypt(env, msg->msg_sa, msg->msg_data, e);
sa->sa_hdr.sh_initiator = sa->sa_hdr.sh_initiator ? 0 : 1;
}
if (e == NULL)
goto done;
/*
* Parse decrypted payload
*/
bzero(&emsg, sizeof(emsg));
memcpy(&emsg, msg, sizeof(*msg));
emsg.msg_data = e;
emsg.msg_e = 1;
emsg.msg_parent = msg;
TAILQ_INIT(&emsg.msg_proposals);
ret = ikev2_pld_payloads(env, &emsg, 0, ibuf_size(e),
pld->pld_nextpayload, 0);
done:
ibuf_release(e);
return (ret);
}
int
ikev2_validate_nonce(struct iked_message *msg, size_t offset, size_t left,
struct ikev2_payload *pld)
{
size_t pld_length;
/* This will actually be caught by earlier checks. */
pld_length = betoh16(pld->pld_length);
if (pld_length < sizeof(*pld)) {
log_debug("%s: malformed payload: specified length smaller "
"than minimum size (%zu < %zu)", __func__, pld_length,
sizeof(*pld));
return (-1);
}
return (0);
}
char *
eap_identity_response(struct eap_message *eap)
{
size_t len;
char *str;
u_int8_t *ptr = (u_int8_t *)eap;
len = betoh16(eap->eap_length) - sizeof(*eap);
ptr += sizeof(*eap);
if (len == 0 || (str = get_string(ptr, len)) == NULL) {
log_info("%s: invalid identity response, length %zu",
__func__, len);
return (NULL);
}
log_debug("%s: identity '%s' length %zd", __func__, str, len);
return (str);
}
int
ikev2_pld_auth(struct iked *env, struct ikev2_payload *pld,
struct iked_message *msg, size_t offset, size_t left)
{
struct ikev2_auth auth;
struct iked_id *idp;
u_int8_t *buf;
size_t len;
struct iked_sa *sa = msg->msg_sa;
u_int8_t *msgbuf = ibuf_data(msg->msg_data);
if (ikev2_validate_auth(msg, offset, left, pld, &auth))
return (-1);
offset += sizeof(auth);
buf = msgbuf + offset;
len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(auth);
log_debug("%s: method %s length %zu",
__func__, print_map(auth.auth_method, ikev2_auth_map), len);
print_hex(buf, 0, len);
if (!ikev2_msg_frompeer(msg))
return (0);
/* The AUTH payload indicates if the responder wants EAP or not */
if (!sa_stateok(sa, IKEV2_STATE_EAP))
sa_state(env, sa, IKEV2_STATE_AUTH_REQUEST);
idp = &msg->msg_parent->msg_auth;
if (idp->id_type) {
log_debug("%s: duplicate auth payload", __func__);
return (-1);
}
ibuf_release(idp->id_buf);
idp->id_type = auth.auth_method;
idp->id_offset = 0;
if ((idp->id_buf = ibuf_new(buf, len)) == NULL)
return (-1);
return (0);
}
int
ikev2_pld_certreq(struct iked *env, struct ikev2_payload *pld,
struct iked_message *msg, off_t offset)
{
struct iked_sa *sa = msg->msg_sa;
struct ikev2_cert cert;
u_int8_t *buf;
size_t len;
u_int8_t *msgbuf = ibuf_data(msg->msg_data);
memcpy(&cert, msgbuf + offset, sizeof(cert));
offset += sizeof(cert);
buf = msgbuf + offset;
len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(cert);
log_debug("%s: type %s signatures length %d",
__func__, print_map(cert.cert_type, ikev2_cert_map), len);
print_hex(buf, 0, len);
if (!ikev2_msg_frompeer(msg))
return (0);
if (!len || (len % SHA_DIGEST_LENGTH) != 0) {
log_debug("%s: invalid certificate request", __func__);
return (-1);
}
if (msg->msg_sa == NULL)
return (-1);
/* Optional certreq for PSK */
if (sa->sa_hdr.sh_initiator)
sa->sa_stateinit |= IKED_REQ_CERT;
else
sa->sa_statevalid |= IKED_REQ_CERT;
ca_setreq(env, &sa->sa_hdr, &sa->sa_policy->pol_localid,
cert.cert_type, buf, len, PROC_CERT);
return (0);
}
uint32_t
print_vdit_boot_info(uint8_t *buf, uint32_t size)
{
struct vdit_boot_info_entry entry;
if (size < sizeof entry) {
printf("\tTRUNCATED ENTRY (%02x bytes, expected %02zx)\n",
size, sizeof entry);
return 0;
}
memcpy(&entry, buf, sizeof entry);
entry.version = betoh16(entry.version);
printf("\tboot info: version %02x", entry.version);
print_vdit_instance_id(&entry.default_swap, " default swap");
print_vdit_instance_id(&entry.default_root, " default root");
printf("\n");
return size - sizeof entry;
}
void
maxds_refresh(void *arg)
{
struct maxds_softc *sc = arg;
u_int8_t cmd;
u_int16_t data;
iic_acquire_bus(sc->sc_tag, 0);
cmd = DS1631_TEMP;
if (iic_exec(sc->sc_tag, I2C_OP_READ_WITH_STOP,
sc->sc_addr, &cmd, sizeof cmd, &data, sizeof data, 0) == 0) {
sc->sc_sensor[MAXDS_TEMP].value = 273150000 +
(int)(betoh16(data)) / 8 * 31250;
sc->sc_sensor[MAXDS_TEMP].flags &= ~SENSOR_FINVALID;
} else
sc->sc_sensor[MAXDS_TEMP].flags |= SENSOR_FINVALID;
iic_release_bus(sc->sc_tag, 0);
}
uint32_t
print_vdit_subdriver_info(uint8_t *buf, uint32_t size)
{
struct vdit_subdriver_entry entry;
vdit_id_t id;
if (size < sizeof entry) {
printf("\tTRUNCATED ENTRY (%02x bytes, expected %02zx)\n",
size, sizeof entry);
return 0;
}
memcpy(&entry, buf, sizeof entry);
entry.version = betoh16(entry.version);
printf("\tsubdriver: version %02x", entry.version);
memcpy(&id, &entry.subdriver_id, sizeof id);
id = betoh32(id);
printf(" id %08x name \"%s\"\n", id, entry.name);
return size - sizeof entry;
}
int
lmtemp_temp_read(struct lmtemp_softc *sc, uint8_t which, int *valp)
{
u_int8_t cmd;
u_int16_t data = 0x0000;
int error;
cmd = which;
error = iic_exec(sc->sc_tag, I2C_OP_READ_WITH_STOP,
sc->sc_addr, &cmd, sizeof cmd, &data, sizeof data, 0);
if (error)
return (error);
/* Some chips return transient 0's.. we try next time */
if (data == 0x0000)
return (1);
/* convert to half-degrees C */
*valp = betoh16(data) / (1 << (16 - sc->sc_bits));
return (0);
}
int
ikev2_pld_cert(struct iked *env, struct ikev2_payload *pld,
struct iked_message *msg, size_t offset, size_t left)
{
struct ikev2_cert cert;
u_int8_t *buf;
size_t len;
struct iked_id *certid;
u_int8_t *msgbuf = ibuf_data(msg->msg_data);
if (ikev2_validate_cert(msg, offset, left, pld, &cert))
return (-1);
offset += sizeof(cert);
buf = msgbuf + offset;
len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(cert);
log_debug("%s: type %s length %zu",
__func__, print_map(cert.cert_type, ikev2_cert_map), len);
print_hex(buf, 0, len);
if (!ikev2_msg_frompeer(msg))
return (0);
certid = &msg->msg_parent->msg_cert;
if (certid->id_type) {
log_debug("%s: duplicate cert payload", __func__);
return (-1);
}
if ((certid->id_buf = ibuf_new(buf, len)) == NULL) {
log_debug("%s: failed to save cert", __func__);
return (-1);
}
certid->id_type = cert.cert_type;
certid->id_offset = 0;
return (0);
}
int
ikev2_validate_pld(struct iked_message *msg, size_t offset, size_t left,
struct ikev2_payload *pld)
{
u_int8_t *msgbuf = ibuf_data(msg->msg_data);
size_t pld_length;
/* We need at least the generic header. */
if (left < sizeof(*pld)) {
log_debug("%s: malformed payload: too short for generic "
"header (%zu < %zu)", __func__, left, sizeof(*pld));
return (-1);
}
memcpy(pld, msgbuf + offset, sizeof(*pld));
/*
* We need at least the specified number of bytes.
* pld_length is the full size of the payload including
* the generic payload header.
*/
pld_length = betoh16(pld->pld_length);
if (left < pld_length) {
log_debug("%s: malformed payload: shorter than specified "
"(%zu < %zu)", __func__, left, pld_length);
return (-1);
}
/*
* Sanity check the specified payload size, it must
* be at last the size of the generic payload header.
*/
if (pld_length < sizeof(*pld)) {
log_debug("%s: malformed payload: shorter than minimum "
"header size (%zu < %zu)", __func__, pld_length,
sizeof(*pld));
return (-1);
}
return (0);
}
static zx_status_t GetRandom(Device* dev, void* buf, uint16_t count, size_t* actual) {
static_assert(MAX_RAND_BYTES <= UINT32_MAX, "");
if (count > MAX_RAND_BYTES) {
count = MAX_RAND_BYTES;
}
struct tpm_getrandom_cmd cmd;
uint32_t resp_len = tpm_init_getrandom(&cmd, count);
fbl::unique_free_ptr<tpm_getrandom_resp> resp(
reinterpret_cast<tpm_getrandom_resp*>(malloc(resp_len)));
size_t actual_read;
uint16_t bytes_returned;
if (!resp) {
return ZX_ERR_NO_MEMORY;
}
zx_status_t status = dev->ExecuteCmd(0, (uint8_t*)&cmd, sizeof(cmd),
(uint8_t*)resp.get(), resp_len, &actual_read);
if (status != ZX_OK) {
return status;
}
if (actual_read < sizeof(*resp) ||
actual_read != betoh32(resp->hdr.total_len)) {
return ZX_ERR_BAD_STATE;
}
bytes_returned = betoh16(resp->bytes_returned);
if (actual_read != sizeof(*resp) + bytes_returned ||
resp->hdr.tag != htobe16(TPM_ST_NO_SESSIONS) ||
bytes_returned > count ||
resp->hdr.return_code != htobe32(TPM_SUCCESS)) {
return ZX_ERR_BAD_STATE;
}
memcpy(buf, resp->bytes, bytes_returned);
mandatory_memset(resp->bytes, 0, bytes_returned);
*actual = bytes_returned;
return ZX_OK;
}
int
ikev2_pld_nonce(struct iked *env, struct ikev2_payload *pld,
struct iked_message *msg, size_t offset, size_t left)
{
size_t len;
u_int8_t *buf;
u_int8_t *msgbuf = ibuf_data(msg->msg_data);
if (ikev2_validate_nonce(msg, offset, left, pld))
return (-1);
buf = msgbuf + offset;
len = betoh16(pld->pld_length) - sizeof(*pld);
if (len == 0) {
log_debug("%s: malformed payload: no NONCE given", __func__);
return (-1);
}
/* This will actually be caught by earlier checks. */
if (left < len) {
log_debug("%s: malformed payload: smaller than specified "
"(%zu < %zu)", __func__, left, len);
return (-1);
}
print_hex(buf, 0, len);
if (ikev2_msg_frompeer(msg)) {
ibuf_release(msg->msg_nonce);
if ((msg->msg_nonce = ibuf_new(buf, len)) == NULL) {
log_debug("%s: failed to get peer nonce", __func__);
return (-1);
}
msg->msg_parent->msg_nonce = msg->msg_nonce;
}
return (0);
}
int
ikev2_pld_nonce(struct iked *env, struct ikev2_payload *pld,
struct iked_message *msg, off_t offset)
{
size_t len;
u_int8_t *buf;
u_int8_t *msgbuf = ibuf_data(msg->msg_data);
buf = msgbuf + offset;
len = betoh16(pld->pld_length) - sizeof(*pld);
print_hex(buf, 0, len);
if (ikev2_msg_frompeer(msg)) {
ibuf_release(msg->msg_nonce);
if ((msg->msg_nonce = ibuf_new(buf, len)) == NULL) {
log_debug("%s: failed to get peer nonce", __func__);
return (-1);
}
msg->msg_parent->msg_nonce = msg->msg_nonce;
}
return (0);
}
int
ikev2_pld_attr(struct iked *env, struct ikev2_transform *xfrm,
struct iked_message *msg, off_t offset, int total)
{
struct ikev2_attribute attr;
u_int type;
u_int8_t *msgbuf = ibuf_data(msg->msg_data);
memcpy(&attr, msgbuf + offset, sizeof(attr));
type = betoh16(attr.attr_type) & ~IKEV2_ATTRAF_TV;
log_debug("%s: attribute type %s length %d total %d",
__func__, print_map(type, ikev2_attrtype_map),
betoh16(attr.attr_length), total);
if (betoh16(attr.attr_type) & IKEV2_ATTRAF_TV) {
/* Type-Value attribute */
offset += sizeof(attr);
total -= sizeof(attr);
if (type == IKEV2_ATTRTYPE_KEY_LENGTH)
msg->msg_attrlength = betoh16(attr.attr_length);
} else {
/* Type-Length-Value attribute */
print_hex(msgbuf, offset + sizeof(attr),
betoh16(attr.attr_length) - sizeof(attr));
offset += betoh16(attr.attr_length);
total -= betoh16(attr.attr_length);
}
if (total > 0) {
/* Next attribute */
ikev2_pld_attr(env, xfrm, msg, offset, total);
}
return (0);
}
void
safte_read_encstat(void *arg)
{
struct safte_softc *sc = (struct safte_softc *)arg;
struct safte_readbuf_cmd cmd;
int flags, i;
struct safte_sensor *s;
u_int16_t oot;
rw_enter_write(&sc->sc_lock);
memset(&cmd, 0, sizeof(cmd));
cmd.opcode = READ_BUFFER;
cmd.flags |= SAFTE_RD_MODE;
cmd.bufferid = SAFTE_RD_ENCSTAT;
cmd.length = htobe16(sc->sc_encbuflen);
flags = SCSI_DATA_IN;
#ifndef SCSIDEBUG
flags |= SCSI_SILENT;
#endif
if (cold)
flags |= SCSI_AUTOCONF;
if (scsi_scsi_cmd(sc->sc_link, (struct scsi_generic *)&cmd,
sizeof(cmd), sc->sc_encbuf, sc->sc_encbuflen, 2, 30000, NULL,
flags) != 0) {
rw_exit_write(&sc->sc_lock);
return;
}
for (i = 0; i < sc->sc_nsensors; i++) {
s = &sc->sc_sensors[i];
s->se_sensor.flags &= ~SENSOR_FUNKNOWN;
DPRINTF(("%s: %d type: %d field: 0x%02x\n", DEVNAME(sc), i,
s->se_type, *s->se_field));
switch (s->se_type) {
case SAFTE_T_FAN:
switch (*s->se_field) {
case SAFTE_FAN_OP:
s->se_sensor.value = 1;
s->se_sensor.status = SENSOR_S_OK;
break;
case SAFTE_FAN_MF:
s->se_sensor.value = 0;
s->se_sensor.status = SENSOR_S_CRIT;
break;
case SAFTE_FAN_NOTINST:
case SAFTE_FAN_UNKNOWN:
default:
s->se_sensor.value = 0;
s->se_sensor.status = SENSOR_S_UNKNOWN;
s->se_sensor.flags |= SENSOR_FUNKNOWN;
break;
}
break;
case SAFTE_T_PWRSUP:
switch (*s->se_field) {
case SAFTE_PWR_OP_ON:
s->se_sensor.value = 1;
s->se_sensor.status = SENSOR_S_OK;
break;
case SAFTE_PWR_OP_OFF:
s->se_sensor.value = 0;
s->se_sensor.status = SENSOR_S_OK;
break;
case SAFTE_PWR_MF_ON:
s->se_sensor.value = 1;
s->se_sensor.status = SENSOR_S_CRIT;
break;
case SAFTE_PWR_MF_OFF:
s->se_sensor.value = 0;
s->se_sensor.status = SENSOR_S_CRIT;
break;
case SAFTE_PWR_NOTINST:
case SAFTE_PWR_PRESENT:
case SAFTE_PWR_UNKNOWN:
s->se_sensor.value = 0;
s->se_sensor.status = SENSOR_S_UNKNOWN;
s->se_sensor.flags |= SENSOR_FUNKNOWN;
break;
}
break;
case SAFTE_T_DOORLOCK:
switch (*s->se_field) {
case SAFTE_DOOR_LOCKED:
s->se_sensor.value = 1;
s->se_sensor.status = SENSOR_S_OK;
break;
case SAFTE_DOOR_UNLOCKED:
s->se_sensor.value = 0;
s->se_sensor.status = SENSOR_S_CRIT;
break;
case SAFTE_DOOR_UNKNOWN:
s->se_sensor.value = 0;
s->se_sensor.status = SENSOR_S_CRIT;
s->se_sensor.flags |= SENSOR_FUNKNOWN;
break;
}
break;
case SAFTE_T_ALARM:
switch (*s->se_field) {
case SAFTE_SPKR_OFF:
s->se_sensor.value = 0;
s->se_sensor.status = SENSOR_S_OK;
break;
case SAFTE_SPKR_ON:
s->se_sensor.value = 1;
s->se_sensor.status = SENSOR_S_CRIT;
break;
}
break;
case SAFTE_T_TEMP:
s->se_sensor.value = safte_temp2uK(*s->se_field,
sc->sc_celsius);
break;
}
}
oot = betoh16(*sc->sc_temperrs);
for (i = 0; i < sc->sc_ntemps; i++)
sc->sc_temps[i].se_sensor.status =
(oot & (1 << i)) ? SENSOR_S_CRIT : SENSOR_S_OK;
rw_exit_write(&sc->sc_lock);
}
