init ems server code
This commit is contained in:
941
plat/public_bak/src/asn1.c
Normal file
941
plat/public_bak/src/asn1.c
Normal file
@@ -0,0 +1,941 @@
|
||||
/********************************************************************/
|
||||
/*Title: asn1.c */
|
||||
/*Descr: ASN.1 Encoding/Decoding Module */
|
||||
/*Author: Liang Hanxi */
|
||||
/*Create: 2002-1-15 */
|
||||
/*Version: */
|
||||
/*Modify: 2002-10-22 */
|
||||
/********************************************************************/
|
||||
|
||||
|
||||
#include "./include/asn1.h"
|
||||
#include <string.h>
|
||||
#include <ctype.h>
|
||||
#include <stdio.h>
|
||||
|
||||
/*@ignore@*/
|
||||
void AsnEncode(u_char *msg_buf,u_int maxlen,ASN_BUF *asnbuf)
|
||||
{
|
||||
asnbuf->msgbuf=msg_buf;
|
||||
memset(msg_buf,0,2);
|
||||
asnbuf->msglen=0;
|
||||
asnbuf->maxlen=maxlen;
|
||||
}
|
||||
|
||||
void asn_encode(u_char *msg_buf,ASN_BUF *asnbuf)
|
||||
{
|
||||
AsnEncode(msg_buf,256,asnbuf);
|
||||
}
|
||||
|
||||
void asn_encode_v3(u_char *msg_buf,u_int maxlen,ASN_BUF *asnbuf)
|
||||
{
|
||||
AsnEncode(msg_buf,maxlen,asnbuf);
|
||||
}
|
||||
|
||||
int add_tlv(const char *tag_seq,u_int length,u_char *pvalue,u_char tlv_type,ASN_BUF *asnbuf)
|
||||
{
|
||||
return AddTLV(tag_seq,length,pvalue,tlv_type,asnbuf);
|
||||
}
|
||||
|
||||
int AddTLV(const char *tag_seq,u_int length,u_char *pvalue,u_char tlv_type,ASN_BUF *asnbuf)
|
||||
{
|
||||
u_short tags[50][2];
|
||||
int level;
|
||||
int rest,swaplen=0,oldlen=0;
|
||||
int ntag,nlen;
|
||||
int addval,temp,i,rtn; //temp variable
|
||||
u_char flag; //temp variable
|
||||
u_char *ptlv,*pt,*ptr[50],asn_swap[1024];
|
||||
|
||||
if(asnbuf->msglen+length+5>asnbuf->maxlen)
|
||||
{
|
||||
printf("No room for new tlv.maxlen=%d,msglen=%d,addlen=%d\n",asnbuf->maxlen,asnbuf->msglen,length);
|
||||
return -1;
|
||||
}
|
||||
/*search position for new data--->*/
|
||||
level = parse_tags(tag_seq,tags);
|
||||
pt = asnbuf->msgbuf; //start pointer
|
||||
rest = asnbuf->msglen % 1024; //remain length
|
||||
for(i = 1; i < level; i++)
|
||||
{
|
||||
if(tags[i][1] == 0) //according to tagcode
|
||||
rtn = findtlv(pt, tags[i][0], &ptlv, rest);
|
||||
else //according to order
|
||||
rtn = findtlv2(pt, tags[i][1], &ptlv, rest);
|
||||
|
||||
if(rtn == -1) //length check error
|
||||
return -1;
|
||||
else if(rtn == 1) //not found,create it
|
||||
{
|
||||
swaplen = asnbuf->msglen - (ptlv - asnbuf->msgbuf);
|
||||
if(swaplen > 0)
|
||||
memcpy(asn_swap, ptlv, swaplen);
|
||||
pt = ptlv;
|
||||
while(i < level)
|
||||
{
|
||||
//printf("create level=%d\n",i);
|
||||
ntag = addtagcode(pt, tags[i][0], 0xA0);
|
||||
nlen = addlength(pt + ntag, 0);
|
||||
ptr[i] = pt + ntag;
|
||||
addval = modifylength(ptr, i - 1, ntag + nlen);
|
||||
pt += addval;
|
||||
asnbuf->msglen += addval;
|
||||
i ++;
|
||||
}
|
||||
rest = 0;
|
||||
}
|
||||
else //found,continue
|
||||
{
|
||||
if((rtn & 0x20) == 0)
|
||||
return -1;
|
||||
|
||||
ntag=gettagcode(ptlv,&temp,&flag);
|
||||
nlen=getlength(ptlv+ntag,&rest);
|
||||
ptr[i]=ptlv+ntag;
|
||||
pt=ptlv+ntag+nlen;
|
||||
}
|
||||
}
|
||||
//insert new data
|
||||
if(tags[level][1]==0)
|
||||
{
|
||||
rtn=findtlv(pt,tags[level][0],&ptlv,rest);
|
||||
|
||||
if(rtn<0)
|
||||
return -1;
|
||||
else if(rtn!=1)
|
||||
{
|
||||
oldlen=0;
|
||||
oldlen=gettagcode(ptlv,&temp,&flag);
|
||||
oldlen+=getlength(ptlv+oldlen,&temp);
|
||||
oldlen+=temp;
|
||||
}
|
||||
if(swaplen==0)
|
||||
{
|
||||
swaplen=asnbuf->msglen-(ptlv+oldlen-asnbuf->msgbuf);
|
||||
if(swaplen>0)
|
||||
memcpy(asn_swap,ptlv+oldlen,swaplen);
|
||||
}
|
||||
|
||||
ntag=addtagcode(ptlv,tags[level][0],tlv_type);
|
||||
nlen=addlength(ptlv+ntag,length);
|
||||
|
||||
memcpy(ptlv+ntag+nlen,pvalue,length);
|
||||
//modify parent's length
|
||||
addval=modifylength(ptr,level-1,ntag+nlen+length);
|
||||
asnbuf->msglen+=addval;
|
||||
ptlv+=addval;
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
rtn=findtlv2(pt,tags[level][1],&ptlv,rest);
|
||||
if(rtn<0)
|
||||
return -1;
|
||||
if(swaplen==0)
|
||||
{
|
||||
swaplen=asnbuf->msglen-(ptlv-asnbuf->msgbuf);
|
||||
if(swaplen>0)
|
||||
memcpy(asn_swap,ptlv,swaplen);
|
||||
|
||||
}
|
||||
ntag=addtagcode(ptlv,tags[level][0],tlv_type);
|
||||
nlen=addlength(ptlv+ntag,length);
|
||||
memcpy(ptlv+ntag+nlen,pvalue,length);
|
||||
//modify parent's length
|
||||
addval=modifylength(ptr,level-1,ntag+nlen+length);
|
||||
asnbuf->msglen+=addval;
|
||||
ptlv+=addval;
|
||||
}
|
||||
if(swaplen>0)
|
||||
memcpy(ptlv,asn_swap,swaplen);
|
||||
return asnbuf->msglen;
|
||||
}
|
||||
|
||||
|
||||
int asn_decode(u_char *msg_buf,int ntlv,ASN_BUF *asnbuf)
|
||||
{
|
||||
return AsnDecode(msg_buf,256,ntlv,NULL,asnbuf);
|
||||
}
|
||||
|
||||
int asn_decode_v2(u_char *msg_buf,int ntlv,int *errpos,ASN_BUF *asnbuf)
|
||||
{
|
||||
return AsnDecode(msg_buf,256,ntlv,errpos,asnbuf);
|
||||
}
|
||||
|
||||
int asn_decode_v3(u_char *msg_buf,int msglen,int *errpos,ASN_BUF *asnbuf)
|
||||
{
|
||||
return AsnDecode(msg_buf,msglen,100,errpos,asnbuf);
|
||||
}
|
||||
|
||||
int AsnDecode(u_char *msg_buf,int msglen,int maxtlv,int *errpos,ASN_BUF *asnbuf)
|
||||
{
|
||||
ASN_TLV *p1=NULL,*p2;
|
||||
int ntlv=0,retval,sublen=0;
|
||||
|
||||
asnbuf->msgbuf = msg_buf;
|
||||
asnbuf->tlvcount=0;
|
||||
asnbuf->pos = 0;
|
||||
asnbuf->msglen = msglen;
|
||||
asnbuf->errmsg[0]='\0';
|
||||
|
||||
while(asnbuf->pos < msglen && ntlv<maxtlv)
|
||||
{
|
||||
if(errpos!=NULL)
|
||||
*errpos = asnbuf->pos;
|
||||
|
||||
retval=parse_tlv(&p2,asnbuf);
|
||||
|
||||
if(retval<0)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
else if(retval==0)
|
||||
{
|
||||
return sublen;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(p1!=NULL)
|
||||
p1->pnext = p2;
|
||||
p1 = p2;
|
||||
sublen+=retval;
|
||||
}
|
||||
ntlv++;
|
||||
}
|
||||
if(errpos!=NULL)
|
||||
*errpos = asnbuf->pos;
|
||||
|
||||
return sublen;
|
||||
}
|
||||
|
||||
int get_tlv(const char *tag_seq,u_char *pvalue,ASN_BUF *asnbuf)
|
||||
{
|
||||
return get_tlv_v2(tag_seq,pvalue,NULL,asnbuf);
|
||||
}
|
||||
|
||||
int get_tlv_v2(const char *tag_seq,u_char *pvalue,u_char *flag,ASN_BUF *asnbuf)
|
||||
{
|
||||
u_short tags[30][2];
|
||||
int depth=1,order=1;
|
||||
ASN_TLV *p1=&asnbuf->heap[0];
|
||||
parse_tags(tag_seq,tags);
|
||||
|
||||
if(asnbuf->tlvcount < 1)
|
||||
return -1;
|
||||
|
||||
while(depth<tags[0][0]+1 && (order<=tags[depth][1] || tags[depth][1]==0))
|
||||
{
|
||||
//printf("depth=%d/%d,order=%d,tagcode=%d/%d\n",depth,tags[0][0],order,p1->tagcode,tags[depth][0]);
|
||||
if((p1->tagcode == tags[depth][0] || tags[depth][0]==0xFFFF) && (tags[depth][1]==0 || order==tags[depth][1]))
|
||||
{
|
||||
if(depth==tags[0][0])
|
||||
{
|
||||
if(flag!=NULL)
|
||||
*flag=p1->tagtype;
|
||||
|
||||
if(p1->length < 0)
|
||||
return -1;
|
||||
else if(p1->length <= 256)
|
||||
{
|
||||
memcpy(pvalue,p1->pvalue,p1->length);
|
||||
return p1->length;
|
||||
}
|
||||
else
|
||||
{
|
||||
memcpy(pvalue,p1->pvalue,256);
|
||||
return 256;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(p1->psub==NULL)
|
||||
return -1;
|
||||
p1=p1->psub;
|
||||
depth++;
|
||||
order=1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(p1->pnext == NULL)
|
||||
return -1;
|
||||
p1=p1->pnext;
|
||||
order++;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int GetTLV(const char *tag_seq,int maxlen,u_char *pvalue,u_char flag,ASN_BUF *asnbuf)
|
||||
{
|
||||
u_short tags[30][2];
|
||||
int depth=1,order=1;
|
||||
ASN_TLV *p1=&asnbuf->heap[0];
|
||||
parse_tags(tag_seq,tags);
|
||||
|
||||
if(asnbuf->tlvcount < 1)
|
||||
return -1;
|
||||
|
||||
while(depth<tags[0][0]+1 && (order<=tags[depth][1] || tags[depth][1]==0))
|
||||
{
|
||||
//printf("depth=%d/%d,order=%d,tagcode=%d/%d\n",depth,tags[0][0],order,p1->tagcode,tags[depth][0]);
|
||||
//printf("flag:%02X/%02X\n",p1->tagtype,flag);
|
||||
if((p1->tagcode == tags[depth][0] || tags[depth][0]==0xFFFF) && (depth<tags[0][0] || p1->tagtype == flag) && (tags[depth][1]==0 || order==tags[depth][1]))
|
||||
{
|
||||
if(depth==tags[0][0])
|
||||
{
|
||||
if(p1->length < 0)
|
||||
return -1;
|
||||
else if(p1->length <= maxlen)
|
||||
{
|
||||
memcpy(pvalue,p1->pvalue,p1->length);
|
||||
return p1->length;
|
||||
}
|
||||
else
|
||||
{
|
||||
memcpy(pvalue,p1->pvalue,maxlen);
|
||||
return maxlen;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(p1->psub==NULL)
|
||||
return -1;
|
||||
p1=p1->psub;
|
||||
depth++;
|
||||
order=1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(p1->pnext == NULL)
|
||||
return -1;
|
||||
p1=p1->pnext;
|
||||
order++;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int GetAnyTLV(const char *tag_seq,int maxlen,u_char *pvalue,u_char *pflag,u_short *ptagcode,ASN_BUF *asnbuf)
|
||||
{
|
||||
u_short tags[30][2];
|
||||
int depth=1,order=1;
|
||||
ASN_TLV *p1=&asnbuf->heap[0];
|
||||
parse_tags(tag_seq,tags);
|
||||
|
||||
if(asnbuf->tlvcount < 1)
|
||||
return -1;
|
||||
|
||||
while(depth<tags[0][0]+1 && (order<=tags[depth][1] || tags[depth][1]==0))
|
||||
{
|
||||
//printf("depth=%d/%d,order=%d,tagcode=%d/%d\n",depth,tags[0][0],order,p1->tagcode,tags[depth][0]);
|
||||
//printf("flag:%02X/%02X\n",p1->tagtype,flag);
|
||||
if((p1->tagcode == tags[depth][0] || tags[depth][0]==0xFFFF) && (tags[depth][1]==0 || order==tags[depth][1]))
|
||||
{
|
||||
if(depth==tags[0][0])
|
||||
{
|
||||
if(pflag!=NULL)
|
||||
*pflag = p1->tagtype;
|
||||
if(ptagcode != NULL)
|
||||
*ptagcode = p1->tagcode;
|
||||
|
||||
if(p1->length < 0)
|
||||
return -1;
|
||||
else if(p1->length <= maxlen)
|
||||
{
|
||||
memcpy(pvalue,p1->pvalue,p1->length);
|
||||
return p1->length;
|
||||
}
|
||||
else
|
||||
{
|
||||
memcpy(pvalue,p1->pvalue,maxlen);
|
||||
return maxlen;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(p1->psub==NULL)
|
||||
return -1;
|
||||
p1=p1->psub;
|
||||
depth++;
|
||||
order=1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(p1->pnext == NULL)
|
||||
return -1;
|
||||
p1=p1->pnext;
|
||||
order++;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int parse_tlv(ASN_TLV **pnode,ASN_BUF *asnbuf)
|
||||
//idefinite length
|
||||
{
|
||||
int ntag,nlen,length,sublen=0,retval;
|
||||
u_int tagcode;
|
||||
u_char tagtype;
|
||||
u_char *msgbuf=asnbuf->msgbuf;
|
||||
ASN_TLV *ptlv,*p1=NULL,*p2;
|
||||
|
||||
if(asnbuf->pos == asnbuf->msglen)
|
||||
{
|
||||
*pnode=NULL;
|
||||
return 0;
|
||||
}
|
||||
|
||||
if(asnbuf->pos+2 > asnbuf->msglen || asnbuf->tlvcount >=256)
|
||||
{
|
||||
sprintf(asnbuf->errmsg,"Length check error(rest=%d)/exceed limit(tlvcount=%d)\n",asnbuf->msglen-asnbuf->pos,asnbuf->tlvcount);
|
||||
return -1;
|
||||
}
|
||||
//printf("tlvcount=%d\n",asnbuf->tlvcount);
|
||||
*pnode = ptlv = &asnbuf->heap[asnbuf->tlvcount++];
|
||||
ptlv->psub = ptlv->pnext = NULL;
|
||||
ptlv->length = -1;
|
||||
|
||||
if((ntag=gettagcode(msgbuf+asnbuf->pos,&tagcode,&tagtype))<1)
|
||||
{
|
||||
sprintf(asnbuf->errmsg,"Wrong encoding of tag(pos=%d:%02X %02X %02X)\n",asnbuf->pos,msgbuf[asnbuf->pos],msgbuf[asnbuf->pos+1],msgbuf[asnbuf->pos+2]);
|
||||
return -1;
|
||||
}
|
||||
else
|
||||
asnbuf->pos+=ntag;
|
||||
|
||||
if((nlen=getlength(msgbuf+asnbuf->pos,&length))<1)
|
||||
{
|
||||
sprintf(asnbuf->errmsg,"Wrong encoding of Length(pos=%d:%02X %02X %02X)\n",asnbuf->pos,msgbuf[asnbuf->pos],msgbuf[asnbuf->pos+1],msgbuf[asnbuf->pos+2]);
|
||||
return -1;
|
||||
}
|
||||
else
|
||||
asnbuf->pos+=nlen;
|
||||
|
||||
if(asnbuf->pos + length >asnbuf->msglen)
|
||||
{
|
||||
sprintf(asnbuf->errmsg,"Length Check error.length=%d,rest=%d(pos=%d:%02X %02X %02X)\n",length,asnbuf->msglen-asnbuf->pos,asnbuf->pos,msgbuf[asnbuf->pos],msgbuf[asnbuf->pos+1],msgbuf[asnbuf->pos+2]);
|
||||
return -1;
|
||||
}
|
||||
|
||||
ptlv->tagtype=tagtype;
|
||||
ptlv->tagcode=tagcode;
|
||||
ptlv->pvalue=msgbuf+asnbuf->pos;
|
||||
|
||||
if((tagtype & 0x20)==0) //primitive
|
||||
{
|
||||
if(length==-1)
|
||||
{
|
||||
sprintf(asnbuf->errmsg,"Wrong Encoding of Length:primitive form with indefinite length\n(pos=%d)",asnbuf->pos);
|
||||
return -1;
|
||||
}
|
||||
else
|
||||
{
|
||||
ptlv->length = length;
|
||||
asnbuf->pos += length;
|
||||
return length+ntag+nlen;
|
||||
}
|
||||
}
|
||||
else //constructor
|
||||
{
|
||||
if(length >=0) //short/long definite length
|
||||
{
|
||||
ptlv->length = length;
|
||||
while(sublen<length)
|
||||
{
|
||||
retval=parse_tlv(&p2,asnbuf);
|
||||
if(retval<0)
|
||||
return -1;
|
||||
else if(retval==0)
|
||||
{
|
||||
sprintf(asnbuf->errmsg,"Length Check error.length=%d,sublen=%d(pos=%d:%02X %02X %02X)\n",length,sublen,asnbuf->pos,msgbuf[asnbuf->pos],msgbuf[asnbuf->pos+1],msgbuf[asnbuf->pos+2]);
|
||||
return -1;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(p1==NULL)
|
||||
p1=ptlv->psub = p2;
|
||||
else
|
||||
{
|
||||
p1->pnext = p2;
|
||||
p1 = p2;
|
||||
}
|
||||
sublen+=retval;
|
||||
}
|
||||
}
|
||||
if(sublen>length)
|
||||
{
|
||||
sprintf(asnbuf->errmsg,"Length Check error.length=%d,sublen=%d(pos=%d:%02X %02X %02X)\n",length,sublen,asnbuf->pos,msgbuf[asnbuf->pos],msgbuf[asnbuf->pos+1],msgbuf[asnbuf->pos+2]);
|
||||
return -1;
|
||||
}
|
||||
ptlv->length = sublen;
|
||||
return sublen+ntag+nlen;
|
||||
}
|
||||
else //indefinite length
|
||||
{
|
||||
while(msgbuf[asnbuf->pos]!=0 || msgbuf[asnbuf->pos+1]!=0)
|
||||
{
|
||||
retval=parse_tlv(&p2,asnbuf);
|
||||
if(retval<0)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
else if(retval==0)
|
||||
{
|
||||
ptlv->length = sublen;
|
||||
return sublen+ntag+nlen;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(p1==NULL)
|
||||
p1=ptlv->psub = p2;
|
||||
else
|
||||
{
|
||||
p1->pnext = p2;
|
||||
p1 = p2;
|
||||
}
|
||||
sublen+=retval;
|
||||
}
|
||||
}
|
||||
asnbuf->pos += 2;
|
||||
ptlv->length = sublen;
|
||||
return sublen+ntag+nlen+2;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
int findtlv(u_char *pasn,u_int tagcode,u_char **pnext,int rest)
|
||||
//according to tagcode
|
||||
{
|
||||
int ntag,nlen,nvalue,pos=0;
|
||||
u_int curtag;
|
||||
u_char type;
|
||||
//printf("findtlv rest=%d\n",rest);
|
||||
if(rest==0)
|
||||
{
|
||||
*pnext=pasn;
|
||||
return 1;
|
||||
}
|
||||
else if(rest<0)
|
||||
return -1;
|
||||
ntag=gettagcode(pasn,&curtag,&type);
|
||||
if((nlen=getlength(pasn+ntag,&nvalue)) < 0 || nvalue < 0)
|
||||
return -1;
|
||||
//printf("ntag=%d,nlen=%d,nvalue=%d\n",ntag,nlen,nvalue);
|
||||
if(curtag<tagcode)
|
||||
{
|
||||
if((pos = ntag + nlen + nvalue) <= 0)
|
||||
return -1;
|
||||
return findtlv(pasn+pos,tagcode,pnext,rest-pos);
|
||||
}
|
||||
else if(curtag==tagcode)
|
||||
{
|
||||
*pnext=pasn;
|
||||
return type;
|
||||
}
|
||||
else
|
||||
{
|
||||
*pnext=pasn;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
int findtlv2(u_char *pasn,int index,u_char **pnext,int rest)
|
||||
//according to position
|
||||
{
|
||||
u_int curtag,ntag,nlen,nvalue;
|
||||
u_char type;
|
||||
(*pnext)=pasn;
|
||||
|
||||
while(--index>0 && rest>0)
|
||||
{
|
||||
ntag=gettagcode((*pnext),&curtag,&type);
|
||||
nlen=getlength((*pnext)+ntag,&nvalue);
|
||||
(*pnext)+=(ntag+nlen+nvalue);
|
||||
rest-=(ntag+nlen+nvalue);
|
||||
}
|
||||
|
||||
if(index==0 && rest>0) //found
|
||||
{
|
||||
ntag=gettagcode((*pnext),&curtag,&type);
|
||||
return type;
|
||||
}
|
||||
else if(rest==0) //seek to end(not found)
|
||||
return 1;
|
||||
else
|
||||
return -1;
|
||||
}
|
||||
|
||||
int gettagcode(u_char *asnbuf,u_int *tagcode,u_char *type)
|
||||
{
|
||||
u_int pos=0;
|
||||
*type=asnbuf[pos] & 0xE0;
|
||||
*tagcode=asnbuf[pos] & 0x1F;
|
||||
pos++;
|
||||
if(*tagcode==0x1F)
|
||||
{
|
||||
*tagcode=0;
|
||||
while((asnbuf[pos] & 0x80)==0x80 && pos<3)
|
||||
{
|
||||
*tagcode=(*tagcode) * 0x80 + (asnbuf[pos++] & 0x7F);
|
||||
}
|
||||
*tagcode=(*tagcode) * 0x80 + (asnbuf[pos++] & 0x7F);
|
||||
}
|
||||
return pos;
|
||||
}
|
||||
|
||||
int getlength(u_char *asnbuf,int *len)
|
||||
{
|
||||
u_int pos=0,lenbyte;
|
||||
|
||||
if((asnbuf[pos] & 0x80)==0)
|
||||
*len=asnbuf[pos++] & 0x7F;
|
||||
else
|
||||
{
|
||||
*len=0;
|
||||
lenbyte=asnbuf[pos++] & 0x7F;
|
||||
if(lenbyte>3 || lenbyte<0)
|
||||
return -1;
|
||||
|
||||
if(lenbyte==0)
|
||||
{
|
||||
*len=-1;
|
||||
return 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
while(lenbyte-->0)
|
||||
*len=(*len)*0x100 + asnbuf[pos++];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return pos;
|
||||
}
|
||||
|
||||
int addtagcode(u_char *msg_buf,u_int tagcode,u_char type)
|
||||
{
|
||||
u_int tagbyte=0,pos=0;
|
||||
int i=3;
|
||||
u_char buf[4];
|
||||
msg_buf[pos]=type & 0xE0;
|
||||
if(tagcode<31)
|
||||
msg_buf[pos++]+=tagcode;
|
||||
else
|
||||
{
|
||||
msg_buf[pos++]+=0x1F;
|
||||
while(i>=0 && tagcode>0)
|
||||
{
|
||||
buf[i]=tagcode & 0x7F;
|
||||
buf[i]|=0x80;
|
||||
tagcode=tagcode >> 7;
|
||||
i--;
|
||||
}
|
||||
buf[3]&=0x7F;
|
||||
tagbyte=3-i;
|
||||
memcpy(msg_buf+pos,buf+i+1,tagbyte);
|
||||
pos+=tagbyte;
|
||||
}
|
||||
return pos;
|
||||
}
|
||||
int addlength(u_char *msg_buf,u_int len)
|
||||
{
|
||||
u_int lenbyte=0,pos=0;
|
||||
int i=3;
|
||||
u_char buf[4];
|
||||
|
||||
if(len>=0 && len<=127)
|
||||
msg_buf[pos++]=len;
|
||||
else if(len>0)
|
||||
{
|
||||
while(i>=0 && len>0)
|
||||
{
|
||||
buf[i]=len & 0xFF;
|
||||
len=len>>8;
|
||||
i--;
|
||||
}
|
||||
lenbyte=3-i;
|
||||
msg_buf[pos++]=lenbyte | 0x80;
|
||||
memcpy(msg_buf+pos,buf+i+1,lenbyte);
|
||||
pos+=lenbyte;
|
||||
}
|
||||
return pos;
|
||||
}
|
||||
|
||||
int parse_tags(const char *tag_seq,u_short (*tags)[2])
|
||||
{
|
||||
u_short i,j=0,level=1,len;
|
||||
len=strlen(tag_seq);
|
||||
|
||||
tags[1][0]=0xFFFF;
|
||||
tags[1][1]=0;
|
||||
for(i=0;i<len;i++)
|
||||
{
|
||||
if(isdigit(tag_seq[i]))
|
||||
{
|
||||
if(tags[level][j]==0xFFFF)
|
||||
tags[level][j]=0;
|
||||
tags[level][j]=tags[level][j]*10+tag_seq[i]-'0';
|
||||
}
|
||||
else if(tag_seq[i]=='-')
|
||||
j=1;
|
||||
else if(tag_seq[i]=='.')
|
||||
{
|
||||
j=0;
|
||||
level++;
|
||||
tags[level][0]=0xFFFF;
|
||||
tags[level][1]=0;
|
||||
}
|
||||
else if(isspace(tag_seq[i])==0)
|
||||
return -1;
|
||||
}
|
||||
tags[0][0]=level;
|
||||
return level;
|
||||
}
|
||||
|
||||
int modifylength(u_char *ptr[],int level,int addval)
|
||||
{
|
||||
int lenbyte1,lenbyte2,len;
|
||||
u_char swapbuf[1024];
|
||||
|
||||
if(level<1)
|
||||
return addval;
|
||||
lenbyte1=getlength(ptr[level],&len);
|
||||
memcpy(swapbuf,ptr[level]+lenbyte1,2);
|
||||
lenbyte2=addlength(ptr[level],len+addval);
|
||||
if(lenbyte2>lenbyte1)
|
||||
{
|
||||
memcpy(swapbuf+2,ptr[level]+lenbyte1+2,len+addval-2);
|
||||
memcpy(ptr[level]+lenbyte2,swapbuf,len+addval);
|
||||
}
|
||||
return modifylength(ptr,level-1,addval+lenbyte2-lenbyte1);
|
||||
}
|
||||
|
||||
void showbuf(u_char *buf,int len)
|
||||
{
|
||||
int i;
|
||||
for(i=0;i<len;i++)
|
||||
printf("%02X ",buf[i]);
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
/////////////////////////////////////////////////
|
||||
/*Functions for add/get special data types --->*/
|
||||
/////////////////////////////////////////////////
|
||||
int add_bool(const char *tag_seq,u_char value,u_char tlv_type,ASN_BUF *asnbuf)
|
||||
{
|
||||
return add_tlv(tag_seq,1,&value,tlv_type,asnbuf);
|
||||
}
|
||||
|
||||
u_char get_bool(const char *tag_seq,ASN_BUF *asnbuf)
|
||||
{
|
||||
u_char pvalue[10];
|
||||
get_tlv(tag_seq,pvalue,asnbuf);
|
||||
return pvalue[0];
|
||||
}
|
||||
|
||||
int add_int(const char *tag_seq,int value,u_char tlv_type,ASN_BUF *asnbuf)
|
||||
{
|
||||
return AddInteger(tag_seq,value,tlv_type,asnbuf);
|
||||
}
|
||||
|
||||
int AddInteger(const char *tag_seq,int value,u_char tlv_type,ASN_BUF *asnbuf)
|
||||
{
|
||||
u_char buf[4];
|
||||
u_int *p_uint=(u_int *)buf;
|
||||
int i;
|
||||
|
||||
*p_uint=htonl(value);
|
||||
|
||||
if(value<0)
|
||||
{
|
||||
for(i=0;i<4;i++)
|
||||
{
|
||||
if(buf[i]!=0xFF)
|
||||
{
|
||||
if((buf[i] & 0x80)==0)
|
||||
i--;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for(i=0;i<4;i++)
|
||||
{
|
||||
if(buf[i]!=0)
|
||||
{
|
||||
if((buf[i] & 0x80)==0x80)
|
||||
i--;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(i>=4)
|
||||
i=3;
|
||||
|
||||
return add_tlv(tag_seq,4-i,buf+i,tlv_type,asnbuf);
|
||||
}
|
||||
|
||||
int GetInteger(const char *tag_seq,int *rtn,u_char flag,ASN_BUF *asnbuf)
|
||||
{
|
||||
u_char pvalue[10];
|
||||
int length,i;
|
||||
length=GetTLV(tag_seq,5,pvalue,flag,asnbuf);
|
||||
if(length==-1)
|
||||
return -1;
|
||||
|
||||
if((pvalue[0] & 0x80)==0x80)
|
||||
*rtn=-1;
|
||||
else
|
||||
*rtn=0;
|
||||
|
||||
for(i=0;i<length;i++)
|
||||
*rtn=((*rtn)<<8)|(pvalue[i]);
|
||||
return length;
|
||||
}
|
||||
|
||||
int get_int(const char *tag_seq,ASN_BUF *asnbuf)
|
||||
{
|
||||
u_char pvalue[10];
|
||||
int length,i,value;
|
||||
length=get_tlv(tag_seq,pvalue,asnbuf);
|
||||
if(length==-1)
|
||||
return -1;
|
||||
|
||||
if((pvalue[0] & 0x80)==0x80)
|
||||
value=-1;
|
||||
else
|
||||
value=0;
|
||||
|
||||
for(i=0;i<length;i++)
|
||||
value=(value<<8)|(pvalue[i]);
|
||||
return value;
|
||||
}
|
||||
|
||||
int get_int_v2(const char *tag_seq,int *rtn,ASN_BUF *asnbuf)
|
||||
{
|
||||
u_char pvalue[10];
|
||||
int length,i;
|
||||
length=get_tlv(tag_seq,pvalue,asnbuf);
|
||||
if(length==-1)
|
||||
return -1;
|
||||
|
||||
if((pvalue[0] & 0x80)==0x80)
|
||||
*rtn=-1;
|
||||
else
|
||||
*rtn=0;
|
||||
|
||||
for(i=0;i<length;i++)
|
||||
*rtn=((*rtn)<<8)|(pvalue[i]);
|
||||
return length;
|
||||
}
|
||||
|
||||
int add_null(const char *tag_seq,u_char tlv_type,ASN_BUF *asnbuf)
|
||||
{
|
||||
u_char pvalue[2];
|
||||
return add_tlv(tag_seq,0,pvalue,tlv_type,asnbuf);
|
||||
|
||||
}
|
||||
int get_null(const char *tag_seq,ASN_BUF *asnbuf)
|
||||
{
|
||||
u_char pvalue[50];
|
||||
if(get_tlv(tag_seq,pvalue,asnbuf)>=0)
|
||||
return 1;
|
||||
else
|
||||
return -1;
|
||||
}
|
||||
int add_oid(const char *tag_seq,const char *oidstr,u_char tlv_type,ASN_BUF *asnbuf)
|
||||
{
|
||||
u_char pvalue[50],buf[4];
|
||||
int oid[20],len,i,j,level=0,pos=0;
|
||||
len=strlen(oidstr);
|
||||
memset(oid,0,sizeof(oid));
|
||||
for(i=0;i<len;i++)
|
||||
{
|
||||
if(isdigit(oidstr[i]))
|
||||
oid[level]=oid[level]*10+oidstr[i]-'0';
|
||||
else if(oidstr[i]=='.')
|
||||
level++;
|
||||
}
|
||||
oid[1]=oid[0]*40 + oid[1];
|
||||
for(i=1;i<=level;i++)
|
||||
{
|
||||
j=3;
|
||||
do
|
||||
{
|
||||
|
||||
buf[j]=oid[i] & 0x7F;
|
||||
buf[j]|=0x80;
|
||||
oid[i]=oid[i]>>7;
|
||||
j--;
|
||||
}while(oid[i]>0 && j>=0);
|
||||
buf[3]&=0x7F;
|
||||
len=3-j;
|
||||
memcpy(pvalue+pos,buf+j+1,len);
|
||||
pos+=len;
|
||||
}
|
||||
return add_tlv(tag_seq,pos,pvalue,tlv_type,asnbuf);
|
||||
}
|
||||
|
||||
int get_oid(const char *tag_seq,char *oid,ASN_BUF *asnbuf)
|
||||
{
|
||||
u_char pvalue[50];
|
||||
char tempstr[50],firstoctet=1;
|
||||
int length,i,temp=0;
|
||||
length=get_tlv(tag_seq,pvalue,asnbuf);
|
||||
|
||||
if(length<=0)
|
||||
return -1;
|
||||
|
||||
for(i=0;i<length;i++)
|
||||
{
|
||||
temp=temp * 0x80 +(pvalue[i] & 0x7F);
|
||||
|
||||
if((pvalue[i] & 0x80)==0x00)
|
||||
{
|
||||
if(firstoctet==1)
|
||||
{
|
||||
if(temp<40)
|
||||
sprintf(tempstr,"0.%d",temp);
|
||||
else if(temp<80)
|
||||
sprintf(tempstr,"1.%d",temp-40);
|
||||
else
|
||||
sprintf(tempstr,"2.%d",temp-80);
|
||||
firstoctet=0;
|
||||
}
|
||||
else
|
||||
sprintf(tempstr+strlen(tempstr),".%d",temp);
|
||||
temp=0;
|
||||
}
|
||||
}
|
||||
strcpy(oid,tempstr);
|
||||
return 1;
|
||||
}
|
||||
|
||||
int DecodeInteger(u_char *buf, u_char len)
|
||||
{
|
||||
int ret;
|
||||
u_char i, pad = 0, *ptr = (u_char *)&ret;
|
||||
|
||||
if(buf[0] & 0x80)
|
||||
pad = 0xFF;
|
||||
|
||||
for(i = 0; i < 4; i++)
|
||||
{
|
||||
if(len > 0)
|
||||
{
|
||||
len--;
|
||||
ptr[i] = buf[len];
|
||||
}
|
||||
else
|
||||
ptr[i] = pad;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
/*@end@*/
|
||||
Reference in New Issue
Block a user