diff --git a/src/core/pbuf.c b/src/core/pbuf.c index ef6812c9..0b5af7c5 100644 --- a/src/core/pbuf.c +++ b/src/core/pbuf.c @@ -1125,13 +1125,11 @@ pbuf_take_at(struct pbuf *buf, const void *dataptr, u16_t len, u16_t offset) if ((q != NULL) && (q->tot_len >= target_offset + len)) { u16_t remaining_len = len; const u8_t* src_ptr = (const u8_t*)dataptr; - if (target_offset > 0) { - /* copy the part that goes into the first pbuf */ - u16_t first_copy_len = LWIP_MIN(q->len - target_offset, len); - MEMCPY(((u8_t*)q->payload) + target_offset, dataptr, first_copy_len); - remaining_len -= first_copy_len; - src_ptr += first_copy_len; - } + /* copy the part that goes into the first pbuf */ + u16_t first_copy_len = LWIP_MIN(q->len - target_offset, len); + MEMCPY(((u8_t*)q->payload) + target_offset, dataptr, first_copy_len); + remaining_len -= first_copy_len; + src_ptr += first_copy_len; if (remaining_len > 0) { return pbuf_take(q->next, src_ptr, remaining_len); } diff --git a/test/unit/core/test_pbuf.c b/test/unit/core/test_pbuf.c index 211465ec..8711136c 100644 --- a/test/unit/core/test_pbuf.c +++ b/test/unit/core/test_pbuf.c @@ -139,6 +139,86 @@ START_TEST(test_pbuf_queueing_bigger_than_64k) } END_TEST +/* Test for bug that writing with pbuf_take_at() did nothing + * and returned ERR_OK when writing at beginning of a pbuf + * in the chain. + */ +START_TEST(test_pbuf_take_at_edge) +{ + err_t res; + u8_t *out; + int i; + u8_t testdata[] = { 0x01, 0x08, 0x82, 0x02 }; + struct pbuf *p = pbuf_alloc(PBUF_RAW, 1024, PBUF_POOL); + struct pbuf *q = p->next; + /* alloc big enough to get a chain of pbufs */ + fail_if(p->tot_len == p->len); + memset(p->payload, 0, p->len); + memset(q->payload, 0, q->len); + + /* copy data to the beginning of first pbuf */ + res = pbuf_take_at(p, &testdata, sizeof(testdata), 0); + fail_unless(res == ERR_OK); + + out = p->payload; + for (i = 0; i < sizeof(testdata); i++) { + fail_unless(out[i] == testdata[i], + "Bad data at pos %d, was %02X, expected %02X", i, out[i], testdata[i]); + } + + /* copy data to the just before end of first pbuf */ + res = pbuf_take_at(p, &testdata, sizeof(testdata), p->len - 1); + fail_unless(res == ERR_OK); + + out = p->payload; + fail_unless(out[p->len - 1] == testdata[0], + "Bad data at pos %d, was %02X, expected %02X", p->len - 1, out[p->len - 1], testdata[0]); + out = q->payload; + for (i = 1; i < sizeof(testdata); i++) { + fail_unless(out[i-1] == testdata[i], + "Bad data at pos %d, was %02X, expected %02X", p->len - 1 + i, out[i-1], testdata[i]); + } + + /* copy data to the beginning of second pbuf */ + res = pbuf_take_at(p, &testdata, sizeof(testdata), p->len); + fail_unless(res == ERR_OK); + + out = p->payload; + for (i = 0; i < sizeof(testdata); i++) { + fail_unless(out[i] == testdata[i], + "Bad data at pos %d, was %02X, expected %02X", p->len+i, out[i], testdata[i]); + } +} +END_TEST + +/* Verify pbuf_put_at()/pbuf_get_at() when using + * offsets equal to beginning of new pbuf in chain + */ +START_TEST(test_pbuf_get_put_at_edge) +{ + u8_t *out; + u8_t testdata = 0x01; + u8_t getdata; + struct pbuf *p = pbuf_alloc(PBUF_RAW, 1024, PBUF_POOL); + struct pbuf *q = p->next; + /* alloc big enough to get a chain of pbufs */ + fail_if(p->tot_len == p->len); + memset(p->payload, 0, p->len); + memset(q->payload, 0, q->len); + + /* put byte at the beginning of second pbuf */ + pbuf_put_at(p, p->len, testdata); + + out = q->payload; + fail_unless(*out == testdata, + "Bad data at pos %d, was %02X, expected %02X", p->len, *out, testdata); + + getdata = pbuf_get_at(p, p->len); + fail_unless(*out == getdata, + "pbuf_get_at() returned bad data at pos %d, was %02X, expected %02X", p->len, getdata, *out); +} +END_TEST + /** Create the suite including all tests for this module */ Suite * pbuf_suite(void) @@ -146,7 +226,9 @@ pbuf_suite(void) testfunc tests[] = { TESTFUNC(test_pbuf_copy_zero_pbuf), TESTFUNC(test_pbuf_split_64k_on_small_pbufs), - TESTFUNC(test_pbuf_queueing_bigger_than_64k) + TESTFUNC(test_pbuf_queueing_bigger_than_64k), + TESTFUNC(test_pbuf_take_at_edge), + TESTFUNC(test_pbuf_get_put_at_edge) }; return create_suite("PBUF", tests, sizeof(tests)/sizeof(testfunc), pbuf_setup, pbuf_teardown); }