942 lines
18 KiB
C
942 lines
18 KiB
C
/********************************************************************/
|
|
/*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@*/
|