sensorgm.nc

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

includes AM;
includes PolyOne;

module SensorGM { 
  provides {
	 interface Sensor;
  }
  uses {
  	 interface Channel;
  	 interface ComputeKey;
  	 interface Primitive;
  }
}

implementation {  

	uint16_t ID;       //node ID
	uint16_t ctr=0;	   //initial counter
	Secret_Cefts *secret; //secret coefficient of this sensor
	uint8_t *comp_list;//compromised list, one bit per node 64 bytes for 256 raw and 256 column enough!!!

	uint8_t sendflag=0;
 	uint8_t Msg[17];
	uint8_t rnd[8];
	
	command void Sensor.init(uint16_t sID, uint8_t *sec, uint16_t *cl)
	{
	 	ID=sID;
	 	secret=(Secret_Cefts *)sec;
	 	comp_list=(uint8_t*)cl;
	 	call Primitive.PRG(rnd);//generate a random number
	}
 
	command uint8_t Sensor.establish_key(uint16_t dID)
	{
		uint8_t key[8];
		uint16_t tmp_id;
	 	if((ID&0xff)==(dID&0xff)) {
			dbg(DBG_USR1,"node %d have common subr with node %d.\n",ID,dID);
	    		call ComputeKey.compute((uint8_t *)&(secret->subr),dID>>8,key);
			signal Sensor.keyEstablished(dID,key);
			return 0;
	 	} else if((ID>>8)==(dID>>8)) {
			dbg(DBG_USR1,"node %d have common subc with node %d.\n",ID,dID);
	    		call ComputeKey.compute((uint8_t *)&(secret->subc),dID&0xff,key);
			signal Sensor.keyEstablished(dID,key);
			return 0;
		} else if(sendflag<2) {
			dbg(DBG_USR1,"node %d try to setup a key with node %d.\n",ID,dID);
	 		Msg[0] = 1; 

	 		if (!(comp_list[32+(dID>>11)]&(0x1<<((dID>>8)&0x7)))) {
	 			tmp_id=(ID&0xff)+(dID&0xff00);
		    		call ComputeKey.compute((uint8_t *)&(secret->subr),dID>>8,key);
		    		dbg(DBG_USR1,"intermediate node 2: %d.\n",tmp_id);
	 		} else if(!(comp_list[(dID&0xff)>>3]&(0x1<<(dID&0x7)))) {
	 			tmp_id=(ID&0xff00)+(dID&0xff);
	 			call ComputeKey.compute((uint8_t *)&(secret->subc),dID&0xff,key);
	 			dbg(DBG_USR1,"intermediate node 1: %d. (%d,%d)\n",tmp_id,ID,dID);
	 		} else return 1;
			ctr++;
	 		memcpy(Msg+1,(uint8_t *)&tmp_id,2);
 			memcpy(Msg+3,(uint8_t *)&ID,2);
	 		memcpy(Msg+5,(uint8_t *)&dID,2);
	 		memcpy(Msg+7,(uint8_t *)&ctr,2);
			call Primitive.PRF(rnd,ctr,Msg+9); //generate K_c
			dbg(DBG_USR1,"random generated key %X%X%X%X%X%X%X%X.\n",Msg[9],Msg[10],Msg[11],Msg[12],Msg[13],Msg[14],Msg[15],Msg[16]);
			call Primitive.encrypt(key,Msg+9,Msg+9);
			dbg(DBG_USR1,"node %d send request to node %d.\n",ID,tmp_id);
			if(!sendflag) {
				call Channel.send(TOS_BCAST_ADDR,Msg);	
				sendflag=1;
				return 0;
			} else sendflag=2;
		} 
		return 2;
 	}
 
	event result_t Channel.sendDone(uint8_t *msgP,result_t success){
		sendflag--;
		dbg(DBG_USR1,"node %d Send Success ! \n",ID);
		if(sendflag) {
			if(Msg[0]==1) {
				call Channel.send(TOS_BCAST_ADDR,Msg);	
			} else if(Msg[0]==2) {
				call Channel.send(TOS_BCAST_ADDR,Msg);	
			} else sendflag=0;
		}
		return SUCCESS;
	}

	event uint8_t* Channel.receive(uint8_t *msg){
		uint8_t key[8];

		uint16_t tmp_id;
		uint16_t src_id;
		uint16_t dst_id;
		uint16_t ctr_tp;
		
		memcpy((uint8_t *)&tmp_id,msg+1,2);
		memcpy((uint8_t *)&src_id,msg+3,2);
		memcpy((uint8_t *)&dst_id,msg+5,2);

		if(msg[0]==1&&sendflag<2) {
			if(ID!=tmp_id) return msg;
			dbg(DBG_USR1,"node %d received message from %d to setup key with %d\n",tmp_id,src_id,dst_id);
			Msg[0]=2;	//path discovery
			memcpy(Msg+1,msg+1,16);

			if((tmp_id&0xff)==(src_id&0xff) && (tmp_id>>8)==(dst_id>>8)) {
		   		call ComputeKey.compute((uint8_t *)&secret->subr,src_id>>8,key);
				call Primitive.decrypt(key,Msg+9,Msg+9);
				dbg(DBG_USR1,"random generated key %X%X%X%X%X%X%X%X.\n",Msg[9],Msg[10],Msg[11],Msg[12],Msg[13],Msg[14],Msg[15],Msg[16]);
		   		call ComputeKey.compute((uint8_t *)&secret->subc,dst_id&0xff,key);
		    		call Primitive.encrypt(key,Msg+9,Msg+9);
		    		dbg(DBG_USR1,"find right path at 1 \n");
			} else if ((tmp_id&0xff)==(dst_id&0xff) && (tmp_id>>8)==(src_id>>8)) {
		   		call ComputeKey.compute((uint8_t *)&secret->subc,src_id&0xff,key);
				call Primitive.decrypt(key,Msg+9,Msg+9);
		   		call ComputeKey.compute((uint8_t *)&secret->subr,dst_id>>8,key);
		    		call Primitive.encrypt(key,Msg+9,Msg+9);
		    		dbg(DBG_USR1,"find right path at 2 \n");
			} else return msg;
			if(!sendflag) {
				dbg(DBG_USR1,"node %d send %d message to help him establish key with %d \n",tmp_id,dst_id,src_id);
				call Channel.send(TOS_BCAST_ADDR,Msg);	
				sendflag=1;
			} else {
				sendflag=2;
			}
		} else if(msg[0]==2) {
			if(dst_id!=ID) return msg;
			memcpy(Msg+1,msg+1,16);
			if((ID&0xff)==(tmp_id&0xff)) {
		   		call ComputeKey.compute((uint8_t *)&secret->subr,tmp_id&0xff,key);
				call Primitive.decrypt(key,Msg+9,Msg+9);
			} else if((ID>>8)==(tmp_id>>8)) {
		   		call ComputeKey.compute((uint8_t *)&secret->subc,tmp_id>>8,key);
		   		dbg(DBG_USR1,"cypher txt %X%X%X%X%X%X%X%X.\n",Msg[9],Msg[10],Msg[11],Msg[12],Msg[13],Msg[14],Msg[15],Msg[16]);
				call Primitive.decrypt(key,Msg+9,Msg+9);
				dbg(DBG_USR1,"random generated key %X%X%X%X%X%X%X%X.\n",Msg[9],Msg[10],Msg[11],Msg[12],Msg[13],Msg[14],Msg[15],Msg[16]);
			} else return msg;
			dbg(DBG_USR1,"key between %d and %d through %d decrypted\n",src_id,dst_id,tmp_id);
			memcpy((uint8_t *)&ctr_tp,msg+7,2);
			signal Sensor.keyEstablished(src_id,key);
		}
    		return msg;
	}
}