1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | /* |
3 | * LAPB release 002 |
4 | * |
5 | * This code REQUIRES 2.1.15 or higher/ NET3.038 |
6 | * |
7 | * History |
8 | * LAPB 001 Jonathan Naylor Started Coding |
9 | */ |
10 | |
11 | #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt |
12 | |
13 | #include <linux/errno.h> |
14 | #include <linux/types.h> |
15 | #include <linux/socket.h> |
16 | #include <linux/in.h> |
17 | #include <linux/kernel.h> |
18 | #include <linux/timer.h> |
19 | #include <linux/string.h> |
20 | #include <linux/sockios.h> |
21 | #include <linux/net.h> |
22 | #include <linux/inet.h> |
23 | #include <linux/skbuff.h> |
24 | #include <linux/slab.h> |
25 | #include <net/sock.h> |
26 | #include <linux/uaccess.h> |
27 | #include <linux/fcntl.h> |
28 | #include <linux/mm.h> |
29 | #include <linux/interrupt.h> |
30 | #include <net/lapb.h> |
31 | |
32 | /* |
33 | * This routine purges all the queues of frames. |
34 | */ |
35 | void lapb_clear_queues(struct lapb_cb *lapb) |
36 | { |
37 | skb_queue_purge(list: &lapb->write_queue); |
38 | skb_queue_purge(list: &lapb->ack_queue); |
39 | } |
40 | |
41 | /* |
42 | * This routine purges the input queue of those frames that have been |
43 | * acknowledged. This replaces the boxes labelled "V(a) <- N(r)" on the |
44 | * SDL diagram. |
45 | */ |
46 | void lapb_frames_acked(struct lapb_cb *lapb, unsigned short nr) |
47 | { |
48 | struct sk_buff *skb; |
49 | int modulus; |
50 | |
51 | modulus = (lapb->mode & LAPB_EXTENDED) ? LAPB_EMODULUS : LAPB_SMODULUS; |
52 | |
53 | /* |
54 | * Remove all the ack-ed frames from the ack queue. |
55 | */ |
56 | if (lapb->va != nr) |
57 | while (skb_peek(list_: &lapb->ack_queue) && lapb->va != nr) { |
58 | skb = skb_dequeue(list: &lapb->ack_queue); |
59 | kfree_skb(skb); |
60 | lapb->va = (lapb->va + 1) % modulus; |
61 | } |
62 | } |
63 | |
64 | void lapb_requeue_frames(struct lapb_cb *lapb) |
65 | { |
66 | struct sk_buff *skb, *skb_prev = NULL; |
67 | |
68 | /* |
69 | * Requeue all the un-ack-ed frames on the output queue to be picked |
70 | * up by lapb_kick called from the timer. This arrangement handles the |
71 | * possibility of an empty output queue. |
72 | */ |
73 | while ((skb = skb_dequeue(list: &lapb->ack_queue)) != NULL) { |
74 | if (!skb_prev) |
75 | skb_queue_head(list: &lapb->write_queue, newsk: skb); |
76 | else |
77 | skb_append(old: skb_prev, newsk: skb, list: &lapb->write_queue); |
78 | skb_prev = skb; |
79 | } |
80 | } |
81 | |
82 | /* |
83 | * Validate that the value of nr is between va and vs. Return true or |
84 | * false for testing. |
85 | */ |
86 | int lapb_validate_nr(struct lapb_cb *lapb, unsigned short nr) |
87 | { |
88 | unsigned short vc = lapb->va; |
89 | int modulus; |
90 | |
91 | modulus = (lapb->mode & LAPB_EXTENDED) ? LAPB_EMODULUS : LAPB_SMODULUS; |
92 | |
93 | while (vc != lapb->vs) { |
94 | if (nr == vc) |
95 | return 1; |
96 | vc = (vc + 1) % modulus; |
97 | } |
98 | |
99 | return nr == lapb->vs; |
100 | } |
101 | |
102 | /* |
103 | * This routine is the centralised routine for parsing the control |
104 | * information for the different frame formats. |
105 | */ |
106 | int lapb_decode(struct lapb_cb *lapb, struct sk_buff *skb, |
107 | struct lapb_frame *frame) |
108 | { |
109 | frame->type = LAPB_ILLEGAL; |
110 | |
111 | lapb_dbg(2, "(%p) S%d RX %3ph\n" , lapb->dev, lapb->state, skb->data); |
112 | |
113 | /* We always need to look at 2 bytes, sometimes we need |
114 | * to look at 3 and those cases are handled below. |
115 | */ |
116 | if (!pskb_may_pull(skb, len: 2)) |
117 | return -1; |
118 | |
119 | if (lapb->mode & LAPB_MLP) { |
120 | if (lapb->mode & LAPB_DCE) { |
121 | if (skb->data[0] == LAPB_ADDR_D) |
122 | frame->cr = LAPB_COMMAND; |
123 | if (skb->data[0] == LAPB_ADDR_C) |
124 | frame->cr = LAPB_RESPONSE; |
125 | } else { |
126 | if (skb->data[0] == LAPB_ADDR_C) |
127 | frame->cr = LAPB_COMMAND; |
128 | if (skb->data[0] == LAPB_ADDR_D) |
129 | frame->cr = LAPB_RESPONSE; |
130 | } |
131 | } else { |
132 | if (lapb->mode & LAPB_DCE) { |
133 | if (skb->data[0] == LAPB_ADDR_B) |
134 | frame->cr = LAPB_COMMAND; |
135 | if (skb->data[0] == LAPB_ADDR_A) |
136 | frame->cr = LAPB_RESPONSE; |
137 | } else { |
138 | if (skb->data[0] == LAPB_ADDR_A) |
139 | frame->cr = LAPB_COMMAND; |
140 | if (skb->data[0] == LAPB_ADDR_B) |
141 | frame->cr = LAPB_RESPONSE; |
142 | } |
143 | } |
144 | |
145 | skb_pull(skb, len: 1); |
146 | |
147 | if (lapb->mode & LAPB_EXTENDED) { |
148 | if (!(skb->data[0] & LAPB_S)) { |
149 | if (!pskb_may_pull(skb, len: 2)) |
150 | return -1; |
151 | /* |
152 | * I frame - carries NR/NS/PF |
153 | */ |
154 | frame->type = LAPB_I; |
155 | frame->ns = (skb->data[0] >> 1) & 0x7F; |
156 | frame->nr = (skb->data[1] >> 1) & 0x7F; |
157 | frame->pf = skb->data[1] & LAPB_EPF; |
158 | frame->control[0] = skb->data[0]; |
159 | frame->control[1] = skb->data[1]; |
160 | skb_pull(skb, len: 2); |
161 | } else if ((skb->data[0] & LAPB_U) == 1) { |
162 | if (!pskb_may_pull(skb, len: 2)) |
163 | return -1; |
164 | /* |
165 | * S frame - take out PF/NR |
166 | */ |
167 | frame->type = skb->data[0] & 0x0F; |
168 | frame->nr = (skb->data[1] >> 1) & 0x7F; |
169 | frame->pf = skb->data[1] & LAPB_EPF; |
170 | frame->control[0] = skb->data[0]; |
171 | frame->control[1] = skb->data[1]; |
172 | skb_pull(skb, len: 2); |
173 | } else if ((skb->data[0] & LAPB_U) == 3) { |
174 | /* |
175 | * U frame - take out PF |
176 | */ |
177 | frame->type = skb->data[0] & ~LAPB_SPF; |
178 | frame->pf = skb->data[0] & LAPB_SPF; |
179 | frame->control[0] = skb->data[0]; |
180 | frame->control[1] = 0x00; |
181 | skb_pull(skb, len: 1); |
182 | } |
183 | } else { |
184 | if (!(skb->data[0] & LAPB_S)) { |
185 | /* |
186 | * I frame - carries NR/NS/PF |
187 | */ |
188 | frame->type = LAPB_I; |
189 | frame->ns = (skb->data[0] >> 1) & 0x07; |
190 | frame->nr = (skb->data[0] >> 5) & 0x07; |
191 | frame->pf = skb->data[0] & LAPB_SPF; |
192 | } else if ((skb->data[0] & LAPB_U) == 1) { |
193 | /* |
194 | * S frame - take out PF/NR |
195 | */ |
196 | frame->type = skb->data[0] & 0x0F; |
197 | frame->nr = (skb->data[0] >> 5) & 0x07; |
198 | frame->pf = skb->data[0] & LAPB_SPF; |
199 | } else if ((skb->data[0] & LAPB_U) == 3) { |
200 | /* |
201 | * U frame - take out PF |
202 | */ |
203 | frame->type = skb->data[0] & ~LAPB_SPF; |
204 | frame->pf = skb->data[0] & LAPB_SPF; |
205 | } |
206 | |
207 | frame->control[0] = skb->data[0]; |
208 | |
209 | skb_pull(skb, len: 1); |
210 | } |
211 | |
212 | return 0; |
213 | } |
214 | |
215 | /* |
216 | * This routine is called when the HDLC layer internally generates a |
217 | * command or response for the remote machine ( eg. RR, UA etc. ). |
218 | * Only supervisory or unnumbered frames are processed, FRMRs are handled |
219 | * by lapb_transmit_frmr below. |
220 | */ |
221 | void lapb_send_control(struct lapb_cb *lapb, int frametype, |
222 | int poll_bit, int type) |
223 | { |
224 | struct sk_buff *skb; |
225 | unsigned char *dptr; |
226 | |
227 | if ((skb = alloc_skb(LAPB_HEADER_LEN + 3, GFP_ATOMIC)) == NULL) |
228 | return; |
229 | |
230 | skb_reserve(skb, LAPB_HEADER_LEN + 1); |
231 | |
232 | if (lapb->mode & LAPB_EXTENDED) { |
233 | if ((frametype & LAPB_U) == LAPB_U) { |
234 | dptr = skb_put(skb, len: 1); |
235 | *dptr = frametype; |
236 | *dptr |= poll_bit ? LAPB_SPF : 0; |
237 | } else { |
238 | dptr = skb_put(skb, len: 2); |
239 | dptr[0] = frametype; |
240 | dptr[1] = (lapb->vr << 1); |
241 | dptr[1] |= poll_bit ? LAPB_EPF : 0; |
242 | } |
243 | } else { |
244 | dptr = skb_put(skb, len: 1); |
245 | *dptr = frametype; |
246 | *dptr |= poll_bit ? LAPB_SPF : 0; |
247 | if ((frametype & LAPB_U) == LAPB_S) /* S frames carry NR */ |
248 | *dptr |= (lapb->vr << 5); |
249 | } |
250 | |
251 | lapb_transmit_buffer(lapb, skb, type); |
252 | } |
253 | |
254 | /* |
255 | * This routine generates FRMRs based on information previously stored in |
256 | * the LAPB control block. |
257 | */ |
258 | void lapb_transmit_frmr(struct lapb_cb *lapb) |
259 | { |
260 | struct sk_buff *skb; |
261 | unsigned char *dptr; |
262 | |
263 | if ((skb = alloc_skb(LAPB_HEADER_LEN + 7, GFP_ATOMIC)) == NULL) |
264 | return; |
265 | |
266 | skb_reserve(skb, LAPB_HEADER_LEN + 1); |
267 | |
268 | if (lapb->mode & LAPB_EXTENDED) { |
269 | dptr = skb_put(skb, len: 6); |
270 | *dptr++ = LAPB_FRMR; |
271 | *dptr++ = lapb->frmr_data.control[0]; |
272 | *dptr++ = lapb->frmr_data.control[1]; |
273 | *dptr++ = (lapb->vs << 1) & 0xFE; |
274 | *dptr = (lapb->vr << 1) & 0xFE; |
275 | if (lapb->frmr_data.cr == LAPB_RESPONSE) |
276 | *dptr |= 0x01; |
277 | dptr++; |
278 | *dptr++ = lapb->frmr_type; |
279 | |
280 | lapb_dbg(1, "(%p) S%d TX FRMR %5ph\n" , |
281 | lapb->dev, lapb->state, |
282 | &skb->data[1]); |
283 | } else { |
284 | dptr = skb_put(skb, len: 4); |
285 | *dptr++ = LAPB_FRMR; |
286 | *dptr++ = lapb->frmr_data.control[0]; |
287 | *dptr = (lapb->vs << 1) & 0x0E; |
288 | *dptr |= (lapb->vr << 5) & 0xE0; |
289 | if (lapb->frmr_data.cr == LAPB_RESPONSE) |
290 | *dptr |= 0x10; |
291 | dptr++; |
292 | *dptr++ = lapb->frmr_type; |
293 | |
294 | lapb_dbg(1, "(%p) S%d TX FRMR %3ph\n" , |
295 | lapb->dev, lapb->state, &skb->data[1]); |
296 | } |
297 | |
298 | lapb_transmit_buffer(lapb, skb, LAPB_RESPONSE); |
299 | } |
300 | |