Testing the integrity of stun message. See http://tools.ietf.org/html/rfc5389#section-15.4 and http://tools.ietf.org/html/rfc5769#section-2.2 (validates correctly, code is still work in progress).

Reader.cpp

#include <stun/Reader.h>
#include <openssl/engine.h>
#include <openssl/hmac.h>
#include <openssl/evp.h>


namespace stun {

  /* --------------------------------------------------------------------- */

  static bool stun_validate_cookie(uint32_t cookie);

  /* --------------------------------------------------------------------- */

  Reader::Reader() 
    :dx(0)
    ,on_message(NULL)
    ,user(NULL)
  {
  }

  /* @todo - we can optimize this part by not copying but setting pointers to the 
             members of the Message. */
  void Reader::process(uint8_t* data, uint32_t nbytes) {

    if (!data) {
      printf("Error: received invalid data in Reader::process().\n");
      return;
    }

    if (nbytes < 2) {
      return;
    }

    /* @todo check first 2 bits */
    //    if (buffer[0] != 0x00 || buffer[1] != 0x00) {
    //      printf("Warning: no STUN message.\n");
    //      return;
    //    }

    std::copy(data, data + nbytes, std::back_inserter(buffer));
    
    /* A stun message must at least contain 20 bytes. */
    if (buffer.size() < 20) {
      return;
    }
    printf("Data to process: %u bytes, %lu.\n", nbytes, buffer.size());

    
    /* create the base message */
    Message msg;
    msg.type = readU16();
    msg.length = readU16();
    msg.cookie = readU32();
    msg.transaction[0] = readU32();
    msg.transaction[1] = readU32();
    msg.transaction[2] = readU32();
    std::copy(data, data + nbytes, std::back_inserter(msg.buffer));

    /* validate */
    if (!stun_validate_cookie(msg.cookie)) {
      printf("Error: invalid STUN cookie.\n");
      return;
    }

    /* parse the rest of the message */
    int c = 0;
    uint16_t attr_type;
    uint16_t attr_length;
    uint32_t prev_dx;
    
    while (bytesLeft() >= 4) {
      
      Attribute* attr = NULL;
      prev_dx = dx;
      attr_type = readU16();
      attr_length = readU16();

      printf("Msg: %s, Type: %s, Length: %d, bytes left: %u, current index: %ld\n", 
             message_type_to_string(msg.type).c_str(),
             attribute_type_to_string(attr_type).c_str(), 
             attr_length, 
             bytesLeft(), 
             dx);

      switch (attr_type) {

        /* no parsing needed for these */
        case STUN_ATTR_USE_CANDIDATE: {
          break;
        }

        case STUN_ATTR_USERNAME: { 
          Username* username = new Username();
          username->value = readString(attr_length);
          attr = (Attribute*) username;
          break;
        }

        case STUN_ATTR_SOFTWARE: { 
          Software* software = new Software();
          software->value = readString(attr_length);
          attr = (Attribute*) software;
          break;
        }

        case STUN_ATTR_XOR_MAPPED_ADDRESS: {
          XorMappedAddress* address = readXorMappedAddress();
          if (address) {
            attr = (Attribute*) address;
          }
          break;
        }

        case STUN_ATTR_PRIORITY: {
          skip(4); /* skip priority for now: http://tools.ietf.org/html/rfc5245#section-4.1.2.1 */
          break;
        } 

        case STUN_ATTR_MESSAGE_INTEGRITY: {     
          MessageIntegrity* integ = new MessageIntegrity();
          memcpy(integ->hash, &buffer[dx], 20);
          integ->offset = dx - 4;
          attr = (Attribute*) integ;
          skip(20);
          break;
        }

        case STUN_ATTR_FINGERPRINT: {
          skip(4); /* CRC32-bit, see http://tools.ietf.org/html/rfc5389#section-15.5 */
          break;
        }

        case STUN_ATTR_ICE_CONTROLLING:
        case STUN_ATTR_ICE_CONTROLLED: {
          skip(8);
          break;
        }

        default: {
          printf("Warning: unhandled STUN attribute %s of length: %u\n", attribute_type_to_string(attr_type).c_str(), attr_length);
          break;
        }
      }

      /* Padding: http://tools.ietf.org/html/rfc5389#section-15, must be 32bit aligned */
      while ( (dx & 0x03) != 0 && bytesLeft() > 0) {
        dx++;
      }

      /* when we parsed an attribute, we set the members and append it to the message */
      if (attr) {
        attr->length = attr_length;
        attr->type = attr_type;
        attr->nbytes = dx - prev_dx;
        msg.addAttribute(attr);
      }

      /* reset vars used while parsing */
      attr = NULL;
      attr_length = 0;
      prev_dx = dx;
    }

    /* and erase any read data. */
    buffer.erase(buffer.begin(), buffer.begin() + dx);
    dx = 0;

    if (on_message) {
      on_message(&msg, user);
    }
  }

