soc: rpm-smd: Support new format of rpm messaging

Support new format of rpm messaging to reduce response time of rpm
acking and increasing the number of messages that the rpm fifo can
process.

RPM message
====================

Old format:
uint32_t service_type;
uint32_t request_len;
uint32_t msg_id;
enum msm_rpm_set set;
uint32_t resource_type;
uint32_t resource_id;
uint32_t data_len;

New format:
unsigned request_hdr;
unsigned msg_id;
unsigned resource_type;
unsigned request_details;

RPM ACK message
====================

Old format:
uint32_t req;
uint32_t req_len;
uint32_t rsc_id;
uint32_t msg_len;
uint32_t id_ack;

New format:
unsigned request_hdr;
uint32_t id_ack;

CRs-fixed: 993338
Change-Id: I85cb0223ce45de2712c20a3a0edffe2a84b80f01
Signed-off-by: Archana Sathyakumar <asathyak@codeaurora.org>

2 files changed, 413 insertions, 110 deletions

diff --git a/Documentation/devicetree/bindings/arm/msm/rpm-smd.txt b/Documentation/devicetree/bindings/arm/msm/rpm-smd.txt
index 128bd0f799d2..4cba3ecaeb90 100644
--- a/Documentation/devicetree/bindings/arm/msm/rpm-smd.txt
+++ b/Documentation/devicetree/bindings/arm/msm/rpm-smd.txt
@@ -26,11 +26,14 @@ Required properties
 Optional properties
 - rpm-standalone: Allow RPM driver to run in standalone mode irrespective of RPM
 			channel presence.
+- reg: Contains the memory address at which rpm messaging format version is
+  stored. If this field is not present, the target only supports v0 format.
 
 Example:
 
-	qcom,rpm-smd {
+	qcom,rpm-smd@68150 {
 		compatible = "qcom,rpm-smd", "qcom,rpm-glink";
+		reg = <0x68150 0x3200>;
 		qcom,rpm-channel-name = "rpm_requests";
 		qcom,rpm-channel-type = 15; /* SMD_APPS_RPM */
 		qcom,glink-edge = "rpm";
diff --git a/drivers/soc/qcom/rpm-smd.c b/drivers/soc/qcom/rpm-smd.c
index b1fa57da7241..fdff6f2140a3 100644
--- a/drivers/soc/qcom/rpm-smd.c
+++ b/drivers/soc/qcom/rpm-smd.c
@@ -25,6 +25,7 @@
 #include <linux/irq.h>
 #include <linux/list.h>
 #include <linux/mutex.h>
+#include <linux/of_address.h>
 #include <linux/spinlock.h>
 #include <linux/string.h>
 #include <linux/device.h>
@@ -88,6 +89,28 @@ static struct glink_apps_rpm_data *glink_data;
 #define MAX_ERR_BUFFER_SIZE 128
 #define MAX_WAIT_ON_ACK 24
 #define INIT_ERROR 1
+#define V1_PROTOCOL_VERSION 0x31726576 /* rev1 */
+#define V0_PROTOCOL_VERSION 0 /* rev0 */
+#define RPM_MSG_TYPE_OFFSET 16
+#define RPM_MSG_TYPE_SIZE 8
+#define RPM_SET_TYPE_OFFSET 28
+#define RPM_SET_TYPE_SIZE 4
+#define RPM_REQ_LEN_OFFSET 0
+#define RPM_REQ_LEN_SIZE 16
+#define RPM_MSG_VERSION_OFFSET 24
+#define RPM_MSG_VERSION_SIZE 8
+#define RPM_MSG_VERSION 1
+#define RPM_MSG_SET_OFFSET 28
+#define RPM_MSG_SET_SIZE 4
+#define RPM_RSC_ID_OFFSET 16
+#define RPM_RSC_ID_SIZE 12
+#define RPM_DATA_LEN_OFFSET 0
+#define RPM_DATA_LEN_SIZE 16
+#define RPM_HDR_SIZE ((rpm_msg_fmt_ver == RPM_MSG_V0_FMT) ?\
+		sizeof(struct rpm_v0_hdr) : sizeof(struct rpm_v1_hdr))
+#define GET_FIELD(offset, size) (((1U << (offset + size)) - 1) - \
+		((1U << offset) - 1))
+#define CLEAR_FIELD(offset, size) (~GET_FIELD(offset, size))
 
 static ATOMIC_NOTIFIER_HEAD(msm_rpm_sleep_notifier);
 static bool standalone;
@@ -111,17 +134,29 @@ enum {
 	MSM_RPM_MSG_TYPE_NR,
 };
 
