sensorm.nc

tinyos密钥管理组件 用在tinyos仿真里的

includes AM;
includes PolyOne;

module SensorM { 
  provides {
	 interface Sensor;
  }
  uses {
  	 interface Channel;
  	 interface ComputeKey;
  	 interface Random;
  	 interface Primitive;
	 interface Leds;
  }
}

implementation {  

	uint16_t sID,dID;
	uint16_t *compromised;
	PolyShare *secret;
 	bool sendflag=0,have;
 	uint8_t Msg[29];
	
// check whether *p and *pmy have the same poly_id  
    result_t check(uint8_t *p,PolyShare *pmy,uint8_t *kk)
    {
	uint8_t i,j;
	bool same;

	same=FALSE;
	i=0;
	while((same==FALSE)&&(i<ss)){
		j=0;
		while((same==FALSE)&&(j<ss)){
	    		if(p[i]==pmy[j].poly_id){
			same=TRUE;
			kk[0]=p[i];  // Id of the same polynomial 
			dbg(DBG_USR1,"same poly_id= %d\n",kk[0]);
			kk[1]=j; // pmy[j] is secret[j]
			}
	 		j++ ;
	  	}
		i++;
	}
	return same;
    }

 command void Sensor.init(uint16_t ID, uint8_t *sec, uint16_t *comp)
 {
 	sID=ID;
 	secret=(PolyShare *)sec;
 	compromised = comp;
	call Leds.init();
	
 }

 command uint8_t Sensor.establish_key(uint16_t ID)
 {
 	uint8_t i;
 	
 	have=0;
 	
  	dID=ID;	
 	Msg[0] = 1; // type1: node_s want to establish key with node_d  
	memcpy(Msg+1,(uint8_t *)&sID,2); // source sensor node
	memcpy(Msg+3,(uint8_t *)&dID,2); // destination node
	for(i=0;i<ss;i++)
		Msg[5+i] = secret[i].poly_id;

	if (sendflag==0) {
		sendflag = 1;
		call Channel.send(dID,Msg);
		call Leds.redOn();	
		dbg(DBG_USR1,"node%d send request to node%d.\n",sID,dID);
		return 0;
	}
	return 1;
 }
 
 event result_t Channel.sendDone(uint8_t *msgP,result_t success){
	sendflag = 0;
	dbg(DBG_USR1,"node %d Send Success ! \n",sID);
	call Leds.redOff();
	return SUCCESS;
 }
   
 event uint8_t* Channel.receive(uint8_t *msg){

	uint16_t node_s,node_d,node;
	uint16_t key[4],keys[4],keyd[4];
 	uint8_t cks[2],ckd[2];
 	uint8_t i,j,type;
	bool same,sames,samed,cp;
	
	type = msg[0];    
	memcpy((uint8_t *)&node_s,msg+1,2); // source sensor node
	memcpy((uint8_t *)&node_d,msg+3,2); // destination node

	call Leds.yellowOn();
	
   	if(type==1){  // type1: request to establish common key!
		dbg(DBG_USR1,"node %d received message from %d. type1: request to establish common key! \n",sID,node_s);

		same = check(msg+5,secret,ckd);
		dbg(DBG_USR1,"same= %d\n",same);
		if(same==1){ // have the same poly_id
			j = ckd[1];  // secret[j]
	    		call ComputeKey.compute((uint8_t *)&secret[j],node_s,(uint8_t *)keyd);
	    		dbg(DBG_USR1,"keyd=%X  %X  %X  %X\n",keyd[0],keyd[1],keyd[2],keyd[3]);
		    	msg[0] = 2; // type2  
		    	msg[5] = ckd[0];  // poly_id
//			memcpy(msg+19, (uint8_t *)keyd, 8); // test
		    	signal Sensor.keyEstablished(node_s,(uint8_t *)keyd);
		}
		else{   // have no common poly_id, send its poly_id to node_s  
			msg[0] = 3; // type3 
			for(i=0;i<ss;i++)
				msg[5+i] = secret[i].poly_id;
    		}

		memcpy(msg+1,(uint8_t *)&sID,2); // source sensor node
		memcpy(msg+3,(uint8_t *)&node_s,2); // destination node
		if (sendflag==0) {
			sendflag=1;
//			call Channel.send(TOS_UART_ADDR,msg); // test
			call Channel.send(node_s,msg);
			dbg(DBG_USR1,"node%d send answer to node%d.\n",sID,node_s);
		}
     	}		
	
	if(type==2){ // type2: have the same poly_id!
     		dbg(DBG_USR1,"node%d received answer from node%d. type2: have the same poly_id!\n",sID,node_s);
		ckd[0] = msg[5];

		i=0;  // find which poly_id is the same poly_id  
		while(ckd[0]!=secret[i].poly_id){
			 i++;
		}
		dbg(DBG_USR1,"find poly_id= %d\n",secret[i].poly_id);
    
       	// compute the common key  
		call ComputeKey.compute((uint8_t *)&secret[i],node_s,(uint8_t *)keys);	
		dbg(DBG_USR1,"keys= %X  %X  %X  %X\n",keys[0],keys[1],keys[2],keys[3]);

		signal Sensor.keyEstablished(dID,(uint8_t *)keys);
		
//		memcpy(msg, (uint8_t *)&sID, 2);  // test
//		memcpy(msg+2, (uint8_t *)&dID, 2);
//		msg[4] = ckd[0];
//  		memcpy(msg+5, (uint8_t *)keys, 8);
//	  	if (sendflag==0) {
//			sendflag=1;
//			node=TOS_UART_ADDR;
//			call Channel.send(node, msg);
//			call Leds.redOn();
//		}	

    	}

    	if(type==3){ // have no same poly_id, need to call pathdiscovery   
		dbg(DBG_USR1,"node %d received answer from node %d. type3: have no same poly_id ! \n",sID,node_s);

		msg[0] = 4; // type4: pathdiscovery 
		memcpy(msg+1,(uint8_t *)&sID,2); // source sensor node
		memcpy(msg+3,(uint8_t *)&node_s,2); // destination node
		for(i=0;i<ss;i++)
			msg[5+ss+i] = secret[i].poly_id;
		msg[5+ss+ss] = 1; // connect times
		node = TOS_BCAST_ADDR; 	// broadcast the message  
//		node = 0xffff;
		if (sendflag==0) {
			call Channel.send(node,msg);
			dbg(DBG_USR1,"node %d broadcast message.\n",sID);
			sendflag=1;
		}
	}
	
	// every sensor which recieved the message check itself
	if((type==4)&&(node_s!=sID)&&(node_d!=sID)) {   
		dbg(DBG_USR1,"node%d received message type4: request to establish path key!\n",sID);
		msg[5+ss+ss] = msg[5+ss+ss]-1;
		sames = check(msg+5+ss,secret,cks);
		samed = check(msg+5,secret,ckd);
		dbg(DBG_USR1,"sames=%d,  samed=%d\n",sames,samed);
		if((sames==1)&&(samed==1)){	// node_my send *cks *shares to node_s  
			j=cks[1];
			i=ckd[1];
			call ComputeKey.compute((uint8_t *)&secret[j], node_s, (uint8_t *)keys);
			call ComputeKey.compute((uint8_t *)&secret[i], node_d, (uint8_t *)keyd);
			// generate a random number
			key[0] = call Random.rand();
			key[1] = call Random.rand();
			key[2] = call Random.rand();
			key[3] = call Random.rand();
			dbg(DBG_USR1,"random key=%X  %X  %X  %X\n",key[0],key[1],key[2],key[3]);
			// encrypt key[0] with keys and keyd separately
			call Primitive.encrypt((uint8_t *)keys, (uint8_t *)key, (uint8_t *)keys);
			call Primitive.encrypt((uint8_t *)keyd, (uint8_t *)key, (uint8_t *)keyd);
			
			msg[0] = 5; // type5: send path key  
			memcpy(msg+1,(uint8_t *)&sID,2); // source sensor node
			memcpy(msg+3,(uint8_t *)&node_s,2); // destination node
		    	msg[5] = ckd[0];
		    	memcpy(msg+6,(uint8_t *)keyd,8);
			msg[14] = cks[0];
			memcpy(msg+15,(uint8_t *)keys,8);
	      		if(sendflag==0) {
				call Channel.send(node_s,msg);
//				call Channel.send(TOS_UART_ADDR, msg);  // test
				dbg(DBG_USR1,"node%d send answer to node%d.\n",sID,node_s);
	      			sendflag = 1;
			}
		}
		else if(msg[5+ss+ss]>0){  //connect times
			node = TOS_BCAST_ADDR;
//			node = 0xffff;
			if (sendflag==0) {
				call Channel.send(node,msg);
				dbg(DBG_USR1,"node %d broadcast further.\n",sID);
				sendflag=1;
			}
		}
	}

	if(type==5){ // receive path key result 
		dbg(DBG_USR1,"node%d received path key from node%d. type5 \n",sID,node_s);
		cp=0;
		for(i=0;i<ss*(T+1);i++) {
			if (node_s == compromised[i]) {
				cp=1;
				dbg(DBG_USR1,"!!! node_s %d is compromised, remove it\n", node_s);
				break;
			}
		}
		if(have==0&&cp==0) {
			cks[0] = msg[14]; // same poly_id of node_s 
			// encrypted message
			
			i=0;  // find which poly_id is the same poly_id  
			while(cks[0]!=secret[i].poly_id){
				 i++;
			}
			dbg(DBG_USR1,"find poly_id= %d\n",secret[i].poly_id);
		       	// compute the common key  
			call ComputeKey.compute((uint8_t *)&secret[i],node_s,(uint8_t *)keys);	
			dbg(DBG_USR1,"keys= %X  %X  %X  %X\n",keys[0],keys[1],keys[2],keys[3]);
			// decrypt key with keys
  			call Primitive.decrypt((uint8_t *)keys, msg+15, (uint8_t *)key);			
  			dbg(DBG_USR1,"key=%X  %X  %X  %X\n",key[0],key[1],key[2],key[3]);
  			
			msg[0] = 6;
			memcpy(msg+1,(uint8_t *)&sID,2); // source sensor node
			memcpy(msg+3,(uint8_t *)&dID,2); // destination node
			memcpy(msg+14,(uint8_t *)&node_s,2); // intermediate node_id
//		      		memcpy(msg+16, (uint8_t *)key, 8); // test
		      		
	      		if(sendflag==0) {
				call Channel.send(dID,msg);
				dbg(DBG_USR1,"node%d send answer to node%d.\n",sID,dID);
	      			sendflag = 1;
			}

			have=1;
			signal Sensor.keyEstablished(dID,(uint8_t *)key);
		}
	}

	if(type==6) {
		dbg(DBG_USR1,"node%d received path key from node%d. type6 \n",sID,node_s);
		memcpy((uint8_t *)&node, msg+14,2);
		ckd[0] = msg[5]; // same poly_id of node_s 

		i=0;  // find which poly_id is the same poly_id  
		while(ckd[0]!=secret[i].poly_id){
			 i++;
		}
		dbg(DBG_USR1,"find poly_id= %d\n",secret[i].poly_id);
    
       		// compute the common key  
		call ComputeKey.compute((uint8_t *)&secret[i],node,(uint8_t *)keyd);	
		dbg(DBG_USR1,"keyd= %X  %X  %X  %X\n",keyd[0],keyd[1],keyd[2],keyd[3]);
		// decrypt the key with keyd
		call Primitive.decrypt((uint8_t *)keyd, msg+6, (uint8_t *)key);	
		dbg(DBG_USR1,"key=%X  %X  %X  %X\n",key[0],key[1],key[2],key[3]);

		signal Sensor.keyEstablished(node_s,(uint8_t *)key);
		
//		memcpy(msg, (uint8_t *)&sID, 2); // test
//		memcpy(msg+2, (uint8_t *)&node_s, 2);
//		memcpy(msg+4, (uint8_t *)key, 8);
//		memset(msg+12,0,2);
//		if (sendflag==0) {
//			sendflag=1;
//			call Channel.send(TOS_UART_ADDR, msg);
//		}				
	}
	
//	call Leds.yellowOff();
    return msg;
 }
   
}