#include "../../debug/debug.h"#include "../link/frame.h"#include "packet.h"int readNetworkPkt(int serial_port_fd, networkPkt *p) {  frame f;  int i;  dbg(DBG_NETWORK, FRIVOLOUS, "calling readFrame in readPacket\n");  if (readGoodFrame(serial_port_fd, &f) != SUCCESS) {    dbg(DBG_NETWORK, CRITICAL, "ERROR: could not read frame in readPacket!\n");    return FAILURE;  }  dbg(DBG_NETWORK, FRIVOLOUS, "readFrame returned SUCCESS\n");  if (f.data_size < sizeof(TOS_Hdr)) {    dbg(DBG_NETWORK, AVERAGE, "f.data_size (%d) too small!\n", f.data_size);    return FAILURE;  }  p->hdr.address = (((uint16_t)(f.data[1]) & 0xFF) << 8);  p->hdr.address |= ((uint16_t)(f.data[0])) & 0xFF;  p->hdr.port = f.data[2] & 0xFF;  p->hdr.group = f.data[3] & 0xFF;  p->hdr.length = f.data[4] & 0xFF;  if (p->hdr.length != f.data_size - sizeof(TOS_Hdr)) {    dbg(DBG_NETWORK, AVERAGE, "packet length (%d) not correct vs. frame.data_size-sizeof(TOS_Hdr) (%d)-(%d)\n", p->hdr.length, f.data_size, sizeof(TOS_Hdr));    return FAILURE;  }  for (i=0; i<p->hdr.length; i++) {    p->data[i] = f.data[i+sizeof(TOS_Hdr)];  }  return SUCCESS;}void printPacket(int debug_type, int debug_level, FILE *fd, networkPkt *p) {  int i = 0;  fdbg(debug_type, debug_level, fd, "address: %04x\n", p->hdr.address);  fdbg(debug_type, debug_level, fd, "   port:   %02x\n", p->hdr.port);  fdbg(debug_type, debug_level, fd, "  group:   %02x\n", p->hdr.group);  fdbg(debug_type, debug_level, fd, " length:   %02x\n", p->hdr.length);  fdbg(debug_type, debug_level, fd, "   data: ");  for (i=0; i<p->hdr.length; i++) {    fdbg(debug_type, debug_level, fd, "%02x ", p->data[i]);  }  fdbg(debug_type, debug_level, fd, "\n");}int writeNetworkPkt(int serial_port_fd, networkPkt *p) {  int i;  int j;  uint8_t data[MTU];  if (!p) {    dbg(DBG_LINK, CRITICAL, "ERROR: packet is null in writeNetworkPkt\n");    return FAILURE;  }  if (p->hdr.length > PKT_MAX_DATA_SIZE) {    dbg(DBG_LINK, CRITICAL, "ERROR: data size is too big in writeNetworkPkt\n");    return FAILURE;  }  i = 0;  data[i++] = p->hdr.address & 0xFF;  data[i++] = ((p->hdr.address & 0xFF00) >> 8) & 0xFF;  data[i++] = p->hdr.port;  data[i++] = p->hdr.group;  data[i++] = p->hdr.length;  for (j=0; j<p->hdr.length; j++) {    data[i++] = p->data[j];  }  return writeFrame(serial_port_fd, data, i);}