mtp.c

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

    res = poll(fds, this_host->n_schannels, 20);

    gettimeofday(&now, NULL);
    tdiff = timediff_usec(now, last);
#ifndef MTP_OVER_UDP
    if (tdiff > 5000)
      if (this_host->n_schannels)
	fifo_log(m, LOG_NOTICE, "Excessive poll delay %d!\n", tdiff);//xxxx
#endif

    if(res < 0) {
      if(errno == EINTR) {
        /* Just try again. */
      } else {
        fifo_log(m, LOG_NOTICE, "poll() failure, errno=%d: %s\n",
		 errno, strerror(errno));
      }
    } else if(res > 0) {

      for (i = 0; i < this_host->n_schannels; i++) {
	if(fds[i].revents & POLLPRI) {
	  mtp2_fetch_zap_event(&mtp2_state[i]);
	}
      }
      /* Do the read before write, so that we can send any responses
         immediately (since we will usually/always also have a ready
         POLLOUT condition). */
      for (i = 0; i < this_host->n_schannels; i++) {
	m = &mtp2_state[i];
	if(fds[i].revents & POLLIN) {
	  unsigned char buf[1024];
	  int count = 0;

	  for(;;) {
	    res = read(fds[i].fd, buf, sizeof(buf));
	    if(res == 0) {
	      /* EOF. */
	      break;
	    } else if(res < 0) {
	      if(errno == EAGAIN || errno == EWOULDBLOCK) {
		/* Done until next successful poll(). */
		break;
	      } else if(errno == EINTR) {
		/* Try again. */
	      } else if(errno == ELAST) {
		mtp2_fetch_zap_event(m);
	      } else {
		/* Some unexpected error. */
		fifo_log(m, LOG_DEBUG, "Error reading zaptel device '%s', errno=%d: %s.\n", m->name, errno, strerror(errno));
		break;
	      }
	    } else {
	      /* Got some data. */
	      count += res;
#ifdef DO_RAW_DUMPS
	      mtp2_dump_raw(m, buf, res, 0);
#endif
	      mtp2_read_su(m, buf, res);
	    }
	  }
	  //if(count > 2*ZAP_BUF_SIZE) fifo_log(m, LOG_NOTICE, "%d bytes read (%d buffers).\n", count, count/ZAP_BUF_SIZE);
#ifndef MTP_OVER_UDP
	  if(count >= NUM_ZAP_BUF*ZAP_BUF_SIZE) {
	    fifo_log(m, LOG_NOTICE, "Full Zaptel input buffer detected, incoming "
		     "packets may have been lost on link '%s' (count=%d.\n", m->name, count);
	  }
#endif
	}
      }
      for (i = 0; i < this_host->n_schannels; i++) {
	m = &mtp2_state[i];
	if(fds[i].revents & POLLOUT) {
	  int count = 0;

	  for(;;) {
	    /* We buffer an extra ZAP_BUF_SIZE bytes locally. This creates
	       extra latency, but it is necessary to be able to detect write()
	       buffer underrun by doing an extra write() to see the EAGAIN
	       return.
	    */
	    if(!m->zap_buf_full) {
	      mtp2_fill_zaptel_buf(m, m->zap_buf);
	      m->zap_buf_full = 1;
	    }
	    res = write(fds[i].fd, m->zap_buf, ZAP_BUF_SIZE);
	    if(res == 0) {
	      /* EOF. */
	      break;
	    } else if(res < 0) {
	      if(errno == EAGAIN || errno == EWOULDBLOCK) {
		/* Done until next successful poll(). */
		break;
	      } else if(errno == EINTR) {
		/* Try again. */
	      } else if(errno == ELAST) {
		mtp2_fetch_zap_event(m);
	      } else {
		/* Some unexpected error. */
		fifo_log(m, LOG_DEBUG, "Error writing zaptel device '%s', errno=%d: %s.\n", m->name, errno, strerror(errno));
		break;
	      }
	    } else {
	      /* Successful write. */
	      count += res;
#ifdef DO_RAW_DUMPS
	      mtp2_dump_raw(m, m->zap_buf, res, 1);
#endif
	      m->zap_buf_full = 0;
#ifdef MTP_OVER_UDP
	      break;
#endif
	    }
	  }
	  //if(count > 2*ZAP_BUF_SIZE) fifo_log(m, LOG_NOTICE, "%d bytes written (%d buffers).\n", count, count/ZAP_BUF_SIZE);
	  if(count >= NUM_ZAP_BUF*ZAP_BUF_SIZE) {
	    fifo_log(m, LOG_NOTICE, "Empty Zaptel output buffer detected, outgoing "
		     "packets may have been lost on link '%s'.\n", m->name);
	  }
#ifdef MTP_OVER_UDP
	  if ((m->state != MTP2_READY) && (m->state != MTP2_INSERVICE))
	    res = poll(fds, 0, 20); /* sending lssu, small delay */
	  else
	    res = poll(fds, 0, 10); /* sending msu or fisu */
#endif
	}
      }
    }

    for (lsi = 0; lsi < n_linksets; lsi++) {
      m = NULL;
      if (!linksets[lsi].enabled)
	continue;
#ifdef xxxx
      n_inservice = 0;
      for (i = 0; i < this_host->n_schannels; i++) {
	struct mtp2_state* trym = &mtp2_state[last_send_ix];
	last_send_ix = (last_send_ix + 1) % this_host->n_schannels;
	if (trym->link->linkset != &linksets[lsi])
	  continue;
	if (trym->state != MTP2_INSERVICE)
	  continue;
	m = trym;
	n_inservice += 1;

	/* Handle requests from the channel driver threads.
	   We don't pull in requests when the retransmit buffer is full (otherwise
	   we would loose messages as we cannot fetch from the lffifo
	   out-of-order). */
      }
      if (!n_inservice) {
	for (i = 0; i < this_host->n_schannels; i++) {
	  struct mtp2_state* trym = &mtp2_state[last_send_ix];
	  last_send_ix = (last_send_ix + 1) % this_host->n_schannels;
	  if (!is_combined_linkset(trym->link->linkset, &linksets[lsi]))
	    continue;
	  if (trym->state != MTP2_INSERVICE)
	    continue;
	  m = trym;
	  n_inservice += 1;
	}
      }
#endif
      m = get_inservice_schannel(linksets[lsi].links[0]);
      if (m) {
	while(MTP_NEXT_SEQ(m->retrans_last_sent) != m->retrans_last_acked &&
	      (res = lffifo_get(sendbuf[lsi], fifobuf, sizeof(fifobuf))) != 0) {
	  if(res < 0) {
	    fifo_log(m, LOG_ERROR, "Got oversize packet in MTP request buffer -> choking on link '%s'.\n", m->name);
	    break;
	  }
	  req = (struct mtp_req *)fifobuf;
	  switch(req->typ) {
	  case MTP_REQ_ISUP:
	  case MTP_REQ_ISUP_FORWARD: {
	    if (req->isup.slink) {
	      struct mtp2_state* targetm = req->isup.slink->mtp;
	      if (targetm && (targetm->state == MTP2_INSERVICE) &&
		  (MTP_NEXT_SEQ(targetm->retrans_last_sent) != targetm->retrans_last_acked)) /* Not full */
		m = targetm;
	    }
	    int subservice = SS7_PROTO_ISUP | (m->subservice << 4);
	    mtp3_set_sls(m->sls, req->buf); // xxx is this necessary?
	    fifo_log(m, LOG_DEBUG, "Queue MSU, lsi=%d, last_send_ix=%d, linkset=%s, m->link=%s\n", lsi, last_send_ix, linksets[lsi].name, m->link->name);
	    mtp2_queue_msu(m, subservice, req->buf, req->len);
#if 1
	    if (testfailover) {
	      int tx_len = m->retrans_buf[m->retrans_last_sent].len;
	      close(m->fd);
	      m->fd = -1;
	      fds[m->slinkno].fd = -1;
	      testfailover = 0;
	      fifo_log(m, LOG_DEBUG, "Closing link with tx_len %d, on link '%s'\n", tx_len, m->name);
	    }
#endif
	  }
	    break;
	  case MTP_REQ_SCCP: {
	    if (req->sccp.slink) {
	      struct mtp2_state* targetm = req->sccp.slink->mtp;
	      if (targetm && (targetm->state == MTP2_INSERVICE) &&
		  (MTP_NEXT_SEQ(targetm->retrans_last_sent) != targetm->retrans_last_acked)) /* Not full */
		m = targetm;
	    }
	    int subservice = SS7_PROTO_SCCP | (m->subservice << 4);
	    fifo_log(m, LOG_DEBUG, "Queue MSU, lsi=%d, last_send_ix=%d, linkset=%s, m->link=%s\n", lsi, last_send_ix, linksets[lsi].name, m->link->name);
	    mtp2_queue_msu(m, subservice, req->buf, req->len);
	  }
	    break;
	  case MTP_REQ_LINK_DOWN:
	    fifo_log(m, LOG_ERROR, "Got MTP_REQ_LINK_DOWN packet in MTP send buffer???.\n");
	    break;
	  case MTP_REQ_LINK_UP:
	    fifo_log(m, LOG_ERROR, "Got MTP_REQ_LINK_UP packet in MTP send buffer???.\n");
	    break;
	  case MTP_REQ_REGISTER_L4:
	    break;

	  }
	}
      }
      else if (cluster_receivers_alive(&linksets[lsi])) {
	while((res = lffifo_get(sendbuf[lsi], fifobuf, sizeof(fifobuf))) != 0) {
	  if(res < 0) {
	    fifo_log(m, LOG_ERROR, "Got oversize packet in MTP request buffer -> choking on link '%s'.\n", m->name);
	    break;
	  }
	  req = (struct mtp_req *)fifobuf;
	  switch(req->typ) {
	  case MTP_REQ_ISUP:
	    cluster_mtp_forward(req);
	    break;
	  case MTP_REQ_ISUP_FORWARD:
	    cluster_mtp_forward(req);
	    break;
	  default:;
	    /* Ignore other requests */
	  }
	}
      }
      else {
	res = lffifo_get(sendbuf[lsi], fifobuf, sizeof(fifobuf));
	if(res < 0) {
	  fifo_log(m, LOG_ERROR, "Got oversize packet in MTP request buffer -> choking on link '%s'.\n", m->name);
	  break;
	}
	if (res > 0)
	  fifo_log(m, LOG_WARNING, "No signalling links inservice and no cluster receivers alive, dropping packet!\n");
      }
    }
    while ((res = lffifo_get(controlbuf, fifobuf, sizeof(fifobuf))) != 0) {
      int link_ix = 0;
      if (!this_host->n_schannels)
	continue; // No MTP signalling channels available, ignore control requests
      m = &mtp2_state[0];
      if(res < 0) {
	fifo_log(m, LOG_ERROR, "Got oversize packet in MTP control buffer.\n");
	break;
      }
      req = (struct mtp_req *)fifobuf;
      switch(req->typ) {
      case MTP_REQ_ISUP:
	fifo_log(m, LOG_ERROR, "Got ISUP packet in MTP control buffer???.\n");
	break;
      case MTP_REQ_ISUP_FORWARD:
	fifo_log(m, LOG_ERROR, "Got MTP_REQ_ISUP_FORWARD packet in MTP send buffer???.\n");
	break;
      case MTP_REQ_LINK_DOWN:
	link_ix = req->link.link_ix;
	m = &mtp2_state[link_ix];
	fifo_log(m, LOG_DEBUG, "Taking link down on request on link '%s'.\n", m->name);
	m->state = MTP2_DOWN;
	mtp2_cleanup(m);
	l4down(m);
	break;
      case MTP_REQ_LINK_UP:
	link_ix = req->link.link_ix;
	m = &mtp2_state[link_ix];
	start_initial_alignment(m, "CLI link up");
	break;
      case MTP_REQ_SCCP:
	break;
      case MTP_REQ_REGISTER_L4:
	break;
      }
    }

    mtp_sched_runq(mtp2_sched);
  }

  return NULL;
}

