mtp.c

来自「asterisk 中国七号驱动修改代码可以在ASTERISIK-1。4上编译pa」· C语言 代码 · 共 2,101 行 · 第 1/5 页

	0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a,
	0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72,
	0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9,
	0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1,
	0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738,
	0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70,
	0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7,
	0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
	0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036,
	0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e,
	0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5,
	0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd,
	0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134,
	0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c,
	0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3,
	0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb,
	0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
	0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a,
	0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1,
	0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9,
	0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330,
	0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78
};

/* Queue an MSU (in the retransmit buffer) for sending down the link. */
static void mtp2_queue_msu(mtp2_t *m, int sio, unsigned char *sif, int len) {
  int i;

  if(m->state != MTP2_INSERVICE) {
    if(m->state != MTP2_READY) {
      fifo_log(m, LOG_DEBUG, "Got MSU (sio=%d), but link not in service, discarding on link '%s'.\n", sio, m->name);
      return;
    }
  }
  if(len < 2) {
    fifo_log(m, LOG_ERROR, "Got illegal MSU length %d < 2, dropping frame on link '%s'.\n", len, m->name);
    return;
  }

  i = MTP_NEXT_SEQ(m->retrans_last_sent);
  if(i == m->retrans_last_acked) {
    fifo_log(m, LOG_WARNING, "MTP retransmit buffer full, MSU lost on link '%s'.\n", m->name);
    return;
  }

  m->retrans_buf[i].buf[0] = 0; /* BSN Will be set correctly when transmitted */
  m->retrans_buf[i].buf[1] = 0; /* FSN Will be set correctly when transmitted */
  m->retrans_buf[i].buf[2] = (len >= 62 ? 63 : 1 + len);
  m->retrans_buf[i].buf[3] = sio;
  memcpy(&(m->retrans_buf[i].buf[4]), sif, len);
  m->retrans_buf[i].len = len + 4;
  m->retrans_last_sent = i;
  /* Start transmitting the new SU, unless we were already (re-)transmitting. */
  if(m->retrans_seq == -1) {
    m->retrans_seq = i;
    /* Start timer T7 "excessive delay of acknowledgement". */
    mtp2_t7_start(m);
  }
}

void mtp3_put_label(int sls, int opc, int dpc, unsigned char *buf) {
  buf[0] = dpc & 0xff;
  buf[1] = ((dpc & 0x3f00) >> 8) | ((opc & 0x0003) << 6);
  buf[2] = ((opc & 0x03fc) >> 2);
  buf[3] = ((opc & 0x3c00) >> 10) | (sls << 4);
}

static void mtp3_set_sls(int sls, unsigned char *buf) {
  buf[3] = (buf[3] & 0xff0f) | (sls << 4);
}

/* Handle Q.707 test-and-maintenance procedure.
   Send a periodic SLTM message, listen for SLTA.
*/
static int mtp3_send_sltm(void *data); /* For mutual recursion */
static int timeout_sltm_t1(void *data) {
  mtp2_t *m = data;

  if(m->sltm_tries == 1) {
    fifo_log(m, LOG_WARNING, "No SLTA received within Q.707 timer T1, trying again on link '%s'.\n", m->name);
    mtp3_send_sltm(m);
    m->sltm_tries = 2;
    return 1;                   /* Ask to run again on next timer expire */
  } else  {
    fifo_log(m, LOG_ERROR, "No SLTA received within Q.707 timer T1, faulting link on link '%s'.\n", m->name);
    m->sltm_t1 = -1;
    mtp3_link_fail(m, 0);
    return 0;                   /* Do not re-schedule */
  }
}

static unsigned char sltm_pattern[15] =
  { 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88,
    0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff };

