pc_d51.c

卡耐基.梅隆大学的机器人仿真软件（Redhat linux 9下安装）

      dummy = smc_in(1);
      DELAY10US(10);
      aux_reg = smc_in(1);
      
      if (((aux_reg & 0x10) == 0) || ((aux_reg & 0x20) == 0)) {
	return 0x16;
      } 
      dummy = smc_in(1);
      smc_out(1, 0x1e);
      DELAYMS(10);
      aux_reg = smc_in(1);
      if (((aux_reg & 0x10) != 0) && ((aux_reg & 0x20) != 0) && 
	  ((aux_reg & 0x04) != 0)) {
	mask = 1 << (smc_in(7) & 0x07); 
	b = smc_in(7) >> 3;
	d51_net_map[b] |= mask;
      } 
      smc_out(7, smc_in(7) + 1);
    } 
  } else { 
    smc_out(7, 1);
    while (smc_in(7) != 0) {
      dummy = smc_in(1);
      aux_reg = smc_in(1);
      while ((aux_reg & 0x40) == 0) {	
	aux_reg = smc_in(1);
      } 
      dummy = smc_in(1);      
      smc_out(1, 0x1e);
      recon_cnt = 0;
      aux_reg = smc_in(1);
      while (!(((aux_reg & 0x40) != 0) || (recon_cnt > 3) || 
	       ((aux_reg & 0x80) != 0))) {
	if ((smc_in(0) & 0x04) != 0) {
	  recon_cnt++;
	  smc_out(1, 0x1e);
	} 
	if ((aux_reg & 0x04) != 0) {
	  mask = 1 << (smc_in(7) & 0x07);
	  c = smc_in(7) >> 3;
	  d51_net_map[c] |= mask;
	  break;
	} 
	  
	aux_reg = smc_in(1);
      } 
      smc_out(7, smc_in(7) + 1);
    } 
  } 
  
  return status;
}

unsigned char d51_free_node()
{
  unsigned char mask; 
  unsigned char i2; 

  if (d51_network_map() == 0) {
    i2 = 1; 
    while (i2 != 0) {
      mask = 1 << (i2 & 0x07);
      d51_net_map[smc_in(7) >> 3] & (mask == 0) ? 0x01 : 0xff;
      /* WARNING - This appears broken */
      return i2;

      i2++;
    } 
  } 
  
  return 0;
}

void d51_clear_diag()
{
  int i2; 

  for (i2 = 0; i2 < 13; i2++) {
    d51_diag_cnt[i2] = 0;
  } 
  d51_diag_cnt[7] = d51_params[5];
}

void d51_tokens(unsigned char ntokens)
{
  
  unsigned char recon_cnt; 
  unsigned char aux_reg; 

  dummy = smc_in(1);
  smc_out(1, 0x1e);
  while (ntokens--) {
    recon_cnt = 0;
    aux_reg = smc_in(1);
    while (!(((aux_reg & 0x40) != 0) || (recon_cnt > 3) || 
	     ((aux_reg & 0x80) != 0))) {
      if ((smc_in(0) & 0x04) != 0) {
	recon_cnt++;
	smc_out(1, 0x1e);
      } 
      aux_reg = smc_in(1);
    } 
  } 
}

void d51_exit()
{
  smc_out(1, 1);
  smc_out(1, 2);
  smc_out(6, smc_in(6) & 0xdf);
  if (d51_params[2] == 1) {
    while (1) 
      ;
  }  
} 