  uint32_t Reader::bytesLeft() {
    if (buffer.size() == 0) { 
      return 0;
    }

    return buffer.size() - dx;
  }

  uint8_t Reader::readU8() {
    dx++;
    return buffer[dx - 1];
  }

  uint16_t Reader::readU16() {

    if (bytesLeft() < 2) { 
      printf("Error: trying to readU16() in STUN, but the buffer is not big enough.\n");
      return 0;
    }

    uint16_t result;
    uint8_t* dst = (uint8_t*)&result;

    dst[0] = buffer[dx + 1];
    dst[1] = buffer[dx + 0];

    dx += 2;

    return result;
  }

  uint32_t Reader::readU32() {

    if (bytesLeft() < 4) {
      printf("Error: trying to readU32() in STUN, but the buffer is not big enough.\n");
      return 0;
    }

    uint32_t result;
    uint8_t* dst = (uint8_t*)&result;

    dst[0] = buffer[dx + 3];
    dst[1] = buffer[dx + 2];
    dst[2] = buffer[dx + 1];
    dst[3] = buffer[dx + 0];

    dx += 4;

    return result;
  }

  void Reader::skip(uint32_t nbytes) {
    if (dx + nbytes > buffer.size()) {
      printf("Error: trying to skip %u bytes, but we only have %u left in STUN.\n", nbytes, bytesLeft());
      return;
    }
    dx += nbytes;
  }


  StringValue Reader::readString(uint16_t len) {
    StringValue v;

    if (bytesLeft() < len) {
      printf("Error: trying to read a StringValue from the buffer, but the buffer is not big enough.\n");
      return v;
    }

    std::copy(ptr(), ptr() + len, std::back_inserter(v.buffer));
    dx += len;
    return v;
  }

  /* See http://tools.ietf.org/html/rfc5389#section-15.2 */
  XorMappedAddress* Reader::readXorMappedAddress() {

    if (bytesLeft() < 8) {
      printf("Error: cannot read a XorMappedAddress as the buffer is too small in stun::Reader.\n");
      return NULL;
    }
    
    XorMappedAddress* addr = new XorMappedAddress();
    uint32_t ip = 0;
    uint8_t cookie[] = { 0x42, 0xA4, 0x12, 0x21 }; 
    uint8_t* port_ptr = (uint8_t*) &addr->port;
    uint8_t* ip_ptr = (uint8_t*) &ip;
    unsigned char ip_addr[16];

    /* skip the first byte */
    skip(1); 

    /* read family: 0x01 = IP4, 0x02 = IP6 */
    addr->family = readU8();
    if (addr->family != 0x01 && addr->family != 0x02) {
      printf("Error: invalid family for the xor mapped address in stun::Reader.\n");
      delete addr;
      return NULL;
    }

    /* read the port. */
    addr->port = readU16();
    port_ptr[0] = port_ptr[0] ^ cookie[2];
    port_ptr[1] = port_ptr[1] ^ cookie[3];

    /* read the address part. */
    if (addr->family == 0x01) {

      ip = readU32();

      ip_ptr[0] = ip_ptr[0] ^ cookie[0];
      ip_ptr[1] = ip_ptr[1] ^ cookie[1];
      ip_ptr[2] = ip_ptr[2] ^ cookie[2];
      ip_ptr[3] = ip_ptr[3] ^ cookie[3];

      sprintf((char*)ip_addr, "%u.%u.%u.%u", ip_ptr[3], ip_ptr[2], ip_ptr[1], ip_ptr[0]);

      std::copy(ip_addr, ip_addr + 16, std::back_inserter(addr->address));
    }
    else if (addr->family == 0x02) {
      /* @todo read the address for ipv6 in stun::Reader::readXorMappedAddress(). */
      printf("Warning: we have to implement the IPv6 address in stun::Reader.\n");
      delete addr;
      return NULL;
    }
    else {
      printf("Warning: we shouldn't arrive here in stun::Reader.\n");
      delete addr;
      return NULL;
    }

    return addr;
  }

