Diff: TinyLink.cpp

Revision:

0:3fa89b8e47ad

Child:

1:32482de2ec7d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TinyLink.cpp	Thu Oct 02 21:58:30 2014 +0000
@@ -0,0 +1,175 @@
+#include "TinyLink.h"
+
+#include "Crc.h"
+
+uint32_t TinyLink::checksumFrame(uint8_t* data, size_t length, uint8_t checksumHeader) {
+    return crc::CRC32(crc::CRC32(data, length), checksumHeader);
+}
+
+uint8_t TinyLink::checksumHeader(uint16_t flags, uint16_t length) {
+    uint8_t a = (flags  & 0x00FF);
+    uint8_t b = (flags  & 0xFF00) >> 8;
+    uint8_t c = (length & 0x00FF);
+    uint8_t d = (length & 0xFF00) >> 8;
+
+    return a ^ b ^ c ^ d;
+}
+
+TinyLink::TinyLink(FILE& _handle, uint8_t* _buffer, size_t _length) : handle(_handle), buffer(_buffer) {
+    this->maxLength = _length - LEN_HEADER - LEN_BODY;
+
+    this->state = WAITING_FOR_PREAMBLE;
+    this->index = 0;
+}
+
+size_t TinyLink::write(frame_t* frame) {
+    size_t result = 0;
+
+    // Don't exceed max length
+    if (frame->length > this->maxLength) {
+        return 0;
+    }
+
+    // Send frame header
+    uint32_t start = PREAMBLE;
+    uint8_t checksumHeader = this->checksumHeader(frame->flags, frame->length);
+
+    result += fwrite(&start, sizeof(uint32_t), 1, &this->handle);
+    result += fwrite(&frame->flags, sizeof(uint16_t), 1, &this->handle);
+    result += fwrite(&frame->length, sizeof(uint16_t), 1, &this->handle);
+    result += fwrite(&checksumHeader, sizeof(uint8_t), 1, &this->handle);
+
+    // Send body
+    if (frame->length > 0 ) {
+        uint32_t checksum2 = this->checksumFrame(frame->data, frame->length, checksumHeader);
+
+        result += fwrite(frame->data, sizeof(uint8_t), frame->length, &this->handle);
+        result += fwrite(&checksum2, sizeof(uint32_t), 1, &this->handle);
+    }
+
+    // Return number of bytes written
+    return result;
+}
+
+size_t TinyLink::reset() {
+    frame_t frame;
+    
+    frame.length = 0;
+    frame.flags = FLAG_RESET;
+    frame.data = NULL;
+    
+    return this->write(&frame);
+}
+
+size_t TinyLink::write(uint8_t* buffer, size_t length, uint16_t flags) {
+    frame_t frame;
+    
+    frame.length = length;
+    frame.flags = flags;
+    frame.data = buffer;
+    
+    return this->write(&frame);
+}
+
+size_t TinyLink::write(uint8_t* buffer, size_t length) {
+    frame_t frame;
+    
+    frame.length = length;
+    frame.flags = FLAG_NONE;
+    frame.data = buffer;
+    
+    return this->write(&frame);
+}
+
+int TinyLink::read(size_t limit, frame_t* frame) {
+    return this->read(limit, frame, true);
+}
+
+int TinyLink::read(size_t limit, frame_t* frame, uint8_t ignoreDamaged) {
+    // Read next up to `limit' bytes
+    while (limit--) {
+        fread(&this->buffer[this->index++], sizeof(uint8_t), 1, &this->handle);
+
+        // Decide what to do
+        switch (this->state) {
+            case WAITING_FOR_PREAMBLE:
+                if (this->index >= LEN_PREAMBLE) {
+                    uint32_t start = *((uint32_t*) &this->buffer[this->index - 4]);
+
+                    if (start == PREAMBLE) {
+                        // Preamble found, advance state
+                        this->state = WAITING_FOR_HEADER;
+                        this->index = 0;
+                    } else if (this->index == this->maxLength + LEN_HEADER + LEN_BODY) {
+                        // Preamble not found and stream is full. Copy last four
+                        // bytes, because the next byte may form the preamble
+                        // together with the last three bytes.
+                        *((uint32_t*) this->buffer[0]) = start;
+                        this->index = 4;
+                    }
+                }
+
+                break;
+            case WAITING_FOR_HEADER:
+                if (this->index == LEN_HEADER) {
+                    uint16_t flags = (uint16_t) this->buffer[0];
+                    uint16_t length = (uint16_t) this->buffer[2];
+                    uint8_t checksum = this->buffer[4];
+
+                    if (checksum == this->checksumHeader(flags, length) && length <= this->maxLength) {
+                        if (length > 0) {
+                            this->state = WAITING_FOR_BODY;
+                        } else {
+                            frame->flags = FLAG_RESET;
+                            frame->length = 0;
+                            frame->damaged = 0;
+                            frame->data = NULL;
+
+                            // Reset to start state
+                            this->state = WAITING_FOR_PREAMBLE;
+                            this->index = 0;
+
+                            // Return success
+                            return 1;
+                        }
+                    } else {
+                        // Reset to start state
+                        this->state = WAITING_FOR_PREAMBLE;
+                        this->index = 0;
+                    }
+                }
+
+                break;
+            case WAITING_FOR_BODY:
+                if (this->index == LEN_HEADER + frame->length + LEN_CRC) {
+                    uint16_t flags = (uint16_t) this->buffer[0];
+                    uint16_t length = (uint16_t) this->buffer[2];
+
+                    uint8_t checksumA = this->buffer[9];
+                    uint32_t checksumB = (uint32_t) this->buffer[this->index - 4];
+
+                    uint8_t damaged = (checksumB != this->checksumFrame(&this->buffer[LEN_HEADER], length, checksumA));
+
+                    // Reset to start state
+                    this->state = WAITING_FOR_PREAMBLE;
+                    this->index = 0;
+
+                    // Copy bytes
+                    if (!damaged || !ignoreDamaged) {
+                        frame->flags = flags;
+                        frame->length = length;
+                        frame->damaged = damaged;
+                        memcpy(&this->buffer[LEN_HEADER], &frame->data, length);
+
+                        // Return success
+                        return 1;
+                    }
+                }
+
+                break;
+        }
+    }
+
+    // No frames processed
+    return 0;
+}
\ No newline at end of file