C++ wpabuf_head_u8函数代码示例

static int are_ies_equal(const struct wpa_bss *old,
			 const struct wpa_scan_res *new_res, u32 ie)
{
	const u8 *old_ie, *new_ie;
	struct wpabuf *old_ie_buff = NULL;
	struct wpabuf *new_ie_buff = NULL;
	int new_ie_len, old_ie_len, ret, is_multi;

	switch (ie) {
	case WPA_IE_VENDOR_TYPE:
		old_ie = wpa_bss_get_vendor_ie(old, ie);
		new_ie = wpa_scan_get_vendor_ie(new_res, ie);
		is_multi = 0;
		break;
	case WPS_IE_VENDOR_TYPE:
		old_ie_buff = wpa_bss_get_vendor_ie_multi(old, ie);
		new_ie_buff = wpa_scan_get_vendor_ie_multi(new_res, ie);
		is_multi = 1;
		break;
	case WLAN_EID_RSN:
	case WLAN_EID_SUPP_RATES:
	case WLAN_EID_EXT_SUPP_RATES:
		old_ie = wpa_bss_get_ie(old, ie);
		new_ie = wpa_scan_get_ie(new_res, ie);
		is_multi = 0;
		break;
	default:
		wpa_printf(MSG_DEBUG, "bss: %s: cannot compare IEs", __func__);
		return 0;
	}

	if (is_multi) {
		/* in case of multiple IEs stored in buffer */
		old_ie = old_ie_buff ? wpabuf_head_u8(old_ie_buff) : NULL;
		new_ie = new_ie_buff ? wpabuf_head_u8(new_ie_buff) : NULL;
		old_ie_len = old_ie_buff ? wpabuf_len(old_ie_buff) : 0;
		new_ie_len = new_ie_buff ? wpabuf_len(new_ie_buff) : 0;
	} else {
		/* in case of single IE */
		old_ie_len = old_ie ? old_ie[1] + 2 : 0;
		new_ie_len = new_ie ? new_ie[1] + 2 : 0;
	}

	if (!old_ie || !new_ie)
		ret = !old_ie && !new_ie;
	else
		ret = (old_ie_len == new_ie_len &&
		       os_memcmp(old_ie, new_ie, old_ie_len) == 0);

	wpabuf_free(old_ie_buff);
	wpabuf_free(new_ie_buff);

	return ret;
}

int wpas_mbo_ie(struct wpa_supplicant *wpa_s, u8 *buf, size_t len)
{
	struct wpabuf *mbo;
	int res;

	if (len < MBO_IE_HEADER + 3 + 7)
		return 0;

	/* Leave room for the MBO IE header */
	mbo = wpabuf_alloc(len - MBO_IE_HEADER);
	if (!mbo)
		return 0;

	/* Add non-preferred channels attribute */
	wpas_mbo_non_pref_chan_attrs(wpa_s, mbo, 0);

	/*
	 * Send cellular capabilities attribute even if AP does not advertise
	 * cellular capabilities.
	 */
	wpabuf_put_u8(mbo, MBO_ATTR_ID_CELL_DATA_CAPA);
	wpabuf_put_u8(mbo, 1);
	wpabuf_put_u8(mbo, wpa_s->conf->mbo_cell_capa);

	res = mbo_add_ie(buf, len, wpabuf_head_u8(mbo), wpabuf_len(mbo));
	if (!res)
		wpa_printf(MSG_ERROR, "Failed to add MBO IE");

	wpabuf_free(mbo);
	return res;
}

static struct wpabuf * eap_tnc_build_msg(struct eap_tnc_data *data, u8 id)
{
	struct wpabuf *req;
	u8 flags;
	size_t send_len, plen;

	wpa_printf(MSG_DEBUG, "EAP-TNC: Generating Request");

