static int mac802154_parse_frame_start(struct sk_buff *skb)
{
int hlen;
struct ieee802154_hdr hdr;
hlen = ieee802154_hdr_pull(skb, &hdr);
if (hlen < 0)
return -EINVAL;
skb->mac_len = hlen;
pr_debug("fc: %04x dsn: %02x\n", le16_to_cpup((__le16 *)&hdr.fc),
hdr.seq);
mac_cb(skb)->flags = hdr.fc.type;
if (hdr.fc.ack_request)
mac_cb(skb)->flags |= MAC_CB_FLAG_ACKREQ;
if (hdr.fc.security_enabled)
mac_cb(skb)->flags |= MAC_CB_FLAG_SECEN;
mac802154_print_addr("destination", &hdr.dest);
mac802154_print_addr("source", &hdr.source);
mac_cb(skb)->source = hdr.source;
mac_cb(skb)->dest = hdr.dest;
if (hdr.fc.security_enabled) {
u64 key;
pr_debug("seclevel %i\n", hdr.sec.level);
switch (hdr.sec.key_id_mode) {
case IEEE802154_SCF_KEY_IMPLICIT:
pr_debug("implicit key\n");
break;
case IEEE802154_SCF_KEY_INDEX:
pr_debug("key %02x\n", hdr.sec.key_id);
break;
case IEEE802154_SCF_KEY_SHORT_INDEX:
pr_debug("key %04x:%04x %02x\n",
le32_to_cpu(hdr.sec.short_src) >> 16,
le32_to_cpu(hdr.sec.short_src) & 0xffff,
hdr.sec.key_id);
break;
case IEEE802154_SCF_KEY_HW_INDEX:
key = swab64((__force u64) hdr.sec.extended_src);
pr_debug("key source %8phC %02x\n", &key,
hdr.sec.key_id);
break;
}
return -EINVAL;
}
static void
mac802154_subif_rx(struct ieee802154_dev *hw, struct sk_buff *skb, u8 lqi)
{
struct mac802154_priv *priv = mac802154_to_priv(hw);
mac_cb(skb)->lqi = lqi;
skb->protocol = htons(ETH_P_IEEE802154);
skb_reset_mac_header(skb);
BUILD_BUG_ON(sizeof(struct ieee802154_mac_cb) > sizeof(skb->cb));
if (!(priv->hw.flags & IEEE802154_HW_OMIT_CKSUM)) {
u16 crc;
if (skb->len < 2) {
pr_debug("got invalid frame\n");
goto out;
}
crc = crc_ccitt(0, skb->data, skb->len);
if (crc) {
pr_debug("CRC mismatch\n");
goto out;
}
skb_trim(skb, skb->len - 2); /* CRC */
}
mac802154_monitors_rx(priv, skb);
out:
dev_kfree_skb(skb);
return;
}
static int mac802154_header_create(struct sk_buff *skb,
struct net_device *dev,
unsigned short type,
const void *daddr,
const void *saddr,
unsigned len)
{
struct ieee802154_hdr hdr;
struct mac802154_sub_if_data *priv = netdev_priv(dev);
struct ieee802154_mac_cb *cb = mac_cb(skb);
int hlen;
if (!daddr)
return -EINVAL;
memset(&hdr.fc, 0, sizeof(hdr.fc));
hdr.fc.type = cb->type;
hdr.fc.security_enabled = cb->secen;
hdr.fc.ack_request = cb->ackreq;
hdr.seq = ieee802154_mlme_ops(dev)->get_dsn(dev);
if (mac802154_set_header_security(priv, &hdr, cb) < 0)
return -EINVAL;
if (!saddr) {
spin_lock_bh(&priv->mib_lock);
if (priv->short_addr == cpu_to_le16(IEEE802154_ADDR_BROADCAST) ||
priv->short_addr == cpu_to_le16(IEEE802154_ADDR_UNDEF) ||
priv->pan_id == cpu_to_le16(IEEE802154_PANID_BROADCAST)) {
hdr.source.mode = IEEE802154_ADDR_LONG;
hdr.source.extended_addr = priv->extended_addr;
} else {
hdr.source.mode = IEEE802154_ADDR_SHORT;
hdr.source.short_addr = priv->short_addr;
}
hdr.source.pan_id = priv->pan_id;
spin_unlock_bh(&priv->mib_lock);
} else {
hdr.source = *(const struct ieee802154_addr *)saddr;
}
hdr.dest = *(const struct ieee802154_addr *)daddr;
hlen = ieee802154_hdr_push(skb, &hdr);
if (hlen < 0)
return -EINVAL;
skb_reset_mac_header(skb);
skb->mac_len = hlen;
if (len > ieee802154_max_payload(&hdr))
return -EMSGSIZE;
return hlen;
}
static int mac802154_header_create(struct sk_buff *skb,
struct net_device *dev,
unsigned short type,
const void *_daddr,
const void *_saddr,
unsigned len)
{
const struct ieee802154_addr *saddr = _saddr;
const struct ieee802154_addr *daddr = _daddr;
struct ieee802154_addr dev_addr;
struct mac802154_sub_if_data *priv = netdev_priv(dev);
int pos = 2;
u8 head[MAC802154_FRAME_HARD_HEADER_LEN];
u16 fc;
if (!daddr)
return -EINVAL;
head[pos++] = mac_cb(skb)->seq; /* DSN/BSN */
fc = mac_cb_type(skb);
if (mac_cb_is_ackreq(skb))
fc |= IEEE802154_FC_ACK_REQ;
if (!saddr) {
spin_lock_bh(&priv->mib_lock);
if (priv->short_addr == IEEE802154_ADDR_BROADCAST ||
priv->short_addr == IEEE802154_ADDR_UNDEF ||
priv->pan_id == IEEE802154_PANID_BROADCAST) {
dev_addr.addr_type = IEEE802154_ADDR_LONG;
memcpy(dev_addr.hwaddr, dev->dev_addr,
IEEE802154_ADDR_LEN);
} else {
dev_addr.addr_type = IEEE802154_ADDR_SHORT;
dev_addr.short_addr = priv->short_addr;
}
dev_addr.pan_id = priv->pan_id;
saddr = &dev_addr;
spin_unlock_bh(&priv->mib_lock);
}
if (daddr->addr_type != IEEE802154_ADDR_NONE) {
fc |= (daddr->addr_type << IEEE802154_FC_DAMODE_SHIFT);
head[pos++] = daddr->pan_id & 0xff;
head[pos++] = daddr->pan_id >> 8;
if (daddr->addr_type == IEEE802154_ADDR_SHORT) {
head[pos++] = daddr->short_addr & 0xff;
head[pos++] = daddr->short_addr >> 8;
} else {
int lowpan_frag_rcv(struct sk_buff *skb, const u8 frag_type)
{
struct lowpan_frag_queue *fq;
struct net *net = dev_net(skb->dev);
struct lowpan_frag_info *frag_info = lowpan_cb(skb);
struct ieee802154_addr source, dest;
int err;
source = mac_cb(skb)->source;
dest = mac_cb(skb)->dest;
err = lowpan_get_frag_info(skb, frag_type, frag_info);
if (err < 0)
goto err;
if (frag_info->d_size > IPV6_MIN_MTU) {
net_warn_ratelimited("lowpan_frag_rcv: datagram size exceeds MTU\n");
goto err;
}
fq = fq_find(net, frag_info, &source, &dest);
if (fq != NULL) {
int ret;
spin_lock(&fq->q.lock);
ret = lowpan_frag_queue(fq, skb, frag_type);
spin_unlock(&fq->q.lock);
inet_frag_put(&fq->q, &lowpan_frags);
return ret;
}
err:
kfree_skb(skb);
return -1;
}
static int xbee_header_create(struct sk_buff *skb,
struct net_device *dev,
unsigned short type,
const void *_daddr,
const void *_saddr,
unsigned len)
{
struct ieee802154_mac_cb* cb;
struct ieee802154_addr* da = (struct ieee802154_addr*) _daddr;
cb = mac_cb(skb);
memcpy(&(cb->dest.extended_addr),&(da->extended_addr), 8);
printk(KERN_ALERT "MAC DST in header addr %llx\n", cb->dest.extended_addr);
printk(KERN_ALERT "Adding xbee header\n");
//print_hex_dump(KERN_ALERT, "", DUMP_PREFIX_OFFSET, 16, 1, (const char*) _saddr, 8, false);
return 0;
}
static int
ieee802154_subif_frame(struct ieee802154_sub_if_data *sdata,
struct sk_buff *skb, const struct ieee802154_hdr *hdr)
{
struct wpan_dev *wpan_dev = &sdata->wpan_dev;
__le16 span, sshort;
int rc;
pr_debug("getting packet via slave interface %s\n", sdata->dev->name);
span = wpan_dev->pan_id;
sshort = wpan_dev->short_addr;
switch (mac_cb(skb)->dest.mode) {
case IEEE802154_ADDR_NONE:
if (mac_cb(skb)->dest.mode != IEEE802154_ADDR_NONE)
/* FIXME: check if we are PAN coordinator */
skb->pkt_type = PACKET_OTHERHOST;
else
/* ACK comes with both addresses empty */
skb->pkt_type = PACKET_HOST;
break;
case IEEE802154_ADDR_LONG:
if (mac_cb(skb)->dest.pan_id != span &&
mac_cb(skb)->dest.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST))
skb->pkt_type = PACKET_OTHERHOST;
else if (mac_cb(skb)->dest.extended_addr == wpan_dev->extended_addr)
skb->pkt_type = PACKET_HOST;
else
skb->pkt_type = PACKET_OTHERHOST;
break;
case IEEE802154_ADDR_SHORT:
if (mac_cb(skb)->dest.pan_id != span &&
mac_cb(skb)->dest.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST))
skb->pkt_type = PACKET_OTHERHOST;
else if (mac_cb(skb)->dest.short_addr == sshort)
skb->pkt_type = PACKET_HOST;
else if (mac_cb(skb)->dest.short_addr ==
cpu_to_le16(IEEE802154_ADDR_BROADCAST))
skb->pkt_type = PACKET_BROADCAST;
else
skb->pkt_type = PACKET_OTHERHOST;
break;
default:
pr_debug("invalid dest mode\n");
goto fail;
}
skb->dev = sdata->dev;
/* TODO this should be moved after netif_receive_skb call, otherwise
* wireshark will show a mac header with security fields and the
* payload is already decrypted.
*/
rc = mac802154_llsec_decrypt(&sdata->sec, skb);
if (rc) {
pr_debug("decryption failed: %i\n", rc);
goto fail;
}
sdata->dev->stats.rx_packets++;
sdata->dev->stats.rx_bytes += skb->len;
switch (mac_cb(skb)->type) {
case IEEE802154_FC_TYPE_DATA:
return ieee802154_deliver_skb(skb);
default:
pr_warn("ieee802154: bad frame received (type = %d)\n",
mac_cb(skb)->type);
goto fail;
}
fail:
kfree_skb(skb);
return NET_RX_DROP;
}
static int
mac802154_subif_frame(struct mac802154_sub_if_data *sdata, struct sk_buff *skb,
const struct ieee802154_hdr *hdr)
{
__le16 span, sshort;
int rc;
pr_debug("getting packet via slave interface %s\n", sdata->dev->name);
spin_lock_bh(&sdata->mib_lock);
span = sdata->pan_id;
sshort = sdata->short_addr;
switch (mac_cb(skb)->dest.mode) {
case IEEE802154_ADDR_NONE:
if (mac_cb(skb)->dest.mode != IEEE802154_ADDR_NONE)
/* FIXME: check if we are PAN coordinator */
skb->pkt_type = PACKET_OTHERHOST;
else
/* ACK comes with both addresses empty */
skb->pkt_type = PACKET_HOST;
break;
case IEEE802154_ADDR_LONG:
if (mac_cb(skb)->dest.pan_id != span &&
mac_cb(skb)->dest.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST))
skb->pkt_type = PACKET_OTHERHOST;
else if (mac_cb(skb)->dest.extended_addr == sdata->extended_addr)
skb->pkt_type = PACKET_HOST;
else
skb->pkt_type = PACKET_OTHERHOST;
break;
case IEEE802154_ADDR_SHORT:
if (mac_cb(skb)->dest.pan_id != span &&
mac_cb(skb)->dest.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST))
skb->pkt_type = PACKET_OTHERHOST;
else if (mac_cb(skb)->dest.short_addr == sshort)
skb->pkt_type = PACKET_HOST;
else if (mac_cb(skb)->dest.short_addr ==
cpu_to_le16(IEEE802154_ADDR_BROADCAST))
skb->pkt_type = PACKET_BROADCAST;
else
skb->pkt_type = PACKET_OTHERHOST;
break;
default:
spin_unlock_bh(&sdata->mib_lock);
pr_debug("invalid dest mode\n");
kfree_skb(skb);
return NET_RX_DROP;
}
spin_unlock_bh(&sdata->mib_lock);
skb->dev = sdata->dev;
rc = mac802154_llsec_decrypt(&sdata->sec, skb);
if (rc) {
pr_debug("decryption failed: %i\n", rc);
kfree_skb(skb);
return NET_RX_DROP;
}
sdata->dev->stats.rx_packets++;
sdata->dev->stats.rx_bytes += skb->len;
switch (mac_cb(skb)->type) {
case IEEE802154_FC_TYPE_DATA:
return mac802154_process_data(sdata->dev, skb);
default:
pr_warn("ieee802154: bad frame received (type = %d)\n",
mac_cb(skb)->type);
kfree_skb(skb);
return NET_RX_DROP;
}
}
/*
* Create the skb for the upper layer
*/
void xbee_rx(struct net_device *dev, unsigned char *data, int len, unsigned char* addr) {
struct xbee_priv *priv = netdev_priv(dev);
struct sk_buff *skb;
struct ieee802154_mac_cb* cb;
struct ieee802154_hdr hdr;
int hlen;
int packet_stat;
memset(&hdr.fc, 0, sizeof(hdr.fc));
skb = dev_alloc_skb(len+sizeof(struct ieee802154_hdr));
if (!skb) {
if (printk_ratelimit())
printk(KERN_NOTICE "[NET] xbee rx: low on mem - packet dropped\n");
priv->stats.rx_dropped++;
return;
}
skb_reserve(skb, len);
skb->dev = dev;
//skb->pkt_type = PACKET_HOST;
skb->protocol = htons(ETH_P_IEEE802154);
cb = mac_cb(skb);
hdr.source.mode = IEEE802154_ADDR_LONG;
hdr.source.extended_addr = be64_to_cpup((__be64*)addr);
hdr.source.pan_id = xbee_get_pan_id(dev);
//cb->source.mode = IEEE802154_ADDR_LONG;
//cb->source.pan_id = xbee_get_pan_id(dev);
//cb->source.extended_addr = be64_to_cpup((__be64*)addr);
hdr.dest.mode = IEEE802154_ADDR_LONG;
hdr.dest.extended_addr = IEEE802154_ADDR_BROADCAST;
hdr.dest.pan_id = xbee_get_pan_id(dev);
hlen = ieee802154_hdr_push(skb, &hdr);
if (hlen < 0)
return ;
skb->mac_len = hlen;
skb_reset_mac_header(skb);
if (len > ieee802154_max_payload(&hdr))
return ;
// Put all the data into the socket buffer
memcpy(skb_push(skb, len), data, len);
skb->ip_summed = CHECKSUM_UNNECESSARY; // don't check it (does this make any difference?)
skb->len = len;
packet_stat = netif_rx(skb);
if(packet_stat == NET_RX_SUCCESS) {
printk(KERN_ALERT "[NET] Packet received successfully\n");
} else if(packet_stat == NET_RX_DROP) {
printk(KERN_ALERT "[NET] Packet was dropped!\n");
}
printk(KERN_ALERT "MAC SRC addr %llx\n", hdr.source.extended_addr);
dev->stats.rx_packets++;
dev->stats.rx_bytes += skb->len;
}
static netdev_tx_t ieee802154_xbee_xmit(struct sk_buff *skb,
struct net_device *dev)
{
struct ieee802154_mac_cb* cb;
int framelen, datalen;
char *frame;
struct xbee_priv *priv = netdev_priv(dev);
struct xbee_tx_header header = {
.length = 0x00,
.api_id = 0x00,
.frame_id = 0x02,
.options = 0x04
};
dev->stats.tx_packets++;
dev->stats.tx_bytes += skb->len;
if(main_tty == NULL) {
printk(KERN_ALERT "No tty attached!\nMake sure ldisc_daemon is running\n");
return 0;
}
printk(KERN_ALERT "[NET tx called, %d bytes in sk_buff, pkt type %hu, protocol %hu]\n", skb->len, skb->pkt_type, skb->protocol);
cb = mac_cb(skb);
//header.address = cpu_to_be64(0x000000000000FFFF); // So far broadcast
header.address = cpu_to_be64p( (__be64*) &(cb->dest.extended_addr) );
printk(KERN_ALERT "MAC DST addr %llx\n", cb->dest.extended_addr);
header.length = cpu_to_be16(skb->len + 1 + 10);
datalen = skb->len + 1;
// Allocate buffer to hold entire serial frame, including start byte and checksum
frame = kmalloc(sizeof(struct xbee_tx_header) + (2*datalen) + 2, GFP_KERNEL);
/* Assemble the frame */
/* Escaping the XBee header */
printk(KERN_ALERT "Adding XBEE header- length is %lu\n", sizeof(header) + 1);
framelen = escape_into((frame + 1), &header, sizeof(header));
printk(KERN_ALERT "Adding data - length is %u\n", datalen);
framelen += escape_into((frame + framelen + 1 ), (skb->data), datalen );
framelen++; // Checksum char at the beginning
/* save the timestamp */
dev->trans_start = jiffies;
//Stop the interface from sending more data, get the spinlock, and send it!
netif_stop_queue(dev);
spin_lock(&priv->lock);
xbee_hw_tx(frame, framelen, dev);
//Free the skbuff, we've copied all the data out
dev_kfree_skb(skb);
spin_unlock(&priv->lock);
return NETDEV_TX_OK;
}
/*
* Copies bytes from one array into another, escaping them if necessary.
* Returns the size of the new array, including character escapes.
* The destination array MUST be at least twice the size of the source
*/
static int escape_into(char *dest, const void *srcarray, int len) {
int i, j=0;
const char *src = srcarray;
for(i=0; i<len; i++) {
char unesc = *(src + i);
// We support API mode 1 (escape characters is for API 2, if we want to enable mode 2 we need to implement escape management at RX)
if( unesc == 0x7D || unesc == 0x7E || unesc == 0x11 || unesc == 0x13) {
dest[j] = 0x7D;
dest[j+1] = unesc ^ 0x20;
j+=2;
} else {
dest[j] = unesc;
j++;
}
}
return j;
}
