olsrd_secure.c

wifi 无线网络路由协议OLSR linux下C代码

int
add_signature(olsr_u8_t *pck, int *size)
{
  struct s_olsrmsg *msg;
#ifdef DEBUG
  int i, j;
  char *sigmsg;                                                                                        
#endif
  
  olsr_printf(2, "[ENC]Adding signature for packet size %d\n", *size);
  fflush(stdout);
  
  msg = (struct s_olsrmsg *)&pck[*size];
  /* Update size */
  ((struct olsr*)pck)->olsr_packlen = htons(*size + sizeof(struct s_olsrmsg));
  
  /* Fill packet header */
  msg->olsr_msgtype = MESSAGE_TYPE;
  msg->olsr_vtime = 0;
  msg->olsr_msgsize = htons(sizeof(struct s_olsrmsg));
  memcpy(&msg->originator, &olsr_cnf->main_addr, olsr_cnf->ipsize);
  msg->ttl = 1;
  msg->hopcnt = 0;
  msg->seqno = htons(get_msg_seqno());
  
  /* Fill subheader */
  msg->sig.type = ONE_CHECKSUM;
  msg->sig.algorithm = SCHEME;
  memset(&msg->sig.reserved, 0, 2);
  
  /* Add timestamp */
  msg->sig.timestamp = htonl(now.tv_sec);
  olsr_printf(3, "[ENC]timestamp: %ld\n", now.tv_sec);
  
  /* Set the new size */
  *size = *size + sizeof(struct s_olsrmsg);
  
  {
  olsr_u8_t checksum_cache[512 + KEYLENGTH];
  /* Create packet + key cache */
  /* First the OLSR packet + signature message - digest */
  memcpy(checksum_cache, pck, *size - SIGNATURE_SIZE);
  /* Then the key */
  memcpy(&checksum_cache[*size - SIGNATURE_SIZE], aes_key, KEYLENGTH);
  
  /* Create the hash */
  CHECKSUM(checksum_cache, (*size - SIGNATURE_SIZE) + KEYLENGTH, &pck[*size - SIGNATURE_SIZE]);
  } 

#ifdef DEBUG
  olsr_printf(1, "Signature message:\n");

  j = 0;
  sigmsg = (char *)msg;

  for(i = 0; i < sizeof(struct s_olsrmsg); i++)
    {
      olsr_printf(1, "  %3i", (u_char) sigmsg[i]);
      j++;
      if(j == 4)
	{
	  olsr_printf(1, "\n");
	  j = 0;
	}
    }
#endif

  olsr_printf(3, "[ENC] Message signed\n");

  return 1;
}



int
validate_packet(char *pck, int *size)
{
  int packetsize;
  olsr_u8_t sha1_hash[SIGNATURE_SIZE];
  struct s_olsrmsg *sig;
  time_t rec_time;

#ifdef DEBUG
  int i, j;
  char *sigmsg;
#endif

  /* Find size - signature message */
  packetsize = *size - sizeof(struct s_olsrmsg);

  if(packetsize < 4)
    return 0;

  sig = (struct s_olsrmsg *)&pck[packetsize];

  //olsr_printf(1, "Size: %d\n", packetsize);

#ifdef DEBUG
  olsr_printf(1, "Input message:\n");
  
  j = 0;
  sigmsg = (char *)sig;

  for(i = 0; i < sizeof(struct s_olsrmsg); i++)
    {
      olsr_printf(1, "  %3i", (u_char) sigmsg[i]);
      j++;
      if(j == 4)
	{
	  olsr_printf(1, "\n");
	  j = 0;
	}
    }
#endif

  /* Sanity check first */
  if((sig->olsr_msgtype != MESSAGE_TYPE) || 
     (sig->olsr_vtime) ||
     (sig->olsr_msgsize != ntohs(sizeof(struct s_olsrmsg))) ||
     (sig->ttl != 1) ||
     (sig->hopcnt != 0))
    {
      olsr_printf(1, "[ENC]Packet not sane!\n");
      return 0;
    }

  /* Check scheme and type */
  switch(sig->sig.type)
    {
    case(ONE_CHECKSUM):
      switch(sig->sig.algorithm)
	{
	case(SCHEME):
	  goto one_checksum_SHA; /* Ahhh... fix this */
	  break;
	  
	}
      break;

    default:
      olsr_printf(1, "[ENC]Unsupported sceme: %d enc: %d!\n", sig->sig.type, sig->sig.algorithm);
      return 0;
      break;

    }
  //olsr_printf(1, "Packet sane...\n");

 one_checksum_SHA:
 
 {
  olsr_u8_t checksum_cache[512 + KEYLENGTH];
  /* Create packet + key cache */
  /* First the OLSR packet + signature message - digest */
  memcpy(checksum_cache, pck, *size - SIGNATURE_SIZE);
  /* Then the key */
  memcpy(&checksum_cache[*size - SIGNATURE_SIZE], aes_key, KEYLENGTH);

  /* generate SHA-1 */
  CHECKSUM(checksum_cache, *size - SIGNATURE_SIZE + KEYLENGTH, sha1_hash);
 }

#ifdef DEBUG
  olsr_printf(1, "Recevied hash:\n");
  
  sigmsg = (char *)sig->sig.signature;

  for(i = 0; i < SIGNATURE_SIZE; i++)
    {
      olsr_printf(1, " %3i", (u_char) sigmsg[i]);
    }
  olsr_printf(1, "\n");

  olsr_printf(1, "Calculated hash:\n");
  
  sigmsg = sha1_hash;

  for(i = 0; i < SIGNATURE_SIZE; i++)
    {
      olsr_printf(1, " %3i", (u_char) sigmsg[i]);
    }
  olsr_printf(1, "\n");
#endif

  if(memcmp(sha1_hash, sig->sig.signature, SIGNATURE_SIZE) != 0)
    {
      olsr_printf(1, "[ENC]Signature missmatch\n");
      return 0;
    }

  /* Check timestamp */
  rec_time = ntohl(sig->sig.timestamp);

  if(!check_timestamp((union olsr_ip_addr *)&sig->originator, rec_time))
    {
      olsr_printf(1, "[ENC]Timestamp missmatch in packet from %s!\n",
		  olsr_ip_to_string((union olsr_ip_addr *)&sig->originator));
      return 0;
    }

  olsr_printf(1, "[ENC]Received timestamp %ld diff: %ld\n", rec_time, now.tv_sec - rec_time);

  /* Remove signature message */
  *size = packetsize;
  return 1;
}