	flags = EAP_TNC_VERSION;
	send_len = wpabuf_len(data->out_buf) - data->out_used;
	if (1 + send_len > data->fragment_size) {
		send_len = data->fragment_size - 1;
		flags |= EAP_TNC_FLAGS_MORE_FRAGMENTS;
		if (data->out_used == 0) {
			flags |= EAP_TNC_FLAGS_LENGTH_INCLUDED;
			send_len -= 4;
		}
	}

	plen = 1 + send_len;
	if (flags & EAP_TNC_FLAGS_LENGTH_INCLUDED)
		plen += 4;
	req = eap_msg_alloc(EAP_VENDOR_IETF, EAP_TYPE_TNC, plen,
			    EAP_CODE_REQUEST, id);
	if (req == NULL)
		return NULL;

	wpabuf_put_u8(req, flags); /* Flags */
	if (flags & EAP_TNC_FLAGS_LENGTH_INCLUDED)
		wpabuf_put_be32(req, wpabuf_len(data->out_buf));

	wpabuf_put_data(req, wpabuf_head_u8(data->out_buf) + data->out_used,
			send_len);
	data->out_used += send_len;

	if (data->out_used == wpabuf_len(data->out_buf)) {
		wpa_printf(MSG_DEBUG, "EAP-TNC: Sending out %lu bytes "
			   "(message sent completely)",
			   (unsigned long) send_len);
		wpabuf_free(data->out_buf);
		data->out_buf = NULL;
		data->out_used = 0;
		if (data->was_fail)
			eap_tnc_set_state(data, FAIL);
		else if (data->was_done)
			eap_tnc_set_state(data, DONE);
	} else {
		wpa_printf(MSG_DEBUG, "EAP-TNC: Sending out %lu bytes "
			   "(%lu more to send)", (unsigned long) send_len,
			   (unsigned long) wpabuf_len(data->out_buf) -
			   data->out_used);
		if (data->state == FAIL)
			data->was_fail = 1;
		else if (data->state == DONE)
			data->was_done = 1;
		eap_tnc_set_state(data, WAIT_FRAG_ACK);
	}

	return req;
}

u8 * eap_sim_msg_add_mac(struct eap_sim_msg *msg, u8 attr)
{
	u8 *pos = eap_sim_msg_add(msg, attr, 0, NULL, EAP_SIM_MAC_LEN);
	if (pos)
		msg->mac = (pos - wpabuf_head_u8(msg->buf)) + 4;
	return pos;
}

int radius_msg_verify(struct radius_msg *msg, const u8 *secret,
		      size_t secret_len, struct radius_msg *sent_msg, int auth)
{
	const u8 *addr[4];
	size_t len[4];
	u8 hash[MD5_MAC_LEN];

	if (sent_msg == NULL) {
		printf("No matching Access-Request message found\n");
		return 1;
	}

	if (auth &&
	    radius_msg_verify_msg_auth(msg, secret, secret_len,
				       sent_msg->hdr->authenticator)) {
		return 1;
	}

	/* ResponseAuth = MD5(Code+ID+Length+RequestAuth+Attributes+Secret) */
	addr[0] = (u8 *) msg->hdr;
	len[0] = 1 + 1 + 2;
	addr[1] = sent_msg->hdr->authenticator;
	len[1] = MD5_MAC_LEN;
	addr[2] = wpabuf_head_u8(msg->buf) + sizeof(struct radius_hdr);
	len[2] = wpabuf_len(msg->buf) - sizeof(struct radius_hdr);
	addr[3] = secret;
	len[3] = secret_len;
	md5_vector(4, addr, len, hash);
	if (os_memcmp(hash, msg->hdr->authenticator, MD5_MAC_LEN) != 0) {
		printf("Response Authenticator invalid!\n");
		return 1;
	}

	return 0;
}

