Squashandroid-20111012

Change-Id: If0ae9fa8067740ab2ede33703c79ec134f204a5e

1 files changed, 1942 insertions, 691 deletions

diff --git a/drivers/dma/ste_dma40.c b/drivers/dma/ste_dma40.c
index c426829f6ab..27a7debc6fa 100644
--- a/drivers/dma/ste_dma40.c
+++ b/drivers/dma/ste_dma40.c
@@ -1,24 +1,33 @@
 /*
- * driver/dma/ste_dma40.c
- *
- * Copyright (C) ST-Ericsson 2007-2010
+ * Copyright (C) ST-Ericsson SA 2007-2010
+ * Author: Per Friden <per.friden@stericsson.com> for ST-Ericsson
+ * Author: Jonas Aaberg <jonas.aberg@stericsson.com> for ST-Ericsson
  * License terms: GNU General Public License (GPL) version 2
- * Author: Per Friden <per.friden@stericsson.com>
- * Author: Jonas Aaberg <jonas.aberg@stericsson.com>
- *
  */
 
 #include <linux/kernel.h>
-#include <linux/slab.h>
 #include <linux/dmaengine.h>
 #include <linux/platform_device.h>
 #include <linux/clk.h>
 #include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/version.h>
+#include <linux/pm.h>
+#include <linux/err.h>
 
 #include <plat/ste_dma40.h>
 
+#include <mach/regulator.h>
+
 #include "ste_dma40_ll.h"
 
+#ifdef CONFIG_STE_DMA40_DEBUG
+#include "ste_dma40_debug.h"
+#define MARK sted40_history_text((char *)__func__)
+#else
+#define MARK
+#endif
+
 #define D40_NAME "dma40"
 
 #define D40_PHY_CHAN -1
@@ -30,14 +39,21 @@
 /* Maximum iterations taken before giving up suspending a channel */
 #define D40_SUSPEND_MAX_IT 500
 
+/* Hardware requirement on LCLA alignment */
+#define LCLA_ALIGNMENT 0x40000
+
+/* Max number of links per event group */
+#define D40_LCLA_LINK_PER_EVENT_GRP 128
+#define D40_LCLA_END D40_LCLA_LINK_PER_EVENT_GRP
+
+/* Attempts before giving up to trying to get pages that are aligned */
+#define MAX_LCLA_ALLOC_ATTEMPTS 256
+
+/* Bit markings for allocation map */
 #define D40_ALLOC_FREE		(1 << 31)
 #define D40_ALLOC_PHY		(1 << 30)
 #define D40_ALLOC_LOG_FREE	0
 
-/* The number of free d40_desc to keep in memory before starting
- * to kfree() them */
-#define D40_DESC_CACHE_SIZE 50
-
 /* Hardware designer of the block */
 #define D40_PERIPHID2_DESIGNER 0x8
 
@@ -56,6 +72,51 @@ enum d40_command {
 	D40_DMA_SUSPENDED	= 3
 };
 
+/*
+ * These are the registers that has to be saved and later restored
+ * when the DMA hw is powered off.
+ * TODO: Add save/restore of D40_DREG_GCC on dma40 v3 or later, if that works.
+ */
+static u32 d40_backup_regs[] = {
+	D40_DREG_LCPA,
+	D40_DREG_LCLA,
+	D40_DREG_PRMSE,
+	D40_DREG_PRMSO,
+	D40_DREG_PRMOE,
+	D40_DREG_PRMOO,
+};
+
+/* TODO: Check if all these registers have to be saved/restored on dma40 v3 */
+static u32 d40_backup_regs_v3[] = {
+	D40_DREG_PSEG1,
+	D40_DREG_PSEG2,
+	D40_DREG_PSEG3,
+	D40_DREG_PSEG4,
+	D40_DREG_PCEG1,
+	D40_DREG_PCEG2,
+	D40_DREG_PCEG3,
+	D40_DREG_PCEG4,
+	D40_DREG_RSEG1,
+	D40_DREG_RSEG2,
+	D40_DREG_RSEG3,
+	D40_DREG_RSEG4,
+	D40_DREG_RCEG1,
+	D40_DREG_RCEG2,
+	D40_DREG_RCEG3,
+	D40_DREG_RCEG4,
+};
+
+static u32 d40_backup_regs_chan[] = {
+	D40_CHAN_REG_SSCFG,
+	D40_CHAN_REG_SSELT,
+	D40_CHAN_REG_SSPTR,
+	D40_CHAN_REG_SSLNK,
+	D40_CHAN_REG_SDCFG,
+	D40_CHAN_REG_SDELT,
+	D40_CHAN_REG_SDPTR,
+	D40_CHAN_REG_SDLNK,
+};
+
 /**
  * struct d40_lli_pool - Structure for keeping LLIs in memory
  *
@@ -68,9 +129,9 @@ enum d40_command {
  */
 struct d40_lli_pool {
 	void	*base;
-	int	size;
+	int	 size;
 	/* Space for dst and src, plus an extra for padding */
-	u8	pre_alloc_lli[3 * sizeof(struct d40_phy_lli)];
+	u8	 pre_alloc_lli[3 * sizeof(struct d40_phy_lli)];
 };
 
 /**
@@ -80,54 +141,58 @@ struct d40_lli_pool {
  * points into the lli_pool, to base if lli_len > 1 or to pre_alloc_lli if
  * lli_len equals one.
  * @lli_log: Same as above but for logical channels.
+ * @last_lcla: lcla used for last link (logical channels)
  * @lli_pool: The pool with two entries pre-allocated.
- * @lli_len: Number of LLI's in lli_pool
- * @lli_tcount: Number of LLIs processed in the transfer. When equals lli_len
- * then this transfer job is done.
+ * @lli_len: Number of llis of current descriptor.
+ * @lli_current: Number of transfered llis.
+ * @lcla_alloc: Number of LCLA entries allocated.
  * @txd: DMA engine struct. Used for among other things for communication
  * during a transfer.
  * @node: List entry.
- * @dir: The transfer direction of this job.
  * @is_in_client_list: true if the client owns this descriptor.
+ * @is_hw_linked: true if this job will automatically be continued for
+ * the previous one.
+ * @cyclic: true if this is a cyclic job
  *
  * This descriptor is used for both logical and physical transfers.
  */
-
 struct d40_desc {
 	/* LLI physical */
 	struct d40_phy_lli_bidir	 lli_phy;
 	/* LLI logical */
 	struct d40_log_lli_bidir	 lli_log;
+	struct d40_log_lli		*last_lcla;
 
 	struct d40_lli_pool		 lli_pool;
-	u32				 lli_len;
-	u32				 lli_tcount;
+	int				 lli_len;
+	int				 lli_current;
+	int				 lcla_alloc;
 
 	struct dma_async_tx_descriptor	 txd;
 	struct list_head		 node;
 
-	enum dma_data_direction		 dir;
 	bool				 is_in_client_list;
+	bool				 is_hw_linked;
+	bool				 cyclic;
 };
 
 /**
  * struct d40_lcla_pool - LCLA pool settings and data.
  *
- * @base: The virtual address of LCLA.
- * @phy: Physical base address of LCLA.
- * @base_size: size of lcla.
+ * @base: The virtual address of LCLA. 18 bit aligned.
+ * @base_unaligned: The orignal kmalloc pointer, if kmalloc is used.
+ * This pointer is only there for clean-up on error.
+ * @pages: The number of pages needed for all physical channels.
+ * Only used later for clean-up on error
  * @lock: Lock to protect the content in this struct.
- * @alloc_map: Mapping between physical channel and LCLA entries.
- * @num_blocks: The number of entries of alloc_map. Equals to the
- * number of physical channels.
+ * @alloc_map: big map over which LCLA entry is own by which job.
  */
 struct d40_lcla_pool {
 	void		*base;
-	dma_addr_t	 phy;
-	resource_size_t  base_size;
+	void		*base_unaligned;
+	int		 pages;
 	spinlock_t	 lock;
-	u32		*alloc_map;
-	int		 num_blocks;
+	struct d40_desc	**alloc_map;
 };
 
 /**
@@ -135,17 +200,17 @@ struct d40_lcla_pool {
  * channels.
  *
  * @lock: A lock protection this entity.
+ * @reserved: True if used by secure world or otherwise.
  * @num: The physical channel number of this entity.
  * @allocated_src: Bit mapped to show which src event line's are mapped to
  * this physical channel. Can also be free or physically allocated.
  * @allocated_dst: Same as for src but is dst.
  * allocated_dst and allocated_src uses the D40_ALLOC* defines as well as
- * event line number. Both allocated_src and allocated_dst can not be
- * allocated to a physical channel, since the interrupt handler has then
- * no way of figure out which one the interrupt belongs to.
+ * event line number.
  */
 struct d40_phy_res {
 	spinlock_t lock;
+	bool	   reserved;
 	int	   num;
 	u32	   allocated_src;
 	u32	   allocated_dst;
@@ -163,22 +228,25 @@ struct d40_base;
  * @pending_tx: The number of pending transfers. Used between interrupt handler
  * and tasklet.
  * @busy: Set to true when transfer is ongoing on this channel.
- * @phy_chan: Pointer to physical channel which this instance runs on.
+ * @phy_chan: Pointer to physical channel which this instance runs on. If this
+ * point is NULL, then the channel is not allocated.
  * @chan: DMA engine handle.
  * @tasklet: Tasklet that gets scheduled from interrupt context to complete a
  * transfer and call client callback.
  * @client: Cliented owned descriptor list.
  * @active: Active descriptor.
+ * @done: Completed jobs
  * @queue: Queued jobs.
- * @free: List of free descripts, ready to be reused.
- * @free_len: Number of descriptors in the free list.
  * @dma_cfg: The client configuration of this dma channel.
+ * @configured: whether the dma_cfg configuration is valid
  * @base: Pointer to the device instance struct.
+ * @cdesc: Cyclic descriptor
  * @src_def_cfg: Default cfg register setting for src.
  * @dst_def_cfg: Default cfg register setting for dst.
  * @log_def: Default logical channel settings.
- * @lcla: Space for one dst src pair for logical channel transfers.
  * @lcpa: Pointer to dst and src lcpa settings.
+ * @src_dev_addr: device source address for the channel transfer.
+ * @dst_dev_addr: device destination address for the channel transfer.
  *
  * This struct can either "be" a logical or a physical channel.
  */
@@ -194,17 +262,22 @@ struct d40_chan {
 	struct tasklet_struct		 tasklet;
 	struct list_head		 client;
 	struct list_head		 active;
+	struct list_head		 done;
 	struct list_head		 queue;
-	struct list_head		 free;
-	int				 free_len;
 	struct stedma40_chan_cfg	 dma_cfg;
+	bool				 configured;
 	struct d40_base			*base;
+	struct stedma40_cyclic_desc	*cdesc;
 	/* Default register configurations */
 	u32				 src_def_cfg;
 	u32				 dst_def_cfg;
 	struct d40_def_lcsp		 log_def;
-	struct d40_lcla_elem		 lcla;
 	struct d40_log_lli_full		*lcpa;
+	/* Runtime reconfiguration */
+	dma_addr_t			runtime_addr;
+	enum dma_data_direction		runtime_direction;
+	dma_addr_t			 src_dev_addr;
+	dma_addr_t			 dst_dev_addr;
 };
 
 /**
@@ -215,7 +288,9 @@ struct d40_chan {
  * the same physical register.
  * @dev: The device structure.
  * @virtbase: The virtual base address of the DMA's register.
+ * @rev: silicon revision detected.
  * @clk: Pointer to the DMA clock structure.
+ * @regulator: Pointer to the regulator struct that powers the DMA block.
  * @phy_start: Physical memory start of the DMA registers.
  * @phy_size: Size of the DMA register map.
  * @irq: The IRQ number.
@@ -240,13 +315,23 @@ struct d40_chan {
  * @lcpa_base: The virtual mapped address of LCPA.
  * @phy_lcpa: The physical address of the LCPA.
  * @lcpa_size: The size of the LCPA area.
+ * @desc_slab: cache for descriptors.
+ * @usage: The number of dma executions. Used by suspend to determite if
+ * the dma can suspend or not.
+ * @reg_val_backup: Here the values of some hardware registers are stored
+ * before the DMA is powered off. They are restored when the power is back on.
+ * @reg_val_backup_v3: Backup of registers that only exits on dma40 v3 and
+ * later.
+ * @reg_val_backup_chan: Backup data for standard channel parameter registers.
  */
 struct d40_base {
 	spinlock_t			 interrupt_lock;
 	spinlock_t			 execmd_lock;
 	struct device			 *dev;
 	void __iomem			 *virtbase;
+	u8				  rev:4;
 	struct clk			 *clk;
+	struct ux500_regulator		 *regulator;
 	phys_addr_t			  phy_start;
 	resource_size_t			  phy_size;
 	int				  irq;
@@ -266,6 +351,14 @@ struct d40_base {
 	void				 *lcpa_base;
 	dma_addr_t			  phy_lcpa;
 	resource_size_t			  lcpa_size;
+	struct kmem_cache		 *desc_slab;
+	int				  usage;
+	spinlock_t			  usage_lock;
+	u32				  reg_val_backup
+					  [ARRAY_SIZE(d40_backup_regs)];
+	u32				  reg_val_backup_v3
+					  [ARRAY_SIZE(d40_backup_regs_v3)];
+	u32				 *reg_val_backup_chan;
 };
 
 /**
@@ -295,6 +388,21 @@ struct d40_reg_val {
 	unsigned int val;
 };
 
+static struct device *chan2dev(struct d40_chan *d40c)
+{
+	return &d40c->chan.dev->device;
+}
+
+static bool chan_is_physical(struct d40_chan *chan)
+{
+	return chan->log_num == D40_PHY_CHAN;
+}
+
+static bool chan_is_logical(struct d40_chan *chan)
+{
+	return !chan_is_physical(chan);
+}
+
 static int d40_pool_lli_alloc(struct d40_desc *d40d,
 			      int lli_len, bool is_log)
 {
@@ -330,9 +438,6 @@ static int d40_pool_lli_alloc(struct d40_desc *d40d,
 					      align);
 		d40d->lli_phy.dst = PTR_ALIGN(d40d->lli_phy.src + lli_len,
 					      align);
-
-		d40d->lli_phy.src_addr = virt_to_phys(d40d->lli_phy.src);
-		d40d->lli_phy.dst_addr = virt_to_phys(d40d->lli_phy.dst);
 	}
 
 	return 0;
@@ -347,27 +452,67 @@ static void d40_pool_lli_free(struct d40_desc *d40d)
 	d40d->lli_log.dst = NULL;
 	d40d->lli_phy.src = NULL;
 	d40d->lli_phy.dst = NULL;
-	d40d->lli_phy.src_addr = 0;
-	d40d->lli_phy.dst_addr = 0;
+	d40d->last_lcla = NULL;
 }
 
-static dma_cookie_t d40_assign_cookie(struct d40_chan *d40c,
-				      struct d40_desc *desc)
+static int d40_lcla_alloc_one(struct d40_chan *d40c,
+			      struct d40_desc *d40d)
 {
-	dma_cookie_t cookie = d40c->chan.cookie;
+	unsigned long flags;
+	int i;
+	int ret = -EINVAL;
 
-	if (++cookie < 0)
-		cookie = 1;
+	spin_lock_irqsave(&d40c->base->lcla_pool.lock, flags);
 
-	d40c->chan.cookie = cookie;
-	desc->txd.cookie = cookie;
+	/*
+	 * Allocate both src and dst at the same time, therefore the half
+	 * start on 1 since 0 can't be used since zero is used as end marker.
+	 */
+	for (i = 1 ; i < D40_LCLA_LINK_PER_EVENT_GRP / 2; i++) {
+		int idx = d40c->phy_chan->num * D40_LCLA_LINK_PER_EVENT_GRP + i;
+
+		if (!d40c->base->lcla_pool.alloc_map[idx]) {
+			d40c->base->lcla_pool.alloc_map[idx] = d40d;
+			d40d->lcla_alloc++;
+			ret = i;
+			break;
+		}
+	}
+
+	spin_unlock_irqrestore(&d40c->base->lcla_pool.lock, flags);
 
-	return cookie;
+	return ret;
 }
 
-static void d40_desc_reset(struct d40_desc *d40d)
+static int d40_lcla_free_all(struct d40_chan *d40c,
+			     struct d40_desc *d40d)
 {
-	d40d->lli_tcount = 0;
+	unsigned long flags;
+	int i;
+	int ret = -EINVAL;
+
+	if (chan_is_physical(d40c))
+		return 0;
+
+	spin_lock_irqsave(&d40c->base->lcla_pool.lock, flags);
+
+	for (i = 1 ; i < D40_LCLA_LINK_PER_EVENT_GRP / 2; i++) {
+		int idx = d40c->phy_chan->num * D40_LCLA_LINK_PER_EVENT_GRP + i;
+
+		if (d40c->base->lcla_pool.alloc_map[idx] == d40d) {
+			d40c->base->lcla_pool.alloc_map[idx] = NULL;
+			d40d->lcla_alloc--;
+			if (d40d->lcla_alloc == 0) {
+				ret = 0;
+				break;
+			}
+		}
+	}
+
+	spin_unlock_irqrestore(&d40c->base->lcla_pool.lock, flags);
+
+	return ret;
+
 }
 
 static void d40_desc_remove(struct d40_desc *d40d)
@@ -377,45 +522,37 @@ static void d40_desc_remove(struct d40_desc *d40d)
 
 static struct d40_desc *d40_desc_get(struct d40_chan *d40c)
 {
-	struct d40_desc *desc;
-	struct d40_desc *d;
-	struct d40_desc *_d;
+	struct d40_desc *desc = NULL;
 
 	if (!list_empty(&d40c->client)) {
-		list_for_each_entry_safe(d, _d, &d40c->client, node)
+		struct d40_desc *d;
+		struct d40_desc *_d;
+
+		list_for_each_entry_safe(d, _d, &d40c->client, node) {
 			if (async_tx_test_ack(&d->txd)) {
 				d40_pool_lli_free(d);
 				d40_desc_remove(d);
 				desc = d;
-				goto out;
+				memset(desc, 0, sizeof(struct d40_desc));
+				break;
 			}
+		}
 	}
 
-	if (list_empty(&d40c->free)) {
-		/* Alloc new desc because we're out of used ones */
-		desc = kzalloc(sizeof(struct d40_desc), GFP_NOWAIT);
-		if (desc == NULL)
-			goto out;
+	if (!desc)
+		desc = kmem_cache_zalloc(d40c->base->desc_slab, GFP_NOWAIT);
+
+	if (desc)
 		INIT_LIST_HEAD(&desc->node);
-	} else {
-		/* Reuse an old desc. */
-		desc = list_first_entry(&d40c->free,
-					struct d40_desc,
-					node);
-		list_del(&desc->node);
-		d40c->free_len--;
-	}
-out:
+
 	return desc;
 }
 
 static void d40_desc_free(struct d40_chan *d40c, struct d40_desc *d40d)
 {
-	if (d40c->free_len < D40_DESC_CACHE_SIZE) {
-		list_add_tail(&d40d->node, &d40c->free);
-		d40c->free_len++;
-	} else
-		kfree(d40d);
+
+	d40_lcla_free_all(d40c, d40d);
+	kmem_cache_free(d40c->base->desc_slab, d40d);
 }
 
 static void d40_desc_submit(struct d40_chan *d40c, struct d40_desc *desc)
@@ -423,6 +560,112 @@ static void d40_desc_submit(struct d40_chan *d40c, struct d40_desc *desc)
 	list_add_tail(&desc->node, &d40c->active);
 }
 
