fmd: fmd22 integration

- Fix for PCD: key mask not properly enabled in exact match table
- Fix for PFC mapping function
- Added counters for miss entry in match and hash tables
- Added counter for IPv4 options in IP fragmentation

Change-Id: I1626afc661d412c518172d405860a33d801cd005
Signed-off-by: Mandy Lavi <mandy.lavi@freescale.com>
Reviewed-on: http://git.am.freescale.net:8181/6251
Tested-by: Review Code-CDREVIEW <CDREVIEW@freescale.com>
Reviewed-by: Marian-Cornel Chereji <marian.chereji@freescale.com>
Reviewed-by: Jose Rivera <German.Rivera@freescale.com>

Conflicts:
	drivers/net/ethernet/freescale/fman/Peripherals/FM/Port/fm_port.c
	drivers/net/ethernet/freescale/fman/Peripherals/FM/Port/fm_port.h
Change-Id: If356927d0cd4e22e8949a0106c2a403fcf1343a2
Reviewed-on: http://git.am.freescale.net:8181/7665
Reviewed-by: Madalin-Cristian Bucur <madalin.bucur@freescale.com>
Tested-by: Madalin-Cristian Bucur <madalin.bucur@freescale.com>

5 files changed, 2421 insertions, 1545 deletions

diff --git a/drivers/net/ethernet/freescale/fman/Peripherals/FM/Port/Makefile b/drivers/net/ethernet/freescale/fman/Peripherals/FM/Port/Makefile
index f8dbc8a..c94c54c 100644
--- a/drivers/net/ethernet/freescale/fman/Peripherals/FM/Port/Makefile
+++ b/drivers/net/ethernet/freescale/fman/Peripherals/FM/Port/Makefile
@@ -12,4 +12,4 @@ EXTRA_CFLAGS += -I$(NCSW_FM_INC)
 
 obj-y		+= fsl-ncsw-Pcd.o
 
-fsl-ncsw-Pcd-objs	:=   fm_port.o fm_port_im.o
+fsl-ncsw-Pcd-objs	:=   fm_port.o fm_port_im.o fman_port.o
diff --git a/drivers/net/ethernet/freescale/fman/Peripherals/FM/Port/fm_port.c b/drivers/net/ethernet/freescale/fman/Peripherals/FM/Port/fm_port.c
index b2dbcac..edb86f6 100644
--- a/drivers/net/ethernet/freescale/fman/Peripherals/FM/Port/fm_port.c
+++ b/drivers/net/ethernet/freescale/fman/Peripherals/FM/Port/fm_port.c
@@ -43,8 +43,10 @@
 #include "debug_ext.h"
 #include "fm_muram_ext.h"
 
+#include "fman_common.h"
 #include "fm_port.h"
 
+#include "common/general.h"
 
 /****************************************/
 /*       static functions               */
@@ -53,13 +55,14 @@
 static t_Error CheckInitParameters(t_FmPort *p_FmPort)
 {
     t_FmPortDriverParam *p_Params = p_FmPort->p_FmPortDriverParam;
+    struct fman_port_cfg    *p_DfltConfig = &p_Params->dfltCfg;
     t_Error             ans = E_OK;
     uint32_t            unusedMask;
 
     if (p_FmPort->imEn)
     {
         if (p_FmPort->portType == e_FM_PORT_TYPE_RX_10G)
-            if (p_FmPort->fifoDeqPipelineDepth > 2)
+            if (p_FmPort->p_FmPortDriverParam->dfltCfg.tx_fifo_deq_pipeline_depth > 2)
                 RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("fifoDeqPipelineDepth for IM 10G can't be larger than 2"));
 
         if ((ans = FmPortImCheckInitParameters(p_FmPort)) != E_OK)
@@ -70,20 +73,24 @@ static t_Error CheckInitParameters(t_FmPort *p_FmPort)
         /****************************************/
         /*   Rx only                            */
         /****************************************/
-        if ((p_FmPort->portType == e_FM_PORT_TYPE_RX) || (p_FmPort->portType == e_FM_PORT_TYPE_RX_10G))
+        if ((p_FmPort->portType == e_FM_PORT_TYPE_RX) ||
+            (p_FmPort->portType == e_FM_PORT_TYPE_RX_10G))
         {
             /* external buffer pools */
             if (!p_Params->extBufPools.numOfPoolsUsed)
                  RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("extBufPools.numOfPoolsUsed=0. At least one buffer pool must be defined"));
 
-            if (FmSpCheckBufPoolsParams(&p_Params->extBufPools, p_Params->p_BackupBmPools, &p_Params->bufPoolDepletion)!= E_OK)
+            if (FmSpCheckBufPoolsParams(&p_Params->extBufPools,
+                                        p_Params->p_BackupBmPools,
+                                        &p_Params->bufPoolDepletion)!= E_OK)
                  RETURN_ERROR(MAJOR, E_INVALID_VALUE, NO_MSG);
 
             /* Check that part of IC that needs copying is small enough to enter start margin */
             if (p_Params->intContext.size && (p_Params->intContext.size + p_Params->intContext.extBufOffset > p_Params->bufMargins.startMargins))
                 RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("intContext.size is larger than start margins"));
 
-            if (p_Params->liodnOffset & ~FM_LIODN_OFFSET_MASK)
+            if ((p_Params->liodnOffset != DPAA_LIODN_DONT_OVERRIDE) &&
+                (p_Params->liodnOffset & ~FM_LIODN_OFFSET_MASK))
                 RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("liodnOffset is larger than %d", FM_LIODN_OFFSET_MASK+1));
 
 #ifdef FM_NO_BACKUP_POOLS
@@ -102,15 +109,18 @@ static t_Error CheckInitParameters(t_FmPort *p_FmPort)
                  RETURN_ERROR(MAJOR, E_INVALID_VALUE, (" deqSubPortal has to be in the range of 0 - %d", FM_MAX_NUM_OF_SUB_PORTALS));
 
             /* to protect HW internal-context from overwrite */
-            if ((p_Params->intContext.size) && (p_Params->intContext.intContextOffset < MIN_TX_INT_OFFSET))
+            if ((p_Params->intContext.size) &&
+                (p_Params->intContext.intContextOffset < MIN_TX_INT_OFFSET))
                 RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("non-Rx intContext.intContextOffset can't be smaller than %d", MIN_TX_INT_OFFSET));
 
-            if ((p_FmPort->portType == e_FM_PORT_TYPE_TX) || (p_FmPort->portType == e_FM_PORT_TYPE_TX_10G)
-                    /* in O/H DEFAULT_notSupported indicates that it is not suppported and should not be checked */
-                    || (p_FmPort->fifoDeqPipelineDepth != DEFAULT_notSupported))
+            if ((p_FmPort->portType == e_FM_PORT_TYPE_TX) ||
+                (p_FmPort->portType == e_FM_PORT_TYPE_TX_10G)
+                    /* in O/H DEFAULT_notSupported indicates that it is not supported and should not be checked */
+                    || (p_FmPort->p_FmPortDriverParam->dfltCfg.tx_fifo_deq_pipeline_depth != DEFAULT_notSupported))
             {
                 /* Check that not larger than 8 */
-                if ((!p_FmPort->fifoDeqPipelineDepth) ||( p_FmPort->fifoDeqPipelineDepth > MAX_FIFO_PIPELINE_DEPTH))
+                if ((!p_FmPort->p_FmPortDriverParam->dfltCfg.tx_fifo_deq_pipeline_depth) ||
+                    ( p_FmPort->p_FmPortDriverParam->dfltCfg.tx_fifo_deq_pipeline_depth > MAX_FIFO_PIPELINE_DEPTH))
                     RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("fifoDeqPipelineDepth can't be larger than %d", MAX_FIFO_PIPELINE_DEPTH));
             }
         }
@@ -118,9 +128,10 @@ static t_Error CheckInitParameters(t_FmPort *p_FmPort)
         /****************************************/
         /*   Rx Or Offline Parsing              */
         /****************************************/
-        if ((p_FmPort->portType == e_FM_PORT_TYPE_RX) || (p_FmPort->portType == e_FM_PORT_TYPE_RX_10G) || (p_FmPort->portType == e_FM_PORT_TYPE_OH_OFFLINE_PARSING))
+        if ((p_FmPort->portType == e_FM_PORT_TYPE_RX) ||
+            (p_FmPort->portType == e_FM_PORT_TYPE_RX_10G) ||
+            (p_FmPort->portType == e_FM_PORT_TYPE_OH_OFFLINE_PARSING))
         {
-
             if (!p_Params->dfltFqid)
                  RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("dfltFqid must be between 1 and 2^24-1"));
 #if defined(FM_CAPWAP_SUPPORT) && defined(FM_LOCKUP_ALIGNMENT_ERRATA_FMAN_SW004)
@@ -146,18 +157,17 @@ static t_Error CheckInitParameters(t_FmPort *p_FmPort)
     if ((p_FmPort->portType == e_FM_PORT_TYPE_RX) ||
         (p_FmPort->portType == e_FM_PORT_TYPE_RX_10G))
     {
-        /* Check that divisible by 256 and not larger than 256 */
-        if (p_Params->rxFifoPriElevationLevel % BMI_FIFO_UNITS)
+        if (p_DfltConfig->rx_pri_elevation % BMI_FIFO_UNITS)
             RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("rxFifoPriElevationLevel has to be divisible by %d", BMI_FIFO_UNITS));
-        if (!p_Params->rxFifoPriElevationLevel || (p_Params->rxFifoPriElevationLevel > BMI_MAX_FIFO_SIZE))
-            RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("rxFifoPriElevationLevel has to be in the range of 256 - %d", BMI_MAX_FIFO_SIZE));
-        if (p_Params->rxFifoThreshold % BMI_FIFO_UNITS)
+        if ((p_DfltConfig->rx_pri_elevation < BMI_FIFO_UNITS) || (p_DfltConfig->rx_pri_elevation > MAX_PORT_FIFO_SIZE))
+            RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("rxFifoPriElevationLevel has to be in the range of 256 - %d", MAX_PORT_FIFO_SIZE));
+        if (p_DfltConfig->rx_fifo_thr % BMI_FIFO_UNITS)
             RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("rxFifoThreshold has to be divisible by %d", BMI_FIFO_UNITS));
-        if (!p_Params->rxFifoThreshold ||(p_Params->rxFifoThreshold > BMI_MAX_FIFO_SIZE))
-            RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("rxFifoThreshold has to be in the range of 256 - %d", BMI_MAX_FIFO_SIZE));
+        if ((p_DfltConfig->rx_fifo_thr < BMI_FIFO_UNITS) ||(p_DfltConfig->rx_fifo_thr > MAX_PORT_FIFO_SIZE))
+            RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("rxFifoThreshold has to be in the range of 256 - %d", MAX_PORT_FIFO_SIZE));
 
         /* Check that not larger than 16 */
-        if (p_Params->cutBytesFromEnd > FRAME_END_DATA_SIZE)
+        if (p_DfltConfig->rx_cut_end_bytes > FRAME_END_DATA_SIZE)
             RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("cutBytesFromEnd can't be larger than %d", FRAME_END_DATA_SIZE));
 
         if (FmSpCheckBufMargins(&p_Params->bufMargins)!= E_OK)
@@ -174,7 +184,7 @@ static t_Error CheckInitParameters(t_FmPort *p_FmPort)
         if (p_FmPort->fmRevInfo.majorRev == 4)
         {
             /* Check that not larger than 16 */
-            if (p_Params->cutBytesFromEnd + p_Params->cheksumLastBytesIgnore > FRAME_END_DATA_SIZE)
+            if (p_DfltConfig->rx_cut_end_bytes + p_DfltConfig->checksum_bytes_ignore > FRAME_END_DATA_SIZE)
                 RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("cheksumLastBytesIgnore + cutBytesFromEnd can't be larger than %d", FRAME_END_DATA_SIZE));
         }
 #endif /* FM_CSI_CFED_LIMIT */
@@ -193,18 +203,17 @@ static t_Error CheckInitParameters(t_FmPort *p_FmPort)
     if ((p_FmPort->portType == e_FM_PORT_TYPE_TX) ||
         (p_FmPort->portType == e_FM_PORT_TYPE_TX_10G))
     {
-        /* Check that divisible by 256 and not larger than 256 */
-        if (p_Params->txFifoMinFillLevel % BMI_FIFO_UNITS)
+        if (p_DfltConfig->tx_fifo_min_level % BMI_FIFO_UNITS)
             RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("txFifoMinFillLevel has to be divisible by %d", BMI_FIFO_UNITS));
-        if (p_Params->txFifoMinFillLevel > (BMI_MAX_FIFO_SIZE - 256))
-            RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("txFifoMinFillLevel has to be in the range of 0 - %d", BMI_MAX_FIFO_SIZE));
-        if (p_Params->txFifoLowComfLevel % BMI_FIFO_UNITS)
+        if (p_DfltConfig->tx_fifo_min_level > (MAX_PORT_FIFO_SIZE - 256))
+            RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("txFifoMinFillLevel has to be in the range of 0 - %d", (MAX_PORT_FIFO_SIZE - 256)));
+        if (p_DfltConfig->tx_fifo_low_comf_level % BMI_FIFO_UNITS)
             RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("txFifoLowComfLevel has to be divisible by %d", BMI_FIFO_UNITS));
-        if (!p_Params->txFifoLowComfLevel || (p_Params->txFifoLowComfLevel > BMI_MAX_FIFO_SIZE))
-            RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("txFifoLowComfLevel has to be in the range of 256 - %d", BMI_MAX_FIFO_SIZE));
+        if ((p_DfltConfig->tx_fifo_low_comf_level < BMI_FIFO_UNITS) || (p_DfltConfig->tx_fifo_low_comf_level > MAX_PORT_FIFO_SIZE))
+            RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("txFifoLowComfLevel has to be in the range of 256 - %d", MAX_PORT_FIFO_SIZE));
 
         if (p_FmPort->portType == e_FM_PORT_TYPE_TX)
-            if (p_FmPort->fifoDeqPipelineDepth > 2)
+            if (p_FmPort->p_FmPortDriverParam->dfltCfg.tx_fifo_deq_pipeline_depth > 2)
                 RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("fifoDeqPipelineDepth for 1G can't be larger than 2"));
     }
 
@@ -212,7 +221,7 @@ static t_Error CheckInitParameters(t_FmPort *p_FmPort)
     /*   Non Tx Ports                       */
     /****************************************/
     /* If discard override was selected , no frames may be discarded. */
-    else if (p_Params->frmDiscardOverride && p_Params->errorsToDiscard)
+    else if (p_DfltConfig->discard_override && p_Params->errorsToDiscard)
         RETURN_ERROR(MAJOR, E_CONFLICT,
                      ("errorsToDiscard is not empty, but frmDiscardOverride selected (all discarded frames to be enqueued to error queue)."));
 
@@ -260,17 +269,18 @@ static t_Error CheckInitParameters(t_FmPort *p_FmPort)
 #ifndef FM_DEQ_PIPELINE_PARAMS_FOR_OP
         if ((!((p_FmPort->fmRevInfo.majorRev == 4) ||
                (p_FmPort->fmRevInfo.majorRev >= 6))) &&
-            (p_FmPort->fifoDeqPipelineDepth != DEFAULT_notSupported))
+            (p_FmPort->p_FmPortDriverParam->dfltCfg.tx_fifo_deq_pipeline_depth != DEFAULT_notSupported))
                 /* this is an indication that user called config for this mode which is not supported in this integration */
                 RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("fifoDeqPipelineDepth is available for Tx ports only"));
 #endif /* !FM_DEQ_PIPELINE_PARAMS_FOR_OP */
     }
+
     /****************************************/
     /*   All ports                          */
     /****************************************/
-
     /* Check that not larger than 16 */
-    if ((p_Params->cheksumLastBytesIgnore > FRAME_END_DATA_SIZE) && ((p_Params->cheksumLastBytesIgnore != DEFAULT_notSupported)))
+    if ((p_Params->cheksumLastBytesIgnore > FRAME_END_DATA_SIZE) &&
+        ((p_Params->cheksumLastBytesIgnore != DEFAULT_notSupported)))
         RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("cheksumLastBytesIgnore can't be larger than %d", FRAME_END_DATA_SIZE));
 
     if (FmSpCheckIntContextParams(&p_Params->intContext)!= E_OK)
@@ -285,8 +295,8 @@ static t_Error CheckInitParameters(t_FmPort *p_FmPort)
          RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("openDmas.num can't be larger than %d", MAX_NUM_OF_DMAS));
     if (p_Params->setNumOfOpenDmas && (p_FmPort->openDmas.extra > MAX_NUM_OF_EXTRA_DMAS))
          RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("openDmas.extra can't be larger than %d", MAX_NUM_OF_EXTRA_DMAS));
-    if (p_Params->setSizeOfFifo && (!p_FmPort->fifoBufs.num || (p_FmPort->fifoBufs.num > BMI_MAX_FIFO_SIZE)))
-         RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("fifoBufs.num has to be in the range of 256 - %d", BMI_MAX_FIFO_SIZE));
+    if (p_Params->setSizeOfFifo && (!p_FmPort->fifoBufs.num || (p_FmPort->fifoBufs.num > MAX_PORT_FIFO_SIZE)))
+         RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("fifoBufs.num has to be in the range of 256 - %d", MAX_PORT_FIFO_SIZE));
     if (p_Params->setSizeOfFifo && (p_FmPort->fifoBufs.num % BMI_FIFO_UNITS))
          RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("fifoBufs.num has to be divisible by %d", BMI_FIFO_UNITS));
 
@@ -302,7 +312,7 @@ static t_Error CheckInitParameters(t_FmPort *p_FmPort)
 
 static t_Error VerifySizeOfFifo(t_FmPort *p_FmPort)
 {
-    uint32_t                minFifoSizeRequired = 0;
+    uint32_t    minFifoSizeRequired = 0, optFifoSizeForB2B = 0;
 
     /*************************/
     /*    TX PORTS           */
@@ -310,19 +320,20 @@ static t_Error VerifySizeOfFifo(t_FmPort *p_FmPort)
     if ((p_FmPort->portType == e_FM_PORT_TYPE_TX) ||
         (p_FmPort->portType == e_FM_PORT_TYPE_TX_10G))
     {
-        minFifoSizeRequired = (uint32_t)DIV_CEIL(p_FmPort->maxFrameLength, BMI_FIFO_UNITS);
-        if (p_FmPort->imEn)
-            minFifoSizeRequired += 3*BMI_FIFO_UNITS;
-        else
-            minFifoSizeRequired += (p_FmPort->fifoDeqPipelineDepth+3)*BMI_FIFO_UNITS;
+        minFifoSizeRequired = (uint32_t)
+            (ROUND_UP(p_FmPort->maxFrameLength, BMI_FIFO_UNITS) + (3*BMI_FIFO_UNITS));
+        if (!p_FmPort->imEn)
+            minFifoSizeRequired += p_FmPort->p_FmPortDriverParam->dfltCfg.tx_fifo_deq_pipeline_depth * BMI_FIFO_UNITS;
 
-        /* add some margin for back to back capability to improve performance
-         * allows the hardware to pipeline new frame dma while the previous
-         * frame not yet transmitted. */
+        optFifoSizeForB2B = minFifoSizeRequired;
+
+        /* Add some margin for back-to-back capability to improve performance,
+           allows the hardware to pipeline new frame dma while the previous
+           frame not yet transmitted. */
         if (p_FmPort->portType == e_FM_PORT_TYPE_TX_10G)
-            minFifoSizeRequired += 3*BMI_FIFO_UNITS;
+            optFifoSizeForB2B += 3*BMI_FIFO_UNITS;
         else
-            minFifoSizeRequired += 2*BMI_FIFO_UNITS;
+            optFifoSizeForB2B += 2*BMI_FIFO_UNITS;
     }
 
     /*************************/
@@ -331,8 +342,10 @@ static t_Error VerifySizeOfFifo(t_FmPort *p_FmPort)
     else if (((p_FmPort->portType == e_FM_PORT_TYPE_RX) ||
               (p_FmPort->portType == e_FM_PORT_TYPE_RX_10G)) &&
              p_FmPort->imEn)
-        minFifoSizeRequired = (uint32_t)(DIV_CEIL(p_FmPort->maxFrameLength, BMI_FIFO_UNITS) +
-                                         (4*BMI_FIFO_UNITS));
+    {
+        optFifoSizeForB2B = minFifoSizeRequired = (uint32_t)
+            (ROUND_UP(p_FmPort->maxFrameLength, BMI_FIFO_UNITS) + (4*BMI_FIFO_UNITS));
+    }
 
     /*************************/
     /*    RX non-IM PORTS    */
@@ -341,52 +354,61 @@ static t_Error VerifySizeOfFifo(t_FmPort *p_FmPort)
               (p_FmPort->portType == e_FM_PORT_TYPE_RX_10G)) &&
              !p_FmPort->imEn)
     {
-#ifdef FM_FIFO_ALLOCATION_ALG
         if (p_FmPort->fmRevInfo.majorRev == 4)
         {
             if (p_FmPort->rxPoolsParams.numOfPools == 1)
                 minFifoSizeRequired = 8*BMI_FIFO_UNITS;
             else
-                minFifoSizeRequired = (uint32_t)(DIV_CEIL(p_FmPort->rxPoolsParams.secondLargestBufSize, BMI_FIFO_UNITS) +
-                                                 (7*BMI_FIFO_UNITS));
+                minFifoSizeRequired = (uint32_t)
+                    (ROUND_UP(p_FmPort->rxPoolsParams.secondLargestBufSize, BMI_FIFO_UNITS) + (7*BMI_FIFO_UNITS));
         }
         else
-#endif /* FM_FIFO_ALLOCATION_ALG */
-            minFifoSizeRequired = (uint32_t)(DIV_CEIL(p_FmPort->rxPoolsParams.largestBufSize, BMI_FIFO_UNITS) +
-                                             (7*BMI_FIFO_UNITS));
+        {
+#if (DPAA_VERSION >= 11)
+            minFifoSizeRequired = (uint32_t)
+                (ROUND_UP(p_FmPort->maxFrameLength, BMI_FIFO_UNITS) + (5*BMI_FIFO_UNITS));
+                /* 4 according to spec + 1 for FOF>0 */
+#else
+            minFifoSizeRequired = (uint32_t)
+                (ROUND_UP(MIN(p_FmPort->maxFrameLength, p_FmPort->rxPoolsParams.largestBufSize), BMI_FIFO_UNITS)
+                 + (7*BMI_FIFO_UNITS));
+#endif /* (DPAA_VERSION >= 11) */
+        }
 
+        optFifoSizeForB2B = minFifoSizeRequired;
+
+        /* Add some margin for back-to-back capability to improve performance,
+           allows the hardware to pipeline new frame dma while the previous
+           frame not yet transmitted. */
         if (p_FmPort->portType == e_FM_PORT_TYPE_RX_10G)
-            minFifoSizeRequired += 12*BMI_FIFO_UNITS;
+            optFifoSizeForB2B += 8*BMI_FIFO_UNITS;
         else
-            minFifoSizeRequired += 3*BMI_FIFO_UNITS;
+            optFifoSizeForB2B += 3*BMI_FIFO_UNITS;
     }
 
     /* For O/H ports, check fifo size and update if necessary */
     else if ((p_FmPort->portType == e_FM_PORT_TYPE_OH_OFFLINE_PARSING) ||
              (p_FmPort->portType == e_FM_PORT_TYPE_OH_HOST_COMMAND))
-        minFifoSizeRequired = (uint32_t)((p_FmPort->tasks.num + 4)*BMI_FIFO_UNITS);
-
-    /* for all ports - verify size  */
-    if (minFifoSizeRequired && (p_FmPort->fifoBufs.num < minFifoSizeRequired))
-        RETURN_ERROR(MAJOR, E_INVALID_VALUE,
-                     ("User defined FIFO size should be enlarged to %d",
-                      minFifoSizeRequired));
-
-    /* check if pool size is not too big */
-    /* This is a definition problem in which if the fifo for the RX port
-       is lower than the largest pool size the hardware will allocate scatter gather
-       buffers even though the frame size can fit in a single buffer. */
-    if (((p_FmPort->portType == e_FM_PORT_TYPE_RX) ||
-         (p_FmPort->portType == e_FM_PORT_TYPE_RX_10G))
-        && !p_FmPort->imEn)
     {
-        if (p_FmPort->rxPoolsParams.largestBufSize > p_FmPort->fifoBufs.num)
-            RETURN_ERROR(MAJOR, E_INVALID_VALUE,
-                         ("Frame larger than port Fifo size (%u) will be split to more "\
-                          "than a single buffer (S/G) even if shorter than largest buffer size (%u)",
-                          p_FmPort->fifoBufs.num, p_FmPort->rxPoolsParams.largestBufSize));
+#if (DPAA_VERSION >= 11)
+        optFifoSizeForB2B = minFifoSizeRequired = (uint32_t)
+            (ROUND_UP(p_FmPort->maxFrameLength, BMI_FIFO_UNITS) + ((p_FmPort->p_FmPortDriverParam->dfltCfg.tx_fifo_deq_pipeline_depth + 5) * BMI_FIFO_UNITS));
+            /* 4 according to spec + 1 for FOF>0 */
+#else
+        optFifoSizeForB2B = minFifoSizeRequired = (uint32_t)((p_FmPort->tasks.num + 2) * BMI_FIFO_UNITS);
+#endif /* (DPAA_VERSION >= 11) */
     }
 
+    ASSERT_COND(minFifoSizeRequired > 0);
+    ASSERT_COND(optFifoSizeForB2B >= minFifoSizeRequired);
+
+    /* Verify the size  */
+    if (p_FmPort->fifoBufs.num < minFifoSizeRequired)
+        DBG(INFO, ("FIFO size is %d and should be enlarged to %d bytes", p_FmPort->fifoBufs.num, minFifoSizeRequired));
+    else if (p_FmPort->fifoBufs.num < optFifoSizeForB2B)
+        DBG(INFO, ("For back-to-back frames processing, FIFO size may need to be enlarged to %d bytes", optFifoSizeForB2B));
+
+
     return E_OK;
 }
 
@@ -403,75 +425,44 @@ static t_Error SetExtBufferPools(t_FmPort *p_FmPort)
 {
     t_FmExtPools                *p_ExtBufPools = &p_FmPort->p_FmPortDriverParam->extBufPools;
     t_FmBufPoolDepletion        *p_BufPoolDepletion = &p_FmPort->p_FmPortDriverParam->bufPoolDepletion;
-    volatile uint32_t           *p_ExtBufRegs;
-    volatile uint32_t           *p_BufPoolDepletionReg;
-    bool                        rxPort;
     uint8_t                     orderedArray[FM_PORT_MAX_NUM_OF_EXT_POOLS];
     uint16_t                    sizesArray[BM_MAX_NUM_OF_POOLS];
-    uint8_t                     numOfPools;
-    int                         i=0, j=0;
-    uint32_t                    tmpReg, vector;
+    int                         i=0, j=0, err;
+    struct fman_port_bpools     bpools;
 
     memset(&orderedArray, 0, sizeof(uint8_t) * FM_PORT_MAX_NUM_OF_EXT_POOLS);
     memset(&sizesArray, 0, sizeof(uint16_t) * BM_MAX_NUM_OF_POOLS);
     memcpy(&p_FmPort->extBufPools, p_ExtBufPools, sizeof(t_FmExtPools));
 
-    switch (p_FmPort->portType)
-    {
-        case (e_FM_PORT_TYPE_RX_10G):
-        case (e_FM_PORT_TYPE_RX):
-            p_ExtBufRegs = p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_ebmpi;
-            p_BufPoolDepletionReg = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rmpd;
-            rxPort = TRUE;
-            break;
-        case (e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
-            p_ExtBufRegs = p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_oebmpi;
-            p_BufPoolDepletionReg = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ompd;
-            rxPort = FALSE;
-            break;
-        default:
-            RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("Not available for port type"));
-    }
-
     FmSpSetBufPoolsInAscOrderOfBufSizes(p_ExtBufPools, orderedArray, sizesArray);
 
-    /* build the register value */
-    for (i=0;i<p_ExtBufPools->numOfPoolsUsed;i++)
+    /* Prepare flibs bpools structure */
+    memset(&bpools, 0, sizeof(struct fman_port_bpools));
+    bpools.count = p_ExtBufPools->numOfPoolsUsed;
+    bpools.counters_enable = TRUE;
+    for (i=0; i<p_ExtBufPools->numOfPoolsUsed; i++)
     {
-        tmpReg = BMI_EXT_BUF_POOL_VALID | BMI_EXT_BUF_POOL_EN_COUNTER;
-        tmpReg |= ((uint32_t)orderedArray[i] << BMI_EXT_BUF_POOL_ID_SHIFT);
-        tmpReg |= sizesArray[orderedArray[i]];
-        /* functionality available only for some deriviatives (limited by config) */
+        bpools.bpool[i].bpid = orderedArray[i];
+        bpools.bpool[i].size = sizesArray[orderedArray[i]];
+         /* functionality available only for some derivatives (limited by config) */
         if (p_FmPort->p_FmPortDriverParam->p_BackupBmPools)
-            for (j=0;j<p_FmPort->p_FmPortDriverParam->p_BackupBmPools->numOfBackupPools;j++)
+            for (j=0; j<p_FmPort->p_FmPortDriverParam->p_BackupBmPools->numOfBackupPools; j++)
                 if (orderedArray[i] == p_FmPort->p_FmPortDriverParam->p_BackupBmPools->poolIds[j])
                 {
-                    tmpReg |= BMI_EXT_BUF_POOL_BACKUP;
+                    bpools.bpool[i].is_backup = TRUE;
                     break;
                 }
-        WRITE_UINT32(*(p_ExtBufRegs+i), tmpReg);
     }
 
-    if (p_FmPort->p_FmPortDriverParam->p_BackupBmPools)
-        XX_Free(p_FmPort->p_FmPortDriverParam->p_BackupBmPools);
-
-    numOfPools = (uint8_t)(rxPort ? FM_PORT_MAX_NUM_OF_EXT_POOLS:FM_PORT_MAX_NUM_OF_OBSERVED_EXT_POOLS);
-
-    /* clear unused pools */
-    for (i=p_ExtBufPools->numOfPoolsUsed;i<numOfPools;i++)
-        WRITE_UINT32(*(p_ExtBufRegs+i), 0);
-
     /* save pools parameters for later use */
     p_FmPort->rxPoolsParams.numOfPools = p_ExtBufPools->numOfPoolsUsed;
     p_FmPort->rxPoolsParams.largestBufSize = sizesArray[orderedArray[p_ExtBufPools->numOfPoolsUsed-1]];
     p_FmPort->rxPoolsParams.secondLargestBufSize = sizesArray[orderedArray[p_ExtBufPools->numOfPoolsUsed-2]];
 
     /* FMBM_RMPD reg. - pool depletion */
-    tmpReg = 0;
     if (p_BufPoolDepletion->poolsGrpModeEnable)
     {
-        /* calculate vector for number of pools depletion */
-        vector = 0;
+        bpools.grp_bp_depleted_num = p_BufPoolDepletion->numOfPools;
         for (i=0;i<BM_MAX_NUM_OF_POOLS;i++)
         {
             if (p_BufPoolDepletion->poolsToConsider[i])
@@ -480,21 +471,16 @@ static t_Error SetExtBufferPools(t_FmPort *p_FmPort)
                 {
                     if (i == orderedArray[j])
                     {
-                        vector |= 0x80000000 >> j;
+                        bpools.bpool[j].grp_bp_depleted = TRUE;
                         break;
                     }
                 }
             }
         }
-        /* configure num of pools and vector for number of pools mode */
-        tmpReg |= (((uint32_t)p_BufPoolDepletion->numOfPools - 1) << BMI_POOL_DEP_NUM_OF_POOLS_SHIFT);
-        tmpReg |= vector;
     }
 
     if (p_BufPoolDepletion->singlePoolModeEnable)
     {
-        /* calculate vector for number of pools depletion */
-        vector = 0;
         for (i=0;i<BM_MAX_NUM_OF_POOLS;i++)
         {
             if (p_BufPoolDepletion->poolsToConsiderForSingleMode[i])
@@ -503,13 +489,12 @@ static t_Error SetExtBufferPools(t_FmPort *p_FmPort)
                 {
                     if (i == orderedArray[j])
                      {
-                        vector |= 0x00000080 >> j;
+                        bpools.bpool[j].single_bp_depleted = TRUE;
                         break;
                     }
                 }
             }
         }
-        tmpReg |= vector;
     }
 
 #if (DPAA_VERSION >= 11)
@@ -517,19 +502,23 @@ static t_Error SetExtBufferPools(t_FmPort *p_FmPort)
     if (p_BufPoolDepletion->poolsGrpModeEnable ||
         p_BufPoolDepletion->singlePoolModeEnable)
     {
-        vector = 0;
         for (i=0; i<FM_MAX_NUM_OF_PFC_PRIORITIES; i++)
         {
             if (p_BufPoolDepletion->pfcPrioritiesEn[i] == TRUE)
             {
-                vector |= 0x00008000 >> i;
+                bpools.bpool[i].pfc_priorities_en = TRUE;
             }
         }
-        tmpReg |= vector;
     }
 #endif /* (DPAA_VERSION >= 11) */
 
-    WRITE_UINT32(*p_BufPoolDepletionReg, tmpReg);
+    /* Issue flibs function */
+    err = fman_port_set_bpools(&p_FmPort->port, &bpools);
+    if (err != 0)
+        RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("fman_port_set_bpools"));
+
+    if (p_FmPort->p_FmPortDriverParam->p_BackupBmPools)
+        XX_Free(p_FmPort->p_FmPortDriverParam->p_BackupBmPools);
 
     return E_OK;
 }
@@ -544,749 +533,319 @@ static t_Error ClearPerfCnts(t_FmPort *p_FmPort)
     return E_OK;
 }
 
