Itp packets.c

From ITPWiki

Jump to: navigation, search

This not part of the ITP Plugfest, but may be incorporated in the future.

Version 0.2. Endian fixes. Still Experimental (Version 0.1 itp_packets.c.v0p1)

/*******************************************************
 *
 * File: itp_packets.c
 * Author: Greg Lee
 *
 * This file provides a mean to create and parse packets for the ITP
 * protocol.  It packs the binary data obeying guidelines set forward
 * in the RFC 4506, XDR (External Data Rpresentation) Standard.  It
 * can be found at:
 *
 *  http://tools.ietf.org/html/rfc4506
 *
 * Version 0.2
**********************************************************/ 

#include <string.h>
#include "itp_packets.h"


/******************************************************************
 * The pack() function packs data into a buffer that is a multiple of
 * four bytes as per RCF 4506.  Data packed with this function may be
 * unpacked with unpack().
 *
 * INPUT:
 *	packet		- This is a pointer to an allocate peice of
 *				memory that should be large enough to 
 *				accommodate the input data.
 *	data		- A pointer to the data that is to be packed 
 *				into packet.  It may be any type of 
 *				interger.
 *	bytes		- The number of bytes in the data type for data.
 *				It can be any number.
 * OUTPUT:
 *			- The output is the number of bytes the data now 
 *				consumes of the packet.
 *
*************************************************************************/

uint32_t pack(uint8_t *packet, uint8_t *data, uint8_t bytes)
{
  uint32_t	ind = 0;

  // Insert data.
  for(ind = 0; ind < bytes; ind++)
#ifdef LIT_END
  *(packet+ind) = *(data + bytes - ind - 1);
#elif defined BIG_END
  *(packet+ind) = *(data + ind);
#elif defined PDP_END
#endif


  // Pad the right side with 0s until the returned packet has a length where ind%4 == 0.
  for(; ((ind) % 4); ind++)
    *(packet+ind) = 0x00;

  return ind;
}

uint32_t pack_array(uint8_t *packet, uint8_t *data, uint8_t bytes, uint8_t num_mems)
{
  uint32_t	ind, ret = 0;

  for(ind=0; ind < num_mems; ind++)
    ret += pack((packet + ret), data + (ind * bytes), bytes);

  return ret;
}

/******************************************************************
 * The unpack() function unpacks data from a buffer that is a multiple
 * of four bytes as per RCF 4506.  Data unpacked with this function
 * must be packed with pack().
 *
 * INPUT:
 *	packet		- This is a pointer to an allocate peice of
 *				memory that should contain the information
 *	data		- A pointer to the data that is the information
 *				to be placed into packet.  It may be any type of 
 *				interger.
 *	bytes		- The number of bytes in the data type for data.
 *				It can be any number.
 * OUTPUT:
 *			- The output is the number of bytes data 
 *				unpacked from the packet.
 *
*************************************************************************/

uint32_t unpack(uint8_t *packet, uint8_t *data, uint8_t bytes)
{
  uint32_t	ind = 0, offset = bytes % 4;

  for(ind = 0; ind < bytes; ind++)
#if defined LIT_END
    *(data + ind) = *(packet + bytes - ind - 1);
#elif defined BIG_END
    *(data + ind) = *(packet + ind);
#elif defined PDP_END
#endif

  return ind;
}


uint32_t unpack_array(uint8_t *packet, uint8_t *data, uint8_t bytes, uint8_t num_mems)
{
  uint32_t	ind, ret = 0;

  for(ind=0; ind < num_mems; ind++)
      ret += unpack((packet + ret), data + (ind * bytes), bytes);

  return ret;
}


/******************************************************************
 * The pack_u_struct() function packs elements of the struct u_struct
 * structure.  The data may be unpacked from the packet using
 * unpack_u_struct().
 *
 * INPUT:
 *	packet		- A pointer to the packet data.
 *	pu		- A pointer to a struct u_struct structure
 *				containing the data for packing.
 * OUTPUT:
 *			- The number of bytes packed into packet.
 *
*******************************************************************/

