datagram.c

  1// SPDX-License-Identifier: GPL-2.0-only
  2/*
  3 * File: datagram.c
  4 *
  5 * Datagram (ISI) Phonet sockets
  6 *
  7 * Copyright (C) 2008 Nokia Corporation.
  8 *
  9 * Authors: Sakari Ailus <sakari.ailus@nokia.com>
 10 *          Rémi Denis-Courmont
 11 */
 12
 13#include <linux/kernel.h>
 14#include <linux/slab.h>
 15#include <linux/socket.h>
 16#include <asm/ioctls.h>
 17#include <net/sock.h>
 18
 19#include <linux/phonet.h>
 20#include <linux/export.h>
 21#include <net/phonet/phonet.h>
 22
 23static int pn_backlog_rcv(struct sock *sk, struct sk_buff *skb);
 24
 25/* associated socket ceases to exist */
 26static void pn_sock_close(struct sock *sk, long timeout)
 27{
 28	sk_common_release(sk);
 29}
 30
 31static int pn_ioctl(struct sock *sk, int cmd, int *karg)
 32{
 33	struct sk_buff *skb;
 34
 35	switch (cmd) {
 36	case SIOCINQ:
 37		spin_lock_bh(&sk->sk_receive_queue.lock);
 38		skb = skb_peek(&sk->sk_receive_queue);
 39		*karg = skb ? skb->len : 0;
 40		spin_unlock_bh(&sk->sk_receive_queue.lock);
 41		return 0;
 42
 43	case SIOCPNADDRESOURCE:
 44	case SIOCPNDELRESOURCE: {
 45			u32 res = *karg;
 46			if (res >= 256)
 47				return -EINVAL;
 48			if (cmd == SIOCPNADDRESOURCE)
 49				return pn_sock_bind_res(sk, res);
 50			else
 51				return pn_sock_unbind_res(sk, res);
 52		}
 53	}
 54
 55	return -ENOIOCTLCMD;
 56}
 57
 58/* Destroy socket. All references are gone. */
 59static void pn_destruct(struct sock *sk)
 60{
 61	skb_queue_purge(&sk->sk_receive_queue);
 62}
 63
 64static int pn_init(struct sock *sk)
 65{
 66	sk->sk_destruct = pn_destruct;
 67	return 0;
 68}
 69
 70static int pn_sendmsg(struct sock *sk, struct msghdr *msg, size_t len)
 71{
 72	DECLARE_SOCKADDR(struct sockaddr_pn *, target, msg->msg_name);
 73	struct sk_buff *skb;
 74	int err;
 75
 76	if (msg->msg_flags & ~(MSG_DONTWAIT|MSG_EOR|MSG_NOSIGNAL|
 77				MSG_CMSG_COMPAT))
 78		return -EOPNOTSUPP;
 79
 80	if (target == NULL)
 81		return -EDESTADDRREQ;
 82
 83	if (msg->msg_namelen < sizeof(struct sockaddr_pn))
 84		return -EINVAL;
 85
 86	if (target->spn_family != AF_PHONET)
 87		return -EAFNOSUPPORT;
 88
 89	skb = sock_alloc_send_skb(sk, MAX_PHONET_HEADER + len,
 90					msg->msg_flags & MSG_DONTWAIT, &err);
 91	if (skb == NULL)
 92		return err;
 93	skb_reserve(skb, MAX_PHONET_HEADER);
 94
 95	err = memcpy_from_msg((void *)skb_put(skb, len), msg, len);
 96	if (err < 0) {
 97		kfree_skb(skb);
 98		return err;
 99	}
100
101	/*
102	 * Fill in the Phonet header and
103	 * finally pass the packet forwards.
104	 */
105	err = pn_skb_send(sk, skb, target);
106
107	/* If ok, return len. */
108	return (err >= 0) ? len : err;
109}
110
111static int pn_recvmsg(struct sock *sk, struct msghdr *msg, size_t len,
112		      int flags, int *addr_len)
113{
114	struct sk_buff *skb = NULL;
115	struct sockaddr_pn sa;
116	int rval = -EOPNOTSUPP;
117	int copylen;
118
119	if (flags & ~(MSG_PEEK|MSG_TRUNC|MSG_DONTWAIT|MSG_NOSIGNAL|
120			MSG_CMSG_COMPAT))
121		goto out_nofree;
122
123	skb = skb_recv_datagram(sk, flags, &rval);
124	if (skb == NULL)
125		goto out_nofree;
126
127	pn_skb_get_src_sockaddr(skb, &sa);
128
129	copylen = skb->len;
130	if (len < copylen) {
131		msg->msg_flags |= MSG_TRUNC;
132		copylen = len;
133	}
134
135	rval = skb_copy_datagram_msg(skb, 0, msg, copylen);
136	if (rval) {
137		rval = -EFAULT;
138		goto out;
139	}
140
141	rval = (flags & MSG_TRUNC) ? skb->len : copylen;
142
143	if (msg->msg_name != NULL) {
144		__sockaddr_check_size(sizeof(sa));
145		memcpy(msg->msg_name, &sa, sizeof(sa));
146		*addr_len = sizeof(sa);
147	}
148
149out:
150	skb_free_datagram(sk, skb);
151
152out_nofree:
153	return rval;
154}
155
156/* Queue an skb for a sock. */
157static int pn_backlog_rcv(struct sock *sk, struct sk_buff *skb)
158{
159	int err = sock_queue_rcv_skb(sk, skb);
160
161	if (err < 0)
162		kfree_skb(skb);
163	return err ? NET_RX_DROP : NET_RX_SUCCESS;
164}
165
166/* Module registration */
167static struct proto pn_proto = {
168	.close		= pn_sock_close,
169	.ioctl		= pn_ioctl,
170	.init		= pn_init,
171	.sendmsg	= pn_sendmsg,
172	.recvmsg	= pn_recvmsg,
173	.backlog_rcv	= pn_backlog_rcv,
174	.hash		= pn_sock_hash,
175	.unhash		= pn_sock_unhash,
176	.get_port	= pn_sock_get_port,
177	.obj_size	= sizeof(struct pn_sock),
178	.owner		= THIS_MODULE,
179	.name		= "PHONET",
180};
181
182static const struct phonet_protocol pn_dgram_proto = {
183	.ops		= &phonet_dgram_ops,
184	.prot		= &pn_proto,
185	.sock_type	= SOCK_DGRAM,
186};
187
188int __init isi_register(void)
189{
190	return phonet_proto_register(PN_PROTO_PHONET, &pn_dgram_proto);
191}
192
193void __exit isi_unregister(void)
194{
195	phonet_proto_unregister(PN_PROTO_PHONET, &pn_dgram_proto);
196}