AdvertisingDataBuilder::setServiceData size issue

I’m testing advertisement builder functionality from sourcecode.
I am having issue with size. If data is bigger then 2 bytes i got BLE_ERROR_BUFFER_OVERFLOW.
Here is code that i am testing with:

uint8_t data[] = {0x01, 0x02, 0x03, 0x04};
UUID companyId(0x11BA);
_advDataBuilder.setServiceData(companyId, data);
error =, _advDataBuilder.getAdvertisingData());

The weird thing is the condition for size:

if (total_size > data.size() - (_payload_length - old_size)) {

total_size is 8
data.size() is 4
_payload_length is 3
old_size is 0

total_size in most cases is bigger than data.size() - _payload_length if _payload_length is 3

Any suggestions are welcome.

Maybe this helps clarify…