You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 
 

517 lines
13 KiB

/*
Copyright Jeroen Vreeken (jeroen@vreeken.net), 2015
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <dml/dml_packet.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
int dml_packet_send_hello(struct dml_connection *dc, uint32_t flags, char *ident)
{
uint8_t data[4 + strlen(ident)];
data[0] = (flags >> 24) & 0xff;
data[1] = (flags >> 16) & 0xff;
data[2] = (flags >> 8) & 0xff;
data[3] = (flags >> 0) & 0xff;
memcpy(data + 4, ident, strlen(ident));
return dml_connection_send(dc, data, DML_PACKET_HELLO, 4 + strlen(ident));
}
int dml_packet_parse_hello(uint8_t *data, uint16_t len, uint32_t *flags, char **ident)
{
if (len < 4)
return -1;
if (flags)
*flags = (data[0] << 24) + (data[1] << 16) + (data[2] << 8) + data[3];
if (ident) {
*ident = malloc(len - 4 + 1);
if (!*ident)
return -1;
(*ident)[len - 4] = 0;
memcpy(*ident, data + 4, len - 4);
}
return 0;
}
int dml_packet_send_update(struct dml_connection *dc, uint32_t flags)
{
uint8_t data[4];
data[0] = (flags >> 24) & 0xff;
data[1] = (flags >> 16) & 0xff;
data[2] = (flags >> 8) & 0xff;
data[3] = (flags >> 0) & 0xff;
return dml_connection_send(dc, data, DML_PACKET_UPDATE, 4);
}
int dml_packet_parse_update(uint8_t *data, uint16_t len, uint32_t *flags)
{
if (len < 4)
return -1;
if (flags)
*flags = (data[0] << 24) + (data[1] << 16) + (data[2] << 8) + data[3];
return 0;
}
int dml_packet_send_description(struct dml_connection *dc,
uint8_t id[DML_ID_SIZE], uint8_t version, uint32_t bps, char *mime,
char *name, char *alias, char *description)
{
uint8_t data[DML_ID_SIZE + 1 + 4 + strlen(mime) + 1 +
strlen(name) + 1 + strlen(alias) + 1 + strlen(description) + 1];
size_t pos = 0;
if (version != DML_PACKET_DESCRIPTION_VERSION_0)
return -1;
memcpy(data, id, DML_ID_SIZE);
pos += DML_ID_SIZE;
data[pos] = version;
pos++;
data[pos + 0] = (bps >> 24) & 0xff;
data[pos + 1] = (bps >> 16) & 0xff;
data[pos + 2] = (bps >> 8) & 0xff;
data[pos + 3] = (bps >> 0) & 0xff;
pos += 4;
strcpy((char *)data + pos, mime);
pos += strlen(mime) + 1;
strcpy((char *)data + pos, name);
pos += strlen(name) + 1;
strcpy((char *)data + pos, alias);
pos += strlen(alias) + 1;
strcpy((char *)data + pos, description);
pos += strlen(description) + 1;
return dml_connection_send(dc, data, DML_PACKET_DESCRIPTION, pos);
}
int dml_packet_parse_description(uint8_t *data, uint16_t len,
uint8_t id[DML_ID_SIZE], uint8_t *version, uint32_t *bps, char **mime,
char **name, char **alias, char **description)
{
size_t pos_mime, pos_name, pos_alias, pos_description;
size_t size_mime, size_name, size_alias, size_description;
if (len < (DML_ID_SIZE + 1 + 4))
return -1;
*version = data[DML_ID_SIZE];
if (*version != DML_PACKET_DESCRIPTION_VERSION_0)
return -1;
memcpy(id, data, DML_ID_SIZE);
*bps =
(data[DML_ID_SIZE + 1] << 24) |
(data[DML_ID_SIZE + 2] << 16) |
(data[DML_ID_SIZE + 3] << 8) |
(data[DML_ID_SIZE + 4]);
pos_mime = DML_ID_SIZE + 1 + 4;
for (pos_name = pos_mime; pos_name < len && data[pos_name]; pos_name++);
if (pos_name < len - 1)
pos_name++;
for (pos_alias = pos_name; pos_alias < len && data[pos_alias]; pos_alias++);
if (pos_alias < len - 1)
pos_alias++;
for (pos_description = pos_alias; pos_description < len && data[pos_description]; pos_description++);
if (pos_description < len - 1)
pos_description++;
size_mime = pos_name - pos_mime;
size_name = pos_alias - pos_name;
size_alias = pos_description - pos_alias;
size_description = len - pos_description;
if (mime) {
*mime = malloc(size_mime);
if (*mime) {
memcpy(*mime, data + pos_mime, size_mime);
}
}
if (name) {
*name = malloc(size_name);
if (*name) {
memcpy(*name, data + pos_name, size_name);
}
}
if (alias) {
*alias = malloc(size_alias);
if (*alias) {
memcpy(*alias, data + pos_alias, size_alias);
}
}
if (description) {
*description = malloc(size_description);
if (*description) {
memcpy(*description, data + pos_description, size_description);
}
}
return 0;
}
int dml_packet_send_route(struct dml_connection *dc,
uint8_t id[DML_ID_SIZE], uint8_t hops)
{
uint8_t data[DML_ID_SIZE + 1];
memcpy(data, id, DML_ID_SIZE);
data[DML_ID_SIZE] = hops;
return dml_connection_send(dc, data, DML_PACKET_ROUTE, sizeof(data));
}
int dml_packet_parse_route(uint8_t *data, uint16_t len,
uint8_t id[DML_ID_SIZE], uint8_t *hops)
{
if (len < DML_ID_SIZE + 1)
return -1;
memcpy(id, data, DML_ID_SIZE);
if (hops)
*hops = data[DML_ID_SIZE];
return 0;
}
int dml_packet_send_req_description(struct dml_connection *dc,
uint8_t id_req[DML_ID_SIZE])
{
return dml_connection_send(dc, id_req, DML_PACKET_REQ_DESCRIPTION, DML_ID_SIZE);
}
int dml_packet_parse_req_description(uint8_t *data, uint16_t len,
uint8_t id_req[DML_ID_SIZE])
{
if (len < DML_ID_SIZE)
return -1;
memcpy(id_req, data, DML_ID_SIZE);
return 0;
}
int dml_packet_send_req_certificate(struct dml_connection *dc,
uint8_t id_req[DML_ID_SIZE])
{
return dml_connection_send(dc, id_req, DML_PACKET_REQ_CERTIFICATE, DML_ID_SIZE);
}
int dml_packet_parse_req_certificate(uint8_t *data, uint16_t len,
uint8_t id_req[DML_ID_SIZE])
{
if (len < DML_ID_SIZE)
return -1;
memcpy(id_req, data, DML_ID_SIZE);
return 0;
}
int dml_packet_send_req_header(struct dml_connection *dc,
uint8_t id_req[DML_ID_SIZE])
{
return dml_connection_send(dc, id_req, DML_PACKET_REQ_HEADER, DML_ID_SIZE);
}
int dml_packet_parse_req_header(uint8_t *data, uint16_t len,
uint8_t id_req[DML_ID_SIZE])
{
if (len < DML_ID_SIZE)
return -1;
memcpy(id_req, data, DML_ID_SIZE);
return 0;
}
int dml_packet_send_certificate(struct dml_connection *dc,
uint8_t id[DML_ID_SIZE], void *data, size_t len)
{
uint8_t payload[DML_ID_SIZE + len];
memcpy(payload, id, DML_ID_SIZE);
memcpy(payload + DML_ID_SIZE, data, len);
return dml_connection_send(dc, payload, DML_PACKET_CERTIFICATE, DML_ID_SIZE + len);
}
int dml_packet_parse_certificate(uint8_t *data, uint16_t len,
uint8_t id[DML_ID_SIZE], void **cert_data, size_t *cert_len)
{
uint8_t *data8 = data;
size_t plen;
if (len < DML_ID_SIZE)
return -1;
plen = len - DML_ID_SIZE;
memcpy(id, data, DML_ID_SIZE);
*cert_len = plen;
*cert_data = malloc(plen);
if (*cert_data) {
memcpy(*cert_data, data8 + DML_ID_SIZE, plen);
return 0;
}
return -1;
}
int dml_packet_send_header(struct dml_connection *dc,
uint8_t id[DML_ID_SIZE], uint8_t sig[DML_SIG_SIZE], void *data, size_t len)
{
uint8_t payload[DML_ID_SIZE + DML_SIG_SIZE + len];
memcpy(payload, id, DML_ID_SIZE);
memcpy(payload + DML_ID_SIZE, data, len);
memcpy(payload + DML_ID_SIZE + len, sig, DML_SIG_SIZE);
return dml_connection_send(dc, payload, DML_PACKET_HEADER, DML_ID_SIZE + DML_SIG_SIZE + len);
}
int dml_packet_parse_header(uint8_t *data, uint16_t len,
uint8_t id[DML_ID_SIZE], uint8_t sig[DML_SIG_SIZE], void **header_data, size_t *header_len)
{
uint8_t *data8 = data;
size_t plen;
if (len < DML_ID_SIZE + DML_SIG_SIZE)
return -1;
plen = len - DML_ID_SIZE - DML_SIG_SIZE;
memcpy(id, data, DML_ID_SIZE);
memcpy(sig, data8 + DML_ID_SIZE + plen, DML_SIG_SIZE);
*header_len = plen;
*header_data = malloc(plen);
if (*header_data) {
memcpy(*header_data, data8 + DML_ID_SIZE, plen);
return 0;
}
return -1;
}
int dml_packet_send_connect(struct dml_connection *dc,
uint8_t id[DML_ID_SIZE], uint16_t packet_id)
{
uint8_t payload[DML_ID_SIZE + 2];
memcpy(payload, id, DML_ID_SIZE);
payload[DML_ID_SIZE + 0] = (packet_id >> 8) & 0xff;
payload[DML_ID_SIZE + 1] = packet_id & 0xff;
return dml_connection_send(dc, payload, DML_PACKET_CONNECT, DML_ID_SIZE + 2);
}
int dml_packet_parse_connect(uint8_t *data, uint16_t len,
uint8_t id[DML_ID_SIZE], uint16_t *packet_id)
{
if (len < DML_ID_SIZE + 2)
return -1;
memcpy(id, data, DML_ID_SIZE);
*packet_id = (data[DML_ID_SIZE + 0] << 8) | data[DML_ID_SIZE + 1];
return 0;
}
int dml_packet_send_disc(struct dml_connection *dc,
uint8_t id[DML_ID_SIZE], uint8_t reason)
{
uint8_t payload[DML_ID_SIZE + 1];
memcpy(payload, id, DML_ID_SIZE);
payload[DML_ID_SIZE] = reason;
return dml_connection_send(dc, payload, DML_PACKET_DISC, DML_ID_SIZE + 1);
}
int dml_packet_parse_disc(uint8_t *data, uint16_t len,
uint8_t id[DML_ID_SIZE], uint8_t *reason)
{
if (len < DML_ID_SIZE + 1)
return -1;
memcpy(id, data, DML_ID_SIZE);
*reason = data[DML_ID_SIZE + 0];
return 0;
}
int dml_packet_send_req_disc(struct dml_connection *dc,
uint8_t id[DML_ID_SIZE])
{
return dml_connection_send(dc, id, DML_PACKET_REQ_DISC, DML_ID_SIZE);
}
int dml_packet_parse_req_disc(uint8_t *data, uint16_t len,
uint8_t id[DML_ID_SIZE])
{
if (len < DML_ID_SIZE)
return -1;
memcpy(id, data, DML_ID_SIZE);
return 0;
}
int dml_packet_send_req_reverse(struct dml_connection *dc,
uint8_t id[DML_ID_SIZE], uint8_t rev_id[DML_ID_SIZE], uint8_t action, uint16_t status)
{
uint8_t payload[DML_ID_SIZE + DML_ID_SIZE + 1 + 2];
memcpy(payload, id, DML_ID_SIZE);
memcpy(payload + DML_ID_SIZE, rev_id, DML_ID_SIZE);
payload[DML_ID_SIZE + DML_ID_SIZE] = action;
payload[DML_ID_SIZE + DML_ID_SIZE + 1] = (status >> 8) & 0xff;
payload[DML_ID_SIZE + DML_ID_SIZE + 2] = (status >> 0) & 0xff;
return dml_connection_send(dc, payload, DML_PACKET_REQ_REVERSE, DML_ID_SIZE + DML_ID_SIZE + 1 + 2);
}
int dml_packet_parse_req_reverse(uint8_t *data, uint16_t len,
uint8_t id[DML_ID_SIZE], uint8_t rev_id[DML_ID_SIZE], uint8_t *action, uint16_t *status)
{
if (len < DML_ID_SIZE + DML_ID_SIZE + 1)
return -1;
memcpy(id, data, DML_ID_SIZE);
memcpy(rev_id, data + DML_ID_SIZE, DML_ID_SIZE);
*action = data[DML_ID_SIZE + DML_ID_SIZE];
if (*status) {
if (len >= (DML_ID_SIZE + DML_ID_SIZE + 1 + 2)) {
*status =
(data[DML_ID_SIZE + DML_ID_SIZE + 1] << 8) |
(data[DML_ID_SIZE + DML_ID_SIZE + 2] << 0);
} else
*status = 0;
}
return 0;
}
int dml_packet_send_data(struct dml_connection *dc,
uint16_t packet_id, void *data, size_t len, uint64_t timestamp,
struct dml_crypto_key *dk)
{
uint8_t payload[len + DML_SIG_SIZE + sizeof(uint64_t)];
memcpy(payload, data, len);
payload[len + 0] = (timestamp >> 56) & 0xff;
payload[len + 1] = (timestamp >> 48) & 0xff;
payload[len + 2] = (timestamp >> 40) & 0xff;
payload[len + 3] = (timestamp >> 32) & 0xff;
payload[len + 4] = (timestamp >> 24) & 0xff;
payload[len + 5] = (timestamp >> 16) & 0xff;
payload[len + 6] = (timestamp >> 8) & 0xff;
payload[len + 7] = (timestamp) & 0xff;
dml_crypto_sign(payload + len + sizeof(uint64_t),
payload, len + sizeof(uint64_t), dk);
return dml_connection_send(dc, payload, packet_id, len + DML_SIG_SIZE + sizeof(uint64_t));;
}
int dml_packet_parse_data(uint8_t *data, uint16_t len,
void **payload_data, size_t *payload_len, uint64_t *timestamp,
struct dml_crypto_key *dk)
{
if (len < DML_SIG_SIZE + sizeof(uint64_t))
return -1;
size_t plen = len - DML_SIG_SIZE - sizeof(uint64_t);
*payload_len = plen;
bool verified = dml_crypto_verify(data, plen + sizeof(uint64_t),
data + plen + sizeof(uint64_t), dk);
*payload_data = data;
*timestamp =
((uint64_t)data[plen + 0] << 56) |
((uint64_t)data[plen + 1] << 48) |
((uint64_t)data[plen + 2] << 40) |
((uint64_t)data[plen + 3] << 32) |
((uint64_t)data[plen + 4] << 24) |
((uint64_t)data[plen + 5] << 16) |
((uint64_t)data[plen + 6] << 8) |
((uint64_t)data[plen + 7]);
if (!verified) {
int i;
fprintf(stderr, "invalid signature\n");
for (i = 0; i < plen; i++) {
fprintf(stderr, "%02x ", data[i]);
}
fprintf(stderr, "\n");
for (i = 0; i < sizeof(uint64_t); i++) {
fprintf(stderr, "%02x ", data[plen + i]);
}
fprintf(stderr, "\n");
for (i = 0; i < 256/8; i++) {
fprintf(stderr, "%02x ", data[plen + sizeof(uint64_t) + i]);
}
fprintf(stderr, "\n");
for (i = 0; i < 256/8; i++) {
fprintf(stderr, "%02x ", data[plen + sizeof(uint64_t) +256/8 + i]);
}
fprintf(stderr, "\n");
return -1;
}
return 0;
}
int dml_packet_parse_data_unverified(uint8_t *data, uint16_t len,
void **payload_data, size_t *payload_len, uint64_t *timestamp)
{
if (len < DML_SIG_SIZE + sizeof(uint64_t))
return -1;
size_t plen = len - DML_SIG_SIZE - sizeof(uint64_t);
*payload_len = plen;
*payload_data = data;
*timestamp =
((uint64_t)data[plen + 0] << 56) |
((uint64_t)data[plen + 1] << 48) |
((uint64_t)data[plen + 2] << 40) |
((uint64_t)data[plen + 3] << 32) |
((uint64_t)data[plen + 4] << 24) |
((uint64_t)data[plen + 5] << 16) |
((uint64_t)data[plen + 6] << 8) |
((uint64_t)data[plen + 7]);
return 0;
}