-static t_Error BmiRxPortInit(t_FmPort *p_FmPort)
+
+static t_Error InitLowLevelDriver(t_FmPort *p_FmPort)
 {
-    t_FmPortRxBmiRegs       *p_Regs = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs;
-    uint32_t                tmpReg;
-    t_FmPortDriverParam     *p_Params = p_FmPort->p_FmPortDriverParam;
-    uint32_t                errorsToEnq = 0;
-    t_FmPortPerformanceCnt  performanceContersParams;
+    t_FmPortDriverParam     *p_DriverParams = p_FmPort->p_FmPortDriverParam;
+    struct fman_port_params portParams;
+    uint32_t                tmpVal;
     t_Error                 err;
 
-    /* check that port is not busy */
-    if (GET_UINT32(p_Regs->fmbm_rcfg) & BMI_PORT_CFG_EN)
-         RETURN_ERROR(MAJOR, E_INVALID_STATE,
-                      ("Port(%d,%d) is already enabled",p_FmPort->portType, p_FmPort->portId));
+    /* Set up flibs parameters and issue init function */
 
-    /* Set Config register */
-    tmpReg = 0;
-    if (p_FmPort->imEn)
-        tmpReg |= BMI_PORT_CFG_IM;
-    /* No discard - all error frames go to error queue */
-    else if (p_Params->frmDiscardOverride)
-        tmpReg |= BMI_PORT_CFG_FDOVR;
-
-    WRITE_UINT32(p_Regs->fmbm_rcfg, tmpReg);
-
-    /* Configure dma attributes */
-    tmpReg = 0;
-    tmpReg |= (uint32_t)p_Params->dmaSwapData << BMI_DMA_ATTR_SWP_SHIFT;
-    tmpReg |= (uint32_t)p_Params->dmaIntContextCacheAttr << BMI_DMA_ATTR_IC_CACHE_SHIFT;
-    tmpReg |= (uint32_t)p_Params->dmaHeaderCacheAttr << BMI_DMA_ATTR_HDR_CACHE_SHIFT;
-    tmpReg |= (uint32_t)p_Params->dmaScatterGatherCacheAttr << BMI_DMA_ATTR_SG_CACHE_SHIFT;
-    if (p_Params->dmaWriteOptimize)
-        tmpReg |= BMI_DMA_ATTR_WRITE_OPTIMIZE;
-
-    WRITE_UINT32(p_Regs->fmbm_rda, tmpReg);
-
-    /* Configure Rx Fifo params */
-    tmpReg = 0;
-    tmpReg |= ((p_Params->rxFifoPriElevationLevel/BMI_FIFO_UNITS - 1) << BMI_RX_FIFO_PRI_ELEVATION_SHIFT);
-    tmpReg |= ((p_Params->rxFifoThreshold/BMI_FIFO_UNITS - 1) << BMI_RX_FIFO_THRESHOLD_SHIFT);
-
-    WRITE_UINT32(p_Regs->fmbm_rfp, tmpReg);
-
-#ifdef FM_NO_RESTRICT_ON_ACCESS_RSRC
-    if (p_FmPort->fmRevInfo.majorRev < 6)
-        /* always allow access to the extra resources */
-        WRITE_UINT32(p_Regs->fmbm_reth, BMI_RX_FIFO_THRESHOLD_BC);
-#endif /* FM_NO_RESTRICT_ON_ACCESS_RSRC */
-
-     /* frame end parameters */
-    tmpReg = 0;
-    tmpReg |= ((uint32_t)p_Params->cheksumLastBytesIgnore << BMI_RX_FRAME_END_CS_IGNORE_SHIFT);
-    tmpReg |= ((uint32_t)p_Params->cutBytesFromEnd<< BMI_RX_FRAME_END_CUT_SHIFT);
-#ifdef FM_RX_FIFO_CORRUPT_ERRATA_10GMAC_A006320
-    /* zero cutBytesFromEnd field which means that bmi doesn't
-       remove further bytes because the MAC already remove the CRC.
-       the workaround is relevant only in initial rev of FMan v3.
-     */
-    if ((p_FmPort->fmRevInfo.majorRev == 6) && (p_FmPort->fmRevInfo.minorRev == 0))
-        tmpReg &= 0xffe0ffff;
-#endif /* FM_RX_FIFO_CORRUPT_ERRATA_10GMAC_A006320 */
-    WRITE_UINT32(p_Regs->fmbm_rfed, tmpReg);
-
-    /* IC parameters */
-    tmpReg = 0;
-    tmpReg |= (((uint32_t)p_Params->intContext.extBufOffset/OFFSET_UNITS) << BMI_IC_TO_EXT_SHIFT);
-    tmpReg |= (((uint32_t)p_Params->intContext.intContextOffset/OFFSET_UNITS) << BMI_IC_FROM_INT_SHIFT);
-    tmpReg |= (((uint32_t)p_Params->intContext.size/OFFSET_UNITS)  << BMI_IC_SIZE_SHIFT);
-
-    WRITE_UINT32(p_Regs->fmbm_ricp, tmpReg);
-
-    if (!p_FmPort->imEn)
-    {
-        /* Call the external Buffer routine which also checks fifo
-           size and updates it if necessary */
-        /* define external buffer pools and pool depletion*/
-
-        /* check if the largest external buffer pool is large enough */
-        if ((p_Params->bufMargins.startMargins +
-             MIN_EXT_BUF_SIZE +
-             p_Params->bufMargins.endMargins) > p_FmPort->rxPoolsParams.largestBufSize)
-            RETURN_ERROR(MAJOR, E_INVALID_VALUE,
-                         ("bufMargins.startMargins (%d) + minimum buf size (64) + "
-                          "bufMargins.endMargins (%d) is larger than maximum external buffer size (%d)",
-                            p_Params->bufMargins.startMargins,
-                            p_Params->bufMargins.endMargins,
-                            p_FmPort->rxPoolsParams.largestBufSize));
-
-        /* buffer margins */
-        tmpReg = 0;
-        tmpReg |= (((uint32_t)p_Params->bufMargins.startMargins) << BMI_EXT_BUF_MARG_START_SHIFT);
-        tmpReg |= (((uint32_t)p_Params->bufMargins.endMargins) << BMI_EXT_BUF_MARG_END_SHIFT);
-#if (DPAA_VERSION >= 11)
-        if (p_Params->noScatherGather)
-            tmpReg |= BMI_SG_DISABLE;
-#endif
-        WRITE_UINT32(p_Regs->fmbm_rebm, tmpReg);
-    }
-
-    if (p_FmPort->internalBufferOffset)
-    {
-        tmpReg = (uint32_t)((p_FmPort->internalBufferOffset%OFFSET_UNITS) ?
-                            (p_FmPort->internalBufferOffset/OFFSET_UNITS + 1) :
-                            (p_FmPort->internalBufferOffset/OFFSET_UNITS));
-        p_FmPort->internalBufferOffset = (uint8_t)(tmpReg * OFFSET_UNITS);
-        WRITE_UINT32(p_Regs->fmbm_rim, tmpReg << BMI_IM_FOF_SHIFT);
-    }
-
-    /* NIA */
-    if (p_FmPort->imEn)
-        WRITE_UINT32(p_Regs->fmbm_rfne, NIA_ENG_FM_CTL | NIA_FM_CTL_AC_IND_MODE_RX);
-    else
-    {
-        tmpReg = 0;
-        if (p_Params->forwardReuseIntContext)
-            tmpReg |= BMI_PORT_RFNE_FRWD_RPD;
-        /* L3/L4 checksum verify is enabled by default. */
-        /*tmpReg |= BMI_PORT_RFNE_FRWD_DCL4C;*/
-        WRITE_UINT32(p_Regs->fmbm_rfne, tmpReg | GET_NO_PCD_NIA_BMI_AC_ENQ_FRAME());
-    }
-    WRITE_UINT32(p_Regs->fmbm_rfene, NIA_ENG_QMI_ENQ | NIA_ORDER_RESTOR);
-
-    /* command attribute */
-    tmpReg = BMI_CMD_RX_MR_DEF;
-    if (!p_FmPort->imEn)
+    memset(&portParams, 0, sizeof(struct fman_port_params));
+    portParams.discard_mask = p_DriverParams->errorsToDiscard;
+    portParams.dflt_fqid = p_DriverParams->dfltFqid;
+    portParams.err_fqid = p_DriverParams->errFqid;
+    portParams.deq_sp = p_DriverParams->deqSubPortal;
+    portParams.dont_release_buf = p_DriverParams->dontReleaseBuf;
+    switch (p_FmPort->portType)
     {
-        tmpReg |= BMI_CMD_ATTR_ORDER;
-        if (p_Params->syncReq)
-            tmpReg |= BMI_CMD_ATTR_SYNC;
-        tmpReg |= ((uint32_t)p_Params->color << BMI_CMD_ATTR_COLOR_SHIFT);
-    }
+        case(e_FM_PORT_TYPE_RX_10G):
+        case(e_FM_PORT_TYPE_RX):
+            portParams.err_mask = (RX_ERRS_TO_ENQ & ~portParams.discard_mask);
+            if (!p_FmPort->imEn)
+            {
+                if (p_DriverParams->forwardReuseIntContext)
+                    p_DriverParams->dfltCfg.rx_fd_bits = (uint8_t)(BMI_PORT_RFNE_FRWD_RPD >> 24);
+            }
+            break;
 
-    WRITE_UINT32(p_Regs->fmbm_rfca, tmpReg);
+        case(e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
+            portParams.err_mask = (OP_ERRS_TO_ENQ & ~portParams.discard_mask);
+            break;
+            break;
 
-    /* default queues */
-    if (!p_FmPort->imEn)
-    {
-        WRITE_UINT32(p_Regs->fmbm_rfqid, p_Params->dfltFqid);
-        WRITE_UINT32(p_Regs->fmbm_refqid, p_Params->errFqid);
+        default:
+            break;
     }
 
-    /* set counters */
-    WRITE_UINT32(p_Regs->fmbm_rstc, BMI_COUNTERS_EN);
-
-    performanceContersParams.taskCompVal    = (uint8_t)p_FmPort->tasks.num;
-    performanceContersParams.queueCompVal   = 1;
-    performanceContersParams.dmaCompVal     =(uint8_t) p_FmPort->openDmas.num;
-    performanceContersParams.fifoCompVal    = p_FmPort->fifoBufs.num;
-    if ((err = FM_PORT_SetPerformanceCountersParams(p_FmPort, &performanceContersParams)) != E_OK)
-        RETURN_ERROR(MAJOR, err, NO_MSG);
-
-    WRITE_UINT32(p_Regs->fmbm_rpc, BMI_COUNTERS_EN);
-
-    /* error/status mask  - check that if discard OV is set, no
-       discard is required for specific errors.*/
-    WRITE_UINT32(p_Regs->fmbm_rfsdm, p_Params->errorsToDiscard);
-
-    errorsToEnq = (RX_ERRS_TO_ENQ & ~p_Params->errorsToDiscard);
-    WRITE_UINT32(p_Regs->fmbm_rfsem, errorsToEnq);
-
-    return E_OK;
-}
-
-static t_Error BmiTxPortInit(t_FmPort *p_FmPort)
-{
-    t_FmPortTxBmiRegs   *p_Regs = &p_FmPort->p_FmPortBmiRegs->txPortBmiRegs;
-    uint32_t            tmpReg;
-    t_FmPortDriverParam *p_Params = p_FmPort->p_FmPortDriverParam;
-    /*uint32_t            rateCountUnit;*/
-    t_FmPortPerformanceCnt  performanceContersParams;
-
-    /* check that port is not busy */
-    if (GET_UINT32(p_Regs->fmbm_tcfg) & BMI_PORT_CFG_EN)
-         RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Port is already enabled"));
-
-    tmpReg = 0;
-    if (p_FmPort->imEn)
-        tmpReg |= BMI_PORT_CFG_IM;
-
-    WRITE_UINT32(p_Regs->fmbm_tcfg, tmpReg);
+    tmpVal = (uint32_t)((p_FmPort->internalBufferOffset % OFFSET_UNITS) ?
+                        (p_FmPort->internalBufferOffset/OFFSET_UNITS + 1):
+                        (p_FmPort->internalBufferOffset/OFFSET_UNITS));
+    p_FmPort->internalBufferOffset = (uint8_t)(tmpVal * OFFSET_UNITS);
+    p_DriverParams->dfltCfg.int_buf_start_margin = p_FmPort->internalBufferOffset;
 
-    /* Configure dma attributes */
-    tmpReg = 0;
-    tmpReg |= (uint32_t)p_Params->dmaSwapData << BMI_DMA_ATTR_SWP_SHIFT;
-    tmpReg |= (uint32_t)p_Params->dmaIntContextCacheAttr << BMI_DMA_ATTR_IC_CACHE_SHIFT;
-    tmpReg |= (uint32_t)p_Params->dmaHeaderCacheAttr << BMI_DMA_ATTR_HDR_CACHE_SHIFT;
-    tmpReg |= (uint32_t)p_Params->dmaScatterGatherCacheAttr << BMI_DMA_ATTR_SG_CACHE_SHIFT;
+    p_DriverParams->dfltCfg.ext_buf_start_margin = p_DriverParams->bufMargins.startMargins;
+    p_DriverParams->dfltCfg.ext_buf_end_margin = p_DriverParams->bufMargins.endMargins;
 
-    WRITE_UINT32(p_Regs->fmbm_tda, tmpReg);
+    p_DriverParams->dfltCfg.ic_ext_offset = p_DriverParams->intContext.extBufOffset;
+    p_DriverParams->dfltCfg.ic_int_offset = p_DriverParams->intContext.intContextOffset;
+    p_DriverParams->dfltCfg.ic_size = p_DriverParams->intContext.size;
 
-    /* Configure Tx Fifo params */
-    tmpReg = 0;
-    tmpReg |= ((p_Params->txFifoMinFillLevel/BMI_FIFO_UNITS) << BMI_TX_FIFO_MIN_FILL_SHIFT);
-    tmpReg |= (((uint32_t)p_FmPort->fifoDeqPipelineDepth - 1) << BMI_FIFO_PIPELINE_DEPTH_SHIFT);
-    tmpReg |= ((p_Params->txFifoLowComfLevel/BMI_FIFO_UNITS - 1) << BMI_TX_LOW_COMF_SHIFT);
+    p_DriverParams->dfltCfg.stats_counters_enable = TRUE;
+    p_DriverParams->dfltCfg.perf_counters_enable = TRUE;
+    p_DriverParams->dfltCfg.queue_counters_enable = TRUE;
 
-    WRITE_UINT32(p_Regs->fmbm_tfp, tmpReg);
-
-    /* frame end parameters */
-    tmpReg = 0;
-    tmpReg |= ((uint32_t)p_Params->cheksumLastBytesIgnore << BMI_FRAME_END_CS_IGNORE_SHIFT);
-
-    WRITE_UINT32(p_Regs->fmbm_tfed, tmpReg);
-
-    if (!p_FmPort->imEn)
-    {
-        /* IC parameters */
-        tmpReg = 0;
-        tmpReg |= (((uint32_t)p_Params->intContext.extBufOffset/OFFSET_UNITS) << BMI_IC_TO_EXT_SHIFT);
-        tmpReg |= (((uint32_t)p_Params->intContext.intContextOffset/OFFSET_UNITS) << BMI_IC_FROM_INT_SHIFT);
-        tmpReg |= (((uint32_t)p_Params->intContext.size/OFFSET_UNITS)  << BMI_IC_SIZE_SHIFT);
+    p_DriverParams->dfltCfg.perf_cnt_params.task_val    = (uint8_t)p_FmPort->tasks.num;
+    if (p_FmPort->portType == e_FM_PORT_TYPE_OH_OFFLINE_PARSING ||
+        p_FmPort->portType == e_FM_PORT_TYPE_OH_HOST_COMMAND)
+        p_DriverParams->dfltCfg.perf_cnt_params.queue_val   = 0;
+    else
+        p_DriverParams->dfltCfg.perf_cnt_params.queue_val   = 1;
+    p_DriverParams->dfltCfg.perf_cnt_params.dma_val     =(uint8_t) p_FmPort->openDmas.num;
+    p_DriverParams->dfltCfg.perf_cnt_params.fifo_val    = p_FmPort->fifoBufs.num;
 
-        WRITE_UINT32(p_Regs->fmbm_ticp, tmpReg);
-    }
+    if (0 != fman_port_init(&p_FmPort->port, &p_DriverParams->dfltCfg, &portParams))
+        RETURN_ERROR(MAJOR, E_NO_DEVICE, ("fman_port_init"));
 
-    /* NIA */
-    if (p_FmPort->imEn)
+    if (p_FmPort->imEn && ((err = FmPortImInit(p_FmPort)) != E_OK))
+        RETURN_ERROR(MAJOR, err, NO_MSG);
+    else
     {
-        WRITE_UINT32(p_Regs->fmbm_tfdne, NIA_ENG_FM_CTL | NIA_FM_CTL_AC_IND_MODE_TX);
-        WRITE_UINT32(p_Regs->fmbm_tfene, NIA_ENG_FM_CTL | NIA_FM_CTL_AC_IND_MODE_TX);
-#if (DPAA_VERSION >= 11)
-/* TODO - what should be the TFNE for FMan v3 IM? */
-#endif /* (DPAA_VERSION >= 11) */
+    //  from QMIInit
+        if ((p_FmPort->portType != e_FM_PORT_TYPE_RX_10G) &&
+            (p_FmPort->portType != e_FM_PORT_TYPE_RX))
+        {
+            if (p_DriverParams->deqPrefetchOption == e_FM_PORT_DEQ_NO_PREFETCH)
+                FmSetPortPreFetchConfiguration(p_FmPort->h_Fm, p_FmPort->portId, FALSE);
+            else
+                FmSetPortPreFetchConfiguration(p_FmPort->h_Fm, p_FmPort->portId, TRUE);
+        }
     }
-    else
+    /* The code bellow is a trick so the FM will not release the buffer
+       to BM nor will try to enqueue the frame to QM */
+    if (((p_FmPort->portType == e_FM_PORT_TYPE_TX_10G) ||
+         (p_FmPort->portType == e_FM_PORT_TYPE_TX)) &&
+        (!p_FmPort->imEn))
     {
-        WRITE_UINT32(p_Regs->fmbm_tfdne, NIA_ENG_QMI_DEQ);
-        WRITE_UINT32(p_Regs->fmbm_tfene, NIA_ENG_QMI_ENQ | NIA_ORDER_RESTOR);
-#if (DPAA_VERSION >= 11)
-        WRITE_UINT32(p_Regs->fmbm_tfne,
-                     (!p_Params->dfltFqid ?
-                        BMI_EBD_EN | NIA_BMI_AC_FETCH_ALL_FRAME :
-                        NIA_BMI_AC_FETCH_ALL_FRAME));
-#endif /* (DPAA_VERSION >= 11) */
-
-        /* The line bellow is a trick so the FM will not release the buffer
-           to BM nor will try to enq the frame to QM */
-        if (!p_Params->dfltFqid && p_Params->dontReleaseBuf)
+        if (!p_DriverParams->dfltFqid && p_DriverParams->dontReleaseBuf)
         {
             /* override fmbm_tcfqid 0 with a false non-0 value. This will force FM to
-             * act according to tfene. Otherwise, if fmbm_tcfqid is 0 the FM will release
+             * act acording to tfene. Otherwise, if fmbm_tcfqid is 0 the FM will release
              * buffers to BM regardless of fmbm_tfene
              */
-            WRITE_UINT32(p_Regs->fmbm_tcfqid, 0xFFFFFF);
-            WRITE_UINT32(p_Regs->fmbm_tfene, NIA_ENG_BMI | NIA_BMI_AC_TX_RELEASE);
-#if (DPAA_VERSION >= 11)
-            WRITE_UINT32(p_Regs->fmbm_tfne,
-                         (GET_UINT32(p_Regs->fmbm_tfne) & ~BMI_EBD_EN));
-#endif /* (DPAA_VERSION >= 11) */
+            WRITE_UINT32(p_FmPort->port.bmi_regs->tx.fmbm_tcfqid, 0xFFFFFF);
+            WRITE_UINT32(p_FmPort->port.bmi_regs->tx.fmbm_tfene, NIA_ENG_BMI | NIA_BMI_AC_TX_RELEASE);
         }
     }
 
-    /* command attribute */
-    tmpReg = BMI_CMD_TX_MR_DEF;
-    if (p_FmPort->imEn)
-        tmpReg |= BMI_CMD_MR_DEAS;
-    else
-    {
-        tmpReg |= BMI_CMD_ATTR_ORDER;
-        tmpReg |= ((uint32_t)p_Params->color << BMI_CMD_ATTR_COLOR_SHIFT);
-    }
-    WRITE_UINT32(p_Regs->fmbm_tfca, tmpReg);
-
-    /* default queues */
-    if (!p_FmPort->imEn)
-    {
-        if (p_Params->dfltFqid || !p_Params->dontReleaseBuf)
-            WRITE_UINT32(p_Regs->fmbm_tcfqid, p_Params->dfltFqid);
-        WRITE_UINT32(p_Regs->fmbm_tfeqid, p_Params->errFqid);
-    }
-
-    /* statistics & performance counters */
-    WRITE_UINT32(p_Regs->fmbm_tstc, BMI_COUNTERS_EN);
-
-    performanceContersParams.taskCompVal    = (uint8_t)p_FmPort->tasks.num;
-    performanceContersParams.queueCompVal   = 1;
-    performanceContersParams.dmaCompVal     = (uint8_t)p_FmPort->openDmas.num;
-    performanceContersParams.fifoCompVal    = p_FmPort->fifoBufs.num;
-    FM_PORT_SetPerformanceCountersParams(p_FmPort, &performanceContersParams);
-
-    WRITE_UINT32(p_Regs->fmbm_tpc, BMI_COUNTERS_EN);
-
-
     return E_OK;
 }
 
-static t_Error BmiOhPortInit(t_FmPort *p_FmPort)
-{
-    t_FmPortOhBmiRegs       *p_Regs = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs;
-    uint32_t                tmpReg, errorsToEnq = 0;
-    t_FmPortDriverParam     *p_Params = p_FmPort->p_FmPortDriverParam;
-    t_FmPortPerformanceCnt  performanceContersParams;
-
-    /* check that port is not busy */
-    if (GET_UINT32(p_Regs->fmbm_ocfg) & BMI_PORT_CFG_EN)
-         RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Port is already enabled"));
-
-    /* Configure dma attributes */
-    tmpReg = 0;
-    tmpReg |= (uint32_t)p_Params->dmaSwapData << BMI_DMA_ATTR_SWP_SHIFT;
-    tmpReg |= (uint32_t)p_Params->dmaIntContextCacheAttr << BMI_DMA_ATTR_IC_CACHE_SHIFT;
-    tmpReg |= (uint32_t)p_Params->dmaHeaderCacheAttr << BMI_DMA_ATTR_HDR_CACHE_SHIFT;
-    tmpReg |= (uint32_t)p_Params->dmaScatterGatherCacheAttr << BMI_DMA_ATTR_SG_CACHE_SHIFT;
-    if (p_Params->dmaWriteOptimize)
-        tmpReg |= BMI_DMA_ATTR_WRITE_OPTIMIZE;
-
-    WRITE_UINT32(p_Regs->fmbm_oda, tmpReg);
-
-    /* IC parameters */
-    tmpReg = 0;
-    tmpReg |= (((uint32_t)p_Params->intContext.extBufOffset/OFFSET_UNITS) << BMI_IC_TO_EXT_SHIFT);
-    tmpReg |= (((uint32_t)p_Params->intContext.intContextOffset/OFFSET_UNITS) << BMI_IC_FROM_INT_SHIFT);
-    tmpReg |= (((uint32_t)p_Params->intContext.size/OFFSET_UNITS)  << BMI_IC_SIZE_SHIFT);
-
-    WRITE_UINT32(p_Regs->fmbm_oicp, tmpReg);
-
-    /* NIA */
-    WRITE_UINT32(p_Regs->fmbm_ofdne, NIA_ENG_QMI_DEQ);
-
-    if (p_FmPort->portType==e_FM_PORT_TYPE_OH_HOST_COMMAND)
-        WRITE_UINT32(p_Regs->fmbm_ofene, NIA_ENG_QMI_ENQ);
-    else
-        WRITE_UINT32(p_Regs->fmbm_ofene, NIA_ENG_QMI_ENQ | NIA_ORDER_RESTOR);
-
-    /* command attribute */
-    if (p_FmPort->portType==e_FM_PORT_TYPE_OH_HOST_COMMAND)
-        tmpReg =  BMI_CMD_MR_DEAS | BMI_CMD_MR_MA;
-    else
-        tmpReg = BMI_CMD_ATTR_ORDER | BMI_CMD_MR_DEAS | BMI_CMD_MR_MA;
-
-    if (p_Params->syncReq)
-        tmpReg |= BMI_CMD_ATTR_SYNC;
-    tmpReg |= ((uint32_t)p_Params->color << BMI_CMD_ATTR_COLOR_SHIFT);
-    WRITE_UINT32(p_Regs->fmbm_ofca, tmpReg);
-
-    /* No discard - all error frames go to error queue */
-    if (p_Params->frmDiscardOverride)
-        tmpReg = BMI_PORT_CFG_FDOVR;
-    else
-        tmpReg = 0;
-    WRITE_UINT32(p_Regs->fmbm_ocfg, tmpReg);
-
-    if (p_FmPort->portType == e_FM_PORT_TYPE_OH_OFFLINE_PARSING)
-    {
-        WRITE_UINT32(p_Regs->fmbm_ofsdm, p_Params->errorsToDiscard);
-
-        errorsToEnq = (OP_ERRS_TO_ENQ & ~p_Params->errorsToDiscard);
-        WRITE_UINT32(p_Regs->fmbm_ofsem, errorsToEnq);
-
-        /* NIA */
-        WRITE_UINT32(p_Regs->fmbm_ofne, GET_NO_PCD_NIA_BMI_AC_ENQ_FRAME());
-
-#ifndef FM_NO_OP_OBSERVED_POOLS
-        /* Call the external Buffer routine which also checks fifo
-           size and updates it if necessary */
-        if ((p_FmPort->fmRevInfo.majorRev == 4) &&
-            p_Params->enBufPoolDepletion)
-        {
-            /* define external buffer pools */
-            t_Error err = SetExtBufferPools(p_FmPort);
-            if (err)
-                RETURN_ERROR(MAJOR, err, NO_MSG);
-        }
-#endif /* FM_NO_OP_OBSERVED_POOLS */
-    }
-    else
-        /* NIA */
-        WRITE_UINT32(p_Regs->fmbm_ofne, NIA_ENG_FM_CTL | NIA_FM_CTL_AC_HC);
-
-    /* default queues */
-    WRITE_UINT32(p_Regs->fmbm_ofqid, p_Params->dfltFqid);
-    WRITE_UINT32(p_Regs->fmbm_oefqid, p_Params->errFqid);
-
-    if (p_FmPort->internalBufferOffset)
-    {
-        tmpReg = (uint32_t)((p_FmPort->internalBufferOffset % OFFSET_UNITS) ?
-                            (p_FmPort->internalBufferOffset/OFFSET_UNITS + 1):
-                            (p_FmPort->internalBufferOffset/OFFSET_UNITS));
-        p_FmPort->internalBufferOffset = (uint8_t)(tmpReg * OFFSET_UNITS);
-        WRITE_UINT32(p_Regs->fmbm_oim, tmpReg << BMI_IM_FOF_SHIFT);
-    }
-    /* statistics & performance counters */
-    WRITE_UINT32(p_Regs->fmbm_ostc, BMI_COUNTERS_EN);
-
-    performanceContersParams.taskCompVal    = (uint8_t)p_FmPort->tasks.num;
-    performanceContersParams.queueCompVal   = 0;
-    performanceContersParams.dmaCompVal     = (uint8_t)p_FmPort->openDmas.num;
-    performanceContersParams.fifoCompVal    = p_FmPort->fifoBufs.num;
-    FM_PORT_SetPerformanceCountersParams(p_FmPort, &performanceContersParams);
-
-    WRITE_UINT32(p_Regs->fmbm_opc, BMI_COUNTERS_EN);
-#ifdef FM_DEQ_PIPELINE_PARAMS_FOR_OP
-    if ((p_FmPort->fmRevInfo.majorRev == 4) ||
-        (p_FmPort->fmRevInfo.majorRev >= 6))
-    {
-        tmpReg = (((uint32_t)p_FmPort->fifoDeqPipelineDepth - 1) << BMI_FIFO_PIPELINE_DEPTH_SHIFT);
-        WRITE_UINT32(p_Regs->fmbm_ofp, tmpReg);
-    }
-#endif /* FM_DEQ_PIPELINE_PARAMS_FOR_OP */
-
-#ifdef FM_FRAME_END_PARAMS_FOR_OP
-    if (p_FmPort->fmRevInfo.majorRev >= 6)
-    {
-        /* frame end parameters */
-        tmpReg = 0;
-        tmpReg |= ((uint32_t)p_Params->cheksumLastBytesIgnore << BMI_FRAME_END_CS_IGNORE_SHIFT);
-
-        WRITE_UINT32(p_Regs->fmbm_ofed, tmpReg);
-    }
-#endif /* FM_FRAME_END_PARAMS_FOR_OP */
-
-    return E_OK;
-}
 
-static t_Error QmiInit(t_FmPort *p_FmPort)
+static bool CheckRxBmiCounter(t_FmPort *p_FmPort, e_FmPortCounters counter)
 {
-    t_FmPortDriverParam             *p_Params = NULL;
-    uint32_t                        tmpReg;
-
-    p_Params = p_FmPort->p_FmPortDriverParam;
-
-    /* check that port is not busy */
-    if (((p_FmPort->portType != e_FM_PORT_TYPE_RX_10G) &&
-        (p_FmPort->portType != e_FM_PORT_TYPE_RX)) &&
-       (GET_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnc) & QMI_PORT_CFG_EN))
-         RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Port is already enabled"));
-
-    /* enable & clear counters */
-    WRITE_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnc, QMI_PORT_CFG_EN_COUNTERS);
+    UNUSED(p_FmPort);
 
-    /* The following is  done for non-Rx ports only */
-    if ((p_FmPort->portType != e_FM_PORT_TYPE_RX_10G) &&
-        (p_FmPort->portType != e_FM_PORT_TYPE_RX))
+    switch (counter)
     {
-        if ((p_FmPort->portType == e_FM_PORT_TYPE_TX_10G) ||
-            (p_FmPort->portType == e_FM_PORT_TYPE_TX))
-        {
-            /* define dequeue NIA */
-            WRITE_UINT32(p_FmPort->p_FmPortQmiRegs->nonRxQmiRegs.fmqm_pndn, NIA_ENG_BMI | NIA_BMI_AC_TX);
-            /* define enqueue NIA */
-            WRITE_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnen, NIA_ENG_BMI | NIA_BMI_AC_TX_RELEASE);
-        }
-        else  /* for HC & OP */
-        {
-            WRITE_UINT32(p_FmPort->p_FmPortQmiRegs->nonRxQmiRegs.fmqm_pndn, NIA_ENG_BMI | NIA_BMI_AC_FETCH);
-            /* define enqueue NIA */
-            WRITE_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnen, NIA_ENG_BMI | NIA_BMI_AC_RELEASE);
-        }
-
-        /* configure dequeue */
-        tmpReg = 0;
-        if (p_Params->deqHighPriority)
-            tmpReg |= QMI_DEQ_CFG_PRI;
-
-        switch (p_Params->deqType)
-        {
-            case (e_FM_PORT_DEQ_TYPE1):
-                tmpReg |= QMI_DEQ_CFG_TYPE1;
-                break;
-            case (e_FM_PORT_DEQ_TYPE2):
-                tmpReg |= QMI_DEQ_CFG_TYPE2;
-                break;
-            case (e_FM_PORT_DEQ_TYPE3):
-                tmpReg |= QMI_DEQ_CFG_TYPE3;
-                break;
-            default:
-                RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Invalid dequeue type"));
-        }
-
-#ifdef FM_QMI_NO_DEQ_OPTIONS_SUPPORT
-        if (p_FmPort->fmRevInfo.majorRev != 4)
-#endif /* FM_QMI_NO_DEQ_OPTIONS_SUPPORT */
-        switch (p_Params->deqPrefetchOption)
-        {
-            case (e_FM_PORT_DEQ_NO_PREFETCH):
-                /* Do nothing - QMI_DEQ_CFG_PREFETCH_WAITING_TNUM | QMI_DEQ_CFG_PREFETCH_1_FRAME = 0 */
-                break;
-            case (e_FM_PORT_DEQ_PARTIAL_PREFETCH):
-                tmpReg |= QMI_DEQ_CFG_PREFETCH_WAITING_TNUM | QMI_DEQ_CFG_PREFETCH_3_FRAMES;
-                break;
-            case (e_FM_PORT_DEQ_FULL_PREFETCH):
-                tmpReg |= QMI_DEQ_CFG_PREFETCH_NO_TNUM | QMI_DEQ_CFG_PREFETCH_3_FRAMES;
-                break;
-            default:
-                RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Invalid dequeue prefetch option"));
-        }
-#ifdef FM_QMI_NO_DEQ_OPTIONS_SUPPORT
-        if (p_FmPort->fmRevInfo.majorRev != 4)
-#endif /* FM_QMI_NO_DEQ_OPTIONS_SUPPORT */
-        if (p_Params->deqPrefetchOption == e_FM_PORT_DEQ_NO_PREFETCH)
-            FmSetPortPreFetchConfiguration(p_FmPort->h_Fm, p_FmPort->portId, FALSE);
-        else
-            FmSetPortPreFetchConfiguration(p_FmPort->h_Fm, p_FmPort->portId, TRUE);
-
-        tmpReg |= p_Params->deqByteCnt;
-        tmpReg |= (uint32_t)p_Params->deqSubPortal << QMI_DEQ_CFG_SUBPORTAL_SHIFT;
-
-        WRITE_UINT32(p_FmPort->p_FmPortQmiRegs->nonRxQmiRegs.fmqm_pndc, tmpReg);
+        case(e_FM_PORT_COUNTERS_CYCLE):
+        case(e_FM_PORT_COUNTERS_TASK_UTIL):
+        case(e_FM_PORT_COUNTERS_QUEUE_UTIL):
+        case(e_FM_PORT_COUNTERS_DMA_UTIL):
+        case(e_FM_PORT_COUNTERS_FIFO_UTIL):
+        case(e_FM_PORT_COUNTERS_RX_PAUSE_ACTIVATION):
+        case(e_FM_PORT_COUNTERS_FRAME):
+        case(e_FM_PORT_COUNTERS_DISCARD_FRAME):
+        case(e_FM_PORT_COUNTERS_RX_BAD_FRAME):
+        case(e_FM_PORT_COUNTERS_RX_LARGE_FRAME):
+        case(e_FM_PORT_COUNTERS_RX_FILTER_FRAME):
+        case(e_FM_PORT_COUNTERS_RX_LIST_DMA_ERR):
+        case(e_FM_PORT_COUNTERS_RX_OUT_OF_BUFFERS_DISCARD):
+        case(e_FM_PORT_COUNTERS_DEALLOC_BUF):
+        case(e_FM_PORT_COUNTERS_PREPARE_TO_ENQUEUE_COUNTER):
+            return TRUE;
+        default:
+            return FALSE;
     }
-    else /* rx port */
-        /* define enqueue NIA */
-        WRITE_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnen, NIA_ENG_BMI | NIA_BMI_AC_RELEASE);
-
-    return E_OK;
 }
 
