[RFC,03/11] firmware: arm_scmi: Add support for notifications message processing
diff mbox series

Message ID 20200120122333.46217-4-cristian.marussi@arm.com
State New
Headers show
Series
  • SCMI Notifications Support
Related show

Commit Message

Cristian Marussi Jan. 20, 2020, 12:23 p.m. UTC
From: Sudeep Holla <sudeep.holla@arm.com>

Add the mechanisms to distinguish notifications from delayed responses and
to properly fetch notification messages upon reception: notifications
processing does not continue further after the fetch phase.

Signed-off-by: Sudeep Holla <sudeep.holla@arm.com>
Signed-off-by: Cristian Marussi <cristian.marussi@arm.com>
---
 drivers/firmware/arm_scmi/driver.c | 92 +++++++++++++++++++++---------
 1 file changed, 65 insertions(+), 27 deletions(-)

Comments

Cristian Marussi Jan. 23, 2020, 10:58 a.m. UTC | #1
Hi Jim

On 22/01/2020 23:27, Jim Quinlan wrote:
> Hi,
> 
> I tried "git am" on an mbox file  from these commits and get stuck on
> 
>     Applying: firmware: arm_scmi: Add receive buffer support for notifications
>     Applying: firmware: arm_scmi: Update protocol commands and notification list
>     Applying: firmware: arm_scmi: Add support for notifications message processing
>     error: patch failed: drivers/firmware/arm_scmi/driver.c:355
>     error: drivers/firmware/arm_scmi/driver.c: patch does not apply
> 
> 
> Could you please apply this email patchset and let me know if it works for you?  I am doing this onto
> 
>     257d0e20ec4f include: trace: Add SCMI header with trace events     
> 
> 

Sorry... my fault .. it is on top of the following commit on that same branch in fact:

729d3530a504 drivers: firmware: scmi: Extend SCMI transport layer by trace events

10:51 $ git am patch_scmi_notif/ext_V1/final/00*                          
Applying: firmware: arm_scmi: Add receive buffer support for notifications
Applying: firmware: arm_scmi: Update protocol commands and notification list         
Applying: firmware: arm_scmi: Add support for notifications message processing
Applying: firmware: arm_scmi: Add core notifications support                    
Applying: firmware: arm_scmi: Add notifications anti-tampering                                 
Applying: firmware: arm_scmi: Enable core notifications                  
Applying: firmware: arm_scmi: Add Power notifications support             
Applying: firmware: arm_scmi: Add Perf notifications support                
Applying: firmware: arm_scmi: Add Sensor notifications support             
Applying: firmware: arm_scmi: Add Reset notifications support                  
Applying: firmware: arm_scmi: Add Base notifications support            

I'll follow up to my cover to warn about this.

Thanks for trying it out.

Regards

Cristian

