mtp.c

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

static void process_msu(struct mtp2_state* m, unsigned char* buf, int len)
{
  mtp2_t* tm;
  int service_indicator, subservice_field;
  int h0, h1;
  int slt_pattern_len;
  int dpc, opc, slc;
  int li;

  li = buf[2] & 0x3f;
  service_indicator = buf[3] & 0xf;
  subservice_field = (buf[3] & 0xf0) >> 4;
  dpc = buf[4] | ((buf[5] & 0x3f) << 8);
  opc = ((buf[5] & 0xc0) >> 6) | (buf[6] << 2) | ((buf[7] & 0x0f) << 10);
  slc = (buf[7] & 0xf0) >> 4;
  if (m->subservice == -1) {
    m->subservice = subservice_field;
    fifo_log(m, LOG_NOTICE, " Using subservice field from incoming MSU: 0x%x\n", subservice_field);
  }

  switch(service_indicator) {
  case 0x0:
    /* Signalling network management messages. */
    h0 = buf[8] & 0xf;
    h1 = (buf[8] & 0xf0) >> 4;
    tm = findtargetslink(m, slc);
    fifo_log(m, LOG_DEBUG, "Signalling network management, h0=%d, h1=%d, targetslink '%s'\n", h0, h1, tm ? tm->name : "(unknown)");
    if (!tm) {
      fifo_log(m, LOG_DEBUG, "Target signalling link %d not found, received on '%s'\n", slc, m->name);
      break;
    }
    if (h0 == 1) { /* CHM - changeover management */
    }
    else if (h0 == 7 && h1 == 1) {
      fifo_log(m, LOG_DEBUG, "Received tra on '%s', sls %d\n", m->name, slc);
    }
    break;

  case 0x1:
    /* Signalling network testing and maintenance messages. (SLTM) */
    if(li < 7) {
      fifo_log(m, LOG_NOTICE, "Got short SLTM/SLTA (no h0/h1/len), li=%d on link '%s'.\n", li, m->name);
      return;
    }
    h0 = buf[8] & 0xf;
    h1 = (buf[8] & 0xf0) >> 4;
    slt_pattern_len = (buf[9] & 0xf0) >> 4;

    if(li < 7 + slt_pattern_len) {
      fifo_log(m, LOG_NOTICE, "Got short SLTM/SLTA (short pattern), li=%d, "
	       "slt_len=%d on link '%s'.\n", li, slt_pattern_len, m->name);
      return;
    }
    if(h0 == 0x1 && h1 == 0x1) {
      /* signalling link test message. */
      /* Queue a signalling link test acknowledgement message. (SLTA) */

      unsigned char message_slta[21];
      int subservice = m->subservice;

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

      mtp3_put_label(slc, dpc, opc, message_slta);
      message_slta[4] = 0x21;
      message_slta[5] = slt_pattern_len << 4;
      memcpy(&(message_slta[6]), &(buf[10]), slt_pattern_len);
      fifo_log(m, LOG_DEBUG, "Got SLTM, sending SLTA '%s', state=%d.\n", m->name, m->state);
      mtp2_queue_msu(m, (subservice << 4) | 1, message_slta, 6 + slt_pattern_len);
    } else if(h0 == 0x1 && h1 == 0x2) {
      /* signalling link test acknowledgement message. */

      /* Clear the Q.707 timer T1, since the SLTA was received. */
      if(m->sltm_t1 != -1) {
	mtp_sched_del(mtp2_sched, m->sltm_t1);
	m->sltm_t1 = -1;
      }

      /* Q.707 (2.2) conditions for acceptance of SLTA. */
      if(slc == m->sls &&
	 opc == linkpeerpc(m) && dpc == this_host->opc &&
	 0 == memcmp(sltm_pattern, &(buf[10]), sizeof(sltm_pattern))) {
	fifo_log(m, LOG_DEBUG, "Got valid SLTA response on link '%s', state=%d.\n", m->name, m->state);
	l4up(m);
      } else {
	if(m->sltm_tries == 1) {
	  fifo_log(m, LOG_WARNING, "Received invalid SLTA response (slc=%d,opc=%d,dpc=%d), trying again on link '%s'.\n", slc, opc, dpc, m->name);
	  mtp3_send_sltm(m);
	  m->sltm_tries = 2;
	} else {
	  fifo_log(m, LOG_ERROR, "Received invalid SLTA response, faulting link on link '%s'.\n", m->name);
	  mtp3_link_fail(m, 0);
	}
      }
    } else {
      /* Spare. */
      fifo_log(m, LOG_NOTICE, "Got unknown/spare signalling network testing "
	       "and maintenance message code H0=0x%x/H1=0x%x on link '%s'.\n", h0, h1, m->name);
    }
    break;

  case SS7_PROTO_SCCP:
    deliver_l4(m, &(buf[4]), (li == 63 ? len - 4 : li - 1), MTP_EVENT_SCCP);
    break;
  case SS7_PROTO_ISUP:
    deliver_l4(m, &(buf[4]), (li == 63 ? len - 4 : li - 1), MTP_EVENT_ISUP);
    break;
  }
}