int
check_timestamp(union olsr_ip_addr *originator, time_t tstamp)
{
  struct stamp *entry;
  int diff;

  entry = lookup_timestamp_entry(originator);

  if(!entry)
    {
      /* Initiate timestamp negotiation */

      send_challenge(originator);

      return 0;
    }

  if(!entry->validated)
    {
      olsr_printf(1, "[ENC]Message from non-validated host!\n");
      return 0;
    }

  diff = entry->diff - (now.tv_sec - tstamp);

  olsr_printf(3, "[ENC]Timestamp slack: %d\n", diff);

  if((diff > UPPER_DIFF) || (diff < LOWER_DIFF))
    {
      olsr_printf(1, "[ENC]Timestamp scew detected!!\n");
      return 0;
    }

  /* ok - update diff */
  entry->diff = ((now.tv_sec - tstamp) + entry->diff) ?
    ((now.tv_sec - tstamp) + entry->diff) / 2 : 0;

  olsr_printf(3, "[ENC]Diff set to : %d\n", entry->diff);

  /* update validtime */

  entry->valtime = GET_TIMESTAMP(TIMESTAMP_HOLD_TIME * 1000);

  return 1;
}


/**
 * Create and send a timestamp
 * challenge message to new_host
 *
 * The host is registered in the timestamps
 * repository with valid=0
 */

int
send_challenge(union olsr_ip_addr *new_host)
{
  struct challengemsg cmsg;
  struct stamp *entry;
  olsr_u32_t challenge, hash;

  olsr_printf(1, "[ENC]Building CHALLENGE message\n");

  /* Set the size including OLSR packet size */


  challenge = rand() << 16;
  challenge |= rand();

  /* Fill challengemessage */
  cmsg.olsr_msgtype = TYPE_CHALLENGE;
  cmsg.olsr_vtime = 0;
  cmsg.olsr_msgsize = htons(sizeof(struct challengemsg));
  memcpy(&cmsg.originator, &olsr_cnf->main_addr, olsr_cnf->ipsize);
  cmsg.ttl = 1;
  cmsg.hopcnt = 0;
  cmsg.seqno = htons(get_msg_seqno());

  /* Fill subheader */
  memcpy(&cmsg.destination, new_host, olsr_cnf->ipsize);
  cmsg.challenge = htonl(challenge);

  olsr_printf(3, "[ENC]Size: %lu\n", (unsigned long)sizeof(struct challengemsg));

  {
  olsr_u8_t checksum_cache[512 + KEYLENGTH];
  /* Create packet + key cache */
  /* First the OLSR packet + signature message - digest */
  memcpy(checksum_cache, &cmsg, sizeof(struct challengemsg) - SIGNATURE_SIZE);
  /* Then the key */
  memcpy(&checksum_cache[sizeof(struct challengemsg) - SIGNATURE_SIZE], aes_key, KEYLENGTH);

  /* Create the hash */
  CHECKSUM(checksum_cache, 
	   (sizeof(struct challengemsg) - SIGNATURE_SIZE) + KEYLENGTH, 
	   cmsg.signature);
  }
  olsr_printf(3, "[ENC]Sending timestamp request to %s challenge 0x%x\n", 
	      olsr_ip_to_string(new_host),
	      challenge);

  /* Add to buffer */
  net_outbuffer_push(olsr_in_if, &cmsg, sizeof(struct challengemsg));

  /* Send the request */
  net_output(olsr_in_if);

  /* Create new entry */
  entry = malloc(sizeof(struct stamp));
  
  entry->diff = 0;
  entry->validated = 0;
  entry->challenge = challenge;

  memcpy(&entry->addr, new_host, olsr_cnf->ipsize);

  /* update validtime - not validated */
  entry->conftime = GET_TIMESTAMP(EXCHANGE_HOLD_TIME * 1000);

  hash = olsr_hashing(new_host);
  
  /* Queue */
  timestamps[hash].next->prev = entry;
  entry->next = timestamps[hash].next;
  timestamps[hash].next = entry;
  entry->prev = &timestamps[hash];


  return 1;

}

