1 /*
2  * llc_sap.c - driver routines for SAP component.
3  *
4  * Copyright (c) 1997 by Procom Technology, Inc.
5  * 		 2001-2003 by Arnaldo Carvalho de Melo <acme@conectiva.com.br>
6  *
7  * This program can be redistributed or modified under the terms of the
8  * GNU General Public License as published by the Free Software Foundation.
9  * This program is distributed without any warranty or implied warranty
10  * of merchantability or fitness for a particular purpose.
11  *
12  * See the GNU General Public License for more details.
13  */
14 
15 #include <net/llc.h>
16 #include <net/llc_if.h>
17 #include <net/llc_conn.h>
18 #include <net/llc_pdu.h>
19 #include <net/llc_sap.h>
20 #include <net/llc_s_ac.h>
21 #include <net/llc_s_ev.h>
22 #include <net/llc_s_st.h>
23 #include <net/sock.h>
24 #include <net/tcp_states.h>
25 #include <linux/llc.h>
26 #include <linux/slab.h>
27 
llc_mac_header_len(unsigned short devtype)28 static int llc_mac_header_len(unsigned short devtype)
29 {
30 	switch (devtype) {
31 	case ARPHRD_ETHER:
32 	case ARPHRD_LOOPBACK:
33 		return sizeof(struct ethhdr);
34 	}
35 	return 0;
36 }
37 
38 /**
39  *	llc_alloc_frame - allocates sk_buff for frame
40  *	@sk:  socket to allocate frame to
41  *	@dev: network device this skb will be sent over
42  *	@type: pdu type to allocate
43  *	@data_size: data size to allocate
44  *
45  *	Allocates an sk_buff for frame and initializes sk_buff fields.
46  *	Returns allocated skb or %NULL when out of memory.
47  */
llc_alloc_frame(struct sock * sk,struct net_device * dev,u8 type,u32 data_size)48 struct sk_buff *llc_alloc_frame(struct sock *sk, struct net_device *dev,
49 				u8 type, u32 data_size)
50 {
51 	int hlen = type == LLC_PDU_TYPE_U ? 3 : 4;
52 	struct sk_buff *skb;
53 
54 	hlen += llc_mac_header_len(dev->type);
55 	skb = alloc_skb(hlen + data_size, GFP_ATOMIC);
56 
57 	if (skb) {
58 		skb_reset_mac_header(skb);
59 		skb_reserve(skb, hlen);
60 		skb_reset_network_header(skb);
61 		skb_reset_transport_header(skb);
62 		skb->protocol = htons(ETH_P_802_2);
63 		skb->dev      = dev;
64 		if (sk != NULL)
65 			skb_set_owner_w(skb, sk);
66 	}
67 	return skb;
68 }
69 
llc_save_primitive(struct sock * sk,struct sk_buff * skb,u8 prim)70 void llc_save_primitive(struct sock *sk, struct sk_buff *skb, u8 prim)
71 {
72 	struct sockaddr_llc *addr;
73 
74        /* save primitive for use by the user. */
75 	addr		  = llc_ui_skb_cb(skb);
76 
77 	memset(addr, 0, sizeof(*addr));
78 	addr->sllc_family = sk->sk_family;
79 	addr->sllc_arphrd = skb->dev->type;
80 	addr->sllc_test   = prim == LLC_TEST_PRIM;
81 	addr->sllc_xid    = prim == LLC_XID_PRIM;
82 	addr->sllc_ua     = prim == LLC_DATAUNIT_PRIM;
83 	llc_pdu_decode_sa(skb, addr->sllc_mac);
84 	llc_pdu_decode_ssap(skb, &addr->sllc_sap);
85 }
86 
87 /**
88  *	llc_sap_rtn_pdu - Informs upper layer on rx of an UI, XID or TEST pdu.
89  *	@sap: pointer to SAP
90  *	@skb: received pdu
91  */
llc_sap_rtn_pdu(struct llc_sap * sap,struct sk_buff * skb)92 void llc_sap_rtn_pdu(struct llc_sap *sap, struct sk_buff *skb)
93 {
94 	struct llc_sap_state_ev *ev = llc_sap_ev(skb);
95 	struct llc_pdu_un *pdu = llc_pdu_un_hdr(skb);
96 
97 	switch (LLC_U_PDU_RSP(pdu)) {
98 	case LLC_1_PDU_CMD_TEST:
99 		ev->prim = LLC_TEST_PRIM;	break;
100 	case LLC_1_PDU_CMD_XID:
101 		ev->prim = LLC_XID_PRIM;	break;
102 	case LLC_1_PDU_CMD_UI:
103 		ev->prim = LLC_DATAUNIT_PRIM;	break;
104 	}
105 	ev->ind_cfm_flag = LLC_IND;
106 }
107 
108 /**
109  *	llc_find_sap_trans - finds transition for event
110  *	@sap: pointer to SAP
111  *	@skb: happened event
112  *
113  *	This function finds transition that matches with happened event.
114  *	Returns the pointer to found transition on success or %NULL for
115  *	failure.
116  */
llc_find_sap_trans(struct llc_sap * sap,struct sk_buff * skb)117 static struct llc_sap_state_trans *llc_find_sap_trans(struct llc_sap *sap,
118 						      struct sk_buff *skb)
119 {
120 	int i = 0;
121 	struct llc_sap_state_trans *rc = NULL;
122 	struct llc_sap_state_trans **next_trans;
123 	struct llc_sap_state *curr_state = &llc_sap_state_table[sap->state - 1];
124 	/*
125 	 * Search thru events for this state until list exhausted or until
126 	 * its obvious the event is not valid for the current state
127 	 */
128 	for (next_trans = curr_state->transitions; next_trans[i]->ev; i++)
129 		if (!next_trans[i]->ev(sap, skb)) {
130 			rc = next_trans[i]; /* got event match; return it */
131 			break;
132 		}
133 	return rc;
134 }
135 
136 /**
137  *	llc_exec_sap_trans_actions - execute actions related to event
138  *	@sap: pointer to SAP
139  *	@trans: pointer to transition that it's actions must be performed
140  *	@skb: happened event.
141  *
142  *	This function executes actions that is related to happened event.
143  *	Returns 0 for success and 1 for failure of at least one action.
144  */
llc_exec_sap_trans_actions(struct llc_sap * sap,struct llc_sap_state_trans * trans,struct sk_buff * skb)145 static int llc_exec_sap_trans_actions(struct llc_sap *sap,
146 				      struct llc_sap_state_trans *trans,
147 				      struct sk_buff *skb)
148 {
149 	int rc = 0;
150 	const llc_sap_action_t *next_action = trans->ev_actions;
151 
152 	for (; next_action && *next_action; next_action++)
153 		if ((*next_action)(sap, skb))
154 			rc = 1;
155 	return rc;
156 }
157 
158 /**
159  *	llc_sap_next_state - finds transition, execs actions & change SAP state
160  *	@sap: pointer to SAP
161  *	@skb: happened event
162  *
163  *	This function finds transition that matches with happened event, then
164  *	executes related actions and finally changes state of SAP. It returns
165  *	0 on success and 1 for failure.
166  */
llc_sap_next_state(struct llc_sap * sap,struct sk_buff * skb)167 static int llc_sap_next_state(struct llc_sap *sap, struct sk_buff *skb)
168 {
169 	int rc = 1;
170 	struct llc_sap_state_trans *trans;
171 
172 	if (sap->state > LLC_NR_SAP_STATES)
173 		goto out;
174 	trans = llc_find_sap_trans(sap, skb);
175 	if (!trans)
176 		goto out;
177 	/*
178 	 * Got the state to which we next transition; perform the actions
179 	 * associated with this transition before actually transitioning to the
180 	 * next state
181 	 */
182 	rc = llc_exec_sap_trans_actions(sap, trans, skb);
183 	if (rc)
184 		goto out;
185 	/*
186 	 * Transition SAP to next state if all actions execute successfully
187 	 */
188 	sap->state = trans->next_state;
189 out:
190 	return rc;
191 }
192 
193 /**
194  *	llc_sap_state_process - sends event to SAP state machine
195  *	@sap: sap to use
196  *	@skb: pointer to occurred event
197  *
198  *	After executing actions of the event, upper layer will be indicated
199  *	if needed(on receiving an UI frame). sk can be null for the
200  *	datalink_proto case.
201  *
202  *	This function always consumes a reference to the skb.
203  */
llc_sap_state_process(struct llc_sap * sap,struct sk_buff * skb)204 static void llc_sap_state_process(struct llc_sap *sap, struct sk_buff *skb)
205 {
206 	struct llc_sap_state_ev *ev = llc_sap_ev(skb);
207 
208 	ev->ind_cfm_flag = 0;
209 	llc_sap_next_state(sap, skb);
210 
211 	if (ev->ind_cfm_flag == LLC_IND && skb->sk->sk_state != TCP_LISTEN) {
212 		llc_save_primitive(skb->sk, skb, ev->prim);
213 
214 		/* queue skb to the user. */
215 		if (sock_queue_rcv_skb(skb->sk, skb) == 0)
216 			return;
217 	}
218 	kfree_skb(skb);
219 }
220 
221 /**
222  *	llc_build_and_send_test_pkt - TEST interface for upper layers.
223  *	@sap: sap to use
224  *	@skb: packet to send
225  *	@dmac: destination mac address
226  *	@dsap: destination sap
227  *
228  *	This function is called when upper layer wants to send a TEST pdu.
229  *	Returns 0 for success, 1 otherwise.
230  */
llc_build_and_send_test_pkt(struct llc_sap * sap,struct sk_buff * skb,u8 * dmac,u8 dsap)231 void llc_build_and_send_test_pkt(struct llc_sap *sap,
232 				 struct sk_buff *skb, u8 *dmac, u8 dsap)
233 {
234 	struct llc_sap_state_ev *ev = llc_sap_ev(skb);
235 
236 	ev->saddr.lsap = sap->laddr.lsap;
237 	ev->daddr.lsap = dsap;
238 	memcpy(ev->saddr.mac, skb->dev->dev_addr, IFHWADDRLEN);
239 	memcpy(ev->daddr.mac, dmac, IFHWADDRLEN);
240 
241 	ev->type      = LLC_SAP_EV_TYPE_PRIM;
242 	ev->prim      = LLC_TEST_PRIM;
243 	ev->prim_type = LLC_PRIM_TYPE_REQ;
244 	llc_sap_state_process(sap, skb);
245 }
246 
247 /**
248  *	llc_build_and_send_xid_pkt - XID interface for upper layers
249  *	@sap: sap to use
250  *	@skb: packet to send
251  *	@dmac: destination mac address
252  *	@dsap: destination sap
253  *
254  *	This function is called when upper layer wants to send a XID pdu.
255  *	Returns 0 for success, 1 otherwise.
256  */
llc_build_and_send_xid_pkt(struct llc_sap * sap,struct sk_buff * skb,u8 * dmac,u8 dsap)257 void llc_build_and_send_xid_pkt(struct llc_sap *sap, struct sk_buff *skb,
258 				u8 *dmac, u8 dsap)
259 {
260 	struct llc_sap_state_ev *ev = llc_sap_ev(skb);
261 
262 	ev->saddr.lsap = sap->laddr.lsap;
263 	ev->daddr.lsap = dsap;
264 	memcpy(ev->saddr.mac, skb->dev->dev_addr, IFHWADDRLEN);
265 	memcpy(ev->daddr.mac, dmac, IFHWADDRLEN);
266 
267 	ev->type      = LLC_SAP_EV_TYPE_PRIM;
268 	ev->prim      = LLC_XID_PRIM;
269 	ev->prim_type = LLC_PRIM_TYPE_REQ;
270 	llc_sap_state_process(sap, skb);
271 }
272 
273 /**
274  *	llc_sap_rcv - sends received pdus to the sap state machine
275  *	@sap: current sap component structure.
276  *	@skb: received frame.
277  *	@sk:  socket to associate to frame
278  *
279  *	Sends received pdus to the sap state machine.
280  */
llc_sap_rcv(struct llc_sap * sap,struct sk_buff * skb,struct sock * sk)281 static void llc_sap_rcv(struct llc_sap *sap, struct sk_buff *skb,
282 			struct sock *sk)
283 {
284 	struct llc_sap_state_ev *ev = llc_sap_ev(skb);
285 
286 	ev->type   = LLC_SAP_EV_TYPE_PDU;
287 	ev->reason = 0;
288 	skb_orphan(skb);
289 	sock_hold(sk);
290 	skb->sk = sk;
291 	skb->destructor = sock_efree;
292 	llc_sap_state_process(sap, skb);
293 }
294 
llc_dgram_match(const struct llc_sap * sap,const struct llc_addr * laddr,const struct sock * sk)295 static inline bool llc_dgram_match(const struct llc_sap *sap,
296 				   const struct llc_addr *laddr,
297 				   const struct sock *sk)
298 {
299      struct llc_sock *llc = llc_sk(sk);
300 
301      return sk->sk_type == SOCK_DGRAM &&
302 	  llc->laddr.lsap == laddr->lsap &&
303 	  ether_addr_equal(llc->laddr.mac, laddr->mac);
304 }
305 
306 /**
307  *	llc_lookup_dgram - Finds dgram socket for the local sap/mac
308  *	@sap: SAP
309  *	@laddr: address of local LLC (MAC + SAP)
310  *
311  *	Search socket list of the SAP and finds connection using the local
312  *	mac, and local sap. Returns pointer for socket found, %NULL otherwise.
313  */
llc_lookup_dgram(struct llc_sap * sap,const struct llc_addr * laddr)314 static struct sock *llc_lookup_dgram(struct llc_sap *sap,
315 				     const struct llc_addr *laddr)
316 {
317 	struct sock *rc;
318 	struct hlist_nulls_node *node;
319 	int slot = llc_sk_laddr_hashfn(sap, laddr);
320 	struct hlist_nulls_head *laddr_hb = &sap->sk_laddr_hash[slot];
321 
322 	rcu_read_lock_bh();
323 again:
324 	sk_nulls_for_each_rcu(rc, node, laddr_hb) {
325 		if (llc_dgram_match(sap, laddr, rc)) {
326 			/* Extra checks required by SLAB_TYPESAFE_BY_RCU */
327 			if (unlikely(!refcount_inc_not_zero(&rc->sk_refcnt)))
328 				goto again;
329 			if (unlikely(llc_sk(rc)->sap != sap ||
330 				     !llc_dgram_match(sap, laddr, rc))) {
331 				sock_put(rc);
332 				continue;
333 			}
334 			goto found;
335 		}
336 	}
337 	rc = NULL;
338 	/*
339 	 * if the nulls value we got at the end of this lookup is
340 	 * not the expected one, we must restart lookup.
341 	 * We probably met an item that was moved to another chain.
342 	 */
343 	if (unlikely(get_nulls_value(node) != slot))
344 		goto again;
345 found:
346 	rcu_read_unlock_bh();
347 	return rc;
348 }
349 
llc_mcast_match(const struct llc_sap * sap,const struct llc_addr * laddr,const struct sk_buff * skb,const struct sock * sk)350 static inline bool llc_mcast_match(const struct llc_sap *sap,
351 				   const struct llc_addr *laddr,
352 				   const struct sk_buff *skb,
353 				   const struct sock *sk)
354 {
355      struct llc_sock *llc = llc_sk(sk);
356 
357      return sk->sk_type == SOCK_DGRAM &&
358 	  llc->laddr.lsap == laddr->lsap &&
359 	  llc->dev == skb->dev;
360 }
361 
llc_do_mcast(struct llc_sap * sap,struct sk_buff * skb,struct sock ** stack,int count)362 static void llc_do_mcast(struct llc_sap *sap, struct sk_buff *skb,
363 			 struct sock **stack, int count)
364 {
365 	struct sk_buff *skb1;
366 	int i;
367 
368 	for (i = 0; i < count; i++) {
369 		skb1 = skb_clone(skb, GFP_ATOMIC);
370 		if (!skb1) {
371 			sock_put(stack[i]);
372 			continue;
373 		}
374 
375 		llc_sap_rcv(sap, skb1, stack[i]);
376 		sock_put(stack[i]);
377 	}
378 }
379 
380 /**
381  * 	llc_sap_mcast - Deliver multicast PDU's to all matching datagram sockets.
382  *	@sap: SAP
383  *	@laddr: address of local LLC (MAC + SAP)
384  *	@skb: PDU to deliver
385  *
386  *	Search socket list of the SAP and finds connections with same sap.
387  *	Deliver clone to each.
388  */
llc_sap_mcast(struct llc_sap * sap,const struct llc_addr * laddr,struct sk_buff * skb)389 static void llc_sap_mcast(struct llc_sap *sap,
390 			  const struct llc_addr *laddr,
391 			  struct sk_buff *skb)
392 {
393 	int i = 0;
394 	struct sock *sk;
395 	struct sock *stack[256 / sizeof(struct sock *)];
396 	struct llc_sock *llc;
397 	struct hlist_head *dev_hb = llc_sk_dev_hash(sap, skb->dev->ifindex);
398 
399 	spin_lock_bh(&sap->sk_lock);
400 	hlist_for_each_entry(llc, dev_hb, dev_hash_node) {
401 
402 		sk = &llc->sk;
403 
404 		if (!llc_mcast_match(sap, laddr, skb, sk))
405 			continue;
406 
407 		sock_hold(sk);
408 		if (i < ARRAY_SIZE(stack))
409 			stack[i++] = sk;
410 		else {
411 			llc_do_mcast(sap, skb, stack, i);
412 			i = 0;
413 		}
414 	}
415 	spin_unlock_bh(&sap->sk_lock);
416 
417 	llc_do_mcast(sap, skb, stack, i);
418 }
419 
420 
llc_sap_handler(struct llc_sap * sap,struct sk_buff * skb)421 void llc_sap_handler(struct llc_sap *sap, struct sk_buff *skb)
422 {
423 	struct llc_addr laddr;
424 
425 	llc_pdu_decode_da(skb, laddr.mac);
426 	llc_pdu_decode_dsap(skb, &laddr.lsap);
427 
428 	if (is_multicast_ether_addr(laddr.mac)) {
429 		llc_sap_mcast(sap, &laddr, skb);
430 		kfree_skb(skb);
431 	} else {
432 		struct sock *sk = llc_lookup_dgram(sap, &laddr);
433 		if (sk) {
434 			llc_sap_rcv(sap, skb, sk);
435 			sock_put(sk);
436 		} else
437 			kfree_skb(skb);
438 	}
439 }
440