+static void d40_desc_done(struct d40_chan *d40c, struct d40_desc *desc)
+{
+	list_add_tail(&desc->node, &d40c->done);
+}
+
+static int d40_desc_log_lli_to_lcxa(struct d40_chan *d40c,
+				    struct d40_desc *d40d,
+				    bool use_lcpa)
+{
+	struct d40_log_lli_bidir *lli = &d40d->lli_log;
+	int curr_lcla = -EINVAL;
+	int first_lcla = 0;
+
+	if ((d40d->lli_len - d40d->lli_current) > 1 ||
+	     d40d->cyclic || !use_lcpa) {
+
+		curr_lcla = d40_lcla_alloc_one(d40c, d40d);
+		first_lcla = curr_lcla;
+	}
+
+	if (!d40d->cyclic && use_lcpa) {
+		d40_log_lli_lcpa_write(d40c->lcpa,
+				       &lli->dst[d40d->lli_current],
+				       &lli->src[d40d->lli_current],
+				       curr_lcla,
+				       curr_lcla == -EINVAL);
+
+		d40d->lli_current++;
+	}
+
+	/*
+	 * Run only in LCPA space for non-cyclic.  For cyclic, caller
+	 * will handle the error.
+	 */
+	if (first_lcla < 0)
+		return first_lcla;
+
+	for (; d40d->lli_current < d40d->lli_len; d40d->lli_current++) {
+		int lli_current = d40d->lli_current;
+		struct d40_log_lli *lcla;
+		int next_lcla;
+		bool interrupt;
+
+		if (d40d->lli_current + 1 < d40d->lli_len)
+			next_lcla = d40_lcla_alloc_one(d40c, d40d);
+		else
+			next_lcla = d40d->cyclic ? first_lcla : -EINVAL;
+
+		interrupt = d40d->cyclic
+			    ? d40d->txd.flags & DMA_PREP_INTERRUPT
+			    : next_lcla == -EINVAL;
+
+		if (d40d->cyclic && curr_lcla == first_lcla) {
+			/*
+			 * For cyclic transactions, the first link is
+			 * present in both LCPA and LCLA space because
+			 * we can't link back to the one in LCPA space.
+			 */
+			d40_log_lli_lcpa_write(d40c->lcpa,
+					       &lli->dst[lli_current],
+					       &lli->src[lli_current],
+					       next_lcla,
+					       interrupt);
+		}
+
+		lcla = d40c->base->lcla_pool.base +
+			d40c->phy_chan->num * 1024 +
+			8 * curr_lcla * 2;
+
+		d40_log_lli_lcla_write(lcla,
+				       &lli->dst[lli_current],
+				       &lli->src[lli_current],
+				       next_lcla,
+				       interrupt);
+
+		if (d40d->lli_current == d40d->lli_len - 1)
+			d40d->last_lcla = lcla;
+
+		(void) dma_map_single(d40c->base->dev, lcla,
+				      2 * sizeof(struct d40_log_lli),
+				      DMA_TO_DEVICE);
+
+		curr_lcla = next_lcla;
+
+		if (curr_lcla == -EINVAL || curr_lcla == first_lcla) {
+			d40d->lli_current++;
+			break;
+		}
+
+	}
+
+	return first_lcla;
+}
+
+static void d40_desc_load(struct d40_chan *d40c, struct d40_desc *d40d)
+{
+	if (chan_is_physical(d40c)) {
+		d40_phy_lli_write(d40c->base->virtbase,
+				  d40c->phy_chan->num,
+				  d40d->lli_phy.dst,
+				  d40d->lli_phy.src);
+		d40d->lli_current = d40d->lli_len;
+	} else
+		(void) d40_desc_log_lli_to_lcxa(d40c, d40d, true);
+}
+
 static struct d40_desc *d40_first_active_get(struct d40_chan *d40c)
 {
 	struct d40_desc *d;
@@ -454,82 +697,194 @@ static struct d40_desc *d40_first_queued(struct d40_chan *d40c)
 	return d;
 }
 
-/* Support functions for logical channels */
+static struct d40_desc *d40_first_done(struct d40_chan *d40c)
+{
+	if (list_empty(&d40c->done))
+		return NULL;
+
+	return list_first_entry(&d40c->done, struct d40_desc, node);
+}
+
 