int32_t pack_u_struct(void *packet, struct u_struct *pu)
{
  int32_t	total = 0;

  total += pack((uint8_t *)packet+total, (uint8_t *) &(pu->sequence), sizeof(pu->sequence));
  total += pack((uint8_t *)packet+total, (uint8_t *) &(pu->pactyp), sizeof(pu->pactyp));
  total += pack((uint8_t *)packet+total, (uint8_t *) &(pu->version), sizeof(pu->version));

  total += pack_array((uint8_t *)packet+total, (uint8_t *) &(pu->delx), sizeof(pu->delx[0]), sizeof(pu->delx) / sizeof(pu->delx[0]));
  total += pack_array((uint8_t *)packet+total, (uint8_t *) &(pu->dely), sizeof(pu->dely[0]), sizeof(pu->dely) / sizeof(pu->dely[0]));
  total += pack_array((uint8_t *)packet+total, (uint8_t *) &(pu->delz), sizeof(pu->delz[0]), sizeof(pu->delz) / sizeof(pu->delz[0]));

  total += pack_array((uint8_t *)packet+total, (uint8_t *) &(pu->delyaw), sizeof(pu->delyaw[0]), sizeof(pu->delyaw) / sizeof(pu->delyaw[0]));
  total += pack_array((uint8_t *)packet+total, (uint8_t *) &(pu->delpitch), sizeof(pu->delpitch[0]), sizeof(pu->delpitch) / sizeof(pu->delpitch[0]));
  total += pack_array((uint8_t *)packet+total, (uint8_t *) &(pu->delroll), sizeof(pu->delroll[0]), sizeof(pu->delroll) / sizeof(pu->delroll[0]));

  total += pack_array((uint8_t *)packet+total, (uint8_t *) &(pu->buttonstate), sizeof(pu->buttonstate[0]), sizeof(pu->buttonstate) / sizeof(pu->buttonstate[0]));
  total += pack_array((uint8_t *)packet+total, (uint8_t *) &(pu->grasp), sizeof(pu->grasp[0]), sizeof(pu->grasp) / sizeof(pu->grasp[0]));
  total += pack((uint8_t *)packet+total, (uint8_t *) &(pu->surgeon_mode), sizeof(pu->surgeon_mode));

#if defined ITP_VERSION && (ITP_VERSION > 44)
  pu->checksum = packet_checksum(packet, total);
#endif

  total += pack((uint8_t *)packet+total, (uint8_t *) &(pu->checksum), sizeof(pu->checksum));

  return total;
}



/******************************************************************
 * The unpack_u_struct() function unpacks elements to the struct
 * u_struct structure.  The data must be packed intothe packet using
 * pack_u_struct().
 *
 * INPUT:
 *	packet		- A pointer to the packet data buffer.
 *	pu		- A pointer to a struct u_struct structure
 *				containing the data for unpacking.
 * OUTPUT:
 *			- The number of bytes unpacked from packet.
 *
*******************************************************************/

int32_t unpack_u_struct(void *packet, struct u_struct *pu)
{
  int32_t	total = 0;
#if defined ITP_VERSION && (ITP_VERSION > 44)
  int32_t	test_checksum =0;
#endif

  total += unpack((uint8_t *)packet+total, (uint8_t *) &(pu->sequence), sizeof(pu->sequence));
  total += unpack((uint8_t *)packet+total, (uint8_t *) &(pu->pactyp), sizeof(pu->pactyp));
  total += unpack((uint8_t *)packet+total, (uint8_t *) &(pu->version), sizeof(pu->version));

  total += unpack_array((uint8_t *)packet+total, (uint8_t *) &(pu->delx), sizeof(pu->delx[0]), sizeof(pu->delx) / sizeof(pu->delx[0]));
  total += unpack_array((uint8_t *)packet+total, (uint8_t *) &(pu->dely), sizeof(pu->dely[0]), sizeof(pu->dely) / sizeof(pu->dely[0]));
  total += unpack_array((uint8_t *)packet+total, (uint8_t *) &(pu->delz), sizeof(pu->delz[0]), sizeof(pu->delz) / sizeof(pu->delz[0]));

  total += unpack_array((uint8_t *)packet+total, (uint8_t *) &(pu->delyaw), sizeof(pu->delyaw[0]), sizeof(pu->delyaw) / sizeof(pu->delyaw[0]));
  total += unpack_array((uint8_t *)packet+total, (uint8_t *) &(pu->delpitch), sizeof(pu->delpitch[0]), sizeof(pu->delpitch) / sizeof(pu->delpitch[0]));
  total += unpack_array((uint8_t *)packet+total, (uint8_t *) &(pu->delroll), sizeof(pu->delroll[0]), sizeof(pu->delroll) / sizeof(pu->delroll[0]));

  total += unpack_array((uint8_t *)packet+total, (uint8_t *) &(pu->buttonstate), sizeof(pu->buttonstate[0]), sizeof(pu->buttonstate) / sizeof(pu->buttonstate[0]));
  total += unpack_array((uint8_t *)packet+total, (uint8_t *) &(pu->grasp), sizeof(pu->grasp[0]), sizeof(pu->grasp) / sizeof(pu->grasp[0]));
  total += unpack((uint8_t *)packet+total, (uint8_t *) &(pu->surgeon_mode), sizeof(pu->surgeon_mode));

#if defined ITP_VERSION && (ITP_VERSION > 44)
  test_checksum = packet_checksum(packet, total);
#endif

  total += unpack((uint8_t *)packet+total, (uint8_t *) &(pu->checksum), sizeof(pu->checksum));

#if defined ITP_VERSION && (ITP_VERSION > 44)
  if (pu->checksum != test_checksum)
    return ITP_BAD_CHKSUM;
#endif

  return total;
}


