136 lines
2.5 KiB
C++
136 lines
2.5 KiB
C++
#include "link.h"
|
|
#include "roverif.h"
|
|
|
|
#include <avr/io.h>
|
|
#include <avr/interrupt.h>
|
|
|
|
#include <cassert>
|
|
|
|
Link::Link()
|
|
: tx_at_(1), tx_end_(0),
|
|
rx_at_(0), rx_xor_(0) {
|
|
}
|
|
|
|
uint8_t Link::checksum(const uint8_t* p, uint8_t length) {
|
|
uint16_t sum1 = 0x12;
|
|
uint16_t sum2 = 0x34;
|
|
|
|
for (; length != 0; length--, p++) {
|
|
sum1 += *p;
|
|
if (sum1 >= 255) {
|
|
sum1 -= 255;
|
|
}
|
|
sum2 += sum1;
|
|
if (sum2 >= 255) {
|
|
sum2 -= 255;
|
|
}
|
|
}
|
|
|
|
return (uint8_t)(sum1 ^ sum2);
|
|
}
|
|
|
|
inline void Link::disable_tx() {
|
|
UCSR0B &= ~_BV(UDRIE0);
|
|
}
|
|
|
|
inline void Link::enable_tx() {
|
|
UCSR0B |= _BV(UDRIE0);
|
|
}
|
|
|
|
inline void Link::putch(uint8_t ch) {
|
|
UDR0 = ch;
|
|
}
|
|
|
|
void* Link::start() {
|
|
if (tx_at_ > tx_end_) {
|
|
return tx_;
|
|
} else {
|
|
return nullptr;
|
|
}
|
|
}
|
|
|
|
void Link::send(uint8_t length) {
|
|
assert(length > 0);
|
|
assert(tx_at_ > tx_end_);
|
|
|
|
tx_[length] = checksum(tx_, length);
|
|
|
|
tx_end_ = length + 1;
|
|
tx_at_ = 0;
|
|
|
|
enable_tx();
|
|
|
|
sent++;
|
|
}
|
|
|
|
const void* Link::peek(uint8_t* plength) {
|
|
if (!rx_full_) {
|
|
return nullptr;
|
|
} else if (rx_at_ < 2) {
|
|
// Need at least the code and checksum.
|
|
discard();
|
|
rx_errors++;
|
|
return nullptr;
|
|
} else {
|
|
uint8_t len = rx_at_ - 1;
|
|
uint8_t sum = checksum(rx_, len);
|
|
|
|
if (sum != rx_[len]) {
|
|
discard();
|
|
rx_errors++;
|
|
return nullptr;
|
|
} else {
|
|
*plength = len;
|
|
received++;
|
|
return rx_;
|
|
}
|
|
}
|
|
}
|
|
|
|
inline void Link::tx_next() {
|
|
if (tx_at_ > tx_end_) {
|
|
// Nothing to do.
|
|
} else if (tx_at_ == tx_end_) {
|
|
putch(Mark);
|
|
tx_at_++;
|
|
disable_tx();
|
|
} else {
|
|
uint8_t next = tx_[tx_at_];
|
|
|
|
if (next == Mark || next == Escape) {
|
|
putch(Escape);
|
|
tx_[tx_at_] = next ^ Xor;
|
|
} else {
|
|
putch(next);
|
|
tx_at_++;
|
|
}
|
|
}
|
|
}
|
|
|
|
inline void Link::rx_next() {
|
|
uint8_t ch = UDR0;
|
|
|
|
if (!rx_full_) {
|
|
if (ch == Mark) {
|
|
rx_full_ = true;
|
|
} else if (ch == Escape) {
|
|
rx_xor_ = Xor;
|
|
} else {
|
|
if (rx_at_ == sizeof(rx_)) {
|
|
// Overflow
|
|
} else {
|
|
rx_[rx_at_++] = ch ^ rx_xor_;
|
|
rx_xor_ = 0;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
ISR(USART_RX_vect) {
|
|
RoverIf::link.rx_next();
|
|
}
|
|
|
|
ISR(USART_UDRE_vect) {
|
|
RoverIf::link.tx_next();
|
|
}
|