> as you directed.
> 
> Thanks,
> Jim
> 
> 
> 
> On 1/20/20 7:23 AM, Cristian Marussi wrote:
>> From: Sudeep Holla <sudeep.holla@arm.com>
>>
>> Add the mechanisms to distinguish notifications from delayed responses and
>> to properly fetch notification messages upon reception: notifications
>> processing does not continue further after the fetch phase.
>>
>> Signed-off-by: Sudeep Holla <sudeep.holla@arm.com>
>> Signed-off-by: Cristian Marussi <cristian.marussi@arm.com>
>> ---
>>  drivers/firmware/arm_scmi/driver.c | 92 +++++++++++++++++++++---------
>>  1 file changed, 65 insertions(+), 27 deletions(-)
>>
>> diff --git a/drivers/firmware/arm_scmi/driver.c b/drivers/firmware/arm_scmi/driver.c
>> index 9611e8037d77..28ed1f0cb417 100644
>> --- a/drivers/firmware/arm_scmi/driver.c
>> +++ b/drivers/firmware/arm_scmi/driver.c
>> @@ -212,6 +212,15 @@ static void scmi_fetch_response(struct scmi_xfer *xfer,
>>  	memcpy_fromio(xfer->rx.buf, mem->msg_payload + 4, xfer->rx.len);
>>  }
>>  
>> +static void scmi_fetch_notification(struct scmi_xfer *xfer, size_t max_len,
>> +				    struct scmi_shared_mem __iomem *mem)
>> +{
>> +	/* Skip only length of header in payload area i.e 4 bytes */
>> +	xfer->rx.len = min_t(size_t, max_len, ioread32(&mem->length) - 4);
>> +
>> +	memcpy_fromio(xfer->rx.buf, mem->msg_payload, xfer->rx.len);
>> +}
>> +
>>  /**
>>   * pack_scmi_header() - packs and returns 32-bit header
>>   *
>> @@ -339,6 +348,58 @@ __scmi_xfer_put(struct scmi_xfers_info *minfo, struct scmi_xfer *xfer)
>>  	spin_unlock_irqrestore(&minfo->xfer_lock, flags);
>>  }
>>  
>> +static void scmi_handle_notification(struct scmi_chan_info *cinfo, u32 msg_hdr)
>> +{
>> +	struct scmi_xfer *xfer;
>> +	struct device *dev = cinfo->dev;
>> +	struct scmi_info *info = handle_to_scmi_info(cinfo->handle);
>> +	struct scmi_xfers_info *minfo = &info->rx_minfo;
>> +	struct scmi_shared_mem __iomem *mem = cinfo->payload;
>> +
>> +	xfer = scmi_xfer_get(cinfo->handle, minfo);
>> +	if (IS_ERR(xfer)) {
>> +		dev_err(dev, "failed to get free message slot (%ld)\n",
>> +			PTR_ERR(xfer));
>> +		iowrite32(SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE,
>> +			  &mem->channel_status);
>> +		return;
>> +	}
>> +
>> +	unpack_scmi_header(msg_hdr, &xfer->hdr);
>> +	scmi_dump_header_dbg(dev, &xfer->hdr);
>> +	scmi_fetch_notification(xfer, info->desc->max_msg_size, mem);
>> +	__scmi_xfer_put(minfo, xfer);
>> +
>> +	iowrite32(SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE, &mem->channel_status);
>> +}
>> +
>> +static void scmi_handle_xfer_delayed_resp(struct scmi_chan_info *cinfo,
>> +					  u16 xfer_id, bool delayed_resp)
>> +{
>> +	struct scmi_xfer *xfer;
>> +	struct device *dev = cinfo->dev;
>> +	struct scmi_info *info = handle_to_scmi_info(cinfo->handle);
>> +	struct scmi_xfers_info *minfo = &info->tx_minfo;
>> +	struct scmi_shared_mem __iomem *mem = cinfo->payload;
>> +
>> +	/* Are we even expecting this? */
>> +	if (!test_bit(xfer_id, minfo->xfer_alloc_table)) {
>> +		dev_err(dev, "message for %d is not expected!\n", xfer_id);
>> +		return;
>> +	}
>> +
>> +	xfer = &minfo->xfer_block[xfer_id];
>> +
>> +	scmi_dump_header_dbg(dev, &xfer->hdr);
>> +
>> +	scmi_fetch_response(xfer, mem);
>> +
>> +	if (delayed_resp)
>> +		complete(xfer->async_done);
>> +	else
>> +		complete(&xfer->done);
>> +}
>> +
>>  /**
>>   * scmi_rx_callback() - mailbox client callback for receive messages
>>   *
>> @@ -355,41 +416,18 @@ static void scmi_rx_callback(struct mbox_client *cl, void *m)
>>  {
>>  	u8 msg_type;
>>  	u32 msg_hdr;
>> -	u16 xfer_id;
>> -	struct scmi_xfer *xfer;
>>  	struct scmi_chan_info *cinfo = client_to_scmi_chan_info(cl);
>> -	struct device *dev = cinfo->dev;
>> -	struct scmi_info *info = handle_to_scmi_info(cinfo->handle);
>> -	struct scmi_xfers_info *minfo = &info->tx_minfo;
>>  	struct scmi_shared_mem __iomem *mem = cinfo->payload;
>>  
>>  	msg_hdr = ioread32(&mem->msg_header);
>>  	msg_type = MSG_XTRACT_TYPE(msg_hdr);
>> -	xfer_id = MSG_XTRACT_TOKEN(msg_hdr);
>>  
>>  	if (msg_type == MSG_TYPE_NOTIFICATION)
>> -		return; /* Notifications not yet supported */
>> -
>> -	/* Are we even expecting this? */
>> -	if (!test_bit(xfer_id, minfo->xfer_alloc_table)) {
>> -		dev_err(dev, "message for %d is not expected!\n", xfer_id);
>> -		return;
>> -	}
>> -
>> -	xfer = &minfo->xfer_block[xfer_id];
>> -
>> -	scmi_dump_header_dbg(dev, &xfer->hdr);
>> -
>> -	scmi_fetch_response(xfer, mem);
>> -
>> -	trace_scmi_rx_done(xfer->transfer_id, xfer->hdr.id,
>> -			   xfer->hdr.protocol_id, xfer->hdr.seq,
>> -			   msg_type);
>> -
>> -	if (msg_type == MSG_TYPE_DELAYED_RESP)
>> -		complete(xfer->async_done);
>> +		scmi_handle_notification(cinfo, msg_hdr);
>>  	else
>> -		complete(&xfer->done);
>> +		scmi_handle_xfer_delayed_resp(cinfo, MSG_XTRACT_TOKEN(msg_hdr),
>> +					      msg_type);
>> +
>>  }
>>  
>>  /**
> 
>
Jonathan Cameron Jan. 27, 2020, 5:32 p.m. UTC | #2
On Mon, 20 Jan 2020 12:23:25 +0000
Cristian Marussi <cristian.marussi@arm.com> wrote:

> From: Sudeep Holla <sudeep.holla@arm.com>
> 
> Add the mechanisms to distinguish notifications from delayed responses and
> to properly fetch notification messages upon reception: notifications
> processing does not continue further after the fetch phase.
> 
> Signed-off-by: Sudeep Holla <sudeep.holla@arm.com>
> Signed-off-by: Cristian Marussi <cristian.marussi@arm.com>

Couple of bits that seem more interesting than expected inline...

> ---
>  drivers/firmware/arm_scmi/driver.c | 92 +++++++++++++++++++++---------
>  1 file changed, 65 insertions(+), 27 deletions(-)
> 
> diff --git a/drivers/firmware/arm_scmi/driver.c b/drivers/firmware/arm_scmi/driver.c
> index 9611e8037d77..28ed1f0cb417 100644
> --- a/drivers/firmware/arm_scmi/driver.c
> +++ b/drivers/firmware/arm_scmi/driver.c
> @@ -212,6 +212,15 @@ static void scmi_fetch_response(struct scmi_xfer *xfer,
>  	memcpy_fromio(xfer->rx.buf, mem->msg_payload + 4, xfer->rx.len);
>  }
>  
> +static void scmi_fetch_notification(struct scmi_xfer *xfer, size_t max_len,
> +				    struct scmi_shared_mem __iomem *mem)
> +{
> +	/* Skip only length of header in payload area i.e 4 bytes */
> +	xfer->rx.len = min_t(size_t, max_len, ioread32(&mem->length) - 4);
> +
> +	memcpy_fromio(xfer->rx.buf, mem->msg_payload, xfer->rx.len);
> +}
> +
>  /**
>   * pack_scmi_header() - packs and returns 32-bit header
>   *
> @@ -339,6 +348,58 @@ __scmi_xfer_put(struct scmi_xfers_info *minfo, struct scmi_xfer *xfer)
>  	spin_unlock_irqrestore(&minfo->xfer_lock, flags);
>  }
>  
> +static void scmi_handle_notification(struct scmi_chan_info *cinfo, u32 msg_hdr)
> +{
> +	struct scmi_xfer *xfer;
> +	struct device *dev = cinfo->dev;
> +	struct scmi_info *info = handle_to_scmi_info(cinfo->handle);
> +	struct scmi_xfers_info *minfo = &info->rx_minfo;
> +	struct scmi_shared_mem __iomem *mem = cinfo->payload;
> +
> +	xfer = scmi_xfer_get(cinfo->handle, minfo);
> +	if (IS_ERR(xfer)) {
> +		dev_err(dev, "failed to get free message slot (%ld)\n",
> +			PTR_ERR(xfer));
> +		iowrite32(SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE,
> +			  &mem->channel_status);
> +		return;
> +	}
> +
> +	unpack_scmi_header(msg_hdr, &xfer->hdr);
> +	scmi_dump_header_dbg(dev, &xfer->hdr);
> +	scmi_fetch_notification(xfer, info->desc->max_msg_size, mem);
> +	__scmi_xfer_put(minfo, xfer);
> +
> +	iowrite32(SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE, &mem->channel_status);
> +}
> +
> +static void scmi_handle_xfer_delayed_resp(struct scmi_chan_info *cinfo,
> +					  u16 xfer_id, bool delayed_resp)

Hmm. A function called *_delayed_resp that takes a boolean to say if
it is a delayed_resp is in the category of non obvious....  Needs a rename
at the very least.

> +{
> +	struct scmi_xfer *xfer;
> +	struct device *dev = cinfo->dev;
> +	struct scmi_info *info = handle_to_scmi_info(cinfo->handle);
> +	struct scmi_xfers_info *minfo = &info->tx_minfo;
> +	struct scmi_shared_mem __iomem *mem = cinfo->payload;
> +
> +	/* Are we even expecting this? */
> +	if (!test_bit(xfer_id, minfo->xfer_alloc_table)) {
> +		dev_err(dev, "message for %d is not expected!\n", xfer_id);
> +		return;
> +	}
> +
> +	xfer = &minfo->xfer_block[xfer_id];
> +
> +	scmi_dump_header_dbg(dev, &xfer->hdr);
> +
> +	scmi_fetch_response(xfer, mem);
> +
> +	if (delayed_resp)
> +		complete(xfer->async_done);
> +	else
> +		complete(&xfer->done);
> +}
> +
>  /**
>   * scmi_rx_callback() - mailbox client callback for receive messages
>   *
> @@ -355,41 +416,18 @@ static void scmi_rx_callback(struct mbox_client *cl, void *m)
>  {
>  	u8 msg_type;
>  	u32 msg_hdr;
> -	u16 xfer_id;
> -	struct scmi_xfer *xfer;
>  	struct scmi_chan_info *cinfo = client_to_scmi_chan_info(cl);
> -	struct device *dev = cinfo->dev;
> -	struct scmi_info *info = handle_to_scmi_info(cinfo->handle);
> -	struct scmi_xfers_info *minfo = &info->tx_minfo;
>  	struct scmi_shared_mem __iomem *mem = cinfo->payload;
>  
>  	msg_hdr = ioread32(&mem->msg_header);
>  	msg_type = MSG_XTRACT_TYPE(msg_hdr);
> -	xfer_id = MSG_XTRACT_TOKEN(msg_hdr);
>  
>  	if (msg_type == MSG_TYPE_NOTIFICATION)
> -		return; /* Notifications not yet supported */
> -
> -	/* Are we even expecting this? */
> -	if (!test_bit(xfer_id, minfo->xfer_alloc_table)) {
> -		dev_err(dev, "message for %d is not expected!\n", xfer_id);
> -		return;
> -	}
> -
> -	xfer = &minfo->xfer_block[xfer_id];
> -
> -	scmi_dump_header_dbg(dev, &xfer->hdr);
> -
> -	scmi_fetch_response(xfer, mem);
> -
> -	trace_scmi_rx_done(xfer->transfer_id, xfer->hdr.id,
> -			   xfer->hdr.protocol_id, xfer->hdr.seq,
> -			   msg_type);
> -
> -	if (msg_type == MSG_TYPE_DELAYED_RESP)
> -		complete(xfer->async_done);
> +		scmi_handle_notification(cinfo, msg_hdr);
>  	else
> -		complete(&xfer->done);
> +		scmi_handle_xfer_delayed_resp(cinfo, MSG_XTRACT_TOKEN(msg_hdr),
> +					      msg_type);
First I wondered why this wasn't a switch which would make a clear distinction
between notification path and delayed response... 

