diff --git a/pcap_mm.c b/pcap_mm.c
index f7b248e..b4bbae4 100644
--- a/pcap_mm.c
+++ b/pcap_mm.c
@@ -56,8 +56,18 @@ static ssize_t pcap_mm_write(int fd, pcap_pkthdr_t *phdr,
enum pcap_type type,
if ((off_t) (ptr_va_curr - ptr_va_start) + hdrsize + len > map_size)
__pcap_mmap_write_need_remap(fd);
- fmemcpy(ptr_va_curr, &phdr->raw, hdrsize);
+ fmemcpy(ptr_va_curr, &phdr->h.raw, hdrsize);
ptr_va_curr += hdrsize;
+
+ if (pcap_pkthdr_has_ll(phdr, type)) {
+ size_t ll_len = sizeof(struct pcap_pkthdr_ll);
+
+ fmemcpy(ptr_va_curr, &phdr->ll_hdr, ll_len);
+ ptr_va_curr += ll_len;
+ len -= ll_len;
+ hdrsize += ll_len;
+ }
+
fmemcpy(ptr_va_curr, packet, len);
ptr_va_curr += len;
@@ -72,8 +82,22 @@ static ssize_t pcap_mm_read(int fd __maybe_unused,
pcap_pkthdr_t *phdr,
if (unlikely((off_t) (ptr_va_curr + hdrsize - ptr_va_start) > (off_t)
map_size))
return -EIO;
- fmemcpy(&phdr->raw, ptr_va_curr, hdrsize);
+ fmemcpy(&phdr->h.raw, ptr_va_curr, hdrsize);
ptr_va_curr += hdrsize;
+
+ if (pcap_pkthdr_has_ll(phdr, type)) {
+ size_t ll_len = sizeof(struct pcap_pkthdr_ll);
+
+ if (unlikely((off_t) (ptr_va_curr + ll_len - ptr_va_start) >
+ (off_t) map_size))
+ return -EIO;
+
+ fmemcpy(&phdr->ll_hdr, ptr_va_curr, ll_len);
+ ptr_va_curr += hdrsize;
+ hdrsize += ll_len;
+ len -= ll_len;
+ }
+
hdrlen = pcap_get_length(phdr, type);
if (unlikely((off_t) (ptr_va_curr + hdrlen - ptr_va_start) > (off_t)
map_size))
diff --git a/pcap_rw.c b/pcap_rw.c
index b6be922..651cef7 100644
--- a/pcap_rw.c
+++ b/pcap_rw.c
@@ -21,12 +21,25 @@ static ssize_t pcap_rw_write(int fd, pcap_pkthdr_t *phdr,
enum pcap_type type,
const uint8_t *packet, size_t len)
{
ssize_t ret, hdrsize = pcap_get_hdr_length(phdr, type), hdrlen = 0;
+ ssize_t ll_len = 0;
- ret = write_or_die(fd, &phdr->raw, hdrsize);
+ ret = write_or_die(fd, &phdr->h.raw, hdrsize);
if (unlikely(ret != hdrsize))
panic("Failed to write pkt header!\n");
+ if (pcap_pkthdr_has_ll(phdr, type)) {
+ ll_len = sizeof(struct pcap_pkthdr_ll);
+ int ret_ll = write_or_die(fd, (uint8_t *)&phdr->ll_hdr, ll_len);
+
+ if (unlikely(ret_ll != sizeof(phdr->ll_hdr)))
+ panic("Failed to write pkt header!\n");
+ }
+
hdrlen = pcap_get_length(phdr, type);
+ hdrlen -= ll_len;
+ len -= ll_len;
+ hdrsize += ll_len;
+
if (unlikely(hdrlen != (ssize_t) len))
return -EINVAL;
@@ -41,12 +54,25 @@ static ssize_t pcap_rw_read(int fd, pcap_pkthdr_t *phdr,
enum pcap_type type,
uint8_t *packet, size_t len)
{
ssize_t ret, hdrsize = pcap_get_hdr_length(phdr, type), hdrlen = 0;
+ ssize_t ll_len = 0;
- ret = read_or_die(fd, &phdr->raw, hdrsize);
+ ret = read_or_die(fd, &phdr->h.raw, hdrsize);
if (unlikely(ret != hdrsize))
return -EIO;
+ if (pcap_pkthdr_has_ll(phdr, type)) {
+ ll_len = sizeof(struct pcap_pkthdr_ll);
+ int ret_ll = read(fd, &phdr->ll_hdr, ll_len);
+
+ if (unlikely(ret_ll != ll_len))
+ return -EIO;
+ }
+
hdrlen = pcap_get_length(phdr, type);
+ hdrlen -= ll_len;
+ len -= ll_len;
+ hdrsize += ll_len;
+
if (unlikely(hdrlen == 0 || hdrlen > (ssize_t) len))
return -EINVAL;
diff --git a/pcap_sg.c b/pcap_sg.c
index 80c2c5d..fdfef9a 100644
--- a/pcap_sg.c
+++ b/pcap_sg.c
@@ -33,61 +33,40 @@ static ssize_t pcap_sg_write(int fd, pcap_pkthdr_t *phdr,
enum pcap_type type,
iov_slot = 0;
}
- fmemcpy(iov[iov_slot].iov_base, &phdr->raw, hdrsize);
+ fmemcpy(iov[iov_slot].iov_base, &phdr->h.raw, hdrsize);
iov[iov_slot].iov_len = hdrsize;
- fmemcpy(iov[iov_slot].iov_base + iov[iov_slot].iov_len, packet, len);
- ret = (iov[iov_slot].iov_len += len);
+ if (pcap_pkthdr_has_ll(phdr, type)) {
+ ssize_t ll_hdrlen = sizeof(struct pcap_pkthdr_ll);
- iov_slot++;
- return ret;
-}
-
-static ssize_t __pcap_sg_inter_iov_hdr_read(int fd, pcap_pkthdr_t *phdr,
- size_t hdrsize)
-{
- int ret;
- size_t offset = 0;
- ssize_t remainder;
-
- offset = iov[iov_slot].iov_len - iov_off_rd;
- remainder = hdrsize - offset;
- if (remainder < 0)
- remainder = 0;
-
- bug_on(offset + remainder != hdrsize);
-
- fmemcpy(&phdr->raw, iov[iov_slot].iov_base + iov_off_rd, offset);
- iov_off_rd = 0;
- iov_slot++;
+ fmemcpy(iov[iov_slot].iov_base + iov[iov_slot].iov_len,
+ &phdr->ll_hdr, ll_hdrlen);
- if (iov_slot == array_size(iov)) {
- iov_slot = 0;
- ret = readv(fd, iov, array_size(iov));
- if (unlikely(ret <= 0))
- return -EIO;
+ ret = (iov[iov_slot].iov_len += ll_hdrlen);
+ len -= ll_hdrlen;
}
- fmemcpy(&phdr->raw + offset, iov[iov_slot].iov_base + iov_off_rd,
remainder);
- iov_off_rd += remainder;
+ fmemcpy(iov[iov_slot].iov_base + iov[iov_slot].iov_len, packet, len);
+ ret = (iov[iov_slot].iov_len += len);
- return hdrsize;
+ iov_slot++;
+ return ret;
}
-static ssize_t __pcap_sg_inter_iov_data_read(int fd, uint8_t *packet, size_t
hdrlen)
+static ssize_t __pcap_sg_inter_iov_read(int fd, uint8_t *buf, size_t len)
{
int ret;
size_t offset = 0;
ssize_t remainder;
offset = iov[iov_slot].iov_len - iov_off_rd;
- remainder = hdrlen - offset;
+ remainder = len - offset;
if (remainder < 0)
remainder = 0;
- bug_on(offset + remainder != hdrlen);
+ bug_on(offset + remainder != len);
- fmemcpy(packet, iov[iov_slot].iov_base + iov_off_rd, offset);
+ fmemcpy(buf, iov[iov_slot].iov_base + iov_off_rd, offset);
iov_off_rd = 0;
iov_slot++;
@@ -98,10 +77,10 @@ static ssize_t __pcap_sg_inter_iov_data_read(int fd,
uint8_t *packet, size_t hdr
return -EIO;
}
- fmemcpy(packet + offset, iov[iov_slot].iov_base + iov_off_rd,
remainder);
+ fmemcpy(buf + offset, iov[iov_slot].iov_base + iov_off_rd, remainder);
iov_off_rd += remainder;
- return hdrlen;
+ return len;
}
static ssize_t pcap_sg_read(int fd, pcap_pkthdr_t *phdr, enum pcap_type type,
@@ -109,17 +88,39 @@ static ssize_t pcap_sg_read(int fd, pcap_pkthdr_t *phdr,
enum pcap_type type,
{
ssize_t ret = 0;
size_t hdrsize = pcap_get_hdr_length(phdr, type), hdrlen;
+ ssize_t ll_len = 0;
if (likely(iov[iov_slot].iov_len - iov_off_rd >= hdrsize)) {
- fmemcpy(&phdr->raw, iov[iov_slot].iov_base + iov_off_rd,
hdrsize);
+ fmemcpy(&phdr->h.raw, iov[iov_slot].iov_base + iov_off_rd,
hdrsize);
iov_off_rd += hdrsize;
} else {
- ret = __pcap_sg_inter_iov_hdr_read(fd, phdr, hdrsize);
+ ret = __pcap_sg_inter_iov_read(fd, (uint8_t *)phdr, hdrsize);
if (unlikely(ret < 0))
return ret;
}
+ if (pcap_pkthdr_has_ll(phdr, type)) {
+ ll_len = sizeof(phdr->ll_hdr);
+
+ if (likely(iov[iov_slot].iov_len - iov_off_rd >= ll_len)) {
+ fmemcpy((uint8_t *)&phdr->ll_hdr,
iov[iov_slot].iov_base +
+ iov_off_rd, ll_len);
+
+ iov_off_rd += ll_len;
+ } else {
+ ret = __pcap_sg_inter_iov_read(fd, (uint8_t *)phdr,
+ ll_len);
+
+ if (unlikely(ret < 0))
+ return ret;
+ }
+ }
+
hdrlen = pcap_get_length(phdr, type);
+ len -= ll_len;
+ hdrlen -= ll_len;
+ hdrsize += ll_len;
+
if (unlikely(hdrlen == 0 || hdrlen > len))
return -EINVAL;
@@ -127,7 +128,7 @@ static ssize_t pcap_sg_read(int fd, pcap_pkthdr_t *phdr,
enum pcap_type type,
fmemcpy(packet, iov[iov_slot].iov_base + iov_off_rd, hdrlen);
iov_off_rd += hdrlen;
} else {
- ret = __pcap_sg_inter_iov_data_read(fd, packet, hdrlen);
+ ret = __pcap_sg_inter_iov_read(fd, (uint8_t *)packet, hdrlen);
if (unlikely(ret < 0))
return ret;
}