int
parse_cres(char *in_msg)
{
  struct c_respmsg *msg;
  olsr_u8_t sha1_hash[SIGNATURE_SIZE];
  struct stamp *entry;

  msg = (struct c_respmsg *)in_msg;

  olsr_printf(1, "[ENC]Challenge-response message received\n");
  olsr_printf(3, "[ENC]To: %s\n", olsr_ip_to_string((union olsr_ip_addr *)&msg->destination));

  if(if_ifwithaddr((union olsr_ip_addr *)&msg->destination) == NULL)
    {
      olsr_printf(3, "[ENC]Not for us...\n");
      return 0;
    }

  olsr_printf(3, "[ENC]Challenge: 0x%lx\n", (unsigned long)ntohl(msg->challenge)); /* ntohl() returns a unsignedlong onwin32 */

  /* Check signature */

  {
  olsr_u8_t checksum_cache[512 + KEYLENGTH];
  /* Create packet + key cache */
  /* First the OLSR packet + signature message - digest */
  memcpy(checksum_cache, msg, sizeof(struct c_respmsg) - SIGNATURE_SIZE);
  /* Then the key */
  memcpy(&checksum_cache[sizeof(struct c_respmsg) - SIGNATURE_SIZE], aes_key, KEYLENGTH);
  
  /* Create the hash */
  CHECKSUM(checksum_cache, 
	   (sizeof(struct c_respmsg) - SIGNATURE_SIZE) + KEYLENGTH, 
	   sha1_hash);
  }
  
  if(memcmp(sha1_hash, &msg->signature, SIGNATURE_SIZE) != 0)
    {
      olsr_printf(1, "[ENC]Signature missmatch in challenge-response!\n");
      return 0;
    }

  olsr_printf(3, "[ENC]Signature verified\n");


  /* Now to check the digest from the emitted challenge */
  if((entry = lookup_timestamp_entry((union olsr_ip_addr *)&msg->originator)) == NULL)
    {
      olsr_printf(1, "[ENC]Received challenge-response from non-registered node %s!\n",
		  olsr_ip_to_string((union olsr_ip_addr *)&msg->originator));
      return 0;
    }

  /* Generate the digest */
  olsr_printf(3, "[ENC]Entry-challenge 0x%x\n", entry->challenge);

  {
  olsr_u8_t checksum_cache[512 + KEYLENGTH];
  /* First the challenge received */
  memcpy(checksum_cache, &entry->challenge, 4);
  /* Then the local IP */
  memcpy(&checksum_cache[sizeof(olsr_u32_t)], &msg->originator, olsr_cnf->ipsize);

  /* Create the hash */
  CHECKSUM(checksum_cache, 
	   sizeof(olsr_u32_t) + olsr_cnf->ipsize, 
	   sha1_hash);
  }

  if(memcmp(msg->res_sig, sha1_hash, SIGNATURE_SIZE) != 0)
    {
      olsr_printf(1, "[ENC]Error in challenge signature from %s!\n",
		  olsr_ip_to_string((union olsr_ip_addr *)&msg->originator));
      
      return 0;
    }

  olsr_printf(3, "[ENC]Challenge-response signature ok\n");

  /* Update entry! */


  entry->challenge = 0;
  entry->validated = 1;
  entry->diff = now.tv_sec - msg->timestamp;

  /* update validtime - validated entry */
  entry->valtime = GET_TIMESTAMP(TIMESTAMP_HOLD_TIME * 1000);

  olsr_printf(1, "[ENC]%s registered with diff %d!\n",
	      olsr_ip_to_string((union olsr_ip_addr *)&msg->originator),
	      entry->diff);

  /* Send response-response */
  send_rres((union olsr_ip_addr *)&msg->originator, 
	    (union olsr_ip_addr *)&msg->destination, 
	    ntohl(msg->challenge));

  return 1;
}


int
parse_rres(char *in_msg)
{
  struct r_respmsg *msg;