C++ pnp_hdr函数代码示例

/* Control requests are not sent by the pipe service and have a specific
 * message format. */
static int pep_ctrlreq_error(struct sock *sk, struct sk_buff *oskb, u8 code,
				gfp_t priority)
{
	const struct pnpipehdr *oph = pnp_hdr(oskb);
	struct sk_buff *skb;
	struct pnpipehdr *ph;
	struct sockaddr_pn dst;
	u8 data[4] = {
		oph->data[0], /* PEP type */
		code, /* error code, at an unusual offset */
		PAD, PAD,
	};

	skb = pep_alloc_skb(sk, data, 4, priority);
	if (!skb)
		return -ENOMEM;

	ph = pnp_hdr(skb);
	ph->utid = oph->utid;
	ph->message_id = PNS_PEP_CTRL_RESP;
	ph->pipe_handle = oph->pipe_handle;
	ph->data[0] = oph->data[1]; /* CTRL id */

	pn_skb_get_src_sockaddr(oskb, &dst);
	return pn_skb_send(sk, skb, &dst);
}

/*
 * Deliver an skb to a listening sock.
 * Socket lock must be held.
 * We then queue the skb to the right connected sock (if any).
 */
static int pep_do_rcv(struct sock *sk, struct sk_buff *skb)
{
	struct pep_sock *pn = pep_sk(sk);
	struct sock *sknode;
	struct pnpipehdr *hdr;
	struct sockaddr_pn dst;
	u8 pipe_handle;

	if (!pskb_may_pull(skb, sizeof(*hdr)))
		goto drop;

	hdr = pnp_hdr(skb);
	pipe_handle = hdr->pipe_handle;
	if (pipe_handle == PN_PIPE_INVALID_HANDLE)
		goto drop;

	pn_skb_get_dst_sockaddr(skb, &dst);

	/* Look for an existing pipe handle */
	sknode = pep_find_pipe(&pn->hlist, &dst, pipe_handle);
	if (sknode)
		return sk_receive_skb(sknode, skb, 1);

	switch (hdr->message_id) {
	case PNS_PEP_CONNECT_REQ:
		if (sk->sk_state != TCP_LISTEN || sk_acceptq_is_full(sk)) {
			pep_reject_conn(sk, skb, PN_PIPE_ERR_PEP_IN_USE,
					GFP_ATOMIC);
			break;
		}
		skb_queue_head(&sk->sk_receive_queue, skb);
		sk_acceptq_added(sk);
		if (!sock_flag(sk, SOCK_DEAD))
			sk->sk_data_ready(sk, 0);
		return NET_RX_SUCCESS;

	case PNS_PEP_DISCONNECT_REQ:
		pep_reply(sk, skb, PN_PIPE_NO_ERROR, NULL, 0, GFP_ATOMIC);
		break;

	case PNS_PEP_CTRL_REQ:
		pep_ctrlreq_error(sk, skb, PN_PIPE_INVALID_HANDLE, GFP_ATOMIC);
		break;

	case PNS_PEP_RESET_REQ:
	case PNS_PEP_ENABLE_REQ:
	case PNS_PEP_DISABLE_REQ:
		/* invalid handle is not even allowed here! */
		break;

	default:
		if ((1 << sk->sk_state)
				& ~(TCPF_CLOSE|TCPF_LISTEN|TCPF_CLOSE_WAIT))
			/* actively connected socket */
			return pipe_handler_do_rcv(sk, skb);
	}
drop:
	kfree_skb(skb);
	return NET_RX_SUCCESS;
}

static int pep_ctrlreq_error(struct sock *sk, struct sk_buff *oskb, u8 code,
				gfp_t priority)
{
	const struct pnpipehdr *oph = pnp_hdr(oskb);
	struct sk_buff *skb;
	struct pnpipehdr *ph;
	struct sockaddr_pn dst;

	skb = alloc_skb(MAX_PNPIPE_HEADER + 4, priority);
	if (!skb)
		return -ENOMEM;
	skb_set_owner_w(skb, sk);

	skb_reserve(skb, MAX_PHONET_HEADER);
	ph = (struct pnpipehdr *)skb_put(skb, sizeof(*ph) + 4);

	ph->utid = oph->utid;
	ph->message_id = PNS_PEP_CTRL_RESP;
	ph->pipe_handle = oph->pipe_handle;
	ph->data[0] = oph->data[1]; /* CTRL id */
	ph->data[1] = oph->data[0]; /* PEP type */
	ph->data[2] = code; /* error code, at an usual offset */
	ph->data[3] = PAD;
	ph->data[4] = PAD;

	pn_skb_get_src_sockaddr(oskb, &dst);
	return pn_skb_send(sk, skb, &dst);
}

