diff --git a/Tethering/apex/Android.bp b/Tethering/apex/Android.bp
index 611f100..6f0689c 100644
--- a/Tethering/apex/Android.bp
+++ b/Tethering/apex/Android.bp
@@ -66,6 +66,7 @@
     bpfs: [
         "clatd.o_mainline",
         "netd.o_mainline",
+        "dscp_policy.o",
         "offload.o",
         "test.o",
     ],
diff --git a/bpf_progs/Android.bp b/bpf_progs/Android.bp
index fa5af49..cce2b71 100644
--- a/bpf_progs/Android.bp
+++ b/bpf_progs/Android.bp
@@ -42,6 +42,7 @@
         // TODO: remove it when NetworkStatsService is moved into the mainline module and no more
         // calls to JNI in libservices.core.
         "//frameworks/base/services/core/jni",
+        "//packages/modules/Connectivity/service",
         "//packages/modules/Connectivity/service/native/libs/libclat",
         "//packages/modules/Connectivity/Tethering",
         "//packages/modules/Connectivity/service/native",
@@ -56,6 +57,16 @@
 // bpf kernel programs
 //
 bpf {
+    name: "dscp_policy.o",
+    srcs: ["dscp_policy.c"],
+    cflags: [
+        "-Wall",
+        "-Werror",
+    ],
+    sub_dir: "net_shared",
+}
+
+bpf {
     name: "offload.o",
     srcs: ["offload.c"],
     cflags: [
diff --git a/bpf_progs/dscp_policy.c b/bpf_progs/dscp_policy.c
new file mode 100644
index 0000000..9989e6b
--- /dev/null
+++ b/bpf_progs/dscp_policy.c
@@ -0,0 +1,280 @@
+/*
+ * Copyright (C) 2021 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <linux/types.h>
+#include <linux/bpf.h>
+#include <linux/ip.h>
+#include <linux/ipv6.h>
+#include <linux/if_ether.h>
+#include <linux/pkt_cls.h>
+#include <linux/tcp.h>
+#include <stdint.h>
+#include <netinet/in.h>
+#include <netinet/udp.h>
+#include <string.h>
+
+#include "bpf_helpers.h"
+
+#define MAX_POLICIES 16
+#define MAP_A 1
+#define MAP_B 2
+
+#define STRUCT_SIZE(name, size) _Static_assert(sizeof(name) == (size), "Incorrect struct size.")
+
+// TODO: these are already defined in /system/netd/bpf_progs/bpf_net_helpers.h
+// should they be moved to common location?
+static uint64_t (*bpf_get_socket_cookie)(struct __sk_buff* skb) =
+        (void*)BPF_FUNC_get_socket_cookie;
+static int (*bpf_skb_store_bytes)(struct __sk_buff* skb, __u32 offset, const void* from, __u32 len,
+                                  __u64 flags) = (void*)BPF_FUNC_skb_store_bytes;
+static int (*bpf_l3_csum_replace)(struct __sk_buff* skb, __u32 offset, __u64 from, __u64 to,
+                                  __u64 flags) = (void*)BPF_FUNC_l3_csum_replace;
+
+typedef struct {
+    // Add family here to match __sk_buff ?
+    struct in_addr srcIp;
+    struct in_addr dstIp;
+    __be16 srcPort;
+    __be16 dstPort;
+    uint8_t proto;
+    uint8_t dscpVal;
+    uint8_t pad[2];
+} Ipv4RuleEntry;
+STRUCT_SIZE(Ipv4RuleEntry, 2 * 4 + 2 * 2 + 2 * 1 + 2);  // 16, 4 for in_addr
+
+#define SRC_IP_MASK     1
+#define DST_IP_MASK     2
+#define SRC_PORT_MASK   4
+#define DST_PORT_MASK   8
+#define PROTO_MASK      16
+
+typedef struct {
+    struct in6_addr srcIp;
+    struct in6_addr dstIp;
+    __be16 srcPort;
+    __be16 dstPortStart;
+    __be16 dstPortEnd;
+    uint8_t proto;
+    uint8_t dscpVal;
+    uint8_t mask;
+    uint8_t pad[3];
+} Ipv4Policy;
+STRUCT_SIZE(Ipv4Policy, 2 * 16 + 3 * 2 + 3 * 1 + 3);  // 44
+
+typedef struct {
+    struct in6_addr srcIp;
+    struct in6_addr dstIp;
+    __be16 srcPort;
+    __be16 dstPortStart;
+    __be16 dstPortEnd;
+    uint8_t proto;
+    uint8_t dscpVal;
+    uint8_t mask;
+    // should we override this struct to include the param bitmask for linear search?
+    // For mapping socket to policies, all the params should match exactly since we can
+    // pull any missing from the sock itself.
+} Ipv6RuleEntry;
+STRUCT_SIZE(Ipv6RuleEntry, 2 * 16 + 3 * 2 + 3 * 1 + 3);  // 44
+
+// TODO: move to using 1 map. Map v4 address to 0xffff::v4
+DEFINE_BPF_MAP_GRW(ipv4_socket_to_policies_map_A, HASH, uint64_t, Ipv4RuleEntry, MAX_POLICIES,
+        AID_SYSTEM)
+DEFINE_BPF_MAP_GRW(ipv4_socket_to_policies_map_B, HASH, uint64_t, Ipv4RuleEntry, MAX_POLICIES,
+        AID_SYSTEM)
+DEFINE_BPF_MAP_GRW(ipv6_socket_to_policies_map_A, HASH, uint64_t, Ipv6RuleEntry, MAX_POLICIES,
+        AID_SYSTEM)
+DEFINE_BPF_MAP_GRW(ipv6_socket_to_policies_map_B, HASH, uint64_t, Ipv6RuleEntry, MAX_POLICIES,
+        AID_SYSTEM)
+DEFINE_BPF_MAP_GRW(switch_comp_map, ARRAY, int, uint64_t, 1, AID_SYSTEM)
+
+DEFINE_BPF_MAP_GRW(ipv4_dscp_policies_map, ARRAY, uint32_t, Ipv4Policy, MAX_POLICIES,
+        AID_SYSTEM)
+DEFINE_BPF_MAP_GRW(ipv6_dscp_policies_map, ARRAY, uint32_t, Ipv6RuleEntry, MAX_POLICIES,
+        AID_SYSTEM)
+
+DEFINE_BPF_PROG_KVER("schedcls/set_dscp", AID_ROOT, AID_SYSTEM,
+                     schedcls_set_dscp, KVER(5, 4, 0))
+(struct __sk_buff* skb) {
+    int one = 0;
+    uint64_t* selectedMap = bpf_switch_comp_map_lookup_elem(&one);
+
+    // use this with HASH map so map lookup only happens once policies have been added?
+    if (!selectedMap) {
+        return TC_ACT_PIPE;
+    }
+
+    // used for map lookup
+    uint64_t cookie = bpf_get_socket_cookie(skb);
+
+    // Do we need separate maps for ipv4/ipv6
+    if (skb->protocol == htons(ETH_P_IP)) { //maybe bpf_htons()
+        Ipv4RuleEntry* v4Policy;
+        if (*selectedMap == MAP_A) {
+            v4Policy = bpf_ipv4_socket_to_policies_map_A_lookup_elem(&cookie);
+        } else {
+            v4Policy = bpf_ipv4_socket_to_policies_map_B_lookup_elem(&cookie);
+        }
+
+        // How to use bitmask here to compare params efficiently?
+        // TODO: add BPF_PROG_TYPE_SK_SKB prog type to Loader?
+
+        void* data = (void*)(long)skb->data;
+        const void* data_end = (void*)(long)skb->data_end;
+        const struct iphdr* const iph = data;
+
+        // Must have ipv4 header
+        if (data + sizeof(*iph) > data_end) return TC_ACT_PIPE;
+
+        // IP version must be 4
+        if (iph->version != 4) return TC_ACT_PIPE;
+
+        // We cannot handle IP options, just standard 20 byte == 5 dword minimal IPv4 header
+        if (iph->ihl != 5) return TC_ACT_PIPE;
+
+        if (iph->protocol != IPPROTO_UDP) return TC_ACT_PIPE;
+
+        struct udphdr *udp;
+        udp = data + sizeof(struct iphdr); //sizeof(struct ethhdr)
+
+        if ((void*)(udp + 1) > data_end) return TC_ACT_PIPE;
+
+        // Source/destination port in udphdr are stored in be16, need to convert to le16.
+        // This can be done via ntohs or htons. Is there a more preferred way?
+        // Cached policy was found.
+        if (v4Policy && iph->saddr == v4Policy->srcIp.s_addr &&
+                    iph->daddr == v4Policy->dstIp.s_addr &&
+                    ntohs(udp->source) == v4Policy->srcPort &&
+                    ntohs(udp->dest) == v4Policy->dstPort &&
+                    iph->protocol == v4Policy->proto) {
+            // set dscpVal in packet. Least sig 2 bits of TOS
+            // reference ipv4_change_dsfield()
+
+            // TODO: fix checksum...
+            int ecn = iph->tos & 3;
+            uint8_t newDscpVal = (v4Policy->dscpVal << 2) + ecn;
+            int oldDscpVal = iph->tos >> 2;
+            bpf_l3_csum_replace(skb, 1, oldDscpVal, newDscpVal, sizeof(uint8_t));
+            bpf_skb_store_bytes(skb, 1, &newDscpVal, sizeof(uint8_t), 0);
+            return TC_ACT_PIPE;
+        }
+
+        // linear scan ipv4_dscp_policies_map, stored socket params do not match actual
+        int bestScore = -1;
+        uint32_t bestMatch = 0;
+
+        for (register uint64_t i = 0; i < MAX_POLICIES; i++) {
+            int score = 0;
+            uint8_t tempMask = 0;
+            // Using a uint62 in for loop prevents infinite loop during BPF load,
+            // but the key is uint32, so convert back.
+            uint32_t key = i;
+            Ipv4Policy* policy = bpf_ipv4_dscp_policies_map_lookup_elem(&key);
+
+            // if mask is 0 continue, key does not have corresponding policy value
+            if (policy && policy->mask != 0) {
+                if ((policy->mask & SRC_IP_MASK) == SRC_IP_MASK &&
+                        iph->saddr == policy->srcIp.s6_addr32[3]) {
+                    score++;
+                    tempMask |= SRC_IP_MASK;
+                }
+                if ((policy->mask & DST_IP_MASK) == DST_IP_MASK &&
+                        iph->daddr == policy->dstIp.s6_addr32[3]) {
+                    score++;
+                    tempMask |= DST_IP_MASK;
+                }
+                if ((policy->mask & SRC_PORT_MASK) == SRC_PORT_MASK &&
+                        ntohs(udp->source) == htons(policy->srcPort)) {
+                    score++;
+                    tempMask |= SRC_PORT_MASK;
+                }
+                if ((policy->mask & DST_PORT_MASK) == DST_PORT_MASK &&
+                        ntohs(udp->dest) >= htons(policy->dstPortStart) &&
+                        ntohs(udp->dest) <= htons(policy->dstPortEnd)) {
+                    score++;
+                    tempMask |= DST_PORT_MASK;
+                }
+                if ((policy->mask & PROTO_MASK) == PROTO_MASK &&
+                        iph->protocol == policy->proto) {
+                    score++;
+                    tempMask |= PROTO_MASK;
+                }
+
+                if (score > bestScore && tempMask == policy->mask) {
+                    bestMatch = i;
+                    bestScore = score;
+                }
+            }
+        }
+
+        uint8_t newDscpVal = 0; // Can 0 be used as default forwarding value?
+        uint8_t curDscp = iph->tos & 252;
+        if (bestScore > 0) {
+            Ipv4Policy* policy = bpf_ipv4_dscp_policies_map_lookup_elem(&bestMatch);
+            if (policy) {
+                // TODO: if DSCP value is already set ignore?
+                // TODO: update checksum, for testing increment counter...
+                int ecn = iph->tos & 3;
+                newDscpVal = (policy->dscpVal << 2) + ecn;
+            }
+        }
+
+        Ipv4RuleEntry value = {
+            .srcIp.s_addr = iph->saddr,
+            .dstIp.s_addr = iph->daddr,
+            .srcPort = udp->source,
+            .dstPort = udp->dest,
+            .proto = iph->protocol,
+            .dscpVal = newDscpVal,
+        };
+
+        if (!cookie)
+            return TC_ACT_PIPE;
+
+        // Update map
+        if (*selectedMap == MAP_A) {
+            bpf_ipv4_socket_to_policies_map_A_update_elem(&cookie, &value, BPF_ANY);
+        } else {
+            bpf_ipv4_socket_to_policies_map_B_update_elem(&cookie, &value, BPF_ANY);
+        }
+
+        // Need to store bytes after updating map or program will not load.
+        if (newDscpVal != curDscp) {
+            // 1 is the offset (Version/Header length)
+            int oldDscpVal = iph->tos >> 2;
+            bpf_l3_csum_replace(skb, 1, oldDscpVal, newDscpVal, sizeof(uint8_t));
+            bpf_skb_store_bytes(skb, 1, &newDscpVal, sizeof(uint8_t), 0);
+        }
+
+    } else if (skb->protocol == htons(ETH_P_IPV6)) { //maybe bpf_htons()
+        Ipv6RuleEntry* v6Policy;
+        if (*selectedMap == MAP_A) {
+            v6Policy = bpf_ipv6_socket_to_policies_map_A_lookup_elem(&cookie);
+        } else {
+            v6Policy = bpf_ipv6_socket_to_policies_map_B_lookup_elem(&cookie);
+        }
+
+        if (!v6Policy)
+            return TC_ACT_PIPE;
+
+        // TODO: Add code to process IPv6 packet.
+    }
+
+    // Always return TC_ACT_PIPE
+    return TC_ACT_PIPE;
+}
+
+LICENSE("Apache 2.0");
+CRITICAL("Connectivity");
diff --git a/framework/aidl-export/android/net/DscpPolicy.aidl b/framework/aidl-export/android/net/DscpPolicy.aidl
new file mode 100644
index 0000000..8da42ca
--- /dev/null
+++ b/framework/aidl-export/android/net/DscpPolicy.aidl
@@ -0,0 +1,19 @@
+/*
+ * Copyright (C) 2021 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+package android.net;
+
+@JavaOnlyStableParcelable parcelable DscpPolicy;
diff --git a/framework/api/system-current.txt b/framework/api/system-current.txt
index b4b3588..d420958 100644
--- a/framework/api/system-current.txt
+++ b/framework/api/system-current.txt
@@ -93,6 +93,35 @@
     method @Deprecated public void onUpstreamChanged(@Nullable android.net.Network);
   }
 
+  public final class DscpPolicy implements android.os.Parcelable {
+    method @Nullable public java.net.InetAddress getDestinationAddress();
+    method @Nullable public android.util.Range<java.lang.Integer> getDestinationPortRange();
+    method public int getDscpValue();
+    method public int getPolicyId();
+    method public int getProtocol();
+    method @Nullable public java.net.InetAddress getSourceAddress();
+    method public int getSourcePort();
+    field @NonNull public static final android.os.Parcelable.Creator<android.net.DscpPolicy> CREATOR;
+    field public static final int PROTOCOL_ANY = -1; // 0xffffffff
+    field public static final int SOURCE_PORT_ANY = -1; // 0xffffffff
+    field public static final int STATUS_DELETED = 4; // 0x4
+    field public static final int STATUS_INSUFFICIENT_PROCESSING_RESOURCES = 3; // 0x3
+    field public static final int STATUS_POLICY_NOT_FOUND = 5; // 0x5
+    field public static final int STATUS_REQUESTED_CLASSIFIER_NOT_SUPPORTED = 2; // 0x2
+    field public static final int STATUS_REQUEST_DECLINED = 1; // 0x1
+    field public static final int STATUS_SUCCESS = 0; // 0x0
+  }
+
+  public static final class DscpPolicy.Builder {
+    ctor public DscpPolicy.Builder(int, int);
+    method @NonNull public android.net.DscpPolicy build();
+    method @NonNull public android.net.DscpPolicy.Builder setDestinationAddress(@NonNull java.net.InetAddress);
+    method @NonNull public android.net.DscpPolicy.Builder setDestinationPortRange(@NonNull android.util.Range<java.lang.Integer>);
+    method @NonNull public android.net.DscpPolicy.Builder setProtocol(int);
+    method @NonNull public android.net.DscpPolicy.Builder setSourceAddress(@NonNull java.net.InetAddress);
+    method @NonNull public android.net.DscpPolicy.Builder setSourcePort(int);
+  }
+
   public final class InvalidPacketException extends java.lang.Exception {
     ctor public InvalidPacketException(int);
     method public int getError();
@@ -217,6 +246,7 @@
     method public void onAddKeepalivePacketFilter(int, @NonNull android.net.KeepalivePacketData);
     method public void onAutomaticReconnectDisabled();
     method public void onBandwidthUpdateRequested();
+    method public void onDscpPolicyStatusUpdated(int, int);
     method public void onNetworkCreated();
     method public void onNetworkDestroyed();
     method public void onNetworkUnwanted();
@@ -229,6 +259,7 @@
     method public void onStopSocketKeepalive(int);
     method public void onValidationStatus(int, @Nullable android.net.Uri);
     method @NonNull public android.net.Network register();
+    method public void sendAddDscpPolicy(@NonNull android.net.DscpPolicy);
     method public final void sendLinkProperties(@NonNull android.net.LinkProperties);
     method public final void sendNetworkCapabilities(@NonNull android.net.NetworkCapabilities);
     method public final void sendNetworkScore(@NonNull android.net.NetworkScore);
@@ -236,6 +267,8 @@
     method public final void sendQosCallbackError(int, int);
     method public final void sendQosSessionAvailable(int, int, @NonNull android.net.QosSessionAttributes);
     method public final void sendQosSessionLost(int, int, int);
+    method public void sendRemoveAllDscpPolicies();
+    method public void sendRemoveDscpPolicy(int);
     method public final void sendSocketKeepaliveEvent(int, int);
     method @Deprecated public void setLegacySubtype(int, @NonNull String);
     method public void setLingerDuration(@NonNull java.time.Duration);
diff --git a/framework/src/android/net/DscpPolicy.java b/framework/src/android/net/DscpPolicy.java
new file mode 100644
index 0000000..cda8205
--- /dev/null
+++ b/framework/src/android/net/DscpPolicy.java
@@ -0,0 +1,397 @@
+/*
+ * Copyright (C) 2021 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+package android.net;
+
+import android.annotation.IntDef;
+import android.annotation.NonNull;
+import android.annotation.Nullable;
+import android.annotation.SystemApi;
+import android.os.Parcel;
+import android.os.Parcelable;
+import android.util.Range;
+
+import com.android.net.module.util.InetAddressUtils;
+
+import java.lang.annotation.Retention;
+import java.lang.annotation.RetentionPolicy;
+import java.net.Inet6Address;
+import java.net.InetAddress;
+import java.util.Objects;
+
+
+/**
+ * DSCP policy to be set on the requesting NetworkAgent.
+ * @hide
+ */
+@SystemApi
+public final class DscpPolicy implements Parcelable {
+     /**
+     * Indicates that the policy does not specify a protocol.
+     */
+    public static final int PROTOCOL_ANY = -1;
+
+    /**
+     * Indicates that the policy does not specify a port.
+     */
+    public static final int SOURCE_PORT_ANY = -1;
+
+    /**
+     * Policy was successfully added.
+     */
+    public static final int STATUS_SUCCESS = 0;
+
+    /**
+     * Policy was rejected for any reason besides invalid classifier or insufficient resources.
+     */
+    public static final int STATUS_REQUEST_DECLINED = 1;
+
+    /**
+     * Requested policy contained a classifier which is not supported.
+     */
+    public static final int STATUS_REQUESTED_CLASSIFIER_NOT_SUPPORTED = 2;
+
+    /**
+     * TODO: should this error case be supported?
+     */
+    public static final int STATUS_INSUFFICIENT_PROCESSING_RESOURCES = 3;
+
+    /**
+     * Policy was deleted.
+     */
+    public static final int STATUS_DELETED = 4;
+
+    /**
+     * Policy was not found during deletion.
+     */
+    public static final int STATUS_POLICY_NOT_FOUND = 5;
+
+    /** The unique policy ID. Each requesting network is responsible for maintaining policy IDs
+     * unique within that network. In the case where a policy with an existing ID is created, the
+     * new policy will update the existing policy with the same ID.
+     */
+    private final int mPolicyId;
+
+    /** The QoS DSCP marking to be added to packets matching the policy. */
+    private final int mDscp;
+
+    /** The source IP address. */
+    private final @Nullable InetAddress mSrcAddr;
+
+    /** The destination IP address. */
+    private final @Nullable InetAddress mDstAddr;
+
+    /** The source port. */
+    private final int mSrcPort;
+
+    /** The IP protocol that the policy requires. */
+    private final int mProtocol;
+
+    /** Destination port range. Inclusive range. */
+    private final @Nullable Range<Integer> mDstPortRange;
+
+    /**
+     * Implement the Parcelable interface
+     *
+     * @hide
+     */
+    public int describeContents() {
+        return 0;
+    }
+
+    /** @hide */
+    @IntDef(prefix = "STATUS_", value = {
+        STATUS_SUCCESS,
+        STATUS_REQUEST_DECLINED,
+        STATUS_REQUESTED_CLASSIFIER_NOT_SUPPORTED,
+        STATUS_INSUFFICIENT_PROCESSING_RESOURCES,
+        STATUS_DELETED
+    })
+    @Retention(RetentionPolicy.SOURCE)
+    public @interface DscpPolicyStatus {}
+
+    /* package */ DscpPolicy(
+            int policyId,
+            int dscp,
+            @Nullable InetAddress srcAddr,
+            @Nullable InetAddress dstAddr,
+            int srcPort,
+            int protocol,
+            Range<Integer> dstPortRange) {
+        this.mPolicyId = policyId;
+        this.mDscp = dscp;
+        this.mSrcAddr = srcAddr;
+        this.mDstAddr = dstAddr;
+        this.mSrcPort = srcPort;
+        this.mProtocol = protocol;
+        this.mDstPortRange = dstPortRange;
+
+        if (mPolicyId < 1 || mPolicyId > 255) {
+            throw new IllegalArgumentException("Policy ID not in valid range: " + mPolicyId);
+        }
+        if (mDscp < 0 || mDscp > 63) {
+            throw new IllegalArgumentException("DSCP value not in valid range: " + mDscp);
+        }
+        // Since SOURCE_PORT_ANY is the default source port value need to allow it as well.
+        // TODO: Move the default value into this constructor or throw an error from the
+        // instead.
+        if (mSrcPort < -1 || mSrcPort > 65535) {
+            throw new IllegalArgumentException("Source port not in valid range: " + mSrcPort);
+        }
+        if (mDstPortRange != null
+                && (dstPortRange.getLower() < 0 || mDstPortRange.getLower() > 65535)
+                && (mDstPortRange.getUpper() < 0 || mDstPortRange.getUpper() > 65535)) {
+            throw new IllegalArgumentException("Destination port not in valid range");
+        }
+        if (mSrcAddr != null && mDstAddr != null && (mSrcAddr instanceof Inet6Address)
+                != (mDstAddr instanceof Inet6Address)) {
+            throw new IllegalArgumentException("Source/destination address of different family");
+        }
+    }
+
+    /**
+     * The unique policy ID.
+     *
+     * Each requesting network is responsible for maintaining unique
+     * policy IDs. In the case where a policy with an existing ID is created, the new
+     * policy will update the existing policy with the same ID
+     *
+     * @return Policy ID set in Builder.
+     */
+    public int getPolicyId() {
+        return mPolicyId;
+    }
+
+    /**
+     * The QoS DSCP marking to be added to packets matching the policy.
+     *
+     * @return DSCP value set in Builder.
+     */
+    public int getDscpValue() {
+        return mDscp;
+    }
+
+    /**
+     * The source IP address.
+     *
+     * @return Source IP address set in Builder or {@code null} if none was set.
+     */
+    public @Nullable InetAddress getSourceAddress() {
+        return mSrcAddr;
+    }
+
+    /**
+     * The destination IP address.
+     *
+     * @return Destination IP address set in Builder or {@code null} if none was set.
+     */
+    public @Nullable InetAddress getDestinationAddress() {
+        return mDstAddr;
+    }
+
+    /**
+     * The source port.
+     *
+     * @return Source port set in Builder or {@link #SOURCE_PORT_ANY} if no port was set.
+     */
+    public int getSourcePort() {
+        return mSrcPort;
+    }
+
+    /**
+     * The IP protocol that the policy requires.
+     *
+     * @return Protocol set in Builder or {@link #PROTOCOL_ANY} if no protocol was set.
+     *         {@link #PROTOCOL_ANY} indicates that any protocol will be matched.
+     */
+    public int getProtocol() {
+        return mProtocol;
+    }
+
+    /**
+     * Destination port range. Inclusive range.
+     *
+     * @return Range<Integer> set in Builder or {@code null} if none was set.
+     */
+    public @Nullable Range<Integer> getDestinationPortRange() {
+        return mDstPortRange;
+    }
+
+    @Override
+    public String toString() {
+        return "DscpPolicy { "
+                + "policyId = " + mPolicyId + ", "
+                + "dscp = " + mDscp + ", "
+                + "srcAddr = " + mSrcAddr + ", "
+                + "dstAddr = " + mDstAddr + ", "
+                + "srcPort = " + mSrcPort + ", "
+                + "protocol = " + mProtocol + ", "
+                + "dstPortRange = "
+                + (mDstPortRange == null ? "none" : mDstPortRange.toString())
+                + " }";
+    }
+
+    @Override
+    public boolean equals(@Nullable Object o) {
+        if (this == o) return true;
+        if (!(o instanceof DscpPolicy)) return false;
+        DscpPolicy that = (DscpPolicy) o;
+        return true
+                && mPolicyId == that.mPolicyId
+                && mDscp == that.mDscp
+                && Objects.equals(mSrcAddr, that.mSrcAddr)
+                && Objects.equals(mDstAddr, that.mDstAddr)
+                && mSrcPort == that.mSrcPort
+                && mProtocol == that.mProtocol
+                && Objects.equals(mDstPortRange, that.mDstPortRange);
+    }
+
+    @Override
+    public int hashCode() {
+        return Objects.hash(mPolicyId, mDscp, mSrcAddr.hashCode(),
+                mDstAddr.hashCode(), mSrcPort, mProtocol, mDstPortRange.hashCode());
+    }
+
+    /** @hide */
+    @Override
+    public void writeToParcel(@NonNull Parcel dest, int flags) {
+        dest.writeInt(mPolicyId);
+        dest.writeInt(mDscp);
+        InetAddressUtils.parcelInetAddress(dest, mSrcAddr, flags);
+        InetAddressUtils.parcelInetAddress(dest, mDstAddr, flags);
+        dest.writeInt(mSrcPort);
+        dest.writeInt(mProtocol);
+        dest.writeBoolean(mDstPortRange != null ? true : false);
+        if (mDstPortRange != null) {
+            dest.writeInt(mDstPortRange.getLower());
+            dest.writeInt(mDstPortRange.getUpper());
+        }
+    }
+
+    /** @hide */
+    DscpPolicy(@NonNull Parcel in) {
+        this.mPolicyId = in.readInt();
+        this.mDscp = in.readInt();
+        this.mSrcAddr = InetAddressUtils.unparcelInetAddress(in);
+        this.mDstAddr = InetAddressUtils.unparcelInetAddress(in);
+        this.mSrcPort = in.readInt();
+        this.mProtocol = in.readInt();
+        if (in.readBoolean()) {
+            this.mDstPortRange = new Range<Integer>(in.readInt(), in.readInt());
+        } else {
+            this.mDstPortRange = null;
+        }
+    }
+
+    /** @hide */
+    public @SystemApi static final @NonNull Parcelable.Creator<DscpPolicy> CREATOR =
+            new Parcelable.Creator<DscpPolicy>() {
+                @Override
+                public DscpPolicy[] newArray(int size) {
+                    return new DscpPolicy[size];
+                }
+
+                @Override
+                public DscpPolicy createFromParcel(@NonNull android.os.Parcel in) {
+                    return new DscpPolicy(in);
+                }
+            };
+
+    /**
+     * A builder for {@link DscpPolicy}
+     *
+     */
+    public static final class Builder {
+
+        private final int mPolicyId;
+        private final int mDscp;
+        private @Nullable InetAddress mSrcAddr;
+        private @Nullable InetAddress mDstAddr;
+        private int mSrcPort = SOURCE_PORT_ANY;
+        private int mProtocol = PROTOCOL_ANY;
+        private @Nullable Range<Integer> mDstPortRange;
+
+        private long mBuilderFieldsSet = 0L;
+
+        /**
+         * Creates a new Builder.
+         *
+         * @param policyId The unique policy ID. Each requesting network is responsible for
+         *                 maintaining unique policy IDs. In the case where a policy with an
+         *                 existing ID is created, the new policy will update the existing
+         *                 policy with the same ID
+         * @param dscpValue The DSCP value to set.
+         */
+        public Builder(int policyId, int dscpValue) {
+            mPolicyId = policyId;
+            mDscp = dscpValue;
+        }
+
+        /**
+         * Specifies that this policy matches packets with the specified source IP address.
+         */
+        public @NonNull Builder setSourceAddress(@NonNull InetAddress value) {
+            mSrcAddr = value;
+            return this;
+        }
+
+        /**
+         * Specifies that this policy matches packets with the specified destination IP address.
+         */
+        public @NonNull Builder setDestinationAddress(@NonNull InetAddress value) {
+            mDstAddr = value;
+            return this;
+        }
+
+        /**
+         * Specifies that this policy matches packets with the specified source port.
+         */
+        public @NonNull Builder setSourcePort(int value) {
+            mSrcPort = value;
+            return this;
+        }
+
+        /**
+         * Specifies that this policy matches packets with the specified protocol.
+         */
+        public @NonNull Builder setProtocol(int value) {
+            mProtocol = value;
+            return this;
+        }
+
+        /**
+         * Specifies that this policy matches packets with the specified destination port range.
+         */
+        public @NonNull Builder setDestinationPortRange(@NonNull Range<Integer> range) {
+            mDstPortRange = range;
+            return this;
+        }
+
+        /**
+         * Constructs a DscpPolicy with the specified parameters.
+         */
+        public @NonNull DscpPolicy build() {
+            return new DscpPolicy(
+                    mPolicyId,
+                    mDscp,
+                    mSrcAddr,
+                    mDstAddr,
+                    mSrcPort,
+                    mProtocol,
+                    mDstPortRange);
+        }
+    }
+}
diff --git a/framework/src/android/net/INetworkAgent.aidl b/framework/src/android/net/INetworkAgent.aidl
index d941d4b..fa5175c 100644
--- a/framework/src/android/net/INetworkAgent.aidl
+++ b/framework/src/android/net/INetworkAgent.aidl
@@ -48,4 +48,5 @@
     void onQosCallbackUnregistered(int qosCallbackId);
     void onNetworkCreated();
     void onNetworkDestroyed();
+    void onDscpPolicyStatusUpdated(int policyId, int status);
 }
diff --git a/framework/src/android/net/INetworkAgentRegistry.aidl b/framework/src/android/net/INetworkAgentRegistry.aidl
index 9a58add..08536ca 100644
--- a/framework/src/android/net/INetworkAgentRegistry.aidl
+++ b/framework/src/android/net/INetworkAgentRegistry.aidl
@@ -15,6 +15,7 @@
  */
 package android.net;
 
+import android.net.DscpPolicy;
 import android.net.LinkProperties;
 import android.net.Network;
 import android.net.NetworkCapabilities;
@@ -43,4 +44,7 @@
     void sendQosCallbackError(int qosCallbackId, int exceptionType);
     void sendTeardownDelayMs(int teardownDelayMs);
     void sendLingerDuration(int durationMs);
+    void sendAddDscpPolicy(in DscpPolicy policy);
+    void sendRemoveDscpPolicy(int policyId);
+    void sendRemoveAllDscpPolicies();
 }
