leap_nas.c

MD5、RSA加解密,主要用在Linux環境執行。有編寫makefile提供編譯

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
#include <errno.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <openssl/rsa.h>
#include <openssl/md5.h>

#include "utility.h"

#define READ_STATUS  0
#define WRITE_STATUS 1
#define EXCPT_STATUS 2
#define SOCKET int
#define MAX_STRING_LEN  1024
#define BUFSIZE  4096

static char i_host[MAX_STRING_LEN];  /* site name */
static char i_port[MAX_STRING_LEN];  /* port number */
char buf[BUFSIZE];

int sendKeyToController(int keylen, unsigned char key[]);
SOCKET xnet_select(SOCKET s, int sec, int usec, short x);
int tcp_connect(const char *host, const unsigned short port);
int SendData(SOCKET fd,char *buffer,int len);
int ReadData(SOCKET fd,char *buffer,int len);
int GetPubKey(unsigned char *pucPubKeyData,unsigned long KeyDataLen, RSA* *pubRsa);
int decSessionKeybyRsaPubKey(RSA *rsa, unsigned char *InData, unsigned long ulDataLen,
							 unsigned char *ucKey, unsigned long *pulKeyLen);

int encSessionKeybyRsaPriKey(RSA *rsa, unsigned char *ucKey, unsigned long ulKeyLen,
							 unsigned char *outData, unsigned long *pulOutLen)
{
	return (*pulOutLen = RSA_private_encrypt(ulKeyLen, ucKey, outData, rsa, RSA_PKCS1_PADDING));
}


int decSessionKeybyRsaPubKey(RSA *rsa, unsigned char *InData, unsigned long ulDataLen,
							 unsigned char *ucKey, unsigned long *pulKeyLen)
{
	return (*pulKeyLen = RSA_public_decrypt(ulDataLen, InData, ucKey, rsa, 1));
}

int encSessionKeybyRsaPubKey(RSA *rsa, unsigned char *ucKey, unsigned long ulKeyLen,
							 unsigned char *outData, unsigned long *pulOutLen)
{
	return (*pulOutLen = RSA_public_encrypt(ulKeyLen, ucKey, outData, rsa, 1));
}

static char *hexstr(unsigned char *buf,int len)

{	
    const char *set = "0123456789abcdef";	
    static char str[65],*tmp;	
    unsigned char *end;	
    if (len > 32)		
        len = 32;	
    end = buf + len;	
    tmp = &str[0];	
    while (buf < end)		
    {		
        *tmp++ = set[ (*buf) >> 4 ];		
        *tmp++ = set[ (*buf) & 0xF ];
        buf ++;		
    }	
    *tmp = 0;	
    return str;	
}

void xMD5(unsigned char *ucData,unsigned long DataLen,char *MD5_Result)
{
	unsigned char *md;
	
	md = MD5(ucData,DataLen,NULL);
	sprintf(MD5_Result,"%s",hexstr(md,MD5_DIGEST_LENGTH));
}

int GetPriKey(unsigned char *pucPriKeyData, unsigned long KeyDataLen, RSA* *priRsa)
{
	const unsigned char *Pt = pucPriKeyData;
	*priRsa = d2i_RSAPrivateKey(NULL, &Pt, KeyDataLen);
	if(priRsa == NULL)
	{
		printf("priRsa == NULL!\n");
		return 0x22;
	}
	return 0;
}

int GetPubKey(unsigned char *pucPubKeyData,unsigned long KeyDataLen, RSA* *pubRsa)
{
	const unsigned char *Pt = pucPubKeyData;
	*pubRsa = d2i_RSAPublicKey(NULL, &Pt, KeyDataLen);
	if(pubRsa == NULL)
	{
		printf("pubRsa == NULL!\n");
		return 0x31;
	}

	return 0;
}

int ReadData(SOCKET fd,char *buffer,int len)
{
	int n;
	if (xnet_select(fd, 3, 0, READ_STATUS)>0)
    {
        /* display the server response */
        memset(buffer,0,len);
        n = read(fd, buffer, len);
	}
        else
    {
        return 0;
    }
		return  n;
}

int SendData(SOCKET fd,char *buffer,int len)
{
    if (xnet_select(fd, 0, 500, WRITE_STATUS)>0)
    {
        write(fd, buffer, len);
    }
    else
    {
        //WriteLog("Socket I/O Write Timeout %s:%s", i_host, i_port);
        return 0;
    }
    return 1;	
}




