Fix for >16Kb packets

Fixes problems with large packets of >16Kb
- fix #25 CS