static void port_rx(const TBeaconNgTracker* pkt_encrypted, int rssi)
{
int proto, i;
int status;
TBeaconNgTracker pkt;
time_t tloc;
/* decrypt valid packet */
if((status = aes_decr(pkt_encrypted, &pkt, sizeof(pkt), CONFIG_SIGNATURE_SIZE))!=0)
{
fprintf(stderr, "error: failed decrypting packet with error [%i]\n", status);
return;
}
/* white-list supported protocols */
proto = pkt.proto & RFBPROTO_PROTO_MASK;
if(!((proto == 30) || (proto == 31)))
{
fprintf(stderr, "error: uknnown protocol %i\n", proto);
return;
}
time(&tloc);
fprintf(stdout,
"{ \"uid\":\"0x%08X\", \"time_local_s\":%8u, \"time_remote_s\":%8u, \"rssi\":%3i, \"angle\":%3i, \"voltage\":%3.1f, \"tx_power\":%i",
pkt.uid,
(int)tloc,
pkt.epoch,
rssi,
pkt.angle,
pkt.voltage / 10.0,
pkt.tx_power);
/* optionally decode button */
if(pkt.proto & RFBPROTO_PROTO_BUTTON)
fprintf(stdout,", \"button\": 1");
switch(proto)
{
case 30:
if(pkt.p.sighting[0].uid)
{
fprintf(stdout,", \"sighting\": ");
for(i=0; i<CONFIG_SIGHTING_SLOTS; i++)
if(pkt.p.sighting[i].uid)
fprintf(stdout, "%c{\"uid\":\"0x%08X\",\"rssi\":%i}",
i ? ',':'[',
pkt.p.sighting[i].uid,
pkt.p.sighting[i].rx_power
);
fprintf(stdout,"]");
}
break;
}
fprintf(stdout, "}\n\r");
fflush(stdout);
}
int srtp_decrypt(struct srtp *srtp, struct mbuf *mb)
{
struct srtp_stream *strm;
struct rtp_header hdr;
struct comp *comp;
uint64_t ix;
size_t start;
int diff;
int err;
if (!srtp || !mb)
return EINVAL;
comp = &srtp->rtp;
start = mb->pos;
err = rtp_hdr_decode(&hdr, mb);
if (err)
return err;
err = stream_get_seq(&strm, srtp, hdr.ssrc, hdr.seq);
if (err)
return err;
diff = seq_diff(strm->s_l, hdr.seq);
if (diff > 32768)
return ETIMEDOUT;
/* Roll-Over Counter (ROC) */
if (diff <= -32768) {
strm->roc++;
strm->s_l = 0;
}
ix = srtp_get_index(strm->roc, strm->s_l, hdr.seq);
if (comp->hmac) {
uint8_t tag_calc[SHA_DIGEST_LENGTH];
uint8_t tag_pkt[SHA_DIGEST_LENGTH];
size_t pld_start, tag_start;
if (mbuf_get_left(mb) < comp->tag_len)
return EBADMSG;
pld_start = mb->pos;
tag_start = mb->end - comp->tag_len;
mb->pos = tag_start;
err = mbuf_read_mem(mb, tag_pkt, comp->tag_len);
if (err)
return err;
mb->pos = mb->end = tag_start;
err = mbuf_write_u32(mb, htonl(strm->roc));
if (err)
return err;
mb->pos = start;
err = hmac_digest(comp->hmac, tag_calc, sizeof(tag_calc),
mbuf_buf(mb), mbuf_get_left(mb));
if (err)
return err;
mb->pos = pld_start;
mb->end = tag_start;
if (0 != memcmp(tag_calc, tag_pkt, comp->tag_len))
return EAUTH;
/*
* 3.3.2. Replay Protection
*
* Secure replay protection is only possible when
* integrity protection is present.
*/
if (!srtp_replay_check(&strm->replay_rtp, ix))
return EALREADY;
}
if (comp->aes) {
union vect128 iv;
uint8_t *p = mbuf_buf(mb);
srtp_iv_calc(&iv, &comp->k_s, strm->ssrc, ix);
aes_set_iv(comp->aes, iv.u8);
err = aes_decr(comp->aes, p, p, mbuf_get_left(mb));
if (err)
return err;
}
if (hdr.seq > strm->s_l)
strm->s_l = hdr.seq;
mb->pos = start;
return 0;
}
