/********************************************************* * * This source code is part of the Carnegie Mellon Robot * Navigation Toolkit (CARMEN) * * CARMEN Copyright (c) 2002 Michael Montemerlo, Nicholas * Roy, Sebastian Thrun, Dirk Haehnel, Cyrill Stachniss, * and Jared Glover * * CARMEN is free software; you can redistribute it and/or  * modify it under the terms of the GNU General Public  * License as published by the Free Software Foundation;  * either version 2 of the License, or (at your option) * any later version. * * CARMEN is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR * PURPOSE.  See the GNU General Public License for more  * details. * * You should have received a copy of the GNU General  * Public License along with CARMEN; if not, write to the * Free Software Foundation, Inc., 59 Temple Place,  * Suite 330, Boston, MA  02111-1307 USA * ********************************************************/#include <sys/time.h>#include <sys/types.h>#include <sys/stat.h>#include <fcntl.h>#include <unistd.h>#include <stdio.h>unsigned char d51_status = 0; unsigned char d51_diag = 0; unsigned char d51_nextid = 0; unsigned char d51_cmd = 0; unsigned char d51_config = 0; unsigned char d51_tentid = 0; unsigned char d51_setup = 0; unsigned char d51_in_page = 0; unsigned char d51_out_page = 0; unsigned char d51_broadcast = 0; unsigned char d51_rx_page = 0; unsigned char d51_write_stat = -1; unsigned char d51_write_pending = 0; unsigned char inbuf[4096]; unsigned char d51_params[26]; unsigned char j; unsigned char d51_net_map[32]; unsigned int d51_diag_cnt[13]; unsigned int inbuf_in; unsigned char i; unsigned char buffer[16]; unsigned int inbuf_out; unsigned char ta_bit; unsigned char dummy; unsigned char ri_bit; void d51_clear_diag();unsigned char d51_free_node();void d51_tokens(unsigned char ntokens);void d51_check_int();void d51_get_qentry(unsigned char *ptr);void read_data(unsigned char page, unsigned char offset, unsigned char count, 	       unsigned char shortlong, unsigned char *user_buffer);void write_data(unsigned char page, unsigned char offset, unsigned char count,		unsigned char shortlong, unsigned char *user_buffer);unsigned char check_network_status();void DELAY10US(unsigned int us){  struct timeval tv;   long unsigned int sec;   long unsigned int usec;   long unsigned int norm_usec;   gettimeofday(&tv, NULL);  sec = tv.tv_sec;  usec = tv.tv_usec;  do {    gettimeofday(&tv, NULL);    norm_usec = (tv.tv_sec - sec) * 1000000 + (tv.tv_usec - usec);  } while ((10 * us) > norm_usec);}void DELAYMS(unsigned int ms){  struct timeval tv;   long unsigned int sec;   long unsigned int usec;   long unsigned int norm_usec;   gettimeofday(&tv, NULL);  sec = tv.tv_sec;  usec = tv.tv_usec;  do {    gettimeofday(&tv, NULL);    norm_usec = (tv.tv_sec - sec) * 1000000 + (tv.tv_usec - usec);  } while ((1000 * ms) > norm_usec);}int smc_fd = 0;void smc_out(unsigned char reg, unsigned char val){      lseek(smc_fd, reg + 0x2e0, 0);  write(smc_fd, &val, 1);  }unsigned char smc_in(unsigned char reg){    unsigned char val;   lseek(smc_fd, reg + 0x2e0, 0);  read(smc_fd, &val, 1);  return val;}unsigned char d51_init(){  unsigned char return_code = 0;   unsigned int click;   smc_fd = open("/dev/port", 2);  if (smc_fd < 0) {    return -1;  }     inbuf_in = 0;  inbuf_out = 0;  if (d51_params[13] == 0) {    ri_bit = 0x80;    ta_bit = 0x01;  } else {     ri_bit = 0x40;    ta_bit = 0x20;  }     d51_clear_diag();  smc_out(6, smc_in(6) & 0xfc);  smc_out(6, smc_in(6) | 0x02);  smc_out(7, smc_in(7) | d51_params[17]);  smc_out(7, smc_in(7) | d51_params[18]);  smc_out(7, smc_in(7) | d51_params[19]);  smc_out(7, smc_in(7) | d51_params[20]);  smc_out(7, smc_in(7) | d51_params[21]);  smc_out(7, smc_in(7) | d51_params[22]);  smc_out(6, smc_in(6) | d51_params[13]);  smc_out(6, smc_in(6) | d51_params[14]);  smc_out(6, smc_in(6) | d51_params[15]);  smc_out(6, smc_in(6) & 0xfc);  smc_out(6, smc_in(6) | 0x01);  smc_out(7, 0xfe);  smc_out(6, smc_in(6) | 0x80);  DELAYMS(10);  smc_out(6, smc_in(6) & 0x7f);    click = 0;  buffer[0] = 0;  buffer[1] = 0;  while ((buffer[0] != 0xd1) || (buffer[1] != 0xfe)) {    read_data(0, 0, 2, 0, buffer);    DELAYMS(10);    click++;    if (click > 1000) {      return 2;    }   }     smc_out(1, 0x1e);    if (d51_params[0] == 0) {    smc_out(6, smc_in(6) | 0x20);    dummy = d51_free_node();    smc_out(6, smc_in(6) & 0xdf);    if (dummy == 0) {      return 0x15;    } else {       smc_out(6, smc_in(6) & 0xfc);      smc_out(6, smc_in(6) | 0x01);      smc_out(7, dummy);    }         d51_tokens(1);    if ((smc_in(1) & 0x40) != 0) {      return_code = 0x0f;    } else {       return_code = 0;    }      } else {     if (d51_params[0] == 1) {      switch (check_network_status()) {      case 0x00: 	smc_out(6, smc_in(6) & 0xfc);	smc_out(6, smc_in(6) | 0x01);	smc_out(7, d51_params[16]);	d51_tokens(1);	return_code = 0;	break;	      case 0x08:       case 0x0e:       case 0x14: 	smc_out(6, smc_in(6) & 0xfc);	smc_out(6, smc_in(6) | 0x01);	smc_out(7, d51_params[16]);	return_code = 0;	break;      default: 	return -1;      }     }   }   smc_out(1, 0x1e);    if (d51_params[12] == 0) {    smc_out(1, 0x05);  } else {     smc_out(1, 0x0d);  }   d51_in_page = 0;  d51_rx_page = 0;   d51_out_page = d51_params[9];  d51_write_pending = 0;  smc_out(6, smc_in(6) | 0x20);  smc_out(1, (d51_in_page << 3) | 0x04 | d51_params[11]);  if (d51_params[13] == 0x40) {    smc_out(1, ((d51_in_page + 1) << 3) | 0x04 | d51_params[11]);  }   return return_code;}unsigned char d51_read_packet(unsigned char wait_flag, unsigned char *data_ptr){    unsigned char status;   if (wait_flag == 1) {    do {      d51_check_int();    } while ((d51_status & ri_bit) == 0);  } else {     if (wait_flag == 0) {      d51_check_int();      if ((d51_status & ri_bit) == 0)	status = 7;    } else {       return -1;    }   }   if (inbuf_in != inbuf_out) {    status = 0;    d51_get_qentry(data_ptr);  } else {     status = 7;  }     return status;}unsigned char d51_write_packet(unsigned char *data_ptr){  unsigned char count;   unsigned char offset;   unsigned char status;   unsigned char counts[2];   write_data(d51_out_page, 0, 2, 0, data_ptr);  data_ptr++;  if (*data_ptr != 0) {    d51_broadcast = 0;  } else {     d51_broadcast = 1;  }     data_ptr++;  if (*data_ptr != 0) {    if (d51_params[12] != 1) {      return 0x12;    }         count = -data_ptr[1];    data_ptr++;    if (count == 0) {      return 9;    } else {       offset = ~(*data_ptr) + 1;      counts[0] = 0;      counts[1] = offset;      write_data(d51_out_page, 2, 2, 0, counts);      data_ptr++;      write_data(d51_out_page, offset, *(data_ptr - 1), 1, data_ptr);    }   } else {     data_ptr++;    count = *data_ptr;    if ((count == 0) || (count > 0xfd)) {      return 9;    } else {       offset = ~count + 1;      counts[0] = offset;      write_data(d51_out_page, 2, 1, 0, counts);      data_ptr++;      write_data(d51_out_page, offset, count, 0, data_ptr);    }   }     d51_diag_cnt[7] = d51_params[5];  if (d51_params[8] == 1) {    while ((smc_in(0) & ta_bit) == 0)      ;  } else {     if ((smc_in(0) & ta_bit) == 0) {      return 0x0a;    }   }     smc_out(1, (d51_out_page << 3) | 3);  d51_out_page++;  if (d51_out_page > 1) {        d51_out_page = d51_params[9];  }     d51_write_pending = 1;  d51_diag_cnt[4]++;  if (d51_params[7] == 1) {    d51_write_stat = -1;    do {      d51_check_int();    } while (!((d51_write_stat == 0x13) || (d51_write_stat == 0)));    return d51_write_stat;  }     status = 0;  return status;}void d51_get_qentry(unsigned char *ptr){  unsigned int index;   unsigned int count;   unsigned int i2;   index = inbuf_out;  i2 = 0;  count = 2;   while (count != 0) {    ptr[i2] = inbuf[index];    index++;    i2++;    count--;    if (index > 4095) {      index = 0;    }     if (i2 == 2) {      count = inbuf[index];      count <<= 8;      if (index + 1 > 4095) {	count |= inbuf[0];      } else { 	count |= inbuf[index + 1];      }      count += 2;    }   }     inbuf_out = index;}unsigned char d51_network_map(){  unsigned char recon_cnt;   unsigned char aux_reg;   unsigned char mask;   unsigned char status;   int aux;   unsigned char a;   unsigned char b;   unsigned char c;  unsigned char d;     for (aux = 0; aux < 32; aux++) {    d51_net_map[aux] = 0;  }   status = check_network_status();  smc_out(6, smc_in(6) & 0xfc);  smc_out(6, smc_in(6) | 0x01);  mask = 1 << (smc_in(7) & 0x07);  d = (smc_in(7) >> 3);  d51_net_map[d] |= mask;    if (status != 0) {    return status;  }     smc_out(6, smc_in(6) & 0xfc);  smc_out(6, smc_in(6) | 0x03);  mask = 1 << (smc_in(7) & 0x07);  a = smc_in(7) >> 3;  d51_net_map[a] |= mask;    smc_out(6, smc_in(6) & 0xfc);  smc_out(6, smc_in(6));    if ((smc_in(6) & 0x20) == 0) {    smc_out(7, 1);    while (smc_in(7) != 0) {