/* MTP2 reading of signalling units.
 * The buffer pointer is only valid until return from this function, so
 * the data must be copied out as needed.
 */
static void mtp2_read_su(mtp2_t *m, unsigned char *buf, int len) {
  int i = 0;
  int res;
  unsigned char nextbyte;

  for(;;) {
    while(m->h_rx.bits <= 24 && i < len) {
      nextbyte = buf[i++];
      /* Log the byte for debugging link errors. */
      m->backbuf[m->backbuf_idx] = nextbyte;
      m->backbuf_idx = (m->backbuf_idx + 1) % sizeof(m->backbuf);
      fasthdlc_rx_load_nocheck(&(m->h_rx), nextbyte);
      m->readcount++;
      if(m->h_rx.state == 0) {
        /* Q.703 (10.2.3): Octet counting mode. */
        m->emon_ncount = (m->emon_ncount + 1) % 16;
        if(m->emon_ncount == 0) {
          mtp2_emon_count_error(m);
        }
      }
    }

    res = fasthdlc_rx_run(&(m->h_rx));
    if(res & RETURN_DISCARD_FLAG) {
      /* Some problem, like 7 one-bits in a row, or framesize not dvisible by
         8 bits. The fasthdlc now enters a state looking for the next flag
         (octet counting mode). */
      char msg[80];
      sprintf(msg, "MTP2 bitstream frame format error, entering octet counting mode");
      mtp2_bad_frame(m, msg);
      mtp2_octet_counting(m);
      m->rx_len = 0;
      m->rx_crc = 0xffff;
    } else if(res & RETURN_EMPTY_FLAG) {
      if(i >= len) {
        /* No more data for now. */
        break;
      }
      /* Else we are skipping bits looking for a flag (Q.703 "octet counting
         mode"). */
    } else if(res & RETURN_COMPLETE_FLAG) {
      if(m->rx_len == 0) {
        /* Q.703: Multiple flags in sequence are allowed (though
           discouraged). */
      } else if(m->rx_len < 5) {
        /* Q.703: A frame must be at least 5 bytes (plus one for the flag). If
           not, the frame is in error. */
	char msg[80];
	sprintf(msg, "Short MTP2 frame len %d < 5", m->rx_len);
        mtp2_bad_frame(m, msg);
      } else {
        if(m->rx_crc == 0xf0b8) {
          mtp2_good_frame(m, m->rx_buf, m->rx_len-2);
        } else {
	  char msg[80];
	  sprintf(msg, "MTP2 CRC error (CRC=0x%x != 0xf0b8)", m->rx_crc);
          mtp2_bad_frame(m, msg);
        }
      }
      m->rx_len = 0;
      m->rx_crc = 0xffff;
    } else {
      /* Got a data byte. */
      /* Q.703: If more than 272+7 bytes are seen in a frame, discard it and
         enter "octet counting mode". */
      if(m->rx_len >= 272 + 7) {
        /* Switch state into "looking for a frame". */
	char msg[80];
	sprintf(msg, "Overlong MTP2 frame, entering octet counting mode");
        m->h_rx.state = 0;
        mtp2_bad_frame(m, msg);
        mtp2_octet_counting(m);
        m->rx_len = 0;
        m->rx_crc = 0xffff;
      } else {
        m->rx_buf[m->rx_len++] = res;
        m->rx_crc = PPP_FCS(m->rx_crc, res);
      }
    }
  }
}

static void mtp2_fetch_zap_event(mtp2_t *m) {
  int x = 0;
  int res;

  res = io_get_zaptel_event(m->fd, &x);
  fifo_log(m, LOG_NOTICE, "Got event on link '%s': %d (%d/%d).\n", m->name, x, res, errno);
}