static ssize_t tls_pull_func(gnutls_transport_ptr ptr, void *buf,
			     size_t len)
{
	struct tls_connection *conn = (struct tls_connection *) ptr;
	const u8 *end;
	if (conn->pull_buf == NULL) {
		errno = EWOULDBLOCK;
		return -1;
	}

	end = wpabuf_head_u8(conn->pull_buf) + wpabuf_len(conn->pull_buf);
	if ((size_t) (end - conn->pull_buf_offset) < len)
		len = end - conn->pull_buf_offset;
	os_memcpy(buf, conn->pull_buf_offset, len);
	conn->pull_buf_offset += len;
	if (conn->pull_buf_offset == end) {
		wpa_printf(MSG_DEBUG, "%s - pull_buf consumed", __func__);
		wpabuf_free(conn->pull_buf);
		conn->pull_buf = NULL;
		conn->pull_buf_offset = NULL;
	} else {
		wpa_printf(MSG_DEBUG, "%s - %lu bytes remaining in pull_buf",
			   __func__,
			   (unsigned long) (end - conn->pull_buf_offset));
	}
	return len;
}

/**
 * gas_anqp_set_len - Set Query Request/Response Length
 * @buf: GAS message
 *
 * This function is used to update the Query Request/Response Length field once
 * the payload has been filled.
 */
void gas_anqp_set_len(struct wpabuf *buf)
{
	u8 action;
	size_t offset;
	u8 *len;

	if (buf == NULL || wpabuf_len(buf) < 2)
		return;

	action = *(wpabuf_head_u8(buf) + 1);
	switch (action) {
	case WLAN_PA_GAS_INITIAL_REQ:
		offset = 3 + 4;
		break;
	case WLAN_PA_GAS_INITIAL_RESP:
		offset = 7 + 4;
		break;
	case WLAN_PA_GAS_COMEBACK_RESP:
		offset = 8 + 4;
		break;
	default:
		return;
	}

	if (wpabuf_len(buf) < offset + 2)
		return;

	len = wpabuf_mhead_u8(buf) + offset;
	WPA_PUT_LE16(len, (u8 *) wpabuf_put(buf, 0) - len - 2);
}

int eap_sim_msg_add_encr_end(struct eap_sim_msg *msg, u8 *k_encr, int attr_pad)
{
	size_t encr_len;

	if (msg == NULL || k_encr == NULL || msg->iv == 0 || msg->encr == 0)
		return -1;

	encr_len = wpabuf_len(msg->buf) - msg->encr - 4;
	if (encr_len % 16) {
		u8 *pos;
		int pad_len = 16 - (encr_len % 16);
		if (pad_len < 4) {
			wpa_printf(MSG_WARNING, "EAP-SIM: "
				   "eap_sim_msg_add_encr_end - invalid pad_len"
				   " %d", pad_len);
			return -1;
		}
		wpa_printf(MSG_DEBUG, "   *AT_PADDING");
		pos = eap_sim_msg_add(msg, attr_pad, 0, NULL, pad_len - 4);
		if (pos == NULL)
			return -1;
		os_memset(pos + 4, 0, pad_len - 4);
		encr_len += pad_len;
	}
	wpa_printf(MSG_DEBUG, "   (AT_ENCR_DATA data len %lu)",
		   (unsigned long) encr_len);
	wpabuf_mhead_u8(msg->buf)[msg->encr + 1] = encr_len / 4 + 1;
	return aes_128_cbc_encrypt(k_encr, wpabuf_head_u8(msg->buf) + msg->iv,
				   wpabuf_mhead_u8(msg->buf) + msg->encr + 4,
				   encr_len);
}

/**
 * p2p_parse_p2p_ie - Parse P2P IE
 * @buf: Concatenated P2P IE(s) payload
 * @msg: Buffer for returning parsed attributes
 * Returns: 0 on success, -1 on failure
 *
 * Note: Caller is responsible for clearing the msg data structure before
 * calling this function.
 */