void d51_check_int()
{
  unsigned char count[2]; 
  unsigned char offset; 
  unsigned int nbytes; 
  unsigned char good_count; 
  unsigned int address; 
  unsigned int index; 
  unsigned int old_index; 
  unsigned int i2; 
  unsigned int cnt; 
  unsigned int temp; 

  good_count = 0;
  d51_status = smc_in(0);
  d51_diag = smc_in(1);
  
  if ((d51_status & ri_bit) != 0) {
    if ((d51_params[4] & 0x80) != 0) {
      if (d51_params[13] == 0x40) {
	smc_out(1, 8);
      } 
      offset = 0;
      address = (d51_rx_page << 9) + offset;
      smc_out(2, (address >> 8) | 0xc0);
      smc_out(3, address);
      index = inbuf_in;
      old_index = inbuf_in;
      for (i2 = 0; i2 <= 1; i2++) {
	inbuf[index] = smc_in(4);
	index++;
	if (index > 4095) {
	  index = 0;
	} 
      } 
      count[0] = smc_in(4);
      count[1] = smc_in(4);
      if (count[0] == 0) {
	offset = count[1];
	nbytes = 0x200 - offset;
	cnt = nbytes + 4;
	temp = index + 1;
	for (i2 = 1; i2 <= cnt; i2++) {
	  if (temp != inbuf_out) {
	    temp++;
	    if (temp > 4095) {
	      temp = 0;
	    } 
	    good_count = 1;
	  } else { 
	    good_count = 0;
	    break;
	  } 
	} 
	if (good_count == 1) {
	  inbuf[index] = 1;
	  index++;
	  if (index > 4095) {
	    index = 0;
	  } 
	  inbuf[index] = nbytes;
	  index++;
	  if (index > 4095) {
	    index = 0;
	  } 
	  address = (d51_rx_page << 9) + offset;
	  smc_out(2, (address >> 8) | 0xc0);
	  smc_out(3, address);
	  while (nbytes != 0) {
	    inbuf[index] = smc_in(4);
	    index++;
	    nbytes--;
	    if (index > 4095) {
	      index = 0;
	    } 
	    if (index == inbuf_out)
	      break;
	  } 
	} else { 
	  d51_diag_cnt[8]++;
	} 
      } else { 
	offset = count[0];
	nbytes = (unsigned char) (((unsigned char) 
				   ~((unsigned char) count[0])) + 1);
	cnt = nbytes + 4;
	temp = index + 1;
	for (i2 = 0; i2 <= cnt; i2++) {
	  if (temp != inbuf_out) {
	    temp++;
	    if (temp > 4095) {
	      temp = 0;
	    } 
	    good_count = 1;
	  } else { 
	    good_count = 0;
	    break;
	  } 
	} 
	
	if (good_count == 1) {
	  inbuf[index] = 0;
	  index++;
	  if (index > 4095) {
	    index = 0;
	  } 
	  inbuf[index] = nbytes;
	  index++;
	  if (index > 4095) {
	    index = 0;
	  } 
	  address = (d51_rx_page << 9) + offset;
	  smc_out(2, (address >> 8) | 0xc0);
	  smc_out(3, address);
	  while (nbytes != 0) {
	    inbuf[index] = smc_in(4);
	    index++;
	    nbytes--;
	    if (index > 4095) {
	      index = 0;
	    } 
	    if (index == inbuf_out)
	      break;
	  } 
	} else { 
	  d51_diag_cnt[8]++;
	} 
      } 
      if (good_count != 1) {
	inbuf_in = old_index;
      } else { 
	if (index != inbuf_out) {
	  inbuf_in = index;
	} 
      } 
      d51_rx_page = d51_in_page;
      d51_in_page++;
      if (d51_in_page >= d51_params[9])
	d51_in_page = 0;
      if (d51_params[13] == 0) {
	d51_rx_page = d51_in_page;
      } 
      smc_out(1, (d51_in_page << 3) | 0x04 | d51_params[11]);
      d51_diag_cnt[0]++;
    } 
  } 
  
  if (((d51_status & ta_bit) != 0) && ((d51_params[4] & 0x01) != 0) &&
      (d51_write_pending == 1)) {
    if (d51_params[13] == 0x40) {
      smc_out(1, 0);
    } 
    if (d51_broadcast != 0) {
      d51_write_stat = 0;
      d51_diag_cnt[9]++;
    } else { 
      if ((d51_status & 0x02) != 0) {
	d51_write_stat = 0;
	d51_diag_cnt[9]++;
      } else { 
	d51_write_stat = 0x13;
	d51_diag_cnt[10]++;
      } 
    } 
    d51_write_pending = 0;
  } 

  if (((d51_status & 0x04) != 0) && ((d51_params[4] & 0x04) != 0)) {
    smc_out(1, 0x16);
    d51_diag_cnt[2]++;
    if ((d51_diag & 0x80) != 0) 
      d51_diag_cnt[6]++;
  } 
  
  if (((d51_diag & 0x08) != 0) && ((d51_params[4] & 0x08) != 0) && 
      (d51_write_pending == 1)) {
    smc_out(1, 0x0e);
    
    if (d51_diag_cnt[7] == 0) {
      if (d51_params[6] != 0) {
	smc_out(1, 1);
	d51_write_stat = 0x0c;
	d51_diag_cnt[10]++;
	d51_diag_cnt[7] = d51_params[5];
      } else { 
	d51_write_stat = 0x11;
      } 
    } else { 
      d51_diag_cnt[7]--;
    } 
    d51_diag_cnt[1]++;
  } 
  
  if (((d51_diag & 0x02) != 0) && ((d51_params[4] & 0x02) != 0)) {
    smc_out(6, smc_in(6) & 0xfc);
    smc_out(6, smc_in(6) | 0x03);
    d51_nextid = smc_in(7);
    dummy = smc_in(1);
    d51_diag_cnt[3]++;
  } 
} 