static int pipe_snd_status(struct sock *sk, u8 type, u8 status, gfp_t priority)
{
	struct pep_sock *pn = pep_sk(sk);
	struct pnpipehdr *ph;
	struct sk_buff *skb;

	skb = alloc_skb(MAX_PNPIPE_HEADER + 4, priority);
	if (!skb)
		return -ENOMEM;
	skb_set_owner_w(skb, sk);

	skb_reserve(skb, MAX_PNPIPE_HEADER + 4);
	__skb_push(skb, sizeof(*ph) + 4);
	skb_reset_transport_header(skb);
	ph = pnp_hdr(skb);
	ph->utid = 0;
	ph->message_id = PNS_PEP_STATUS_IND;
	ph->pipe_handle = pn->pipe_handle;
	ph->pep_type = PN_PEP_TYPE_COMMON;
	ph->data[1] = type;
	ph->data[2] = PAD;
	ph->data[3] = PAD;
	ph->data[4] = status;

	return pn_skb_send(sk, skb, &pipe_srv);
}

static int pipe_skb_send(struct sock *sk, struct sk_buff *skb)
{
	struct pep_sock *pn = pep_sk(sk);
	struct pnpipehdr *ph;
	int err;

	if (pn_flow_safe(pn->tx_fc) &&
	    !atomic_add_unless(&pn->tx_credits, -1, 0)) {
		kfree_skb(skb);
		return -ENOBUFS;
	}

	skb_push(skb, 3 + pn->aligned);
	skb_reset_transport_header(skb);
	ph = pnp_hdr(skb);
	ph->utid = 0;
	if (pn->aligned) {
		ph->message_id = PNS_PIPE_ALIGNED_DATA;
		ph->data[0] = 0; /* padding */
	} else
		ph->message_id = PNS_PIPE_DATA;
	ph->pipe_handle = pn->pipe_handle;
	err = pn_skb_send(sk, skb, NULL);

	if (err && pn_flow_safe(pn->tx_fc))
		atomic_inc(&pn->tx_credits);
	return err;

}

static int pipe_rcv_created(struct sock *sk, struct sk_buff *skb)
{
	struct pep_sock *pn = pep_sk(sk);
	struct pnpipehdr *hdr = pnp_hdr(skb);
	u8 n_sb = hdr->data[0];

	pn->rx_fc = pn->tx_fc = PN_LEGACY_FLOW_CONTROL;
	__skb_pull(skb, sizeof(*hdr));
	while (n_sb > 0) {
		u8 type, buf[2], len = sizeof(buf);
		u8 *data = pep_get_sb(skb, &type, &len, buf);

		if (data == NULL)
			return -EINVAL;
		switch (type) {
		case PN_PIPE_SB_NEGOTIATED_FC:
			if (len < 2 || (data[0] | data[1]) > 3)
				break;
			pn->tx_fc = data[0] & 3;
			pn->rx_fc = data[1] & 3;
			break;
		}
		n_sb--;
	}
	return 0;
}

/*
 * Deliver an skb to a listening sock.
 * Socket lock must be held.
 * We then queue the skb to the right connected sock (if any).
 */
