got sending packets actually working
diff --git a/bbb_cape/src/bbb/crc.cc b/bbb_cape/src/bbb/crc.cc
index 214f86f..b16eb94 100644
--- a/bbb_cape/src/bbb/crc.cc
+++ b/bbb_cape/src/bbb/crc.cc
@@ -32,11 +32,13 @@
uint32_t r = 0xFFFFFFFF;
- for (size_t i = 0; i < length; ++i) {
- r = (r << 8) ^ table[(r >> 24) ^ data[i]];
+ for (size_t i = 0; i < (length / 4); ++i) {
+ for (int ii = 3; ii >= 0; --ii) {
+ r = (r << 8) ^ table[(r >> 24) ^ data[i * 4 + ii]];
+ }
}
- return ~r;
+ return r;
}
} // namespace cape
diff --git a/bbb_cape/src/bbb/uart_reader.cc b/bbb_cape/src/bbb/uart_reader.cc
index 0a3f3c6..7b5e116 100644
--- a/bbb_cape/src/bbb/uart_reader.cc
+++ b/bbb_cape/src/bbb/uart_reader.cc
@@ -14,6 +14,8 @@
#include "bbb_cape/src/cape/cows.h"
#include "bbb/crc.h"
+#define PACKET_SIZE (DATA_STRUCT_SEND_SIZE - 4)
+
// This is the code for receiving data from the cape via UART.
// NOTE: In order for this to work, you MUST HAVE
// "capemgr.enable_partno=BB_UART1"
@@ -32,9 +34,10 @@
UartReader::UartReader(int32_t baud_rate)
: baud_rate_(baud_rate),
- buf_(new AlignedChar[DATA_STRUCT_SEND_SIZE]),
+ buf_(new AlignedChar[PACKET_SIZE]),
+ unstuffed_data_(new AlignedChar[PACKET_SIZE - 4]),
fd_(open(device, O_RDWR | O_NOCTTY)) {
- static_assert((DATA_STRUCT_SEND_SIZE % 4) == 0,
+ static_assert((PACKET_SIZE % 4) == 0,
"We can't do checksums of lengths that aren't multiples of 4.");
if (fd_ < 0) {
@@ -71,7 +74,7 @@
options.c_oflag = 0;
options.c_lflag = 0;
options.c_cc[VMIN] = 0;
- options.c_cc[VTIME] = 1;
+ options.c_cc[VTIME] = 10;
if (tcsetattr(fd_, TCSANOW, &options) != 0) {
LOG(FATAL, "tcsetattr(%d, TCSANOW, %p) failed with %d: %s\n",
fd_, &options, errno, strerror(errno));
@@ -117,91 +120,107 @@
UartReader::~UartReader() {
delete buf_;
+ delete unstuffed_data_;
if (fd_ > 0) close(fd_);
}
+// TODO(brians): Figure out why this (sometimes?) gets confused right after
+// flashing the cape.
bool UartReader::FindPacket() {
// How many 0 bytes we've found at the front so far.
int zeros_found = 0;
- // How many bytes of the packet we've read in (or -1 if we don't know where
- // the packet is).
- int packet_bytes = -1;
while (true) {
- size_t already_read = ::std::max(packet_bytes, 0);
+ size_t already_read = ::std::max(0, packet_bytes_);
ssize_t new_bytes =
- read(fd_, buf_ + already_read, DATA_STRUCT_SEND_SIZE - already_read);
- LOG(DEBUG, "read %zd, wanted %d\n", new_bytes,
- DATA_STRUCT_SEND_SIZE - already_read);
- for (int i = 0; i < new_bytes; ++i) {
- LOG(DEBUG, "%x\n", buf_[i]);
- }
+ read(fd_, buf_ + already_read, PACKET_SIZE - already_read);
if (new_bytes < 0) {
if (errno == EINTR) continue;
- LOG(WARNING, "read(%d, %p, %zd) failed with %d: %s\n",
- fd_, buf_ + already_read, DATA_STRUCT_SEND_SIZE - already_read,
+ LOG(FATAL, "read(%d, %p, %zd) failed with %d: %s\n",
+ fd_, buf_ + already_read, PACKET_SIZE - already_read,
errno, strerror(errno));
return false;
}
- if (packet_bytes != -1) { // we think there's a packet at the beginning of
- // our buffer
- for (int to_check = packet_bytes; packet_bytes + new_bytes; ++to_check) {
- // We shouldn't find any 0s in the middle of what should be a packet.
+ if (packet_bytes_ == -1) {
+ for (size_t to_check = already_read; to_check < already_read + new_bytes;
+ ++to_check) {
if (buf_[to_check] == 0) {
- packet_bytes = -1;
- memmove(buf_, buf_ + to_check, new_bytes - to_check);
- new_bytes -= to_check;
- break;
- }
- }
- if (packet_bytes != -1) {
- packet_bytes += new_bytes;
- if (packet_bytes == DATA_STRUCT_SEND_SIZE) return true;
- }
- }
- // This can't just be an else because the above code might set it to -1 if
- // it finds 0s in the middle of a packet.
- if (packet_bytes == -1) {
- // Find the beginning of the packet (aka look for four zero bytes).
- for (ssize_t checked = 0; checked < new_bytes; ++checked) {
- if (buf_[checked] == 0) {
++zeros_found;
if (zeros_found == 4) {
- packet_bytes = new_bytes - checked - 1;
- memmove(buf_, buf_ + checked + 1, packet_bytes);
- break;
+ packet_bytes_ = 0;
+ zeros_found = 0;
+ new_bytes -= to_check + 1;
+ memmove(buf_, buf_ + to_check + 1, new_bytes);
+ to_check = 0;
}
} else {
zeros_found = 0;
}
}
}
+ if (packet_bytes_ != -1) { // if we decided that these are good bytes
+ packet_bytes_ += new_bytes;
+ if (packet_bytes_ == PACKET_SIZE) return true;
+ }
}
}
-bool UartReader::GetPacket(DataStruct *packet) {
- if (!FindPacket()) return false;
-
- uint32_t unstuffed = cows_unstuff(reinterpret_cast<uint32_t *>(buf_),
- DATA_STRUCT_SEND_SIZE / 4,
- reinterpret_cast<uint32_t *>(packet));
+bool UartReader::ProcessPacket() {
+ uint32_t unstuffed =
+ cows_unstuff(reinterpret_cast<uint32_t *>(buf_), PACKET_SIZE,
+ reinterpret_cast<uint32_t *>(unstuffed_data_));
if (unstuffed == 0) {
LOG(WARNING, "invalid packet\n");
- } else if (unstuffed != sizeof(packet)) {
+ return false;
+ } else if (unstuffed != (PACKET_SIZE - 4) / 4) {
LOG(WARNING, "packet is %" PRIu32 " words instead of %" PRIu32 "\n",
- unstuffed, DATA_STRUCT_SEND_SIZE / 4);
+ unstuffed, (PACKET_SIZE - 4) / 4);
+ return false;
}
// Make sure the checksum checks out.
- uint32_t checksum;
- memcpy(&checksum, buf_ + DATA_STRUCT_SEND_SIZE - 4, 4);
- if (cape::CalculateChecksum(reinterpret_cast<uint8_t *>(packet),
- sizeof(DataStruct)) != checksum) {
- LOG(WARNING, "Rejecting packet due to checksum failure.\n");
+ uint32_t sent_checksum;
+ memcpy(&sent_checksum, unstuffed_data_ + PACKET_SIZE - 8, 4);
+ uint32_t calculated_checksum = cape::CalculateChecksum(
+ reinterpret_cast<uint8_t *>(unstuffed_data_), PACKET_SIZE - 8);
+ if (sent_checksum != calculated_checksum) {
+ LOG(WARNING, "sent checksum: %" PRIx32 " vs calculated: %" PRIx32"\n",
+ sent_checksum, calculated_checksum);
return false;
}
return true;
}
+bool UartReader::GetPacket(DataStruct *packet) {
+ static_assert(sizeof(*packet) <= PACKET_SIZE - 8,
+ "output data type is too big");
+
+ if (!FindPacket()) return false;
+
+ if (!ProcessPacket()) {
+ packet_bytes_ = -1;
+ int zeros = 0;
+ for (int i = 0; i < PACKET_SIZE; ++i) {
+ if (buf_[i] == 0) {
+ ++zeros;
+ if (zeros == 4) {
+ LOG(INFO, "found another packet start at %d\n", i);
+ packet_bytes_ = PACKET_SIZE - (i + 1);
+ memmove(buf_, buf_ + i + 1, packet_bytes_);
+ return false;
+ }
+ } else {
+ zeros = 0;
+ }
+ }
+ return false;
+ } else {
+ packet_bytes_ = -1;
+ }
+ memcpy(packet, unstuffed_data_, sizeof(*packet));
+
+ return true;
+}
+
} // namespace bbb
diff --git a/bbb_cape/src/bbb/uart_reader.h b/bbb_cape/src/bbb/uart_reader.h
index 9d5a510..ea0cae0 100644
--- a/bbb_cape/src/bbb/uart_reader.h
+++ b/bbb_cape/src/bbb/uart_reader.h
@@ -3,6 +3,8 @@
#include <stdint.h>
+#include <memory>
+
#define DATA_STRUCT_NAME DataStruct
#include "cape/data_struct.h"
#undef DATA_STRUCT_NAME
@@ -24,11 +26,22 @@
// packet is invalid in some way.
bool FindPacket();
- typedef char __attribute__((aligned(8))) AlignedChar;
+ // Processes a packet currently in buf_ and leaves the result in
+ // unstuffed_data_.
+ // Returns true if it succeeds or false if there was something wrong with the
+ // data.
+ bool ProcessPacket();
+
+ typedef char __attribute__((aligned(4))) AlignedChar;
const int32_t baud_rate_;
AlignedChar *const buf_;
+ AlignedChar *const unstuffed_data_;
const int fd_;
+
+ // How many bytes of the packet we've read in (or -1 if we don't know where
+ // the packet is).
+ int packet_bytes_ = -1;
};
} // namespace bbb
diff --git a/bbb_cape/src/bbb/uart_reader_main.cc b/bbb_cape/src/bbb/uart_reader_main.cc
index d0fd028..aadd902 100644
--- a/bbb_cape/src/bbb/uart_reader_main.cc
+++ b/bbb_cape/src/bbb/uart_reader_main.cc
@@ -23,8 +23,7 @@
#endif
//::bbb::UartReader receiver(3000000);
- ::bbb::UartReader receiver(300000);
- //::bbb::UartReader receiver(19200);
+ ::bbb::UartReader receiver(30000);
Time last_packet_time = Time::Now();
while (true) {