However, it seems delayed_resp path also handles other values of msg_type,
though only 0 which is a command I think...

Passing a enum that I think can take 4 values, only 3 of which are defined
into a function as a boolean is 'interesting'. Don't do that.


> +
>  }
>  
>  /**
Cristian Marussi Feb. 14, 2020, 3:28 p.m. UTC | #3
Hi

On 27/01/2020 17:32, Jonathan Cameron wrote:
> On Mon, 20 Jan 2020 12:23:25 +0000
> Cristian Marussi <cristian.marussi@arm.com> wrote:
> 
>> From: Sudeep Holla <sudeep.holla@arm.com>
>>
>> Add the mechanisms to distinguish notifications from delayed responses and
>> to properly fetch notification messages upon reception: notifications
>> processing does not continue further after the fetch phase.
>>
>> Signed-off-by: Sudeep Holla <sudeep.holla@arm.com>
>> Signed-off-by: Cristian Marussi <cristian.marussi@arm.com>
> 
> Couple of bits that seem more interesting than expected inline...
> 
>> ---
>>  drivers/firmware/arm_scmi/driver.c | 92 +++++++++++++++++++++---------
>>  1 file changed, 65 insertions(+), 27 deletions(-)
>>
>> diff --git a/drivers/firmware/arm_scmi/driver.c b/drivers/firmware/arm_scmi/driver.c
>> index 9611e8037d77..28ed1f0cb417 100644
>> --- a/drivers/firmware/arm_scmi/driver.c
>> +++ b/drivers/firmware/arm_scmi/driver.c
>> @@ -212,6 +212,15 @@ static void scmi_fetch_response(struct scmi_xfer *xfer,
>>  	memcpy_fromio(xfer->rx.buf, mem->msg_payload + 4, xfer->rx.len);
>>  }
>>  
>> +static void scmi_fetch_notification(struct scmi_xfer *xfer, size_t max_len,
>> +				    struct scmi_shared_mem __iomem *mem)
>> +{
>> +	/* Skip only length of header in payload area i.e 4 bytes */
>> +	xfer->rx.len = min_t(size_t, max_len, ioread32(&mem->length) - 4);
>> +
>> +	memcpy_fromio(xfer->rx.buf, mem->msg_payload, xfer->rx.len);
>> +}
>> +
>>  /**
>>   * pack_scmi_header() - packs and returns 32-bit header
>>   *
>> @@ -339,6 +348,58 @@ __scmi_xfer_put(struct scmi_xfers_info *minfo, struct scmi_xfer *xfer)
>>  	spin_unlock_irqrestore(&minfo->xfer_lock, flags);
>>  }
>>  
>> +static void scmi_handle_notification(struct scmi_chan_info *cinfo, u32 msg_hdr)
>> +{
>> +	struct scmi_xfer *xfer;
>> +	struct device *dev = cinfo->dev;
>> +	struct scmi_info *info = handle_to_scmi_info(cinfo->handle);
>> +	struct scmi_xfers_info *minfo = &info->rx_minfo;
>> +	struct scmi_shared_mem __iomem *mem = cinfo->payload;
>> +
>> +	xfer = scmi_xfer_get(cinfo->handle, minfo);
>> +	if (IS_ERR(xfer)) {
>> +		dev_err(dev, "failed to get free message slot (%ld)\n",
>> +			PTR_ERR(xfer));
>> +		iowrite32(SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE,
>> +			  &mem->channel_status);
>> +		return;
>> +	}
>> +
>> +	unpack_scmi_header(msg_hdr, &xfer->hdr);
>> +	scmi_dump_header_dbg(dev, &xfer->hdr);
>> +	scmi_fetch_notification(xfer, info->desc->max_msg_size, mem);
>> +	__scmi_xfer_put(minfo, xfer);
>> +
>> +	iowrite32(SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE, &mem->channel_status);
>> +}
>> +
>> +static void scmi_handle_xfer_delayed_resp(struct scmi_chan_info *cinfo,
>> +					  u16 xfer_id, bool delayed_resp)
> 
> Hmm. A function called *_delayed_resp that takes a boolean to say if
> it is a delayed_resp is in the category of non obvious....  Needs a rename
> at the very least.
> 

Changed logic and naming in v2, also because now v2 is on top of Viresh series
which makes transport layer independent. (in fact an additional dedicated patch
is dedicated to extend the transport layer for notifications too)


>> +{
>> +	struct scmi_xfer *xfer;
>> +	struct device *dev = cinfo->dev;
>> +	struct scmi_info *info = handle_to_scmi_info(cinfo->handle);
>> +	struct scmi_xfers_info *minfo = &info->tx_minfo;
>> +	struct scmi_shared_mem __iomem *mem = cinfo->payload;
>> +
>> +	/* Are we even expecting this? */
>> +	if (!test_bit(xfer_id, minfo->xfer_alloc_table)) {
>> +		dev_err(dev, "message for %d is not expected!\n", xfer_id);
>> +		return;
>> +	}
>> +
>> +	xfer = &minfo->xfer_block[xfer_id];
>> +
>> +	scmi_dump_header_dbg(dev, &xfer->hdr);
>> +
>> +	scmi_fetch_response(xfer, mem);
>> +
>> +	if (delayed_resp)
>> +		complete(xfer->async_done);
>> +	else
>> +		complete(&xfer->done);
>> +}
>> +
>>  /**
>>   * scmi_rx_callback() - mailbox client callback for receive messages
>>   *
>> @@ -355,41 +416,18 @@ static void scmi_rx_callback(struct mbox_client *cl, void *m)
>>  {
>>  	u8 msg_type;
>>  	u32 msg_hdr;
>> -	u16 xfer_id;
>> -	struct scmi_xfer *xfer;
>>  	struct scmi_chan_info *cinfo = client_to_scmi_chan_info(cl);
>> -	struct device *dev = cinfo->dev;
>> -	struct scmi_info *info = handle_to_scmi_info(cinfo->handle);
>> -	struct scmi_xfers_info *minfo = &info->tx_minfo;
>>  	struct scmi_shared_mem __iomem *mem = cinfo->payload;
>>  
>>  	msg_hdr = ioread32(&mem->msg_header);
>>  	msg_type = MSG_XTRACT_TYPE(msg_hdr);
>> -	xfer_id = MSG_XTRACT_TOKEN(msg_hdr);
>>  
>>  	if (msg_type == MSG_TYPE_NOTIFICATION)
>> -		return; /* Notifications not yet supported */
>> -
>> -	/* Are we even expecting this? */
>> -	if (!test_bit(xfer_id, minfo->xfer_alloc_table)) {
>> -		dev_err(dev, "message for %d is not expected!\n", xfer_id);
>> -		return;
>> -	}
>> -
>> -	xfer = &minfo->xfer_block[xfer_id];
>> -
>> -	scmi_dump_header_dbg(dev, &xfer->hdr);
>> -
>> -	scmi_fetch_response(xfer, mem);
>> -
>> -	trace_scmi_rx_done(xfer->transfer_id, xfer->hdr.id,
>> -			   xfer->hdr.protocol_id, xfer->hdr.seq,
>> -			   msg_type);
>> -
>> -	if (msg_type == MSG_TYPE_DELAYED_RESP)
>> -		complete(xfer->async_done);
>> +		scmi_handle_notification(cinfo, msg_hdr);
>>  	else
>> -		complete(&xfer->done);
>> +		scmi_handle_xfer_delayed_resp(cinfo, MSG_XTRACT_TOKEN(msg_hdr),
>> +					      msg_type);
> First I wondered why this wasn't a switch which would make a clear distinction
> between notification path and delayed response... 
> 
> However, it seems delayed_resp path also handles other values of msg_type,
> though only 0 which is a command I think...
> 
> Passing a enum that I think can take 4 values, only 3 of which are defined
> into a function as a boolean is 'interesting'. Don't do that.
> 

