frame.c

来自「主要用于无线传感网络的编写的书籍.对于初学者有着很大的用处」· C语言 代码 · 共 249 行

#include <stdio.h>
#include "frame.h"
#include "../physical/serial_connect.h"
#include "../../debug/debug.h"
#include <stdio.h>   /* Standard input/output definitions */
#include <string.h>  /* String function definitions */
#include <unistd.h>  /* UNIX standard function definitions */
#include <fcntl.h>   /* File control definitions */
#include <errno.h>   /* Error number definitions */
#include <termios.h> /* POSIX terminal control definitions */


int computeCRC(uint8_t packet_type, uint8_t *bytes, int numbytes) {
  short crc = 0;
  int i = 0;
  int j = 0;
  unsigned char cur_byte = 0;

  for (i=-1; i<numbytes; i++) {
    if (i == -1)
      cur_byte = packet_type;
    else
      cur_byte = bytes[i];
      
    crc = crc ^ cur_byte << 8;
    for (j=8; j>0; j--) {
      if ((crc & 0x8000) == 0x8000)
        crc = crc << 1 ^ 0x1021;
      else
        crc = crc << 1;
    }
  }
  return crc & 0xFFFF;
}

void printFrame(int dbg_type, int dbg_level, frame *f) {
  int i=0;
  dbg(dbg_type, dbg_level, "Frame: type = %02x\n", f->type);
  dbg(dbg_type, dbg_level, "       data = ");
  for (i=0; i<f->data_size; i++) {
    dbg(dbg_type, dbg_level, "%02x ", f->data[i]);
  }
  dbg(dbg_type, dbg_level, "\n");
  dbg(dbg_type, dbg_level, "        crc = %04x\n", f->crc);
}

int readFrame(int serial_port_fd, frame *f) {
  int insync = 0;
  int numbytes = 0;
  int numrealbytes = 0;
  char temp = '\0';
  int escaped = 0;

  if (serial_port_fd == -1) {
    dbg(DBG_LINK, CRITICAL, "ERROR: the serial port passed to readFrame was invalid!\n");
    return FAILURE;
  }

  dbg(DBG_LINK, FRIVOLOUS, "into readFrame\n");

  while (1) {
    flushInputPort(serial_port_fd);
    while (1) {
      if (insync == FALSE) {
        dbg(DBG_LINK, AVERAGE, "not insync, resyncronizing\n");
        if (read(serial_port_fd, &temp, 1) != 1) {
          continue;
        }
        temp &= 0xFF;
        if (temp == SYNC_BYTE) { 
          dbg(DBG_LINK, FRIVOLOUS, "now we're in sync, on with the show\n");
          insync = TRUE;
          numbytes = 0;
          escaped = FALSE;
        }
      }
      // now we should be insync, unless we actually read the trailing sync byte of a packet first
      // in which case the too-short-packet check will suffice as a check
      if (numrealbytes >= MTU) { // packet too long, drop and resyncronize
        dbg(DBG_LINK, AVERAGE, "numbytes >= MTU, readFrame resyncronizes\n");
        insync = FALSE;
        continue;
      }
      if (read(serial_port_fd, &temp, 1) == 1) {
        temp &= 0xFF;
        numbytes++;
          
        if (escaped) {
          if (temp == SYNC_BYTE) { // can't have SYNC_BYTE after escape character
            dbg(DBG_LINK, AVERAGE, "SYNC_BYTE after ESCAPE_BYTE: readFrame resyncronizes\n");
            insync = FALSE;
            continue;
          }
          dbg(DBG_LINK, FRIVOLOUS, "we are escaped, escaping (%02x) into (%02x)\n", temp, temp ^ 0x20);
          temp ^= 0x20;
          escaped = FALSE;
        }
        else if (temp == ESCAPE_BYTE) {
          dbg(DBG_LINK, FRIVOLOUS, "found escape byte, escaping next byte...\n");
          escaped = TRUE;
          continue;
        }
        else if (temp == SYNC_BYTE) {
          if (numbytes < MINIMUM_FRAME_SIZE) { // sync byte that makes frame too small (i.e. if we caught
            insync = FALSE;                    // the trailing sync byte on resyncronization
            break;                          
          }
          dbg(DBG_LINK, FRIVOLOUS, "found end sync byte, done with packet\n");
          break; // reached the end of this packet
        }
        else if (numbytes == 1) { // first byte is packet type
          dbg(DBG_LINK, FRIVOLOUS, "setting frame type to %02x\n", temp);
          // DO NOT UPDATE numrealbytes HERE SINCE MTU DOES NOT TAKE IT INTO ACCOUNT
          f->type = temp;
          continue;
        }
        f->data[numrealbytes] = temp;
        numrealbytes++;
      }
      else { // error reading byte from serial port, resyncronize
        dbg(DBG_LINK, AVERAGE, "error reading byte: readFrame resyncronizes\n");
        insync = FALSE;
      }
    }
    // if we got here, we read an entire frame
    f->data_size = numrealbytes - 2; // the "- 2" is to make up for the CRC being copied into the 
                                     // data array, when the next level up will see it in f->crc
    f->crc = 0;
    f->crc =  (f->data[numrealbytes-1] & 0xFF) << 8;
    f->crc |=  f->data[numrealbytes-2] & 0xFF;
    f->crc &= 0xFFFF;
    dbg(DBG_LINK, AVERAGE, "setting f->crc....%04x\n", f->crc);
    return SUCCESS;
  }
  return FAILURE;
}

