File content as of revision 3:80ed23c0ae01:

#include "mbed.h"
#include <string.h>
#include <stdlib.h>
#include "swtp_codec.h"

////////////////////////////////////
// First byte for INFO packets...

//  Hdr Ctl  |--- Version --|
//   X   X   X  X  X  X  X  X

#define CONTROL_PKT_IND_BIT (1<<6)
#define HEADER_PKT_IND_BIT  (1<<7)

#define VERSION_MASK         (0x3F) // 00111111
#define CONTROL_PKT_IND_MASK (0xBF) // 10111111
#define HEADER_PKT_IND_MASK  (0x7F) // 01111111
////////////////////////////////////

extern Serial pc;

/**
 * Encodes a given HeaderPacket data structure into byte buffer suitable for transmission.
 * @param packetToFill Byte array to be encoded into.
 * @param packetSize Size of byte buffer
 * @param headerData Structure giving the data that is to be packed.
 */
void encodeHeaderPacket(unsigned char* packetToFill, int packetSize, PacketData* headerData) {

    for (int i = 0; i < packetSize; i++) packetToFill[i] = 0;

    // Control Byte
    packetToFill[0] =  (SWTP_VERSION & VERSION_MASK); // bits 0:5
    packetToFill[0] |= (CONTROL_PKT_IND_MASK);        // bit 6 = 0
    packetToFill[0] &= (HEADER_PKT_IND_BIT);          // bit 7 = 1

    // Mime Type
    packetToFill[1] = (headerData->messageHeader.mimeType & 0x00FF);
    packetToFill[2] = (headerData->messageHeader.mimeType & 0xFF00) >> 8;

    // Content length
    packetToFill[3] = (headerData->messageHeader.payloadLengthBytes >>  0) & 0xFF;
    packetToFill[4] = (headerData->messageHeader.payloadLengthBytes >>  8) & 0xFF;
    packetToFill[5] = (headerData->messageHeader.payloadLengthBytes >> 16) & 0xFF;
    packetToFill[6] = (headerData->messageHeader.payloadLengthBytes >> 24) & 0xFF;

    // First payload sequence number
    packetToFill[7] = (headerData->messageHeader.startPayloadSequenceNumber & 0x00FF);
    packetToFill[8] = (headerData->messageHeader.startPayloadSequenceNumber & 0xFF00) >> 8;

}

/**
 * Decodes a byte array of data into a HeaderPacket.
 * @param packet Byte array to decode from.
 * @param packetSize Number of bytes in array
 * @param headerDataToFill Structure of PacketData to populate
 */
void decodePacket(uint8* packet, int packetSize, PacketData* headerDataToFill) {

    headerDataToFill->version         = (packet[0] & VERSION_MASK);
    headerDataToFill->isMessageHeader = (packet[0] & HEADER_PKT_IND_BIT);
    headerDataToFill->isControlPacket = (packet[0] & CONTROL_PKT_IND_BIT);

    if (headerDataToFill->isMessageHeader) {
        headerDataToFill->messageHeader.mimeType = (packet[2] << 8) | packet[1];
        headerDataToFill->messageHeader.payloadLengthBytes =
            (packet[3] <<  0) |
            (packet[4] <<  8) |
            (packet[5] << 16) |
            (packet[6] << 24)
        ;
        headerDataToFill->messageHeader.startPayloadSequenceNumber = (packet[8] << 8) | packet[7];
    } else if (headerDataToFill->isControlPacket) {
    } else { // A message payload packet

    }

}