static int mtp3_send_sltm(void *data) {
  unsigned char message_sltm[21];
  mtp2_t *m = data;
  int subservice = m->subservice;

  if (subservice == -1)
    subservice = 0x8;

  fifo_log(m, LOG_EVENT, "Sending SLTM to peer on link '%s'....\n", m->name);
  
  mtp3_put_label(m->sls, this_host->opc, linkpeerpc(m), message_sltm);
  message_sltm[4] = 0x11;       /* SLTM */
  message_sltm[5] = 0xf0;       /* Length: 15 */
  memcpy(&(message_sltm[6]), sltm_pattern, sizeof(sltm_pattern));
  mtp2_queue_msu(m, (subservice << 4) | 1, message_sltm, 6 + sizeof(sltm_pattern));

  /* Set up a timer to wait for SLTA. */
  if(m->sltm_t1 == -1) {        /* Only if it is not already running */
    m->sltm_t1 = mtp_sched_add(mtp2_sched, 9000, timeout_sltm_t1, m);
    m->sltm_tries = 1;
  }

  return 1;                     /* Ask to be run again on next timer expire */
}

/* Process a received link status signal unit. */
static void mtp2_process_lssu(mtp2_t *m, unsigned char *buf, int fsn, int fib) {
  int typ;

  typ = buf[3] & 0x07;
  switch(typ) {
    case 0:                   /* Status indication 'O' */
      if(m->state == MTP2_NOT_ALIGNED) {
        t2_stop(m);
        t3_start(m);
        m->state = MTP2_ALIGNED;
      } else if(m->state == MTP2_PROVING) {
        t4_stop(m);
        m->state = MTP2_ALIGNED;
      } else if(m->state == MTP2_READY) {
        abort_initial_alignment(m);
      } else if(m->state == MTP2_INSERVICE) {
	fifo_log(m, LOG_NOTICE, "Got status indication 'O' while INSERVICE on link %s.\n", m->name);
        mtp3_link_fail(m, 0);
      }
      break;

    case 1:                   /* Status indication 'N' */
    case 2:                   /* Status indication 'E' */
      /* ToDo: This shouldn't really be here, I think. */
      m->send_bsn = fsn;
      m->send_bib = fib;

      if(m->state == MTP2_NOT_ALIGNED) {
        t2_stop(m);
        t3_start(m);
        m->state = MTP2_ALIGNED;
      } else if(m->state == MTP2_ALIGNED) {
	fifo_log(m, LOG_DEBUG, "Entering proving state for link '%s'.\n", m->name);
        t3_stop(m);
        t4_start(m);
        m->state = MTP2_PROVING;
        m->error_rate_mon = 0;
        m->emon_ncount = 0;
      } else if(m->state == MTP2_INSERVICE) {
	fifo_log(m, LOG_NOTICE, "Got status indication 'N' or 'E' while INSERVICE on link '%s'.\n", m->name);
        mtp3_link_fail(m, 0);
      }
      break;

    case 3:                   /* Status indication 'OS' */
      if(m->state == MTP2_ALIGNED || m->state == MTP2_PROVING) {
        abort_initial_alignment(m);
      } else if(m->state == MTP2_READY) {
	fifo_log(m, LOG_NOTICE, "Got status indication 'OS' while READY on link '%s'.\n", m->name);
	mtp3_link_fail(m, 1);
      } else if(m->state == MTP2_INSERVICE) {
	fifo_log(m, LOG_NOTICE, "Got status indication 'OS' while INSERVICE on link '%s'.\n", m->name);
        mtp3_link_fail(m, 1);
      }
      break;

    case 4:                   /* Status indication 'PO' */
      /* ToDo: Not implemented. */
/* Don't do this, as the log would explode should this actually happen
   fifo_log(LOG_NOTICE, "Status indication 'PO' not implemented.\n");
*/
      break;

    case 5:                   /* Status indication 'B' */
      /* ToDo: Not implemented. */
      fifo_log(m, LOG_NOTICE, "Status indication 'B' not implemented.\n");
      break;

    default:                  /* Illegal status indication. */
      fifo_log(m, LOG_WARNING, "Got undefined LSSU status %d on link '%s'.\n", typ, m->name);
      mtp3_link_fail(m, 0);
  }
}

