File content as of revision 21:2fa676f214fe:

#include "comms.h"
#include "debug.h"

const int Packet::HEADER_SIZE = 5;

/*  If successful, loads p with a new packet.  User is responsible for deleting.
 *  Returns > 0 on success
              0 on timeout
            < 0 on error
 */
int Comms::receivePacket(Packet **p, int timeout) {
//    PC_PRINTLN("receivePacket: getting header");
    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];
    
    PC_PRINTLNF("receivePacket: cmd = %d", cmd);
    PC_PRINTLNF("receivePacket: packetNumber = %d", packetNumber);
    PC_PRINTLNF("receivePacket: dataLength = %d", dataLength);
    
    uint8_t data[dataLength];
    if (!getBytes(data, dataLength, timeout))
        return 0;
    
    PC_PRINTLN("receivePacket: getting checksum");
    uint16_t checkSum;
    if (!getBytes((uint8_t *) &checkSum, sizeof(checkSum), timeout))
        return 0;
    
    PC_PRINTLN("receivePacket: verifying checksum");
    *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++) {
//        PC_PRINTLNF("getBytes: waiting for byte %d", i);
        if (!getByte(buf+i, timeout)) {
//            PC_PRINTLN("getBytes: failed to get byte");
            return false;
        }
//        PC_PRINTLNF("getBytes: got byte %d", *(buf+i));
    }
    
//    PC_PRINTLN("getBytes: got bytes");
    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) {
//    PC_PRINTLN("getByte: getting byte");
    if (timeout != INT_MAX)
        timer.start();
        
//    PC_PRINTLN("getByte: waiting for bt readable");
    // Spin until byte available or timeout
    while ( !bt.readable() )
        if (timer.read_ms() > timeout)
            return false;
    
//    PC_PRINTLN("getByte: byte available");
    timer.stop();
    timer.reset();
    *byte = bt.getc();
    
//    PC_PRINTLNF("getByte: byte read = %d", *byte);
    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;
    checkSum ^= packetNumber;
    checkSum ^= 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;
}