| Message ID | 20190213175124.3695-1-m.tretter@pengutronix.de |
|---|---|
| Headers | show |
| Series | Add ZynqMP VCU/Allegro DVT H.264 encoder driver | expand |
On Wed, 13 Feb 2019 18:51:21 +0100, Michael Tretter wrote: > This is v3 of the series to add support for the Allegro DVT H.264 encoder > found in the EV family of the Xilinx ZynqMP platform. > > The devicetree bindings now also include the clocks as documented in PG252 > "H.264/H.265 Video Codec Unit v1.2" [0]. > > Most changes involve added or removed v4l2 callbacks that were mentioned in > the review comments to make the v4l2-compliance test suite happy. And here are the test results: v4l2-compliance SHA: 9e375c965f200538817ab712330a86a123124394, 64 bits Compliance test for allegro device /dev/video4: Driver Info: Driver name : allegro Card type : Allegro DVT Video Encoder Bus info : platform:a0009000.al5e Driver version : 5.0.0 Capabilities : 0x84208000 Video Memory-to-Memory Streaming Extended Pix Format Device Capabilities Device Caps : 0x04208000 Video Memory-to-Memory Streaming Extended Pix Format Detected Stateful Encoder Required ioctls: test VIDIOC_QUERYCAP: OK Allow for multiple opens: test second /dev/video4 open: OK test VIDIOC_QUERYCAP: OK test VIDIOC_G/S_PRIORITY: OK test for unlimited opens: OK Debug ioctls: test VIDIOC_DBG_G/S_REGISTER: OK test VIDIOC_LOG_STATUS: OK (Not Supported) Input ioctls: test VIDIOC_G/S_TUNER/ENUM_FREQ_BANDS: OK (Not Supported) test VIDIOC_G/S_FREQUENCY: OK (Not Supported) test VIDIOC_S_HW_FREQ_SEEK: OK (Not Supported) test VIDIOC_ENUMAUDIO: OK (Not Supported) test VIDIOC_G/S/ENUMINPUT: OK (Not Supported) test VIDIOC_G/S_AUDIO: OK (Not Supported) Inputs: 0 Audio Inputs: 0 Tuners: 0 Output ioctls: test VIDIOC_G/S_MODULATOR: OK (Not Supported) test VIDIOC_G/S_FREQUENCY: OK (Not Supported) test VIDIOC_ENUMAUDOUT: OK (Not Supported) test VIDIOC_G/S/ENUMOUTPUT: OK (Not Supported) test VIDIOC_G/S_AUDOUT: OK (Not Supported) Outputs: 0 Audio Outputs: 0 Modulators: 0 Input/Output configuration ioctls: test VIDIOC_ENUM/G/S/QUERY_STD: OK (Not Supported) test VIDIOC_ENUM/G/S/QUERY_DV_TIMINGS: OK (Not Supported) test VIDIOC_DV_TIMINGS_CAP: OK (Not Supported) test VIDIOC_G/S_EDID: OK (Not Supported) Control ioctls: test VIDIOC_QUERY_EXT_CTRL/QUERYMENU: OK test VIDIOC_QUERYCTRL: OK test VIDIOC_G/S_CTRL: OK test VIDIOC_G/S/TRY_EXT_CTRLS: OK test VIDIOC_(UN)SUBSCRIBE_EVENT/DQEVENT: OK test VIDIOC_G/S_JPEGCOMP: OK (Not Supported) Standard Controls: 8 Private Controls: 0 Format ioctls: test VIDIOC_ENUM_FMT/FRAMESIZES/FRAMEINTERVALS: OK test VIDIOC_G/S_PARM: OK (Not Supported) test VIDIOC_G_FBUF: OK (Not Supported) test VIDIOC_G_FMT: OK test VIDIOC_TRY_FMT: OK test VIDIOC_S_FMT: OK test VIDIOC_G_SLICED_VBI_CAP: OK (Not Supported) test Cropping: OK (Not Supported) test Composing: OK (Not Supported) test Scaling: OK Codec ioctls: test VIDIOC_(TRY_)ENCODER_CMD: OK test VIDIOC_G_ENC_INDEX: OK (Not Supported) test VIDIOC_(TRY_)DECODER_CMD: OK (Not Supported) Buffer ioctls: test VIDIOC_REQBUFS/CREATE_BUFS/QUERYBUF: OK test VIDIOC_EXPBUF: OK test Requests: OK (Not Supported) Test input 0: Streaming ioctls: test read/write: OK (Not Supported) test blocking wait: OK Video Capture: Captured 60 buffers test MMAP (no poll): OK Video Capture: Captured 60 buffers test MMAP (select): OK test USERPTR (no poll): OK (Not Supported) test USERPTR (select): OK (Not Supported) test DMABUF: Cannot test, specify --expbuf-device Total for allegro device /dev/video4: 50, Succeeded: 50, Failed: 0, Warnings: 0 > > I also updated the firmware loading and device probing to properly handle > failures and correctly reset or stop the MCU. > > Still on my list is a check against the documentation of the memory-to-memory > stateful video encoder interface. If there are fixes necessary, I will include > them in a v4. > > I did not introduce a separate queue for internal buffers for encoded frames. > I am still considering this split, but I want to have tracing in place before > doing this. The changes will completely internal to the driver and can be done > later anyway. > > Each patch also contains a more detailed changelog. > > Michael
On 2/13/19 6:51 PM, Michael Tretter wrote: > Add a V4L2 mem-to-mem driver for Allegro DVT video IP cores as found in > the EV family of the Xilinx ZynqMP SoC. The Zynq UltraScale+ Device > Technical Reference Manual uses the term VCU (Video Codec Unit) for the > encoder, decoder and system integration block. > > This driver takes care of interacting with the MicroBlaze MCU that > controls the actual IP cores. The IP cores and MCU are integrated in the > FPGA. The xlnx_vcu driver is responsible for configuring the clocks and > providing information about the codec configuration. > > The driver currently only supports the H.264 video encoder. > > Signed-off-by: Michael Tretter <m.tretter@pengutronix.de> > --- <snip> > +static void allegro_finish_frame(struct allegro_channel *channel, > + struct mcu_msg_encode_frame_response *msg) > +{ > + struct allegro_dev *dev = channel->dev; > + struct vb2_v4l2_buffer *src_buf; > + struct vb2_v4l2_buffer *dst_buf; > + struct { > + u32 offset; > + u32 size; > + } *partition; > + enum vb2_buffer_state state = VB2_BUF_STATE_ERROR; > + > + src_buf = v4l2_m2m_src_buf_remove(channel->fh.m2m_ctx); > + > + dst_buf = v4l2_m2m_dst_buf_remove(channel->fh.m2m_ctx); > + dst_buf->sequence = channel->csequence++; > + > + if (msg->error_code) { > + v4l2_err(&dev->v4l2_dev, > + "channel %d: error while encoding frame: %x\n", > + channel->mcu_channel_id, msg->error_code); > + goto err; > + } > + > + if (msg->partition_table_size != 1) { > + v4l2_warn(&dev->v4l2_dev, > + "channel %d: only handling first partition table entry (%d entries)\n", > + channel->mcu_channel_id, msg->partition_table_size); > + } > + > + if (msg->partition_table_offset + > + msg->partition_table_size * sizeof(*partition) > > + vb2_plane_size(&dst_buf->vb2_buf, 0)) { > + v4l2_err(&dev->v4l2_dev, > + "channel %d: partition table outside of dst_buf\n", > + channel->mcu_channel_id); > + goto err; > + } > + > + partition = > + vb2_plane_vaddr(&dst_buf->vb2_buf, 0) + msg->partition_table_offset; > + if (partition->offset + partition->size > > + vb2_plane_size(&dst_buf->vb2_buf, 0)) { > + v4l2_err(&dev->v4l2_dev, > + "channel %d: encoded frame is outside of dst_buf (offset 0x%x, size 0x%x)\n", > + channel->mcu_channel_id, partition->offset, > + partition->size); > + goto err; > + } > + > + v4l2_dbg(2, debug, &dev->v4l2_dev, > + "channel %d: encoded frame of size %d is at offset 0x%x\n", > + channel->mcu_channel_id, partition->size, partition->offset); > + > + /* > + * TODO The partition->offset differs from the ENCODER_STREAM_OFFSET. > + * Does the encoder add any data before its configured offset that we > + * need to handle? > + */ > + > + vb2_set_plane_payload(&dst_buf->vb2_buf, 0, > + partition->offset + partition->size); > + > + state = VB2_BUF_STATE_DONE; > + > + dst_buf->vb2_buf.timestamp = src_buf->vb2_buf.timestamp; > + dst_buf->field = src_buf->field; > + > + dst_buf->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK; > + dst_buf->flags |= src_buf->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK; > + if (src_buf->flags & V4L2_BUF_FLAG_TIMECODE) { > + dst_buf->timecode = src_buf->timecode; > + dst_buf->flags |= V4L2_BUF_FLAG_TIMECODE; > + } else { > + dst_buf->flags &= ~V4L2_BUF_FLAG_TIMECODE; > + } Use v4l2_m2m_buf_copy_metadata() instead of copying this manually. > + if (msg->is_idr) { > + dst_buf->flags |= V4L2_BUF_FLAG_KEYFRAME; > + dst_buf->flags &= ~V4L2_BUF_FLAG_PFRAME; > + } else { > + dst_buf->flags |= V4L2_BUF_FLAG_PFRAME; > + dst_buf->flags &= ~V4L2_BUF_FLAG_KEYFRAME; > + } > + > + v4l2_dbg(1, debug, &dev->v4l2_dev, > + "channel %d: encoded frame (%s%s, %d bytes)\n", > + channel->mcu_channel_id, > + msg->is_idr ? "IDR, " : "", > + msg->slice_type == AL_ENC_SLICE_TYPE_I ? "I slice" : > + msg->slice_type == AL_ENC_SLICE_TYPE_P ? "P slice" : "unknown", > + partition->size); > + > +err: > + v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE); > + > + if (channel->stop) { > + const struct v4l2_event eos_event = { > + .type = V4L2_EVENT_EOS > + }; > + dst_buf->flags |= V4L2_BUF_FLAG_LAST; > + v4l2_event_queue_fh(&channel->fh, &eos_event); > + } > + > + v4l2_m2m_buf_done(dst_buf, state); > + v4l2_m2m_job_finish(dev->m2m_dev, channel->fh.m2m_ctx); > +} > + > +static int allegro_handle_init(struct allegro_dev *dev, > + struct mcu_msg_init_reply *msg) > +{ > + complete(&dev->init_complete); > + > + return 0; > +} > + > +static int > +allegro_handle_create_channel(struct allegro_dev *dev, > + struct mcu_msg_create_channel_response *msg) > +{ > + struct allegro_channel *channel; > + int err = 0; > + > + channel = allegro_find_channel_by_user_id(dev, msg->user_id); > + if (IS_ERR(channel)) { > + v4l2_warn(&dev->v4l2_dev, > + "received %s for unknown user %d\n", > + msg_type_name(msg->header.type), > + msg->user_id); > + return -EINVAL; > + } > + > + if (msg->error_code) { > + v4l2_err(&dev->v4l2_dev, > + "user %d: failed to create channel: error %d", > + channel->user_id, msg->error_code); > + return -EINVAL; > + } > + > + channel->mcu_channel_id = msg->channel_id; > + v4l2_dbg(1, debug, &dev->v4l2_dev, > + "user %d: channel has channel id %d\n", > + channel->user_id, channel->mcu_channel_id); > + > + v4l2_dbg(1, debug, &dev->v4l2_dev, > + "channel %d: intermediate buffers: %d x %d bytes\n", > + channel->mcu_channel_id, msg->int_buffers_count, > + msg->int_buffers_size); > + err = allocate_intermediate_buffers(channel, msg->int_buffers_count, > + msg->int_buffers_size); > + if (err) { > + v4l2_err(&dev->v4l2_dev, > + "channel %d: failed to allocate intermediate buffers", > + channel->mcu_channel_id); > + goto out; > + } > + allegro_mcu_push_buffer_intermediate(channel); > + if (err) { > + v4l2_err(&dev->v4l2_dev, > + "channel %d: failed to push reference buffers", > + channel->mcu_channel_id); > + goto out; > + } > + > + v4l2_dbg(1, debug, &dev->v4l2_dev, > + "channel %d: reference buffers: %d x %d bytes\n", > + channel->mcu_channel_id, msg->rec_buffers_count, > + msg->rec_buffers_size); > + err = allocate_reference_buffers(channel, msg->rec_buffers_count, > + msg->rec_buffers_size); > + if (err) { > + v4l2_err(&dev->v4l2_dev, > + "channel %d: failed to allocate reference buffers", > + channel->mcu_channel_id); > + goto out; > + } > + err = allegro_mcu_push_buffer_reference(channel); > + if (err) { > + v4l2_err(&dev->v4l2_dev, > + "channel %d: failed to push reference buffers", > + channel->mcu_channel_id); > + goto out; > + } > + > + channel->created = true; > + > + /* > + * FIXME Need to send CHANNEL_DESTROY if we fail to allocate the > + * intermediate or reference buffers? > + */ > +out: > + complete(&channel->completion); > + return err; > +} > + > +static int > +allegro_handle_destroy_channel(struct allegro_dev *dev, > + struct mcu_msg_destroy_channel_response *msg) > +{ > + struct allegro_channel *channel; > + > + channel = allegro_find_channel_by_channel_id(dev, msg->channel_id); > + if (IS_ERR(channel)) { > + v4l2_err(&dev->v4l2_dev, > + "received %s for unknown channel %d\n", > + msg_type_name(msg->header.type), > + msg->channel_id); > + return -EINVAL; > + } > + > + v4l2_dbg(2, debug, &dev->v4l2_dev, > + "user %d: vcu destroyed channel %d\n", > + channel->user_id, channel->mcu_channel_id); > + complete(&channel->completion); > + > + return 0; > +} > + > +static int > +allegro_handle_encode_frame(struct allegro_dev *dev, > + struct mcu_msg_encode_frame_response *msg) > +{ > + struct allegro_channel *channel; > + > + channel = allegro_find_channel_by_channel_id(dev, msg->channel_id); > + if (IS_ERR(channel)) { > + v4l2_err(&dev->v4l2_dev, > + "received %s for unknown channel %d\n", > + msg_type_name(msg->header.type), > + msg->channel_id); > + return -EINVAL; > + } > + > + allegro_finish_frame(channel, msg); > + > + return 0; > +} > + > +static int allegro_receive_message(struct allegro_dev *dev) > +{ > + struct mcu_msg_header *header; > + ssize_t size; > + int err = 0; > + > + /* > + * FIXME header is struct mcu_msg_header, but we are allocating memory > + * for a struct mcu_msg_encode_one_frm_response, because we only need > + * to parse the header before we can determine the type and size of > + * the actual message and struct mcu_msg_encode_one_frm_response is > + * currently the largest message. > + */ > + header = kmalloc(sizeof(struct mcu_msg_encode_frame_response), > + GFP_KERNEL); > + if (!header) > + return -ENOMEM; > + > + size = allegro_mbox_read(dev, &dev->mbox_status, header, > + sizeof(struct mcu_msg_encode_frame_response)); > + if (size < sizeof(*header)) { > + v4l2_err(&dev->v4l2_dev, > + "invalid mbox message (%ld): must be at least %lu\n", > + size, sizeof(*header)); > + err = -EINVAL; > + goto out; > + } > + > + switch(header->type) { > + case MCU_MSG_TYPE_INIT: > + err = allegro_handle_init(dev, > + (struct mcu_msg_init_reply *) header); > + break; > + case MCU_MSG_TYPE_CREATE_CHANNEL: > + err = allegro_handle_create_channel(dev, > + (struct mcu_msg_create_channel_response *)header); > + break; > + case MCU_MSG_TYPE_DESTROY_CHANNEL: > + err = allegro_handle_destroy_channel(dev, > + (struct mcu_msg_destroy_channel_response *)header); > + break; > + case MCU_MSG_TYPE_ENCODE_FRAME: > + err = allegro_handle_encode_frame(dev, > + (struct mcu_msg_encode_frame_response *)header); > + break; > + default: > + v4l2_warn(&dev->v4l2_dev, > + "%s: unknown message %s\n", > + __func__, msg_type_name(header->type)); > + err = -EINVAL; > + break; > + } > + > +out: > + kfree(header); > + > + return err; > +} > + > +irqreturn_t allegro_hardirq(int irq, void *data) > +{ > + struct allegro_dev *dev = data; > + unsigned int status; > + > + regmap_read(dev->regmap, AL5_ITC_CPU_IRQ_STA, &status); > + if (!(status & AL5_ITC_CPU_IRQ_STA_TRIGGERED)) > + return IRQ_NONE; > + > + regmap_write(dev->regmap, AL5_ITC_CPU_IRQ_CLR, status); > + > + /* > + * The downstream driver suggests to wait until the clear has > + * propagated through the hardware, i.e., > + * > + * !(read(AL5_ITC_CPU_IRQ_STA) & AL5_ITC_CPU_IRQ_STA_TRIGGERED) > + * > + * I assume that waiting for the interrupt to be cleared is not > + * required and just continue execution. > + */ > + > + return IRQ_WAKE_THREAD; > +} > + > +irqreturn_t allegro_irq_thread(int irq, void *data) > +{ > + struct allegro_dev *dev = data; > + > + allegro_receive_message(dev); > + > + return IRQ_HANDLED; > +} > + > +static void allegro_copy_firmware(struct allegro_dev *dev, > + const u8 * const buf, size_t size) > +{ > + int err = 0; > + > + v4l2_dbg(1, debug, &dev->v4l2_dev, > + "copy mcu firmware (%lu B) to SRAM\n", size); > + err = regmap_bulk_write(dev->sram, 0x0, buf, size / 4); > + if (err) > + v4l2_err(&dev->v4l2_dev, > + "failed to copy firmware: %d\n", err); > +} > + > +static void allegro_copy_fw_codec(struct allegro_dev *dev, > + const u8 * const buf, size_t size) > +{ > + int err; > + dma_addr_t icache_offset, dcache_offset; > + > + /* > + * The downstream allocates 600 KB for the codec firmware to have some > + * extra space for "possible extensions." My tests were fine with > + * allocating just enough memory for the actual firmware, but I am not > + * sure that the firmware really does not use the remaining space. > + */ > + err = allegro_alloc_buffer(dev, &dev->firmware, size); > + if (err) { > + v4l2_err(&dev->v4l2_dev, > + "failed to allocate %lu bytes for firmware\n", size); > + return; > + } > + > + v4l2_dbg(1, debug, &dev->v4l2_dev, > + "copy codec firmware (%ld B) to phys 0x%llx", > + size, dev->firmware.paddr); > + memcpy(dev->firmware.vaddr, buf, size); > + > + regmap_write(dev->regmap, AXI_ADDR_OFFSET_IP, > + upper_32_bits(dev->firmware.paddr)); > + > + /* > + * TODO I am not sure what the icache and dcache offsets are doing. > + * Copy behavior from downstream driver for now. > + */ > + icache_offset = dev->firmware.paddr - MCU_CACHE_OFFSET; > + v4l2_dbg(2, debug, &dev->v4l2_dev, > + "icache_offset: msb = 0x%x, lsb = 0x%x\n", > + upper_32_bits(icache_offset), lower_32_bits(icache_offset)); > + regmap_write(dev->regmap, AL5_ICACHE_ADDR_OFFSET_MSB, > + upper_32_bits(icache_offset)); > + regmap_write(dev->regmap, AL5_ICACHE_ADDR_OFFSET_LSB, > + lower_32_bits(icache_offset)); > + > + dcache_offset = > + (dev->firmware.paddr & 0xffffffff00000000) - MCU_CACHE_OFFSET; > + v4l2_dbg(2, debug, &dev->v4l2_dev, > + "dcache_offset: msb = 0x%x, lsb = 0x%x\n", > + upper_32_bits(dcache_offset), lower_32_bits(dcache_offset)); > + regmap_write(dev->regmap, AL5_DCACHE_ADDR_OFFSET_MSB, > + upper_32_bits(dcache_offset)); > + regmap_write(dev->regmap, AL5_DCACHE_ADDR_OFFSET_LSB, > + lower_32_bits(dcache_offset)); > +} > + > +static void allegro_free_fw_codec(struct allegro_dev *dev) > +{ > + allegro_free_buffer(dev, &dev->firmware); > +} > + > +/* > + * Control functions for the MCU > + */ > + > +static int allegro_mcu_enable_interrupts(struct allegro_dev *dev) > +{ > + return regmap_write(dev->regmap, AL5_ITC_CPU_IRQ_MSK, BIT(0)); > +} > + > +static int allegro_mcu_disable_interrupts(struct allegro_dev *dev) > +{ > + return regmap_write(dev->regmap, AL5_ITC_CPU_IRQ_MSK, 0); > +} > + > +static int allegro_mcu_wait_for_sleep(struct allegro_dev *dev) > +{ > + unsigned long timeout; > + unsigned int status; > + > + timeout = jiffies + msecs_to_jiffies(100); > + while (regmap_read(dev->regmap, AL5_MCU_STA, &status) == 0 && > + status != AL5_MCU_STA_SLEEP) { > + if (time_after(jiffies, timeout)) > + return -ETIMEDOUT; > + cpu_relax(); > + } > + > + return 0; > +} > + > +static int allegro_mcu_start(struct allegro_dev *dev) > +{ > + unsigned long timeout; > + unsigned int status; > + int err; > + > + err = regmap_write(dev->regmap, AL5_MCU_WAKEUP, BIT(0)); > + if (err) > + return err; > + > + timeout = jiffies + msecs_to_jiffies(100); > + while (regmap_read(dev->regmap, AL5_MCU_STA, &status) == 0 && > + status == AL5_MCU_STA_SLEEP) { > + if (time_after(jiffies, timeout)) > + return -ETIMEDOUT; > + cpu_relax(); > + } > + > + err = regmap_write(dev->regmap, AL5_MCU_WAKEUP, 0); > + if (err) > + return err; > + > + return 0; > +} > + > +static int allegro_mcu_reset(struct allegro_dev *dev) > +{ > + int err; > + > + err = regmap_write(dev->regmap, > + AL5_MCU_RESET_MODE, AL5_MCU_RESET_MODE_SLEEP); > + if (err < 0) > + return err; > + > + err = regmap_write(dev->regmap, AL5_MCU_RESET, AL5_MCU_RESET_SOFT); > + if (err < 0) > + return err; > + > + return allegro_mcu_wait_for_sleep(dev); > +} > + > +/* > + * Create the MCU channel > + * > + * After the channel has been created, the picture size, format, colorspace > + * and framerate are fixed. Also the codec, profile, bitrate, etc. cannot be > + * changed anymore. > + * > + * The channel can be created only once. The MCU will accept source buffers > + * and stream buffers only after a channel has been created. > + */ > +static int allegro_create_channel(struct allegro_channel *channel) > +{ > + struct allegro_dev *dev = channel->dev; > + unsigned long timeout; > + int err; > + enum v4l2_mpeg_video_h264_level min_level; > + > + if (channel->created) { > + v4l2_warn(&dev->v4l2_dev, > + "channel already has been created\n"); > + return 0; > + } > + > + channel->user_id = allegro_next_user_id(dev); > + if (channel->user_id < 0) { > + v4l2_err(&dev->v4l2_dev, > + "no free channels available\n"); > + return -EBUSY; > + } > + set_bit(channel->user_id, &dev->channel_user_ids); > + > + v4l2_dbg(1, debug, &dev->v4l2_dev, > + "user %d: creating channel (%4.4s, %dx%d@%d) \n", > + channel->user_id, > + (char *)&channel->codec, channel->width, channel->height, 25); > + > + min_level = select_minimum_h264_level(channel->width, channel->height); > + if (channel->level < min_level) { > + v4l2_warn(&dev->v4l2_dev, > + "user %d: selected Level %s too low: increasing to Level %s\n", > + channel->user_id, > + v4l2_ctrl_get_menu(V4L2_CID_MPEG_VIDEO_H264_LEVEL)[channel->level], > + v4l2_ctrl_get_menu(V4L2_CID_MPEG_VIDEO_H264_LEVEL)[min_level]); > + channel->level = min_level; > + } > + > + reinit_completion(&channel->completion); > + allegro_mcu_send_create_channel(dev, channel); > + timeout = wait_for_completion_timeout(&channel->completion, > + msecs_to_jiffies(5000)); > + if (timeout == 0) { > + err = -ETIMEDOUT; > + goto err; > + } > + if (!channel->created) { > + /* FIXME Return a proper error code */ > + err = -1; > + goto err; > + } > + > + v4l2_dbg(1, debug, &dev->v4l2_dev, > + "channel %d: accepting buffers\n", > + channel->mcu_channel_id); > + > + return 0; > + > +err: > + clear_bit(channel->user_id, &dev->channel_user_ids); > + channel->user_id = -1; > + return err; > +} > + > +static void allegro_destroy_channel(struct allegro_channel *channel) > +{ > + struct allegro_dev *dev = channel->dev; > + unsigned long timeout; > + > + if (!channel->created) { > + v4l2_warn(&dev->v4l2_dev, > + "cannot destroy channel: has not been created\n"); > + return; > + } > + > + reinit_completion(&channel->completion); > + allegro_mcu_send_destroy_channel(dev, channel); > + timeout = wait_for_completion_timeout(&channel->completion, > + msecs_to_jiffies(5000)); > + if (timeout == 0) > + v4l2_warn(&dev->v4l2_dev, > + "timeout while destroying channel %d\n", > + channel->mcu_channel_id); > + > + channel->mcu_channel_id = -1; > + channel->created = false; > + clear_bit(channel->user_id, &dev->channel_user_ids); > + channel->user_id = -1; > + > + destroy_intermediate_buffers(channel); > + destroy_reference_buffers(channel); > +} > + > +static void allegro_set_default_params(struct allegro_channel *channel) > +{ > + channel->width = ALLEGRO_WIDTH_DEFAULT; > + channel->height = ALLEGRO_HEIGHT_DEFAULT; > + channel->stride = round_up(channel->width, 32); > + > + channel->colorspace = V4L2_COLORSPACE_REC709; > + channel->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT; > + channel->quantization = V4L2_QUANTIZATION_DEFAULT; > + channel->xfer_func = V4L2_XFER_FUNC_DEFAULT; > + > + channel->pixelformat = V4L2_PIX_FMT_NV12; > + channel->sizeimage_raw = channel->stride * channel->height * 3 / 2; > + > + channel->codec = V4L2_PIX_FMT_H264; > + channel->profile = V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE; > + channel->level = > + select_minimum_h264_level(channel->width, channel->height); > + channel->sizeimage_encoded = > + estimate_stream_size(channel->width, channel->height); > + > + channel->bitrate_mode = V4L2_MPEG_VIDEO_BITRATE_MODE_CBR; > + channel->bitrate = maximum_bitrate(channel->level); > + channel->bitrate_peak = maximum_bitrate(channel->level); > + channel->cpb_size = maximum_cpb_size(channel->level); > + channel->gop_size = ALLEGRO_GOP_SIZE_DEFAULT; > +} > + > +static int allegro_queue_setup(struct vb2_queue *vq, > + unsigned int *nbuffers, unsigned int *nplanes, > + unsigned int sizes[], > + struct device *alloc_devs[]) > +{ > + struct allegro_channel *channel = vb2_get_drv_priv(vq); > + struct allegro_dev *dev = channel->dev; > + > + v4l2_dbg(2, debug, &dev->v4l2_dev, > + "%s: queue setup[%s]: nplanes = %d\n", > + vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT ? "output" : "capture", > + *nplanes == 0 ? "REQBUFS" : "CREATE_BUFS", *nplanes); > + > + if (*nplanes != 0) { > + if (*nplanes != 1) > + return -EINVAL; > + if (vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) { > + if (sizes[0] < channel->sizeimage_encoded) > + return -EINVAL; > + > + } else { > + if (sizes[0] < channel->sizeimage_raw) > + return -EINVAL; > + } > + } else { > + *nplanes = 1; > + if (vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) { > + sizes[0] = channel->sizeimage_encoded; > + } else { > + sizes[0] = channel->sizeimage_raw; > + } > + } > + > + return 0; > +} > + > +static int allegro_buf_prepare(struct vb2_buffer *vb) > +{ > + struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); > + struct allegro_channel *channel = vb2_get_drv_priv(vb->vb2_queue); > + struct allegro_dev *dev = channel->dev; > + > + if (V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) { > + if (vbuf->field == V4L2_FIELD_ANY) > + vbuf->field = V4L2_FIELD_NONE; > + if (vbuf->field != V4L2_FIELD_NONE) { > + v4l2_err(&dev->v4l2_dev, > + "channel %d: unsupported field\n", > + channel->mcu_channel_id); > + return -EINVAL; > + } > + } > + > + return 0; > +} > + > +static void allegro_buf_queue(struct vb2_buffer *vb) > +{ > + struct allegro_channel *channel = vb2_get_drv_priv(vb->vb2_queue); > + struct allegro_dev *dev = channel->dev; > + struct vb2_queue *vq = vb->vb2_queue; > + struct allegro_buffer al_buf; > + > + if (vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) { > + al_buf.paddr = vb2_dma_contig_plane_dma_addr(vb, 0); > + al_buf.size = vb2_plane_size(vb, 0); > + > + v4l2_dbg(1, debug, &dev->v4l2_dev, > + "channel %d: queuing stream buffer: paddr: 0x%08llx, %ld bytes\n", > + channel->mcu_channel_id, al_buf.paddr, al_buf.size); > + allegro_mcu_send_put_stream_buffer(dev, channel, al_buf); > + } > + > + v4l2_m2m_buf_queue(channel->fh.m2m_ctx, to_vb2_v4l2_buffer(vb)); > +} > + > +static int allegro_start_streaming(struct vb2_queue *q, unsigned int count) > +{ > + struct allegro_channel *channel = vb2_get_drv_priv(q); > + struct allegro_dev *dev = channel->dev; > + > + v4l2_dbg(2, debug, &dev->v4l2_dev, > + "%s: start streaming\n", > + q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT ? "output" : "capture"); > + > + if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) { > + channel->osequence = 0; > + } else { > + channel->csequence = 0; > + } > + > + return 0; > +} > + > +static void allegro_stop_streaming(struct vb2_queue *q) > +{ > + struct allegro_channel *channel = vb2_get_drv_priv(q); > + struct allegro_dev *dev = channel->dev; > + struct vb2_v4l2_buffer *buffer; > + > + v4l2_dbg(2, debug, &dev->v4l2_dev, > + "%s: stop streaming\n", > + q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT ? "output" : "capture"); > + > + if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) { > + while ((buffer = v4l2_m2m_src_buf_remove(channel->fh.m2m_ctx))) > + v4l2_m2m_buf_done(buffer, VB2_BUF_STATE_ERROR); > + } else if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) { > + /* > + * FIXME The channel is created during queue_setup(). > + * Therefore, destroying it in stop_streaming might result in > + * a start_streaming() without a channel, which is really bad. > + */ I think this is fixed now, is this an outdated comment? There are a few FIXMEs and TODOs in this source, it's probably a good idea to check them and fix them where it makes sense for v4. > + allegro_destroy_channel(channel); > + while ((buffer = v4l2_m2m_dst_buf_remove(channel->fh.m2m_ctx))) > + v4l2_m2m_buf_done(buffer, VB2_BUF_STATE_ERROR); > + } > +} Regards, Hans
On 2/13/19 6:51 PM, Michael Tretter wrote: > The allegro hardware encoder does not write SPS/PPS nal units into the > encoded video stream. Therefore, we need to write the units in software. > > The implementation follows Rec. ITU-T H.264 (04/2017) to allow to > convert between a C struct and the RBSP representation of the SPS and > PPS nal units. > > The allegro driver writes the nal units into the v4l2 capture buffer in > front of the actual video data which is written at an offset by the IP > core. The remaining gap is filled with a filler nal unit. > > Signed-off-by: Michael Tretter <m.tretter@pengutronix.de> > --- > v2 -> v3: > none > > v1 -> v2: > - clean up debug log levels > - fix missing error handling in allegro_h264_write_sps > - enable configuration of frame size > - enable configuration of bit rate and CPB size > --- > drivers/staging/media/allegro-dvt/Makefile | 4 +- > .../staging/media/allegro-dvt/allegro-core.c | 182 ++- > drivers/staging/media/allegro-dvt/nal-h264.c | 1278 +++++++++++++++++ > drivers/staging/media/allegro-dvt/nal-h264.h | 188 +++ > 4 files changed, 1650 insertions(+), 2 deletions(-) > create mode 100644 drivers/staging/media/allegro-dvt/nal-h264.c > create mode 100644 drivers/staging/media/allegro-dvt/nal-h264.h > > diff --git a/drivers/staging/media/allegro-dvt/Makefile b/drivers/staging/media/allegro-dvt/Makefile > index bc30addee47f..eee9713c10e3 100644 > --- a/drivers/staging/media/allegro-dvt/Makefile > +++ b/drivers/staging/media/allegro-dvt/Makefile > @@ -1,4 +1,6 @@ > # SPDX-License-Identifier: GPL-2.0 > -allegro-objs := allegro-core.o > +ccflags-y += -I$(src) > + > +allegro-objs := allegro-core.o nal-h264.o > > obj-$(CONFIG_VIDEO_ALLEGRO_DVT) += allegro.o > diff --git a/drivers/staging/media/allegro-dvt/allegro-core.c b/drivers/staging/media/allegro-dvt/allegro-core.c > index d0e1b2a574a6..8fa53c171110 100644 > --- a/drivers/staging/media/allegro-dvt/allegro-core.c > +++ b/drivers/staging/media/allegro-dvt/allegro-core.c > @@ -26,6 +26,8 @@ > #include <media/videobuf2-dma-contig.h> > #include <media/videobuf2-v4l2.h> > > +#include "nal-h264.h" > + > /* > * PG252 June 6, 2018 (H.264/H.265 Video Codec Unit v1.1) Chapter 3 > * > @@ -1281,6 +1283,131 @@ static int allocate_reference_buffers(struct allegro_channel *channel, > n, PAGE_ALIGN(size)); > } > > +static ssize_t allegro_h264_write_sps(struct allegro_channel *channel, > + void *dest, size_t n) > +{ > + struct allegro_dev *dev = channel->dev; > + struct nal_h264_sps *sps; > + ssize_t size; > + unsigned int size_mb = SIZE_MACROBLOCK; > + /* Calculation of crop units in Rec. ITU-T H.264 (04/2017) p. 76 */ > + unsigned int crop_unit_x = 2; > + unsigned int crop_unit_y = 2; > + > + sps = kzalloc(sizeof(*sps), GFP_KERNEL); > + if (!sps) > + return -ENOMEM; > + > + sps->profile_idc = nal_h264_profile_from_v4l2(channel->profile); > + sps->constraint_set0_flag = 0; > + sps->constraint_set1_flag = 1; > + sps->constraint_set2_flag = 0; > + sps->constraint_set3_flag = 0; > + sps->constraint_set4_flag = 0; > + sps->constraint_set5_flag = 0; > + sps->level_idc = nal_h264_level_from_v4l2(channel->level); > + sps->seq_parameter_set_id = 0; > + sps->log2_max_frame_num_minus4 = 0; > + sps->pic_order_cnt_type = 0; > + sps->log2_max_pic_order_cnt_lsb_minus4 = 6; > + sps->max_num_ref_frames = 3; > + sps->gaps_in_frame_num_value_allowed_flag = 0; > + sps->pic_width_in_mbs_minus1 = > + DIV_ROUND_UP(channel->width, size_mb) - 1; > + sps->pic_height_in_map_units_minus1 = > + DIV_ROUND_UP(channel->height, size_mb) - 1; > + sps->frame_mbs_only_flag = 1; > + sps->mb_adaptive_frame_field_flag = 0; > + sps->direct_8x8_inference_flag = 1; > + sps->frame_cropping_flag = > + (channel->width % size_mb) || (channel->height % size_mb); > + if (sps->frame_cropping_flag) { > + sps->crop_left = 0; > + sps->crop_right = (round_up(channel->width, size_mb) - channel->width) / crop_unit_x; > + sps->crop_top = 0; > + sps->crop_bottom = (round_up(channel->height, size_mb) - channel->height) / crop_unit_y; > + } > + sps->vui_parameters_present_flag = 1; > + sps->vui.aspect_ratio_info_present_flag = 0; > + sps->vui.overscan_info_present_flag = 0; > + sps->vui.video_signal_type_present_flag = 1; > + sps->vui.video_format = 1; > + sps->vui.video_full_range_flag = 0; > + sps->vui.colour_description_present_flag = 1; > + sps->vui.colour_primaries = 5; > + sps->vui.transfer_characteristics = 5; > + sps->vui.matrix_coefficients = 5; > + sps->vui.chroma_loc_info_present_flag = 1; > + sps->vui.chroma_sample_loc_type_top_field = 0; > + sps->vui.chroma_sample_loc_type_bottom_field = 0; > + sps->vui.timing_info_present_flag = 1; > + sps->vui.num_units_in_tick = 1; > + sps->vui.time_scale = 50; > + sps->vui.fixed_frame_rate_flag = 1; > + sps->vui.nal_hrd_parameters_present_flag = 0; > + sps->vui.vcl_hrd_parameters_present_flag = 1; > + sps->vui.vcl_hrd_parameters.cpb_cnt_minus1 = 0; > + sps->vui.vcl_hrd_parameters.bit_rate_scale = 0; > + sps->vui.vcl_hrd_parameters.cpb_size_scale = 1; > + /* See Rec. ITU-T H.264 (04/2017) p. 410 E-53 */ > + sps->vui.vcl_hrd_parameters.bit_rate_value_minus1[0] = > + channel->bitrate_peak / (1 << (6 + sps->vui.vcl_hrd_parameters.bit_rate_scale)) - 1; > + /* See Rec. ITU-T H.264 (04/2017) p. 410 E-54 */ > + sps->vui.vcl_hrd_parameters.cpb_size_value_minus1[0] = > + (channel->cpb_size * 1000) / (1 << (4 + sps->vui.vcl_hrd_parameters.cpb_size_scale)) - 1; > + sps->vui.vcl_hrd_parameters.cbr_flag[0] = 1; > + sps->vui.vcl_hrd_parameters.initial_cpb_removal_delay_length_minus1 = 31; > + sps->vui.vcl_hrd_parameters.cpb_removal_delay_length_minus1 = 31; > + sps->vui.vcl_hrd_parameters.dpb_output_delay_length_minus1 = 31; > + sps->vui.vcl_hrd_parameters.time_offset_length = 0; > + sps->vui.low_delay_hrd_flag = 0; > + sps->vui.pic_struct_present_flag = 1; > + sps->vui.bitstream_restriction_flag = 0; > + > + size = nal_h264_write_sps(&dev->plat_dev->dev, dest, n, sps); > + > + kfree(sps); > + > + return size; > +} > + > +static ssize_t allegro_h264_write_pps(struct allegro_channel *channel, > + void *dest, size_t n) > +{ > + struct allegro_dev *dev = channel->dev; > + struct nal_h264_pps *pps; > + ssize_t size; > + > + pps = kzalloc(sizeof(*pps), GFP_KERNEL); > + if (!pps) > + return -ENOMEM; > + > + pps->pic_parameter_set_id = 0; > + pps->seq_parameter_set_id = 0; > + pps->entropy_coding_mode_flag = 0; > + pps->bottom_field_pic_order_in_frame_present_flag = 0; > + pps->num_slice_groups_minus1 = 0; > + pps->num_ref_idx_l0_default_active_minus1 = 2; > + pps->num_ref_idx_l1_default_active_minus1 = 2; > + pps->weighted_pred_flag = 0; > + pps->weighted_bipred_idc = 0; > + pps->pic_init_qp_minus26 = 0; > + pps->pic_init_qs_minus26 = 0; > + pps->chroma_qp_index_offset = 0; > + pps->deblocking_filter_control_present_flag = 1; > + pps->constrained_intra_pred_flag = 0; > + pps->redundant_pic_cnt_present_flag = 0; > + pps->transform_8x8_mode_flag = 0; > + pps->pic_scaling_matrix_present_flag = 0; > + pps->second_chroma_qp_index_offset = 0; > + > + size = nal_h264_write_pps(&dev->plat_dev->dev, dest, n, pps); > + > + kfree(pps); > + > + return size; > +} > + > static void allegro_finish_frame(struct allegro_channel *channel, > struct mcu_msg_encode_frame_response *msg) > { > @@ -1292,6 +1419,9 @@ static void allegro_finish_frame(struct allegro_channel *channel, > u32 size; > } *partition; > enum vb2_buffer_state state = VB2_BUF_STATE_ERROR; > + char *curr; > + ssize_t len; > + ssize_t free; > > src_buf = v4l2_m2m_src_buf_remove(channel->fh.m2m_ctx); > > @@ -1340,10 +1470,60 @@ static void allegro_finish_frame(struct allegro_channel *channel, > * Does the encoder add any data before its configured offset that we > * need to handle? > */ > - > vb2_set_plane_payload(&dst_buf->vb2_buf, 0, > partition->offset + partition->size); > > + curr = vb2_plane_vaddr(&dst_buf->vb2_buf, 0); > + free = partition->offset; > + if (msg->is_idr) { > + len = allegro_h264_write_sps(channel, curr, free); > + if (len < 0) { > + v4l2_err(&dev->v4l2_dev, > + "not enough space for sequence parameter set: %ld left\n", > + free); > + goto err; > + } > + curr += len; > + free -= len; > + v4l2_dbg(1, debug, &dev->v4l2_dev, > + "channel %d: wrote %ld byte SPS nal unit\n", > + channel->mcu_channel_id, len); > + } > + > + if (msg->slice_type == AL_ENC_SLICE_TYPE_I) { > + len = allegro_h264_write_pps(channel, curr, free); > + if (len < 0) { > + v4l2_err(&dev->v4l2_dev, > + "not enough space for picture parameter set: %ld left\n", > + free); > + goto err; > + } > + curr += len; > + free -= len; > + v4l2_dbg(1, debug, &dev->v4l2_dev, > + "channel %d: wrote %ld byte PPS nal unit\n", > + channel->mcu_channel_id, len); > + } > + > + len = nal_h264_write_filler(&dev->plat_dev->dev, curr, free); > + if (len < 0) { > + v4l2_err(&dev->v4l2_dev, > + "failed to write %ld filler data\n", free); > + goto err; > + } > + curr += len; > + free -= len; > + v4l2_dbg(2, debug, &dev->v4l2_dev, > + "channel %d: wrote %ld bytes filler nal unit\n", > + channel->mcu_channel_id, len); > + > + if (free != 0) { > + v4l2_err(&dev->v4l2_dev, > + "non-VCL NAL units do not fill space until VCL NAL unit: %ld bytes left\n", > + free); > + goto err; > + } > + > state = VB2_BUF_STATE_DONE; > > dst_buf->vb2_buf.timestamp = src_buf->vb2_buf.timestamp; > diff --git a/drivers/staging/media/allegro-dvt/nal-h264.c b/drivers/staging/media/allegro-dvt/nal-h264.c > new file mode 100644 > index 000000000000..83bc98200c1a > --- /dev/null > +++ b/drivers/staging/media/allegro-dvt/nal-h264.c > @@ -0,0 +1,1278 @@ > +// SPDX-License-Identifier: GPL-2.0 > +/* > + * Copyright (C) 2019 Pengutronix, Michael Tretter <kernel@pengutronix.de> > + * > + * Convert NAL units between raw byte sequence payloads (RBSP) and C structs > + * > + * The conversion is defined in "ITU-T Rec. H.264 (04/2017) Advanced video > + * coding for generic audiovisual services". Decoder drivers may use the > + * parser to parse RBSP from encoded streams and configure the hardware, if > + * the hardware is not able to parse RBSP itself. Encoder drivers may use the > + * generator to generate the RBSP for SPS/PPS nal units and add them to the > + * encoded stream if the hardware does not generate the units. > + */ > + > +#include <linux/kernel.h> > +#include <linux/types.h> > +#include <linux/string.h> > +#include <linux/v4l2-controls.h> > + > +#include <linux/device.h> > +#include <linux/export.h> > +#include <linux/log2.h> > + > +#include <nal-h264.h> > + > +struct rbsp { > + char *buf; > + int size; > + int pos; > + int num_consecutive_zeros; > +}; > + > +int nal_h264_profile_from_v4l2(enum v4l2_mpeg_video_h264_profile profile) > +{ > + switch (profile) { > + case V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE: > + return 66; > + case V4L2_MPEG_VIDEO_H264_PROFILE_MAIN: > + return 77; > + case V4L2_MPEG_VIDEO_H264_PROFILE_EXTENDED: > + return 88; > + case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH: > + return 100; > + default: > + return -EINVAL; > + } > +} > + > +int nal_h264_level_from_v4l2(enum v4l2_mpeg_video_h264_level level) > +{ > + switch (level) { > + case V4L2_MPEG_VIDEO_H264_LEVEL_1_0: > + return 10; > + case V4L2_MPEG_VIDEO_H264_LEVEL_1B: > + return 9; > + case V4L2_MPEG_VIDEO_H264_LEVEL_1_1: > + return 11; > + case V4L2_MPEG_VIDEO_H264_LEVEL_1_2: > + return 12; > + case V4L2_MPEG_VIDEO_H264_LEVEL_1_3: > + return 13; > + case V4L2_MPEG_VIDEO_H264_LEVEL_2_0: > + return 20; > + case V4L2_MPEG_VIDEO_H264_LEVEL_2_1: > + return 21; > + case V4L2_MPEG_VIDEO_H264_LEVEL_2_2: > + return 22; > + case V4L2_MPEG_VIDEO_H264_LEVEL_3_0: > + return 30; > + case V4L2_MPEG_VIDEO_H264_LEVEL_3_1: > + return 31; > + case V4L2_MPEG_VIDEO_H264_LEVEL_3_2: > + return 32; > + case V4L2_MPEG_VIDEO_H264_LEVEL_4_0: > + return 40; > + case V4L2_MPEG_VIDEO_H264_LEVEL_4_1: > + return 41; > + case V4L2_MPEG_VIDEO_H264_LEVEL_4_2: > + return 42; > + case V4L2_MPEG_VIDEO_H264_LEVEL_5_0: > + return 50; > + case V4L2_MPEG_VIDEO_H264_LEVEL_5_1: > + return 51; > + default: > + return -EINVAL; > + } > +} > + > +static int rbsp_read_bits(struct rbsp *rbsp, int num, int *val); > +static int rbsp_write_bits(struct rbsp *rbsp, int num, int val); > + > +static int add_emulation_prevention_three_byte(struct rbsp *rbsp) > +{ > + rbsp->num_consecutive_zeros = 0; > + /* > + * We are not actually the emulation_prevention_three_byte, but the 2 > + * one bits of the byte and the 6 zero bits of the next byte. > + * Therefore, the discarded byte shifted by 6 bits. > + */ > + rbsp_write_bits(rbsp, 8, (0x3 << 6)); > + > + return 0; > +} > + > +static int discard_emulation_prevention_three_byte(struct rbsp *rbsp) > +{ > + unsigned int tmp = 0; > + > + rbsp->num_consecutive_zeros = 0; > + /* > + * We are not actually discarding the emulation_prevention_three_byte, > + * but the 2 one bits of the byte and the 6 zero bits of the next > + * byte. Therefore, the discarded byte shifted by 6 bits. > + */ > + rbsp_read_bits(rbsp, 8, &tmp); > + if (tmp != (0x3 << 6)) > + return -EINVAL; > + > + return 0; > +} > + > +static inline int rbsp_read_bit(struct rbsp *rbsp) > +{ > + int shift; > + int ofs; > + int bit; > + int err; > + > + if (rbsp->num_consecutive_zeros == 22) { > + err = discard_emulation_prevention_three_byte(rbsp); > + if (err) > + return err; > + } > + > + shift = 7 - (rbsp->pos % 8); > + ofs = rbsp->pos++ / 8; > + > + if (ofs >= rbsp->size) > + return -EINVAL; > + > + bit = (rbsp->buf[ofs] >> shift) & 1; > + > + /* > + * Counting zeros for the emulation_prevention_three_byte only starts > + * at byte boundaries. > + */ > + if (bit == 1 || > + (rbsp->num_consecutive_zeros < 7 && (rbsp->pos % 8 == 0))) > + rbsp->num_consecutive_zeros = 0; > + else > + rbsp->num_consecutive_zeros++; > + > + return bit; > +} > + > +static inline int rbsp_write_bit(struct rbsp *rbsp, int bit) > +{ > + int shift; > + int ofs; > + > + if (rbsp->num_consecutive_zeros == 22) > + add_emulation_prevention_three_byte(rbsp); > + > + shift = 7 - (rbsp->pos % 8); > + ofs = rbsp->pos++ / 8; > + > + if (ofs >= rbsp->size) > + return -EINVAL; > + > + rbsp->buf[ofs] &= ~(1 << shift); > + rbsp->buf[ofs] |= bit << shift; > + > + /* > + * Counting zeros for the emulation_prevention_three_byte only starts > + * at byte boundaries. > + */ > + if (bit == 1 || > + (rbsp->num_consecutive_zeros < 7 && (rbsp->pos % 8 == 0))) { > + rbsp->num_consecutive_zeros = 0; > + } else { > + rbsp->num_consecutive_zeros++; > + } > + > + return 0; > +} > + > +static inline int rbsp_read_bits(struct rbsp *rbsp, int num, int *val) > +{ > + int i, ret; > + int tmp = 0; > + > + if (num > 32) > + return -EINVAL; > + > + for (i = 0; i < num; i++) { > + ret = rbsp_read_bit(rbsp); > + if (ret < 0) > + return ret; > + tmp |= ret << (num - i - 1); > + } > + > + if (val) > + *val = tmp; > + > + return 0; > +} > + > +static int rbsp_write_bits(struct rbsp *rbsp, int num, int value) > +{ > + int ret; > + > + while (num--) { > + ret = rbsp_write_bit(rbsp, (value >> num) & 1); > + if (ret) > + return ret; > + } > + > + return 0; > +} > + > +static int rbsp_read_uev(struct rbsp *rbsp, unsigned int *val) > +{ > + int leading_zero_bits = 0; > + unsigned int tmp = 0; > + int ret; > + > + while ((ret = rbsp_read_bit(rbsp)) == 0) > + leading_zero_bits++; > + if (ret < 0) > + return ret; > + > + if (leading_zero_bits > 0) { > + ret = rbsp_read_bits(rbsp, leading_zero_bits, &tmp); > + if (ret) > + return ret; > + } > + > + if (val) > + *val = (1 << leading_zero_bits) - 1 + tmp; > + > + return 0; > +} > + > +static int rbsp_write_uev(struct rbsp *rbsp, unsigned int value) > +{ > + int i; > + int ret; > + int tmp = value + 1; > + int leading_zero_bits = fls(tmp) - 1; > + > + for (i = 0; i < leading_zero_bits; i++) { > + ret = rbsp_write_bit(rbsp, 0); > + if (ret) > + return ret; > + } > + > + return rbsp_write_bits(rbsp, leading_zero_bits + 1, tmp); > +} > + > +static int rbsp_read_sev(struct rbsp *rbsp, int *val) > +{ > + unsigned int tmp; > + int ret; > + > + ret = rbsp_read_uev(rbsp, &tmp); > + if (ret) > + return ret; > + > + if (val) { > + if (tmp & 1) > + *val = (tmp + 1) / 2; > + else > + *val = -(tmp / 2); > + } > + > + return 0; > +} > + > +static int rbsp_write_sev(struct rbsp *rbsp, int val) > +{ > + unsigned int tmp; > + > + if (val > 0) > + tmp = (2 * val) | 1; > + else > + tmp = -2 * val; > + > + return rbsp_write_uev(rbsp, tmp); > +} > + > +#define READ_BIT(field) \ > + do { \ > + int ret = rbsp_read_bit(rbsp); \ > + if (ret < 0) \ > + return ret; \ > + s->field = ret; \ > + } while (0) > + > +#define READ_BITS(num, field) \ > + do { \ > + int val; \ > + int ret = rbsp_read_bits(rbsp, (num), &val); \ > + if (ret) \ > + return ret; \ > + s->field = val; \ > + } while (0) > + > +#define READ_UEV(field) \ > + do { \ > + int ret = rbsp_read_uev(rbsp, &s->field); \ > + if (ret) \ > + return ret; \ > + } while (0) > + > +#define READ_SEV(field) \ > + do { \ > + int ret = rbsp_read_sev(rbsp, &s->field); \ > + if (ret) \ > + return ret; \ > + } while (0) > + > +#define WRITE_BIT(field) \ > + do { \ > + int ret = rbsp_write_bit(rbsp, s->field); \ > + if (ret < 0) \ > + return ret; \ > + } while (0) > + > +#define WRITE_BITS(num, field) \ > + do { \ > + int ret = rbsp_write_bits(rbsp, (num), s->field); \ > + if (ret) \ > + return ret; \ > + } while (0) > + > +#define WRITE_UEV(field) \ > + do { \ > + int ret = rbsp_write_uev(rbsp, s->field); \ > + if (ret) \ > + return ret; \ > + } while (0) > + > +#define WRITE_SEV(field) \ > + do { \ > + int ret = rbsp_write_sev(rbsp, s->field); \ > + if (ret) \ > + return ret; \ > + } while (0) > + > +#define PRINT_BIT(field) \ > + dev_dbg(dev, "field: %u\n", s->field) \ > + > +#define PRINT_BITS(num, field) \ > + dev_dbg(dev, "field: %u\n", s->field) \ > + > +#define PRINT_UEV(field) \ > + dev_dbg(dev, "field: %u\n", s->field) \ > + > +#define PRINT_SEV(field) \ > + dev_dbg(dev, "field: %d\n", s->field) \ > + > +static int nal_h264_write_trailing_bits(const struct device *dev, > + struct rbsp *rbsp) > +{ > + rbsp_write_bit(rbsp, 1); > + while (rbsp->pos % 8) > + rbsp_write_bit(rbsp, 0); > + > + return 0; > +} > + > +static int nal_h264_write_hrd_parameters(const struct device *dev, > + struct rbsp *rbsp, > + struct nal_h264_hrd_parameters *hrd) > +{ > + struct nal_h264_hrd_parameters *s = hrd; > + int i; > + > + WRITE_UEV(cpb_cnt_minus1); > + WRITE_BITS(4, bit_rate_scale); > + WRITE_BITS(4, cpb_size_scale); > + > + for (i = 0; i <= hrd->cpb_cnt_minus1; i++) { > + WRITE_UEV(bit_rate_value_minus1[i]); > + WRITE_UEV(cpb_size_value_minus1[i]); > + WRITE_BIT(cbr_flag[i]); > + } > + > + WRITE_BITS(5, initial_cpb_removal_delay_length_minus1); > + WRITE_BITS(5, cpb_removal_delay_length_minus1); > + WRITE_BITS(5, dpb_output_delay_length_minus1); > + WRITE_BITS(5, time_offset_length); > + > + return 0; > +} > + > +static int nal_h264_read_hrd_parameters(const struct device *dev, > + struct rbsp *rbsp, > + struct nal_h264_hrd_parameters *hrd) > +{ > + struct nal_h264_hrd_parameters *s = hrd; > + unsigned int i; > + > + READ_UEV(cpb_cnt_minus1); > + READ_BITS(4, bit_rate_scale); > + READ_BITS(4, cpb_size_scale); > + > + for (i = 0; i <= hrd->cpb_cnt_minus1; i++) { > + READ_UEV(bit_rate_value_minus1[i]); > + READ_UEV(cpb_size_value_minus1[i]); > + READ_BIT(cbr_flag[i]); > + } > + > + READ_BITS(5, initial_cpb_removal_delay_length_minus1); > + READ_BITS(5, cpb_removal_delay_length_minus1); > + READ_BITS(5, dpb_output_delay_length_minus1); > + READ_BITS(5, time_offset_length); > + > + return 0; > +} > + > +static void nal_h264_print_hrd_parameters(const struct device *dev, > + struct nal_h264_hrd_parameters *hrd) > +{ > + struct nal_h264_hrd_parameters *s = hrd; > + unsigned int i; > + > + if (!hrd) > + return; > + > + PRINT_UEV(cpb_cnt_minus1); > + PRINT_BITS(4, bit_rate_scale); > + PRINT_BITS(4, cpb_size_scale); > + > + for (i = 0; i <= s->cpb_cnt_minus1; i++) { > + PRINT_UEV(bit_rate_value_minus1[i]); > + PRINT_UEV(cpb_size_value_minus1[i]); > + PRINT_BIT(cbr_flag[i]); > + } > + > + PRINT_BITS(5, initial_cpb_removal_delay_length_minus1); > + PRINT_BITS(5, cpb_removal_delay_length_minus1); > + PRINT_BITS(5, dpb_output_delay_length_minus1); > + PRINT_BITS(5, time_offset_length); > +} > + > +static int nal_h264_read_vui_parameters(const struct device *dev, > + struct rbsp *rbsp, > + struct nal_h264_vui_parameters *vui) > +{ > + struct nal_h264_vui_parameters *s = vui; > + int err; > + > + READ_BIT(aspect_ratio_info_present_flag); > + if (vui->aspect_ratio_info_present_flag) { > + READ_BITS(8, aspect_ratio_idc); > + if (vui->aspect_ratio_idc == 255) { > + READ_BITS(16, sar_width); > + READ_BITS(16, sar_height); > + } > + } > + > + READ_BIT(overscan_info_present_flag); > + if (vui->overscan_info_present_flag) > + READ_BIT(overscan_appropriate_flag); > + > + READ_BIT(video_signal_type_present_flag); > + if (vui->video_signal_type_present_flag) { > + READ_BITS(3, video_format); > + READ_BIT(video_full_range_flag); > + READ_BIT(colour_description_present_flag); > + > + if (vui->colour_description_present_flag) { > + READ_BITS(8, colour_primaries); > + READ_BITS(8, transfer_characteristics); > + READ_BITS(8, matrix_coefficients); > + } > + } > + > + READ_BIT(chroma_loc_info_present_flag); > + if (vui->chroma_loc_info_present_flag) { > + READ_UEV(chroma_sample_loc_type_top_field); > + READ_UEV(chroma_sample_loc_type_bottom_field); > + } > + > + READ_BIT(timing_info_present_flag); > + if (vui->timing_info_present_flag) { > + READ_BITS(32, num_units_in_tick); > + READ_BITS(32, time_scale); > + READ_BIT(fixed_frame_rate_flag); > + } > + > + READ_BIT(nal_hrd_parameters_present_flag); > + if (vui->nal_hrd_parameters_present_flag) { > + err = nal_h264_read_hrd_parameters(dev, rbsp, > + &vui->nal_hrd_parameters); > + if (err) > + return err; > + } > + > + READ_BIT(vcl_hrd_parameters_present_flag); > + if (vui->vcl_hrd_parameters_present_flag) { > + err = nal_h264_read_hrd_parameters(dev, rbsp, > + &vui->vcl_hrd_parameters); > + if (err) > + return err; > + } > + > + if (vui->nal_hrd_parameters_present_flag || > + vui->vcl_hrd_parameters_present_flag) > + READ_BIT(low_delay_hrd_flag); > + > + READ_BIT(pic_struct_present_flag); > + > + READ_BIT(bitstream_restriction_flag); > + if (vui->bitstream_restriction_flag) { > + READ_BIT(motion_vectors_over_pic_boundaries_flag); > + READ_UEV(max_bytes_per_pic_denom); > + READ_UEV(max_bits_per_mb_denom); > + READ_UEV(log2_max_mv_length_horizontal); > + READ_UEV(log21_max_mv_length_vertical); > + READ_UEV(max_num_reorder_frames); > + READ_UEV(max_dec_frame_buffering); > + } > + > + return 0; > +} > + > +static ssize_t nal_h264_write_vui_parameters(const struct device *dev, > + struct rbsp *rbsp, > + struct nal_h264_vui_parameters *vui) > +{ > + struct nal_h264_vui_parameters *s = vui; > + int err; > + > + WRITE_BIT(aspect_ratio_info_present_flag); > + if (vui->aspect_ratio_info_present_flag) { > + WRITE_BITS(8, aspect_ratio_idc); > + if (vui->aspect_ratio_idc == 255) { > + WRITE_BITS(16, sar_width); > + WRITE_BITS(16, sar_height); > + } > + } > + > + WRITE_BIT(overscan_info_present_flag); > + if (vui->overscan_info_present_flag) > + WRITE_BIT(overscan_appropriate_flag); > + > + WRITE_BIT(video_signal_type_present_flag); > + if (vui->video_signal_type_present_flag) { > + WRITE_BITS(3, video_format); > + WRITE_BIT(video_full_range_flag); > + WRITE_BIT(colour_description_present_flag); > + > + if (vui->colour_description_present_flag) { > + WRITE_BITS(8, colour_primaries); > + WRITE_BITS(8, transfer_characteristics); > + WRITE_BITS(8, matrix_coefficients); > + } > + } > + > + WRITE_BIT(chroma_loc_info_present_flag); > + if (vui->chroma_loc_info_present_flag) { > + WRITE_UEV(chroma_sample_loc_type_top_field); > + WRITE_UEV(chroma_sample_loc_type_bottom_field); > + } > + > + WRITE_BIT(timing_info_present_flag); > + if (vui->timing_info_present_flag) { > + WRITE_BITS(32, num_units_in_tick); > + WRITE_BITS(32, time_scale); > + WRITE_BIT(fixed_frame_rate_flag); > + } > + > + WRITE_BIT(nal_hrd_parameters_present_flag); > + if (vui->nal_hrd_parameters_present_flag) { > + err = nal_h264_write_hrd_parameters(dev, rbsp, > + &vui->nal_hrd_parameters); > + if (err) > + return err; > + } > + > + WRITE_BIT(vcl_hrd_parameters_present_flag); > + if (vui->vcl_hrd_parameters_present_flag) { > + err = nal_h264_write_hrd_parameters(dev, rbsp, > + &vui->vcl_hrd_parameters); > + if (err) > + return err; > + } > + > + if (vui->nal_hrd_parameters_present_flag || > + vui->vcl_hrd_parameters_present_flag) > + WRITE_BIT(low_delay_hrd_flag); > + > + WRITE_BIT(pic_struct_present_flag); > + > + WRITE_BIT(bitstream_restriction_flag); > + if (vui->bitstream_restriction_flag) { > + WRITE_BIT(motion_vectors_over_pic_boundaries_flag); > + WRITE_UEV(max_bytes_per_pic_denom); > + WRITE_UEV(max_bits_per_mb_denom); > + WRITE_UEV(log2_max_mv_length_horizontal); > + WRITE_UEV(log21_max_mv_length_vertical); > + WRITE_UEV(max_num_reorder_frames); > + WRITE_UEV(max_dec_frame_buffering); > + } > + > + return 0; > +} > + > +static void nal_h264_print_vui_parameters(const struct device *dev, > + struct nal_h264_vui_parameters *vui) > +{ > + struct nal_h264_vui_parameters *s = vui; > + > + if (!vui) > + return; > + > + PRINT_BIT(aspect_ratio_info_present_flag); > + if (vui->aspect_ratio_info_present_flag) { > + PRINT_BITS(8, aspect_ratio_idc); > + if (vui->aspect_ratio_idc == 255) { > + PRINT_BITS(16, sar_width); > + PRINT_BITS(16, sar_height); > + } > + } > + > + PRINT_BIT(overscan_info_present_flag); > + if (vui->overscan_info_present_flag) > + PRINT_BIT(overscan_appropriate_flag); > + > + PRINT_BIT(video_signal_type_present_flag); > + if (vui->video_signal_type_present_flag) { > + PRINT_BITS(3, video_format); > + PRINT_BIT(video_full_range_flag); > + PRINT_BIT(colour_description_present_flag); > + > + if (vui->colour_description_present_flag) { > + PRINT_BITS(8, colour_primaries); > + PRINT_BITS(8, transfer_characteristics); > + PRINT_BITS(8, matrix_coefficients); > + } > + } > + > + PRINT_BIT(chroma_loc_info_present_flag); > + if (vui->chroma_loc_info_present_flag) { > + PRINT_UEV(chroma_sample_loc_type_top_field); > + PRINT_UEV(chroma_sample_loc_type_bottom_field); > + } > + > + PRINT_BIT(timing_info_present_flag); > + if (vui->timing_info_present_flag) { > + PRINT_BITS(32, num_units_in_tick); > + PRINT_BITS(32, time_scale); > + PRINT_BIT(fixed_frame_rate_flag); > + } > + > + PRINT_BIT(nal_hrd_parameters_present_flag); > + if (vui->nal_hrd_parameters_present_flag) > + nal_h264_print_hrd_parameters(dev, &vui->nal_hrd_parameters); > + > + PRINT_BIT(vcl_hrd_parameters_present_flag); > + if (vui->vcl_hrd_parameters_present_flag) > + nal_h264_print_hrd_parameters(dev, &vui->vcl_hrd_parameters); > + > + if (vui->nal_hrd_parameters_present_flag || > + vui->vcl_hrd_parameters_present_flag) > + PRINT_BIT(low_delay_hrd_flag); > + > + PRINT_BIT(pic_struct_present_flag); > + > + PRINT_BIT(bitstream_restriction_flag); > + if (vui->bitstream_restriction_flag) { > + PRINT_BIT(motion_vectors_over_pic_boundaries_flag); > + PRINT_UEV(max_bytes_per_pic_denom); > + PRINT_UEV(max_bits_per_mb_denom); > + PRINT_UEV(log2_max_mv_length_horizontal); > + PRINT_UEV(log21_max_mv_length_vertical); > + PRINT_UEV(max_num_reorder_frames); > + PRINT_UEV(max_dec_frame_buffering); > + } > +} > + > +static int nal_h264_rbsp_write_sps(const struct device *dev, > + struct rbsp *rbsp, struct nal_h264_sps *sps) > +{ > + struct nal_h264_sps *s = sps; > + unsigned int i; > + int err; > + > + if (rbsp->size < 3) > + return -EINVAL; > + > + WRITE_BITS(8, profile_idc); > + WRITE_BIT(constraint_set0_flag); > + WRITE_BIT(constraint_set1_flag); > + WRITE_BIT(constraint_set2_flag); > + WRITE_BIT(constraint_set3_flag); > + WRITE_BIT(constraint_set4_flag); > + WRITE_BIT(constraint_set5_flag); > + WRITE_BITS(2, reserved_zero_2bits); > + WRITE_BITS(8, level_idc); > + > + WRITE_UEV(seq_parameter_set_id); > + > + if (sps->profile_idc == 100 || sps->profile_idc == 110 || > + sps->profile_idc == 122 || sps->profile_idc == 244 || > + sps->profile_idc == 44 || sps->profile_idc == 83 || > + sps->profile_idc == 86 || sps->profile_idc == 118 || > + sps->profile_idc == 128 || sps->profile_idc == 138 || > + sps->profile_idc == 139 || sps->profile_idc == 134 || > + sps->profile_idc == 135) { > + WRITE_UEV(chroma_format_idc); > + > + if (sps->chroma_format_idc == 3) > + WRITE_BIT(separate_colour_plane_flag); > + > + WRITE_UEV(bit_depth_luma_minus8); > + WRITE_UEV(bit_depth_chroma_minus8); > + WRITE_BIT(qpprime_y_zero_transform_bypass_flag); > + WRITE_BIT(seq_scaling_matrix_present_flag); > + > + if (sps->seq_scaling_matrix_present_flag) { > + dev_err(dev, > + "%s: Handling scaling matrix not supported\n", > + __func__); > + return -EINVAL; > + } > + } > + > + WRITE_UEV(log2_max_frame_num_minus4); > + > + WRITE_UEV(pic_order_cnt_type); > + if (sps->pic_order_cnt_type == 0) { > + WRITE_UEV(log2_max_pic_order_cnt_lsb_minus4); > + } else if (sps->pic_order_cnt_type == 1) { > + WRITE_BIT(delta_pic_order_always_zero_flag); > + WRITE_SEV(offset_for_non_ref_pic); > + WRITE_SEV(offset_for_top_to_bottom_field); > + > + WRITE_UEV(num_ref_frames_in_pic_order_cnt_cycle); > + for (i = 0; i < sps->num_ref_frames_in_pic_order_cnt_cycle; i++) > + WRITE_SEV(offset_for_ref_frame[i]); > + } else { > + dev_err(dev, > + "%s: Invalid pic_order_cnt_type %u\n", __func__, > + sps->pic_order_cnt_type); > + return -EINVAL; > + } > + > + WRITE_UEV(max_num_ref_frames); > + WRITE_BIT(gaps_in_frame_num_value_allowed_flag); > + WRITE_UEV(pic_width_in_mbs_minus1); > + WRITE_UEV(pic_height_in_map_units_minus1); > + > + WRITE_BIT(frame_mbs_only_flag); > + if (!sps->frame_mbs_only_flag) > + WRITE_BIT(mb_adaptive_frame_field_flag); > + > + WRITE_BIT(direct_8x8_inference_flag); > + > + WRITE_BIT(frame_cropping_flag); > + if (sps->frame_cropping_flag) { > + WRITE_UEV(crop_left); > + WRITE_UEV(crop_right); > + WRITE_UEV(crop_top); > + WRITE_UEV(crop_bottom); > + } > + > + WRITE_BIT(vui_parameters_present_flag); > + if (sps->vui_parameters_present_flag) { > + err = nal_h264_write_vui_parameters(dev, rbsp, &sps->vui); > + if (err) > + return err; > + } > + > + return 0; > +} > + > +static int nal_h264_rbsp_read_sps(const struct device *dev, > + struct rbsp *rbsp, struct nal_h264_sps *sps) > +{ > + struct nal_h264_sps *s = sps; > + unsigned int i; > + int err; > + > + if (rbsp->size < 3) > + return -EINVAL; > + > + READ_BITS(8, profile_idc); > + READ_BIT(constraint_set0_flag); > + READ_BIT(constraint_set1_flag); > + READ_BIT(constraint_set2_flag); > + READ_BIT(constraint_set3_flag); > + READ_BIT(constraint_set4_flag); > + READ_BIT(constraint_set5_flag); > + READ_BITS(2, reserved_zero_2bits); > + READ_BITS(8, level_idc); > + > + READ_UEV(seq_parameter_set_id); > + > + if (sps->profile_idc == 100 || sps->profile_idc == 110 || > + sps->profile_idc == 122 || sps->profile_idc == 244 || > + sps->profile_idc == 44 || sps->profile_idc == 83 || > + sps->profile_idc == 86 || sps->profile_idc == 118 || > + sps->profile_idc == 128 || sps->profile_idc == 138 || > + sps->profile_idc == 139 || sps->profile_idc == 134 || > + sps->profile_idc == 135) { > + READ_UEV(chroma_format_idc); > + > + if (sps->chroma_format_idc == 3) > + READ_BIT(separate_colour_plane_flag); > + > + READ_UEV(bit_depth_luma_minus8); > + READ_UEV(bit_depth_chroma_minus8); > + READ_BIT(qpprime_y_zero_transform_bypass_flag); > + READ_BIT(seq_scaling_matrix_present_flag); > + > + if (sps->seq_scaling_matrix_present_flag) { > + dev_err(dev, > + "%s: Handling scaling matrix not supported\n", > + __func__); > + return -EINVAL; > + } > + } > + > + READ_UEV(log2_max_frame_num_minus4); > + > + READ_UEV(pic_order_cnt_type); > + if (sps->pic_order_cnt_type == 0) { > + READ_UEV(log2_max_pic_order_cnt_lsb_minus4); > + } else if (sps->pic_order_cnt_type == 1) { > + READ_BIT(delta_pic_order_always_zero_flag); > + READ_SEV(offset_for_non_ref_pic); > + READ_SEV(offset_for_top_to_bottom_field); > + > + READ_UEV(num_ref_frames_in_pic_order_cnt_cycle); > + for (i = 0; i < sps->num_ref_frames_in_pic_order_cnt_cycle; i++) > + READ_SEV(offset_for_ref_frame[i]); > + } else { > + dev_err(dev, > + "%s: Invalid pic_order_cnt_type %u\n", __func__, > + sps->pic_order_cnt_type); > + return -EINVAL; > + } > + > + READ_UEV(max_num_ref_frames); > + READ_BIT(gaps_in_frame_num_value_allowed_flag); > + READ_UEV(pic_width_in_mbs_minus1); > + READ_UEV(pic_height_in_map_units_minus1); > + > + READ_BIT(frame_mbs_only_flag); > + if (!sps->frame_mbs_only_flag) > + READ_BIT(mb_adaptive_frame_field_flag); > + > + READ_BIT(direct_8x8_inference_flag); > + > + READ_BIT(frame_cropping_flag); > + if (sps->frame_cropping_flag) { > + READ_UEV(crop_left); > + READ_UEV(crop_right); > + READ_UEV(crop_top); > + READ_UEV(crop_bottom); > + } > + > + READ_BIT(vui_parameters_present_flag); > + if (sps->vui_parameters_present_flag) { > + err = nal_h264_read_vui_parameters(dev, rbsp, &sps->vui); > + if (err) > + return err; > + } > + > + return 0; > +} > + > +static int nal_h264_rbsp_write_pps(const struct device *dev, > + struct rbsp *rbsp, struct nal_h264_pps *pps) > +{ > + struct nal_h264_pps *s = pps; > + int i; > + > + WRITE_UEV(pic_parameter_set_id); > + WRITE_UEV(seq_parameter_set_id); > + WRITE_BIT(entropy_coding_mode_flag); > + WRITE_BIT(bottom_field_pic_order_in_frame_present_flag); > + WRITE_UEV(num_slice_groups_minus1); > + if (pps->num_slice_groups_minus1 > 0) { > + WRITE_UEV(slice_group_map_type); > + if (pps->slice_group_map_type == 0) { > + for (i = 0; i < pps->num_slice_groups_minus1; i++) > + WRITE_UEV(run_length_minus1[i]); > + } else if (pps->slice_group_map_type == 2) { > + for (i = 0; i < pps->num_slice_groups_minus1; i++) { > + WRITE_UEV(top_left[i]); > + WRITE_UEV(bottom_right[i]); > + } > + } else if (pps->slice_group_map_type == 3 || > + pps->slice_group_map_type == 4 || > + pps->slice_group_map_type == 5) { > + WRITE_BIT(slice_group_change_direction_flag); > + WRITE_UEV(slice_group_change_rate_minus1); > + } else if (pps->slice_group_map_type == 6) { > + WRITE_UEV(pic_size_in_map_units_minus1); > + for (i = 0; i < pps->pic_size_in_map_units_minus1; i++) > + WRITE_BITS(order_base_2 > + (s->num_slice_groups_minus1 + 1), > + slice_group_id[i]); > + } > + } > + WRITE_UEV(num_ref_idx_l0_default_active_minus1); > + WRITE_UEV(num_ref_idx_l1_default_active_minus1); > + WRITE_BIT(weighted_pred_flag); > + WRITE_BITS(2, weighted_bipred_idc); > + WRITE_SEV(pic_init_qp_minus26); > + WRITE_SEV(pic_init_qs_minus26); > + WRITE_SEV(chroma_qp_index_offset); > + WRITE_BIT(deblocking_filter_control_present_flag); > + WRITE_BIT(constrained_intra_pred_flag); > + WRITE_BIT(redundant_pic_cnt_present_flag); > + if (/* more_rbsp_data() */ false) { > + WRITE_BIT(transform_8x8_mode_flag); > + WRITE_BIT(pic_scaling_matrix_present_flag); > + if (pps->pic_scaling_matrix_present_flag) { > + dev_err(dev, > + "%s: Handling scaling matrix not supported\n", > + __func__); > + return -EINVAL; > + } > + WRITE_SEV(second_chroma_qp_index_offset); > + } > + > + return 0; > +} > + > +static int nal_h264_rbsp_read_pps(const struct device *dev, > + struct rbsp *rbsp, struct nal_h264_pps *pps) > +{ > + struct nal_h264_pps *s = pps; > + unsigned int i; > + > + READ_UEV(pic_parameter_set_id); > + READ_UEV(seq_parameter_set_id); > + READ_BIT(entropy_coding_mode_flag); > + READ_BIT(bottom_field_pic_order_in_frame_present_flag); > + READ_UEV(num_slice_groups_minus1); > + if (s->num_slice_groups_minus1 > 0) { > + READ_UEV(slice_group_map_type); > + if (pps->slice_group_map_type == 0) { > + for (i = 0; i < pps->num_slice_groups_minus1; i++) > + READ_UEV(run_length_minus1[i]); > + } else if (pps->slice_group_map_type == 2) { > + for (i = 0; i < pps->num_slice_groups_minus1; i++) { > + READ_UEV(top_left[i]); > + READ_UEV(bottom_right[i]); > + } > + } else if (s->slice_group_map_type == 3 || > + s->slice_group_map_type == 4 || > + s->slice_group_map_type == 5) { > + READ_BIT(slice_group_change_direction_flag); > + READ_UEV(slice_group_change_rate_minus1); > + } else if (s->slice_group_map_type == 6) { > + READ_UEV(pic_size_in_map_units_minus1); > + for (i = 0; i < s->pic_size_in_map_units_minus1; i++) > + READ_BITS(order_base_2 > + (s->num_slice_groups_minus1 + 1), > + slice_group_id[i]); > + } > + } > + READ_UEV(num_ref_idx_l0_default_active_minus1); > + READ_UEV(num_ref_idx_l1_default_active_minus1); > + READ_BIT(weighted_pred_flag); > + READ_BITS(2, weighted_bipred_idc); > + READ_SEV(pic_init_qp_minus26); > + READ_SEV(pic_init_qs_minus26); > + READ_SEV(chroma_qp_index_offset); > + READ_BIT(deblocking_filter_control_present_flag); > + READ_BIT(constrained_intra_pred_flag); > + READ_BIT(redundant_pic_cnt_present_flag); > + if (/* more_rbsp_data() */ false) { > + READ_BIT(transform_8x8_mode_flag); > + READ_BIT(pic_scaling_matrix_present_flag); > + if (pps->pic_scaling_matrix_present_flag) { > + dev_err(dev, > + "%s: Handling scaling matrix not supported\n", > + __func__); > + return -EINVAL; > + } > + READ_SEV(second_chroma_qp_index_offset); > + } > + > + return 0; > +} > + > +ssize_t nal_h264_write_sps(const struct device *dev, > + void *dest, size_t n, struct nal_h264_sps *sps) > +{ > + struct rbsp rbsp; > + int err; > + u8 *p = dest; > + > + rbsp.buf = p + 5; > + rbsp.size = n - 5; > + rbsp.pos = 0; > + > + err = nal_h264_rbsp_write_sps(dev, &rbsp, sps); > + if (err) > + return err; > + > + err = nal_h264_write_trailing_bits(dev, &rbsp); > + if (err) > + return err; > + > + p[0] = 0x00; > + p[1] = 0x00; > + p[2] = 0x00; > + p[3] = 0x01; > + p[4] = 0x07; > + > + return ((rbsp.pos + 7) / 8) + 5; > +} > +EXPORT_SYMBOL_GPL(nal_h264_write_sps); > + > +ssize_t nal_h264_read_sps(const struct device *dev, > + struct nal_h264_sps *sps, void *src, size_t n) > +{ > + struct rbsp rbsp; > + int err; > + > + rbsp.buf = src; > + rbsp.size = n; > + rbsp.pos = 0; > + > + rbsp.buf += 5; > + rbsp.size -= 5; > + > + err = nal_h264_rbsp_read_sps(dev, &rbsp, sps); > + if (err) > + return err; > + > + return ((rbsp.pos + 7) / 8) + 5; > +} > +EXPORT_SYMBOL_GPL(nal_h264_read_sps); > + > +void nal_h264_print_sps(const struct device *dev, struct nal_h264_sps *sps) > +{ > + struct nal_h264_sps *s = sps; > + unsigned int i; > + > + if (!sps) > + return; > + > + PRINT_BITS(8, profile_idc); > + PRINT_BIT(constraint_set0_flag); > + PRINT_BIT(constraint_set1_flag); > + PRINT_BIT(constraint_set2_flag); > + PRINT_BIT(constraint_set3_flag); > + PRINT_BIT(constraint_set4_flag); > + PRINT_BIT(constraint_set5_flag); > + PRINT_BITS(2, reserved_zero_2bits); > + PRINT_BITS(8, level_idc); > + > + PRINT_UEV(seq_parameter_set_id); > + > + if (sps->profile_idc == 100 || sps->profile_idc == 110 || > + sps->profile_idc == 122 || sps->profile_idc == 244 || > + sps->profile_idc == 44 || sps->profile_idc == 83 || > + sps->profile_idc == 86 || sps->profile_idc == 118 || > + sps->profile_idc == 128 || sps->profile_idc == 138 || > + sps->profile_idc == 139 || sps->profile_idc == 134 || > + sps->profile_idc == 135) { > + PRINT_UEV(chroma_format_idc); > + > + if (sps->chroma_format_idc == 3) > + PRINT_BIT(separate_colour_plane_flag); > + > + PRINT_UEV(bit_depth_luma_minus8); > + PRINT_UEV(bit_depth_chroma_minus8); > + PRINT_BIT(qpprime_y_zero_transform_bypass_flag); > + PRINT_BIT(seq_scaling_matrix_present_flag); > + > + if (sps->seq_scaling_matrix_present_flag) > + dev_err(dev, > + "%s: Handling scaling matrix not supported\n", > + __func__); > + } > + > + PRINT_UEV(log2_max_frame_num_minus4); > + > + PRINT_UEV(pic_order_cnt_type); > + if (sps->pic_order_cnt_type == 0) { > + PRINT_UEV(log2_max_pic_order_cnt_lsb_minus4); > + } else if (sps->pic_order_cnt_type == 1) { > + PRINT_BIT(delta_pic_order_always_zero_flag); > + PRINT_SEV(offset_for_non_ref_pic); > + PRINT_SEV(offset_for_top_to_bottom_field); > + > + PRINT_UEV(num_ref_frames_in_pic_order_cnt_cycle); > + for (i = 0; i < sps->num_ref_frames_in_pic_order_cnt_cycle; i++) > + PRINT_SEV(offset_for_ref_frame[i]); > + } else { > + dev_err(dev, > + "%s: Invalid pic_order_cnt_type %u\n", __func__, > + sps->pic_order_cnt_type); > + } > + > + PRINT_UEV(max_num_ref_frames); > + PRINT_BIT(gaps_in_frame_num_value_allowed_flag); > + PRINT_UEV(pic_width_in_mbs_minus1); > + PRINT_UEV(pic_height_in_map_units_minus1); > + > + PRINT_BIT(frame_mbs_only_flag); > + if (!sps->frame_mbs_only_flag) > + PRINT_BIT(mb_adaptive_frame_field_flag); > + > + PRINT_BIT(direct_8x8_inference_flag); > + > + PRINT_BIT(frame_cropping_flag); > + if (sps->frame_cropping_flag) { > + PRINT_UEV(crop_left); > + PRINT_UEV(crop_right); > + PRINT_UEV(crop_top); > + PRINT_UEV(crop_bottom); > + } > + > + PRINT_BIT(vui_parameters_present_flag); > + if (sps->vui_parameters_present_flag) > + nal_h264_print_vui_parameters(dev, &sps->vui); > +} > +EXPORT_SYMBOL_GPL(nal_h264_print_sps); > + > +ssize_t nal_h264_write_pps(const struct device *dev, > + void *dest, size_t n, struct nal_h264_pps *pps) > +{ > + struct rbsp rbsp; > + int err; > + u8 *p = dest; > + > + rbsp.buf = p + 5; > + rbsp.size = n - 5; > + rbsp.pos = 0; > + > + err = nal_h264_rbsp_write_pps(dev, &rbsp, pps); > + if (err) > + return err; > + > + err = nal_h264_write_trailing_bits(dev, &rbsp); > + if (err) > + return err; > + > + p[0] = 0x00; > + p[1] = 0x00; > + p[2] = 0x00; > + p[3] = 0x01; > + p[4] = 0x08; > + > + return ((rbsp.pos + 7) / 8) + 5; > +} > +EXPORT_SYMBOL_GPL(nal_h264_write_pps); > + > +ssize_t nal_h264_read_pps(const struct device *dev, > + struct nal_h264_pps *pps, void *src, size_t n) > +{ > + struct rbsp rbsp; > + int err; > + > + rbsp.buf = src; > + rbsp.size = n; > + rbsp.pos = 0; > + > + rbsp.buf += 5; > + rbsp.size -= 5; > + > + err = nal_h264_rbsp_read_pps(dev, &rbsp, pps); > + if (err) > + return err; > + > + return ((rbsp.pos + 7) / 8) + 5; > +} > +EXPORT_SYMBOL_GPL(nal_h264_read_pps); > + > +void nal_h264_print_pps(const struct device *dev, struct nal_h264_pps *pps) > +{ > + struct nal_h264_pps *s = pps; > + unsigned int i; > + > + if (!pps) > + return; > + > + PRINT_UEV(pic_parameter_set_id); > + PRINT_UEV(seq_parameter_set_id); > + PRINT_BIT(entropy_coding_mode_flag); > + PRINT_BIT(bottom_field_pic_order_in_frame_present_flag); > + PRINT_UEV(num_slice_groups_minus1); > + if (s->num_slice_groups_minus1 > 0) { > + PRINT_UEV(slice_group_map_type); > + if (pps->slice_group_map_type == 0) { > + for (i = 0; i < pps->num_slice_groups_minus1; i++) > + PRINT_UEV(run_length_minus1[i]); > + } else if (pps->slice_group_map_type == 2) { > + for (i = 0; i < pps->num_slice_groups_minus1; i++) { > + PRINT_UEV(top_left[i]); > + PRINT_UEV(bottom_right[i]); > + } > + } else if (s->slice_group_map_type == 3 || > + s->slice_group_map_type == 4 || > + s->slice_group_map_type == 5) { > + PRINT_BIT(slice_group_change_direction_flag); > + PRINT_UEV(slice_group_change_rate_minus1); > + } else if (s->slice_group_map_type == 6) { > + PRINT_UEV(pic_size_in_map_units_minus1); > + for (i = 0; i < s->pic_size_in_map_units_minus1; i++) > + PRINT_BITS(order_base_2 > + (s->num_slice_groups_minus1 + 1), > + slice_group_id[i]); > + } > + } > + PRINT_UEV(num_ref_idx_l0_default_active_minus1); > + PRINT_UEV(num_ref_idx_l1_default_active_minus1); > + PRINT_BIT(weighted_pred_flag); > + PRINT_BITS(2, weighted_bipred_idc); > + PRINT_SEV(pic_init_qp_minus26); > + PRINT_SEV(pic_init_qs_minus26); > + PRINT_SEV(chroma_qp_index_offset); > + PRINT_BIT(deblocking_filter_control_present_flag); > + PRINT_BIT(constrained_intra_pred_flag); > + PRINT_BIT(redundant_pic_cnt_present_flag); > + if (/* more_rbsp_data() */ false) { > + PRINT_BIT(transform_8x8_mode_flag); > + PRINT_BIT(pic_scaling_matrix_present_flag); > + if (pps->pic_scaling_matrix_present_flag) { > + dev_err(dev, > + "%s: Handling scaling matrix not supported\n", > + __func__); > + } > + PRINT_SEV(second_chroma_qp_index_offset); > + } > +} > +EXPORT_SYMBOL_GPL(nal_h264_print_pps); > + > +ssize_t nal_h264_read_filler(const struct device *dev, void *src, size_t n) > +{ > + char *p = src; > + size_t i = 5; > + > + if (p[0] != 0x00 || p[1] != 0x00 || p[2] != 0x00 || p[3] != 0x01) > + return -EINVAL; > + > + if (p[4] != 0x0c) > + return -EINVAL; > + > + while (p[i] == 0xff && i < n) > + i++; > + > + if (p[i] != 0x80) > + return -EINVAL; > + > + return i; > +} > +EXPORT_SYMBOL_GPL(nal_h264_read_filler); > + > +ssize_t nal_h264_write_filler(const struct device *dev, void *dest, size_t n) > +{ > + char *p = dest; > + > + if (n < 6) > + return -EINVAL; > + > + p[0] = 0x00; > + p[1] = 0x00; > + p[2] = 0x00; > + p[3] = 0x01; > + p[4] = 0x0c; > + memset(p + 5, 0xff, n - 6); > + p[n - 1] = 0x80; > + > + return n; > +} > +EXPORT_SYMBOL_GPL(nal_h264_write_filler); > diff --git a/drivers/staging/media/allegro-dvt/nal-h264.h b/drivers/staging/media/allegro-dvt/nal-h264.h > new file mode 100644 > index 000000000000..bd46e8e42caf > --- /dev/null > +++ b/drivers/staging/media/allegro-dvt/nal-h264.h > @@ -0,0 +1,188 @@ > +/* SPDX-License-Identifier: GPL-2.0 */ > +/* > + * Copyright (C) 2019 Pengutronix, Michael Tretter <kernel@pengutronix.de> > + * > + * Convert NAL units between raw byte sequence payloads (RBSP) and C structs. > + */ > + > +#ifndef __NAL_H264_H__ > +#define __NAL_H264_H__ > + > +#include <linux/kernel.h> > +#include <linux/types.h> > + > +/* Rec. ITU-T H.264 (04/2017) E.1.2 */ > +struct nal_h264_hrd_parameters { > + unsigned int cpb_cnt_minus1; > + unsigned int bit_rate_scale:4; > + unsigned int cpb_size_scale:4; > + struct { > + int bit_rate_value_minus1[16]; > + int cpb_size_value_minus1[16]; > + unsigned int cbr_flag[16]; /* FIXME cbr_flag:1 */ > + }; > + unsigned int initial_cpb_removal_delay_length_minus1:5; > + unsigned int cpb_removal_delay_length_minus1:5; > + unsigned int dpb_output_delay_length_minus1:5; > + unsigned int time_offset_length:5; > +}; > + > +/* Rec. ITU-T H.264 (04/2017) E.1.1 */ > +struct nal_h264_vui_parameters { > + unsigned int aspect_ratio_info_present_flag:1; > + struct { > + unsigned int aspect_ratio_idc:8; > + unsigned int sar_width:16; > + unsigned int sar_height:16; > + }; > + unsigned int overscan_info_present_flag:1; > + unsigned int overscan_appropriate_flag:1; > + unsigned int video_signal_type_present_flag:1; > + struct { > + unsigned int video_format:3; > + unsigned int video_full_range_flag:1; > + unsigned int colour_description_present_flag:1; > + struct { > + unsigned int colour_primaries:8; > + unsigned int transfer_characteristics:8; > + unsigned int matrix_coefficients:8; > + }; > + }; > + unsigned int chroma_loc_info_present_flag:1; > + struct { > + unsigned int chroma_sample_loc_type_top_field; > + unsigned int chroma_sample_loc_type_bottom_field; > + }; > + unsigned int timing_info_present_flag:1; > + struct { > + unsigned int num_units_in_tick:32; > + unsigned int time_scale:32; > + unsigned int fixed_frame_rate_flag:1; > + }; > + unsigned int nal_hrd_parameters_present_flag:1; > + struct nal_h264_hrd_parameters nal_hrd_parameters; > + unsigned int vcl_hrd_parameters_present_flag:1; > + struct nal_h264_hrd_parameters vcl_hrd_parameters; > + unsigned int low_delay_hrd_flag:1; > + unsigned int pic_struct_present_flag:1; > + unsigned int bitstream_restriction_flag:1; > + struct { > + unsigned int motion_vectors_over_pic_boundaries_flag:1; > + unsigned int max_bytes_per_pic_denom; > + unsigned int max_bits_per_mb_denom; > + unsigned int log2_max_mv_length_horizontal; > + unsigned int log21_max_mv_length_vertical; > + unsigned int max_num_reorder_frames; > + unsigned int max_dec_frame_buffering; > + }; > +}; > + > +/* Rec. ITU-T H.264 (04/2017) 7.3.2.1.1 Sequence parameter set data syntax */ > +struct nal_h264_sps { > + unsigned int profile_idc:8; > + unsigned int constraint_set0_flag:1; > + unsigned int constraint_set1_flag:1; > + unsigned int constraint_set2_flag:1; > + unsigned int constraint_set3_flag:1; > + unsigned int constraint_set4_flag:1; > + unsigned int constraint_set5_flag:1; > + unsigned int reserved_zero_2bits:2; > + unsigned int level_idc:8; > + unsigned int seq_parameter_set_id; > + struct { > + unsigned int chroma_format_idc; > + unsigned int separate_colour_plane_flag:1; > + unsigned int bit_depth_luma_minus8; > + unsigned int bit_depth_chroma_minus8; > + unsigned int qpprime_y_zero_transform_bypass_flag:1; > + unsigned int seq_scaling_matrix_present_flag:1; > + }; > + unsigned int log2_max_frame_num_minus4; > + unsigned int pic_order_cnt_type; > + union { > + unsigned int log2_max_pic_order_cnt_lsb_minus4; > + struct { > + unsigned int delta_pic_order_always_zero_flag:1; > + int offset_for_non_ref_pic; > + int offset_for_top_to_bottom_field; > + unsigned int num_ref_frames_in_pic_order_cnt_cycle; > + int offset_for_ref_frame[255]; > + }; > + }; > + unsigned int max_num_ref_frames; > + unsigned int gaps_in_frame_num_value_allowed_flag:1; > + unsigned int pic_width_in_mbs_minus1; > + unsigned int pic_height_in_map_units_minus1; > + unsigned int frame_mbs_only_flag:1; > + unsigned int mb_adaptive_frame_field_flag:1; > + unsigned int direct_8x8_inference_flag:1; > + unsigned int frame_cropping_flag:1; > + struct { > + unsigned int crop_left; > + unsigned int crop_right; > + unsigned int crop_top; > + unsigned int crop_bottom; > + }; > + unsigned int vui_parameters_present_flag:1; > + struct nal_h264_vui_parameters vui; > +}; > + > +/* Rec. ITU-T H.264 (04/2017) 7.3.2.2 Picture parameter set RBSP syntax */ > +struct nal_h264_pps { > + unsigned int pic_parameter_set_id; > + unsigned int seq_parameter_set_id; > + unsigned int entropy_coding_mode_flag:1; > + unsigned int bottom_field_pic_order_in_frame_present_flag:1; > + unsigned int num_slice_groups_minus1; > + unsigned int slice_group_map_type; > + union { > + unsigned int run_length_minus1[8]; > + struct { > + unsigned int top_left[8]; > + unsigned int bottom_right[8]; > + }; > + struct { > + unsigned int slice_group_change_direction_flag:1; > + unsigned int slice_group_change_rate_minus1; > + }; > + struct { > + unsigned int pic_size_in_map_units_minus1; > + unsigned int slice_group_id[8]; > + }; > + }; > + unsigned int num_ref_idx_l0_default_active_minus1; > + unsigned int num_ref_idx_l1_default_active_minus1; > + unsigned int weighted_pred_flag:1; > + unsigned int weighted_bipred_idc:2; > + int pic_init_qp_minus26; > + int pic_init_qs_minus26; > + int chroma_qp_index_offset; > + unsigned int deblocking_filter_control_present_flag:1; > + unsigned int constrained_intra_pred_flag:1; > + unsigned int redundant_pic_cnt_present_flag:1; > + struct { > + unsigned int transform_8x8_mode_flag:1; > + unsigned int pic_scaling_matrix_present_flag:1; > + int second_chroma_qp_index_offset; > + }; > +}; > + > +int nal_h264_level_from_v4l2(enum v4l2_mpeg_video_h264_level level); > +int nal_h264_profile_from_v4l2(enum v4l2_mpeg_video_h264_profile profile); > + > +ssize_t nal_h264_write_sps(const struct device *dev, > + void *dest, size_t n, struct nal_h264_sps *sps); > +ssize_t nal_h264_read_sps(const struct device *dev, > + struct nal_h264_sps *sps, void *src, size_t n); > +void nal_h264_print_sps(const struct device *dev, struct nal_h264_sps *sps); > + > +ssize_t nal_h264_write_pps(const struct device *dev, > + void *dest, size_t n, struct nal_h264_pps *pps); > +ssize_t nal_h264_read_pps(const struct device *dev, > + struct nal_h264_pps *pps, void *src, size_t n); > +void nal_h264_print_pps(const struct device *dev, struct nal_h264_pps *pps); > + > +ssize_t nal_h264_write_filler(const struct device *dev, void *dest, size_t n); > +ssize_t nal_h264_read_filler(const struct device *dev, void *src, size_t n); Can you document these functions? I think there is a good chance that these functions can be reused in other drivers in the future, so having some documentation here would be very useful. Regards, Hans > + > +#endif /* __NAL_H264_H__ */ >