struct cfpkt *cfpkt_split(struct cfpkt *pkt, u16 pos) { struct sk_buff *skb2; struct sk_buff *skb = pkt_to_skb(pkt); struct cfpkt *tmppkt; u8 *split = skb->data + pos; u16 len2nd = skb_tail_pointer(skb) - split; if (unlikely(is_erronous(pkt))) return NULL; if (skb->data + pos > skb_tail_pointer(skb)) { PKT_ERROR(pkt, "trying to split beyond end of packet\n"); return NULL; } /* Create a new packet for the second part of the data */ tmppkt = cfpkt_create_pfx(len2nd + PKT_PREFIX + PKT_POSTFIX, PKT_PREFIX); if (tmppkt == NULL) return NULL; skb2 = pkt_to_skb(tmppkt); if (skb2 == NULL) return NULL; skb_put_data(skb2, split, len2nd); /* Reduce the length of the original packet */ skb_trim(skb, pos); skb2->priority = skb->priority; return skb_to_pkt(skb2); }
int cfpkt_raw_extract(struct cfpkt *pkt, void **buf, unsigned int buflen) { struct sk_buff *skb = pkt_to_skb(pkt); caif_assert(buf != NULL); if (unlikely(is_erronous(pkt))) return -EPROTO; if (unlikely(buflen > skb->len)) { PKT_ERROR(pkt, "cfpkt_raw_extract: buflen too large " "- failed\n"); return -EPROTO; } if (unlikely(buflen > skb_headlen(skb))) { if (unlikely(skb_linearize(skb) != 0)) { PKT_ERROR(pkt, "cfpkt_raw_extract: linearize failed\n"); return -EPROTO; } } *buf = skb->data; skb_pull(skb, buflen); return 1; }
int cfpkt_add_head(struct cfpkt *pkt, const void *data2, u16 len) { struct sk_buff *skb = pkt_to_skb(pkt); struct sk_buff *lastskb; u8 *to; const u8 *data = data2; int ret; if (unlikely(is_erronous(pkt))) return -EPROTO; if (unlikely(skb_headroom(skb) < len)) { PKT_ERROR(pkt, "no headroom\n"); return -EPROTO; } /* Make sure data is writable */ ret = skb_cow_data(skb, 0, &lastskb); if (unlikely(ret < 0)) { PKT_ERROR(pkt, "cow failed\n"); return ret; } to = skb_push(skb, len); memcpy(to, data, len); return 0; }
int cfpkt_raw_append(struct cfpkt *pkt, void **buf, unsigned int buflen) { struct sk_buff *skb = pkt_to_skb(pkt); struct sk_buff *lastskb; caif_assert(buf != NULL); if (unlikely(is_erronous(pkt))) return -EPROTO; /* Make sure SKB is writable */ if (unlikely(skb_cow_data(skb, 0, &lastskb) < 0)) { PKT_ERROR(pkt, "cfpkt_raw_append: skb_cow_data failed\n"); return -EPROTO; } if (unlikely(skb_linearize(skb) != 0)) { PKT_ERROR(pkt, "cfpkt_raw_append: linearize failed\n"); return -EPROTO; } if (unlikely(skb_tailroom(skb) < buflen)) { PKT_ERROR(pkt, "cfpkt_raw_append: buffer too short - failed\n"); return -EPROTO; } *buf = skb_put(skb, buflen); return 1; }
void cfpkt_queue(struct cfpktq *pktq, struct cfpkt *pkt, unsigned short prio) { atomic_inc(&pktq->count); spin_lock(&pktq->lock); skb_queue_tail(&pktq->head, pkt_to_skb(pkt)); spin_unlock(&pktq->lock); }
struct cfpkt *cfpkt_clone_release(struct cfpkt *pkt) { struct cfpkt *clone; clone = skb_to_pkt(skb_clone(pkt_to_skb(pkt), GFP_ATOMIC)); /* Free original packet. */ cfpkt_destroy(pkt); if (!clone) return NULL; return clone; }
int cfpkt_peek_head(struct cfpkt *pkt, void *data, u16 len) { struct sk_buff *skb = pkt_to_skb(pkt); if (skb_headlen(skb) >= len) { memcpy(data, skb->data, len); return 0; } return !cfpkt_extr_head(pkt, data, len) && !cfpkt_add_head(pkt, data, len); }
struct cfpkt *cfpkt_append(struct cfpkt *dstpkt, struct cfpkt *addpkt, u16 expectlen) { struct sk_buff *dst = pkt_to_skb(dstpkt); struct sk_buff *add = pkt_to_skb(addpkt); u16 addlen = skb_headlen(add); u16 neededtailspace; struct sk_buff *tmp; u16 dstlen; u16 createlen; if (unlikely(is_erronous(dstpkt) || is_erronous(addpkt))) { cfpkt_destroy(addpkt); return dstpkt; } if (expectlen > addlen) neededtailspace = expectlen; else neededtailspace = addlen; if (dst->tail + neededtailspace > dst->end) { /* Create a dumplicate of 'dst' with more tail space */ struct cfpkt *tmppkt; dstlen = skb_headlen(dst); createlen = dstlen + neededtailspace; tmppkt = cfpkt_create(createlen + PKT_PREFIX + PKT_POSTFIX); if (tmppkt == NULL) return NULL; tmp = pkt_to_skb(tmppkt); skb_set_tail_pointer(tmp, dstlen); tmp->len = dstlen; memcpy(tmp->data, dst->data, dstlen); cfpkt_destroy(dstpkt); dst = tmp; } memcpy(skb_tail_pointer(dst), add->data, skb_headlen(add)); cfpkt_destroy(addpkt); dst->tail += addlen; dst->len += addlen; return skb_to_pkt(dst); }
int cfpkt_add_body(struct cfpkt *pkt, const void *data, u16 len) { struct sk_buff *skb = pkt_to_skb(pkt); struct sk_buff *lastskb; u8 *to; u16 addlen = 0; if (unlikely(is_erronous(pkt))) return -EPROTO; lastskb = skb; /* Check whether we need to add space at the tail */ if (unlikely(skb_tailroom(skb) < len)) { if (likely(len < PKT_LEN_WHEN_EXTENDING)) addlen = PKT_LEN_WHEN_EXTENDING; else addlen = len; } /* Check whether we need to change the SKB before writing to the tail */ if (unlikely((addlen > 0) || skb_cloned(skb) || skb_shared(skb))) { /* Make sure data is writable */ if (unlikely(skb_cow_data(skb, addlen, &lastskb) < 0)) { PKT_ERROR(pkt, "cfpkt_add_body: cow failed\n"); return -EPROTO; } /* * Is the SKB non-linear after skb_cow_data()? If so, we are * going to add data to the last SKB, so we need to adjust * lengths of the top SKB. */ if (lastskb != skb) { pr_warning("CAIF: %s(): Packet is non-linear\n", __func__); skb->len += len; skb->data_len += len; } } /* All set to put the last SKB and optionally write data there. */ to = skb_put(lastskb, len); if (likely(data)) memcpy(to, data, len); return 0; }
int cfpkt_extr_trail(struct cfpkt *pkt, void *dta, u16 len) { struct sk_buff *skb = pkt_to_skb(pkt); u8 *data = dta; u8 *from; if (unlikely(is_erronous(pkt))) return -EPROTO; if (unlikely(skb_linearize(skb) != 0)) { PKT_ERROR(pkt, "linearize failed\n"); return -EPROTO; } if (unlikely(skb->data + len > skb_tail_pointer(skb))) { PKT_ERROR(pkt, "read beyond end of packet\n"); return -EPROTO; } from = skb_tail_pointer(skb) - len; skb_trim(skb, skb->len - len); memcpy(data, from, len); return 0; }
int cfpkt_setlen(struct cfpkt *pkt, u16 len) { struct sk_buff *skb = pkt_to_skb(pkt); if (unlikely(is_erronous(pkt))) return -EPROTO; if (likely(len <= skb->len)) { if (unlikely(skb->data_len)) ___pskb_trim(skb, len); else skb_trim(skb, len); return cfpkt_getlen(pkt); } /* Need to expand SKB */ if (unlikely(!cfpkt_pad_trail(pkt, len - skb->len))) PKT_ERROR(pkt, "skb_pad_trail failed\n"); return cfpkt_getlen(pkt); }
int cfpkt_extr_head(struct cfpkt *pkt, void *data, u16 len) { struct sk_buff *skb = pkt_to_skb(pkt); u8 *from; if (unlikely(is_erronous(pkt))) return -EPROTO; if (unlikely(len > skb->len)) { PKT_ERROR(pkt, "cfpkt_extr_head read beyond end of packet\n"); return -EPROTO; } if (unlikely(len > skb_headlen(skb))) { if (unlikely(skb_linearize(skb) != 0)) { PKT_ERROR(pkt, "cfpkt_extr_head linearize failed\n"); return -EPROTO; } } from = skb_pull(skb, len); from -= len; memcpy(data, from, len); return 0; }
int cfpkt_add_body(struct cfpkt *pkt, const void *data, u16 len) { struct sk_buff *skb = pkt_to_skb(pkt); struct sk_buff *lastskb; u8 *to; u16 addlen = 0; if (unlikely(is_erronous(pkt))) return -EPROTO; lastskb = skb; /* Check whether we need to add space at the tail */ if (unlikely(skb_tailroom(skb) < len)) { if (likely(len < PKT_LEN_WHEN_EXTENDING)) addlen = PKT_LEN_WHEN_EXTENDING; else addlen = len; } /* Check whether we need to change the SKB before writing to the tail */ if (unlikely((addlen > 0) || skb_cloned(skb) || skb_shared(skb))) { /* Make sure data is writable */ if (unlikely(skb_cow_data(skb, addlen, &lastskb) < 0)) { PKT_ERROR(pkt, "cow failed\n"); return -EPROTO; } } /* All set to put the last SKB and optionally write data there. */ to = pskb_put(skb, lastskb, len); if (likely(data)) memcpy(to, data, len); return 0; }
char *cfpkt_log_pkt(struct cfpkt *pkt, char *buf, int buflen) { struct sk_buff *skb = pkt_to_skb(pkt); char *p = buf; int i; /* * Sanity check buffer length, it needs to be at least as large as * the header info: ~=50+ bytes */ if (buflen < 50) return NULL; snprintf(buf, buflen, "%s: pkt:%p len:%ld(%ld+%ld) {%ld,%ld} data: [", is_erronous(pkt) ? "ERRONOUS-SKB" : (skb->data_len != 0 ? "COMPLEX-SKB" : "SKB"), skb, (long) skb->len, (long) (skb_tail_pointer(skb) - skb->data), (long) skb->data_len, (long) (skb->data - skb->head), (long) (skb_tail_pointer(skb) - skb->head)); p = buf + strlen(buf); for (i = 0; i < skb_tail_pointer(skb) - skb->data && i < 300; i++) { if (p > buf + buflen - 10) { sprintf(p, "..."); p = buf + strlen(buf); break; } sprintf(p, "%02x,", skb->data[i]); p = buf + strlen(buf); } sprintf(p, "]\n"); return buf; }
struct caif_payload_info *cfpkt_info(struct cfpkt *pkt) { return (struct caif_payload_info *)&pkt_to_skb(pkt)->cb; }
inline u16 cfpkt_getlen(struct cfpkt *pkt) { struct sk_buff *skb = pkt_to_skb(pkt); return skb->len; }
void cfpkt_set_prio(struct cfpkt *pkt, int prio) { pkt_to_skb(pkt)->priority = prio; }
void cfpkt_destroy(struct cfpkt *pkt) { struct sk_buff *skb = pkt_to_skb(pkt); kfree_skb(skb); }
inline bool cfpkt_more(struct cfpkt *pkt) { struct sk_buff *skb = pkt_to_skb(pkt); return skb->len > 0; }