gdb2.c

嵌入式开发板连接程序源码

    return;
  }

  gdb_fd = fd;
}

void GDBRequest() {
  JTAGProxyWriteMessage msg_write;
  JTAGProxyReadMessage msg_read;
  JTAGProxyChainMessage msg_chain;
  JTAGProxyWriteResponse resp_write;
  JTAGProxyReadResponse resp_read;
  JTAGProxyChainResponse resp_chain;
  JTAGProxyBlockWriteMessage *msg_bwrite;
  JTAGProxyBlockReadMessage msg_bread;
  JTAGProxyBlockWriteResponse resp_bwrite;
  JTAGProxyBlockReadResponse *resp_bread;
  char *buf;
  unsigned long long data;  
  uint32_t command, length;
  int len, i;
  int err = 0;

  /* First, we must read the incomming command */
  if(gdb_read(&command, sizeof(uint32_t)) < 0) {
    if(gdb_fd) {
      perror("gdb socket - 1");
      close(gdb_fd);
      gdb_fd = 0;
    }
    return;
  }
  if(gdb_read(&length, sizeof(uint32_t)) < 0) {
    if(gdb_fd) {
      perror("gdb socket - 2");
      close(gdb_fd);
      gdb_fd = 0;
    }
    return;
  }
  length = ntohl(length);

  /* Now, verify the protocol and implement the command */
  switch(ntohl(command)) {
    case JTAG_COMMAND_WRITE:
      if(length != sizeof(msg_write) - 8) {
        ProtocolClean(length, JTAG_PROXY_PROTOCOL_ERROR);
        return;
      }
      buf = (char*)&msg_write;
      if(gdb_read(&buf[8], length) < 0) {
        if(gdb_fd) {
          perror("gdb socket - 3");
          close(gdb_fd);
          gdb_fd = 0;
        }
        return;
      }
      msg_write.address = ntohl(msg_write.address);
      msg_write.data_H = ntohl(msg_write.data_H);
      msg_write.data_L = ntohl(msg_write.data_L);
      err = gdb_write_reg(msg_write.address, msg_write.data_L);      
      resp_write.status = htonl(err);      
      if(gdb_write(&resp_write, sizeof(resp_write)) < 0) {
        if(gdb_fd) {
          perror("gdb socket - 4");
          close(gdb_fd);
          gdb_fd = 0;
        }
        return;
      }
      break;
    case JTAG_COMMAND_READ:
      if(length != sizeof(msg_read) - 8) {
        ProtocolClean(length, JTAG_PROXY_PROTOCOL_ERROR);
        return;
      }
      buf = (char*)&msg_read;
      if(gdb_read(&buf[8], length) < 0) {
        if(gdb_fd) {
          perror("gdb socket - 5");
          close(gdb_fd);
          gdb_fd = 0;
        }
        return;
      }
      msg_read.address = ntohl(msg_read.address);
      err = gdb_read_reg(msg_read.address, (unsigned long *)&resp_read.data_L);
      resp_read.status = htonl(err);
      resp_read.data_H = 0;
      resp_read.data_L = htonl(resp_read.data_L);
      if(gdb_write(&resp_read, sizeof(resp_read)) < 0) {
        if(gdb_fd) {
          perror("gdb socket - 6");
          close(gdb_fd);
          gdb_fd = 0;
        }
        return;
        }
      break;
    case JTAG_COMMAND_BLOCK_WRITE:
      if(length < sizeof(JTAGProxyBlockWriteMessage)-8) {
        ProtocolClean(length, JTAG_PROXY_PROTOCOL_ERROR);
        return;
      }
      if(!(buf = (char*)malloc(8+length))) {
        ProtocolClean(length, JTAG_PROXY_OUT_OF_MEMORY);
        return;
      }
      msg_bwrite = (JTAGProxyBlockWriteMessage*)buf;
      if(gdb_read(&buf[8], length) < 0) {
        if(gdb_fd) {
          perror("gdb socket - 5");
          close(gdb_fd);
          gdb_fd = 0;
        }
        free(buf);
        return;
      }
      msg_bwrite->address = ntohl(msg_bwrite->address);
      msg_bwrite->nRegisters = ntohl(msg_bwrite->nRegisters);
      for(i=0;i<msg_bwrite->nRegisters;i++) {
        msg_bwrite->data[i] = ntohl(msg_bwrite->data[i]);
      }
      err = gdb_write_block(msg_bwrite->address, (unsigned long*)msg_bwrite->data, msg_bwrite->nRegisters * 4);
      resp_bwrite.status = htonl(err);
      free(buf);
      msg_bwrite = (JTAGProxyBlockWriteMessage *)NULL;
      buf = (char *)msg_bwrite;
      if(gdb_write(&resp_bwrite, sizeof(resp_bwrite)) < 0) {
        if(gdb_fd) {
          perror("gdb socket - 4");
          close(gdb_fd);
          gdb_fd = 0;
        }
        return;
      }
      break;
    case JTAG_COMMAND_BLOCK_READ:
      if(length != sizeof(msg_bread) - 8) {
        ProtocolClean(length, JTAG_PROXY_PROTOCOL_ERROR);
        return;
      }
      buf = (char*)&msg_bread;
      if(gdb_read(&buf[8], length) < 0) {
        if(gdb_fd) {
          perror("gdb socket - 5");
          close(gdb_fd);
          gdb_fd = 0;
        }
        return;
      }
      msg_bread.address = ntohl(msg_bread.address);
      msg_bread.nRegisters = ntohl(msg_bread.nRegisters);
      len = sizeof(JTAGProxyBlockReadResponse) + 4*(msg_bread.nRegisters-1);
      if(!(buf = (char*)malloc(len))) {
        ProtocolClean(0, JTAG_PROXY_OUT_OF_MEMORY);
        return;
      }
      resp_bread = (JTAGProxyBlockReadResponse*)buf;
      err = gdb_read_block(msg_bread.address, (unsigned long*)resp_bread->data, msg_bread.nRegisters * 4);
      for(i=0;i<msg_bread.nRegisters;i++) {
        /* Read previous, address next one. */
        resp_bread->data[i] = htonl(resp_bread->data[i]);
      }
      resp_bread->status = htonl(err);
      resp_bread->nRegisters = htonl(msg_bread.nRegisters);
      if(gdb_write(resp_bread, len) < 0) {
        if(gdb_fd) {
          perror("gdb socket - 6");
          close(gdb_fd);
          gdb_fd = 0;
        }
        free(buf);
        return;
      }
      free(buf);
      resp_bread = (JTAGProxyBlockReadResponse *)NULL;
      buf = (char *)resp_bread;
      break;
    case JTAG_COMMAND_CHAIN:
      if(length != sizeof(msg_chain) - 8) {
        ProtocolClean(length, JTAG_PROXY_PROTOCOL_ERROR);
        return;
      }
      buf = (char*)&msg_chain;
      if(gdb_read(&buf[8], sizeof(msg_chain)-8) < 0) {
        if(gdb_fd) {
          perror("gdb socket - 7");
          close(gdb_fd);
          gdb_fd = 0;
        }
        return;
      }
      msg_chain.chain = htonl(msg_chain.chain);
      err = gdb_set_chain(msg_chain.chain);
      resp_chain.status = htonl(err);
      if(gdb_write(&resp_chain, sizeof(resp_chain)) < 0) {
        if(gdb_fd) {
          perror("gdb socket - 8");
          close(gdb_fd);
          gdb_fd = 0;
        }
        return;
      }
      break;
    default:
      perror("Unknown JTAG command.");
      ProtocolClean(length, JTAG_PROXY_COMMAND_NOT_IMPLEMENTED);
      break;
  }
}