diff --git a/framework/src/android/net/NetworkAgent.java b/framework/src/android/net/NetworkAgent.java
index adcf338..945e670 100644
--- a/framework/src/android/net/NetworkAgent.java
+++ b/framework/src/android/net/NetworkAgent.java
@@ -25,6 +25,7 @@
 import android.annotation.TestApi;
 import android.compat.annotation.UnsupportedAppUsage;
 import android.content.Context;
+import android.net.DscpPolicy.DscpPolicyStatus;
 import android.os.Build;
 import android.os.Bundle;
 import android.os.ConditionVariable;
@@ -404,6 +405,35 @@
      */
     public static final int EVENT_LINGER_DURATION_CHANGED = BASE + 24;
 
+    /**
+     * Sent by the NetworkAgent to ConnectivityService to set add a DSCP policy.
+     *
+     * @hide
+     */
+    public static final int EVENT_ADD_DSCP_POLICY = BASE + 25;
+
+    /**
+     * Sent by the NetworkAgent to ConnectivityService to set remove a DSCP policy.
+     *
+     * @hide
+     */
+    public static final int EVENT_REMOVE_DSCP_POLICY = BASE + 26;
+
+    /**
+     * Sent by the NetworkAgent to ConnectivityService to remove all DSCP policies.
+     *
+     * @hide
+     */
+    public static final int EVENT_REMOVE_ALL_DSCP_POLICIES = BASE + 27;
+
+    /**
+     * Sent by ConnectivityService to {@link NetworkAgent} to inform the agent of an updated
+     * status for a DSCP policy.
+     *
+     * @hide
+     */
+    public static final int CMD_DSCP_POLICY_STATUS = BASE + 28;
+
     private static NetworkInfo getLegacyNetworkInfo(final NetworkAgentConfig config) {
         final NetworkInfo ni = new NetworkInfo(config.legacyType, config.legacySubType,
                 config.legacyTypeName, config.legacySubTypeName);
@@ -611,6 +641,12 @@
                     onNetworkDestroyed();
                     break;
                 }