  uint8_t* Reader::ptr() {
    return &buffer[dx];
  }

  bool Reader::calculateIntegrity(uint8_t* input, uint32_t nbytes, uint8_t* output, std::string key) {

#if 1
    int nl = 1;
    for(uint32_t i = 0; i < nbytes; ++i, ++nl) {
      printf("%02x ", input[i]);
      if (nl == 4) {
        printf("\n");
        nl = 0;
      }
    }
#endif

    unsigned int len;
    HMAC_CTX ctx;
    HMAC_CTX_init(&ctx);
  
    if (!HMAC_Init_ex(&ctx, (const unsigned char*)key.c_str(), key.size(), EVP_sha1(), NULL)) {
    //if (!HMAC_Init(&ctx, (const unsigned char*)key.c_str(), key.size(), EVP_sha1())) {
      printf("Error: cannot init the HMAC.\n");
    }

    HMAC_Update(&ctx, (const unsigned char*)input, nbytes);
    HMAC_Final(&ctx, output, &len);

    printf("Computed Hash: ");
    for(unsigned int i = 0; i < len; ++i) {
      printf("%02X ", output[i]);
    }
    printf("\n");

    return false;
  }

  /* --------------------------------------------------------------------- */

  static bool stun_validate_cookie(uint32_t cookie) {
    //printf("%02X:%02X:%02X:%02X\n", ptr[3], ptr[2], ptr[1], ptr[0]);
    uint8_t* ptr = (uint8_t*) &cookie;
    return (ptr[3] == 0x21 
            && ptr[2] == 0x12 
            && ptr[1] == 0xA4 
            && ptr[0] == 0x42);

    return true;
  }

} /* namespace stun */

test_stun_message_integrity.cpp

/*
  
  Calculating the HMAC-SHA1 message integrity attribute:
  ------------------------------------------------------

  1. Loop over all attributes and keep track of how many bytes
     you've read from the buffer. When you arrive at the MESSAGE-INTEGRITY
     attribute you need to remember how many bytes you've read so far.
     
  2. Next you need to rewrite the Message-Length field of the
     stun buffer because the HMAC-SHA1 needs to be computed with a 
     STUN buffer that contains the stun header with as message length
     that is computed up to, and including the size of the MESSAGE-INTEGRITY
     attribute. Sometimes a stun message can contain a FINGERPRINT attribute
     after the MESSAGE-INTEGRITY attribute and the Message-Length field 
     will also include the size of this attribute (in practice there 
     may be even more other attributes).

    e.g.

    [ STUN HEADER ]                     - 20 bytes
    [ ATTRIBUTE 1 ]                     - 8 bytes
    [ ATTRIBUTE 2 ]                     - 8 bytes 
    [ MESSAGE-INTEGRITY-ATTRIBUTE ]     - 24 bytes (2 bytes type, 2 bytes size, 20 bytes sha1)
    [ FINGERPRINT ]                     - 8 bytes

    In the message above, the [ STUN HEADER ] will contain a size value of:
    8 + 8 + 24 + 8, though to compute the SHA1, this should be 8 + 8 + 24 (not
    the FINGERPRINT value. 

  3. After rewriting the size, you use all bytes up to, BUT NOT including
     the MESSAGE-INTEGRITY attribute as data to compute the SHA1 


   N.B. be aware of the 32-bit memory alignment

 */
