Fix another buffer overflow issue

Using hard-coded length value easily leads to buffer overflow.
Refactor the code a bit to make it more readable, and more
extensible, and less subject to the buffer overflow coding error.

Patch originally contributed by teng.hong@nxp.com

Change-Id: Id262915302ccea8ae3b0121bf39890ab463aeeb7
related-to-bug: 6328360