e100boot.c

linux平台上的开放源代码的网络摄像机程序.实现视频捕捉,传输以及云台控制等.非常具有参考价值.

/*
 * *BSD doesn't have the Linux specific SOCK_PACKET and ETH_P_ALL (obviously)
 * but the same thing can be accomplished with BPF (Berkley Packet Filter).
 *
 * NOTE: The use of K&R style is intentional, basically because of
 * two reasons, (1) I can't read the strange GNU style and (2) I
 * don't like it. If this is a problem, please re-indent! :-) /tobba
 */
int
InitSendSocket(char *device_name)
{
	struct ifreq ifr;
	int fd;
	u_int ipar = 1;
	u_int buflen = 1579;	/* MTU, must be greater than 1500... */

	if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
		perror("socket");
		exit(EXIT_FAILURE);
	}
	
	sock_addr.sa_family = AF_INET;
	strncpy(sock_addr.sa_data, device_name, strlen(sock_addr.sa_data));
	
	/* Get the interface flags. */
	if ((ioctl(fd, SIOCGIFFLAGS, (caddr_t) &ifr)) < 0) {
		perror("error getting interface flags");
		exit(EXIT_FAILURE);
	}
	
	close(fd);
	
	if ((sock_fd = open("/dev/bpf0", O_RDWR | O_NONBLOCK)) < 0) {
		perror("error opening bpf0");
		exit(EXIT_FAILURE);
	}
	
	fcntl(sock_fd, F_SETFL, O_NONBLOCK);

	if ((ioctl(sock_fd, BIOCSBLEN, &buflen)) < 0) {
		perror("error getting buffer length");
		exit(EXIT_FAILURE);
	}
	
	if ((ioctl(sock_fd, BIOCSETIF, (caddr_t) &ifr)) < 0) {
		perror("error setting interface param");
		exit(EXIT_FAILURE);
	}
	
	if ((ioctl(sock_fd, BIOCPROMISC, NULL)) < 0) {
		perror("error setting device to promiscious mode");
		exit(EXIT_FAILURE);
	}
	
	ipar = 1;
	
	if ((ioctl(sock_fd, BIOCIMMEDIATE, &ipar)) < 0) {
		perror("BIOCIMMEDIATE");
		exit(EXIT_FAILURE);
	}
	
	return sock_fd;
}

#else

int
InitSendSocket(char *device_name)
{
  if ((sock_fd = socket(AF_INET, SOCK_PACKET, htons(ETH_P_ALL))) < 0) {
    perror("Socket call failed:");
    exit(-1);
  }

  fcntl(sock_fd, F_SETFL, O_NDELAY);

  sock_addr.sa_family = AF_INET;
  strcpy(sock_addr.sa_data, device_name);

  return sock_fd;
}

#endif /* __*BSD__ */

/****************************************************************************
*#
*#  FUNCTION NAME: CreateBootLoader
*#
*#  PARAMETERS: None.
*#
*#  DESCRIPTION: Creates boot packets from a boot file or internal loader.
*#
*#---------------------------------------------------------------------------
*#  DATE     NAME     CHANGES
*#  ----     ----     -------
*#  960909   ronny    Initial version
*#***************************************************************************/