int tcp_connect(const char *host, const unsigned short port)
{
	unsigned long non_blocking = 1;
    unsigned long blocking = 0;
    int ret = 0;
    char * transport = "tcp";
    struct hostent      *phe;   /* pointer to host information entry    */
    struct protoent *ppe;       /* pointer to protocol information entry*/
    struct sockaddr_in sin;     /* an Internet endpoint address  */
    SOCKET s;                    /* socket descriptor and socket type    */
    int error;
    

    memset(&sin, 0, sizeof(sin));
    sin.sin_family = AF_INET;

    if ((sin.sin_port = htons(port)) == 0)
    {
        WriteLog("invalid port \"%d\"", port);
        abort();
    }

    /* Map host name to IP address, allowing for dotted decimal */
    if ( phe = gethostbyname(host) )
        memcpy(&sin.sin_addr, phe->h_addr, phe->h_length);
    else
        if ( (sin.sin_addr.s_addr = inet_addr(host)) == INADDR_NONE )
        {
            WriteLog("can't get \"%s\" host entry", host);
            abort();
        }

    /* Map transport protocol name to protocol number */
    if ( (ppe = getprotobyname(transport)) == 0)
    {
        WriteLog("can't get \"%s\" protocol entry", transport);
        abort();
    }

    /* Allocate a socket */
    s = socket(PF_INET, SOCK_STREAM, ppe->p_proto);
    if (s < 0)
    {
        WriteLog("can't create socket: %s", strerror(errno));
        abort();
    }
    ioctl(s,FIONBIO,&non_blocking);
   if (connect(s, (struct sockaddr *)&sin, sizeof(sin)) == -1)
    {
        struct timeval tv;
        fd_set writefds;
        tv.tv_sec = 10; 
        tv.tv_usec = 0;
        FD_ZERO(&writefds);
        FD_SET(s, &writefds);
        if (select(s+1,NULL,&writefds,NULL,&tv) != 0)
        {
            if (FD_ISSET(s,&writefds))
            {
                int len=sizeof(error);
				if (getsockopt(s, SOL_SOCKET, SO_ERROR,  (char *)&error, &len) < 0)
                    goto error_ret;
                if (error != 0)
                    goto error_ret;
            }
            else
                goto error_ret; //timeout or error happen
        }
        else goto error_ret; ;
        ioctl(s,FIONBIO,&blocking);
    }
    else
    {
error_ret:
        close(s);
        WriteLog("can't connect to %s:%d", host, port);
        abort();
    }
    return s;
}

SOCKET xnet_select(SOCKET s, int sec, int usec, short x)
{
    int st = errno;
    struct timeval to;
    fd_set fs;
    to.tv_sec = sec;
    to.tv_usec = usec;
    FD_ZERO(&fs);
    FD_SET(s, &fs);
    switch (x)
    {
    case READ_STATUS:
        st = select(s+1, &fs, 0, 0, &to);
        break;
    case WRITE_STATUS:
        st = select(s+1, 0, &fs, 0, &to);
        break;
    case EXCPT_STATUS:
        st = select(s+1, 0, 0, &fs, &to);
        break;
    }
    return(st);
}