/* Find a frame to transmit and put it in the transmit buffer.

   Q.703 (11.2.2): Pick a frame in descending priority as
   1. Link status signal unit.
   2. Requested retransmission of message signal unit.
   3. New message signal unit.
   4. Fill-in signal unit.
   5. Flag [but we can always send fill-in signal unit].
*/
static void mtp2_pick_frame(mtp2_t *m)
{
  switch(m->state) {
    case MTP2_DOWN:
      /* Send SIOS. */
      m->tx_len = 4;
      m->tx_buffer[0] = m->send_bsn | (m->send_bib << 7);
      m->tx_buffer[1] = m->retrans_last_sent | (m->send_fib << 7);
      m->tx_buffer[2] = 1;      /* Length 1, meaning LSSU. */
      m->tx_buffer[3] = 3;      /* 3 is indication 'SIOS'. */
      return;

    case MTP2_NOT_ALIGNED:
      /* Send SIO. */
      m->tx_len = 4;
      m->tx_buffer[0] = m->send_bsn | (m->send_bib << 7);
      m->tx_buffer[1] = m->retrans_last_sent | (m->send_fib << 7);
      m->tx_buffer[2] = 1;      /* Length 1, meaning LSSU. */
      m->tx_buffer[3] = 0;      /* 0 is indication 'SIO'. */
      return;

    case MTP2_ALIGNED:
    case MTP2_PROVING:
      /* Send SIE or SIN. */
      m->tx_len = 4;
      m->tx_buffer[0] = m->send_bsn | (m->send_bib << 7);
      m->tx_buffer[1] = m->retrans_last_sent | (m->send_fib << 7);
      m->tx_buffer[2] = 1;      /* Length 1, meaning LSSU. */
      m->tx_buffer[3] = 2;
      return;

    case MTP2_READY:
    case MTP2_INSERVICE:
      /* Frame selection. */

      /* If we have something in the retransmission buffer, send it. This
         also handles sending new MSUs, as they are simply appended to the
         retransmit buffer. */
      if(m->retrans_seq != -1) {
        /* Send retransmission. */
        memcpy(m->tx_buffer,
               m->retrans_buf[m->retrans_seq].buf,
               m->retrans_buf[m->retrans_seq].len);
        m->tx_len = m->retrans_buf[m->retrans_seq].len;
        m->tx_buffer[0] = m->send_bsn | (m->send_bib << 7);
        m->tx_buffer[1] = m->retrans_seq | (m->send_fib << 7);

        if(m->retrans_seq == m->retrans_last_sent) {
          /* Retransmission done. */
          m->retrans_seq = -1;
        } else {
          /* Move to the next one. */
          m->retrans_seq = MTP_NEXT_SEQ(m->retrans_seq);
        }

        return;
      }

      /* Send Fill-in signalling unit (FISU) if nothing else is pending. */
      m->tx_len = 3;
      m->tx_buffer[0] = m->send_bsn | (m->send_bib << 7);
      m->tx_buffer[1] = m->retrans_last_sent | (m->send_fib << 7);
      m->tx_buffer[2] = 0;      /* Length 0, meaning FISU. */
      return;

    default:
      fifo_log(m, LOG_ERROR, "Internal: Unknown MTP2 state %d on link '%s'?!?\n", m->state, m->name);
  }
}

/* Fill in a buffer for Zaptel transmission, picking frames as necessary.
   The passed buffer is of size ZAP_BUF_SIZE.
*/
static void mtp2_fill_zaptel_buf(mtp2_t *m, unsigned char *buf) {
  int i;

  for(i = 0; i < ZAP_BUF_SIZE; i++) {
    if(m->h_tx.bits < 8) {
      /* Need some more bits. */
      if(m->tx_do_crc == 1) {
        /* Put first byte of CRC. */
        fasthdlc_tx_load_nocheck(&(m->h_tx), m->tx_crc & 0xff);
        m->tx_do_crc = 2;
      } else if(m->tx_do_crc == 2) {
        /* Put second byte of CRC. */
        fasthdlc_tx_load_nocheck(&(m->h_tx), (m->tx_crc >> 8) & 0xff);
        m->tx_do_crc = 0;
      } else if(m->tx_sofar >= m->tx_len) {
        /* Fetch a new frame. */
#ifdef DROP_PACKETS_PCT
        do {
#endif
        mtp2_pick_frame(m);
#ifdef DROP_PACKETS_PCT
        } while(rand() <= DROP_PACKETS_PCT/100.0*RAND_MAX);
#endif
	if (m->tx_len > 4)
	  fifo_log(m, LOG_DEBUG, "Sending buffer to zaptel len=%d, on link '%s' bsn=%d, fsn=%d.\n", m->tx_len, m->name, m->tx_buffer[0]&0x7f,  m->tx_buffer[1]&0x7f);
        log_frame(m, 1, m->tx_buffer, m->tx_len);
        m->tx_sofar = 0;
        m->tx_crc = 0xffff;
        fasthdlc_tx_frame_nocheck(&(m->h_tx));
        /* A zero-length frame from mtp2_pick_frame() will cause
           sending of a single flag, without crc check bits.
        */
      } else {
        unsigned char data = m->tx_buffer[m->tx_sofar++];
        fasthdlc_tx_load_nocheck(&(m->h_tx), data);
        m->tx_crc = PPP_FCS(m->tx_crc, data);
        if(m->tx_sofar == m->tx_len) {
          /* At frame end, also push the crc bits into the fasthdlc buffer.
             Because of bit stuffing, we might not have room in the buffer for
             8 bits of data + 16 bits of crc, so set a flag to do it later. */
          m->tx_crc ^= 0xffff;
          m->tx_do_crc = 1;
        }
      }
    }

    buf[i] = fasthdlc_tx_run_nocheck(&(m->h_tx));
    m->writecount++;
  }
}