void
CreateBootLoader()
{
  udword xchecksum; //Andrey
  udword *xdata; //Andrey

  struct stat st;
  FILE *fd;
  char *buf = NULL;
  int size_pos = 0x18;
  int addr_pos = 0x28;
  struct packet_header_T *ph;
  int packet_size;
  int header_size;
  int buf_cnt = 0;
  int i;
  udword sum = 0;

  if (create_boot_loader) {
    int image_nbr = 0;
    int found = 0;
    const struct boot_image_info_type *info;

    info = &boot_image_info[image_nbr];

    if (db4) printf("> CreateBootLoader\n");

    while (!found && info->name != NULL) {
      if (strcmp(boot_loader_file, info->name) == 0) {
        st.st_size = info->len;
        buf = (char*) malloc(st.st_size);
        memcpy(buf, info->ptr, st.st_size);
        found = 1;
        printf("Using internal boot loader: %s - %s.\n",
               info->name, info->info);
      }
      else {
        image_nbr++;
        info = &boot_image_info[image_nbr];
      }
    }

    if (!found) {
      if ((fd = Fopen(boot_loader_file, "r")) == NULL) {
        printf("Cannot open bootloader '%s'. %s.\n",
               boot_loader_file, strerror(errno));
        exit(1);
      }

      if (fstat(fileno(fd), &st) == -1) {
        printf("Cannot get filestatus of bootloader '%s'. %s.\n",
               boot_loader_file, strerror(errno));
        exit(1);
      }

      buf = (char*) malloc(st.st_size);
      printf("CreateBootLoader: buf = (char*) malloc(st.st_size); 2\n");
      if (read(fileno(fd), buf, st.st_size) != st.st_size) {
        printf("Read fewer bytes than there should be in %s.\n",
               boot_loader_file);
        exit(1);
      }

      fclose(fd);
    }

    /* Ignore the first two nops */
    st.st_size -= 4;
    buf += 4;

    if (*(udword*)&buf[size_pos] != 0x050fb67f) {
      printf("Bootloader corrupt. Should contain nops but dont. %x\n", *(udword*)&buf[size_pos]);
      exit(1);
    }
    *(udword*)(&buf[size_pos]) = st.st_size - DATA_SIZE; /* How much data to load except data in
							    first packet. */
    if (db3) printf("Inserting boot size 0x%x at 0x%x.\n", (unsigned int)st.st_size - DATA_SIZE, 
		    (unsigned int)&buf[size_pos]);

    if (*(udword*)&buf[addr_pos] != 0x050fb67f) {
      printf("Bootloader corrupt. Should contain nops but dont.\n");
      exit(1);
    }
    *(udword*)(&buf[addr_pos]) = BOOT_ADDRESS + DATA_SIZE;
    if (db3) printf("Inserting boot address 0x%x at 0x%x.\n", 
		    (unsigned int)BOOT_ADDRESS + DATA_SIZE, (unsigned int)&buf[addr_pos]);

    for (i = 0; i != st.st_size; i++) {
      sum += ((byte*)buf)[i];
    }
    if (db1) printf("Checksum 0x%x, bytes %d\n", sum, i);

    while (buf_cnt <= st.st_size) {
      /*      printf("%8.8x\n", last_packet);*/
      /*      printf("next %8.8x\n",last_packet->next);*/

      header_size = seq_nr == 0 ? 
        SIZE_OF_FIRST_HEADER : sizeof(struct packet_header_T);
      packet_size = ((st.st_size - buf_cnt) < DATA_SIZE ? 
                     st.st_size - buf_cnt : DATA_SIZE) + header_size;

      last_packet = allocate_packet(last_packet);

      first_packet = first_packet ? first_packet : last_packet;

      last_packet->size = packet_size;
      last_packet->data = (char*)malloc(packet_size);
      last_packet->seq = seq_nr;

      /*      printf("size %8.8x\n", last_packet->size);*/
      /*      printf("data %8.8x\n",last_packet->data);*/

      ph = (struct packet_header_T*) last_packet->data;
      memcpy(ph->dest, dst_addr_of_device, 6);
      memcpy(ph->src,  eth_addr_local,     6);
      ph->length = htons(packet_size);
      ph->snap1  = htonl(SNAP1);
      ph->snap2  = htonl(SNAP2);
      ph->tag    = htonl(SERVER_TAG);
      ph->seq    = htonl(seq_nr);
      if (seq_nr != 0) {
	xchecksum=ph->seq;			// Andrey
	xdata= (udword *) &buf[buf_cnt];	// Andrey
	for (i=0;i<(int) ((packet_size - header_size)>>2);i++) xchecksum+=xdata[i];	// Andrey
        ph->type   = htonl(BOOT_PACKET);
//        ph->id     = htonl(0);
        ph->id     = xchecksum;
  if (db4) printf("CreateBootLoader: ph->seq=%d, xchecksum=%x",ph->seq,xchecksum);

      }

      memcpy(&last_packet->data[header_size], &buf[buf_cnt], 
             packet_size - header_size);
      if (db3) DecodeSvintoBoot(last_packet->data);
      /*      PrintPacket(last_packet->data, last_packet->size, HEX);*/
      buf_cnt += DATA_SIZE;
      seq_nr++;
    }
  }

  if (db4) printf("< CreateBootLoader\n");
}

struct packet_buf*
allocate_packet(struct packet_buf *p)
{

  if (p) {
    p->next = (struct packet_buf*) malloc(sizeof(struct packet_buf));
    p       = p->next;
  }
  else {
    p = (struct packet_buf*) malloc(sizeof(struct packet_buf));
  }
  p->next = NULL;
  
  return(p);
}