static int pep_do_rcv(struct sock *sk, struct sk_buff *skb)
{
	struct pep_sock *pn = pep_sk(sk);
	struct sock *sknode;
	struct pnpipehdr *hdr;
	struct sockaddr_pn dst;
	u8 pipe_handle;

	if (!pskb_may_pull(skb, sizeof(*hdr)))
		goto drop;

	hdr = pnp_hdr(skb);
	pipe_handle = hdr->pipe_handle;
	if (pipe_handle == PN_PIPE_INVALID_HANDLE)
		goto drop;

	pn_skb_get_dst_sockaddr(skb, &dst);

	/* Look for an existing pipe handle */
	sknode = pep_find_pipe(&pn->hlist, &dst, pipe_handle);
	if (sknode)
		return sk_receive_skb(sknode, skb, 1);

	switch (hdr->message_id) {
	case PNS_PEP_CONNECT_REQ:
		if (sk->sk_state != TCP_LISTEN || sk_acceptq_is_full(sk)) {
			pep_reject_conn(sk, skb, PN_PIPE_ERR_PEP_IN_USE,
					GFP_ATOMIC);
			break;
		}
		skb_queue_head(&sk->sk_receive_queue, skb);
		sk_acceptq_added(sk);
		if (!sock_flag(sk, SOCK_DEAD))

static int pep_do_rcv(struct sock *sk, struct sk_buff *skb)
{
	struct pep_sock *pn = pep_sk(sk);
	struct sock *sknode;
	struct pnpipehdr *hdr;
	struct sockaddr_pn dst;
	int err = NET_RX_SUCCESS;
	u8 pipe_handle;

	if (!pskb_may_pull(skb, sizeof(*hdr)))
		goto drop;

	hdr = pnp_hdr(skb);
	pipe_handle = hdr->pipe_handle;
	if (pipe_handle == PN_PIPE_INVALID_HANDLE)
		goto drop;

	pn_skb_get_dst_sockaddr(skb, &dst);

	/* Look for an existing pipe handle */
	sknode = pep_find_pipe(&pn->hlist, &dst, pipe_handle);
	if (sknode)
		return sk_receive_skb(sknode, skb, 1);

	/* Look for a pipe handle pending accept */
	sknode = pep_find_pipe(&pn->ackq, &dst, pipe_handle);
	if (sknode) {
		sock_put(sknode);
		if (net_ratelimit())
			printk(KERN_WARNING"Phonet unconnected PEP ignored");
		err = NET_RX_DROP;
		goto drop;
	}

	switch (hdr->message_id) {
	case PNS_PEP_CONNECT_REQ:
		err = pep_connreq_rcv(sk, skb);
		break;

	case PNS_PEP_DISCONNECT_REQ:
		pep_reply(sk, skb, PN_PIPE_NO_ERROR, NULL, 0, GFP_ATOMIC);
		break;

	case PNS_PEP_CTRL_REQ:
		pep_ctrlreq_error(sk, skb, PN_PIPE_INVALID_HANDLE, GFP_ATOMIC);
		break;

	case PNS_PEP_RESET_REQ:
	case PNS_PEP_ENABLE_REQ:
	case PNS_PEP_DISABLE_REQ:
		/* invalid handle is not even allowed here! */
	default:
		err = NET_RX_DROP;
	}
drop:
	kfree_skb(skb);
	return err;
}

static int pep_enableresp_rcv(struct sock *sk, struct sk_buff *skb)
{
	struct pnpipehdr *hdr = pnp_hdr(skb);

	if (hdr->error_code != PN_PIPE_NO_ERROR)
		return -ECONNREFUSED;

	return pep_indicate(sk, PNS_PIPE_ENABLED_IND, 0 /* sub-blocks */,
		NULL, 0, GFP_ATOMIC);

}

static int pep_reply(struct sock *sk, struct sk_buff *oskb, u8 code,
			const void *data, int len, gfp_t priority)
{
	const struct pnpipehdr *oph = pnp_hdr(oskb);
	struct pnpipehdr *ph;
	struct sk_buff *skb;
	struct sockaddr_pn peer;

	skb = pep_alloc_skb(sk, data, len, priority);
	if (!skb)
		return -ENOMEM;

	ph = pnp_hdr(skb);
	ph->utid = oph->utid;
	ph->message_id = oph->message_id + 1; /* REQ -> RESP */
	ph->pipe_handle = oph->pipe_handle;
	ph->error_code = code;

	pn_skb_get_src_sockaddr(oskb, &peer);
	return pn_skb_send(sk, skb, &peer);
}

static int pipe_rcv_status(struct sock *sk, struct sk_buff *skb)
{
	struct pep_sock *pn = pep_sk(sk);
	struct pnpipehdr *hdr;
	int wake = 0;

	if (!pskb_may_pull(skb, sizeof(*hdr) + 4))
		return -EINVAL;

	hdr = pnp_hdr(skb);
	if (hdr->data[0] != PN_PEP_TYPE_COMMON) {
		LIMIT_NETDEBUG(KERN_DEBUG"Phonet unknown PEP type: %u\n",
				(unsigned)hdr->data[0]);
		return -EOPNOTSUPP;
	}

	switch (hdr->data[1]) {
	case PN_PEP_IND_FLOW_CONTROL:
		switch (pn->tx_fc) {
		case PN_LEGACY_FLOW_CONTROL:
			switch (hdr->data[4]) {
			case PEP_IND_BUSY:
				atomic_set(&pn->tx_credits, 0);
				break;
			case PEP_IND_READY:
				atomic_set(&pn->tx_credits, wake = 1);
				break;
			}
			break;
		case PN_ONE_CREDIT_FLOW_CONTROL:
			if (hdr->data[4] == PEP_IND_READY)
				atomic_set(&pn->tx_credits, wake = 1);
			break;
		}
		break;

	case PN_PEP_IND_ID_MCFC_GRANT_CREDITS:
		if (pn->tx_fc != PN_MULTI_CREDIT_FLOW_CONTROL)
			break;
		atomic_add(wake = hdr->data[4], &pn->tx_credits);
		break;

	default:
		LIMIT_NETDEBUG(KERN_DEBUG"Phonet unknown PEP indication: %u\n",
				(unsigned)hdr->data[1]);
		return -EOPNOTSUPP;
	}
	if (wake)
		sk->sk_write_space(sk);
	return 0;
}

static int pipe_do_remove(struct sock *sk)
{
	struct pep_sock *pn = pep_sk(sk);
	struct pnpipehdr *ph;
	struct sk_buff *skb;

	skb = pep_alloc_skb(sk, NULL, 0, GFP_KERNEL);
	if (!skb)
		return -ENOMEM;

	ph = pnp_hdr(skb);
	ph->utid = 0;
	ph->message_id = PNS_PIPE_REMOVE_REQ;
	ph->pipe_handle = pn->pipe_handle;
	ph->data[0] = PAD;
	return pn_skb_send(sk, skb, NULL);
}

static int pep_indicate(struct sock *sk, u8 id, u8 code,
			const void *data, int len, gfp_t priority)
{
	struct pep_sock *pn = pep_sk(sk);
	struct pnpipehdr *ph;
	struct sk_buff *skb;

	skb = pep_alloc_skb(sk, data, len, priority);
	if (!skb)
		return -ENOMEM;

	ph = pnp_hdr(skb);
	ph->utid = 0;
	ph->message_id = id;
	ph->pipe_handle = pn->pipe_handle;
	ph->data[0] = code;
	return pn_skb_send(sk, skb, NULL);
}

static int pipe_handler_request(struct sock *sk, u8 id, u8 code,
				const void *data, int len)
{
	struct pep_sock *pn = pep_sk(sk);
	struct pnpipehdr *ph;
	struct sk_buff *skb;

	skb = pep_alloc_skb(sk, data, len, GFP_KERNEL);
	if (!skb)
		return -ENOMEM;

	ph = pnp_hdr(skb);
	ph->utid = id; /* whatever */
	ph->message_id = id;
	ph->pipe_handle = pn->pipe_handle;
	ph->data[0] = code;
	return pn_skb_send(sk, skb, NULL);
}

static int pipe_skb_send(struct sock *sk, struct sk_buff *skb)
{
	struct pep_sock *pn = pep_sk(sk);
	struct pnpipehdr *ph;

	if (pn_flow_safe(pn->tx_fc) &&
	    !atomic_add_unless(&pn->tx_credits, -1, 0)) {
		kfree_skb(skb);
		return -ENOBUFS;
	}

	skb_push(skb, 3);
	skb_reset_transport_header(skb);
	ph = pnp_hdr(skb);
	ph->utid = 0;
	ph->message_id = PNS_PIPE_DATA;
	ph->pipe_handle = pn->pipe_handle;

	return pn_skb_send(sk, skb, &pipe_srv);
}