int p2p_parse_p2p_ie(const struct wpabuf *buf, struct p2p_message *msg)
{
    const u8 *pos = wpabuf_head_u8(buf);
    const u8 *end = pos + wpabuf_len(buf);

    wpa_printf(MSG_DEBUG, "P2P: Parsing P2P IE");

    while (pos < end) {
        u16 attr_len;
        if (pos + 2 >= end) {
            wpa_printf(MSG_DEBUG, "P2P: Invalid P2P attribute");
            return -1;
        }
        attr_len = WPA_GET_LE16(pos + 1);
        wpa_printf(MSG_DEBUG, "P2P: Attribute %d length %u",
                   pos[0], attr_len);
        if (pos + 3 + attr_len > end) {
            wpa_printf(MSG_DEBUG, "P2P: Attribute underflow "
                       "(len=%u left=%d)",
                       attr_len, (int) (end - pos - 3));
            wpa_hexdump(MSG_MSGDUMP, "P2P: Data", pos, end - pos);
            return -1;
        }
        if (p2p_parse_attribute(pos[0], pos + 3, attr_len, msg))
            return -1;
        pos += 3 + attr_len;
    }

    return 0;
}

struct wpabuf * eap_server_tls_build_msg(struct eap_ssl_data *data,
					 int eap_type, int version, u8 id)
{
	struct wpabuf *req;
	u8 flags;
	size_t send_len, plen;

	wpa_printf(MSG_DEBUG, "SSL: Generating Request");
	if (data->tls_out == NULL) {
		wpa_printf(MSG_ERROR, "SSL: tls_out NULL in %s", __func__);
		return NULL;
	}

	flags = version;
	send_len = wpabuf_len(data->tls_out) - data->tls_out_pos;
	if (1 + send_len > data->tls_out_limit) {
		send_len = data->tls_out_limit - 1;
		flags |= EAP_TLS_FLAGS_MORE_FRAGMENTS;
		if (data->tls_out_pos == 0) {
			flags |= EAP_TLS_FLAGS_LENGTH_INCLUDED;
			send_len -= 4;
		}
	}

	plen = 1 + send_len;
	if (flags & EAP_TLS_FLAGS_LENGTH_INCLUDED)
		plen += 4;

	req = eap_msg_alloc(EAP_VENDOR_IETF, eap_type, plen,
			    EAP_CODE_REQUEST, id);
	if (req == NULL)
		return NULL;

	wpabuf_put_u8(req, flags); /* Flags */
	if (flags & EAP_TLS_FLAGS_LENGTH_INCLUDED)
		wpabuf_put_be32(req, wpabuf_len(data->tls_out));

	wpabuf_put_data(req, wpabuf_head_u8(data->tls_out) + data->tls_out_pos,
			send_len);
	data->tls_out_pos += send_len;

	if (data->tls_out_pos == wpabuf_len(data->tls_out)) {
		wpa_printf(MSG_DEBUG, "SSL: Sending out %lu bytes "
			   "(message sent completely)",
			   (unsigned long) send_len);
		wpabuf_free(data->tls_out);
		data->tls_out = NULL;
		data->tls_out_pos = 0;
		data->state = MSG;
	} else {
		wpa_printf(MSG_DEBUG, "SSL: Sending out %lu bytes "
			   "(%lu more to send)", (unsigned long) send_len,
			   (unsigned long) wpabuf_len(data->tls_out) -
			   data->tls_out_pos);
		data->state = WAIT_FRAG_ACK;
	}

	return req;
}

static struct wpabuf *eap_wsc_build_msg(struct eap_wsc_data *data, struct eap_method_ret *ret, u8 id)
{
	struct wpabuf *resp;
	u8 flags;
	size_t send_len, plen;

	ret->ignore = FALSE;
	wpa_printf(MSG_DEBUG, "EAP-WSC: Generating Response");
	ret->allowNotifications = TRUE;