int handleFrame(frame *f) {
  if (!f) return FAILURE;

/*  if (f->crc != computeCRC(f->type, f->data, f->data_size)) {
    dbg(DBG_LINK, CRITICAL, "computed CRC (%0x) does not match f->crc (%0x) in handleFrame \n", computeCRC(f->type, f->data, f->data_size), f->crc);
    return DROPPACKET;
  } */
  if (f->type == P_UNKNOWN) {
    dbg(DBG_LINK, AVERAGE, "f->type == P_UNKNOWN, returning DROPPACKET from handleFrame\n");
    return DROPPACKET;
  }
  if (f->type == P_PACKET_NO_ACK) {
    dbg(DBG_LINK, AVERAGE, "f->type == P_PACKET_NO_ACK, returning GOODPACKET from handleFrame\n");
    return GOODPACKET;
  }
  if (f->type == P_ACK) {
    dbg(DBG_LINK, AVERAGE, "f->type == P_ACK, returning DROPPACKET from handleFrame\n");
    return DROPPACKET;
  }
  if (f->type == P_PACKET_ACK) {
    dbg(DBG_LINK, AVERAGE, "f->type == P_PACKET_ACK, returning GOODPACKET from handleFrame\n");
    return GOODPACKET; // note: when I move to threads, we'll need to handle ACK packets here
  }
  dbg(DBG_LINK, AVERAGE, "unknown packet type (%d): returning DROPPACKET from handleFrame\n", f->type);
  return DROPPACKET;
}

int readGoodFrame(int serial_port_fd, frame *f) {
  int cur_try = 0;
  if (!f) return FAILURE;
  if (serial_port_fd == -1) return FAILURE;
  while (cur_try++ < GOODFRAME_MAXTRIES) {
    if (readFrame(serial_port_fd, f) == FAILURE) {
      return FAILURE;
    }
    if (handleFrame(f) != GOODPACKET) {
      dbg(DBG_LINK, AVERAGE, "WARNING: dropping packet in readGoodFrame...\n");
      printFrame(DBG_LINK, AVERAGE, f);
      continue;
    }
    else {
      return SUCCESS;
    }
  }
  return FAILURE;
}

int writeFrame(int sfd, uint8_t *data, int data_size) {
  int i;
  uint8_t d;
  int crc;

  if (!data) {
    dbg(DBG_LINK, CRITICAL, "ERROR: data is null in writeFrame!\n");
    return FAILURE;
  }
  if (data_size > MTU) {
    dbg(DBG_LINK, CRITICAL, "ERROR: data size too big (> %d) in writeFrame!\n", MTU);
    return FAILURE;
  }
  flushOutputPort(sfd);
  // first, write sync byte
  d = SYNC_BYTE;
  if (write(sfd, &d, 1) < 0) {
    dbg(DBG_LINK, AVERAGE, "ERROR: could not write SYNC_BYTE\n");
    return FAILURE;
  }
  // next, write type byte
  d = P_PACKET_NO_ACK;
  if (write(sfd, &d, 1) < 0) {
    dbg(DBG_LINK, AVERAGE, "ERROR: could not write type byte\n");
    return FAILURE;
  }
  // now, write the data, remembering to escape SYNC_BYTE and ESCAPE_BYTE
  for (i=0; i<data_size; i++) {
    if (data[i] == SYNC_BYTE || data[i] == ESCAPE_BYTE) {
      d = ESCAPE_BYTE;
      if (write(sfd, &d, 1) < 0) {
        dbg(DBG_LINK, AVERAGE, "ERROR: could not write ESCAPE_BYTE before SYNC_BYTE in data in writeFrame\n");
        return FAILURE;
      }
      d = data[i] ^ 0x20;
    }
    else {
      d = data[i];
    }
    if (write(sfd, &d, 1) < 0) {
      dbg(DBG_LINK, AVERAGE, "ERROR: could not write data[%d] (0x%02x) in writeFrame\n", i, data[i]);
      return FAILURE;
    }
  }
  // now send CRC (remember, little endian format)
  crc = computeCRC(P_PACKET_NO_ACK, data, data_size);
  d = crc & 0xFF;
  if (write(sfd, &d, 1) < 0) {
    dbg(DBG_LINK, AVERAGE, "ERROR: could not write least significant byte of crc in writeFrame\n");
    return FAILURE;
  }
  d = ((crc & 0xFF00) >> 8) & 0xFF;
  if (write(sfd, &d, 1) < 0) {
    dbg(DBG_LINK, AVERAGE, "ERROR: could not write most significant byte of crc in writeFrame\n");
    return FAILURE;
  }
  // finish up with a final SYNC_BYTE
  d = SYNC_BYTE;
  if (write(sfd, &d, 1) < 0) {
    dbg(DBG_LINK, AVERAGE, "ERROR: could not write final SYNC_BYTE in writeFrame\n");
    return FAILURE;
  }
  return SUCCESS;
}