diff --git a/libavcodec/h264_mp4toannexb_bsf.c b/libavcodec/h264_mp4toannexb_bsf.c
index 2d447f7550..9cdbf9618f 100644
--- a/libavcodec/h264_mp4toannexb_bsf.c
+++ b/libavcodec/h264_mp4toannexb_bsf.c
@@ -49,13 +49,13 @@ typedef struct H264BSFContext {
 
 static int alloc_and_copy(uint8_t **poutbuf, int *poutbuf_size,
                           const uint8_t *sps_pps, uint32_t sps_pps_size,
-                          const uint8_t *in, uint32_t in_size)
+                          const uint8_t *in, uint32_t in_size, int ps)
 {
     uint32_t offset         = *poutbuf_size;
-    uint8_t nal_header_size = offset ? 3 : 4;
+    uint8_t start_code_size = offset == 0 || ps ? 4 : 3;
     int err;
 
-    *poutbuf_size += sps_pps_size + in_size + nal_header_size;
+    *poutbuf_size += sps_pps_size + in_size + start_code_size;
     if ((err = av_reallocp(poutbuf,
                            *poutbuf_size + AV_INPUT_BUFFER_PADDING_SIZE)) < 0) {
         *poutbuf_size = 0;
@@ -63,9 +63,9 @@ static int alloc_and_copy(uint8_t **poutbuf, int *poutbuf_size,
     }
     if (sps_pps)
         memcpy(*poutbuf + offset, sps_pps, sps_pps_size);
-    memcpy(*poutbuf + sps_pps_size + nal_header_size + offset, in, in_size);
-    if (!offset) {
-        AV_WB32(*poutbuf + sps_pps_size, 1);
+    memcpy(*poutbuf + sps_pps_size + start_code_size + offset, in, in_size);
+    if (start_code_size == 4) {
+        AV_WB32(*poutbuf + offset + sps_pps_size, 1);
     } else {
         (*poutbuf + offset + sps_pps_size)[0] =
         (*poutbuf + offset + sps_pps_size)[1] = 0;
@@ -216,7 +216,7 @@ static int h264_mp4toannexb_filter(AVBitStreamFilterContext *bsfc,
                     if ((ret = alloc_and_copy(poutbuf, poutbuf_size,
                                          ctx->spspps_buf + ctx->sps_offset,
                                          ctx->pps_offset != -1 ? ctx->pps_offset : ctx->spspps_size - ctx->sps_offset,
-                                         buf, nal_size)) < 0)
+                                         buf, nal_size, 1)) < 0)
                         goto fail;
                     ctx->idr_sps_seen = 1;
                     goto next_nal;
@@ -234,7 +234,7 @@ static int h264_mp4toannexb_filter(AVBitStreamFilterContext *bsfc,
         if (ctx->new_idr && unit_type == 5 && !ctx->idr_sps_seen && !ctx->idr_pps_seen) {
             if ((ret=alloc_and_copy(poutbuf, poutbuf_size,
                                ctx->spspps_buf, ctx->spspps_size,
-                               buf, nal_size)) < 0)
+                               buf, nal_size, 1)) < 0)
                 goto fail;
             ctx->new_idr = 0;
         /* if only SPS has been seen, also insert PPS */
@@ -242,15 +242,15 @@ static int h264_mp4toannexb_filter(AVBitStreamFilterContext *bsfc,
             if (ctx->pps_offset == -1) {
                 av_log(avctx, AV_LOG_WARNING, "PPS not present in the stream, nor in AVCC, stream may be unreadable\n");
                 if ((ret = alloc_and_copy(poutbuf, poutbuf_size,
-                                     NULL, 0, buf, nal_size)) < 0)
+                                     NULL, 0, buf, nal_size, 0)) < 0)
                     goto fail;
             } else if ((ret = alloc_and_copy(poutbuf, poutbuf_size,
                                         ctx->spspps_buf + ctx->pps_offset, ctx->spspps_size - ctx->pps_offset,
-                                        buf, nal_size)) < 0)
+                                        buf, nal_size, 1)) < 0)
                 goto fail;
         } else {
             if ((ret=alloc_and_copy(poutbuf, poutbuf_size,
-                               NULL, 0, buf, nal_size)) < 0)
+                               NULL, 0, buf, nal_size, unit_type == 7 || unit_type == 8)) < 0)
                 goto fail;
             if (!ctx->new_idr && unit_type == 1) {
                 ctx->new_idr = 1;