void read_data(unsigned char page, unsigned char offset, unsigned char count, 
	       unsigned char shortlong, unsigned char *user_buffer)
{
  
  
  
  
  unsigned int address; 

  address = (page << 9) + offset;
  smc_out(2, (address >> 8) | 0xc0);
  smc_out(3, address);
  
  while (count != 0) {
    *user_buffer = smc_in(4);
    user_buffer++;
    count--;
  } 
  
  if (shortlong == 1) {
    count = 0;
    do {
      *user_buffer = smc_in(4);
      user_buffer++;
      count--;
    } while (count != 0);
  } 
} 

void write_data(unsigned char page, unsigned char offset, unsigned char count,
		unsigned char shortlong, unsigned char *user_buffer)
{
  
  
  
  
  unsigned int address; 

  address = (page << 9) + offset;
  smc_out(2, ((address >> 8) | 0x40) & 0x7f);
  smc_out(3, address);
  while (count-- != 0) {
    smc_out(4, *user_buffer++);
  } 

  if (shortlong == 1) {
    count = 0;
    do {
      smc_out(4, *user_buffer);
      user_buffer++;
      count--;
    } while (count != 0);
  } 
} 

unsigned char check_network_status()
{
  unsigned char status; 
  unsigned char aux; 
  unsigned char consec_recon; 
  unsigned int cnt; 

  status = 0;
  consec_recon = 0;
  cnt = 0;
  dummy = smc_in(1);
  smc_out(1, 0x1e);
  DELAYMS(200);
  DELAYMS(200);
  DELAYMS(200);
  DELAYMS(240);
  aux = smc_in(1);
  if ((smc_in(6) & 0x20) == 0) {
    if (((aux & 0x40) == 0) && ((aux & 0x20) != 0) && ((aux & 0x10) != 0)) {
      status = 0;
    } else { 
      if ((aux & 0x40) != 0) {
	status = 0x0d;
      } else { 
	if (((aux & 0x10) == 0) && ((aux & 0x20) != 0)) {
	  status = 0x08;
	} else { 
	  if (((aux & 0x10) == 0) && ((aux & 0x20) == 0)) {
	    status = 0x14;
	  } else { 
	    status = -1;
	  } 
	} 
      } 
    } 
  } else { 
    if (((aux & 0x20) != 0) && ((aux & 0x10) != 0)) {
      status = 0;
    } else { 
      if (((aux & 0x10) == 0) && ((aux & 0x20) != 0)) {
	status = 8;
      } else { 
	if (((aux & 0x10) == 0) && ((aux & 0x20) == 0)) {
	  status = 0x14;
	} else {
	  status = -1;
	}
      } 
    } 
  } 

  return status;
}