serval-dna/serval_packetvisualise.c

/* 
Serval Mesh
Copyright (C) 2010-2012 Paul Gardner-Stephen
Copyright (C) 2010-2012 Serval Project Pty Limited
Copyright (C) 2012 Serval Project Inc.

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

/*
  Portions Copyright (C) 2013 Petter Reinholdtsen
  Some rights reserved

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions are met:

  1. Redistributions of source code must retain the above copyright
     notice, this list of conditions and the following disclaimer.

  2. Redistributions in binary form must reproduce the above copyright
     notice, this list of conditions and the following disclaimer in
     the documentation and/or other materials provided with the
     distribution.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  POSSIBILITY OF SUCH DAMAGE.
*/

#include <stdio.h>
#include <stdint.h>
#include "xprintf.h"

#define MAX_SPACES 120
const char *spaces="          ""          ""          ""          "
            "          ""          ""          ""          "
  "          ""          ""          ""          ";
const char *indent(int n)
{
  return &spaces[MAX_SPACES-n];
}

int senderSet=0;
unsigned char senderAddress[32];
static void _dump(XPRINTF xpf, const unsigned char *data, size_t len, size_t ofs, const char *prefix);

#ifdef STANDALONE

int main(int argc,char **argv)
{
  int i,n;
  int len;
  unsigned char buff[8192];

  for(n=0;n<1024;n++)
    {
      int i;
      len=random()%8192;
      for(i=0;i<len;i++) buff[i]=random()&0xff;
      serval_packetvisualise_xpf(XPRINTF_STDIO(stdout), "Fuzz Test", buff, len);
    }
  return 0;
}

#endif

static int serval_packetvisualise_renderaddress(XPRINTF xpf, const unsigned char *packet, size_t *ofs)
{
  unsigned int len = packet[(*ofs)++];
  xprintf(xpf,"(0x%02x) ",len);
  switch (len){
    case 0xFF:
      xprintf(xpf,"<same as sender's address>"); 
      return 0;
    case 0xFE:
      xprintf(xpf,"<same as previous address>");
      return 0;
    case 0xFD:
      xprintf(xpf,"<you>");
      return 0;
    case 0xFC:
      xprintf(xpf,"<me>");
      return 0;
    default:
      if (len>32){
	xprintf(xpf,"<illegal address token 0x%02x>",len);
	return -1;
      }
      unsigned i;
      for (i=0;i<len;i++)
	xprintf(xpf,"%02X",packet[(*ofs)++]);
      if (len<32) xprintf(xpf,"*");
      return 0;
  }
}

static unsigned int get_packed_uint32(const unsigned char *packet, size_t *ofs){
  unsigned int ret=0;
  int shift=0;
  int byte;
  do{
    byte = packet[(*ofs)++];
    ret |= (byte&0x7f)<<shift;
    shift+=7;
  }while(byte & 0x80);
  return ret;
}


static const char * overlay_type(unsigned int type){
  switch(type){
    case 0x20:
      return "SELF_ANNOUNCE_ACK";
    case 0x50:
      return "RHIZOME_ADVERT";
    case 0x60:
      return "PLEASE_EXPLAIN";
    default:
      return "UNKNOWN";
  }
}

