opensm/osm_pkey_mgr.c: Pack switch peer port PKey tables

index 31e14aa..dbab048 100644
@@ -398,6 +398,35 @@ static uint16_t last_used_pkey_index(const osm_port_t * const p_port,
 	return index;
 }
 
+static int update_peer_block(osm_log_t * p_log, osm_sm_t * sm,
+			     osm_physp_t * peer,
+			     osm_pkey_tbl_t * p_peer_pkey_tbl,
+			     ib_pkey_table_t * new_peer_block,
+			     uint16_t peer_block_idx, osm_node_t * p_node)
+{
+	int ret = 0;
+	ib_pkey_table_t *peer_block;
+
+	peer_block = osm_pkey_tbl_block_get(p_peer_pkey_tbl, peer_block_idx);
+	if (!peer_block ||
+	    memcmp(peer_block, new_peer_block, sizeof(*peer_block))) {
+		if (pkey_mgr_update_pkey_entry(sm, peer, new_peer_block,
+					       peer_block_idx) != IB_SUCCESS) {
+			OSM_LOG(p_log, OSM_LOG_ERROR, "ERR 0509: "
+				"pkey_mgr_update_pkey_entry() failed to update "
+				"pkey table block %d for node 0x%016"
+				PRIx64 " port %u (%s)\n",
+				peer_block_idx,
+				cl_ntoh64(osm_node_get_node_guid(p_node)),
+				osm_physp_get_port_num(peer),
+				p_node->print_desc);
+			ret = -1;
+		}
+	}
+
+	return ret;
+}
+
 static int pkey_mgr_update_peer_port(osm_log_t * p_log, osm_sm_t * sm,
 				     const osm_subn_t * p_subn,
 				     const osm_port_t * const p_port,

@@ -405,15 +434,16 @@ static int pkey_mgr_update_peer_port(osm_log_t * p_log, osm_sm_t * sm,
 {
 	osm_physp_t *p_physp, *peer;
 	osm_node_t *p_node;
-	ib_pkey_table_t *block, *peer_block;
+	ib_pkey_table_t *block;
 	const osm_pkey_tbl_t *p_pkey_tbl;
 	osm_pkey_tbl_t *p_peer_pkey_tbl;
-	uint16_t block_index;
+	uint16_t block_index, peer_block_idx;
 	uint16_t peer_max_blocks;
 	uint16_t last_index;
-	ib_api_status_t status = IB_SUCCESS;
-	ib_pkey_table_t empty_block;
-	int ret = 0;
+	ib_pkey_table_t new_peer_block;
+	uint16_t pkey_idx, peer_pkey_idx;
+	ib_net16_t pkey;
+	int ret = 0, loop_exit = 0;
 
 	p_physp = p_port->p_physp;
 	if (!p_physp)

@@ -425,79 +455,92 @@ static int pkey_mgr_update_peer_port(osm_log_t * p_log, osm_sm_t * sm,
 	if (!p_node->sw || !p_node->sw->switch_info.enforce_cap)
 		return 0;
 
-	p_pkey_tbl = osm_physp_get_pkey_tbl(p_physp);
-	peer_max_blocks = pkey_mgr_get_physp_max_blocks(peer);
-	if (peer_max_blocks < p_pkey_tbl->used_blocks) {
-		OSM_LOG(p_log, OSM_LOG_ERROR, "ERR 0508: "
-			"Not enough pkey blocks (%u < %u used) on switch 0x%016"
-			PRIx64 " port %u (%s). Clearing Enforcement bit\n",
-			peer_max_blocks, p_pkey_tbl->used_blocks,
-			cl_ntoh64(osm_node_get_node_guid(p_node)),
-			osm_physp_get_port_num(peer),
-			p_node->print_desc);
-		enforce = FALSE;
-		ret = -1;
-	} else if (peer_max_blocks == p_pkey_tbl->used_blocks) {
-		/* Is last used pkey index beyond switch peer port capacity ? */
-		last_index = (peer_max_blocks - 1) * IB_NUM_PKEY_ELEMENTS_IN_BLOCK +
-			     last_used_pkey_index(p_port, p_pkey_tbl);
-		if (cl_ntoh16(p_node->sw->switch_info.enforce_cap) <= last_index) {
-			OSM_LOG(p_log, OSM_LOG_ERROR, "ERR 0507: "
-				"Not enough pkey entries (%u <= %u) on switch 0x%016"
-				PRIx64 " port %u (%s). Clearing Enforcement bit\n",
-				cl_ntoh16(p_node->sw->switch_info.enforce_cap),
-				last_index,
-				cl_ntoh64(osm_node_get_node_guid(p_node)),
-				osm_physp_get_port_num(peer),
-				p_node->print_desc);
-			enforce = FALSE;
-			ret = -1;
-		}
-	}
-
-	if (pkey_mgr_enforce_partition(p_log, sm, peer, enforce))
-		ret = -1;
-
-	if (enforce == FALSE)
+	if (enforce == FALSE) {
+		pkey_mgr_enforce_partition(p_log, sm, peer, FALSE);
 		return ret;
+	}
 
-	memset(&empty_block, 0, sizeof(ib_pkey_table_t));
-
+	p_pkey_tbl = osm_physp_get_pkey_tbl(p_physp);
+	peer_max_blocks = pkey_mgr_get_physp_max_blocks(peer);
 	p_peer_pkey_tbl = &peer->pkeys;
-	p_peer_pkey_tbl->used_blocks = p_pkey_tbl->used_blocks;
+	peer_block_idx = 0;
+	peer_pkey_idx = 0;
 	for (block_index = 0; block_index < p_pkey_tbl->used_blocks;
 	     block_index++) {
+		if (loop_exit)
+			break;
 		block = osm_pkey_tbl_new_block_get(p_pkey_tbl, block_index);
 		if (!block)
-			block = &empty_block;
-
-		peer_block =
-		    osm_pkey_tbl_block_get(p_peer_pkey_tbl, block_index);
-		if (!peer_block
-		    || memcmp(peer_block, block, sizeof(*peer_block))) {
-			status = pkey_mgr_update_pkey_entry(sm, peer, block,
-							    block_index);
-			if (status != IB_SUCCESS) {
-				OSM_LOG(p_log, OSM_LOG_ERROR, "ERR 0509: "
-					"pkey_mgr_update_pkey_entry() failed to update "
-					"pkey table block %d for node 0x%016"
-					PRIx64 " port %u (%s)\n", block_index,
-					cl_ntoh64(osm_node_get_node_guid
-						  (p_node)),
+			continue;
+		for (pkey_idx = 0; pkey_idx < IB_NUM_PKEY_ELEMENTS_IN_BLOCK;
+		     pkey_idx++) {
+			pkey = block->pkey_entry[pkey_idx];
+			if (ib_pkey_is_invalid(pkey))
+				continue;
+			new_peer_block.pkey_entry[peer_pkey_idx] = pkey;
+			if (peer_block_idx >= peer_max_blocks) {
+				loop_exit = 1;
+				break;
+			}
+			if (++peer_pkey_idx == IB_NUM_PKEY_ELEMENTS_IN_BLOCK) {
+				if (update_peer_block(p_log, sm, peer,
+						      p_peer_pkey_tbl,
+						      &new_peer_block,
+						      peer_block_idx, p_node))
+					ret = -1;
+				peer_pkey_idx = 0;
+				peer_block_idx++;
+			}
+		}
+	}
+
+	if (peer_block_idx < peer_max_blocks) {
+		if (peer_pkey_idx) {
+			/* Handle partial last block */
+			for (; peer_pkey_idx < IB_NUM_PKEY_ELEMENTS_IN_BLOCK;
+			     peer_pkey_idx++)
+				new_peer_block.pkey_entry[peer_pkey_idx] = 0;
+			if (update_peer_block(p_log, sm, peer, p_peer_pkey_tbl,
+					      &new_peer_block, peer_block_idx,
+					      p_node))
+				ret = -1;
+		} else
+			peer_block_idx--;
+
+		p_peer_pkey_tbl->used_blocks = peer_block_idx + 1;
+		if (p_peer_pkey_tbl->used_blocks == peer_max_blocks) {
+			/* Is last used pkey index beyond switch peer port capacity ? */
+			last_index = peer_block_idx * IB_NUM_PKEY_ELEMENTS_IN_BLOCK +
+				     last_used_pkey_index(p_port,
+							  p_peer_pkey_tbl);
+			if (cl_ntoh16(p_node->sw->switch_info.enforce_cap) <= last_index) {
+				OSM_LOG(p_log, OSM_LOG_ERROR, "ERR 0507: "
+					"Not enough pkey entries (%u <= %u) on switch 0x%016"
+					PRIx64 " port %u (%s). Clearing Enforcement bit\n",
+					cl_ntoh16(p_node->sw->switch_info.enforce_cap),
+					last_index,
+					cl_ntoh64(osm_node_get_node_guid(p_node)),
 					osm_physp_get_port_num(peer),
 					p_node->print_desc);
+				enforce = FALSE;
 				ret = -1;
 			}
 		}
+	} else {
+		p_peer_pkey_tbl->used_blocks = peer_max_blocks;
+		enforce = FALSE;
 	}
 
 	if (!ret)
 		OSM_LOG(p_log, OSM_LOG_DEBUG,
-			"Pkey table was updated for node 0x%016" PRIx64
-			" port %u (%s)\n",
+			"Pkey table was successfully updated for node 0x%016"
+			PRIx64 " port %u (%s)\n",
 			cl_ntoh64(osm_node_get_node_guid(p_node)),
 			osm_physp_get_port_num(peer), p_node->print_desc);
 
+	if (pkey_mgr_enforce_partition(p_log, sm, peer, enforce))
+		ret = -1;
+
 	return ret;
 }