-static t_Error BmiRxPortCheckAndGetCounterPtr(t_FmPort *p_FmPort, e_FmPortCounters counter, volatile uint32_t **p_Ptr)
+static bool CheckTxBmiCounter(t_FmPort *p_FmPort, e_FmPortCounters counter)
 {
-    t_FmPortRxBmiRegs   *p_BmiRegs = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs;
-
-     /* check that counters are enabled */
-    switch (counter)
-    {
-        case (e_FM_PORT_COUNTERS_CYCLE):
-        case (e_FM_PORT_COUNTERS_TASK_UTIL):
-        case (e_FM_PORT_COUNTERS_QUEUE_UTIL):
-        case (e_FM_PORT_COUNTERS_DMA_UTIL):
-        case (e_FM_PORT_COUNTERS_FIFO_UTIL):
-        case (e_FM_PORT_COUNTERS_RX_PAUSE_ACTIVATION):
-            /* performance counters - may be read when disabled */
-            break;
-        case (e_FM_PORT_COUNTERS_FRAME):
-        case (e_FM_PORT_COUNTERS_DISCARD_FRAME):
-        case (e_FM_PORT_COUNTERS_RX_BAD_FRAME):
-        case (e_FM_PORT_COUNTERS_RX_LARGE_FRAME):
-        case (e_FM_PORT_COUNTERS_RX_FILTER_FRAME):
-        case (e_FM_PORT_COUNTERS_RX_LIST_DMA_ERR):
-        case (e_FM_PORT_COUNTERS_RX_OUT_OF_BUFFERS_DISCARD):
-        case (e_FM_PORT_COUNTERS_DEALLOC_BUF):
-        case (e_FM_PORT_COUNTERS_PREPARE_TO_ENQUEUE_COUNTER):
-            if (!(GET_UINT32(p_BmiRegs->fmbm_rstc) & BMI_COUNTERS_EN))
-               RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Requested counter was not enabled"));
-            break;
-         default:
-            RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Requested counter is not available for Rx ports"));
-    }
+    UNUSED(p_FmPort);
 
-    /* Set counter */
     switch (counter)
     {
-        case (e_FM_PORT_COUNTERS_CYCLE):
-            *p_Ptr = &p_BmiRegs->fmbm_rccn;
-            break;
-        case (e_FM_PORT_COUNTERS_TASK_UTIL):
-            *p_Ptr = &p_BmiRegs->fmbm_rtuc;
-            break;
-        case (e_FM_PORT_COUNTERS_QUEUE_UTIL):
-            *p_Ptr = &p_BmiRegs->fmbm_rrquc;
-            break;
-        case (e_FM_PORT_COUNTERS_DMA_UTIL):
-            *p_Ptr = &p_BmiRegs->fmbm_rduc;
-            break;
-        case (e_FM_PORT_COUNTERS_FIFO_UTIL):
-            *p_Ptr = &p_BmiRegs->fmbm_rfuc;
-            break;
-        case (e_FM_PORT_COUNTERS_RX_PAUSE_ACTIVATION):
-            *p_Ptr = &p_BmiRegs->fmbm_rpac;
-            break;
-        case (e_FM_PORT_COUNTERS_FRAME):
-            *p_Ptr = &p_BmiRegs->fmbm_rfrc;
-            break;
-        case (e_FM_PORT_COUNTERS_DISCARD_FRAME):
-            *p_Ptr = &p_BmiRegs->fmbm_rfcd;
-            break;
-        case (e_FM_PORT_COUNTERS_RX_BAD_FRAME):
-            *p_Ptr = &p_BmiRegs->fmbm_rfbc;
-            break;
-        case (e_FM_PORT_COUNTERS_RX_LARGE_FRAME):
-            *p_Ptr = &p_BmiRegs->fmbm_rlfc;
-            break;
-        case (e_FM_PORT_COUNTERS_RX_FILTER_FRAME):
-            *p_Ptr = &p_BmiRegs->fmbm_rffc;
-            break;
-        case (e_FM_PORT_COUNTERS_RX_LIST_DMA_ERR):
-            *p_Ptr = &p_BmiRegs->fmbm_rfldec;
-            break;
-        case (e_FM_PORT_COUNTERS_RX_OUT_OF_BUFFERS_DISCARD):
-            *p_Ptr = &p_BmiRegs->fmbm_rodc;
-            break;
-        case (e_FM_PORT_COUNTERS_DEALLOC_BUF):
-            *p_Ptr = &p_BmiRegs->fmbm_rbdc;
-            break;
-        case (e_FM_PORT_COUNTERS_PREPARE_TO_ENQUEUE_COUNTER):
-            *p_Ptr = &p_BmiRegs->fmbm_rpec;
-            break;
+        case(e_FM_PORT_COUNTERS_CYCLE):
+        case(e_FM_PORT_COUNTERS_TASK_UTIL):
+        case(e_FM_PORT_COUNTERS_QUEUE_UTIL):
+        case(e_FM_PORT_COUNTERS_DMA_UTIL):
+        case(e_FM_PORT_COUNTERS_FIFO_UTIL):
+        case(e_FM_PORT_COUNTERS_FRAME):
+        case(e_FM_PORT_COUNTERS_DISCARD_FRAME):
+        case(e_FM_PORT_COUNTERS_LENGTH_ERR):
+        case(e_FM_PORT_COUNTERS_UNSUPPRTED_FORMAT):
+        case(e_FM_PORT_COUNTERS_DEALLOC_BUF):
+            return TRUE;
         default:
-            RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Requested counter is not available for Rx ports"));
+            return FALSE;
     }
-
-    return E_OK;
 }
 
-static t_Error BmiTxPortCheckAndGetCounterPtr(t_FmPort *p_FmPort, e_FmPortCounters counter, volatile uint32_t **p_Ptr)
+static bool CheckOhBmiCounter(t_FmPort *p_FmPort, e_FmPortCounters counter)
 {
-    t_FmPortTxBmiRegs   *p_BmiRegs = &p_FmPort->p_FmPortBmiRegs->txPortBmiRegs;
-
-     /* check that counters are enabled */
     switch (counter)
     {
-        case (e_FM_PORT_COUNTERS_CYCLE):
-        case (e_FM_PORT_COUNTERS_TASK_UTIL):
-        case (e_FM_PORT_COUNTERS_QUEUE_UTIL):
-        case (e_FM_PORT_COUNTERS_DMA_UTIL):
-        case (e_FM_PORT_COUNTERS_FIFO_UTIL):
-            /* performance counters - may be read when disabled */
-            break;
-        case (e_FM_PORT_COUNTERS_FRAME):
-        case (e_FM_PORT_COUNTERS_DISCARD_FRAME):
-        case (e_FM_PORT_COUNTERS_LENGTH_ERR):
-        case (e_FM_PORT_COUNTERS_UNSUPPRTED_FORMAT):
-        case (e_FM_PORT_COUNTERS_DEALLOC_BUF):
-            if (!(GET_UINT32(p_BmiRegs->fmbm_tstc) & BMI_COUNTERS_EN))
-               RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Requested counter was not enabled"));
-            break;
+        case(e_FM_PORT_COUNTERS_CYCLE):
+        case(e_FM_PORT_COUNTERS_TASK_UTIL):
+        case(e_FM_PORT_COUNTERS_DMA_UTIL):
+        case(e_FM_PORT_COUNTERS_FIFO_UTIL):
+        case(e_FM_PORT_COUNTERS_FRAME):
+        case(e_FM_PORT_COUNTERS_DISCARD_FRAME):
+        case(e_FM_PORT_COUNTERS_RX_LIST_DMA_ERR):
+        case(e_FM_PORT_COUNTERS_WRED_DISCARD):
+        case(e_FM_PORT_COUNTERS_LENGTH_ERR):
+        case(e_FM_PORT_COUNTERS_UNSUPPRTED_FORMAT):
+        case(e_FM_PORT_COUNTERS_DEALLOC_BUF):
+            return TRUE;
+        case(e_FM_PORT_COUNTERS_RX_FILTER_FRAME):
+            if (p_FmPort->portType == e_FM_PORT_TYPE_OH_HOST_COMMAND)
+                return FALSE;
+            else
+                return TRUE;
         default:
-            RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Requested counter is not available for Tx ports"));
+            return FALSE;
     }
+}
 