/******************************************************************
 * The pack_v_struct() function packs elements of the struct
 * v_struct structure.  The data may be unpacked from the packet using
 * unpack_v_struct().
 *
 * INPUT:
 *	packet		- A pointer to the packet data buffer.
 *	pv		- A pointer to a struct v_struct structure
 *				containing the data for packing.
 *				the data for packing.
 * OUTPUT:
 *			- The number of bytes packed into packet.
 *
*******************************************************************/

int32_t pack_v_struct(void *packet, struct v_struct *pv)
{
  int32_t	total = 0;

  total += pack((uint8_t *)packet+total, (uint8_t *) &(pv->sequence), sizeof(pv->sequence));
  total += pack((uint8_t *)packet+total, (uint8_t *) &(pv->pactyp), sizeof(pv->pactyp));
  total += pack((uint8_t *)packet+total, (uint8_t *) &(pv->version), sizeof(pv->version));

  total += pack((uint8_t *)packet+total, (uint8_t *) &(pv->fx), sizeof(pv->fx));
  total += pack((uint8_t *)packet+total, (uint8_t *) &(pv->fy), sizeof(pv->fy));
  total += pack((uint8_t *)packet+total, (uint8_t *) &(pv->fz), sizeof(pv->fz));

  total += pack((uint8_t *)packet+total, (uint8_t *) &(pv->runlevel), sizeof(pv->runlevel));
  total += pack((uint8_t *)packet+total, (uint8_t *) &(pv->jointflags), sizeof(pv->jointflags));
 
#if defined ITP_VERSION && (ITP_VERSION > 44)
  pv->checksum = packet_checksum(packet, total);
#endif

  total += pack((uint8_t *)packet+total, (uint8_t *) &(pv->checksum), sizeof(pv->checksum));

  return total;
}


/******************************************************************
 * The unpack_v_struct() function packs elements of the struct
 * v_struct structure.  The data may be unpacked from the packet using
 * pack_v_struct().
 *
 * INPUT:
 *	packet		- A pointer to the packet data buffer.
 *	pv		- A pointer to a struct v_struct structure
 *				containing the data for packing.
 *				the data for packing.
 * OUTPUT:
 *			- The number of bytes un packed from the packet.
 *
*******************************************************************/

int32_t unpack_v_struct(void *packet, struct v_struct *pv)
{

  int32_t	total = 0;
#if defined ITP_VERSION && (ITP_VERSION > 44)
  int32_t	test_checksum =0;
#endif

  total += unpack((uint8_t *)packet+total, (uint8_t *) &(pv->sequence), sizeof(pv->sequence));
  total += unpack((uint8_t *)packet+total, (uint8_t *) &(pv->pactyp), sizeof(pv->pactyp));
  total += unpack((uint8_t *)packet+total, (uint8_t *) &(pv->version), sizeof(pv->version));

  total += unpack((uint8_t *)packet+total, (uint8_t *) &(pv->fx), sizeof(pv->fx));
  total += unpack((uint8_t *)packet+total, (uint8_t *) &(pv->fy), sizeof(pv->fy));
  total += unpack((uint8_t *)packet+total, (uint8_t *) &(pv->fz), sizeof(pv->fz));

  total += unpack((uint8_t *)packet+total, (uint8_t *) &(pv->runlevel), sizeof(pv->runlevel));
  total += unpack((uint8_t *)packet+total, (uint8_t *) &(pv->jointflags), sizeof(pv->jointflags));

#if defined ITP_VERSION && (ITP_VERSION > 44)
  test_checksum = packet_checksum(packet, total);
#endif
 
  total += unpack((uint8_t *)packet+total, (uint8_t *) &(pv->checksum), sizeof(pv->checksum));

#if defined ITP_VERSION && (ITP_VERSION > 44)
  if (pu->checksum != test_checksum)
    return ITP_BAD_CHKSUM;
#endif

  return total;
}


Personal tools