mtp.c

asterisk 中国七号驱动修改代码可以在ASTERISIK-1。4上编译pa

	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);
  }
}