Move finding of header sizes into a function
Fix leaving data in packet after storage

Move spare data was not reducing the filled
size by the moved data size.