blob: 2934b58cacdcbdc11b796314a640dc8027ad07f1 [file] [log] [blame]
Harald Welted38f1052011-02-05 19:13:00 +01001/* OpenBSC minimal LAPD implementation */
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +01002
Harald Welted38f1052011-02-05 19:13:00 +01003/* (C) 2009 by oystein@homelien.no
Harald Welted38f1052011-02-05 19:13:00 +01004 * (C) 2009 by Holger Hans Peter Freyther <zecke@selfish.org>
5 * (C) 2010 by Digium and Matthew Fredrickson <creslin@digium.com>
Harald Weltedcf42e62011-02-13 11:58:21 +01006 * (C) 2011 by Harald Welte <laforge@gnumonks.org>
Harald Welted38f1052011-02-05 19:13:00 +01007 *
8 * All Rights Reserved
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 * You should have received a copy of the GNU General Public License along
21 * with this program; if not, write to the Free Software Foundation, Inc.,
22 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
23 *
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +010024 */
25
Harald Weltedcf42e62011-02-13 11:58:21 +010026/* TODO:
27 * detect RR timeout and set SAP state back to SABM_RETRANSMIT
28 * use of value_string
29 * further code cleanup (spaghetti)
30 */
31
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +010032#include <stdio.h>
33#include <string.h>
Harald Welte1a00d822011-02-11 18:34:51 +010034#include <errno.h>
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +010035
36#include "lapd.h"
Harald Welted38f1052011-02-05 19:13:00 +010037
Pablo Neira Ayuso136f4532011-03-22 16:47:59 +010038#include <osmocom/core/linuxlist.h>
39#include <osmocom/core/talloc.h>
40#include <osmocom/core/msgb.h>
41#include <osmocom/core/timer.h>
Harald Welted38f1052011-02-05 19:13:00 +010042#include <openbsc/debug.h>
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +010043
Harald Weltedcf42e62011-02-13 11:58:21 +010044#define SABM_INTERVAL 0, 300000
45
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +010046typedef enum {
Harald Welte30fe6412011-02-04 20:34:08 +010047 LAPD_TEI_NONE = 0,
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +010048 LAPD_TEI_ASSIGNED,
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +010049 LAPD_TEI_ACTIVE,
50} lapd_tei_state;
51
52const char *lapd_tei_states[] = {
53 "NONE",
54 "ASSIGNED",
55 "ACTIVE",
56};
57
58typedef enum {
Harald Welte30fe6412011-02-04 20:34:08 +010059 LAPD_TYPE_NONE = 0,
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +010060
61 LAPD_TYPE_I,
62 LAPD_TYPE_S,
63 LAPD_TYPE_U,
64} lapd_msg_type;
65
66typedef enum {
Harald Welte7e859bc2011-02-04 20:36:50 +010067 /* commands/responses */
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +010068 LAPD_CMD_NONE = 0,
69
70 LAPD_CMD_I,
71 LAPD_CMD_RR,
72 LAPD_CMD_RNR,
73 LAPD_CMD_REJ,
74
75 LAPD_CMD_SABME,
76 LAPD_CMD_DM,
77 LAPD_CMD_UI,
78 LAPD_CMD_DISC,
79 LAPD_CMD_UA,
80 LAPD_CMD_FRMR,
81 LAPD_CMD_XID,
82} lapd_cmd_type;
83
84const char *lapd_cmd_types[] = {
85 "NONE",
86
87 "I",
88 "RR",
89 "RNR",
90 "REJ",
91
92 "SABME",
93 "DM",
94 "UI",
95 "DISC",
96 "UA",
97 "FRMR",
98 "XID",
99
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100100};
101
Harald Weltedcf42e62011-02-13 11:58:21 +0100102enum lapd_sap_state {
103 SAP_STATE_INACTIVE,
104 SAP_STATE_SABM_RETRANS,
105 SAP_STATE_ACTIVE,
106};
107
108const char *lapd_sap_states[] = {
109 "INACTIVE",
110 "SABM_RETRANS",
111 "ACTIVE",
112};
113
Harald Welte30fe6412011-02-04 20:34:08 +0100114const char *lapd_msg_types = "?ISU";
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100115
Harald Weltedcf42e62011-02-13 11:58:21 +0100116/* structure representing an allocated TEI within a LAPD instance */
Harald Welted38f1052011-02-05 19:13:00 +0100117struct lapd_tei {
118 struct llist_head list;
Harald Weltedcf42e62011-02-13 11:58:21 +0100119 struct lapd_instance *li;
Harald Welted38f1052011-02-05 19:13:00 +0100120 uint8_t tei;
Harald Welte30fe6412011-02-04 20:34:08 +0100121 lapd_tei_state state;
Harald Weltedcf42e62011-02-13 11:58:21 +0100122
123 struct llist_head sap_list;
124};
125
126/* Structure representing a SAP within a TEI. We use this for TE-mode to
127 * re-transmit SABM */
128struct lapd_sap {
129 struct llist_head list;
130 struct lapd_tei *tei;
131 uint8_t sapi;
132 enum lapd_sap_state state;
133
Harald Weltea0fe72d2011-02-14 15:51:57 +0100134 /* A valid N(R) value is one that is in the range V(A) ≤ N(R) ≤ V(S). */
135 int vs; /* next to be transmitted */
136 int va; /* last acked by peer */
137 int vr; /* next expected to be received */
138
Pablo Neira Ayusobf540cb2011-05-06 12:11:06 +0200139 struct osmo_timer_list sabme_timer; /* timer to re-transmit SABM message */
Harald Welted38f1052011-02-05 19:13:00 +0100140};
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100141
Harald Welte7e859bc2011-02-04 20:36:50 +0100142/* 3.5.2.2 Send state variable V(S)
143 * Each point-to-point data link connection endpoint shall have an associated V(S) when using I frame
144 * commands. V(S) denotes the sequence number of the next I frame to be transmitted. The V(S) can
145 * take on the value 0 through n minus 1. The value of V(S) shall be incremented by 1 with each
146 * successive I frame transmission, and shall not exceed V(A) by more than the maximum number of
147 * outstanding I frames k. The value of k may be in the range of 1 ≤ k ≤ 127.
148 *
149 * 3.5.2.3 Acknowledge state variable V(A)
150 * Each point-to-point data link connection endpoint shall have an associated V(A) when using I frame
151 * commands and supervisory frame commands/responses. V(A) identifies the last I frame that has been
152 * acknowledged by its peer [V(A) − 1 equals the N(S) of the last acknowledged I frame]. V(A) can
153 * take on the value 0 through n minus 1. The value of V(A) shall be updated by the valid N(R) values
154 * received from its peer (see 3.5.2.6). A valid N(R) value is one that is in the range V(A) ≤ N(R) ≤
155 * V(S).
156 *
157 * 3.5.2.5 Receive state variable V(R)
158 * Each point-to-point data link connection endpoint shall have an associated V(R) when using I frame
159 * commands and supervisory frame commands/responses. V(R) denotes the sequence number of the
160 * next in-sequence I frame expected to be received. V(R) can take on the value 0 through n minus 1.
161 * The value of V(R) shall be incremented by one with the receipt of an error-free, in-sequence I frame
162 * whose N(S) equals V(R).
163 */
Harald Weltea0fe72d2011-02-14 15:51:57 +0100164#define LAPD_NS(sap) (sap->vs)
165#define LAPD_NR(sap) (sap->vr)
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100166
Harald Welte7e859bc2011-02-04 20:36:50 +0100167/* 3.5.2.4 Send sequence number N(S)
168 * Only I frames contain N(S), the send sequence number of transmitted I frames. At the time that an in-
169 * sequence I frame is designated for transmission, the value of N(S) is set equal to V(S).
170 *
171 * 3.5.2.6 Receive sequence number N(R)
172 * All I frames and supervisory frames contain N(R), the expected send sequence number of the next
173 * received I frame. At the time that a frame of the above types is designated for transmission, the value
174 * of N(R) is set equal to V(R). N(R) indicates that the data link layer entity transmitting the N(R) has
175 * correctly received all I frames numbered up to and including N(R) − 1.
176 */
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100177
Harald Weltedcf42e62011-02-13 11:58:21 +0100178/* Resolve TEI structure from given numeric TEI */
Harald Welted38f1052011-02-05 19:13:00 +0100179static struct lapd_tei *teip_from_tei(struct lapd_instance *li, uint8_t tei)
Harald Welte30fe6412011-02-04 20:34:08 +0100180{
Harald Welted38f1052011-02-05 19:13:00 +0100181 struct lapd_tei *lt;
182
183 llist_for_each_entry(lt, &li->tei_list, list) {
184 if (lt->tei == tei)
185 return lt;
186 }
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100187 return NULL;
188};
189
Harald Welted38f1052011-02-05 19:13:00 +0100190static void lapd_tei_set_state(struct lapd_tei *teip, int newstate)
Harald Welte30fe6412011-02-04 20:34:08 +0100191{
Harald Welte6e4c26a2011-08-09 21:41:35 +0200192 LOGP(DMI, LOGL_INFO, "LAPD state change on TEI %d: %s -> %s\n", teip->tei,
Harald Welte30fe6412011-02-04 20:34:08 +0100193 lapd_tei_states[teip->state], lapd_tei_states[newstate]);
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100194 teip->state = newstate;
195};
196
Harald Weltedcf42e62011-02-13 11:58:21 +0100197/* Allocate a new TEI */
198struct lapd_tei *lapd_tei_alloc(struct lapd_instance *li, uint8_t tei)
Harald Welte1a00d822011-02-11 18:34:51 +0100199{
200 struct lapd_tei *teip;
201
202 teip = talloc_zero(li, struct lapd_tei);
203 if (!teip)
Harald Weltedcf42e62011-02-13 11:58:21 +0100204 return NULL;
Harald Welte1a00d822011-02-11 18:34:51 +0100205
Harald Weltedcf42e62011-02-13 11:58:21 +0100206 teip->li = li;
Harald Welte1a00d822011-02-11 18:34:51 +0100207 teip->tei = tei;
208 llist_add(&teip->list, &li->tei_list);
Harald Weltedcf42e62011-02-13 11:58:21 +0100209 INIT_LLIST_HEAD(&teip->sap_list);
Harald Welte1a00d822011-02-11 18:34:51 +0100210
211 lapd_tei_set_state(teip, LAPD_TEI_ASSIGNED);
212
Harald Weltedcf42e62011-02-13 11:58:21 +0100213 return teip;
Harald Welte1a00d822011-02-11 18:34:51 +0100214}
215
Harald Weltedcf42e62011-02-13 11:58:21 +0100216/* Find a SAP within a given TEI */
217static struct lapd_sap *lapd_sap_find(struct lapd_tei *teip, uint8_t sapi)
218{
219 struct lapd_sap *sap;
220
221 llist_for_each_entry(sap, &teip->sap_list, list) {
222 if (sap->sapi == sapi)
223 return sap;
224 }
225
226 return NULL;
227}
228
229static void sabme_timer_cb(void *_sap);
230
231/* Allocate a new SAP within a given TEI */
232static struct lapd_sap *lapd_sap_alloc(struct lapd_tei *teip, uint8_t sapi)
233{
234 struct lapd_sap *sap = talloc_zero(teip, struct lapd_sap);
235
Harald Welte6e4c26a2011-08-09 21:41:35 +0200236 LOGP(DMI, LOGL_INFO, "LAPD Allocating SAP for SAPI=%u / TEI=%u\n",
Harald Weltedcf42e62011-02-13 11:58:21 +0100237 sapi, teip->tei);
238
239 sap->sapi = sapi;
240 sap->tei = teip;
241 sap->sabme_timer.cb = &sabme_timer_cb;
242 sap->sabme_timer.data = sap;
243
244 llist_add(&sap->list, &teip->sap_list);
245
246 return sap;
247}
248
249static void lapd_sap_set_state(struct lapd_tei *teip, uint8_t sapi,
250 enum lapd_sap_state newstate)
251{
252 struct lapd_sap *sap = lapd_sap_find(teip, sapi);
253 if (!sap)
254 return;
255
Harald Welte6e4c26a2011-08-09 21:41:35 +0200256 LOGP(DMI, LOGL_INFO, "LAPD state change on TEI %u / SAPI %u: "
257 "%s -> %s\n", teip->tei, sapi,
258 lapd_sap_states[sap->state], lapd_sap_states[newstate]);
Harald Weltedcf42e62011-02-13 11:58:21 +0100259 switch (sap->state) {
260 case SAP_STATE_SABM_RETRANS:
261 if (newstate != SAP_STATE_SABM_RETRANS)
Pablo Neira Ayusobf540cb2011-05-06 12:11:06 +0200262 osmo_timer_del(&sap->sabme_timer);
Harald Weltedcf42e62011-02-13 11:58:21 +0100263 break;
264 default:
265 if (newstate == SAP_STATE_SABM_RETRANS)
Pablo Neira Ayusobf540cb2011-05-06 12:11:06 +0200266 osmo_timer_schedule(&sap->sabme_timer, SABM_INTERVAL);
Harald Weltedcf42e62011-02-13 11:58:21 +0100267 break;
268 }
269
270 sap->state = newstate;
271};
272
273/* Input function into TEI manager */
Harald Welted38f1052011-02-05 19:13:00 +0100274static void lapd_tei_receive(struct lapd_instance *li, uint8_t *data, int len)
Harald Welte30fe6412011-02-04 20:34:08 +0100275{
Harald Welte8fc66a02011-02-05 19:51:05 +0100276 uint8_t entity = data[0];
277 uint8_t ref = data[1];
278 uint8_t mt = data[3];
279 uint8_t action = data[4] >> 1;
280 uint8_t e = data[4] & 1;
Harald Welted38f1052011-02-05 19:13:00 +0100281 uint8_t resp[8];
282 struct lapd_tei *teip;
283
Harald Welte6e4c26a2011-08-09 21:41:35 +0200284 DEBUGP(DMI, "LAPD TEIMGR: entity %x, ref %x, mt %x, action %x, e %x\n",
285 entity, ref, mt, action, e);
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100286
287 switch (mt) {
Harald Welted38f1052011-02-05 19:13:00 +0100288 case 0x01: /* IDENTITY REQUEST */
Harald Welte6e4c26a2011-08-09 21:41:35 +0200289 DEBUGP(DMI, "LAPD TEIMGR: identity request for TEI %u\n", action);
Harald Welte716d2a42011-02-05 17:29:05 +0100290
Harald Welte8fc66a02011-02-05 19:51:05 +0100291 teip = teip_from_tei(li, action);
Harald Welted38f1052011-02-05 19:13:00 +0100292 if (!teip) {
Harald Welte8fc66a02011-02-05 19:51:05 +0100293 LOGP(DMI, LOGL_INFO, "TEI MGR: New TEI %u\n", action);
Holger Hans Peter Freyther13673742011-04-04 19:17:58 +0200294 teip = lapd_tei_alloc(li, action);
Harald Welte30fe6412011-02-04 20:34:08 +0100295 }
Harald Welted38f1052011-02-05 19:13:00 +0100296
297 /* Send ACCEPT */
298 memmove(resp, "\xfe\xff\x03\x0f\x00\x00\x02\x00", 8);
Harald Welte8fc66a02011-02-05 19:51:05 +0100299 resp[7] = (action << 1) | 1;
Harald Welted38f1052011-02-05 19:13:00 +0100300 li->transmit_cb(resp, 8, li->cbdata);
301
302 if (teip->state == LAPD_TEI_NONE)
303 lapd_tei_set_state(teip, LAPD_TEI_ASSIGNED);
304 break;
Harald Welte30fe6412011-02-04 20:34:08 +0100305 default:
Harald Welte6e4c26a2011-08-09 21:41:35 +0200306 LOGP(DMI, LOGL_NOTICE, "LAPD TEIMGR: unknown mt %x action %x\n",
Harald Welte0ae57552011-02-05 18:33:12 +0100307 mt, action);
308 break;
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100309 };
310};
311
Harald Weltedcf42e62011-02-13 11:58:21 +0100312/* General input function for any data received for this LAPD instance */
Pablo Neira Ayusocd986562011-08-09 23:15:38 +0200313uint8_t *
314lapd_receive(struct lapd_instance *li, uint8_t * data, unsigned int len,
315 int *ilen, lapd_mph_type *prim, int *error)
Harald Welte30fe6412011-02-04 20:34:08 +0100316{
Harald Welte716d2a42011-02-05 17:29:05 +0100317 uint8_t sapi, cr, tei, command;
318 int pf, ns, nr;
319 uint8_t *contents;
Harald Welted38f1052011-02-05 19:13:00 +0100320 struct lapd_tei *teip;
Harald Weltea0fe72d2011-02-14 15:51:57 +0100321 struct lapd_sap *sap;
Harald Welte716d2a42011-02-05 17:29:05 +0100322
323 uint8_t resp[8];
324 int l = 0;
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100325
326 *ilen = 0;
327 *prim = 0;
328
329 if (len < 2) {
Harald Welte6e4c26a2011-08-09 21:41:35 +0200330 LOGP(DMI, LOGL_ERROR, "LAPD receive len %d < 2, ignoring\n", len);
Pablo Neira Ayusocd986562011-08-09 23:15:38 +0200331 *error = LAPD_ERR_BAD_LEN;
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100332 return NULL;
333 };
334
335 if ((data[0] & 1) != 0 || (data[1] & 1) != 1) {
Harald Welte6e4c26a2011-08-09 21:41:35 +0200336 LOGP(DMI, LOGL_ERROR, "LAPD address field %x/%x not well formed\n",
337 data[0], data[1]);
Pablo Neira Ayusocd986562011-08-09 23:15:38 +0200338 *error = LAPD_ERR_BAD_ADDR;
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100339 return NULL;
340 };
341
Harald Welte716d2a42011-02-05 17:29:05 +0100342 sapi = data[0] >> 2;
343 cr = (data[0] >> 1) & 1;
344 tei = data[1] >> 1;
Harald Welted38f1052011-02-05 19:13:00 +0100345 command = li->network_side ^ cr;
Harald Welte0abc11a2011-02-05 17:16:26 +0100346 //DEBUGP(DMI, " address sapi %x tei %d cmd %d cr %d\n", sapi, tei, command, cr);
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100347
348 if (len < 3) {
Harald Welte6e4c26a2011-08-09 21:41:35 +0200349 LOGP(DMI, LOGL_ERROR, "LAPD receive len %d < 3, ignoring\n", len);
Pablo Neira Ayusocd986562011-08-09 23:15:38 +0200350 *error = LAPD_ERR_BAD_LEN;
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100351 return NULL;
352 };
353
354 lapd_msg_type typ = 0;
355 lapd_cmd_type cmd = 0;
Harald Welte716d2a42011-02-05 17:29:05 +0100356 pf = -1;
357 ns = -1;
358 nr = -1;
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100359 if ((data[2] & 1) == 0) {
360 typ = LAPD_TYPE_I;
Harald Welte52a0b122011-08-09 21:24:47 +0200361 if (len < 4) {
362 LOGP(DMI, LOGL_ERROR, "LAPD I frame, len %d < 4\n", len);
Pablo Neira Ayusocd986562011-08-09 23:15:38 +0200363 *error = LAPD_ERR_BAD_LEN;
Harald Welte52a0b122011-08-09 21:24:47 +0200364 return NULL;
365 }
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100366 ns = data[2] >> 1;
367 nr = data[3] >> 1;
368 pf = data[3] & 1;
369 cmd = LAPD_CMD_I;
370 } else if ((data[2] & 3) == 1) {
371 typ = LAPD_TYPE_S;
Harald Welte52a0b122011-08-09 21:24:47 +0200372 if (len < 4) {
373 LOGP(DMI, LOGL_ERROR, "LAPD S frame, len %d < 4\n", len);
Pablo Neira Ayusocd986562011-08-09 23:15:38 +0200374 *error = LAPD_ERR_BAD_LEN;
Harald Welte52a0b122011-08-09 21:24:47 +0200375 return NULL;
376 }
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100377 nr = data[3] >> 1;
378 pf = data[3] & 1;
379 switch (data[2]) {
Harald Welte30fe6412011-02-04 20:34:08 +0100380 case 0x1:
381 cmd = LAPD_CMD_RR;
382 break;
383 case 0x5:
384 cmd = LAPD_CMD_RNR;
385 break;
386 case 0x9:
387 cmd = LAPD_CMD_REJ;
388 break;
389 default:
Harald Welte6e4c26a2011-08-09 21:41:35 +0200390 LOGP(DMI, LOGL_ERROR, "LAPD unknown S cmd %x\n", data[2]);
Pablo Neira Ayusocd986562011-08-09 23:15:38 +0200391 *error = LAPD_ERR_UNKNOWN_S_CMD;
Harald Welte0ae57552011-02-05 18:33:12 +0100392 return NULL;
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100393 };
394 } else if ((data[2] & 3) == 3) {
395 typ = LAPD_TYPE_U;
396 pf = (data[2] >> 4) & 1;
Harald Welte30fe6412011-02-04 20:34:08 +0100397 int val = data[2] & ~(1 << 4);
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100398 switch (val) {
Harald Welte30fe6412011-02-04 20:34:08 +0100399 case 0x6f:
400 cmd = LAPD_CMD_SABME;
401 break;
402 case 0x0f:
403 cmd = LAPD_CMD_DM;
404 break;
405 case 0x03:
406 cmd = LAPD_CMD_UI;
407 break;
408 case 0x43:
409 cmd = LAPD_CMD_DISC;
410 break;
411 case 0x63:
412 cmd = LAPD_CMD_UA;
413 break;
414 case 0x87:
415 cmd = LAPD_CMD_FRMR;
416 break;
417 case 0xaf:
418 cmd = LAPD_CMD_XID;
419 break;
420
421 default:
Harald Welte6e4c26a2011-08-09 21:41:35 +0200422 LOGP(DMI, LOGL_ERROR, "LAPD unknown U cmd %x "
Harald Welte0ae57552011-02-05 18:33:12 +0100423 "(pf %x data %x)\n", val, pf, data[2]);
Pablo Neira Ayusocd986562011-08-09 23:15:38 +0200424 *error = LAPD_ERR_UNKNOWN_U_CMD;
Harald Welte0ae57552011-02-05 18:33:12 +0100425 return NULL;
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100426 };
427 };
Harald Welte30fe6412011-02-04 20:34:08 +0100428
Harald Welte716d2a42011-02-05 17:29:05 +0100429 contents = &data[4];
Harald Welte30fe6412011-02-04 20:34:08 +0100430 if (typ == LAPD_TYPE_U)
431 contents--;
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100432 *ilen = len - (contents - data);
Harald Welte30fe6412011-02-04 20:34:08 +0100433
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100434 if (tei == 127)
Harald Welted38f1052011-02-05 19:13:00 +0100435 lapd_tei_receive(li, contents, *ilen);
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100436
Harald Welted38f1052011-02-05 19:13:00 +0100437 teip = teip_from_tei(li, tei);
Harald Welte716d2a42011-02-05 17:29:05 +0100438 if (!teip) {
Harald Welte6e4c26a2011-08-09 21:41:35 +0200439 LOGP(DMI, LOGL_NOTICE, "LAPD Unknown TEI %u\n", tei);
Pablo Neira Ayusocd986562011-08-09 23:15:38 +0200440 *error = LAPD_ERR_UNKNOWN_TEI;
Harald Welte716d2a42011-02-05 17:29:05 +0100441 return NULL;
442 }
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100443
Harald Weltea0fe72d2011-02-14 15:51:57 +0100444 sap = lapd_sap_find(teip, sapi);
445 if (!sap) {
Harald Welte6e4c26a2011-08-09 21:41:35 +0200446 LOGP(DMI, LOGL_INFO, "LAPD No SAP for TEI=%u / SAPI=%u, "
Harald Weltea0fe72d2011-02-14 15:51:57 +0100447 "allocating\n", tei, sapi);
448 sap = lapd_sap_alloc(teip, sapi);
449 }
450
451 DEBUGP(DMI, "<- %c %s sapi %x tei %3d cmd %x pf %x ns %3d nr %3d "
452 "ilen %d teip %p vs %d va %d vr %d len %d\n",
453 lapd_msg_types[typ], lapd_cmd_types[cmd], sapi, tei, command, pf,
454 ns, nr, *ilen, teip, sap->vs, sap->va, sap->vr, len);
455
Harald Welte716d2a42011-02-05 17:29:05 +0100456 switch (cmd) {
457 case LAPD_CMD_I:
Harald Weltea0fe72d2011-02-14 15:51:57 +0100458 if (ns != sap->vr) {
Harald Welte6e4c26a2011-08-09 21:41:35 +0200459 DEBUGP(DMI, "LAPD ns %d != vr %d\n", ns, sap->vr);
Harald Weltea0fe72d2011-02-14 15:51:57 +0100460 if (ns == ((sap->vr - 1) & 0x7f)) {
Harald Welte6e4c26a2011-08-09 21:41:35 +0200461 LOGP(DMI, LOGL_NOTICE, "LAPD double frame, "
462 "ignoring\n");
Harald Welte716d2a42011-02-05 17:29:05 +0100463 cmd = 0; // ignore
464 } else {
Harald Welte6e4c26a2011-08-09 21:41:35 +0200465 LOGP(DMI, LOGL_ERROR, "LAPD Out of order "
Harald Welte52a0b122011-08-09 21:24:47 +0200466 "ns %d != vr %d, ignoring\n", ns, sap->vr);
467 return NULL;
Harald Welte716d2a42011-02-05 17:29:05 +0100468 };
469 } else {
470 //printf("IN SEQUENCE\n");
Harald Weltea0fe72d2011-02-14 15:51:57 +0100471 sap->vr = (ns + 1) & 0x7f; // FIXME: hack!
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100472 };
Harald Welte716d2a42011-02-05 17:29:05 +0100473
474 break;
475 case LAPD_CMD_UI:
476 break;
477 case LAPD_CMD_SABME:
Harald Weltea0fe72d2011-02-14 15:51:57 +0100478 sap->vs = 0;
479 sap->vr = 0;
480 sap->va = 0;
Harald Welte716d2a42011-02-05 17:29:05 +0100481
482 // ua
483 resp[l++] = data[0];
484 resp[l++] = (tei << 1) | 1;
485 resp[l++] = 0x73;
Harald Welted38f1052011-02-05 19:13:00 +0100486 li->transmit_cb(resp, l, li->cbdata);
Harald Welte716d2a42011-02-05 17:29:05 +0100487 if (teip->state != LAPD_TEI_ACTIVE) {
488 if (teip->state == LAPD_TEI_ASSIGNED) {
489 lapd_tei_set_state(teip,
490 LAPD_TEI_ACTIVE);
491 //printf("ASSIGNED and ACTIVE\n");
492 } else {
493#if 0
494 DEBUGP(DMI, "rr in strange state, send rej\n");
495
496 // rej
Harald Weltea0fe72d2011-02-14 15:51:57 +0100497 resp[l++] = (sap-> sapi << 2) | (li->network_side ? 0 : 2);
Harald Welte716d2a42011-02-05 17:29:05 +0100498 resp[l++] = (tei << 1) | 1;
499 resp[l++] = 0x09; //rej
Harald Weltea0fe72d2011-02-14 15:51:57 +0100500 resp[l++] = ((sap->vr + 1) << 1) | 0;
Harald Welted38f1052011-02-05 19:13:00 +0100501 li->transmit_cb(resp, l, li->cbdata);
Harald Welte716d2a42011-02-05 17:29:05 +0100502 pf = 0; // dont reply
503#endif
504 };
505 };
506
507 *prim = LAPD_MPH_ACTIVATE_IND;
508 break;
Harald Welte1a00d822011-02-11 18:34:51 +0100509 case LAPD_CMD_UA:
Harald Weltea0fe72d2011-02-14 15:51:57 +0100510 sap->vs = 0;
511 sap->vr = 0;
512 sap->va = 0;
Harald Welte1a00d822011-02-11 18:34:51 +0100513 lapd_tei_set_state(teip, LAPD_TEI_ACTIVE);
Harald Weltedcf42e62011-02-13 11:58:21 +0100514 lapd_sap_set_state(teip, sapi, SAP_STATE_ACTIVE);
Harald Welte1a00d822011-02-11 18:34:51 +0100515 *prim = LAPD_MPH_ACTIVATE_IND;
516 break;
Harald Welte716d2a42011-02-05 17:29:05 +0100517 case LAPD_CMD_RR:
Harald Weltea0fe72d2011-02-14 15:51:57 +0100518 sap->va = (nr & 0x7f);
Harald Welte716d2a42011-02-05 17:29:05 +0100519#if 0
520 if (teip->state != LAPD_TEI_ACTIVE) {
521 if (teip->state == LAPD_TEI_ASSIGNED) {
522 lapd_tei_set_state(teip, LAPD_TEI_ACTIVE);
523 *prim = LAPD_MPH_ACTIVATE_IND;
524 //printf("ASSIGNED and ACTIVE\n");
525 } else {
526#if 0
527 DEBUGP(DMI, "rr in strange " "state, send rej\n");
528
529 // rej
Harald Weltea0fe72d2011-02-14 15:51:57 +0100530 resp[l++] = (sap-> sapi << 2) | (li->network_side ? 0 : 2);
Harald Welte716d2a42011-02-05 17:29:05 +0100531 resp[l++] = (tei << 1) | 1;
532 resp[l++] = 0x09; //rej
533 resp[l++] =
Harald Weltea0fe72d2011-02-14 15:51:57 +0100534 ((sap->vr + 1) << 1) | 0;
Harald Welted38f1052011-02-05 19:13:00 +0100535 li->transmit_cb(resp, l, li->cbdata);
Harald Welte716d2a42011-02-05 17:29:05 +0100536 pf = 0; // dont reply
537#endif
538 };
539 };
540#endif
541 if (pf) {
542 // interrogating us, send rr
543 resp[l++] = data[0];
544 resp[l++] = (tei << 1) | 1;
545 resp[l++] = 0x01; // rr
Harald Weltea0fe72d2011-02-14 15:51:57 +0100546 resp[l++] = (LAPD_NR(sap) << 1) | (data[3] & 1); // pf bit from req
Harald Welte716d2a42011-02-05 17:29:05 +0100547
Harald Welted38f1052011-02-05 19:13:00 +0100548 li->transmit_cb(resp, l, li->cbdata);
Harald Welte716d2a42011-02-05 17:29:05 +0100549
550 };
551 break;
552 case LAPD_CMD_FRMR:
553 // frame reject
554#if 0
555 if (teip->state == LAPD_TEI_ACTIVE)
556 *prim = LAPD_MPH_DEACTIVATE_IND;
557 lapd_tei_set_state(teip, LAPD_TEI_ASSIGNED);
558#endif
Harald Welte0ae57552011-02-05 18:33:12 +0100559 LOGP(DMI, LOGL_NOTICE, "frame reject, ignoring\n");
Harald Welte716d2a42011-02-05 17:29:05 +0100560 break;
561 case LAPD_CMD_DISC:
562 // disconnect
563 resp[l++] = data[0];
564 resp[l++] = (tei << 1) | 1;
565 resp[l++] = 0x73;
Harald Welted38f1052011-02-05 19:13:00 +0100566 li->transmit_cb(resp, l, li->cbdata);
Harald Welte716d2a42011-02-05 17:29:05 +0100567 lapd_tei_set_state(teip, LAPD_TEI_NONE);
568 break;
569 default:
Harald Welte0ae57552011-02-05 18:33:12 +0100570 LOGP(DMI, LOGL_NOTICE, "unknown cmd for tei %d (cmd %x)\n",
571 tei, cmd);
572 break;
Harald Welte716d2a42011-02-05 17:29:05 +0100573 }
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100574
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100575 if (typ == LAPD_TYPE_I) {
Harald Welte0ae57552011-02-05 18:33:12 +0100576 /* send rr
577 * Thu Jan 22 19:17:13 2009 <4000> sangoma.c:340 read (62/25) 4: fa 33 01 0a
578 * lapd <- S RR sapi 3e tei 25 cmd 0 pf 0 ns -1 nr 5 ilen 0 teip 0x613800 vs 7 va 5 vr 2 len 4
579 */
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100580
Harald Welte0ae57552011-02-05 18:33:12 +0100581 /* interrogating us, send rr */
Harald Welte6e4c26a2011-08-09 21:41:35 +0200582 DEBUGP(DMI, "LAPD Sending RR response\n");
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100583 resp[l++] = data[0];
584 resp[l++] = (tei << 1) | 1;
Harald Welte30fe6412011-02-04 20:34:08 +0100585 resp[l++] = 0x01; // rr
Harald Weltea0fe72d2011-02-14 15:51:57 +0100586 resp[l++] = (LAPD_NR(sap) << 1) | (data[3] & 1); // pf bit from req
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100587
Harald Welted38f1052011-02-05 19:13:00 +0100588 li->transmit_cb(resp, l, li->cbdata);
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100589
Matthew Fredrickson69245a02010-03-15 12:24:39 -0500590 if (cmd != 0) {
591 *prim = LAPD_DL_DATA_IND;
592 return contents;
593 }
594 } else if (tei != 127 && typ == LAPD_TYPE_U && cmd == LAPD_CMD_UI) {
595 *prim = LAPD_DL_UNITDATA_IND;
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100596 return contents;
Matthew Fredrickson69245a02010-03-15 12:24:39 -0500597 }
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100598
Pablo Neira Ayusocd986562011-08-09 23:15:38 +0200599 *error = LAPD_ERR_BAD_CMD;
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100600 return NULL;
601};
602
Harald Weltedcf42e62011-02-13 11:58:21 +0100603/* low-level function to send a single SABM message */
604static int lapd_send_sabm(struct lapd_instance *li, uint8_t tei, uint8_t sapi)
Harald Welte1a00d822011-02-11 18:34:51 +0100605{
606 struct msgb *msg = msgb_alloc_headroom(1024, 128, "LAPD SABM");
607 if (!msg)
608 return -ENOMEM;
609
Harald Welte6e4c26a2011-08-09 21:41:35 +0200610 LOGP(DMI, LOGL_INFO, "LAPD Sending SABM for TEI=%u, SAPI=%u\n", tei, sapi);
Harald Welte1a00d822011-02-11 18:34:51 +0100611
Harald Welte1a00d822011-02-11 18:34:51 +0100612 msgb_put_u8(msg, (sapi << 2) | (li->network_side ? 2 : 0));
613 msgb_put_u8(msg, (tei << 1) | 1);
614 msgb_put_u8(msg, 0x7F);
615
616 li->transmit_cb(msg->data, msg->len, li->cbdata);
617
618 msgb_free(msg);
619
620 return 0;
621}
622
Harald Weltedcf42e62011-02-13 11:58:21 +0100623/* timer call-back function for SABM re-transmission */
624static void sabme_timer_cb(void *_sap)
625{
626 struct lapd_sap *sap = _sap;
627
628 lapd_send_sabm(sap->tei->li, sap->tei->tei, sap->sapi);
629
Harald Welte909212f2011-02-13 15:42:07 +0100630 if (sap->state == SAP_STATE_SABM_RETRANS)
Pablo Neira Ayusobf540cb2011-05-06 12:11:06 +0200631 osmo_timer_schedule(&sap->sabme_timer, SABM_INTERVAL);
Harald Weltedcf42e62011-02-13 11:58:21 +0100632}
633
634/* Start a (user-side) SAP for the specified TEI/SAPI on the LAPD instance */
635int lapd_sap_start(struct lapd_instance *li, uint8_t tei, uint8_t sapi)
636{
637 struct lapd_sap *sap;
638 struct lapd_tei *teip;
639
640 teip = teip_from_tei(li, tei);
641 if (!teip)
642 teip = lapd_tei_alloc(li, tei);
643
644 sap = lapd_sap_find(teip, sapi);
645 if (sap)
646 return -EEXIST;
647
648 sap = lapd_sap_alloc(teip, sapi);
649
650 lapd_sap_set_state(teip, sapi, SAP_STATE_SABM_RETRANS);
651
652 return 0;
653}
654
655/* Stop a (user-side) SAP for the specified TEI/SAPI on the LAPD instance */
656int lapd_sap_stop(struct lapd_instance *li, uint8_t tei, uint8_t sapi)
657{
658 struct lapd_tei *teip;
659 struct lapd_sap *sap;
660
661 teip = teip_from_tei(li, tei);
662 if (!teip)
663 return -ENODEV;
664
665 sap = lapd_sap_find(teip, sapi);
666 if (!sap)
667 return -ENODEV;
668
Harald Welte909212f2011-02-13 15:42:07 +0100669 lapd_sap_set_state(teip, sapi, SAP_STATE_INACTIVE);
Harald Weltedcf42e62011-02-13 11:58:21 +0100670
671 llist_del(&sap->list);
672 talloc_free(sap);
673
674 return 0;
675}
676
677/* Transmit Data (I-Frame) on the given LAPD Instance / TEI / SAPI */
Harald Welte4ee2eaf2011-02-05 20:20:50 +0100678void lapd_transmit(struct lapd_instance *li, uint8_t tei, uint8_t sapi,
Harald Welted38f1052011-02-05 19:13:00 +0100679 uint8_t *data, unsigned int len)
Harald Welte30fe6412011-02-04 20:34:08 +0100680{
Harald Welted38f1052011-02-05 19:13:00 +0100681 struct lapd_tei *teip = teip_from_tei(li, tei);
Harald Weltea0fe72d2011-02-14 15:51:57 +0100682 struct lapd_sap *sap;
Harald Welted38f1052011-02-05 19:13:00 +0100683
684 if (!teip) {
Harald Welte6e4c26a2011-08-09 21:41:35 +0200685 LOGP(DMI, LOGL_ERROR, "LAPD Cannot transmit on "
686 "non-existing TEI %u\n", tei);
Harald Welted38f1052011-02-05 19:13:00 +0100687 return;
688 }
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100689
Harald Weltea0fe72d2011-02-14 15:51:57 +0100690 sap = lapd_sap_find(teip, sapi);
691 if (!sap) {
Harald Welte6e4c26a2011-08-09 21:41:35 +0200692 LOGP(DMI, LOGL_INFO, "LAPD Tx on unknown SAPI=%u "
693 "in TEI=%u, allocating\n", sapi, tei);
Harald Weltea0fe72d2011-02-14 15:51:57 +0100694 sap = lapd_sap_alloc(teip, sapi);
695 }
696
Harald Welte0ae57552011-02-05 18:33:12 +0100697 /* prepend stuff */
Harald Welte30fe6412011-02-04 20:34:08 +0100698 uint8_t buf[10000];
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100699 memset(buf, 0, sizeof(buf));
Harald Welte30fe6412011-02-04 20:34:08 +0100700 memmove(buf + 4, data, len);
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100701 len += 4;
702
Harald Welte4ee2eaf2011-02-05 20:20:50 +0100703 buf[0] = (sapi << 2) | (li->network_side ? 2 : 0);
704 buf[1] = (tei << 1) | 1;
Harald Weltea0fe72d2011-02-14 15:51:57 +0100705 buf[2] = (LAPD_NS(sap) << 1);
706 buf[3] = (LAPD_NR(sap) << 1) | 0;
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100707
Harald Weltea0fe72d2011-02-14 15:51:57 +0100708 sap->vs = (sap->vs + 1) & 0x7f;
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100709
Harald Welted38f1052011-02-05 19:13:00 +0100710 li->transmit_cb(buf, len, li->cbdata);
Matthew Fredricksonbc6649e2010-02-16 22:01:36 +0100711};
Harald Welted38f1052011-02-05 19:13:00 +0100712
Harald Weltedcf42e62011-02-13 11:58:21 +0100713/* Allocate a new LAPD instance */
Harald Welte1a00d822011-02-11 18:34:51 +0100714struct lapd_instance *lapd_instance_alloc(int network_side,
715 void (*tx_cb)(uint8_t *data, int len,
Harald Welted38f1052011-02-05 19:13:00 +0100716 void *cbdata), void *cbdata)
717{
718 struct lapd_instance *li;
719
720 li = talloc_zero(NULL, struct lapd_instance);
721 if (!li)
722 return NULL;
723
724 li->transmit_cb = tx_cb;
725 li->cbdata = cbdata;
Harald Welte1a00d822011-02-11 18:34:51 +0100726 li->network_side = network_side;
Harald Welted38f1052011-02-05 19:13:00 +0100727 INIT_LLIST_HEAD(&li->tei_list);
728
729 return li;
730}