AdvertisingDataBuilder::setServiceData size issue

Hi,
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:

_advDataBuilder.setFlags();
uint8_t data[] = {0x01, 0x02, 0x03, 0x04};
UUID companyId(0x11BA);
_advDataBuilder.setServiceData(companyId, data);
error = _ble.gap().setAdvertisingPayload(ble::LEGACY_ADVERTISING_HANDLE, _advDataBuilder.getAdvertisingData());

The weird thing is the condition for size:

if (total_size > data.size() - (_payload_length - old_size)) {
  return BLE_ERROR_BUFFER_OVERFLOW;
}

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…