serval-dna/hlrdata.c

561 lines
16 KiB
C
Raw Normal View History

/*
Serval Distributed Numbering Architecture (DNA)
Copyright (C) 2010 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 "mphlr.h"
int hlrGetRecordLength(unsigned char *hlr,int hofs);
int bcompare(unsigned char *a,unsigned char *b,size_t len)
{
int i;
for(i=0;i<len;i++) if (a[i]<b[i]) return -1; else if (a[i]>b[i]) return 1;
return 0;
}
int seedHlr()
{
/* If HLR is entry, create a default entry */
int offset=0;
if (!nextHlr(hlr,&offset)) {
/* There is an entry, so nothing to do */
return 0;
}
/* There wasn't an entry, so make one.
Pick a random SID (it will pick one for us).
For completeness we will pick a random DID, which also makes life easier for us anyway
by allowing the use of createHlr().
*/
{
int i,ofs=0;
char sid[65];
char did[65];
/* Make DID start with 2 through 9, as 1 is special in many number spaces. */
did[0]='2'+random()%8;
/* Then add 10 more digits, which is what we do in the mobile phone software */
for(i=1;i<11;i++) did[i]='0'+random()%10; did[11]=0;
if (createHlr(did,sid)) return WHY("Failed to seed HLR with home entry");
if (hlrSetVariable(hlr,ofs,VAR_LOCATIONS,0,(unsigned char *)"4000@",5)) return WHY("Could not set location while seeding HLR");
}
return 0;
}
int nextHlr(unsigned char *hlr,int *ofs)
{
int record_length;
if (!ofs) return setReason("nextHlr passed NULL pointer.");
if (*ofs>=hlr_size) return -1;
/* Get length of this record */
record_length =hlr[(*ofs)+3]<<0;
record_length|=hlr[(*ofs)+2]<<8;
record_length|=hlr[(*ofs)+1]<<16;
record_length|=hlr[(*ofs)+0]<<24;
if (record_length<1) return -1;
(*ofs)+=record_length;
return 0;
}
char sid_string[SID_SIZE*2+1];
char *hlrSid(unsigned char *hlr,int ofs)
{
int o=ofs+4;
extractSid(hlr,&o,sid_string);
return sid_string;
}
int findHlr(unsigned char *hlr,int *ofs,char *sid,char *did)
{
unsigned int record_length;
int match=0;
int records_searched=0;
int pid_len=0;
unsigned char packed_id[40];
if ((*ofs)>=hlr_size) return 0;
if (debug>4) fprintf(stderr,"Searching for HLR record sid=[%s]/did=[%s]\n",sid?sid:"NULL",did?did:"NULL");
if (did&&did[0]) {
/* Make packed version of DID so that we can compare faster with the DIDs in the HLR */
if (stowDid(packed_id,&pid_len,did)) return setReason("DID appears to be invalid");
/* Find significant length of packed DID */
for(pid_len=0;pid_len<DID_MAXSIZE;pid_len++) if ((packed_id[pid_len]&0x0f)==0x0f) { pid_len++; break; }
if (debug>1) dump("Searching for DID records that match",packed_id,pid_len);
}
if (sid&&sid[0]) {
/* Make packed version of SID for fast comparison */
if (stowSid(packed_id,pid_len,sid)) return setReason("SID appears to be invalid");
pid_len=SID_SIZE;
}
while(!match)
{
/* Get length of this record */
record_length =hlr[(*ofs)+3]<<0;
record_length|=hlr[(*ofs)+2]<<8;
record_length|=hlr[(*ofs)+1]<<16;
record_length|=hlr[(*ofs)+0]<<24;
if (!record_length) return 0;
if (debug>4) fprintf(stderr,"Considering HLR entry @ 0x%x\n",*ofs);
records_searched++;
if (sid&&sid[0]) {
/* Lookup by SID, so just see if it matches */
if (!bcompare(packed_id,&hlr[(*ofs)+4],SID_SIZE)) {
if (debug>1) fprintf(stderr,"Found requested SID at address 0x%x.\n",*ofs);
match=1;
}
}
if (did&&did[0]) {
/* Lookup by did, so see if there are any matching DID entries for this subscriber */
int rofs=(*ofs);
struct hlrentry_handle *h=openhlrentry(hlr,rofs);
while(h)
{
/* Search through variables for matching DIDs */
if (debug>2) {
fprintf(stderr,"Considering variable 0x%02x, instance %d.\n",
h->var_id,h->var_instance);
dump("variable value",h->value,h->value_len);
}
if (h->var_id==VAR_DIDS) { /* DID entry */
if (debug>2) fprintf(stderr,"Checking DID against record DID\n");
if (!bcompare(packed_id,h->value,pid_len)) {
if (debug>1) fprintf(stderr,"Found matching DID in HLR record #%d\n",records_searched);
match=1;
break;
}
}
else
{
if (debug>2) fprintf(stderr,"Skipping non-DID variable while searching for DID.\n");
}
h=hlrentrygetent(h);
}
}
if ((!sid)&&(!did)) match=1;
/* For each match ... */
if (match)
{
if (debug>4) fprintf(stderr,"Returning HLR entry @ 0x%x\n",*ofs);
return 1;
}
/* Consider next record */
(*ofs)+=record_length;
if ((*ofs)>=hlr_size) return 0;
}
return 0;
}
int createHlr(char *did,char *sid) {
int i;
int record_offset=0;
/* Generate random SID */
for(i=1;i<64;i++) sid[i]=hexdigit[random()&0xf]; sid[64]=0;
/* But make sure first digit is non-zero as required by the overlay mesh */
sid[0]=hexdigit[1+(random()&0xe)];
if (debug>1) fprintf(stderr,"Creating new HLR entry with sid %s\n",sid);
/* Find first free byte of HLR.
Keep calling findHlr() until we find the end. */
while((i=hlrGetRecordLength(hlr,record_offset))>0)
{
record_offset+=i;
if (debug>1) fprintf(stderr,"Skipping %d bytes to 0x%x\n",i,record_offset);
}
if (i<0) return setReason("Corrupt HLR: Negative length field encountered.");
if (record_offset>=hlr_size)
{
/* No space */
return setReason("No space in HLR for a new record");
}
else
{
/* We have found space, but is it enough? */
int bytes=hlr_size-record_offset;
if (bytes<1024) return setReason("<1KB space in HLR");
if (debug>2) fprintf(stderr,"Creating new HLR entry @ 0x%x\n",record_offset);
/* Write shiny fresh new record.
32bit - record length
32 bytes - SID
Total length = 4+32=36 bytes.
*/
if (stowSid(hlr,record_offset+4,sid)) return setReason("Could not store SID in new HLR entry");
/* Write length last of all to make entry valid */
hlr[record_offset]=0;
hlr[record_offset+1]=0;
hlr[record_offset+2]=0;
hlr[record_offset+3]=36;
/* Store the DID */
{
unsigned char packeddid[DID_MAXSIZE];
int pdidlen=0;
stowDid(packeddid,&pdidlen,did);
/* Work out reduced length of DID */
for(pdidlen=1;pdidlen<DID_MAXSIZE;pdidlen++) if (packeddid[pdidlen-1]==0xff) break;
hlrSetVariable(hlr,record_offset,VAR_DIDS,0x00,packeddid,pdidlen);
}
if (debug) fprintf(stderr,"Created new 36 byte HLR record for DID=[%s] @ 0x%x with SID=[%s]\n",
did,record_offset,sid);
if (debug>2) dump("after HLR create",&hlr[0],256);
return 0;
}
return setReason("Unreachable code turned out not to be");
}
struct hlrentry_handle hlr_handle;
int hlrGetRecordLength(unsigned char *hlr,int hofs)
{
int record_length;
record_length =hlr[hofs+3]<<0;
record_length|=hlr[hofs+2]<<8;
record_length|=hlr[hofs+1]<<16;
record_length|=hlr[hofs+0]<<24;
if (debug>2) fprintf(stderr,"HLR record @ 0x%x is %d bytes long.\n",hofs,record_length);
2011-10-02 11:11:13 +00:00
if (record_length<0) {
// fix corrupt entries
if (debug>2) fprintf(stderr,"HLR record @ 0x%x ZEROED.\n",hofs);
2011-10-02 11:11:13 +00:00
hlr[hofs+3]=0;
hlr[hofs+2]=0;
hlr[hofs+1]=0;
hlr[hofs+0]=0;
return 0;
}
2011-10-02 10:48:16 +00:00
return record_length;
}
int hlrSetRecordLength(unsigned char *hlr,int hofs,int length)
{
2011-10-02 11:11:13 +00:00
if (length<0) length=0;
hlr[hofs+3]=length&0xff;
hlr[hofs+2]=(length>>8)&0xff;
hlr[hofs+1]=(length>>16)&0xff;
hlr[hofs+0]=(length>>24)&0xff;
return 0;
}
/*
XXX We could return a fancy struct, and maybe we should.
But returning a long long and using the two 32bit halves is easy for now, and has a
certain efficiency to it. */
struct hlrentry_handle *openhlrentry(unsigned char *hlr,int hofs)
{
int record_length=hlrGetRecordLength(hlr,hofs);
/* If record has zero length, then open fails */
2011-10-02 11:02:01 +00:00
if (record_length<1)
{
if (debug>2) fprintf(stderr,"HLR record is zero length -- aborting.\n");
return NULL;
}
bzero(&hlr_handle,sizeof(hlr_handle));
hlr_handle.record_length=record_length;
hlr_handle.hlr=hlr;
hlr_handle.hlr_offset=hofs;
hlr_handle.var_id=-1;
hlr_handle.var_instance=-1;
hlr_handle.value=NULL;
hlr_handle.value_len=-1;
hlr_handle.entry_offset=0;
/* Return offset of start of HLR entry and the offset of the first variable */
return hlrentrygetent(&hlr_handle);
}
struct hlrentry_handle *hlrentrygetent(struct hlrentry_handle *h)
{
int ptr;
if (!h) return NULL;
if (h->entry_offset==0)
{
/* First entry */
if (debug>2) fprintf(stderr,"Considering first entry of HLR record.\n");
h->entry_offset=HLR_RECORD_LEN_SIZE+SID_SIZE;
}
else
{
/* subsequent entry */
if (debug>2) fprintf(stderr,"Considering entry @ 0x%x\n",h->entry_offset);
h->entry_offset+=1+2+h->value_len+(h->var_id&0x80?1:0);
}
/* XXX Check if end of record */
if (h->entry_offset>=h->record_length) {
if (debug>2) fprintf(stderr,"Reached end of HLR record (%d>=%d).\n",h->entry_offset,h->record_length);
return NULL;
}
/* XXX Extract variable */
ptr=h->hlr_offset+h->entry_offset;
if (debug>2) fprintf(stderr,"Extracting HLR variable @ 0x%x\n",ptr);
h->var_id=hlr[ptr];
h->value_len=(hlr[ptr+1]<<8)+hlr[ptr+2];
ptr+=3;
if (h->var_id&0x80) h->var_instance=hlr[ptr++];
h->value=&h->hlr[ptr];
return h;
}
int hlrGetVariable(unsigned char *hlr,int hofs,int varid,int varinstance,
unsigned char *value,int *len)
{
struct hlrentry_handle *h;
int hlr_offset=-1;
h=openhlrentry(hlr,hofs);
/* Find the place in the HLR record where this variable is */
while(h)
{
if ((h->var_id<varid)
||(h->var_id==varid&&h->var_instance<varinstance))
hlr_offset=h->entry_offset;
else
{
/* Value is here if anywhere */
if (h->var_id>varid||h->var_instance>varinstance)
return setReason("No such variable instance");
if (h->value_len>*len) return setReason("Value too long for buffer");
bcopy(h->value,value,h->value_len);
*len=h->value_len;
return 0;
break;
}
h=hlrentrygetent(h);
}
return setReason("No such variable instance");
}
int hlrSetVariable(unsigned char *hlr,int hofs,int varid,int varinstance,
unsigned char *value,int len)
{
/* hlr & hofs identify the start of a HLR entry. */
struct hlrentry_handle *h;
int hlr_offset=-1;
int hlr_size=hlrGetRecordLength(hlr,hofs);
if (debug) fprintf(stderr,"hlrSetVariable(varid=%02x, instance=%02x, len=%d)\n",
varid,varinstance,len);
h=openhlrentry(hlr,hofs);
/* Find the place in the HLR record where this variable should go */
while(h)
{
if (debug>1) fprintf(stderr,"h->var_id=%02x, h->h->var_instance=%02x, h->entry_offset=%x\n",
h->var_id,h->var_instance,h->entry_offset);
if ((h->var_id<varid)
||((h->var_id&0x80)&&(h->var_id==varid&&h->var_instance<varinstance)))
{
hlr_offset=h->entry_offset;
if (debug>1) fprintf(stderr,"Found variable instance prior: hlr_offset=%d.\n",hlr_offset);
}
else
{
/* Value goes here */
if (debug>1) fprintf(stderr,"Found variable instance to overwrite: hlr_offset=%d.\n",hlr_offset);
hlr_offset=h->entry_offset;
break;
}
h=hlrentrygetent(h);
}
/* XXX Race condition: If power is lost half way through here, then it is possible for
the record to be left in an inconsistent state */
if (h&&hlr_offset>-1)
{
if (debug>2) printf("hlr_offset=%d\n",hlr_offset);
if (h&&h->var_id==varid&&((h->var_instance==varinstance)||(!(h->var_id&0x80))))
{
int existing_size;
/* Replace existing value */
if (debug) fprintf(stderr,"Replacing value in HLR:\n");
existing_size=1+2+((h->var_id&0x80)?1:0)+h->value_len;
hlrMakeSpace(hlr,hofs,hlr_offset,1+2+len+((varid&0x80)?1:0)-existing_size);
}
else
{
/* Insert value here */
if (debug) fprintf(stderr,"Inserting value in HLR\n");
hlrMakeSpace(hlr,hofs,hlr_offset,1+2+len+((varid&0x80)?1:0));
}
}
else
{
/* HLR record has no entries, or this entry needs to go at the end,
so insert value at end of the record */
if (debug) fprintf(stderr,"Inserting value at end of HLR @ 0x%x\n",hlr_size);
hlrMakeSpace(hlr,hofs,hlr_size,1+2+len+((varid&0x80)?1:0));
hlr_offset=hlr_size;
}
return hlrStowValue(hlr,hofs,hlr_offset,varid,varinstance,value,len);
}
int hlrStowValue(unsigned char *hlr,int hofs,int hlr_offset,
int varid,int varinstance,unsigned char *value,int len)
{
int ptr=hofs+hlr_offset;
hlr[ptr++]=varid;
hlr[ptr++]=(len>>8)&0xff;
hlr[ptr++]=len&0xff;
if (varid&0x80) hlr[ptr++]=varinstance;
bcopy(value,&hlr[ptr],len);
ptr+=len;
return 0;
}
int hlrMakeSpace(unsigned char *hlr,int hofs,int hlr_offset,int bytes)
{
int length;
int shifted_bytes=hlr_size-(hofs+hlr_offset+bytes);
/* Don't read past end of file */
if (bytes<0) shifted_bytes+=bytes;
/* Deal with easy case first */
if (!bytes) return 0;
if (debug>2) {
fprintf(stderr,"hlrMakeSpace: Inserting %d bytes at offset %d with hofs=%d. shifted bytes=%d\n",
bytes,hlr_offset,hofs,shifted_bytes);
fflush(stderr);
}
/* Shift rest of HLR up/down.
If down, back-fill bytes with zeros. */
bcopy(&hlr[hofs+hlr_offset],&hlr[hofs+hlr_offset+bytes],
shifted_bytes);
if (bytes<0) bzero(&hlr[hlr_size+bytes],0-bytes);
/* Update record length */
length=hlrGetRecordLength(hlr,hofs);
length+=bytes;
hlrSetRecordLength(hlr,hofs,length);
if (debug>1) fprintf(stderr,"hlrMakeSpace: HLR entry now %d bytes long.\n",length);
return 0;
}
int hlrDump(unsigned char *hlr,int hofs)
{
struct hlrentry_handle *h=openhlrentry(hlr,hofs);
fprintf(stderr,"Dumping HLR entry @ 0x%x\n",hofs);
while(h)
{
fprintf(stderr," var=%02x",h->var_id);
if (h->var_id&0x80) fprintf(stderr,"/%02x",h->var_instance);
fprintf(stderr," len=%d\n",h->value_len);
h=hlrentrygetent(h);
}
return 0;
}
int openHlrFile(char *backing_file,int size)
{
/* Get backing store */
if (!backing_file)
{
if (size<0) exit(setReason("You must provide an HLR file or size"));
/* transitory storage of HLR data, so just malloc() the memory */
hlr=calloc(size,1);
if (!hlr) exit(setReason("Failed to calloc() HLR database."));
if (debug) fprintf(stderr,"Allocated %d byte temporary HLR store\n",size);
}
else
{
unsigned char zero[8192];
FILE *f=fopen(backing_file,"r+");
if (!f) f=fopen(backing_file,"w+");
if (!f) exit(setReason("Could not open backing file."));
bzero(&zero[0],8192);
fseek(f,0,SEEK_END);
errno=0;
/* Obtain size from existing backing file */
if (size<0) size=ftell(f);
while(ftell(f)<size)
{
int r;
fseek(f,0,SEEK_END);
if ((r=fwrite(zero,8192,1,f))!=1)
{
perror("fwrite");
exit(setReason("Could not enlarge backing file to requested size (short write)"));
}
fseek(f,0,SEEK_END);
}
if (errno) perror("fseek");
if (fwrite("",1,1,f)!=1)
{
fprintf(stderr,"Failed to set backing file size.\n");
perror("fwrite");
}
hlr=(unsigned char *)mmap(NULL,size,PROT_READ|PROT_WRITE,MAP_SHARED|MAP_NORESERVE,fileno(f),0);
if (hlr==MAP_FAILED) {
perror("mmap");
exit(setReason("Memory mapping of HLR backing file failed."));
}
if (debug) fprintf(stderr,"Allocated %d byte HLR store backed by file `%s'\n",
size,backing_file);
}
hlr_size=size;
return 0;
}