+                case CMD_DSCP_POLICY_STATUS: {
+                    onDscpPolicyStatusUpdated(
+                            msg.arg1 /* Policy ID */,
+                            msg.arg2 /* DSCP Policy Status */);
+                    break;
+                }
             }
         }
     }
@@ -761,6 +797,13 @@
         public void onNetworkDestroyed() {
             mHandler.sendMessage(mHandler.obtainMessage(CMD_NETWORK_DESTROYED));
         }
+
+        @Override
+        public void onDscpPolicyStatusUpdated(final int policyId,
+                @DscpPolicyStatus final int status) {
+            mHandler.sendMessage(mHandler.obtainMessage(
+                    CMD_DSCP_POLICY_STATUS, policyId, status));
+        }
     }
 
     /**
@@ -1104,6 +1147,11 @@
     public void onNetworkDestroyed() {}
 
     /**
+     * Called when when the DSCP Policy status has changed.
+     */
+    public void onDscpPolicyStatusUpdated(int policyId, @DscpPolicyStatus int status) {}
+
+    /**
      * Requests that the network hardware send the specified packet at the specified interval.
      *
      * @param slot the hardware slot on which to start the keepalive.
@@ -1317,6 +1365,30 @@
         queueOrSendMessage(ra -> ra.sendLingerDuration((int) durationMs));
     }
 
+    /**
+     * Add a DSCP Policy.
+     * @param policy the DSCP policy to be added.
+     */
+    public void sendAddDscpPolicy(@NonNull final DscpPolicy policy) {
+        Objects.requireNonNull(policy);
+        queueOrSendMessage(ra -> ra.sendAddDscpPolicy(policy));
+    }
+
+    /**
+     * Remove the specified DSCP policy.
+     * @param policyId the ID corresponding to a specific DSCP Policy.
+     */
+    public void sendRemoveDscpPolicy(final int policyId) {
+        queueOrSendMessage(ra -> ra.sendRemoveDscpPolicy(policyId));
+    }
+
+    /**
+     * Remove all the DSCP policies on this network.
+     */
+    public void sendRemoveAllDscpPolicies() {
+        queueOrSendMessage(ra -> ra.sendRemoveAllDscpPolicies());
+    }
+
     /** @hide */
     protected void log(final String s) {
         Log.d(LOG_TAG, "NetworkAgent: " + s);
diff --git a/service/Android.bp b/service/Android.bp
index 3afd635..306bd42 100644
--- a/service/Android.bp
+++ b/service/Android.bp
@@ -36,6 +36,7 @@
     stl: "libc++_static",
     static_libs: [
         "libnet_utils_device_common_bpfjni",
+        "libtcutils",
     ],
     shared_libs: [
         "liblog",
diff --git a/service/jni/com_android_net_module_util/onload.cpp b/service/jni/com_android_net_module_util/onload.cpp
index 1d17622..07ae31c 100644
--- a/service/jni/com_android_net_module_util/onload.cpp
+++ b/service/jni/com_android_net_module_util/onload.cpp
@@ -20,6 +20,7 @@
 namespace android {
 
 int register_com_android_net_module_util_BpfMap(JNIEnv* env, char const* class_name);
+int register_com_android_net_module_util_TcUtils(JNIEnv* env, char const* class_name);
 
 extern "C" jint JNI_OnLoad(JavaVM* vm, void*) {
     JNIEnv *env;
@@ -31,6 +32,9 @@
     if (register_com_android_net_module_util_BpfMap(env,
             "com/android/connectivity/com/android/net/module/util/BpfMap") < 0) return JNI_ERR;
 
+    if (register_com_android_net_module_util_TcUtils(env,
+            "com/android/connectivity/com/android/net/module/util/TcUtils") < 0) return JNI_ERR;
+
     return JNI_VERSION_1_6;
 }
 
diff --git a/service/src/com/android/server/ConnectivityService.java b/service/src/com/android/server/ConnectivityService.java
index 145eade..d2c0e18 100644
--- a/service/src/com/android/server/ConnectivityService.java
+++ b/service/src/com/android/server/ConnectivityService.java
@@ -126,6 +126,7 @@
 import android.net.ConnectivitySettingsManager;
 import android.net.DataStallReportParcelable;
 import android.net.DnsResolverServiceManager;
+import android.net.DscpPolicy;
 import android.net.ICaptivePortal;
 import android.net.IConnectivityDiagnosticsCallback;
 import android.net.IConnectivityManager;
@@ -220,6 +221,7 @@
 import android.os.UserManager;
 import android.provider.Settings;
 import android.sysprop.NetworkProperties;
+import android.system.ErrnoException;
 import android.telephony.TelephonyManager;
 import android.text.TextUtils;
 import android.util.ArrayMap;
@@ -250,6 +252,7 @@
 import com.android.server.connectivity.ConnectivityFlags;
 import com.android.server.connectivity.DnsManager;
 import com.android.server.connectivity.DnsManager.PrivateDnsValidationUpdate;
+import com.android.server.connectivity.DscpPolicyTracker;
 import com.android.server.connectivity.FullScore;
 import com.android.server.connectivity.KeepaliveTracker;
 import com.android.server.connectivity.LingerMonitor;
@@ -389,6 +392,7 @@
     protected IDnsResolver mDnsResolver;
     @VisibleForTesting
     protected INetd mNetd;
+    private DscpPolicyTracker mDscpPolicyTracker = null;
     private NetworkStatsManager mStatsManager;
     private NetworkPolicyManager mPolicyManager;
     private final NetdCallback mNetdCallback;
@@ -1489,6 +1493,12 @@
                 new NetworkScore.Builder().setLegacyInt(0).build(), mContext, null,
                 new NetworkAgentConfig(), this, null, null, 0, INVALID_UID,
                 mLingerDelayMs, mQosCallbackTracker, mDeps);
+
+        try {
+            mDscpPolicyTracker = new DscpPolicyTracker();
+        } catch (ErrnoException e) {
+            loge("Unable to create DscpPolicyTracker");
+        }
     }
 
     private static NetworkCapabilities createDefaultNetworkCapabilitiesForUid(int uid) {
@@ -3406,6 +3416,25 @@
                     nai.setLingerDuration((int) arg.second);
                     break;
                 }
+                case NetworkAgent.EVENT_ADD_DSCP_POLICY: {
+                    DscpPolicy policy = (DscpPolicy) arg.second;
+                    if (mDscpPolicyTracker != null) {
+                        mDscpPolicyTracker.addDscpPolicy(nai, policy);
+                    }
+                    break;
+                }
+                case NetworkAgent.EVENT_REMOVE_DSCP_POLICY: {
+                    if (mDscpPolicyTracker != null) {
+                        mDscpPolicyTracker.removeDscpPolicy(nai, (int) arg.second);
+                    }
+                    break;
+                }
+                case NetworkAgent.EVENT_REMOVE_ALL_DSCP_POLICIES: {
+                    if (mDscpPolicyTracker != null) {
+                        mDscpPolicyTracker.removeAllDscpPolicies(nai);
+                    }
+                    break;
+                }
             }
         }
 
