This is required by Marlin specification. Also added unittests. Issue #381. Change-Id: Icae1213db4c0915720ed07e6b2bb768fa9156a5c