?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
treewide: remove FSF address
[osmocom-bb.git] / src / host / gprsdecode / rlcmac.c
blobdc11fd30d5314133f0873d19bb21e58d4c0c1c48
1 /*
2 * (C) 2017-2018 by sysmocom - s.f.m.c. GmbH, Author: Max <msuraev@sysmocom.de>
3 * (C) 2018 by Vadim Yanitskiy <axilirator@gmail.com>
4 * (C) 2011-2012 by Luca Melette <luca@srlabs.de>
5 *
6 * All Rights Reserved
7 *
8 * SPDX-License-Identifier: GPL-2.0+
9 *
10 * This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU General Public License for more details.
19 *
20 */
21
22 #include <stdio.h>
23 #include <errno.h>
24 #include <string.h>
25 #include <stdint.h>
26 #include <stdlib.h>
27 #include <stdbool.h>
28 #include <sys/time.h>
29
30 #include <osmocom/core/gsmtap.h>
31
32 #include "l1ctl_proto.h"
33 #include "rlcmac.h"
34 #include "gsmtap.h"
35
36 static struct gprs_tbf tbf_table[32 * 2];
37
38 static inline int too_old(uint32_t current_fn, uint32_t test_fn)
39 {
40 uint32_t delta = (current_fn - test_fn) & 0xffffffff;
41
42 /* More and less 30 seconds from now */
43 return abs(delta) > OLD_TIME;
44 }
45
46 static inline int bsn_is_next(uint8_t first, uint8_t second)
47 {
48 return ((first + 1) % 128) == second;
49 }
50
51 void print_pkt(uint8_t *msg, size_t len)
52 {
53 size_t i;
54
55 printf("MSG: ");
56 for (i = 0; i < len; i++)
57 printf("%.02x", msg[i]);
58 printf("\n");
59 }
60
61 void process_blocks(struct gprs_tbf *t, bool ul)
62 {
63 uint8_t llc_data[65536], llc_first_bsn, llc_last_bsn = 0;
64 unsigned skip, llc_len = 0;
65 uint8_t bsn, bsn2, li_off;
66 uint32_t current_fn;
67 struct gprs_frag *f;
68 struct gprs_lime *l;
69
70 /* Get current "time", oldest unreassembled frag */
71 bsn = t->start_bsn;
72 while (t->frags[bsn].len == 0) {
73 bsn = (bsn + 1) % 128;
74 if (bsn == t->start_bsn) {
75 printf("no valid blocks in current TBF!\n");
76 fflush(stdout);
77 return;
78 }
79 }
80 current_fn = t->frags[bsn].fn;
81 t->start_bsn = bsn;
82
83 /* Walk through fragments, mark reassembled/used blocks */
84 skip = 0;
85 for (bsn = t->start_bsn; bsn != ((t->last_bsn + 1) % 128); bsn = (bsn + 1) % 128) {
86 /* Get fragment descriptor */
87 f = &t->frags[bsn];
88
89 printf(" bsn %d ", bsn);
90 fflush(stdout);
91
92 /* Already processed or null */
93 if (!f->len) {
94 printf("null\n");
95 fflush(stdout);
96 llc_len = 0;
97 skip = 1;
98 continue;
99 }
100
101 /* Check fragment age */
102 if (too_old(current_fn, f->fn)) {
103 printf("old segment\n");
104 fflush(stdout);
105 llc_len = 0;
106 skip = 1;
107 continue;
108 }
109
110 /* Update "time" */
111 current_fn = f->fn;
112
113 if (llc_len && !bsn_is_next(llc_last_bsn, bsn)) {
114 printf("missing bsn, previous %d\n", llc_last_bsn);
115 fflush(stdout);
116 llc_len = 0;
117 skip = 1;
118 continue;
119 }
120
121 /* Check for multiple blocks/parts */
122 if (f->n_blocks == 0) {
123 /* Check if first part of message */
124 if (!llc_len)
125 llc_first_bsn = bsn;
126
127 /* Append data to buffer */
128 memcpy(&llc_data[llc_len], f->data, f->len);
129
130 llc_len += f->len;
131
132 llc_last_bsn = bsn;
133
134 /* Last TBF block? (very rare condition) */
135 if (f->last) {
136 printf("end of TBF\n");
137 fflush(stdout);
138 print_pkt(llc_data, llc_len);
139
140 gsmtap_send_llc(llc_data, llc_len, ul);
141
142 /* Reset all fragments */
143 for (bsn2 = 0; bsn2 < 128; bsn2++) {
144 f = &t->frags[bsn2];
145 f->len = 0;
146 f->n_blocks = 0;
147 }
148
149 /* Reset buffer state */
150 llc_len = 0;
151 t->start_bsn = 0;
152 }
153 } else {
154 /* Multiple data parts */
155 unsigned i;
156 li_off = 0;
157 for (i = 0; i < f->n_blocks; i++) {
158 printf("\nlime %d\n", i);
159 fflush(stdout);
160 l = &f->blocks[i];
161 if (l->used) {
162 if (llc_len) {
163 printf("\nlime error!\n");
164 fflush(stdout);
165 llc_len = 0;
166 }
167 } else {
168 if (!llc_len)
169 llc_first_bsn = bsn;
170
171 /* Append data to buffer */
172 memcpy(&llc_data[llc_len], &f->data[li_off], l->li);
173
174 llc_len += l->li;
175
176 llc_last_bsn = bsn;
177
178 if (!l->e || !l->m || (l->e && l->m)) {
179 /* Message ends here */
180 printf("end of message reached\n");
181 fflush(stdout);
182 print_pkt(llc_data, llc_len);
183
184 gsmtap_send_llc(llc_data, llc_len, ul);
185
186 /* Mark frags as used */
187 l->used = 1;
188 if (llc_first_bsn != bsn) {
189 }
190
191
192 llc_len = 0;
193 if (!skip)
194 t->start_bsn = bsn;
195 }
196 }
197
198 li_off += l->li;
199 }
200
201 /* Is spare data valid? */
202 if (l->m) {
203 if (llc_len) {
204 printf("spare and buffer not empty!\n");
205 print_pkt(llc_data, llc_len);
206 fflush(stdout);
207 }
208 if ((f->len > li_off) && (f->len-li_off < 65536)) {
209 memcpy(llc_data, &f->data[li_off], f->len-li_off);
210 llc_len = f->len - li_off;
211 llc_first_bsn = bsn;
212 llc_last_bsn = bsn;
213 t->start_bsn = bsn;
214 }
215 }
216
217 }
218 }
219
220 /* Shift window if needed */
221 if (((t->last_bsn - t->start_bsn) % 128) > 64) {
222 t->start_bsn = (t->last_bsn - 64) % 128;
223 printf("shifting window\n");
224 fflush(stdout);
225 }
226 }
227
228 void rlc_data_handler(struct gprs_message *gm)
229 {
230 int ul, off, d_bsn;
231 uint8_t tfi, bsn, cv = 1, fbi = 0;
232 uint32_t d_same_bsn, d_last_bsn;
233 struct gprs_tbf *t, *t_prev;
234 struct gprs_frag *f;
235 struct gprs_lime *l;
236
237 tfi = (gm->msg[1] & 0x3e) >> 1;
238 bsn = (gm->msg[2] & 0xfe) >> 1;
239
240 /* Get "end of TBF" according to direction */
241 ul = !!(gm->arfcn & GSMTAP_ARFCN_F_UPLINK);
242 if (ul) {
243 cv = (gm->msg[0] & 0x3c) >> 2;
244 printf("TFI %d BSN %d CV %d ", tfi, bsn, cv);
245 } else {
246 fbi = (gm->msg[1] & 0x01);
247 printf("TFI %d BSN %d FBI %d ", tfi, bsn, fbi);
248 }
249
250 /* Get TBF descriptor for TFI,UL couple */
251 t = &tbf_table[2 * tfi + ul];
252
253 d_same_bsn = (gm->fn - t->frags[bsn].fn) & 0xffffffff;
254 d_last_bsn = (gm->fn - t->frags[t->last_bsn].fn) & 0xffffffff;
255 d_bsn = (bsn - t->last_bsn) % 128;
256
257 printf("\nfn_same_bsn %d fn_last_bsn %d delta_bsn %d old_len %d\n",
258 d_same_bsn, d_last_bsn, d_bsn, t->frags[bsn].len);
259
260 /* New / old fragment decision */
261 if (d_same_bsn > OLD_TIME) {
262 if (d_last_bsn > OLD_TIME) {
263 /* New TBF is starting, close old one... */
264 t_prev = &tbf_table[2 * ((tfi + 1) % 32) + ul];
265 printf("clearing TBF %d, first %d last %d\n",
266 (tfi + 1) % 32, t_prev->start_bsn, t_prev->last_bsn);
267 f = &t_prev->frags[t_prev->last_bsn];
268
269 /* ...only if data is present */
270 if (f->len) {
271 f->last = 1;
272 process_blocks(t_prev, ul);
273 }
274
275 printf("new TBF, starting from %d\n", bsn);
276 t->start_bsn = 0;
277 t->last_bsn = bsn;
278 memset(t->frags, 0, 128 * sizeof(struct gprs_frag));
279 } else {
280 /* Fresh frag, current TBF */
281 if ((d_bsn >= 0) || (d_bsn < -64)) {
282 /* New frag */
283 t->last_bsn = bsn;
284 } else {
285 /* Out of sequence / duplicate */
286 t->frags[bsn].fn = gm->fn;
287 printf("duplicate\n");
288 fflush(stdout);
289 return;
290 }
291 }
292 } else {
293 if (d_last_bsn > OLD_TIME) {
294 printf("fucking error last_bsn!\n");
295 fflush(stdout);
296 return;
297 } else {
298 /* Fresh frag, current TBF */
299 if (d_bsn > 0) {
300 printf("fucking error d_bsn!\n");
301 fflush(stdout);
302 return;
303 } else {
304 if (d_bsn < -64) {
305 /* New frag */
306 t->last_bsn = bsn;
307 } else {
308 /* Duplicate */
309 t->frags[bsn].fn = gm->fn;
310 printf("duplicate2\n");
311 fflush(stdout);
312 return;
313 }
314 }
315 }
316 }
317
318 /* Get fragment struct for current BSN */
319 f = &t->frags[bsn];
320
321 /* Scan for LI_M_E entries */
322 off = 2;
323 f->n_blocks = 0;
324 while (!(gm->msg[off++] & 0x01)) {
325 l = &f->blocks[f->n_blocks++];
326 l->li = (gm->msg[off] & 0xfc) >> 2;
327 l->m = (gm->msg[off] & 0x02) >> 1;
328 l->e = (gm->msg[off] & 0x01);
329 l->used = 0;
330 }
331
332 /* End of TBF? */
333 f->last = (!cv || fbi) ? 1 : 0;
334
335 /* Optional fields for uplink, indicated in TI and PI */
336 if (ul) {
337 if (gm->msg[1] & 0x01) {
338 printf("TLLI 0x%.02x%.02x%.02x%.02x ", gm->msg[off],
339 gm->msg[off+1], gm->msg[off + 2], gm->msg[off + 3]);
340 off += 4;
341 }
342 if (gm->msg[1] & 0x40) {
343 printf("PFI %d ", gm->msg[off]);
344 off += 1;
345 }
346 }
347
348 /* Copy data part of message */
349 f->len = gm->len - off;
350 f->fn = gm->fn;
351 memcpy(f->data, &gm->msg[off], f->len);
352
353 process_blocks(t, ul);
354 }
355
356 int rlc_type_handler(struct gprs_message *gm)
357 {
358 bool ul = !!(gm->arfcn & GSMTAP_ARFCN_F_UPLINK);
359 uint8_t rlc_type = (gm->msg[0] & 0xc0) >> 6;
360 int rc = 0;
361
362 /* Determine the RLC type */
363 switch (rlc_type) {
364 case 0:
365 printf("TS %d ", gm->tn);
366
367 switch(gm->len) {
368 case 23:
369 printf("CS1 ");
370 break;
371 case 33:
372 printf("CS2 ");
373 break;
374 case 39:
375 printf("CS3 ");
376 break;
377 case 53:
378 printf("CS4 ");
379 break;
380 default:
381 printf("unknown (M)CS ");
382 }
383
384 printf(ul ? "UL " : "DL ");
385
386 gsmtap_send_rlcmac(gm->msg, gm->len, gm->tn, ul);
387
388 printf("DATA ");
389 rlc_data_handler(gm);
390 printf("\n");
391 fflush(stdout);
392 break;
393
394 /* Control block */
395 case 1:
396 case 2:
397 gsmtap_send_rlcmac(gm->msg, gm->len, gm->tn,
398 !!(gm->arfcn & GSMTAP_ARFCN_F_UPLINK));
399 rc = 0;
400 break;
401
402 /* Reserved */
403 case 3:
404 printf("RLC type: reserved\n");
405 rc = 0;
406 break;
407
408 default:
409 printf("Unrecognized RLC type: %d\n", rlc_type);
410 return -EINVAL;
411 }
412
413 return rc;
414 }
osmocom-bb mirror