-    /* Set counter */
-    switch (counter)
+static t_Error BmiPortCheckAndGetCounterType(t_FmPort                      *p_FmPort,
+                                             e_FmPortCounters              counter,
+                                             enum fman_port_stats_counters *p_StatsType,
+                                             enum fman_port_perf_counters  *p_PerfType,
+                                             bool                          *p_IsStats)
+{
+    volatile uint32_t   *p_Reg;
+    bool                isValid;
+
+    switch (p_FmPort->portType)
     {
-        case (e_FM_PORT_COUNTERS_CYCLE):
-           *p_Ptr = &p_BmiRegs->fmbm_tccn;
-            break;
-        case (e_FM_PORT_COUNTERS_TASK_UTIL):
-           *p_Ptr = &p_BmiRegs->fmbm_ttuc;
-            break;
-        case (e_FM_PORT_COUNTERS_QUEUE_UTIL):
-            *p_Ptr = &p_BmiRegs->fmbm_ttcquc;
-            break;
-        case (e_FM_PORT_COUNTERS_DMA_UTIL):
-           *p_Ptr = &p_BmiRegs->fmbm_tduc;
+        case(e_FM_PORT_TYPE_RX_10G):
+        case(e_FM_PORT_TYPE_RX):
+            p_Reg = &p_FmPort->port.bmi_regs->rx.fmbm_rstc;
+            isValid = CheckRxBmiCounter(p_FmPort, counter);
             break;
-        case (e_FM_PORT_COUNTERS_FIFO_UTIL):
-           *p_Ptr = &p_BmiRegs->fmbm_tfuc;
+        case(e_FM_PORT_TYPE_TX_10G):
+        case(e_FM_PORT_TYPE_TX):
+            p_Reg = &p_FmPort->port.bmi_regs->tx.fmbm_tstc;
+            isValid = CheckTxBmiCounter(p_FmPort, counter);
             break;
-        case (e_FM_PORT_COUNTERS_FRAME):
-           *p_Ptr = &p_BmiRegs->fmbm_tfrc;
-            break;
-        case (e_FM_PORT_COUNTERS_DISCARD_FRAME):
-           *p_Ptr = &p_BmiRegs->fmbm_tfdc;
-            break;
-        case (e_FM_PORT_COUNTERS_LENGTH_ERR):
-           *p_Ptr = &p_BmiRegs->fmbm_tfledc;
-            break;
-        case (e_FM_PORT_COUNTERS_UNSUPPRTED_FORMAT):
-            *p_Ptr = &p_BmiRegs->fmbm_tfufdc;
-            break;
-        case (e_FM_PORT_COUNTERS_DEALLOC_BUF):
-            *p_Ptr = &p_BmiRegs->fmbm_tbdc;
+        case(e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
+        case(e_FM_PORT_TYPE_OH_HOST_COMMAND):
+            p_Reg = &p_FmPort->port.bmi_regs->oh.fmbm_ostc;
+            isValid = CheckOhBmiCounter(p_FmPort, counter);
             break;
         default:
-            RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Requested counter is not available for Tx ports"));
+            RETURN_ERROR(MINOR, E_INVALID_STATE, ("Unsupported port type"));
     }
 
-    return E_OK;
-}
+    if (!isValid)
+        RETURN_ERROR(MINOR, E_INVALID_STATE,
+                ("Requested counter is not available for this port type"));
 
-static t_Error BmiOhPortCheckAndGetCounterPtr(t_FmPort *p_FmPort, e_FmPortCounters counter, volatile uint32_t **p_Ptr)
-{
-    t_FmPortOhBmiRegs   *p_BmiRegs = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs;
-
-    /* check that counters are enabled */
+     /* check that counters are enabled */
     switch (counter)
     {
-        case (e_FM_PORT_COUNTERS_CYCLE):
-        case (e_FM_PORT_COUNTERS_TASK_UTIL):
-        case (e_FM_PORT_COUNTERS_DMA_UTIL):
-        case (e_FM_PORT_COUNTERS_FIFO_UTIL):
+        case(e_FM_PORT_COUNTERS_CYCLE):
+        case(e_FM_PORT_COUNTERS_TASK_UTIL):
+        case(e_FM_PORT_COUNTERS_QUEUE_UTIL):
+        case(e_FM_PORT_COUNTERS_DMA_UTIL):
+        case(e_FM_PORT_COUNTERS_FIFO_UTIL):
+        case(e_FM_PORT_COUNTERS_RX_PAUSE_ACTIVATION):
             /* performance counters - may be read when disabled */
             break;
-        case (e_FM_PORT_COUNTERS_FRAME):
-        case (e_FM_PORT_COUNTERS_DISCARD_FRAME):
-        case (e_FM_PORT_COUNTERS_RX_LIST_DMA_ERR):
-        case (e_FM_PORT_COUNTERS_RX_OUT_OF_BUFFERS_DISCARD):
-        case (e_FM_PORT_COUNTERS_WRED_DISCARD):
-        case (e_FM_PORT_COUNTERS_LENGTH_ERR):
-        case (e_FM_PORT_COUNTERS_UNSUPPRTED_FORMAT):
-        case (e_FM_PORT_COUNTERS_DEALLOC_BUF):
-            if (!(GET_UINT32(p_BmiRegs->fmbm_ostc) & BMI_COUNTERS_EN))
-               RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Requested counter was not enabled"));
+        case(e_FM_PORT_COUNTERS_FRAME):
+        case(e_FM_PORT_COUNTERS_DISCARD_FRAME):
+        case(e_FM_PORT_COUNTERS_DEALLOC_BUF):
+        case(e_FM_PORT_COUNTERS_RX_BAD_FRAME):
+        case(e_FM_PORT_COUNTERS_RX_LARGE_FRAME):
+        case(e_FM_PORT_COUNTERS_RX_FILTER_FRAME):
+        case(e_FM_PORT_COUNTERS_RX_LIST_DMA_ERR):
+        case(e_FM_PORT_COUNTERS_RX_OUT_OF_BUFFERS_DISCARD):
+        case(e_FM_PORT_COUNTERS_LENGTH_ERR):
+        case(e_FM_PORT_COUNTERS_UNSUPPRTED_FORMAT):
+        case(e_FM_PORT_COUNTERS_WRED_DISCARD):
+            if (!(GET_UINT32(*p_Reg) & BMI_COUNTERS_EN))
+               RETURN_ERROR(MINOR, E_INVALID_STATE, ("Requested counter was not enabled"));
             break;
-        case (e_FM_PORT_COUNTERS_RX_FILTER_FRAME): /* only valid for offline parsing */
-            /* only driver uses host command port, so ASSERT rather than  RETURN_ERROR */
-            ASSERT_COND(p_FmPort->portType != e_FM_PORT_TYPE_OH_HOST_COMMAND);
-            if (!(GET_UINT32(p_BmiRegs->fmbm_ostc) & BMI_COUNTERS_EN))
-               RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Requested counter was not enabled"));
+         default:
             break;
-        default:
-            RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Requested counter(%d) is not available for O/H ports", counter));
     }
 
     /* Set counter */
     switch (counter)
     {
-        case (e_FM_PORT_COUNTERS_CYCLE):
-           *p_Ptr = &p_BmiRegs->fmbm_occn;
+        case(e_FM_PORT_COUNTERS_CYCLE):
+            *p_PerfType = E_FMAN_PORT_PERF_CNT_CYCLE;
+            *p_IsStats = FALSE;
             break;
-        case (e_FM_PORT_COUNTERS_TASK_UTIL):
-           *p_Ptr = &p_BmiRegs->fmbm_otuc;
+        case(e_FM_PORT_COUNTERS_TASK_UTIL):
+            *p_PerfType = E_FMAN_PORT_PERF_CNT_TASK_UTIL;
             break;
-        case (e_FM_PORT_COUNTERS_DMA_UTIL):
-           *p_Ptr = &p_BmiRegs->fmbm_oduc;
+        case(e_FM_PORT_COUNTERS_QUEUE_UTIL):
+            *p_PerfType = E_FMAN_PORT_PERF_CNT_QUEUE_UTIL;
+            *p_IsStats = FALSE;
             break;
-        case (e_FM_PORT_COUNTERS_FIFO_UTIL):
-           *p_Ptr = &p_BmiRegs->fmbm_ofuc;
+        case(e_FM_PORT_COUNTERS_DMA_UTIL):
+            *p_PerfType = E_FMAN_PORT_PERF_CNT_DMA_UTIL;
+            *p_IsStats = FALSE;
             break;
-        case (e_FM_PORT_COUNTERS_FRAME):
-           *p_Ptr = &p_BmiRegs->fmbm_ofrc;
+        case(e_FM_PORT_COUNTERS_FIFO_UTIL):
+            *p_PerfType = E_FMAN_PORT_PERF_CNT_FIFO_UTIL;
+            *p_IsStats = FALSE;
             break;
-        case (e_FM_PORT_COUNTERS_DISCARD_FRAME):
-           *p_Ptr = &p_BmiRegs->fmbm_ofdc;
+        case(e_FM_PORT_COUNTERS_RX_PAUSE_ACTIVATION):
+            *p_PerfType = E_FMAN_PORT_PERF_CNT_RX_PAUSE;
+            *p_IsStats = FALSE;
             break;
-        case (e_FM_PORT_COUNTERS_RX_FILTER_FRAME):
-           *p_Ptr = &p_BmiRegs->fmbm_offc;
+        case(e_FM_PORT_COUNTERS_FRAME):
+            *p_StatsType = E_FMAN_PORT_STATS_CNT_FRAME;
+            *p_IsStats = TRUE;
             break;
-        case (e_FM_PORT_COUNTERS_RX_LIST_DMA_ERR):
-          *p_Ptr = &p_BmiRegs->fmbm_ofldec;
+        case(e_FM_PORT_COUNTERS_DISCARD_FRAME):
+            *p_StatsType = E_FMAN_PORT_STATS_CNT_DISCARD;
+            *p_IsStats = TRUE;
             break;
-        case (e_FM_PORT_COUNTERS_WRED_DISCARD):
-           *p_Ptr = &p_BmiRegs->fmbm_ofwdc;
+        case(e_FM_PORT_COUNTERS_DEALLOC_BUF):
+            *p_StatsType = E_FMAN_PORT_STATS_CNT_DEALLOC_BUF;
+            *p_IsStats = TRUE;
             break;
-        case (e_FM_PORT_COUNTERS_LENGTH_ERR):
-           *p_Ptr = &p_BmiRegs->fmbm_ofledc;
+        case(e_FM_PORT_COUNTERS_RX_BAD_FRAME):
+            *p_StatsType = E_FMAN_PORT_STATS_CNT_RX_BAD_FRAME;
+            *p_IsStats = TRUE;
             break;
-        case (e_FM_PORT_COUNTERS_UNSUPPRTED_FORMAT):
-            *p_Ptr = &p_BmiRegs->fmbm_ofufdc;
+        case(e_FM_PORT_COUNTERS_RX_LARGE_FRAME):
+            *p_StatsType = E_FMAN_PORT_STATS_CNT_RX_LARGE_FRAME;
+            *p_IsStats = TRUE;
             break;
-        case (e_FM_PORT_COUNTERS_DEALLOC_BUF):
-            *p_Ptr = &p_BmiRegs->fmbm_obdc;
+        case(e_FM_PORT_COUNTERS_RX_OUT_OF_BUFFERS_DISCARD):
+            *p_StatsType = E_FMAN_PORT_STATS_CNT_RX_OUT_OF_BUF;
+            *p_IsStats = TRUE;
             break;
-        case (e_FM_PORT_COUNTERS_RX_OUT_OF_BUFFERS_DISCARD):
-            *p_Ptr = &p_BmiRegs->fmbm_oodc;
+        case(e_FM_PORT_COUNTERS_RX_FILTER_FRAME):
+            *p_StatsType = E_FMAN_PORT_STATS_CNT_FILTERED_FRAME;
             break;
-        case (e_FM_PORT_COUNTERS_PREPARE_TO_ENQUEUE_COUNTER):
-            *p_Ptr = &p_BmiRegs->fmbm_opec;
+        case(e_FM_PORT_COUNTERS_RX_LIST_DMA_ERR):
+            *p_StatsType = E_FMAN_PORT_STATS_CNT_DMA_ERR;
+            *p_IsStats = TRUE;
             break;
+        case(e_FM_PORT_COUNTERS_WRED_DISCARD):
+            *p_StatsType = E_FMAN_PORT_STATS_CNT_WRED_DISCARD;
+            *p_IsStats = TRUE;
+        case(e_FM_PORT_COUNTERS_LENGTH_ERR):
+            *p_StatsType = E_FMAN_PORT_STATS_CNT_LEN_ERR;
+            *p_IsStats = TRUE;
+        case(e_FM_PORT_COUNTERS_UNSUPPRTED_FORMAT):
+            *p_StatsType = E_FMAN_PORT_STATS_CNT_UNSUPPORTED_FORMAT;
+            *p_IsStats = TRUE;
         default:
-            RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Requested counter is not available for O/H ports"));
+            break;
     }
 
     return E_OK;
 }
 
+
 static t_Error AdditionalPrsParams(t_FmPort *p_FmPort, t_FmPcdPrsAdditionalHdrParams *p_HdrParams, uint32_t *p_SoftSeqAttachReg)
 {
     uint8_t                     hdrNum, Ipv4HdrNum;
@@ -1495,18 +1054,18 @@ static t_Error SetPcd(t_FmPort *p_FmPort, t_FmPortPcdParams *p_PcdParams)
     {
         case (e_FM_PORT_TYPE_RX_10G):
         case (e_FM_PORT_TYPE_RX):
-            p_BmiNia = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rfne;
-            p_BmiPrsNia = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rfpne;
-            p_BmiPrsStartOffset = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rpso;
-            p_BmiInitPrsResult = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rprai[0];
-            p_BmiCcBase = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rccb;
+            p_BmiNia = &p_FmPort->port.bmi_regs->rx.fmbm_rfne;
+            p_BmiPrsNia = &p_FmPort->port.bmi_regs->rx.fmbm_rfpne;
+            p_BmiPrsStartOffset = &p_FmPort->port.bmi_regs->rx.fmbm_rpso;
+            p_BmiInitPrsResult = &p_FmPort->port.bmi_regs->rx.fmbm_rprai[0];
+            p_BmiCcBase = &p_FmPort->port.bmi_regs->rx.fmbm_rccb;
             break;
-        case (e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
-            p_BmiNia = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ofne;
-            p_BmiPrsNia = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ofpne;
-            p_BmiPrsStartOffset = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_opso;
-            p_BmiInitPrsResult = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_oprai[0];
-            p_BmiCcBase = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_occb;
+        case(e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
+            p_BmiNia = &p_FmPort->port.bmi_regs->oh.fmbm_ofne;
+            p_BmiPrsNia = &p_FmPort->port.bmi_regs->oh.fmbm_ofpne;
+            p_BmiPrsStartOffset = &p_FmPort->port.bmi_regs->oh.fmbm_opso;
+            p_BmiInitPrsResult = &p_FmPort->port.bmi_regs->oh.fmbm_oprai[0];
+            p_BmiCcBase = &p_FmPort->port.bmi_regs->oh.fmbm_occb;
             break;
         default:
             RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Invalid port type"));
@@ -1869,12 +1428,12 @@ static t_Error DeletePcd(t_FmPort *p_FmPort)
     {
         case (e_FM_PORT_TYPE_RX_10G):
         case (e_FM_PORT_TYPE_RX):
-            p_BmiNia = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rfne;
-            p_BmiPrsStartOffset = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rpso;
+            p_BmiNia = &p_FmPort->port.bmi_regs->rx.fmbm_rfne;
+            p_BmiPrsStartOffset = &p_FmPort->port.bmi_regs->rx.fmbm_rpso;
             break;
         case (e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
-            p_BmiNia = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ofne;
-            p_BmiPrsStartOffset = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_opso;
+            p_BmiNia = &p_FmPort->port.bmi_regs->oh.fmbm_ofne;
+            p_BmiPrsStartOffset = &p_FmPort->port.bmi_regs->oh.fmbm_opso;
             break;
         default:
             RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Invalid port type"));
@@ -1935,9 +1494,9 @@ static t_Error AttachPCD(t_FmPort *p_FmPort)
 
     /* get PCD registers pointers */
     if (p_FmPort->portType == e_FM_PORT_TYPE_OH_OFFLINE_PARSING)
-        p_BmiNia = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ofne;
+        p_BmiNia = &p_FmPort->port.bmi_regs->oh.fmbm_ofne;
     else
-        p_BmiNia = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rfne;
+        p_BmiNia = &p_FmPort->port.bmi_regs->rx.fmbm_rfne;
 
     /* check that current NIA is BMI to BMI */
     if ((GET_UINT32(*p_BmiNia) & ~BMI_RFNE_FDCS_MASK) != GET_NO_PCD_NIA_BMI_AC_ENQ_FRAME())
@@ -1951,9 +1510,9 @@ static t_Error AttachPCD(t_FmPort *p_FmPort)
     if (p_FmPort->requiredAction & UPDATE_NIA_CMNE)
     {
         if (p_FmPort->portType == e_FM_PORT_TYPE_OH_OFFLINE_PARSING)
-            WRITE_UINT32(p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ocmne, p_FmPort->savedBmiCmne);
+            WRITE_UINT32(p_FmPort->port.bmi_regs->oh.fmbm_ocmne, p_FmPort->savedBmiCmne);
         else
-            WRITE_UINT32(p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rcmne, p_FmPort->savedBmiCmne);
+            WRITE_UINT32(p_FmPort->port.bmi_regs->rx.fmbm_rcmne, p_FmPort->savedBmiCmne);
     }
 
     if (p_FmPort->requiredAction & UPDATE_NIA_PNEN)
@@ -1962,25 +1521,25 @@ static t_Error AttachPCD(t_FmPort *p_FmPort)
     if (p_FmPort->requiredAction & UPDATE_NIA_FENE)
     {
         if (p_FmPort->portType == e_FM_PORT_TYPE_OH_OFFLINE_PARSING)
-            WRITE_UINT32(p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ofene, p_FmPort->savedBmiFene);
+            WRITE_UINT32(p_FmPort->port.bmi_regs->oh.fmbm_ofene, p_FmPort->savedBmiFene);
         else
-            WRITE_UINT32(p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rfene, p_FmPort->savedBmiFene);
+            WRITE_UINT32(p_FmPort->port.bmi_regs->rx.fmbm_rfene, p_FmPort->savedBmiFene);
     }
 
     if (p_FmPort->requiredAction & UPDATE_NIA_FPNE)
     {
         if (p_FmPort->portType == e_FM_PORT_TYPE_OH_OFFLINE_PARSING)
-            WRITE_UINT32(p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ofpne, p_FmPort->savedBmiFpne);
+            WRITE_UINT32(p_FmPort->port.bmi_regs->oh.fmbm_ofpne, p_FmPort->savedBmiFpne);
         else
-            WRITE_UINT32(p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rfpne, p_FmPort->savedBmiFpne);
+            WRITE_UINT32(p_FmPort->port.bmi_regs->rx.fmbm_rfpne, p_FmPort->savedBmiFpne);
     }
 
     WRITE_UINT32(*p_BmiNia, p_FmPort->savedBmiNia);
 
     if (p_FmPort->requiredAction & UPDATE_NIA_PNDN)
     {
-        p_FmPort->origNonRxQmiRegsPndn = GET_UINT32(p_FmPort->p_FmPortQmiRegs->nonRxQmiRegs.fmqm_pndn);
-        WRITE_UINT32(p_FmPort->p_FmPortQmiRegs->nonRxQmiRegs.fmqm_pndn, p_FmPort->savedNonRxQmiRegsPndn);
+        p_FmPort->origNonRxQmiRegsPndn = GET_UINT32(p_FmPort->port.qmi_regs->fmqm_pndn);
+        WRITE_UINT32(p_FmPort->port.qmi_regs->fmqm_pndn, p_FmPort->savedNonRxQmiRegsPndn);
     }
 
     return E_OK;
@@ -1994,12 +1553,12 @@ static t_Error DetachPCD(t_FmPort *p_FmPort)
 
     /* get PCD registers pointers */
     if (p_FmPort->requiredAction & UPDATE_NIA_PNDN)
-        WRITE_UINT32(p_FmPort->p_FmPortQmiRegs->nonRxQmiRegs.fmqm_pndn, p_FmPort->origNonRxQmiRegsPndn);
+        WRITE_UINT32(p_FmPort->port.qmi_regs->fmqm_pndn, p_FmPort->origNonRxQmiRegsPndn);
 
     if (p_FmPort->portType == e_FM_PORT_TYPE_OH_OFFLINE_PARSING)
-        p_BmiNia = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ofne;
+        p_BmiNia = &p_FmPort->port.bmi_regs->oh.fmbm_ofne;
     else
-        p_BmiNia = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rfne;
+        p_BmiNia = &p_FmPort->port.bmi_regs->rx.fmbm_rfne;
 
     WRITE_UINT32(*p_BmiNia, (p_FmPort->savedBmiNia & BMI_RFNE_FDCS_MASK) |
                             GET_NO_PCD_NIA_BMI_AC_ENQ_FRAME());
@@ -2010,13 +1569,13 @@ static t_Error DetachPCD(t_FmPort *p_FmPort)
     if (p_FmPort->requiredAction & UPDATE_NIA_FENE)
     {
         if (p_FmPort->portType == e_FM_PORT_TYPE_OH_OFFLINE_PARSING)
-            WRITE_UINT32(p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ofene, NIA_ENG_QMI_ENQ | NIA_ORDER_RESTOR);
+            WRITE_UINT32(p_FmPort->port.bmi_regs->oh.fmbm_ofene, NIA_ENG_QMI_ENQ | NIA_ORDER_RESTOR);
         else
-            WRITE_UINT32(p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rfene, NIA_ENG_QMI_ENQ | NIA_ORDER_RESTOR);
+            WRITE_UINT32(p_FmPort->port.bmi_regs->rx.fmbm_rfene, NIA_ENG_QMI_ENQ | NIA_ORDER_RESTOR);
     }
 
     if (p_FmPort->requiredAction & UPDATE_NIA_PNEN)
-        WRITE_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnen, NIA_ENG_BMI | NIA_BMI_AC_RELEASE);
+        WRITE_UINT32(p_FmPort->port.qmi_regs->fmqm_pnen, NIA_ENG_BMI | NIA_BMI_AC_RELEASE);
 
     if (p_FmPort->requiredAction & UPDATE_FMFP_PRC_WITH_ONE_RISC_ONLY)
         if (FmSetNumOfRiscsPerPort(p_FmPort->h_Fm, p_FmPort->hardwarePortId, 2, p_FmPort->orFmanCtrl)!= E_OK)
@@ -2049,7 +1608,7 @@ void FmPortSetMacsecLcv(t_Handle h_FmPort)
         return;
     }
 
-    p_BmiCfgReg = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rcfg;
+    p_BmiCfgReg = &p_FmPort->port.bmi_regs->rx.fmbm_rcfg;
     /* get LCV for MACSEC */
     if ((p_FmPort->h_FmPcd) && ((lcv = FmPcdGetMacsecLcv(p_FmPort->h_FmPcd, p_FmPort->netEnvId))!= 0))
     {
@@ -2080,7 +1639,7 @@ void FmPortSetMacsecCmd(t_Handle h_FmPort, uint8_t dfltSci)
         return;
     }
 
-    p_BmiCfgReg = &p_FmPort->p_FmPortBmiRegs->txPortBmiRegs.fmbm_tfca;
+    p_BmiCfgReg = &p_FmPort->port.bmi_regs->tx.fmbm_tfca;
     tmpReg = GET_UINT32(*p_BmiCfgReg) & ~BMI_CMD_ATTR_MACCMD_MASK;
     tmpReg |= BMI_CMD_ATTR_MACCMD_SECURED;
     tmpReg |= (((uint32_t)dfltSci << BMI_CMD_ATTR_MACCMD_SC_SHIFT) & BMI_CMD_ATTR_MACCMD_SC_MASK);
@@ -2204,6 +1763,16 @@ t_Error FmPortGetSetCcParams(t_Handle h_FmPort, t_FmPortGetSetCcParams *p_CcPara
         p_CcParams->getCcParams.revInfo.minorRev = p_FmPort->fmRevInfo.minorRev;
         p_CcParams->getCcParams.type &= ~FM_REV;
     }
+    if (p_CcParams->getCcParams.type & DISCARD_MASK)
+    {
+        if (p_FmPort->portType == e_FM_PORT_TYPE_OH_OFFLINE_PARSING)
+            p_CcParams->getCcParams.discardMask =
+                GET_UINT32(p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ofsdm);
+        else
+            p_CcParams->getCcParams.discardMask =
+                GET_UINT32(p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rfsdm);
+        p_CcParams->getCcParams.type &= ~DISCARD_MASK;
+    }
     if (p_CcParams->getCcParams.type & MANIP_EXTRA_SPACE)
     {
         p_CcParams->getCcParams.internalBufferOffset = p_FmPort->internalBufferOffset;
@@ -2212,9 +1781,9 @@ t_Error FmPortGetSetCcParams(t_Handle h_FmPort, t_FmPortGetSetCcParams *p_CcPara
     if (p_CcParams->getCcParams.type & GET_NIA_FPNE)
     {
         if (p_FmPort->portType == e_FM_PORT_TYPE_OH_OFFLINE_PARSING)
-            p_CcParams->getCcParams.nia = GET_UINT32(p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ofpne);
+            p_CcParams->getCcParams.nia = GET_UINT32(p_FmPort->port.bmi_regs->oh.fmbm_ofpne);
         else
-            p_CcParams->getCcParams.nia = GET_UINT32(p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rfpne);
+            p_CcParams->getCcParams.nia = GET_UINT32(p_FmPort->port.bmi_regs->rx.fmbm_rfpne);
         p_CcParams->getCcParams.type &= ~GET_NIA_FPNE;
     }
     if (p_CcParams->getCcParams.type & GET_NIA_PNDN)
@@ -2297,18 +1866,19 @@ t_Error FmPortGetSetCcParams(t_Handle h_FmPort, t_FmPortGetSetCcParams *p_CcPara
         !(p_FmPort->requiredAction & UPDATE_PSO))
     {
         /* get PCD registers pointers */
-         switch (p_FmPort->portType)
-         {
-             case (e_FM_PORT_TYPE_RX_10G):
-             case (e_FM_PORT_TYPE_RX):
-                 p_BmiPrsStartOffset = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rpso;
-                 break;
-             case (e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
-                 p_BmiPrsStartOffset = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_opso;
-                 break;
-             default:
-                 RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Invalid port type"));
-         }
+        switch (p_FmPort->portType)
+        {
+            case (e_FM_PORT_TYPE_RX_10G):
+            case (e_FM_PORT_TYPE_RX):
+                p_BmiPrsStartOffset = &p_FmPort->port.bmi_regs->rx.fmbm_rpso;
+                break;
+            case (e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
+                p_BmiPrsStartOffset = &p_FmPort->port.bmi_regs->oh.fmbm_opso;
+                break;
+            default:
+                RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Invalid port type"));
+        }
+
         /* set start parsing offset */
         tmpInt = (int)GET_UINT32(*p_BmiPrsStartOffset)+ p_CcParams->setCcParams.psoSize;
         if (tmpInt>0)
@@ -2316,7 +1886,6 @@ t_Error FmPortGetSetCcParams(t_Handle h_FmPort, t_FmPortGetSetCcParams *p_CcPara
 
         p_FmPort->requiredAction |= UPDATE_PSO;
         p_FmPort->savedPrsStartOffset = p_CcParams->setCcParams.psoSize;
-
     }
     else if (p_CcParams->setCcParams.type & UPDATE_PSO)
     {
@@ -2385,7 +1954,45 @@ t_Handle FM_PORT_Config(t_FmPortParams *p_FmPortParams)
             DBG(WARNING, ("Use non-zero portId for OP port due to insufficient resources on portId 0."));
     }
 
-    /* Initialize FM port parameters for initialization phase only */
+    /* Set up FM port parameters for initialization phase only */
+
+    /* First, fill in flibs struct */
+    fman_port_defconfig(&p_FmPort->p_FmPortDriverParam->dfltCfg, (enum fman_port_type)p_FmPort->portType);
+    /* Overwrite some integration specific parameters */
+    p_FmPort->p_FmPortDriverParam->dfltCfg.rx_pri_elevation = DEFAULT_PORT_rxFifoPriElevationLevel;
+    p_FmPort->p_FmPortDriverParam->dfltCfg.rx_fifo_thr = DEFAULT_PORT_rxFifoThreshold;
+
+#if defined(FM_OP_NO_VSP_NO_RELEASE_ERRATA_FMAN_A006675) || defined(FM_ERROR_VSP_NO_MATCH_SW006)
+    p_FmPort->p_FmPortDriverParam->dfltCfg.errata_A006675 = TRUE;
+#else
+    p_FmPort->p_FmPortDriverParam->dfltCfg.errata_A006675 = FALSE;
+#endif
+    if ((p_FmPort->fmRevInfo.majorRev == 6) && (p_FmPort->fmRevInfo.minorRev == 0))
+        p_FmPort->p_FmPortDriverParam->dfltCfg.errata_A006320 = TRUE;
+    else
+        p_FmPort->p_FmPortDriverParam->dfltCfg.errata_A006320 = FALSE;
+
+    /* Excessive Threshold register - exists for pre-FMv3 chips only */
+    if (p_FmPort->fmRevInfo.majorRev < 6)
+    {
+#ifdef FM_NO_RESTRICT_ON_ACCESS_RSRC
+        p_FmPort->p_FmPortDriverParam->dfltCfg.excessive_threshold_register = TRUE;
+#endif
+        p_FmPort->p_FmPortDriverParam->dfltCfg.fmbm_rebm_has_sgd = FALSE;
+        p_FmPort->p_FmPortDriverParam->dfltCfg.fmbm_tfne_has_features = FALSE;
+    }
+    else
+    {
+        p_FmPort->p_FmPortDriverParam->dfltCfg.excessive_threshold_register = FALSE;
+        p_FmPort->p_FmPortDriverParam->dfltCfg.fmbm_rebm_has_sgd = TRUE;
+        p_FmPort->p_FmPortDriverParam->dfltCfg.fmbm_tfne_has_features = TRUE;
+    }
+    if (p_FmPort->fmRevInfo.majorRev == 4)
+        p_FmPort->p_FmPortDriverParam->dfltCfg.qmi_deq_options_support = FALSE;
+    else
+        p_FmPort->p_FmPortDriverParam->dfltCfg.qmi_deq_options_support = TRUE;
+
+    /* Continue with other parameters */
     p_FmPort->p_FmPortDriverParam->baseAddr                         = baseAddr;
     /* set memory map pointers */
     p_FmPort->p_FmPortQmiRegs     = (t_FmPortQmiRegs *)UINT_TO_PTR(baseAddr + QMI_PORT_REGS_OFFSET);
@@ -2398,14 +2005,13 @@ t_Handle FM_PORT_Config(t_FmPortParams *p_FmPortParams)
     p_FmPort->p_FmPortDriverParam->bufferPrefixContent.passAllOtherPCDInfo
                                                                     = DEFAULT_PORT_bufferPrefixContent_passTimeStamp;
     p_FmPort->p_FmPortDriverParam->bufferPrefixContent.dataAlign    = DEFAULT_PORT_bufferPrefixContent_dataAlign;
-    p_FmPort->p_FmPortDriverParam->dmaSwapData                      = (e_FmDmaSwapOption)DEFAULT_PORT_dmaSwapData;
+/*    p_FmPort->p_FmPortDriverParam->dmaSwapData                      = (e_FmDmaSwapOption)DEFAULT_PORT_dmaSwapData;
     p_FmPort->p_FmPortDriverParam->dmaIntContextCacheAttr           = (e_FmDmaCacheOption)DEFAULT_PORT_dmaIntContextCacheAttr;
     p_FmPort->p_FmPortDriverParam->dmaHeaderCacheAttr               = (e_FmDmaCacheOption)DEFAULT_PORT_dmaHeaderCacheAttr;
     p_FmPort->p_FmPortDriverParam->dmaScatterGatherCacheAttr        = (e_FmDmaCacheOption)DEFAULT_PORT_dmaScatterGatherCacheAttr;
     p_FmPort->p_FmPortDriverParam->dmaWriteOptimize                 = DEFAULT_PORT_dmaWriteOptimize;
-    p_FmPort->p_FmPortDriverParam->liodnBase                        = p_FmPortParams->liodnBase;
+*/    p_FmPort->p_FmPortDriverParam->liodnBase                        = p_FmPortParams->liodnBase;
     p_FmPort->p_FmPortDriverParam->cheksumLastBytesIgnore           = DEFAULT_PORT_cheksumLastBytesIgnore;
-    p_FmPort->p_FmPortDriverParam->color                            = DEFAULT_PORT_color;
 
     p_FmPort->maxFrameLength                                        = DEFAULT_PORT_maxFrameLength;
     /* resource distribution. */
@@ -2455,16 +2061,7 @@ t_Handle FM_PORT_Config(t_FmPortParams *p_FmPortParams)
         }
 #endif /* FM_HEAVY_TRAFFIC_SEQUENCER_HANG_ERRATA_FMAN_A006981 */
 
-    if (p_FmPort->portType == e_FM_PORT_TYPE_OH_HOST_COMMAND)
-        p_FmPort->p_FmPortDriverParam->syncReq                      = DEFAULT_PORT_syncReqForHc;
-    else
-        p_FmPort->p_FmPortDriverParam->syncReq                      = DEFAULT_PORT_syncReq;
-
     /* Port type specific initialization: */
-    if ((p_FmPort->portType != e_FM_PORT_TYPE_TX) &&
-        (p_FmPort->portType != e_FM_PORT_TYPE_TX_10G))
-        p_FmPort->p_FmPortDriverParam->frmDiscardOverride           = DEFAULT_PORT_frmDiscardOverride;
-
     switch (p_FmPort->portType)
     {
     case (e_FM_PORT_TYPE_RX):
@@ -2506,7 +2103,7 @@ t_Handle FM_PORT_Config(t_FmPortParams *p_FmPortParams)
         if (1) /* if (p_FmPort->fmRevInfo.majorRev < 6) */
         {
             p_FmPort->p_FmPortDriverParam->txFifoMinFillLevel       = DEFAULT_PORT_txFifoMinFillLevel;
-            p_FmPort->fifoDeqPipelineDepth                          =
+            p_FmPort->p_FmPortDriverParam->dfltCfg.tx_fifo_deq_pipeline_depth                          =
                 (uint8_t)((p_FmPort->portType == e_FM_PORT_TYPE_TX) ?
                           DEFAULT_PORT_fifoDeqPipelineDepth_1G :
                           DEFAULT_PORT_fifoDeqPipelineDepth_10G);
@@ -2518,7 +2115,7 @@ t_Handle FM_PORT_Config(t_FmPortParams *p_FmPortParams)
             tmpReg = GET_UINT32(p_FmPort->p_FmPortBmiRegs->txPortBmiRegs.fmbm_tfp);
             p_FmPort->p_FmPortDriverParam->txFifoMinFillLevel =
                 ((tmpReg & BMI_TX_FIFO_MIN_FILL_MASK) >> BMI_TX_FIFO_MIN_FILL_SHIFT) * BMI_FIFO_UNITS ;
-            p_FmPort->fifoDeqPipelineDepth =
+            p_FmPort->p_FmPortDriverParam->dfltCfg.tx_fifo_deq_pipeline_depth =
                 (uint8_t)(((tmpReg & BMI_FIFO_PIPELINE_DEPTH_MASK) >> BMI_FIFO_PIPELINE_DEPTH_SHIFT) + 1);
             p_FmPort->p_FmPortDriverParam->txFifoLowComfLevel =
                 (((tmpReg & BMI_TX_LOW_COMF_MASK) >> BMI_TX_LOW_COMF_SHIFT) + 1) * BMI_FIFO_UNITS;
@@ -2548,18 +2145,18 @@ t_Handle FM_PORT_Config(t_FmPortParams *p_FmPortParams)
 
 #ifdef FM_NO_GUARANTEED_RESET_VALUES
     if (1) /* if (p_FmPort->fmRevInfo.majorRev < 6) */
-        p_FmPort->fifoDeqPipelineDepth                              = DEFAULT_PORT_fifoDeqPipelineDepth_OH;
+        p_FmPort->p_FmPortDriverParam->dfltCfg.tx_fifo_deq_pipeline_depth                              = DEFAULT_PORT_fifoDeqPipelineDepth_OH;
     else
 #endif /* FM_NO_GUARANTEED_RESET_VALUES */
     {
         tmpReg = GET_UINT32(p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ofp);
-        p_FmPort->fifoDeqPipelineDepth =
+        p_FmPort->p_FmPortDriverParam->dfltCfg.tx_fifo_deq_pipeline_depth =
             (uint8_t)(((tmpReg & BMI_FIFO_PIPELINE_DEPTH_MASK) >> BMI_FIFO_PIPELINE_DEPTH_SHIFT) + 1);
         if ((p_FmPort->portType == e_FM_PORT_TYPE_OH_HOST_COMMAND) &&
             (p_FmPortParams->portId != FM_OH_PORT_ID))
         {
             /* Overwrite HC defaults */
-            p_FmPort->fifoDeqPipelineDepth = DEFAULT_PORT_fifoDeqPipelineDepth_OH;
+            p_FmPort->p_FmPortDriverParam->dfltCfg.tx_fifo_deq_pipeline_depth = DEFAULT_PORT_fifoDeqPipelineDepth_OH;
         }
     }
 
@@ -2571,7 +2168,7 @@ t_Handle FM_PORT_Config(t_FmPortParams *p_FmPortParams)
 #ifndef FM_DEQ_PIPELINE_PARAMS_FOR_OP
     if (!((p_FmPort->fmRevInfo.majorRev == 4) ||
           (p_FmPort->fmRevInfo.majorRev >= 6)))
-            p_FmPort->fifoDeqPipelineDepth                          = DEFAULT_notSupported;
+            p_FmPort->p_FmPortDriverParam->dfltCfg.tx_fifo_deq_pipeline_depth                          = DEFAULT_notSupported;
 #endif /* !FM_DEQ_PIPELINE_PARAMS_FOR_OP */
         break;
 
@@ -2592,7 +2189,7 @@ t_Handle FM_PORT_Config(t_FmPortParams *p_FmPortParams)
     {
         if ((p_FmPort->portType == e_FM_PORT_TYPE_TX) ||
             (p_FmPort->portType == e_FM_PORT_TYPE_TX_10G))
-            p_FmPort->fifoDeqPipelineDepth = DEFAULT_PORT_fifoDeqPipelineDepth_IM;
+            p_FmPort->p_FmPortDriverParam->dfltCfg.tx_fifo_deq_pipeline_depth = DEFAULT_PORT_fifoDeqPipelineDepth_IM;
         FmPortConfigIM(p_FmPort, p_FmPortParams);
     }
     else
@@ -2667,20 +2264,21 @@ t_Handle FM_PORT_Config(t_FmPortParams *p_FmPortParams)
 t_Error FM_PORT_Init(t_Handle h_FmPort)
 {
     t_FmPort                        *p_FmPort = (t_FmPort*)h_FmPort;
-    t_FmPortDriverParam             *p_Params;
-    t_Error                         err = E_OK;
+    t_FmPortDriverParam             *p_DriverParams;
+    t_Error                         errCode;
     t_FmInterModulePortInitParams   fmParams;
+    t_FmRevisionInfo                revInfo;
 
     SANITY_CHECK_RETURN_ERROR(h_FmPort, E_INVALID_HANDLE);
     SANITY_CHECK_RETURN_ERROR(p_FmPort->p_FmPortDriverParam, E_INVALID_HANDLE);
 
-    err = FmSpBuildBufferStructure(&p_FmPort->p_FmPortDriverParam->intContext,
-                                   &p_FmPort->p_FmPortDriverParam->bufferPrefixContent,
-                                   &p_FmPort->p_FmPortDriverParam->bufMargins,
-                                   &p_FmPort->bufferOffsets,
-                                   &p_FmPort->internalBufferOffset);
-    if (err != E_OK)
-        RETURN_ERROR(MAJOR, err, NO_MSG);
+    errCode = FmSpBuildBufferStructure(&p_FmPort->p_FmPortDriverParam->intContext,
+                                       &p_FmPort->p_FmPortDriverParam->bufferPrefixContent,
+                                       &p_FmPort->p_FmPortDriverParam->bufMargins,
+                                       &p_FmPort->bufferOffsets,
+                                       &p_FmPort->internalBufferOffset);
+    if (errCode != E_OK)
+        RETURN_ERROR(MAJOR, errCode, NO_MSG);
 #ifdef FM_HEAVY_TRAFFIC_HANG_ERRATA_FMAN_A005669
     if ((p_FmPort->p_FmPortDriverParam->bcbWorkaround) &&
            (p_FmPort->portType == e_FM_PORT_TYPE_RX))
@@ -2694,19 +2292,58 @@ t_Error FM_PORT_Init(t_Handle h_FmPort)
 
     CHECK_INIT_PARAMETERS(p_FmPort, CheckInitParameters);
 
-    p_Params = p_FmPort->p_FmPortDriverParam;
-
-    if  ((p_FmPort->portType == e_FM_PORT_TYPE_RX_10G) ||
+    p_DriverParams = p_FmPort->p_FmPortDriverParam;
+
+    /* Set up flibs port structure */
+    memset(&p_FmPort->port, 0, sizeof(struct fman_port));
+    p_FmPort->port.type = (enum fman_port_type)p_FmPort->portType;
+    FM_GetRevision(p_FmPort->h_Fm, &revInfo);
+    p_FmPort->port.fm_rev_maj = revInfo.majorRev;
+    p_FmPort->port.fm_rev_min = revInfo.minorRev;
+    p_FmPort->port.bmi_regs = (union fman_port_bmi_regs *)
+            UINT_TO_PTR(p_DriverParams->baseAddr + BMI_PORT_REGS_OFFSET);
+    p_FmPort->port.qmi_regs = (struct fman_port_qmi_regs *)
+            UINT_TO_PTR(p_DriverParams->baseAddr + QMI_PORT_REGS_OFFSET);
+    p_FmPort->port.ext_pools_num = (uint8_t)((revInfo.majorRev == 4) ? 4 : 8);
+    p_FmPort->port.im_en = p_FmPort->imEn;
+    p_FmPort->p_FmPortPrsRegs     = (t_FmPortPrsRegs *)UINT_TO_PTR(p_DriverParams->baseAddr + PRS_PORT_REGS_OFFSET);
+
+    if  (((p_FmPort->portType == e_FM_PORT_TYPE_RX_10G) ||
          (p_FmPort->portType == e_FM_PORT_TYPE_RX))
-        if (!p_FmPort->imEn)
-        {
+        && !p_FmPort->imEn)
+    {
             /* Call the external Buffer routine which also checks fifo
              size and updates it if necessary */
             /* define external buffer pools and pool depletion*/
-            err = SetExtBufferPools(p_FmPort);
-            if (err)
-                RETURN_ERROR(MAJOR, err, NO_MSG);
+            errCode = SetExtBufferPools(p_FmPort);
+            if (errCode)
+                RETURN_ERROR(MAJOR, errCode, NO_MSG);
+            /* check if the largest external buffer pool is large enough */
+            if (p_DriverParams->bufMargins.startMargins + MIN_EXT_BUF_SIZE + p_DriverParams->bufMargins.endMargins >
+                p_FmPort->rxPoolsParams.largestBufSize)
+                    RETURN_ERROR(MAJOR, E_INVALID_VALUE,
+                             ("bufMargins.startMargins (%d) + minimum buf size (64) + bufMargins.endMargins (%d) is larger than maximum external buffer size (%d)",
+                              p_DriverParams->bufMargins.startMargins,
+                              p_DriverParams->bufMargins.endMargins,
+                              p_FmPort->rxPoolsParams.largestBufSize));
+    }
+    if (p_FmPort->portType == e_FM_PORT_TYPE_OH_OFFLINE_PARSING)
+    {
+        {
+#ifdef FM_NO_OP_OBSERVED_POOLS
+            t_FmRevisionInfo        revInfo;
+
+            FM_GetRevision(p_FmPort->h_Fm, &revInfo);
+            if ((revInfo.majorRev == 4) && (p_DriverParams->enBufPoolDepletion))
+#endif /* FM_NO_OP_OBSERVED_POOLS */
+            {
+                /* define external buffer pools */
+                errCode = SetExtBufferPools(p_FmPort);
+                if (errCode)
+                    RETURN_ERROR(MAJOR, errCode, NO_MSG);
+            }
         }
+    }
 
     /************************************************************/
     /* Call FM module routine for communicating parameters      */
@@ -2718,18 +2355,19 @@ t_Error FM_PORT_Init(t_Handle h_FmPort)
     fmParams.numOfExtraTasks    = (uint8_t)p_FmPort->tasks.extra;
     fmParams.numOfOpenDmas      = (uint8_t)p_FmPort->openDmas.num;
     fmParams.numOfExtraOpenDmas = (uint8_t)p_FmPort->openDmas.extra;
+
     if (p_FmPort->fifoBufs.num)
     {
-        err = VerifySizeOfFifo(p_FmPort);
-        if (err)
-            RETURN_ERROR(MAJOR, err, NO_MSG);
+        errCode = VerifySizeOfFifo(p_FmPort);
+        if (errCode != E_OK)
+            RETURN_ERROR(MAJOR, errCode, NO_MSG);
     }
     fmParams.sizeOfFifo         = p_FmPort->fifoBufs.num;
     fmParams.extraSizeOfFifo    = p_FmPort->fifoBufs.extra;
     fmParams.independentMode    = p_FmPort->imEn;
-    fmParams.liodnOffset        = p_Params->liodnOffset;
-    fmParams.liodnBase          = p_Params->liodnBase;
-    fmParams.deqPipelineDepth   = p_FmPort->fifoDeqPipelineDepth;
+    fmParams.liodnOffset        = p_DriverParams->liodnOffset;
+    fmParams.liodnBase          = p_DriverParams->liodnBase;
+    fmParams.deqPipelineDepth   = p_FmPort->p_FmPortDriverParam->dfltCfg.tx_fifo_deq_pipeline_depth;
     fmParams.maxFrameLength     = p_FmPort->maxFrameLength;
 #ifndef FM_DEQ_PIPELINE_PARAMS_FOR_OP
     if ((p_FmPort->portType == e_FM_PORT_TYPE_OH_OFFLINE_PARSING) ||
@@ -2745,9 +2383,9 @@ t_Error FM_PORT_Init(t_Handle h_FmPort)
 #endif /* !FM_DEQ_PIPELINE_PARAMS_FOR_OP */
 
 
-    err = FmGetSetPortParams(p_FmPort->h_Fm, &fmParams);
-    if (err)
-        RETURN_ERROR(MAJOR, err, NO_MSG);
+    errCode = FmGetSetPortParams(p_FmPort->h_Fm, &fmParams);
+    if (errCode)
+        RETURN_ERROR(MAJOR, errCode, NO_MSG);
 
     /* get params for use in init */
     p_FmPort->fmMuramPhysBaseAddr =
@@ -2767,41 +2405,9 @@ t_Error FM_PORT_Init(t_Handle h_FmPort)
     }
 #endif /* FM_NO_GUARANTEED_RESET_VALUES */
 
-    /**********************/
-    /* Init BMI Registers */
-    /**********************/
-    switch (p_FmPort->portType)
-    {
-        case (e_FM_PORT_TYPE_RX_10G):
-        case (e_FM_PORT_TYPE_RX):
-            err = BmiRxPortInit(p_FmPort);
-            if (err)
-                RETURN_ERROR(MAJOR, err, NO_MSG);
-            break;
-        case (e_FM_PORT_TYPE_TX_10G):
-        case (e_FM_PORT_TYPE_TX):
-            err = BmiTxPortInit(p_FmPort);
-            if (err)
-                RETURN_ERROR(MAJOR, err, NO_MSG);
-            break;
-        case (e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
-        case (e_FM_PORT_TYPE_OH_HOST_COMMAND):
-            err = BmiOhPortInit(p_FmPort);
-            if (err)
-                RETURN_ERROR(MAJOR, err, NO_MSG);
-            break;
-        default:
-            RETURN_ERROR(MAJOR, E_INVALID_STATE, NO_MSG);
-    }
-
-    /**********************/
-    /* Init QMI Registers */
-    /**********************/
-    if (!p_FmPort->imEn && ((err = QmiInit(p_FmPort)) != E_OK))
-        RETURN_ERROR(MAJOR, err, NO_MSG);
-
-    if (p_FmPort->imEn && ((err = FmPortImInit(p_FmPort)) != E_OK))
-        RETURN_ERROR(MAJOR, err, NO_MSG);
+    errCode = InitLowLevelDriver(p_FmPort);
+    if (errCode != E_OK)
+        RETURN_ERROR(MAJOR, errCode, NO_MSG);
 
     FmPortDriverParamFree(p_FmPort);
 
@@ -2875,7 +2481,7 @@ t_Error FM_PORT_Free(t_Handle h_FmPort)
 
     fmParams.hardwarePortId = p_FmPort->hardwarePortId;
     fmParams.portType = (e_FmPortType)p_FmPort->portType;
-    fmParams.deqPipelineDepth = p_FmPort->fifoDeqPipelineDepth;
+    fmParams.deqPipelineDepth = p_FmPort->p_FmPortDriverParam->dfltCfg.tx_fifo_deq_pipeline_depth;
 
     FmFreePortParams(p_FmPort->h_Fm, &fmParams);
 
@@ -2949,7 +2555,7 @@ t_Error FM_PORT_ConfigDeqHighPriority(t_Handle h_FmPort, bool highPri)
     if ((p_FmPort->portType == e_FM_PORT_TYPE_RX_10G) || (p_FmPort->portType == e_FM_PORT_TYPE_RX))
         RETURN_ERROR(MAJOR, E_NO_MEMORY, ("not available for Rx ports"));
 
-    p_FmPort->p_FmPortDriverParam->deqHighPriority = highPri;
+    p_FmPort->p_FmPortDriverParam->dfltCfg.deq_high_pri = highPri;
 
     return E_OK;
 }
@@ -2963,7 +2569,7 @@ t_Error FM_PORT_ConfigDeqType(t_Handle h_FmPort, e_FmPortDeqType deqType)
     if ((p_FmPort->portType == e_FM_PORT_TYPE_RX_10G) || (p_FmPort->portType == e_FM_PORT_TYPE_RX))
         RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("not available for Rx ports"));
 
-    p_FmPort->p_FmPortDriverParam->deqType = deqType;
+    p_FmPort->p_FmPortDriverParam->dfltCfg.deq_type = (enum fman_port_deq_type)deqType;
 
     return E_OK;
 }
@@ -2976,7 +2582,8 @@ t_Error FM_PORT_ConfigDeqPrefetchOption(t_Handle h_FmPort, e_FmPortDeqPrefetchOp
     SANITY_CHECK_RETURN_ERROR(p_FmPort->p_FmPortDriverParam, E_INVALID_HANDLE);
     if ((p_FmPort->portType == e_FM_PORT_TYPE_RX_10G) || (p_FmPort->portType == e_FM_PORT_TYPE_RX))
         RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("not available for Rx ports"));
-    p_FmPort->p_FmPortDriverParam->deqPrefetchOption = deqPrefetchOption;
+    p_FmPort->p_FmPortDriverParam->dfltCfg.deq_prefetch_opt = (enum fman_port_deq_prefetch)deqPrefetchOption;
+
     return E_OK;
 }
 
@@ -3006,7 +2613,7 @@ t_Error FM_PORT_ConfigDeqByteCnt(t_Handle h_FmPort, uint16_t deqByteCnt)
     if ((p_FmPort->portType == e_FM_PORT_TYPE_RX_10G) || (p_FmPort->portType == e_FM_PORT_TYPE_RX))
         RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("not available for Rx ports"));
 
-    p_FmPort->p_FmPortDriverParam->deqByteCnt = deqByteCnt;
+    p_FmPort->p_FmPortDriverParam->dfltCfg.deq_byte_cnt = deqByteCnt;
 
     return E_OK;
 }
@@ -3019,21 +2626,21 @@ t_Error FM_PORT_ConfigBufferPrefixContent(t_Handle h_FmPort, t_FmBufferPrefixCon
     SANITY_CHECK_RETURN_ERROR(p_FmPort->p_FmPortDriverParam, E_INVALID_HANDLE);
 
     memcpy(&p_FmPort->p_FmPortDriverParam->bufferPrefixContent, p_FmBufferPrefixContent, sizeof(t_FmBufferPrefixContent));
-    /* if dataAlign was not initialized by user, we return to driver's deafult */
+    /* if dataAlign was not initialized by user, we return to driver's default */
     if (!p_FmPort->p_FmPortDriverParam->bufferPrefixContent.dataAlign)
         p_FmPort->p_FmPortDriverParam->bufferPrefixContent.dataAlign = DEFAULT_PORT_bufferPrefixContent_dataAlign;
 
     return E_OK;
 }
 
-t_Error FM_PORT_ConfigCheksumLastBytesIgnore(t_Handle h_FmPort, uint8_t cheksumLastBytesIgnore)
+t_Error FM_PORT_ConfigCheksumLastBytesIgnore(t_Handle h_FmPort, uint8_t checksumLastBytesIgnore)
 {
     t_FmPort *p_FmPort = (t_FmPort*)h_FmPort;
 
     SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
     SANITY_CHECK_RETURN_ERROR(p_FmPort->p_FmPortDriverParam, E_INVALID_HANDLE);
 
-    p_FmPort->p_FmPortDriverParam->cheksumLastBytesIgnore = cheksumLastBytesIgnore;
+    p_FmPort->p_FmPortDriverParam->dfltCfg.checksum_bytes_ignore = checksumLastBytesIgnore;
 
     return E_OK;
 }
@@ -3047,7 +2654,7 @@ t_Error FM_PORT_ConfigCutBytesFromEnd(t_Handle h_FmPort, uint8_t cutBytesFromEnd
     if ((p_FmPort->portType != e_FM_PORT_TYPE_RX_10G) && (p_FmPort->portType != e_FM_PORT_TYPE_RX))
         RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("available for Rx ports only"));
 
-    p_FmPort->p_FmPortDriverParam->cutBytesFromEnd = cutBytesFromEnd;
+    p_FmPort->p_FmPortDriverParam->dfltCfg.rx_cut_end_bytes = cutBytesFromEnd;
 
     return E_OK;
 }
@@ -3122,7 +2729,7 @@ t_Error FM_PORT_ConfigDfltColor(t_Handle h_FmPort, e_FmPortColor color)
 
     SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
     SANITY_CHECK_RETURN_ERROR(p_FmPort->p_FmPortDriverParam, E_INVALID_HANDLE);
-    p_FmPort->p_FmPortDriverParam->color = color;
+    p_FmPort->p_FmPortDriverParam->dfltCfg.color = (enum fman_port_color)color;
 
     return E_OK;
 }
@@ -3137,7 +2744,7 @@ t_Error FM_PORT_ConfigSyncReq(t_Handle h_FmPort, bool syncReq)
     if ((p_FmPort->portType == e_FM_PORT_TYPE_TX_10G) && (p_FmPort->portType == e_FM_PORT_TYPE_TX))
         RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("Not available for Tx ports"));
 
-    p_FmPort->p_FmPortDriverParam->syncReq = syncReq;
+    p_FmPort->p_FmPortDriverParam->dfltCfg.sync_req = syncReq;
 
     return E_OK;
 }
@@ -3152,7 +2759,7 @@ t_Error FM_PORT_ConfigFrmDiscardOverride(t_Handle h_FmPort, bool override)
     if ((p_FmPort->portType == e_FM_PORT_TYPE_TX_10G) && (p_FmPort->portType == e_FM_PORT_TYPE_TX))
         RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("not available for Tx ports"));
 
-    p_FmPort->p_FmPortDriverParam->frmDiscardOverride = override;
+    p_FmPort->p_FmPortDriverParam->dfltCfg.discard_override = override;
 
     return E_OK;
 }
@@ -3179,7 +2786,7 @@ t_Error FM_PORT_ConfigDmaSwapData(t_Handle h_FmPort, e_FmDmaSwapOption swapData)
     SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
     SANITY_CHECK_RETURN_ERROR(p_FmPort->p_FmPortDriverParam, E_INVALID_HANDLE);
 
-    p_FmPort->p_FmPortDriverParam->dmaSwapData = swapData;
+    p_FmPort->p_FmPortDriverParam->dfltCfg.dma_swap_data = (enum fman_port_dma_swap)swapData;
 
     return E_OK;
 }
@@ -3191,7 +2798,8 @@ t_Error FM_PORT_ConfigDmaIcCacheAttr(t_Handle h_FmPort, e_FmDmaCacheOption intCo
     SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
     SANITY_CHECK_RETURN_ERROR(p_FmPort->p_FmPortDriverParam, E_INVALID_HANDLE);
 
-    p_FmPort->p_FmPortDriverParam->dmaIntContextCacheAttr = intContextCacheAttr;
+    p_FmPort->p_FmPortDriverParam->dfltCfg.dma_ic_stash_on =
+        (bool)(intContextCacheAttr == e_FM_DMA_STASH);
 
     return E_OK;
 }
@@ -3203,7 +2811,8 @@ t_Error FM_PORT_ConfigDmaHdrAttr(t_Handle h_FmPort, e_FmDmaCacheOption headerCac
     SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
     SANITY_CHECK_RETURN_ERROR(p_FmPort->p_FmPortDriverParam, E_INVALID_HANDLE);
 
-    p_FmPort->p_FmPortDriverParam->dmaHeaderCacheAttr = headerCacheAttr;
+    p_FmPort->p_FmPortDriverParam->dfltCfg.dma_header_stash_on =
+        (bool)(headerCacheAttr == e_FM_DMA_STASH);
 
     return E_OK;
 }
@@ -3215,7 +2824,8 @@ t_Error FM_PORT_ConfigDmaScatterGatherAttr(t_Handle h_FmPort, e_FmDmaCacheOption
     SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
     SANITY_CHECK_RETURN_ERROR(p_FmPort->p_FmPortDriverParam, E_INVALID_HANDLE);
 
-    p_FmPort->p_FmPortDriverParam->dmaScatterGatherCacheAttr = scatterGatherCacheAttr;
+    p_FmPort->p_FmPortDriverParam->dfltCfg.dma_sg_stash_on =
+        (bool)(scatterGatherCacheAttr == e_FM_DMA_STASH);
 
     return E_OK;
 }
@@ -3230,7 +2840,7 @@ t_Error FM_PORT_ConfigDmaWriteOptimize(t_Handle h_FmPort, bool optimize)
     if ((p_FmPort->portType == e_FM_PORT_TYPE_TX_10G) || (p_FmPort->portType == e_FM_PORT_TYPE_TX))
         RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("Not available for Tx ports"));
 
-    p_FmPort->p_FmPortDriverParam->dmaWriteOptimize = optimize;
+    p_FmPort->p_FmPortDriverParam->dfltCfg.dma_write_optimize = optimize;
 
     return E_OK;
 }
@@ -3306,7 +2916,7 @@ t_Error FM_PORT_ConfigTxFifoMinFillLevel(t_Handle h_FmPort, uint32_t minFillLeve
     if ((p_FmPort->portType != e_FM_PORT_TYPE_TX_10G) && (p_FmPort->portType != e_FM_PORT_TYPE_TX))
         RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("available for Tx ports only"));
 
-    p_FmPort->p_FmPortDriverParam->txFifoMinFillLevel = minFillLevel;
+    p_FmPort->p_FmPortDriverParam->dfltCfg.tx_fifo_min_level = minFillLevel;
 
     return E_OK;
 }
@@ -3324,7 +2934,7 @@ t_Error FM_PORT_ConfigFifoDeqPipelineDepth(t_Handle h_FmPort, uint8_t deqPipelin
     if (p_FmPort->imEn)
         RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("Not available for IM ports!"));
 
-    p_FmPort->fifoDeqPipelineDepth = deqPipelineDepth;
+    p_FmPort->p_FmPortDriverParam->dfltCfg.tx_fifo_deq_pipeline_depth = deqPipelineDepth;
 
     return E_OK;
 }
@@ -3338,7 +2948,7 @@ t_Error FM_PORT_ConfigTxFifoLowComfLevel(t_Handle h_FmPort, uint32_t fifoLowComf
     if ((p_FmPort->portType != e_FM_PORT_TYPE_TX_10G) && (p_FmPort->portType != e_FM_PORT_TYPE_TX))
         RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("available for Tx ports only"));
 
-    p_FmPort->p_FmPortDriverParam->txFifoLowComfLevel = fifoLowComfLevel;
+    p_FmPort->p_FmPortDriverParam->dfltCfg.tx_fifo_low_comf_level = fifoLowComfLevel;
 
     return E_OK;
 }
@@ -3352,7 +2962,7 @@ t_Error FM_PORT_ConfigRxFifoThreshold(t_Handle h_FmPort, uint32_t fifoThreshold)
     if ((p_FmPort->portType != e_FM_PORT_TYPE_RX_10G) && (p_FmPort->portType != e_FM_PORT_TYPE_RX))
         RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("available for Rx ports only"));
 
-    p_FmPort->p_FmPortDriverParam->rxFifoThreshold = fifoThreshold;
+    p_FmPort->p_FmPortDriverParam->dfltCfg.rx_fifo_thr = fifoThreshold;
 
     return E_OK;
 }