void *mtp_thread_main(void *data) {
  struct mtp2_state *m = NULL;
  int i, lsi;
  int res;
  struct pollfd fds[MAX_SCHANNELS];
  unsigned char fifobuf[MTP_REQ_MAX_SIZE];
  struct mtp_req *req;
  int last_send_ix = 0;
#ifdef MTP_OVER_UDP
  int sent_fisu[MAX_SCHANNELS] = {0,};
  int sent_bsn[MAX_SCHANNELS] = {0,};
#endif
  ast_verbose(VERBOSE_PREFIX_3 "Starting MTP thread, pid=%d.\n", getpid());

  /* These counters are used to generate timestamps for raw dumps.
     The write count is offset with one zaptel buffer size to account for
     the buffer-introduced write latency. This way the dump timings should
     approximately reflect the time that the last byte of the frame went
     out on the wire. */
  for (i = 0; i < this_host->n_schannels; i++) {
    m = &mtp2_state[i];
    m->readcount = 0;
    m->writecount = ZAP_BUF_SIZE;

    fds[i].fd = m->fd;
    fds[i].events = POLLIN|POLLPRI|POLLOUT;
  }
  while(!stop_mtp_thread) {
    struct timeval now;
    struct timeval last;
    int tdiff;

    for (i = 0; i < this_host->n_schannels; i++) {
      m = &mtp2_state[i];
#ifdef MTP_OVER_UDP
      if (0)
	if (((m->state == MTP2_READY || m->state == MTP2_INSERVICE)))
	  ast_log(LOG_DEBUG, "Poll2, state=%d, retrans_seq=%d last_sent=%d last_ack=%d, send_bsn=%d sent_bsn=%d sent_fisu=%d\n", m->state, m->retrans_seq, m->retrans_last_sent, m->retrans_last_acked, m->send_bsn, sent_bsn[i], sent_fisu[i]);
      if (((m->state == MTP2_READY || m->state == MTP2_INSERVICE) && ((m->retrans_seq != -1) || (m->retrans_last_acked != m->retrans_last_sent))) ||
	  ((m->state != MTP2_READY && m->state != MTP2_INSERVICE)) ||
	  (m->send_bsn != sent_bsn[i])) {
	/* avoid sending FISU */
	fds[i].events = POLLIN|POLLPRI|POLLOUT;
	sent_fisu[i] = 0;
	sent_bsn[i] = m->send_bsn;
      }
      else {
	if (sent_fisu[i] < 100) {
	  fds[i].events = POLLIN|POLLPRI|POLLOUT;
	  sent_fisu[i]++;
	  sent_bsn[i] = m->send_bsn;
	}
	else
	  fds[i].events = POLLIN|POLLPRI;
      }
#endif
    }
#ifdef TESTINPUT
    {
      int cmdfd = open("/tmp/mtp3d.sock", O_RDONLY | O_NONBLOCK);

      if (cmdfd != -1) {
	struct pollfd cmdfds[1];
	cmdfds[0].fd = cmdfd;
	cmdfds[0].events = POLLIN;
	res = poll(cmdfds, 1, 100);
	if (res > 0) {
	  unsigned char buf[1024];
	  res = read(cmdfd, buf, sizeof(buf));
	  if (res > 0) {
	    m = &mtp2_state[0];
	    log_frame(m, 0, buf, res);
	    process_msu(m, buf, res);
	  }
	}
	close(cmdfd);
      }
    }
#endif

    /* No need to calculate timeout with ast_sched_wait, as we will be
       woken up every 2 msec. anyway to read/write zaptel buffers. */
    gettimeofday(&last, NULL);