1
2
3
4
5
6
7
8
9
10
11
12
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
27static int llc_mac_header_len(unsigned short devtype)
28{
29 switch (devtype) {
30 case ARPHRD_ETHER:
31 case ARPHRD_LOOPBACK:
32 return sizeof(struct ethhdr);
33#ifdef CONFIG_TR
34 case ARPHRD_IEEE802_TR:
35 return sizeof(struct trh_hdr);
36#endif
37 }
38 return 0;
39}
40
41
42
43
44
45
46
47
48
49
50struct sk_buff *llc_alloc_frame(struct sock *sk, struct net_device *dev,
51 u8 type, u32 data_size)
52{
53 int hlen = type == LLC_PDU_TYPE_U ? 3 : 4;
54 struct sk_buff *skb;
55
56 hlen += llc_mac_header_len(dev->type);
57 skb = alloc_skb(hlen + data_size, GFP_ATOMIC);
58
59 if (skb) {
60 skb_reset_mac_header(skb);
61 skb_reserve(skb, hlen);
62 skb_reset_network_header(skb);
63 skb_reset_transport_header(skb);
64 skb->protocol = htons(ETH_P_802_2);
65 skb->dev = dev;
66 if (sk != NULL)
67 skb_set_owner_w(skb, sk);
68 }
69 return skb;
70}
71
72void llc_save_primitive(struct sock *sk, struct sk_buff* skb, u8 prim)
73{
74 struct sockaddr_llc *addr;
75
76
77 addr = llc_ui_skb_cb(skb);
78
79 memset(addr, 0, sizeof(*addr));
80 addr->sllc_family = sk->sk_family;
81 addr->sllc_arphrd = skb->dev->type;
82 addr->sllc_test = prim == LLC_TEST_PRIM;
83 addr->sllc_xid = prim == LLC_XID_PRIM;
84 addr->sllc_ua = prim == LLC_DATAUNIT_PRIM;
85 llc_pdu_decode_sa(skb, addr->sllc_mac);
86 llc_pdu_decode_ssap(skb, &addr->sllc_sap);
87}
88
89
90
91
92
93
94void llc_sap_rtn_pdu(struct llc_sap *sap, struct sk_buff *skb)
95{
96 struct llc_sap_state_ev *ev = llc_sap_ev(skb);
97 struct llc_pdu_un *pdu = llc_pdu_un_hdr(skb);
98
99 switch (LLC_U_PDU_RSP(pdu)) {
100 case LLC_1_PDU_CMD_TEST:
101 ev->prim = LLC_TEST_PRIM; break;
102 case LLC_1_PDU_CMD_XID:
103 ev->prim = LLC_XID_PRIM; break;
104 case LLC_1_PDU_CMD_UI:
105 ev->prim = LLC_DATAUNIT_PRIM; break;
106 }
107 ev->ind_cfm_flag = LLC_IND;
108}
109
110
111
112
113
114
115
116
117
118
119static struct llc_sap_state_trans *llc_find_sap_trans(struct llc_sap *sap,
120 struct sk_buff* skb)
121{
122 int i = 0;
123 struct llc_sap_state_trans *rc = NULL;
124 struct llc_sap_state_trans **next_trans;
125 struct llc_sap_state *curr_state = &llc_sap_state_table[sap->state - 1];
126
127
128
129
130 for (next_trans = curr_state->transitions; next_trans[i]->ev; i++)
131 if (!next_trans[i]->ev(sap, skb)) {
132 rc = next_trans[i];
133 break;
134 }
135 return rc;
136}
137
138
139
140
141
142
143
144
145
146
147static int llc_exec_sap_trans_actions(struct llc_sap *sap,
148 struct llc_sap_state_trans *trans,
149 struct sk_buff *skb)
150{
151 int rc = 0;
152 llc_sap_action_t *next_action = trans->ev_actions;
153
154 for (; next_action && *next_action; next_action++)
155 if ((*next_action)(sap, skb))
156 rc = 1;
157 return rc;
158}
159
160
161
162
163
164
165
166
167
168
169static int llc_sap_next_state(struct llc_sap *sap, struct sk_buff *skb)
170{
171 int rc = 1;
172 struct llc_sap_state_trans *trans;
173
174 if (sap->state > LLC_NR_SAP_STATES)
175 goto out;
176 trans = llc_find_sap_trans(sap, skb);
177 if (!trans)
178 goto out;
179
180
181
182
183
184 rc = llc_exec_sap_trans_actions(sap, trans, skb);
185 if (rc)
186 goto out;
187
188
189
190 sap->state = trans->next_state;
191out:
192 return rc;
193}
194
195
196
197
198
199
200
201
202
203
204static 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
209
210
211
212
213 skb_get(skb);
214 ev->ind_cfm_flag = 0;
215 llc_sap_next_state(sap, skb);
216 if (ev->ind_cfm_flag == LLC_IND) {
217 if (skb->sk->sk_state == TCP_LISTEN)
218 kfree_skb(skb);
219 else {
220 llc_save_primitive(skb->sk, skb, ev->prim);
221
222
223 if (sock_queue_rcv_skb(skb->sk, skb))
224 kfree_skb(skb);
225 }
226 }
227 kfree_skb(skb);
228}
229
230
231
232
233
234
235
236
237
238
239
240void llc_build_and_send_test_pkt(struct llc_sap *sap,
241 struct sk_buff *skb, u8 *dmac, u8 dsap)
242{
243 struct llc_sap_state_ev *ev = llc_sap_ev(skb);
244
245 ev->saddr.lsap = sap->laddr.lsap;
246 ev->daddr.lsap = dsap;
247 memcpy(ev->saddr.mac, skb->dev->dev_addr, IFHWADDRLEN);
248 memcpy(ev->daddr.mac, dmac, IFHWADDRLEN);
249
250 ev->type = LLC_SAP_EV_TYPE_PRIM;
251 ev->prim = LLC_TEST_PRIM;
252 ev->prim_type = LLC_PRIM_TYPE_REQ;
253 llc_sap_state_process(sap, skb);
254}
255
256
257
258
259
260
261
262
263
264
265
266void llc_build_and_send_xid_pkt(struct llc_sap *sap, struct sk_buff *skb,
267 u8 *dmac, u8 dsap)
268{
269 struct llc_sap_state_ev *ev = llc_sap_ev(skb);
270
271 ev->saddr.lsap = sap->laddr.lsap;
272 ev->daddr.lsap = dsap;
273 memcpy(ev->saddr.mac, skb->dev->dev_addr, IFHWADDRLEN);
274 memcpy(ev->daddr.mac, dmac, IFHWADDRLEN);
275
276 ev->type = LLC_SAP_EV_TYPE_PRIM;
277 ev->prim = LLC_XID_PRIM;
278 ev->prim_type = LLC_PRIM_TYPE_REQ;
279 llc_sap_state_process(sap, skb);
280}
281
282
283
284
285
286
287
288
289static void llc_sap_rcv(struct llc_sap *sap, struct sk_buff *skb,
290 struct sock *sk)
291{
292 struct llc_sap_state_ev *ev = llc_sap_ev(skb);
293
294 ev->type = LLC_SAP_EV_TYPE_PDU;
295 ev->reason = 0;
296 skb->sk = sk;
297 llc_sap_state_process(sap, skb);
298}
299
300
301
302
303
304
305
306
307
308static struct sock *llc_lookup_dgram(struct llc_sap *sap,
309 const struct llc_addr *laddr)
310{
311 struct sock *rc;
312 struct hlist_node *node;
313
314 read_lock_bh(&sap->sk_list.lock);
315 sk_for_each(rc, node, &sap->sk_list.list) {
316 struct llc_sock *llc = llc_sk(rc);
317
318 if (rc->sk_type == SOCK_DGRAM &&
319 llc->laddr.lsap == laddr->lsap &&
320 llc_mac_match(llc->laddr.mac, laddr->mac)) {
321 sock_hold(rc);
322 goto found;
323 }
324 }
325 rc = NULL;
326found:
327 read_unlock_bh(&sap->sk_list.lock);
328 return rc;
329}
330
331
332
333
334
335
336
337
338
339static void llc_sap_mcast(struct llc_sap *sap,
340 const struct llc_addr *laddr,
341 struct sk_buff *skb)
342{
343 struct sock *sk;
344 struct hlist_node *node;
345
346 read_lock_bh(&sap->sk_list.lock);
347 sk_for_each(sk, node, &sap->sk_list.list) {
348 struct llc_sock *llc = llc_sk(sk);
349 struct sk_buff *skb1;
350
351 if (sk->sk_type != SOCK_DGRAM)
352 continue;
353
354 if (llc->laddr.lsap != laddr->lsap)
355 continue;
356
357 if (llc->dev != skb->dev)
358 continue;
359
360 skb1 = skb_clone(skb, GFP_ATOMIC);
361 if (!skb1)
362 break;
363
364 sock_hold(sk);
365 llc_sap_rcv(sap, skb1, sk);
366 sock_put(sk);
367 }
368 read_unlock_bh(&sap->sk_list.lock);
369}
370
371
372void llc_sap_handler(struct llc_sap *sap, struct sk_buff *skb)
373{
374 struct llc_addr laddr;
375
376 llc_pdu_decode_da(skb, laddr.mac);
377 llc_pdu_decode_dsap(skb, &laddr.lsap);
378
379 if (llc_mac_multicast(laddr.mac)) {
380 llc_sap_mcast(sap, &laddr, skb);
381 kfree_skb(skb);
382 } else {
383 struct sock *sk = llc_lookup_dgram(sap, &laddr);
384 if (sk) {
385 llc_sap_rcv(sap, skb, sk);
386 sock_put(sk);
387 } else
388 kfree_skb(skb);
389 }
390}
391