struct boot_files_T*
allocate_boot_file(struct boot_files_T *bf)
{

  if (bf) {
    bf->next = (struct boot_files_T*) malloc(sizeof(struct boot_files_T));
    bf       = bf->next;
  }
  else {
    bf = (struct boot_files_T*) malloc(sizeof(struct boot_files_T));
  }
  bf->next = NULL;
  
  return(bf);
}

/****************************************************************************
*#
*#  FUNCTION NAME: CreateBootCmds
*#
*#  PARAMETERS: None.
*#
*#  DESCRIPTION: Creates boot packets from the boot commands.
*#
*#---------------------------------------------------------------------------
*#  DATE     NAME     CHANGES
*#  ----     ----     -------
*#  980818   ronny    Initial version
*#***************************************************************************/

void
CreateBootCmds()
{
  udword xchecksum; //Andrey
  udword *xdata; //Andrey
  int i; //Andrey
  struct packet_header_T *ph;

  if (db4) printf("***> CreateBootCmds\n");

  last_packet = allocate_packet(last_packet);

  boot_cmds_packet = last_packet;

  last_packet->size = SIZE_OF_BOOT_CMDS + sizeof(struct packet_header_T);
  last_packet->data = (char *) malloc(last_packet->size);
  last_packet->seq = seq_nr;
  /* Create packet header. */
  ph = (struct packet_header_T *) last_packet->data;
  memcpy(ph->dest, dst_addr_of_device, 6);
  memcpy(ph->src, eth_addr_local, 6);
  ph->length = htons(last_packet->size);
  ph->snap1 = htonl(SNAP1);
  ph->snap2 = htonl(SNAP2);
  ph->tag = htonl(SERVER_TAG);
  ph->seq = htonl(seq_nr);
  seq_nr++;
  ph->type = htonl(BOOT_CMDS);
//  ph->id = htonl(0);
  xchecksum=ph->seq;			// Andrey
  xdata= (udword *) boot_cmds;	// Andrey
  for (i=0;i<(int) ((SIZE_OF_BOOT_CMDS)>>2);i++) xchecksum+=htonl(xdata[i]);	// Andrey
  ph->id     = xchecksum;	// Andrey
  if (db4) printf("CreateBootCmds: ph->seq=%d, xchecksum=%x",ph->seq,xchecksum);

  if (db3) DecodeSvintoBoot(last_packet->data);
  if (db4) printf("<*** CreateBootCmds\n");


}

void
FinishBootCmds()
{
  int i;

  for (i = 0; i != boot_cmds_cnt; i++) {
    boot_cmds[i] = htonl(boot_cmds[i]);
    if (db3) printf("%8.8x\n", boot_cmds[i]);
  }

  /* Copy boot commands into packet. */
  memcpy(&boot_cmds_packet->data[sizeof(struct packet_header_T)], boot_cmds,
	 boot_cmds_cnt * sizeof(udword));

}

/****************************************************************************
*#
*#  FUNCTION NAME: CreateBootPacket
*#
*#  PARAMETERS: None.
*#
*#  DESCRIPTION: 
*#
*#---------------------------------------------------------------------------
*#  DATE     NAME     CHANGES
*#  ----     ----     -------
*#  960909   ronny    Initial version
*#***************************************************************************/

