File content as of revision 18:06b718f8e6fd:

#include "comms.h"

const int Packet::HEADER_SIZE = 5;

/*  Loads p with a new packet.  
 *  Returns > 0 on success
              0 on timeout
            < 0 on error
 */
int Comms::receivePacket(Packet **p, unsigned timeout) {
    uint8_t headerBuf[Packet::HEADER_SIZE];
    if (!getBytes(headerBuf, Packet::HEADER_SIZE, timeout))
        return 0;
    
    uint8_t cmd = headerBuf[0];
    uint16_t packetNumber = headerBuf[1];
    uint16_t dataLength = headerBuf[3];
    
    uint8_t data[dataLength];
    if (!getBytes(data, dataLength, timeout))
        return 0;
        
    uint16_t checkSum;
    if (!getBytes((uint8_t *) &checkSum, sizeof(checkSum), timeout))
        return 0;
    
    *p = Packet::createAndVerify(cmd, packetNumber, dataLength, data, checkSum);
    if (*p == NULL)
        return -1;
    
    return 1;
}

/* Sends packet p, and deletes it */
void Comms::sendPacket(Packet *p) {
    sendBytes((uint8_t *)&p->cmd, sizeof(p->cmd));
    sendBytes((uint8_t *)&p->packetNumber, sizeof(p->packetNumber));
    sendBytes((uint8_t *)&p->dataLength, sizeof(p->dataLength));
    sendBytes((uint8_t *) p->data, p->dataLength);
    sendBytes((uint8_t *)&p->checkSum, sizeof(p->checkSum));
}

void Comms::sendBytes(uint8_t *bytes, int nElms) {
    for (int i = 0; i < nElms; i++)
        bt.putc(bytes[i]);   
}

void Comms::sendByte(uint8_t byte) {
    bt.putc(byte);
}

/* Loads buf with a byte from the bluetooth stream.
 * Returns true if successful, false on timeout. */
bool Comms::getBytes(uint8_t *buf, int n, int timeout) {
    for (int i = 0; i < n; i++)
        if (!getByte(buf+i, timeout))
            return false;
            
    return true;
}

/* Loads c with a byte from the bluetooth stream.
 * Returns true if successful, false on timeout. */
bool Comms::getByte(uint8_t *byte, int timeout) {
    if (timeout != INT_MAX)
        timer.start();
        
    // Spin until byte available or timeout
    while ( !bt.readable() )
        if (timer.read_ms() > timeout)
            return false;
    
    timer.stop();
    timer.reset();
    *byte = bt.getc();
    return true;
}

/* Returns a new packet.  Copies dataLength bytes from data, and verifies the checkSum.
 * If the given checkSum is incorrect, returns NULL.
 * User is responsible for deleting the returned packet. */
Packet *Packet::createAndVerify(uint8_t cmd, uint16_t packetNumber, uint16_t dataLength, uint8_t *data, uint16_t checkSum) {
    if (dataLength > 0 && data == NULL)
        return NULL;
    
    Packet *p = new Packet(cmd, packetNumber, dataLength, data);
    if (p->checkSum != checkSum)
        return NULL;
    else
        return p;
}

/* Returns a new packet.  Copies dataLength bytes from data, and generates the checksum.
 * User is responsible for deleting the returned packet. */
Packet *Packet::create(uint8_t cmd, uint16_t packetNumber, uint16_t dataLength, uint8_t *data) {
    if (dataLength > 0 && data == NULL)
        return NULL;
        
    return new Packet(cmd, packetNumber, dataLength, data);
}

/* Copies dataLength bytes from data */
Packet::Packet(uint8_t cmd, uint16_t packetNumber, uint16_t dataLength, uint8_t *data) {
    checkSum = 0;
    
    this->cmd = cmd;
    this->packetNumber = packetNumber;
    this->dataLength = dataLength;
    
    checkSum += cmd + packetNumber + dataLength;
    
    this->data = new uint8_t[dataLength];
    for (int i = 0; i < dataLength; i++) {
        this->data[i] = data[i];
        checkSum += data[i];
    }
}

Packet::~Packet() {
    delete[] data;
}