diff --git a/drivers/nfc/pn544_hci.c b/drivers/nfc/pn544_hci.c index 9458d53cdb59..d90aecfce739 100644 --- a/drivers/nfc/pn544_hci.c +++ b/drivers/nfc/pn544_hci.c @@ -128,6 +128,8 @@ static struct nfc_hci_gate pn544_gates[] = { /* Largest headroom needed for outgoing custom commands */ #define PN544_CMDS_HEADROOM 2 +#define PN544_FRAME_HEADROOM 1 +#define PN544_FRAME_TAILROOM 2 struct pn544_hci_info { struct i2c_client *i2c_dev; @@ -576,15 +578,40 @@ static int pn544_hci_ready(struct nfc_shdlc *shdlc) return 0; } +static void pn544_hci_add_len_crc(struct sk_buff *skb) +{ + u16 crc; + int len; + + len = skb->len + 2; + *skb_push(skb, 1) = len; + + crc = crc_ccitt(0xffff, skb->data, skb->len); + crc = ~crc; + *skb_put(skb, 1) = crc & 0xff; + *skb_put(skb, 1) = crc >> 8; +} + +static void pn544_hci_remove_len_crc(struct sk_buff *skb) +{ + skb_pull(skb, PN544_FRAME_HEADROOM); + skb_trim(skb, PN544_FRAME_TAILROOM); +} + static int pn544_hci_xmit(struct nfc_shdlc *shdlc, struct sk_buff *skb) { struct pn544_hci_info *info = nfc_shdlc_get_clientdata(shdlc); struct i2c_client *client = info->i2c_dev; + int r; if (info->hard_fault != 0) return info->hard_fault; - return pn544_hci_i2c_write(client, skb->data, skb->len); + pn544_hci_add_len_crc(skb); + r = pn544_hci_i2c_write(client, skb->data, skb->len); + pn544_hci_remove_len_crc(skb); + + return r; } static int pn544_hci_start_poll(struct nfc_shdlc *shdlc, @@ -874,7 +901,8 @@ static int __devinit pn544_hci_probe(struct i2c_client *client, info->shdlc = nfc_shdlc_allocate(&pn544_shdlc_ops, &init_data, protocols, - PN544_CMDS_HEADROOM, 0, + PN544_FRAME_HEADROOM + PN544_CMDS_HEADROOM, + PN544_FRAME_TAILROOM, PN544_HCI_LLC_MAX_PAYLOAD, dev_name(&client->dev)); if (!info->shdlc) { diff --git a/net/nfc/hci/shdlc.c b/net/nfc/hci/shdlc.c index 824fb09384ed..ed8796b78f39 100644 --- a/net/nfc/hci/shdlc.c +++ b/net/nfc/hci/shdlc.c @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -30,7 +29,6 @@ #include #define SHDLC_LLC_HEAD_ROOM 2 -#define SHDLC_LLC_TAIL_ROOM 2 #define SHDLC_MAX_WINDOW 4 #define SHDLC_SREJ_SUPPORT false @@ -94,28 +92,13 @@ static struct sk_buff *nfc_shdlc_alloc_skb(struct nfc_shdlc *shdlc, struct sk_buff *skb; skb = alloc_skb(shdlc->client_headroom + SHDLC_LLC_HEAD_ROOM + - shdlc->client_tailroom + SHDLC_LLC_TAIL_ROOM + - payload_len, GFP_KERNEL); + shdlc->client_tailroom + payload_len, GFP_KERNEL); if (skb) skb_reserve(skb, shdlc->client_headroom + SHDLC_LLC_HEAD_ROOM); return skb; } -static void nfc_shdlc_add_len_crc(struct sk_buff *skb) -{ - u16 crc; - int len; - - len = skb->len + 2; - *skb_push(skb, 1) = len; - - crc = crc_ccitt(0xffff, skb->data, skb->len); - crc = ~crc; - *skb_put(skb, 1) = crc & 0xff; - *skb_put(skb, 1) = crc >> 8; -} - /* immediately sends an S frame. */ static int nfc_shdlc_send_s_frame(struct nfc_shdlc *shdlc, enum sframe_type sframe_type, int nr) @@ -131,8 +114,6 @@ static int nfc_shdlc_send_s_frame(struct nfc_shdlc *shdlc, *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_S | (sframe_type << 3) | nr; - nfc_shdlc_add_len_crc(skb); - r = shdlc->ops->xmit(shdlc, skb); kfree_skb(skb); @@ -151,8 +132,6 @@ static int nfc_shdlc_send_u_frame(struct nfc_shdlc *shdlc, *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_U | uframe_modifier; - nfc_shdlc_add_len_crc(skb); - r = shdlc->ops->xmit(shdlc, skb); kfree_skb(skb); @@ -509,8 +488,6 @@ static void nfc_shdlc_handle_send_queue(struct nfc_shdlc *shdlc) shdlc->nr); /* SHDLC_DUMP_SKB("shdlc frame written", skb); */ - nfc_shdlc_add_len_crc(skb); - r = shdlc->ops->xmit(shdlc, skb); if (r < 0) { shdlc->hard_fault = r; @@ -880,7 +857,7 @@ struct nfc_shdlc *nfc_shdlc_allocate(struct nfc_shdlc_ops *ops, shdlc->hdev = nfc_hci_allocate_device(&shdlc_ops, init_data, protocols, tx_headroom + SHDLC_LLC_HEAD_ROOM, - tx_tailroom + SHDLC_LLC_TAIL_ROOM, + tx_tailroom, max_link_payload); if (shdlc->hdev == NULL) goto err_allocdev;