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;
skb = alloc_skb(MAX_PNPIPE_HEADER + len, priority);
if (!skb)
return -ENOMEM;
skb_set_owner_w(skb, sk);
skb_reserve(skb, MAX_PNPIPE_HEADER);
__skb_put(skb, len);
skb_copy_to_linear_data(skb, data, len);
__skb_push(skb, sizeof(*ph));
skb_reset_transport_header(skb);
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;
return pn_skb_send(sk, skb, &pipe_srv);
}
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);
}
/* 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);
}
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_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 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 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 pn_sendmsg(struct kiocb *iocb, struct sock *sk,
struct msghdr *msg, size_t len)
{
struct sockaddr_pn *target;
struct sk_buff *skb;
int err;
if (msg->msg_flags & ~(MSG_DONTWAIT|MSG_EOR|MSG_NOSIGNAL|
MSG_CMSG_COMPAT))
return -EOPNOTSUPP;
if (msg->msg_name == NULL)
return -EDESTADDRREQ;
if (msg->msg_namelen < sizeof(struct sockaddr_pn))
return -EINVAL;
target = (struct sockaddr_pn *)msg->msg_name;
if (target->spn_family != AF_PHONET)
return -EAFNOSUPPORT;
skb = sock_alloc_send_skb(sk, MAX_PHONET_HEADER + len,
msg->msg_flags & MSG_DONTWAIT, &err);
if (skb == NULL)
return err;
skb_reserve(skb, MAX_PHONET_HEADER);
err = memcpy_fromiovec((void *)skb_put(skb, len), msg->msg_iov, len);
if (err < 0) {
kfree_skb(skb);
return err;
}
/*
* Fill in the Phonet header and
* finally pass the packet forwards.
*/
err = pn_skb_send(sk, skb, target);
/* If ok, return len. */
return (err >= 0) ? len : err;
}
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);
}
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 pn_sendmsg(struct kiocb *iocb, struct sock *sk,
struct msghdr *msg, size_t len)
{
struct sockaddr_pn *target;
struct sk_buff *skb;
int err;
if (msg->msg_flags & MSG_OOB)
return -EOPNOTSUPP;
if (msg->msg_name == NULL)
return -EDESTADDRREQ;
if (msg->msg_namelen < sizeof(struct sockaddr_pn))
return -EINVAL;
target = (struct sockaddr_pn *)msg->msg_name;
if (target->spn_family != AF_PHONET)
return -EAFNOSUPPORT;
skb = sock_alloc_send_skb(sk, MAX_PHONET_HEADER + len,
msg->msg_flags & MSG_DONTWAIT, &err);
if (skb == NULL)
return err;
skb_reserve(skb, MAX_PHONET_HEADER);
err = memcpy_fromiovec((void *)skb_put(skb, len), msg->msg_iov, len);
if (err < 0) {
kfree_skb(skb);
return err;
}
err = pn_skb_send(sk, skb, target);
return (err >= 0) ? len : err;
}