Reworked the logic in v2

Thanks

Regards

Cristian
> 
>> +
>>  }
>>  
>>  /**
> 
>

Patch
diff mbox series

diff --git a/drivers/firmware/arm_scmi/driver.c b/drivers/firmware/arm_scmi/driver.c
index 9611e8037d77..28ed1f0cb417 100644
--- a/drivers/firmware/arm_scmi/driver.c
+++ b/drivers/firmware/arm_scmi/driver.c
@@ -212,6 +212,15 @@  static void scmi_fetch_response(struct scmi_xfer *xfer,
 	memcpy_fromio(xfer->rx.buf, mem->msg_payload + 4, xfer->rx.len);
 }
 
+static void scmi_fetch_notification(struct scmi_xfer *xfer, size_t max_len,
+				    struct scmi_shared_mem __iomem *mem)
+{
+	/* Skip only length of header in payload area i.e 4 bytes */
+	xfer->rx.len = min_t(size_t, max_len, ioread32(&mem->length) - 4);
+
+	memcpy_fromio(xfer->rx.buf, mem->msg_payload, xfer->rx.len);
+}
+
 /**
  * pack_scmi_header() - packs and returns 32-bit header
  *
@@ -339,6 +348,58 @@  __scmi_xfer_put(struct scmi_xfers_info *minfo, struct scmi_xfer *xfer)
 	spin_unlock_irqrestore(&minfo->xfer_lock, flags);
 }
 
+static void scmi_handle_notification(struct scmi_chan_info *cinfo, u32 msg_hdr)
+{
+	struct scmi_xfer *xfer;
+	struct device *dev = cinfo->dev;
+	struct scmi_info *info = handle_to_scmi_info(cinfo->handle);
+	struct scmi_xfers_info *minfo = &info->rx_minfo;
+	struct scmi_shared_mem __iomem *mem = cinfo->payload;
+
+	xfer = scmi_xfer_get(cinfo->handle, minfo);
+	if (IS_ERR(xfer)) {
+		dev_err(dev, "failed to get free message slot (%ld)\n",
+			PTR_ERR(xfer));
+		iowrite32(SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE,
+			  &mem->channel_status);
+		return;
+	}
+
+	unpack_scmi_header(msg_hdr, &xfer->hdr);
+	scmi_dump_header_dbg(dev, &xfer->hdr);
+	scmi_fetch_notification(xfer, info->desc->max_msg_size, mem);
+	__scmi_xfer_put(minfo, xfer);
+
+	iowrite32(SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE, &mem->channel_status);
+}
+
+static void scmi_handle_xfer_delayed_resp(struct scmi_chan_info *cinfo,
+					  u16 xfer_id, bool delayed_resp)
+{
+	struct scmi_xfer *xfer;
+	struct device *dev = cinfo->dev;
+	struct scmi_info *info = handle_to_scmi_info(cinfo->handle);
+	struct scmi_xfers_info *minfo = &info->tx_minfo;
+	struct scmi_shared_mem __iomem *mem = cinfo->payload;
+
+	/* Are we even expecting this? */
+	if (!test_bit(xfer_id, minfo->xfer_alloc_table)) {
+		dev_err(dev, "message for %d is not expected!\n", xfer_id);
+		return;
+	}
+
+	xfer = &minfo->xfer_block[xfer_id];
+
+	scmi_dump_header_dbg(dev, &xfer->hdr);
+
+	scmi_fetch_response(xfer, mem);
+
+	if (delayed_resp)
+		complete(xfer->async_done);
+	else
+		complete(&xfer->done);
+}
+
 /**
  * scmi_rx_callback() - mailbox client callback for receive messages
  *
@@ -355,41 +416,18 @@  static void scmi_rx_callback(struct mbox_client *cl, void *m)
 {
 	u8 msg_type;
 	u32 msg_hdr;
-	u16 xfer_id;
-	struct scmi_xfer *xfer;
 	struct scmi_chan_info *cinfo = client_to_scmi_chan_info(cl);
-	struct device *dev = cinfo->dev;
-	struct scmi_info *info = handle_to_scmi_info(cinfo->handle);
-	struct scmi_xfers_info *minfo = &info->tx_minfo;
 	struct scmi_shared_mem __iomem *mem = cinfo->payload;
 
 	msg_hdr = ioread32(&mem->msg_header);
 	msg_type = MSG_XTRACT_TYPE(msg_hdr);
-	xfer_id = MSG_XTRACT_TOKEN(msg_hdr);
 
 	if (msg_type == MSG_TYPE_NOTIFICATION)
-		return; /* Notifications not yet supported */
-
-	/* Are we even expecting this? */
-	if (!test_bit(xfer_id, minfo->xfer_alloc_table)) {
-		dev_err(dev, "message for %d is not expected!\n", xfer_id);
-		return;
-	}
-
-	xfer = &minfo->xfer_block[xfer_id];
-
-	scmi_dump_header_dbg(dev, &xfer->hdr);
-
-	scmi_fetch_response(xfer, mem);
-
-	trace_scmi_rx_done(xfer->transfer_id, xfer->hdr.id,
-			   xfer->hdr.protocol_id, xfer->hdr.seq,
-			   msg_type);
-
-	if (msg_type == MSG_TYPE_DELAYED_RESP)
-		complete(xfer->async_done);
+		scmi_handle_notification(cinfo, msg_hdr);
 	else
-		complete(&xfer->done);
+		scmi_handle_xfer_delayed_resp(cinfo, MSG_XTRACT_TOKEN(msg_hdr),
+					      msg_type);
+
 }
 
 /**