static const char * port_name(uint32_t port){
  switch(port){
    case 1:
      return "KEY_MAP_REQUEST";
    case 2:
      return "LINK_STATE";
    case 4:
      return "STUN_REQUEST";
    case 5:
      return "STUN";
    case 6:
      return "PROBE";
    case 7:
      return "ECHO";
    case 8:
      return "TRACE";
    case 10:
      return "DNA_LOOKUP";
    case 12:
      return "VOMP";
    case 13:
      return "RHIZOME_REQUEST";
    case 14:
      return "RHIZOME_RESPONSE";
    case 15:
      return "DIRECTORY";
    case 16:
      return "RHIZOME_MANIFEST_REQUEST";
    case 17:
      return "RHIZOME_SYNC";
    case 0x3f:
      return "NO_REPLY";
    default:
      return "UNKNOWN";
  }
}
int isOverlayPacket(XPRINTF xpf, const unsigned char *packet, size_t *ofs, size_t len)
{
  unsigned int version = packet[(*ofs)++];
  if (version > 1)
    return 0;
  xprintf(xpf,"%sPacket version %d (0x%02x)\n",
	  indent(4),version,version);
  
  unsigned int encapsulation = packet[(*ofs)++];
    xprintf(xpf,"%sEncapsulation (0x%02x);",
	    indent(4),encapsulation);
  switch (encapsulation){
    case 1:
      xprintf(xpf, " OVERLAY\n");
      break;
    case 2:
      xprintf(xpf, " SINGLE\n");
      break;
    default:
      xprintf(xpf, " UNKNOWN\n");
      // TODO dump remainder?
      return -1;
  }

  xprintf(xpf, "%sSender; ", indent(4));
  int ret=serval_packetvisualise_renderaddress(xpf,packet,ofs);
  xprintf(xpf, "\n");
  if (ret)
    return ret;
  
  unsigned int packet_flags = packet[(*ofs)++];
  xprintf(xpf, "%sFlags (0x%02x);", indent(4), packet_flags);
  if (packet_flags & 1)
    xprintf(xpf, " UNICAST");
  if (packet_flags & 2)
    xprintf(xpf, " HAS_INTERFACE");
  if (packet_flags & 4)
    xprintf(xpf, " HAS_SEQUENCE");
  xprintf(xpf, "\n");
  
  if (packet_flags & 2)
    xprintf(xpf, "%sSender Interface; 0x%02x\n", indent(4), packet[(*ofs)++]);
  if (packet_flags & 4)
    xprintf(xpf, "%sSequence; 0x%02x\n", indent(4), packet[(*ofs)++]);
  
  while((*ofs)<len){
    unsigned int payload_flags = packet[(*ofs)++];
    xprintf(xpf, "%sFlags (0x%02x);", indent(6), payload_flags);
    if (payload_flags & 1)
      xprintf(xpf, " SENDER_SAME");
    if (payload_flags & 2)
      xprintf(xpf, " TO_BROADCAST");
    if (payload_flags & 4)
      xprintf(xpf, " ONE_HOP");
    if (payload_flags & 16)
      xprintf(xpf, " ENCRYPTED");
    if (payload_flags & 32)
      xprintf(xpf, " SIGNED");
    if (payload_flags & 64)
      xprintf(xpf, " ACK_SOON");
    if (payload_flags & 128)
      xprintf(xpf, " OVERLAY_TYPE");
    xprintf(xpf, "\n");
    
    if (!(payload_flags & 1)){
      xprintf(xpf, "%sSender; ", indent(6));
      int ret=serval_packetvisualise_renderaddress(xpf,packet,ofs);
      xprintf(xpf, "\n");
      if (ret)
	return ret;
    }
    
    if (payload_flags & 2){
      if (!(payload_flags & 4)){
	xprintf(xpf, "%sBroadcast ID; 0x", indent(6));
	int i;
	for (i=0;i<8;i++)
	  xprintf(xpf,"%02X",packet[(*ofs)++]);
	xprintf(xpf, "\n");
      }
    }else{
      xprintf(xpf, "%sDestination; ", indent(6));
      int ret=serval_packetvisualise_renderaddress(xpf,packet,ofs);
      xprintf(xpf, "\n");
      if (ret)
	return ret;
      if (!(payload_flags & 4)){
	xprintf(xpf, "%sNext Hop; ", indent(6));
	int ret=serval_packetvisualise_renderaddress(xpf,packet,ofs);
	xprintf(xpf, "\n");
	if (ret)
	  return ret;
      }
    }
    
    if (!(payload_flags & 4)){
      unsigned int ttl_qos = packet[(*ofs)++];
      xprintf(xpf, "%sTTL, QOS (0x%02x); %d, %d\n", indent(6), ttl_qos, ttl_qos & 0x1F, (ttl_qos >> 5) & 3);
    }
    
    int o_type=0;
    if (payload_flags & 128){
      o_type = packet[(*ofs)++];
      xprintf(xpf, "%sOverlay Type (0x%02x); %s\n", indent(6), o_type, overlay_type(o_type));
    }
    
    if (version >=1){
      xprintf(xpf, "%sMDP Sequence; 0x%02x\n", indent(6), packet[(*ofs)++]);
    }
    
    uint16_t payload_len = 0;
    if (encapsulation==1){
      payload_len=packet[(*ofs)++]<<8;
      payload_len|=packet[(*ofs)++];
      xprintf(xpf, "%sPayload length; 0x%04x\n", indent(6), payload_len);
      if (payload_len > len - *ofs)
	return -1;
    }else{
      payload_len = len - *ofs;
    }
    const unsigned char *payload_start = &packet[*ofs];
    (*ofs)+=payload_len;
    
    if (payload_flags & 16){
      xprintf(xpf, "%sPayload unreadable due to encryption\n", indent(8));
      continue;
    }
    
    if (payload_flags & 32){
      payload_len -= 64;
    }
    
    if (!(payload_flags & 128)){
      size_t payload_offset=0;
      uint32_t dest_port_raw = get_packed_uint32(payload_start, &payload_offset);
      
      int same = dest_port_raw&1;
      uint32_t dest_port = dest_port_raw >> 1;
      
      xprintf(xpf, "%sDestination Port (0x%04x); %d %s\n", indent(8), dest_port_raw, dest_port, port_name(dest_port));
      if (same){
	xprintf(xpf, "%sSource Port; SAME\n", indent(8));
      }else{
	uint32_t src_port = get_packed_uint32(payload_start, &payload_offset);
	xprintf(xpf, "%sSource Port; %d %s\n", indent(8), src_port, port_name(src_port));
      }
      payload_start += payload_offset;
      payload_len -= payload_offset;
    }
    
    xprintf(xpf, "%sPayload body;\n", indent(8));
    _dump(xpf, payload_start, payload_len, 0, indent(10));
    
    if (payload_flags & 32){
      xprintf(xpf, "%sSignature;\n", indent(8));
      _dump(xpf, payload_start+payload_len, 64, 0, indent(10));
    }
  }

  return 1;
}