-static int d40_lcla_id_get(struct d40_chan *d40c,
-			   struct d40_lcla_pool *pool)
+#ifdef CONFIG_PM
+static void d40_save_restore_registers(struct d40_base *base, bool save)
 {
-	int src_id = 0;
-	int dst_id = 0;
-	struct d40_log_lli *lcla_lidx_base =
-		pool->base + d40c->phy_chan->num * 1024;
 	int i;
-	int lli_per_log = d40c->base->plat_data->llis_per_log;
+	int j;
+	int idx;
+	void __iomem *addr;
 
-	if (d40c->lcla.src_id >= 0 && d40c->lcla.dst_id >= 0)
-		return 0;
+	/* Enable all clocks -- revisit after HW bug is fixed */
+	if (!save)
+		writel(D40_DREG_GCC_ENABLE_ALL, base->virtbase + D40_DREG_GCC);
 
-	if (pool->num_blocks > 32)
-		return -EINVAL;
+	/* Save/Restore channel specific registers */
+	for (i = 0; i < base->num_phy_chans; i++) {
+		if (base->phy_res[i].reserved)
+			continue;
 
-	spin_lock(&pool->lock);
+		addr = base->virtbase + D40_DREG_PCBASE + i * D40_DREG_PCDELTA;
+		idx = i * ARRAY_SIZE(d40_backup_regs_chan);
 
-	for (i = 0; i < pool->num_blocks; i++) {
-		if (!(pool->alloc_map[d40c->phy_chan->num] & (0x1 << i))) {
-			pool->alloc_map[d40c->phy_chan->num] |= (0x1 << i);
-			break;
+		for (j = 0; j < ARRAY_SIZE(d40_backup_regs_chan); j++) {
+			if (save)
+				base->reg_val_backup_chan[idx + j] =
+					readl(addr + d40_backup_regs_chan[j]);
+			else
+				writel(base->reg_val_backup_chan[idx + j],
+				       addr + d40_backup_regs_chan[j]);
 		}
 	}
-	src_id = i;
-	if (src_id >= pool->num_blocks)
-		goto err;
 
-	for (; i < pool->num_blocks; i++) {
-		if (!(pool->alloc_map[d40c->phy_chan->num] & (0x1 << i))) {
-			pool->alloc_map[d40c->phy_chan->num] |= (0x1 << i);
-			break;
+	/* Save/Restore global registers */
+	for (i = 0 ; i < ARRAY_SIZE(d40_backup_regs); i++) {
+		addr = base->virtbase + d40_backup_regs[i];
+
+		if (save)
+			base->reg_val_backup[i] = readl(addr);
+		else
+			writel(base->reg_val_backup[i], addr);
+	}
+
+	/* Save/Restore registers only existing on dma40 v3 and later */
+	if (base->rev >= 3) {
+		for (i = 0 ; i < ARRAY_SIZE(d40_backup_regs_v3); i++) {
+			addr = base->virtbase + d40_backup_regs_v3[i];
+
+			if (save)
+				base->reg_val_backup_v3[i] = readl(addr);
+			else
+				writel(base->reg_val_backup_v3[i], addr);
 		}
 	}
+}
+#else
+static void d40_save_restore_registers(struct d40_base *base, bool save)
+{
+}
+#endif
 
-	dst_id = i;
-	if (dst_id == src_id)
-		goto err;
+static void d40_power_off(struct d40_base *base, int phy_num)
+{
+	u32 gcc;
+	int i;
+	int j;
+	int p;
+
+	/*
+	 * Disable the rest of the code because of GCC register HW bugs on v1
+	 * which are not worth working around.  Revisit later.
+	 */
+	return;
+
+	/*
+	 * Power off event group related to physical channel, if
+	 * the other physical channels that belong to the same
+	 * event group are not in use
+	 */
 
-	d40c->lcla.src_id = src_id;
-	d40c->lcla.dst_id = dst_id;
-	d40c->lcla.dst = lcla_lidx_base + dst_id * lli_per_log + 1;
-	d40c->lcla.src = lcla_lidx_base + src_id * lli_per_log + 1;
+	for (j = 0; j < base->num_phy_chans; j += D40_GROUP_SIZE) {
 
+		for (i = 0; i < 2; i++) {
+			p = (((phy_num & (base->num_phy_chans - 1)) + i)
+			     & (D40_GROUP_SIZE - 1)) + j;
+			if (p == phy_num)
+				continue;
+			/*
+			 * If another physical channel in the same group is
+			 * allocated, just return.
+			 */
+			if (base->phy_res[p].allocated_dst == D40_ALLOC_PHY ||
+			    base->phy_res[p].allocated_src == D40_ALLOC_PHY) {
+				return;
+			}
+		}
+	}
 
-	spin_unlock(&pool->lock);
-	return 0;
-err:
-	spin_unlock(&pool->lock);
-	return -EINVAL;
+	/* The GCC register is protected via the usage_lock */
+	gcc = readl(base->virtbase + D40_DREG_GCC);
+
+	gcc &= ~D40_DREG_GCC_EVTGRP_ENA(D40_PHYS_TO_GROUP(phy_num),
+					D40_DREG_GCC_SRC);
+	gcc &= ~D40_DREG_GCC_EVTGRP_ENA(D40_PHYS_TO_GROUP(phy_num),
+					D40_DREG_GCC_DST);
+
+	writel(gcc, base->virtbase + D40_DREG_GCC);
 }
 
-static void d40_lcla_id_put(struct d40_chan *d40c,
-			    struct d40_lcla_pool *pool,
-			    int id)
+static void d40_power_on(struct d40_base *base, int phy_num)
 {
-	if (id < 0)
-		return;
+	u32 gcc;
 
-	d40c->lcla.src_id = -1;
-	d40c->lcla.dst_id = -1;
+	/*
+	 * Disable the rest of the code because of GCC register HW bugs on v1
+	 * which are not worth working around.  Revisit later.
+	 */
+	return;
 
-	spin_lock(&pool->lock);
-	pool->alloc_map[d40c->phy_chan->num] &= (~(0x1 << id));
-	spin_unlock(&pool->lock);
+	/* The GCC register is protected via the usage_lock */
+	gcc = readl(base->virtbase + D40_DREG_GCC);
+
+	gcc |= D40_DREG_GCC_EVTGRP_ENA(D40_PHYS_TO_GROUP(phy_num),
+				       D40_DREG_GCC_SRC);
+	gcc |= D40_DREG_GCC_EVTGRP_ENA(D40_PHYS_TO_GROUP(phy_num),
+				       D40_DREG_GCC_DST);
+
+	writel(gcc, base->virtbase + D40_DREG_GCC);
 }
 
-static int d40_channel_execute_command(struct d40_chan *d40c,
-				       enum d40_command command)
+static void d40_usage_inc(struct d40_chan *d40c)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&d40c->base->usage_lock, flags);
+
+	d40c->base->usage++;
+
+	if (d40c->base->usage == 1) {
+		if (d40c->base->regulator)
+			ux500_regulator_atomic_enable(d40c->base->regulator);
+		d40_save_restore_registers(d40c->base, false);
+	}
+	d40_power_on(d40c->base, d40c->phy_chan->num);
+
+	spin_unlock_irqrestore(&d40c->base->usage_lock, flags);
+}
+
+static void d40_usage_dec(struct d40_chan *d40c)
 {
-	int status, i;
+	unsigned long flags;
+
+	spin_lock_irqsave(&d40c->base->usage_lock, flags);
+
+	d40_power_off(d40c->base, d40c->phy_chan->num);
+
+	d40c->base->usage--;
+
+	if (d40c->base->usage == 0) {
+		d40_save_restore_registers(d40c->base, true);
+		if (d40c->base->regulator)
+			ux500_regulator_atomic_disable(d40c->base->regulator);
+	}
+
+	spin_unlock_irqrestore(&d40c->base->usage_lock, flags);
+}
+
+static int __d40_execute_command_phy(struct d40_chan *d40c,
+				     enum d40_command command)
+{
+	u32 status;
+	int i;
 	void __iomem *active_reg;
 	int ret = 0;
 	unsigned long flags;
+	u32 wmask;
+
+	if (command == D40_DMA_STOP) {
+		ret = __d40_execute_command_phy(d40c, D40_DMA_SUSPEND_REQ);
+		if (ret)
+			return ret;
+	}
 
 	spin_lock_irqsave(&d40c->base->execmd_lock, flags);
 
@@ -547,7 +902,14 @@ static int d40_channel_execute_command(struct d40_chan *d40c,
 			goto done;
 	}
 
-	writel(command << D40_CHAN_POS(d40c->phy_chan->num), active_reg);
+#ifdef CONFIG_STE_DMA40_DEBUG
+	if (command == D40_DMA_RUN)
+		sted40_history_snapshot();
+#endif
+
+	wmask = 0xffffffff & ~(D40_CHAN_POS_MASK(d40c->phy_chan->num));
+	writel(wmask | (command << D40_CHAN_POS(d40c->phy_chan->num)),
+	       active_reg);
 
 	if (command == D40_DMA_SUSPEND_REQ) {
 
@@ -573,6 +935,9 @@ static int d40_channel_execute_command(struct d40_chan *d40c,
 				"[%s]: unable to suspend the chl %d (log: %d) status %x\n",
 				__func__, d40c->phy_chan->num, d40c->log_num,
 				status);
+#ifdef CONFIG_STE_DMA40_DEBUG
+			sted40_history_dump();
+#endif
 			dump_stack();
 			ret = -EBUSY;
 		}
@@ -586,135 +951,195 @@ done:
 static void d40_term_all(struct d40_chan *d40c)
 {
 	struct d40_desc *d40d;
-	struct d40_desc *d;
-	struct d40_desc *_d;
+
+	/* Release completed descriptors */
+	while ((d40d = d40_first_done(d40c))) {
+		d40_desc_remove(d40d);
+		d40_desc_free(d40c, d40d);
+	}
 
 	/* Release active descriptors */
 	while ((d40d = d40_first_active_get(d40c))) {
 		d40_desc_remove(d40d);
-
-		/* Return desc to free-list */
 		d40_desc_free(d40c, d40d);
 	}
 
 	/* Release queued descriptors waiting for transfer */
 	while ((d40d = d40_first_queued(d40c))) {
 		d40_desc_remove(d40d);
-
-		/* Return desc to free-list */
 		d40_desc_free(d40c, d40d);
 	}
 
-	/* Release client owned descriptors */
-	if (!list_empty(&d40c->client))
-		list_for_each_entry_safe(d, _d, &d40c->client, node) {
-			d40_pool_lli_free(d);
-			d40_desc_remove(d);
-			/* Return desc to free-list */
-			d40_desc_free(d40c, d40d);
-		}
-
-	d40_lcla_id_put(d40c, &d40c->base->lcla_pool,
-			d40c->lcla.src_id);
-	d40_lcla_id_put(d40c, &d40c->base->lcla_pool,
-			d40c->lcla.dst_id);
-
 	d40c->pending_tx = 0;
-	d40c->busy = false;
 }
 
-static void d40_config_set_event(struct d40_chan *d40c, bool do_enable)
+static void __d40_config_set_event(struct d40_chan *d40c, bool enable,
+				   u32 event, int reg)
 {
-	u32 val;
-	unsigned long flags;
+	void __iomem *addr = d40c->base->virtbase + D40_DREG_PCBASE
+			     + d40c->phy_chan->num * D40_DREG_PCDELTA + reg;
+	int tries;
 
-	if (do_enable)
-		val = D40_ACTIVATE_EVENTLINE;
-	else
-		val = D40_DEACTIVATE_EVENTLINE;
+	if (!enable) {
+		writel((D40_DEACTIVATE_EVENTLINE << D40_EVENTLINE_POS(event))
+		       | ~D40_EVENTLINE_MASK(event), addr);
+		return;
+	}
 
-	spin_lock_irqsave(&d40c->phy_chan->lock, flags);
+	/*
+	 * The hardware sometimes doesn't register the enable when src and dst
+	 * event lines are active on the same logical channel.  Retry to ensure
+	 * it does.  Usually only one retry is sufficient.
+	 */
+	tries = 100;
+	while (--tries) {
+		writel((D40_ACTIVATE_EVENTLINE << D40_EVENTLINE_POS(event))
+		       | ~D40_EVENTLINE_MASK(event), addr);
 
+		if (readl(addr) & D40_EVENTLINE_MASK(event))
+			break;
+	}
+
+	if (tries != 99)
+		dev_dbg(chan2dev(d40c),
+			"[%s] workaround enable S%cLNK (%d tries)\n",
+			__func__, reg == D40_CHAN_REG_SSLNK ? 'S' : 'D',
+			100 - tries);
+
+	WARN_ON(!tries);
+}
+
+static void d40_config_set_event(struct d40_chan *d40c, bool do_enable)
+{
 	/* Enable event line connected to device (or memcpy) */
 	if ((d40c->dma_cfg.dir ==  STEDMA40_PERIPH_TO_MEM) ||
 	    (d40c->dma_cfg.dir == STEDMA40_PERIPH_TO_PERIPH)) {
 		u32 event = D40_TYPE_TO_EVENT(d40c->dma_cfg.src_dev_type);
 
-		writel((val << D40_EVENTLINE_POS(event)) |
-		       ~D40_EVENTLINE_MASK(event),
-		       d40c->base->virtbase + D40_DREG_PCBASE +
-		       d40c->phy_chan->num * D40_DREG_PCDELTA +
-		       D40_CHAN_REG_SSLNK);
+		__d40_config_set_event(d40c, do_enable, event,
+				       D40_CHAN_REG_SSLNK);
 	}
+
 	if (d40c->dma_cfg.dir !=  STEDMA40_PERIPH_TO_MEM) {
 		u32 event = D40_TYPE_TO_EVENT(d40c->dma_cfg.dst_dev_type);
 
-		writel((val << D40_EVENTLINE_POS(event)) |
-		       ~D40_EVENTLINE_MASK(event),
-		       d40c->base->virtbase + D40_DREG_PCBASE +
-		       d40c->phy_chan->num * D40_DREG_PCDELTA +
-		       D40_CHAN_REG_SDLNK);
+		__d40_config_set_event(d40c, do_enable, event,
+				       D40_CHAN_REG_SDLNK);
 	}
-
-	spin_unlock_irqrestore(&d40c->phy_chan->lock, flags);
 }
 
 static u32 d40_chan_has_events(struct d40_chan *d40c)
 {
-	u32 val = 0;
+	u32 val;
 
-	/* If SSLNK or SDLNK is zero all events are disabled */
-	if ((d40c->dma_cfg.dir ==  STEDMA40_PERIPH_TO_MEM) ||
-	    (d40c->dma_cfg.dir == STEDMA40_PERIPH_TO_PERIPH))
-		val = readl(d40c->base->virtbase + D40_DREG_PCBASE +
-			    d40c->phy_chan->num * D40_DREG_PCDELTA +
-			    D40_CHAN_REG_SSLNK);
-
-	if (d40c->dma_cfg.dir !=  STEDMA40_PERIPH_TO_MEM)
-		val = readl(d40c->base->virtbase + D40_DREG_PCBASE +
-			    d40c->phy_chan->num * D40_DREG_PCDELTA +
-			    D40_CHAN_REG_SDLNK);
+	val = readl(d40c->base->virtbase + D40_DREG_PCBASE +
+		    d40c->phy_chan->num * D40_DREG_PCDELTA +
+		    D40_CHAN_REG_SSLNK);
+
+	val |= readl(d40c->base->virtbase + D40_DREG_PCBASE +
+		     d40c->phy_chan->num * D40_DREG_PCDELTA +
+		     D40_CHAN_REG_SDLNK);
 	return val;
 }
 
-static void d40_config_enable_lidx(struct d40_chan *d40c)
+static int
+__d40_execute_command_log(struct d40_chan *d40c, enum d40_command command)
+{
+	unsigned long flags;
+	int ret = 0;
+
+	spin_lock_irqsave(&d40c->phy_chan->lock, flags);
+
+	switch (command) {
+	case D40_DMA_STOP:
+	case D40_DMA_SUSPEND_REQ:
+		ret = __d40_execute_command_phy(d40c, D40_DMA_SUSPEND_REQ);
+		if (ret)
+			goto out;
+
+		d40_config_set_event(d40c, false);
+
+		if (d40_chan_has_events(d40c))
+			ret = __d40_execute_command_phy(d40c, D40_DMA_RUN);
+		else if (command == D40_DMA_STOP)
+			ret = __d40_execute_command_phy(d40c, command);
+		break;
+
+	case D40_DMA_RUN:
+		if (d40c->base->rev == 0) {
+			ret = __d40_execute_command_phy(d40c,
+							D40_DMA_SUSPEND_REQ);
+			if (ret)
+				goto out;
+		}
+
+		d40_config_set_event(d40c, true);
+
+		ret = __d40_execute_command_phy(d40c, command);
+		break;
+
+	case D40_DMA_SUSPENDED:
+		BUG();
+		break;
+	}
+
+out:
+	spin_unlock_irqrestore(&d40c->phy_chan->lock, flags);
+	return ret;
+}
+
+static int d40_channel_execute_command(struct d40_chan *d40c,
+				       enum d40_command command)
+{
+	if (chan_is_logical(d40c))
+		return __d40_execute_command_log(d40c, command);
+	else
+		return __d40_execute_command_phy(d40c, command);
+}
+
+static u32 d40_get_prmo(struct d40_chan *d40c)
 {
-	/* Set LIDX for lcla */
-	writel((d40c->phy_chan->num << D40_SREG_ELEM_LOG_LIDX_POS) &
-	       D40_SREG_ELEM_LOG_LIDX_MASK,
-	       d40c->base->virtbase + D40_DREG_PCBASE +
-	       d40c->phy_chan->num * D40_DREG_PCDELTA + D40_CHAN_REG_SDELT);
+	static const unsigned int phy_map[] = {
+		[STEDMA40_PCHAN_BASIC_MODE]
+			= D40_DREG_PRMO_PCHAN_BASIC,
+		[STEDMA40_PCHAN_MODULO_MODE]
+			= D40_DREG_PRMO_PCHAN_MODULO,
+		[STEDMA40_PCHAN_DOUBLE_DST_MODE]
+			= D40_DREG_PRMO_PCHAN_DOUBLE_DST,
+	};
+	static const unsigned int log_map[] = {
+		[STEDMA40_LCHAN_SRC_PHY_DST_LOG]
+			= D40_DREG_PRMO_LCHAN_SRC_PHY_DST_LOG,
+		[STEDMA40_LCHAN_SRC_LOG_DST_PHY]
+			= D40_DREG_PRMO_LCHAN_SRC_LOG_DST_PHY,
+		[STEDMA40_LCHAN_SRC_LOG_DST_LOG]
+			= D40_DREG_PRMO_LCHAN_SRC_LOG_DST_LOG,
+	};
 
-	writel((d40c->phy_chan->num << D40_SREG_ELEM_LOG_LIDX_POS) &
-	       D40_SREG_ELEM_LOG_LIDX_MASK,
-	       d40c->base->virtbase + D40_DREG_PCBASE +
-	       d40c->phy_chan->num * D40_DREG_PCDELTA + D40_CHAN_REG_SSELT);
+	if (chan_is_physical(d40c))
+		return phy_map[d40c->dma_cfg.mode_opt];
+	else
+		return log_map[d40c->dma_cfg.mode_opt];
 }
 
-static int d40_config_write(struct d40_chan *d40c)
+static void d40_config_write(struct d40_chan *d40c)
 {
 	u32 addr_base;
 	u32 var;
-	int res;
-
-	res = d40_channel_execute_command(d40c, D40_DMA_SUSPEND_REQ);
-	if (res)
-		return res;
 
 	/* Odd addresses are even addresses + 4 */
 	addr_base = (d40c->phy_chan->num % 2) * 4;
 	/* Setup channel mode to logical or physical */
-	var = ((u32)(d40c->log_num != D40_PHY_CHAN) + 1) <<
+	var = ((u32)(chan_is_logical(d40c)) + 1) <<
 		D40_CHAN_POS(d40c->phy_chan->num);
 	writel(var, d40c->base->virtbase + D40_DREG_PRMSE + addr_base);
 
 	/* Setup operational mode option register */
-	var = ((d40c->dma_cfg.channel_type >> STEDMA40_INFO_CH_MODE_OPT_POS) &
-	       0x3) << D40_CHAN_POS(d40c->phy_chan->num);
+	var = d40_get_prmo(d40c) << D40_CHAN_POS(d40c->phy_chan->num);
 
 	writel(var, d40c->base->virtbase + D40_DREG_PRMOE + addr_base);
 
-	if (d40c->log_num != D40_PHY_CHAN) {
+	if (chan_is_logical(d40c)) {
 		/* Set default config for CFG reg */
 		writel(d40c->src_def_cfg,
 		       d40c->base->virtbase + D40_DREG_PCBASE +
@@ -725,38 +1150,292 @@ static int d40_config_write(struct d40_chan *d40c)
 		       d40c->phy_chan->num * D40_DREG_PCDELTA +
 		       D40_CHAN_REG_SDCFG);
 
-		d40_config_enable_lidx(d40c);
+		/* Set LIDX for lcla */
+		writel((d40c->phy_chan->num << D40_SREG_ELEM_LOG_LIDX_POS) &
+		       D40_SREG_ELEM_LOG_LIDX_MASK,
+		       d40c->base->virtbase + D40_DREG_PCBASE +
+		       d40c->phy_chan->num * D40_DREG_PCDELTA +
+		       D40_CHAN_REG_SDELT);
+
+		writel((d40c->phy_chan->num << D40_SREG_ELEM_LOG_LIDX_POS) &
+		       D40_SREG_ELEM_LOG_LIDX_MASK,
+		       d40c->base->virtbase + D40_DREG_PCBASE +
+		       d40c->phy_chan->num * D40_DREG_PCDELTA +
+		       D40_CHAN_REG_SSELT);
+
+		/* Clear LNK which will be used by d40_chan_has_events() */
+		writel(0,
+		       d40c->base->virtbase + D40_DREG_PCBASE +
+		       d40c->phy_chan->num * D40_DREG_PCDELTA +
+		       D40_CHAN_REG_SSLNK);
+		writel(0,
+		       d40c->base->virtbase + D40_DREG_PCBASE +
+		       d40c->phy_chan->num * D40_DREG_PCDELTA +
+		       D40_CHAN_REG_SDLNK);
 	}
+}
+
+static u32 d40_residue(struct d40_chan *d40c)
+{
+	u32 num_elt;
+
+	if (chan_is_logical(d40c))
+		num_elt = (readl(&d40c->lcpa->lcsp2) &
+			   D40_MEM_LCSP2_ECNT_MASK) >> D40_MEM_LCSP2_ECNT_POS;
+	else
+		num_elt = (readl(d40c->base->virtbase + D40_DREG_PCBASE +
+				 d40c->phy_chan->num * D40_DREG_PCDELTA +
+				 D40_CHAN_REG_SDELT) &
+			   D40_SREG_ELEM_PHY_ECNT_MASK) >>
+			D40_SREG_ELEM_PHY_ECNT_POS;
+	return num_elt * (1 << d40c->dma_cfg.dst_info.data_width);
+}
+
+static bool d40_tx_is_linked(struct d40_chan *d40c)
+{
+	bool is_link;
+
+	if (chan_is_logical(d40c))
+		is_link = readl(&d40c->lcpa->lcsp3) &  D40_MEM_LCSP3_DLOS_MASK;
+	else
+		is_link = readl(d40c->base->virtbase + D40_DREG_PCBASE +
+				d40c->phy_chan->num * D40_DREG_PCDELTA +
+				D40_CHAN_REG_SDLNK) &
+			D40_SREG_LNK_PHYS_LNK_MASK;
+	return is_link;
+}
+
+static int d40_pause(struct dma_chan *chan)
+{
+	struct d40_chan *d40c =
+		container_of(chan, struct d40_chan, chan);
+	int res = 0;
+	unsigned long flags;
+
+	if (!d40c->busy)
+		return 0;
+
+	d40_usage_inc(d40c);
+	spin_lock_irqsave(&d40c->lock, flags);
+
+	res = d40_channel_execute_command(d40c, D40_DMA_SUSPEND_REQ);
+
+	d40_usage_dec(d40c);
+
+	spin_unlock_irqrestore(&d40c->lock, flags);
 	return res;
 }
 
-static void d40_desc_load(struct d40_chan *d40c, struct d40_desc *d40d)
+static int d40_resume(struct dma_chan *chan)
 {
+	struct d40_chan *d40c =
+		container_of(chan, struct d40_chan, chan);
+	int res = 0;
+	unsigned long flags;
 
-	if (d40d->lli_phy.dst && d40d->lli_phy.src) {
-		d40_phy_lli_write(d40c->base->virtbase,
-				  d40c->phy_chan->num,
-				  d40d->lli_phy.dst,
-				  d40d->lli_phy.src);
-		d40d->lli_tcount = d40d->lli_len;
-	} else if (d40d->lli_log.dst && d40d->lli_log.src) {
-		u32 lli_len;
-		struct d40_log_lli *src = d40d->lli_log.src;
-		struct d40_log_lli *dst = d40d->lli_log.dst;
+	if (!d40c->busy)
+		return 0;
 
-		src += d40d->lli_tcount;
-		dst += d40d->lli_tcount;
+	spin_lock_irqsave(&d40c->lock, flags);
 
-		if (d40d->lli_len <= d40c->base->plat_data->llis_per_log)
-			lli_len = d40d->lli_len;
-		else
-			lli_len = d40c->base->plat_data->llis_per_log;
-		d40d->lli_tcount += lli_len;
-		d40_log_lli_write(d40c->lcpa, d40c->lcla.src,
-				  d40c->lcla.dst,
-				  dst, src,
-				  d40c->base->plat_data->llis_per_log);
+	d40_usage_inc(d40c);
+
+	/* If bytes left to transfer or linked tx resume job */
+	if (d40_residue(d40c) || d40_tx_is_linked(d40c))
+		res = d40_channel_execute_command(d40c, D40_DMA_RUN);
+
+	d40_usage_dec(d40c);
+	spin_unlock_irqrestore(&d40c->lock, flags);
+	return res;
+}
+
+static bool __d40_tx_submit_link_log(struct d40_chan *d40c,
+				     struct d40_desc *d40d,
+				     struct d40_desc *prev)
+{
+	struct d40_log_lli *lastlcla = prev->last_lcla;
+	struct d40_log_lli_bidir *lli = &prev->lli_log;
+	int lastidx = prev->lli_len - 1;
+	int los;
+
+	los = d40_desc_log_lli_to_lcxa(d40c, d40d, false);
+	if (los < 0)
+		return false;
+
+	dma_sync_single_for_cpu(d40c->base->dev, virt_to_phys(lastlcla),
+				2 * sizeof(struct d40_log_lli),
+				DMA_TO_DEVICE);
+
+	d40_log_lli_lcla_write(lastlcla,
+			       &lli->dst[lastidx],
+			       &lli->src[lastidx],
+			       los,
+			       true);
+
+	dma_sync_single_for_device(d40c->base->dev, virt_to_phys(lastlcla),
+				   2 * sizeof(struct d40_log_lli),
+				   DMA_TO_DEVICE);
+
+	return true;
+}
+
+static bool d40_tx_submit_link_log(struct d40_chan *d40c,
+				   struct d40_desc *d40d,
+				   struct d40_desc *prev)
+{
+	struct d40_log_lli_bidir *lli = &prev->lli_log;
+	int lastidx = prev->lli_len - 1;
+	unsigned int secnt;
+	int active_slos;
+	int last_slos;
+
+	/*
+	 * An only-LCPA transfer will either be transmitting the last link or
+	 * be completed.
+	 */
+	if (!prev->last_lcla)
+		return false;
+
+	/* We don't support linking to incompletely linked transfers */
+	if (prev->lli_current != prev->lli_len)
+		return false;
+
+	/* A transfer must be ongoing. */
+	secnt = (d40c->lcpa->lcsp0 & D40_MEM_LCSP0_ECNT_MASK)
+		 >> D40_MEM_LCSP0_ECNT_POS;
+	if (!secnt) {
+		dev_vdbg(d40c->base->dev, "%s: secnt %u\n", __func__, secnt);
+		return false;
+	}
+
+	/*
+	 * We don't support linking while the last link is being transmitted.
+	 */
+	active_slos = (d40c->lcpa->lcsp1 & D40_MEM_LCSP1_SLOS_MASK)
+		       >> D40_MEM_LCSP1_SLOS_POS;
+	last_slos = (lli->src[lastidx].lcsp13 & D40_MEM_LCSP1_SLOS_MASK)
+		    >> D40_MEM_LCSP1_SLOS_POS;
+	if (active_slos == last_slos) {
+		dev_vdbg(d40c->base->dev, "%s: activ %d last %d secnt %u\n",
+			 __func__, active_slos, last_slos, secnt);
+		return false;
+	}
+
+	dev_vdbg(d40c->base->dev,
+		 "%s: active %#x last %#x, lli_len %d secnt %u\n",
+		 __func__, active_slos, last_slos, prev->lli_len, secnt);
+
+	/*
+	 * We're either transferring one of this desc's links or one from an
+	 * earlier linked one.  Either way, update this desc's last link.
+	 */
+	return __d40_tx_submit_link_log(d40c, d40d, prev);
+}
+
+static bool __d40_tx_submit_link_phy(struct d40_chan *d40c,
+				     struct d40_desc *d40d,
+				     struct d40_desc *prev)
+{
+	int lastidx = prev->lli_len - 1;
+	struct d40_phy_lli *lastsrc = &prev->lli_phy.src[lastidx];
+	struct d40_phy_lli *lastdst = &prev->lli_phy.dst[lastidx];
+	struct d40_phy_lli *thissrc = &d40d->lli_phy.src[0];
+	struct d40_phy_lli *thisdst = &d40d->lli_phy.dst[0];
+	unsigned int val;
+
+	dma_sync_single_for_cpu(d40c->base->dev, virt_to_phys(lastsrc),
+				sizeof(*lastsrc), DMA_TO_DEVICE);
+
+	dma_sync_single_for_cpu(d40c->base->dev, virt_to_phys(lastdst),
+				sizeof(*lastdst), DMA_TO_DEVICE);
+
+	/* Keep the settings */
+	val = lastsrc->reg_lnk & ~D40_SREG_LNK_PHYS_LNK_MASK;
+	lastsrc->reg_lnk = val | virt_to_phys(thissrc);
+
+	val = lastdst->reg_lnk & ~D40_SREG_LNK_PHYS_LNK_MASK;
+	lastdst->reg_lnk = val | virt_to_phys(thisdst);
+
+	dma_sync_single_for_device(d40c->base->dev, virt_to_phys(lastsrc),
+				   sizeof(*lastsrc), DMA_TO_DEVICE);
+
+	dma_sync_single_for_device(d40c->base->dev, virt_to_phys(lastdst),
+				   sizeof(*lastdst), DMA_TO_DEVICE);
+
+	return true;
+}
+
+static bool d40_tx_submit_link_phy(struct d40_chan *d40c,
+				   struct d40_desc *d40d,
+				   struct d40_desc *prev)
+{
+
+	struct d40_phy_lli *lastsrc = &prev->lli_phy.src[prev->lli_len - 1];
+	unsigned int active_lnk;
+	unsigned int last_lnk;
+	unsigned int secnt;
+
+	/* A transfer must be ongoing. */
+	secnt = readl(d40c->base->virtbase + D40_DREG_PCBASE +
+		    d40c->phy_chan->num * D40_DREG_PCDELTA +
+		    D40_CHAN_REG_SSELT) >> D40_SREG_ELEM_LOG_ECNT_POS;
+	if (!secnt) {
+		dev_vdbg(d40c->base->dev, "%s: secnt %u\n", __func__, secnt);
+		return false;
+	}
+
+	active_lnk = readl(d40c->base->virtbase + D40_DREG_PCBASE +
+			   d40c->phy_chan->num * D40_DREG_PCDELTA +
+			   D40_CHAN_REG_SSLNK);
+
+	dma_sync_single_for_cpu(d40c->base->dev, virt_to_phys(lastsrc),
+				sizeof(*lastsrc), DMA_TO_DEVICE);
+
+	last_lnk = lastsrc->reg_lnk;
+
+	dma_sync_single_for_device(d40c->base->dev, virt_to_phys(lastsrc),
+				   sizeof(*lastsrc), DMA_TO_DEVICE);
+
+	/*
+	 * We don't support linking while the last link is being transmitted.
+	 */
+	if (active_lnk == last_lnk) {
+		dev_vdbg(d40c->base->dev, "%s: active %d last %d secnt %u\n",
+			 __func__, active_lnk, last_lnk, secnt);
+		return false;
 	}
+
+	dev_vdbg(d40c->base->dev,
+		"%s: activ %#x last %#x, lli_len %d secnt %u\n",
+		__func__, active_lnk, last_lnk, prev->lli_len, secnt);
+
+	/*
+	 * We're either transferring one of this desc's links or one from an
+	 * earlier linked one.  Either way, update this desc's last link.
+	 */
+	return __d40_tx_submit_link_phy(d40c, d40d, prev);
+}
+
+static bool d40_tx_submit_link(struct d40_chan *d40c, struct d40_desc *d40d)
+{
+	struct d40_desc *prev;
+	bool hwlinked;
+
+	if (list_empty(&d40c->active))
+		return false;
+
+	prev = list_entry(d40c->active.prev, struct d40_desc, node);
+
+	if (chan_is_physical(d40c))
+		hwlinked = d40_tx_submit_link_phy(d40c, d40d, prev);
+	else
+		hwlinked = d40_tx_submit_link_log(d40c, d40d, prev);
+
+	if (hwlinked)
+		dev_dbg(d40c->base->dev, "%s: hwlinked d40d %p prev %p\n",
+			__func__, d40d, prev);
+
+	return hwlinked;
 }
 
 static dma_cookie_t d40_tx_submit(struct dma_async_tx_descriptor *tx)
@@ -767,31 +1446,37 @@ static dma_cookie_t d40_tx_submit(struct dma_async_tx_descriptor *tx)
 	struct d40_desc *d40d = container_of(tx, struct d40_desc, txd);
 	unsigned long flags;
 
+	d40_usage_inc(d40c);
+
+	(void) d40_pause(&d40c->chan);
+
 	spin_lock_irqsave(&d40c->lock, flags);
 
-	tx->cookie = d40_assign_cookie(d40c, d40d);
+	d40c->chan.cookie++;
+
+	if (d40c->chan.cookie < 0)
+		d40c->chan.cookie = 1;
 
-	d40_desc_queue(d40c, d40d);
+	d40d->txd.cookie = d40c->chan.cookie;
+
+	d40d->is_hw_linked = d40_tx_submit_link(d40c, d40d);
+	if (d40d->is_hw_linked) {
+		/* Linked, therefore already active */
+		d40_desc_submit(d40c, d40d);
+	} else
+		d40_desc_queue(d40c, d40d);
 
 	spin_unlock_irqrestore(&d40c->lock, flags);
 
+	(void) d40_resume(&d40c->chan);
+	d40_usage_dec(d40c);
+
 	return tx->cookie;
 }
 
 static int d40_start(struct d40_chan *d40c)
 {
-	int err;
-
-	if (d40c->log_num != D40_PHY_CHAN) {
-		err = d40_channel_execute_command(d40c, D40_DMA_SUSPEND_REQ);
-		if (err)
-			return err;
-		d40_config_set_event(d40c, true);
-	}
-
-	err = d40_channel_execute_command(d40c, D40_DMA_RUN);
-
-	return err;
+	return d40_channel_execute_command(d40c, D40_DMA_RUN);
 }
 
 static struct d40_desc *d40_queue_start(struct d40_chan *d40c)
@@ -803,7 +1488,10 @@ static struct d40_desc *d40_queue_start(struct d40_chan *d40c)
 	d40d = d40_first_queued(d40c);
 
 	if (d40d != NULL) {
-		d40c->busy = true;
+		if (!d40c->busy) {
+			d40_usage_inc(d40c);
+			d40c->busy = true;
+		}
 
 		/* Remove from queue */
 		d40_desc_remove(d40d);
@@ -811,14 +1499,25 @@ static struct d40_desc *d40_queue_start(struct d40_chan *d40c)
 		/* Add to active queue */
 		d40_desc_submit(d40c, d40d);
 
-		/* Initiate DMA job */
-		d40_desc_load(d40c, d40d);
+		/*
+		 * If this job is already linked in hw,
+		 * do not submit it.
+		 */
 
-		/* Start dma job */
-		err = d40_start(d40c);
+		/*
+		 * TODO: make sure is_hw_linked work all the time with logical
+		 * jobs
+		 */
+		if (!d40d->is_hw_linked) {
+			/* Initiate DMA job */
+			d40_desc_load(d40c, d40d);
 
-		if (err)
-			return NULL;
+			/* Start dma job */
+			err = d40_start(d40c);
+
+			if (err)
+				return NULL;
+		}
 	}
 
 	return d40d;
@@ -828,49 +1527,77 @@ static struct d40_desc *d40_queue_start(struct d40_chan *d40c)
 static void dma_tc_handle(struct d40_chan *d40c)
 {
 	struct d40_desc *d40d;
+	bool islastactive;
 
-	if (!d40c->phy_chan)
+	if (d40c->cdesc) {
+		d40c->pending_tx++;
+		tasklet_schedule(&d40c->tasklet);
 		return;
+	}
 
 	/* Get first active entry from list */
+redo:
 	d40d = d40_first_active_get(d40c);
 
 	if (d40d == NULL)
 		return;
 
-	if (d40d->lli_tcount < d40d->lli_len) {
+	d40_lcla_free_all(d40c, d40d);
 
+	if (d40d->lli_current < d40d->lli_len) {
 		d40_desc_load(d40c, d40d);
 		/* Start dma job */
 		(void) d40_start(d40c);
 		return;
 	}
 
-	if (d40_queue_start(d40c) == NULL)
+	/*
+	 * More than one active happens when we have
+	 * hw linked transfers.
+	 */
+	islastactive = list_is_last(&d40d->node, &d40c->active);
+	if (islastactive && d40_queue_start(d40c) == NULL) {
 		d40c->busy = false;
+		d40_usage_dec(d40c);
+	}
+
+	d40_desc_remove(d40d);
+	d40_desc_done(d40c, d40d);
 
 	d40c->pending_tx++;
 	tasklet_schedule(&d40c->tasklet);
 
+	/*
+	 * When we have multiple active transfers, there is a chance that we
+	 * might miss some link interrupts if the time to perform each link is
+	 * very small (mostly with mem-to-mem transfers).  So, if the hardware
+	 * is not transmitting any more links, assume that all the active
+	 * transfers are complete.
+	 */
+	if (!islastactive && !d40_tx_is_linked(d40c))
+		goto redo;
 }
 
 static void dma_tasklet(unsigned long data)
 {
 	struct d40_chan *d40c = (struct d40_chan *) data;
-	struct d40_desc *d40d_fin;
+	struct d40_desc *d40d;
 	unsigned long flags;
 	dma_async_tx_callback callback;
 	void *callback_param;
 
 	spin_lock_irqsave(&d40c->lock, flags);
 
-	/* Get first active entry from list */
-	d40d_fin = d40_first_active_get(d40c);
-
-	if (d40d_fin == NULL)
-		goto err;
+	if (d40c->cdesc)
+		d40d = d40c->cdesc->d40d;
+	else {
+		/* Get first active entry from list */
+		d40d = d40_first_done(d40c);
+		if (d40d == NULL)
+			goto err;
 
-	d40c->completed = d40d_fin->txd.cookie;
+		d40c->completed = d40d->txd.cookie;
+	}
 
 	/*
 	 * If terminating a channel pending_tx is set to zero.
@@ -882,20 +1609,23 @@ static void dma_tasklet(unsigned long data)
 	}
 
 	/* Callback to client */
-	callback = d40d_fin->txd.callback;
-	callback_param = d40d_fin->txd.callback_param;
-
-	if (async_tx_test_ack(&d40d_fin->txd)) {
-		d40_pool_lli_free(d40d_fin);
-		d40_desc_remove(d40d_fin);
-		/* Return desc to free-list */
-		d40_desc_free(d40c, d40d_fin);
+
+	if (d40c->cdesc) {
+		callback = d40c->cdesc->period_callback;
+		callback_param = d40c->cdesc->period_callback_param;
 	} else {
-		d40_desc_reset(d40d_fin);
-		if (!d40d_fin->is_in_client_list) {
-			d40_desc_remove(d40d_fin);
-			list_add_tail(&d40d_fin->node, &d40c->client);
-			d40d_fin->is_in_client_list = true;
+		callback = d40d->txd.callback;
+		callback_param = d40d->txd.callback_param;
+
+		if (async_tx_test_ack(&d40d->txd)) {
+			d40_pool_lli_free(d40d);
+			d40_desc_remove(d40d);
+			d40_desc_free(d40c, d40d);
+		} else if (!d40d->is_in_client_list) {
+			d40_desc_remove(d40d);
+			d40_lcla_free_all(d40c, d40d);
+			list_add_tail(&d40d->node, &d40c->client);
+			d40d->is_in_client_list = true;
 		}
 	}
 
@@ -906,12 +1636,12 @@ static void dma_tasklet(unsigned long data)
 
 	spin_unlock_irqrestore(&d40c->lock, flags);
 
-	if (callback)
+	if (callback && (d40d->txd.flags & DMA_PREP_INTERRUPT))
 		callback(callback_param);
 
 	return;
 
- err:
+err:
 	/* Rescue manouver if receiving double interrupts */
 	if (d40c->pending_tx > 0)
 		d40c->pending_tx--;
@@ -920,7 +1650,7 @@ static void dma_tasklet(unsigned long data)
 
 static irqreturn_t d40_handle_interrupt(int irq, void *data)
 {
-	static const struct d40_interrupt_lookup il[] = {
+	static struct d40_interrupt_lookup il[] = {
 		{D40_DREG_LCTIS0, D40_DREG_LCICR0, false,  0},
 		{D40_DREG_LCTIS1, D40_DREG_LCICR1, false, 32},
 		{D40_DREG_LCTIS2, D40_DREG_LCICR2, false, 64},
@@ -935,7 +1665,6 @@ static irqreturn_t d40_handle_interrupt(int irq, void *data)
 
 	int i;
 	u32 regs[ARRAY_SIZE(il)];
-	u32 tmp;
 	u32 idx;
 	u32 row;
 	long chan = -1;
@@ -944,7 +1673,9 @@ static irqreturn_t d40_handle_interrupt(int irq, void *data)
 	struct d40_base *base = data;
 
 	spin_lock_irqsave(&base->interrupt_lock, flags);
-
+#ifdef CONFIG_STE_DMA40_DEBUG
+	sted40_history_text("IRQ enter");
+#endif
 	/* Read interrupt status of both logical and physical channels */
 	for (i = 0; i < ARRAY_SIZE(il); i++)
 		regs[i] = readl(base->virtbase + il[i].src);
@@ -961,49 +1692,83 @@ static irqreturn_t d40_handle_interrupt(int irq, void *data)
 		row = chan / BITS_PER_LONG;
 		idx = chan & (BITS_PER_LONG - 1);
 
-		/* ACK interrupt */
-		tmp = readl(base->virtbase + il[row].clr);
-		tmp |= 1 << idx;
-		writel(tmp, base->virtbase + il[row].clr);
-
 		if (il[row].offset == D40_PHY_CHAN)
 			d40c = base->lookup_phy_chans[idx];
 		else
 			d40c = base->lookup_log_chans[il[row].offset + idx];
+
+		if (!d40c) {
+			/*
+			 * No error because this can happen if something else
+			 * in the system is using the channel.
+			 */
+			continue;
+		}
+
+		/* ACK interrupt */
+		writel(1 << idx, base->virtbase + il[row].clr);
+
 		spin_lock(&d40c->lock);
 
-		if (!il[row].is_error)
+		if (!il[row].is_error) {
+
 			dma_tc_handle(d40c);
-		else
-			dev_err(base->dev, "[%s] IRQ chan: %ld offset %d idx %d\n",
+		} else {
+			dev_err(base->dev, "[%s] Error IRQ chan: %ld offset %d idx %d\n",
 				__func__, chan, il[row].offset, idx);
+#ifdef CONFIG_STE_DMA40_DEBUG
+			sted40_history_dump();
+#endif
+		}
 
 		spin_unlock(&d40c->lock);
 	}
-
+#ifdef CONFIG_STE_DMA40_DEBUG
+	sted40_history_text("IRQ leave");
+#endif
 	spin_unlock_irqrestore(&base->interrupt_lock, flags);
 
 	return IRQ_HANDLED;
 }
 
-
 static int d40_validate_conf(struct d40_chan *d40c,
 			     struct stedma40_chan_cfg *conf)
 {
 	int res = 0;
 	u32 dst_event_group = D40_TYPE_TO_GROUP(conf->dst_dev_type);
 	u32 src_event_group = D40_TYPE_TO_GROUP(conf->src_dev_type);
-	bool is_log = (conf->channel_type & STEDMA40_CHANNEL_IN_OPER_MODE)
-		== STEDMA40_CHANNEL_IN_LOG_MODE;
+	bool is_log = conf->mode == STEDMA40_MODE_LOGICAL;
 
-	if (d40c->dma_cfg.dir == STEDMA40_MEM_TO_PERIPH &&
+	if (!conf->dir) {
+		dev_err(&d40c->chan.dev->device, "[%s] Invalid direction.\n",
+			__func__);
+		res = -EINVAL;
+	}
+
+	if (conf->dst_dev_type != STEDMA40_DEV_DST_MEMORY &&
+	   d40c->base->plat_data->dev_tx[conf->dst_dev_type] == 0) {
+		dev_err(&d40c->chan.dev->device,
+			"[%s] Invalid TX channel address (%d)\n",
+			__func__, conf->dst_dev_type);
+		res = -EINVAL;
+	}
+
+	if (conf->src_dev_type != STEDMA40_DEV_SRC_MEMORY &&
+	   d40c->base->plat_data->dev_rx[conf->src_dev_type] == 0) {
+		dev_err(&d40c->chan.dev->device,
+			"[%s] Invalid RX channel address (%d)\n",
+			__func__, conf->src_dev_type);
+		res = -EINVAL;
+	}
+
+	if (conf->dir == STEDMA40_MEM_TO_PERIPH &&
 	    dst_event_group == STEDMA40_DEV_DST_MEMORY) {
 		dev_err(&d40c->chan.dev->device, "[%s] Invalid dst\n",
 			__func__);
 		res = -EINVAL;
 	}
 
-	if (d40c->dma_cfg.dir == STEDMA40_PERIPH_TO_MEM &&
+	if (conf->dir == STEDMA40_PERIPH_TO_MEM &&
 	    src_event_group == STEDMA40_DEV_SRC_MEMORY) {
 		dev_err(&d40c->chan.dev->device, "[%s] Invalid src\n",
 			__func__);
@@ -1038,11 +1803,16 @@ static int d40_validate_conf(struct d40_chan *d40c,
 	return res;
 }
 
-static bool d40_alloc_mask_set(struct d40_phy_res *phy, bool is_src,
-			       int log_event_line, bool is_log)
+static bool d40_alloc_mask_set(struct d40_phy_res *phy,
+			       bool is_src, int log_event_line, bool is_log,
+			       bool *first_user)
 {
 	unsigned long flags;
 	spin_lock_irqsave(&phy->lock, flags);
+
+	*first_user = ((phy->allocated_src | phy->allocated_dst)
+			== D40_ALLOC_FREE);
+
 	if (!is_log) {
 		/* Physical interrupts are masked per physical full channel */
 		if (phy->allocated_src == D40_ALLOC_FREE &&
@@ -1097,7 +1867,6 @@ static bool d40_alloc_mask_free(struct d40_phy_res *phy, bool is_src,
 
 	spin_lock_irqsave(&phy->lock, flags);
 	if (!log_event_line) {
-		/* Physical interrupts are masked per physical full channel */
 		phy->allocated_dst = D40_ALLOC_FREE;
 		phy->allocated_src = D40_ALLOC_FREE;
 		is_free = true;
@@ -1124,7 +1893,7 @@ out:
 	return is_free;
 }
 
-static int d40_allocate_channel(struct d40_chan *d40c)
+static int d40_allocate_channel(struct d40_chan *d40c, bool *first_phy_user)
 {
 	int dev_type;
 	int event_group;
@@ -1134,9 +1903,7 @@ static int d40_allocate_channel(struct d40_chan *d40c)
 	int j;
 	int log_num;
 	bool is_src;
-	bool is_log = (d40c->dma_cfg.channel_type & STEDMA40_CHANNEL_IN_OPER_MODE)
-		== STEDMA40_CHANNEL_IN_LOG_MODE;
-
+	bool is_log = d40c->dma_cfg.mode == STEDMA40_MODE_LOGICAL;
 
 	phys = d40c->base->phy_res;
 
@@ -1162,15 +1929,18 @@ static int d40_allocate_channel(struct d40_chan *d40c)
 			for (i = 0; i < d40c->base->num_phy_chans; i++) {
 
 				if (d40_alloc_mask_set(&phys[i], is_src,
-						       0, is_log))
+						       0, is_log,
+						       first_phy_user))
 					goto found_phy;
 			}
 		} else
 			for (j = 0; j < d40c->base->num_phy_chans; j += 8) {
 				int phy_num = j  + event_group * 2;
 				for (i = phy_num; i < phy_num + 2; i++) {
-					if (d40_alloc_mask_set(&phys[i], is_src,
-							       0, is_log))
+					if (d40_alloc_mask_set(&phys[i],
+							       is_src, 0,
+							       is_log,
+							       first_phy_user))
 						goto found_phy;
 				}
 			}
@@ -1194,13 +1964,15 @@ found_phy:
 		if (is_src) {
 			for (i = phy_num; i < phy_num + 2; i++) {
 				if (d40_alloc_mask_set(&phys[i], is_src,
-						       event_line, is_log))
+						       event_line, is_log,
+						       first_phy_user))
 					goto found_log;
 			}
 		} else {
 			for (i = phy_num + 1; i >= phy_num; i--) {
 				if (d40_alloc_mask_set(&phys[i], is_src,
-						       event_line, is_log))
+						       event_line, is_log,
+						       first_phy_user))
 					goto found_log;
 			}
 		}
@@ -1221,30 +1993,6 @@ out:
 
 }
 
-static int d40_config_chan(struct d40_chan *d40c,
-			   struct stedma40_chan_cfg *info)
-{
-
-	/* Fill in basic CFG register values */
-	d40_phy_cfg(&d40c->dma_cfg, &d40c->src_def_cfg,
-		    &d40c->dst_def_cfg, d40c->log_num != D40_PHY_CHAN);
-
-	if (d40c->log_num != D40_PHY_CHAN) {
-		d40_log_cfg(&d40c->dma_cfg,
-			    &d40c->log_def.lcsp1, &d40c->log_def.lcsp3);
-
-		if (d40c->dma_cfg.dir == STEDMA40_PERIPH_TO_MEM)
-			d40c->lcpa = d40c->base->lcpa_base +
-				d40c->dma_cfg.src_dev_type * 32;
-		else
-			d40c->lcpa = d40c->base->lcpa_base +
-				d40c->dma_cfg.dst_dev_type * 32 + 16;
-	}
-
-	/* Write channel configuration to the DMA */
-	return d40_config_write(d40c);
-}
-
 static int d40_config_memcpy(struct d40_chan *d40c)
 {
 	dma_cap_mask_t cap = d40c->chan.device->cap_mask;
@@ -1267,18 +2015,27 @@ static int d40_config_memcpy(struct d40_chan *d40c)
 	return 0;
 }
 
-
 static int d40_free_dma(struct d40_chan *d40c)
 {
 
 	int res = 0;
-	u32 event, dir;
+	u32 event;
 	struct d40_phy_res *phy = d40c->phy_chan;
 	bool is_src;
+	struct d40_desc *d;
+	struct d40_desc *_d;
 
 	/* Terminate all queued and active transfers */
 	d40_term_all(d40c);
 
+	/* Release client owned descriptors */
+	if (!list_empty(&d40c->client))
+		list_for_each_entry_safe(d, _d, &d40c->client, node) {
+			d40_pool_lli_free(d);
+			d40_desc_remove(d);
+			d40_desc_free(d40c, d);
+		}
+
 	if (phy == NULL) {
 		dev_err(&d40c->chan.dev->device, "[%s] phy == null\n",
 			__func__);
@@ -1292,22 +2049,12 @@ static int d40_free_dma(struct d40_chan *d40c)
 		return -EINVAL;
 	}
 
-
-	res = d40_channel_execute_command(d40c, D40_DMA_SUSPEND_REQ);
-	if (res) {
-		dev_err(&d40c->chan.dev->device, "[%s] suspend\n",
-			__func__);
-		return res;
-	}
-
 	if (d40c->dma_cfg.dir == STEDMA40_MEM_TO_PERIPH ||
 	    d40c->dma_cfg.dir == STEDMA40_MEM_TO_MEM) {
 		event = D40_TYPE_TO_EVENT(d40c->dma_cfg.dst_dev_type);
-		dir = D40_CHAN_REG_SDLNK;
 		is_src = false;
 	} else if (d40c->dma_cfg.dir == STEDMA40_PERIPH_TO_MEM) {
 		event = D40_TYPE_TO_EVENT(d40c->dma_cfg.src_dev_type);
-		dir = D40_CHAN_REG_SSLNK;
 		is_src = true;
 	} else {
 		dev_err(&d40c->chan.dev->device,
@@ -1315,79 +2062,32 @@ static int d40_free_dma(struct d40_chan *d40c)
 		return -EINVAL;
 	}
 
-	if (d40c->log_num != D40_PHY_CHAN) {
-		/*
-		 * Release logical channel, deactivate the event line during
-		 * the time physical res is suspended.
-		 */
-		writel((D40_DEACTIVATE_EVENTLINE << D40_EVENTLINE_POS(event)) &
-		       D40_EVENTLINE_MASK(event),
-		       d40c->base->virtbase + D40_DREG_PCBASE +
-		       phy->num * D40_DREG_PCDELTA + dir);
-
-		d40c->base->lookup_log_chans[d40c->log_num] = NULL;
-
-		/*
-		 * Check if there are more logical allocation
-		 * on this phy channel.
-		 */
-		if (!d40_alloc_mask_free(phy, is_src, event)) {
-			/* Resume the other logical channels if any */
-			if (d40_chan_has_events(d40c)) {
-				res = d40_channel_execute_command(d40c,
-								  D40_DMA_RUN);
-				if (res) {
-					dev_err(&d40c->chan.dev->device,
-						"[%s] Executing RUN command\n",
-						__func__);
-					return res;
-				}
-			}
-			return 0;
-		}
-	} else
-		d40_alloc_mask_free(phy, is_src, 0);
+	d40_usage_inc(d40c);
 
-	/* Release physical channel */
 	res = d40_channel_execute_command(d40c, D40_DMA_STOP);
 	if (res) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Failed to stop channel\n", __func__);
+		dev_err(&d40c->chan.dev->device, "[%s] stop failed\n",
+			__func__);
+		d40_usage_dec(d40c);
 		return res;
 	}
-	d40c->phy_chan = NULL;
-	/* Invalidate channel type */
-	d40c->dma_cfg.channel_type = 0;
-	d40c->base->lookup_phy_chans[phy->num] = NULL;
-
-	return 0;
-
 
-}
-
-static int d40_pause(struct dma_chan *chan)
-{
-	struct d40_chan *d40c =
-		container_of(chan, struct d40_chan, chan);
-	int res;
+	d40_alloc_mask_free(phy, is_src, chan_is_logical(d40c) ? event : 0);
 
-	unsigned long flags;
+	if (chan_is_logical(d40c))
+		d40c->base->lookup_log_chans[d40c->log_num] = NULL;
+	else
+		d40c->base->lookup_phy_chans[phy->num] = NULL;
 
-	spin_lock_irqsave(&d40c->lock, flags);
+	d40_usage_dec(d40c);
+	if (d40c->busy)
+		d40_usage_dec(d40c);
+	d40c->busy = false;
 
-	res = d40_channel_execute_command(d40c, D40_DMA_SUSPEND_REQ);
-	if (res == 0) {
-		if (d40c->log_num != D40_PHY_CHAN) {
-			d40_config_set_event(d40c, false);
-			/* Resume the other logical channels if any */
-			if (d40_chan_has_events(d40c))
-				res = d40_channel_execute_command(d40c,
-								  D40_DMA_RUN);
-		}
-	}
+	d40c->phy_chan = NULL;
+	d40c->configured = false;
 
-	spin_unlock_irqrestore(&d40c->lock, flags);
-	return res;
+	return 0;
 }
 
 static bool d40_is_paused(struct d40_chan *d40c)
@@ -1397,11 +2097,10 @@ static bool d40_is_paused(struct d40_chan *d40c)
 	void __iomem *active_reg;
 	u32 status;
 	u32 event;
-	int res;
 
 	spin_lock_irqsave(&d40c->lock, flags);
 
-	if (d40c->log_num == D40_PHY_CHAN) {
+	if (chan_is_physical(d40c)) {
 		if (d40c->phy_chan->num % 2 == 0)
 			active_reg = d40c->base->virtbase + D40_DREG_ACTIVE;
 		else
@@ -1416,111 +2115,34 @@ static bool d40_is_paused(struct d40_chan *d40c)
 		goto _exit;
 	}
 
-	res = d40_channel_execute_command(d40c, D40_DMA_SUSPEND_REQ);
-	if (res != 0)
-		goto _exit;
-
 	if (d40c->dma_cfg.dir == STEDMA40_MEM_TO_PERIPH ||
-	    d40c->dma_cfg.dir == STEDMA40_MEM_TO_MEM)
+	    d40c->dma_cfg.dir == STEDMA40_MEM_TO_MEM) {
 		event = D40_TYPE_TO_EVENT(d40c->dma_cfg.dst_dev_type);
-	else if (d40c->dma_cfg.dir == STEDMA40_PERIPH_TO_MEM)
+		status = readl(d40c->base->virtbase + D40_DREG_PCBASE +
+			       d40c->phy_chan->num * D40_DREG_PCDELTA +
+			       D40_CHAN_REG_SDLNK);
+	} else if (d40c->dma_cfg.dir == STEDMA40_PERIPH_TO_MEM) {
 		event = D40_TYPE_TO_EVENT(d40c->dma_cfg.src_dev_type);
-	else {
+		status = readl(d40c->base->virtbase + D40_DREG_PCBASE +
+			       d40c->phy_chan->num * D40_DREG_PCDELTA +
+			       D40_CHAN_REG_SSLNK);
+	} else {
 		dev_err(&d40c->chan.dev->device,
 			"[%s] Unknown direction\n", __func__);
 		goto _exit;
 	}
-	status = d40_chan_has_events(d40c);
+
 	status = (status & D40_EVENTLINE_MASK(event)) >>
 		D40_EVENTLINE_POS(event);
 
 	if (status != D40_DMA_RUN)
 		is_paused = true;
-
-	/* Resume the other logical channels if any */
-	if (d40_chan_has_events(d40c))
-		res = d40_channel_execute_command(d40c,
-						  D40_DMA_RUN);
-
 _exit:
 	spin_unlock_irqrestore(&d40c->lock, flags);
 	return is_paused;
 
 }
 
-
-static bool d40_tx_is_linked(struct d40_chan *d40c)
-{
-	bool is_link;
-
-	if (d40c->log_num != D40_PHY_CHAN)
-		is_link = readl(&d40c->lcpa->lcsp3) &  D40_MEM_LCSP3_DLOS_MASK;
-	else
-		is_link = readl(d40c->base->virtbase + D40_DREG_PCBASE +
-				d40c->phy_chan->num * D40_DREG_PCDELTA +
-				D40_CHAN_REG_SDLNK) &
-			D40_SREG_LNK_PHYS_LNK_MASK;
-	return is_link;
-}
-
-static u32 d40_residue(struct d40_chan *d40c)
-{
-	u32 num_elt;
-
-	if (d40c->log_num != D40_PHY_CHAN)
-		num_elt = (readl(&d40c->lcpa->lcsp2) &  D40_MEM_LCSP2_ECNT_MASK)
-			>> D40_MEM_LCSP2_ECNT_POS;
-	else
-		num_elt = (readl(d40c->base->virtbase + D40_DREG_PCBASE +
-				 d40c->phy_chan->num * D40_DREG_PCDELTA +
-				 D40_CHAN_REG_SDELT) &
-			   D40_SREG_ELEM_PHY_ECNT_MASK) >> D40_SREG_ELEM_PHY_ECNT_POS;
-	return num_elt * (1 << d40c->dma_cfg.dst_info.data_width);
-}
-
-static int d40_resume(struct dma_chan *chan)
-{
-	struct d40_chan *d40c =
-		container_of(chan, struct d40_chan, chan);
-	int res = 0;
-	unsigned long flags;
-
-	spin_lock_irqsave(&d40c->lock, flags);
-
-	if (d40c->log_num != D40_PHY_CHAN) {
-		res = d40_channel_execute_command(d40c, D40_DMA_SUSPEND_REQ);
-		if (res)
-			goto out;
-
-		/* If bytes left to transfer or linked tx resume job */
-		if (d40_residue(d40c) || d40_tx_is_linked(d40c)) {
-			d40_config_set_event(d40c, true);
-			res = d40_channel_execute_command(d40c, D40_DMA_RUN);
-		}
-	} else if (d40_residue(d40c) || d40_tx_is_linked(d40c))
-		res = d40_channel_execute_command(d40c, D40_DMA_RUN);
-
-out:
-	spin_unlock_irqrestore(&d40c->lock, flags);
-	return res;
-}
-
-static u32 stedma40_residue(struct dma_chan *chan)
-{
-	struct d40_chan *d40c =
-		container_of(chan, struct d40_chan, chan);
-	u32 bytes_left;
-	unsigned long flags;
-
-	spin_lock_irqsave(&d40c->lock, flags);
-	bytes_left = d40_residue(d40c);
-	spin_unlock_irqrestore(&d40c->lock, flags);
-
-	return bytes_left;
-}
-
-/* Public DMA functions in addition to the DMA engine framework */
-
 int stedma40_set_psize(struct dma_chan *chan,
 		       int src_psize,
 		       int dst_psize)
@@ -1531,11 +2153,13 @@ int stedma40_set_psize(struct dma_chan *chan,
 
 	spin_lock_irqsave(&d40c->lock, flags);
 
-	if (d40c->log_num != D40_PHY_CHAN) {
+	if (chan_is_logical(d40c)) {
 		d40c->log_def.lcsp1 &= ~D40_MEM_LCSP1_SCFG_PSIZE_MASK;
 		d40c->log_def.lcsp3 &= ~D40_MEM_LCSP1_SCFG_PSIZE_MASK;
-		d40c->log_def.lcsp1 |= src_psize << D40_MEM_LCSP1_SCFG_PSIZE_POS;
-		d40c->log_def.lcsp3 |= dst_psize << D40_MEM_LCSP1_SCFG_PSIZE_POS;
+		d40c->log_def.lcsp1 |= src_psize
+				       << D40_MEM_LCSP1_SCFG_PSIZE_POS;
+		d40c->log_def.lcsp3 |= dst_psize
+				       << D40_MEM_LCSP1_SCFG_PSIZE_POS;
 		goto out;
 	}
 
@@ -1566,37 +2190,31 @@ struct dma_async_tx_descriptor *stedma40_memcpy_sg(struct dma_chan *chan,
 						   struct scatterlist *sgl_dst,
 						   struct scatterlist *sgl_src,
 						   unsigned int sgl_len,
-						   unsigned long flags)
+						   unsigned long dma_flags)
 {
 	int res;
 	struct d40_desc *d40d;
 	struct d40_chan *d40c = container_of(chan, struct d40_chan,
 					     chan);
-	unsigned long flg;
-	int lli_max = d40c->base->plat_data->llis_per_log;
+	unsigned long flags;
 
+	if (d40c->phy_chan == NULL) {
+		dev_err(&d40c->chan.dev->device,
+			"[%s] Unallocated channel.\n", __func__);
+		return ERR_PTR(-EINVAL);
+	}
 
-	spin_lock_irqsave(&d40c->lock, flg);
+	spin_lock_irqsave(&d40c->lock, flags);
 	d40d = d40_desc_get(d40c);
 
 	if (d40d == NULL)
 		goto err;
 
-	memset(d40d, 0, sizeof(struct d40_desc));
 	d40d->lli_len = sgl_len;
+	d40d->lli_current = 0;
+	d40d->txd.flags = dma_flags;
 
-	d40d->txd.flags = flags;
-
-	if (d40c->log_num != D40_PHY_CHAN) {
-		if (sgl_len > 1)
-			/*
-			 * Check if there is space available in lcla. If not,
-			 * split list into 1-length and run only in lcpa
-			 * space.
-			 */
-			if (d40_lcla_id_get(d40c,
-					    &d40c->base->lcla_pool) != 0)
-				lli_max = 1;
+	if (chan_is_logical(d40c)) {
 
 		if (d40_pool_lli_alloc(d40d, sgl_len, true) < 0) {
 			dev_err(&d40c->chan.dev->device,
@@ -1604,25 +2222,17 @@ struct dma_async_tx_descriptor *stedma40_memcpy_sg(struct dma_chan *chan,
 			goto err;
 		}
 
-		(void) d40_log_sg_to_lli(d40c->lcla.src_id,
-					 sgl_src,
+		(void) d40_log_sg_to_lli(sgl_src,
 					 sgl_len,
 					 d40d->lli_log.src,
 					 d40c->log_def.lcsp1,
-					 d40c->dma_cfg.src_info.data_width,
-					 flags & DMA_PREP_INTERRUPT, lli_max,
-					 d40c->base->plat_data->llis_per_log);
+					 d40c->dma_cfg.src_info.data_width);
 
-		(void) d40_log_sg_to_lli(d40c->lcla.dst_id,
-					 sgl_dst,
+		(void) d40_log_sg_to_lli(sgl_dst,
 					 sgl_len,
 					 d40d->lli_log.dst,
 					 d40c->log_def.lcsp3,
-					 d40c->dma_cfg.dst_info.data_width,
-					 flags & DMA_PREP_INTERRUPT, lli_max,
-					 d40c->base->plat_data->llis_per_log);
-
-
+					 d40c->dma_cfg.dst_info.data_width);
 	} else {
 		if (d40_pool_lli_alloc(d40d, sgl_len, false) < 0) {
 			dev_err(&d40c->chan.dev->device,
@@ -1634,11 +2244,12 @@ struct dma_async_tx_descriptor *stedma40_memcpy_sg(struct dma_chan *chan,
 					sgl_len,
 					0,
 					d40d->lli_phy.src,
-					d40d->lli_phy.src_addr,
+					virt_to_phys(d40d->lli_phy.src),
 					d40c->src_def_cfg,
 					d40c->dma_cfg.src_info.data_width,
 					d40c->dma_cfg.src_info.psize,
-					true);
+					false,
+					false);
 
 		if (res < 0)
 			goto err;
@@ -1647,28 +2258,32 @@ struct dma_async_tx_descriptor *stedma40_memcpy_sg(struct dma_chan *chan,
 					sgl_len,
 					0,
 					d40d->lli_phy.dst,
-					d40d->lli_phy.dst_addr,
+					virt_to_phys(d40d->lli_phy.dst),
 					d40c->dst_def_cfg,
 					d40c->dma_cfg.dst_info.data_width,
 					d40c->dma_cfg.dst_info.psize,
-					true);
+					false,
+					false);
 
 		if (res < 0)
 			goto err;
 
 		(void) dma_map_single(d40c->base->dev, d40d->lli_phy.src,
 				      d40d->lli_pool.size, DMA_TO_DEVICE);
+
 	}
 
 	dma_async_tx_descriptor_init(&d40d->txd, chan);
 
 	d40d->txd.tx_submit = d40_tx_submit;
 
-	spin_unlock_irqrestore(&d40c->lock, flg);
+	spin_unlock_irqrestore(&d40c->lock, flags);
 
 	return &d40d->txd;
 err:
-	spin_unlock_irqrestore(&d40c->lock, flg);
+	if (d40d)
+		d40_desc_free(d40c, d40d);
+	spin_unlock_irqrestore(&d40c->lock, flags);
 	return NULL;
 }
 EXPORT_SYMBOL(stedma40_memcpy_sg);
@@ -1687,10 +2302,45 @@ bool stedma40_filter(struct dma_chan *chan, void *data)
 	} else
 		err = d40_config_memcpy(d40c);
 
+	if (!err)
+		d40c->configured = true;
+
 	return err == 0;
 }
 EXPORT_SYMBOL(stedma40_filter);
 
+static void __d40_set_prio_rt(struct d40_chan *d40c, int dev_type, bool src)
+{
+	bool realtime = d40c->dma_cfg.realtime;
+	bool highprio = d40c->dma_cfg.high_priority;
+	u32 prioreg = highprio ? D40_DREG_PSEG1 : D40_DREG_PCEG1;
+	u32 rtreg = realtime ? D40_DREG_RSEG1 : D40_DREG_RCEG1;
+	u32 event = D40_TYPE_TO_EVENT(dev_type);
+	u32 group = D40_TYPE_TO_GROUP(dev_type);
+	u32 bit = 1 << event;
+
+	/* Destination event lines are stored in the upper halfword */
+	if (!src)
+		bit <<= 16;
+
+	writel(bit, d40c->base->virtbase + prioreg + group * 4);
+	writel(bit, d40c->base->virtbase + rtreg + group * 4);
+}
+
+static void d40_set_prio_realtime(struct d40_chan *d40c)
+{
+	if (d40c->base->rev < 3)
+		return;
+
+	if ((d40c->dma_cfg.dir ==  STEDMA40_PERIPH_TO_MEM) ||
+	    (d40c->dma_cfg.dir == STEDMA40_PERIPH_TO_PERIPH))
+		__d40_set_prio_rt(d40c, d40c->dma_cfg.src_dev_type, true);
+
+	if ((d40c->dma_cfg.dir ==  STEDMA40_MEM_TO_PERIPH) ||
+	    (d40c->dma_cfg.dir == STEDMA40_PERIPH_TO_PERIPH))
+		__d40_set_prio_rt(d40c, d40c->dma_cfg.dst_dev_type, false);
+}
+
 /* DMA ENGINE functions */
 static int d40_alloc_chan_resources(struct dma_chan *chan)
 {
@@ -1698,46 +2348,63 @@ static int d40_alloc_chan_resources(struct dma_chan *chan)
 	unsigned long flags;
 	struct d40_chan *d40c =
 		container_of(chan, struct d40_chan, chan);
+	bool is_free_phy;
+
+	d40_usage_inc(d40c);
 
 	spin_lock_irqsave(&d40c->lock, flags);
 
 	d40c->completed = chan->cookie = 1;
 
-	/*
-	 * If no dma configuration is set (channel_type == 0)
-	 * use default configuration
-	 */
-	if (d40c->dma_cfg.channel_type == 0) {
+	if (!d40c->configured) {
 		err = d40_config_memcpy(d40c);
-		if (err)
-			goto err_alloc;
+		if (err) {
+			dev_err(&d40c->chan.dev->device,
+				"[%s] Failed to configure memcpy channel\n",
+				__func__);
+			goto fail;
+		}
 	}
 
-	err = d40_allocate_channel(d40c);
+	err = d40_allocate_channel(d40c, &is_free_phy);
 	if (err) {
 		dev_err(&d40c->chan.dev->device,
 			"[%s] Failed to allocate channel\n", __func__);
-		goto err_alloc;
+		goto fail;
 	}
 
-	err = d40_config_chan(d40c, &d40c->dma_cfg);
-	if (err) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Failed to configure channel\n",
-			__func__);
-		goto err_config;
+	/* Fill in basic CFG register values */
+	d40_phy_cfg(&d40c->dma_cfg, &d40c->src_def_cfg,
+		    &d40c->dst_def_cfg, chan_is_logical(d40c));
+
+	d40_set_prio_realtime(d40c);
+
+	if (chan_is_logical(d40c)) {
+		d40_log_cfg(&d40c->dma_cfg,
+			    &d40c->log_def.lcsp1, &d40c->log_def.lcsp3);
+
+		if (d40c->dma_cfg.dir == STEDMA40_PERIPH_TO_MEM)
+			d40c->lcpa = d40c->base->lcpa_base +
+				d40c->dma_cfg.src_dev_type *
+				D40_LCPA_CHAN_SIZE;
+		else
+			d40c->lcpa = d40c->base->lcpa_base +
+				d40c->dma_cfg.dst_dev_type *
+				D40_LCPA_CHAN_SIZE + D40_LCPA_CHAN_DST_DELTA;
 	}
 
-	spin_unlock_irqrestore(&d40c->lock, flags);
-	return 0;
+	/*
+	 * Only write channel configuration to the DMA if the physical
+	 * resource is free. In case of multiple logical channels
+	 * on the same physical resource, only the first write is necessary.
+	 */
 
- err_config:
-	(void) d40_free_dma(d40c);
- err_alloc:
+	if (is_free_phy)
+		d40_config_write(d40c);
+fail:
+	d40_usage_dec(d40c);
 	spin_unlock_irqrestore(&d40c->lock, flags);
-	dev_err(&d40c->chan.dev->device,
-		"[%s] Channel allocation failed\n", __func__);
-	return -EINVAL;
+	return err;
 }
 
 static void d40_free_chan_resources(struct dma_chan *chan)
@@ -1747,6 +2414,12 @@ static void d40_free_chan_resources(struct dma_chan *chan)
 	int err;
 	unsigned long flags;
 
+	if (d40c->phy_chan == NULL) {
+		dev_err(&d40c->chan.dev->device,
+			"[%s] Cannot free unallocated channel\n", __func__);
+		return;
+	}
+
 	spin_lock_irqsave(&d40c->lock, flags);
 
 	err = d40_free_dma(d40c);
@@ -1761,15 +2434,21 @@ static struct dma_async_tx_descriptor *d40_prep_memcpy(struct dma_chan *chan,
 						       dma_addr_t dst,
 						       dma_addr_t src,
 						       size_t size,
-						       unsigned long flags)
+						       unsigned long dma_flags)
 {
 	struct d40_desc *d40d;
 	struct d40_chan *d40c = container_of(chan, struct d40_chan,
 					     chan);
-	unsigned long flg;
+	unsigned long flags;
 	int err = 0;
 
-	spin_lock_irqsave(&d40c->lock, flg);
+	if (d40c->phy_chan == NULL) {
+		dev_err(&d40c->chan.dev->device,
+			"[%s] Channel is not allocated.\n", __func__);
+		return ERR_PTR(-EINVAL);
+	}
+
+	spin_lock_irqsave(&d40c->lock, flags);
 	d40d = d40_desc_get(d40c);
 
 	if (d40d == NULL) {
@@ -1778,45 +2457,44 @@ static struct dma_async_tx_descriptor *d40_prep_memcpy(struct dma_chan *chan,
 		goto err;
 	}
 
-	memset(d40d, 0, sizeof(struct d40_desc));
+	d40_usage_inc(d40c);
 
-	d40d->txd.flags = flags;
+	d40d->txd.flags = dma_flags;
 
 	dma_async_tx_descriptor_init(&d40d->txd, chan);
 
 	d40d->txd.tx_submit = d40_tx_submit;
 
-	if (d40c->log_num != D40_PHY_CHAN) {
+	if (chan_is_logical(d40c)) {
 
 		if (d40_pool_lli_alloc(d40d, 1, true) < 0) {
 			dev_err(&d40c->chan.dev->device,
 				"[%s] Out of memory\n", __func__);
-			goto err;
+			goto err2;
 		}
 		d40d->lli_len = 1;
+		d40d->lli_current = 0;
 
 		d40_log_fill_lli(d40d->lli_log.src,
 				 src,
 				 size,
-				 0,
 				 d40c->log_def.lcsp1,
 				 d40c->dma_cfg.src_info.data_width,
-				 true, true);
+				 true);
 
 		d40_log_fill_lli(d40d->lli_log.dst,
 				 dst,
 				 size,
-				 0,
 				 d40c->log_def.lcsp3,
 				 d40c->dma_cfg.dst_info.data_width,
-				 true, true);
+				 true);
 
 	} else {
 
 		if (d40_pool_lli_alloc(d40d, 1, false) < 0) {
 			dev_err(&d40c->chan.dev->device,
 				"[%s] Out of memory\n", __func__);
-			goto err;
+			goto err2;
 		}
 
 		err = d40_phy_fill_lli(d40d->lli_phy.src,
@@ -1848,28 +2526,79 @@ static struct dma_async_tx_descriptor *d40_prep_memcpy(struct dma_chan *chan,
 				      d40d->lli_pool.size, DMA_TO_DEVICE);
 	}
 
-	spin_unlock_irqrestore(&d40c->lock, flg);
+	d40_usage_dec(d40c);
+	spin_unlock_irqrestore(&d40c->lock, flags);
 	return &d40d->txd;
 
 err_fill_lli:
 	dev_err(&d40c->chan.dev->device,
 		"[%s] Failed filling in PHY LLI\n", __func__);
-	d40_pool_lli_free(d40d);
+err2:
+	d40_usage_dec(d40c);
 err:
-	spin_unlock_irqrestore(&d40c->lock, flg);
+	if (d40d)
+		d40_desc_free(d40c, d40d);
+	spin_unlock_irqrestore(&d40c->lock, flags);
 	return NULL;
 }
 
+static dma_addr_t d40_dev_rx_addr(struct d40_chan *d40c)
+{
+	dma_addr_t dev_addr = 0;
+
+	if (d40c->runtime_addr)
+		dev_addr = d40c->runtime_addr;
+	else if (d40c->src_dev_addr)
+		dev_addr = d40c->src_dev_addr;
+	else
+		dev_addr = d40c->base->plat_data->
+			   dev_rx[d40c->dma_cfg.src_dev_type];
+
+	return dev_addr;
+}
+
+static dma_addr_t d40_dev_tx_addr(struct d40_chan *d40c)
+{
+	dma_addr_t dev_addr = 0;
+
+	if (d40c->runtime_addr)
+		dev_addr = d40c->runtime_addr;
+	else if (d40c->dst_dev_addr)
+		dev_addr = d40c->dst_dev_addr;
+	else
+		dev_addr = d40c->base->plat_data->
+			   dev_tx[d40c->dma_cfg.dst_dev_type];
+
+	return dev_addr;
+}
+
+int stedma40_set_dev_addr(struct dma_chan *chan,
+			  dma_addr_t src_dev_addr,
+			  dma_addr_t dst_dev_addr)
+{
+	struct d40_chan *d40c = container_of(chan, struct d40_chan, chan);
+	unsigned long flags;
+
+	spin_lock_irqsave(&d40c->lock, flags);
+
+	d40c->src_dev_addr = src_dev_addr;
+	d40c->dst_dev_addr = dst_dev_addr;
+
+	spin_unlock_irqrestore(&d40c->lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL(stedma40_set_dev_addr);
+
 static int d40_prep_slave_sg_log(struct d40_desc *d40d,
 				 struct d40_chan *d40c,
 				 struct scatterlist *sgl,
 				 unsigned int sg_len,
 				 enum dma_data_direction direction,
-				 unsigned long flags)
+				 unsigned long dma_flags)
 {
 	dma_addr_t dev_addr = 0;
 	int total_size;
-	int lli_max = d40c->base->plat_data->llis_per_log;
 
 	if (d40_pool_lli_alloc(d40d, sg_len, true) < 0) {
 		dev_err(&d40c->chan.dev->device,
@@ -1878,43 +2607,22 @@ static int d40_prep_slave_sg_log(struct d40_desc *d40d,
 	}
 
 	d40d->lli_len = sg_len;
-	d40d->lli_tcount = 0;
-
-	if (sg_len > 1)
-		/*
-		 * Check if there is space available in lcla.
-		 * If not, split list into 1-length and run only
-		 * in lcpa space.
-		 */
-		if (d40_lcla_id_get(d40c, &d40c->base->lcla_pool) != 0)
-			lli_max = 1;
+	d40d->lli_current = 0;
 
-	if (direction == DMA_FROM_DEVICE) {
-		dev_addr = d40c->base->plat_data->dev_rx[d40c->dma_cfg.src_dev_type];
-		total_size = d40_log_sg_to_dev(&d40c->lcla,
-					       sgl, sg_len,
-					       &d40d->lli_log,
-					       &d40c->log_def,
-					       d40c->dma_cfg.src_info.data_width,
-					       d40c->dma_cfg.dst_info.data_width,
-					       direction,
-					       flags & DMA_PREP_INTERRUPT,
-					       dev_addr, lli_max,
-					       d40c->base->plat_data->llis_per_log);
-	} else if (direction == DMA_TO_DEVICE) {
-		dev_addr = d40c->base->plat_data->dev_tx[d40c->dma_cfg.dst_dev_type];
-		total_size = d40_log_sg_to_dev(&d40c->lcla,
-					       sgl, sg_len,
-					       &d40d->lli_log,
-					       &d40c->log_def,
-					       d40c->dma_cfg.src_info.data_width,
-					       d40c->dma_cfg.dst_info.data_width,
-					       direction,
-					       flags & DMA_PREP_INTERRUPT,
-					       dev_addr, lli_max,
-					       d40c->base->plat_data->llis_per_log);
-	} else
+	if (direction == DMA_FROM_DEVICE)
+		dev_addr = d40_dev_rx_addr(d40c);
+	else if (direction == DMA_TO_DEVICE)
+		dev_addr = d40_dev_tx_addr(d40c);
+	else
 		return -EINVAL;
+
+	total_size = d40_log_sg_to_dev(sgl, sg_len,
+				       &d40d->lli_log,
+				       &d40c->log_def,
+				       d40c->dma_cfg.src_info.data_width,
+				       d40c->dma_cfg.dst_info.data_width,
+				       direction,
+				       dev_addr);
 	if (total_size < 0)
 		return -EINVAL;
 
@@ -1926,7 +2634,7 @@ static int d40_prep_slave_sg_phy(struct d40_desc *d40d,
 				 struct scatterlist *sgl,
 				 unsigned int sgl_len,
 				 enum dma_data_direction direction,
-				 unsigned long flags)
+				 unsigned long dma_flags)
 {
 	dma_addr_t src_dev_addr;
 	dma_addr_t dst_dev_addr;
@@ -1939,13 +2647,13 @@ static int d40_prep_slave_sg_phy(struct d40_desc *d40d,
 	}
 
 	d40d->lli_len = sgl_len;
-	d40d->lli_tcount = 0;
+	d40d->lli_current = 0;
 
 	if (direction == DMA_FROM_DEVICE) {
 		dst_dev_addr = 0;
-		src_dev_addr = d40c->base->plat_data->dev_rx[d40c->dma_cfg.src_dev_type];
+		src_dev_addr = d40_dev_rx_addr(d40c);
 	} else if (direction == DMA_TO_DEVICE) {
-		dst_dev_addr = d40c->base->plat_data->dev_tx[d40c->dma_cfg.dst_dev_type];
+		dst_dev_addr = d40_dev_tx_addr(d40c);
 		src_dev_addr = 0;
 	} else
 		return -EINVAL;
@@ -1954,11 +2662,12 @@ static int d40_prep_slave_sg_phy(struct d40_desc *d40d,
 				sgl_len,
 				src_dev_addr,
 				d40d->lli_phy.src,
-				d40d->lli_phy.src_addr,
+				virt_to_phys(d40d->lli_phy.src),
 				d40c->src_def_cfg,
 				d40c->dma_cfg.src_info.data_width,
 				d40c->dma_cfg.src_info.psize,
-				true);
+				d40d->cyclic,
+				d40d->txd.flags & DMA_PREP_INTERRUPT);
 	if (res < 0)
 		return res;
 
@@ -1966,11 +2675,12 @@ static int d40_prep_slave_sg_phy(struct d40_desc *d40d,
 				sgl_len,
 				dst_dev_addr,
 				d40d->lli_phy.dst,
-				d40d->lli_phy.dst_addr,
+				virt_to_phys(d40d->lli_phy.dst),
 				d40c->dst_def_cfg,
 				d40c->dma_cfg.dst_info.data_width,
 				d40c->dma_cfg.dst_info.psize,
-				 true);
+				d40d->cyclic,
+				d40d->txd.flags & DMA_PREP_INTERRUPT);
 	if (res < 0)
 		return res;
 
@@ -1979,53 +2689,66 @@ static int d40_prep_slave_sg_phy(struct d40_desc *d40d,
 	return 0;
 }
 
-static struct dma_async_tx_descriptor *d40_prep_slave_sg(struct dma_chan *chan,
-							 struct scatterlist *sgl,
-							 unsigned int sg_len,
-							 enum dma_data_direction direction,
-							 unsigned long flags)
+static struct dma_async_tx_descriptor *
+d40_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
+		  unsigned int sg_len, enum dma_data_direction direction,
+		  unsigned long dma_flags)
 {
 	struct d40_desc *d40d;
 	struct d40_chan *d40c = container_of(chan, struct d40_chan,
 					     chan);
-	unsigned long flg;
+	unsigned long flags;
 	int err;
 
+
+	if (d40c->phy_chan == NULL) {
+		dev_err(&d40c->chan.dev->device,
+			"[%s] Cannot prepare unallocated channel\n", __func__);
+		return ERR_PTR(-EINVAL);
+	}
+
 	if (d40c->dma_cfg.pre_transfer)
 		d40c->dma_cfg.pre_transfer(chan,
 					   d40c->dma_cfg.pre_transfer_data,
 					   sg_dma_len(sgl));
 
-	spin_lock_irqsave(&d40c->lock, flg);
+	spin_lock_irqsave(&d40c->lock, flags);
 	d40d = d40_desc_get(d40c);
-	spin_unlock_irqrestore(&d40c->lock, flg);
 
 	if (d40d == NULL)
-		return NULL;
-
-	memset(d40d, 0, sizeof(struct d40_desc));
+		goto err;
+	d40_usage_inc(d40c);
 
-	if (d40c->log_num != D40_PHY_CHAN)
+	if (chan_is_logical(d40c))
 		err = d40_prep_slave_sg_log(d40d, d40c, sgl, sg_len,
-					    direction, flags);
+					    direction, dma_flags);
 	else
 		err = d40_prep_slave_sg_phy(d40d, d40c, sgl, sg_len,
-					    direction, flags);
+					    direction, dma_flags);
+	d40_usage_dec(d40c);
+
 	if (err) {
 		dev_err(&d40c->chan.dev->device,
 			"[%s] Failed to prepare %s slave sg job: %d\n",
 			__func__,
-			d40c->log_num != D40_PHY_CHAN ? "log" : "phy", err);
-		return NULL;
+			chan_is_logical(d40c) ? "log" : "phy", err);
+		goto err;
 	}
 
-	d40d->txd.flags = flags;
+	d40d->txd.flags = dma_flags;
 
 	dma_async_tx_descriptor_init(&d40d->txd, chan);
 
 	d40d->txd.tx_submit = d40_tx_submit;
 
+	spin_unlock_irqrestore(&d40c->lock, flags);
 	return &d40d->txd;
+
+err:
+	if (d40d)
+		d40_desc_free(d40c, d40d);
+	spin_unlock_irqrestore(&d40c->lock, flags);
+	return NULL;
 }
 
 static enum dma_status d40_tx_status(struct dma_chan *chan,
@@ -2033,10 +2756,18 @@ static enum dma_status d40_tx_status(struct dma_chan *chan,
 				     struct dma_tx_state *txstate)
 {
 	struct d40_chan *d40c = container_of(chan, struct d40_chan, chan);
+	unsigned long flags;
 	dma_cookie_t last_used;
 	dma_cookie_t last_complete;
 	int ret;
 
+	if (d40c->phy_chan == NULL) {
+		dev_err(&d40c->chan.dev->device,
+			"[%s] Cannot read status of unallocated channel\n",
+			__func__);
+		return -EINVAL;
+	}
+
 	last_complete = d40c->completed;
 	last_used = chan->cookie;
 
@@ -2045,8 +2776,14 @@ static enum dma_status d40_tx_status(struct dma_chan *chan,
 	else
 		ret = dma_async_is_complete(cookie, last_complete, last_used);
 
-	dma_set_tx_state(txstate, last_complete, last_used,
-			 stedma40_residue(chan));
+	if (txstate) {
+		txstate->last = last_complete;
+		txstate->used = last_used;
+
+		spin_lock_irqsave(&d40c->lock, flags);
+		txstate->residue = d40_residue(d40c);
+		spin_unlock_irqrestore(&d40c->lock, flags);
+	}
 
 	return ret;
 }
@@ -2056,6 +2793,12 @@ static void d40_issue_pending(struct dma_chan *chan)
 	struct d40_chan *d40c = container_of(chan, struct d40_chan, chan);
 	unsigned long flags;
 
+	if (d40c->phy_chan == NULL) {
+		dev_err(&d40c->chan.dev->device,
+			"[%s] Channel is not allocated!\n", __func__);
+		return;
+	}
+
 	spin_lock_irqsave(&d40c->lock, flags);
 
 	/* Busy means that pending jobs are already being processed */
@@ -2065,28 +2808,381 @@ static void d40_issue_pending(struct dma_chan *chan)
 	spin_unlock_irqrestore(&d40c->lock, flags);
 }
 
+static void d40_terminate_all(struct dma_chan *chan)
+{
+	unsigned long flags;
+	struct d40_chan *d40c = container_of(chan, struct d40_chan, chan);
+	int ret;
+
+	spin_lock_irqsave(&d40c->lock, flags);
+
+	d40_usage_inc(d40c);
+
+	ret = d40_channel_execute_command(d40c, D40_DMA_STOP);
+	if (ret)
+		dev_err(&d40c->chan.dev->device,
+			"[%s] Failed to stop channel\n", __func__);
+
+	d40_term_all(d40c);
+	d40_usage_dec(d40c);
+	if (d40c->busy)
+		d40_usage_dec(d40c);
+	d40c->busy = false;
+
+	spin_unlock_irqrestore(&d40c->lock, flags);
+}
+
+/* Runtime reconfiguration extension */
+static void d40_set_runtime_config(struct dma_chan *chan,
+			       struct dma_slave_config *config)
+{
+	struct d40_chan *d40c = container_of(chan, struct d40_chan, chan);
+	struct stedma40_chan_cfg *cfg = &d40c->dma_cfg;
+	enum dma_slave_buswidth config_addr_width;
+	dma_addr_t config_addr;
+	u32 config_maxburst;
+	enum stedma40_periph_data_width addr_width;
+	int psize;
+
+	if (config->direction == DMA_FROM_DEVICE) {
+		dma_addr_t dev_addr_rx =
+			d40c->base->plat_data->dev_rx[cfg->src_dev_type];
+
+		config_addr = config->src_addr;
+		if (dev_addr_rx)
+			dev_dbg(d40c->base->dev,
+				"channel has a pre-wired RX address %08x "
+				"overriding with %08x\n",
+				dev_addr_rx, config_addr);
+		if (cfg->dir != STEDMA40_PERIPH_TO_MEM)
+			dev_dbg(d40c->base->dev,
+				"channel was not configured for peripheral "
+				"to memory transfer (%d) overriding\n",
+				cfg->dir);
+		cfg->dir = STEDMA40_PERIPH_TO_MEM;
+
+		config_addr_width = config->src_addr_width;
+		config_maxburst = config->src_maxburst;
+
+	} else if (config->direction == DMA_TO_DEVICE) {
+		dma_addr_t dev_addr_tx =
+			d40c->base->plat_data->dev_tx[cfg->dst_dev_type];
+
+		config_addr = config->dst_addr;
+		if (dev_addr_tx)
+			dev_dbg(d40c->base->dev,
+				"channel has a pre-wired TX address %08x "
+				"overriding with %08x\n",
+				dev_addr_tx, config_addr);
+		if (cfg->dir != STEDMA40_MEM_TO_PERIPH)
+			dev_dbg(d40c->base->dev,
+				"channel was not configured for memory "
+				"to peripheral transfer (%d) overriding\n",
+				cfg->dir);
+		cfg->dir = STEDMA40_MEM_TO_PERIPH;
+
+		config_addr_width = config->dst_addr_width;
+		config_maxburst = config->dst_maxburst;
+
+	} else {
+		dev_err(d40c->base->dev,
+			"unrecognized channel direction %d\n",
+			config->direction);
+		return;
+	}
+
+	switch (config_addr_width) {
+	case DMA_SLAVE_BUSWIDTH_1_BYTE:
+		addr_width = STEDMA40_BYTE_WIDTH;
+		break;
+	case DMA_SLAVE_BUSWIDTH_2_BYTES:
+		addr_width = STEDMA40_HALFWORD_WIDTH;
+		break;
+	case DMA_SLAVE_BUSWIDTH_4_BYTES:
+		addr_width = STEDMA40_WORD_WIDTH;
+		break;
+	case DMA_SLAVE_BUSWIDTH_8_BYTES:
+		addr_width = STEDMA40_DOUBLEWORD_WIDTH;
+		break;
+	default:
+		dev_err(d40c->base->dev,
+			"illegal peripheral address width "
+			"requested (%d)\n",
+			config->src_addr_width);
+		return;
+	}
+
+	if (chan_is_logical(d40c)) {
+		if (config_maxburst >= 16)
+			psize = STEDMA40_PSIZE_LOG_16;
+		else if (config_maxburst >= 8)
+			psize = STEDMA40_PSIZE_LOG_8;
+		else if (config_maxburst >= 4)
+			psize = STEDMA40_PSIZE_LOG_4;
+		else
+			psize = STEDMA40_PSIZE_LOG_1;
+	} else {
+		if (config_maxburst >= 16)
+			psize = STEDMA40_PSIZE_PHY_16;
+		else if (config_maxburst >= 8)
+			psize = STEDMA40_PSIZE_PHY_8;
+		else if (config_maxburst >= 4)
+			psize = STEDMA40_PSIZE_PHY_4;
+		else
+			psize = STEDMA40_PSIZE_PHY_1;
+	}
+
+	/* Set up all the endpoint configs */
+	cfg->src_info.data_width = addr_width;
+	cfg->src_info.psize = psize;
+	cfg->src_info.flow_ctrl = STEDMA40_NO_FLOW_CTRL;
+	cfg->dst_info.data_width = addr_width;
+	cfg->dst_info.psize = psize;
+	cfg->dst_info.flow_ctrl = STEDMA40_NO_FLOW_CTRL;
+
+	/* Fill in register values */
+	if (chan_is_logical(d40c))
+		d40_log_cfg(cfg, &d40c->log_def.lcsp1, &d40c->log_def.lcsp3);
+	else
+		d40_phy_cfg(cfg, &d40c->src_def_cfg,
+			    &d40c->dst_def_cfg, false);
+
+	/* These settings will take precedence later */
+	d40c->runtime_addr = config_addr;
+	d40c->runtime_direction = config->direction;
+	dev_dbg(d40c->base->dev,
+		"configured channel %s for %s, data width %d, "
+		"maxburst %d bytes, LE, no flow control\n",
+		dma_chan_name(chan),
+		(config->direction == DMA_FROM_DEVICE) ? "RX" : "TX",
+		config_addr_width,
+		config_maxburst);
+}
+
 static int d40_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
 		       unsigned long arg)
 {
-	unsigned long flags;
 	struct d40_chan *d40c = container_of(chan, struct d40_chan, chan);
 
+	if (d40c->phy_chan == NULL) {
+		dev_err(&d40c->chan.dev->device,
+			"[%s] Channel is not allocated!\n", __func__);
+		return -EINVAL;
+	}
+
 	switch (cmd) {
 	case DMA_TERMINATE_ALL:
-		spin_lock_irqsave(&d40c->lock, flags);
-		d40_term_all(d40c);
-		spin_unlock_irqrestore(&d40c->lock, flags);
+		d40_terminate_all(chan);
 		return 0;
 	case DMA_PAUSE:
 		return d40_pause(chan);
 	case DMA_RESUME:
 		return d40_resume(chan);
+	case DMA_SLAVE_CONFIG:
+		d40_set_runtime_config(chan,
+			(struct dma_slave_config *) arg);
+		return 0;
+	default:
+		break;
 	}
 
 	/* Other commands are unimplemented */
 	return -ENXIO;
 }
 
+dma_addr_t stedma40_get_src_addr(struct dma_chan *chan)
+{
+	struct d40_chan *d40c = container_of(chan, struct d40_chan, chan);
+	dma_addr_t addr;
+
+	if (chan_is_physical(d40c))
+		addr = readl(d40c->base->virtbase + D40_DREG_PCBASE +
+			     d40c->phy_chan->num * D40_DREG_PCDELTA +
+			     D40_CHAN_REG_SSPTR);
+	else {
+		unsigned long lower;
+		unsigned long upper;
+
+		/*
+		 * There is a potential for overflow between the time the two
+		 * halves of the pointer are read.
+		 */
+		lower = d40c->lcpa->lcsp0 & D40_MEM_LCSP0_SPTR_MASK;
+		upper = d40c->lcpa->lcsp1 & D40_MEM_LCSP1_SPTR_MASK;
+
+		addr = upper | lower;
+	}
+
+	return addr;
+}
+EXPORT_SYMBOL(stedma40_get_src_addr);
+
+dma_addr_t stedma40_get_dst_addr(struct dma_chan *chan)
+{
+	struct d40_chan *d40c = container_of(chan, struct d40_chan, chan);
+	dma_addr_t addr;
+
+	if (chan_is_physical(d40c))
+		addr = readl(d40c->base->virtbase + D40_DREG_PCBASE +
+			     d40c->phy_chan->num * D40_DREG_PCDELTA +
+			     D40_CHAN_REG_SDPTR);
+	else {
+		unsigned long lower;
+		unsigned long upper;
+
+		lower = d40c->lcpa->lcsp2 & D40_MEM_LCSP2_DPTR_MASK;
+		upper = d40c->lcpa->lcsp3 & D40_MEM_LCSP3_DPTR_MASK;
+
+		addr = upper | lower;
+	}
+
+	return addr;
+}
+EXPORT_SYMBOL(stedma40_get_dst_addr);
+
+int stedma40_cyclic_start(struct dma_chan *chan)
+{
+	struct d40_chan *d40c = container_of(chan, struct d40_chan, chan);
+	unsigned long flags;
+	int ret = -EINVAL;
+
+	spin_lock_irqsave(&d40c->lock, flags);
+
+	if (!d40c->cdesc)
+		goto out;
+
+	d40_usage_inc(d40c);
+
+	ret = d40_start(d40c);
+	if (!ret)
+		d40c->busy = true;
+	else
+		d40_usage_dec(d40c);
+
+out:
+	spin_unlock_irqrestore(&d40c->lock, flags);
+	return ret;
+}
+EXPORT_SYMBOL(stedma40_cyclic_start);
+
+void stedma40_cyclic_stop(struct dma_chan *chan)
+{
+	d40_terminate_all(chan);
+}
+EXPORT_SYMBOL(stedma40_cyclic_stop);
+
+void stedma40_cyclic_free(struct dma_chan *chan)
+{
+	struct d40_chan *d40c = container_of(chan, struct d40_chan, chan);
+	struct stedma40_cyclic_desc *cdesc;
+	unsigned long flags;
+
+	spin_lock_irqsave(&d40c->lock, flags);
+
+	cdesc = d40c->cdesc;
+	if (!cdesc) {
+		spin_unlock_irqrestore(&d40c->lock, flags);
+		return;
+	}
+
+	d40c->cdesc = NULL;
+	d40_lcla_free_all(d40c, cdesc->d40d);
+
+	spin_unlock_irqrestore(&d40c->lock, flags);
+
+	kfree(cdesc);
+}
+EXPORT_SYMBOL(stedma40_cyclic_free);
+
+struct stedma40_cyclic_desc *
+stedma40_cyclic_prep_sg(struct dma_chan *chan,
+			struct scatterlist *sgl,
+			unsigned int sg_len,
+			enum dma_data_direction direction,
+			unsigned long dma_flags)
+{
+	struct d40_chan *d40c = container_of(chan, struct d40_chan, chan);
+	struct stedma40_cyclic_desc *cdesc;
+	struct d40_desc *d40d;
+	unsigned long flags;
+	void *mem;
+	int err;
+
+	mem = kzalloc(sizeof(struct stedma40_cyclic_desc)
+		      + sizeof(struct d40_desc), GFP_ATOMIC);
+	if (!mem)
+		return ERR_PTR(-ENOMEM);
+
+	cdesc = mem;
+	d40d = cdesc->d40d = mem + sizeof(struct stedma40_cyclic_desc);
+
+	spin_lock_irqsave(&d40c->lock, flags);
+
+	if (d40c->phy_chan == NULL) {
+		dev_err(&chan->dev->device,
+			"[%s] Cannot prepare unallocated channel\n", __func__);
+		err = -EINVAL;
+		goto out;
+	}
+
+	if (d40c->cdesc || d40c->busy) {
+		dev_err(&d40c->chan.dev->device,
+			"[%s] Cannot prepare cyclic job for busy channel\n",
+			__func__);
+		err = -EBUSY;
+		goto out;
+	}
+
+	d40d->cyclic = true;
+	d40d->txd.flags = dma_flags;
+	INIT_LIST_HEAD(&d40d->node);
+
+	d40_usage_inc(d40c);
+
+	if (chan_is_logical(d40c))
+		err = d40_prep_slave_sg_log(d40d, d40c, sgl, sg_len,
+					    direction, dma_flags);
+	else
+		err = d40_prep_slave_sg_phy(d40d, d40c, sgl, sg_len,
+					    direction, dma_flags);
+
+	if (err) {
+		dev_err(&d40c->chan.dev->device,
+			"[%s] Failed to prepare %s slave sg job: %d\n",
+			__func__,
+			chan_is_logical(d40c) ? "log" : "phy", err);
+		goto out2;
+	}
+
+	d40_desc_load(d40c, d40d);
+
+	/*
+	 * Couldn't get enough LCLA.  We don't support splitting of cyclic
+	 * jobs.
+	 */
+	if (d40d->lli_current != d40d->lli_len) {
+		dev_err(&chan->dev->device,
+			"[%s] Couldn't prepare cyclic job: not enough LCLA",
+			__func__);
+		err = -EBUSY;
+		goto out2;
+	}
+
+	d40c->cdesc = cdesc;
+	d40_usage_dec(d40c);
+	spin_unlock_irqrestore(&d40c->lock, flags);
+	return cdesc;
+out2:
+	d40_usage_dec(d40c);
+out:
+	if (d40c->phy_chan)
+		d40_lcla_free_all(d40c, cdesc->d40d);
+	kfree(cdesc);
+	spin_unlock_irqrestore(&d40c->lock, flags);
+	return ERR_PTR(err);
+}
+EXPORT_SYMBOL(stedma40_cyclic_prep_sg);
+
 /* Initialization functions */
 
 static void __init d40_chan_init(struct d40_base *base, struct dma_device *dma,
@@ -2103,21 +3199,15 @@ static void __init d40_chan_init(struct d40_base *base, struct dma_device *dma,
 		d40c->base = base;
 		d40c->chan.device = dma;
 
-		/* Invalidate lcla element */
-		d40c->lcla.src_id = -1;
-		d40c->lcla.dst_id = -1;
-
 		spin_lock_init(&d40c->lock);
 
 		d40c->log_num = D40_PHY_CHAN;
 
-		INIT_LIST_HEAD(&d40c->free);
+		INIT_LIST_HEAD(&d40c->done);
 		INIT_LIST_HEAD(&d40c->active);
 		INIT_LIST_HEAD(&d40c->queue);
 		INIT_LIST_HEAD(&d40c->client);
 
-		d40c->free_len = 0;
-
 		tasklet_init(&d40c->tasklet, dma_tasklet,
 			     (unsigned long) d40c);
 
@@ -2142,8 +3232,8 @@ static int __init d40_dmaengine_init(struct d40_base *base,
 	base->dma_slave.device_prep_dma_memcpy = d40_prep_memcpy;
 	base->dma_slave.device_prep_slave_sg = d40_prep_slave_sg;
 	base->dma_slave.device_tx_status = d40_tx_status;
-	base->dma_slave.device_issue_pending = d40_issue_pending;
 	base->dma_slave.device_control = d40_control;
+	base->dma_slave.device_issue_pending = d40_issue_pending;
 	base->dma_slave.dev = base->dev;
 
 	err = dma_async_device_register(&base->dma_slave);
@@ -2166,8 +3256,8 @@ static int __init d40_dmaengine_init(struct d40_base *base,
 	base->dma_memcpy.device_prep_dma_memcpy = d40_prep_memcpy;
 	base->dma_memcpy.device_prep_slave_sg = d40_prep_slave_sg;
 	base->dma_memcpy.device_tx_status = d40_tx_status;
-	base->dma_memcpy.device_issue_pending = d40_issue_pending;
 	base->dma_memcpy.device_control = d40_control;
+	base->dma_memcpy.device_issue_pending = d40_issue_pending;
 	base->dma_memcpy.dev = base->dev;
 	/*
 	 * This controller can only access address at even
@@ -2196,8 +3286,9 @@ static int __init d40_dmaengine_init(struct d40_base *base,
 	base->dma_both.device_prep_dma_memcpy = d40_prep_memcpy;
 	base->dma_both.device_prep_slave_sg = d40_prep_slave_sg;
 	base->dma_both.device_tx_status = d40_tx_status;
-	base->dma_both.device_issue_pending = d40_issue_pending;
 	base->dma_both.device_control = d40_control;
+	base->dma_both.device_issue_pending = d40_issue_pending;
+
 	base->dma_both.dev = base->dev;
 	base->dma_both.copy_align = 2;
 	err = dma_async_device_register(&base->dma_both);
@@ -2217,6 +3308,26 @@ failure1:
 	return err;
 }
 
+/* Suspend resume functionality */
+#ifdef CONFIG_PM
+static int d40_pm_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	unsigned long flags;
+	int ret = 0;
+	struct d40_base *base = platform_get_drvdata(pdev);
+
+	spin_lock_irqsave(&base->usage_lock, flags);
+	if (base->usage)
+		ret = -EINVAL;
+	spin_unlock_irqrestore(&base->usage_lock, flags);
+
+	return ret;
+
+}
+#else
+#define d40_pm_suspend NULL
+#endif
+
 /* Initialization functions. */
 
 static int __init d40_phy_res_init(struct d40_base *base)
@@ -2224,6 +3335,7 @@ static int __init d40_phy_res_init(struct d40_base *base)
 	int i;
 	int num_phy_chans_avail = 0;
 	u32 val[2];
+	u32 gcc = D40_DREG_GCC_ENA;
 	int odd_even_bit = -2;
 
 	val[0] = readl(base->virtbase + D40_DREG_PRSME);
@@ -2232,17 +3344,37 @@ static int __init d40_phy_res_init(struct d40_base *base)
 	for (i = 0; i < base->num_phy_chans; i++) {
 		base->phy_res[i].num = i;
 		odd_even_bit += 2 * ((i % 2) == 0);
-		if (((val[i % 2] >> odd_even_bit) & 3) == 1) {
+		if (((val[i % 2] >> odd_even_bit) & D40_DREG_PRSM_MODE_MASK)
+		      == D40_DREG_PRSM_MODE_SECURE) {
 			/* Mark security only channels as occupied */
 			base->phy_res[i].allocated_src = D40_ALLOC_PHY;
 			base->phy_res[i].allocated_dst = D40_ALLOC_PHY;
+			base->phy_res[i].reserved = true;
+
+			gcc |= D40_DREG_GCC_EVTGRP_ENA(D40_PHYS_TO_GROUP(i),
+						       D40_DREG_GCC_DST);
+			gcc |= D40_DREG_GCC_EVTGRP_ENA(D40_PHYS_TO_GROUP(i),
+						       D40_DREG_GCC_SRC);
+
 		} else {
 			base->phy_res[i].allocated_src = D40_ALLOC_FREE;
 			base->phy_res[i].allocated_dst = D40_ALLOC_FREE;
+			base->phy_res[i].reserved = false;
 			num_phy_chans_avail++;
 		}
 		spin_lock_init(&base->phy_res[i].lock);
 	}
+
+	/* Mark disabled channels as occupied */
+	for (i = 0; base->plat_data->disabled_channels[i] != -1; i++) {
+		int chan = base->plat_data->disabled_channels[i];
+
+		base->phy_res[chan].allocated_src = D40_ALLOC_PHY;
+		base->phy_res[chan].allocated_dst = D40_ALLOC_PHY;
+		base->phy_res[chan].reserved = true;
+		num_phy_chans_avail--;
+	}
+
 	dev_info(base->dev, "%d of %d physical DMA channels available\n",
 		 num_phy_chans_avail, base->num_phy_chans);
 
@@ -2260,6 +3392,9 @@ static int __init d40_phy_res_init(struct d40_base *base)
 		val[0] = val[0] >> 2;
 	}
 
+	/* Enable all clocks -- revisit after HW bug is fixed */
+	writel(D40_DREG_GCC_ENABLE_ALL, base->virtbase + D40_DREG_GCC);
+
 	return num_phy_chans_avail;
 }
 
@@ -2271,9 +3406,10 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 		{ .reg = D40_DREG_PERIPHID1, .val = 0x0000},
 		/*
 		 * D40_DREG_PERIPHID2 Depends on HW revision:
-		 *  MOP500/HREF ED has 0x0008,
+		 *  DB8500ed has 0x0008,
 		 *  ? has 0x0018,
-		 *  HREF V1 has 0x0028
+		 *  DB8500v1 has 0x0028
+		 *  DB8500v2 has 0x0038
 		 */
 		{ .reg = D40_DREG_PERIPHID3, .val = 0x0000},
 
@@ -2285,12 +3421,14 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 	};
 	struct stedma40_platform_data *plat_data;
 	struct clk *clk = NULL;
+	struct ux500_regulator *regulator = NULL;
 	void __iomem *virtbase = NULL;
 	struct resource *res = NULL;
 	struct d40_base *base = NULL;
 	int num_log_chans = 0;
 	int num_phy_chans;
 	int i;
+	u32 val;
 
 	clk = clk_get(&pdev->dev, NULL);
 
@@ -2300,6 +3438,20 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 		goto failure;
 	}
 
+	regulator = ux500_regulator_get(&pdev->dev);
+	if (IS_ERR(regulator)) {
+		dev_warn(&pdev->dev, "[%s] No matching regulator found\n",
+			__func__);
+		regulator = NULL;
+	}
+
+	if (regulator)
+		ux500_regulator_atomic_enable(regulator);
+
+	/*
+	 * Since the secure world does not handle clock, we have to
+	 * let it run all the time
+	 */
 	clk_enable(clk);
 
 	/* Get IO for DMAC base address */
@@ -2329,12 +3481,13 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 		}
 	}
 
-	i = readl(virtbase + D40_DREG_PERIPHID2);
+	/* Get silicon revision */
+	val = readl(virtbase + D40_DREG_PERIPHID2);
 
-	if ((i & 0xf) != D40_PERIPHID2_DESIGNER) {
+	if ((val & 0xf) != D40_PERIPHID2_DESIGNER) {
 		dev_err(&pdev->dev,
 			"[%s] Unknown designer! Got %x wanted %x\n",
-			__func__, i & 0xf, D40_PERIPHID2_DESIGNER);
+			__func__, val & 0xf, D40_PERIPHID2_DESIGNER);
 		goto failure;
 	}
 
@@ -2342,7 +3495,7 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 	num_phy_chans = 4 * (readl(virtbase + D40_DREG_ICFG) & 0x7) + 4;
 
 	dev_info(&pdev->dev, "hardware revision: %d @ 0x%x\n",
-		 (i >> 4) & 0xf, res->start);
+		 (val >> 4) & 0xf, res->start);
 
 	plat_data = pdev->dev.platform_data;
 
@@ -2364,7 +3517,9 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 		goto failure;
 	}
 
+	base->rev = (val >> 4) & 0xf;
 	base->clk = clk;
+	base->regulator = regulator;
 	base->num_phy_chans = num_phy_chans;
 	base->num_log_chans = num_log_chans;
 	base->phy_start = res->start;
@@ -2374,6 +3529,7 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 	base->dev = &pdev->dev;
 	base->phy_chans = ((void *)base) + ALIGN(sizeof(struct d40_base), 4);
 	base->log_chans = &base->phy_chans[num_phy_chans];
+	base->usage = 1;
 
 	base->phy_res = kzalloc(num_phy_chans * sizeof(struct d40_phy_res),
 				GFP_KERNEL);
@@ -2397,15 +3553,34 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 		if (!base->lookup_log_chans)
 			goto failure;
 	}
-	base->lcla_pool.alloc_map = kzalloc(num_phy_chans * sizeof(u32),
+
+	base->reg_val_backup_chan = kmalloc(base->num_phy_chans *
+					    sizeof(d40_backup_regs_chan),
 					    GFP_KERNEL);
+	if (!base->reg_val_backup_chan)
+		goto failure;
+
+	base->lcla_pool.alloc_map =
+		kzalloc(num_phy_chans * sizeof(struct d40_desc *)
+			* D40_LCLA_LINK_PER_EVENT_GRP, GFP_KERNEL);
 	if (!base->lcla_pool.alloc_map)
 		goto failure;
 
+	base->desc_slab = kmem_cache_create(D40_NAME, sizeof(struct d40_desc),
+					    0, SLAB_HWCACHE_ALIGN,
+					    NULL);
+	if (base->desc_slab == NULL)
+		goto failure;
+
 	return base;
 
 failure:
-	if (clk) {
+	if (regulator) {
+		ux500_regulator_atomic_disable(regulator);
+		ux500_regulator_put(regulator);
+	}
+
+	if (!IS_ERR(clk)) {
 		clk_disable(clk);
 		clk_put(clk);
 	}
@@ -2419,6 +3594,7 @@ failure:
 
 	if (base) {
 		kfree(base->lcla_pool.alloc_map);
+		kfree(base->reg_val_backup_chan);
 		kfree(base->lookup_log_chans);
 		kfree(base->lookup_phy_chans);
 		kfree(base->phy_res);
@@ -2431,9 +3607,9 @@ failure:
 static void __init d40_hw_init(struct d40_base *base)
 {
 
-	static const struct d40_reg_val dma_init_reg[] = {
+	static struct d40_reg_val dma_init_reg[] = {
 		/* Clock every part of the DMA block from start */
-		{ .reg = D40_DREG_GCC,    .val = 0x0000ff01},
+		{ .reg = D40_DREG_GCC,    .val = D40_DREG_GCC_ENABLE_ALL},
 
 		/* Interrupts on all logical channels */
 		{ .reg = D40_DREG_LCMIS0, .val = 0xFFFFFFFF},
@@ -2495,6 +3671,81 @@ static void __init d40_hw_init(struct d40_base *base)
 
 }
 
+static int __init d40_lcla_allocate(struct d40_base *base)
+{
+	unsigned long *page_list;
+	int i;
+	int j;
+	int ret = 0;
+
+	/*
+	 * This is somewhat ugly. We need 8192 bytes that are 18 bit aligned,
+	 * To full fill this hardware requirement without wasting 256 kb
+	 * we allocate pages until we get an aligned one.
+	 */
+	page_list = kmalloc(sizeof(unsigned long) * MAX_LCLA_ALLOC_ATTEMPTS,
+			    GFP_KERNEL);
+
+	if (!page_list) {
+		ret = -ENOMEM;
+		goto failure;
+	}
+
+	/* Calculating how many pages that are required */
+	base->lcla_pool.pages = SZ_1K * base->num_phy_chans / PAGE_SIZE;
+
+	for (i = 0; i < MAX_LCLA_ALLOC_ATTEMPTS; i++) {
+		page_list[i] = __get_free_pages(GFP_KERNEL,
+						base->lcla_pool.pages);
+		if (!page_list[i]) {
+
+			dev_err(base->dev,
+				"[%s] Failed to allocate %d pages.\n",
+				__func__, base->lcla_pool.pages);
+
+			for (j = 0; j < i; j++)
+				free_pages(page_list[j], base->lcla_pool.pages);
+			goto failure;
+		}
+
+		if ((virt_to_phys((void *)page_list[i]) &
+		     (LCLA_ALIGNMENT - 1)) == 0)
+			break;
+	}
+
+	for (j = 0; j < i; j++)
+		free_pages(page_list[j], base->lcla_pool.pages);
+
+	if (i < MAX_LCLA_ALLOC_ATTEMPTS) {
+		base->lcla_pool.base = (void *)page_list[i];
+	} else {
+		/*
+		 * After many attempts, no succees with finding the correct
+		 * alignment try with allocating a big buffer.
+		 */
+		dev_warn(base->dev,
+			 "[%s] Failed to get %d pages @ 18 bit align.\n",
+			 __func__, base->lcla_pool.pages);
+		base->lcla_pool.base_unaligned = kmalloc(SZ_1K *
+							 base->num_phy_chans +
+							 LCLA_ALIGNMENT,
+							 GFP_KERNEL);
+		if (!base->lcla_pool.base_unaligned) {
+			ret = -ENOMEM;
+			goto failure;
+		}
+
+		base->lcla_pool.base = PTR_ALIGN(base->lcla_pool.base_unaligned,
+						 LCLA_ALIGNMENT);
+	}
+
+	writel(virt_to_phys(base->lcla_pool.base),
+	       base->virtbase + D40_DREG_LCLA);
+failure:
+	kfree(page_list);
+	return ret;
+}
+
 static int __init d40_probe(struct platform_device *pdev)
 {
 	int err;
@@ -2503,6 +3754,7 @@ static int __init d40_probe(struct platform_device *pdev)
 	struct resource *res = NULL;
 	int num_reserved_chans;
 	u32 val;
+	unsigned long flags;
 
 	base = d40_hw_detect_init(pdev);
 
@@ -2515,6 +3767,7 @@ static int __init d40_probe(struct platform_device *pdev)
 
 	spin_lock_init(&base->interrupt_lock);
 	spin_lock_init(&base->execmd_lock);
+	spin_lock_init(&base->usage_lock);
 
 	/* Get IO for logical channel parameter address */
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "lcpa");
@@ -2554,48 +3807,25 @@ static int __init d40_probe(struct platform_device *pdev)
 			__func__);
 		goto failure;
 	}
-	/* Get IO for logical channel link address */
-	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "lcla");
-	if (!res) {
-		ret = -ENOENT;
-		dev_err(&pdev->dev,
-			"[%s] No \"lcla\" resource defined\n",
-			__func__);
-		goto failure;
-	}
 
-	base->lcla_pool.base_size = resource_size(res);
-	base->lcla_pool.phy = res->start;
+	ret = d40_lcla_allocate(base);
 
-	if (request_mem_region(res->start, resource_size(res),
-			       D40_NAME " I/O lcla") == NULL) {
-		ret = -EBUSY;
-		dev_err(&pdev->dev,
-			"[%s] Failed to request LCLA region 0x%x-0x%x\n",
-			__func__, res->start, res->end);
+	if (ret) {
+		dev_err(&pdev->dev, "[%s] Failed to allocate LCLA area\n",
+			__func__);
 		goto failure;
-	}
-	val = readl(base->virtbase + D40_DREG_LCLA);
-	if (res->start != val && val != 0) {
-		dev_warn(&pdev->dev,
-			 "[%s] Mismatch LCLA dma 0x%x, def 0x%x\n",
-			 __func__, val, res->start);
-	} else
-		writel(res->start, base->virtbase + D40_DREG_LCLA);
 
-	base->lcla_pool.base = ioremap(res->start, resource_size(res));
-	if (!base->lcla_pool.base) {
-		ret = -ENOMEM;
-		dev_err(&pdev->dev,
-			"[%s] Failed to ioremap LCLA 0x%x-0x%x\n",
-			__func__, res->start, res->end);
-		goto failure;
 	}
 
+#ifdef CONFIG_STE_DMA40_DEBUG
+	sted40_history_set_virtbase(base->virtbase,
+				    base->lcpa_base,
+				    base->lcpa_size,
+				    base->lcla_pool.base,
+				    SZ_1K * base->num_phy_chans);
+#endif
 	spin_lock_init(&base->lcla_pool.lock);
 
-	base->lcla_pool.num_blocks = base->num_phy_chans;
-
 	base->irq = platform_get_irq(pdev, 0);
 
 	ret = request_irq(base->irq, d40_handle_interrupt, 0, D40_NAME, base);
@@ -2611,16 +3841,31 @@ static int __init d40_probe(struct platform_device *pdev)
 
 	d40_hw_init(base);
 
+	spin_lock_irqsave(&base->usage_lock, flags);
+
+	base->usage--;
+	d40_save_restore_registers(base, true);
+
+	if (base->regulator)
+		ux500_regulator_atomic_disable(base->regulator);
+
+	spin_unlock_irqrestore(&base->usage_lock, flags);
+
 	dev_info(base->dev, "initialized\n");
 	return 0;
 
 failure:
 	if (base) {
+		if (base->desc_slab)
+			kmem_cache_destroy(base->desc_slab);
 		if (base->virtbase)
 			iounmap(base->virtbase);
-		if (base->lcla_pool.phy)
-			release_mem_region(base->lcla_pool.phy,
-					   base->lcla_pool.base_size);
+		if (!base->lcla_pool.base_unaligned && base->lcla_pool.base)
+			free_pages((unsigned long)base->lcla_pool.base,
+				   base->lcla_pool.pages);
+
+		kfree(base->lcla_pool.base_unaligned);
+
 		if (base->phy_lcpa)
 			release_mem_region(base->phy_lcpa,
 					   base->lcpa_size);
@@ -2632,6 +3877,11 @@ failure:
 			clk_put(base->clk);
 		}
 
+		if (base->regulator) {
+			ux500_regulator_atomic_disable(base->regulator);
+			ux500_regulator_put(base->regulator);
+		}
+
 		kfree(base->lcla_pool.alloc_map);
 		kfree(base->lookup_log_chans);
 		kfree(base->lookup_phy_chans);
@@ -2648,6 +3898,7 @@ static struct platform_driver d40_driver = {
 		.owner = THIS_MODULE,
 		.name  = D40_NAME,
 	},
+	.suspend = d40_pm_suspend,
 };
 
 int __init stedma40_init(void)