@@ -3366,7 +2976,7 @@ t_Error FM_PORT_ConfigRxFifoPriElevationLevel(t_Handle h_FmPort, uint32_t priEle
     if ((p_FmPort->portType != e_FM_PORT_TYPE_RX_10G) && (p_FmPort->portType != e_FM_PORT_TYPE_RX))
         RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("available for Rx ports only"));
 
-    p_FmPort->p_FmPortDriverParam->rxFifoPriElevationLevel = priElevationLevel;
+    p_FmPort->p_FmPortDriverParam->dfltCfg.rx_pri_elevation = priElevationLevel;
 
     return E_OK;
 }
@@ -3428,8 +3038,8 @@ t_Error FM_PORT_SetSizeOfFifo(t_Handle h_FmPort, t_FmPortRsrc *p_SizeOfFifo)
     SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
     SANITY_CHECK_RETURN_ERROR(!p_FmPort->p_FmPortDriverParam, E_INVALID_HANDLE);
 
-    if (!p_SizeOfFifo->num || (p_SizeOfFifo->num > BMI_MAX_FIFO_SIZE))
-         RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("SizeOfFifo-num has to be in the range of 256 - %d", BMI_MAX_FIFO_SIZE));
+    if (!p_SizeOfFifo->num || (p_SizeOfFifo->num > MAX_PORT_FIFO_SIZE))
+         RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("SizeOfFifo-num has to be in the range of 256 - %d", MAX_PORT_FIFO_SIZE));
     if (p_SizeOfFifo->num % BMI_FIFO_UNITS)
          RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("SizeOfFifo-num has to be divisible by %d", BMI_FIFO_UNITS));
     if ((p_FmPort->portType == e_FM_PORT_TYPE_RX) || (p_FmPort->portType == e_FM_PORT_TYPE_RX_10G))
@@ -3524,92 +3134,27 @@ uint8_t * FM_PORT_GetBufferHashResult(t_Handle h_FmPort, char *p_Data)
 
 t_Error FM_PORT_Disable(t_Handle h_FmPort)
 {
-    t_FmPort                    *p_FmPort = (t_FmPort*)h_FmPort;
-    volatile uint32_t           *p_BmiCfgReg = NULL;
-    volatile uint32_t           *p_BmiStatusReg = NULL;
-    bool                        rxPort = FALSE;
-    int                         tries;
+    t_FmPort *p_FmPort = (t_FmPort*)h_FmPort;
+    int err;
 
     SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
     SANITY_CHECK_RETURN_ERROR(!p_FmPort->p_FmPortDriverParam, E_INVALID_STATE);
 
-    switch (p_FmPort->portType)
-    {
-        case (e_FM_PORT_TYPE_RX_10G):
-        case (e_FM_PORT_TYPE_RX):
-            p_BmiCfgReg = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rcfg;
-            p_BmiStatusReg = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rst;
-            rxPort = TRUE;
-            break;
-        case (e_FM_PORT_TYPE_TX_10G):
-        case (e_FM_PORT_TYPE_TX):
-             p_BmiCfgReg = &p_FmPort->p_FmPortBmiRegs->txPortBmiRegs.fmbm_tcfg;
-             p_BmiStatusReg = &p_FmPort->p_FmPortBmiRegs->txPortBmiRegs.fmbm_tst;
-            break;
-        case (e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
-        case (e_FM_PORT_TYPE_OH_HOST_COMMAND):
-            p_BmiCfgReg = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ocfg;
-            p_BmiStatusReg = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ost;
-            break;
-        default:
-            RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Invalid port type"));
-    }
-
-    /* check if port is already disabled */
-    if (!(GET_UINT32(*p_BmiCfgReg) & BMI_PORT_CFG_EN))
-    {
-        if (!rxPort && !p_FmPort->imEn)
-        {
-            if (!(GET_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnc)& QMI_PORT_CFG_EN))
-                /* port is disabled */
-                return E_OK;
-            else
-                RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Inconsistency: Port's QMI is enabled but BMI disabled"));
-        }
-        /* port is disabled */
-        return E_OK;
-    }
-
-    /* Disable QMI */
-    if (!rxPort && !p_FmPort->imEn)
-    {
-        WRITE_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnc,
-                     GET_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnc) & ~QMI_PORT_CFG_EN);
-        /* wait for QMI to finish Handling dequeue tnums */
-        tries=1000;
-        while ((GET_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pns) & QMI_PORT_STATUS_DEQ_FD_BSY) &&
-                --tries)
-            XX_UDelay(1);
-        if (!tries)
-        {
-            WRITE_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnc,
-                         GET_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnc) | QMI_PORT_CFG_EN);
-            RETURN_ERROR(MAJOR, E_BUSY, ("%s: can't disable! QMI busy", p_FmPort->name));
-        }
-    }
-
-    /* Disable BMI */
-    WRITE_UINT32(*p_BmiCfgReg, GET_UINT32(*p_BmiCfgReg) & ~BMI_PORT_CFG_EN);
-
     if (p_FmPort->imEn)
         FmPortImDisable(p_FmPort);
 
-    tries=5000;
-    while ((GET_UINT32(*p_BmiStatusReg) & BMI_PORT_STATUS_BSY) &&
-            --tries)
-        XX_UDelay(1);
-
-    if (!tries)
+    err = fman_port_disable(&p_FmPort->port);
+    if (err == -EBUSY)
     {
-        if (!rxPort && !p_FmPort->imEn)
-            WRITE_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnc,
-                         GET_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnc) | QMI_PORT_CFG_EN);
-        WRITE_UINT32(*p_BmiCfgReg, GET_UINT32(*p_BmiCfgReg) | BMI_PORT_CFG_EN);
-
-        RETURN_ERROR(MAJOR, E_BUSY, ("%s: can't disable! BMI Busy", p_FmPort->name));
+        fman_port_enable(&p_FmPort->port);
+        RETURN_ERROR(MINOR, E_BUSY, ("%s: can't disable! BMI or QMI is Busy", p_FmPort->name));
+    }
+    else if (err != 0)
+    {
+        RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("fman_port_disable"));
     }
 
-    p_FmPort->enabled = 0;
+    p_FmPort->enabled = FALSE;
 
     return E_OK;
 }
@@ -3617,58 +3162,26 @@ t_Error FM_PORT_Disable(t_Handle h_FmPort)
 t_Error FM_PORT_Enable(t_Handle h_FmPort)
 {
     t_FmPort                    *p_FmPort = (t_FmPort*)h_FmPort;
-    volatile uint32_t           *p_BmiCfgReg = NULL;
-    bool                        rxPort = FALSE;
+    int err;
 
     SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
     SANITY_CHECK_RETURN_ERROR(!p_FmPort->p_FmPortDriverParam, E_INVALID_STATE);
 
-    switch (p_FmPort->portType)
-    {
-        case (e_FM_PORT_TYPE_RX_10G):
-        case (e_FM_PORT_TYPE_RX):
-            p_BmiCfgReg = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rcfg;
-            rxPort = TRUE;
-            break;
-        case (e_FM_PORT_TYPE_TX_10G):
-        case (e_FM_PORT_TYPE_TX):
-             p_BmiCfgReg = &p_FmPort->p_FmPortBmiRegs->txPortBmiRegs.fmbm_tcfg;
-            break;
-        case (e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
-        case (e_FM_PORT_TYPE_OH_HOST_COMMAND):
-            p_BmiCfgReg = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ocfg;
-            break;
-        default:
-            RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Invalid port type"));
-    }
-
-    /* check if port is already enabled */
-    if (GET_UINT32(*p_BmiCfgReg) & BMI_PORT_CFG_EN)
-    {
-        if (!rxPort && !p_FmPort->imEn)
-        {
-            if (GET_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnc)& QMI_PORT_CFG_EN)
-                /* port is enabled */
-                return E_OK;
-            else
-                RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Inconsistency: Port's BMI is enabled but QMI disabled"));
-        }
-        /* port is enabled */
-        return E_OK;
-    }
+    /* Used by FM_PORT_Free routine as indication
+       if to disable port. Thus set it to TRUE prior
+       to enabling itself. This way if part of enable
+       process fails there will be still things
+       to disable during Free. For example, if BMI
+       enable succeeded but QMI failed, still  BMI
+       needs to be disabled by Free. */
+    p_FmPort->enabled = TRUE;
 
     if (p_FmPort->imEn)
         FmPortImEnable(p_FmPort);
 
-    /* Enable QMI */
-    if (!rxPort && !p_FmPort->imEn)
-        WRITE_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnc,
-                     GET_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnc) | QMI_PORT_CFG_EN);
-
-    /* Enable BMI */
-    WRITE_UINT32(*p_BmiCfgReg, GET_UINT32(*p_BmiCfgReg) | BMI_PORT_CFG_EN);
-
-    p_FmPort->enabled = 1;
+    err = fman_port_enable(&p_FmPort->port);
+    if (err != 0)
+        RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("fman_port_enable"));
 
     return E_OK;
 }
@@ -3676,33 +3189,25 @@ t_Error FM_PORT_Enable(t_Handle h_FmPort)
 t_Error FM_PORT_SetRateLimit(t_Handle h_FmPort, t_FmPortRateLimit *p_RateLimit)
 {
     t_FmPort            *p_FmPort = (t_FmPort*)h_FmPort;
-    uint32_t            tmpRateLimit, tmpRateLimitScale;
-    volatile uint32_t   *p_RateLimitReg, *p_RateLimitScaleReg;
     uint8_t             factor, countUnitBit;
     uint16_t            baseGran;
+    struct fman_port_rate_limiter params;
+    int                 err;
 
     SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
     SANITY_CHECK_RETURN_ERROR(!p_FmPort->p_FmPortDriverParam, E_INVALID_HANDLE);
 
-    if ((p_FmPort->portType == e_FM_PORT_TYPE_RX_10G) || (p_FmPort->portType == e_FM_PORT_TYPE_RX) ||
-                                                (p_FmPort->portType == e_FM_PORT_TYPE_OH_HOST_COMMAND))
-        RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("available for Tx and Offline parsing ports only"));
-
-    switch (p_FmPort->portType)
+    switch(p_FmPort->portType)
     {
-        case (e_FM_PORT_TYPE_TX_10G):
-        case (e_FM_PORT_TYPE_TX):
-            p_RateLimitReg = &p_FmPort->p_FmPortBmiRegs->txPortBmiRegs.fmbm_trlmt;
-            p_RateLimitScaleReg = &p_FmPort->p_FmPortBmiRegs->txPortBmiRegs.fmbm_trlmts;
-            baseGran = 16000;
+        case(e_FM_PORT_TYPE_TX_10G):
+        case(e_FM_PORT_TYPE_TX):
+            baseGran = BMI_RATE_LIMIT_GRAN_TX;
             break;
-        case (e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
-            p_RateLimitReg = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_orlmt;
-            p_RateLimitScaleReg = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_orlmts;
-            baseGran = 10000;
+        case(e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
+            baseGran = BMI_RATE_LIMIT_GRAN_OP;
            break;
         default:
-            RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Invalid port type"));
+           RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("available for Tx and Offline parsing ports only"));
     }
 
     countUnitBit = (uint8_t)FmGetTimeStampScale(p_FmPort->h_Fm);  /* TimeStamp per nano seconds units */
@@ -3721,38 +3226,23 @@ t_Error FM_PORT_SetRateLimit(t_Handle h_FmPort, t_FmPortRateLimit *p_RateLimit)
     if (p_RateLimit->rateLimit > ((uint32_t)baseGran * (1<<10) * (uint32_t)factor))
         RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("Rate limit is too large"));
 
-    tmpRateLimit = (uint32_t)(p_RateLimit->rateLimit*factor/baseGran - 1);
-
-    if (!p_RateLimit->maxBurstSize || (p_RateLimit->maxBurstSize > MAX_BURST_SIZE))
-        RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("maxBurstSize must be between 1K and %dk", MAX_BURST_SIZE));
+    if (!p_RateLimit->maxBurstSize || (p_RateLimit->maxBurstSize > BMI_RATE_LIMIT_MAX_BURST_SIZE))
+        RETURN_ERROR(MAJOR, E_INVALID_VALUE,
+                     ("maxBurstSize must be between 1K and %dk", BMI_RATE_LIMIT_MAX_BURST_SIZE));
 
-    tmpRateLimitScale = ((31 - (uint32_t)countUnitBit) << BMI_COUNT_RATE_UNIT_SHIFT) | BMI_RATE_LIMIT_EN;
+    params.count_1micro_bit = (uint8_t)FmGetTimeStampScale(p_FmPort->h_Fm);
+    params.high_burst_size_gran = FALSE;
+    params.burst_size = p_RateLimit->maxBurstSize;
+    params.rate = p_RateLimit->rateLimit;
+    params.rate_factor = E_FMAN_PORT_RATE_DOWN_NONE;
 
-    if (p_FmPort->portType != e_FM_PORT_TYPE_OH_OFFLINE_PARSING)
-        tmpRateLimit |= (uint32_t)(p_RateLimit->maxBurstSize - 1) << BMI_MAX_BURST_SHIFT;
-    else
+    if (p_FmPort->portType == e_FM_PORT_TYPE_OH_OFFLINE_PARSING)
     {
 #ifndef FM_NO_ADVANCED_RATE_LIMITER
 
         if ((p_FmPort->fmRevInfo.majorRev == 4) || (p_FmPort->fmRevInfo.majorRev >= 6))
         {
-            switch (p_RateLimit->rateLimitDivider)
-            {
-                case (e_FM_PORT_DUAL_RATE_LIMITER_NONE):
-                    break;
-                case (e_FM_PORT_DUAL_RATE_LIMITER_SCALE_DOWN_BY_2):
-                    tmpRateLimitScale |= BMI_RATE_LIMIT_SCALE_BY_2;
-                    break;
-                case (e_FM_PORT_DUAL_RATE_LIMITER_SCALE_DOWN_BY_4):
-                    tmpRateLimitScale |= BMI_RATE_LIMIT_SCALE_BY_4;
-                    break;
-                case (e_FM_PORT_DUAL_RATE_LIMITER_SCALE_DOWN_BY_8):
-                    tmpRateLimitScale |= BMI_RATE_LIMIT_SCALE_BY_8;
-                    break;
-                default:
-                    break;
-            }
-            tmpRateLimit |= BMI_RATE_LIMIT_BURST_SIZE_GRAN;
+            params.high_burst_size_gran = TRUE;
         }
         else
 #endif /* ! FM_NO_ADVANCED_RATE_LIMITER */
@@ -3768,11 +3258,13 @@ t_Error FM_PORT_SetRateLimit(t_Handle h_FmPort, t_FmPortRateLimit *p_RateLimit)
             else
                 p_RateLimit->maxBurstSize = (uint16_t)(p_RateLimit->maxBurstSize/1000);
         }
-        tmpRateLimit |= (uint32_t)(p_RateLimit->maxBurstSize - 1) << BMI_MAX_BURST_SHIFT;
-
+        params.rate_factor = (enum fman_port_rate_limiter_scale_down)p_RateLimit->rateLimitDivider;
+        params.burst_size = p_RateLimit->maxBurstSize;
     }
-    WRITE_UINT32(*p_RateLimitScaleReg, tmpRateLimitScale);
-    WRITE_UINT32(*p_RateLimitReg, tmpRateLimit);
+
+    err = fman_port_set_rate_limiter(&p_FmPort->port, &params);
+    if (err != 0)
+        RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("fman_port_set_rate_limiter"));
 
     return E_OK;
 }
@@ -3780,33 +3272,20 @@ t_Error FM_PORT_SetRateLimit(t_Handle h_FmPort, t_FmPortRateLimit *p_RateLimit)
 t_Error FM_PORT_DeleteRateLimit(t_Handle h_FmPort)
 {
     t_FmPort            *p_FmPort = (t_FmPort*)h_FmPort;
-    volatile uint32_t   *p_RateLimitReg, *p_RateLimitScaleReg;
+    int                 err;
 
     SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
     SANITY_CHECK_RETURN_ERROR(!p_FmPort->p_FmPortDriverParam, E_INVALID_HANDLE);
 
-    if ((p_FmPort->portType == e_FM_PORT_TYPE_RX_10G) || (p_FmPort->portType == e_FM_PORT_TYPE_RX) ||
-                                                (p_FmPort->portType == e_FM_PORT_TYPE_OH_HOST_COMMAND))
-        RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("available for Tx and Offline parsing ports only"));
-
-    switch (p_FmPort->portType)
-    {
-        case (e_FM_PORT_TYPE_TX_10G):
-        case (e_FM_PORT_TYPE_TX):
-            p_RateLimitReg = &p_FmPort->p_FmPortBmiRegs->txPortBmiRegs.fmbm_trlmt;
-            p_RateLimitScaleReg = &p_FmPort->p_FmPortBmiRegs->txPortBmiRegs.fmbm_trlmts;
-            break;
-        case (e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
-            p_RateLimitReg = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_orlmt;
-            p_RateLimitScaleReg = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_orlmts;
-           break;
-        default:
-            RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Invalid port type"));
-    }
-
-    WRITE_UINT32(*p_RateLimitScaleReg, 0);
-    WRITE_UINT32(*p_RateLimitReg, 0);
+    if ((p_FmPort->portType == e_FM_PORT_TYPE_RX_10G) ||
+       (p_FmPort->portType == e_FM_PORT_TYPE_RX) ||
+       (p_FmPort->portType == e_FM_PORT_TYPE_OH_HOST_COMMAND))
+        RETURN_ERROR(MAJOR, E_INVALID_OPERATION,
+                ("available for Tx and Offline parsing ports only"));
 
+    err = fman_port_delete_rate_limiter(&p_FmPort->port);
+    if (err != 0)
+        RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("fman_port_set_rate_limiter"));
     return E_OK;
 }
 
@@ -3840,82 +3319,39 @@ t_Error FM_PORT_SetPfcPrioritiesMappingToQmanWQ(t_Handle h_FmPort, uint8_t prio,
 t_Error FM_PORT_SetFrameQueueCounters(t_Handle h_FmPort, bool enable)
 {
     t_FmPort                *p_FmPort = (t_FmPort*)h_FmPort;
-    uint32_t                tmpReg;
+    int                     err;
 
     SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
     SANITY_CHECK_RETURN_ERROR(!p_FmPort->p_FmPortDriverParam, E_INVALID_STATE);
 
-    tmpReg = GET_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnc);
-    if (enable)
-        tmpReg |= QMI_PORT_CFG_EN_COUNTERS ;
-    else
-        tmpReg &= ~QMI_PORT_CFG_EN_COUNTERS;
-
-    WRITE_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnc, tmpReg);
-
+    err = fman_port_set_queue_cnt_mode(&p_FmPort->port, enable);
+    if (err != 0)
+        RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("fman_port_set_perf_cnt_mode"));
     return E_OK;
 }
 
 t_Error FM_PORT_SetPerformanceCounters(t_Handle h_FmPort, bool enable)
 {
     t_FmPort                *p_FmPort = (t_FmPort*)h_FmPort;
-    volatile uint32_t       *p_BmiPcReg = NULL;
+    int                     err;
 
     SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
     SANITY_CHECK_RETURN_ERROR(!p_FmPort->p_FmPortDriverParam, E_INVALID_STATE);
 
-    switch (p_FmPort->portType)
-    {
-        case (e_FM_PORT_TYPE_RX_10G):
-        case (e_FM_PORT_TYPE_RX):
-            p_BmiPcReg = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rpc;
-            break;
-        case (e_FM_PORT_TYPE_TX_10G):
-        case (e_FM_PORT_TYPE_TX):
-            p_BmiPcReg = &p_FmPort->p_FmPortBmiRegs->txPortBmiRegs.fmbm_tpc;
-            break;
-        case (e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
-        case (e_FM_PORT_TYPE_OH_HOST_COMMAND):
-            p_BmiPcReg = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_opc;
-            break;
-        default:
-            RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Invalid port type"));
-    }
-
-    if (enable)
-        WRITE_UINT32(*p_BmiPcReg, BMI_COUNTERS_EN);
-    else
-        WRITE_UINT32(*p_BmiPcReg, 0);
-
+    err = fman_port_set_perf_cnt_mode(&p_FmPort->port, enable);
+    if (err != 0)
+        RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("fman_port_set_perf_cnt_mode"));
     return E_OK;
 }
 
 t_Error FM_PORT_SetPerformanceCountersParams(t_Handle h_FmPort, t_FmPortPerformanceCnt *p_FmPortPerformanceCnt)
 {
     t_FmPort                *p_FmPort = (t_FmPort*)h_FmPort;
-    uint32_t                tmpReg;
-    volatile uint32_t       *p_BmiPcpReg = NULL;
+    struct fman_port_perf_cnt_params    params;
+    int                                 err;
 
     SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
 
-    switch (p_FmPort->portType)
-    {
-        case (e_FM_PORT_TYPE_RX_10G):
-        case (e_FM_PORT_TYPE_RX):
-            p_BmiPcpReg = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rpcp;
-            break;
-        case (e_FM_PORT_TYPE_TX_10G):
-        case (e_FM_PORT_TYPE_TX):
-            p_BmiPcpReg = &p_FmPort->p_FmPortBmiRegs->txPortBmiRegs.fmbm_tpcp;
-            break;
-        case (e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
-        case (e_FM_PORT_TYPE_OH_HOST_COMMAND):
-            p_BmiPcpReg = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_opcp;
-            break;
-        default:
-            RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Invalid port type"));
-    }
-
     /* check parameters */
     if (!p_FmPortPerformanceCnt->taskCompVal ||
         (p_FmPortPerformanceCnt->taskCompVal > p_FmPort->tasks.num))
@@ -3968,14 +3404,14 @@ t_Error FM_PORT_SetPerformanceCountersParams(t_Handle h_FmPort, t_FmPortPerforma
             RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Invalid port type"));
     }
 
-    tmpReg = 0;
-    tmpReg |= ((uint32_t)(p_FmPortPerformanceCnt->queueCompVal - 1) << BMI_PERFORMANCE_PORT_COMP_SHIFT);
-    tmpReg |= ((uint32_t)(p_FmPortPerformanceCnt->dmaCompVal- 1) << BMI_PERFORMANCE_DMA_COMP_SHIFT);
-    tmpReg |= ((uint32_t)(p_FmPortPerformanceCnt->fifoCompVal/BMI_FIFO_UNITS - 1) << BMI_PERFORMANCE_FIFO_COMP_SHIFT);
-    if ((p_FmPort->portType != e_FM_PORT_TYPE_OH_OFFLINE_PARSING) && (p_FmPort->portType != e_FM_PORT_TYPE_OH_HOST_COMMAND))
-        tmpReg |= ((uint32_t)(p_FmPortPerformanceCnt->taskCompVal - 1) << BMI_PERFORMANCE_TASK_COMP_SHIFT);
+    params.task_val     = p_FmPortPerformanceCnt->taskCompVal;
+    params.queue_val    = p_FmPortPerformanceCnt->queueCompVal;
+    params.dma_val      = p_FmPortPerformanceCnt->dmaCompVal;
+    params.fifo_val     = p_FmPortPerformanceCnt->fifoCompVal;
 
-    WRITE_UINT32(*p_BmiPcpReg, tmpReg);
+    err = fman_port_set_perf_cnt_params(&p_FmPort->port, &params);
+    if (err != 0)
+        RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("fman_port_set_perf_cnt_params"));
 
     return E_OK;
 }
@@ -4070,68 +3506,29 @@ t_Error FM_PORT_AnalyzePerformanceParams(t_Handle h_FmPort)
 
 t_Error FM_PORT_SetStatisticsCounters(t_Handle h_FmPort, bool enable)
 {
-    t_FmPort                *p_FmPort = (t_FmPort*)h_FmPort;
-    uint32_t                tmpReg;
-    volatile uint32_t       *p_BmiStcReg = NULL;
+    t_FmPort    *p_FmPort = (t_FmPort*)h_FmPort;
+    int         err;
 
     SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
     SANITY_CHECK_RETURN_ERROR(!p_FmPort->p_FmPortDriverParam, E_INVALID_STATE);
 
-    switch (p_FmPort->portType)
-    {
-        case (e_FM_PORT_TYPE_RX_10G):
-        case (e_FM_PORT_TYPE_RX):
-            p_BmiStcReg = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rstc;
-            break;
-        case (e_FM_PORT_TYPE_TX_10G):
-        case (e_FM_PORT_TYPE_TX):
-            p_BmiStcReg = &p_FmPort->p_FmPortBmiRegs->txPortBmiRegs.fmbm_tstc;
-            break;
-        case (e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
-        case (e_FM_PORT_TYPE_OH_HOST_COMMAND):
-            p_BmiStcReg = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ostc;
-            break;
-        default:
-            RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Invalid port type"));
-    }
-
-    tmpReg = GET_UINT32(*p_BmiStcReg);
-
-    if (enable)
-        tmpReg |= BMI_COUNTERS_EN;
-    else
-        tmpReg &= ~BMI_COUNTERS_EN;
-
-    WRITE_UINT32(*p_BmiStcReg, tmpReg);
-
+    err = fman_port_set_stats_cnt_mode(&p_FmPort->port, enable);
+    if (err != 0)
+        RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("fman_port_set_stats_cnt_mode"));
     return E_OK;
 }
 
 t_Error FM_PORT_SetErrorsRoute(t_Handle h_FmPort, fmPortFrameErrSelect_t errs)
 {
     t_FmPort                *p_FmPort = (t_FmPort*)h_FmPort;
-    volatile uint32_t       *p_ErrQReg, *p_ErrDiscard;
-
-    switch (p_FmPort->portType)
-    {
-        case (e_FM_PORT_TYPE_RX_10G):
-        case (e_FM_PORT_TYPE_RX):
-            p_ErrQReg = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rfsem;
-            p_ErrDiscard = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rfsdm;
-            break;
-        case (e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
-            p_ErrQReg = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ofsem;
-            p_ErrDiscard = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ofsdm;
-            break;
-        default:
-           RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("available for Rx and offline parsing ports only"));
-    }
+    volatile uint32_t       *p_ErrDiscard = NULL;
+    int                     err;
 
-    if (GET_UINT32(*p_ErrDiscard) & errs)
-        RETURN_ERROR(MAJOR, E_INVALID_SELECTION, ("Selectd Errors that were configured to cause frame discard."));
+    UNUSED(p_ErrDiscard);
+    err = fman_port_set_err_mask(&p_FmPort->port, (uint32_t)errs);
+    if (err != 0)
+        RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("fman_port_set_err_mask"));
 
-    WRITE_UINT32(*p_ErrQReg, errs);
- 
 #ifdef FM_ERROR_VSP_NO_MATCH_SW006
     if (p_FmPort->fmRevInfo.majorRev >= 6)
     {
@@ -4139,18 +3536,29 @@ t_Error FM_PORT_SetErrorsRoute(t_Handle h_FmPort, fmPortFrameErrSelect_t errs)
 
         FmPortSetGprFunc(p_FmPort, e_FM_PORT_GPR_MURAM_PAGE, (void**)&p_ParamsPage);
         ASSERT_COND(p_ParamsPage);
+        switch (p_FmPort->portType)
+        {
+            case (e_FM_PORT_TYPE_RX_10G):
+            case (e_FM_PORT_TYPE_RX):
+                p_ErrDiscard = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rfsdm;
+                break;
+            case (e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
+                p_ErrDiscard = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ofsdm;
+                break;
+            default:
+               RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("available for Rx and offline parsing ports only"));
+        }
         WRITE_UINT32(p_ParamsPage->errorsDiscardMask, GET_UINT32(*p_ErrDiscard) | errs);
     }
 #endif /* FM_ERROR_VSP_NO_MATCH_SW006 */
- 
+
     return E_OK;
 }
 
 t_Error FM_PORT_SetAllocBufCounter(t_Handle h_FmPort, uint8_t poolId, bool enable)
 {
     t_FmPort                *p_FmPort = (t_FmPort*)h_FmPort;
-    uint32_t                tmpReg;
-    int                     i;
+    int                     err;
 
     SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
     SANITY_CHECK_RETURN_ERROR(poolId<BM_MAX_NUM_OF_POOLS, E_INVALID_HANDLE);
@@ -4159,48 +3567,40 @@ t_Error FM_PORT_SetAllocBufCounter(t_Handle h_FmPort, uint8_t poolId, bool enabl
     if ((p_FmPort->portType != e_FM_PORT_TYPE_RX_10G) && (p_FmPort->portType != e_FM_PORT_TYPE_RX))
         RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("available for Rx ports only"));
 
-    for (i=0 ; i< FM_PORT_MAX_NUM_OF_EXT_POOLS ; i++)
-    {
-        tmpReg = GET_UINT32(p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_ebmpi[i]);
-        if ((uint8_t)((tmpReg & BMI_EXT_BUF_POOL_ID_MASK) >> BMI_EXT_BUF_POOL_ID_SHIFT) == poolId)
-        {
-            if (enable)
-                tmpReg |= BMI_EXT_BUF_POOL_EN_COUNTER;
-            else
-                tmpReg &= ~BMI_EXT_BUF_POOL_EN_COUNTER;
-            WRITE_UINT32(p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_ebmpi[i], tmpReg);
-            break;
-        }
-    }
-    if (i == FM_PORT_MAX_NUM_OF_EXT_POOLS)
-        RETURN_ERROR(MAJOR, E_INVALID_VALUE,("poolId %d is not included in this ports pools", poolId));
-
+    err = fman_port_set_bpool_cnt_mode(&p_FmPort->port, poolId, enable);
+    if (err != 0)
+        RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("fman_port_set_bpool_cnt_mode"));
     return E_OK;
 }
 
 uint32_t FM_PORT_GetCounter(t_Handle h_FmPort, e_FmPortCounters counter)
 {
-    t_FmPort            *p_FmPort = (t_FmPort*)h_FmPort;
-    bool                bmiCounter = FALSE;
-    volatile uint32_t   *p_Reg;
+    t_FmPort                        *p_FmPort = (t_FmPort*)h_FmPort;
+    bool                            bmiCounter = FALSE;
+    enum fman_port_stats_counters   statsType;
+    enum fman_port_perf_counters    perfType;
+    enum fman_port_qmi_counters   queueType;
+    bool                            isStats;
+    t_Error                         errCode;
 
     SANITY_CHECK_RETURN_VALUE(p_FmPort, E_INVALID_HANDLE, 0);
     SANITY_CHECK_RETURN_ERROR(!p_FmPort->p_FmPortDriverParam, E_INVALID_STATE);
 
     switch (counter)
     {
-        case (e_FM_PORT_COUNTERS_DEQ_TOTAL):
-        case (e_FM_PORT_COUNTERS_DEQ_FROM_DEFAULT):
-        case (e_FM_PORT_COUNTERS_DEQ_CONFIRM):
+        case(e_FM_PORT_COUNTERS_DEQ_TOTAL):
+        case(e_FM_PORT_COUNTERS_DEQ_FROM_DEFAULT):
+        case(e_FM_PORT_COUNTERS_DEQ_CONFIRM ):
             /* check that counter is available for the port type */
             if ((p_FmPort->portType == e_FM_PORT_TYPE_RX) ||
-                (p_FmPort->portType == e_FM_PORT_TYPE_RX_10G))
+               (p_FmPort->portType == e_FM_PORT_TYPE_RX_10G))
             {
-                REPORT_ERROR(MAJOR, E_INVALID_STATE, ("Requested counter is not available for Rx ports"));
+                REPORT_ERROR(MINOR, E_INVALID_STATE,
+                        ("Requested counter is not available for Rx ports"));
                 return 0;
             }
             bmiCounter = FALSE;
-        case (e_FM_PORT_COUNTERS_ENQ_TOTAL):
+        case(e_FM_PORT_COUNTERS_ENQ_TOTAL):
             bmiCounter = FALSE;
             break;
         default: /* BMI counters (or error - will be checked in BMI routine )*/
@@ -4210,83 +3610,76 @@ uint32_t FM_PORT_GetCounter(t_Handle h_FmPort, e_FmPortCounters counter)
 
     if (bmiCounter)
     {
-        switch (p_FmPort->portType)
+        errCode = BmiPortCheckAndGetCounterType(p_FmPort, counter, &statsType, &perfType, &isStats);
+        if (errCode != E_OK)
         {
-            case (e_FM_PORT_TYPE_RX_10G):
-            case (e_FM_PORT_TYPE_RX):
-                if (BmiRxPortCheckAndGetCounterPtr(p_FmPort, counter, &p_Reg))
-                {
-                    REPORT_ERROR(MAJOR, E_INVALID_STATE, NO_MSG);
-                    return 0;
-                }
-                break;
-            case (e_FM_PORT_TYPE_TX_10G):
-            case (e_FM_PORT_TYPE_TX):
-                if (BmiTxPortCheckAndGetCounterPtr(p_FmPort, counter, &p_Reg))
-                {
-                    REPORT_ERROR(MAJOR, E_INVALID_STATE, NO_MSG);
-                    return 0;
-                }
-                break;
-            case (e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
-            case (e_FM_PORT_TYPE_OH_HOST_COMMAND):
-                if (BmiOhPortCheckAndGetCounterPtr(p_FmPort, counter, &p_Reg))
-                {
-                    REPORT_ERROR(MAJOR, E_INVALID_STATE, NO_MSG);
-                    return 0;
-                }
-                break;
-            default:
-                REPORT_ERROR(MAJOR, E_INVALID_STATE, ("Unsupported port type"));
-                return 0;
+            REPORT_ERROR(MINOR, errCode, NO_MSG);
+            return 0;
         }
-        return GET_UINT32(*p_Reg);
+        if (isStats)
+            return fman_port_get_stats_counter(&p_FmPort->port, statsType);
+        else
+            return fman_port_get_perf_counter(&p_FmPort->port, perfType);
     }
     else /* QMI counter */
     {
         /* check that counters are enabled */
-        if (!(GET_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnc) & QMI_PORT_CFG_EN_COUNTERS))
+        if (!(GET_UINT32(p_FmPort->port.qmi_regs->fmqm_pnc) & QMI_PORT_CFG_EN_COUNTERS))
+
         {
-            REPORT_ERROR(MAJOR, E_INVALID_STATE, ("Requested counter was not enabled"));
+            REPORT_ERROR(MINOR, E_INVALID_STATE, ("Requested counter was not enabled"));
             return 0;
         }
 
         /* Set counter */
         switch (counter)
         {
-           case (e_FM_PORT_COUNTERS_ENQ_TOTAL):
-                return GET_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnetfc);
-            case (e_FM_PORT_COUNTERS_DEQ_TOTAL):
-                return GET_UINT32(p_FmPort->p_FmPortQmiRegs->nonRxQmiRegs.fmqm_pndtfc);
-            case (e_FM_PORT_COUNTERS_DEQ_FROM_DEFAULT):
-                return GET_UINT32(p_FmPort->p_FmPortQmiRegs->nonRxQmiRegs.fmqm_pndfdc);
-            case (e_FM_PORT_COUNTERS_DEQ_CONFIRM):
-                return GET_UINT32(p_FmPort->p_FmPortQmiRegs->nonRxQmiRegs.fmqm_pndcc);
+           case(e_FM_PORT_COUNTERS_ENQ_TOTAL):
+                queueType = E_FMAN_PORT_ENQ_TOTAL;
+               break;
+            case(e_FM_PORT_COUNTERS_DEQ_TOTAL):
+                queueType = E_FMAN_PORT_DEQ_TOTAL;
+               break;
+            case(e_FM_PORT_COUNTERS_DEQ_FROM_DEFAULT):
+                queueType = E_FMAN_PORT_DEQ_FROM_DFLT;
+               break;
+            case(e_FM_PORT_COUNTERS_DEQ_CONFIRM):
+                queueType = E_FMAN_PORT_DEQ_CONFIRM;
+               break;
             default:
-                REPORT_ERROR(MAJOR, E_INVALID_STATE, ("Requested counter is not available"));
-                return 0;
+               REPORT_ERROR(MINOR, E_INVALID_STATE, ("Requested counter is not available"));
+               return 0;
         }
+
+        return fman_port_get_qmi_counter(&p_FmPort->port, queueType);
     }