int serval_packetvisualise_xpf(XPRINTF xpf, const char *message, const unsigned char *packet, size_t len)
{
  if (message)
    xprintf(xpf, "%s: ",message);
  xprintf(xpf,"Packet body of %d (0x%x) bytes:\n",(int)len,(int)len);
  _dump(xpf, packet, len, 0, "    ");
  size_t ofs=0;
  xprintf(xpf,"  Packet Structure:\n");
  if (isOverlayPacket(xpf,packet,&ofs,len)) { }
  if (ofs<len) {
    xprintf(xpf,"  WARNING: The last %d (0x%x) bytes of the packet were not parsed.\n",(int)(len-ofs),(int)(len-ofs));
  }
  return 0;
}

static void _dump(XPRINTF xpf, const unsigned char *data, size_t len, size_t ofs, const char *prefix)
{
  unsigned i;
  for (i = ofs & 0xFFFFFFF0; i < len; i += 16) {
    xprintf(xpf, "%s%04x:", prefix, i);
    unsigned j;
    for (j = 0; j < 16; ++j)
      if (i + j >= ofs && i + j < len)
	xprintf(xpf," %02x", data[i+j]);
      else
	xprintf(xpf, "   ");
    xprintf(xpf, "    ");
    for (j = 0; j < 16 && i + j < len; ++j)
      xputc(i + j < ofs ? ' ' : data[i+j] >= ' ' && data[i+j] < 0x7c ? data[i+j] : '.', xpf);
    xputc('\n', xpf);
  }
}

int serval_packetvisualise(const char *message, const unsigned char *packet, size_t len)
{
  return serval_packetvisualise_xpf(XPRINTF_STDIO(stdout),message, packet, len);
}