diff --git a/service/src/com/android/server/connectivity/NetworkDiagnostics.java b/service/src/com/android/server/connectivity/NetworkDiagnostics.java
index 4eba74b6e2..15d0925d8d 100644
--- a/service/src/com/android/server/connectivity/NetworkDiagnostics.java
+++ b/service/src/com/android/server/connectivity/NetworkDiagnostics.java
@@ -19,6 +19,9 @@ package com.android.server.connectivity;
 import static android.system.OsConstants.*;
 
 import static com.android.net.module.util.NetworkStackConstants.ICMP_HEADER_LEN;
+import static com.android.net.module.util.NetworkStackConstants.IPV4_HEADER_MIN_LEN;
+import static com.android.net.module.util.NetworkStackConstants.IPV6_HEADER_LEN;
+import static com.android.net.module.util.NetworkStackConstants.IPV6_MIN_MTU;
 
 import android.annotation.NonNull;
 import android.annotation.Nullable;
@@ -35,6 +38,7 @@ import android.system.ErrnoException;
 import android.system.Os;
 import android.system.StructTimeval;
 import android.text.TextUtils;
+import android.util.Log;
 import android.util.Pair;
 
 import com.android.internal.util.IndentingPrintWriter;
@@ -174,7 +178,7 @@ public class NetworkDiagnostics {
         }
     }
 
-    private final Map<InetAddress, Measurement> mIcmpChecks = new HashMap<>();
+    private final Map<Pair<InetAddress, Integer>, Measurement> mIcmpChecks = new HashMap<>();
     private final Map<Pair<InetAddress, InetAddress>, Measurement> mExplicitSourceIcmpChecks =
             new HashMap<>();
     private final Map<InetAddress, Measurement> mDnsUdpChecks = new HashMap<>();
@@ -207,17 +211,21 @@ public class NetworkDiagnostics {
             mLinkProperties.addDnsServer(TEST_DNS6);
         }
 
+        final int mtu = mLinkProperties.getMtu();
         for (RouteInfo route : mLinkProperties.getRoutes()) {
             if (route.getType() == RouteInfo.RTN_UNICAST && route.hasGateway()) {
                 InetAddress gateway = route.getGateway();
-                prepareIcmpMeasurement(gateway);
+                // Use mtu in the route if exists. Otherwise, use the one in the link property.
+                final int routeMtu = route.getMtu();
+                prepareIcmpMeasurements(gateway, (routeMtu > 0) ? routeMtu : mtu);
                 if (route.isIPv6Default()) {
                     prepareExplicitSourceIcmpMeasurements(gateway);
                 }
             }
         }
+
         for (InetAddress nameserver : mLinkProperties.getDnsServers()) {
-            prepareIcmpMeasurement(nameserver);
+            prepareIcmpMeasurements(nameserver, mtu);
             prepareDnsMeasurement(nameserver);
 
             // Unlike the DnsResolver which doesn't do certificate validation in opportunistic mode,
@@ -263,11 +271,50 @@ public class NetworkDiagnostics {
                 localAddr.getHostAddress(), inetSockAddr.getPort());
     }
 
-    private void prepareIcmpMeasurement(InetAddress target) {
-        if (!mIcmpChecks.containsKey(target)) {
-            Measurement measurement = new Measurement();
-            measurement.thread = new Thread(new IcmpCheck(target, 0, measurement));
-            mIcmpChecks.put(target, measurement);
+    private static int getHeaderLen(@NonNull InetAddress target) {
+        // Convert IPv4 mapped v6 address to v4 if any.
+        try {
+            final InetAddress addr = InetAddress.getByAddress(target.getAddress());
+            // An ICMPv6 header is technically 4 bytes, but the implementation in IcmpCheck#run()
+            // will always fill in another 4 bytes padding in the v6 diagnostic packets, so the size
+            // before icmp data is always 8 bytes in the implementation of ICMP diagnostics for both
+            // v4 and v6 packets. Thus, it's fine to use the v4 header size in the length
+            // calculation.
+            if (addr instanceof Inet6Address) {
+                return IPV6_HEADER_LEN + ICMP_HEADER_LEN;
+            }
+        } catch (UnknownHostException e) {
+            Log.e(TAG, "Create InetAddress fail(" + target + "): " + e);
+        }
+
+        return IPV4_HEADER_MIN_LEN + ICMP_HEADER_LEN;
+    }
+
+    private void prepareIcmpMeasurements(@NonNull InetAddress target, int targetNetworkMtu) {
+        // Test with different size payload ICMP.
+        // 1. Test with 0 payload.
+        addPayloadIcmpMeasurement(target, 0);
+        final int header = getHeaderLen(target);
+        // 2. Test with full size MTU.
+        addPayloadIcmpMeasurement(target, targetNetworkMtu - header);
+        // 3. If v6, make another measurement with the full v6 min MTU, unless that's what
+        //    was done above.
+        if ((target instanceof Inet6Address) && (targetNetworkMtu != IPV6_MIN_MTU)) {
+            addPayloadIcmpMeasurement(target, IPV6_MIN_MTU - header);
+        }
+    }
+
+    private void addPayloadIcmpMeasurement(@NonNull InetAddress target, int payloadLen) {
+        // This can happen if the there is no mtu filled(which is 0) in the link property.
+        // The value becomes negative after minus header length.
+        if (payloadLen < 0) return;
+
+        final Pair<InetAddress, Integer> lenTarget =
+                new Pair<>(target, Integer.valueOf(payloadLen));
+        if (!mIcmpChecks.containsKey(lenTarget)) {
+            final Measurement measurement = new Measurement();
+            measurement.thread = new Thread(new IcmpCheck(target, payloadLen, measurement));
+            mIcmpChecks.put(lenTarget, measurement);
         }
     }
 
@@ -336,8 +383,8 @@ public class NetworkDiagnostics {
         ArrayList<Measurement> measurements = new ArrayList(totalMeasurementCount());
 
         // Sort measurements IPv4 first.
-        for (Map.Entry<InetAddress, Measurement> entry : mIcmpChecks.entrySet()) {
-            if (entry.getKey() instanceof Inet4Address) {
+        for (Map.Entry<Pair<InetAddress, Integer>, Measurement> entry : mIcmpChecks.entrySet()) {
+            if (entry.getKey().first instanceof Inet4Address) {
                 measurements.add(entry.getValue());
             }
         }
@@ -359,8 +406,8 @@ public class NetworkDiagnostics {
         }
 
         // IPv6 measurements second.
-        for (Map.Entry<InetAddress, Measurement> entry : mIcmpChecks.entrySet()) {
-            if (entry.getKey() instanceof Inet6Address) {
+        for (Map.Entry<Pair<InetAddress, Integer>, Measurement> entry : mIcmpChecks.entrySet()) {
+            if (entry.getKey().first instanceof Inet6Address) {
                 measurements.add(entry.getValue());
             }
         }