Bas Stottelaar / Mbed 2 deprecated TinyLinkTest

Dependencies:   mbed

Committer:
basilfx
Date:
Thu Oct 02 21:59:38 2014 +0000
Revision:
1:32482de2ec7d
Parent:
0:3fa89b8e47ad
Working version, for question forum.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
basilfx 0:3fa89b8e47ad 1 #include "TinyLink.h"
basilfx 0:3fa89b8e47ad 2
basilfx 0:3fa89b8e47ad 3 #include "Crc.h"
basilfx 0:3fa89b8e47ad 4
basilfx 0:3fa89b8e47ad 5 uint32_t TinyLink::checksumFrame(uint8_t* data, size_t length, uint8_t checksumHeader) {
basilfx 0:3fa89b8e47ad 6 return crc::CRC32(crc::CRC32(data, length), checksumHeader);
basilfx 0:3fa89b8e47ad 7 }
basilfx 0:3fa89b8e47ad 8
basilfx 0:3fa89b8e47ad 9 uint8_t TinyLink::checksumHeader(uint16_t flags, uint16_t length) {
basilfx 0:3fa89b8e47ad 10 uint8_t a = (flags & 0x00FF);
basilfx 0:3fa89b8e47ad 11 uint8_t b = (flags & 0xFF00) >> 8;
basilfx 0:3fa89b8e47ad 12 uint8_t c = (length & 0x00FF);
basilfx 0:3fa89b8e47ad 13 uint8_t d = (length & 0xFF00) >> 8;
basilfx 0:3fa89b8e47ad 14
basilfx 0:3fa89b8e47ad 15 return a ^ b ^ c ^ d;
basilfx 0:3fa89b8e47ad 16 }
basilfx 0:3fa89b8e47ad 17
basilfx 1:32482de2ec7d 18 TinyLink::TinyLink(FILE* _handle, uint8_t* _buffer, size_t _length) : handle(_handle), buffer(_buffer) {
basilfx 0:3fa89b8e47ad 19 this->maxLength = _length - LEN_HEADER - LEN_BODY;
basilfx 0:3fa89b8e47ad 20
basilfx 0:3fa89b8e47ad 21 this->state = WAITING_FOR_PREAMBLE;
basilfx 0:3fa89b8e47ad 22 this->index = 0;
basilfx 0:3fa89b8e47ad 23 }
basilfx 0:3fa89b8e47ad 24
basilfx 0:3fa89b8e47ad 25 size_t TinyLink::write(frame_t* frame) {
basilfx 0:3fa89b8e47ad 26 size_t result = 0;
basilfx 0:3fa89b8e47ad 27
basilfx 0:3fa89b8e47ad 28 // Don't exceed max length
basilfx 0:3fa89b8e47ad 29 if (frame->length > this->maxLength) {
basilfx 0:3fa89b8e47ad 30 return 0;
basilfx 0:3fa89b8e47ad 31 }
basilfx 0:3fa89b8e47ad 32
basilfx 0:3fa89b8e47ad 33 // Send frame header
basilfx 0:3fa89b8e47ad 34 uint32_t start = PREAMBLE;
basilfx 0:3fa89b8e47ad 35 uint8_t checksumHeader = this->checksumHeader(frame->flags, frame->length);
basilfx 0:3fa89b8e47ad 36
basilfx 1:32482de2ec7d 37 result += fwrite(&start, sizeof(uint32_t), 1, this->handle);
basilfx 1:32482de2ec7d 38 result += fwrite(&frame->flags, sizeof(uint16_t), 1, this->handle);
basilfx 1:32482de2ec7d 39 result += fwrite(&frame->length, sizeof(uint16_t), 1, this->handle);
basilfx 1:32482de2ec7d 40 result += fwrite(&checksumHeader, sizeof(uint8_t), 1, this->handle);
basilfx 0:3fa89b8e47ad 41
basilfx 0:3fa89b8e47ad 42 // Send body
basilfx 0:3fa89b8e47ad 43 if (frame->length > 0 ) {
basilfx 0:3fa89b8e47ad 44 uint32_t checksum2 = this->checksumFrame(frame->data, frame->length, checksumHeader);
basilfx 0:3fa89b8e47ad 45
basilfx 1:32482de2ec7d 46 result += fwrite(frame->data, sizeof(uint8_t), frame->length, this->handle);
basilfx 1:32482de2ec7d 47 result += fwrite(&checksum2, sizeof(uint32_t), 1, this->handle);
basilfx 0:3fa89b8e47ad 48 }
basilfx 0:3fa89b8e47ad 49
basilfx 0:3fa89b8e47ad 50 // Return number of bytes written
basilfx 0:3fa89b8e47ad 51 return result;
basilfx 0:3fa89b8e47ad 52 }
basilfx 0:3fa89b8e47ad 53
basilfx 0:3fa89b8e47ad 54 size_t TinyLink::reset() {
basilfx 0:3fa89b8e47ad 55 frame_t frame;
basilfx 0:3fa89b8e47ad 56
basilfx 0:3fa89b8e47ad 57 frame.length = 0;
basilfx 0:3fa89b8e47ad 58 frame.flags = FLAG_RESET;
basilfx 0:3fa89b8e47ad 59 frame.data = NULL;
basilfx 0:3fa89b8e47ad 60
basilfx 0:3fa89b8e47ad 61 return this->write(&frame);
basilfx 0:3fa89b8e47ad 62 }
basilfx 0:3fa89b8e47ad 63
basilfx 0:3fa89b8e47ad 64 size_t TinyLink::write(uint8_t* buffer, size_t length, uint16_t flags) {
basilfx 0:3fa89b8e47ad 65 frame_t frame;
basilfx 0:3fa89b8e47ad 66
basilfx 0:3fa89b8e47ad 67 frame.length = length;
basilfx 0:3fa89b8e47ad 68 frame.flags = flags;
basilfx 0:3fa89b8e47ad 69 frame.data = buffer;
basilfx 0:3fa89b8e47ad 70
basilfx 0:3fa89b8e47ad 71 return this->write(&frame);
basilfx 0:3fa89b8e47ad 72 }
basilfx 0:3fa89b8e47ad 73
basilfx 0:3fa89b8e47ad 74 size_t TinyLink::write(uint8_t* buffer, size_t length) {
basilfx 0:3fa89b8e47ad 75 frame_t frame;
basilfx 0:3fa89b8e47ad 76
basilfx 0:3fa89b8e47ad 77 frame.length = length;
basilfx 0:3fa89b8e47ad 78 frame.flags = FLAG_NONE;
basilfx 0:3fa89b8e47ad 79 frame.data = buffer;
basilfx 0:3fa89b8e47ad 80
basilfx 0:3fa89b8e47ad 81 return this->write(&frame);
basilfx 0:3fa89b8e47ad 82 }
basilfx 0:3fa89b8e47ad 83
basilfx 0:3fa89b8e47ad 84 int TinyLink::read(size_t limit, frame_t* frame) {
basilfx 0:3fa89b8e47ad 85 return this->read(limit, frame, true);
basilfx 0:3fa89b8e47ad 86 }
basilfx 0:3fa89b8e47ad 87
basilfx 0:3fa89b8e47ad 88 int TinyLink::read(size_t limit, frame_t* frame, uint8_t ignoreDamaged) {
basilfx 0:3fa89b8e47ad 89 // Read next up to `limit' bytes
basilfx 0:3fa89b8e47ad 90 while (limit--) {
basilfx 1:32482de2ec7d 91 fread(&this->buffer[this->index++], sizeof(uint8_t), 1, this->handle);
basilfx 0:3fa89b8e47ad 92
basilfx 0:3fa89b8e47ad 93 // Decide what to do
basilfx 0:3fa89b8e47ad 94 switch (this->state) {
basilfx 0:3fa89b8e47ad 95 case WAITING_FOR_PREAMBLE:
basilfx 0:3fa89b8e47ad 96 if (this->index >= LEN_PREAMBLE) {
basilfx 0:3fa89b8e47ad 97 uint32_t start = *((uint32_t*) &this->buffer[this->index - 4]);
basilfx 0:3fa89b8e47ad 98
basilfx 0:3fa89b8e47ad 99 if (start == PREAMBLE) {
basilfx 0:3fa89b8e47ad 100 // Preamble found, advance state
basilfx 0:3fa89b8e47ad 101 this->state = WAITING_FOR_HEADER;
basilfx 0:3fa89b8e47ad 102 this->index = 0;
basilfx 0:3fa89b8e47ad 103 } else if (this->index == this->maxLength + LEN_HEADER + LEN_BODY) {
basilfx 0:3fa89b8e47ad 104 // Preamble not found and stream is full. Copy last four
basilfx 0:3fa89b8e47ad 105 // bytes, because the next byte may form the preamble
basilfx 0:3fa89b8e47ad 106 // together with the last three bytes.
basilfx 0:3fa89b8e47ad 107 *((uint32_t*) this->buffer[0]) = start;
basilfx 0:3fa89b8e47ad 108 this->index = 4;
basilfx 0:3fa89b8e47ad 109 }
basilfx 0:3fa89b8e47ad 110 }
basilfx 0:3fa89b8e47ad 111
basilfx 0:3fa89b8e47ad 112 break;
basilfx 0:3fa89b8e47ad 113 case WAITING_FOR_HEADER:
basilfx 0:3fa89b8e47ad 114 if (this->index == LEN_HEADER) {
basilfx 0:3fa89b8e47ad 115 uint16_t flags = (uint16_t) this->buffer[0];
basilfx 0:3fa89b8e47ad 116 uint16_t length = (uint16_t) this->buffer[2];
basilfx 0:3fa89b8e47ad 117 uint8_t checksum = this->buffer[4];
basilfx 0:3fa89b8e47ad 118
basilfx 0:3fa89b8e47ad 119 if (checksum == this->checksumHeader(flags, length) && length <= this->maxLength) {
basilfx 0:3fa89b8e47ad 120 if (length > 0) {
basilfx 0:3fa89b8e47ad 121 this->state = WAITING_FOR_BODY;
basilfx 0:3fa89b8e47ad 122 } else {
basilfx 0:3fa89b8e47ad 123 frame->flags = FLAG_RESET;
basilfx 0:3fa89b8e47ad 124 frame->length = 0;
basilfx 0:3fa89b8e47ad 125 frame->damaged = 0;
basilfx 0:3fa89b8e47ad 126 frame->data = NULL;
basilfx 0:3fa89b8e47ad 127
basilfx 0:3fa89b8e47ad 128 // Reset to start state
basilfx 0:3fa89b8e47ad 129 this->state = WAITING_FOR_PREAMBLE;
basilfx 0:3fa89b8e47ad 130 this->index = 0;
basilfx 0:3fa89b8e47ad 131
basilfx 0:3fa89b8e47ad 132 // Return success
basilfx 0:3fa89b8e47ad 133 return 1;
basilfx 0:3fa89b8e47ad 134 }
basilfx 0:3fa89b8e47ad 135 } else {
basilfx 0:3fa89b8e47ad 136 // Reset to start state
basilfx 0:3fa89b8e47ad 137 this->state = WAITING_FOR_PREAMBLE;
basilfx 0:3fa89b8e47ad 138 this->index = 0;
basilfx 0:3fa89b8e47ad 139 }
basilfx 0:3fa89b8e47ad 140 }
basilfx 0:3fa89b8e47ad 141
basilfx 0:3fa89b8e47ad 142 break;
basilfx 0:3fa89b8e47ad 143 case WAITING_FOR_BODY:
basilfx 0:3fa89b8e47ad 144 if (this->index == LEN_HEADER + frame->length + LEN_CRC) {
basilfx 0:3fa89b8e47ad 145 uint16_t flags = (uint16_t) this->buffer[0];
basilfx 0:3fa89b8e47ad 146 uint16_t length = (uint16_t) this->buffer[2];
basilfx 0:3fa89b8e47ad 147
basilfx 0:3fa89b8e47ad 148 uint8_t checksumA = this->buffer[9];
basilfx 0:3fa89b8e47ad 149 uint32_t checksumB = (uint32_t) this->buffer[this->index - 4];
basilfx 0:3fa89b8e47ad 150
basilfx 0:3fa89b8e47ad 151 uint8_t damaged = (checksumB != this->checksumFrame(&this->buffer[LEN_HEADER], length, checksumA));
basilfx 0:3fa89b8e47ad 152
basilfx 0:3fa89b8e47ad 153 // Reset to start state
basilfx 0:3fa89b8e47ad 154 this->state = WAITING_FOR_PREAMBLE;
basilfx 0:3fa89b8e47ad 155 this->index = 0;
basilfx 0:3fa89b8e47ad 156
basilfx 0:3fa89b8e47ad 157 // Copy bytes
basilfx 0:3fa89b8e47ad 158 if (!damaged || !ignoreDamaged) {
basilfx 0:3fa89b8e47ad 159 frame->flags = flags;
basilfx 0:3fa89b8e47ad 160 frame->length = length;
basilfx 0:3fa89b8e47ad 161 frame->damaged = damaged;
basilfx 0:3fa89b8e47ad 162 memcpy(&this->buffer[LEN_HEADER], &frame->data, length);
basilfx 0:3fa89b8e47ad 163
basilfx 0:3fa89b8e47ad 164 // Return success
basilfx 0:3fa89b8e47ad 165 return 1;
basilfx 0:3fa89b8e47ad 166 }
basilfx 0:3fa89b8e47ad 167 }
basilfx 0:3fa89b8e47ad 168
basilfx 0:3fa89b8e47ad 169 break;
basilfx 0:3fa89b8e47ad 170 }
basilfx 0:3fa89b8e47ad 171 }
basilfx 0:3fa89b8e47ad 172
basilfx 0:3fa89b8e47ad 173 // No frames processed
basilfx 0:3fa89b8e47ad 174 return 0;
basilfx 0:3fa89b8e47ad 175 }