void mtp_thread_signal_stop(void) {
  /* No need for explicit thread wakeup; the mtp thread wakes up every
     2msec anyway on the zaptel device. */
  stop_mtp_thread = 1;
}

struct lffifo **mtp_get_send_fifo(void) {
  return sendbuf;
}

int get_receive_pipe(void) {
  return receivepipe[0];
}

struct lffifo *mtp_get_receive_fifo(void) {
  return receivebuf;
}

struct lffifo *mtp_get_control_fifo(void) {
  return controlbuf;
}

static void mtp_cleanup_link(struct mtp2_state* m) {
  if(m->fd != -1) {
    close(m->fd);
    m->fd = -1;
  }
}

void mtp_cleanup(void) {
  int i;
  if(mtp2_sched) {
    mtp_sched_context_destroy(mtp2_sched);
    mtp2_sched = NULL;
  }
  for (i = 0; i < n_linksets; i++) {
    if(sendbuf[i]) {
      lffifo_free(sendbuf[i]);
      sendbuf[i] = NULL;
    }
  }
  if(receivebuf) {
    lffifo_free(receivebuf);
    receivebuf = NULL;
  }
  if(controlbuf) {
    lffifo_free(controlbuf);
    controlbuf = NULL;
  }

  if(receivepipe[0] != -1) {
    close(receivepipe[0]);
    receivepipe[0] = -1;
  }
  if(receivepipe[1] != -1) {
    close(receivepipe[1]);
    receivepipe[1] = -1;
  }

  if (this_host)
    for (i = 0; i < this_host->n_schannels; i++) {
      mtp_cleanup_link(&mtp2_state[i]);
    }
}