int main (int argc, char *argv[])
{
    SOCKET fd;
    int i,j,n,ret;
    FILE* fp;
    char szSN[64],szHostName[64],szKeyServerIP[32],szPort[10];
    char szActiveKey[128];
	RSA *pRsaPubKey_KMC = NULL;
	RSA *pRsaPriKey_NAS = NULL;
	RSA *pRsaPubKey_NAS = NULL;
	int PubKey_NAS_len=0; 
	int PriKey_NAS_len = 0;
    unsigned long encrypted_len = 0;
    unsigned char ucEncryptedKey[4096] = {0};
    unsigned long decrypted_len = 0;
    unsigned char ucDecryptedKey[4096] = {0};
	unsigned char ucKey[4096] = {0};
	char szPriMD5[64] = {0};
	char szPubMD5[64] ={0};

	WriteLog("Program start.");

   	fp = fopen("/etc/sn.txt","r");
	if (fp==NULL) {
		WriteLog("Can't open sn.txt file.");
		return(0);
	}
    get_param_value(fp,"SN",szSN,"=");
    get_param_value(fp,"HOSTNAME",szHostName,"=");
    get_param_value(fp,"KEYSERVERIP",szKeyServerIP,"=");
    get_param_value(fp,"PORT",szPort,"=");
    WriteLog("Serial Number = %s",szSN);
    WriteLog("Host Name = %s",szHostName);
    WriteLog("Key Server IP = %s",szKeyServerIP);
    WriteLog("Key Server Port = %s",szPort);
	fclose(fp);
    strcpy(i_host,szKeyServerIP);
    strcpy(i_port,szPort);

    /* make connection to the server */
    fd = tcp_connect(szKeyServerIP, (unsigned short)atoi(szPort));
    //fd = tcp_connect(i_host, (unsigned short)atoi(i_port));

	// connected .....

	// INIT_REG , contain serial number and host name
	strcpy(buf,szSN);
	strcat(buf,"-");
	strcat(buf,szHostName);

	if (SendData(fd,buf,strlen(buf))==0)
	{
		WriteLog("Socket I/O Write Timeout %s:%s", i_host, i_port);
		return 0;
	}
	n = ReadData(fd,buf,BUFSIZE);
	if (strstr(buf,"OK")==0) {
		WriteLog("Error : %s",buf);
		return 0;
	}
	else
		WriteLog("Receive OK message,Waitting for KMC-pub key");
	// Waitting for KMC-pub
	n = ReadData(fd,buf,BUFSIZE);
	WriteLog("Receve KMC-pub, size=%d",n);
	fp = fopen("public.key","wb");
	if (fp==NULL) {
		WriteLog("Can't create public.key file");
		return 0;
	}
	fwrite(buf,1,n,fp);

	pRsaPubKey_KMC = RSA_new();
    GetPubKey(buf, n, &pRsaPubKey_KMC);
	memset(buf,0,sizeof(buf));
	strcpy(buf,szSN);
	strcat(buf,"-");
	strcat(buf,szHostName);
	WriteLog("encSessionKeybyRsaPubKey-->%s",buf);
	encSessionKeybyRsaPubKey(pRsaPubKey_KMC, buf, strlen(buf), ucEncryptedKey, &encrypted_len);	
    if (xnet_select(fd, 0, 500, WRITE_STATUS)>0)
    {
       n = write(fd, ucEncryptedKey, encrypted_len);
    }
    else
    {
        WriteLog("Socket I/O Write Timeout %s:%s", i_host, i_port);
        return 0;
    }
	fclose(fp);
	WriteLog("Waitting for DK-pub/prk");
	n = ReadData(fd,buf,BUFSIZE);
	WriteLog("Private Key size=%s",buf);
	PriKey_NAS_len = atoi(buf);
	WriteLog("Waitting for NAS private key...");
	i = atoi(buf);
	j = 0;
	fp = fopen("pri.key","wb");
	while (j<i) 
	{
		n = ReadData(fd,buf,BUFSIZE);
		if (n==0) {
			WriteLog("Receive NAS Private Key error");
			return 0;
		}
		decSessionKeybyRsaPubKey(pRsaPubKey_KMC, buf, n, ucDecryptedKey, &decrypted_len);
		j += decrypted_len;
		fwrite(ucDecryptedKey,1,decrypted_len,fp);
		SendData(fd,"OK",2);  
	}
	WriteLog("done.");
	fclose(fp);
	n = ReadData(fd,buf,BUFSIZE);
	WriteLog("Public Key size=%s",buf);
	WriteLog("Waitting for NAS public key...");
	i = atoi(buf);
	PubKey_NAS_len =i;
	j = 0;
	
	fp = fopen("pub.key","wb");
	while (j<i) 
	{
		n = ReadData(fd,buf,BUFSIZE);
		if (n==0) {
			WriteLog("Recevie NAS Public key error");
			return 0;
		}
		decSessionKeybyRsaPubKey(pRsaPubKey_KMC, buf, n, ucDecryptedKey, &decrypted_len);
		j += decrypted_len;
		fwrite(ucDecryptedKey,1,decrypted_len,fp);
		SendData(fd,"OK",2);  
	}
	WriteLog("done.");
	fclose(fp);
	pRsaPriKey_NAS = RSA_new();
	pRsaPubKey_NAS = RSA_new();
	fp = fopen("pri.key","rb");
	fread(ucKey,1,PriKey_NAS_len,fp);
	xMD5(ucKey,PriKey_NAS_len,szPriMD5);
	GetPriKey(ucKey, PriKey_NAS_len, &pRsaPriKey_NAS);
	memset(ucKey,0,sizeof(ucKey));
	fclose(fp);
	fp = fopen("pub.key","rb");
	fread(ucKey,1,PubKey_NAS_len,fp);
	xMD5(ucKey,PubKey_NAS_len,szPubMD5);
	GetPubKey(ucKey, PubKey_NAS_len, &pRsaPubKey_NAS);
	memset(ucKey,0,sizeof(ucKey));
	fclose(fp);

	WriteLog("------MD5 Calc Result------");
	WriteLog("Private Key MD5=%s",szPriMD5);
	WriteLog("Public Key MD5=%s",szPubMD5);
	WriteLog("-----------------------------------");
	// Receive MD5 for Public / Private Key
	WriteLog("Waiting for Private/Public Key(MD5) data from KMC");
	n = ReadData(fd,buf,BUFSIZE);
	decSessionKeybyRsaPubKey(pRsaPubKey_KMC, buf, n, ucDecryptedKey, &decrypted_len);
	WriteLog("Private Key[MD5] =%s",ucDecryptedKey);
	SendData(fd,"OK",2);  
	n = ReadData(fd,buf,BUFSIZE);
	decSessionKeybyRsaPubKey(pRsaPubKey_KMC, buf, n, ucDecryptedKey, &decrypted_len);
	WriteLog("Public Key[MD5] =%s",ucDecryptedKey);

	encSessionKeybyRsaPriKey(pRsaPriKey_NAS, szPriMD5, strlen(szPriMD5), ucEncryptedKey, &encrypted_len);
    if (xnet_select(fd, 0, 500, WRITE_STATUS)>0)
    {
       n = write(fd, ucEncryptedKey, encrypted_len);
    }
    else
    {
        WriteLog("Socket I/O Write Timeout %s:%s", i_host, i_port);
        return 0;
    }
	n = ReadData(fd,buf,BUFSIZE);  // Waiting from OK from KMC
	encSessionKeybyRsaPriKey(pRsaPriKey_NAS, szPubMD5, strlen(szPubMD5), ucEncryptedKey, &encrypted_len);
    if (xnet_select(fd, 0, 500, WRITE_STATUS)>0)
    {
       n = write(fd, ucEncryptedKey, encrypted_len);
    }
    else
    {
        WriteLog("Socket I/O Write Timeout %s:%s", i_host, i_port);
        return 0;
    }
	n = ReadData(fd,buf,BUFSIZE);  // Waiting from OK from KMC

	// Got_DK_KP_Msg
	memset(buf,0,sizeof(buf));
	memset(ucEncryptedKey,0,sizeof(ucEncryptedKey));
	strcpy(buf,"REQUIRE ACTIVE KEY");
	encSessionKeybyRsaPriKey(pRsaPriKey_NAS, buf, strlen(buf), ucEncryptedKey, &encrypted_len);	
    if (xnet_select(fd, 0, 500, WRITE_STATUS)>0)
    {
       n = write(fd, ucEncryptedKey, encrypted_len);
    }
    else
    {
        WriteLog("Socket I/O Write Timeout %s:%s", i_host, i_port);
        return 0;
    }
	WriteLog("Waitting for active key");
	n = ReadData(fd,buf,BUFSIZE);
	memset(ucDecryptedKey,0,sizeof(ucDecryptedKey));
	decSessionKeybyRsaPubKey(pRsaPubKey_KMC, buf, n, ucDecryptedKey, &decrypted_len);
	WriteLog("Receive active key, length=%d",strlen(ucDecryptedKey));
	for (i=0;i<decrypted_len;i++) {
		WriteLog("Key = %2x",ucDecryptedKey[i]);
	}
	memset(buf,0,sizeof(buf));
	memset(ucEncryptedKey,0,sizeof(ucEncryptedKey));
	int sendResult = sendKeyToController(decrypted_len,ucDecryptedKey);
	if (sendResult==0)
	{
		// fail
		WriteLog("Send active key to device fail !");
		strcpy(buf,"ERROR");
	}
	else 
	{
		strcpy(buf,"GOT_THE_DK");
	}

	
	encSessionKeybyRsaPriKey(pRsaPriKey_NAS, buf, strlen(buf), ucEncryptedKey, &encrypted_len);	
    if (xnet_select(fd, 0, 500, WRITE_STATUS)>0)
    {
       n = write(fd, ucEncryptedKey, encrypted_len);
    }
    else
    {
        WriteLog("Socket I/O Write Timeout %s:%s", i_host, i_port);
        return 0;
    }
	if (sendResult==0) {
		return 0;
	}
	WriteLog("Waitting for commit");
	n = ReadData(fd,buf,BUFSIZE);
	memset(ucDecryptedKey,0,sizeof(ucDecryptedKey));
	decSessionKeybyRsaPubKey(pRsaPubKey_KMC, buf, n, ucDecryptedKey, &decrypted_len);
	WriteLog("Recevie commit message : %s",ucDecryptedKey);
	remove("pri.key");
	remove("pub.key");
	remove("public.key");
	return(0);
}