1 files changed, 42 insertions, 20 deletions

diff --git a/drivers/gpu/drm/i915/intel_sdvo.c b/drivers/gpu/drm/i915/intel_sdvo.c
index c01d97db0061..79d308da29ff 100644
--- a/drivers/gpu/drm/i915/intel_sdvo.c
+++ b/drivers/gpu/drm/i915/intel_sdvo.c
@@ -894,6 +894,45 @@ static void intel_sdvo_dump_hdmi_buf(struct intel_sdvo *intel_sdvo)
 }
 #endif
 
+static bool intel_sdvo_write_infoframe(struct intel_sdvo *intel_sdvo,
+                                       unsigned if_index, uint8_t tx_rate,
+                                       uint8_t *data, unsigned length)
+{
+        uint8_t set_buf_index[2] = { if_index, 0 };
+        uint8_t hbuf_size, tmp[8];
+        int i;
+
+        if (!intel_sdvo_set_value(intel_sdvo,
+                                  SDVO_CMD_SET_HBUF_INDEX,
+                                  set_buf_index, 2))
+                return false;
+
+        if (!intel_sdvo_get_value(intel_sdvo, SDVO_CMD_GET_HBUF_INFO,
+                                  &hbuf_size, 1))
+                return false;
+
+        /* Buffer size is 0 based, hooray! */
+        hbuf_size++;
+
+        DRM_DEBUG_KMS("writing sdvo hbuf: %i, hbuf_size %i, hbuf_size: %i\n",
+                      if_index, length, hbuf_size);
+
+        for (i = 0; i < hbuf_size; i += 8) {
+                memset(tmp, 0, 8);
+                if (i < length)
+                        memcpy(tmp, data + i, min_t(unsigned, 8, length - i));
+
+                if (!intel_sdvo_set_value(intel_sdvo,
+                                          SDVO_CMD_SET_HBUF_DATA,
+                                          tmp, 8))
+                        return false;
+        }
+
+        return intel_sdvo_set_value(intel_sdvo,
+                                    SDVO_CMD_SET_HBUF_TXRATE,
+                                    &tx_rate, 1);
+}
+
 static bool intel_sdvo_set_avi_infoframe(struct intel_sdvo *intel_sdvo)
 {
         struct dip_infoframe avi_if = {
@@ -901,11 +940,7 @@ static bool intel_sdvo_set_avi_infoframe(struct intel_sdvo *intel_sdvo)
                 .ver = DIP_VERSION_AVI,
                 .len = DIP_LEN_AVI,
         };
-        uint8_t tx_rate = SDVO_HBUF_TX_VSYNC;
-        uint8_t set_buf_index[2] = { 1, 0 };
         uint8_t sdvo_data[4 + sizeof(avi_if.body.avi)];
-        uint64_t *data = (uint64_t *)sdvo_data;
-        unsigned i;
 
         intel_dip_infoframe_csum(&avi_if);
 
@@ -915,22 +950,9 @@ static bool intel_sdvo_set_avi_infoframe(struct intel_sdvo *intel_sdvo)
         sdvo_data[3] = avi_if.checksum;
         memcpy(&sdvo_data[4], &avi_if.body, sizeof(avi_if.body.avi));
 
-        if (!intel_sdvo_set_value(intel_sdvo,
-                                  SDVO_CMD_SET_HBUF_INDEX,
-                                  set_buf_index, 2))
-                return false;
-
-        for (i = 0; i < sizeof(sdvo_data); i += 8) {
-                if (!intel_sdvo_set_value(intel_sdvo,
-                                          SDVO_CMD_SET_HBUF_DATA,
-                                          data, 8))
-                        return false;
-                data++;
-        }
-
-        return intel_sdvo_set_value(intel_sdvo,
-                                    SDVO_CMD_SET_HBUF_TXRATE,
-                                    &tx_rate, 1);
+        return intel_sdvo_write_infoframe(intel_sdvo, SDVO_HBUF_INDEX_AVI_IF,
+                                          SDVO_HBUF_TX_VSYNC,
+                                          sdvo_data, sizeof(sdvo_data));
 }
 
 static bool intel_sdvo_set_tv_format(struct intel_sdvo *intel_sdvo)