static void mtp_init_link_data(struct mtp2_state* m) {
  m->state = MTP2_DOWN;

  m->send_fib = 1;
  m->send_bsn = 0x7f;
  m->send_bib = 1;

  m->send_sltm = 0;

  m->schannel = -1;
  m->slinkno = -1;
  m->link = NULL;

  m->fd = -1;

  m->rx_len = 0;
  m->rx_crc = 0xffff;

  m->tx_len = 0;
  m->tx_sofar = 0;
  m->tx_do_crc = 0;
  m->tx_crc = 0xffff;

  m->retrans_seq = -1;
  m->retrans_last_acked = 0x7f;
  m->retrans_last_sent = 0x7f;

  m->error_rate_mon = 0;
  m->emon_ncount = 0;
  m->emon_dcount = 0;

  m->mtp2_t1 = -1;
  m->mtp2_t2 = -1;
  m->mtp2_t3 = -1;
  m->mtp2_t4 = -1;
  m->mtp2_t7 = -1;

  m->level4_up = 0;

  m->zap_buf_full = 0;

  m->sltm_t1 = -1;
  m->sltm_t2 = -1;

  m->mtp3_t17 = -1;
}

static int mtp_init_link(struct mtp2_state* m, struct link* link, int slinkno) {
  mtp_init_link_data(m);
  m->link = link;
  link->mtp = m;
  fifo_log(m, LOG_DEBUG, "init link %s, linkset %s, schannel %d.\n", link->name, link->linkset->name, link->schannel);
  if(linkpeerpc(m) < 0 || linkpeerpc(m) >= (1<<14)) {
    ast_log(LOG_ERROR, "Invalid value 0x%x for linkpeerpc.\n", linkpeerpc(m));
    goto fail;
  }
  m->send_sltm = link->send_sltm;
  m->schannel = link->schannel;
  m->slinkno = slinkno;
  m->sls = link->