/* * * Author Karsten Keil * * Copyright 2008 by Karsten Keil * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License version 2 as * published by the Free Software Foundation. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * */ #include "layer2.h" #include #include #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) memcpy(skb_put(skb, len), 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 */ int *val = (int *)arg; int ret = 0; 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: ret = -EINVAL; } return ret; } /* 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); if (cskb) dev_kfree_skb(cskb); if (skb) 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) { debug = deb; teifsmu.state_count = TEI_STATE_COUNT; teifsmu.event_count = TEI_EVENT_COUNT; teifsmu.strEvent = strTeiEvent; teifsmu.strState = strTeiState; mISDN_FsmNew(&teifsmu, TeiFnListUser, ARRAY_SIZE(TeiFnListUser)); teifsmn.state_count = TEI_STATE_COUNT; teifsmn.event_count = TEI_EVENT_COUNT; teifsmn.strEvent = strTeiEvent; teifsmn.strState = strTeiState; mISDN_FsmNew(&teifsmn, TeiFnListNet, ARRAY_SIZE(TeiFnListNet)); deactfsm.state_count = DEACT_STATE_COUNT; deactfsm.event_count = DEACT_EVENT_COUNT; deactfsm.strEvent = strDeactEvent; deactfsm.strState = strDeactState; mISDN_FsmNew(&deactfsm, DeactFnList, ARRAY_SIZE(DeactFnList)); return 0; } void TEIFree(void) { mISDN_FsmFree(&teifsmu); mISDN_FsmFree(&teifsmn); mISDN_FsmFree(&deactfsm); }