+static void panel_simple_dsi_cmds_cleanup(struct panel_simple *p)
+{
+ if (p->on_cmds) {
+ kfree(p->on_cmds->buf);
+ kfree(p->on_cmds->cmds);
+ }
+
+ if (p->off_cmds) {
+ kfree(p->off_cmds->buf);
+ kfree(p->off_cmds->cmds);
+ }
+}
+
+static int panel_simple_dsi_parse_dcs_cmds(struct device *dev,
+ const u8 *data, int blen,
+ struct dsi_panel_cmds *pcmds)
+{
+ int len;
+ char *buf, *bp;
+ struct dsi_ctrl_hdr *dchdr;
+ int i, cnt;
+
+ if (!pcmds)
+ return -EINVAL;
+
+ buf = kmemdup(data, blen, GFP_KERNEL);
+ if (!buf)
+ return -ENOMEM;
+
+ /* scan dcs commands */
+ bp = buf;
+ len = blen;
+ cnt = 0;
+ while (len > sizeof(*dchdr)) {
+ dchdr = (struct dsi_ctrl_hdr *)bp;
+ dchdr->dlen = ntohs(dchdr->dlen);
+
+ if (dchdr->dlen > len) {
+ dev_err(dev, "%s: error, len=%d", __func__,
+ dchdr->dlen);
+ return -EINVAL;
+ }
+
+ bp += sizeof(*dchdr);
+ len -= sizeof(*dchdr);
+ bp += dchdr->dlen;
+ len -= dchdr->dlen;
+ cnt++;
+ }
+
+ if (len != 0) {
+ dev_err(dev, "%s: dcs_cmd=%x len=%d error!",
+ __func__, buf[0], blen);
+ kfree(buf);
+ return -EINVAL;
+ }
+
+ pcmds->cmds = kcalloc(cnt, sizeof(struct dsi_cmd_desc), GFP_KERNEL);
+ if (!pcmds->cmds) {
+ kfree(buf);
+ return -ENOMEM;
+ }
+
+ pcmds->cmd_cnt = cnt;
+ pcmds->buf = buf;
+ pcmds->blen = blen;
+
+ bp = buf;
+ len = blen;
+ for (i = 0; i < cnt; i++) {
+ dchdr = (struct dsi_ctrl_hdr *)bp;
+ len -= sizeof(*dchdr);
+ bp += sizeof(*dchdr);
+ pcmds->cmds[i].dchdr = *dchdr;
+ pcmds->cmds[i].payload = bp;
+ bp += dchdr->dlen;
+ len -= dchdr->dlen;
+ }
+
+ dev_info(dev, "%s: dcs_cmd=%x len=%d, cmd_cnt=%d\n", __func__,
+ pcmds->buf[0], pcmds->blen, pcmds->cmd_cnt);
+ return 0;
+}
+
+static int panel_simple_dsi_send_cmds(struct panel_simple *panel,
+ struct dsi_panel_cmds *cmds)
+{
+ struct mipi_dsi_device *dsi = panel->dsi;
+ int i, err;
+
+ if (!cmds)
+ return -EINVAL;
+
+ for (i = 0; i < cmds->cmd_cnt; i++) {
+ struct dsi_cmd_desc *cmd = &cmds->cmds[i];
+
+ switch (cmd->dchdr.dtype) {
+ case MIPI_DSI_GENERIC_SHORT_WRITE_0_PARAM:
+ case MIPI_DSI_GENERIC_SHORT_WRITE_1_PARAM:
+ case MIPI_DSI_GENERIC_SHORT_WRITE_2_PARAM:
+ case MIPI_DSI_GENERIC_LONG_WRITE:
+ err = mipi_dsi_generic_write(dsi, cmd->payload,
+ cmd->dchdr.dlen);
+ break;
+ case MIPI_DSI_DCS_SHORT_WRITE:
+ case MIPI_DSI_DCS_SHORT_WRITE_PARAM:
+ case MIPI_DSI_DCS_LONG_WRITE:
+ err = mipi_dsi_dcs_write_buffer(dsi, cmd->payload,
+ cmd->dchdr.dlen);
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ if (err)
+ dev_err(panel->dev, "failed to write dcs cmd: %d\n",
+ err);
+
+ if (cmd->dchdr.wait)
+ msleep(cmd->dchdr.wait);
+ }
+
+ return 0;
+}
+