#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;}