-static const uint32_t msm_rpm_request_service[MSM_RPM_MSG_TYPE_NR] = {
+static const uint32_t msm_rpm_request_service_v1[MSM_RPM_MSG_TYPE_NR] = {
 	0x716572, /* 'req\0' */
 };
 
-/*the order of fields matter and reflect the order expected by the RPM*/
-struct rpm_request_header {
+enum {
+	RPM_V1_REQUEST_SERVICE,
+	RPM_V1_SYSTEMDB_SERVICE,
+	RPM_V1_COMMAND_SERVICE,
+	RPM_V1_ACK_SERVICE,
+	RPM_V1_NACK_SERVICE,
+} msm_rpm_request_service_v2;
+
+struct rpm_v0_hdr {
 	uint32_t service_type;
 	uint32_t request_len;
 };
 
-struct rpm_message_header {
+struct rpm_v1_hdr {
+	uint32_t request_hdr;
+};
+
+struct rpm_message_header_v0 {
+	struct rpm_v0_hdr hdr;
 	uint32_t msg_id;
 	enum msm_rpm_set set;
 	uint32_t resource_type;
@@ -129,6 +164,26 @@ struct rpm_message_header {
 	uint32_t data_len;
 };
 
+struct rpm_message_header_v1 {
+	struct rpm_v1_hdr hdr;
+	uint32_t msg_id;
+	uint32_t resource_type;
+	uint32_t request_details;
+};
+
+struct msm_rpm_ack_msg_v0 {
+	uint32_t req;
+	uint32_t req_len;
+	uint32_t rsc_id;
+	uint32_t msg_len;
+	uint32_t id_ack;
+};
+
+struct msm_rpm_ack_msg_v1 {
+	uint32_t request_hdr;
+	uint32_t id_ack;
+};
+
 struct kvp {
 	unsigned int k;
 	unsigned int s;
@@ -147,49 +202,252 @@ struct slp_buf {
 	char *buf;
 	bool valid;
 };
+
+enum rpm_msg_fmts {
+	RPM_MSG_V0_FMT,
+	RPM_MSG_V1_FMT
+};
+
+static uint32_t rpm_msg_fmt_ver;
+module_param_named(
+	rpm_msg_fmt_ver, rpm_msg_fmt_ver, uint, S_IRUGO
+);
+
 static struct rb_root tr_root = RB_ROOT;
 static int (*msm_rpm_send_buffer)(char *buf, uint32_t size, bool noirq);
 static int msm_rpm_send_smd_buffer(char *buf, uint32_t size, bool noirq);
 static int msm_rpm_glink_send_buffer(char *buf, uint32_t size, bool noirq);
 static uint32_t msm_rpm_get_next_msg_id(void);
 
-static inline unsigned int get_rsc_type(char *buf)
+static inline uint32_t get_offset_value(uint32_t val, uint32_t offset,
+		uint32_t size)
 {
-	struct rpm_message_header *h;
-	h = (struct rpm_message_header *)
-		(buf + sizeof(struct rpm_request_header));
-	return h->resource_type;
+	return (((val) & GET_FIELD(offset, size))
+		>> offset);
 }
 
-static inline unsigned int get_rsc_id(char *buf)
+static inline void change_offset_value(uint32_t *val, uint32_t offset,
+		uint32_t size, int32_t val1)
 {
-	struct rpm_message_header *h;
-	h = (struct rpm_message_header *)
-		(buf + sizeof(struct rpm_request_header));
-	return h->resource_id;
+	uint32_t member = *val;
+	uint32_t offset_val = get_offset_value(member, offset, size);
+	uint32_t mask = (1 << size) - 1;
+
+	offset_val += val1;
+	*val &= CLEAR_FIELD(offset, size);
+	*val |= ((offset_val & mask) << offset);
 }
 
-#define get_data_len(buf) \
-	(((struct rpm_message_header *) \
-	  (buf + sizeof(struct rpm_request_header)))->data_len)
+static inline void set_offset_value(uint32_t *val, uint32_t offset,
+		uint32_t size, uint32_t val1)
+{
+	uint32_t mask = (1 << size) - 1;
 
-#define get_req_len(buf) \
-	(((struct rpm_request_header *)(buf))->request_len)
+	*val &= CLEAR_FIELD(offset, size);
+	*val |= ((val1 & mask) << offset);
+}
+static uint32_t get_msg_id(char *buf)
+{
+	if (rpm_msg_fmt_ver == RPM_MSG_V0_FMT)
+		return ((struct rpm_message_header_v0 *)buf)->msg_id;
 
-#define get_msg_id(buf) \
-	(((struct rpm_message_header *) \
-	  (buf + sizeof(struct rpm_request_header)))->msg_id)
+	return ((struct rpm_message_header_v1 *)buf)->msg_id;
 
+}
+
+static uint32_t get_ack_msg_id(char *buf)
+{
+	if (rpm_msg_fmt_ver == RPM_MSG_V0_FMT)
+		return ((struct msm_rpm_ack_msg_v0 *)buf)->id_ack;
+
+	return ((struct msm_rpm_ack_msg_v1 *)buf)->id_ack;
+
+}
+
+static uint32_t get_rsc_type(char *buf)
+{
+	if (rpm_msg_fmt_ver == RPM_MSG_V0_FMT)
+		return ((struct rpm_message_header_v0 *)buf)->resource_type;
+
+	return ((struct rpm_message_header_v1 *)buf)->resource_type;
+
+}
+
+static uint32_t get_set_type(char *buf)
+{
+	if (rpm_msg_fmt_ver == RPM_MSG_V0_FMT)
+		return ((struct rpm_message_header_v0 *)buf)->set;
+
+	return get_offset_value(((struct rpm_message_header_v1 *)buf)->
+			request_details, RPM_SET_TYPE_OFFSET,
+			RPM_SET_TYPE_SIZE);
+}
+
+static uint32_t get_data_len(char *buf)
+{
+	if (rpm_msg_fmt_ver == RPM_MSG_V0_FMT)
+		return ((struct rpm_message_header_v0 *)buf)->data_len;
+
+	return get_offset_value(((struct rpm_message_header_v1 *)buf)->
+			request_details, RPM_DATA_LEN_OFFSET,
+			RPM_DATA_LEN_SIZE);
+}
+
+static uint32_t get_rsc_id(char *buf)
+{
+	if (rpm_msg_fmt_ver == RPM_MSG_V0_FMT)
+		return ((struct rpm_message_header_v0 *)buf)->resource_id;
+
+	return get_offset_value(((struct rpm_message_header_v1 *)buf)->
+			request_details, RPM_RSC_ID_OFFSET,
+			RPM_RSC_ID_SIZE);
+}
+
+static uint32_t get_ack_req_len(char *buf)
+{
+	if (rpm_msg_fmt_ver == RPM_MSG_V0_FMT)
+		return ((struct msm_rpm_ack_msg_v0 *)buf)->req_len;
+
+	return get_offset_value(((struct msm_rpm_ack_msg_v1 *)buf)->
+			request_hdr, RPM_REQ_LEN_OFFSET,
+			RPM_REQ_LEN_SIZE);
+}
+
+static uint32_t get_ack_msg_type(char *buf)
+{
+	if (rpm_msg_fmt_ver == RPM_MSG_V0_FMT)
+		return ((struct msm_rpm_ack_msg_v0 *)buf)->req;
+
+	return get_offset_value(((struct msm_rpm_ack_msg_v1 *)buf)->
+			request_hdr, RPM_MSG_TYPE_OFFSET,
+			RPM_MSG_TYPE_SIZE);
+}
+
+static uint32_t get_req_len(char *buf)
+{
+	if (rpm_msg_fmt_ver == RPM_MSG_V0_FMT)
+		return ((struct rpm_message_header_v0 *)buf)->hdr.request_len;
+
+	return get_offset_value(((struct rpm_message_header_v1 *)buf)->
+			hdr.request_hdr, RPM_REQ_LEN_OFFSET,
+			RPM_REQ_LEN_SIZE);
+}
+
+static void set_msg_ver(char *buf, uint32_t val)
+{
+	if (rpm_msg_fmt_ver) {
+		set_offset_value(&((struct rpm_message_header_v1 *)buf)->
+			hdr.request_hdr, RPM_MSG_VERSION_OFFSET,
+			RPM_MSG_VERSION_SIZE, val);
+	} else {
+		set_offset_value(&((struct rpm_message_header_v1 *)buf)->
+			hdr.request_hdr, RPM_MSG_VERSION_OFFSET,
+			RPM_MSG_VERSION_SIZE, 0);
+	}
+}
+
+static void set_req_len(char *buf, uint32_t val)
+{
+	if (rpm_msg_fmt_ver == RPM_MSG_V0_FMT) {
+		((struct rpm_message_header_v0 *)buf)->hdr.request_len = val;
+	} else {
+		set_offset_value(&((struct rpm_message_header_v1 *)buf)->
+			hdr.request_hdr, RPM_REQ_LEN_OFFSET,
+			RPM_REQ_LEN_SIZE, val);
+	}
+}
+
+static void change_req_len(char *buf, int32_t val)
+{
+	if (rpm_msg_fmt_ver == RPM_MSG_V0_FMT) {
+		((struct rpm_message_header_v0 *)buf)->hdr.request_len += val;
+	} else {
+		change_offset_value(&((struct rpm_message_header_v1 *)buf)->
+			hdr.request_hdr, RPM_REQ_LEN_OFFSET,
+			RPM_REQ_LEN_SIZE, val);
+	}
+}
+
+static void set_msg_type(char *buf, uint32_t val)
+{
+	if (rpm_msg_fmt_ver == RPM_MSG_V0_FMT) {
+		((struct rpm_message_header_v0 *)buf)->hdr.service_type =
+			msm_rpm_request_service_v1[val];
+	} else {
+		set_offset_value(&((struct rpm_message_header_v1 *)buf)->
+			hdr.request_hdr, RPM_MSG_TYPE_OFFSET,
+			RPM_MSG_TYPE_SIZE, RPM_V1_REQUEST_SERVICE);
+	}
+}
+
+static void set_rsc_id(char *buf, uint32_t val)
+{
+	if (rpm_msg_fmt_ver == RPM_MSG_V0_FMT)
+		((struct rpm_message_header_v0 *)buf)->resource_id = val;
+	else
+		set_offset_value(&((struct rpm_message_header_v1 *)buf)->
+			request_details, RPM_RSC_ID_OFFSET,
+			RPM_RSC_ID_SIZE, val);
+}
+
+static void set_data_len(char *buf, uint32_t val)
+{
+	if (rpm_msg_fmt_ver == RPM_MSG_V0_FMT)
+		((struct rpm_message_header_v0 *)buf)->data_len = val;
+	else
+		set_offset_value(&((struct rpm_message_header_v1 *)buf)->
+			request_details, RPM_DATA_LEN_OFFSET,
+			RPM_DATA_LEN_SIZE, val);
+}
+static void change_data_len(char *buf, int32_t val)
+{
+	if (rpm_msg_fmt_ver == RPM_MSG_V0_FMT)
+		((struct rpm_message_header_v0 *)buf)->data_len += val;
+	else
+		change_offset_value(&((struct rpm_message_header_v1 *)buf)->
+			request_details, RPM_DATA_LEN_OFFSET,
+			RPM_DATA_LEN_SIZE, val);
+}
+
+static void set_set_type(char *buf, uint32_t val)
+{
+	if (rpm_msg_fmt_ver == RPM_MSG_V0_FMT)
+		((struct rpm_message_header_v0 *)buf)->set = val;
+	else
+		set_offset_value(&((struct rpm_message_header_v1 *)buf)->
+			request_details, RPM_SET_TYPE_OFFSET,
+			RPM_SET_TYPE_SIZE, val);
+}
+static void set_msg_id(char *buf, uint32_t val)
+{
+	if (rpm_msg_fmt_ver == RPM_MSG_V0_FMT)
+		((struct rpm_message_header_v0 *)buf)->msg_id = val;
+	else
+		((struct rpm_message_header_v1 *)buf)->msg_id = val;
+
+}
+
+static void set_rsc_type(char *buf, uint32_t val)
+{
+	if (rpm_msg_fmt_ver == RPM_MSG_V0_FMT)
+		((struct rpm_message_header_v0 *)buf)->resource_type = val;
+	else
+		((struct rpm_message_header_v1 *)buf)->resource_type = val;
+}
 
 static inline int get_buf_len(char *buf)
 {
-	return get_req_len(buf) + sizeof(struct rpm_request_header);
+	return get_req_len(buf) + RPM_HDR_SIZE;
 }
 
 static inline struct kvp *get_first_kvp(char *buf)
 {
-	return (struct kvp *)(buf + sizeof(struct rpm_request_header)
-			+ sizeof(struct rpm_message_header));
+	if (rpm_msg_fmt_ver == RPM_MSG_V0_FMT)
+		return (struct kvp *)(buf +
+				sizeof(struct rpm_message_header_v0));
+	else
+		return (struct kvp *)(buf +
+				sizeof(struct rpm_message_header_v1));
 }
 
 static inline struct kvp *get_next_kvp(struct kvp *k)
@@ -203,7 +461,7 @@ static inline void *get_data(struct kvp *k)
 }
 
 
-static void delete_kvp(char *msg, struct kvp *d)
+static void delete_kvp(char *buf, struct kvp *d)
 {
 	struct kvp *n;
 	int dec;
@@ -211,12 +469,13 @@ static void delete_kvp(char *msg, struct kvp *d)
 
 	n = get_next_kvp(d);
 	dec = (void *)n - (void *)d;
-	size = get_data_len(msg) - ((void *)n - (void *)get_first_kvp(msg));
+	size = get_data_len(buf) -
+		((void *)n - (void *)get_first_kvp(buf));
 
 	memcpy((void *)d, (void *)n, size);
 
-	get_data_len(msg) -= dec;
-	get_req_len(msg) -= dec;
+	change_data_len(buf, -dec);
+	change_req_len(buf, -dec);
 }
 
 static inline void update_kvp_data(struct kvp *dest, struct kvp *src)
@@ -226,20 +485,23 @@ static inline void update_kvp_data(struct kvp *dest, struct kvp *src)
 
 static void add_kvp(char *buf, struct kvp *n)
 {
-	uint32_t inc = sizeof(*n) + n->s;
-	BUG_ON((get_req_len(buf) + inc) > MAX_SLEEP_BUFFER);
+	int32_t inc = sizeof(*n) + n->s;
+
+	if (get_req_len(buf) + inc > MAX_SLEEP_BUFFER) {
+		WARN_ON(get_req_len(buf) + inc > MAX_SLEEP_BUFFER);
+		return;
+	}
 
 	memcpy(buf + get_buf_len(buf), n, inc);
 
-	get_data_len(buf) += inc;
-	get_req_len(buf) += inc;
+	change_data_len(buf, inc);
+	change_req_len(buf, inc);
 }
 
 static struct slp_buf *tr_search(struct rb_root *root, char *slp)
 {
 	unsigned int type = get_rsc_type(slp);
 	unsigned int id = get_rsc_id(slp);
-
 	struct rb_node *node = root->rb_node;
 
 	while (node) {
@@ -265,7 +527,6 @@ static int tr_insert(struct rb_root *root, struct slp_buf *slp)
 {
 	unsigned int type = get_rsc_type(slp->buf);
 	unsigned int id = get_rsc_id(slp->buf);
-
 	struct rb_node **node = &(root->rb_node), *parent = NULL;
 
 	while (*node) {
@@ -295,7 +556,8 @@ static int tr_insert(struct rb_root *root, struct slp_buf *slp)
 
 #define for_each_kvp(buf, k) \
 	for (k = (struct kvp *)get_first_kvp(buf); \
-		((void *)k - (void *)get_first_kvp(buf)) < get_data_len(buf);\
+		((void *)k - (void *)get_first_kvp(buf)) < \
+		 get_data_len(buf);\
 		k = get_next_kvp(k))
 
 
@@ -333,8 +595,7 @@ static void tr_update(struct slp_buf *s, char *buf)
 static atomic_t msm_rpm_msg_id = ATOMIC_INIT(0);
 
 struct msm_rpm_request {
-	struct rpm_request_header req_hdr;
-	struct rpm_message_header msg_hdr;
+	uint8_t *client_buf;
 	struct msm_rpm_kvp_data *kvp;
 	uint32_t num_elements;
 	uint32_t write_idx;
@@ -357,13 +618,7 @@ struct msm_rpm_wait_data {
 };
 DEFINE_SPINLOCK(msm_rpm_list_lock);
 
-struct msm_rpm_ack_msg {
-	uint32_t req;
-	uint32_t req_len;
-	uint32_t rsc_id;
-	uint32_t msg_len;
-	uint32_t id_ack;
-};
+
 
 LIST_HEAD(msm_rpm_ack_list);
 
@@ -371,26 +626,37 @@ static struct tasklet_struct data_tasklet;
 
 static inline uint32_t msm_rpm_get_msg_id_from_ack(uint8_t *buf)
 {
-	return ((struct msm_rpm_ack_msg *)buf)->id_ack;
+	return get_ack_msg_id(buf);
 }
 
 static inline int msm_rpm_get_error_from_ack(uint8_t *buf)
 {
 	uint8_t *tmp;
-	uint32_t req_len = ((struct msm_rpm_ack_msg *)buf)->req_len;
-
-	struct msm_rpm_ack_msg *tmp_buf = (struct msm_rpm_ack_msg *)buf;
+	uint32_t req_len = get_ack_req_len(buf);
+	uint32_t msg_type = get_ack_msg_type(buf);
 	int rc = -ENODEV;
+	uint32_t err;
+	uint32_t ack_msg_size = rpm_msg_fmt_ver ?
+			sizeof(struct msm_rpm_ack_msg_v1) :
+			sizeof(struct msm_rpm_ack_msg_v0);
+
+	if (rpm_msg_fmt_ver == RPM_MSG_V0_FMT &&
+			msg_type == RPM_V1_ACK_SERVICE) {
+		return 0;
+	} else if (rpm_msg_fmt_ver && msg_type == RPM_V1_NACK_SERVICE) {
+		err = *(uint32_t *)(buf + sizeof(struct msm_rpm_ack_msg_v1));
+		return err;
+	}
 
-	req_len -= sizeof(struct msm_rpm_ack_msg);
+	req_len -= ack_msg_size;
 	req_len += 2 * sizeof(uint32_t);
 	if (!req_len)
 		return 0;
 
 	pr_err("%s:rpm returned error or nack req_len: %d id_ack: %d\n",
-				__func__, tmp_buf->req_len, tmp_buf->id_ack);
+				__func__, req_len, get_ack_msg_id(buf));
 
-	tmp = buf + sizeof(struct msm_rpm_ack_msg);
+	tmp = buf + ack_msg_size;
 
 	if (memcmp(tmp, ERR, sizeof(uint32_t))) {
 		pr_err("%s rpm returned error\n", __func__);
@@ -440,9 +706,9 @@ int msm_rpm_smd_buffer_request(struct msm_rpm_request *cdata,
 		/* handle unsent requests */
 		tr_update(slp, buf);
 	}
-	trace_rpm_smd_sleep_set(cdata->msg_hdr.msg_id,
-				cdata->msg_hdr.resource_type,
-				cdata->msg_hdr.resource_id);
+	trace_rpm_smd_sleep_set(get_msg_id(cdata->client_buf),
+			get_rsc_type(cdata->client_buf),
+			get_req_len(cdata->client_buf));
 
 	spin_unlock_irqrestore(&slp_buffer_lock, flags);
 
@@ -454,8 +720,9 @@ static void msm_rpm_print_sleep_buffer(struct slp_buf *s)
 	int pos;
 	int buflen = DEBUG_PRINT_BUFFER_SIZE;
 	char ch[5] = {0};
-	u32 type;
 	struct kvp *e;
+	uint32_t type;
+	unsigned int id;
 
 	if (!s)
 		return;
@@ -464,13 +731,15 @@ static void msm_rpm_print_sleep_buffer(struct slp_buf *s)
 		return;
 
 	type = get_rsc_type(s->buf);
+	id = get_rsc_id(s->buf);
+
 	memcpy(ch, &type, sizeof(u32));
 
 	pos = scnprintf(buf, buflen,
 			"Sleep request type = 0x%08x(%s)",
-			get_rsc_type(s->buf), ch);
+			type, ch);
 	pos += scnprintf(buf + pos, buflen - pos, " id = 0%x",
-			get_rsc_id(s->buf));
+			id);
 	for_each_kvp(s->buf, e) {
 		uint32_t i;
 		char *data = get_data(e);
@@ -544,6 +813,8 @@ static int msm_rpm_flush_requests(bool print)
 	for (t = rb_first(&tr_root); t; t = rb_next(t)) {
 
 		struct slp_buf *s = rb_entry(t, struct slp_buf, node);
+		unsigned int type = get_rsc_type(s->buf);
+		unsigned int id = get_rsc_id(s->buf);
 
 		if (!s->valid)
 			continue;
@@ -551,7 +822,7 @@ static int msm_rpm_flush_requests(bool print)
 		if (print)
 			msm_rpm_print_sleep_buffer(s);
 
-		get_msg_id(s->buf) = msm_rpm_get_next_msg_id();
+		set_msg_id(s->buf, msm_rpm_get_next_msg_id());
 
 		if (!glink_enabled)
 			ret = msm_rpm_send_smd_buffer(s->buf,
@@ -561,9 +832,7 @@ static int msm_rpm_flush_requests(bool print)
 					get_buf_len(s->buf), true);
 
 		WARN_ON(ret != get_buf_len(s->buf));
-		trace_rpm_smd_send_sleep_set(get_msg_id(s->buf),
-					get_rsc_type(s->buf),
-					get_rsc_id(s->buf));
+		trace_rpm_smd_send_sleep_set(get_msg_id(s->buf), type, id);
 
 		s->valid = false;
 		count++;
@@ -587,13 +856,13 @@ static int msm_rpm_flush_requests(bool print)
 	return 0;
 }
 
-static void msm_rpm_notify_sleep_chain(struct rpm_message_header *hdr,
+static void msm_rpm_notify_sleep_chain(char *buf,
 		struct msm_rpm_kvp_data *kvp)
 {
 	struct msm_rpm_notifier_data notif;
 
-	notif.rsc_type = hdr->resource_type;
-	notif.rsc_id = hdr->resource_id;
+	notif.rsc_type = get_rsc_type(buf);
+	notif.rsc_id = get_req_len(buf);
 	notif.key = kvp->key;
 	notif.size = kvp->nbytes;
 	notif.value = kvp->value;
@@ -618,7 +887,7 @@ static int msm_rpm_add_kvp_data_common(struct msm_rpm_request *handle,
 		return  -EINVAL;
 
 	data_size = ALIGN(size, SZ_4);
-	msg_size = data_size + sizeof(struct rpm_request_header);
+	msg_size = data_size + 8;
 
 	for (i = 0; i < handle->write_idx; i++) {
 		if (handle->kvp[i].key != key)
@@ -657,9 +926,10 @@ static int msm_rpm_add_kvp_data_common(struct msm_rpm_request *handle,
 	}
 
 	if (!handle->kvp[i].valid)
-		handle->msg_hdr.data_len += msg_size;
+		change_data_len(handle->client_buf, msg_size);
 	else
-		handle->msg_hdr.data_len += (data_size - handle->kvp[i].nbytes);
+		change_data_len(handle->client_buf,
+			(data_size - handle->kvp[i].nbytes));
 
 	handle->kvp[i].nbytes = data_size;
 	handle->kvp[i].key = key;
@@ -675,6 +945,7 @@ static struct msm_rpm_request *msm_rpm_create_request_common(
 		int num_elements, bool noirq)
 {
 	struct msm_rpm_request *cdata;
+	uint32_t buf_size;
 
 	if (probe_status)
 		return ERR_PTR(probe_status);
@@ -687,10 +958,19 @@ static struct msm_rpm_request *msm_rpm_create_request_common(
 		goto cdata_alloc_fail;
 	}
 
-	cdata->msg_hdr.set = set;
-	cdata->msg_hdr.resource_type = rsc_type;
-	cdata->msg_hdr.resource_id = rsc_id;
-	cdata->msg_hdr.data_len = 0;
+	if (rpm_msg_fmt_ver == RPM_MSG_V0_FMT)
+		buf_size = sizeof(struct rpm_message_header_v0);
+	else
+		buf_size = sizeof(struct rpm_message_header_v1);
+
+	cdata->client_buf = kzalloc(buf_size, GFP_FLAG(noirq));
+
+	if (!cdata->client_buf)
+		goto cdata_alloc_fail;
+
+	set_set_type(cdata->client_buf, set);
+	set_rsc_type(cdata->client_buf, rsc_type);
+	set_rsc_id(cdata->client_buf, rsc_id);
 
 	cdata->num_elements = num_elements;
 	cdata->write_idx = 0;
@@ -730,6 +1010,7 @@ void msm_rpm_free_request(struct msm_rpm_request *handle)
 	for (i = 0; i < handle->num_elements; i++)
 		kfree(handle->kvp[i].value);
 	kfree(handle->kvp);
+	kfree(handle->client_buf);
 	kfree(handle->buf);
 	kfree(handle);
 }
@@ -973,6 +1254,7 @@ static void msm_rpm_log_request(struct msm_rpm_request *cdata)
 	int j, prev_valid;
 	int valid_count = 0;
 	int pos = 0;
+	uint32_t res_type, rsc_id;
 
 	name[4] = 0;
 
@@ -983,18 +1265,20 @@ static void msm_rpm_log_request(struct msm_rpm_request *cdata)
 	pos += scnprintf(buf + pos, buflen - pos, "%sRPM req: ", KERN_INFO);
 	if (msm_rpm_debug_mask & MSM_RPM_LOG_REQUEST_SHOW_MSG_ID)
 		pos += scnprintf(buf + pos, buflen - pos, "msg_id=%u, ",
-				cdata->msg_hdr.msg_id);
+				get_msg_id(cdata->client_buf));
 	pos += scnprintf(buf + pos, buflen - pos, "s=%s",
-		(cdata->msg_hdr.set == MSM_RPM_CTX_ACTIVE_SET ? "act" : "slp"));
+		(get_set_type(cdata->client_buf) ==
+				MSM_RPM_CTX_ACTIVE_SET ? "act" : "slp"));
 
+	res_type = get_rsc_type(cdata->client_buf);
+	rsc_id = get_rsc_id(cdata->client_buf);
 	if ((msm_rpm_debug_mask & MSM_RPM_LOG_REQUEST_PRETTY)
 	    && (msm_rpm_debug_mask & MSM_RPM_LOG_REQUEST_RAW)) {
 		/* Both pretty and raw formatting */
-		memcpy(name, &cdata->msg_hdr.resource_type, sizeof(uint32_t));
+		memcpy(name, &res_type, sizeof(uint32_t));
 		pos += scnprintf(buf + pos, buflen - pos,
 			", rsc_type=0x%08X (%s), rsc_id=%u; ",
-			cdata->msg_hdr.resource_type, name,
-			cdata->msg_hdr.resource_id);
+			res_type, name, rsc_id);
 
 		for (i = 0, prev_valid = 0; i < cdata->write_idx; i++) {
 			if (!cdata->kvp[i].valid)
@@ -1034,9 +1318,9 @@ static void msm_rpm_log_request(struct msm_rpm_request *cdata)
 		}
 	} else if (msm_rpm_debug_mask & MSM_RPM_LOG_REQUEST_PRETTY) {
 		/* Pretty formatting only */
-		memcpy(name, &cdata->msg_hdr.resource_type, sizeof(uint32_t));
+		memcpy(name, &res_type, sizeof(uint32_t));
 		pos += scnprintf(buf + pos, buflen - pos, " %s %u; ", name,
-				cdata->msg_hdr.resource_id);
+			rsc_id);
 
 		for (i = 0, prev_valid = 0; i < cdata->write_idx; i++) {
 			if (!cdata->kvp[i].valid)
@@ -1065,9 +1349,7 @@ static void msm_rpm_log_request(struct msm_rpm_request *cdata)
 	} else {
 		/* Raw formatting only */
 		pos += scnprintf(buf + pos, buflen - pos,
-			", rsc_type=0x%08X, rsc_id=%u; ",
-			cdata->msg_hdr.resource_type,
-			cdata->msg_hdr.resource_id);
+			", rsc_type=0x%08X, rsc_id=%u; ", res_type, rsc_id);
 
 		for (i = 0, prev_valid = 0; i < cdata->write_idx; i++) {
 			if (!cdata->kvp[i].valid)
@@ -1169,21 +1451,25 @@ static int msm_rpm_send_data(struct msm_rpm_request *cdata,
 	int ret;
 	uint32_t i;
 	uint32_t msg_size;
-	int req_hdr_sz, msg_hdr_sz;
+	int msg_hdr_sz, req_hdr_sz;
+	uint32_t data_len = get_data_len(cdata->client_buf);
+	uint32_t set = get_set_type(cdata->client_buf);
+	uint32_t msg_id;
 
 	if (probe_status)
 		return probe_status;
 
-	if (!cdata->msg_hdr.data_len)
+	if (!data_len)
 		return 1;
 
-	req_hdr_sz = sizeof(cdata->req_hdr);
-	msg_hdr_sz = sizeof(cdata->msg_hdr);
+	msg_hdr_sz = rpm_msg_fmt_ver ? sizeof(struct rpm_message_header_v1) :
+			sizeof(struct rpm_message_header_v0);
 
-	cdata->req_hdr.service_type = msm_rpm_request_service[msg_type];
+	req_hdr_sz = RPM_HDR_SIZE;
+	set_msg_type(cdata->client_buf, msg_type);
 
-	cdata->req_hdr.request_len = cdata->msg_hdr.data_len + msg_hdr_sz;
-	msg_size = cdata->req_hdr.request_len + req_hdr_sz;
+	set_req_len(cdata->client_buf, data_len + msg_hdr_sz - req_hdr_sz);
+	msg_size = get_req_len(cdata->client_buf) + req_hdr_sz;
 
 	/* populate data_len */
 	if (msg_size > cdata->numbytes) {
@@ -1199,8 +1485,7 @@ static int msm_rpm_send_data(struct msm_rpm_request *cdata,
 
 	tmpbuff = cdata->buf;
 
-	tmpbuff += req_hdr_sz + msg_hdr_sz;
-
+	tmpbuff += msg_hdr_sz;
 	for (i = 0; (i < cdata->write_idx); i++) {
 		/* Sanity check */
 		BUG_ON((tmpbuff - cdata->buf) > cdata->numbytes);
@@ -1217,22 +1502,23 @@ static int msm_rpm_send_data(struct msm_rpm_request *cdata,
 		memcpy(tmpbuff, cdata->kvp[i].value, cdata->kvp[i].nbytes);
 		tmpbuff += cdata->kvp[i].nbytes;
 
-		if (cdata->msg_hdr.set == MSM_RPM_CTX_SLEEP_SET)
-			msm_rpm_notify_sleep_chain(&cdata->msg_hdr,
+		if (set == MSM_RPM_CTX_SLEEP_SET)
+			msm_rpm_notify_sleep_chain(cdata->client_buf,
 					&cdata->kvp[i]);
 
 	}
 
-	memcpy(cdata->buf, &cdata->req_hdr, req_hdr_sz + msg_hdr_sz);
-
-	if ((cdata->msg_hdr.set == MSM_RPM_CTX_SLEEP_SET) &&
+	memcpy(cdata->buf, cdata->client_buf, msg_hdr_sz);
+	if ((set == MSM_RPM_CTX_SLEEP_SET) &&
 		!msm_rpm_smd_buffer_request(cdata, msg_size,
 			GFP_FLAG(noirq)))
 		return 1;
 
-	cdata->msg_hdr.msg_id = msm_rpm_get_next_msg_id();
-
-	memcpy(cdata->buf + req_hdr_sz, &cdata->msg_hdr, msg_hdr_sz);
+	msg_id = msm_rpm_get_next_msg_id();
+	/* Set the version bit for new protocol */
+	set_msg_ver(cdata->buf, rpm_msg_fmt_ver);
+	set_msg_id(cdata->buf, msg_id);
+	set_msg_id(cdata->client_buf, msg_id);
 
 	if (msm_rpm_debug_mask
 	    & (MSM_RPM_LOG_REQUEST_PRETTY | MSM_RPM_LOG_REQUEST_RAW))
@@ -1242,30 +1528,30 @@ static int msm_rpm_send_data(struct msm_rpm_request *cdata,
 		for (i = 0; (i < cdata->write_idx); i++)
 			cdata->kvp[i].valid = false;
 
-		cdata->msg_hdr.data_len = 0;
-		ret = cdata->msg_hdr.msg_id;
+		set_data_len(cdata->client_buf, 0);
+		ret = msg_id;
 		return ret;
 	}
 
 	if (!noack)
-		msm_rpm_add_wait_list(cdata->msg_hdr.msg_id);
+		msm_rpm_add_wait_list(msg_id);
 
 	ret = msm_rpm_send_buffer(&cdata->buf[0], msg_size, noirq);
 
 	if (ret == msg_size) {
 		for (i = 0; (i < cdata->write_idx); i++)
 			cdata->kvp[i].valid = false;
-		cdata->msg_hdr.data_len = 0;
-		ret = cdata->msg_hdr.msg_id;
-		trace_rpm_smd_send_active_set(cdata->msg_hdr.msg_id,
-					cdata->msg_hdr.resource_type,
-					cdata->msg_hdr.resource_id);
+		set_data_len(cdata->client_buf, 0);
+		ret = msg_id;
+		trace_rpm_smd_send_active_set(msg_id,
+			get_rsc_type(cdata->client_buf),
+			get_rsc_id(cdata->client_buf));
 	} else if (ret < msg_size) {
 		struct msm_rpm_wait_data *rc;
 		ret = 0;
 		pr_err("Failed to write data msg_size:%d ret:%d msg_id:%d\n",
-				msg_size, ret, cdata->msg_hdr.msg_id);
-		rc = msm_rpm_get_entry_from_msg_id(cdata->msg_hdr.msg_id);
+				msg_size, ret, msg_id);
+		rc = msm_rpm_get_entry_from_msg_id(msg_id);
 		if (rc)
 			msm_rpm_free_list_entry(rc);
 	}
@@ -1801,6 +2087,8 @@ static int msm_rpm_dev_probe(struct platform_device *pdev)
 {
 	char *key = NULL;
 	int ret = 0;
+	void __iomem *reg_base;
+	uint32_t version = V0_PROTOCOL_VERSION; /* set to default v0 format */
 
 	/*
 	 * Check for standalone support
@@ -1812,6 +2100,18 @@ static int msm_rpm_dev_probe(struct platform_device *pdev)
 		goto skip_init;
 	}
 
+	reg_base = of_iomap(pdev->dev.of_node, 0);
+
+	if (reg_base) {
+		version = readq_relaxed(reg_base);
+		iounmap(reg_base);
+	}
+
+	if (version == V1_PROTOCOL_VERSION)
+		rpm_msg_fmt_ver = RPM_MSG_V1_FMT;
+
+	pr_debug("RPM-SMD running version %d/n", rpm_msg_fmt_ver);
+
 	ret = msm_rpm_dev_glink_probe(pdev);
 	if (!ret) {
 		pr_info("APSS-RPM communication over GLINK\n");