static int netdev_send(struct vport *vport, struct sk_buff *skb)
{
struct netdev_vport *netdev_vport = netdev_vport_priv(vport);
int mtu = netdev_vport->dev->mtu;
int len;
if (unlikely(packet_length(skb) > mtu && !skb_is_gso(skb))) {
net_warn_ratelimited("%s: dropped over-mtu packet: %d > %d\n",
ovs_dp_name(vport->dp),
packet_length(skb), mtu);
goto error;
}
if (unlikely(skb_warn_if_lro(skb)))
goto error;
skb->dev = netdev_vport->dev;
len = skb->len;
dev_queue_xmit(skb);
return len;
error:
kfree_skb(skb);
ovs_vport_record_error(vport, VPORT_E_TX_DROPPED);
return 0;
}
uint16_t SNMPClass::writeHeaders(SNMP_PDU *pdu)
{
byte i;
// SNMP community string
if(_dstType == SNMP_PDU_SET){
for(i = _setSize-1; i >= 0 && i <= 30; i--){
_packet[_packetPos--] = (byte)_setCommName[i];
}
_packet[_packetPos--] = (byte)_setSize; // length
}else if(_dstType == SNMP_PDU_TRAP || _dstType == SNMP_PDU_TRAP2 || _dstType == SNMP_PDU_INFORM_REQUEST){
for(i = _trapSize-1; i >= 0 && i <= 30; i--){
_packet[_packetPos--] = (byte)_trapCommName[i];
}
_packet[_packetPos--] = (byte)_trapSize; // length
}else {
for(i = _getSize-1; i >= 0 && i <= 30; i--){
_packet[_packetPos--] = (byte)_getCommName[i];
}
_packet[_packetPos--] = (byte)_getSize; // length
}
_packet[_packetPos--] = (byte)SNMP_SYNTAX_OCTETS;// type
// version
_packet[_packetPos--] = (byte)pdu->version;// value
_packet[_packetPos--] = 0x01; // length
_packet[_packetPos--] = (byte)SNMP_SYNTAX_INT;//type
//start of header
//length of all data after this point
_packet[_packetPos] = lsb(packet_length()+_extra_data_size);
_packet[_packetPos-1] = msb(packet_length()+_extra_data_size);
_packetPos -= 2;
_packet[_packetPos--] = 0x82;//Sending length in two octets
_packet[_packetPos--] = (byte)SNMP_SYNTAX_SEQUENCE;
//packet type
if ( _dstType == SNMP_PDU_SET ) {
_packetSize += _setSize;
} else if ( _dstType == SNMP_PDU_TRAP ) {
_packetSize += _trapSize;
} else {
_packetSize += _getSize;
}
return _packetPos;
}
int packet_read(int fd, char **src_buf, size_t *src_len,
char *buffer, unsigned size, int options)
{
int len, ret;
char linelen[4];
ret = get_packet_data(fd, src_buf, src_len, linelen, 4, options);
if (ret < 0)
return ret;
len = packet_length(linelen);
if (len < 0)
die("protocol error: bad line length character: %.4s", linelen);
if (!len) {
packet_trace("0000", 4, 0);
return 0;
}
len -= 4;
if (len >= size)
die("protocol error: bad line length %d", len);
ret = get_packet_data(fd, src_buf, src_len, buffer, len, options);
if (ret < 0)
return ret;
if ((options & PACKET_READ_CHOMP_NEWLINE) &&
len && buffer[len-1] == '\n')
len--;
buffer[len] = 0;
packet_trace(buffer, len, 0);
return len;
}
int packet_get_line(struct strbuf *out,
char **src_buf, size_t *src_len)
{
int len;
if (*src_len < 4)
return -1;
len = packet_length(*src_buf);
if (len < 0)
return -1;
if (!len) {
*src_buf += 4;
*src_len -= 4;
packet_trace("0000", 4, 0);
return 0;
}
if (*src_len < len)
return -2;
*src_buf += 4;
*src_len -= 4;
len -= 4;
strbuf_add(out, *src_buf, len);
*src_buf += len;
*src_len -= len;
packet_trace(out->buf, out->len, 0);
return len;
}
int sick_transaction(sick_t *s, uint8_t *request, uint8_t responsetype, uint8_t *response, int timeoutms)
{
pthread_mutex_lock(&s->writelock);
int res = 0;
int trials = 0;
int requestlen;
retry:
requestlen = packet_length(request);
if (s->log_packets_file) {
for (int i = 0; i < requestlen; i++) {
if (i%16 == 0)
fprintf(s->log_packets_file, "TX %04x : ", i);
fprintf(s->log_packets_file, "%02x ", request[i]);
if ((i%16) == 15 || i+1 == requestlen)
fprintf(s->log_packets_file, "\n");
}
}
// send the request
res = write_fully(s->serialfd, request, requestlen);
if (res < 0) {
printf("write failed\n");
goto exit;
}
// wait for response
s->writereqid = responsetype;
s->writedata = response;
s->writevalid = 0;
struct timespec ts;
timespec_now(&ts);
timespec_addms(&ts, timeoutms);
pthread_cond_timedwait(&s->writecond, &s->writelock, &ts);
if (!s->writevalid) {
trials++;
if (trials > SICK_OP_MAX_RETRIES) {
res = -2;
goto exit;
}
goto retry;
}
exit:
s->writedata = NULL;
s->writereqid = 0;
pthread_mutex_unlock(&s->writelock);
return res;
}
void ovs_vport_send(struct vport *vport, struct sk_buff *skb)
{
int mtu = vport->dev->mtu;
if (unlikely(packet_length(skb) > mtu && !skb_is_gso(skb))) {
net_warn_ratelimited("%s: dropped over-mtu packet: %d > %d\n",
vport->dev->name,
packet_length(skb), mtu);
vport->dev->stats.tx_errors++;
goto drop;
}
skb->dev = vport->dev;
vport->ops->send(skb);
return;
drop:
kfree_skb(skb);
}
/* callbacks */
void stdin_cb(evutil_socket_t sock, short ev, void *arg) {
UNUSED(ev);
UNUSED(arg);
static ssize_t len = 0, r = 0;
struct proto_packet *packet = sendbuff_back(&packets);
struct proto_header *header = &packet->header;
TRY_SYS(r = read(sock, packet->data + len, psize - len));
if (r) {
len += r;
if (len == psize) {
/* packet is full, init */
header_init(header, last_seqno++, len, PROTO_DATA);
ASSERT(psize + (int) sizeof(struct proto_header) == packets.hpsize);
ASSERT(packets.hpsize == (int) packet_length(packet));
/* send */
EXPECT(write(mcast_sock, packet, packets.hpsize) == packets.hpsize,
"Sending streming data failed.");
/* start new packet */
len = 0;
sendbuff_next(&packets);
}
} else {
if (len) {
/* end of input, init packet as is */
header_init(header, last_seqno++, len, PROTO_DATA);
/* send */
ASSERT(len + sizeof(struct proto_header));
len = packet_length(packet);
EXPECT(write(mcast_sock, packet, len) == len,
"Sending streaming data failed.");
/* everything sent */
len = 0;
}
/* end of input file, exit */
event_base_loopexit(base, NULL);
}
}
void ovs_vport_send(struct vport *vport, struct sk_buff *skb, u8 mac_proto)
{
int mtu = vport->dev->mtu;
switch (vport->dev->type) {
case ARPHRD_NONE:
if (mac_proto == MAC_PROTO_ETHERNET) {
skb_reset_network_header(skb);
skb_reset_mac_len(skb);
skb->protocol = htons(ETH_P_TEB);
} else if (mac_proto != MAC_PROTO_NONE) {
WARN_ON_ONCE(1);
goto drop;
}
break;
case ARPHRD_ETHER:
if (mac_proto != MAC_PROTO_ETHERNET)
goto drop;
break;
default:
goto drop;
}
if (unlikely(packet_length(skb, vport->dev) > mtu &&
!skb_is_gso(skb))) {
net_warn_ratelimited("%s: dropped over-mtu packet: %d > %d\n",
vport->dev->name,
packet_length(skb, vport->dev), mtu);
vport->dev->stats.tx_errors++;
goto drop;
}
skb->dev = vport->dev;
vport->ops->send(skb);
return;
drop:
kfree_skb(skb);
}
static int netdev_send(struct vport *vport, struct sk_buff *skb)
{
struct netdev_vport *netdev_vport = netdev_vport_priv(vport);
int mtu = netdev_vport->dev->mtu;
int len;
if (unlikely(packet_length(skb) > mtu && !skb_is_gso(skb))) {
net_warn_ratelimited("%s: dropped over-mtu packet: %d > %d\n",
netdev_vport->dev->name,
packet_length(skb), mtu);
goto drop;
}
skb->dev = netdev_vport->dev;
len = skb->len;
dev_queue_xmit(skb);
return len;
drop:
kfree_skb(skb);
return 0;
}
int vnsw_dev_queue_push_xmit(struct sk_buff *skb)
{
/* drop mtu oversized packets except gso */
if (packet_length(skb) > skb->dev->mtu && !skb_is_gso(skb))
kfree_skb(skb);
else {
/* ip_fragment doesn't copy the MAC header */
if (nf_bridge_maybe_copy_header(skb))
kfree_skb(skb);
else {
skb_push(skb, ETH_HLEN);
dev_queue_xmit(skb);
}
}
return 0;
}
int sick_get_type(sick_t *s, char *buf, int bufmax)
{
uint8_t payload[1], request[SICK_MAX_MSG_LENGTH], response[SICK_MAX_MSG_LENGTH];
payload[0] = 0x3a;
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 typelen = packet_length(response) - 10;
if (typelen > bufmax-1)
typelen = bufmax - 1;
memcpy(buf, &response[5], typelen);
buf[typelen] = 0;
return 0;
}
int packet_read_line(int fd, char *buffer, unsigned size)
{
int len;
char linelen[4];
safe_read(fd, linelen, 4);
len = packet_length(linelen);
if (len < 0)
die("protocol error: bad line length character: %.4s", linelen);
if (!len) {
packet_trace("0000", 4, 0);
return 0;
}
len -= 4;
if (len >= size)
die("protocol error: bad line length %d", len);
safe_read(fd, buffer, len);
buffer[len] = 0;
packet_trace(buffer, len, 0);
return len;
}
void sick_display_telegram(unsigned char *b)
{
if (b[0]!=0x02) {
printf("invalid telegram\n");
return;
}
if (b[4]==0x92) {
printf("NACK/Incorrect command\n");
}
int packetlength = packet_length(b);
for (int i = 0; i < packetlength; i++) {
if ((i%16)==0)
printf("%04X : ",i);
printf("%02X ", b[i]);
if ((i%16)==15)
printf("\n");
}
printf("\n");
}
/* function: ipv6_packet
* takes an ipv6 packet and hands it off to the layer 4 protocol function
* out - output packet
* packet - packet data
* len - size of packet
* returns: the highest position in the output clat_packet that's filled in
*/
int ipv6_packet(clat_packet out, clat_packet_index pos, const uint8_t *packet, size_t len) {
const struct ip6_hdr *ip6 = (struct ip6_hdr *) packet;
struct iphdr *ip_targ = (struct iphdr *) out[pos].iov_base;
struct ip6_frag *frag_hdr = NULL;
uint8_t protocol;
const uint8_t *next_header;
size_t len_left;
uint32_t old_sum, new_sum;
int iov_len;
if(len < sizeof(struct ip6_hdr)) {
logmsg_dbg(ANDROID_LOG_ERROR, "ipv6_packet/too short for an ip6 header: %d", len);
return 0;
}
if(IN6_IS_ADDR_MULTICAST(&ip6->ip6_dst)) {
log_bad_address("ipv6_packet/multicast %s->%s", &ip6->ip6_src, &ip6->ip6_dst);
return 0; // silently ignore
}
// If the packet is not from the plat subnet to the local subnet, or vice versa, drop it, unless
// it's an ICMP packet (which can come from anywhere). We do not send IPv6 packets from the plat
// subnet to the local subnet, but these can appear as inner packets in ICMP errors, so we need
// to translate them. We accept third-party ICMPv6 errors, even though their source addresses
// cannot be translated, so that things like unreachables and traceroute will work. fill_ip_header
// takes care of faking a source address for them.
if (!(is_in_plat_subnet(&ip6->ip6_src) &&
IN6_ARE_ADDR_EQUAL(&ip6->ip6_dst, &Global_Clatd_Config.ipv6_local_subnet)) &&
!(is_in_plat_subnet(&ip6->ip6_dst) &&
IN6_ARE_ADDR_EQUAL(&ip6->ip6_src, &Global_Clatd_Config.ipv6_local_subnet)) &&
ip6->ip6_nxt != IPPROTO_ICMPV6) {
log_bad_address("ipv6_packet/wrong source address: %s->%s", &ip6->ip6_src, &ip6->ip6_dst);
return 0;
}
next_header = packet + sizeof(struct ip6_hdr);
len_left = len - sizeof(struct ip6_hdr);
protocol = ip6->ip6_nxt;
/* Fill in the IPv4 header. We need to do this before we translate the packet because TCP and
* UDP include parts of the IP header in the checksum. Set the length to zero because we don't
* know it yet.
*/
fill_ip_header(ip_targ, 0, protocol, ip6);
out[pos].iov_len = sizeof(struct iphdr);
// If there's a Fragment header, parse it and decide what the next header is.
// Do this before calculating the pseudo-header checksum because it updates the next header value.
if (protocol == IPPROTO_FRAGMENT) {
frag_hdr = (struct ip6_frag *) next_header;
if (len_left < sizeof(*frag_hdr)) {
logmsg_dbg(ANDROID_LOG_ERROR, "ipv6_packet/too short for fragment header: %d", len);
return 0;
}
next_header += sizeof(*frag_hdr);
len_left -= sizeof(*frag_hdr);
protocol = parse_frag_header(frag_hdr, ip_targ);
}
// ICMP and ICMPv6 have different protocol numbers.
if (protocol == IPPROTO_ICMPV6) {
protocol = IPPROTO_ICMP;
ip_targ->protocol = IPPROTO_ICMP;
}
/* Calculate the pseudo-header checksum.
* Technically, the length that is used in the pseudo-header checksum is the transport layer
* length, which is not the same as len_left in the case of fragmented packets. But since
* translation does not change the transport layer length, the checksum is unaffected.
*/
old_sum = ipv6_pseudo_header_checksum(ip6, len_left, protocol);
new_sum = ipv4_pseudo_header_checksum(ip_targ, len_left);
// Does not support IPv6 extension headers except Fragment.
if (frag_hdr && (frag_hdr->ip6f_offlg & IP6F_OFF_MASK)) {
iov_len = generic_packet(out, pos + 2, next_header, len_left);
} else if (protocol == IPPROTO_ICMP) {
iov_len = icmp6_packet(out, pos + 2, (const struct icmp6_hdr *) next_header, len_left);
} else if (protocol == IPPROTO_TCP) {
iov_len = tcp_packet(out, pos + 2, (const struct tcphdr *) next_header, old_sum, new_sum,
len_left);
} else if (protocol == IPPROTO_UDP) {
iov_len = udp_packet(out, pos + 2, (const struct udphdr *) next_header, old_sum, new_sum,
len_left);
} else if (protocol == IPPROTO_GRE) {
iov_len = generic_packet(out, pos + 2, next_header, len_left);
} else {
#if CLAT_DEBUG
logmsg(ANDROID_LOG_ERROR, "ipv6_packet/unknown next header type: %x", ip6->ip6_nxt);
logcat_hexdump("ipv6/nxthdr", packet, len);
#endif
return 0;
}
// Set the length and calculate the checksum.
ip_targ->tot_len = htons(ntohs(ip_targ->tot_len) + packet_length(out, pos));
ip_targ->check = ip_checksum(ip_targ, sizeof(struct iphdr));
return iov_len;
}
static int netdev_send(struct vport *vport, struct sk_buff *skb)
{
struct netdev_vport *netdev_vport = netdev_vport_priv(vport);
int mtu = netdev_vport->dev->mtu;
int len;
if (unlikely(packet_length(skb) > mtu && !skb_is_gso(skb))) {
if (net_ratelimit())
pr_warn("%s: dropped over-mtu packet: %d > %d\n",
ovs_dp_name(vport->dp), packet_length(skb), mtu);
goto error;
}
if (unlikely(skb_warn_if_lro(skb)))
goto error;
skb->dev = netdev_vport->dev;
forward_ip_summed(skb, true);
if (vlan_tx_tag_present(skb) && !dev_supports_vlan_tx(skb->dev)) {
int features;
features = netif_skb_features(skb);
if (!vlan_tso)
features &= ~(NETIF_F_TSO | NETIF_F_TSO6 |
NETIF_F_UFO | NETIF_F_FSO);
if (netif_needs_gso(skb, features)) {
struct sk_buff *nskb;
nskb = skb_gso_segment(skb, features);
if (!nskb) {
if (unlikely(skb_cloned(skb) &&
pskb_expand_head(skb, 0, 0, GFP_ATOMIC))) {
kfree_skb(skb);
return 0;
}
skb_shinfo(skb)->gso_type &= ~SKB_GSO_DODGY;
goto tag;
}
if (IS_ERR(nskb)) {
kfree_skb(skb);
return 0;
}
consume_skb(skb);
skb = nskb;
len = 0;
do {
nskb = skb->next;
skb->next = NULL;
skb = __vlan_put_tag(skb, vlan_tx_tag_get(skb));
if (likely(skb)) {
len += skb->len;
vlan_set_tci(skb, 0);
dev_queue_xmit(skb);
}
skb = nskb;
} while (skb);
return len;
}
tag:
skb = __vlan_put_tag(skb, vlan_tx_tag_get(skb));
if (unlikely(!skb))
return 0;
vlan_set_tci(skb, 0);
}
len = skb->len;
dev_queue_xmit(skb);
return len;
error:
kfree_skb(skb);
ovs_vport_record_error(vport, VPORT_E_TX_DROPPED);
return 0;
}
uint32_t SNMPClass::send_message(SNMP_PDU *pdu, IPAddress to_address, uint16_t to_port, byte *temp_buff, char *extra_data)
{
memset(_packet, 0, SNMP_MAX_PACKET_LEN);
_packetPos = SNMP_MAX_PACKET_LEN-1;
int32_u u;
int t = 0;
_extra_data_size = 0;
if(extra_data != NULL){
_extra_data_size = strlen(extra_data);
}
// Varbind List
if(pdu->type == SNMP_PDU_RESPONSE){
t = pdu->add_data_private(&pdu->value,_packet + _packetPos,true,temp_buff, _extra_data_size);
_packetPos -= t;
//length of entire value being passed
_packet[_packetPos--] = lsb(t+_extra_data_size);
_packet[_packetPos--] = msb(t+_extra_data_size);
_packet[_packetPos--] = 0x82;//Sending length in two octets
}else if(pdu->type == SNMP_PDU_TRAP2 || pdu->type == SNMP_PDU_INFORM_REQUEST){
//set and increment requestId
pdu->requestId = requestCounter++;
for (t = pdu->value.size-1; t >= 0; t-- ) {
_packet[_packetPos--] = pdu->value.data[t];
}
//length of entire value being passed
_packet[_packetPos--] = lsb(pdu->value.size);
_packet[_packetPos--] = msb(pdu->value.size);
_packet[_packetPos--] = 0x82;//Sending length in two octets
}
_packet[_packetPos--] = (byte)SNMP_SYNTAX_SEQUENCE;// type
// Error Index (size always 4 e.g. 4-byte int)
u.int32 = pdu->errorIndex;
_packet[_packetPos--] = u.data[0];
_packet[_packetPos--] = u.data[1];
_packet[_packetPos--] = u.data[2];
_packet[_packetPos--] = u.data[3];
_packet[_packetPos--] = 0x04;
_packet[_packetPos--] = (byte)SNMP_SYNTAX_INT;// type
// Error (size always 4 e.g. 4-byte int)
u.int32 = pdu->error;
_packet[_packetPos--] = u.data[0];
_packet[_packetPos--] = u.data[1];
_packet[_packetPos--] = u.data[2];
_packet[_packetPos--] = u.data[3];
_packet[_packetPos--] = 0x04;
_packet[_packetPos--] = (byte)SNMP_SYNTAX_INT;// type
// Request ID (size always 4 e.g. 4-byte int)
u.int32 = pdu->requestId;
_packet[_packetPos--] = u.data[0];
_packet[_packetPos--] = u.data[1];
_packet[_packetPos--] = u.data[2];
_packet[_packetPos--] = u.data[3];
_packet[_packetPos--] = 0x04;
_packet[_packetPos--] = (byte)SNMP_SYNTAX_INT;// type
//length value of all previous data
_packet[_packetPos] = lsb(packet_length()+_extra_data_size);
_packet[_packetPos-1] = msb(packet_length()+_extra_data_size);
_packetPos -= 2;
_packet[_packetPos--] = 0x82;//Sending length in two octets
// SNMP PDU type
_packet[_packetPos--] = (byte)pdu->type;
//data needed for header
_dstType = pdu->type;
//byte header_size = this->writeHeaders(pdu);
this->writeHeaders(pdu);
_packetSize = packet_length();
// Serial.println("Outgoing: ");
// for(byte i = 0; i < _packetSize; i++){
// Serial.print(_packet[_packetPos+1+i],HEX);
// Serial.print("-");
// }
// Serial.println();
this->writePacket(to_address, to_port, extra_data);
return pdu->requestId;
}
/* function: ipv4_packet
* translates an ipv4 packet
* out - output packet
* packet - packet data
* len - size of packet
* returns: the highest position in the output clat_packet that's filled in
*/
int ipv4_packet(clat_packet out, int pos, const char *packet, size_t len) {
const struct iphdr *header = (struct iphdr *) packet;
struct ip6_hdr *ip6_targ = (struct ip6_hdr *) out[pos].iov_base;
uint16_t frag_flags;
uint8_t nxthdr;
const char *next_header;
size_t len_left;
uint32_t checksum;
int iov_len;
if(len < sizeof(struct iphdr)) {
logmsg_dbg(ANDROID_LOG_ERROR, "ip_packet/too short for an ip header");
return 0;
}
frag_flags = ntohs(header->frag_off);
if(frag_flags & IP_MF) { // this could theoretically be supported, but isn't
logmsg_dbg(ANDROID_LOG_ERROR, "ip_packet/more fragments set, dropping");
return 0;
}
if(header->ihl < 5) {
logmsg_dbg(ANDROID_LOG_ERROR, "ip_packet/ip header length set to less than 5: %x", header->ihl);
return 0;
}
if((size_t) header->ihl * 4 > len) { // ip header length larger than entire packet
logmsg_dbg(ANDROID_LOG_ERROR, "ip_packet/ip header length set too large: %x", header->ihl);
return 0;
}
if(header->version != 4) {
logmsg_dbg(ANDROID_LOG_ERROR, "ip_packet/ip header version not 4: %x", header->version);
return 0;
}
/* rfc6145 - If any IPv4 options are present in the IPv4 packet, they MUST be
* ignored and the packet translated normally; there is no attempt to
* translate the options.
*/
next_header = packet + header->ihl*4;
len_left = len - header->ihl * 4;
nxthdr = header->protocol;
if (nxthdr == IPPROTO_ICMP) {
// ICMP and ICMPv6 have different protocol numbers.
nxthdr = IPPROTO_ICMPV6;
}
/* Fill in the IPv6 header. We need to do this before we translate the packet because TCP and
* UDP include parts of the IP header in the checksum. Set the length to zero because we don't
* know it yet.
*/
fill_ip6_header(ip6_targ, 0, nxthdr, header);
out[pos].iov_len = sizeof(struct ip6_hdr);
// Calculate the pseudo-header checksum.
checksum = ipv6_pseudo_header_checksum(0, ip6_targ, len_left);
if (nxthdr == IPPROTO_ICMPV6) {
iov_len = icmp_packet(out, pos + 1, (const struct icmphdr *) next_header, checksum, len_left);
} else if (nxthdr == IPPROTO_TCP) {
iov_len = tcp_packet(out, pos + 1, (const struct tcphdr *) next_header, checksum, len_left);
} else if (nxthdr == IPPROTO_UDP) {
iov_len = udp_packet(out, pos + 1, (const struct udphdr *) next_header, checksum, len_left);
} else if (nxthdr == IPPROTO_GRE) {
iov_len = generic_packet(out, pos + 1, next_header, len_left);
} else {
#if CLAT_DEBUG
logmsg_dbg(ANDROID_LOG_ERROR, "ip_packet/unknown protocol: %x",header->protocol);
logcat_hexdump("ipv4/protocol", packet, len);
#endif
return 0;
}
// Set the length.
ip6_targ->ip6_plen = htons(packet_length(out, pos));
return iov_len;
}
static inline uint16_t packet_dsize(struct packet_t * packet)
{
return packet_length(packet) - 5;
}
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;
}
