+struct adv7511_cfg_read_infoframe {
+ const char *desc;
+ u8 present_reg;
+ u8 present_mask;
+ u8 header[3];
+ u16 payload_addr;
+};
+
+static u8 hdmi_infoframe_checksum(u8 *ptr, size_t size)
+{
+ u8 csum = 0;
+ size_t i;
+
+ /* compute checksum */
+ for (i = 0; i < size; i++)
+ csum += ptr[i];
+
+ return 256 - csum;
+}
+
+static void log_infoframe(struct v4l2_subdev *sd, const struct adv7511_cfg_read_infoframe *cri)
+{
+ struct i2c_client *client = v4l2_get_subdevdata(sd);
+ struct device *dev = &client->dev;
+ union hdmi_infoframe frame;
+ u8 buffer[32];
+ u8 len;
+ int i;
+
+ if (!(adv7511_rd(sd, cri->present_reg) & cri->present_mask)) {
+ v4l2_info(sd, "%s infoframe not transmitted\n", cri->desc);
+ return;
+ }
+
+ memcpy(buffer, cri->header, sizeof(cri->header));
+
+ len = buffer[2];
+
+ if (len + 4 > sizeof(buffer)) {
+ v4l2_err(sd, "%s: invalid %s infoframe length %d\n", __func__, cri->desc, len);
+ return;
+ }
+
+ if (cri->payload_addr >= 0x100) {
+ for (i = 0; i < len; i++)
+ buffer[i + 4] = adv7511_pktmem_rd(sd, cri->payload_addr + i - 0x100);
+ } else {
+ for (i = 0; i < len; i++)
+ buffer[i + 4] = adv7511_rd(sd, cri->payload_addr + i);
+ }
+ buffer[3] = 0;
+ buffer[3] = hdmi_infoframe_checksum(buffer, len + 4);
+
+ if (hdmi_infoframe_unpack(&frame, buffer) < 0) {
+ v4l2_err(sd, "%s: unpack of %s infoframe failed\n", __func__, cri->desc);
+ return;
+ }
+
+ hdmi_infoframe_log(KERN_INFO, dev, &frame);
+}
+
+static void adv7511_log_infoframes(struct v4l2_subdev *sd)
+{
+ static const struct adv7511_cfg_read_infoframe cri[] = {
+ { "AVI", 0x44, 0x10, { 0x82, 2, 13 }, 0x55 },
+ { "Audio", 0x44, 0x08, { 0x84, 1, 10 }, 0x73 },
+ { "SDP", 0x40, 0x40, { 0x83, 1, 25 }, 0x103 },
+ };
+ int i;
+
+ for (i = 0; i < ARRAY_SIZE(cri); i++)
+ log_infoframe(sd, &cri[i]);
+}
+