#include <stdio.h>
#include <vector>
#include <stun/Reader.h>
#include <stun/Writer.h>

static void on_stun_message(stun::Message* msg, void* user);

int main() {

  printf("\n\ntest_stun_message_integrity\n\n");

  /* from: http://tools.ietf.org/html/rfc5769#section-2.2 */
  const unsigned char respv4[] =
    "\x01\x01\x00\x3c"
    "\x21\x12\xa4\x42"
    "\xb7\xe7\xa7\x01\xbc\x34\xd6\x86\xfa\x87\xdf\xae"
    "\x80\x22\x00\x0b"
    "\x74\x65\x73\x74\x20\x76\x65\x63\x74\x6f\x72\x20"
    "\x00\x20\x00\x08"
    "\x00\x01\xa1\x47\xe1\x12\xa6\x43"
    "\x00\x08\x00\x14"
    "\x2b\x91\xf5\x99\xfd\x9e\x90\xc3\x8c\x74\x89\xf9"
    "\x2a\xf9\xba\x53\xf0\x6b\xe7\xd7"
    "\x80\x28\x00\x04"
    "\xc0\x7d\x4c\x96";

  /* we use our reader to parse the message */
  stun::Reader reader;
  reader.on_message = on_stun_message;
  reader.process((uint8_t*)respv4, sizeof(respv4) - 1); /* we do -1 to exclude the string terminating nul char. */

  return 0;
}

static void on_stun_message(stun::Message* msg, void* user) {

  printf("Successfully parsed a stun message.\n");

  stun::MessageIntegrity* integ;
  if (msg->find(&integ)) { 
    
    /* We are rewriting the STUN Message-Length header here because it will contain the size 
       which includes the FINGERPRINT element that is stored in the test data. We loop over
       all attributes until we find the MESSAGE-INTEGRITY attribute. We start with a 
       size of 24 which is the size of the MESSAGE-INTEGRITY attribute which is also used
       in the Message-Length when we compute the HMAC-SHA1
    */

    uint16_t msg_size = 24;
    for (size_t i = 0; i < msg->attributes.size(); ++i) {
      if (msg->attributes[i]->type == stun::STUN_ATTR_MESSAGE_INTEGRITY) {
        break;
      }
      msg_size += msg->attributes[i]->nbytes;  /* nbytes contains the real number of bytes in a message, including the padding */
    }

    printf("%u\n", msg_size);

    /* Now we need to rewrite the Message-Length field big endian */
    uint8_t* new_len = (uint8_t*)& msg_size;
    msg->buffer[2] = new_len[1];
    msg->buffer[3] = new_len[0];

    uint8_t sha[20];
    stun::Reader reader;

    /* Ok, this part is a bit confusing. We have 2 different size variables to take into account:

       1. the size that is encoded in the Message-Lenth field which includes the number of bytes
          of all attributes up to and including the MESSAGE-INTEGRITY attribute which is 24 bytes.

       2. but we only need to compute the SHA using the bytes up to, but EXCLUDING the 
          MESSAGE-INTEGRITY element, but INCLUDING the Stun message header which is 20 bytes. 
          So here we simply subtract 4 bytes from the computed msg_size.
    */
    uint32_t data_size = msg_size - 4;
    reader.calculateIntegrity(&msg->buffer[0], data_size, sha, "VOkJxbRl1RmTxUk/WvJxBt");
  }
}