	flags = 0;
	send_len = wpabuf_len(data->out_buf) - data->out_used;
	if (2 + send_len > data->fragment_size) {
		send_len = data->fragment_size - 2;
		flags |= WSC_FLAGS_MF;
		if (data->out_used == 0) {
			flags |= WSC_FLAGS_LF;
			send_len -= 2;
		}
	}
	plen = 2 + send_len;
	if (flags & WSC_FLAGS_LF) {
		plen += 2;
	}
	resp = eap_msg_alloc(EAP_VENDOR_WFA, EAP_VENDOR_TYPE_WSC, plen, EAP_CODE_RESPONSE, id);
	if (resp == NULL) {
		return NULL;
	}

	wpabuf_put_u8(resp, data->out_op_code);	/* Op-Code */
	wpabuf_put_u8(resp, flags);	/* Flags */
	if (flags & WSC_FLAGS_LF) {
		wpabuf_put_be16(resp, wpabuf_len(data->out_buf));
	}

	wpabuf_put_data(resp, wpabuf_head_u8(data->out_buf) + data->out_used, send_len);
	data->out_used += send_len;

	ret->methodState = METHOD_MAY_CONT;
	ret->decision = DECISION_FAIL;

	if (data->out_used == wpabuf_len(data->out_buf)) {
		wpa_printf(MSG_DEBUG, "EAP-WSC: Sending out %lu bytes " "(message sent completely)", (unsigned long)send_len);
		wpabuf_free(data->out_buf);
		data->out_buf = NULL;
		data->out_used = 0;
		if ((data->state == FAIL && data->out_op_code == WSC_ACK) || data->out_op_code == WSC_NACK || data->out_op_code == WSC_Done) {
			eap_wsc_state(data, FAIL);
			ret->methodState = METHOD_DONE;
		} else {
			eap_wsc_state(data, MESG);
		}
	} else {
		wpa_printf(MSG_DEBUG, "EAP-WSC: Sending out %lu bytes " "(%lu more to send)", (unsigned long)send_len, (unsigned long)wpabuf_len(data->out_buf) - data->out_used);
		eap_wsc_state(data, WAIT_FRAG_ACK);
	}

	return resp;
}

static struct wpabuf * eap_tnc_build_msg(struct eap_tnc_data *data,
					 struct eap_method_ret *ret, u8 id)
{
	struct wpabuf *resp;
	u8 flags;
	size_t send_len, plen;

	ret->ignore = FALSE;
	wpa_printf(MSG_DEBUG, "EAP-TNC: Generating Response");
	ret->allowNotifications = TRUE;

	flags = EAP_TNC_VERSION;
	send_len = wpabuf_len(data->out_buf) - data->out_used;
	if (1 + send_len > data->fragment_size) {
		send_len = data->fragment_size - 1;
		flags |= EAP_TNC_FLAGS_MORE_FRAGMENTS;
		if (data->out_used == 0) {
			flags |= EAP_TNC_FLAGS_LENGTH_INCLUDED;
			send_len -= 4;
		}
	}

	plen = 1 + send_len;
	if (flags & EAP_TNC_FLAGS_LENGTH_INCLUDED)
		plen += 4;
	resp = eap_msg_alloc(EAP_VENDOR_IETF, EAP_TYPE_TNC, plen,
			     EAP_CODE_RESPONSE, id);
	if (resp == NULL)
		return NULL;

	wpabuf_put_u8(resp, flags); /* Flags */
	if (flags & EAP_TNC_FLAGS_LENGTH_INCLUDED)
		wpabuf_put_be32(resp, wpabuf_len(data->out_buf));

	wpabuf_put_data(resp, wpabuf_head_u8(data->out_buf) + data->out_used,
			send_len);
	data->out_used += send_len;

	ret->methodState = METHOD_MAY_CONT;
	ret->decision = DECISION_FAIL;

