MFC r312882, r321983 and r321984:

Use the busdma API to allocate all DMA-able memory.

The MLX5 driver has four different types of DMA allocations which are
now allocated using busdma:

1) The 4K firmware DMA-able blocks. One busdma object per 4K allocation.
2) Data for firmware commands use the 4K firmware blocks split into four 1K blocks.
3) The 4K firmware blocks are also used for doorbell pages.
4) The RQ-, SQ- and CQ- DMA rings. One busdma object per allocation.

After this patch the mlx5en driver can be used with DMAR enabled in
the FreeBSD kernel.

Sponsored by:	Mellanox Technologies

5 files changed, 581 insertions, 472 deletions

diff --git a/sys/dev/mlx5/device.h b/sys/dev/mlx5/device.h
index de1d7f9..47b263a 100644
--- a/sys/dev/mlx5/device.h
+++ b/sys/dev/mlx5/device.h
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2013-2015, Mellanox Technologies, Ltd.  All rights reserved.
+ * Copyright (c) 2013-2017, Mellanox Technologies, Ltd.  All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -103,6 +103,7 @@ __mlx5_mask(typ, fld))
 enum {
 	MLX5_MAX_COMMANDS		= 32,
 	MLX5_CMD_DATA_BLOCK_SIZE	= 512,
+	MLX5_CMD_MBOX_SIZE		= 1024,
 	MLX5_PCI_CMD_XPORT		= 7,
 	MLX5_MKEY_BSF_OCTO_SIZE		= 4,
 	MLX5_MAX_PSVS			= 4,
@@ -523,6 +524,11 @@ struct mlx5_cmd_prot_block {
 	u8		sig;
 };
 
+#define	MLX5_NUM_CMDS_IN_ADAPTER_PAGE \
+	(MLX5_ADAPTER_PAGE_SIZE / MLX5_CMD_MBOX_SIZE)
+CTASSERT(MLX5_CMD_MBOX_SIZE >= sizeof(struct mlx5_cmd_prot_block));
+CTASSERT(MLX5_CMD_MBOX_SIZE <= MLX5_ADAPTER_PAGE_SIZE);
+
 enum {
 	MLX5_CQE_SYND_FLUSHED_IN_ERROR = 5,
 };
diff --git a/sys/dev/mlx5/driver.h b/sys/dev/mlx5/driver.h
index e5e8a70..3371969 100644
--- a/sys/dev/mlx5/driver.h
+++ b/sys/dev/mlx5/driver.h
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2013-2015, Mellanox Technologies, Ltd.  All rights reserved.
+ * Copyright (c) 2013-2017, Mellanox Technologies, Ltd.  All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -258,13 +258,26 @@ struct mlx5_cmd_first {
 	__be32		data[4];
 };
 
-struct mlx5_cmd_msg {
-	struct list_head		list;
-	struct cache_ent	       *cache;
-	u32				len;
-	struct mlx5_cmd_first		first;
-	struct mlx5_cmd_mailbox	       *next;
-};
+struct cache_ent;
+struct mlx5_fw_page {
+	union {
+		struct rb_node rb_node;
+		struct list_head list;
+	};
+	struct mlx5_cmd_first first;
+	struct mlx5_core_dev *dev;
+	bus_dmamap_t dma_map;
+	bus_addr_t dma_addr;
+	void *virt_addr;
+	struct cache_ent *cache;
+	u32 numpages;
+	u16 load_done;
+#define	MLX5_LOAD_ST_NONE 0
+#define	MLX5_LOAD_ST_SUCCESS 1
+#define	MLX5_LOAD_ST_FAILURE 2
+	u16 func_id;
+};
+#define	mlx5_cmd_msg mlx5_fw_page
 
 struct mlx5_cmd_debug {
 	struct dentry	       *dbg_root;
@@ -304,9 +317,16 @@ struct mlx5_cmd_stats {
 };
 
 struct mlx5_cmd {
-	void	       *cmd_alloc_buf;
-	dma_addr_t	alloc_dma;
-	int		alloc_size;
+	struct mlx5_fw_page *cmd_page;
+	bus_dma_tag_t dma_tag;
+	struct sx dma_sx;
+	struct mtx dma_mtx;
+#define	MLX5_DMA_OWNED(dev) mtx_owned(&(dev)->cmd.dma_mtx)
+#define	MLX5_DMA_LOCK(dev) mtx_lock(&(dev)->cmd.dma_mtx)
+#define	MLX5_DMA_UNLOCK(dev) mtx_unlock(&(dev)->cmd.dma_mtx)
+	struct cv dma_cv;
+#define	MLX5_DMA_DONE(dev) cv_broadcast(&(dev)->cmd.dma_cv)
+#define	MLX5_DMA_WAIT(dev) cv_wait(&(dev)->cmd.dma_cv, &(dev)->cmd.dma_mtx)
 	void	       *cmd_buf;
 	dma_addr_t	dma;
 	u16		cmdif_rev;
@@ -331,7 +351,6 @@ struct mlx5_cmd {
 	struct semaphore pages_sem;
 	int	mode;
 	struct mlx5_cmd_work_ent *ent_arr[MLX5_MAX_COMMANDS];
-	struct pci_pool *pool;
 	struct mlx5_cmd_debug dbg;
 	struct cmd_msg_cache cache;
 	int checksum_disabled;
@@ -345,24 +364,18 @@ struct mlx5_port_caps {
 	u8	ext_port_cap;
 };
 
-struct mlx5_cmd_mailbox {
-	void	       *buf;
-	dma_addr_t	dma;
-	struct mlx5_cmd_mailbox *next;
-};
-
-struct mlx5_buf_list {
-	void		       *buf;
-	dma_addr_t		map;
-};
-
 struct mlx5_buf {
-	struct mlx5_buf_list	direct;
-	struct mlx5_buf_list   *page_list;
-	int			nbufs;
+	bus_dma_tag_t		dma_tag;
+	bus_dmamap_t		dma_map;
+	struct mlx5_core_dev   *dev;
+	struct {
+		void	       *buf;
+	} direct;
+	u64		       *page_list;
 	int			npages;
 	int			size;
 	u8			page_shift;
+	u8			load_done;
 };
 
 struct mlx5_eq {
@@ -521,7 +534,6 @@ struct mlx5_priv {
 	struct rb_root		page_root;
 	s64			fw_pages;
 	atomic_t		reg_pages;
-	struct list_head	free_list;
 	s64			pages_per_func[MLX5_MAX_NUMBER_OF_VFS];
 	struct mlx5_core_health health;
 
@@ -664,7 +676,7 @@ struct mlx5_vport_counters {
 };
 
 enum {
-	MLX5_DB_PER_PAGE = PAGE_SIZE / L1_CACHE_BYTES,
+	MLX5_DB_PER_PAGE = MLX5_ADAPTER_PAGE_SIZE / L1_CACHE_BYTES,
 };
 
 struct mlx5_core_dct {
@@ -688,6 +700,7 @@ enum {
 struct mlx5_db_pgdir {
 	struct list_head	list;
 	DECLARE_BITMAP(bitmap, MLX5_DB_PER_PAGE);
+	struct mlx5_fw_page    *fw_page;
 	__be32		       *db_page;
 	dma_addr_t		db_dma;
 };
@@ -697,6 +710,7 @@ typedef void (*mlx5_cmd_cbk_t)(int status, void *context);
 struct mlx5_cmd_work_ent {
 	struct mlx5_cmd_msg    *in;
 	struct mlx5_cmd_msg    *out;
+	int			uin_size;
 	void		       *uout;
 	int			uout_size;
 	mlx5_cmd_cbk_t		callback;
@@ -721,13 +735,10 @@ struct mlx5_pas {
 	u8	log_sz;
 };
 
-static inline void *mlx5_buf_offset(struct mlx5_buf *buf, int offset)
+static inline void *
+mlx5_buf_offset(struct mlx5_buf *buf, int offset)
 {
-	if (likely(BITS_PER_LONG == 64 || buf->nbufs == 1))
-		return buf->direct.buf + offset;
-	else
-		return buf->page_list[offset >> PAGE_SHIFT].buf +
-			(offset & (PAGE_SIZE - 1));
+	return ((char *)buf->direct.buf + offset);
 }
 
 
@@ -816,8 +827,9 @@ void mlx5_health_cleanup(void);
 void  __init mlx5_health_init(void);
 void mlx5_start_health_poll(struct mlx5_core_dev *dev);
 void mlx5_stop_health_poll(struct mlx5_core_dev *dev);
-int mlx5_buf_alloc_node(struct mlx5_core_dev *dev, int size, int max_direct,
-			struct mlx5_buf *buf, int node);
+
+#define	mlx5_buf_alloc_node(dev, size, direct, buf, node) \
+	mlx5_buf_alloc(dev, size, direct, buf)
 int mlx5_buf_alloc(struct mlx5_core_dev *dev, int size, int max_direct,
 		   struct mlx5_buf *buf);
 void mlx5_buf_free(struct mlx5_core_dev *dev, struct mlx5_buf *buf);
@@ -845,6 +857,12 @@ int mlx5_core_alloc_pd(struct mlx5_core_dev *dev, u32 *pdn);
 int mlx5_core_dealloc_pd(struct mlx5_core_dev *dev, u32 pdn);
 int mlx5_core_mad_ifc(struct mlx5_core_dev *dev, void *inb, void *outb,
 		      u16 opmod, u8 port);
+void mlx5_fwp_flush(struct mlx5_fw_page *fwp);
+void mlx5_fwp_invalidate(struct mlx5_fw_page *fwp);
+struct mlx5_fw_page *mlx5_fwp_alloc(struct mlx5_core_dev *dev, gfp_t flags, unsigned num);
+void mlx5_fwp_free(struct mlx5_fw_page *fwp);
+u64 mlx5_fwp_get_dma(struct mlx5_fw_page *fwp, size_t offset);
+void *mlx5_fwp_get_virt(struct mlx5_fw_page *fwp, size_t offset);
 void mlx5_pagealloc_init(struct mlx5_core_dev *dev);
 void mlx5_pagealloc_cleanup(struct mlx5_core_dev *dev);
 int mlx5_pagealloc_start(struct mlx5_core_dev *dev);
diff --git a/sys/dev/mlx5/mlx5_core/mlx5_alloc.c b/sys/dev/mlx5/mlx5_core/mlx5_alloc.c
index 513fc6f..1dabbd9 100644
--- a/sys/dev/mlx5/mlx5_core/mlx5_alloc.c
+++ b/sys/dev/mlx5/mlx5_core/mlx5_alloc.c
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2013-2015, Mellanox Technologies, Ltd.  All rights reserved.
+ * Copyright (c) 2013-2017, Mellanox Technologies, Ltd.  All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -40,106 +40,110 @@
  * multiple pages, so we don't require too much contiguous memory.
  */
 
-static void *mlx5_dma_zalloc_coherent_node(struct mlx5_core_dev *dev,
-					   size_t size, dma_addr_t *dma_handle,
-					   int node)
+static void
+mlx5_buf_load_mem_cb(void *arg, bus_dma_segment_t *segs, int nseg, int error)
 {
-	void *cpu_handle;
+	struct mlx5_buf *buf;
+	uint8_t owned;
+	int x;
 
-	cpu_handle = dma_zalloc_coherent(&dev->pdev->dev, size,
-					 dma_handle, GFP_KERNEL);
-	return cpu_handle;
-}
+	buf = (struct mlx5_buf *)arg;
+	owned = MLX5_DMA_OWNED(buf->dev);
 
-int mlx5_buf_alloc_node(struct mlx5_core_dev *dev, int size, int max_direct,
-			struct mlx5_buf *buf, int node)
-{
-	dma_addr_t t;
-
-	buf->size = size;
-	if (size <= max_direct) {
-		buf->nbufs        = 1;
-		buf->npages       = 1;
-		buf->page_shift   = (u8)get_order(size) + PAGE_SHIFT;
-		buf->direct.buf   = mlx5_dma_zalloc_coherent_node(dev, size,
-								  &t, node);
-		if (!buf->direct.buf)
-			return -ENOMEM;
-
-		buf->direct.map = t;
-
-		while (t & ((1 << buf->page_shift) - 1)) {
-			--buf->page_shift;
-			buf->npages *= 2;
+	if (!owned)
+		MLX5_DMA_LOCK(buf->dev);
+
+	if (error == 0) {
+		for (x = 0; x != nseg; x++) {
+			buf->page_list[x] = segs[x].ds_addr;
+			KASSERT(segs[x].ds_len == PAGE_SIZE, ("Invalid segment size"));
 		}
+		buf->load_done = MLX5_LOAD_ST_SUCCESS;
 	} else {
-		int i;
-
-		buf->direct.buf  = NULL;
-		buf->nbufs       = (size + PAGE_SIZE - 1) / PAGE_SIZE;
-		buf->npages      = buf->nbufs;
-		buf->page_shift  = PAGE_SHIFT;
-		buf->page_list   = kcalloc(buf->nbufs, sizeof(*buf->page_list),
-					   GFP_KERNEL);
-
-		for (i = 0; i < buf->nbufs; i++) {
-			buf->page_list[i].buf =
-				mlx5_dma_zalloc_coherent_node(dev, PAGE_SIZE,
-							      &t, node);
+		buf->load_done = MLX5_LOAD_ST_FAILURE;
+	}
+	MLX5_DMA_DONE(buf->dev);
 
-			buf->page_list[i].map = t;
-		}
+	if (!owned)
+		MLX5_DMA_UNLOCK(buf->dev);
+}
 
-		if (BITS_PER_LONG == 64) {
-			struct page **pages;
-
-			pages = kmalloc(sizeof(*pages) * (buf->nbufs + 1),
-					GFP_KERNEL);
-			for (i = 0; i < buf->nbufs; i++)
-				pages[i] = virt_to_page(buf->page_list[i].buf);
-			pages[buf->nbufs] = pages[0];
-			buf->direct.buf = vmap(pages, buf->nbufs + 1, VM_MAP,
-					       PAGE_KERNEL);
-			kfree(pages);
-			if (!buf->direct.buf)
-				goto err_free;
-		}
+int
+mlx5_buf_alloc(struct mlx5_core_dev *dev, int size,
+    int max_direct, struct mlx5_buf *buf)
+{
+	int err;
+
+	buf->npages = howmany(size, PAGE_SIZE);
+	buf->page_shift = PAGE_SHIFT;
+	buf->load_done = MLX5_LOAD_ST_NONE;
+	buf->dev = dev;
+	buf->page_list = kcalloc(buf->npages, sizeof(*buf->page_list),
+	    GFP_KERNEL);
+
+	err = -bus_dma_tag_create(
+	    bus_get_dma_tag(dev->pdev->dev.bsddev),
+	    PAGE_SIZE,		/* alignment */
+	    0,			/* no boundary */
+	    BUS_SPACE_MAXADDR,	/* lowaddr */
+	    BUS_SPACE_MAXADDR,	/* highaddr */
+	    NULL, NULL,		/* filter, filterarg */
+	    PAGE_SIZE * buf->npages,	/* maxsize */
+	    buf->npages,	/* nsegments */
+	    PAGE_SIZE,		/* maxsegsize */
+	    0,			/* flags */
+	    NULL, NULL,		/* lockfunc, lockfuncarg */
+	    &buf->dma_tag);
+
+	if (err != 0)
+		goto err_dma_tag;
+
+	/* allocate memory */
+	err = -bus_dmamem_alloc(buf->dma_tag, &buf->direct.buf,
+	    BUS_DMA_WAITOK | BUS_DMA_COHERENT, &buf->dma_map);
+	if (err != 0)
+		goto err_dma_alloc;
+
+	/* load memory into DMA */
+	MLX5_DMA_LOCK(dev);
+	err = bus_dmamap_load(
+	    buf->dma_tag, buf->dma_map, buf->direct.buf,
+	    PAGE_SIZE * buf->npages, &mlx5_buf_load_mem_cb,
+	    buf, BUS_DMA_WAITOK | BUS_DMA_COHERENT);
+
+	while (buf->load_done == MLX5_LOAD_ST_NONE)
+		MLX5_DMA_WAIT(dev);
+	MLX5_DMA_UNLOCK(dev);
+
+	/* check for error */
+	if (buf->load_done != MLX5_LOAD_ST_SUCCESS) {
+		err = -ENOMEM;
+		goto err_dma_load;
 	}
 
-	return 0;
+	/* clean memory */
+	memset(buf->direct.buf, 0, PAGE_SIZE * buf->npages);
 
-err_free:
-	mlx5_buf_free(dev, buf);
+	/* flush memory to RAM */
+	bus_dmamap_sync(buf->dev->cmd.dma_tag, buf->dma_map, BUS_DMASYNC_PREWRITE);
+	return (0);
 
-	return -ENOMEM;
+err_dma_load:
+	bus_dmamem_free(buf->dma_tag, buf->direct.buf, buf->dma_map);
+err_dma_alloc:
+	bus_dma_tag_destroy(buf->dma_tag);
+err_dma_tag:
+	kfree(buf->page_list);
+	return (err);
 }
 
-int mlx5_buf_alloc(struct mlx5_core_dev *dev, int size, int max_direct,
-		   struct mlx5_buf *buf)
-{
-	return mlx5_buf_alloc_node(dev, size, max_direct,
-				   buf, dev->priv.numa_node);
-}
-EXPORT_SYMBOL_GPL(mlx5_buf_alloc);
-
-
 void mlx5_buf_free(struct mlx5_core_dev *dev, struct mlx5_buf *buf)
 {
-	if (buf->nbufs == 1)
-		dma_free_coherent(&dev->pdev->dev, buf->size, buf->direct.buf,
-				  buf->direct.map);
-	else {
-		int i;
-		if (BITS_PER_LONG == 64 && buf->direct.buf)
-			vunmap(buf->direct.buf);
-
-		for (i = 0; i < buf->nbufs; i++)
-			if (buf->page_list[i].buf)
-				dma_free_coherent(&dev->pdev->dev, PAGE_SIZE,
-						  buf->page_list[i].buf,
-						  buf->page_list[i].map);
-		kfree(buf->page_list);
-	}
+
+	bus_dmamap_unload(buf->dma_tag, buf->dma_map);
+	bus_dmamem_free(buf->dma_tag, buf->direct.buf, buf->dma_map);
+	bus_dma_tag_destroy(buf->dma_tag);
+	kfree(buf->page_list);
 }
 EXPORT_SYMBOL_GPL(mlx5_buf_free);
 
@@ -152,8 +156,17 @@ static struct mlx5_db_pgdir *mlx5_alloc_db_pgdir(struct mlx5_core_dev *dev,
 
 	bitmap_fill(pgdir->bitmap, MLX5_DB_PER_PAGE);
 
-	pgdir->db_page = mlx5_dma_zalloc_coherent_node(dev, PAGE_SIZE,
-						       &pgdir->db_dma, node);
+	pgdir->fw_page = mlx5_fwp_alloc(dev, GFP_KERNEL, 1);
+	if (pgdir->fw_page != NULL) {
+		pgdir->db_page = pgdir->fw_page->virt_addr;
+		pgdir->db_dma = pgdir->fw_page->dma_addr;
+
+		/* clean allocated memory */
+		memset(pgdir->db_page, 0, MLX5_ADAPTER_PAGE_SIZE);
+
+		/* flush memory to RAM */
+		mlx5_fwp_flush(pgdir->fw_page);
+	}
 	if (!pgdir->db_page) {
 		kfree(pgdir);
 		return NULL;
@@ -228,8 +241,7 @@ void mlx5_db_free(struct mlx5_core_dev *dev, struct mlx5_db *db)
 	__set_bit(db->index, db->u.pgdir->bitmap);
 
 	if (bitmap_full(db->u.pgdir->bitmap, MLX5_DB_PER_PAGE)) {
-		dma_free_coherent(&(dev->pdev->dev), PAGE_SIZE,
-				  db->u.pgdir->db_page, db->u.pgdir->db_dma);
+		mlx5_fwp_free(db->u.pgdir->fw_page);
 		list_del(&db->u.pgdir->list);
 		kfree(db->u.pgdir);
 	}
@@ -238,19 +250,12 @@ void mlx5_db_free(struct mlx5_core_dev *dev, struct mlx5_db *db)
 }
 EXPORT_SYMBOL_GPL(mlx5_db_free);
 
-
-void mlx5_fill_page_array(struct mlx5_buf *buf, __be64 *pas)
+void
+mlx5_fill_page_array(struct mlx5_buf *buf, __be64 *pas)
 {
-	u64 addr;
 	int i;
 
-	for (i = 0; i < buf->npages; i++) {
-		if (buf->nbufs == 1)
-			addr = buf->direct.map + ((u64)i << buf->page_shift);
-		else
-			addr = buf->page_list[i].map;
-
-		pas[i] = cpu_to_be64(addr);
-	}
+	for (i = 0; i != buf->npages; i++)
+		pas[i] = cpu_to_be64(buf->page_list[i]);
 }
 EXPORT_SYMBOL_GPL(mlx5_fill_page_array);
diff --git a/sys/dev/mlx5/mlx5_core/mlx5_cmd.c b/sys/dev/mlx5/mlx5_core/mlx5_cmd.c
index 5bf6eb5..1105a47 100644
--- a/sys/dev/mlx5/mlx5_core/mlx5_cmd.c
+++ b/sys/dev/mlx5/mlx5_core/mlx5_cmd.c
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2013-2015, Mellanox Technologies, Ltd.  All rights reserved.
+ * Copyright (c) 2013-2017, Mellanox Technologies, Ltd.  All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -77,6 +77,7 @@ enum {
 
 static struct mlx5_cmd_work_ent *alloc_cmd(struct mlx5_cmd *cmd,
 					   struct mlx5_cmd_msg *in,
+					   int uin_size,
 					   struct mlx5_cmd_msg *out,
 					   void *uout, int uout_size,
 					   mlx5_cmd_cbk_t cbk,
@@ -90,6 +91,7 @@ static struct mlx5_cmd_work_ent *alloc_cmd(struct mlx5_cmd *cmd,
 		return ERR_PTR(-ENOMEM);
 
 	ent->in		= in;
+	ent->uin_size	= uin_size;
 	ent->out	= out;
 	ent->uout	= uout;
 	ent->uout_size	= uout_size;
@@ -192,14 +194,26 @@ static void calc_block_sig(struct mlx5_cmd_prot_block *block, u8 token,
 	}
 }
 
-static void calc_chain_sig(struct mlx5_cmd_msg *msg, u8 token, int csum)
+static void
+calc_chain_sig(struct mlx5_cmd_msg *msg, u8 token, int csum)
 {
-	struct mlx5_cmd_mailbox *next = msg->next;
+	size_t i;
 
-	while (next) {
-		calc_block_sig(next->buf, token, csum);
-		next = next->next;
+	for (i = 0; i != (msg->numpages * MLX5_NUM_CMDS_IN_ADAPTER_PAGE); i++) {
+		struct mlx5_cmd_prot_block *block;
+
+		block = mlx5_fwp_get_virt(msg, i * MLX5_CMD_MBOX_SIZE);
+
+		/* compute signature */
+		calc_block_sig(block, token, csum);
+
+		/* check for last block */
+		if (block->next == 0)
+			break;
 	}
+
+	/* make sure data gets written to RAM */
+	mlx5_fwp_flush(msg);
 }
 
 static void set_signature(struct mlx5_cmd_work_ent *ent, int csum)
@@ -235,10 +249,11 @@ static void free_cmd(struct mlx5_cmd_work_ent *ent)
 	kfree(ent);
 }
 
-
-static int verify_signature(struct mlx5_cmd_work_ent *ent)
+static int
+verify_signature(struct mlx5_cmd_work_ent *ent)
 {
-	struct mlx5_cmd_mailbox *next = ent->out->next;
+	struct mlx5_cmd_msg *msg = ent->out;
+	size_t i;
 	int err;
 	u8 sig;
 
@@ -246,15 +261,21 @@ static int verify_signature(struct mlx5_cmd_work_ent *ent)
 	if (sig != 0xff)
 		return -EINVAL;
 
-	while (next) {
-		err = verify_block_sig(next->buf);
-		if (err)
-			return err;
+	for (i = 0; i != (msg->numpages * MLX5_NUM_CMDS_IN_ADAPTER_PAGE); i++) {
+		struct mlx5_cmd_prot_block *block;
 
-		next = next->next;
-	}
+		block = mlx5_fwp_get_virt(msg, i * MLX5_CMD_MBOX_SIZE);
 
-	return 0;
+		/* compute signature */
+		err = verify_block_sig(block);
+		if (err != 0)
+			return (err);
+
+		/* check for last block */
+		if (block->next == 0)
+			break;
+	}
+	return (0);
 }
 
 static void dump_buf(void *buf, int size, int data_only, int offset)
@@ -681,9 +702,10 @@ static void dump_command(struct mlx5_core_dev *dev,
 {
 	u16 op = be16_to_cpu(((struct mlx5_inbox_hdr *)(ent->lay->in))->opcode);
 	struct mlx5_cmd_msg *msg = input ? ent->in : ent->out;
-	struct mlx5_cmd_mailbox *next = msg->next;
+	size_t i;
 	int data_only;
-	u32 offset = 0;
+	int offset = 0;
+	int msg_len = input ? ent->uin_size : ent->uout_size;
 	int dump_len;
 
 	data_only = !!(mlx5_core_debug_mask & (1 << MLX5_CMD_DATA));
@@ -711,17 +733,28 @@ static void dump_command(struct mlx5_core_dev *dev,
 		offset += sizeof(*ent->lay);
 	}
 
-	while (next && offset < msg->len) {
+	for (i = 0; i != (msg->numpages * MLX5_NUM_CMDS_IN_ADAPTER_PAGE); i++) {
+		struct mlx5_cmd_prot_block *block;
+
+		block = mlx5_fwp_get_virt(msg, i * MLX5_CMD_MBOX_SIZE);
+
 		if (data_only) {
-			dump_len = min_t(int, MLX5_CMD_DATA_BLOCK_SIZE, msg->len - offset);
-			dump_buf(next->buf, dump_len, 1, offset);
+			if (offset >= msg_len)
+				break;
+			dump_len = min_t(int,
+			    MLX5_CMD_DATA_BLOCK_SIZE, msg_len - offset);
+
+			dump_buf(block->data, dump_len, 1, offset);
 			offset += MLX5_CMD_DATA_BLOCK_SIZE;
 		} else {
 			mlx5_core_dbg(dev, "command block:\n");
-			dump_buf(next->buf, sizeof(struct mlx5_cmd_prot_block), 0, offset);
-			offset += sizeof(struct mlx5_cmd_prot_block);
+			dump_buf(block, sizeof(*block), 0, offset);
+			offset += sizeof(*block);
 		}
-		next = next->next;
+
+		/* check for last block */
+		if (block->next == 0)
+			break;
 	}
 
 	if (data_only)
@@ -982,12 +1015,12 @@ static void cmd_work_handler(struct work_struct *work)
 	memset(lay, 0, sizeof(*lay));
 	memcpy(lay->in, ent->in->first.data, sizeof(lay->in));
 	ent->op = be32_to_cpu(lay->in[0]) >> 16;
-	if (ent->in->next)
-		lay->in_ptr = cpu_to_be64(ent->in->next->dma);
-	lay->inlen = cpu_to_be32(ent->in->len);
-	if (ent->out->next)
-		lay->out_ptr = cpu_to_be64(ent->out->next->dma);
-	lay->outlen = cpu_to_be32(ent->out->len);
+	if (ent->in->numpages != 0)
+		lay->in_ptr = cpu_to_be64(mlx5_fwp_get_dma(ent->in, 0));
+	if (ent->out->numpages != 0)
+		lay->out_ptr = cpu_to_be64(mlx5_fwp_get_dma(ent->out, 0));
+	lay->inlen = cpu_to_be32(ent->uin_size);
+	lay->outlen = cpu_to_be32(ent->uout_size);
 	lay->type = MLX5_PCI_CMD_XPORT;
 	lay->token = ent->token;
 	lay->status_own = CMD_OWNER_HW;
@@ -997,14 +1030,14 @@ static void cmd_work_handler(struct work_struct *work)
 	ent->busy = 0;
 	/* ring doorbell after the descriptor is valid */
 	mlx5_core_dbg(dev, "writing 0x%x to command doorbell\n", 1 << ent->idx);
-	wmb();
+	/* make sure data is written to RAM */
+	mlx5_fwp_flush(cmd->cmd_page);
 	iowrite32be(1 << ent->idx, &dev->iseg->cmd_dbell);
 	mmiowb();
 	/* if not in polling don't use ent after this point*/
 	if (cmd->mode == CMD_MODE_POLLING) {
 		poll_timeout(ent);
 		/* make sure we read the descriptor after ownership is SW */
-		rmb();
 		mlx5_cmd_comp_handler(dev, 1U << ent->idx);
 	}
 }
@@ -1078,6 +1111,7 @@ static int wait_func(struct mlx5_core_dev *dev, struct mlx5_cmd_work_ent *ent)
  *    2. page queue commands do not support asynchrous completion
  */
 static int mlx5_cmd_invoke(struct mlx5_core_dev *dev, struct mlx5_cmd_msg *in,
+			   int uin_size,
 			   struct mlx5_cmd_msg *out, void *uout, int uout_size,
 			   mlx5_cmd_cbk_t callback,
 			   void *context, int page_queue, u8 *status)
@@ -1092,8 +1126,8 @@ static int mlx5_cmd_invoke(struct mlx5_core_dev *dev, struct mlx5_cmd_msg *in,
 	if (callback && page_queue)
 		return -EINVAL;
 
-	ent = alloc_cmd(cmd, in, out, uout, uout_size, callback, context,
-			page_queue);
+	ent = alloc_cmd(cmd, in, uin_size, out, uout, uout_size, callback,
+			context, page_queue);
 	if (IS_ERR(ent))
 		return PTR_ERR(ent);
 
@@ -1138,159 +1172,98 @@ out:
 	return err;
 }
 
-static int mlx5_copy_to_msg(struct mlx5_cmd_msg *to, void *from, int size)
+static int mlx5_copy_to_msg(struct mlx5_cmd_msg *to, void *from, size_t size)
 {
-	struct mlx5_cmd_prot_block *block;
-	struct mlx5_cmd_mailbox *next;
-	int copy;
-
-	if (!to || !from)
-		return -ENOMEM;
-
-	copy = min_t(int, size, sizeof(to->first.data));
-	memcpy(to->first.data, from, copy);
-	size -= copy;
-	from += copy;
-
-	next = to->next;
-	while (size) {
-		if (!next) {
-			/* this is a BUG */
-			return -ENOMEM;
-		}
+	size_t delta;
+	size_t i;
 
-		copy = min_t(int, size, MLX5_CMD_DATA_BLOCK_SIZE);
-		block = next->buf;
-		memcpy(block->data, from, copy);
-		from += copy;
-		size -= copy;
-		next = next->next;
-	}
+	if (to == NULL || from == NULL)
+		return (-ENOMEM);
 
-	return 0;
-}
+	delta = min_t(size_t, size, sizeof(to->first.data));
+	memcpy(to->first.data, from, delta);
+	from = (char *)from + delta;
+	size -= delta;
 
-static int mlx5_copy_from_msg(void *to, struct mlx5_cmd_msg *from, int size)
-{
-	struct mlx5_cmd_prot_block *block;
-	struct mlx5_cmd_mailbox *next;
-	int copy;
-
-	if (!to || !from)
-		return -ENOMEM;
-
-	copy = min_t(int, size, sizeof(from->first.data));
-	memcpy(to, from->first.data, copy);
-	size -= copy;
-	to += copy;
-
-	next = from->next;
-	while (size) {
-		if (!next) {
-			/* this is a BUG */
-			return -ENOMEM;
-		}
+	for (i = 0; size != 0; i++) {
+		struct mlx5_cmd_prot_block *block;
 
-		copy = min_t(int, size, MLX5_CMD_DATA_BLOCK_SIZE);
-		block = next->buf;
+		block = mlx5_fwp_get_virt(to, i * MLX5_CMD_MBOX_SIZE);
 
-		memcpy(to, block->data, copy);
-		to += copy;
-		size -= copy;
-		next = next->next;
+		delta = min_t(size_t, size, MLX5_CMD_DATA_BLOCK_SIZE);
+		memcpy(block->data, from, delta);
+		from = (char *)from + delta;
+		size -= delta;
 	}
-
-	return 0;
+	return (0);
 }
 
-static struct mlx5_cmd_mailbox *alloc_cmd_box(struct mlx5_core_dev *dev,
-					      gfp_t flags)
+static int mlx5_copy_from_msg(void *to, struct mlx5_cmd_msg *from, int size)
 {
-	struct mlx5_cmd_mailbox *mailbox;
+	size_t delta;
+	size_t i;
 
-	mailbox = kmalloc(sizeof(*mailbox), flags);
-	if (!mailbox)
-		return ERR_PTR(-ENOMEM);
+	if (to == NULL || from == NULL)
+		return (-ENOMEM);
 
-	mailbox->buf = pci_pool_alloc(dev->cmd.pool, flags,
-				      &mailbox->dma);
-	if (!mailbox->buf) {
-		mlx5_core_dbg(dev, "failed allocation\n");
-		kfree(mailbox);
-		return ERR_PTR(-ENOMEM);
-	}
-	memset(mailbox->buf, 0, sizeof(struct mlx5_cmd_prot_block));
-	mailbox->next = NULL;
+	delta = min_t(size_t, size, sizeof(from->first.data));
+	memcpy(to, from->first.data, delta);
+	to = (char *)to + delta;
+	size -= delta;
 
-	return mailbox;
-}
+	for (i = 0; size != 0; i++) {
+		struct mlx5_cmd_prot_block *block;
 
-static void free_cmd_box(struct mlx5_core_dev *dev,
-			 struct mlx5_cmd_mailbox *mailbox)
-{
-	pci_pool_free(dev->cmd.pool, mailbox->buf, mailbox->dma);
-	kfree(mailbox);
+		block = mlx5_fwp_get_virt(from, i * MLX5_CMD_MBOX_SIZE);
+
+		delta = min_t(size_t, size, MLX5_CMD_DATA_BLOCK_SIZE);
+		memcpy(to, block->data, delta);
+		to = (char *)to + delta;
+		size -= delta;
+	}
+	return (0);
 }
 
-static struct mlx5_cmd_msg *mlx5_alloc_cmd_msg(struct mlx5_core_dev *dev,
-					       gfp_t flags, int size)
+static struct mlx5_cmd_msg *
+mlx5_alloc_cmd_msg(struct mlx5_core_dev *dev, gfp_t flags, size_t size)
 {
-	struct mlx5_cmd_mailbox *tmp, *head = NULL;
-	struct mlx5_cmd_prot_block *block;
 	struct mlx5_cmd_msg *msg;
-	int blen;
-	int err;
-	int n;
-	int i;
+	size_t blen;
+	size_t n;
+	size_t i;
 
-	msg = kzalloc(sizeof(*msg), flags);
-	if (!msg)
-		return ERR_PTR(-ENOMEM);
+	blen = size - min_t(size_t, sizeof(msg->first.data), size);
+	n = howmany(blen, MLX5_CMD_DATA_BLOCK_SIZE);
 
-	blen = size - min_t(int, sizeof(msg->first.data), size);
-	n = (blen + MLX5_CMD_DATA_BLOCK_SIZE - 1) / MLX5_CMD_DATA_BLOCK_SIZE;
+	msg = mlx5_fwp_alloc(dev, flags, howmany(n, MLX5_NUM_CMDS_IN_ADAPTER_PAGE));
+	if (msg == NULL)
+		return (ERR_PTR(-ENOMEM));
 
-	for (i = 0; i < n; i++) {
-		tmp = alloc_cmd_box(dev, flags);
-		if (IS_ERR(tmp)) {
-			mlx5_core_warn(dev, "failed allocating block\n");
-			err = PTR_ERR(tmp);
-			goto err_alloc;
-		}
+	for (i = 0; i != n; i++) {
+		struct mlx5_cmd_prot_block *block;
 
-		block = tmp->buf;
-		tmp->next = head;
-		block->next = cpu_to_be64(tmp->next ? tmp->next->dma : 0);
-		block->block_num = cpu_to_be32(n - i - 1);
-		head = tmp;
-	}
-	msg->next = head;
-	msg->len = size;
-	return msg;
+		block = mlx5_fwp_get_virt(msg, i * MLX5_CMD_MBOX_SIZE);
 
-err_alloc:
-	while (head) {
-		tmp = head->next;
-		free_cmd_box(dev, head);
-		head = tmp;
+		memset(block, 0, MLX5_CMD_MBOX_SIZE);
+
+		if (i != (n - 1)) {
+			u64 dma = mlx5_fwp_get_dma(msg, (i + 1) * MLX5_CMD_MBOX_SIZE);
+			block->next = cpu_to_be64(dma);
+		}
+		block->block_num = cpu_to_be32(i);
 	}
-	kfree(msg);
 
-	return ERR_PTR(err);
+	/* make sure initial data is written to RAM */
+	mlx5_fwp_flush(msg);
+
+	return (msg);
 }
 
-static void mlx5_free_cmd_msg(struct mlx5_core_dev *dev,
-				  struct mlx5_cmd_msg *msg)
+static void
+mlx5_free_cmd_msg(struct mlx5_core_dev *dev, struct mlx5_cmd_msg *msg)
 {
-	struct mlx5_cmd_mailbox *head = msg->next;
-	struct mlx5_cmd_mailbox *next;
 
-	while (head) {
-		next = head->next;
-		free_cmd_box(dev, head);
-		head = next;
-	}
-	kfree(msg);
+	mlx5_fwp_free(msg);
 }
 
 static void set_wqname(struct mlx5_core_dev *dev)
@@ -1356,6 +1329,9 @@ void mlx5_cmd_comp_handler(struct mlx5_core_dev *dev, u32 vector)
 	struct mlx5_cmd_work_ent *ent;
 	int i;
 
+	/* make sure data gets read from RAM */
+	mlx5_fwp_invalidate(cmd->cmd_page);
+
 	while (vector != 0) {
 		i = ffs(vector) - 1;
 		vector &= ~(1U << i);
@@ -1363,6 +1339,8 @@ void mlx5_cmd_comp_handler(struct mlx5_core_dev *dev, u32 vector)
 		ent->ts2 = ktime_get_ns();
 		memcpy(ent->out->first.data, ent->lay->out,
 		       sizeof(ent->lay->out));
+		/* make sure data gets read from RAM */
+		mlx5_fwp_invalidate(ent->out);
 		dump_command(dev, ent, 0);
 		if (!ent->ret) {
 			if (!cmd->checksum_disabled)
@@ -1432,10 +1410,6 @@ static struct mlx5_cmd_msg *alloc_msg(struct mlx5_core_dev *dev, int in_size,
 		if (!list_empty(&ent->head)) {
 			msg = list_entry(ent->head.next, struct mlx5_cmd_msg,
 					 list);
-			/* For cached lists, we must explicitly state what is
-			 * the real size
-			 */
-			msg->len = in_size;
 			list_del(&msg->list);
 		}
 		spin_unlock_irq(&ent->lock);
@@ -1460,12 +1434,11 @@ static int cmd_exec_helper(struct mlx5_core_dev *dev,
 	struct mlx5_cmd_msg *inb;
 	struct mlx5_cmd_msg *outb;
 	int pages_queue;
-	gfp_t gfp;
+	const gfp_t gfp = GFP_KERNEL;
 	int err;
 	u8 status = 0;
 
 	pages_queue = is_manage_pages(in);
-	gfp = callback ? GFP_ATOMIC : GFP_KERNEL;
 
 	inb = alloc_msg(dev, in_size, gfp);
 	if (IS_ERR(inb)) {
@@ -1485,8 +1458,8 @@ static int cmd_exec_helper(struct mlx5_core_dev *dev,
 		goto out_in;
 	}
 
-	err = mlx5_cmd_invoke(dev, inb, outb, out, out_size, callback, context,
-			      pages_queue, &status);
+	err = mlx5_cmd_invoke(dev, inb, in_size, outb, out, out_size, callback,
+			      context, pages_queue, &status);
 	if (err) {
 		if (err == -ETIMEDOUT)
 			return err;
@@ -1583,44 +1556,67 @@ ex_err:
 	return err;
 }
 
-static int alloc_cmd_page(struct mlx5_core_dev *dev, struct mlx5_cmd *cmd)
+static int
+alloc_cmd_page(struct mlx5_core_dev *dev, struct mlx5_cmd *cmd)
 {
-	struct device *ddev = &dev->pdev->dev;
-	cmd->cmd_alloc_buf = dma_zalloc_coherent(ddev, MLX5_ADAPTER_PAGE_SIZE,
-						 &cmd->alloc_dma, GFP_KERNEL);
-	if (!cmd->cmd_alloc_buf)
-		return -ENOMEM;
-
-	/* make sure it is aligned to 4K */
-	if (!((uintptr_t)cmd->cmd_alloc_buf & (MLX5_ADAPTER_PAGE_SIZE - 1))) {
-		cmd->cmd_buf = cmd->cmd_alloc_buf;
-		cmd->dma = cmd->alloc_dma;
-		cmd->alloc_size = MLX5_ADAPTER_PAGE_SIZE;
-		return 0;
-	}
-
-	dma_free_coherent(ddev, MLX5_ADAPTER_PAGE_SIZE, cmd->cmd_alloc_buf, cmd->alloc_dma);
-	cmd->cmd_alloc_buf = dma_zalloc_coherent(ddev, 2 * MLX5_ADAPTER_PAGE_SIZE - 1,
-						 &cmd->alloc_dma, GFP_KERNEL);
-	if (!cmd->cmd_alloc_buf)
-		return -ENOMEM;
+	int err;
 
-	cmd->cmd_buf = PTR_ALIGN(cmd->cmd_alloc_buf, MLX5_ADAPTER_PAGE_SIZE);
-	cmd->dma = ALIGN(cmd->alloc_dma, MLX5_ADAPTER_PAGE_SIZE);
-	cmd->alloc_size = 2 * MLX5_ADAPTER_PAGE_SIZE - 1;
-	return 0;
+	sx_init(&cmd->dma_sx, "MLX5-DMA-SX");
+	mtx_init(&cmd->dma_mtx, "MLX5-DMA-MTX", NULL, MTX_DEF);
+	cv_init(&cmd->dma_cv, "MLX5-DMA-CV");
+
+	/*
+	 * Create global DMA descriptor tag for allocating
+	 * 4K firmware pages:
+	 */
+	err = -bus_dma_tag_create(
+	    bus_get_dma_tag(dev->pdev->dev.bsddev),
+	    MLX5_ADAPTER_PAGE_SIZE,	/* alignment */
+	    0,				/* no boundary */
+	    BUS_SPACE_MAXADDR,		/* lowaddr */
+	    BUS_SPACE_MAXADDR,		/* highaddr */
+	    NULL, NULL,			/* filter, filterarg */
+	    MLX5_ADAPTER_PAGE_SIZE,	/* maxsize */
+	    1,				/* nsegments */
+	    MLX5_ADAPTER_PAGE_SIZE,	/* maxsegsize */
+	    0,				/* flags */
+	    NULL, NULL,			/* lockfunc, lockfuncarg */
+	    &cmd->dma_tag);
+	if (err != 0)
+		goto failure_destroy_sx;
+
+	cmd->cmd_page = mlx5_fwp_alloc(dev, GFP_KERNEL, 1);
+	if (cmd->cmd_page == NULL) {
+		err = -ENOMEM;
+		goto failure_alloc_page;
+	}
+	cmd->dma = mlx5_fwp_get_dma(cmd->cmd_page, 0);
+	cmd->cmd_buf = mlx5_fwp_get_virt(cmd->cmd_page, 0);
+	return (0);
+
+failure_alloc_page:
+	bus_dma_tag_destroy(cmd->dma_tag);
+
+failure_destroy_sx:
+	cv_destroy(&cmd->dma_cv);
+	mtx_destroy(&cmd->dma_mtx);
+	sx_destroy(&cmd->dma_sx);
+	return (err);
 }
 
-static void free_cmd_page(struct mlx5_core_dev *dev, struct mlx5_cmd *cmd)
+static void
+free_cmd_page(struct mlx5_core_dev *dev, struct mlx5_cmd *cmd)
 {
-	struct device *ddev = &dev->pdev->dev;
-	dma_free_coherent(ddev, cmd->alloc_size, cmd->cmd_alloc_buf, cmd->alloc_dma);
+
+	mlx5_fwp_free(cmd->cmd_page);
+	bus_dma_tag_destroy(cmd->dma_tag);
+	cv_destroy(&cmd->dma_cv);
+	mtx_destroy(&cmd->dma_mtx);
+	sx_destroy(&cmd->dma_sx);
 }
 
 int mlx5_cmd_init(struct mlx5_core_dev *dev)
 {
-	int size = sizeof(struct mlx5_cmd_prot_block);
-	int align = roundup_pow_of_two(size);
 	struct mlx5_cmd *cmd = &dev->cmd;
 	u32 cmd_h, cmd_l;
 	u16 cmd_if_rev;
@@ -1633,10 +1629,6 @@ int mlx5_cmd_init(struct mlx5_core_dev *dev)
 		return -EINVAL;
 	}
 
-	cmd->pool = pci_pool_create("mlx5_cmd", dev->pdev, size, align, 0);
-	if (!cmd->pool)
-		return -ENOMEM;
-
 	err = alloc_cmd_page(dev, cmd);
 	if (err)
 		goto err_free_pool;
@@ -1716,8 +1708,6 @@ err_free_page:
 	free_cmd_page(dev, cmd);
 
 err_free_pool:
-	pci_pool_destroy(cmd->pool);
-
 	return err;
 }
 EXPORT_SYMBOL(mlx5_cmd_init);
@@ -1730,7 +1720,6 @@ void mlx5_cmd_cleanup(struct mlx5_core_dev *dev)
 	destroy_workqueue(cmd->wq);
 	destroy_msg_cache(dev);
 	free_cmd_page(dev, cmd);
-	pci_pool_destroy(cmd->pool);
 }
 EXPORT_SYMBOL(mlx5_cmd_cleanup);
 
diff --git a/sys/dev/mlx5/mlx5_core/mlx5_pagealloc.c b/sys/dev/mlx5/mlx5_core/mlx5_pagealloc.c
index c60daef..ffd2265 100644
--- a/sys/dev/mlx5/mlx5_core/mlx5_pagealloc.c
+++ b/sys/dev/mlx5/mlx5_core/mlx5_pagealloc.c
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2013-2015, Mellanox Technologies, Ltd.  All rights reserved.
+ * Copyright (c) 2013-2017, Mellanox Technologies, Ltd.  All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,6 +31,8 @@
 #include <dev/mlx5/driver.h>
 #include "mlx5_core.h"
 
+CTASSERT((uintptr_t)PAGE_MASK > (uintptr_t)PAGE_SIZE);
+
 struct mlx5_pages_req {
 	struct mlx5_core_dev *dev;
 	u16	func_id;
@@ -38,15 +40,6 @@ struct mlx5_pages_req {
 	struct work_struct work;
 };
 
-struct mlx5_fw_page {
-	struct rb_node		rb_node;
-	u64			addr;
-	struct page	       *page;
-	u16			func_id;
-	unsigned long		bitmask;
-	struct list_head	list;
-	unsigned		free_count;
-};
 
 struct mlx5_manage_pages_inbox {
 	struct mlx5_inbox_hdr	hdr;
@@ -67,48 +60,188 @@ enum {
 	MAX_RECLAIM_TIME_MSECS	= 5000,
 };
 
-enum {
-	MLX5_MAX_RECLAIM_TIME_MILI	= 5000,
-	MLX5_NUM_4K_IN_PAGE		= PAGE_SIZE / MLX5_ADAPTER_PAGE_SIZE,
-};
+static void
+mlx5_fwp_load_mem_cb(void *arg, bus_dma_segment_t *segs, int nseg, int error)
+{
+	struct mlx5_fw_page *fwp;
+	uint8_t owned;
+
+	fwp = (struct mlx5_fw_page *)arg;
+	owned = MLX5_DMA_OWNED(fwp->dev);
+
+	if (!owned)
+		MLX5_DMA_LOCK(fwp->dev);
+
+	if (error == 0) {
+		KASSERT(nseg == 1, ("Number of segments is different from 1"));
+		fwp->dma_addr = segs->ds_addr;
+		fwp->load_done = MLX5_LOAD_ST_SUCCESS;
+	} else {
+		fwp->load_done = MLX5_LOAD_ST_FAILURE;
+	}
+	MLX5_DMA_DONE(fwp->dev);
+
+	if (!owned)
+		MLX5_DMA_UNLOCK(fwp->dev);
+}
+
+void
+mlx5_fwp_flush(struct mlx5_fw_page *fwp)
+{
+	unsigned num = fwp->numpages;
+
+	while (num--)
+		bus_dmamap_sync(fwp[num].dev->cmd.dma_tag, fwp[num].dma_map, BUS_DMASYNC_PREWRITE);
+}
+
+void
+mlx5_fwp_invalidate(struct mlx5_fw_page *fwp)
+{
+	unsigned num = fwp->numpages;
+
+	while (num--) {
+		bus_dmamap_sync(fwp[num].dev->cmd.dma_tag, fwp[num].dma_map, BUS_DMASYNC_POSTREAD);
+		bus_dmamap_sync(fwp[num].dev->cmd.dma_tag, fwp[num].dma_map, BUS_DMASYNC_PREREAD);
+	}
+}
+
+struct mlx5_fw_page *
+mlx5_fwp_alloc(struct mlx5_core_dev *dev, gfp_t flags, unsigned num)
+{
+	struct mlx5_fw_page *fwp;
+	unsigned x;
+	int err;
+
+	/* check for special case */
+	if (num == 0) {
+		fwp = kzalloc(sizeof(*fwp), flags);
+		if (fwp != NULL)
+			fwp->dev = dev;
+		return (fwp);
+	}
+
+	/* we need sleeping context for this function */
+	if (flags & M_NOWAIT)
+		return (NULL);
+
+	fwp = kzalloc(sizeof(*fwp) * num, flags);
+
+	/* serialize loading the DMA map(s) */
+	sx_xlock(&dev->cmd.dma_sx);
+
+	for (x = 0; x != num; x++) {
+		/* store pointer to MLX5 core device */
+		fwp[x].dev = dev;
+		/* store number of pages left from the array */
+		fwp[x].numpages = num - x;
+
+		/* allocate memory */
+		err = bus_dmamem_alloc(dev->cmd.dma_tag, &fwp[x].virt_addr,
+		    BUS_DMA_WAITOK | BUS_DMA_COHERENT, &fwp[x].dma_map);
+		if (err != 0)
+			goto failure;
+
+		/* load memory into DMA */
+		MLX5_DMA_LOCK(dev);
+		err = bus_dmamap_load(
+		    dev->cmd.dma_tag, fwp[x].dma_map, fwp[x].virt_addr,
+		    MLX5_ADAPTER_PAGE_SIZE, &mlx5_fwp_load_mem_cb,
+		    fwp + x, BUS_DMA_WAITOK | BUS_DMA_COHERENT);
+
+		while (fwp[x].load_done == MLX5_LOAD_ST_NONE)
+			MLX5_DMA_WAIT(dev);
+		MLX5_DMA_UNLOCK(dev);
+
+		/* check for error */
+		if (fwp[x].load_done != MLX5_LOAD_ST_SUCCESS) {
+			bus_dmamem_free(dev->cmd.dma_tag, fwp[x].virt_addr,
+			    fwp[x].dma_map);
+			goto failure;
+		}
+	}
+	sx_xunlock(&dev->cmd.dma_sx);
+	return (fwp);
+
+failure:
+	while (x--) {
+		bus_dmamap_unload(dev->cmd.dma_tag, fwp[x].dma_map);
+		bus_dmamem_free(dev->cmd.dma_tag, fwp[x].virt_addr, fwp[x].dma_map);
+	}
+	sx_xunlock(&dev->cmd.dma_sx);
+	return (NULL);
+}
+
+void
+mlx5_fwp_free(struct mlx5_fw_page *fwp)
+{
+	struct mlx5_core_dev *dev;
+	unsigned num;
+
+	/* be NULL safe */
+	if (fwp == NULL)
+		return;
+
+	/* check for special case */
+	if (fwp->numpages == 0) {
+		kfree(fwp);
+		return;
+	}
+
+	num = fwp->numpages;
+	dev = fwp->dev;
+
+	while (num--) {
+		bus_dmamap_unload(dev->cmd.dma_tag, fwp[num].dma_map);
+		bus_dmamem_free(dev->cmd.dma_tag, fwp[num].virt_addr, fwp[num].dma_map);
+	}
+
+	kfree(fwp);
+}
 
-static int insert_page(struct mlx5_core_dev *dev, u64 addr, struct page *page, u16 func_id)
+u64
+mlx5_fwp_get_dma(struct mlx5_fw_page *fwp, size_t offset)
+{
+	size_t index = (offset / MLX5_ADAPTER_PAGE_SIZE);
+	KASSERT(index < fwp->numpages, ("Invalid offset: %lld", (long long)offset));
+
+	return ((fwp + index)->dma_addr + (offset % MLX5_ADAPTER_PAGE_SIZE));
+}
+
+void *
+mlx5_fwp_get_virt(struct mlx5_fw_page *fwp, size_t offset)
+{
+	size_t index = (offset / MLX5_ADAPTER_PAGE_SIZE);
+	KASSERT(index < fwp->numpages, ("Invalid offset: %lld", (long long)offset));
+
+	return ((char *)(fwp + index)->virt_addr + (offset % MLX5_ADAPTER_PAGE_SIZE));
+}
+
+static int
+mlx5_insert_fw_page_locked(struct mlx5_core_dev *dev, struct mlx5_fw_page *nfp)
 {
 	struct rb_root *root = &dev->priv.page_root;
 	struct rb_node **new = &root->rb_node;
 	struct rb_node *parent = NULL;
-	struct mlx5_fw_page *nfp;
 	struct mlx5_fw_page *tfp;
-	int i;
 
 	while (*new) {
 		parent = *new;
 		tfp = rb_entry(parent, struct mlx5_fw_page, rb_node);
-		if (tfp->addr < addr)
+		if (tfp->dma_addr < nfp->dma_addr)
 			new = &parent->rb_left;
-		else if (tfp->addr > addr)
+		else if (tfp->dma_addr > nfp->dma_addr)
 			new = &parent->rb_right;
 		else
-			return -EEXIST;
+			return (-EEXIST);
 	}
 
-	nfp = kzalloc(sizeof(*nfp), GFP_KERNEL);
-
-	nfp->addr = addr;
-	nfp->page = page;
-	nfp->func_id = func_id;
-	nfp->free_count = MLX5_NUM_4K_IN_PAGE;
-	for (i = 0; i < MLX5_NUM_4K_IN_PAGE; i++)
-		set_bit(i, &nfp->bitmask);
-
 	rb_link_node(&nfp->rb_node, parent, new);
 	rb_insert_color(&nfp->rb_node, root);
-	list_add(&nfp->list, &dev->priv.free_list);
-
-	return 0;
+	return (0);
 }
 
-static struct mlx5_fw_page *find_fw_page(struct mlx5_core_dev *dev, u64 addr)
+static struct mlx5_fw_page *
+mlx5_remove_fw_page_locked(struct mlx5_core_dev *dev, bus_addr_t addr)
 {
 	struct rb_root *root = &dev->priv.page_root;
 	struct rb_node *tmp = root->rb_node;
@@ -117,17 +250,61 @@ static struct mlx5_fw_page *find_fw_page(struct mlx5_core_dev *dev, u64 addr)
 
 	while (tmp) {
 		tfp = rb_entry(tmp, struct mlx5_fw_page, rb_node);
-		if (tfp->addr < addr) {
+		if (tfp->dma_addr < addr) {
 			tmp = tmp->rb_left;
-		} else if (tfp->addr > addr) {
+		} else if (tfp->dma_addr > addr) {
 			tmp = tmp->rb_right;
 		} else {
+			rb_erase(&tfp->rb_node, &dev->priv.page_root);
 			result = tfp;
 			break;
 		}
 	}
+	return (result);
+}
 
-	return result;
+static int
+alloc_4k(struct mlx5_core_dev *dev, u64 *addr, u16 func_id)
+{
+	struct mlx5_fw_page *fwp;
+	int err;
+
+	fwp = mlx5_fwp_alloc(dev, GFP_KERNEL, 1);
+	if (fwp == NULL)
+		return (-ENOMEM);
+
+	fwp->func_id = func_id;
+
+	MLX5_DMA_LOCK(dev);
+	err = mlx5_insert_fw_page_locked(dev, fwp);
+	MLX5_DMA_UNLOCK(dev);
+
+	if (err != 0) {
+		mlx5_fwp_free(fwp);
+	} else {
+		/* make sure cached data is cleaned */
+		mlx5_fwp_invalidate(fwp);
+
+		/* store DMA address */
+		*addr = fwp->dma_addr;
+	}
+	return (err);
+}
+
+static void
+free_4k(struct mlx5_core_dev *dev, u64 addr)
+{
+	struct mlx5_fw_page *fwp;
+
+	MLX5_DMA_LOCK(dev);
+	fwp = mlx5_remove_fw_page_locked(dev, addr);
+	MLX5_DMA_UNLOCK(dev);
+
+	if (fwp == NULL) {
+		mlx5_core_warn(dev, "Cannot free 4K page at 0x%llx\n", (long long)addr);
+		return;
+	}
+	mlx5_fwp_free(fwp);
 }
 
 static int mlx5_cmd_query_pages(struct mlx5_core_dev *dev, u16 *func_id,
@@ -154,90 +331,6 @@ static int mlx5_cmd_query_pages(struct mlx5_core_dev *dev, u16 *func_id,
 	return 0;
 }
 
-static int alloc_4k(struct mlx5_core_dev *dev, u64 *addr)
-{
-	struct mlx5_fw_page *fp;
-	unsigned n;
-
-	if (list_empty(&dev->priv.free_list))
-		return -ENOMEM;
-
-	fp = list_entry(dev->priv.free_list.next, struct mlx5_fw_page, list);
-	n = find_first_bit(&fp->bitmask, 8 * sizeof(fp->bitmask));
-	if (n >= MLX5_NUM_4K_IN_PAGE) {
-		mlx5_core_warn(dev, "alloc 4k bug\n");
-		return -ENOENT;
-	}
-	clear_bit(n, &fp->bitmask);
-	fp->free_count--;
-	if (!fp->free_count)
-		list_del(&fp->list);
-
-	*addr = fp->addr + n * MLX5_ADAPTER_PAGE_SIZE;
-
-	return 0;
-}
-
-static void free_4k(struct mlx5_core_dev *dev, u64 addr)
-{
-	struct mlx5_fw_page *fwp;
-	int n;
-
-	fwp = find_fw_page(dev, addr & PAGE_MASK);
-	if (!fwp) {
-		mlx5_core_warn(dev, "page not found\n");
-		return;
-	}
-
-	n = (addr & ~PAGE_MASK) >> MLX5_ADAPTER_PAGE_SHIFT;
-	fwp->free_count++;
-	set_bit(n, &fwp->bitmask);
-	if (fwp->free_count == MLX5_NUM_4K_IN_PAGE) {
-		rb_erase(&fwp->rb_node, &dev->priv.page_root);
-		if (fwp->free_count != 1)
-			list_del(&fwp->list);
-		dma_unmap_page(&dev->pdev->dev, addr & PAGE_MASK, PAGE_SIZE,
-			       DMA_BIDIRECTIONAL);
-		__free_page(fwp->page);
-		kfree(fwp);
-	} else if (fwp->free_count == 1) {
-		list_add(&fwp->list, &dev->priv.free_list);
-	}
-}
-
-static int alloc_system_page(struct mlx5_core_dev *dev, u16 func_id)
-{
-	struct page *page;
-	u64 addr;
-	int err;
-
-	page = alloc_page(GFP_HIGHUSER);
-	if (!page) {
-		mlx5_core_warn(dev, "failed to allocate page\n");
-		return -ENOMEM;
-	}
-	addr = dma_map_page(&dev->pdev->dev, page, 0,
-			    PAGE_SIZE, DMA_BIDIRECTIONAL);
-	if (dma_mapping_error(&dev->pdev->dev, addr)) {
-		mlx5_core_warn(dev, "failed dma mapping page\n");
-		err = -ENOMEM;
-		goto out_alloc;
-	}
-	err = insert_page(dev, addr, page, func_id);
-	if (err) {
-		mlx5_core_err(dev, "failed to track allocated page\n");
-		goto out_mapping;
-	}
-
-	return 0;
-
-out_mapping:
-	dma_unmap_page(&dev->pdev->dev, addr, PAGE_SIZE, DMA_BIDIRECTIONAL);
-
-out_alloc:
-	__free_page(page);
-	return err;
-}
 static int give_pages(struct mlx5_core_dev *dev, u16 func_id, int npages,
 		      int notify_fail)
 {
@@ -259,16 +352,9 @@ static int give_pages(struct mlx5_core_dev *dev, u16 func_id, int npages,
 	memset(&out, 0, sizeof(out));
 
 	for (i = 0; i < npages; i++) {
-retry:
-		err = alloc_4k(dev, &addr);
-		if (err) {
-			if (err == -ENOMEM)
-				err = alloc_system_page(dev, func_id);
-			if (err)
-				goto out_alloc;
-
-			goto retry;
-		}
+		err = alloc_4k(dev, &addr, func_id);
+		if (err)
+			goto out_alloc;
 		in->pas[i] = cpu_to_be64(addr);
 	}
 
@@ -301,6 +387,9 @@ retry:
 out_alloc:
 	if (notify_fail) {
 		nin = kzalloc(sizeof(*nin), GFP_KERNEL);
+		if (!nin)
+			goto out_4k;
+
 		memset(&out, 0, sizeof(out));
 		nin->hdr.opcode = cpu_to_be16(MLX5_CMD_OP_MANAGE_PAGES);
 		nin->hdr.opmod = cpu_to_be16(MLX5_PAGES_CANT_GIVE);
@@ -309,6 +398,8 @@ out_alloc:
 			mlx5_core_warn(dev, "page notify failed\n");
 		kfree(nin);
 	}
+
+out_4k:
 	for (i--; i >= 0; i--)
 		free_4k(dev, be64_to_cpu(in->pas[i]));
 out_free:
@@ -477,7 +568,7 @@ int mlx5_reclaim_startup_pages(struct mlx5_core_dev *dev)
 			fwp = rb_entry(p, struct mlx5_fw_page, rb_node);
 			if (dev->state == MLX5_DEVICE_STATE_INTERNAL_ERROR) {
 				--dev->priv.fw_pages;
-				free_4k(dev, fwp->addr);
+				free_4k(dev, fwp->dma_addr);
 				nclaimed = 1;
 			} else {
 				err = reclaim_pages(dev, fwp->func_id,
@@ -504,8 +595,8 @@ int mlx5_reclaim_startup_pages(struct mlx5_core_dev *dev)
 
 void mlx5_pagealloc_init(struct mlx5_core_dev *dev)
 {
+
 	dev->priv.page_root = RB_ROOT;
-	INIT_LIST_HEAD(&dev->priv.free_list);
 }
 
 void mlx5_pagealloc_cleanup(struct mlx5_core_dev *dev)