h264parse: fix and optimize NAL collection function.