diff --git a/grc/CMakeLists.txt b/grc/CMakeLists.txt
index 43e25723a8be312b24349ae63d471357a71f9f6e..93d7f990a80f11dd2a674129d6cba14bb514ed4a 100644
--- a/grc/CMakeLists.txt
+++ b/grc/CMakeLists.txt
@@ -14,5 +14,6 @@ install(FILES
     elen90089_packet_phy_tx.block.yml
     elen90089_packet_mac_tx.block.yml
     elen90089_constellation_decoder_cf.block.yml
+    elen90089_costas_loop_cc.block.yml
     DESTINATION share/gnuradio/grc/blocks
 )
diff --git a/grc/elen90089_corr_est_cc.block.yml b/grc/elen90089_corr_est_cc.block.yml
index 4db7551c6370f735e316c251db60cfc6167ab274..959d1456d4e3722e6d26f618afed53612970d699 100644
--- a/grc/elen90089_corr_est_cc.block.yml
+++ b/grc/elen90089_corr_est_cc.block.yml
@@ -24,9 +24,9 @@ outputs:
 - label: out
   domain: stream
   dtype: complex
-- label: cfo
-  domain: message
-  optional: True
+#- label: cfo
+#  domain: message
+#  optional: True
 - label: corr
   domain: stream
   dtype: float
diff --git a/grc/elen90089_costas_loop_cc.block.yml b/grc/elen90089_costas_loop_cc.block.yml
new file mode 100644
index 0000000000000000000000000000000000000000..3e38fb64cd2e201252097af02c28e50a7c9b3fd1
--- /dev/null
+++ b/grc/elen90089_costas_loop_cc.block.yml
@@ -0,0 +1,49 @@
+id: elen90089_costas_loop_cc
+label: CDC Costas Loop
+category: '[elen90089]'
+
+templates:
+  imports: import elen90089
+  make: elen90089.costas_loop_cc(${w}, ${order}, ${use_snr})
+  callbacks:
+  - set_loop_bandwidth(${w})
+
+parameters:
+- id: w
+  label: Loop Bandwidth
+  dtype: real
+- id: order
+  label: Order
+  dtype: int
+- id: use_snr
+  label: Use SNR
+  dtype: enum
+  default: 'False'
+  options: ['True', 'False']
+  option_labels: ['Yes', 'No']
+  hide: part
+
+inputs:
+- domain: stream
+  dtype: complex
+- domain: message
+  id: noise
+  optional: true
+
+outputs:
+- domain: stream
+  dtype: complex
+- label: frequency
+  domain: stream
+  dtype: float
+  optional: true
+- label: phase
+  domain: stream
+  dtype: float
+  optional: true
+- label: error
+  domain: stream
+  dtype: float
+  optional: true
+
+file_format: 1
diff --git a/include/elen90089/CMakeLists.txt b/include/elen90089/CMakeLists.txt
index a43d267181d68f69e89d71b393f515f5d2026269..50bc03873444b5c928046935011c72bbffa20c87 100644
--- a/include/elen90089/CMakeLists.txt
+++ b/include/elen90089/CMakeLists.txt
@@ -16,5 +16,6 @@ install(FILES
     symbol_mapper_c.h
     header_format_cdc.h
     constellation_decoder_cf.h
+    costas_loop_cc.h
     DESTINATION include/elen90089
 )
diff --git a/include/elen90089/constellation_decoder_cf.h b/include/elen90089/constellation_decoder_cf.h
index bb2c50b4e347799c6fd77609da78035e381e0d6d..f6f3768b44d45ad794838d5a3e6b129905dfb303 100644
--- a/include/elen90089/constellation_decoder_cf.h
+++ b/include/elen90089/constellation_decoder_cf.h
@@ -45,7 +45,7 @@ public:
      *
      * \param bps               (int) Bits per symbol of constellation
      * \param soft_decisions    (bool) Use soft decision making