static void l4up(mtp2_t* m)
{
  struct mtp_event link_up_event;
  if (m->level4_up)
    return;
  m->level4_up = 1;
  if (m->state == MTP2_INSERVICE) {
    /* Tell user part about the successful link activation. */
    link_up_event.typ = MTP_EVENT_STATUS;
    link_up_event.status.link_state = MTP_EVENT_STATUS_LINK_UP;
    link_up_event.status.link = m->link;
    link_up_event.len = 0;
    mtp_put(m, &link_up_event);
  }
}

static void l4down(mtp2_t* m)
{
  struct mtp_event link_down_event;
	/* Notify user-parts. */
  if (!m->level4_up)
    return;
  m->level4_up = 0;
  link_down_event.typ = MTP_EVENT_STATUS;
  link_down_event.status.link_state = MTP_EVENT_STATUS_LINK_DOWN;
  link_down_event.status.link = m->link;
  link_down_event.len = 0;
  mtp_put(m, &link_down_event);
}

/* Process a received frame.

   The frame has already been checked for correct crc and for being at least
   5 bytes long.
*/
static void mtp2_good_frame(mtp2_t *m, unsigned char *buf, int len) {
  int fsn, fib, bsn, bib;
  int li;

  /* Count this frame into the error rate monitor counters. */
  mtp2_error_mon_count_frame(m);

#ifdef DROP_PACKETS_PCT
  if(rand() <= DROP_PACKETS_PCT/100.0*RAND_MAX) {
    return;
  }
#endif

  if(m->state == MTP2_DOWN) {
    return;
  }

  log_frame(m, 0, buf, len);

  bsn = buf[0] & 0x7f;
  bib = buf[0] >> 7;
  fsn = buf[1] & 0x7f;
  fib = buf[1] >> 7;

  li = buf[2] & 0x3f;

  if(li + 3 > len) {
    fifo_log(m, LOG_NOTICE, "Got unreasonable length indicator %d (len=%d) on link '%s'.\n",
	     li, len, m->name);
    return;
  }

  if(li == 1 || li == 2) {
    /* Link status signal unit. */
    mtp2_process_lssu(m, buf, fsn, fib);
    return;
  }

  /* Process the BSN of the signal unit.
     According to Q.703 (5), only FISU and MSU should have FSN and BSN
     processing done. */
  if(m->state != MTP2_INSERVICE) {
    if(m->state == MTP2_READY) {
      t1_stop(m);
      t7_stop(m);
      m->send_fib = bib;
      m->send_bsn = fsn;
      m->send_bib = fib;
      //      m->retrans_last_acked = fsn;xxx
      //      m->retrans_last_sent = fsn;xxx
      m->retrans_last_acked = bsn;//xxx
      m->error_rate_mon = 0;
      m->emon_dcount = 0;
      m->emon_ncount = 0;
      m->level4_up = 0;

      /* Send TRA (traffic restart allowed) immediately, since we have no
         routing capabilities that could be prohibited/restricted. */
      m->state = MTP2_INSERVICE;
      unsigned char message_tra[5];
      int subservice = m->subservice;

      if (subservice == -1)
        subservice = 0x8;

      fifo_log(m, LOG_NOTICE, "Sending TRA to peer on link '%s'....\n", m->name);
      mtp3_put_label(m->sls, this_host->opc, linkpeerpc(m), message_tra);
      message_tra[4] = 0x17; /* TRA */
      mtp2_queue_msu(m, (subservice << 4) | 0, message_tra, 5);

      /* Send an initial SLTM message, and send it periodic afterwards. */
      if (m->send_sltm) {
	mtp3_send_sltm(m);
	if(m->sltm_t2 != -1) {
	  fifo_log(m, LOG_DEBUG, "SLTM timer T2 not cleared, restarted (%d)\n", m->sltm_t2);
	  mtp_sched_del(mtp2_sched, m->sltm_t2);
	}

	m->sltm_t2 = mtp_sched_add(mtp2_sched, 61000, mtp3_send_sltm, m);
      }
      else {
	l4up(m);
      }
    } else {
      return;
    }
  }
  else if(m->state == MTP2_READY) {
    t1_stop(m);
    t7_stop(m);
  }

  /* ToDo: Check for FIB flipover when we haven't requested retransmission,
     and fault the frame if so. See last part of Q.703 (5.3.2). */

  /* Process the BSN of the received frame. */
  if((m->retrans_last_acked <= m->retrans_last_sent &&
      (bsn < m->retrans_last_acked || bsn > m->retrans_last_sent)) ||
     (m->retrans_last_acked > m->retrans_last_sent &&
      (bsn < m->retrans_last_acked && bsn > m->retrans_last_sent))) {
    /* They asked for a retransmission of a sequence number not available. */
    fifo_log(m, LOG_DEBUG, "Received illegal BSN=%d (retrans=%d,%d) on link '%s', len=%d, si=02%02x, state=%d, count=%d.\n",
	     bsn, m->retrans_last_acked, m->retrans_last_sent, m->name, len, m->tx_buffer[3] & 0xf, m->state, m->bsn_errors);
    /* ToDo: Fault the link if this happens again within the next
       two SUs, see second last paragraph of Q.703 (5.3.1). */
    if (m->bsn_errors++ > 2) {
      m->bsn_errors = 0;
      mtp3_link_fail(m, 1);
    }
    return;
  }
  m->bsn_errors = 0;

  /* Reset timer T7 if new acknowledgement received (Q.703 (5.3.1) last
     paragraph). */
  if(m->retrans_last_acked != bsn) {
    t7_stop(m);
    m->retrans_last_acked = bsn;
    if(m->retrans_last_acked != m->retrans_last_sent) {
      mtp2_t7_start(m);
    }
  }

  if(bib != m->send_fib) {
    /* Received negative acknowledge, start re-transmission. */
    m->send_fib = bib;
    if(bsn == m->retrans_last_sent) {
      /* Nothing to re-transmit. */
      m->retrans_seq = -1;
    } else {
      m->retrans_seq = MTP_NEXT_SEQ(bsn);
    }
  }

  /* Process the signal unit content. */
  if(li == 0) {
    /* Fill-in signal unit. */

    /* Process the FSN of the received frame. */
    if(fsn != m->send_bsn) {
      /* This indicates the loss of a message. */
      if(fib == m->send_bib) {
        /* Send a negative acknowledgement, to request retransmission. */
        m->send_bib = !m->send_bib;
      }
    }
  } else {
    /* Message signal unit. */
    /* Process the FSN of the received frame. */
    if(fsn == m->send_bsn) {
      /* Q.703 (5.2.2.c.i): Redundant retransmission. */
      return;
    } else if(fsn == MTP_NEXT_SEQ(m->send_bsn)) {
      /* Q.703 (5.2.2.c.ii). */
      if(fib == m->send_bib) {
        /* Successful frame reception. Do explicit acknowledge on next frame. */
        m->send_bsn = fsn;
      } else {
        /* Drop frame waiting for retransmissions to arrive. */
        return;
      }
    } else {
      /* Q.703 (5.2.2.c.iii). Frame lost before this frame, discart it
         (will be retransmitted in-order later). */
      if(fib == m->send_bib) {
        /* Send a negative acknowledgement, to request retransmission. */
        m->send_bib = !m->send_bib;
      }
      return;
    }

    /* Length indicator (li) is number of bytes in MSU after LI, so the valid
       bytes are buf[0] through buf[(li + 3) - 1]. */
    if(li < 5) {
      fifo_log(m, LOG_NOTICE, "Got short MSU (no label), li=%d on link '%s'.\n", li, m->name);
      return;
    }
    {
      char pbuf[1000], hex[30];
      int i;
      strcpy(pbuf, "");
      for(i = 0; i < li - 1 && i + 4 < len; i++) {
	sprintf(hex, " %02x", buf[i + 4]);
	strcat(pbuf, hex);
      }
      fifo_log(m, LOG_DEBUG, "Got MSU on link '%s' sio=%d slc=%d m.sls=%d bsn=%d/%d, fsn=%d/%d, sio=%02x, len=%d:%s\n", m->name, buf[3] & 0xf, (buf[7] & 0xf0) >> 4, m->sls, bib, bsn, fib, fsn, buf[3], li, pbuf);
    }
    process_msu(m, buf, len);
  }
}