+
+    return 0;
 }
 
 t_Error FM_PORT_ModifyCounter(t_Handle h_FmPort, e_FmPortCounters counter, uint32_t value)
 {
-    t_FmPort            *p_FmPort = (t_FmPort*)h_FmPort;
-    bool                bmiCounter = FALSE;
-    volatile uint32_t   *p_Reg;
+    t_FmPort                        *p_FmPort = (t_FmPort*)h_FmPort;
+    bool                            bmiCounter = FALSE;
+    enum fman_port_stats_counters   statsType;
+    enum fman_port_perf_counters    perfType;
+    enum fman_port_qmi_counters   queueType;
+    bool                            isStats;
+    t_Error                         errCode;
 
     SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
     SANITY_CHECK_RETURN_ERROR(!p_FmPort->p_FmPortDriverParam, E_INVALID_STATE);
 
     switch (counter)
     {
-        case (e_FM_PORT_COUNTERS_DEQ_TOTAL):
-        case (e_FM_PORT_COUNTERS_DEQ_FROM_DEFAULT):
-        case (e_FM_PORT_COUNTERS_DEQ_CONFIRM ):
+        case(e_FM_PORT_COUNTERS_DEQ_TOTAL):
+        case(e_FM_PORT_COUNTERS_DEQ_FROM_DEFAULT):
+        case(e_FM_PORT_COUNTERS_DEQ_CONFIRM ):
             /* check that counter is available for the port type */
-            if ((p_FmPort->portType == e_FM_PORT_TYPE_RX) || (p_FmPort->portType == e_FM_PORT_TYPE_RX_10G))
-                        RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Requested counter is not available for Rx ports"));
-        case (e_FM_PORT_COUNTERS_ENQ_TOTAL):
+            if ((p_FmPort->portType == e_FM_PORT_TYPE_RX) ||
+               (p_FmPort->portType == e_FM_PORT_TYPE_RX_10G))
+                        RETURN_ERROR(MINOR, E_INVALID_STATE, ("Requested counter is not available for Rx ports"));
+        case(e_FM_PORT_COUNTERS_ENQ_TOTAL):
             bmiCounter = FALSE;
             break;
         default: /* BMI counters (or error - will be checked in BMI routine )*/
@@ -4296,131 +3689,78 @@ t_Error FM_PORT_ModifyCounter(t_Handle h_FmPort, e_FmPortCounters counter, uint3
 
     if (bmiCounter)
     {
-        switch (p_FmPort->portType)
+        errCode = BmiPortCheckAndGetCounterType(p_FmPort, counter, &statsType, &perfType, &isStats);
+        if (errCode != E_OK)
         {
-            case (e_FM_PORT_TYPE_RX_10G):
-            case (e_FM_PORT_TYPE_RX):
-               if (BmiRxPortCheckAndGetCounterPtr(p_FmPort, counter, &p_Reg))
-                    RETURN_ERROR(MAJOR, E_INVALID_STATE, NO_MSG);
-                break;
-            case (e_FM_PORT_TYPE_TX_10G):
-            case (e_FM_PORT_TYPE_TX):
-               if (BmiTxPortCheckAndGetCounterPtr(p_FmPort, counter, &p_Reg))
-                    RETURN_ERROR(MAJOR, E_INVALID_STATE, NO_MSG);
-                break;
-            case (e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
-            case (e_FM_PORT_TYPE_OH_HOST_COMMAND):
-               if (BmiOhPortCheckAndGetCounterPtr(p_FmPort, counter, &p_Reg))
-                    RETURN_ERROR(MAJOR, E_INVALID_STATE, NO_MSG);
-                 break;
-            default:
-               RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Unsupported port type"));
+            RETURN_ERROR(MINOR, errCode, NO_MSG);
         }
-        WRITE_UINT32(*p_Reg, value);
+        if (isStats)
+            fman_port_set_stats_counter(&p_FmPort->port, statsType, value);
+        else
+            fman_port_set_perf_counter(&p_FmPort->port, perfType, value);
     }
     else /* QMI counter */
     {
-
         /* check that counters are enabled */
-        if (!(GET_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnc) & QMI_PORT_CFG_EN_COUNTERS))
-                RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Requested counter was not enabled"));
+        if (!(GET_UINT32(p_FmPort->port.qmi_regs->fmqm_pnc) & QMI_PORT_CFG_EN_COUNTERS))
+        {
+            RETURN_ERROR(MINOR, E_INVALID_STATE, ("Requested counter was not enabled"));
+        }
 
         /* Set counter */
         switch (counter)
         {
-           case (e_FM_PORT_COUNTERS_ENQ_TOTAL):
-                WRITE_UINT32(p_FmPort->p_FmPortQmiRegs->fmqm_pnetfc, value);
-                break;
-            case (e_FM_PORT_COUNTERS_DEQ_TOTAL):
-                WRITE_UINT32(p_FmPort->p_FmPortQmiRegs->nonRxQmiRegs.fmqm_pndtfc, value);
-                break;
-            case (e_FM_PORT_COUNTERS_DEQ_FROM_DEFAULT):
-                WRITE_UINT32(p_FmPort->p_FmPortQmiRegs->nonRxQmiRegs.fmqm_pndfdc, value);
-                break;
-            case (e_FM_PORT_COUNTERS_DEQ_CONFIRM):
-                WRITE_UINT32(p_FmPort->p_FmPortQmiRegs->nonRxQmiRegs.fmqm_pndcc, value);
-                break;
+           case(e_FM_PORT_COUNTERS_ENQ_TOTAL):
+                queueType = E_FMAN_PORT_ENQ_TOTAL;
+               break;
+            case(e_FM_PORT_COUNTERS_DEQ_TOTAL):
+                queueType = E_FMAN_PORT_DEQ_TOTAL;
+               break;
+            case(e_FM_PORT_COUNTERS_DEQ_FROM_DEFAULT):
+                queueType = E_FMAN_PORT_DEQ_FROM_DFLT;
+               break;
+            case(e_FM_PORT_COUNTERS_DEQ_CONFIRM):
+                queueType = E_FMAN_PORT_DEQ_CONFIRM;
+               break;
             default:
-                RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Requested counter is not available"));
+               RETURN_ERROR(MINOR, E_INVALID_STATE, ("Requested counter is not available"));
         }
+
+        fman_port_set_qmi_counter(&p_FmPort->port, queueType, value);
     }
 
     return E_OK;
 }
 
+
 uint32_t FM_PORT_GetAllocBufCounter(t_Handle h_FmPort, uint8_t poolId)
 {
     t_FmPort        *p_FmPort = (t_FmPort*)h_FmPort;
-    uint32_t        extPoolReg;
-    uint8_t         tmpPool;
-    uint8_t         i;
 
     SANITY_CHECK_RETURN_VALUE(p_FmPort, E_INVALID_HANDLE, 0);
     SANITY_CHECK_RETURN_ERROR(!p_FmPort->p_FmPortDriverParam, E_INVALID_STATE);
 
     if ((p_FmPort->portType != e_FM_PORT_TYPE_RX) && (p_FmPort->portType == e_FM_PORT_TYPE_RX_10G))
     {
-        REPORT_ERROR(MAJOR, E_INVALID_STATE, ("Requested counter is not available for non-Rx ports"));
+        REPORT_ERROR(MINOR, E_INVALID_STATE, ("Requested counter is not available for non-Rx ports"));
         return 0;
     }
-
-    for (i=0;i<FM_PORT_MAX_NUM_OF_EXT_POOLS;i++)
-    {
-        extPoolReg = GET_UINT32(p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_ebmpi[i]);
-        if (extPoolReg & BMI_EXT_BUF_POOL_VALID)
-        {
-            tmpPool = (uint8_t)((extPoolReg & BMI_EXT_BUF_POOL_ID_MASK) >> BMI_EXT_BUF_POOL_ID_SHIFT);
-            if (tmpPool == poolId)
-            {
-                if (extPoolReg & BMI_EXT_BUF_POOL_EN_COUNTER)
-                    return  GET_UINT32(p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_acnt[i]);
-                else
-                {
-                    REPORT_ERROR(MAJOR, E_INVALID_STATE, ("Requested counter is not enabled"));
-                    return 0;
-                }
-            }
-        }
-    }
-    REPORT_ERROR(MAJOR, E_INVALID_STATE, ("Pool %d is not used", poolId));
-    return 0;
+    return fman_port_get_bpool_counter(&p_FmPort->port, poolId);
 }
 
 t_Error FM_PORT_ModifyAllocBufCounter(t_Handle h_FmPort, uint8_t poolId, uint32_t value)
 {
     t_FmPort        *p_FmPort = (t_FmPort *)h_FmPort;
-    uint32_t        extPoolReg;
-    uint8_t         tmpPool;
-    uint8_t         i;
 
     SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
     SANITY_CHECK_RETURN_ERROR(!p_FmPort->p_FmPortDriverParam, E_INVALID_STATE);
 
     if ((p_FmPort->portType != e_FM_PORT_TYPE_RX) && (p_FmPort->portType == e_FM_PORT_TYPE_RX_10G))
-        RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Requested counter is not available for non-Rx ports"));
-
+        RETURN_ERROR(MINOR, E_INVALID_STATE, ("Requested counter is not available for non-Rx ports"));
 
-    for (i=0;i<FM_PORT_MAX_NUM_OF_EXT_POOLS;i++)
-    {
-        extPoolReg = GET_UINT32(p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_ebmpi[i]);
-        if (extPoolReg & BMI_EXT_BUF_POOL_VALID)
-        {
-            tmpPool = (uint8_t)((extPoolReg & BMI_EXT_BUF_POOL_ID_MASK) >> BMI_EXT_BUF_POOL_ID_SHIFT);
-            if (tmpPool == poolId)
-            {
-                if (extPoolReg & BMI_EXT_BUF_POOL_EN_COUNTER)
-                {
-                    WRITE_UINT32(p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_acnt[i], value);
-                    return E_OK;
-                }
-                else
-                    RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Requested counter is not enabled"));
-            }
-        }
-    }
-    RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Pool %d is not used", poolId));
+    fman_port_set_bpool_counter(&p_FmPort->port, poolId, value);
+    return E_OK;
 }
-
 bool FM_PORT_IsStalled(t_Handle h_FmPort)
 {
     t_FmPort    *p_FmPort = (t_FmPort*)h_FmPort;
@@ -4452,7 +3792,7 @@ t_Error FM_PORT_ReleaseStalled(t_Handle h_FmPort)
 t_Error FM_PORT_SetRxL4ChecksumVerify(t_Handle h_FmPort, bool l4Checksum)
 {
     t_FmPort *p_FmPort = (t_FmPort*)h_FmPort;
-    uint32_t tmpReg;
+    int err;
 
     SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
     SANITY_CHECK_RETURN_ERROR(!p_FmPort->p_FmPortDriverParam, E_INVALID_STATE);
@@ -4461,12 +3801,16 @@ t_Error FM_PORT_SetRxL4ChecksumVerify(t_Handle h_FmPort, bool l4Checksum)
         (p_FmPort->portType != e_FM_PORT_TYPE_RX))
         RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("available for Rx ports only"));
 
-    tmpReg = GET_UINT32(p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rfne);
     if (l4Checksum)
-        tmpReg &= ~BMI_PORT_RFNE_FRWD_DCL4C;
+        err = fman_port_modify_rx_fd_bits(&p_FmPort->port,
+                                          (uint8_t)(BMI_PORT_RFNE_FRWD_DCL4C >> 24),
+                                          TRUE);
     else
-        tmpReg |= BMI_PORT_RFNE_FRWD_DCL4C;
-    WRITE_UINT32(p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rfne, tmpReg);
+        err = fman_port_modify_rx_fd_bits(&p_FmPort->port,
+                                          (uint8_t)(BMI_PORT_RFNE_FRWD_DCL4C >> 24),
+                                          FALSE);
+    if (err != 0)
+        RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("fman_port_modify_rx_fd_bits"));
 
     return E_OK;
 }
@@ -4555,7 +3899,7 @@ t_Error FM_PORT_VSPAlloc(t_Handle h_FmPort, t_FmPortVSPAllocParams *p_VSPParams)
 
     p_FmPort->vspe = TRUE;
     p_FmPort->dfltRelativeId = p_VSPParams->dfltRelativeId;
- 
+
     tmpReg = GET_UINT32(*p_BmiStorageProfileId) & ~BMI_SP_ID_MASK;
     tmpReg |= (uint32_t)hwStoragePrflId<<BMI_SP_ID_SHIFT;
     WRITE_UINT32(*p_BmiStorageProfileId, tmpReg);