	if (data->out_used == wpabuf_len(data->out_buf)) {
		wpa_printf(MSG_DEBUG, "EAP-TNC: Sending out %lu bytes "
			   "(message sent completely)",
			   (unsigned long) send_len);
		wpabuf_free(data->out_buf);
		data->out_buf = NULL;
		data->out_used = 0;
	} else {
		wpa_printf(MSG_DEBUG, "EAP-TNC: Sending out %lu bytes "
			   "(%lu more to send)", (unsigned long) send_len,
			   (unsigned long) wpabuf_len(data->out_buf) -
			   data->out_used);
		data->state = WAIT_FRAG_ACK;
	}

	return resp;
}

static struct wpabuf * eap_wsc_build_msg(struct eap_wsc_data *data, u8 id)
{
	struct wpabuf *req;
	u8 flags;
	size_t send_len, plen;

	flags = 0;
	send_len = wpabuf_len(data->out_buf) - data->out_used;
	if (2 + send_len > data->fragment_size) {
		send_len = data->fragment_size - 2;
		flags |= WSC_FLAGS_MF;
		if (data->out_used == 0) {
			flags |= WSC_FLAGS_LF;
			send_len -= 2;
		}
	}
	plen = 2 + send_len;
	if (flags & WSC_FLAGS_LF)
		plen += 2;
	req = eap_msg_alloc(EAP_VENDOR_WFA, EAP_VENDOR_TYPE_WSC, plen,
			    EAP_CODE_REQUEST, id);
	if (req == NULL) {
		wpa_printf(MSG_ERROR, "EAP-WSC: Failed to allocate memory for "
			   "request");
		return NULL;
	}

	wpabuf_put_u8(req, data->out_op_code); /* Op-Code */
	wpabuf_put_u8(req, flags); /* Flags */
	if (flags & WSC_FLAGS_LF)
		wpabuf_put_be16(req, wpabuf_len(data->out_buf));

	wpabuf_put_data(req, wpabuf_head_u8(data->out_buf) + data->out_used,
			send_len);
	data->out_used += send_len;

	if (data->out_used == wpabuf_len(data->out_buf)) {
		wpa_printf(MSG_DEBUG, "EAP-WSC: Sending out %lu bytes "
			   "(message sent completely)",
			   (unsigned long) send_len);
		wpabuf_free(data->out_buf);
		data->out_buf = NULL;
		data->out_used = 0;
		eap_wsc_state(data, MESG);
	} else {
		wpa_printf(MSG_DEBUG, "EAP-WSC: Sending out %lu bytes "
			   "(%lu more to send)", (unsigned long) send_len,
			   (unsigned long) wpabuf_len(data->out_buf) -
			   data->out_used);
		eap_wsc_state(data, WAIT_FRAG_ACK);
	}

	return req;
}

static void wpas_mbo_non_pref_chan_changed(struct wpa_supplicant *wpa_s)
{
	struct wpabuf *buf;

	buf = wpabuf_alloc(512);
	if (!buf)
		return;

	wpas_mbo_non_pref_chan_attrs(wpa_s, buf, 1);
	wpas_mbo_send_wnm_notification(wpa_s, wpabuf_head_u8(buf),
				       wpabuf_len(buf));
	wpabuf_free(buf);
}

int eap_sim_msg_add_encr_start(struct eap_sim_msg *msg, u8 attr_iv,
			       u8 attr_encr)
{
	u8 *pos = eap_sim_msg_add(msg, attr_iv, 0, NULL, EAP_SIM_IV_LEN);
	if (pos == NULL)
		return -1;
	msg->iv = (pos - wpabuf_head_u8(msg->buf)) + 4;
	if (random_get_bytes(wpabuf_mhead_u8(msg->buf) + msg->iv,
			     EAP_SIM_IV_LEN)) {
		msg->iv = 0;
		return -1;
	}

	pos = eap_sim_msg_add(msg, attr_encr, 0, NULL, 0);
	if (pos == NULL) {
		msg->iv = 0;
		return -1;
	}
	msg->encr = pos - wpabuf_head_u8(msg->buf);

	return 0;
}