n_gsm: Fix message length handling when building header