251 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*3 * TW5864 driver - H.264 headers generation functions4 *5 * Copyright (C) 2016 Bluecherry, LLC <maintainers@bluecherrydvr.com>6 */7 8#include <linux/log2.h>9 10#include "tw5864.h"11 12static u8 marker[] = { 0x00, 0x00, 0x00, 0x01 };13 14/*15 * Exponential-Golomb coding functions16 *17 * These functions are used for generation of H.264 bitstream headers.18 *19 * This code is derived from tw5864 reference driver by manufacturers, which20 * itself apparently was derived from x264 project.21 */22 23/* Bitstream writing context */24struct bs {25 u8 *buf; /* pointer to buffer beginning */26 u8 *buf_end; /* pointer to buffer end */27 u8 *ptr; /* pointer to current byte in buffer */28 unsigned int bits_left; /* number of available bits in current byte */29};30 31static void bs_init(struct bs *s, void *buf, int size)32{33 s->buf = buf;34 s->ptr = buf;35 s->buf_end = s->ptr + size;36 s->bits_left = 8;37}38 39static int bs_len(struct bs *s)40{41 return s->ptr - s->buf;42}43 44static void bs_write(struct bs *s, int count, u32 bits)45{46 if (s->ptr >= s->buf_end - 4)47 return;48 while (count > 0) {49 if (count < 32)50 bits &= (1 << count) - 1;51 if (count < s->bits_left) {52 *s->ptr = (*s->ptr << count) | bits;53 s->bits_left -= count;54 break;55 }56 *s->ptr = (*s->ptr << s->bits_left) |57 (bits >> (count - s->bits_left));58 count -= s->bits_left;59 s->ptr++;60 s->bits_left = 8;61 }62}63 64static void bs_write1(struct bs *s, u32 bit)65{66 if (s->ptr < s->buf_end) {67 *s->ptr <<= 1;68 *s->ptr |= bit;69 s->bits_left--;70 if (s->bits_left == 0) {71 s->ptr++;72 s->bits_left = 8;73 }74 }75}76 77static void bs_write_ue(struct bs *s, u32 val)78{79 if (val == 0) {80 bs_write1(s, 1);81 } else {82 val++;83 bs_write(s, 2 * fls(val) - 1, val);84 }85}86 87static void bs_write_se(struct bs *s, int val)88{89 bs_write_ue(s, val <= 0 ? -val * 2 : val * 2 - 1);90}91 92static void bs_rbsp_trailing(struct bs *s)93{94 bs_write1(s, 1);95 if (s->bits_left != 8)96 bs_write(s, s->bits_left, 0x00);97}98 99/* H.264 headers generation functions */100 101static int tw5864_h264_gen_sps_rbsp(u8 *buf, size_t size, int width, int height)102{103 struct bs bs, *s;104 105 s = &bs;106 bs_init(s, buf, size);107 bs_write(s, 8, 0x42); /* profile_idc, baseline */108 bs_write(s, 1, 1); /* constraint_set0_flag */109 bs_write(s, 1, 1); /* constraint_set1_flag */110 bs_write(s, 1, 0); /* constraint_set2_flag */111 bs_write(s, 5, 0); /* reserved_zero_5bits */112 bs_write(s, 8, 0x1e); /* level_idc */113 bs_write_ue(s, 0); /* seq_parameter_set_id */114 bs_write_ue(s, ilog2(MAX_GOP_SIZE) - 4); /* log2_max_frame_num_minus4 */115 bs_write_ue(s, 0); /* pic_order_cnt_type */116 /* log2_max_pic_order_cnt_lsb_minus4 */117 bs_write_ue(s, ilog2(MAX_GOP_SIZE) - 4);118 bs_write_ue(s, 1); /* num_ref_frames */119 bs_write(s, 1, 0); /* gaps_in_frame_num_value_allowed_flag */120 bs_write_ue(s, width / 16 - 1); /* pic_width_in_mbs_minus1 */121 bs_write_ue(s, height / 16 - 1); /* pic_height_in_map_units_minus1 */122 bs_write(s, 1, 1); /* frame_mbs_only_flag */123 bs_write(s, 1, 0); /* direct_8x8_inference_flag */124 bs_write(s, 1, 0); /* frame_cropping_flag */125 bs_write(s, 1, 0); /* vui_parameters_present_flag */126 bs_rbsp_trailing(s);127 return bs_len(s);128}129 130static int tw5864_h264_gen_pps_rbsp(u8 *buf, size_t size, int qp)131{132 struct bs bs, *s;133 134 s = &bs;135 bs_init(s, buf, size);136 bs_write_ue(s, 0); /* pic_parameter_set_id */137 bs_write_ue(s, 0); /* seq_parameter_set_id */138 bs_write(s, 1, 0); /* entropy_coding_mode_flag */139 bs_write(s, 1, 0); /* pic_order_present_flag */140 bs_write_ue(s, 0); /* num_slice_groups_minus1 */141 bs_write_ue(s, 0); /* i_num_ref_idx_l0_active_minus1 */142 bs_write_ue(s, 0); /* i_num_ref_idx_l1_active_minus1 */143 bs_write(s, 1, 0); /* weighted_pred_flag */144 bs_write(s, 2, 0); /* weighted_bipred_idc */145 bs_write_se(s, qp - 26); /* pic_init_qp_minus26 */146 bs_write_se(s, qp - 26); /* pic_init_qs_minus26 */147 bs_write_se(s, 0); /* chroma_qp_index_offset */148 bs_write(s, 1, 0); /* deblocking_filter_control_present_flag */149 bs_write(s, 1, 0); /* constrained_intra_pred_flag */150 bs_write(s, 1, 0); /* redundant_pic_cnt_present_flag */151 bs_rbsp_trailing(s);152 return bs_len(s);153}154 155static int tw5864_h264_gen_slice_head(u8 *buf, size_t size,156 unsigned int idr_pic_id,157 unsigned int frame_gop_seqno,158 int *tail_nb_bits, u8 *tail)159{160 struct bs bs, *s;161 int is_i_frame = frame_gop_seqno == 0;162 163 s = &bs;164 bs_init(s, buf, size);165 bs_write_ue(s, 0); /* first_mb_in_slice */166 bs_write_ue(s, is_i_frame ? 2 : 5); /* slice_type - I or P */167 bs_write_ue(s, 0); /* pic_parameter_set_id */168 bs_write(s, ilog2(MAX_GOP_SIZE), frame_gop_seqno); /* frame_num */169 if (is_i_frame)170 bs_write_ue(s, idr_pic_id);171 172 /* pic_order_cnt_lsb */173 bs_write(s, ilog2(MAX_GOP_SIZE), frame_gop_seqno);174 175 if (is_i_frame) {176 bs_write1(s, 0); /* no_output_of_prior_pics_flag */177 bs_write1(s, 0); /* long_term_reference_flag */178 } else {179 bs_write1(s, 0); /* num_ref_idx_active_override_flag */180 bs_write1(s, 0); /* ref_pic_list_reordering_flag_l0 */181 bs_write1(s, 0); /* adaptive_ref_pic_marking_mode_flag */182 }183 184 bs_write_se(s, 0); /* slice_qp_delta */185 186 if (s->bits_left != 8) {187 *tail = ((s->ptr[0]) << s->bits_left);188 *tail_nb_bits = 8 - s->bits_left;189 } else {190 *tail = 0;191 *tail_nb_bits = 0;192 }193 194 return bs_len(s);195}196 197void tw5864_h264_put_stream_header(u8 **buf, size_t *space_left, int qp,198 int width, int height)199{200 int nal_len;201 202 /* SPS */203 memcpy(*buf, marker, sizeof(marker));204 *buf += 4;205 *space_left -= 4;206 207 **buf = 0x67; /* SPS NAL header */208 *buf += 1;209 *space_left -= 1;210 211 nal_len = tw5864_h264_gen_sps_rbsp(*buf, *space_left, width, height);212 *buf += nal_len;213 *space_left -= nal_len;214 215 /* PPS */216 memcpy(*buf, marker, sizeof(marker));217 *buf += 4;218 *space_left -= 4;219 220 **buf = 0x68; /* PPS NAL header */221 *buf += 1;222 *space_left -= 1;223 224 nal_len = tw5864_h264_gen_pps_rbsp(*buf, *space_left, qp);225 *buf += nal_len;226 *space_left -= nal_len;227}228 229void tw5864_h264_put_slice_header(u8 **buf, size_t *space_left,230 unsigned int idr_pic_id,231 unsigned int frame_gop_seqno,232 int *tail_nb_bits, u8 *tail)233{234 int nal_len;235 236 memcpy(*buf, marker, sizeof(marker));237 *buf += 4;238 *space_left -= 4;239 240 /* Frame NAL header */241 **buf = (frame_gop_seqno == 0) ? 0x25 : 0x21;242 *buf += 1;243 *space_left -= 1;244 245 nal_len = tw5864_h264_gen_slice_head(*buf, *space_left, idr_pic_id,246 frame_gop_seqno, tail_nb_bits,247 tail);248 *buf += nal_len;249 *space_left -= nal_len;250}251