forked from Qortal/Brooklyn
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1416 lines
33 KiB
1416 lines
33 KiB
// SPDX-License-Identifier: GPL-2.0-only |
|
/* |
|
* |
|
* Author Karsten Keil <[email protected]> |
|
* |
|
* Copyright 2008 by Karsten Keil <[email protected]> |
|
*/ |
|
#include "layer2.h" |
|
#include <linux/random.h> |
|
#include <linux/slab.h> |
|
#include "core.h" |
|
|
|
#define ID_REQUEST 1 |
|
#define ID_ASSIGNED 2 |
|
#define ID_DENIED 3 |
|
#define ID_CHK_REQ 4 |
|
#define ID_CHK_RES 5 |
|
#define ID_REMOVE 6 |
|
#define ID_VERIFY 7 |
|
|
|
#define TEI_ENTITY_ID 0xf |
|
|
|
#define MGR_PH_ACTIVE 16 |
|
#define MGR_PH_NOTREADY 17 |
|
|
|
#define DATIMER_VAL 10000 |
|
|
|
static u_int *debug; |
|
|
|
static struct Fsm deactfsm = {NULL, 0, 0, NULL, NULL}; |
|
static struct Fsm teifsmu = {NULL, 0, 0, NULL, NULL}; |
|
static struct Fsm teifsmn = {NULL, 0, 0, NULL, NULL}; |
|
|
|
enum { |
|
ST_L1_DEACT, |
|
ST_L1_DEACT_PENDING, |
|
ST_L1_ACTIV, |
|
}; |
|
#define DEACT_STATE_COUNT (ST_L1_ACTIV + 1) |
|
|
|
static char *strDeactState[] = |
|
{ |
|
"ST_L1_DEACT", |
|
"ST_L1_DEACT_PENDING", |
|
"ST_L1_ACTIV", |
|
}; |
|
|
|
enum { |
|
EV_ACTIVATE, |
|
EV_ACTIVATE_IND, |
|
EV_DEACTIVATE, |
|
EV_DEACTIVATE_IND, |
|
EV_UI, |
|
EV_DATIMER, |
|
}; |
|
|
|
#define DEACT_EVENT_COUNT (EV_DATIMER + 1) |
|
|
|
static char *strDeactEvent[] = |
|
{ |
|
"EV_ACTIVATE", |
|
"EV_ACTIVATE_IND", |
|
"EV_DEACTIVATE", |
|
"EV_DEACTIVATE_IND", |
|
"EV_UI", |
|
"EV_DATIMER", |
|
}; |
|
|
|
static void |
|
da_debug(struct FsmInst *fi, char *fmt, ...) |
|
{ |
|
struct manager *mgr = fi->userdata; |
|
struct va_format vaf; |
|
va_list va; |
|
|
|
if (!(*debug & DEBUG_L2_TEIFSM)) |
|
return; |
|
|
|
va_start(va, fmt); |
|
|
|
vaf.fmt = fmt; |
|
vaf.va = &va; |
|
|
|
printk(KERN_DEBUG "mgr(%d): %pV\n", mgr->ch.st->dev->id, &vaf); |
|
|
|
va_end(va); |
|
} |
|
|
|
static void |
|
da_activate(struct FsmInst *fi, int event, void *arg) |
|
{ |
|
struct manager *mgr = fi->userdata; |
|
|
|
if (fi->state == ST_L1_DEACT_PENDING) |
|
mISDN_FsmDelTimer(&mgr->datimer, 1); |
|
mISDN_FsmChangeState(fi, ST_L1_ACTIV); |
|
} |
|
|
|
static void |
|
da_deactivate_ind(struct FsmInst *fi, int event, void *arg) |
|
{ |
|
mISDN_FsmChangeState(fi, ST_L1_DEACT); |
|
} |
|
|
|
static void |
|
da_deactivate(struct FsmInst *fi, int event, void *arg) |
|
{ |
|
struct manager *mgr = fi->userdata; |
|
struct layer2 *l2; |
|
u_long flags; |
|
|
|
read_lock_irqsave(&mgr->lock, flags); |
|
list_for_each_entry(l2, &mgr->layer2, list) { |
|
if (l2->l2m.state > ST_L2_4) { |
|
/* have still activ TEI */ |
|
read_unlock_irqrestore(&mgr->lock, flags); |
|
return; |
|
} |
|
} |
|
read_unlock_irqrestore(&mgr->lock, flags); |
|
/* All TEI are inactiv */ |
|
if (!test_bit(OPTION_L1_HOLD, &mgr->options)) { |
|
mISDN_FsmAddTimer(&mgr->datimer, DATIMER_VAL, EV_DATIMER, |
|
NULL, 1); |
|
mISDN_FsmChangeState(fi, ST_L1_DEACT_PENDING); |
|
} |
|
} |
|
|
|
static void |
|
da_ui(struct FsmInst *fi, int event, void *arg) |
|
{ |
|
struct manager *mgr = fi->userdata; |
|
|
|
/* restart da timer */ |
|
if (!test_bit(OPTION_L1_HOLD, &mgr->options)) { |
|
mISDN_FsmDelTimer(&mgr->datimer, 2); |
|
mISDN_FsmAddTimer(&mgr->datimer, DATIMER_VAL, EV_DATIMER, |
|
NULL, 2); |
|
} |
|
} |
|
|
|
static void |
|
da_timer(struct FsmInst *fi, int event, void *arg) |
|
{ |
|
struct manager *mgr = fi->userdata; |
|
struct layer2 *l2; |
|
u_long flags; |
|
|
|
/* check again */ |
|
read_lock_irqsave(&mgr->lock, flags); |
|
list_for_each_entry(l2, &mgr->layer2, list) { |
|
if (l2->l2m.state > ST_L2_4) { |
|
/* have still activ TEI */ |
|
read_unlock_irqrestore(&mgr->lock, flags); |
|
mISDN_FsmChangeState(fi, ST_L1_ACTIV); |
|
return; |
|
} |
|
} |
|
read_unlock_irqrestore(&mgr->lock, flags); |
|
/* All TEI are inactiv */ |
|
mISDN_FsmChangeState(fi, ST_L1_DEACT); |
|
_queue_data(&mgr->ch, PH_DEACTIVATE_REQ, MISDN_ID_ANY, 0, NULL, |
|
GFP_ATOMIC); |
|
} |
|
|
|
static struct FsmNode DeactFnList[] = |
|
{ |
|
{ST_L1_DEACT, EV_ACTIVATE_IND, da_activate}, |
|
{ST_L1_ACTIV, EV_DEACTIVATE_IND, da_deactivate_ind}, |
|
{ST_L1_ACTIV, EV_DEACTIVATE, da_deactivate}, |
|
{ST_L1_DEACT_PENDING, EV_ACTIVATE, da_activate}, |
|
{ST_L1_DEACT_PENDING, EV_UI, da_ui}, |
|
{ST_L1_DEACT_PENDING, EV_DATIMER, da_timer}, |
|
}; |
|
|
|
enum { |
|
ST_TEI_NOP, |
|
ST_TEI_IDREQ, |
|
ST_TEI_IDVERIFY, |
|
}; |
|
|
|
#define TEI_STATE_COUNT (ST_TEI_IDVERIFY + 1) |
|
|
|
static char *strTeiState[] = |
|
{ |
|
"ST_TEI_NOP", |
|
"ST_TEI_IDREQ", |
|
"ST_TEI_IDVERIFY", |
|
}; |
|
|
|
enum { |
|
EV_IDREQ, |
|
EV_ASSIGN, |
|
EV_ASSIGN_REQ, |
|
EV_DENIED, |
|
EV_CHKREQ, |
|
EV_CHKRESP, |
|
EV_REMOVE, |
|
EV_VERIFY, |
|
EV_TIMER, |
|
}; |
|
|
|
#define TEI_EVENT_COUNT (EV_TIMER + 1) |
|
|
|
static char *strTeiEvent[] = |
|
{ |
|
"EV_IDREQ", |
|
"EV_ASSIGN", |
|
"EV_ASSIGN_REQ", |
|
"EV_DENIED", |
|
"EV_CHKREQ", |
|
"EV_CHKRESP", |
|
"EV_REMOVE", |
|
"EV_VERIFY", |
|
"EV_TIMER", |
|
}; |
|
|
|
static void |
|
tei_debug(struct FsmInst *fi, char *fmt, ...) |
|
{ |
|
struct teimgr *tm = fi->userdata; |
|
struct va_format vaf; |
|
va_list va; |
|
|
|
if (!(*debug & DEBUG_L2_TEIFSM)) |
|
return; |
|
|
|
va_start(va, fmt); |
|
|
|
vaf.fmt = fmt; |
|
vaf.va = &va; |
|
|
|
printk(KERN_DEBUG "sapi(%d) tei(%d): %pV\n", |
|
tm->l2->sapi, tm->l2->tei, &vaf); |
|
|
|
va_end(va); |
|
} |
|
|
|
|
|
|
|
static int |
|
get_free_id(struct manager *mgr) |
|
{ |
|
DECLARE_BITMAP(ids, 64) = { [0 ... BITS_TO_LONGS(64) - 1] = 0 }; |
|
int i; |
|
struct layer2 *l2; |
|
|
|
list_for_each_entry(l2, &mgr->layer2, list) { |
|
if (l2->ch.nr > 63) { |
|
printk(KERN_WARNING |
|
"%s: more as 63 layer2 for one device\n", |
|
__func__); |
|
return -EBUSY; |
|
} |
|
__set_bit(l2->ch.nr, ids); |
|
} |
|
i = find_next_zero_bit(ids, 64, 1); |
|
if (i < 64) |
|
return i; |
|
printk(KERN_WARNING "%s: more as 63 layer2 for one device\n", |
|
__func__); |
|
return -EBUSY; |
|
} |
|
|
|
static int |
|
get_free_tei(struct manager *mgr) |
|
{ |
|
DECLARE_BITMAP(ids, 64) = { [0 ... BITS_TO_LONGS(64) - 1] = 0 }; |
|
int i; |
|
struct layer2 *l2; |
|
|
|
list_for_each_entry(l2, &mgr->layer2, list) { |
|
if (l2->ch.nr == 0) |
|
continue; |
|
if ((l2->ch.addr & 0xff) != 0) |
|
continue; |
|
i = l2->ch.addr >> 8; |
|
if (i < 64) |
|
continue; |
|
i -= 64; |
|
|
|
__set_bit(i, ids); |
|
} |
|
i = find_first_zero_bit(ids, 64); |
|
if (i < 64) |
|
return i + 64; |
|
printk(KERN_WARNING "%s: more as 63 dynamic tei for one device\n", |
|
__func__); |
|
return -1; |
|
} |
|
|
|
static void |
|
teiup_create(struct manager *mgr, u_int prim, int len, void *arg) |
|
{ |
|
struct sk_buff *skb; |
|
struct mISDNhead *hh; |
|
int err; |
|
|
|
skb = mI_alloc_skb(len, GFP_ATOMIC); |
|
if (!skb) |
|
return; |
|
hh = mISDN_HEAD_P(skb); |
|
hh->prim = prim; |
|
hh->id = (mgr->ch.nr << 16) | mgr->ch.addr; |
|
if (len) |
|
skb_put_data(skb, arg, len); |
|
err = mgr->up->send(mgr->up, skb); |
|
if (err) { |
|
printk(KERN_WARNING "%s: err=%d\n", __func__, err); |
|
dev_kfree_skb(skb); |
|
} |
|
} |
|
|
|
static u_int |
|
new_id(struct manager *mgr) |
|
{ |
|
u_int id; |
|
|
|
id = mgr->nextid++; |
|
if (id == 0x7fff) |
|
mgr->nextid = 1; |
|
id <<= 16; |
|
id |= GROUP_TEI << 8; |
|
id |= TEI_SAPI; |
|
return id; |
|
} |
|
|
|
static void |
|
do_send(struct manager *mgr) |
|
{ |
|
if (!test_bit(MGR_PH_ACTIVE, &mgr->options)) |
|
return; |
|
|
|
if (!test_and_set_bit(MGR_PH_NOTREADY, &mgr->options)) { |
|
struct sk_buff *skb = skb_dequeue(&mgr->sendq); |
|
|
|
if (!skb) { |
|
test_and_clear_bit(MGR_PH_NOTREADY, &mgr->options); |
|
return; |
|
} |
|
mgr->lastid = mISDN_HEAD_ID(skb); |
|
mISDN_FsmEvent(&mgr->deact, EV_UI, NULL); |
|
if (mgr->ch.recv(mgr->ch.peer, skb)) { |
|
dev_kfree_skb(skb); |
|
test_and_clear_bit(MGR_PH_NOTREADY, &mgr->options); |
|
mgr->lastid = MISDN_ID_NONE; |
|
} |
|
} |
|
} |
|
|
|
static void |
|
do_ack(struct manager *mgr, u_int id) |
|
{ |
|
if (test_bit(MGR_PH_NOTREADY, &mgr->options)) { |
|
if (id == mgr->lastid) { |
|
if (test_bit(MGR_PH_ACTIVE, &mgr->options)) { |
|
struct sk_buff *skb; |
|
|
|
skb = skb_dequeue(&mgr->sendq); |
|
if (skb) { |
|
mgr->lastid = mISDN_HEAD_ID(skb); |
|
if (!mgr->ch.recv(mgr->ch.peer, skb)) |
|
return; |
|
dev_kfree_skb(skb); |
|
} |
|
} |
|
mgr->lastid = MISDN_ID_NONE; |
|
test_and_clear_bit(MGR_PH_NOTREADY, &mgr->options); |
|
} |
|
} |
|
} |
|
|
|
static void |
|
mgr_send_down(struct manager *mgr, struct sk_buff *skb) |
|
{ |
|
skb_queue_tail(&mgr->sendq, skb); |
|
if (!test_bit(MGR_PH_ACTIVE, &mgr->options)) { |
|
_queue_data(&mgr->ch, PH_ACTIVATE_REQ, MISDN_ID_ANY, 0, |
|
NULL, GFP_KERNEL); |
|
} else { |
|
do_send(mgr); |
|
} |
|
} |
|
|
|
static int |
|
dl_unit_data(struct manager *mgr, struct sk_buff *skb) |
|
{ |
|
if (!test_bit(MGR_OPT_NETWORK, &mgr->options)) /* only net send UI */ |
|
return -EINVAL; |
|
if (!test_bit(MGR_PH_ACTIVE, &mgr->options)) |
|
_queue_data(&mgr->ch, PH_ACTIVATE_REQ, MISDN_ID_ANY, 0, |
|
NULL, GFP_KERNEL); |
|
skb_push(skb, 3); |
|
skb->data[0] = 0x02; /* SAPI 0 C/R = 1 */ |
|
skb->data[1] = 0xff; /* TEI 127 */ |
|
skb->data[2] = UI; /* UI frame */ |
|
mISDN_HEAD_PRIM(skb) = PH_DATA_REQ; |
|
mISDN_HEAD_ID(skb) = new_id(mgr); |
|
skb_queue_tail(&mgr->sendq, skb); |
|
do_send(mgr); |
|
return 0; |
|
} |
|
|
|
static unsigned int |
|
random_ri(void) |
|
{ |
|
u16 x; |
|
|
|
get_random_bytes(&x, sizeof(x)); |
|
return x; |
|
} |
|
|
|
static struct layer2 * |
|
findtei(struct manager *mgr, int tei) |
|
{ |
|
struct layer2 *l2; |
|
u_long flags; |
|
|
|
read_lock_irqsave(&mgr->lock, flags); |
|
list_for_each_entry(l2, &mgr->layer2, list) { |
|
if ((l2->sapi == 0) && (l2->tei > 0) && |
|
(l2->tei != GROUP_TEI) && (l2->tei == tei)) |
|
goto done; |
|
} |
|
l2 = NULL; |
|
done: |
|
read_unlock_irqrestore(&mgr->lock, flags); |
|
return l2; |
|
} |
|
|
|
static void |
|
put_tei_msg(struct manager *mgr, u_char m_id, unsigned int ri, int tei) |
|
{ |
|
struct sk_buff *skb; |
|
u_char bp[8]; |
|
|
|
bp[0] = (TEI_SAPI << 2); |
|
if (test_bit(MGR_OPT_NETWORK, &mgr->options)) |
|
bp[0] |= 2; /* CR:=1 for net command */ |
|
bp[1] = (GROUP_TEI << 1) | 0x1; |
|
bp[2] = UI; |
|
bp[3] = TEI_ENTITY_ID; |
|
bp[4] = ri >> 8; |
|
bp[5] = ri & 0xff; |
|
bp[6] = m_id; |
|
bp[7] = ((tei << 1) & 0xff) | 1; |
|
skb = _alloc_mISDN_skb(PH_DATA_REQ, new_id(mgr), 8, bp, GFP_ATOMIC); |
|
if (!skb) { |
|
printk(KERN_WARNING "%s: no skb for tei msg\n", __func__); |
|
return; |
|
} |
|
mgr_send_down(mgr, skb); |
|
} |
|
|
|
static void |
|
tei_id_request(struct FsmInst *fi, int event, void *arg) |
|
{ |
|
struct teimgr *tm = fi->userdata; |
|
|
|
if (tm->l2->tei != GROUP_TEI) { |
|
tm->tei_m.printdebug(&tm->tei_m, |
|
"assign request for already assigned tei %d", |
|
tm->l2->tei); |
|
return; |
|
} |
|
tm->ri = random_ri(); |
|
if (*debug & DEBUG_L2_TEI) |
|
tm->tei_m.printdebug(&tm->tei_m, |
|
"assign request ri %d", tm->ri); |
|
put_tei_msg(tm->mgr, ID_REQUEST, tm->ri, GROUP_TEI); |
|
mISDN_FsmChangeState(fi, ST_TEI_IDREQ); |
|
mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 1); |
|
tm->nval = 3; |
|
} |
|
|
|
static void |
|
tei_id_assign(struct FsmInst *fi, int event, void *arg) |
|
{ |
|
struct teimgr *tm = fi->userdata; |
|
struct layer2 *l2; |
|
u_char *dp = arg; |
|
int ri, tei; |
|
|
|
ri = ((unsigned int) *dp++ << 8); |
|
ri += *dp++; |
|
dp++; |
|
tei = *dp >> 1; |
|
if (*debug & DEBUG_L2_TEI) |
|
tm->tei_m.printdebug(fi, "identity assign ri %d tei %d", |
|
ri, tei); |
|
l2 = findtei(tm->mgr, tei); |
|
if (l2) { /* same tei is in use */ |
|
if (ri != l2->tm->ri) { |
|
tm->tei_m.printdebug(fi, |
|
"possible duplicate assignment tei %d", tei); |
|
tei_l2(l2, MDL_ERROR_RSP, 0); |
|
} |
|
} else if (ri == tm->ri) { |
|
mISDN_FsmDelTimer(&tm->timer, 1); |
|
mISDN_FsmChangeState(fi, ST_TEI_NOP); |
|
tei_l2(tm->l2, MDL_ASSIGN_REQ, tei); |
|
} |
|
} |
|
|
|
static void |
|
tei_id_test_dup(struct FsmInst *fi, int event, void *arg) |
|
{ |
|
struct teimgr *tm = fi->userdata; |
|
struct layer2 *l2; |
|
u_char *dp = arg; |
|
int tei, ri; |
|
|
|
ri = ((unsigned int) *dp++ << 8); |
|
ri += *dp++; |
|
dp++; |
|
tei = *dp >> 1; |
|
if (*debug & DEBUG_L2_TEI) |
|
tm->tei_m.printdebug(fi, "foreign identity assign ri %d tei %d", |
|
ri, tei); |
|
l2 = findtei(tm->mgr, tei); |
|
if (l2) { /* same tei is in use */ |
|
if (ri != l2->tm->ri) { /* and it wasn't our request */ |
|
tm->tei_m.printdebug(fi, |
|
"possible duplicate assignment tei %d", tei); |
|
mISDN_FsmEvent(&l2->tm->tei_m, EV_VERIFY, NULL); |
|
} |
|
} |
|
} |
|
|
|
static void |
|
tei_id_denied(struct FsmInst *fi, int event, void *arg) |
|
{ |
|
struct teimgr *tm = fi->userdata; |
|
u_char *dp = arg; |
|
int ri, tei; |
|
|
|
ri = ((unsigned int) *dp++ << 8); |
|
ri += *dp++; |
|
dp++; |
|
tei = *dp >> 1; |
|
if (*debug & DEBUG_L2_TEI) |
|
tm->tei_m.printdebug(fi, "identity denied ri %d tei %d", |
|
ri, tei); |
|
} |
|
|
|
static void |
|
tei_id_chk_req(struct FsmInst *fi, int event, void *arg) |
|
{ |
|
struct teimgr *tm = fi->userdata; |
|
u_char *dp = arg; |
|
int tei; |
|
|
|
tei = *(dp + 3) >> 1; |
|
if (*debug & DEBUG_L2_TEI) |
|
tm->tei_m.printdebug(fi, "identity check req tei %d", tei); |
|
if ((tm->l2->tei != GROUP_TEI) && ((tei == GROUP_TEI) || |
|
(tei == tm->l2->tei))) { |
|
mISDN_FsmDelTimer(&tm->timer, 4); |
|
mISDN_FsmChangeState(&tm->tei_m, ST_TEI_NOP); |
|
put_tei_msg(tm->mgr, ID_CHK_RES, random_ri(), tm->l2->tei); |
|
} |
|
} |
|
|
|
static void |
|
tei_id_remove(struct FsmInst *fi, int event, void *arg) |
|
{ |
|
struct teimgr *tm = fi->userdata; |
|
u_char *dp = arg; |
|
int tei; |
|
|
|
tei = *(dp + 3) >> 1; |
|
if (*debug & DEBUG_L2_TEI) |
|
tm->tei_m.printdebug(fi, "identity remove tei %d", tei); |
|
if ((tm->l2->tei != GROUP_TEI) && |
|
((tei == GROUP_TEI) || (tei == tm->l2->tei))) { |
|
mISDN_FsmDelTimer(&tm->timer, 5); |
|
mISDN_FsmChangeState(&tm->tei_m, ST_TEI_NOP); |
|
tei_l2(tm->l2, MDL_REMOVE_REQ, 0); |
|
} |
|
} |
|
|
|
static void |
|
tei_id_verify(struct FsmInst *fi, int event, void *arg) |
|
{ |
|
struct teimgr *tm = fi->userdata; |
|
|
|
if (*debug & DEBUG_L2_TEI) |
|
tm->tei_m.printdebug(fi, "id verify request for tei %d", |
|
tm->l2->tei); |
|
put_tei_msg(tm->mgr, ID_VERIFY, 0, tm->l2->tei); |
|
mISDN_FsmChangeState(&tm->tei_m, ST_TEI_IDVERIFY); |
|
mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 2); |
|
tm->nval = 2; |
|
} |
|
|
|
static void |
|
tei_id_req_tout(struct FsmInst *fi, int event, void *arg) |
|
{ |
|
struct teimgr *tm = fi->userdata; |
|
|
|
if (--tm->nval) { |
|
tm->ri = random_ri(); |
|
if (*debug & DEBUG_L2_TEI) |
|
tm->tei_m.printdebug(fi, "assign req(%d) ri %d", |
|
4 - tm->nval, tm->ri); |
|
put_tei_msg(tm->mgr, ID_REQUEST, tm->ri, GROUP_TEI); |
|
mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 3); |
|
} else { |
|
tm->tei_m.printdebug(fi, "assign req failed"); |
|
tei_l2(tm->l2, MDL_ERROR_RSP, 0); |
|
mISDN_FsmChangeState(fi, ST_TEI_NOP); |
|
} |
|
} |
|
|
|
static void |
|
tei_id_ver_tout(struct FsmInst *fi, int event, void *arg) |
|
{ |
|
struct teimgr *tm = fi->userdata; |
|
|
|
if (--tm->nval) { |
|
if (*debug & DEBUG_L2_TEI) |
|
tm->tei_m.printdebug(fi, |
|
"id verify req(%d) for tei %d", |
|
3 - tm->nval, tm->l2->tei); |
|
put_tei_msg(tm->mgr, ID_VERIFY, 0, tm->l2->tei); |
|
mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 4); |
|
} else { |
|
tm->tei_m.printdebug(fi, "verify req for tei %d failed", |
|
tm->l2->tei); |
|
tei_l2(tm->l2, MDL_REMOVE_REQ, 0); |
|
mISDN_FsmChangeState(fi, ST_TEI_NOP); |
|
} |
|
} |
|
|
|
static struct FsmNode TeiFnListUser[] = |
|
{ |
|
{ST_TEI_NOP, EV_IDREQ, tei_id_request}, |
|
{ST_TEI_NOP, EV_ASSIGN, tei_id_test_dup}, |
|
{ST_TEI_NOP, EV_VERIFY, tei_id_verify}, |
|
{ST_TEI_NOP, EV_REMOVE, tei_id_remove}, |
|
{ST_TEI_NOP, EV_CHKREQ, tei_id_chk_req}, |
|
{ST_TEI_IDREQ, EV_TIMER, tei_id_req_tout}, |
|
{ST_TEI_IDREQ, EV_ASSIGN, tei_id_assign}, |
|
{ST_TEI_IDREQ, EV_DENIED, tei_id_denied}, |
|
{ST_TEI_IDVERIFY, EV_TIMER, tei_id_ver_tout}, |
|
{ST_TEI_IDVERIFY, EV_REMOVE, tei_id_remove}, |
|
{ST_TEI_IDVERIFY, EV_CHKREQ, tei_id_chk_req}, |
|
}; |
|
|
|
static void |
|
tei_l2remove(struct layer2 *l2) |
|
{ |
|
put_tei_msg(l2->tm->mgr, ID_REMOVE, 0, l2->tei); |
|
tei_l2(l2, MDL_REMOVE_REQ, 0); |
|
list_del(&l2->ch.list); |
|
l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL); |
|
} |
|
|
|
static void |
|
tei_assign_req(struct FsmInst *fi, int event, void *arg) |
|
{ |
|
struct teimgr *tm = fi->userdata; |
|
u_char *dp = arg; |
|
|
|
if (tm->l2->tei == GROUP_TEI) { |
|
tm->tei_m.printdebug(&tm->tei_m, |
|
"net tei assign request without tei"); |
|
return; |
|
} |
|
tm->ri = ((unsigned int) *dp++ << 8); |
|
tm->ri += *dp++; |
|
if (*debug & DEBUG_L2_TEI) |
|
tm->tei_m.printdebug(&tm->tei_m, |
|
"net assign request ri %d teim %d", tm->ri, *dp); |
|
put_tei_msg(tm->mgr, ID_ASSIGNED, tm->ri, tm->l2->tei); |
|
mISDN_FsmChangeState(fi, ST_TEI_NOP); |
|
} |
|
|
|
static void |
|
tei_id_chk_req_net(struct FsmInst *fi, int event, void *arg) |
|
{ |
|
struct teimgr *tm = fi->userdata; |
|
|
|
if (*debug & DEBUG_L2_TEI) |
|
tm->tei_m.printdebug(fi, "id check request for tei %d", |
|
tm->l2->tei); |
|
tm->rcnt = 0; |
|
put_tei_msg(tm->mgr, ID_CHK_REQ, 0, tm->l2->tei); |
|
mISDN_FsmChangeState(&tm->tei_m, ST_TEI_IDVERIFY); |
|
mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 2); |
|
tm->nval = 2; |
|
} |
|
|
|
static void |
|
tei_id_chk_resp(struct FsmInst *fi, int event, void *arg) |
|
{ |
|
struct teimgr *tm = fi->userdata; |
|
u_char *dp = arg; |
|
int tei; |
|
|
|
tei = dp[3] >> 1; |
|
if (*debug & DEBUG_L2_TEI) |
|
tm->tei_m.printdebug(fi, "identity check resp tei %d", tei); |
|
if (tei == tm->l2->tei) |
|
tm->rcnt++; |
|
} |
|
|
|
static void |
|
tei_id_verify_net(struct FsmInst *fi, int event, void *arg) |
|
{ |
|
struct teimgr *tm = fi->userdata; |
|
u_char *dp = arg; |
|
int tei; |
|
|
|
tei = dp[3] >> 1; |
|
if (*debug & DEBUG_L2_TEI) |
|
tm->tei_m.printdebug(fi, "identity verify req tei %d/%d", |
|
tei, tm->l2->tei); |
|
if (tei == tm->l2->tei) |
|
tei_id_chk_req_net(fi, event, arg); |
|
} |
|
|
|
static void |
|
tei_id_ver_tout_net(struct FsmInst *fi, int event, void *arg) |
|
{ |
|
struct teimgr *tm = fi->userdata; |
|
|
|
if (tm->rcnt == 1) { |
|
if (*debug & DEBUG_L2_TEI) |
|
tm->tei_m.printdebug(fi, |
|
"check req for tei %d successful\n", tm->l2->tei); |
|
mISDN_FsmChangeState(fi, ST_TEI_NOP); |
|
} else if (tm->rcnt > 1) { |
|
/* duplicate assignment; remove */ |
|
tei_l2remove(tm->l2); |
|
} else if (--tm->nval) { |
|
if (*debug & DEBUG_L2_TEI) |
|
tm->tei_m.printdebug(fi, |
|
"id check req(%d) for tei %d", |
|
3 - tm->nval, tm->l2->tei); |
|
put_tei_msg(tm->mgr, ID_CHK_REQ, 0, tm->l2->tei); |
|
mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 4); |
|
} else { |
|
tm->tei_m.printdebug(fi, "check req for tei %d failed", |
|
tm->l2->tei); |
|
mISDN_FsmChangeState(fi, ST_TEI_NOP); |
|
tei_l2remove(tm->l2); |
|
} |
|
} |
|
|
|
static struct FsmNode TeiFnListNet[] = |
|
{ |
|
{ST_TEI_NOP, EV_ASSIGN_REQ, tei_assign_req}, |
|
{ST_TEI_NOP, EV_VERIFY, tei_id_verify_net}, |
|
{ST_TEI_NOP, EV_CHKREQ, tei_id_chk_req_net}, |
|
{ST_TEI_IDVERIFY, EV_TIMER, tei_id_ver_tout_net}, |
|
{ST_TEI_IDVERIFY, EV_CHKRESP, tei_id_chk_resp}, |
|
}; |
|
|
|
static void |
|
tei_ph_data_ind(struct teimgr *tm, u_int mt, u_char *dp, int len) |
|
{ |
|
if (test_bit(FLG_FIXED_TEI, &tm->l2->flag)) |
|
return; |
|
if (*debug & DEBUG_L2_TEI) |
|
tm->tei_m.printdebug(&tm->tei_m, "tei handler mt %x", mt); |
|
if (mt == ID_ASSIGNED) |
|
mISDN_FsmEvent(&tm->tei_m, EV_ASSIGN, dp); |
|
else if (mt == ID_DENIED) |
|
mISDN_FsmEvent(&tm->tei_m, EV_DENIED, dp); |
|
else if (mt == ID_CHK_REQ) |
|
mISDN_FsmEvent(&tm->tei_m, EV_CHKREQ, dp); |
|
else if (mt == ID_REMOVE) |
|
mISDN_FsmEvent(&tm->tei_m, EV_REMOVE, dp); |
|
else if (mt == ID_VERIFY) |
|
mISDN_FsmEvent(&tm->tei_m, EV_VERIFY, dp); |
|
else if (mt == ID_CHK_RES) |
|
mISDN_FsmEvent(&tm->tei_m, EV_CHKRESP, dp); |
|
} |
|
|
|
static struct layer2 * |
|
create_new_tei(struct manager *mgr, int tei, int sapi) |
|
{ |
|
unsigned long opt = 0; |
|
unsigned long flags; |
|
int id; |
|
struct layer2 *l2; |
|
struct channel_req rq; |
|
|
|
if (!mgr->up) |
|
return NULL; |
|
if ((tei >= 0) && (tei < 64)) |
|
test_and_set_bit(OPTION_L2_FIXEDTEI, &opt); |
|
if (mgr->ch.st->dev->Dprotocols & ((1 << ISDN_P_TE_E1) | |
|
(1 << ISDN_P_NT_E1))) { |
|
test_and_set_bit(OPTION_L2_PMX, &opt); |
|
rq.protocol = ISDN_P_NT_E1; |
|
} else { |
|
rq.protocol = ISDN_P_NT_S0; |
|
} |
|
l2 = create_l2(mgr->up, ISDN_P_LAPD_NT, opt, tei, sapi); |
|
if (!l2) { |
|
printk(KERN_WARNING "%s:no memory for layer2\n", __func__); |
|
return NULL; |
|
} |
|
l2->tm = kzalloc(sizeof(struct teimgr), GFP_KERNEL); |
|
if (!l2->tm) { |
|
kfree(l2); |
|
printk(KERN_WARNING "%s:no memory for teimgr\n", __func__); |
|
return NULL; |
|
} |
|
l2->tm->mgr = mgr; |
|
l2->tm->l2 = l2; |
|
l2->tm->tei_m.debug = *debug & DEBUG_L2_TEIFSM; |
|
l2->tm->tei_m.userdata = l2->tm; |
|
l2->tm->tei_m.printdebug = tei_debug; |
|
l2->tm->tei_m.fsm = &teifsmn; |
|
l2->tm->tei_m.state = ST_TEI_NOP; |
|
l2->tm->tval = 2000; /* T202 2 sec */ |
|
mISDN_FsmInitTimer(&l2->tm->tei_m, &l2->tm->timer); |
|
write_lock_irqsave(&mgr->lock, flags); |
|
id = get_free_id(mgr); |
|
list_add_tail(&l2->list, &mgr->layer2); |
|
write_unlock_irqrestore(&mgr->lock, flags); |
|
if (id < 0) { |
|
l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL); |
|
printk(KERN_WARNING "%s:no free id\n", __func__); |
|
return NULL; |
|
} else { |
|
l2->ch.nr = id; |
|
__add_layer2(&l2->ch, mgr->ch.st); |
|
l2->ch.recv = mgr->ch.recv; |
|
l2->ch.peer = mgr->ch.peer; |
|
l2->ch.ctrl(&l2->ch, OPEN_CHANNEL, NULL); |
|
/* We need open here L1 for the manager as well (refcounting) */ |
|
rq.adr.dev = mgr->ch.st->dev->id; |
|
id = mgr->ch.st->own.ctrl(&mgr->ch.st->own, OPEN_CHANNEL, &rq); |
|
if (id < 0) { |
|
printk(KERN_WARNING "%s: cannot open L1\n", __func__); |
|
l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL); |
|
l2 = NULL; |
|
} |
|
} |
|
return l2; |
|
} |
|
|
|
static void |
|
new_tei_req(struct manager *mgr, u_char *dp) |
|
{ |
|
int tei, ri; |
|
struct layer2 *l2; |
|
|
|
ri = dp[0] << 8; |
|
ri += dp[1]; |
|
if (!mgr->up) |
|
goto denied; |
|
if (!(dp[3] & 1)) /* Extension bit != 1 */ |
|
goto denied; |
|
if (dp[3] != 0xff) |
|
tei = dp[3] >> 1; /* 3GPP TS 08.56 6.1.11.2 */ |
|
else |
|
tei = get_free_tei(mgr); |
|
if (tei < 0) { |
|
printk(KERN_WARNING "%s:No free tei\n", __func__); |
|
goto denied; |
|
} |
|
l2 = create_new_tei(mgr, tei, CTRL_SAPI); |
|
if (!l2) |
|
goto denied; |
|
else |
|
mISDN_FsmEvent(&l2->tm->tei_m, EV_ASSIGN_REQ, dp); |
|
return; |
|
denied: |
|
put_tei_msg(mgr, ID_DENIED, ri, GROUP_TEI); |
|
} |
|
|
|
static int |
|
ph_data_ind(struct manager *mgr, struct sk_buff *skb) |
|
{ |
|
int ret = -EINVAL; |
|
struct layer2 *l2, *nl2; |
|
u_char mt; |
|
|
|
if (skb->len < 8) { |
|
if (*debug & DEBUG_L2_TEI) |
|
printk(KERN_DEBUG "%s: short mgr frame %d/8\n", |
|
__func__, skb->len); |
|
goto done; |
|
} |
|
|
|
if ((skb->data[0] >> 2) != TEI_SAPI) /* not for us */ |
|
goto done; |
|
if (skb->data[0] & 1) /* EA0 formal error */ |
|
goto done; |
|
if (!(skb->data[1] & 1)) /* EA1 formal error */ |
|
goto done; |
|
if ((skb->data[1] >> 1) != GROUP_TEI) /* not for us */ |
|
goto done; |
|
if ((skb->data[2] & 0xef) != UI) /* not UI */ |
|
goto done; |
|
if (skb->data[3] != TEI_ENTITY_ID) /* not tei entity */ |
|
goto done; |
|
mt = skb->data[6]; |
|
switch (mt) { |
|
case ID_REQUEST: |
|
case ID_CHK_RES: |
|
case ID_VERIFY: |
|
if (!test_bit(MGR_OPT_NETWORK, &mgr->options)) |
|
goto done; |
|
break; |
|
case ID_ASSIGNED: |
|
case ID_DENIED: |
|
case ID_CHK_REQ: |
|
case ID_REMOVE: |
|
if (test_bit(MGR_OPT_NETWORK, &mgr->options)) |
|
goto done; |
|
break; |
|
default: |
|
goto done; |
|
} |
|
ret = 0; |
|
if (mt == ID_REQUEST) { |
|
new_tei_req(mgr, &skb->data[4]); |
|
goto done; |
|
} |
|
list_for_each_entry_safe(l2, nl2, &mgr->layer2, list) { |
|
tei_ph_data_ind(l2->tm, mt, &skb->data[4], skb->len - 4); |
|
} |
|
done: |
|
return ret; |
|
} |
|
|
|
int |
|
l2_tei(struct layer2 *l2, u_int cmd, u_long arg) |
|
{ |
|
struct teimgr *tm = l2->tm; |
|
|
|
if (test_bit(FLG_FIXED_TEI, &l2->flag)) |
|
return 0; |
|
if (*debug & DEBUG_L2_TEI) |
|
printk(KERN_DEBUG "%s: cmd(%x)\n", __func__, cmd); |
|
switch (cmd) { |
|
case MDL_ASSIGN_IND: |
|
mISDN_FsmEvent(&tm->tei_m, EV_IDREQ, NULL); |
|
break; |
|
case MDL_ERROR_IND: |
|
if (test_bit(MGR_OPT_NETWORK, &tm->mgr->options)) |
|
mISDN_FsmEvent(&tm->tei_m, EV_CHKREQ, &l2->tei); |
|
if (test_bit(MGR_OPT_USER, &tm->mgr->options)) |
|
mISDN_FsmEvent(&tm->tei_m, EV_VERIFY, NULL); |
|
break; |
|
case MDL_STATUS_UP_IND: |
|
if (test_bit(MGR_OPT_NETWORK, &tm->mgr->options)) |
|
mISDN_FsmEvent(&tm->mgr->deact, EV_ACTIVATE, NULL); |
|
break; |
|
case MDL_STATUS_DOWN_IND: |
|
if (test_bit(MGR_OPT_NETWORK, &tm->mgr->options)) |
|
mISDN_FsmEvent(&tm->mgr->deact, EV_DEACTIVATE, NULL); |
|
break; |
|
case MDL_STATUS_UI_IND: |
|
if (test_bit(MGR_OPT_NETWORK, &tm->mgr->options)) |
|
mISDN_FsmEvent(&tm->mgr->deact, EV_UI, NULL); |
|
break; |
|
} |
|
return 0; |
|
} |
|
|
|
void |
|
TEIrelease(struct layer2 *l2) |
|
{ |
|
struct teimgr *tm = l2->tm; |
|
u_long flags; |
|
|
|
mISDN_FsmDelTimer(&tm->timer, 1); |
|
write_lock_irqsave(&tm->mgr->lock, flags); |
|
list_del(&l2->list); |
|
write_unlock_irqrestore(&tm->mgr->lock, flags); |
|
l2->tm = NULL; |
|
kfree(tm); |
|
} |
|
|
|
static int |
|
create_teimgr(struct manager *mgr, struct channel_req *crq) |
|
{ |
|
struct layer2 *l2; |
|
unsigned long opt = 0; |
|
unsigned long flags; |
|
int id; |
|
struct channel_req l1rq; |
|
|
|
if (*debug & DEBUG_L2_TEI) |
|
printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n", |
|
__func__, dev_name(&mgr->ch.st->dev->dev), |
|
crq->protocol, crq->adr.dev, crq->adr.channel, |
|
crq->adr.sapi, crq->adr.tei); |
|
if (crq->adr.tei > GROUP_TEI) |
|
return -EINVAL; |
|
if (crq->adr.tei < 64) |
|
test_and_set_bit(OPTION_L2_FIXEDTEI, &opt); |
|
if (crq->adr.tei == 0) |
|
test_and_set_bit(OPTION_L2_PTP, &opt); |
|
if (test_bit(MGR_OPT_NETWORK, &mgr->options)) { |
|
if (crq->protocol == ISDN_P_LAPD_TE) |
|
return -EPROTONOSUPPORT; |
|
if ((crq->adr.tei != 0) && (crq->adr.tei != 127)) |
|
return -EINVAL; |
|
if (mgr->up) { |
|
printk(KERN_WARNING |
|
"%s: only one network manager is allowed\n", |
|
__func__); |
|
return -EBUSY; |
|
} |
|
} else if (test_bit(MGR_OPT_USER, &mgr->options)) { |
|
if (crq->protocol == ISDN_P_LAPD_NT) |
|
return -EPROTONOSUPPORT; |
|
if ((crq->adr.tei >= 64) && (crq->adr.tei < GROUP_TEI)) |
|
return -EINVAL; /* dyn tei */ |
|
} else { |
|
if (crq->protocol == ISDN_P_LAPD_NT) |
|
test_and_set_bit(MGR_OPT_NETWORK, &mgr->options); |
|
if (crq->protocol == ISDN_P_LAPD_TE) |
|
test_and_set_bit(MGR_OPT_USER, &mgr->options); |
|
} |
|
l1rq.adr = crq->adr; |
|
if (mgr->ch.st->dev->Dprotocols |
|
& ((1 << ISDN_P_TE_E1) | (1 << ISDN_P_NT_E1))) |
|
test_and_set_bit(OPTION_L2_PMX, &opt); |
|
if ((crq->protocol == ISDN_P_LAPD_NT) && (crq->adr.tei == 127)) { |
|
mgr->up = crq->ch; |
|
id = DL_INFO_L2_CONNECT; |
|
teiup_create(mgr, DL_INFORMATION_IND, sizeof(id), &id); |
|
if (test_bit(MGR_PH_ACTIVE, &mgr->options)) |
|
teiup_create(mgr, PH_ACTIVATE_IND, 0, NULL); |
|
crq->ch = NULL; |
|
if (!list_empty(&mgr->layer2)) { |
|
read_lock_irqsave(&mgr->lock, flags); |
|
list_for_each_entry(l2, &mgr->layer2, list) { |
|
l2->up = mgr->up; |
|
l2->ch.ctrl(&l2->ch, OPEN_CHANNEL, NULL); |
|
} |
|
read_unlock_irqrestore(&mgr->lock, flags); |
|
} |
|
return 0; |
|
} |
|
l2 = create_l2(crq->ch, crq->protocol, opt, |
|
crq->adr.tei, crq->adr.sapi); |
|
if (!l2) |
|
return -ENOMEM; |
|
l2->tm = kzalloc(sizeof(struct teimgr), GFP_KERNEL); |
|
if (!l2->tm) { |
|
kfree(l2); |
|
printk(KERN_ERR "kmalloc teimgr failed\n"); |
|
return -ENOMEM; |
|
} |
|
l2->tm->mgr = mgr; |
|
l2->tm->l2 = l2; |
|
l2->tm->tei_m.debug = *debug & DEBUG_L2_TEIFSM; |
|
l2->tm->tei_m.userdata = l2->tm; |
|
l2->tm->tei_m.printdebug = tei_debug; |
|
if (crq->protocol == ISDN_P_LAPD_TE) { |
|
l2->tm->tei_m.fsm = &teifsmu; |
|
l2->tm->tei_m.state = ST_TEI_NOP; |
|
l2->tm->tval = 1000; /* T201 1 sec */ |
|
if (test_bit(OPTION_L2_PMX, &opt)) |
|
l1rq.protocol = ISDN_P_TE_E1; |
|
else |
|
l1rq.protocol = ISDN_P_TE_S0; |
|
} else { |
|
l2->tm->tei_m.fsm = &teifsmn; |
|
l2->tm->tei_m.state = ST_TEI_NOP; |
|
l2->tm->tval = 2000; /* T202 2 sec */ |
|
if (test_bit(OPTION_L2_PMX, &opt)) |
|
l1rq.protocol = ISDN_P_NT_E1; |
|
else |
|
l1rq.protocol = ISDN_P_NT_S0; |
|
} |
|
mISDN_FsmInitTimer(&l2->tm->tei_m, &l2->tm->timer); |
|
write_lock_irqsave(&mgr->lock, flags); |
|
id = get_free_id(mgr); |
|
list_add_tail(&l2->list, &mgr->layer2); |
|
write_unlock_irqrestore(&mgr->lock, flags); |
|
if (id >= 0) { |
|
l2->ch.nr = id; |
|
l2->up->nr = id; |
|
crq->ch = &l2->ch; |
|
/* We need open here L1 for the manager as well (refcounting) */ |
|
id = mgr->ch.st->own.ctrl(&mgr->ch.st->own, OPEN_CHANNEL, |
|
&l1rq); |
|
} |
|
if (id < 0) |
|
l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL); |
|
return id; |
|
} |
|
|
|
static int |
|
mgr_send(struct mISDNchannel *ch, struct sk_buff *skb) |
|
{ |
|
struct manager *mgr; |
|
struct mISDNhead *hh = mISDN_HEAD_P(skb); |
|
int ret = -EINVAL; |
|
|
|
mgr = container_of(ch, struct manager, ch); |
|
if (*debug & DEBUG_L2_RECV) |
|
printk(KERN_DEBUG "%s: prim(%x) id(%x)\n", |
|
__func__, hh->prim, hh->id); |
|
switch (hh->prim) { |
|
case PH_DATA_IND: |
|
mISDN_FsmEvent(&mgr->deact, EV_UI, NULL); |
|
ret = ph_data_ind(mgr, skb); |
|
break; |
|
case PH_DATA_CNF: |
|
do_ack(mgr, hh->id); |
|
ret = 0; |
|
break; |
|
case PH_ACTIVATE_IND: |
|
test_and_set_bit(MGR_PH_ACTIVE, &mgr->options); |
|
if (mgr->up) |
|
teiup_create(mgr, PH_ACTIVATE_IND, 0, NULL); |
|
mISDN_FsmEvent(&mgr->deact, EV_ACTIVATE_IND, NULL); |
|
do_send(mgr); |
|
ret = 0; |
|
break; |
|
case PH_DEACTIVATE_IND: |
|
test_and_clear_bit(MGR_PH_ACTIVE, &mgr->options); |
|
if (mgr->up) |
|
teiup_create(mgr, PH_DEACTIVATE_IND, 0, NULL); |
|
mISDN_FsmEvent(&mgr->deact, EV_DEACTIVATE_IND, NULL); |
|
ret = 0; |
|
break; |
|
case DL_UNITDATA_REQ: |
|
return dl_unit_data(mgr, skb); |
|
} |
|
if (!ret) |
|
dev_kfree_skb(skb); |
|
return ret; |
|
} |
|
|
|
static int |
|
free_teimanager(struct manager *mgr) |
|
{ |
|
struct layer2 *l2, *nl2; |
|
|
|
test_and_clear_bit(OPTION_L1_HOLD, &mgr->options); |
|
if (test_bit(MGR_OPT_NETWORK, &mgr->options)) { |
|
/* not locked lock is taken in release tei */ |
|
mgr->up = NULL; |
|
if (test_bit(OPTION_L2_CLEANUP, &mgr->options)) { |
|
list_for_each_entry_safe(l2, nl2, &mgr->layer2, list) { |
|
put_tei_msg(mgr, ID_REMOVE, 0, l2->tei); |
|
mutex_lock(&mgr->ch.st->lmutex); |
|
list_del(&l2->ch.list); |
|
mutex_unlock(&mgr->ch.st->lmutex); |
|
l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL); |
|
} |
|
test_and_clear_bit(MGR_OPT_NETWORK, &mgr->options); |
|
} else { |
|
list_for_each_entry_safe(l2, nl2, &mgr->layer2, list) { |
|
l2->up = NULL; |
|
} |
|
} |
|
} |
|
if (test_bit(MGR_OPT_USER, &mgr->options)) { |
|
if (list_empty(&mgr->layer2)) |
|
test_and_clear_bit(MGR_OPT_USER, &mgr->options); |
|
} |
|
mgr->ch.st->dev->D.ctrl(&mgr->ch.st->dev->D, CLOSE_CHANNEL, NULL); |
|
return 0; |
|
} |
|
|
|
static int |
|
ctrl_teimanager(struct manager *mgr, void *arg) |
|
{ |
|
/* currently we only have one option */ |
|
unsigned int *val = (unsigned int *)arg; |
|
|
|
switch (val[0]) { |
|
case IMCLEAR_L2: |
|
if (val[1]) |
|
test_and_set_bit(OPTION_L2_CLEANUP, &mgr->options); |
|
else |
|
test_and_clear_bit(OPTION_L2_CLEANUP, &mgr->options); |
|
break; |
|
case IMHOLD_L1: |
|
if (val[1]) |
|
test_and_set_bit(OPTION_L1_HOLD, &mgr->options); |
|
else |
|
test_and_clear_bit(OPTION_L1_HOLD, &mgr->options); |
|
break; |
|
default: |
|
return -EINVAL; |
|
} |
|
return 0; |
|
} |
|
|
|
/* This function does create a L2 for fixed TEI in NT Mode */ |
|
static int |
|
check_data(struct manager *mgr, struct sk_buff *skb) |
|
{ |
|
struct mISDNhead *hh = mISDN_HEAD_P(skb); |
|
int ret, tei, sapi; |
|
struct layer2 *l2; |
|
|
|
if (*debug & DEBUG_L2_CTRL) |
|
printk(KERN_DEBUG "%s: prim(%x) id(%x)\n", |
|
__func__, hh->prim, hh->id); |
|
if (test_bit(MGR_OPT_USER, &mgr->options)) |
|
return -ENOTCONN; |
|
if (hh->prim != PH_DATA_IND) |
|
return -ENOTCONN; |
|
if (skb->len != 3) |
|
return -ENOTCONN; |
|
if (skb->data[0] & 3) /* EA0 and CR must be 0 */ |
|
return -EINVAL; |
|
sapi = skb->data[0] >> 2; |
|
if (!(skb->data[1] & 1)) /* invalid EA1 */ |
|
return -EINVAL; |
|
tei = skb->data[1] >> 1; |
|
if (tei > 63) /* not a fixed tei */ |
|
return -ENOTCONN; |
|
if ((skb->data[2] & ~0x10) != SABME) |
|
return -ENOTCONN; |
|
/* We got a SABME for a fixed TEI */ |
|
if (*debug & DEBUG_L2_CTRL) |
|
printk(KERN_DEBUG "%s: SABME sapi(%d) tei(%d)\n", |
|
__func__, sapi, tei); |
|
l2 = create_new_tei(mgr, tei, sapi); |
|
if (!l2) { |
|
if (*debug & DEBUG_L2_CTRL) |
|
printk(KERN_DEBUG "%s: failed to create new tei\n", |
|
__func__); |
|
return -ENOMEM; |
|
} |
|
ret = l2->ch.send(&l2->ch, skb); |
|
return ret; |
|
} |
|
|
|
void |
|
delete_teimanager(struct mISDNchannel *ch) |
|
{ |
|
struct manager *mgr; |
|
struct layer2 *l2, *nl2; |
|
|
|
mgr = container_of(ch, struct manager, ch); |
|
/* not locked lock is taken in release tei */ |
|
list_for_each_entry_safe(l2, nl2, &mgr->layer2, list) { |
|
mutex_lock(&mgr->ch.st->lmutex); |
|
list_del(&l2->ch.list); |
|
mutex_unlock(&mgr->ch.st->lmutex); |
|
l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL); |
|
} |
|
list_del(&mgr->ch.list); |
|
list_del(&mgr->bcast.list); |
|
skb_queue_purge(&mgr->sendq); |
|
kfree(mgr); |
|
} |
|
|
|
static int |
|
mgr_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg) |
|
{ |
|
struct manager *mgr; |
|
int ret = -EINVAL; |
|
|
|
mgr = container_of(ch, struct manager, ch); |
|
if (*debug & DEBUG_L2_CTRL) |
|
printk(KERN_DEBUG "%s(%x, %p)\n", __func__, cmd, arg); |
|
switch (cmd) { |
|
case OPEN_CHANNEL: |
|
ret = create_teimgr(mgr, arg); |
|
break; |
|
case CLOSE_CHANNEL: |
|
ret = free_teimanager(mgr); |
|
break; |
|
case CONTROL_CHANNEL: |
|
ret = ctrl_teimanager(mgr, arg); |
|
break; |
|
case CHECK_DATA: |
|
ret = check_data(mgr, arg); |
|
break; |
|
} |
|
return ret; |
|
} |
|
|
|
static int |
|
mgr_bcast(struct mISDNchannel *ch, struct sk_buff *skb) |
|
{ |
|
struct manager *mgr = container_of(ch, struct manager, bcast); |
|
struct mISDNhead *hhc, *hh = mISDN_HEAD_P(skb); |
|
struct sk_buff *cskb = NULL; |
|
struct layer2 *l2; |
|
u_long flags; |
|
int ret; |
|
|
|
read_lock_irqsave(&mgr->lock, flags); |
|
list_for_each_entry(l2, &mgr->layer2, list) { |
|
if ((hh->id & MISDN_ID_SAPI_MASK) == |
|
(l2->ch.addr & MISDN_ID_SAPI_MASK)) { |
|
if (list_is_last(&l2->list, &mgr->layer2)) { |
|
cskb = skb; |
|
skb = NULL; |
|
} else { |
|
if (!cskb) |
|
cskb = skb_copy(skb, GFP_ATOMIC); |
|
} |
|
if (cskb) { |
|
hhc = mISDN_HEAD_P(cskb); |
|
/* save original header behind normal header */ |
|
hhc++; |
|
*hhc = *hh; |
|
hhc--; |
|
hhc->prim = DL_INTERN_MSG; |
|
hhc->id = l2->ch.nr; |
|
ret = ch->st->own.recv(&ch->st->own, cskb); |
|
if (ret) { |
|
if (*debug & DEBUG_SEND_ERR) |
|
printk(KERN_DEBUG |
|
"%s ch%d prim(%x) addr(%x)" |
|
" err %d\n", |
|
__func__, l2->ch.nr, |
|
hh->prim, l2->ch.addr, ret); |
|
} else |
|
cskb = NULL; |
|
} else { |
|
printk(KERN_WARNING "%s ch%d addr %x no mem\n", |
|
__func__, ch->nr, ch->addr); |
|
goto out; |
|
} |
|
} |
|
} |
|
out: |
|
read_unlock_irqrestore(&mgr->lock, flags); |
|
dev_kfree_skb(cskb); |
|
dev_kfree_skb(skb); |
|
return 0; |
|
} |
|
|
|
static int |
|
mgr_bcast_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg) |
|
{ |
|
|
|
return -EINVAL; |
|
} |
|
|
|
int |
|
create_teimanager(struct mISDNdevice *dev) |
|
{ |
|
struct manager *mgr; |
|
|
|
mgr = kzalloc(sizeof(struct manager), GFP_KERNEL); |
|
if (!mgr) |
|
return -ENOMEM; |
|
INIT_LIST_HEAD(&mgr->layer2); |
|
rwlock_init(&mgr->lock); |
|
skb_queue_head_init(&mgr->sendq); |
|
mgr->nextid = 1; |
|
mgr->lastid = MISDN_ID_NONE; |
|
mgr->ch.send = mgr_send; |
|
mgr->ch.ctrl = mgr_ctrl; |
|
mgr->ch.st = dev->D.st; |
|
set_channel_address(&mgr->ch, TEI_SAPI, GROUP_TEI); |
|
add_layer2(&mgr->ch, dev->D.st); |
|
mgr->bcast.send = mgr_bcast; |
|
mgr->bcast.ctrl = mgr_bcast_ctrl; |
|
mgr->bcast.st = dev->D.st; |
|
set_channel_address(&mgr->bcast, 0, GROUP_TEI); |
|
add_layer2(&mgr->bcast, dev->D.st); |
|
mgr->deact.debug = *debug & DEBUG_MANAGER; |
|
mgr->deact.userdata = mgr; |
|
mgr->deact.printdebug = da_debug; |
|
mgr->deact.fsm = &deactfsm; |
|
mgr->deact.state = ST_L1_DEACT; |
|
mISDN_FsmInitTimer(&mgr->deact, &mgr->datimer); |
|
dev->teimgr = &mgr->ch; |
|
return 0; |
|
} |
|
|
|
int TEIInit(u_int *deb) |
|
{ |
|
int res; |
|
debug = deb; |
|
teifsmu.state_count = TEI_STATE_COUNT; |
|
teifsmu.event_count = TEI_EVENT_COUNT; |
|
teifsmu.strEvent = strTeiEvent; |
|
teifsmu.strState = strTeiState; |
|
res = mISDN_FsmNew(&teifsmu, TeiFnListUser, ARRAY_SIZE(TeiFnListUser)); |
|
if (res) |
|
goto error; |
|
teifsmn.state_count = TEI_STATE_COUNT; |
|
teifsmn.event_count = TEI_EVENT_COUNT; |
|
teifsmn.strEvent = strTeiEvent; |
|
teifsmn.strState = strTeiState; |
|
res = mISDN_FsmNew(&teifsmn, TeiFnListNet, ARRAY_SIZE(TeiFnListNet)); |
|
if (res) |
|
goto error_smn; |
|
deactfsm.state_count = DEACT_STATE_COUNT; |
|
deactfsm.event_count = DEACT_EVENT_COUNT; |
|
deactfsm.strEvent = strDeactEvent; |
|
deactfsm.strState = strDeactState; |
|
res = mISDN_FsmNew(&deactfsm, DeactFnList, ARRAY_SIZE(DeactFnList)); |
|
if (res) |
|
goto error_deact; |
|
return 0; |
|
|
|
error_deact: |
|
mISDN_FsmFree(&teifsmn); |
|
error_smn: |
|
mISDN_FsmFree(&teifsmu); |
|
error: |
|
return res; |
|
} |
|
|
|
void TEIFree(void) |
|
{ |
|
mISDN_FsmFree(&teifsmu); |
|
mISDN_FsmFree(&teifsmn); |
|
mISDN_FsmFree(&deactfsm); |
|
}
|
|
|