diff --git a/service/src/com/android/server/connectivity/DscpPolicyTracker.java b/service/src/com/android/server/connectivity/DscpPolicyTracker.java
new file mode 100644
index 0000000..0512a44
--- /dev/null
+++ b/service/src/com/android/server/connectivity/DscpPolicyTracker.java
@@ -0,0 +1,275 @@
+/*
+ * Copyright (C) 2021 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+package com.android.server.connectivity;
+
+import static android.net.DscpPolicy.STATUS_DELETED;
+import static android.net.DscpPolicy.STATUS_INSUFFICIENT_PROCESSING_RESOURCES;
+import static android.net.DscpPolicy.STATUS_POLICY_NOT_FOUND;
+import static android.net.DscpPolicy.STATUS_SUCCESS;
+import static android.system.OsConstants.ETH_P_ALL;
+
+import android.annotation.NonNull;
+import android.net.DscpPolicy;
+import android.os.RemoteException;
+import android.system.ErrnoException;
+import android.util.Log;
+import android.util.SparseIntArray;
+
+import com.android.net.module.util.BpfMap;
+import com.android.net.module.util.Struct;
+import com.android.net.module.util.TcUtils;
+
+import java.io.IOException;
+import java.net.Inet4Address;
+import java.net.Inet6Address;
+import java.net.NetworkInterface;
+import java.util.HashSet;
+import java.util.Set;
+
+/**
+ * DscpPolicyTracker has a single entry point from ConnectivityService handler.
+ * This guarantees that all code runs on the same thread and no locking is needed.
+ */
+public class DscpPolicyTracker {
+    static {
+        System.loadLibrary("com_android_connectivity_com_android_net_module_util_jni");
+    }
+
+    // After tethering and clat priorities.
+    static final short PRIO_DSCP = 5;
+
+    private static final String TAG = DscpPolicyTracker.class.getSimpleName();
+    private static final String PROG_PATH =
+            "/sys/fs/bpf/prog_dscp_policy_schedcls_set_dscp";
+    // Name is "map + *.o + map_name + map". Can probably shorten this
+    private static final String IPV4_POLICY_MAP_PATH = makeMapPath(
+            "dscp_policy_ipv4_dscp_policies");
+    private static final String IPV6_POLICY_MAP_PATH = makeMapPath(
+            "dscp_policy_ipv6_dscp_policies");
+    private static final int MAX_POLICIES = 16;
+
+    private static String makeMapPath(String which) {
+        return "/sys/fs/bpf/map_" + which + "_map";
+    }
+
+    private Set<String> mAttachedIfaces;
+
+    private final BpfMap<Struct.U32, DscpPolicyValue> mBpfDscpIpv4Policies;
+    private final BpfMap<Struct.U32, DscpPolicyValue> mBpfDscpIpv6Policies;
+    private final SparseIntArray mPolicyIdToBpfMapIndex;
+
+    public DscpPolicyTracker() throws ErrnoException {
+        mAttachedIfaces = new HashSet<String>();
+
+        mPolicyIdToBpfMapIndex = new SparseIntArray(MAX_POLICIES);
+        mBpfDscpIpv4Policies = new BpfMap<Struct.U32, DscpPolicyValue>(IPV4_POLICY_MAP_PATH,
+                BpfMap.BPF_F_RDWR, Struct.U32.class, DscpPolicyValue.class);
+        mBpfDscpIpv6Policies = new BpfMap<Struct.U32, DscpPolicyValue>(IPV6_POLICY_MAP_PATH,
+                BpfMap.BPF_F_RDWR, Struct.U32.class, DscpPolicyValue.class);
+    }
+
+    private int getFirstFreeIndex() {
+        for (int i = 0; i < MAX_POLICIES; i++) {
+            if (mPolicyIdToBpfMapIndex.indexOfValue(i) < 0) return i;
+        }
+        return MAX_POLICIES;
+    }
+
+    private void sendStatus(NetworkAgentInfo nai, int policyId, int status) {
+        try {
+            nai.networkAgent.onDscpPolicyStatusUpdated(policyId, status);
+        } catch (RemoteException e) {
+            Log.d(TAG, "Failed update policy status: ", e);
+        }
+    }
+
+    private boolean matchesIpv4(DscpPolicy policy) {
+        return ((policy.getDestinationAddress() == null
+                       || policy.getDestinationAddress() instanceof Inet4Address)
+            && (policy.getSourceAddress() == null
+                        || policy.getSourceAddress() instanceof Inet4Address));
+    }
+
+    private boolean matchesIpv6(DscpPolicy policy) {
+        return ((policy.getDestinationAddress() == null
+                       || policy.getDestinationAddress() instanceof Inet6Address)
+            && (policy.getSourceAddress() == null
+                        || policy.getSourceAddress() instanceof Inet6Address));
+    }
+
+    private int addDscpPolicyInternal(DscpPolicy policy) {
+        // If there is no existing policy with a matching ID, and we are already at
+        // the maximum number of policies then return INSUFFICIENT_PROCESSING_RESOURCES.
+        final int existingIndex = mPolicyIdToBpfMapIndex.get(policy.getPolicyId(), -1);
+        if (existingIndex == -1 && mPolicyIdToBpfMapIndex.size() >= MAX_POLICIES) {
+            return STATUS_INSUFFICIENT_PROCESSING_RESOURCES;
+        }
+
+        // Currently all classifiers are supported, if any are removed return
+        // STATUS_REQUESTED_CLASSIFIER_NOT_SUPPORTED,
+        // and for any other generic error STATUS_REQUEST_DECLINED
+
+        int addIndex = 0;
+        // If a policy with a matching ID exists, replace it, otherwise use the next free
+        // index for the policy.
+        if (existingIndex != -1) {
+            addIndex = mPolicyIdToBpfMapIndex.get(policy.getPolicyId());
+        } else {
+            addIndex = getFirstFreeIndex();
+        }
+
+        try {
+            mPolicyIdToBpfMapIndex.put(policy.getPolicyId(), addIndex);
+
+            // Add v4 policy to mBpfDscpIpv4Policies if source and destination address
+            // are both null or if they are both instances of Inet6Address.
+            if (matchesIpv4(policy)) {
+                mBpfDscpIpv4Policies.insertOrReplaceEntry(
+                        new Struct.U32(addIndex),
+                        new DscpPolicyValue(policy.getSourceAddress(),
+                            policy.getDestinationAddress(),
+                            policy.getSourcePort(), policy.getDestinationPortRange(),
+                            (short) policy.getProtocol(), (short) policy.getDscpValue()));
+            }
+
+            // Add v6 policy to mBpfDscpIpv6Policies if source and destination address
+            // are both null or if they are both instances of Inet6Address.
+            if (matchesIpv6(policy)) {
+                mBpfDscpIpv6Policies.insertOrReplaceEntry(
+                        new Struct.U32(addIndex),
+                        new DscpPolicyValue(policy.getSourceAddress(),
+                                policy.getDestinationAddress(),
+                                policy.getSourcePort(), policy.getDestinationPortRange(),
+                                (short) policy.getProtocol(), (short) policy.getDscpValue()));
+            }
+        } catch (ErrnoException e) {
+            Log.e(TAG, "Failed to insert policy into map: ", e);
+            return STATUS_INSUFFICIENT_PROCESSING_RESOURCES;
+        }
+
+        return STATUS_SUCCESS;
+    }
+
+    /**
+     * Add the provided DSCP policy to the bpf map. Attach bpf program dscp_policy to iface
+     * if not already attached. Response will be sent back to nai with status.
+     *
+     * STATUS_SUCCESS - if policy was added successfully
+     * STATUS_INSUFFICIENT_PROCESSING_RESOURCES - if max policies were already set
+     */
+    public void addDscpPolicy(NetworkAgentInfo nai, DscpPolicy policy) {
+        if (!mAttachedIfaces.contains(nai.linkProperties.getInterfaceName())) {
+            if (!attachProgram(nai.linkProperties.getInterfaceName())) {
+                Log.e(TAG, "Unable to attach program");
+                sendStatus(nai, policy.getPolicyId(), STATUS_INSUFFICIENT_PROCESSING_RESOURCES);
+                return;
+            }
+        }
+
+        int status = addDscpPolicyInternal(policy);
+        sendStatus(nai, policy.getPolicyId(), status);
+    }
+
+    private void removePolicyFromMap(NetworkAgentInfo nai, int policyId, int index) {
+        int status = STATUS_POLICY_NOT_FOUND;
+        try {
+            mBpfDscpIpv4Policies.replaceEntry(new Struct.U32(index), DscpPolicyValue.NONE);
+            mBpfDscpIpv6Policies.replaceEntry(new Struct.U32(index), DscpPolicyValue.NONE);
+            status = STATUS_DELETED;
+        } catch (ErrnoException e) {
+            Log.e(TAG, "Failed to delete policy from map: ", e);
+        }
+
+        sendStatus(nai, policyId, status);
+    }
+
+    /**
+     * Remove specified DSCP policy and detach program if no other policies are active.
+     */
+    public void removeDscpPolicy(NetworkAgentInfo nai, int policyId) {
+        if (!mAttachedIfaces.contains(nai.linkProperties.getInterfaceName())) {
+            // Nothing to remove since program is not attached. Send update back for policy id.
+            sendStatus(nai, policyId, STATUS_POLICY_NOT_FOUND);
+            return;
+        }
+
+        if (mPolicyIdToBpfMapIndex.get(policyId, -1) != -1) {
+            removePolicyFromMap(nai, policyId, mPolicyIdToBpfMapIndex.get(policyId));
+            mPolicyIdToBpfMapIndex.delete(policyId);
+        }
+
+        // TODO: detach should only occur if no more policies are present on the nai's iface.
+        if (mPolicyIdToBpfMapIndex.size() == 0) {
+            detachProgram(nai.linkProperties.getInterfaceName());
+        }
+    }
+
+    /**
+     * Remove all DSCP policies and detach program.
+     */
+    // TODO: Remove all should only remove policies from corresponding nai iface.
+    public void removeAllDscpPolicies(NetworkAgentInfo nai) {
+        if (!mAttachedIfaces.contains(nai.linkProperties.getInterfaceName())) {
+            // Nothing to remove since program is not attached. Send update for policy
+            // id 0. The status update must contain a policy ID, and 0 is an invalid id.
+            sendStatus(nai, 0, STATUS_SUCCESS);
+            return;
+        }
+
+        for (int i = 0; i < mPolicyIdToBpfMapIndex.size(); i++) {
+            removePolicyFromMap(nai, mPolicyIdToBpfMapIndex.keyAt(i),
+                    mPolicyIdToBpfMapIndex.valueAt(i));
+        }
+        mPolicyIdToBpfMapIndex.clear();
+
+        // Can detach program since no policies are active.
+        detachProgram(nai.linkProperties.getInterfaceName());
+    }
+
+    /**
+     * Attach BPF program
+     */
+    private boolean attachProgram(@NonNull String iface) {
+        // TODO: attach needs to be per iface not program.
+
+        try {
+            NetworkInterface netIface = NetworkInterface.getByName(iface);
+            TcUtils.tcFilterAddDevBpf(netIface.getIndex(), false, PRIO_DSCP, (short) ETH_P_ALL,
+                    PROG_PATH);
+        } catch (IOException e) {
+            Log.e(TAG, "Unable to attach to TC on " + iface + ": " + e);
+            return false;
+        }
+        mAttachedIfaces.add(iface);
+        return true;
+    }
+
+    /**
+     * Detach BPF program
+     */
+    public void detachProgram(@NonNull String iface) {
+        try {
+            NetworkInterface netIface = NetworkInterface.getByName(iface);
+            if (netIface != null) {
+                TcUtils.tcFilterDelDev(netIface.getIndex(), false, PRIO_DSCP, (short) ETH_P_ALL);
+            }
+        } catch (IOException e) {
+            Log.e(TAG, "Unable to detach to TC on " + iface + ": " + e);
+        }
+        mAttachedIfaces.remove(iface);
+    }
+}
diff --git a/service/src/com/android/server/connectivity/DscpPolicyValue.java b/service/src/com/android/server/connectivity/DscpPolicyValue.java
new file mode 100644
index 0000000..cb40306
--- /dev/null
+++ b/service/src/com/android/server/connectivity/DscpPolicyValue.java
@@ -0,0 +1,180 @@
+/*
+ * Copyright (C) 2021 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+package com.android.server.connectivity;
+
+import android.util.Log;
+import android.util.Range;
+
+import com.android.net.module.util.Struct;
+import com.android.net.module.util.Struct.Field;
+import com.android.net.module.util.Struct.Type;
+
+import java.net.Inet4Address;
+import java.net.InetAddress;
+import java.net.UnknownHostException;
+
+/** Value type for DSCP setting and rewriting to DSCP policy BPF maps. */
+public class DscpPolicyValue extends Struct {
+    private static final String TAG = DscpPolicyValue.class.getSimpleName();
+
+    // TODO: add the interface index.
+    @Field(order = 0, type = Type.ByteArray, arraysize = 16)
+    public final byte[] src46;
+
+    @Field(order = 1, type = Type.ByteArray, arraysize = 16)
+    public final byte[] dst46;
+
+    @Field(order = 2, type = Type.UBE16)
+    public final int srcPort;
+
+    @Field(order = 3, type = Type.UBE16)
+    public final int dstPortStart;
+
+    @Field(order = 4, type = Type.UBE16)
+    public final int dstPortEnd;
+
+    @Field(order = 5, type = Type.U8)
+    public final short proto;
+
+    @Field(order = 6, type = Type.U8)
+    public final short dscp;
+
+    @Field(order = 7, type = Type.U8, padding = 3)
+    public final short mask;
+
+    private static final int SRC_IP_MASK = 0x1;
+    private static final int DST_IP_MASK = 0x02;
+    private static final int SRC_PORT_MASK = 0x4;
+    private static final int DST_PORT_MASK = 0x8;
+    private static final int PROTO_MASK = 0x10;
+
+    private boolean ipEmpty(final byte[] ip) {
+        for (int i = 0; i < ip.length; i++) {
+            if (ip[i] != 0) return false;
+        }
+        return true;
+    }
+
+    private byte[] toIpv4MappedAddressBytes(InetAddress ia) {
+        final byte[] addr6 = new byte[16];
+        if (ia != null) {
+            final byte[] addr4 = ia.getAddress();
+            addr6[10] = (byte) 0xff;
+            addr6[11] = (byte) 0xff;
+            addr6[12] = addr4[0];
+            addr6[13] = addr4[1];
+            addr6[14] = addr4[2];
+            addr6[15] = addr4[3];
+        }
+        return addr6;
+    }
+
+    private byte[] toAddressField(InetAddress addr) {
+        if (addr == null) {
+            return EMPTY_ADDRESS_FIELD;
+        } else if (addr instanceof Inet4Address) {
+            return toIpv4MappedAddressBytes(addr);
+        } else {
+            return addr.getAddress();
+        }
+    }
+
+    private static final byte[] EMPTY_ADDRESS_FIELD =
+            InetAddress.parseNumericAddress("::").getAddress();
+
+    private short makeMask(final byte[] src46, final byte[] dst46, final int srcPort,
+            final int dstPortStart, final short proto, final short dscp) {
+        short mask = 0;
+        if (src46 != EMPTY_ADDRESS_FIELD) {
+            mask |= SRC_IP_MASK;
+        }
+        if (dst46 != EMPTY_ADDRESS_FIELD) {
+            mask |=  DST_IP_MASK;
+        }
+        if (srcPort != -1) {
+            mask |=  SRC_PORT_MASK;
+        }
+        if (dstPortStart != -1 && dstPortEnd != -1) {
+            mask |=  DST_PORT_MASK;
+        }
+        if (proto != -1) {
+            mask |=  PROTO_MASK;
+        }
+        return mask;
+    }
+
+    // This constructor is necessary for BpfMap#getValue since all values must be
+    // in the constructor.
+    public DscpPolicyValue(final InetAddress src46, final InetAddress dst46, final int srcPort,
+            final int dstPortStart, final int dstPortEnd, final short proto,
+            final short dscp) {
+        this.src46 = toAddressField(src46);
+        this.dst46 = toAddressField(dst46);
+
+        // These params need to be stored as 0 because uints are used in BpfMap.
+        // If they are -1 BpfMap write will throw errors.
+        this.srcPort = srcPort != -1 ? srcPort : 0;
+        this.dstPortStart = dstPortStart != -1 ? dstPortStart : 0;
+        this.dstPortEnd = dstPortEnd != -1 ? dstPortEnd : 0;
+        this.proto = proto != -1 ? proto : 0;
+
+        this.dscp = dscp;
+        // Use member variables for IP since byte[] is needed and api variables for everything else
+        // so -1 is passed into mask if parameter is not present.
+        this.mask = makeMask(this.src46, this.dst46, srcPort, dstPortStart, proto, dscp);
+    }
+
+    public DscpPolicyValue(final InetAddress src46, final InetAddress dst46, final int srcPort,
+            final Range<Integer> dstPort, final short proto,
+            final short dscp) {
+        this(src46, dst46, srcPort, dstPort != null ? dstPort.getLower() : -1,
+                dstPort != null ? dstPort.getUpper() : -1, proto, dscp);
+    }
+
+    public static final DscpPolicyValue NONE = new DscpPolicyValue(
+            null /* src46 */, null /* dst46 */, -1 /* srcPort */,
+            -1 /* dstPortStart */, -1 /* dstPortEnd */, (short) -1 /* proto */,
+            (short) 0 /* dscp */);
+
+    @Override
+    public String toString() {
+        String srcIpString = "empty";
+        String dstIpString = "empty";
+
+        // Separate try/catch for IP's so it's easier to debug.
+        try {
+            srcIpString = InetAddress.getByAddress(src46).getHostAddress();
+        }  catch (UnknownHostException e) {
+            Log.e(TAG, "Invalid SRC IP address", e);
+        }
+
+        try {
+            dstIpString = InetAddress.getByAddress(src46).getHostAddress();
+        }  catch (UnknownHostException e) {
+            Log.e(TAG, "Invalid DST IP address", e);
+        }
+
+        try {
+            return String.format(
+                    "src46: %s, dst46: %s, srcPort: %d, dstPortStart: %d, dstPortEnd: %d,"
+                    + " protocol: %d, dscp %s", srcIpString, dstIpString, srcPort, dstPortStart,
+                    dstPortEnd, proto, dscp);
+        } catch (IllegalArgumentException e) {
+            return String.format("String format error: " + e);
+        }
+    }
+}
diff --git a/service/src/com/android/server/connectivity/NetworkAgentInfo.java b/service/src/com/android/server/connectivity/NetworkAgentInfo.java
index b7f3ed9..1a7248a 100644
--- a/service/src/com/android/server/connectivity/NetworkAgentInfo.java
+++ b/service/src/com/android/server/connectivity/NetworkAgentInfo.java
@@ -24,6 +24,7 @@
 import android.annotation.Nullable;
 import android.content.Context;
 import android.net.CaptivePortalData;