static void ProtocolClean(int length, int32_t err) {
  char buf[4096];

  err = htonl(err);
  if((gdb_read(buf, length) < 0) || (gdb_write(&err, sizeof(err)) < 0) && gdb_fd) {
    perror("gdb socket - 9");
    close(gdb_fd);
    gdb_fd = 0;
  }
}

static int gdb_write(void* buf, int len) {
  int n;
  char* w_buf = (char*)buf;
  struct pollfd block;

  while(len) {
    if((n = write(gdb_fd, w_buf, len)) < 0) {
      switch(errno) {
        case EWOULDBLOCK: /* or EAGAIN */
          /* We've been called on a descriptor marked
             for nonblocking I/O. We better simulate
             blocking behavior. */
          block.fd = gdb_fd;
          block.events = POLLOUT;
          block.revents = 0;
          poll(&block, 1, -1);
          continue;
        case EINTR:
          continue;
        case EPIPE:
          close(gdb_fd);
          gdb_fd = 0;
          return -1;
        default:
          return -1;
        }
      } else {
        len -= n;
        w_buf += n;
      }
  }
  return 0;
}

static int gdb_read(void* buf, int len) {
  int n;
  char* r_buf = (char*)buf;
  struct pollfd block;

  while(len) {
    if((n = read(gdb_fd, r_buf, len)) < 0) {
      switch(errno) {
        case EWOULDBLOCK: /* or EAGAIN */
          /* We've been called on a descriptor marked
       for nonblocking I/O. We better simulate
       blocking behavior. */
          block.fd = gdb_fd;
          block.events = POLLIN;
          block.revents = 0;
          poll(&block, 1, -1);
          continue;
        case EINTR:
          continue;
        default:
          return -1;
        }
    } else if(n == 0) {
      close(gdb_fd);
      gdb_fd = 0;
      return -1;
    } else {
      len -= n;
      r_buf += n;
    }
  }
  return 0;
}