@@ -4632,10 +3976,10 @@ t_Error FM_PORT_PcdKgModifyInitialScheme (t_Handle h_FmPort, t_FmPcdKgSchemeSele
     {
         case (e_FM_PORT_TYPE_RX_10G):
         case (e_FM_PORT_TYPE_RX):
-            p_BmiHpnia = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rfpne;
+            p_BmiHpnia = &p_FmPort->port.bmi_regs->rx.fmbm_rfpne;
             break;
         case (e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
-            p_BmiHpnia = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ofpne;
+            p_BmiHpnia = &p_FmPort->port.bmi_regs->oh.fmbm_ofpne;
             break;
         default:
             RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("available for Rx and offline parsing ports only"));
@@ -4701,13 +4045,13 @@ t_Error     FM_PORT_PcdPlcrModifyInitialProfile (t_Handle h_FmPort, t_Handle h_P
     {
         case (e_FM_PORT_TYPE_RX_10G):
         case (e_FM_PORT_TYPE_RX):
-            p_BmiNia = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rfne;
-            p_BmiHpnia = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rfpne;
+            p_BmiNia = &p_FmPort->port.bmi_regs->rx.fmbm_rfne;
+            p_BmiHpnia = &p_FmPort->port.bmi_regs->rx.fmbm_rfpne;
             tmpReg = GET_UINT32(*p_BmiNia) & BMI_RFNE_FDCS_MASK;
             break;
         case (e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
-            p_BmiNia = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ofne;
-            p_BmiHpnia = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ofpne;
+            p_BmiNia = &p_FmPort->port.bmi_regs->oh.fmbm_ofne;
+            p_BmiHpnia = &p_FmPort->port.bmi_regs->oh.fmbm_ofpne;
             tmpReg = 0;
             break;
         default:
@@ -4763,10 +4107,10 @@ t_Error FM_PORT_PcdCcModifyTree (t_Handle h_FmPort, t_Handle h_CcTree)
     {
         case (e_FM_PORT_TYPE_RX_10G):
         case (e_FM_PORT_TYPE_RX):
-            p_BmiNia = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rfne;
+            p_BmiNia = &p_FmPort->port.bmi_regs->rx.fmbm_rfne;
             break;
         case (e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
-            p_BmiNia = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ofne;
+            p_BmiNia = &p_FmPort->port.bmi_regs->oh.fmbm_ofne;
             break;
         default:
             RETURN_ERROR(MAJOR, E_INVALID_OPERATION, ("available for Rx and offline parsing ports only"));
@@ -4794,10 +4138,10 @@ t_Error FM_PORT_PcdCcModifyTree (t_Handle h_FmPort, t_Handle h_CcTree)
         {
             case (e_FM_PORT_TYPE_RX_10G):
             case (e_FM_PORT_TYPE_RX):
-                p_BmiCcBase = &p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rccb;
+                p_BmiCcBase = &p_FmPort->port.bmi_regs->rx.fmbm_rccb;
                 break;
             case (e_FM_PORT_TYPE_OH_OFFLINE_PARSING):
-                p_BmiCcBase = &p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_occb;
+                p_BmiCcBase = &p_FmPort->port.bmi_regs->oh.fmbm_occb;
                 break;
             default:
                 RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Invalid port type"));
@@ -5176,7 +4520,7 @@ t_Error FM_PORT_SetPCD(t_Handle h_FmPort, t_FmPortPcdParams *p_PcdParam)
             RELEASE_LOCK(p_FmPort->lock);
             RETURN_ERROR(MAJOR, err, NO_MSG);
          }
- 
+
         FmPortSetGprFunc(p_FmPort, e_FM_PORT_GPR_MURAM_PAGE, (void**)&p_ParamsPage);
         ASSERT_COND(p_ParamsPage);
 
@@ -5695,14 +5039,12 @@ t_Error FM_PORT_DumpRegs(t_Handle h_FmPort)
 t_Error FM_PORT_AddCongestionGrps(t_Handle h_FmPort, t_FmPortCongestionGrps *p_CongestionGrps)
 {
     t_FmPort            *p_FmPort = (t_FmPort*)h_FmPort;
-    bool                tmpArray[FM_PORT_NUM_OF_CONGESTION_GRPS], opPort;
     uint8_t             priorityTmpArray[FM_PORT_NUM_OF_CONGESTION_GRPS];
-    int                 i;
-    uint8_t             mod;
-    uint32_t            tmpReg = 0;
+    uint8_t             mod, index;
+    uint32_t            i, grpsMap[FMAN_PORT_CG_MAP_NUM];
+    int                 err;
 #if (DPAA_VERSION >= 11)
     int                 j;
-    t_Error             err;
 #endif /* (DPAA_VERSION >= 11) */
 
     SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
@@ -5729,19 +5071,22 @@ t_Error FM_PORT_AddCongestionGrps(t_Handle h_FmPort, t_FmPortCongestionGrps *p_C
         (p_FmPort->portType != e_FM_PORT_TYPE_OH_OFFLINE_PARSING))
         RETURN_ERROR(MAJOR, E_NOT_SUPPORTED, ("Available for Rx & OP ports only"));
 
-    opPort = (bool)((p_FmPort->portType == e_FM_PORT_TYPE_OH_OFFLINE_PARSING) ? TRUE:FALSE);
-
-    /* to minimize memory access (groups may belong to the same regsiter, and may
-       be out of order), we first collect all information into a 256 booleans array,
-       representing each possible group. */
+    /* Prepare groups map array */
+    memset(grpsMap, 0, FMAN_PORT_CG_MAP_NUM * sizeof(uint32_t));
+    for (i = 0; i < p_CongestionGrps->numOfCongestionGrpsToConsider; i++)
+    {
+        index = (uint8_t)(p_CongestionGrps->congestionGrpsToConsider[i] / 32);
+        mod = (uint8_t)(p_CongestionGrps->congestionGrpsToConsider[i] % 32);
+        if (p_FmPort->fmRevInfo.majorRev != 4)
+            grpsMap[7 - index] |= (uint32_t)(1 << mod);
+        else
+            grpsMap[0] |= (uint32_t)(1 << mod);
+    }
 
-    memset(&tmpArray, 0, FM_PORT_NUM_OF_CONGESTION_GRPS*sizeof(bool));
     memset(&priorityTmpArray, 0, FM_PORT_NUM_OF_CONGESTION_GRPS*sizeof(uint8_t));
 
     for (i=0; i<p_CongestionGrps->numOfCongestionGrpsToConsider; i++)
     {
-        tmpArray[p_CongestionGrps->congestionGrpsToConsider[i]] = TRUE;
-
 #if (DPAA_VERSION >= 11)
         for (j=0;j<FM_MAX_NUM_OF_PFC_PRIORITIES;j++)
             if (p_CongestionGrps->pfcPrioritiesEn[i][j])
@@ -5749,35 +5094,18 @@ t_Error FM_PORT_AddCongestionGrps(t_Handle h_FmPort, t_FmPortCongestionGrps *p_C
 #endif /* (DPAA_VERSION >= 11) */
     }
 
+#if (DPAA_VERSION >= 11)
     for (i=0; i<FM_PORT_NUM_OF_CONGESTION_GRPS; i++)
     {
-        mod = (uint8_t)(i%32);
-        /* each 32 congestion groups are represented by a register */
-        if (mod == 0) /* first in a 32 bunch of congestion groups, get the currest register state  */
-            tmpReg = opPort ? GET_UINT32(p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ocgm):
-                              GET_UINT32(p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rcgm[FM_PORT_CG_REG_NUM(i)]);
-
-        /* set in the register, the bit representing the relevant congestion group. */
-
-        if (tmpArray[i])
-        {
-            tmpReg |=  (0x00000001 << (uint32_t)mod);
-
-#if (DPAA_VERSION >= 11)
             err = FmSetCongestionGroupPFCpriority(p_FmPort->h_Fm,i,priorityTmpArray[i]);
             if (err)
                 return err;
+     }
 #endif /* (DPAA_VERSION >= 11) */
-        }
 
-        if (mod == 31) /* last in a 32 bunch of congestion groups - write the corresponding register */
-        {
-            if (opPort)
-                WRITE_UINT32(p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ocgm, tmpReg);
-            else
-                WRITE_UINT32(p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rcgm[FM_PORT_CG_REG_NUM(i)], tmpReg);
-        }
-    }
+    err = fman_port_add_congestion_grps(&p_FmPort->port, grpsMap);
+    if (err != 0)
+        RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("fman_port_add_congestion_grps"));
 
     return  E_OK;
 }
@@ -5785,77 +5113,73 @@ t_Error FM_PORT_AddCongestionGrps(t_Handle h_FmPort, t_FmPortCongestionGrps *p_C
 t_Error FM_PORT_RemoveCongestionGrps(t_Handle h_FmPort, t_FmPortCongestionGrps *p_CongestionGrps)
 {
     t_FmPort            *p_FmPort = (t_FmPort*)h_FmPort;
-    bool                tmpArray[FM_PORT_NUM_OF_CONGESTION_GRPS], opPort;
-    int                 i;
-    uint8_t             mod;
-    uint32_t            tmpReg = 0;
+    uint8_t             mod, index;
+    uint32_t            i, grpsMap[FMAN_PORT_CG_MAP_NUM];
+    int                 err;
 
-    SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
 
-    /* un-necessary check of the indexes; probably will be needed in the future when there
-       will be more CGs available ....
-    for (i=0; i<p_CongestionGrps->numOfCongestionGrpsToConsider; i++)
-        if (p_CongestionGrps->congestionGrpsToConsider[i] >= FM_PORT_NUM_OF_CONGESTION_GRPS)
-            RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("CG id!"));
-    */
+    SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
 
-#ifdef FM_NO_OP_OBSERVED_CGS
-    if ((p_FmPort->fmRevInfo.majorRev != 4) &&
-        (p_FmPort->fmRevInfo.majorRev < 6))
     {
+#ifdef FM_NO_OP_OBSERVED_CGS
+        t_FmRevisionInfo    revInfo;
+
+        FM_GetRevision(p_FmPort->h_Fm, &revInfo);
+        if (revInfo.majorRev != 4)
+        {
+            if ((p_FmPort->portType != e_FM_PORT_TYPE_RX_10G) &&
+               (p_FmPort->portType != e_FM_PORT_TYPE_RX))
+                RETURN_ERROR(MAJOR, E_NOT_SUPPORTED, ("Available for Rx ports only"));
+        }
+        else
+#endif /* FM_NO_OP_OBSERVED_CGS */
         if ((p_FmPort->portType != e_FM_PORT_TYPE_RX_10G) &&
-                (p_FmPort->portType != e_FM_PORT_TYPE_RX))
-            RETURN_ERROR(MAJOR, E_NOT_SUPPORTED, ("Available for Rx ports only"));
+           (p_FmPort->portType != e_FM_PORT_TYPE_RX) &&
+           (p_FmPort->portType != e_FM_PORT_TYPE_OH_OFFLINE_PARSING))
+            RETURN_ERROR(MAJOR, E_NOT_SUPPORTED, ("Available for Rx & OP ports only"));
     }
-    else
-#endif /* FM_NO_OP_OBSERVED_CGS */
-    if ((p_FmPort->portType != e_FM_PORT_TYPE_RX_10G) &&
-        (p_FmPort->portType != e_FM_PORT_TYPE_RX) &&
-        (p_FmPort->portType != e_FM_PORT_TYPE_OH_OFFLINE_PARSING))
-        RETURN_ERROR(MAJOR, E_NOT_SUPPORTED, ("Available for Rx & OP ports only"));
-
-    opPort = (bool)((p_FmPort->portType == e_FM_PORT_TYPE_OH_OFFLINE_PARSING) ? TRUE:FALSE);
 
-    /* to minimize memory access (groups may belong to the same regsiter, and may
-    be out of order), we first collect all information into a 256 booleans array,
-    representing each possible group. */
-    memset(&tmpArray, 0, FM_PORT_NUM_OF_CONGESTION_GRPS*sizeof(bool));
-    for (i=0; i<p_CongestionGrps->numOfCongestionGrpsToConsider; i++)
-        tmpArray[p_CongestionGrps->congestionGrpsToConsider[i]] = TRUE;
-
-    for (i=0; i<FM_PORT_NUM_OF_CONGESTION_GRPS; i++)
+    /* Prepare groups map array */
+    memset(grpsMap, 0, FMAN_PORT_CG_MAP_NUM * sizeof(uint32_t));
+    for (i = 0; i < p_CongestionGrps->numOfCongestionGrpsToConsider; i++)
     {
-        mod = (uint8_t)(i%32);
-        /* each 32 congestion groups are represented by a register */
-        if (mod == 0) /* first in a 32 bunch of congestion groups, get the currest register state  */
-            tmpReg = opPort ?   GET_UINT32(p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ocgm):
-                                GET_UINT32(p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rcgm[FM_PORT_CG_REG_NUM(i)]);
-
-        /* set in the register, the bit representing the relevant congestion group. */
-        if (tmpArray[i])
-        {
-            tmpReg &=  ~(0x00000001 << (uint32_t)mod);
+        index = (uint8_t)(p_CongestionGrps->congestionGrpsToConsider[i] / 32);
+        mod = (uint8_t)(p_CongestionGrps->congestionGrpsToConsider[i] % 32);
+        if (p_FmPort->fmRevInfo.majorRev != 4)
+            grpsMap[7 - index] |= (uint32_t)(1 << mod);
+        else
+            grpsMap[0] |= (uint32_t)(1 << mod);
+    }
 
 #if (DPAA_VERSION >= 11)
-            {
-                t_Error err = FmSetCongestionGroupPFCpriority(p_FmPort->h_Fm, i, 0);
-                if (err)
-                    return err;
-            }
-#endif /* (DPAA_VERSION >= 11) */
-        }
-        if (mod == 31) /* last in a 32 bunch of congestion groups - write the corresponding register */
-        {
-            if (opPort)
-                WRITE_UINT32(p_FmPort->p_FmPortBmiRegs->ohPortBmiRegs.fmbm_ocgm, tmpReg);
-            else
-                WRITE_UINT32(p_FmPort->p_FmPortBmiRegs->rxPortBmiRegs.fmbm_rcgm[FM_PORT_CG_REG_NUM(i)], tmpReg);
-        }
+    for (i=0; i<p_CongestionGrps->numOfCongestionGrpsToConsider; i++)
+    {
+    t_Error err = FmSetCongestionGroupPFCpriority(p_FmPort->h_Fm,
+        p_CongestionGrps->congestionGrpsToConsider[i], 0);
+    if (err)
+        return err;
     }
+#endif /* (DPAA_VERSION >= 11) */
 
+    err = fman_port_remove_congestion_grps(&p_FmPort->port, grpsMap);
+    if (err != 0)
+        RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("fman_port_remove_congestion_grps"));
     return  E_OK;
 }
 
 #if (DPAA_VERSION >= 11)
+t_Error FM_PORT_GetIPv4OptionsCount(t_Handle h_FmPort, uint32_t *p_Ipv4OptionsCount)
+{
+    t_FmPort    *p_FmPort = (t_FmPort*)h_FmPort;
+
+    SANITY_CHECK_RETURN_ERROR(p_FmPort, E_INVALID_HANDLE);
+    SANITY_CHECK_RETURN_ERROR((p_FmPort->portType == e_FM_PORT_TYPE_OH_OFFLINE_PARSING), E_INVALID_VALUE);
+    SANITY_CHECK_RETURN_ERROR(p_FmPort->p_ParamsPage, E_INVALID_STATE);
+    SANITY_CHECK_RETURN_ERROR(p_Ipv4OptionsCount, E_NULL_POINTER);
+
+    *p_Ipv4OptionsCount = GET_UINT32(p_FmPort->p_ParamsPage->ipfOptionsCounter);
+
+    return E_OK;
+}
 #endif /* (DPAA_VERSION >= 11) */
 
diff --git a/drivers/net/ethernet/freescale/fman/Peripherals/FM/Port/fm_port.h b/drivers/net/ethernet/freescale/fman/Peripherals/FM/Port/fm_port.h
index 12be3c2..d4cc285 100644
--- a/drivers/net/ethernet/freescale/fman/Peripherals/FM/Port/fm_port.h
+++ b/drivers/net/ethernet/freescale/fman/Peripherals/FM/Port/fm_port.h
@@ -47,6 +47,7 @@
 #include "fm_sp_common.h"
 #include "fsl_fman_sp.h"
 
+#include "fsl_fman_port.h"
 
 #define __ERR_MODULE__  MODULE_FM_PORT
 
@@ -54,6 +55,7 @@
 #define MIN_EXT_BUF_SIZE                                64
 #define DATA_ALIGNMENT                                  64
 #define MAX_LIODN_OFFSET                                64
+#define MAX_PORT_FIFO_SIZE                              MIN(BMI_MAX_FIFO_SIZE, 1024*BMI_FIFO_UNITS)
 
 /**************************************************************************//**
  @Description       Memory Map defines
@@ -105,8 +107,8 @@
 #define DEFAULT_notSupported                            0xff
 
 #if (DPAA_VERSION < 11)
-#define DEFAULT_PORT_rxFifoPriElevationLevel            BMI_MAX_FIFO_SIZE
-#define DEFAULT_PORT_rxFifoThreshold                    (BMI_MAX_FIFO_SIZE*3/4)
+#define DEFAULT_PORT_rxFifoPriElevationLevel            MAX_PORT_FIFO_SIZE
+#define DEFAULT_PORT_rxFifoThreshold                    (MAX_PORT_FIFO_SIZE*3/4)
 
 #define DEFAULT_PORT_txFifoMinFillLevel                 0
 #define DEFAULT_PORT_txFifoLowComfLevel                 (5*KILOBYTE)
@@ -124,26 +126,22 @@
                  ((type) == e_FM_PORT_TYPE_OH_OFFLINE_PARSING)) ? 3 : 1))
 
 #define DEFAULT_PORT_extraNumOfTasks(type)                  \
-    (uint32_t)((((type) == e_FM_PORT_TYPE_RX_10G) ||        \
-                ((type) == e_FM_PORT_TYPE_TX_10G)) ? 8 :    \
-               ((((type) == e_FM_PORT_TYPE_RX) ||           \
-                 ((type) == e_FM_PORT_TYPE_TX)) ? 2 : 0))
+    (uint32_t)(((type) == e_FM_PORT_TYPE_RX_10G)  ? 8 :    \
+               (((type) == e_FM_PORT_TYPE_RX) ? 2 : 0))
 
 #define DEFAULT_PORT_numOfOpenDmas(type)                    \
     (uint32_t)((((type) == e_FM_PORT_TYPE_TX_10G) ||        \
                 ((type) == e_FM_PORT_TYPE_RX_10G)) ? 8 : 1 )
 
 #define DEFAULT_PORT_extraNumOfOpenDmas(type)               \
-    (uint32_t)((((type) == e_FM_PORT_TYPE_RX_10G) ||        \
-                ((type) == e_FM_PORT_TYPE_TX_10G)) ? 8 :    \
-               ((((type) == e_FM_PORT_TYPE_RX) ||           \
-                 ((type) == e_FM_PORT_TYPE_TX)) ? 1 : 0))
+    (uint32_t)(((type) == e_FM_PORT_TYPE_RX_10G) ? 8 :    \
+               (((type) == e_FM_PORT_TYPE_RX) ? 1 : 0))
 
 #define DEFAULT_PORT_numOfFifoBufs(type)                    \
     (uint32_t)((((type) == e_FM_PORT_TYPE_RX_10G) ||        \
                 ((type) == e_FM_PORT_TYPE_TX_10G)) ? 48 :   \
-               ((((type) == e_FM_PORT_TYPE_RX) ||           \
-                 ((type) == e_FM_PORT_TYPE_TX)) ? 44 : 8))
+                ((type) == e_FM_PORT_TYPE_RX) ? 45 :        \
+                ((type) == e_FM_PORT_TYPE_TX) ? 44 : 8)
 
 #define DEFAULT_PORT_extraNumOfFifoBufs             0
 
@@ -161,7 +159,7 @@
 
 #define DEFAULT_PORT_numOfTasks(type)                       \
     (uint32_t)((((type) == e_FM_PORT_TYPE_RX_10G) ||        \
-                ((type) == e_FM_PORT_TYPE_TX_10G)) ? 14 :  \
+                ((type) == e_FM_PORT_TYPE_TX_10G)) ? 14 :   \
                (((type) == e_FM_PORT_TYPE_RX) ||            \
                  ((type) == e_FM_PORT_TYPE_TX)) ? 4 :       \
                  ((type) == e_FM_PORT_TYPE_OH_OFFLINE_PARSING) ? 6 : 1)
@@ -170,17 +168,17 @@
 
 #define DEFAULT_PORT_numOfOpenDmas(type)                    \
     (uint32_t)(((type) == e_FM_PORT_TYPE_RX_10G) ? 8 :      \
-               ((type) == e_FM_PORT_TYPE_TX_10G) ? 12 :    \
+               ((type) == e_FM_PORT_TYPE_TX_10G) ? 12 :     \
                ((type) == e_FM_PORT_TYPE_RX)     ? 2 :      \
                ((type) == e_FM_PORT_TYPE_TX)     ? 3 :      \
-               ((type) == e_FM_PORT_TYPE_OH_HOST_COMMAND) ? 2 : 4 )
+               ((type) == e_FM_PORT_TYPE_OH_HOST_COMMAND) ? 2 : 4)
 
 #define DEFAULT_PORT_extraNumOfOpenDmas(type)       0
 
 #define DEFAULT_PORT_numOfFifoBufs(type)                   \
     (uint32_t) (((type) == e_FM_PORT_TYPE_RX_10G) ? 96 : \
                 ((type) == e_FM_PORT_TYPE_TX_10G) ? 64 : \
-                ((type) == e_FM_PORT_TYPE_OH_HOST_COMMAND) ? 10 : 50 )
+                ((type) == e_FM_PORT_TYPE_OH_HOST_COMMAND) ? 10 : 50)
 
 #define DEFAULT_PORT_extraNumOfFifoBufs             0
 
@@ -491,7 +489,6 @@ typedef _Packed struct
 #define BMI_SP_ID_MASK                          0xff000000
 #define BMI_SP_ID_SHIFT                         24
 #define BMI_SP_EN                               0x01000000
-#define BMI_EBD_EN                              0x80000000
 #endif /* (DPAA_VERSION >= 11) */
 
 #define BMI_PORT_CFG_EN                         0x80000000
@@ -500,7 +497,6 @@ typedef _Packed struct
 #define BMI_PORT_CFG_IM                         0x01000000
 #define BMI_PORT_STATUS_BSY                     0x80000000
 #define BMI_COUNTERS_EN                         0x80000000
-#define BMI_DMA_ATTR_WRITE_OPTIMIZE             FMAN_SP_DMA_ATTR_WRITE_OPTIMIZE
 
 #define BMI_PORT_RFNE_FRWD_DCL4C                0x10000000
 #define BMI_PORT_RFNE_FRWD_RPD                  0x40000000
@@ -510,7 +506,6 @@ typedef _Packed struct
 #define BMI_CMD_MR_SLEAC                        0x00100000
 #define BMI_CMD_MR_MA                           0x00080000
 #define BMI_CMD_MR_DEAS                         0x00040000
-#define BMI_CMD_TX_MR_DEF                       (0)
 #define BMI_CMD_RX_MR_DEF                       (BMI_CMD_MR_LEAC | \
                                                  BMI_CMD_MR_SLEAC | \
                                                  BMI_CMD_MR_MA | \
@@ -523,11 +518,6 @@ typedef _Packed struct
 #define BMI_CMD_ATTR_MACCMD_SECURED             0x00001000
 #define BMI_CMD_ATTR_MACCMD_SC_MASK             0x00000f00
 
-#define BMI_EXT_BUF_POOL_EN_COUNTER             FMAN_SP_EXT_BUF_POOL_EN_COUNTER
-#define BMI_EXT_BUF_POOL_VALID                  FMAN_SP_EXT_BUF_POOL_VALID
-
-#define BMI_EXT_BUF_POOL_BACKUP                 FMAN_SP_EXT_BUF_POOL_BACKUP
-
 #define BMI_EXT_BUF_POOL_ID_MASK                0x003F0000
 #define BMI_STATUS_RX_MASK_UNUSED               (uint32_t)(~(FM_PORT_FRM_ERR_DMA                    | \
                                                              FM_PORT_FRM_ERR_PHYSICAL               | \
@@ -592,7 +582,6 @@ typedef _Packed struct
 
 /* shifts */
 #define BMI_PORT_CFG_MS_SEL_SHIFT               16
-#define BMI_DMA_ATTR_SWP_SHIFT                  FMAN_SP_DMA_ATTR_SWP_SHIFT
 #define BMI_DMA_ATTR_IC_CACHE_SHIFT             FMAN_SP_DMA_ATTR_IC_CACHE_SHIFT
 #define BMI_DMA_ATTR_HDR_CACHE_SHIFT            FMAN_SP_DMA_ATTR_HDR_CACHE_SHIFT
 #define BMI_DMA_ATTR_SG_CACHE_SHIFT             FMAN_SP_DMA_ATTR_SG_CACHE_SHIFT
@@ -606,14 +595,10 @@ typedef _Packed struct
 #define BMI_RX_FRAME_END_CS_IGNORE_SHIFT        24
 #define BMI_RX_FRAME_END_CUT_SHIFT              16
 
-#define BMI_IC_TO_EXT_SHIFT                     FMAN_SP_IC_TO_EXT_SHIFT
-#define BMI_IC_FROM_INT_SHIFT                   FMAN_SP_IC_FROM_INT_SHIFT
 #define BMI_IC_SIZE_SHIFT                       FMAN_SP_IC_SIZE_SHIFT
 
 #define BMI_INT_BUF_MARG_SHIFT                  28
 
-#define BMI_EXT_BUF_MARG_START_SHIFT            FMAN_SP_EXT_BUF_MARG_START_SHIFT
-#define BMI_SG_DISABLE                          FMAN_SP_SG_DISABLE
 #define BMI_EXT_BUF_MARG_END_SHIFT              FMAN_SP_EXT_BUF_MARG_END_SHIFT
 
 #define BMI_CMD_ATTR_COLOR_SHIFT                26
@@ -623,16 +608,11 @@ typedef _Packed struct
 #define BMI_CMD_ATTR_MACCMD_SECURED_SHIFT       12
 #define BMI_CMD_ATTR_MACCMD_SC_SHIFT            8
 
-#define BMI_POOL_DEP_NUM_OF_POOLS_SHIFT         FMAN_SP_POOL_DEP_NUM_OF_POOLS_SHIFT
 #define BMI_POOL_DEP_NUM_OF_POOLS_VECTOR_SHIFT  24
 
-#define BMI_EXT_BUF_POOL_ID_SHIFT               FMAN_SP_EXT_BUF_POOL_ID_SHIFT
 #define BMI_TX_FIFO_MIN_FILL_SHIFT              16
-#define BMI_FIFO_PIPELINE_DEPTH_SHIFT           12
 #define BMI_TX_LOW_COMF_SHIFT                   0
 
-#define BMI_FRAME_END_CS_IGNORE_SHIFT           24
-
 #define BMI_PERFORMANCE_TASK_COMP_SHIFT         24
 #define BMI_PERFORMANCE_PORT_COMP_SHIFT         16
 #define BMI_PERFORMANCE_DMA_COMP_SHIFT          12
@@ -658,6 +638,7 @@ typedef _Packed struct
 #define MAX_BURST_SIZE                          1024
 #define MIN_NUM_OF_OP_DMAS                      2
 
+
 /**************************************************************************//**
  @Description       QMI defines
 *//***************************************************************************/
@@ -810,6 +791,7 @@ typedef struct {
 
 
 typedef struct {
+    struct fman_port_cfg                dfltCfg;
     uint32_t                            dfltFqid;
     uint32_t                            confFqid;
     uint32_t                            errFqid;
@@ -873,6 +855,7 @@ typedef struct t_FmPortRxPoolsParams
 } t_FmPortRxPoolsParams;
 
 typedef struct {
+    struct fman_port            port;
     t_Handle                    h_Fm;
     t_Handle                    h_FmPcd;
     t_Handle                    h_FmMuram;
@@ -902,7 +885,6 @@ typedef struct {
     /* Independent-Mode parameters support */
     bool                        imEn;
     t_FmMacIm                   im;
-    uint8_t                     fifoDeqPipelineDepth;
     volatile bool               lock;
     t_Handle                    h_Spinlock;
     t_FmPortExceptionCallback   *f_Exception;
diff --git a/drivers/net/ethernet/freescale/fman/Peripherals/FM/Port/fm_port_im.c b/drivers/net/ethernet/freescale/fman/Peripherals/FM/Port/fm_port_im.c
index bf358a5..7f70cc9 100644
--- a/drivers/net/ethernet/freescale/fman/Peripherals/FM/Port/fm_port_im.c
+++ b/drivers/net/ethernet/freescale/fman/Peripherals/FM/Port/fm_port_im.c
@@ -59,7 +59,7 @@ static void ImException(t_Handle h_FmPort, uint32_t event)
 {
     t_FmPort *p_FmPort = (t_FmPort*)h_FmPort;
 
-    ASSERT_COND(((event & IM_EV_RX) && FmIsMaster(p_FmPort->h_Fm)) ||
+    ASSERT_COND(((event & (IM_EV_RX | IM_EV_BSY)) && FmIsMaster(p_FmPort->h_Fm)) ||
                 !FmIsMaster(p_FmPort->h_Fm));
 
     if (event & IM_EV_RX)
diff --git a/drivers/net/ethernet/freescale/fman/Peripherals/FM/Port/fman_port.c b/drivers/net/ethernet/freescale/fman/Peripherals/FM/Port/fman_port.c
new file mode 100755
index 0000000..2e4ff3b
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fman/Peripherals/FM/Port/fman_port.c
@@ -0,0 +1,1570 @@
+/*
+ * Copyright 2008-2012 Freescale Semiconductor Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Freescale Semiconductor nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL Freescale Semiconductor BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+#include "common/general.h"
+
+#include "fman_common.h"
+#include "fsl_fman_port.h"
+
+
+/* problem Eyal: the following should not be here*/
+#define NIA_FM_CTL_AC_NO_IPACC_PRE_BMI_ENQ_FRAME        0x00000028
+
+static uint32_t get_no_pcd_nia_bmi_ac_enc_frame(struct fman_port_cfg *cfg)
+{
+    if (cfg->errata_A006675)
+        return NIA_ENG_FM_CTL |
+            NIA_FM_CTL_AC_NO_IPACC_PRE_BMI_ENQ_FRAME;
+    else
+        return NIA_ENG_BMI | NIA_BMI_AC_ENQ_FRAME;
+}
+
+static int init_bmi_rx(struct fman_port *port,
+        struct fman_port_cfg *cfg,
+        struct fman_port_params *params)
+{
+    struct fman_port_rx_bmi_regs *regs = &port->bmi_regs->rx;
+    uint32_t tmp;
+
+    /* Rx Configuration register */
+    tmp = 0;
+    if (port->im_en)
+        tmp |= BMI_PORT_CFG_IM;
+    else if (cfg->discard_override)
+        tmp |= BMI_PORT_CFG_FDOVR;
+    iowrite32be(tmp, &regs->fmbm_rcfg);
+
+    /* DMA attributes */
+    tmp = (uint32_t)cfg->dma_swap_data << BMI_DMA_ATTR_SWP_SHIFT;
+    if (cfg->dma_ic_stash_on)
+        tmp |= BMI_DMA_ATTR_IC_STASH_ON;
+    if (cfg->dma_header_stash_on)
+        tmp |= BMI_DMA_ATTR_HDR_STASH_ON;
+    if (cfg->dma_sg_stash_on)
+        tmp |= BMI_DMA_ATTR_SG_STASH_ON;
+    if (cfg->dma_write_optimize)
+        tmp |= BMI_DMA_ATTR_WRITE_OPTIMIZE;
+    iowrite32be(tmp, &regs->fmbm_rda);
+
+    /* Rx FIFO parameters */
+    tmp = (cfg->rx_pri_elevation / FMAN_PORT_BMI_FIFO_UNITS - 1) <<
+            BMI_RX_FIFO_PRI_ELEVATION_SHIFT;
+    tmp |= cfg->rx_fifo_thr / FMAN_PORT_BMI_FIFO_UNITS - 1;
+    iowrite32be(tmp, &regs->fmbm_rfp);
+
+    if (cfg->excessive_threshold_register)
+        /* always allow access to the extra resources */
+        iowrite32be(BMI_RX_FIFO_THRESHOLD_ETHE, &regs->fmbm_reth);
+
+    /* Frame end data */
+    tmp = (uint32_t)cfg->checksum_bytes_ignore <<
+            BMI_RX_FRAME_END_CS_IGNORE_SHIFT;
+    tmp |= (uint32_t)cfg->rx_cut_end_bytes <<
+            BMI_RX_FRAME_END_CUT_SHIFT;
+    if (cfg->errata_A006320)
+        tmp &= 0xffe0ffff;
+    iowrite32be(tmp, &regs->fmbm_rfed);
+
+    /* Internal context parameters */
+    tmp = ((uint32_t)cfg->ic_ext_offset / FMAN_PORT_IC_OFFSET_UNITS) <<
+            BMI_IC_TO_EXT_SHIFT;
+    tmp |= ((uint32_t)cfg->ic_int_offset / FMAN_PORT_IC_OFFSET_UNITS) <<
+            BMI_IC_FROM_INT_SHIFT;
+    tmp |= cfg->ic_size / FMAN_PORT_IC_OFFSET_UNITS;
+    iowrite32be(tmp, &regs->fmbm_ricp);
+
+    /* Internal buffer offset */
+    tmp = ((uint32_t)cfg->int_buf_start_margin / FMAN_PORT_IC_OFFSET_UNITS)
+            << BMI_INT_BUF_MARG_SHIFT;
+    iowrite32be(tmp, &regs->fmbm_rim);
+
+    /* External buffer margins */
+    if (!port->im_en)
+    {
+        tmp = (uint32_t)cfg->ext_buf_start_margin <<
+                BMI_EXT_BUF_MARG_START_SHIFT;
+        tmp |= (uint32_t)cfg->ext_buf_end_margin;
+        if (cfg->fmbm_rebm_has_sgd && cfg->no_scatter_gather)
+            tmp |= BMI_SG_DISABLE;
+        iowrite32be(tmp, &regs->fmbm_rebm);
+    }
+
+    /* Frame attributes */
+    tmp = BMI_CMD_RX_MR_DEF;
+    if (!port->im_en)
+    {
+        tmp |= BMI_CMD_ATTR_ORDER;
+        tmp |= (uint32_t)cfg->color << BMI_CMD_ATTR_COLOR_SHIFT;
+        if (cfg->sync_req)
+            tmp |= BMI_CMD_ATTR_SYNC;
+    }
+    iowrite32be(tmp, &regs->fmbm_rfca);
+
+    /* NIA */
+    if (port->im_en)
+        tmp = NIA_ENG_FM_CTL | NIA_FM_CTL_AC_IND_MODE_RX;
+    else
+    {
+        tmp = (uint32_t)cfg->rx_fd_bits << BMI_NEXT_ENG_FD_BITS_SHIFT;
+        tmp |= get_no_pcd_nia_bmi_ac_enc_frame(cfg);
+    }
+    iowrite32be(tmp, &regs->fmbm_rfne);
+
+    /* Enqueue NIA */
+    iowrite32be(NIA_ENG_QMI_ENQ | NIA_ORDER_RESTOR, &regs->fmbm_rfene);
+
+    /* Default/error queues */
+    if (!port->im_en)
+    {
+        iowrite32be((params->dflt_fqid & 0x00FFFFFF), &regs->fmbm_rfqid);
+        iowrite32be((params->err_fqid & 0x00FFFFFF), &regs->fmbm_refqid);
+    }
+
+    /* Discard/error masks */
+    iowrite32be(params->discard_mask, &regs->fmbm_rfsdm);
+    iowrite32be(params->err_mask, &regs->fmbm_rfsem);
+
+    /* Statistics counters */
+    tmp = 0;
+    if (cfg->stats_counters_enable)
+        tmp = BMI_COUNTERS_EN;
+    iowrite32be(tmp, &regs->fmbm_rstc);
+
+    /* Performance counters */
+    fman_port_set_perf_cnt_params(port, &cfg->perf_cnt_params);
+    tmp = 0;
+    if (cfg->perf_counters_enable)
+        tmp = BMI_COUNTERS_EN;
+    iowrite32be(tmp, &regs->fmbm_rpc);
+
+    return 0;
+}
+
+static int init_bmi_tx(struct fman_port *port,
+        struct fman_port_cfg *cfg,
+        struct fman_port_params *params)
+{
+    struct fman_port_tx_bmi_regs *regs = &port->bmi_regs->tx;
+    uint32_t tmp;
+
+    /* Tx Configuration register */
+    tmp = 0;
+    if (port->im_en)
+        tmp |= BMI_PORT_CFG_IM;
+    iowrite32be(tmp, &regs->fmbm_tcfg);
+
+    /* DMA attributes */
+    tmp = (uint32_t)cfg->dma_swap_data << BMI_DMA_ATTR_SWP_SHIFT;
+    if (cfg->dma_ic_stash_on)
+        tmp |= BMI_DMA_ATTR_IC_STASH_ON;
+    if (cfg->dma_header_stash_on)
+        tmp |= BMI_DMA_ATTR_HDR_STASH_ON;
+    if (cfg->dma_sg_stash_on)
+        tmp |= BMI_DMA_ATTR_SG_STASH_ON;
+    iowrite32be(tmp, &regs->fmbm_tda);
+
+    /* Tx FIFO parameters */
+    tmp = (cfg->tx_fifo_min_level / FMAN_PORT_BMI_FIFO_UNITS) <<
+            BMI_TX_FIFO_MIN_FILL_SHIFT;
+    tmp |= ((uint32_t)cfg->tx_fifo_deq_pipeline_depth - 1) <<
+            BMI_FIFO_PIPELINE_DEPTH_SHIFT;
+    tmp |= (uint32_t)(cfg->tx_fifo_low_comf_level /
+            FMAN_PORT_BMI_FIFO_UNITS - 1);
+    iowrite32be(tmp, &regs->fmbm_tfp);
+
+    /* Frame end data */
+    tmp = (uint32_t)cfg->checksum_bytes_ignore <<
+            BMI_FRAME_END_CS_IGNORE_SHIFT;
+    iowrite32be(tmp, &regs->fmbm_tfed);
+
+    /* Internal context parameters */
+    if (!port->im_en)
+    {
+        tmp = ((uint32_t)cfg->ic_ext_offset / FMAN_PORT_IC_OFFSET_UNITS) <<
+                BMI_IC_TO_EXT_SHIFT;
+        tmp |= ((uint32_t)cfg->ic_int_offset / FMAN_PORT_IC_OFFSET_UNITS) <<
+                BMI_IC_FROM_INT_SHIFT;
+        tmp |= cfg->ic_size / FMAN_PORT_IC_OFFSET_UNITS;
+        iowrite32be(tmp, &regs->fmbm_ticp);
+    }
+    /* Frame attributes */
+    tmp = BMI_CMD_TX_MR_DEF;
+    if (port->im_en)
+        tmp |= BMI_CMD_MR_DEAS;
+    else
+    {
+        tmp |= BMI_CMD_ATTR_ORDER;
+        tmp |= (uint32_t)cfg->color << BMI_CMD_ATTR_COLOR_SHIFT;
+    }
+    iowrite32be(tmp, &regs->fmbm_tfca);
+
+    /* Dequeue NIA + enqueue NIA */
+    if (port->im_en)
+    {
+        iowrite32be(NIA_ENG_FM_CTL | NIA_FM_CTL_AC_IND_MODE_TX, &regs->fmbm_tfdne);
+        iowrite32be(NIA_ENG_FM_CTL | NIA_FM_CTL_AC_IND_MODE_TX, &regs->fmbm_tfene);
+    }
+    else
+    {
+        iowrite32be(NIA_ENG_QMI_DEQ, &regs->fmbm_tfdne);
+        iowrite32be(NIA_ENG_QMI_ENQ | NIA_ORDER_RESTOR, &regs->fmbm_tfene);
+        if (cfg->fmbm_tfne_has_features)
+            iowrite32be(!params->dflt_fqid ?
+                BMI_EBD_EN | NIA_BMI_AC_FETCH_ALL_FRAME :
+                NIA_BMI_AC_FETCH_ALL_FRAME, &regs->fmbm_tfne);
+        if (!params->dflt_fqid && params->dont_release_buf)
+        {
+            iowrite32be(0x00FFFFFF, &regs->fmbm_tcfqid);
+            iowrite32be(NIA_ENG_BMI | NIA_BMI_AC_TX_RELEASE, &regs->fmbm_tfene);
+            if (cfg->fmbm_tfne_has_features)
+                iowrite32be(ioread32be(&regs->fmbm_tfne) & ~BMI_EBD_EN, &regs->fmbm_tfne);
+        }
+    }
+
+    /* Confirmation/error queues */
+    if (!port->im_en)
+    {
+        if (params->dflt_fqid || !params->dont_release_buf)
+            iowrite32be(params->dflt_fqid & 0x00FFFFFF, &regs->fmbm_tcfqid);
+        iowrite32be((params->err_fqid & 0x00FFFFFF), &regs->fmbm_tefqid);
+    }
+    /* Statistics counters */
+    tmp = 0;
+    if (cfg->stats_counters_enable)
+        tmp = BMI_COUNTERS_EN;
+    iowrite32be(tmp, &regs->fmbm_tstc);
+
+    /* Performance counters */
+    fman_port_set_perf_cnt_params(port, &cfg->perf_cnt_params);
+    tmp = 0;
+    if (cfg->perf_counters_enable)
+        tmp = BMI_COUNTERS_EN;
+    iowrite32be(tmp, &regs->fmbm_tpc);
+
+    return 0;
+}
+
+static int init_bmi_oh(struct fman_port *port,
+        struct fman_port_cfg *cfg,
+        struct fman_port_params *params)
+{
+    struct fman_port_oh_bmi_regs *regs = &port->bmi_regs->oh;
+    uint32_t tmp;
+
+    /* OP Configuration register */
+    tmp = 0;
+    if (cfg->discard_override)
+        tmp |= BMI_PORT_CFG_FDOVR;
+    iowrite32be(tmp, &regs->fmbm_ocfg);
+
+    /* DMA attributes */
+    tmp = (uint32_t)cfg->dma_swap_data << BMI_DMA_ATTR_SWP_SHIFT;
+    if (cfg->dma_ic_stash_on)
+        tmp |= BMI_DMA_ATTR_IC_STASH_ON;
+    if (cfg->dma_header_stash_on)
+        tmp |= BMI_DMA_ATTR_HDR_STASH_ON;
+    if (cfg->dma_sg_stash_on)
+        tmp |= BMI_DMA_ATTR_SG_STASH_ON;
+    if (cfg->dma_write_optimize)
+        tmp |= BMI_DMA_ATTR_WRITE_OPTIMIZE;
+    iowrite32be(tmp, &regs->fmbm_oda);
+
+    /* Tx FIFO parameters */
+    tmp = ((uint32_t)cfg->tx_fifo_deq_pipeline_depth - 1) <<
+            BMI_FIFO_PIPELINE_DEPTH_SHIFT;
+    iowrite32be(tmp, &regs->fmbm_ofp);
+
+    /* Internal context parameters */
+    tmp = ((uint32_t)cfg->ic_ext_offset / FMAN_PORT_IC_OFFSET_UNITS) <<
+            BMI_IC_TO_EXT_SHIFT;
+    tmp |= ((uint32_t)cfg->ic_int_offset / FMAN_PORT_IC_OFFSET_UNITS) <<
+            BMI_IC_FROM_INT_SHIFT;
+    tmp |= cfg->ic_size / FMAN_PORT_IC_OFFSET_UNITS;
+    iowrite32be(tmp, &regs->fmbm_oicp);
+
+    /* Frame attributes */
+    tmp = BMI_CMD_OP_MR_DEF;
+    tmp |= (uint32_t)cfg->color << BMI_CMD_ATTR_COLOR_SHIFT;
+    if (cfg->sync_req)
+        tmp |= BMI_CMD_ATTR_SYNC;
+    if (port->type == E_FMAN_PORT_TYPE_OP)
+        tmp |= BMI_CMD_ATTR_ORDER;
+    iowrite32be(tmp, &regs->fmbm_ofca);
+
+    /* Internal buffer offset */
+    tmp = ((uint32_t)cfg->int_buf_start_margin / FMAN_PORT_IC_OFFSET_UNITS)
+            << BMI_INT_BUF_MARG_SHIFT;
+    iowrite32be(tmp, &regs->fmbm_oim);
+
+    /* Dequeue NIA */
+    iowrite32be(NIA_ENG_QMI_DEQ, &regs->fmbm_ofdne);
+
+    /* NIA and Enqueue NIA */
+    if (port->type == E_FMAN_PORT_TYPE_HC) {
+        iowrite32be(NIA_ENG_FM_CTL | NIA_FM_CTL_AC_HC,
+                &regs->fmbm_ofne);
+        iowrite32be(NIA_ENG_QMI_ENQ, &regs->fmbm_ofene);
+    } else {
+        iowrite32be(get_no_pcd_nia_bmi_ac_enc_frame(cfg),
+                &regs->fmbm_ofne);
+        iowrite32be(NIA_ENG_QMI_ENQ | NIA_ORDER_RESTOR,
+                &regs->fmbm_ofene);
+    }
+
+    /* Default/error queues */
+    iowrite32be((params->dflt_fqid & 0x00FFFFFF), &regs->fmbm_ofqid);
+    iowrite32be((params->err_fqid & 0x00FFFFFF), &regs->fmbm_oefqid);
+
+    /* Discard/error masks */
+    if (port->type == E_FMAN_PORT_TYPE_OP) {
+        iowrite32be(params->discard_mask, &regs->fmbm_ofsdm);
+        iowrite32be(params->err_mask, &regs->fmbm_ofsem);
+    }
+
+    /* Statistics counters */
+    tmp = 0;
+    if (cfg->stats_counters_enable)
+        tmp = BMI_COUNTERS_EN;
+    iowrite32be(tmp, &regs->fmbm_ostc);
+
+    /* Performance counters */
+    fman_port_set_perf_cnt_params(port, &cfg->perf_cnt_params);
+    tmp = 0;
+    if (cfg->perf_counters_enable)
+        tmp = BMI_COUNTERS_EN;
+    iowrite32be(tmp, &regs->fmbm_opc);
+
+    return 0;
+}
+
+static int init_qmi(struct fman_port *port,
+        struct fman_port_cfg *cfg,
+        struct fman_port_params *params)
+{
+    struct fman_port_qmi_regs *regs = port->qmi_regs;
+    uint32_t tmp;
+
+    tmp = 0;
+    if (cfg->queue_counters_enable)
+        tmp |= QMI_PORT_CFG_EN_COUNTERS;
+    iowrite32be(tmp, &regs->fmqm_pnc);
+
+    /* Rx port configuration */
+    if ((port->type == E_FMAN_PORT_TYPE_RX) ||
+            (port->type == E_FMAN_PORT_TYPE_RX_10G)) {
+        /* Enqueue NIA */
+        iowrite32be(NIA_ENG_BMI | NIA_BMI_AC_RELEASE, &regs->fmqm_pnen);
+        return 0;
+    }
+
+    /* Continue with Tx and O/H port configuration */
+    if ((port->type == E_FMAN_PORT_TYPE_TX) ||
+            (port->type == E_FMAN_PORT_TYPE_TX_10G)) {
+        /* Enqueue NIA */
+        iowrite32be(NIA_ENG_BMI | NIA_BMI_AC_TX_RELEASE,
+                &regs->fmqm_pnen);
+        /* Dequeue NIA */
+        iowrite32be(NIA_ENG_BMI | NIA_BMI_AC_TX, &regs->fmqm_pndn);
+    } else {
+        /* Enqueue NIA */
+        iowrite32be(NIA_ENG_BMI | NIA_BMI_AC_RELEASE, &regs->fmqm_pnen);
+        /* Dequeue NIA */
+        iowrite32be(NIA_ENG_BMI | NIA_BMI_AC_FETCH, &regs->fmqm_pndn);
+    }
+
+    /* Dequeue Configuration register */
+    tmp = 0;
+    if (cfg->deq_high_pri)
+        tmp |= QMI_DEQ_CFG_PRI;
+
+    switch (cfg->deq_type) {
+    case E_FMAN_PORT_DEQ_BY_PRI:
+        tmp |= QMI_DEQ_CFG_TYPE1;
+        break;
+    case E_FMAN_PORT_DEQ_ACTIVE_FQ:
+        tmp |= QMI_DEQ_CFG_TYPE2;
+        break;
+    case E_FMAN_PORT_DEQ_ACTIVE_FQ_NO_ICS:
+        tmp |= QMI_DEQ_CFG_TYPE3;
+        break;
+    default:
+        return -EINVAL;
+    }
+
+    if (cfg->qmi_deq_options_support) {
+        if ((port->type == E_FMAN_PORT_TYPE_HC) &&
+            (cfg->deq_prefetch_opt != E_FMAN_PORT_DEQ_NO_PREFETCH))
+            return -EINVAL;
+
+        switch (cfg->deq_prefetch_opt) {
+        case E_FMAN_PORT_DEQ_NO_PREFETCH:
+            break;
+        case E_FMAN_PORT_DEQ_PART_PREFETCH:
+            tmp |= QMI_DEQ_CFG_PREFETCH_PARTIAL;
+            break;
+        case E_FMAN_PORT_DEQ_FULL_PREFETCH:
+            tmp |= QMI_DEQ_CFG_PREFETCH_FULL;
+            break;
+        default:
+            return -EINVAL;
+        }
+    }
+    tmp |= (uint32_t)(params->deq_sp & QMI_DEQ_CFG_SP_MASK) <<
+            QMI_DEQ_CFG_SP_SHIFT;
+    tmp |= cfg->deq_byte_cnt;
+    iowrite32be(tmp, &regs->fmqm_pndc);
+
+    return 0;
+}
+
+static void get_rx_stats_reg(struct fman_port *port,
+        enum fman_port_stats_counters counter,
+        uint32_t **stats_reg)
+{
+    struct fman_port_rx_bmi_regs *regs = &port->bmi_regs->rx;
+
+    switch (counter) {
+    case E_FMAN_PORT_STATS_CNT_FRAME:
+        *stats_reg = &regs->fmbm_rfrc;
+        break;
+    case E_FMAN_PORT_STATS_CNT_DISCARD:
+        *stats_reg = &regs->fmbm_rfdc;
+        break;
+    case E_FMAN_PORT_STATS_CNT_DEALLOC_BUF:
+        *stats_reg = &regs->fmbm_rbdc;
+        break;
+    case E_FMAN_PORT_STATS_CNT_RX_BAD_FRAME:
+        *stats_reg = &regs->fmbm_rfbc;
+        break;
+    case E_FMAN_PORT_STATS_CNT_RX_LARGE_FRAME:
+        *stats_reg = &regs->fmbm_rlfc;
+        break;
+    case E_FMAN_PORT_STATS_CNT_RX_OUT_OF_BUF:
+        *stats_reg = &regs->fmbm_rodc;
+        break;
+    case E_FMAN_PORT_STATS_CNT_FILTERED_FRAME:
+        *stats_reg = &regs->fmbm_rffc;
+        break;
+    case E_FMAN_PORT_STATS_CNT_DMA_ERR:
+        *stats_reg = &regs->fmbm_rfldec;
+        break;
+    default:
+        *stats_reg = NULL;
+    }
+}
+
+static void get_tx_stats_reg(struct fman_port *port,
+        enum fman_port_stats_counters counter,
+        uint32_t **stats_reg)
+{
+    struct fman_port_tx_bmi_regs *regs = &port->bmi_regs->tx;
+
+    switch (counter) {
+    case E_FMAN_PORT_STATS_CNT_FRAME:
+        *stats_reg = &regs->fmbm_tfrc;
+        break;
+    case E_FMAN_PORT_STATS_CNT_DISCARD:
+        *stats_reg = &regs->fmbm_tfdc;
+        break;
+    case E_FMAN_PORT_STATS_CNT_DEALLOC_BUF:
+        *stats_reg = &regs->fmbm_tbdc;
+        break;
+    case E_FMAN_PORT_STATS_CNT_LEN_ERR:
+        *stats_reg = &regs->fmbm_tfledc;
+        break;
+    case E_FMAN_PORT_STATS_CNT_UNSUPPORTED_FORMAT:
+        *stats_reg = &regs->fmbm_tfufdc;
+        break;
+    default:
+        *stats_reg = NULL;
+    }
+}
+
+static void get_oh_stats_reg(struct fman_port *port,
+        enum fman_port_stats_counters counter,
+        uint32_t **stats_reg)
+{
+    struct fman_port_oh_bmi_regs *regs = &port->bmi_regs->oh;
+
+    switch (counter) {
+    case E_FMAN_PORT_STATS_CNT_FRAME:
+        *stats_reg = &regs->fmbm_ofrc;
+        break;
+    case E_FMAN_PORT_STATS_CNT_DISCARD:
+        *stats_reg = &regs->fmbm_ofdc;
+        break;
+    case E_FMAN_PORT_STATS_CNT_DEALLOC_BUF:
+        *stats_reg = &regs->fmbm_obdc;
+        break;
+    case E_FMAN_PORT_STATS_CNT_FILTERED_FRAME:
+        *stats_reg = &regs->fmbm_offc;
+        break;
+    case E_FMAN_PORT_STATS_CNT_DMA_ERR:
+        *stats_reg = &regs->fmbm_ofldec;
+        break;
+    case E_FMAN_PORT_STATS_CNT_LEN_ERR:
+        *stats_reg = &regs->fmbm_ofledc;
+        break;
+    case E_FMAN_PORT_STATS_CNT_UNSUPPORTED_FORMAT:
+        *stats_reg = &regs->fmbm_ofufdc;
+        break;
+    case E_FMAN_PORT_STATS_CNT_WRED_DISCARD:
+        *stats_reg = &regs->fmbm_ofwdc;
+        break;
+    default:
+        *stats_reg = NULL;
+    }
+}
+
+static void get_rx_perf_reg(struct fman_port *port,
+        enum fman_port_perf_counters counter,
+        uint32_t **perf_reg)
+{
+    struct fman_port_rx_bmi_regs *regs = &port->bmi_regs->rx;
+
+    switch (counter) {
+    case E_FMAN_PORT_PERF_CNT_CYCLE:
+        *perf_reg = &regs->fmbm_rccn;
+        break;
+    case E_FMAN_PORT_PERF_CNT_TASK_UTIL:
+        *perf_reg = &regs->fmbm_rtuc;
+        break;
+    case E_FMAN_PORT_PERF_CNT_QUEUE_UTIL:
+        *perf_reg = &regs->fmbm_rrquc;
+        break;
+    case E_FMAN_PORT_PERF_CNT_DMA_UTIL:
+        *perf_reg = &regs->fmbm_rduc;
+        break;
+    case E_FMAN_PORT_PERF_CNT_FIFO_UTIL:
+        *perf_reg = &regs->fmbm_rfuc;
+        break;
+    case E_FMAN_PORT_PERF_CNT_RX_PAUSE:
+        *perf_reg = &regs->fmbm_rpac;
+        break;
+    default:
+        *perf_reg = NULL;
+    }
+}
+
+static void get_tx_perf_reg(struct fman_port *port,
+        enum fman_port_perf_counters counter,
+        uint32_t **perf_reg)
+{
+    struct fman_port_tx_bmi_regs *regs = &port->bmi_regs->tx;
+
+    switch (counter) {
+    case E_FMAN_PORT_PERF_CNT_CYCLE:
+        *perf_reg = &regs->fmbm_tccn;
+        break;
+    case E_FMAN_PORT_PERF_CNT_TASK_UTIL:
+        *perf_reg = &regs->fmbm_ttuc;
+        break;
+    case E_FMAN_PORT_PERF_CNT_QUEUE_UTIL:
+        *perf_reg = &regs->fmbm_ttcquc;
+        break;
+    case E_FMAN_PORT_PERF_CNT_DMA_UTIL:
+        *perf_reg = &regs->fmbm_tduc;
+        break;
+    case E_FMAN_PORT_PERF_CNT_FIFO_UTIL:
+        *perf_reg = &regs->fmbm_tfuc;
+        break;
+    default:
+        *perf_reg = NULL;
+    }
+}
+
+static void get_oh_perf_reg(struct fman_port *port,
+        enum fman_port_perf_counters counter,
+        uint32_t **perf_reg)
+{
+    struct fman_port_oh_bmi_regs *regs = &port->bmi_regs->oh;
+
+    switch (counter) {
+    case E_FMAN_PORT_PERF_CNT_CYCLE:
+        *perf_reg = &regs->fmbm_occn;
+        break;
+    case E_FMAN_PORT_PERF_CNT_TASK_UTIL:
+        *perf_reg = &regs->fmbm_otuc;
+        break;
+    case E_FMAN_PORT_PERF_CNT_DMA_UTIL:
+        *perf_reg = &regs->fmbm_oduc;
+        break;
+    case E_FMAN_PORT_PERF_CNT_FIFO_UTIL:
+        *perf_reg = &regs->fmbm_ofuc;
+        break;
+    default:
+        *perf_reg = NULL;
+    }
+}
+
+static void get_qmi_counter_reg(struct fman_port *port,
+        enum fman_port_qmi_counters  counter,
+        uint32_t **queue_reg)
+{
+    struct fman_port_qmi_regs *regs = port->qmi_regs;
+
+    switch (counter) {
+    case E_FMAN_PORT_ENQ_TOTAL:
+        *queue_reg = &regs->fmqm_pnetfc;
+        break;
+    case E_FMAN_PORT_DEQ_TOTAL:
+        if ((port->type == E_FMAN_PORT_TYPE_RX) ||
+                (port->type == E_FMAN_PORT_TYPE_RX_10G))
+            /* Counter not available for Rx ports */
+            *queue_reg = NULL;
+        else
+            *queue_reg = &regs->fmqm_pndtfc;
+        break;
+    case E_FMAN_PORT_DEQ_FROM_DFLT:
+        if ((port->type == E_FMAN_PORT_TYPE_RX) ||
+                (port->type == E_FMAN_PORT_TYPE_RX_10G))
+            /* Counter not available for Rx ports */
+            *queue_reg = NULL;
+        else
+            *queue_reg = &regs->fmqm_pndfdc;
+        break;
+    case E_FMAN_PORT_DEQ_CONFIRM:
+        if ((port->type == E_FMAN_PORT_TYPE_RX) ||
+                (port->type == E_FMAN_PORT_TYPE_RX_10G))
+            /* Counter not available for Rx ports */
+            *queue_reg = NULL;
+        else
+            *queue_reg = &regs->fmqm_pndcc;
+        break;
+    default:
+        *queue_reg = NULL;
+    }
+}
+
+void fman_port_defconfig(struct fman_port_cfg *cfg, enum fman_port_type type)
+{
+    cfg->dma_swap_data = E_FMAN_PORT_DMA_NO_SWAP;
+    cfg->dma_ic_stash_on = FALSE;
+    cfg->dma_header_stash_on = FALSE;
+    cfg->dma_sg_stash_on = FALSE;
+    cfg->dma_write_optimize = TRUE;
+    cfg->color = E_FMAN_PORT_COLOR_GREEN;
+    cfg->discard_override = FALSE;
+    cfg->checksum_bytes_ignore = 0;
+    cfg->rx_cut_end_bytes = 4;
+    cfg->rx_pri_elevation = ((0x3FF + 1) * FMAN_PORT_BMI_FIFO_UNITS);
+    cfg->rx_fifo_thr = ((0x3FF + 1) * FMAN_PORT_BMI_FIFO_UNITS);
+    cfg->rx_fd_bits = 0;
+    cfg->ic_ext_offset = 0;
+    cfg->ic_int_offset = 0;
+    cfg->ic_size = 0;
+    cfg->int_buf_start_margin = 0;
+    cfg->ext_buf_start_margin = 0;
+    cfg->ext_buf_end_margin = 0;
+    cfg->tx_fifo_min_level  = 0;
+    cfg->tx_fifo_low_comf_level = (5 * KILOBYTE);
+    cfg->stats_counters_enable = TRUE;
+    cfg->perf_counters_enable = TRUE;
+    cfg->deq_type = E_FMAN_PORT_DEQ_BY_PRI;
+
+    if (type == E_FMAN_PORT_TYPE_HC) {
+        cfg->sync_req = FALSE;
+        cfg->deq_prefetch_opt = E_FMAN_PORT_DEQ_NO_PREFETCH;
+    } else {
+        cfg->sync_req = TRUE;
+        cfg->deq_prefetch_opt = E_FMAN_PORT_DEQ_FULL_PREFETCH;
+    }
+
+    if (type == E_FMAN_PORT_TYPE_TX_10G) {
+        cfg->tx_fifo_deq_pipeline_depth = 4;
+        cfg->deq_high_pri = TRUE;
+        cfg->deq_byte_cnt = 0x1400;
+    } else {
+        if ((type == E_FMAN_PORT_TYPE_HC) ||
+                (type == E_FMAN_PORT_TYPE_OP))
+            cfg->tx_fifo_deq_pipeline_depth = 2;
+        else
+            cfg->tx_fifo_deq_pipeline_depth = 1;
+
+        cfg->deq_high_pri = FALSE;
+        cfg->deq_byte_cnt = 0x400;
+    }
+    cfg->no_scatter_gather = DEFAULT_FMAN_SP_NO_SCATTER_GATHER;
+}
+
+static uint8_t fman_port_find_bpool(struct fman_port *port, uint8_t bpid)
+{
+    uint32_t *bp_reg, tmp;
+    uint8_t i, id;
+
+    /* Find the pool */
+    bp_reg = port->bmi_regs->rx.fmbm_ebmpi;
+    for (i = 0; i < port->ext_pools_num; i++) {
+        tmp = ioread32be(&bp_reg[i]);
+        id = (uint8_t)((tmp & BMI_EXT_BUF_POOL_ID_MASK) >>
+                BMI_EXT_BUF_POOL_ID_SHIFT);
+
+        if (id == bpid)
+            break;
+    }
+
+    return i;
+}
+
+int fman_port_init(struct fman_port *port,
+        struct fman_port_cfg *cfg,
+        struct fman_port_params *params)
+{
+    int err;
+
+    /* Init BMI registers */
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_RX:
+    case E_FMAN_PORT_TYPE_RX_10G:
+        err = init_bmi_rx(port, cfg, params);
+        break;
+    case E_FMAN_PORT_TYPE_TX:
+    case E_FMAN_PORT_TYPE_TX_10G:
+        err = init_bmi_tx(port, cfg, params);
+        break;
+    case E_FMAN_PORT_TYPE_OP:
+    case E_FMAN_PORT_TYPE_HC:
+        err = init_bmi_oh(port, cfg, params);
+        break;
+    default:
+        return -EINVAL;
+    }
+
+    if (err)
+        return err;
+
+    /* Init QMI registers */
+    if (!port->im_en)
+    {
+        err = init_qmi(port, cfg, params);
+        return err;
+    }
+    return 0;
+}
+
+int fman_port_enable(struct fman_port *port)
+{
+    uint32_t *bmi_cfg_reg, tmp;
+    bool rx_port;
+
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_RX:
+    case E_FMAN_PORT_TYPE_RX_10G:
+        bmi_cfg_reg = &port->bmi_regs->rx.fmbm_rcfg;
+        rx_port = TRUE;
+        break;
+    case E_FMAN_PORT_TYPE_TX:
+    case E_FMAN_PORT_TYPE_TX_10G:
+        bmi_cfg_reg = &port->bmi_regs->tx.fmbm_tcfg;
+        rx_port = FALSE;
+        break;
+    case E_FMAN_PORT_TYPE_OP:
+    case E_FMAN_PORT_TYPE_HC:
+        bmi_cfg_reg = &port->bmi_regs->oh.fmbm_ocfg;
+        rx_port = FALSE;
+        break;
+    default:
+        return -EINVAL;
+    }
+
+    /* Enable QMI */
+    if (!rx_port) {
+        tmp = ioread32be(&port->qmi_regs->fmqm_pnc) | QMI_PORT_CFG_EN;
+        iowrite32be(tmp, &port->qmi_regs->fmqm_pnc);
+    }
+
+    /* Enable BMI */
+    tmp = ioread32be(bmi_cfg_reg) | BMI_PORT_CFG_EN;
+    iowrite32be(tmp, bmi_cfg_reg);
+
+    return 0;
+}
+
+int fman_port_disable(const struct fman_port *port)
+{
+    uint32_t *bmi_cfg_reg, *bmi_status_reg, tmp;
+    bool rx_port, failure = FALSE;
+    int count;
+
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_RX:
+    case E_FMAN_PORT_TYPE_RX_10G:
+        bmi_cfg_reg = &port->bmi_regs->rx.fmbm_rcfg;
+        bmi_status_reg = &port->bmi_regs->rx.fmbm_rst;
+        rx_port = TRUE;
+        break;
+    case E_FMAN_PORT_TYPE_TX:
+    case E_FMAN_PORT_TYPE_TX_10G:
+        bmi_cfg_reg = &port->bmi_regs->tx.fmbm_tcfg;
+        bmi_status_reg = &port->bmi_regs->tx.fmbm_tst;
+        rx_port = FALSE;
+        break;
+    case E_FMAN_PORT_TYPE_OP:
+    case E_FMAN_PORT_TYPE_HC:
+        bmi_cfg_reg = &port->bmi_regs->oh.fmbm_ocfg;
+        bmi_status_reg = &port->bmi_regs->oh.fmbm_ost;
+        rx_port = FALSE;
+        break;
+    default:
+        return -EINVAL;
+    }
+
+    /* Disable QMI */
+    if (!rx_port) {
+        tmp = ioread32be(&port->qmi_regs->fmqm_pnc) & ~QMI_PORT_CFG_EN;
+        iowrite32be(tmp, &port->qmi_regs->fmqm_pnc);
+
+        /* Wait for QMI to finish FD handling */
+        count = 100;
+        do {
+            udelay(10);
+            tmp = ioread32be(&port->qmi_regs->fmqm_pns);
+        } while ((tmp & QMI_PORT_STATUS_DEQ_FD_BSY) && --count);
+
+        if (count == 0)
+        {
+            /* Timeout */
+            iowrite32be(ioread32be(&port->qmi_regs->fmqm_pnc) |
+                QMI_PORT_CFG_EN, &port->qmi_regs->fmqm_pnc);
+            failure = TRUE;
+        }
+    }
+
+    /* Disable BMI */
+    tmp = ioread32be(bmi_cfg_reg) & ~BMI_PORT_CFG_EN;
+    iowrite32be(tmp, bmi_cfg_reg);
+
+    /* Wait for graceful stop end */
+    count = 500;
+    do {
+        udelay(10);
+        tmp = ioread32be(bmi_status_reg);
+    } while ((tmp & BMI_PORT_STATUS_BSY) && --count);
+
+    if (count == 0)
+    {
+        /* Timeout */
+        iowrite32be(ioread32be(&port->qmi_regs->fmqm_pnc) |
+            QMI_PORT_CFG_EN, &port->qmi_regs->fmqm_pnc);
+        iowrite32be(ioread32be(bmi_cfg_reg) | BMI_PORT_CFG_EN,
+            bmi_cfg_reg);
+        failure = TRUE;
+    }
+
+    if (failure)
+        return -EBUSY;
+
+    return 0;
+}
+
+int fman_port_set_bpools(const struct fman_port *port,
+        const struct fman_port_bpools *bp)
+{
+    uint32_t tmp, *bp_reg, *bp_depl_reg;
+    uint8_t i, max_bp_num;
+    bool grp_depl_used = FALSE, rx_port;
+
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_RX:
+    case E_FMAN_PORT_TYPE_RX_10G:
+        max_bp_num = port->ext_pools_num;
+        rx_port = TRUE;
+        bp_reg = port->bmi_regs->rx.fmbm_ebmpi;
+        bp_depl_reg = &port->bmi_regs->rx.fmbm_mpd;
+        break;
+    case E_FMAN_PORT_TYPE_OP:
+        if (port->fm_rev_maj != 4)
+            return -EINVAL;
+        max_bp_num = FMAN_PORT_OBS_EXT_POOLS_NUM;
+        rx_port = FALSE;
+        bp_reg = port->bmi_regs->oh.fmbm_oebmpi;
+        bp_depl_reg = &port->bmi_regs->oh.fmbm_ompd;
+        break;
+    default:
+        return -EINVAL;
+    }
+
+    if (rx_port) {
+        /* Check buffers are provided in ascending order */
+        for (i = 0; i < (bp->count-1); i++) {
+            if (bp->bpool[i].size > bp->bpool[i+1].size)
+                return -EINVAL;
+        }
+    }
+
+    /* Set up external buffers pools */
+    for (i = 0; i < bp->count; i++) {
+        tmp = BMI_EXT_BUF_POOL_VALID;
+        tmp |= ((uint32_t)bp->bpool[i].bpid <<
+            BMI_EXT_BUF_POOL_ID_SHIFT) & BMI_EXT_BUF_POOL_ID_MASK;
+
+        if (rx_port) {
+            if (bp->counters_enable)
+                tmp |= BMI_EXT_BUF_POOL_EN_COUNTER;
+
+            if (bp->bpool[i].is_backup)
+                tmp |= BMI_EXT_BUF_POOL_BACKUP;
+
+            tmp |= (uint32_t)bp->bpool[i].size;
+        }
+
+        iowrite32be(tmp, &bp_reg[i]);
+    }
+
+    /* Clear unused pools */
+    for (i = bp->count; i < max_bp_num; i++)
+        iowrite32be(0, &bp_reg[i]);
+
+    /* Pools depletion */
+    tmp = 0;
+    for (i = 0; i < bp->count; i++) {
+        if (bp->bpool[i].grp_bp_depleted) {
+            grp_depl_used = TRUE;
+            tmp |= 0x80000000 >> i;
+        }
+
+        if (bp->bpool[i].single_bp_depleted)
+            tmp |= 0x80 >> i;
+
+        if (bp->bpool[i].pfc_priorities_en)
+            tmp |= 0x0100 << i;
+    }
+
+    if (grp_depl_used)
+        tmp |= ((uint32_t)bp->grp_bp_depleted_num - 1) <<
+            BMI_POOL_DEP_NUM_OF_POOLS_SHIFT;
+
+    iowrite32be(tmp, bp_depl_reg);
+    return 0;
+}
+
+int fman_port_set_rate_limiter(struct fman_port *port,
+        struct fman_port_rate_limiter *rate_limiter)
+{
+    uint32_t *rate_limit_reg, *rate_limit_scale_reg;
+    uint32_t granularity, tmp;
+    uint8_t usec_bit, factor;
+
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_TX:
+    case E_FMAN_PORT_TYPE_TX_10G:
+        rate_limit_reg = &port->bmi_regs->tx.fmbm_trlmt;
+        rate_limit_scale_reg = &port->bmi_regs->tx.fmbm_trlmts;
+        granularity = BMI_RATE_LIMIT_GRAN_TX;
+        break;
+    case E_FMAN_PORT_TYPE_OP:
+        rate_limit_reg = &port->bmi_regs->oh.fmbm_orlmt;
+        rate_limit_scale_reg = &port->bmi_regs->oh.fmbm_orlmts;
+        granularity = BMI_RATE_LIMIT_GRAN_OP;
+        break;
+    default:
+        return -EINVAL;
+    }
+
+    /* Factor is per 1 usec count */
+    factor = 1;
+    usec_bit = rate_limiter->count_1micro_bit;
+
+    /* If rate limit is too small for an 1usec factor, adjust timestamp
+     * scale and multiply the factor */
+    while (rate_limiter->rate < (granularity / factor)) {
+        if (usec_bit == 31)
+            /* Can't configure rate limiter - rate is too small */
+            return -EINVAL;
+
+        usec_bit++;
+        factor <<= 1;
+    }
+
+    /* Figure out register value. The "while" above quarantees that
+     * (rate_limiter->rate * factor / granularity) >= 1 */
+    tmp = (uint32_t)(rate_limiter->rate * factor / granularity - 1);
+
+    /* Check rate limit isn't too large */
+    if (tmp >= BMI_RATE_LIMIT_MAX_RATE_IN_GRAN_UNITS)
+        return -EINVAL;
+
+    /* Check burst size is in allowed range */
+    if ((rate_limiter->burst_size == 0) ||
+            (rate_limiter->burst_size >
+                BMI_RATE_LIMIT_MAX_BURST_SIZE))
+        return -EINVAL;
+
+    tmp |= (uint32_t)(rate_limiter->burst_size - 1) <<
+            BMI_RATE_LIMIT_MAX_BURST_SHIFT;
+
+    if ((port->type == E_FMAN_PORT_TYPE_OP) &&
+            (port->fm_rev_maj == 4)) {
+        if (rate_limiter->high_burst_size_gran)
+            tmp |= BMI_RATE_LIMIT_HIGH_BURST_SIZE_GRAN;
+    }
+
+    iowrite32be(tmp, rate_limit_reg);
+
+    /* Set up rate limiter scale register */
+    tmp = BMI_RATE_LIMIT_SCALE_EN;
+    tmp |= (31 - (uint32_t)usec_bit) << BMI_RATE_LIMIT_SCALE_TSBS_SHIFT;
+
+    if ((port->type == E_FMAN_PORT_TYPE_OP) &&
+            (port->fm_rev_maj == 4))
+        tmp |= rate_limiter->rate_factor;
+
+    iowrite32be(tmp, rate_limit_scale_reg);
+
+    return 0;
+}
+
+int fman_port_delete_rate_limiter(struct fman_port *port)
+{
+    uint32_t *rate_limit_scale_reg;
+
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_TX:
+    case E_FMAN_PORT_TYPE_TX_10G:
+        rate_limit_scale_reg = &port->bmi_regs->tx.fmbm_trlmts;
+        break;
+    case E_FMAN_PORT_TYPE_OP:
+        rate_limit_scale_reg = &port->bmi_regs->oh.fmbm_orlmts;
+        break;
+    default:
+        return -EINVAL;
+    }
+
+    iowrite32be(0, rate_limit_scale_reg);
+    return 0;
+}
+
+int fman_port_set_err_mask(struct fman_port *port, uint32_t err_mask)
+{
+    uint32_t *err_mask_reg;
+
+    /* Obtain register address */
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_RX:
+    case E_FMAN_PORT_TYPE_RX_10G:
+        err_mask_reg = &port->bmi_regs->rx.fmbm_rfsem;
+        break;
+    case E_FMAN_PORT_TYPE_OP:
+        err_mask_reg = &port->bmi_regs->oh.fmbm_ofsem;
+        break;
+    default:
+        return -EINVAL;
+    }
+
+    iowrite32be(err_mask, err_mask_reg);
+    return 0;
+}
+
+int fman_port_set_discard_mask(struct fman_port *port, uint32_t discard_mask)
+{
+    uint32_t *discard_mask_reg;
+
+    /* Obtain register address */
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_RX:
+    case E_FMAN_PORT_TYPE_RX_10G:
+        discard_mask_reg = &port->bmi_regs->rx.fmbm_rfsdm;
+        break;
+    case E_FMAN_PORT_TYPE_OP:
+        discard_mask_reg = &port->bmi_regs->oh.fmbm_ofsdm;
+        break;
+    default:
+        return -EINVAL;
+    }
+
+    iowrite32be(discard_mask, discard_mask_reg);
+    return 0;
+}
+
+int fman_port_modify_rx_fd_bits(struct fman_port *port,
+        uint8_t rx_fd_bits,
+        bool add)
+{
+    uint32_t    tmp;
+
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_RX:
+    case E_FMAN_PORT_TYPE_RX_10G:
+        break;
+    default:
+        return -EINVAL;
+    }
+
+    tmp = ioread32be(&port->bmi_regs->rx.fmbm_rfne);
+
+    if (add)
+        tmp |= (uint32_t)rx_fd_bits << BMI_NEXT_ENG_FD_BITS_SHIFT;
+    else
+        tmp &= ~((uint32_t)rx_fd_bits << BMI_NEXT_ENG_FD_BITS_SHIFT);
+
+    iowrite32be(tmp, &port->bmi_regs->rx.fmbm_rfne);
+    return 0;
+}
+
+int fman_port_set_perf_cnt_params(struct fman_port *port,
+        struct fman_port_perf_cnt_params *params)
+{
+    uint32_t *pcp_reg, tmp;
+
+    /* Obtain register address and check parameters are in range */
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_RX:
+    case E_FMAN_PORT_TYPE_RX_10G:
+        pcp_reg = &port->bmi_regs->rx.fmbm_rpcp;
+        if ((params->queue_val == 0) ||
+            (params->queue_val > MAX_PERFORMANCE_RX_QUEUE_COMP))
+            return -EINVAL;
+        break;
+    case E_FMAN_PORT_TYPE_TX:
+    case E_FMAN_PORT_TYPE_TX_10G:
+        pcp_reg = &port->bmi_regs->tx.fmbm_tpcp;
+        if ((params->queue_val == 0) ||
+            (params->queue_val > MAX_PERFORMANCE_TX_QUEUE_COMP))
+            return -EINVAL;
+        break;
+    case E_FMAN_PORT_TYPE_OP:
+    case E_FMAN_PORT_TYPE_HC:
+        pcp_reg = &port->bmi_regs->oh.fmbm_opcp;
+        if (params->queue_val != 0)
+            return -EINVAL;
+        break;
+    default:
+        return -EINVAL;
+    }
+
+    if ((params->task_val == 0) ||
+            (params->task_val > MAX_PERFORMANCE_TASK_COMP))
+        return -EINVAL;
+    if ((params->dma_val == 0) ||
+            (params->dma_val > MAX_PERFORMANCE_DMA_COMP))
+        return -EINVAL;
+    if ((params->fifo_val == 0) ||
+            ((params->fifo_val / FMAN_PORT_BMI_FIFO_UNITS) >
+                MAX_PERFORMANCE_FIFO_COMP))
+        return -EINVAL;
+    tmp = (uint32_t)(params->task_val - 1) <<
+            BMI_PERFORMANCE_TASK_COMP_SHIFT;
+    tmp |= (uint32_t)(params->dma_val - 1) <<
+            BMI_PERFORMANCE_DMA_COMP_SHIFT;
+    tmp |= (uint32_t)(params->fifo_val / FMAN_PORT_BMI_FIFO_UNITS - 1);
+
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_RX:
+    case E_FMAN_PORT_TYPE_RX_10G:
+    case E_FMAN_PORT_TYPE_TX:
+    case E_FMAN_PORT_TYPE_TX_10G:
+        tmp |= (uint32_t)(params->queue_val - 1) <<
+            BMI_PERFORMANCE_QUEUE_COMP_SHIFT;
+        break;
+    default:
+        break;
+    }
+
+
+    iowrite32be(tmp, pcp_reg);
+    return 0;
+}
+
+int fman_port_set_stats_cnt_mode(struct fman_port *port, bool enable)
+{
+    uint32_t *stats_reg, tmp;
+
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_RX:
+    case E_FMAN_PORT_TYPE_RX_10G:
+        stats_reg = &port->bmi_regs->rx.fmbm_rstc;
+        break;
+    case E_FMAN_PORT_TYPE_TX:
+    case E_FMAN_PORT_TYPE_TX_10G:
+        stats_reg = &port->bmi_regs->tx.fmbm_tstc;
+        break;
+    case E_FMAN_PORT_TYPE_OP:
+    case E_FMAN_PORT_TYPE_HC:
+        stats_reg = &port->bmi_regs->oh.fmbm_ostc;
+        break;
+    default:
+        return -EINVAL;
+    }
+
+    tmp = ioread32be(stats_reg);
+
+    if (enable)
+        tmp |= BMI_COUNTERS_EN;
+    else
+        tmp &= ~BMI_COUNTERS_EN;
+
+    iowrite32be(tmp, stats_reg);
+    return 0;
+}
+
+int fman_port_set_perf_cnt_mode(struct fman_port *port, bool enable)
+{
+    uint32_t *stats_reg, tmp;
+
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_RX:
+    case E_FMAN_PORT_TYPE_RX_10G:
+        stats_reg = &port->bmi_regs->rx.fmbm_rpc;
+        break;
+    case E_FMAN_PORT_TYPE_TX:
+    case E_FMAN_PORT_TYPE_TX_10G:
+        stats_reg = &port->bmi_regs->tx.fmbm_tpc;
+        break;
+    case E_FMAN_PORT_TYPE_OP:
+    case E_FMAN_PORT_TYPE_HC:
+        stats_reg = &port->bmi_regs->oh.fmbm_opc;
+        break;
+    default:
+        return -EINVAL;
+    }
+
+    tmp = ioread32be(stats_reg);
+
+    if (enable)
+        tmp |= BMI_COUNTERS_EN;
+    else
+        tmp &= ~BMI_COUNTERS_EN;
+
+    iowrite32be(tmp, stats_reg);
+    return 0;
+}
+
+int fman_port_set_queue_cnt_mode(struct fman_port *port, bool enable)
+{
+    uint32_t tmp;
+
+    tmp = ioread32be(&port->qmi_regs->fmqm_pnc);
+
+    if (enable)
+        tmp |= QMI_PORT_CFG_EN_COUNTERS;
+    else
+        tmp &= ~QMI_PORT_CFG_EN_COUNTERS;
+
+    iowrite32be(tmp, &port->qmi_regs->fmqm_pnc);
+    return 0;
+}
+
+int fman_port_set_bpool_cnt_mode(struct fman_port *port,
+        uint8_t bpid,
+        bool enable)
+{
+    uint8_t index;
+    uint32_t tmp;
+
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_RX:
+    case E_FMAN_PORT_TYPE_RX_10G:
+        break;
+    default:
+        return -EINVAL;
+    }
+
+    /* Find the pool */
+    index = fman_port_find_bpool(port, bpid);
+    if (index == port->ext_pools_num)
+        /* Not found */
+        return -EINVAL;
+
+    tmp = ioread32be(&port->bmi_regs->rx.fmbm_ebmpi[index]);
+
+    if (enable)
+        tmp |= BMI_EXT_BUF_POOL_EN_COUNTER;
+    else
+        tmp &= ~BMI_EXT_BUF_POOL_EN_COUNTER;
+
+    iowrite32be(tmp, &port->bmi_regs->rx.fmbm_ebmpi[index]);
+    return 0;
+}
+
+uint32_t fman_port_get_stats_counter(struct fman_port *port,
+        enum fman_port_stats_counters  counter)
+{
+    uint32_t *stats_reg, ret_val;
+
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_RX:
+    case E_FMAN_PORT_TYPE_RX_10G:
+        get_rx_stats_reg(port, counter, &stats_reg);
+        break;
+    case E_FMAN_PORT_TYPE_TX:
+    case E_FMAN_PORT_TYPE_TX_10G:
+        get_tx_stats_reg(port, counter, &stats_reg);
+        break;
+    case E_FMAN_PORT_TYPE_OP:
+    case E_FMAN_PORT_TYPE_HC:
+        get_oh_stats_reg(port, counter, &stats_reg);
+        break;
+    default:
+        stats_reg = NULL;
+    }
+
+    if (stats_reg == NULL)
+        return 0;
+
+    ret_val = ioread32be(stats_reg);
+    return ret_val;
+}
+
+void fman_port_set_stats_counter(struct fman_port *port,
+        enum fman_port_stats_counters counter,
+        uint32_t value)
+{
+    uint32_t *stats_reg;
+
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_RX:
+    case E_FMAN_PORT_TYPE_RX_10G:
+        get_rx_stats_reg(port, counter, &stats_reg);
+        break;
+    case E_FMAN_PORT_TYPE_TX:
+    case E_FMAN_PORT_TYPE_TX_10G:
+        get_tx_stats_reg(port, counter, &stats_reg);
+        break;
+    case E_FMAN_PORT_TYPE_OP:
+    case E_FMAN_PORT_TYPE_HC:
+        get_oh_stats_reg(port, counter, &stats_reg);
+        break;
+    default:
+        stats_reg = NULL;
+    }
+
+    if (stats_reg == NULL)
+        return;
+
+    iowrite32be(value, stats_reg);
+}
+
+uint32_t fman_port_get_perf_counter(struct fman_port *port,
+        enum fman_port_perf_counters counter)
+{
+    uint32_t *perf_reg, ret_val;
+
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_RX:
+    case E_FMAN_PORT_TYPE_RX_10G:
+        get_rx_perf_reg(port, counter, &perf_reg);
+        break;
+    case E_FMAN_PORT_TYPE_TX:
+    case E_FMAN_PORT_TYPE_TX_10G:
+        get_tx_perf_reg(port, counter, &perf_reg);
+        break;
+    case E_FMAN_PORT_TYPE_OP:
+    case E_FMAN_PORT_TYPE_HC:
+        get_oh_perf_reg(port, counter, &perf_reg);
+        break;
+    default:
+        perf_reg = NULL;
+    }
+
+    if (perf_reg == NULL)
+        return 0;
+
+    ret_val = ioread32be(perf_reg);
+    return ret_val;
+}
+
+void fman_port_set_perf_counter(struct fman_port *port,
+        enum fman_port_perf_counters counter,
+        uint32_t value)
+{
+    uint32_t *perf_reg;
+
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_RX:
+    case E_FMAN_PORT_TYPE_RX_10G:
+        get_rx_perf_reg(port, counter, &perf_reg);
+        break;
+    case E_FMAN_PORT_TYPE_TX:
+    case E_FMAN_PORT_TYPE_TX_10G:
+        get_tx_perf_reg(port, counter, &perf_reg);
+        break;
+    case E_FMAN_PORT_TYPE_OP:
+    case E_FMAN_PORT_TYPE_HC:
+        get_oh_perf_reg(port, counter, &perf_reg);
+        break;
+    default:
+        perf_reg = NULL;
+    }
+
+    if (perf_reg == NULL)
+        return;
+
+    iowrite32be(value, perf_reg);
+}
+
+uint32_t fman_port_get_qmi_counter(struct fman_port *port,
+        enum fman_port_qmi_counters  counter)
+{
+    uint32_t *queue_reg, ret_val;
+
+    get_qmi_counter_reg(port, counter, &queue_reg);
+
+    if (queue_reg == NULL)
+        return 0;
+
+    ret_val = ioread32be(queue_reg);
+    return ret_val;
+}
+
+void fman_port_set_qmi_counter(struct fman_port *port,
+        enum fman_port_qmi_counters counter,
+        uint32_t value)
+{
+    uint32_t *queue_reg;
+
+    get_qmi_counter_reg(port, counter, &queue_reg);
+
+    if (queue_reg == NULL)
+        return;
+
+    iowrite32be(value, queue_reg);
+}
+
+uint32_t fman_port_get_bpool_counter(struct fman_port *port, uint8_t bpid)
+{
+    uint8_t index;
+    uint32_t ret_val;
+
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_RX:
+    case E_FMAN_PORT_TYPE_RX_10G:
+        break;
+    default:
+        return 0;
+    }
+
+    /* Find the pool */
+    index = fman_port_find_bpool(port, bpid);
+    if (index == port->ext_pools_num)
+        /* Not found */
+        return 0;
+
+    ret_val = ioread32be(&port->bmi_regs->rx.fmbm_acnt[index]);
+    return ret_val;
+}
+
+void fman_port_set_bpool_counter(struct fman_port *port,
+        uint8_t bpid,
+        uint32_t value)
+{
+    uint8_t index;
+
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_RX:
+    case E_FMAN_PORT_TYPE_RX_10G:
+        break;
+    default:
+        return;
+    }
+
+    /* Find the pool */
+    index = fman_port_find_bpool(port, bpid);
+    if (index == port->ext_pools_num)
+        /* Not found */
+        return;
+
+    iowrite32be(value, &port->bmi_regs->rx.fmbm_acnt[index]);
+}
+
+int fman_port_add_congestion_grps(struct fman_port *port,
+        uint32_t grps_map[FMAN_PORT_CG_MAP_NUM])
+{
+    int i;
+    uint32_t tmp, *grp_map_reg;
+    uint8_t max_grp_map_num;
+
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_RX:
+    case E_FMAN_PORT_TYPE_RX_10G:
+        if (port->fm_rev_maj == 4)
+            max_grp_map_num = 1;
+        else
+            max_grp_map_num = FMAN_PORT_CG_MAP_NUM;
+        grp_map_reg = port->bmi_regs->rx.fmbm_rcgm;
+        break;
+    case E_FMAN_PORT_TYPE_OP:
+        max_grp_map_num = 1;
+        if (port->fm_rev_maj != 4)
+            return -EINVAL;
+        grp_map_reg = &port->bmi_regs->oh.fmbm_ocgm;
+        break;
+    default:
+        return -EINVAL;
+    }
+
+    for (i = (max_grp_map_num - 1); i >= 0; i--) {
+        if (grps_map[i] == 0)
+            continue;
+        tmp = ioread32be(&grp_map_reg[i]);
+        tmp |= grps_map[i];
+        iowrite32be(tmp, &grp_map_reg[i]);
+    }
+
+    return 0;
+}
+
+int fman_port_remove_congestion_grps(struct fman_port *port,
+        uint32_t grps_map[FMAN_PORT_CG_MAP_NUM])
+{
+    int i;
+    uint32_t tmp, *grp_map_reg;
+    uint8_t max_grp_map_num;
+
+    switch (port->type) {
+    case E_FMAN_PORT_TYPE_RX:
+    case E_FMAN_PORT_TYPE_RX_10G:
+        if (port->fm_rev_maj == 4)
+            max_grp_map_num = 1;
+        else
+            max_grp_map_num = FMAN_PORT_CG_MAP_NUM;
+        grp_map_reg = port->bmi_regs->rx.fmbm_rcgm;
+        break;
+    case E_FMAN_PORT_TYPE_OP:
+        max_grp_map_num = 1;
+        if (port->fm_rev_maj != 4)
+            return -EINVAL;
+        grp_map_reg = &port->bmi_regs->oh.fmbm_ocgm;
+        break;
+    default:
+        return -EINVAL;
+    }
+
+    for (i = (max_grp_map_num - 1); i >= 0; i--) {
+        if (grps_map[i] == 0)
+            continue;
+        tmp = ioread32be(&grp_map_reg[i]);
+        tmp &= ~grps_map[i];
+        iowrite32be(tmp, &grp_map_reg[i]);
+    }
+    return 0;
+}