mirror of
https://github.com/servalproject/serval-dna.git
synced 2024-12-24 15:26:43 +00:00
329 lines
7.9 KiB
C
329 lines
7.9 KiB
C
/*
|
|
Serval Mesh
|
|
Copyright (C) 2013 Alexandra Sclapari
|
|
Copyright (C) 2013 Paul Gardner-Stephen
|
|
|
|
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 2
|
|
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, write to the Free Software
|
|
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
|
|
*/
|
|
|
|
#include "serval.h"
|
|
|
|
void hex_dump(unsigned char *data, int size)
|
|
{
|
|
int i; // index in data...
|
|
int j; // index in line...
|
|
char temp[8];
|
|
char buffer[128];
|
|
char *ascii;
|
|
|
|
memset(buffer, 0, 128);
|
|
|
|
printf("---------> Dump <--------- (%d bytes from %p)\n", size, data);
|
|
|
|
// Printing the ruler...
|
|
printf(" +0 +4 +8 +c 0 4 8 c \n");
|
|
|
|
// Hex portion of the line is 8 (the padding) + 3 * 16 = 52 chars long
|
|
// We add another four bytes padding and place the ASCII version...
|
|
ascii = buffer + 58;
|
|
memset(buffer, ' ', 58 + 16);
|
|
buffer[58 + 16] = '\n';
|
|
buffer[58 + 17] = '\0';
|
|
buffer[0] = '+';
|
|
buffer[1] = '0';
|
|
buffer[2] = '0';
|
|
buffer[3] = '0';
|
|
buffer[4] = '0';
|
|
for (i = 0, j = 0; i < size; i++, j++)
|
|
{
|
|
if (j == 16)
|
|
{
|
|
printf("%s", buffer);
|
|
memset(buffer, ' ', 58 + 16);
|
|
|
|
sprintf(temp, "+%04x", i);
|
|
memcpy(buffer, temp, 5);
|
|
|
|
j = 0;
|
|
}
|
|
|
|
sprintf(temp, "%02x", 0xff & data[i]);
|
|
memcpy(buffer + 8 + (j * 3), temp, 2);
|
|
if ((data[i] > 31) && (data[i] < 127))
|
|
ascii[j] = data[i];
|
|
else
|
|
ascii[j] = '.';
|
|
}
|
|
|
|
if (j != 0)
|
|
printf("%s", buffer);
|
|
}
|
|
|
|
int encode_length_forwards(unsigned char *buffer,int *offset,
|
|
unsigned int length)
|
|
{
|
|
if (length<0xff) { buffer[(*offset)++]=length; return 0; }
|
|
buffer[(*offset)++]=0xff;
|
|
int i;
|
|
for(i=0;i<32;i+=8) buffer[(*offset)++]=(length>>i)&0xff;
|
|
return 0;
|
|
}
|
|
|
|
int encode_length_backwards(unsigned char *buffer,int *offset,
|
|
unsigned int length)
|
|
{
|
|
if (length<0xff) { buffer[(*offset)++]=length; return 0; }
|
|
int i;
|
|
for(i=0;i<32;i+=8) buffer[(*offset)++]=(length>>i)&0xff;
|
|
buffer[(*offset)++]=0xff;
|
|
return 0;
|
|
}
|
|
|
|
int decode_length_forwards(unsigned char *buffer,int *offset,
|
|
unsigned int *length)
|
|
{
|
|
*length=0;
|
|
switch(buffer[*offset]) {
|
|
case 0xff:
|
|
(*offset)++;
|
|
int i;
|
|
for(i=0;i<32;i+=8) (*length)|=buffer[(*offset)++]<<i;
|
|
return 0;
|
|
default:
|
|
*length=buffer[(*offset)++];
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
int decode_length_backwards(unsigned char *buffer,int offset,
|
|
unsigned int *length)
|
|
{
|
|
// it is assumed that offset points to the first byte after the end
|
|
// of the length field.
|
|
offset--;
|
|
*length=0;
|
|
switch(buffer[offset]) {
|
|
case 0xff:
|
|
offset-=4;
|
|
int i;
|
|
for(i=0;i<32;i+=8) (*length)|=buffer[offset++]<<i;
|
|
return 0;
|
|
default:
|
|
*length=buffer[offset];
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
|
|
int pack_time(unsigned char *buffer,int *offset,uint64_t time)
|
|
{
|
|
int i;
|
|
for(i=0;i<64;i+=8) { buffer[(*offset)++]=(time>>i)&0xff; }
|
|
return 0;
|
|
}
|
|
|
|
int unpack_time(unsigned char *buffer,int *offset,uint64_t *time)
|
|
{
|
|
int i;
|
|
*time=0;
|
|
for(i=0;i<64;i+=8) (*time)|=(((uint64_t)buffer[(*offset)++])<<i);
|
|
return 0;
|
|
}
|
|
|
|
|
|
unsigned char did_chars[16]="0123456789+#*abX";
|
|
|
|
int unpack_did(unsigned char *buffer,int *offset,char *did_out)
|
|
{
|
|
int i;
|
|
|
|
for(i=0;i<64;i++)
|
|
{
|
|
int n=buffer[(*offset)+i/2];
|
|
if (i&1) n=n>>4; else n=n&0xf;
|
|
if (n!=15) did_out[i]=did_chars[n];
|
|
else {
|
|
did_out[i]=0;
|
|
(*offset)+=((i+1)/2);
|
|
if (!(i&1)) (*offset)++;
|
|
return 0;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int pack_did(unsigned char *buffer,int *offset,const char *did)
|
|
{
|
|
int i;
|
|
int highlow=0;
|
|
for(i=0;i<64;i++) {
|
|
int j;
|
|
for(j=0;j<16;j++) if (did_chars[j]==tolower(did[i])) break;
|
|
if (!did[i]) j=15;
|
|
if (j>=16) return -1; // illegal character
|
|
if (highlow) {
|
|
buffer[(*offset)++]|=j<<4;
|
|
}
|
|
else buffer[*offset]=j;
|
|
highlow^=1;
|
|
if (j==15) {
|
|
if (highlow) (*offset)++;
|
|
return 0;
|
|
}
|
|
}
|
|
return -1; // number too long
|
|
}
|
|
|
|
int serialize_meshms(unsigned char *buffer,int *offset,unsigned int length,const char *sender_did,const char *recipient_did, unsigned long long time, const char *payload, int payload_length)
|
|
{
|
|
int ret = 0;
|
|
|
|
encode_length_forwards(buffer,offset,length);
|
|
pack_did(buffer,offset,sender_did);
|
|
pack_did(buffer,offset,recipient_did);
|
|
pack_time(buffer,offset,time);
|
|
|
|
int i=0;
|
|
for(i;i<payload_length;i++)
|
|
{
|
|
buffer[(*offset)++]=payload[i];
|
|
}
|
|
|
|
encode_length_backwards(buffer,offset,length);
|
|
hex_dump(buffer,*offset);
|
|
|
|
return ret;
|
|
}
|
|
|
|
|
|
int deserialize_meshms(unsigned char *buffer,int *offset, int buffer_size)
|
|
{
|
|
|
|
cli_delim("\n");
|
|
cli_puts("offset|length|sender|recipient|date|message");cli_delim("\n");
|
|
|
|
int ret = 0;
|
|
int i=0;
|
|
|
|
unsigned int length =0;
|
|
|
|
while (i < buffer_size) {
|
|
unsigned int start_offset=*offset;
|
|
|
|
cli_printf("%d",*offset);cli_puts("|");
|
|
|
|
int length_length=*offset;
|
|
decode_length_forwards(buffer,offset,&length);
|
|
length_length=(*offset)-length_length;
|
|
cli_printf("%d",length);cli_puts("|");
|
|
|
|
char sender_did_out[64];
|
|
unpack_did(buffer,offset,sender_did_out);
|
|
cli_printf("%s",sender_did_out);cli_puts("|");
|
|
|
|
char recipient_did_out[64];
|
|
unpack_did(buffer,offset,recipient_did_out);
|
|
cli_printf("%s",recipient_did_out);cli_puts("|");
|
|
|
|
unsigned long long time = 0;
|
|
unpack_time(buffer,offset,&time);
|
|
cli_printf("%lld",time);cli_puts("|");
|
|
|
|
int j=0;
|
|
int payload_end=start_offset+length-length_length;
|
|
printf("\nstart_offset=%d, offset=%d, payload_end=%d, length=%d, length_length=%d\n",
|
|
start_offset,*offset,payload_end,length,length_length);
|
|
for(j=*offset;j<payload_end;j++)
|
|
{
|
|
cli_printf("%c", buffer[(*offset)++]);
|
|
}
|
|
|
|
cli_delim("\n");
|
|
i = i+length;
|
|
*offset=i;
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
#ifdef STANDALONE
|
|
int main(int argc,char **argv)
|
|
{
|
|
unsigned char buffer[64];
|
|
int offset=0;
|
|
char *did=argv[1];
|
|
|
|
|
|
printf("return value = %d\n",pack_did(buffer,&offset,did));
|
|
|
|
int i;
|
|
|
|
for(i=0;i<offset;i++) printf("%02x\n",buffer[i]);
|
|
|
|
int offset_out=0;
|
|
char did_out[64];
|
|
|
|
printf("return value = %d\n",unpack_did(buffer,&offset_out,did_out));
|
|
printf("did_out='%s', offset_out=%d (should be %d)\n",
|
|
did_out,offset_out,offset);
|
|
|
|
uint64_t t=time(0)*1000LL;
|
|
offset=0;
|
|
pack_time(buffer,&offset,t);
|
|
printf("stowing time = 0x%llx\n",t);
|
|
for(i=0;i<offset;i++) printf("%02x\n",buffer[i]);
|
|
offset-=8;
|
|
t=0;
|
|
unpack_time(buffer,&offset,&t);
|
|
printf("extracted time = 0x%llx\n",t);
|
|
|
|
offset=0;
|
|
encode_length_backwards(buffer,&offset,123);
|
|
for(i=0;i<offset;i++) printf("%02x\n",buffer[i]);
|
|
unsigned int length;
|
|
decode_length_backwards(buffer,offset,&length);
|
|
printf("decoding reverse-encoded length=123 yields %d (offset was %d, should be 1)\n",
|
|
length,offset);
|
|
|
|
offset=0;
|
|
encode_length_backwards(buffer,&offset,0x1234567);
|
|
for(i=0;i<offset;i++) printf("%02x\n",buffer[i]);
|
|
decode_length_backwards(buffer,offset,&length);
|
|
printf("decoding reverse-encoded length=0x1234567 yields 0x%x (offset was %d, should be 5)\n",
|
|
length,offset);
|
|
|
|
offset=0;
|
|
encode_length_forwards(buffer,&offset,123);
|
|
for(i=0;i<offset;i++) printf("%02x\n",buffer[i]);
|
|
decode_length_forwards(buffer,&offset,&length);
|
|
offset=0;
|
|
printf("decoding forward-encoded length=123 yields %d (offset is %d, should be 1)\n",
|
|
length,offset);
|
|
|
|
offset=0;
|
|
encode_length_forwards(buffer,&offset,0x1234567);
|
|
for(i=0;i<offset;i++) printf("%02x\n",buffer[i]);
|
|
offset=0;
|
|
decode_length_forwards(buffer,&offset,&length);
|
|
printf("decoding reverse-encoded length=0x1234567 yields 0x%x (offset is %d, should be 5)\n",
|
|
length,offset);
|
|
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
#endif
|