packet.c

主要用于无线传感网络的编写的书籍.对于初学者有着很大的用处

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