+import android.net.DscpPolicy;
 import android.net.IDnsResolver;
 import android.net.INetd;
 import android.net.INetworkAgent;
@@ -700,6 +701,24 @@
             mHandler.obtainMessage(NetworkAgent.EVENT_LINGER_DURATION_CHANGED,
                     new Pair<>(NetworkAgentInfo.this, durationMs)).sendToTarget();
         }
+
+        @Override
+        public void sendAddDscpPolicy(final DscpPolicy policy) {
+            mHandler.obtainMessage(NetworkAgent.EVENT_ADD_DSCP_POLICY,
+                    new Pair<>(NetworkAgentInfo.this, policy)).sendToTarget();
+        }
+
+        @Override
+        public void sendRemoveDscpPolicy(final int policyId) {
+            mHandler.obtainMessage(NetworkAgent.EVENT_REMOVE_DSCP_POLICY,
+                    new Pair<>(NetworkAgentInfo.this, policyId)).sendToTarget();
+        }
+
+        @Override
+        public void sendRemoveAllDscpPolicies() {
+            mHandler.obtainMessage(NetworkAgent.EVENT_REMOVE_ALL_DSCP_POLICIES,
+                    new Pair<>(NetworkAgentInfo.this, null)).sendToTarget();
+        }
     }
 
     /**
diff --git a/tests/cts/net/src/android/net/cts/DscpPolicyTest.kt b/tests/cts/net/src/android/net/cts/DscpPolicyTest.kt
new file mode 100644
index 0000000..1f39fee
--- /dev/null
+++ b/tests/cts/net/src/android/net/cts/DscpPolicyTest.kt
@@ -0,0 +1,522 @@
+/*
+ * Copyright (C) 2021 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+package android.net.cts
+
+import android.net.cts.util.CtsNetUtils.TestNetworkCallback
+
+import android.app.Instrumentation
+import android.Manifest.permission.MANAGE_TEST_NETWORKS
+import android.content.Context
+import android.net.ConnectivityManager
+import android.net.cts.util.CtsNetUtils
+import android.net.DscpPolicy
+import android.net.DscpPolicy.STATUS_DELETED
+import android.net.DscpPolicy.STATUS_SUCCESS
+import android.net.InetAddresses
+import android.net.IpPrefix
+import android.net.LinkAddress
+import android.net.LinkProperties
+import android.net.NetworkAgent
+import android.net.NetworkAgentConfig
+import android.net.NetworkCapabilities
+import android.net.NetworkCapabilities.NET_CAPABILITY_INTERNET
+import android.net.NetworkCapabilities.NET_CAPABILITY_NOT_RESTRICTED
+import android.net.NetworkCapabilities.NET_CAPABILITY_NOT_ROAMING
+import android.net.NetworkCapabilities.NET_CAPABILITY_NOT_SUSPENDED
+import android.net.NetworkCapabilities.NET_CAPABILITY_NOT_VCN_MANAGED
+import android.net.NetworkCapabilities.NET_CAPABILITY_NOT_VPN
+import android.net.NetworkCapabilities.NET_CAPABILITY_TRUSTED
+import android.net.NetworkCapabilities.TRANSPORT_TEST
+import android.net.NetworkRequest
+import android.net.TestNetworkInterface
+import android.net.TestNetworkManager
+import android.net.RouteInfo
+import android.net.util.SocketUtils
+import android.os.Build
+import android.os.HandlerThread
+import android.os.Looper
+import android.platform.test.annotations.AppModeFull
+import android.system.Os
+import android.system.OsConstants
+import android.system.OsConstants.AF_INET
+import android.system.OsConstants.IPPROTO_IP
+import android.system.OsConstants.IPPROTO_UDP
+import android.system.OsConstants.SOCK_DGRAM
+import android.system.OsConstants.SOCK_NONBLOCK
+import android.util.Log
+import android.util.Range
+import androidx.test.InstrumentationRegistry
+import androidx.test.runner.AndroidJUnit4
+import com.android.modules.utils.build.SdkLevel
+import com.android.testutils.CompatUtil
+import com.android.testutils.DevSdkIgnoreRule
+import com.android.testutils.OffsetFilter
+import com.android.testutils.assertParcelingIsLossless
+import com.android.testutils.runAsShell
+import com.android.testutils.SC_V2
+import com.android.testutils.TapPacketReader
+import com.android.testutils.TestableNetworkAgent
+import com.android.testutils.TestableNetworkAgent.CallbackEntry.OnNetworkCreated
+import com.android.testutils.TestableNetworkAgent.CallbackEntry.OnDscpPolicyStatusUpdated
+import com.android.testutils.TestableNetworkCallback
+import org.junit.After
+import org.junit.AfterClass
+import org.junit.Before
+import org.junit.BeforeClass
+import org.junit.Rule
+import org.junit.Test
+import org.junit.runner.RunWith
+import java.net.Inet4Address
+import java.net.Inet6Address
+import java.net.InetAddress
+import java.net.InetSocketAddress
+import java.net.ServerSocket
+import java.nio.ByteBuffer
+import java.nio.ByteOrder
+import java.util.UUID
+import kotlin.test.assertEquals
+import kotlin.test.assertNotNull
+import kotlin.test.assertTrue
+import kotlin.concurrent.thread
+import kotlin.test.fail
+
+private const val MAX_PACKET_LENGTH = 1500
+
+private val instrumentation: Instrumentation
+    get() = InstrumentationRegistry.getInstrumentation()
+
+private const val TAG = "DscpPolicyTest"
+private const val PACKET_TIMEOUT_MS = 2_000L
+
+@AppModeFull(reason = "Instant apps cannot create test networks")
+@RunWith(AndroidJUnit4::class)
+class DscpPolicyTest {
+    @JvmField
+    @Rule
+    val ignoreRule = DevSdkIgnoreRule(ignoreClassUpTo = SC_V2)
+
+    private val LOCAL_IPV4_ADDRESS = InetAddresses.parseNumericAddress("192.0.2.1")
+    private val TEST_TARGET_IPV4_ADDR =
+            InetAddresses.parseNumericAddress("8.8.8.8") as Inet4Address
+
+    private val realContext = InstrumentationRegistry.getContext()
+    private val cm = realContext.getSystemService(ConnectivityManager::class.java)
+
+    private val agentsToCleanUp = mutableListOf<NetworkAgent>()
+    private val callbacksToCleanUp = mutableListOf<TestableNetworkCallback>()
+
+    private val handlerThread = HandlerThread(DscpPolicyTest::class.java.simpleName)
+
+    private lateinit var iface: TestNetworkInterface
+    private lateinit var tunNetworkCallback: TestNetworkCallback
+    private lateinit var reader: TapPacketReader
+
+    @Before
+    fun setUp() {
+        runAsShell(MANAGE_TEST_NETWORKS) {
+            val tnm = realContext.getSystemService(TestNetworkManager::class.java)
+
+            iface = tnm.createTunInterface( Array(1){ LinkAddress(LOCAL_IPV4_ADDRESS, 32) } )
+            assertNotNull(iface)
+        }
+
+        handlerThread.start()
+        reader = TapPacketReader(
+                handlerThread.threadHandler,
+                iface.fileDescriptor.fileDescriptor,
+                MAX_PACKET_LENGTH)
+        reader.startAsyncForTest()
+    }
+
+    @After
+    fun tearDown() {
+        agentsToCleanUp.forEach { it.unregister() }
+        callbacksToCleanUp.forEach { cm.unregisterNetworkCallback(it) }
+        reader.handler.post { reader.stop() }
+        Os.close(iface.fileDescriptor.fileDescriptor)
+        handlerThread.quitSafely()
+    }
+
+    private fun requestNetwork(request: NetworkRequest, callback: TestableNetworkCallback) {
+        cm.requestNetwork(request, callback)
+        callbacksToCleanUp.add(callback)
+    }
+
+    private fun makeTestNetworkRequest(specifier: String? = null): NetworkRequest {
+        return NetworkRequest.Builder()
+                .clearCapabilities()
+                .addCapability(NET_CAPABILITY_NOT_RESTRICTED)
+                .addTransportType(TRANSPORT_TEST)
+                .also {
+                    if (specifier != null) {
+                        it.setNetworkSpecifier(CompatUtil.makeTestNetworkSpecifier(specifier))
+                    }
+                }
+                .build()
+    }
+
+    private fun createConnectedNetworkAgent(
+        context: Context = realContext,
+        specifier: String? = iface.getInterfaceName(),
+    ): Pair<TestableNetworkAgent, TestableNetworkCallback> {
+        val callback = TestableNetworkCallback()
+        // Ensure this NetworkAgent is never unneeded by filing a request with its specifier.
+        requestNetwork(makeTestNetworkRequest(specifier = specifier), callback)
+
+        val nc = NetworkCapabilities().apply {
+            addTransportType(TRANSPORT_TEST)
+            removeCapability(NET_CAPABILITY_TRUSTED)
+            removeCapability(NET_CAPABILITY_INTERNET)
+            addCapability(NET_CAPABILITY_NOT_SUSPENDED)
+            addCapability(NET_CAPABILITY_NOT_ROAMING)
+            addCapability(NET_CAPABILITY_NOT_VPN)
+            addCapability(NET_CAPABILITY_NOT_VCN_MANAGED)
+            if (null != specifier) {
+                setNetworkSpecifier(CompatUtil.makeTestNetworkSpecifier(specifier))
+            }
+        }
+        val lp = LinkProperties().apply {
+            addLinkAddress(LinkAddress(LOCAL_IPV4_ADDRESS, 32))
+            addRoute(RouteInfo(IpPrefix("0.0.0.0/0"), null, null))
+            setInterfaceName(iface.getInterfaceName())
+        }
+        val config = NetworkAgentConfig.Builder().build()
+        val agent = TestableNetworkAgent(context, handlerThread.looper, nc, lp, config)
+        agentsToCleanUp.add(agent)
+
+        // Connect the agent and verify initial status callbacks.
+        runAsShell(MANAGE_TEST_NETWORKS) { agent.register() }
+        agent.markConnected()
+        agent.expectCallback<OnNetworkCreated>()
+        agent.expectSignalStrengths(intArrayOf())
+        agent.expectValidationBypassedStatus()
+        val network = agent.network ?: fail("Expected a non-null network")
+        return agent to callback
+    }
+
+    fun ByteArray.toHex(): String = joinToString(separator = "") {
+        eachByte -> "%02x".format(eachByte)
+    }
+
+    fun checkDscpValue(
+        agent : TestableNetworkAgent,
+        callback : TestableNetworkCallback,
+        dscpValue : Int = 0,
+        dstPort : Int = 0,
+    ) {
+        val testString = "test string"
+        val testPacket = ByteBuffer.wrap(testString.toByteArray(Charsets.UTF_8))
+        var packetFound = false
+
+        val socket = Os.socket(AF_INET, SOCK_DGRAM or SOCK_NONBLOCK, IPPROTO_UDP)
+        agent.network.bindSocket(socket)
+
+        val originalPacket = testPacket.readAsArray()
+        Os.sendto(socket, originalPacket, 0 /* bytesOffset */, originalPacket.size,
+                0 /* flags */, TEST_TARGET_IPV4_ADDR, dstPort)
+
+        Os.close(socket)
+        generateSequence { reader.poll(PACKET_TIMEOUT_MS) }.forEach { packet ->
+            val buffer = ByteBuffer.wrap(packet, 0, packet.size).order(ByteOrder.BIG_ENDIAN)
+            val ip_ver = buffer.get()
+            val tos = buffer.get()
+            val length = buffer.getShort()
+            val id = buffer.getShort()
+            val offset = buffer.getShort()
+            val ttl = buffer.get()
+            val ipType = buffer.get()
+            val checksum = buffer.getShort()
+
+            val ipAddr = ByteArray(4)
+            buffer.get(ipAddr)
+            val srcIp = Inet4Address.getByAddress(ipAddr);
+            buffer.get(ipAddr)
+            val dstIp = Inet4Address.getByAddress(ipAddr);
+            val packetSrcPort = buffer.getShort().toInt()
+            val packetDstPort = buffer.getShort().toInt()
+
+            // TODO: Add source port comparison.
+            if (srcIp == LOCAL_IPV4_ADDRESS && dstIp == TEST_TARGET_IPV4_ADDR &&
+                    packetDstPort == dstPort) {
+                assertEquals(dscpValue, (tos.toInt().shr(2)))
+                packetFound = true
+            }
+        }
+        assertTrue(packetFound)
+    }
+
+    fun doRemovePolicyTest(
+        agent : TestableNetworkAgent,
+        callback : TestableNetworkCallback,
+        policyId : Int
+    ) {
+        val portNumber = 1111 * policyId
+        agent.sendRemoveDscpPolicy(policyId)
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(policyId, it.policyId)
+            assertEquals(STATUS_DELETED, it.status)
+            checkDscpValue(agent, callback, dstPort = portNumber)
+        }
+    }
+
+    @Test
+    fun testDscpPolicyAddPolicies(): Unit = createConnectedNetworkAgent().let { (agent, callback) ->
+        val policy = DscpPolicy.Builder(1, 1)
+                .setDestinationPortRange(Range(4444, 4444)).build()
+        agent.sendAddDscpPolicy(policy)
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(1, it.policyId)
+            assertEquals(STATUS_SUCCESS, it.status)
+        }
+
+        checkDscpValue(agent, callback, dscpValue = 1, dstPort = 4444)
+
+        agent.sendRemoveDscpPolicy(1)
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(1, it.policyId)
+            assertEquals(STATUS_DELETED, it.status)
+        }
+
+        val policy2 = DscpPolicy.Builder(1, 4)
+                .setDestinationPortRange(Range(5555, 5555)).setSourceAddress(LOCAL_IPV4_ADDRESS)
+                .setDestinationAddress(TEST_TARGET_IPV4_ADDR).setProtocol(IPPROTO_UDP).build()
+        agent.sendAddDscpPolicy(policy2)
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(1, it.policyId)
+            assertEquals(STATUS_SUCCESS, it.status)
+        }
+
+        checkDscpValue(agent, callback, dscpValue = 4, dstPort = 5555)
+
+        agent.sendRemoveDscpPolicy(1)
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(1, it.policyId)
+            assertEquals(STATUS_DELETED, it.status)
+        }
+    }
+
+    @Test
+    // Remove policies in the same order as addition.
+    fun testRemoveDscpPolicy_RemoveSameOrderAsAdd(): Unit = createConnectedNetworkAgent().let {
+                (agent, callback) ->
+        val policy = DscpPolicy.Builder(1, 1).setDestinationPortRange(Range(1111, 1111)).build()
+        agent.sendAddDscpPolicy(policy)
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(1, it.policyId)
+            assertEquals(STATUS_SUCCESS, it.status)
+            checkDscpValue(agent, callback, dscpValue = 1, dstPort = 1111)
+        }
+
+        val policy2 = DscpPolicy.Builder(2, 1).setDestinationPortRange(Range(2222, 2222)).build()
+        agent.sendAddDscpPolicy(policy2)
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(2, it.policyId)
+            assertEquals(STATUS_SUCCESS, it.status)
+            checkDscpValue(agent, callback, dscpValue = 1, dstPort = 2222)
+        }
+
+        val policy3 = DscpPolicy.Builder(3, 1).setDestinationPortRange(Range(3333, 3333)).build()
+        agent.sendAddDscpPolicy(policy3)
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(3, it.policyId)
+            assertEquals(STATUS_SUCCESS, it.status)
+            checkDscpValue(agent, callback, dscpValue = 1, dstPort = 3333)
+        }
+
+        /* Remove Policies and check CE is no longer set */
+        doRemovePolicyTest(agent, callback, 1)
+        doRemovePolicyTest(agent, callback, 2)
+        doRemovePolicyTest(agent, callback, 3)
+    }
+
+    @Test
+    fun testRemoveDscpPolicy_RemoveImmediatelyAfterAdd(): Unit =
+            createConnectedNetworkAgent().let{ (agent, callback) ->
+        val policy = DscpPolicy.Builder(1, 1).setDestinationPortRange(Range(1111, 1111)).build()
+        agent.sendAddDscpPolicy(policy)
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(1, it.policyId)
+            assertEquals(STATUS_SUCCESS, it.status)
+            checkDscpValue(agent, callback, dscpValue = 1, dstPort = 1111)
+        }
+        doRemovePolicyTest(agent, callback, 1)
+
+        val policy2 = DscpPolicy.Builder(2, 1).setDestinationPortRange(Range(2222, 2222)).build()
+        agent.sendAddDscpPolicy(policy2)
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(2, it.policyId)
+            assertEquals(STATUS_SUCCESS, it.status)
+            checkDscpValue(agent, callback, dscpValue = 1, dstPort = 2222)
+        }
+        doRemovePolicyTest(agent, callback, 2)
+
+        val policy3 = DscpPolicy.Builder(3, 1).setDestinationPortRange(Range(3333, 3333)).build()
+        agent.sendAddDscpPolicy(policy3)
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(3, it.policyId)
+            assertEquals(STATUS_SUCCESS, it.status)
+            checkDscpValue(agent, callback, dscpValue = 1, dstPort = 3333)
+        }
+        doRemovePolicyTest(agent, callback, 3)
+    }
+
+    @Test
+    // Remove policies in reverse order from addition.
+    fun testRemoveDscpPolicy_RemoveReverseOrder(): Unit =
+            createConnectedNetworkAgent().let { (agent, callback) ->
+        val policy = DscpPolicy.Builder(1, 1).setDestinationPortRange(Range(1111, 1111)).build()
+        agent.sendAddDscpPolicy(policy)
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(1, it.policyId)
+            assertEquals(STATUS_SUCCESS, it.status)
+            checkDscpValue(agent, callback, dscpValue = 1, dstPort = 1111)
+        }
+
+        val policy2 = DscpPolicy.Builder(2, 1).setDestinationPortRange(Range(2222, 2222)).build()
+        agent.sendAddDscpPolicy(policy2)
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(2, it.policyId)
+            assertEquals(STATUS_SUCCESS, it.status)
+            checkDscpValue(agent, callback, dscpValue = 1, dstPort = 2222)
+        }
+
+        val policy3 = DscpPolicy.Builder(3, 1).setDestinationPortRange(Range(3333, 3333)).build()
+        agent.sendAddDscpPolicy(policy3)
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(3, it.policyId)
+            assertEquals(STATUS_SUCCESS, it.status)
+            checkDscpValue(agent, callback, dscpValue = 1, dstPort = 3333)
+        }
+
+        /* Remove Policies and check CE is no longer set */
+        doRemovePolicyTest(agent, callback, 3)
+        doRemovePolicyTest(agent, callback, 2)
+        doRemovePolicyTest(agent, callback, 1)
+    }
+
+    @Test
+    fun testRemoveDscpPolicy_InvalidPolicy(): Unit = createConnectedNetworkAgent().let {
+                (agent, callback) ->
+        agent.sendRemoveDscpPolicy(3)
+        // Is there something to add in TestableNetworkCallback to NOT expect a callback?
+        // Or should we send STATUS_DELETED in any case or a different STATUS?
+    }
+
+    @Test
+    fun testRemoveAllDscpPolicies(): Unit = createConnectedNetworkAgent().let { (agent, callback) ->
+        val policy = DscpPolicy.Builder(1, 1)
+                .setDestinationPortRange(Range(1111, 1111)).build()
+        agent.sendAddDscpPolicy(policy)
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(1, it.policyId)
+            assertEquals(STATUS_SUCCESS, it.status)
+            checkDscpValue(agent, callback, dscpValue = 1, dstPort = 1111)
+        }
+
+        val policy2 = DscpPolicy.Builder(2, 1)
+                .setDestinationPortRange(Range(2222, 2222)).build()
+        agent.sendAddDscpPolicy(policy2)
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(2, it.policyId)
+            assertEquals(STATUS_SUCCESS, it.status)
+            checkDscpValue(agent, callback, dscpValue = 1, dstPort = 2222)
+        }
+
+        val policy3 = DscpPolicy.Builder(3, 1)
+                .setDestinationPortRange(Range(3333, 3333)).build()
+        agent.sendAddDscpPolicy(policy3)
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(3, it.policyId)
+            assertEquals(STATUS_SUCCESS, it.status)
+            checkDscpValue(agent, callback, dscpValue = 1, dstPort = 3333)
+        }
+
+        agent.sendRemoveAllDscpPolicies()
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(1, it.policyId)
+            assertEquals(STATUS_DELETED, it.status)
+            checkDscpValue(agent, callback, dstPort = 1111)
+        }
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(2, it.policyId)
+            assertEquals(STATUS_DELETED, it.status)
+            checkDscpValue(agent, callback, dstPort = 2222)
+        }
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(3, it.policyId)
+            assertEquals(STATUS_DELETED, it.status)
+            checkDscpValue(agent, callback, dstPort = 3333)
+        }
+    }
+
+    @Test
+    fun testAddDuplicateDscpPolicy(): Unit = createConnectedNetworkAgent().let {
+                (agent, callback) ->
+        val policy = DscpPolicy.Builder(1, 1).setDestinationPortRange(Range(4444, 4444)).build()
+        agent.sendAddDscpPolicy(policy)
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(1, it.policyId)
+            assertEquals(STATUS_SUCCESS, it.status)
+            checkDscpValue(agent, callback, dscpValue = 1, dstPort = 4444)
+        }
+
+        // TODO: send packet on socket and confirm that changing the DSCP policy
+        // updates the mark to the new value.
+
+        val policy2 = DscpPolicy.Builder(1, 1).setDestinationPortRange(Range(5555, 5555)).build()
+        agent.sendAddDscpPolicy(policy2)
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(1, it.policyId)
+            assertEquals(STATUS_SUCCESS, it.status)
+
+            // Sending packet with old policy should fail
+            checkDscpValue(agent, callback, dstPort = 4444)
+            checkDscpValue(agent, callback, dscpValue = 1, dstPort = 5555)
+        }
+
+        agent.sendRemoveDscpPolicy(1)
+        agent.expectCallback<OnDscpPolicyStatusUpdated>().let {
+            assertEquals(1, it.policyId)
+            assertEquals(STATUS_DELETED, it.status)
+        }
+    }
+
+    @Test
+    fun testParcelingDscpPolicyIsLossless(): Unit = createConnectedNetworkAgent().let {
+                (agent, callback) ->
+        // Check that policy with partial parameters is lossless.
+        val policy = DscpPolicy.Builder(1, 1).setDestinationPortRange(Range(4444, 4444)).build()
+        assertParcelingIsLossless(policy);
+
+        // Check that policy with all parameters is lossless.
+        val policy2 = DscpPolicy.Builder(1, 1).setDestinationPortRange(Range(4444, 4444))
+                .setSourceAddress(LOCAL_IPV4_ADDRESS)
+                .setDestinationAddress(TEST_TARGET_IPV4_ADDR)
+                .setProtocol(IPPROTO_UDP).build()
+        assertParcelingIsLossless(policy2);
+    }
+}
+
+private fun ByteBuffer.readAsArray(): ByteArray {
+    val out = ByteArray(remaining())
+    get(out)
+    return out
+}
+
+private fun <T> Context.assertHasService(manager: Class<T>): T {
+    return getSystemService(manager) ?: fail("Service $manager not found")
+}
\ No newline at end of file