-     * \length_tag_name         (string) Tag key identifying length of symbol
+     * \param length_tag_name   (string) Tag key identifying length of symbol
      *                          blocks
      */
     static sptr make(int bps,
diff --git a/include/elen90089/costas_loop_cc.h b/include/elen90089/costas_loop_cc.h
new file mode 100644
index 0000000000000000000000000000000000000000..8dd19a32aad00f54ef5404f2348bfdc961851b99
--- /dev/null
+++ b/include/elen90089/costas_loop_cc.h
@@ -0,0 +1,39 @@
+/* -*- c++ -*- */
+/*
+ * Copyright 2022 University of Melbourne.
+ *
+ * SPDX-License-Identifier: GPL-3.0-or-later
+ */
+
+#ifndef INCLUDED_ELEN90089_COSTAS_LOOP_CC_H
+#define INCLUDED_ELEN90089_COSTAS_LOOP_CC_H
+
+#include <elen90089/api.h>
+#include <gnuradio/sync_block.h>
+#include <gnuradio/blocks/control_loop.h>
+
+namespace gr {
+namespace elen90089 {
+
+/*!
+ * \brief <+description of block+>
+ * \ingroup elen90089
+ *
+ */
+class ELEN90089_API costas_loop_cc : virtual public gr::sync_block,
+                                     virtual public gr::blocks::control_loop
+{
+public:
+    typedef std::shared_ptr<costas_loop_cc> sptr;
+
+    static sptr make(float loop_bw,
+                     unsigned int order,
+                     bool use_snr = false);
+
+    virtual float error() const = 0;
+};
+
+} // namespace elen90089
+} // namespace gr
+
+#endif /* INCLUDED_ELEN90089_COSTAS_LOOP_CC_H */
diff --git a/lib/CMakeLists.txt b/lib/CMakeLists.txt
index e6353a612f8d78d98a47f9b7e6cf2cb9a3015eee..a7f1c7aa2cce171725233568b52b71f245344305 100644
--- a/lib/CMakeLists.txt
+++ b/lib/CMakeLists.txt
@@ -17,6 +17,7 @@ list(APPEND elen90089_sources
     symbol_mapper_c_impl.cc
     header_format_cdc.cc
     constellation_decoder_cf_impl.cc
+    costas_loop_cc_impl.cc
 )
 
 set(elen90089_sources "${elen90089_sources}" PARENT_SCOPE)
diff --git a/lib/corr_est_cc_impl.cc b/lib/corr_est_cc_impl.cc
index cd9e757d3c1bcd4a6cddb2c6c180e11d335d9bb2..d27140e4704178596aa2753d8e524ed02bf0a886 100644
--- a/lib/corr_est_cc_impl.cc
+++ b/lib/corr_est_cc_impl.cc
@@ -37,7 +37,7 @@ corr_est_cc_impl::corr_est_cc_impl(const std::vector<gr_complex>& sequence,
     set_max_noutput_items(s_nitems);
     set_sequence(sequence);
 
-    message_port_register_out(pmt::mp("cfo"));
+    //message_port_register_out(pmt::mp("cfo"));
 }
 
 corr_est_cc_impl::~corr_est_cc_impl() {}
@@ -151,7 +151,7 @@ int corr_est_cc_impl::work(int noutput_items,
 
         if (corr_mag[i] > d_threshold) {
             // coarse frequency estimate
-            double freq_est = estimate_freq_offset(&in[i]);
+            //double freq_est = estimate_freq_offset(&in[i]);
             
             // single-tap (inverse) channel estimate
             gr_complex chan_est = d_corr[i] * d_scale;
@@ -164,14 +164,14 @@ int corr_est_cc_impl::work(int noutput_items,
 
             // send frequency correction message
             int offset = nitems_written(0) + i;
-            pmt::pmt_t cmd = pmt::make_dict();
-            cmd = pmt::dict_add(cmd,
-                                pmt::mp("offset"),
-                                pmt::from_long(offset)); 
-            cmd = pmt::dict_add(cmd,
-                                pmt::mp("inc"),
-                                pmt::from_double(-freq_est));
-            message_port_pub(pmt::mp("cfo"), cmd);
+            //pmt::pmt_t cmd = pmt::make_dict();
+            //cmd = pmt::dict_add(cmd,
+            //                    pmt::mp("offset"),
+            //                    pmt::from_long(offset));
+            //cmd = pmt::dict_add(cmd,
+            //                    pmt::mp("inc"),
+            //                    pmt::from_double(-freq_est));
+            //message_port_pub(pmt::mp("cfo"), cmd);
 
             // tag output
             for(int ch = 0; ch < output_items.size(); ch++) {
@@ -179,14 +179,14 @@ int corr_est_cc_impl::work(int noutput_items,
                              pmt::intern("corr_start"),
                              pmt::from_double(corr_mag[i]),
                              d_src_id);
-                add_item_tag(ch, offset,
-                             pmt::intern("freq_est"),
-                             pmt::from_double(freq_est),
-                             d_src_id);
                 add_item_tag(ch, offset,
                              pmt::intern("chan_est"),
                              pmt::from_complex(chan_est),
                              d_src_id);
+                add_item_tag(ch, offset,
+                             pmt::intern("phase_est"), // reset Costas Loop
+                             pmt::from_double(0.0),
+                             d_src_id);
                 add_item_tag(ch, offset,
                              pmt::intern("time_est"),
                              pmt::from_double(time_est),
diff --git a/lib/costas_loop_cc_impl.cc b/lib/costas_loop_cc_impl.cc
new file mode 100644
index 0000000000000000000000000000000000000000..ac439fe66c7fe5933264f9ae1eedf92bbd763465
--- /dev/null
+++ b/lib/costas_loop_cc_impl.cc
@@ -0,0 +1,151 @@
+/* -*- c++ -*- */
+/*
+ * Copyright 2022 University of Melbourne.
+ *
+ * SPDX-License-Identifier: GPL-3.0-or-later
+ */
+
+#include "costas_loop_cc_impl.h"
+#include <gnuradio/expj.h>
+#include <gnuradio/io_signature.h>
+#include <gnuradio/math.h>
+#include <gnuradio/sincos.h>
+
+namespace gr {
+namespace elen90089 {
+
+costas_loop_cc::sptr costas_loop_cc::make(float loop_bw,
+                                          unsigned int order,
+                                          bool use_snr)
+{
+    return gnuradio::make_block_sptr<costas_loop_cc_impl>(
+        loop_bw, order, use_snr);
+}
+
+static int ios[] = { sizeof(gr_complex), sizeof(float), sizeof(float), sizeof(float) };
+static std::vector<int> iosig(ios, ios + sizeof(ios) / sizeof(int));
+
+costas_loop_cc_impl::costas_loop_cc_impl(float loop_bw,
+                                         unsigned int order,
+                                         bool use_snr)
+    : sync_block("costas_loop_cc",
+                 io_signature::make(1, 1, sizeof(gr_complex)),
+                 io_signature::makev(1, 4, iosig)),
+      blocks::control_loop(loop_bw, 1.0, -1.0),
+      d_error(0),
+      d_noise(1.0),
+      d_use_snr(use_snr),
+      d_order(order)
+{
+    message_port_register_in(pmt::mp("noise"));
+    set_msg_handler(pmt::mp("noise"),
+                    [this](pmt::pmt_t msg) { this->handle_set_noise(msg); });
+}
+
+costas_loop_cc_impl::~costas_loop_cc_impl() {}
+
+void costas_loop_cc_impl::handle_set_noise(pmt::pmt_t msg)
+{
+    if (pmt::is_real(msg)) {
+        d_noise = pmt::to_double(msg);
+        d_noise = powf(10.0f, d_noise / 10.0f);
+    }
+}
+
+int costas_loop_cc_impl::work(int noutput_items,
+                              gr_vector_const_void_star& input_items,
+                              gr_vector_void_star& output_items)
+{
+    const gr_complex* iptr = (gr_complex*)input_items[0];
+    gr_complex* optr = (gr_complex*)output_items[0];
+    float* freq_optr = output_items.size() >= 2 ? (float*)output_items[1] : NULL;
+    float* phase_optr = output_items.size() >= 3 ? (float*)output_items[2] : NULL;
+    float* error_optr = output_items.size() >= 4 ? (float*)output_items[3] : NULL;
+
+    std::vector<tag_t> tags;
+    get_tags_in_range(tags,
+                      0,
+                      nitems_read(0),
+                      nitems_read(0) + noutput_items,
+                      pmt::intern("phase_est"));
+
+    // change in modulation order
+    std::vector<tag_t> bps_tags;
+    get_tags_in_range(bps_tags,
+                      0,
+                      nitems_read(0),
+                      nitems_read(0) + noutput_items,
+                      pmt::intern("bps"));
+
+    // Get this out of the for loop if not used:
+    bool has_additional_outputs = false;
+    if (freq_optr)
+        has_additional_outputs = true;
+    else if (phase_optr)
+        has_additional_outputs = true;
+    else if (error_optr)
+        has_additional_outputs = true;
+
+    for (int i = 0; i < noutput_items; i++) {
+        if (!tags.empty()) {
+            if (tags[0].offset - nitems_read(0) == (size_t)i) {
+                d_phase = (float)pmt::to_double(tags[0].value);
+                tags.erase(tags.begin());
+            }
+        }
+        if (!bps_tags.empty()) {
+            if (bps_tags[0].offset - nitems_read(0) == (size_t)i) {
+                int bps = pmt::to_long(bps_tags[0].value);
+                assert(bps > 0 && bps < 4);
+                d_order = 0x1 << bps;
+                bps_tags.erase(bps_tags.begin());
+            }
+        }
+
+        const gr_complex nco_out = gr_expj(-d_phase);
+
+        gr::fast_cc_multiply(optr[i], iptr[i], nco_out);
+
+        // EXPENSIVE LINE with function pointer, switch was about 20% faster in testing.
+        // Left in for logic justification/reference. d_error = phase_detector_2(optr[i]);
+        switch (d_order) {
+        case 2:
+            if (d_use_snr)
+                d_error = phase_detector_snr_2(optr[i]);
+            else
+                d_error = phase_detector_2(optr[i]);
+            break;
+        case 4:
+            if (d_use_snr)
+                d_error = phase_detector_snr_4(optr[i]);
+            else
+                d_error = phase_detector_4(optr[i]);
+            break;
+        case 8:
+            if (d_use_snr)
+                d_error = phase_detector_snr_8(optr[i]);
+            else
+                d_error = phase_detector_8(optr[i]);
+            break;
+        }
+        d_error = gr::branchless_clip(d_error, 1.0);
+
+        advance_loop(d_error);
+        phase_wrap();
+        frequency_limit();
+
+        if (has_additional_outputs) {
+            if (freq_optr)
+                freq_optr[i] = d_freq;
+            if (phase_optr)
+                phase_optr[i] = d_phase;
+            if (error_optr)
+                error_optr[i] = d_error;
+        }
+    }
+
+    return noutput_items;
+}
+
+} /* namespace elen90089 */
+} /* namespace gr */
diff --git a/lib/costas_loop_cc_impl.h b/lib/costas_loop_cc_impl.h
new file mode 100644
index 0000000000000000000000000000000000000000..f005eec630c9748b44fde511fa082e2c5deaf3fa
--- /dev/null
+++ b/lib/costas_loop_cc_impl.h
@@ -0,0 +1,92 @@
+/* -*- c++ -*- */
+/*
+ * Copyright 2022 University of Melbourne.
+ *
+ * SPDX-License-Identifier: GPL-3.0-or-later
+ */
+
+#ifndef INCLUDED_ELEN90089_COSTAS_LOOP_CC_IMPL_H
+#define INCLUDED_ELEN90089_COSTAS_LOOP_CC_IMPL_H
+
+#include <elen90089/costas_loop_cc.h>
+
+namespace gr {
+namespace elen90089 {
+
+class costas_loop_cc_impl : public costas_loop_cc
+{
+private:
+    float d_error;
+    float d_noise;
+    bool d_use_snr;
+    int d_order;
+
+    float phase_detector_8(gr_complex sample) const // for 8PSK
+    {
+        const float K = (sqrtf(2.0) - 1);
+        if (fabsf(sample.real()) >= fabsf(sample.imag())) {
+            return ((sample.real() > 0.0f ? 1.0f : -1.0f) * sample.imag() -
+                    (sample.imag() > 0.0f ? 1.0f : -1.0f) * sample.real() * K);
+        } else {
+            return ((sample.real() > 0.0f ? 1.0f : -1.0f) * sample.imag() * K -
+                    (sample.imag() > 0.0f ? 1.0f : -1.0f) * sample.real());
+        }
+    };
+
+    float phase_detector_4(gr_complex sample) const // for QPSK
+    {
+        return ((sample.real() > 0.0f ? 1.0f : -1.0f) * sample.imag() -
+                (sample.imag() > 0.0f ? 1.0f : -1.0f) * sample.real());
+    };
+
+    float phase_detector_2(gr_complex sample) const // for BPSK
+    {
+        return (sample.real() * sample.imag());
+    }
+
+    float phase_detector_snr_8(gr_complex sample) const // for 8PSK
+    {
+        const float K = (sqrtf(2.0) - 1.0);
+        const float snr = std::norm(sample) / d_noise;
+        if (fabsf(sample.real()) >= fabsf(sample.imag())) {
+            return ((blocks::tanhf_lut(snr * sample.real()) * sample.imag()) -
+                    (blocks::tanhf_lut(snr * sample.imag()) * sample.real() * K));
+        } else {
+            return ((blocks::tanhf_lut(snr * sample.real()) * sample.imag() * K) -
+                    (blocks::tanhf_lut(snr * sample.imag()) * sample.real()));
+        }
+    };
+
+    float phase_detector_snr_4(gr_complex sample) const // for QPSK
+    {
+        const float snr = std::norm(sample) / d_noise;
+        return ((blocks::tanhf_lut(snr * sample.real()) * sample.imag()) -
+                (blocks::tanhf_lut(snr * sample.imag()) * sample.real()));
+    };
+
+    float phase_detector_snr_2(gr_complex sample) const // for BPSK
+    {
+        const float snr = std::norm(sample) / d_noise;
+        return blocks::tanhf_lut(snr * sample.real()) * sample.imag();
+    };
+
+public:
+    costas_loop_cc_impl(float loop_bw,
+                        unsigned int order,
+                        bool use_snr = false);
+
+    ~costas_loop_cc_impl() override;
+
+    float error() const override { return d_error; };
+
+    void handle_set_noise(pmt::pmt_t msg);
+
+    int work(int noutput_items,
+             gr_vector_const_void_star& input_items,
+             gr_vector_void_star& output_items) override;
+};
+
+} // namespace elen90089
+} // namespace gr
+
+#endif /* INCLUDED_ELEN90089_COSTAS_LOOP_CC_IMPL_H */
diff --git a/lib/header_format_cdc.cc b/lib/header_format_cdc.cc
index efcac9486d04fce0be3813de7827f441df1d463e..7de63b2326ee8ff47eb44041f077cb16e1cc4861 100644
--- a/lib/header_format_cdc.cc
+++ b/lib/header_format_cdc.cc
@@ -107,6 +107,10 @@ int header_format_cdc::header_payload()
         d_info, pmt::intern("payload symbols"), pmt::from_long(8*pktlen/d_bps));
     d_info = pmt::dict_add(
         d_info, pmt::intern("bps"), pmt::from_long(bps));
+    // hack to trigger reset of Costas loop phase in payload rx chain
+    // make sure CPO is removed before Costas block
+    d_info = pmt::dict_add(
+        d_info, pmt::intern("phase_est"), pmt::from_double(0.0));
 
     return static_cast<int>(pktlen);
 }
diff --git a/python/bindings/CMakeLists.txt b/python/bindings/CMakeLists.txt
index b5b89a83c55c18fb648b72d65b05b5e5d31c773c..9e0882b13b763a87efbcf157eeec24d4411b5f8f 100644
--- a/python/bindings/CMakeLists.txt
+++ b/python/bindings/CMakeLists.txt
@@ -34,6 +34,7 @@ list(APPEND elen90089_python_files
     symbol_mapper_c_python.cc
     header_format_cdc_python.cc
     constellation_decoder_cf_python.cc
+    costas_loop_cc_python.cc
     python_bindings.cc)
 
 GR_PYBIND_MAKE_OOT(elen90089
diff --git a/python/bindings/costas_loop_cc_python.cc b/python/bindings/costas_loop_cc_python.cc
new file mode 100644
index 0000000000000000000000000000000000000000..e238f5b480dc26fcea5d9a7e0038f55962dfa033
--- /dev/null
+++ b/python/bindings/costas_loop_cc_python.cc
@@ -0,0 +1,49 @@
+/*
+ * Copyright 2022 Free Software Foundation, Inc.
+ *
+ * This file is part of GNU Radio
+ *
+ * SPDX-License-Identifier: GPL-3.0-or-later
+ *
+ */
+
+/***********************************************************************************/
+/* This file is automatically generated using bindtool and can be manually edited  */
+/* The following lines can be configured to regenerate this file during cmake      */
+/* If manual edits are made, the following tags should be modified accordingly.    */
+/* BINDTOOL_GEN_AUTOMATIC(0)                                                       */
+/* BINDTOOL_USE_PYGCCXML(0)                                                        */
+/* BINDTOOL_HEADER_FILE(costas_loop_cc.h)                                        */
+/* BINDTOOL_HEADER_FILE_HASH(bacca9183ea68e73c7a05fcc743d5516)                     */
+/***********************************************************************************/
+
+#include <pybind11/complex.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+namespace py = pybind11;
+
+#include <elen90089/costas_loop_cc.h>
+// pydoc.h is automatically generated in the build directory
+#include <costas_loop_cc_pydoc.h>
+
+void bind_costas_loop_cc(py::module& m)
+{
+    using costas_loop_cc = ::gr::elen90089::costas_loop_cc;
+
+    py::class_<costas_loop_cc,
+               gr::sync_block,
+               gr::block,
+               gr::basic_block,
+               std::shared_ptr<costas_loop_cc>>(m, "costas_loop_cc", D(costas_loop_cc))
+
+        .def(py::init(&costas_loop_cc::make),
+             py::arg("loop_bw"),
+             py::arg("order"),
+             py::arg("use_snr") = false,
+             D(costas_loop_cc,make))
+
+        .def("error",
+             &costas_loop_cc::error,
+             D(costas_loop_cc,error));
+}
diff --git a/python/bindings/docstrings/costas_loop_cc_pydoc_template.h b/python/bindings/docstrings/costas_loop_cc_pydoc_template.h
new file mode 100644
index 0000000000000000000000000000000000000000..2f1169402bdb0a018a4e952b2b83f169f2902731
--- /dev/null
+++ b/python/bindings/docstrings/costas_loop_cc_pydoc_template.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright 2022 Free Software Foundation, Inc.
+ *
+ * This file is part of GNU Radio
+ *
+ * SPDX-License-Identifier: GPL-3.0-or-later
+ *
+ */
+#include "pydoc_macros.h"
+#define D(...) DOC(gr,elen90089, __VA_ARGS__ )
+/*
+  This file contains placeholders for docstrings for the Python bindings.
+  Do not edit! These were automatically extracted during the binding process
+  and will be overwritten during the build process
+ */
+
+
+ 
+ static const char *__doc_gr_elen90089_costas_loop_cc = R"doc()doc";
+
+
+ static const char *__doc_gr_elen90089_costas_loop_cc_costas_loop_cc_0 = R"doc()doc";
+
+
+ static const char *__doc_gr_elen90089_costas_loop_cc_costas_loop_cc_1 = R"doc()doc";
+
+
+ static const char *__doc_gr_elen90089_costas_loop_cc_make = R"doc()doc";
+
+
+ static const char *__doc_gr_elen90089_costas_loop_cc_error = R"doc()doc";
+
+  
diff --git a/python/bindings/python_bindings.cc b/python/bindings/python_bindings.cc
index 05ab0ceb79ab46242dbb907dc22920580d4342ee..3725944e4b3c1fcd00d9f181fad9b7dfdfd7be42 100644
--- a/python/bindings/python_bindings.cc
+++ b/python/bindings/python_bindings.cc
@@ -26,6 +26,7 @@ namespace py = pybind11;
     void bind_symbol_mapper_c(py::module& m);
     void bind_header_format_cdc(py::module& m);
     void bind_constellation_decoder_cf(py::module& m);
+    void bind_costas_loop_cc(py::module& m);
 // ) END BINDING_FUNCTION_PROTOTYPES
 
 
@@ -60,5 +61,6 @@ PYBIND11_MODULE(elen90089_python, m)
     bind_symbol_mapper_c(m);
     bind_header_format_cdc(m);
     bind_constellation_decoder_cf(m);
+    bind_costas_loop_cc(m);
     // ) END BINDING_FUNCTION_CALLS
 }
\ No newline at end of file