5 files changed, 583 insertions, 470 deletions

diff --git a/sys/dev/mlx5/device.h b/sys/dev/mlx5/device.h
index de1d7f9010cb..47b263ad7b21 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 e5e8a70e3000..3371969d20d2 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 513fc6fc7d2c..1dabbd9f586f 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 5bf6eb5b5d23..5cb257ce3127 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);
@@ -1485,8 +1459,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 +1557,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 +1630,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 +1709,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 +1721,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 c60daef600a4..3f74ace3d778 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,191 @@ 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;
+
+	/* serialize unloading the DMA maps */
+	sx_xlock(&dev->cmd.dma_sx);
+	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);
+	}
+	sx_xunlock(&dev->cmd.dma_sx);
+
+	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 +253,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 +334,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 +355,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 +390,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 +401,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 +571,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 +598,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)