struct packet_buf*
CreateNewBootPacket()
{
  udword xchecksum; //Andrey
  udword *xdata; //Andrey
  int i; //Andrey

  static char buf[DATA_SIZE];
  struct packet_header_T *ph;
  int packet_size;
  int header_size;
#if 0
  int i;
  udword sum;
#endif
  int size = 0;
  int padding = 0;
  
  static struct boot_files_T *bf = NULL;

  if (db3) printf("> CreateNewBootPacket\n");

  bf = bf ? bf : first_boot_file;

  while(bf) {
    if (!bf->fd) {
      if (strcmp(bf->fileName, "-") == 0) {
        bf->fd = stdin;
      }
      else {
        bf->fd = fopen(bf->fileName, "r");
      }
      
      if (bf->fd == NULL) {
        printf("Cannot open boot file %s. Exiting\n", bf->fileName);
        exit(0);
      }
      if (db3) printf("Opening boot file %s\n", bf->fileName);
    }
  
    if (!padding)
    {
      size = fread(buf, 1, DATA_SIZE, bf->fd);
      if (size == 0)
      {
        printf("Nothing more to read. Read: %d/%d\n", 
               bf->size_sent, bf->size);
        padding = 1;
      }
    }
    if (padding)
    {

      if (bf->size_sent < bf->size)
      {
        printf("padding...\n");
        size = (bf->size - bf->size_sent > DATA_SIZE) ? 
          DATA_SIZE : bf->size - bf->size_sent;
        memset(buf, 0, size);
      }
      else
      {
        printf("All written\n");
        padding = 0;
        size = 0;
      }
    }
    
    if (size != 0) {
      if (db2) printf("size: %d %d/%d\n", size, bf->size_sent, bf->size);
      bf->size_sent += size;
      last_packet = allocate_packet(last_packet);

#if 0
      /* Calculate checksum. */
      sum = 0;
      for (i = 0; i != size; i++) {
        sum += ((byte*)buf)[i];
      }
      if (db2) printf("Checksum 0x%x, bytes %d\n", sum, i);
     
#endif 
      /* Figure out size of packet. */
      header_size = seq_nr == 0 ? 
        SIZE_OF_FIRST_HEADER : sizeof(struct packet_header_T);
      packet_size = ((size) < DATA_SIZE ? size : DATA_SIZE) +	header_size;
    

      if (packet_size < 64) {
        printf(
          "Last packet from file %s, contained a packet smaller than 64 "
          "bytes. \n"
          "This is not allowed in the Ethernet standard. Will pad with %d "
          "bytes.\n", bf->fileName, 60-packet_size);
	
        *(bf->size_p) += 60-packet_size;
        packet_size = 60;
      }
    
      last_packet->size = packet_size;
      last_packet->data = (char*)malloc(packet_size);
    
      /*      printf("size %8.8x\n", last_packet->size);*/
      /*      printf("data %8.8x\n",last_packet->data);*/
    
      /* Initialize ethernet header. */
      ph = (struct packet_header_T*) last_packet->data;
      memcpy(ph->dest, dst_addr_of_device, 6);
      memcpy(ph->src,  eth_addr_local,     6);
      /*      printf("packet_size %d\n", packet_size);*/
      ph->length = htons(packet_size);
      ph->snap1  = htonl(SNAP1);
      ph->snap2  = htonl(SNAP2);
      ph->tag    = htonl(SERVER_TAG);
      ph->seq    = htonl(seq_nr);
      last_packet->seq = seq_nr;
      if (seq_nr != 0) {
        ph->type   = htonl(BOOT_PACKET);
	xchecksum=ph->seq;			// Andrey
	xdata= (udword *) buf;	// Andrey
	for (i=0;i<(int) ((packet_size - header_size)>>2);i++) xchecksum+=xdata[i];	// Andrey
        ph->id     = xchecksum;
  if (db4) printf("CreateBootLoader: ph->seq=%d, xchecksum=%x",ph->seq,xchecksum);

//        ph->id     = htonl(0);
      }
 
   
      /* Copy data in place. */
      memcpy(&last_packet->data[header_size], buf, packet_size - header_size);
      if (db2) DecodeSvintoBoot(last_packet->data);
      /*      PrintPacket(last_packet->data, last_packet->size, HEX);*/
      seq_nr++;
    
      if (db3) printf("< CreateNewBootPacket\n");
      return(last_packet);
    }  
    else 
    {			/* Nothing read from fd. */
      fclose(bf->fd);
      bf = bf->next;
    }
  }
  
  if (db3) printf("< CreateNewBootPacket\n");
  return(NULL);
}
  
/****************************************************************************
 *#
 *#  FUNCTION NAME: timeval_subtract
 *#
 *#  PARAMETERS: 
 *#
 *#  DESCRIPTION: Subtract x-y, and return result.
 *#
 *#  DATE     NAME     CHANGES
 *#  ----     ----     -------
 *#  970128   ronny    Initial version
 *#
 *#***************************************************************************/

struct timeval
timeval_subtract(struct timeval *x, struct timeval *y)
{
  struct timeval diff;

  diff.tv_sec  = x->tv_sec  - y->tv_sec;
  diff.tv_usec = x->tv_usec - y->tv_usec;

  if (diff.tv_usec < 0) {
    diff.tv_sec--;
    diff.tv_usec = 1000000 + diff.tv_usec;
  }

  return diff;
}

/****************** END OF FILE e100boot.c **********************************/