fixed src address recognition

packet.c

@@ -57,6 +57,63 @@ void cc2520_packet_create_ack(u8 *pkt, u8 *buf)
 	*ptr = IEEE154_ACK_FRAME_LENGTH;
 }
 
+u64 cc2520_packet_get_src(u8 *buf)
+{
+	// TODO: Make this a little smarter.
+	// Right now we're assuming that the
+	// PAN ID is always present. It's easy
+	// to construct packets that don't
+	// follow this contract.
+
+	ieee154_simple_header_t *hdr;
+	u64 ret;
+
+	u8 src_addr_mode;
+	u8 dest_addr_mode;
+	bool pan_compression;
+
+	u8 src_addr_offset;
+
+	ret = 0;
+	src_addr_offset = 4;
+	hdr = cc2520_packet_get_header(buf);
+
+	src_addr_mode = ((hdr->fcf >> IEEE154_FCF_SRC_ADDR_MODE) & 0x03);
+	dest_addr_mode = ((hdr->fcf >> IEEE154_FCF_DEST_ADDR_MODE) & 0x03);
+	pan_compression = ((hdr->fcf >> IEEE154_FCF_INTRAPAN) & 0x01) == 1;
+
+	if (dest_addr_mode == IEEE154_ADDR_SHORT) {
+		src_addr_offset += 4;
+	}
+	else if (dest_addr_mode == IEEE154_ADDR_EXT) {
+		src_addr_offset += 10;
+	}
+	// WARNING: this is a weird edge case from the
+	// 802.15.4 spec. 
+	// NOTE: Assuming we're on LE arch. 
+	else if (pan_compression) {
+		src_addr_offset += 2;
+	}
+
+	if (src_addr_mode == IEEE154_ADDR_SHORT) {
+		if (!pan_compression) {
+			src_addr_offset += 2;
+		}
+		memcpy(&ret, buf + src_addr_offset, 2);
+	}
+	else if (src_addr_mode == IEEE154_ADDR_EXT) {
+		if (!pan_compression) {
+			src_addr_offset += 2;
+		}
+		memcpy(&ret, buf + src_addr_offset, 8);
+	}
+
+	INFO((KERN_INFO "[cc2520] - src_mode: %d dest_mode: %d pan_comp: %d src_addr_offset: %d\n", 
+		src_addr_mode, dest_addr_mode, pan_compression, src_addr_offset));
+
+	return ret;
+}
+
 ieee154_simple_header_t* cc2520_packet_get_header(u8 *buf)
 {
 	// Ignore the length

packet.h

@@ -93,6 +93,6 @@ bool cc2520_packet_requires_ack_wait(u8 *buf);
 void cc2520_packet_create_ack(u8 *pkt, u8 *buf);
 bool cc2520_packet_is_ack(u8* buf);
 bool cc2520_packet_is_ack_to(u8* pending, u8 * buf);
-
+u64 cc2520_packet_get_src(u8 *buf);
 
 #endif

unique.c

@@ -10,7 +10,7 @@
 
 struct node_list{
 	struct list_head list;
-	u16 src;
+	u64 src;
 	u8 dsn;
 };
 
@@ -59,13 +59,13 @@ static void cc2520_unique_rx_done(u8 *buf, u8 len)
 {
 	struct node_list *tmp;
 	u8 dsn;
-	u16 src;
+	u64 src;
 	bool found;
 	bool drop;
 
 
 	dsn = cc2520_packet_get_header(buf)->dsn;
-	src = cc2520_packet_get_header(buf)->src;
+	src = cc2520_packet_get_src(buf);
 
 	found = false;
 	drop = false;
@@ -89,7 +89,7 @@ static void cc2520_unique_rx_done(u8 *buf, u8 len)
 			tmp->dsn = dsn;
 			tmp->src = src;
 			list_add(&(tmp->list), &nodes);
-			INFO((KERN_INFO "[cc2520] - unique found new mote: %d\n", src));
+			INFO((KERN_INFO "[cc2520] - unique found new mote: %lld\n", src));
 		}
 		else {
 			INFO((KERN_INFO "[cc2520] - alloc failed.\n"));