PPPoE::PPPoE(const uint8_t* buffer, uint32_t total_sz)
: tags_size_() {
InputMemoryStream stream(buffer, total_sz);
stream.read(header_);
stream.size(std::min(stream.size(), (size_t)payload_length()));
// If this is a session data packet
if (code() == 0) {
if (stream) {
inner_pdu(
new RawPDU(stream.pointer(), stream.size())
);
}
}
else {
while (stream) {
TagTypes opt_type = static_cast<TagTypes>(stream.read<uint16_t>());
uint16_t opt_len = stream.read_be<uint16_t>();
if (!stream.can_read(opt_len)) {
throw malformed_packet();
}
add_tag(tag(opt_type, opt_len, stream.pointer()));
stream.skip(opt_len);
}
}
}
bool Message::send(Client& client) {
uint32_t variable_header_len = variable_header_length();
uint32_t remaining_length = variable_header_len + payload_length();
uint32_t packet_length = fixed_header_length(remaining_length);
if (_payload_callback == NULL)
packet_length += remaining_length;
else
packet_length += variable_header_len;
uint8_t *packet = new uint8_t[packet_length];
uint32_t pos = 0;
write_fixed_header(packet, pos, remaining_length);
write_variable_header(packet, pos);
write_payload(packet, pos);
uint32_t sent = client.write(const_cast<const uint8_t*>(packet), packet_length);
delete [] packet;
if (sent != packet_length)
return false;
if (_payload_callback != NULL)
return _payload_callback(client);
return true;
}
PPPoE::PPPoE(const uint8_t *buffer, uint32_t total_sz)
: _tags_size()
{
if(total_sz < sizeof(_header))
throw malformed_packet();
std::memcpy(&_header, buffer, sizeof(_header));
buffer += sizeof(_header);
total_sz -= sizeof(_header);
total_sz = std::min(total_sz, (uint32_t)payload_length());
const uint8_t *end = buffer + total_sz;
while(buffer < end) {
if(buffer + sizeof(uint32_t) * 2 > end)
throw malformed_packet();
uint16_t opt_type;
std::memcpy(&opt_type, buffer, sizeof(uint16_t));
uint16_t opt_len;
std::memcpy(&opt_len, buffer + sizeof(uint16_t), sizeof(uint16_t));
buffer += sizeof(uint16_t) * 2;
total_sz -= sizeof(uint16_t) * 2;
if(Endian::be_to_host(opt_len) > total_sz)
throw malformed_packet();
add_tag(
tag(
static_cast<TagTypes>(opt_type),
Endian::be_to_host(opt_len),
buffer
)
);
buffer += Endian::be_to_host(opt_len);
total_sz -= Endian::be_to_host(opt_len);
}
}
void PPPoE::write_serialization(uint8_t* buffer, uint32_t total_sz, const PDU *) {
OutputMemoryStream stream(buffer, total_sz);
if (tags_size_ > 0) {
payload_length(tags_size_);
}
stream.write(header_);
for (tags_type::const_iterator it = tags_.begin(); it != tags_.end(); ++it) {
stream.write<uint16_t>(it->option());
stream.write(Endian::host_to_be<uint16_t>(it->length_field()));
stream.write(it->data_ptr(), it->data_size());
}
}
bool Message::send(Client& client) {
uint32_t remaining_length = variable_header_length() + payload_length();
uint32_t packet_length = fixed_header_length(remaining_length) + remaining_length;
uint8_t *packet = new uint8_t[packet_length];
uint32_t pos = 0;
write_fixed_header(packet, pos, remaining_length);
write_variable_header(packet, pos);
write_payload(packet, pos);
uint32_t sent = client.write(const_cast<const uint8_t*>(packet), packet_length);
delete [] packet;
return (sent == packet_length);
}
static int sick_compute_checksum(uint8_t *data)
{
int payloadlen = payload_length(data);
int crc = 0;
int ab = 0;
for (int i = 0; i < payloadlen + 4; i++) {
ab <<= 8;
ab |= data[i];
crc <<= 1;
if (crc&0x10000)
crc ^= 0x8005;
crc ^= ab;
}
return crc & 0xffff;
}
int sick_get_status(sick_t *s, uint8_t *statbuf, int statbufmax)
{
uint8_t payload[1], request[SICK_MAX_MSG_LENGTH], response[SICK_MAX_MSG_LENGTH];
payload[0] = 0x31;
make_telegram(payload, 1, request);
int res = sick_transaction(s, request, payload[0] | 0x80, response, SICK_OP_TIMEOUT_MS);
if (res < 0)
return res;
int length = payload_length(response);
if (length > statbufmax)
length = statbufmax;
memcpy(statbuf, &response[5], length);
// huh! The telegram doesn't seem to match the documentation!
// sick_display_telegram(response);
return 0;
}
static int packet_length(uint8_t *data)
{
return payload_length(data) + 6;
}
int sick_read_packet(sick_t *s, uint8_t *data)
{
int res;
int datalen = 0; // how many bytes of data[] are valid?
int chk, chk2;
int want;
readmore:
// printf("%i\n", rxlen);
want = 4 - datalen;
if (want > 0) {
// we're willing to wait forever for these bytes
// (this prevents us from spinning in a poll loop
res = read_fully_timeout(s->serialfd, &data[datalen], want, -1);
rxlen += res;
if (res <= 0)
return -1;
datalen += want;
}
// two cases: either the 4 bytes consistute a good header, or
// we skip along to the next occurence of an STX and try
// again.
// is this header good?
int payloadlen = payload_length(data);
if (data[0] != 0x02 || data[1] != 0x80 || payloadlen >= (SICK_MAX_MSG_LENGTH - 6)) {
goto resync;
}
// this header is good. read the message (plus checksum bytes)
want = payloadlen + 6 - datalen;
if (want > 0) {
res = read_fully_timeout(s->serialfd, &data[datalen], want, SICK_RX_TIMEOUT_MS);
rxlen+=res;
if (res <= 0)
return -2;
datalen += want;
}
// is the checksum good?
chk = data[4+payloadlen] + (data[5+payloadlen]<<8);
chk2 = sick_compute_checksum(data);
if (chk != chk2) {
printf("bad chk: %04X != %04X\n", chk, chk2);
goto resync;
}
// good packet received!
if (s->log_packets_file) {
for (int i = 0; i < datalen; i++) {
if (i%16 == 0)
fprintf(s->log_packets_file, "RX %04x : ", i);
fprintf(s->log_packets_file, "%02x ", data[i]);
if ((i%16) == 15 || i+1 == datalen)
fprintf(s->log_packets_file, "\n");
}
}
// is this the response to a request?
pthread_mutex_lock(&s->writelock);
if (s->writedata != NULL && data[4] == s->writereqid) {
memcpy(s->writedata, data, packet_length(data));
s->writevalid = 1;
pthread_cond_signal(&s->writecond);
}
pthread_mutex_unlock(&s->writelock);
// is it a laser scan?
if (data[4] == 0xb0) {
sick_handle_scan_b0(s, data);
} else if (data[4] == 0xf5) {
sick_handle_scan_f5(s, data);
}
return 0;
resync:
for (int i = 1; i < datalen; i++) {
if (data[i] == 0x02) {
memmove(data, &data[i], datalen - i);
datalen = datalen - i;
goto readmore;
}
}
// no STX found, start from scratch
datalen = 0;
goto readmore;
}
