diff --git a/include/libswiftnav/correlate.h b/include/libswiftnav/correlate.h
index 7bacc09d..1df62828 100644
--- a/include/libswiftnav/correlate.h
+++ b/include/libswiftnav/correlate.h
@@ -1,6 +1,7 @@
 /*
- * Copyright (C) 2013 Swift Navigation Inc.
+ * Copyright (C) 2013,2016 Swift Navigation Inc.
  * Contact: Fergus Noble <fergus@swift-nav.com>
+ * Contact: Adel Mamin <adelm@exafore.com>
  *
  * This source is subject to the license found in the file 'LICENSE' which must
  * be be distributed together with this source. All other rights reserved.
@@ -15,12 +16,31 @@
 
 #include <libswiftnav/common.h>
 
-void track_correlate(s8* samples, s8* code,
-                     double* init_code_phase, double code_step,
-                     double* init_carr_phase, double carr_step,
-                     double* I_E, double* Q_E,
-                     double* I_P, double* Q_P,
-                     double* I_L, double* Q_L,
-                     u32* num_samples);
+void l1_ca_track_correlate(const s8* samples, size_t samples_len,
+                           const s8* code,
+                           double* init_code_phase, double code_step,
+                           double* init_carr_phase, double carr_step,
+                           double* I_E, double* Q_E,
+                           double* I_P, double* Q_P,
+                           double* I_L, double* Q_L, u32* num_samples);
+
+void l2c_cm_track_correlate(const s8* samples, size_t samples_len,
+                            const s8* code,
+                            double* init_code_phase, double code_step,
+                            double* init_carr_phase, double carr_step,
+                            double* I_E, double* Q_E,
+                            double* I_P, double* Q_P,
+                            double* I_L, double* Q_L, u32* num_samples);
+
+void glo_ca_track_correlate(const s8* samples, size_t samples_len,
+                            const s8* code,
+                            double* init_code_phase,
+                            double code_step,
+                            double* init_carr_phase,
+                            double carr_step,
+                            double* I_E, double* Q_E,
+                            double* I_P, double* Q_P,
+                            double* I_L, double* Q_L,
+                            u32* num_samples);
 
 #endif /* LIBSWIFTNAV_CORRELATE_H */
diff --git a/include/libswiftnav/counter_checker.h b/include/libswiftnav/counter_checker.h
new file mode 100644
index 00000000..09f4b5c7
--- /dev/null
+++ b/include/libswiftnav/counter_checker.h
@@ -0,0 +1,121 @@
+/*
+ * Copyright (C) 2016 Swift Navigation Inc.
+ * Contact: Adel Mamin <adelm@exafore.com>
+ *
+ * This source is subject to the license found in the file 'LICENSE' which must
+ * be be distributed together with this source. All other rights reserved.
+ *
+ * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
+ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
+ */
+
+/**
+  @file
+
+  This utility helps to verify the integrity of samples data from Piksi v3 HW.
+  The regular Piksi v3 samples data format in one byte is:
+  RF4 RF3 RF2 RF1
+   00  00  00  00
+
+  RF1 - GPS L1
+  RF2 - GLONASS L1
+  RF3 - GLONASS L2
+  RF4 - GPS L2
+
+  This utility is expecting RF3 and RF2 to have 4 bits counter,
+  which is continuosly incremented with modulo SAMPLE_COUNTER_MODULO.
+  RF2 is expected to have two least significant bits
+  and RF3 - two most significant bits.
+  Therefore, the bit indexes are:
+  RF3 RF2
+  5&4 3&2
+*/
+
+#ifndef LIBSWIFTNAV_COUNTER_CHECKER_H
+#define LIBSWIFTNAV_COUNTER_CHECKER_H
+
+#include <stdint.h>
+#include <stddef.h>
+#include <libswiftnav/common.h>
+
+/** How many bytes we read from samples stream at a time. */
+#define SAMPLE_CHUNK_SIZE (1024 * 1024) /* [bytes] */
+
+/** The counter embedded into the sample data stream is expected
+    to have this modulo. */
+#define SAMPLE_COUNTER_MODULO 13
+
+/** The counter bit size. */
+#define SAMPLE_COUNTER_BITS   4 /* [bits] */
+
+/** Teh counter bit offset within a byte. */
+#define SAMPLE_COUNTER_OFFSET 2 /* [bits] */
+
+/** How many bytes we read from samples file at a time. */
+#define COUNTER_CHECKER_CHUNK_SIZE (1024 * 1024) /* [bytes] */
+
+/** Get \e num of bits at \e offset in \e data */
+#define GET_BITS(data, offset, num) \
+  ( ((data) >> (offset)) & ((1 << (num)) - 1) )
+
+/** Get \e num of bits at \e offset in \e data */
+#define SET_BITS(data, offset, num, bits) \
+  ( ( ~( ((1 << (num)) - 1) << (offset) ) & (data) ) | \
+  ( ( (bits) & ((1 << (num)) - 1) ) << (offset) ) )
+
+/** Get GPS counter
+ * \param data Data byte
+ * \return The counter value
+ */
+uint8_t get_gps_counter(uint8_t data);
+
+/** Get Glonass counter
+ * \param data Data byte
+ * \return The counter value
+ */
+uint8_t get_glo_counter(uint8_t data);
+
+uint8_t set_gps_counter(uint8_t data, uint8_t counter);
+uint8_t set_glo_counter(uint8_t data, uint8_t counter);
+
+/** A data mismatch descriptor. */
+struct mismatch {
+  size_t offset;                /**! Data offset [bytes]. */
+  uint8_t data;                 /**! Data. */
+  uint8_t expected_counter;     /**! The expected counter value. */
+  uint8_t actual_counter;       /**! The actual counter value. */
+};
+
+/** Mismatch array data */
+struct mismatch_data {
+  /** The sample data counter mismatch incidents are stored here. */
+  struct mismatch data[COUNTER_CHECKER_CHUNK_SIZE];
+  /** How many valid entries there are in \e mismatch array. */
+  size_t counter;
+};
+
+struct callbacks {
+  size_t (*read)(void *ptr, size_t size, void *f);
+  void (*rewind)(void *f);
+  uint8_t (*get_counter)(uint8_t data);
+  uint8_t (*set_counter)(uint8_t data, uint8_t counter);
+};
+
+typedef size_t (*stream_read_t)(void *ptr, size_t size, void *stream);
+typedef void   (*stream_rewind_t)(void *stream);
+
+uint8_t get_rf32_counter(uint8_t data);
+uint8_t set_rf32_counter(uint8_t data, uint8_t counter);
+uint8_t get_rf41_counter(uint8_t data);
+uint8_t set_rf41_counter(uint8_t data, uint8_t counter);
+
+void report_mismatch(const struct mismatch_data *mismatch);
+
+void counter_checker_init(void);
+
+const struct mismatch_data *counter_checker_run(struct callbacks *cbs,
+                                               void *stream,
+                                               size_t data_size);
+
+#endif /* LIBSWIFTNAV_COUNTER_CHECKER_H */
diff --git a/libfec/src/CMakeLists.txt b/libfec/src/CMakeLists.txt
index 9bc4d638..c690d24c 100644
--- a/libfec/src/CMakeLists.txt
+++ b/libfec/src/CMakeLists.txt
@@ -11,4 +11,4 @@ add_library(fec viterbi27.c)
 
 install(TARGETS fec DESTINATION lib${LIB_SUFFIX})
 
-install(FILES ${libfec_HEADERS} DESTINATION include/)
+install(FILES ${libfec_HEADERS} DESTINATION include/libfec/)
diff --git a/python/setup.py b/python/setup.py
index 53f0fafc..ab0514ea 100755
--- a/python/setup.py
+++ b/python/setup.py
@@ -24,19 +24,37 @@
     print "You don't seem to have Cython installed."
     sys.exit(1)
   os.environ['ARCHFLAGS'] = ""
+
+  # Additional library search directories:
+  # - LD_LIBRARY_PATH (if present)
+  # - User local enviroment
+  library_dirs = []
+  # If LD_LIBRARY_PATH has been manually specified, add it to the
+  # library search path
+  if 'LD_LIBRARY_PATH' in os.environ:
+    library_dirs.append(os.environ['LD_LIBRARY_PATH'])
+  library_dirs.append(os.path.expanduser('~/.local/lib'))
+
+  # Additional include directories:
+  # - Numpy includes
+  # - User local enviroment
+  # - Current directory
+  include_dirs = []
+  include_dirs.append(np.get_include())
+  include_dirs.append(os.path.expanduser('~/.local/include'))
+  include_dirs.append('.')
+  # three more includes for travis builds as it does not install libraries
+  include_dirs.append('../include/')
+  include_dirs.append('../libfec/include/')
+  include_dirs.append('../tests/data/l2cbitstream/')
   def make_extension(ext_name):
     ext_path = ext_name.replace('.', os.path.sep) + '.pyx'
-    library_dirs = []
-    # If LD_LIBRARY_PATH has been manually specified, add it to the
-    # library search path
-    if 'LD_LIBRARY_PATH' in os.environ:
-      library_dirs.append(os.environ['LD_LIBRARY_PATH'])
     return Extension(
       ext_name, [ext_path],
-      include_dirs = [np.get_include(), '.', '../include/'],
+      include_dirs = include_dirs,
       extra_compile_args = ['-O0', '-g'],
       extra_link_args = ['-g'],
-      libraries = ['m', 'swiftnav'],
+      libraries = ['m', 'swiftnav', 'l2cbitstream'],
       library_dirs = library_dirs,
     )
   ext_names = [
@@ -44,6 +62,7 @@ def make_extension(ext_name):
     'swiftnav.signal',
     'swiftnav.coord_system',
     'swiftnav.constants',
+    'swiftnav.cnav_msg',
     'swiftnav.nav_msg',
     'swiftnav.pvt',
     'swiftnav.correlate',
diff --git a/python/swiftnav/bits.pyx b/python/swiftnav/bits.pyx
index 2b7b44a9..4fe0d555 100644
--- a/python/swiftnav/bits.pyx
+++ b/python/swiftnav/bits.pyx
@@ -12,10 +12,23 @@
 Bit field packing, unpacking and utility functions.
 
 """
-
+from swiftnav.bits cimport parity as c_parity
 
 def parity(x):
-  raise NotImplementedError
+  '''
+  Cython wrapper for parity function
+
+  Parameters
+  ----------
+  x : int
+    Value for parity computation
+
+  Returns
+  -------
+  int
+    Parity value: 1 or 0.
+  '''
+  return c_parity(x)
 
 def getbitu(buff,  pos,  length):
   raise NotImplementedError
diff --git a/python/swiftnav/cnav_msg.pxd b/python/swiftnav/cnav_msg.pxd
new file mode 100644
index 00000000..04c7659e
--- /dev/null
+++ b/python/swiftnav/cnav_msg.pxd
@@ -0,0 +1,36 @@
+# Copyright (C) 2016 Swift Navigation Inc.
+#
+# This source is subject to the license found in the file 'LICENSE' which must
+# be be distributed together with this source. All other rights reserved.
+#
+# THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
+# EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
+# WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
+
+from common cimport *
+from libcpp cimport bool
+
+cdef extern from "libswiftnav/cnav_msg.h":
+
+  ctypedef struct cnav_msg_decoder_t:
+    pass
+
+  ctypedef struct cnav_msg_t:
+    u8   prn    # SV PRN. 0..31
+    u8   msg_id # Message id. 0..31
+    u32  tow    # GPS ToW in 6-second units. Multiply to 6 to get seconds. 
+    bool alert  # CNAV message alert flag
+
+  void cnav_msg_decoder_init(cnav_msg_decoder_t *dec)
+  bool cnav_msg_decoder_add_symbol(cnav_msg_decoder_t *dec,
+                                   u8 symbol,
+                                   cnav_msg_t *msg,
+                                   u32 *delay)
+
+cdef extern from "libl2cbitstream/l2cbitstream.h":
+  bool get_l2c_message(u8 *au_message, u8 prn, u8 msg_id, u32 tow)
+
+cdef class CNavMsgDecoder:
+  cdef cnav_msg_decoder_t _thisptr
+cdef class CNavMsg:
+  cdef cnav_msg_t _thisptr
diff --git a/python/swiftnav/cnav_msg.pyx b/python/swiftnav/cnav_msg.pyx
new file mode 100644
index 00000000..e478a0a3
--- /dev/null
+++ b/python/swiftnav/cnav_msg.pyx
@@ -0,0 +1,95 @@
+# Copyright (C) 2016 Swift Navigation Inc.
+#
+# This source is subject to the license found in the file 'LICENSE' which must
+# be be distributed together with this source. All other rights reserved.
+#
+# THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
+# EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
+# WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
+
+from fmt_utils import fmt_repr
+from libc.string cimport memcpy, memset
+cimport numpy as np
+import numpy as np
+
+cdef class CNavMsgDecoder:
+
+  def __init__(self, **kwargs):
+    memset(&self._thisptr, 0, sizeof(cnav_msg_decoder_t))
+    cnav_msg_decoder_init(&self._thisptr)
+
+  def __repr__(self):
+    return fmt_repr(self)
+
+  def decode(self, u8 symbol, CNavMsg msg):
+    cdef u32 delay = 0
+    res = cnav_msg_decoder_add_symbol(&self._thisptr,
+                                      symbol,
+                                      &msg._thisptr,
+                                      &delay)
+    return res, delay
+
+cdef class CNavMsg:
+  def __init__(self, **kwargs):
+    memset(&self._thisptr, 0, sizeof(cnav_msg_t))
+    if 'prn' in kwargs:
+      self._thisptr.prn = kwargs.pop('prn')
+    if 'msg_id' in kwargs:
+      self._thisptr.msg_id = kwargs.pop('msg_id')
+    if 'tow' in kwargs:
+      self._thisptr.tow = kwargs.pop('tow')
+    if 'alert' in kwargs:
+      self._thisptr.alert = kwargs.pop('alert')
+
+  def __getattr__(self, k):
+    return self._thisptr.get(k)
+
+  def __repr__(self):
+    return fmt_repr(self)
+
+  def to_dict(self):
+    return self._thisptr
+
+  def from_dict(self, d):
+    self._thisptr = d
+
+  def __reduce__(self):
+    return (rebuild_CNavMsg, tuple([tuple(self.to_dict().items())]))
+  
+  def getPrn(self):
+    return self._thisptr.prn
+
+  def getTow(self):
+    return self._thisptr.tow
+
+  def getMsgId(self):
+    return self._thisptr.msg_id
+
+  def getAlert(self):
+    return self._thisptr.alert
+
+def rebuild_CNavMsg(reduced):
+  """
+  Rebuild CNavMsg for unpickling.
+
+  Parameters
+  ----------
+  reduced: tuple
+    Tuple of dict of NavMsg cnav_msg_t struct fields
+
+  Returns
+  -------
+  out: :class:`CNavMsg` instance
+    Rebuilt :class:`CNavMsg` instance
+  """
+  nm = CNavMsg()
+  nm.from_dict(dict(reduced))
+  return nm
+
+
+cdef class CNavRawMsg:
+  @staticmethod
+  def generate(prn, msg_id, tow):
+    cdef np.ndarray[np.uint8_t, ndim=1, mode="c"] tmp_ = np.ndarray(38, dtype=np.uint8)
+    res = get_l2c_message(&tmp_[0], prn, msg_id, tow)
+    return np.unpackbits(tmp_)[4:]
diff --git a/python/swiftnav/correlate.pxd b/python/swiftnav/correlate.pxd
index c3ee29d0..f4bb3e0d 100644
--- a/python/swiftnav/correlate.pxd
+++ b/python/swiftnav/correlate.pxd
@@ -1,4 +1,5 @@
-# Copyright (C) 2015 Swift Navigation Inc.
+# Copyright (C) 2015,2016 Swift Navigation Inc.
+# Contact: Adel Mamin <adelm@exafore.com>
 #
 # This source is subject to the license found in the file 'LICENSE' which must
 # be be distributed together with this source. All other rights reserved.
@@ -10,10 +11,27 @@
 from common cimport *
 
 cdef extern from "libswiftnav/correlate.h":
-  void track_correlate(s8* samples, s8* code,
+  void l1_ca_track_correlate(s8* samples, size_t samples_len, s8* code,
                        double* init_code_phase, double code_step,
                        double* init_carr_phase, double carr_step,
                        double* I_E, double* Q_E,
                        double* I_P, double* Q_P,
                        double* I_L, double* Q_L,
                        u32* num_samples)
+
+  void l2c_cm_track_correlate(s8* samples, size_t samples_len, s8* code,
+                       double* init_code_phase, double code_step,
+                       double* init_carr_phase, double carr_step,
+                       double* I_E, double* Q_E,
+                       double* I_P, double* Q_P,
+                       double* I_L, double* Q_L,
+                       u32* num_samples)
+
+  void glo_ca_track_correlate(s8* samples, size_t samples_len, s8* code,
+                       double* init_code_phase, double code_step,
+                       double* init_carr_phase, double carr_step,
+                       double* I_E, double* Q_E,
+                       double* I_P, double* Q_P,
+                       double* I_L, double* Q_L,
+                       u32* num_samples);
+
diff --git a/python/swiftnav/correlate.pyx b/python/swiftnav/correlate.pyx
index 2c75e57c..f39a170b 100644
--- a/python/swiftnav/correlate.pyx
+++ b/python/swiftnav/correlate.pyx
@@ -1,4 +1,5 @@
-# Copyright (C) 2012 Swift Navigation Inc.
+# Copyright (C) 2012,2016 Swift Navigation Inc.
+# Contact: Adel Mamin <adelm@exafore.com>
 #
 # This source is subject to the license found in the file 'LICENSE' which must
 # be be distributed together with this source. All other rights reserved.
@@ -29,15 +30,26 @@ cdef extern from "complexobject.h":
   ctypedef class __builtin__.complex [object PyComplexObject]:
     cdef Py_complex cval
 
-def track_correlate_(np.ndarray[char, ndim=1, mode="c"] samples,
-                     code_freq, code_phase, carr_freq, carr_phase,
-                     np.ndarray[char, ndim=1, mode="c"] code,
-                     sampling_freq):
+def track_correlate(np.ndarray[char, ndim=1, mode="c"] samples,
+                    code_freq, code_phase, carr_freq, carr_phase,
+                    np.ndarray[char, ndim=1, mode="c"] code,
+                    sampling_freq, signal):
   cdef double init_code_phase = code_phase
   cdef double init_carr_phase = carr_phase
   cdef double I_E, Q_E, I_P, Q_P, I_L, Q_L
   cdef unsigned int blksize
-  track_correlate(<s8*>&samples[0], <s8*>&code[0],
+  if signal == "l1ca":
+    l1_ca_track_correlate(<s8*>&samples[0], len(samples), <s8*>&code[0],
+                  &init_code_phase, code_freq / sampling_freq,
+                  &init_carr_phase, carr_freq * 2.0 * M_PI / sampling_freq,
+                  &I_E, &Q_E, &I_P, &Q_P, &I_L, &Q_L, &blksize)
+  elif signal == "gloca":
+    glo_ca_track_correlate(<s8*>&samples[0], len(samples), <s8*>&code[0],
+                  &init_code_phase, code_freq / sampling_freq,
+                  &init_carr_phase, carr_freq * 2.0 * M_PI / sampling_freq,
+                  &I_E, &Q_E, &I_P, &Q_P, &I_L, &Q_L, &blksize)
+  else:
+    l2c_cm_track_correlate(<s8*>&samples[0], len(samples), <s8*>&code[0],
                   &init_code_phase, code_freq / sampling_freq,
                   &init_carr_phase, carr_freq * 2.0 * M_PI / sampling_freq,
                   &I_E, &Q_E, &I_P, &Q_P, &I_L, &Q_L, &blksize)
diff --git a/python/swiftnav/edc.pxd b/python/swiftnav/edc.pxd
index 5b8e496b..8fb091e7 100644
--- a/python/swiftnav/edc.pxd
+++ b/python/swiftnav/edc.pxd
@@ -8,6 +8,8 @@
 # WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
 
 from common cimport *
+from libcpp cimport bool
 
 cdef extern from "libswiftnav/edc.h":
   u32 crc24q(const u8 *buf, u32 len, u32 crc)
+  u32 crc24q_bits(u32 crc, const u8 *buf, u32 n_bits, bool invert)
diff --git a/python/swiftnav/ephemeris.pyx b/python/swiftnav/ephemeris.pyx
index 35a3dc81..311bc1d6 100644
--- a/python/swiftnav/ephemeris.pyx
+++ b/python/swiftnav/ephemeris.pyx
@@ -23,12 +23,19 @@ cdef class Ephemeris:
 
   def __init__(self, **kwargs):
     memset(&self._thisptr, 0, sizeof(ephemeris_t))
-    self._thisptr.sid = kwargs.pop('sid')
-    self._thisptr.toe = kwargs.pop('toe')
-    self._thisptr.ura = kwargs.pop('ura')
-    self._thisptr.fit_interval = kwargs.pop('fit_interval')
-    self._thisptr.valid = kwargs.pop('valid')
-    self._thisptr.healthy = kwargs.pop('healthy')
+
+    if 'sid' in kwargs:
+      self._thisptr.sid = kwargs.pop('sid')
+    if 'toe' in kwargs:
+      self._thisptr.toe = kwargs.pop('toe')
+    if 'ura' in kwargs:
+      self._thisptr.ura = kwargs.pop('ura')
+    if 'fit_interval' in kwargs:
+      self._thisptr.fit_interval = kwargs.pop('fit_interval')
+    if 'valid' in kwargs:
+      self._thisptr.valid = kwargs.pop('valid')
+    if 'healthy' in kwargs:
+      self._thisptr.healthy = kwargs.pop('healthy')
     if 'kepler' in kwargs:
       self._thisptr.kepler = kwargs.pop('kepler')
     elif 'xyz' in kwargs:
diff --git a/python/swiftnav/nav_msg.pxd b/python/swiftnav/nav_msg.pxd
index d51ab325..61c758a1 100644
--- a/python/swiftnav/nav_msg.pxd
+++ b/python/swiftnav/nav_msg.pxd
@@ -37,3 +37,4 @@ cdef extern from "libswiftnav/nav_msg.h":
 
 cdef class NavMsg:
   cdef nav_msg_t _thisptr
+
diff --git a/python/swiftnav/nav_msg.pyx b/python/swiftnav/nav_msg.pyx
index f2ddf1db..308ef4aa 100644
--- a/python/swiftnav/nav_msg.pyx
+++ b/python/swiftnav/nav_msg.pyx
@@ -9,7 +9,7 @@
 
 # cython: embedsignature=True
 
-from ephemeris cimport ephemeris_t
+from swiftnav.ephemeris import Ephemeris
 from fmt_utils import fmt_repr
 from time cimport gps_time_t
 from signal cimport gnss_signal_t
@@ -40,9 +40,8 @@ cdef class NavMsg:
   def subframe_ready(self):
     return subframe_ready(&self._thisptr)
 
-  def process_subframe(self, e):
-    cdef ephemeris_t tmp = e._thisptr
-    return process_subframe(&self._thisptr, &tmp)
+  def process_subframe(self, Ephemeris e):
+    return process_subframe(&self._thisptr, &e._thisptr)
 
   def __richcmp__(self, other, op):
     """
diff --git a/python/swiftnav/track.pxd b/python/swiftnav/track.pxd
index e5e23b67..6f28daaa 100644
--- a/python/swiftnav/track.pxd
+++ b/python/swiftnav/track.pxd
@@ -124,6 +124,7 @@ cdef extern from "libswiftnav/track.h":
   void alias_detect_init(alias_detect_t *a, u32 acc_len, float time_diff)
   void alias_detect_first(alias_detect_t *a, float I, float Q)
   float alias_detect_second(alias_detect_t *a, float I, float Q)
+  void alias_detect_reinit(alias_detect_t *a, u32 acc_len, float time_diff)
 
   # Tracking loop: Lock detect
   struct loop_detect_lpf:
@@ -199,7 +200,7 @@ cdef class SimpleTrackingLoop:
   cdef simple_tl_state_t _thisptr
 
 cdef class AidedLoopFilter:
-   cdef aided_lf_state_t _thisptr
+  cdef aided_lf_state_t _thisptr
 
 cdef class AidedTrackingLoop:
   cdef aided_tl_state_t _thisptr
@@ -223,6 +224,6 @@ cdef class ChannelMeasurement:
   cdef channel_measurement_t _thisptr
 
 cdef class NavigationMeasurement:
- cdef navigation_measurement_t _thisptr
+  cdef navigation_measurement_t _thisptr
 
 cdef mk_nav_meas_array(py_nav_meas, u8 n_c_nav_meas, navigation_measurement_t *c_nav_meas)
diff --git a/python/swiftnav/track.pyx b/python/swiftnav/track.pyx
index c6636bdc..512bb357 100644
--- a/python/swiftnav/track.pyx
+++ b/python/swiftnav/track.pyx
@@ -250,7 +250,6 @@ cdef class AidedTrackingLoop:
                   kwargs['carr_k'],
                   kwargs['carr_freq_b1'])
 
-
   def retune(self, code_params, carr_params, loop_freq, carr_freq_igain, carr_to_code):
     """
     Retune the tracking loop.
@@ -267,15 +266,13 @@ cdef class AidedTrackingLoop:
       FLL aiding gain
 
     """
-    self.loop_freq = loop_freq
-    self.code_bw, self.code_zeta, self.code_k = code_params
-    self.carr_bw, self.carr_zeta, self.carr_k = carr_params
-    self.carr_freq_igain = carr_freq_igain
-    aided_tl_retune(&self._thisptr, self.loop_freq,
-                            self.code_bw, self.code_zeta, self.code_k,
-                            self.carr_to_code,
-                            self.carr_bw, self.carr_zeta, self.carr_k,
-                            self.carr_freq_igain)
+    code_bw, code_zeta, code_k = code_params
+    carr_bw, carr_zeta, carr_k = carr_params
+    aided_tl_retune(&self._thisptr, loop_freq,
+                    code_bw, code_zeta, code_k,
+                    carr_to_code,
+                    carr_bw, carr_zeta, carr_k,
+                    carr_freq_igain)
 
   def update(self, E, P, L):
     """
@@ -367,19 +364,20 @@ cdef class LockDetector:
   """
 
   def __cinit__(self, **kwargs):
-    self._thisptr = kwargs
     lock_detect_init(&self._thisptr,
-                     self._thisptr.k1,
-                     self._thisptr.k2,
-                     self._thisptr.thislp,
-                     self._thisptr.lo)
+                     kwargs['k1'],
+                     kwargs['k2'],
+                     kwargs['lp'],
+                     kwargs['lo'])
 
   def reinit(self, k1, k2, lp, lo):
     lock_detect_reinit(&self._thisptr, k1, k2, lp, lo)
 
   def update(self, I, Q, DT):
     lock_detect_update(&self._thisptr, I, Q, DT)
-    return (self._thisptr.outo, self._thisptr.outp)
+    return (self._thisptr.outo, self._thisptr.outp, \
+            self._thisptr.pcount1, self._thisptr.pcount2,\
+            self._thisptr.lpfi.y, self._thisptr.lpfq.y)
 
 
 cdef class AliasDetector:
@@ -393,6 +391,9 @@ cdef class AliasDetector:
   def second(self, I, Q):
     return alias_detect_second(&self._thisptr, I, Q)
 
+  def reinit(self, acc_len, time_diff):
+    return alias_detect_reinit(&self._thisptr, acc_len, time_diff)
+
 cdef class CN0Estimator:
   """
   Wraps the `libswiftnav` :math:`C / N_0` estimator implementation.
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 3258c1f5..67348bef 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -46,6 +46,7 @@ set(libswiftnav_SRCS
   ionosphere.c
   bit_sync.c
   cnav_msg.c
+  counter_checker/counter_checker.c
   ${plover_SRCS}
 
   CACHE INTERNAL ""
diff --git a/src/correlate.c b/src/correlate.c
index afe1f32e..747a6f5b 100644
--- a/src/correlate.c
+++ b/src/correlate.c
@@ -1,6 +1,7 @@
 /*
- * Copyright (C) 2013 Swift Navigation Inc.
+ * Copyright (C) 2013,2016 Swift Navigation Inc.
  * Contact: Fergus Noble <fergus@swift-nav.com>
+ * Contact: Adel Mamin <adelm@exafore.com>
  *
  * This source is subject to the license found in the file 'LICENSE' which must
  * be be distributed together with this source. All other rights reserved.
@@ -11,6 +12,7 @@
  */
 
 #include <math.h>
+#include <stdlib.h>
 
 #ifdef __SSSE3__
 #include <tmmintrin.h>
@@ -22,11 +24,317 @@
  * Correlators used for tracking.
  * \{ */
 
+enum correlator_type {
+  L1CA_CORRELATOR,
+  L2C_CORRELATOR,
+  GLOCA_CORRELATOR
+};
+
+#define L1_CA_CHIPS_PER_PRN_CODE    1023
+#define L2C_CM_CHIPS_PER_PRN_CODE   10230
+#define GLO_CA_CHIPS_PER_PRN_CODE   511
+
+static void track_correlate(enum correlator_type correlator_type,
+                            const s8* restrict samples,
+                            const s8* restrict code,
+                            double* restrict init_code_phase, double code_step,
+                            double* restrict init_carr_phase, double carr_step,
+                            double* restrict I_E, double* restrict Q_E,
+                            double* restrict I_P, double* restrict Q_P,
+                            double* restrict I_L, double* restrict Q_L,
+                            u32 num_samples);
+
+/** Perform L1C/A correlation.
+ *
+ * \param samples          Samples array. One byte per sample.
+ * \param samples_len      Samples array size.
+ * \param code             L1C/A PRN code. One byte per chip: 1023 bytes long.
+ * \param[in,out] init_code_phase  Initial code phase [chips].
+ *                         The function returns the
+ *                         the last unprocessed code phase here.
+ * \param code_step        Code phase increment step [chips].
+ * \param[in,out] init_carr_phase  Initial carrier phase [radians].
+ *                         The function returns the the last unprocessed carrier
+ *                         phase here.
+ * \param carr_step        Carrier phase increment step [radians].
+ * \param[out] I_E         Early replica in-phase correlation component.
+ * \param[out] Q_E         Early replica quadrature correlation component.
+ * \param[out] I_P         Prompt replica in-phase correlation component.
+ * \param[out] Q_P         Prompt replica quadrature correlation component.
+ * \param[out] I_L         Late replica in-phase correlation component.
+ * \param[out] Q_L         Late replica quadrature correlation component.
+ * \param[out] num_samples The number of processed samples from \e samples array.
+ */
+void l1_ca_track_correlate(const s8* restrict samples, size_t samples_len,
+                           const s8* restrict code,
+                           double* init_code_phase, double code_step,
+                           double* init_carr_phase, double carr_step,
+                           double* I_E, double* Q_E,
+                           double* I_P, double* Q_P,
+                           double* I_L, double* Q_L, u32* num_samples)
+{
+  *num_samples = (int)ceil((L1_CA_CHIPS_PER_PRN_CODE - *init_code_phase) /
+                 code_step);
+
+  if (0 == *num_samples) {
+    *num_samples = (int)ceil(2 * L1_CA_CHIPS_PER_PRN_CODE / code_step);
+  }
+
+  if (*num_samples > samples_len) {
+    *num_samples = samples_len;
+  }
+
+  if (0 == *num_samples) {
+    return;
+  }
+
+  track_correlate(L1CA_CORRELATOR, samples, code,
+                  init_code_phase, code_step, init_carr_phase, carr_step,
+                  I_E, Q_E, I_P, Q_P, I_L, Q_L, *num_samples);
+}
+
+/** Perform L2C CM correlation.
+ *
+ * \param samples          Samples array. One byte per sample.
+ * \param samples_len      Samples array size.
+ * \param code             L2C CM PRN code. One byte per chip: 10230 bytes long.
+ * \param[in,out] init_code_phase  Initial code phase [chips].
+ *                         The function returns the
+ *                         the last unprocessed code phase here.
+ * \param code_step        Code phase increment step [chips].
+ * \param[in,out] init_carr_phase  Initial carrier phase [radians].
+ *                         The function returns the the last unprocessed carrier
+ *                         phase here.
+ * \param carr_step        Carrier phase increment step [radians].
+ * \param[out] I_E         Early replica in-phase correlation component.
+ * \param[out] Q_E         Early replica quadrature correlation component.
+ * \param[out] I_P         Prompt replica in-phase correlation component.
+ * \param[out] Q_P         Prompt replica quadrature correlation component.
+ * \param[out] I_L         Late replica in-phase correlation component.
+ * \param[out] Q_L         Late replica quadrature correlation component.
+ * \param[out] num_samples The number of processed samples from \e samples array.
+ */
+void l2c_cm_track_correlate(const s8* samples, size_t samples_len,
+                            const s8* code,
+                            double* init_code_phase, double code_step,
+                            double* init_carr_phase, double carr_step,
+                            double* I_E, double* Q_E,
+                            double* I_P, double* Q_P,
+                            double* I_L, double* Q_L, u32* num_samples)
+{
+  *num_samples = (int)ceil((2 * L2C_CM_CHIPS_PER_PRN_CODE - *init_code_phase) /
+                 code_step);
+
+  if (0 == *num_samples) {
+    *num_samples = (int)ceil(2 * L2C_CM_CHIPS_PER_PRN_CODE / code_step);
+  }
+
+  if (*num_samples > samples_len) {
+    *num_samples = samples_len;
+  }
+
+  if (0 == *num_samples) {
+    return;
+  }
+
+  track_correlate(L2C_CORRELATOR, samples, code,
+                  init_code_phase, code_step, init_carr_phase, carr_step,
+                  I_E, Q_E, I_P, Q_P, I_L, Q_L, *num_samples);
+}
+
+/** Perform GLO CA correlation.
+ *
+ * \param samples          Samples array. One byte per sample.
+ * \param samples_len      Samples array size.
+ * \param code             L2C CM PRN code. One byte per chip: 10230 bytes long.
+ * \param[in,out] init_code_phase  Initial code phase [chips].
+ *                         The function returns the
+ *                         the last unprocessed code phase here.
+ * \param code_step        Code phase increment step [chips].
+ * \param[in,out] init_carr_phase  Initial carrier phase [radians].
+ *                         The function returns the the last unprocessed carrier
+ *                         phase here.
+ * \param carr_step        Carrier phase increment step [radians].
+ * \param[out] I_E         Early replica in-phase correlation component.
+ * \param[out] Q_E         Early replica quadrature correlation component.
+ * \param[out] I_P         Prompt replica in-phase correlation component.
+ * \param[out] Q_P         Prompt replica quadrature correlation component.
+ * \param[out] I_L         Late replica in-phase correlation component.
+ * \param[out] Q_L         Late replica quadrature correlation component.
+ * \param[out] num_samples The number of processed samples from \e samples array.
+ */
+void glo_ca_track_correlate(const s8* samples, size_t samples_len,
+                            const s8* code,
+                            double* init_code_phase,
+                            double code_step,
+                            double* init_carr_phase,
+                            double carr_step,
+                            double* I_E, double* Q_E,
+                            double* I_P, double* Q_P,
+                            double* I_L, double* Q_L,
+                            u32* num_samples)
+{
+  *num_samples = (int)ceil((2 * GLO_CA_CHIPS_PER_PRN_CODE -
+                   * init_code_phase) / code_step);
+
+  if (0 == *num_samples) {
+    *num_samples =
+      (int)ceil(2 * GLO_CA_CHIPS_PER_PRN_CODE / code_step);
+  }
+
+  if (*num_samples > samples_len) {
+    *num_samples = samples_len;
+  }
+
+  if (0 == *num_samples) {
+    return;
+  }
+
+  track_correlate(GLOCA_CORRELATOR, samples, code, init_code_phase,
+                  code_step, init_carr_phase, carr_step, I_E, Q_E, I_P,
+                  Q_P, I_L, Q_L, *num_samples);
+}
+
+/** Produce L1 C/A chip for the given code phase
+ *
+ * \param code       L1 C/A PRN code array. One byte per sample: 1023 bytes long.
+ * \param code_phase Code phase of the chip to return.
+ * \return Code chip.
+ */
+static inline s8 l1_ca_get_chip(const s8 *code, double code_phase)
+{
+  int i;
+  if (code_phase < 0) {
+    i = (int)(code_phase + L1_CA_CHIPS_PER_PRN_CODE);
+  } else if (code_phase >= L1_CA_CHIPS_PER_PRN_CODE) {
+    i = (int)(code_phase - L1_CA_CHIPS_PER_PRN_CODE);
+  } else {
+    i = (int)code_phase;
+  }
+  return code[i];
+}
+
+/** Produce a new L1 C/A code phase given current code phase and the step.
+ *
+ * \param code_phase Current code phase [chips].
+ * \param code_step  Code phase update step [chips].
+ * \return New code phase.
+ */
+static inline double l1_ca_get_code_phase(double code_phase, double code_step)
+{
+  code_phase += code_step;
+  if (code_phase >= L1_CA_CHIPS_PER_PRN_CODE) {
+    code_phase -= L1_CA_CHIPS_PER_PRN_CODE;
+  }
+  return code_phase;
+}
+
+/** Produce L2C CM chip for the given code phase
+ *
+ * \param code       L2C CM PRN code array. One byte per sample: 10230 bytes long.
+ * \param code_phase Code phase of the chip to return.
+ * \return Code chip.
+ */
+static inline s8 l2c_cm_get_chip(const s8 *code, double code_phase)
+{
+  if (code_phase < 0) {
+    code_phase += 2 * L2C_CM_CHIPS_PER_PRN_CODE;
+  } else if (code_phase >= 2 * L2C_CM_CHIPS_PER_PRN_CODE) {
+    code_phase -= 2 * L2C_CM_CHIPS_PER_PRN_CODE;
+  }
+  if ((int)code_phase & 1) {
+    return 0; // hit CL code, which is neglected by design
+  }
+
+  return -code[(int)code_phase / 2]; // otherwise return CM code
+}
+
+/** Produce a new L2C CM code phase given current code phase and the step.
+ *
+ * \param code_phase Current code phase [chips].
+ * \param code_step  Code phase update step [chips].
+ * \return New code phase.
+ */
+static inline double l2c_cm_get_code_phase(double code_phase, double code_step)
+{
+  code_phase += code_step;
+  if (code_phase >= 2 * L2C_CM_CHIPS_PER_PRN_CODE) {
+    code_phase -= 2 * L2C_CM_CHIPS_PER_PRN_CODE;
+  }
+  return code_phase;
+}
+
+/** Produce GLO C/A chip for the given code phase
+ *
+ * \param code       GLO C/A PRN code array.
+ *                   One byte per sample: 511 bytes long.
+ * \param code_phase Code phase of the chip to return.
+ * \return Code chip.
+ */
+static inline s8 glo_ca_get_chip(const s8 *code, double code_phase)
+{
+  int i;
+  if (code_phase < 0) {
+    i = (int)(code_phase + GLO_CA_CHIPS_PER_PRN_CODE);
+  } else if (code_phase >= GLO_CA_CHIPS_PER_PRN_CODE) {
+    i = (int)(code_phase - GLO_CA_CHIPS_PER_PRN_CODE);
+  } else {
+    i = (int)code_phase;
+  }
+  return code[i];
+}
+
+/** Produce a new GLO C/A code phase given current code phase
+ *  and the step.
+ *
+ * \param code_phase Current code phase [chips].
+ * \param code_step  Code phase update step [chips].
+ * \return New code phase.
+ */
+static inline double glo_ca_get_code_phase(double code_phase,
+                                           double code_step)
+{
+  code_phase += code_step;
+  if (code_phase >= GLO_CA_CHIPS_PER_PRN_CODE) {
+    code_phase -= GLO_CA_CHIPS_PER_PRN_CODE;
+  }
+  return code_phase;
+}
+
 #ifndef __SSSE3__
 
-void track_correlate(s8* samples, s8* code,
-                     double* init_code_phase, double code_step, double* init_carr_phase, double carr_step,
-                     double* I_E, double* Q_E, double* I_P, double* Q_P, double* I_L, double* Q_L, u32* num_samples)
+/** Perform correlation.
+ *
+ * \param correlator_type  Correlator type. L1 C/A or L2C CM are supported.
+ * \param samples          Samples array. One byte per sample.
+ * \param code             PRN code. One byte per chip.
+ * \param[in,out] init_code_phase  Initial code phase [chips].
+ *                         The function returns the last unprocessed code
+ *                         phase here.
+ * \param code_step        Code phase increment step [chips].
+ * \param[in,out] init_carr_phase  Initial carrier phase [radians].
+ *                         The function returns the the last unprocessed carrier
+ *                         phase here.
+ * \param carr_step        Carrier phase increment step [radians].
+ * \param[out] I_E         Early replica in-phase correlation component.
+ * \param[out] Q_E         Early replica quadrature correlation component.
+ * \param[out] I_P         Prompt replica in-phase correlation component.
+ * \param[out] Q_P         Prompt replica quadrature correlation component.
+ * \param[out] I_L         Late replica in-phase correlation component.
+ * \param[out] Q_L         Late replica quadrature correlation component.
+ * \param num_samples      The number of samples to correlate from \e samples
+ *                         array.
+ */
+
+static void track_correlate(enum correlator_type correlator_type,
+                            const s8* restrict samples,
+                            const s8* restrict code,
+                            double* restrict init_code_phase, double code_step,
+                            double* restrict init_carr_phase, double carr_step,
+                            double* restrict I_E, double* restrict Q_E,
+                            double* restrict I_P, double* restrict Q_P,
+                            double* restrict I_L, double* restrict Q_L,
+                            u32 num_samples)
 {
   double code_phase = *init_code_phase;
   double carr_phase = *init_carr_phase;
@@ -38,28 +346,46 @@ void track_correlate(s8* samples, s8* code,
 
   *I_E = *Q_E = *I_P = *Q_P = *I_L = *Q_L = 0;
 
-  double code_E, code_P, code_L;
+  s8 code_E, code_P, code_L;
   double baseband_Q, baseband_I;
 
-  *num_samples = (int)ceil((1023.0 - code_phase) / code_step);
+  for (u32 i=0; i<num_samples; i++) {
+    double code_phase_new;
 
-  for (u32 i=0; i<*num_samples; i++) {
-    /*code_E = get_chip(code, (int)ceil(code_phase-0.5));*/
-    /*code_P = get_chip(code, (int)ceil(code_phase));*/
-    /*code_L = get_chip(code, (int)ceil(code_phase+0.5));*/
-    /*code_E = code[(int)ceil(code_phase-0.5)];*/
-    /*code_P = code[(int)ceil(code_phase)];*/
-    /*code_L = code[(int)ceil(code_phase+0.5)];*/
-    code_E = code[(int)(code_phase+0.5)];
-    code_P = code[(int)(code_phase+1.0)];
-    code_L = code[(int)(code_phase+1.5)];
+    /* Note, l1ca_* and l2c_* functions are inline and should not
+       impose much execution overhead */
+    switch (correlator_type) {
+    case L1CA_CORRELATOR:
+      code_E         = l1_ca_get_chip(code, code_phase - 0.5);
+      code_P         = l1_ca_get_chip(code, code_phase);
+      code_L         = l1_ca_get_chip(code, code_phase + 0.5);
+      code_phase_new = l1_ca_get_code_phase(code_phase, code_step);
+      break;
+    case L2C_CORRELATOR:
+      code_E         = l2c_cm_get_chip(code, code_phase - 0.5);
+      code_P         = l2c_cm_get_chip(code, code_phase);
+      code_L         = l2c_cm_get_chip(code, code_phase + 0.5);
+      code_phase_new = l2c_cm_get_code_phase(code_phase, code_step);
+      break;
+    case GLOCA_CORRELATOR:
+      code_E         = glo_ca_get_chip(code, code_phase - 0.5);
+      code_P         = glo_ca_get_chip(code, code_phase);
+      code_L         = glo_ca_get_chip(code, code_phase + 0.5);
+      code_phase_new = glo_ca_get_code_phase(code_phase, code_step);
+      break;
+    default:
+      break;
+    }
 
-    baseband_Q = carr_cos * samples[i];
-    baseband_I = carr_sin * samples[i];
+    baseband_Q = -carr_sin * samples[i];
+    baseband_I = carr_cos * samples[i];
 
+    /* generate a new set of sine and cosine samples using
+       rotation matrix */
     double carr_sin_ = carr_sin*cos_delta + carr_cos*sin_delta;
     double carr_cos_ = carr_cos*cos_delta - carr_sin*sin_delta;
-    double i_mag = (3.0 - carr_sin_*carr_sin_ - carr_cos_*carr_cos_) / 2.0;
+    double i_mag =
+      (3.0 - carr_sin_*carr_sin_ - carr_cos_*carr_cos_) / 2.0;
     carr_sin = carr_sin_ * i_mag;
     carr_cos = carr_cos_ * i_mag;
 
@@ -70,18 +396,24 @@ void track_correlate(s8* samples, s8* code,
     *I_L += code_L * baseband_I;
     *Q_L += code_L * baseband_Q;
 
-    code_phase += code_step;
-    carr_phase += carr_step;
+    code_phase = code_phase_new;
   }
-  *init_code_phase = code_phase - 1023;
-  *init_carr_phase = fmod(carr_phase, 2*M_PI);
+  *init_code_phase = code_phase;
+  *init_carr_phase =
+    fmod(*init_carr_phase + num_samples * carr_step, 2*M_PI);
 }
 
 #else
 
-void track_correlate(s8* samples, s8* code,
-                     double* init_code_phase, double code_step, double* init_carr_phase, double carr_step,
-                     double* I_E, double* Q_E, double* I_P, double* Q_P, double* I_L, double* Q_L, u32* num_samples)
+static void track_correlate(enum correlator_type correlator_type,
+                            const s8* restrict samples,
+                            const s8* restrict code,
+                            double* restrict init_code_phase, double code_step,
+                            double* restrict init_carr_phase, double carr_step,
+                            double* restrict I_E, double* restrict Q_E,
+                            double* restrict I_P, double* restrict Q_P,
+                            double* restrict I_L, double* restrict Q_L,
+                            u32 num_samples)
 {
   double code_phase = *init_code_phase;
 
@@ -90,34 +422,60 @@ void track_correlate(s8* samples, s8* code,
   float sin_delta = sin(carr_step);
   float cos_delta = cos(carr_step);
 
-  *num_samples = (int)ceil((1023.0 - code_phase) / code_step);
-
   __m128 IE_QE_IP_QP;
   __m128 CE_CE_CP_CP;
   __m128 IL_QL_X_X;
   __m128 CL_CL_X_X;
   __m128 S_C_S_C;
+  __m128 C_S_C_S;
   __m128 BI_BQ_BI_BQ;
   __m128 dC_dS_dS_dC;
   __m128 a1, a2, a3;
+  __m128 one_minus_one;
+
+  s8 code_E, code_P, code_L;
 
   IE_QE_IP_QP = _mm_set_ps(0, 0, 0, 0);
   IL_QL_X_X = _mm_set_ps(0, 0, 0, 0);
+  one_minus_one = _mm_set_ps(1, -1, 1, -1);
   S_C_S_C = _mm_set_ps(carr_sin, carr_cos, carr_sin, carr_cos);
+  C_S_C_S = _mm_set_ps(carr_cos, -carr_sin, carr_cos, -carr_sin);
   dC_dS_dS_dC = _mm_set_ps(cos_delta, sin_delta, sin_delta, cos_delta);
 
-  for (u32 i=0; i<*num_samples; i++) {
-    CE_CE_CP_CP = _mm_set_ps(code[(int)(code_phase+0.5)],
-                             code[(int)(code_phase+0.5)],
-                             code[(int)(code_phase+1.0)],
-                             code[(int)(code_phase+1.0)]);
-    CL_CL_X_X = _mm_set_ps(code[(int)(code_phase+1.5)],
-                           code[(int)(code_phase+1.5)],
-                           0, 0);
+  for (u32 i=0; i<num_samples; i++) {
+    double code_phase_new;
+
+    /* Note, l1ca_ and l2c_ functions are inline and should not
+       impose much execution overhead */
+    switch (correlator_type) {
+    case L1CA_CORRELATOR:
+      code_E         = l1_ca_get_chip(code, code_phase - 0.5);
+      code_P         = l1_ca_get_chip(code, code_phase);
+      code_L         = l1_ca_get_chip(code, code_phase + 0.5);
+      code_phase_new = l1_ca_get_code_phase(code_phase, code_step);
+      break;
+    case L2C_CORRELATOR:
+      code_E         = l2c_cm_get_chip(code, code_phase - 0.5);
+      code_P         = l2c_cm_get_chip(code, code_phase);
+      code_L         = l2c_cm_get_chip(code, code_phase + 0.5);
+      code_phase_new = l2c_cm_get_code_phase(code_phase, code_step);
+      break;
+    case GLOCA_CORRELATOR:
+      code_E         = glo_ca_get_chip(code, code_phase - 0.5);
+      code_P         = glo_ca_get_chip(code, code_phase);
+      code_L         = glo_ca_get_chip(code, code_phase + 0.5);
+      code_phase_new = glo_ca_get_code_phase(code_phase, code_step);
+      break;
+    default:
+      break;
+    }
+
+    CE_CE_CP_CP = _mm_set_ps(code_E, code_E, code_P, code_P);
+    CL_CL_X_X = _mm_set_ps(code_L, code_L, 0, 0);
 
     /* Load sample and multiply by sin/cos carrier to mix down to baseband. */
     a1 = _mm_set1_ps((float)samples[i]); // S, S, S, S
-    BI_BQ_BI_BQ = _mm_mul_ps(a1, S_C_S_C);
+    BI_BQ_BI_BQ = _mm_mul_ps(a1, C_S_C_S);
 
     /* Update carrier sin/cos values by multiplying by the constant rotation
      * matrix corresponding to carr_step. */
@@ -126,6 +484,8 @@ void track_correlate(s8* samples, s8* code,
     a3 = _mm_shuffle_ps(a1, a1, _MM_SHUFFLE(2, 1, 2, 1)); // CdS_SdS_CdS_SdS
     // S = SdC + CdS, C = CdC - SdS
     S_C_S_C = _mm_addsub_ps(a2, a3); // C_S_C_S
+    a1 = _mm_shuffle_ps(S_C_S_C, S_C_S_C, _MM_SHUFFLE(2, 3, 0, 1)); // C_S_C_S
+    C_S_C_S = _mm_mul_ps(a1, one_minus_one); // C_S_C_S
 
     /* Multiply code and baseband signal. */
     a1 = _mm_mul_ps(CE_CE_CP_CP, BI_BQ_BI_BQ);
@@ -135,10 +495,10 @@ void track_correlate(s8* samples, s8* code,
     IE_QE_IP_QP = _mm_add_ps(IE_QE_IP_QP, a1);
     IL_QL_X_X   = _mm_add_ps(IL_QL_X_X, a2);
 
-    code_phase += code_step;
+    code_phase = code_phase_new;
   }
-  *init_code_phase = code_phase - 1023;
-  *init_carr_phase = fmod(*init_carr_phase + *num_samples*carr_step, 2*M_PI);
+  *init_code_phase = code_phase;
+  *init_carr_phase = fmod(*init_carr_phase + num_samples*carr_step, 2*M_PI);
 
   float res[8];
   _mm_storeu_ps(res, IE_QE_IP_QP);
diff --git a/src/counter_checker/Makefile b/src/counter_checker/Makefile
new file mode 100644
index 00000000..91e1e931
--- /dev/null
+++ b/src/counter_checker/Makefile
@@ -0,0 +1,12 @@
+.PHONY: all clean
+
+all: counter_checker_rf41 counter_checker_rf32
+
+counter_checker_rf41: counter_checker.c
+	gcc -o counter_checker_rf41 -O3 -Wall -I ../../include -D RF41 -D STANDALONE_SETUP counter_checker.c
+
+counter_checker_rf32: counter_checker.c
+	gcc -o counter_checker_rf32 -O3 -Wall -I ../../include -D RF32 -D STANDALONE_SETUP counter_checker.c
+
+clean:
+	rm counter_checker_rf32 counter_checker_rf41
diff --git a/src/counter_checker/counter_checker.c b/src/counter_checker/counter_checker.c
new file mode 100644
index 00000000..458006ff
--- /dev/null
+++ b/src/counter_checker/counter_checker.c
@@ -0,0 +1,302 @@
+/*
+ * Copyright (C) 2016 Swift Navigation Inc.
+ * Contact: Adel Mamin <adelm@exafore.com>
+ *
+ * This source is subject to the license found in the file 'LICENSE' which must
+ * be be distributed together with this source. All other rights reserved.
+ *
+ * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
+ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
+ */
+
+/**
+  @file
+
+  This utility helps to verify the integrity of samples data from Piksi v3 HW.
+  The regular Piksi v3 samples data format is one byte, which is encoded
+  lile this:
+
+  RF4 RF3 RF2 RF1
+   00  00  00  00
+
+  , where
+
+  RF1 - GPS L1
+  RF2 - GLONASS L1
+  RF3 - GLONASS L2
+  RF4 - GPS L2
+
+  This utility expects RF3 and RF2 to have 4 bits counter,
+  which is continuosly incremented with modulo SAMPLE_COUNTER_MODULO.
+  RF2 is expected to have two least significant bits of the counter
+  and RF3 - two most significant bits.
+  Therefore, the bit indexes are:
+  RF3 RF2
+  5&4 3&2
+*/
+
+#include <stdio.h>
+#include <stdint.h>
+#include <libswiftnav/counter_checker.h>
+
+/** The data is read in chunks, which are stored here. */
+static uint32_t chunk[SAMPLE_CHUNK_SIZE / sizeof(uint32_t)];
+
+/** Get array size */
+#define ARR_SIZE(arr) (sizeof(arr) / sizeof((arr)[0]))
+
+/** Mismatch data */
+static struct mismatch_data mismatch;
+
+/** Checks counter mismatch.
+ *
+ * \param data_offset File offset in bytes.
+ * \param bytes The data bytes to check.
+ * \param byte_index The byte index within \e bytes parameter [0,1,2,3]
+ * \param expected_counter Next expected counter value.
+ *
+ * \return The next expected counter value
+ */
+static inline uint8_t check_counter(size_t data_offset,
+                                    uint8_t (*get_counter)(uint8_t data),
+                                    uint32_t bytes, uint8_t byte_index,
+                                    uint32_t expected_counter)
+{
+  uint8_t actual_counter;
+  uint8_t byte_offset = 8 * byte_index;
+
+  actual_counter = get_counter(bytes >> byte_offset);
+  if (actual_counter != expected_counter) {
+    if (mismatch.counter <= ARR_SIZE(mismatch.data)) {
+      mismatch.data[mismatch.counter].offset = data_offset;
+      mismatch.data[mismatch.counter].data = (bytes >> 8 * byte_index) & 0xFF;
+      mismatch.data[mismatch.counter].expected_counter = expected_counter;
+      mismatch.data[mismatch.counter].actual_counter = actual_counter;
+      mismatch.counter++;
+    }
+  }
+  expected_counter = (actual_counter + 1) % SAMPLE_COUNTER_MODULO;
+
+  return expected_counter;
+}
+
+/** Report counter mismatch issues. */
+void report_mismatch(const struct mismatch_data *mismatch)
+{
+  size_t i;
+  size_t offset_prev = mismatch->data[0].offset;
+
+  if (0 != mismatch->counter) {
+    printf("data_offset,data,expected_counter,actual_counter,offset_diff\n");
+  }
+  for (i = 0; i < mismatch->counter; i++) {
+    printf("%zu,", mismatch->data[i].offset);
+    printf("0x%02X,", mismatch->data[i].data);
+    printf("%d,", mismatch->data[i].expected_counter);
+    printf("%d,", mismatch->data[i].actual_counter);
+    printf("%zu\n", mismatch->data[i].offset - offset_prev);
+    offset_prev = mismatch->data[i].offset;
+  }
+}
+
+/** Get RF3 RF2 counter
+ * \param data Data byte
+ * \return The counter value
+ */
+uint8_t get_rf32_counter(uint8_t data)
+{
+  uint8_t counter = GET_BITS(data, 2, 4);
+  return counter;
+}
+
+/** Set RF3 RF2 counter
+ * \param data Data byte
+ * \param counter The counter value
+ * \return The data byte with embedded counter
+ */
+uint8_t set_rf32_counter(uint8_t data, uint8_t counter)
+{
+  data = SET_BITS(data, 2, 4, counter);
+  return data;
+}
+
+/** Get RF4 RF1 counter
+ * \param data Data byte
+ * \return The counter value
+ */
+uint8_t get_rf41_counter(uint8_t data)
+{
+  uint8_t counter;
+
+  // take RF4 part first
+  counter = GET_BITS(data, 6, 2) << 2;
+  // combine with RF1 part to get full counter
+  counter |= GET_BITS(data, 0, 2);
+
+  return counter;
+}
+
+/** Set RF4 RF1 counter
+ * \param data Data byte
+ * \param counter The counter value
+ * \return The data byte with embedded counter
+ */
+uint8_t set_rf41_counter(uint8_t data, uint8_t counter)
+{
+  data = SET_BITS(data, 0, 2, counter);
+  counter >>= 2;
+  data = SET_BITS(data, 6, 2, counter);
+
+  return data;
+}
+
+/** Initialize sample checker */
+void counter_checker_init(void)
+{
+  mismatch.counter = 0;
+}
+
+/** Runs sample checker
+ *
+ * \param cbs Callbacks for data reading and counter extraction
+ * \param stream Stream itself.
+ * \param data_size Total data size [bytes]
+ * \return Mismatch data pointer. Non NULL even if no mismatch found.
+           The memory pointer is owned by the callee. Do not free it!
+ */
+const struct mismatch_data *counter_checker_run(struct callbacks *cbs,
+                                               void *stream,
+                                               size_t data_size)
+{
+  size_t i;
+  size_t j;
+  size_t chunk_size;
+  size_t chunk_num;
+  uint8_t tmp;
+  uint8_t counter;
+  size_t data_offset;
+
+  /* initialize the counter from the first byte of the samples data */
+  chunk_size = cbs->read(&tmp, 1, stream);
+  counter = cbs->get_counter(tmp);
+  cbs->rewind(stream);
+
+  data_offset = 0;
+
+  /* now let's start the integrity check of the samples data
+     in chunks of size SAMPLE_CHUNK_SIZE */
+  chunk_num = data_size / sizeof(chunk);
+
+  for (i = 0; i < chunk_num; i++) {
+    chunk_size = cbs->read(chunk, sizeof(chunk), stream);
+
+    /* process the chunk in 4 bytes slices */
+    for (j = 0; j < chunk_size / sizeof(chunk[0]); j++) {
+      uint8_t k;
+
+      for (k = 0; k < sizeof(uint32_t); k++) {
+        counter = check_counter(data_offset, cbs->get_counter,
+                                chunk[j], k, counter);
+        data_offset++;
+      }
+    }
+  }
+
+  chunk_size = data_size % sizeof(chunk);
+  chunk_size = cbs->read(chunk, chunk_size, stream);
+
+  /* process the last chunk byte by byte */
+  for (j = 0; j < chunk_size; j++) {
+    counter = check_counter(data_offset, cbs->get_counter,
+                            ((uint8_t*)chunk)[j], 0, counter);
+    data_offset++;
+  }
+
+  return &mismatch;
+}
+
+#ifdef STANDALONE_SETUP
+
+/** Standard C library 'fread' function wrapper  */
+static size_t file_read(void *ptr, size_t size, void *f)
+{
+  size_t ret;
+  ret = fread(ptr, size, 1, (FILE*)f);
+  return ret * size;
+}
+
+/** Standard C library 'rewind' function wrapper */
+static void file_rewind(void *f)
+{
+  rewind((FILE*)f);
+}
+
+static struct callbacks cbs = {
+
+  file_read,
+  file_rewind,
+
+#if defined RF41
+  get_rf41_counter,
+  set_rf41_counter,
+#elif defined RF32
+  get_rf32_counter,
+  set_rf32_counter
+#else
+# error "Please, specify, where counter bits are located."
+#endif
+
+};
+
+int main(int argc, char* argv[])
+{
+  FILE *f;
+  const char* file_name;
+  long file_size;
+  int ret = 0;
+  const struct mismatch_data *mismatch;
+
+
+  if (argc < 2) {
+    printf("Usage:");
+    printf("%s <samples data file name>\n", argv[0]);
+    return -1;
+  }
+
+  file_name = argv[1];
+
+  f = fopen(file_name, "rb");
+  if (NULL == f) {
+    printf("Failed to open file %s\n", file_name);
+    return -1;
+  }
+
+  fseek(f, 0, SEEK_END);
+  file_size = ftell(f);
+  rewind(f);
+
+  if (0 == file_size) {
+    printf("File %s is empty\n", file_name);
+    ret = -1;
+    goto end;
+  }
+
+  counter_checker_init();
+  mismatch = counter_checker_run(&cbs, f, file_size);
+
+  if (0 == mismatch->counter) {
+    printf("The file is OK!\n");
+  } else {
+    report_mismatch(mismatch);
+  }
+
+
+end:
+
+  fclose(f);
+
+  return ret;
+}
+
+#endif  /* #ifdef STANDALONE_SETUP */
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 69183c2d..f1d8718a 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -32,6 +32,7 @@ else (CMAKE_CROSSCOMPILING)
       check_pvt.c
       check_edc.c
       check_bits.c
+      check_correlator.c
       check_memory_pool.c
       check_rtcm3.c
       check_coord_system.c
@@ -47,6 +48,7 @@ else (CMAKE_CROSSCOMPILING)
       check_signal.c
       check_track.c
       check_cnav.c
+      check_counter_checker.c
     )
 
     target_link_libraries(test_libswiftnav ${TEST_LIBS})
diff --git a/tests/check_correlator.c b/tests/check_correlator.c
new file mode 100644
index 00000000..4f588196
--- /dev/null
+++ b/tests/check_correlator.c
@@ -0,0 +1,552 @@
+#include <check.h>
+#include <libswiftnav/correlate.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#define L1CA_CHIPS_PER_PRN_CODE     1023
+#define L2C_CM_CHIPS_PER_PRN_CODE   10230
+#define GLOCA_CHIPS_PER_PRN_CODE    511
+
+enum signal_type {
+  L1CA_SIGNAL,
+  L2C_SIGNAL,
+  GLOCA_SIGNAL  
+};
+
+//#define DUMP_RESULTS
+
+#ifdef DUMP_RESULTS
+#include <time.h>
+
+static clock_t start = 0;
+static clock_t stop = 0;
+
+static void debug_start_timing(void)
+{
+  start = clock();
+}
+
+static void debug_stop_timing(void)
+{
+  stop = clock();
+}
+
+static void debug_dump_results(u32 num_samples,
+                               double I_E, double Q_E,
+                               double I_P, double Q_P,
+                               double I_L, double Q_L)
+{
+  printf("==============================================\n");
+  printf("used %f cycles\n", (double)stop - start);
+
+  printf("num_samples = %d\n", num_samples);
+
+  printf("\n");
+
+  printf("I_E / I_P = %f\n", (0 == I_P) ? 0 : I_E / I_P);
+  printf("I_L / I_P = %f\n", (0 == I_P) ? 0 : I_L / I_P);
+
+  printf("\n");
+
+  printf("I_E = %f\n", I_E);
+  printf("Q_E = %f\n", Q_E);
+  printf("Q_E / I_E = %f\n", (0 == I_E) ? 0 : Q_E / I_E);
+
+  printf("\n");
+
+  printf("I_P = %f\n", I_P);
+  printf("Q_P = %f\n", Q_P);
+  printf("Q_P / I_P = %f\n", (0 == I_P) ? 0 : Q_P / I_P);
+
+  printf("\n");
+
+  printf("I_L = %f\n", I_L);
+  printf("Q_L = %f\n", Q_L);
+  printf("Q_L / I_L = %f\n", (0 == I_L) ? 0 : Q_L / I_L);
+
+  printf("\n");
+}
+#else
+#define debug_dump_results(num_samples, I_E, Q_E, I_P, Q_P, I_L, Q_L)
+#define debug_start_timing()
+#define debug_stop_timing()
+#endif
+
+/* PRN 1. One bit per sample. */
+static const u8 gps_l1ca_code[] = {
+  0xC8, 0x39, 0x49, 0xE5, 0x13, 0xEA, 0xD1, 0x15, 0x59, 0x1E, 0x9F,
+  0xB7, 0x37, 0xCA, 0xA1, 0x00, 0xEA, 0x44, 0xDE, 0x0F, 0x5C, 0xCF,
+  0x60, 0x2F, 0x3E, 0xA6, 0x2D, 0xC6, 0xF5, 0x15, 0x82, 0x01, 0x03,
+  0x1D, 0x81, 0xC6, 0xFF, 0xA7, 0x4B, 0x61, 0x56, 0x27, 0x2D, 0xD8,
+  0xEE, 0xF0, 0xD8, 0x64, 0x90, 0x6D, 0x2D, 0xE2, 0xE0, 0x52, 0x7E,
+  0x0A, 0xB9, 0xF5, 0xF3, 0x31, 0xC6, 0xD5, 0x6C, 0x6E, 0xE0, 0x02,
+  0xCD, 0x9D, 0xA0, 0xAB, 0xAE, 0x94, 0x73, 0x89, 0x45, 0x2D, 0x0A,
+  0xDA, 0xD8, 0xE7, 0xB2, 0x1F, 0x96, 0x88, 0x7D, 0x5C, 0xC9, 0x25,
+  0xFF, 0x87, 0xDE, 0x37, 0x2C, 0x39, 0x50, 0xA5, 0x7E, 0x3D, 0xA7,
+  0x67, 0xEF, 0xA3, 0x1F, 0x01, 0x28, 0xB4, 0x44, 0xD8, 0x1D, 0xA3,
+  0x44, 0x8E, 0x2C, 0xC9, 0xE6, 0xFC, 0xCA, 0x69, 0xAF, 0x36, 0xA7,
+  0x78, 0xD4, 0x42, 0x24, 0xE1, 0xCA, 0x20
+};
+
+static const u8 gps_l2cm_code[] = {
+  0x2B, 0xDE, 0x1E, 0xBA, 0x29, 0x87, 0xCB, 0x12, 0xA8, 0xFA, 0xF0,
+  0xFA, 0xBA, 0xD8, 0x1E, 0xCC, 0x07, 0xB3, 0xF1, 0x84, 0xCB, 0xDA,
+  0x5D, 0xE0, 0x48, 0x62, 0x87, 0xCF, 0xC9, 0x69, 0xE0, 0x39, 0x5C,
+  0x77, 0x26, 0xB1, 0x4B, 0x36, 0xF6, 0xA9, 0xBC, 0x16, 0x85, 0x16,
+  0x39, 0x3A, 0x5D, 0x7B, 0xD6, 0x28, 0xBD, 0x8F, 0xDF, 0x9A, 0xE9,
+  0x10, 0x2E, 0xAD, 0xD0, 0xCA, 0x1C, 0xE4, 0xBE, 0x05, 0x97, 0x38,
+  0xB4, 0x71, 0xC0, 0xB2, 0xEC, 0x7F, 0x4B, 0x34, 0xA4, 0xA3, 0xF6,
+  0x7A, 0x3C, 0x5B, 0x3A, 0xAA, 0x87, 0x98, 0x53, 0xF0, 0xB3, 0x9A,
+  0xC5, 0xE6, 0xD9, 0x7F, 0x26, 0x1D, 0x71, 0xB7, 0x75, 0xA8, 0x0C,
+  0x77, 0x78, 0xCF, 0x3C, 0xC6, 0xF4, 0x4E, 0x20, 0x62, 0x8B, 0x2B,
+  0x2B, 0x4E, 0xBE, 0xF3, 0x67, 0xD1, 0x65, 0x49, 0x11, 0x83, 0xB9,
+  0x21, 0xBA, 0x38, 0xB5, 0x21, 0x3D, 0x5E, 0xC1, 0x7D, 0xF8, 0x43,
+  0x58, 0x37, 0x45, 0xE8, 0x27, 0xAD, 0x99, 0x46, 0xC6, 0x96, 0xB8,
+  0xE6, 0x3B, 0x51, 0xC2, 0x36, 0xD8, 0xA3, 0x9D, 0x39, 0x0A, 0x65,
+  0x1E, 0xEA, 0xEC, 0x8E, 0x0A, 0xF3, 0xC1, 0x0A, 0xA7, 0xEB, 0x80,
+  0x32, 0x82, 0x3D, 0x5B, 0x35, 0xDA, 0xCA, 0xFA, 0x94, 0x83, 0x9A,
+  0x96, 0x6A, 0xB0, 0xD8, 0xD7, 0x69, 0x7C, 0x73, 0xDF, 0xC2, 0x54,
+  0x25, 0x42, 0x4B, 0xA2, 0xED, 0x1B, 0x5E, 0xC7, 0xB4, 0x76, 0xA7,
+  0x1D, 0x9B, 0x4D, 0x03, 0xCB, 0xDA, 0x81, 0xAD, 0xF3, 0x60, 0x76,
+  0x23, 0xC6, 0x24, 0xC1, 0x90, 0xB3, 0x3C, 0xAA, 0x56, 0xED, 0xD3,
+  0x25, 0x17, 0xD6, 0x3E, 0x6F, 0xA0, 0x7B, 0x52, 0x9E, 0xAE, 0xE9,
+  0xD3, 0x26, 0xE3, 0x64, 0xE8, 0x9E, 0xFE, 0xD1, 0xF6, 0x52, 0xBE,
+  0x5A, 0xB2, 0x60, 0xDC, 0xEF, 0x30, 0x16, 0x1E, 0x89, 0xD6, 0xBD,
+  0xB0, 0xE6, 0x68, 0xC0, 0x5F, 0x94, 0x71, 0xBC, 0x0F, 0xD8, 0xB6,
+  0x8C, 0x7F, 0x93, 0x90, 0x96, 0x97, 0xA2, 0x16, 0xA6, 0x2E, 0x86,
+  0xD5, 0x4D, 0xF3, 0x65, 0xDC, 0x7E, 0x3E, 0x7E, 0xC3, 0xA8, 0x9C,
+  0x11, 0x9C, 0x51, 0x0D, 0x8A, 0xE8, 0x6C, 0x35, 0xF3, 0x53, 0x91,
+  0x30, 0x35, 0x65, 0x51, 0x99, 0x7D, 0xFF, 0x95, 0x31, 0x34, 0xCC,
+  0x4B, 0x96, 0x54, 0x1A, 0xA7, 0x4F, 0x81, 0x1D, 0xF7, 0x1B, 0x2C,
+  0xAB, 0xFD, 0xBB, 0x9C, 0xC3, 0x6D, 0x3B, 0xE1, 0x18, 0x55, 0x4E,
+  0x62, 0x6D, 0xE9, 0xF2, 0xB1, 0x34, 0x3E, 0x65, 0x44, 0x37, 0x20,
+  0x35, 0x51, 0xB9, 0x86, 0x40, 0x28, 0x77, 0x21, 0x6F, 0x4E, 0xC1,
+  0x65, 0x12, 0xF5, 0xFA, 0xA0, 0x13, 0xFF, 0xFA, 0x2D, 0x59, 0xDE,
+  0x4C, 0xC5, 0xD0, 0x02, 0xF5, 0x5A, 0x45, 0x98, 0x51, 0x15, 0x80,
+  0x3D, 0xCF, 0xFB, 0x3E, 0x04, 0x48, 0x02, 0x1C, 0x0F, 0x74, 0xB4,
+  0xAE, 0x8D, 0x79, 0xE1, 0x52, 0xE3, 0xC0, 0xC7, 0xC1, 0x70, 0x05,
+  0x25, 0x95, 0xE3, 0xB6, 0xB5, 0x33, 0x98, 0x50, 0xD0, 0x49, 0x6C,
+  0x47, 0x33, 0xB4, 0xB9, 0x73, 0x12, 0x79, 0xDA, 0x8A, 0x4F, 0x9A,
+  0x8A, 0x44, 0xB4, 0xCC, 0x4C, 0x0D, 0x3C, 0x2F, 0xC9, 0x67, 0x04,
+  0x84, 0x8E, 0x41, 0x94, 0x27, 0x26, 0xF7, 0x3B, 0xEC, 0x95, 0x25,
+  0x26, 0x3C, 0x8D, 0x5A, 0x33, 0x0C, 0x3F, 0x65, 0x51, 0x1C, 0xB7,
+  0x8F, 0x22, 0x14, 0xB3, 0xFA, 0x10, 0x6C, 0x29, 0x1E, 0xF5, 0x06,
+  0xF7, 0x4D, 0x68, 0x22, 0xBD, 0xC3, 0x2C, 0x05, 0x05, 0x62, 0xD4,
+  0x5B, 0x32, 0xF7, 0x19, 0xBA, 0x17, 0x5A, 0xB6, 0x41, 0xF7, 0xF0,
+  0xAE, 0x5A, 0xEB, 0x9F, 0x98, 0xF0, 0x84, 0x6D, 0xD9, 0x78, 0x17,
+  0x4D, 0x57, 0x17, 0x89, 0xF0, 0xEF, 0xD2, 0x66, 0x4E, 0x22, 0xD1,
+  0xDB, 0x49, 0x28, 0x9E, 0xC5, 0x4A, 0xC8, 0x53, 0xD3, 0x03, 0x5E,
+  0x23, 0xB6, 0x57, 0x1F, 0xB5, 0x98, 0xA9, 0x1E, 0x1C, 0x23, 0xB9,
+  0xAB, 0x76, 0xCB, 0x79, 0xB2, 0x4B, 0x05, 0x6B, 0x2E, 0xD8, 0x31,
+  0x83, 0xDF, 0x15, 0x8A, 0xFF, 0x03, 0x12, 0xE6, 0x3E, 0x82, 0x67,
+  0x07, 0x99, 0xFF, 0xB0, 0xE0, 0x13, 0xAA, 0xDB, 0x5A, 0x42, 0xB2,
+  0xD8, 0x71, 0x73, 0x20, 0x4A, 0x32, 0x37, 0x52, 0xC4, 0x29, 0x52,
+  0x69, 0x2E, 0xCC, 0xA6, 0xD0, 0x06, 0x05, 0xAA, 0xBD, 0xD5, 0x11,
+  0x16, 0x1F, 0xF2, 0x86, 0x27, 0x82, 0x7B, 0x2E, 0x89, 0x85, 0x16,
+  0xFC, 0x67, 0x47, 0xFA, 0x85, 0x81, 0x6D, 0xE2, 0xBB, 0x97, 0xAE,
+  0xB9, 0x68, 0x78, 0x34, 0x1E, 0x00, 0x3A, 0x66, 0x54, 0x21, 0x0E,
+  0x50, 0xC3, 0x77, 0xEA, 0x7C, 0xD1, 0x81, 0xCE, 0xDD, 0x6E, 0x80,
+  0xD9, 0x97, 0xC4, 0xE8, 0x94, 0x4C, 0xB0, 0x0B, 0xBD, 0x30, 0x0B,
+  0x8A, 0x64, 0xE6, 0x4E, 0xD3, 0x93, 0x57, 0x57, 0xDA, 0xD9, 0xCF,
+  0x35, 0x37, 0x39, 0xAD, 0x7E, 0xC8, 0x94, 0x18, 0xCC, 0x17, 0x16,
+  0xA0, 0x60, 0x97, 0x36, 0xF4, 0x40, 0x49, 0xAB, 0x03, 0x66, 0x61,
+  0x01, 0x00, 0x2A, 0x00, 0x1C, 0xF7, 0xB3, 0x07, 0x52, 0x70, 0x4E,
+  0xC0, 0x31, 0x45, 0xF7, 0x41, 0xD7, 0xE7, 0xDD, 0x93, 0x13, 0x1E,
+  0x17, 0xC9, 0x20, 0x21, 0xC5, 0x3D, 0xB0, 0x90, 0x67, 0x95, 0x6B,
+  0xBC, 0xB4, 0xDA, 0x04, 0xBC, 0x61, 0xB3, 0xD1, 0x8A, 0x5D, 0x49,
+  0xD6, 0xD8, 0x6A, 0xA3, 0x37, 0xC4, 0xFF, 0x00, 0xF6, 0xC3, 0x4A,
+  0x0A, 0xB4, 0xC0, 0xB9, 0xDD, 0x04, 0xA5, 0x05, 0x7C, 0xC7, 0xE2,
+  0xBB, 0x99, 0x86, 0x65, 0x12, 0x88, 0x5D, 0x7C, 0x6E, 0xCF, 0x40,
+  0xA9, 0xCC, 0x8F, 0xF4, 0x4A, 0xEA, 0x0D, 0x1A, 0xAD, 0xF2, 0x98,
+  0x7A, 0x3F, 0xF5, 0xB1, 0x37, 0x68, 0x1F, 0xE1, 0x89, 0xCB, 0x81,
+  0xC5, 0x49, 0xEE, 0x56, 0xBC, 0x5C, 0xC4, 0xAA, 0x8F, 0x5D, 0x6F,
+  0x33, 0x83, 0x07, 0xE3, 0x71, 0xE0, 0xC8, 0xC6, 0xA7, 0xDC, 0x6A,
+  0x2F, 0xE2, 0x5B, 0x19, 0x33, 0xD2, 0x10, 0x3C, 0xB9, 0x56, 0xE5,
+  0xDE, 0x27, 0x69, 0x46, 0x16, 0x12, 0x46, 0xD7, 0xA6, 0xBC, 0x0F,
+  0x3B, 0xF7, 0x99, 0x16, 0x7C, 0x98, 0x29, 0xC1, 0x43, 0x50, 0x9C,
+  0x4C, 0x64, 0x05, 0x27, 0x6E, 0xBD, 0xB1, 0x6A, 0x96, 0xDD, 0x3D,
+  0xDB, 0xB3, 0x6B, 0x1D, 0x01, 0xC5, 0x29, 0x15, 0xAC, 0xB7, 0xCF,
+  0xB3, 0xE8, 0x73, 0x3A, 0x13, 0xBD, 0x98, 0xAD, 0x20, 0x96, 0x4E,
+  0x34, 0xB6, 0xBF, 0x52, 0xEB, 0xA9, 0x3C, 0x88, 0x53, 0x03, 0x3A,
+  0x87, 0x59, 0x3A, 0xE2, 0x6D, 0xA8, 0xF4, 0xD1, 0x1A, 0x72, 0x6F,
+  0xFF, 0xF3, 0xBA, 0x1F, 0x58, 0xCC, 0xA5, 0x40, 0xB8, 0x1F, 0x18,
+  0xD6, 0xF5, 0x66, 0xF8, 0x75, 0xBA, 0x9C, 0xD0, 0xD0, 0x5A, 0xED,
+  0x46, 0xCF, 0xFC, 0xF5, 0xF0, 0x21, 0xB8, 0xB6, 0xF2, 0xD0, 0x53,
+  0x16, 0x60, 0x71, 0x25, 0xB8, 0x12, 0x5D, 0xD6, 0x6A, 0xD5, 0x11,
+  0x00, 0xA5, 0x17, 0xF4, 0x20, 0xAD, 0xF0, 0x7A, 0xE6, 0x47, 0x7F,
+  0x73, 0x91, 0x2A, 0xD1, 0xEA, 0x90, 0x51, 0x1F, 0xB1, 0x4E, 0xFE,
+  0x58, 0x04, 0x78, 0x6E, 0xD3, 0x15, 0x69, 0x09, 0x2F, 0x96, 0x6F,
+  0x92, 0x97, 0x49, 0x18, 0x1A, 0x4B, 0xEF, 0xDB, 0xD1, 0x77, 0xA6,
+  0xFF, 0x31, 0xA7, 0x5D, 0x56, 0xA5, 0xCF, 0x1E, 0x8A, 0x0A, 0xCE,
+  0x4E, 0x75, 0x61, 0x4A, 0xD1, 0x39, 0x13, 0xEC, 0x6A, 0xEB, 0x41,
+  0xB2, 0x1D, 0x8E, 0xA1, 0xD2, 0x56, 0x2E, 0xF9, 0x0F, 0x9C, 0x5C,
+  0xB0, 0x35, 0x05, 0xC3, 0x0C, 0x1D, 0x0E, 0x31, 0x0B, 0xBA, 0x71,
+  0x6A, 0x2C, 0x68, 0xD7, 0xB6, 0x32, 0x6F, 0x3D, 0x2F, 0x93, 0x71,
+  0xBD, 0xF9, 0x2E, 0xA8, 0x13, 0xDF, 0xAA, 0xFA, 0xB1, 0x0B, 0x1D,
+  0x1E, 0x69, 0x76, 0xEB, 0x91, 0xD1, 0x2F, 0x61, 0xE5, 0x7B, 0x9A,
+  0x1E, 0xC4, 0xC8, 0x91, 0x94, 0xC3, 0xAA, 0xE9, 0xA2, 0x64, 0x78,
+  0x83, 0x2B, 0x65, 0x0C, 0x03, 0x2F, 0x7B, 0x1F, 0x05, 0x6F, 0x98,
+  0x67, 0x1F, 0x03, 0x78, 0x92, 0xDB, 0x88, 0x1D, 0x04, 0x83, 0xF6,
+  0xD3, 0x05, 0x9D, 0x46, 0x0F, 0x95, 0x54, 0x40, 0x3C, 0xCC, 0xEC,
+  0x8B, 0x5A, 0xD8, 0xA8, 0x9C, 0x36, 0xD0, 0x18, 0xB8, 0xA6, 0xEC,
+  0xC5, 0xF1, 0x1D, 0xE2, 0xB8, 0x35, 0xE3, 0x3E, 0x0B, 0x92, 0x39,
+  0x27, 0x4B, 0x6F, 0xDD, 0x77, 0x5D, 0x07, 0xB0, 0xB0, 0x5A, 0x87,
+  0x06, 0x9D, 0x43, 0x3E, 0x9A, 0x87, 0x88, 0xB8, 0x39, 0xC4, 0xAC,
+  0x71, 0x46, 0x2A, 0xF7, 0x32, 0xFB, 0x8A, 0x14, 0xB1, 0xFE, 0xAB,
+  0x3D, 0x9A, 0xAF, 0x82, 0x37, 0x43, 0xBE, 0xEA, 0x3D, 0x65, 0xBE,
+  0x31, 0xA7, 0x5C, 0x48, 0xA9, 0xDF, 0x32, 0xB2, 0x63, 0x92, 0x49,
+  0xE6, 0xED, 0xBE, 0xE6, 0x04, 0xD4, 0x2E, 0x7D, 0xB0, 0xB3, 0x88,
+  0xFB, 0x01, 0x52, 0x64, 0x93, 0xA1, 0x95, 0xA9, 0xFA, 0x4D, 0x07,
+  0xF8, 0x5A, 0x24, 0x6A, 0x6A, 0x88, 0x36, 0x3B, 0x20, 0xC1, 0x24,
+  0x59, 0x46, 0x3C, 0xDD, 0x57, 0xD6, 0xAE, 0xA8, 0x7C, 0x5C, 0xCA,
+  0xD6, 0x42, 0xB0, 0xFB, 0x73, 0x90, 0xD6, 0xCC, 0xA9, 0x88, 0x16,
+  0xDB, 0x94, 0xE0, 0xE7, 0x68, 0x9E, 0xE3, 0xC3, 0x9A, 0x69, 0xA1,
+  0x0A, 0xDB, 0xAA, 0x86, 0x42, 0xE9, 0x0D, 0xFE, 0xC6, 0xD5, 0x24,
+  0xDB, 0x8D, 0xBA, 0xA6, 0x48, 0x16, 0x4E, 0xF4, 0x4C, 0xD5, 0xC6,
+  0x8E, 0x58, 0xBB, 0xC4, 0x41, 0x73, 0xDC, 0x4A, 0xAA, 0xD8, 0x2E,
+  0x12, 0x62, 0x83, 0x2A, 0x1A, 0xD1, 0x52, 0xAD, 0x4D, 0x50, 0xCE,
+  0xF1, 0x06, 0x28
+};
+
+static const u8 glo_ca_code[] = {
+  0xFE, 0x0F, 0x7C, 0x5C, 0xC8, 0x25, 0x3B, 0x47, 0x9F, 0x36, 0x2A,
+  0x47, 0x1B, 0x57, 0x13, 0x11, 0x00, 0x84, 0x61, 0x39, 0x56, 0x1B,
+  0xD3, 0x72, 0x28, 0x56, 0x9F, 0xB2, 0x4B, 0x7E, 0x4D, 0x4C, 0xC0,
+  0x63, 0x28, 0xD2, 0xFE, 0x8B, 0x1D, 0x65, 0x9E, 0x3E, 0xE8, 0x35,
+  0xB7, 0x60, 0xB5, 0xF5, 0x50, 0x29, 0x5E, 0x5D, 0xC0, 0xE7, 0x49,
+  0xEB, 0xA8, 0x90, 0xCE, 0x17, 0xB6, 0x68, 0x77, 0x86
+};
+
+struct signal {
+  s8* samples;
+  size_t size;
+};
+
+struct signal generate_signal(enum signal_type signal_type,
+                              double if_freq, double code_freq,
+                              double carr_doppler_freq,
+                              double carr_to_code, double sampling_freq,
+                              s8* prn_code, u32 ms_to_generate)
+{
+  u32 i;
+  double delt;
+  double code_phase = 0;
+  double carr_phase = 0;
+  u32 samples_num;
+  double carr_freq;
+  double carr_phasestep;
+  double code_phasestep;
+  s8* samples;
+  struct signal signal = {NULL, 0};
+
+  samples_num = (u32)(sampling_freq * 1e-3 * ms_to_generate);
+  samples = malloc(samples_num);
+  if (NULL == samples) {
+    return signal;
+  }
+  delt = 1. / sampling_freq;
+  carr_freq = if_freq + carr_doppler_freq;
+  code_freq += carr_doppler_freq * carr_to_code;
+  carr_phasestep = carr_freq * delt;
+  code_phasestep = code_freq * delt;
+
+  for (i = 0; i < samples_num; i++) {
+    s8 code = 0;
+    double ip;
+    switch (signal_type) {
+    case L1CA_SIGNAL:
+    case GLOCA_SIGNAL:
+      code = prn_code[(int)code_phase];
+      break;
+    case L2C_SIGNAL:
+      if ((int)code_phase & 1) {
+        code = 0; // hit CL
+      } else {
+        code = prn_code[(int)code_phase / 2];
+      }
+      break;
+    }
+
+    static bool header = 1;
+    static FILE *f;
+    if (header) {
+      f = fopen("./gen.csv", "w");
+      fprintf(f, "code_phase,prn\n");
+      header = 0;
+    }
+    fprintf(f, "%f,%d\n", code_phase, code);
+
+    ip = code * cos(carr_phase * 2 * M_PI);
+    if (ip > 0) {
+      samples[i] = 1;
+    } else if (ip < 0) {
+      samples[i] = -1;
+    } else {
+      samples[i] = 0;
+    }
+
+    carr_phase += carr_phasestep;
+    code_phase += code_phasestep;
+
+    switch (signal_type) {
+    case L1CA_SIGNAL:
+      if (code_phase >= L1CA_CHIPS_PER_PRN_CODE) {
+        code_phase -= L1CA_CHIPS_PER_PRN_CODE;
+      }
+      break;
+    case L2C_SIGNAL:
+      if (code_phase >= 2 * L2C_CM_CHIPS_PER_PRN_CODE) {
+        code_phase -= 2 * L2C_CM_CHIPS_PER_PRN_CODE;
+      }
+      break;
+    case GLOCA_SIGNAL:
+      if (code_phase >= GLOCA_CHIPS_PER_PRN_CODE) {
+        code_phase -= GLOCA_CHIPS_PER_PRN_CODE;
+      }
+      break;
+    }
+  }
+  signal.samples = samples;
+  signal.size = samples_num;
+
+  return signal;
+}
+
+static s8* get_prn_code(const u8 *code, size_t code_size,
+                      size_t chips_per_prn)
+{
+  s8* prn_code;
+  u32 i = 0;
+  u32 j;
+  u32 k;
+  u8 packed;
+
+  prn_code = malloc(chips_per_prn);
+  if (NULL == prn_code) {
+    return NULL;
+  }
+  j = 0;
+  do {
+    packed = code[j++];
+    for (k = 0; k < 8; k++) {
+      prn_code[i++] = (packed & 0x80) ? 1 : -1;
+      packed <<= 1;
+      if (i >= chips_per_prn) {
+        break;
+      }
+    }
+  } while ((i < chips_per_prn) && (j < code_size));
+  return prn_code;
+}
+
+#define SAMPLING_FREQ_HZ          25e6
+#define L1CA_CHIPPING_RATE_HZ     1.023e6
+#define L2C_CM_CHIPPING_RATE_HZ   1.023e6
+#define GLOCA_CHIPPING_RATE_HZ    0.511e6
+#define IF_FREQUENCY_HZ           2e6
+#define CARRIER_DOPPLER_FREQ_HZ   100
+
+START_TEST(test_l1ca_correlator)
+{
+  struct signal signal;
+  s8* code;
+  double init_code_phase = 0;
+  double init_carr_phase = 0;
+  double I_E;
+  double Q_E;
+  double I_P;
+  double Q_P;
+  double I_L;
+  double Q_L;
+  u32 num_samples;
+
+  code = get_prn_code(gps_l1ca_code, sizeof(gps_l1ca_code),
+                      L1CA_CHIPS_PER_PRN_CODE);
+  fail_if(NULL == code, "Could not allocate PRN code data");
+
+  signal = generate_signal( L1CA_SIGNAL,            /* signal type */
+                            IF_FREQUENCY_HZ,        /* intermediate frequency */
+                            L1CA_CHIPPING_RATE_HZ, /* code frequency */
+                            CARRIER_DOPPLER_FREQ_HZ,/* carr_doppler frequency */
+                            1. / 1540, /* carrier to code scaling factor */
+                            SAMPLING_FREQ_HZ,       /* sampling frequency */
+                            code,                   /* PRN code data */
+                            1);                     /* milliseconds to generate */
+
+  fail_if(NULL == signal.samples, "Could not generate signal data");
+
+  debug_start_timing();
+
+  l1_ca_track_correlate(signal.samples, signal.size,
+    code,
+    &init_code_phase,
+    (L1CA_CHIPPING_RATE_HZ + CARRIER_DOPPLER_FREQ_HZ / 1540.0) / SAMPLING_FREQ_HZ,
+    &init_carr_phase,
+    (IF_FREQUENCY_HZ + CARRIER_DOPPLER_FREQ_HZ) * 2.0 * M_PI / SAMPLING_FREQ_HZ,
+    &I_E, &Q_E,
+    &I_P, &Q_P,
+    &I_L, &Q_L,
+    &num_samples);
+
+  debug_stop_timing();
+
+  debug_dump_results(num_samples, I_E, Q_E, I_P, Q_P, I_L, Q_L);
+
+  u32 expected_samples = SAMPLING_FREQ_HZ / 1000; /* samples per PRN code */
+  fail_if(num_samples != expected_samples);
+
+  fail_if((I_P != 0) && (fabs(I_E / I_P) > 0.5));
+  fail_if((I_P != 0) && (fabs(I_L / I_P) > 0.5));
+
+  fail_if((I_E != 0) && (fabs(Q_E / I_E) > 0.006));
+  fail_if((I_P != 0) && (fabs(Q_P / I_P) > 0.005));
+  fail_if((I_L != 0) && (fabs(Q_L / I_L) > 0.006));
+
+  free(code);
+  free(signal.samples);
+}
+END_TEST
+
+START_TEST(test_l2c_cm_correlator)
+{
+  struct signal signal;
+  s8* code;
+  double init_code_phase = 0;
+  double init_carr_phase = 0;
+  double I_E;
+  double Q_E;
+  double I_P;
+  double Q_P;
+  double I_L;
+  double Q_L;
+  u32 num_samples;
+
+  code = get_prn_code(gps_l2cm_code, sizeof(gps_l2cm_code),
+                      L2C_CM_CHIPS_PER_PRN_CODE);
+  fail_if(NULL == code, "Could not allocate L2C CM PRN code data");
+
+  signal = generate_signal( L2C_SIGNAL,             /* signal type */
+                            IF_FREQUENCY_HZ,        /* intermediate frequency */
+                            L2C_CM_CHIPPING_RATE_HZ,  /* code frequency */
+                            CARRIER_DOPPLER_FREQ_HZ,/* carr_doppler frequency */
+                            1. / 1200, /* carrier to code scaling factor */
+                            SAMPLING_FREQ_HZ,       /* sampling frequency */
+                            code,                   /* PRN code data */
+                            20);                    /* milliseconds to generate */
+
+  fail_if(NULL == signal.samples, "Could not generate signal data");
+
+  debug_start_timing();
+
+  l2c_cm_track_correlate(signal.samples, signal.size,
+    code,
+    &init_code_phase,
+    (L2C_CM_CHIPPING_RATE_HZ + CARRIER_DOPPLER_FREQ_HZ / 1200.0) /\
+      SAMPLING_FREQ_HZ,
+    &init_carr_phase,
+    (IF_FREQUENCY_HZ + CARRIER_DOPPLER_FREQ_HZ) * 2.0 * M_PI / SAMPLING_FREQ_HZ,
+    &I_E, &Q_E,
+    &I_P, &Q_P,
+    &I_L, &Q_L,
+    &num_samples);
+
+  debug_stop_timing();
+
+  debug_dump_results(num_samples, I_E, Q_E, I_P, Q_P, I_L, Q_L);
+
+  u32 expected_samples = 20 * SAMPLING_FREQ_HZ / 1000; /* samples per PRN code */
+  fail_if(num_samples != expected_samples);
+
+  fail_if((I_P != 0) && (fabs(I_E / I_P) > 0.6));
+  fail_if((I_P != 0) && (fabs(I_L / I_P) > 0.6));
+
+  fail_if((I_E != 0) && (fabs(Q_E / I_E) > 0.006));
+  fail_if((I_P != 0) && (fabs(Q_P / I_P) > 0.005));
+  fail_if((I_L != 0) && (fabs(Q_L / I_L) > 0.006));
+
+  free(code);
+  free(signal.samples);
+}
+END_TEST
+
+#define GLOL1_CH0_CARRIER_FREQ_MHZ  1602.0
+
+START_TEST(test_gloca_correlator)
+{
+  struct signal signal;
+  s8* code;
+  double init_code_phase = 0;
+  double init_carr_phase = 0;
+  double I_E;
+  double Q_E;
+  double I_P;
+  double Q_P;
+  double I_L;
+  double Q_L;
+  u32 num_samples;
+  double ca2co_scale = GLOCA_CHIPPING_RATE_HZ /\
+                         GLOL1_CH0_CARRIER_FREQ_MHZ;
+
+  code = get_prn_code(glo_ca_code, sizeof(glo_ca_code),
+                      GLOCA_CHIPS_PER_PRN_CODE);
+  fail_if(NULL == code, "Could not allocate PRN code data");
+
+  signal = generate_signal( GLOCA_SIGNAL,              /* signal type */
+                            IF_FREQUENCY_HZ,     /* intermediate freq */
+                            GLOCA_CHIPPING_RATE_HZ,      /* code freq */
+                            CARRIER_DOPPLER_FREQ_HZ,/* carr dopp freq */
+                            ca2co_scale, /* carr to code scale factor */
+                            SAMPLING_FREQ_HZ,   /* sampling frequency */
+                            code,                    /* PRN code data */
+                            1);           /* milliseconds to generate */
+
+  fail_if(NULL == signal.samples, "Could not generate signal data");
+
+  debug_start_timing();
+
+  glo_ca_track_correlate(signal.samples, signal.size,
+    code,
+    &init_code_phase,
+    (GLOCA_CHIPPING_RATE_HZ + CARRIER_DOPPLER_FREQ_HZ * ca2co_scale)\
+      / SAMPLING_FREQ_HZ,
+    &init_carr_phase,
+    (IF_FREQUENCY_HZ + CARRIER_DOPPLER_FREQ_HZ) * 2.0 * M_PI\
+      / SAMPLING_FREQ_HZ,
+    &I_E, &Q_E,
+    &I_P, &Q_P,
+    &I_L, &Q_L,
+    &num_samples);
+
+  debug_stop_timing();
+
+  debug_dump_results(num_samples, I_E, Q_E, I_P, Q_P, I_L, Q_L);
+
+  /* samples per PRN code */
+  u32 expected_samples = SAMPLING_FREQ_HZ / 1000;
+  fail_if(num_samples != expected_samples);
+
+  fail_if((I_P != 0) && (fabs(I_E / I_P) > 0.6));
+  fail_if((I_P != 0) && (fabs(I_L / I_P) > 0.6));
+
+  fail_if((I_E != 0) && (fabs(Q_E / I_E) > 0.006));
+  fail_if((I_P != 0) && (fabs(Q_P / I_P) > 0.005));
+  fail_if((I_L != 0) && (fabs(Q_L / I_L) > 0.006));
+
+  free(code);
+  free(signal.samples);
+}
+END_TEST
+
+Suite* correlator_suite(void)
+{
+  Suite *s = suite_create("Correlator");
+  TCase *tc_core = tcase_create("Core");
+
+  tcase_add_test(tc_core, test_l1ca_correlator);
+  tcase_add_test(tc_core, test_l2c_cm_correlator);
+  tcase_add_test(tc_core, test_gloca_correlator);
+  suite_add_tcase(s, tc_core);
+
+  return s;
+}
diff --git a/tests/check_counter_checker.c b/tests/check_counter_checker.c
new file mode 100644
index 00000000..3cc79c62
--- /dev/null
+++ b/tests/check_counter_checker.c
@@ -0,0 +1,213 @@
+#include <check.h>
+#include <libswiftnav/counter_checker.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+struct recording {
+  u8* samples;
+  size_t size;
+};
+
+static size_t arr_offset = 0;
+
+static size_t arr_read(void *ptr, size_t size, void *f)
+{
+  memcpy(ptr, (u8*) f + arr_offset, size);
+  arr_offset += size;
+  return size;
+}
+
+static void arr_rewind(void *f)
+{
+  (void)f;
+  arr_offset = 0;
+}
+
+static struct recording generate_recording(u8 counter_start,
+                         uint8_t set_counter(uint8_t data, uint8_t counter),
+                         u8 counter_modulo,
+                         size_t size)
+{
+  size_t i;
+  u8* samples;
+  u8 counter;
+  struct recording recording = {NULL, 0};
+
+  samples = malloc(size);
+  if (NULL == samples) {
+    return recording;
+  }
+
+  counter = counter_start;
+  for (i = 0; i < size; i++) {
+    samples[i] = set_counter(samples[i], counter);
+    counter++;
+    counter %= counter_modulo;
+  }
+  recording.samples = samples;
+  recording.size = size;
+
+  return recording;
+}
+
+START_TEST(test_counters_rf41)
+{
+  u8 data = 0xFF;
+  u8 i;
+  u8 counter;
+
+  for (i = 0; i < 16; i++) {
+    data = set_rf41_counter(data, i);
+    counter = get_rf41_counter(data);
+    fail_if(counter != i, "Unexpected counter value");
+  }
+  data = set_rf41_counter(data, 0);
+  fail_if(0x3C != data, "Unexpected counter value");
+}
+END_TEST
+
+START_TEST(test_counters_rf32)
+{
+  u8 data = 0xFF;
+  u8 i;
+  u8 counter;
+
+  for (i = 0; i < 16; i++) {
+    data = set_rf32_counter(data, i);
+    counter = get_rf32_counter(data);
+    fail_if(counter != i, "Unexpected counter value");
+  }
+  data = set_rf32_counter(data, 0);
+  fail_if(0xC3 != data, "Unexpected counter value");
+}
+END_TEST
+
+START_TEST(test_counter_checker_rf41)
+{
+  size_t i;
+  struct recording recording;
+  const struct mismatch_data *mismatch;
+  size_t size;
+  size_t disc_counter;
+
+  static struct callbacks cbs = {
+    arr_read,
+    arr_rewind,
+    get_rf41_counter,
+    set_rf41_counter,
+  };
+
+  // check sample data with different counter values
+  size = SAMPLE_CHUNK_SIZE + SAMPLE_CHUNK_SIZE / 2;
+  for (i = 0; i < SAMPLE_COUNTER_MODULO; i++) {
+    recording = generate_recording(i, set_rf41_counter,
+                                   SAMPLE_COUNTER_MODULO,
+                                   SAMPLE_CHUNK_SIZE + SAMPLE_CHUNK_SIZE / 2);
+    fail_if(NULL == recording.samples, "Could not allocate recording data");
+    fail_if(size != recording.size, "Recording data size mismatch");
+
+    arr_offset = 0;
+    counter_checker_init();
+    mismatch = counter_checker_run(&cbs, recording.samples, recording.size);
+    fail_if(NULL == mismatch, "Unexpected return value");
+    fail_if(mismatch->counter != 0, "Unexpected counter mismatch found");
+    free(recording.samples);
+  }
+
+  // introduce counter discontinuity
+  size = 2 * SAMPLE_CHUNK_SIZE + SAMPLE_CHUNK_SIZE / 2;
+  recording = generate_recording(0, set_rf41_counter, SAMPLE_COUNTER_MODULO,
+                                 size);
+  fail_if(NULL == recording.samples, "Could not allocate recording data");
+  fail_if(size != recording.size, "Recording data size mismatch");
+
+  disc_counter = 0;
+  for (i = 1; i < size; i += size / 1024) {
+    u8 counter = get_rf41_counter(recording.samples[i - 1]);
+
+    counter += 2;
+    counter %= SAMPLE_COUNTER_MODULO;
+    recording.samples[i] = set_rf41_counter(recording.samples[i], counter);
+
+    disc_counter++;
+  }
+
+  arr_offset = 0;
+  counter_checker_init();
+  mismatch = counter_checker_run(&cbs, recording.samples, recording.size);
+
+  fail_if(mismatch->counter != 2 * disc_counter, "Counter mismatch failure");
+}
+END_TEST
+
+START_TEST(test_counter_checker_rf32)
+{
+  size_t i;
+  struct recording recording;
+  const struct mismatch_data *mismatch;
+  size_t size;
+  size_t disc_counter;
+
+  static struct callbacks cbs = {
+    arr_read,
+    arr_rewind,
+    get_rf32_counter,
+    set_rf32_counter,
+  };
+
+  // check sample data with different counter values
+  size = SAMPLE_CHUNK_SIZE + SAMPLE_CHUNK_SIZE / 2;
+  for (i = 0; i < SAMPLE_COUNTER_MODULO; i++) {
+    recording = generate_recording(i, set_rf32_counter,
+                                   SAMPLE_COUNTER_MODULO,
+                                   SAMPLE_CHUNK_SIZE + SAMPLE_CHUNK_SIZE / 2);
+    fail_if(NULL == recording.samples, "Could not allocate recording data");
+    fail_if(size != recording.size, "Recording data size mismatch");
+
+    arr_offset = 0;
+    counter_checker_init();
+    mismatch = counter_checker_run(&cbs, recording.samples, recording.size);
+    fail_if(NULL == mismatch, "Unexpected return value");
+    fail_if(mismatch->counter != 0, "Unexpected counter mismatch found");
+    free(recording.samples);
+  }
+
+  // introduce counter discontinuity
+  size = 2 * SAMPLE_CHUNK_SIZE + SAMPLE_CHUNK_SIZE / 2;
+  recording = generate_recording(0, set_rf32_counter, SAMPLE_COUNTER_MODULO,
+                                 size);
+  fail_if(NULL == recording.samples, "Could not allocate recording data");
+  fail_if(size != recording.size, "Recording data size mismatch");
+
+  disc_counter = 0;
+  for (i = 1; i < size; i += size / 1024) {
+    u8 counter = get_rf32_counter(recording.samples[i - 1]);
+
+    counter += 2;
+    counter %= SAMPLE_COUNTER_MODULO;
+    recording.samples[i] = set_rf32_counter(recording.samples[i], counter);
+
+    disc_counter++;
+  }
+
+  arr_offset = 0;
+  counter_checker_init();
+  mismatch = counter_checker_run(&cbs, recording.samples, recording.size);
+
+  fail_if(mismatch->counter != 2 * disc_counter, "Counter mismatch failure");
+}
+END_TEST
+
+Suite* counter_checker_suite(void)
+{
+  Suite *s = suite_create("Counter checker");
+  TCase *tc_core = tcase_create("Core");
+
+  tcase_add_test(tc_core, test_counters_rf32);
+  tcase_add_test(tc_core, test_counters_rf41);
+  tcase_add_test(tc_core, test_counter_checker_rf41);
+  tcase_add_test(tc_core, test_counter_checker_rf32);
+  suite_add_tcase(s, tc_core);
+
+  return s;
+}
diff --git a/tests/check_main.c b/tests/check_main.c
index d52d68f7..6603318d 100644
--- a/tests/check_main.c
+++ b/tests/check_main.c
@@ -33,6 +33,8 @@ int main(void)
   srunner_add_suite(sr, signal_test_suite());
   srunner_add_suite(sr, track_test_suite());
   srunner_add_suite(sr, cnav_test_suite());
+  srunner_add_suite(sr, correlator_suite());
+  srunner_add_suite(sr, counter_checker_suite());
 
   srunner_set_fork_status(sr, CK_NOFORK);
   srunner_run_all(sr, CK_NORMAL);
diff --git a/tests/check_suites.h b/tests/check_suites.h
index 71c6ec07..3994ba1c 100644
--- a/tests/check_suites.h
+++ b/tests/check_suites.h
@@ -23,5 +23,7 @@ Suite* ionosphere_suite(void);
 Suite* signal_test_suite(void);
 Suite* track_test_suite(void);
 Suite* cnav_test_suite(void);
+Suite* correlator_suite(void);
+Suite* counter_checker_suite(void);
 
 #endif /* CHECK_SUITES_H */
diff --git a/tests/data/l2cbitstream/libl2cbitstream/l2cbitstream.c b/tests/data/l2cbitstream/libl2cbitstream/l2cbitstream.c
index fcbddf95..a7c84d0a 100644
--- a/tests/data/l2cbitstream/libl2cbitstream/l2cbitstream.c
+++ b/tests/data/l2cbitstream/libl2cbitstream/l2cbitstream.c
@@ -31,8 +31,7 @@
 #define CNAVMSG_CRC_LEN_BITS           (BITS_IN_BYTE * CNAVMSG_CRC_LEN_BYTES)
 #define CNAVMSG_CRC_IDX_BITS           276
 #define CNAVMSG_TOW_COUNT_IDX_BITS     20
-#define CNAVMSG_TOW_COUNT_INC          8 // equals 12 seconds
-#define CNAVMSG_TOW_COUNT_MAX          403199
+#define CNAVMSG_TOW_COUNT_INC          2 // equals 12 seconds
 #define CNAVMSG_TOW_COUNT_LEN_BITS     17
 #define CNAVMSG_TOW_COUNT_IGNORE_LSB   2
 
@@ -47,27 +46,19 @@ u8 get_l2c_message_length(void)
  * and CRC-24q. Bits are right-aligned so four leftmost bits
  * of the 38 byte buffer are always zero.
  *
- * \param au_message      Buffer for the message.
+ * \param[out] au_message      Buffer for the message.
+ * \param[in]  prn        GPS SV PRN number [0..31].
+ * \param[in]  msg_id     Message idntifier [0..31].
+ * \param[in]  tow        ToW in 6 second units [0..131071].
  * \return true if success
  *         false if error
  */
-bool get_l2c_message(u8 *au_message)
+bool get_l2c_message(u8 *au_message, u8 prn, u8 msg_id, u32 tow)
 {
   u32 q_idx = CNAVMSG_OFFSET_BITS;
-  static u32 q_towcount = 0;
-  u32 q_towcount_trunc = 0;
 
   memset(au_message, 0, CNAVMSG_LEN_BYTES);
 
-  q_towcount += CNAVMSG_TOW_COUNT_INC;
-
-  if (CNAVMSG_TOW_COUNT_MAX < q_towcount)
-  {
-    q_towcount = 0;
-  }
-
-  srand(time(NULL) * clock());
-
   /*
    * Offset is 4 -> 4 leftmost bits are always '0'.
    * Preamble.
@@ -76,16 +67,15 @@ bool get_l2c_message(u8 *au_message)
   q_idx += CNAVMSG_PREAMBLE_LEN_BITS;
 
   /* PRN */
-  setbitu(au_message, q_idx, CNAVMSG_PRN_LEN_BITS, rand());
+  setbitu(au_message, q_idx, CNAVMSG_PRN_LEN_BITS, prn);
   q_idx += CNAVMSG_PRN_LEN_BITS;
 
   /* Message Type */
-  setbitu(au_message, q_idx, CNAVMSG_TYPE_LEN_BITS, rand());
+  setbitu(au_message, q_idx, CNAVMSG_TYPE_LEN_BITS, msg_id);
   q_idx += CNAVMSG_TYPE_LEN_BITS;
 
   /* 17 MSBs of the TOW count -> ignore 2 LSBs */
-  q_towcount_trunc = q_towcount >> CNAVMSG_TOW_COUNT_IGNORE_LSB;
-  setbitu(au_message, q_idx, CNAVMSG_TOW_COUNT_LEN_BITS, q_towcount_trunc);
+  setbitu(au_message, q_idx, CNAVMSG_TOW_COUNT_LEN_BITS, tow);
   q_idx += CNAVMSG_TOW_COUNT_LEN_BITS;
 
   /* Random payload */
@@ -116,13 +106,15 @@ static s32 get_aligned_message(u8 *au_msg_write)
   static bool b_offset = true;
   static u8 au_msg_nxt[CNAVMSG_LEN_BYTES] = {0};
   static u8 u_idx = CNAVMSG_LEN_BYTES;
+  u32 q_tow_count = 0;
   u64 t_msg_amount = 0;
 
   /* First call */
   if (u_idx >= CNAVMSG_LEN_BYTES) {
-    if (!get_l2c_message(au_msg_nxt)) {
+    if (!get_l2c_message(au_msg_nxt, rand(), rand(), q_tow_count)) {
         return -1;
     } else {
+      q_tow_count += CNAVMSG_TOW_COUNT_INC;
       u_idx = 0;
     }
   }
@@ -137,9 +129,10 @@ static s32 get_aligned_message(u8 *au_msg_write)
 
     if (u_idx >= CNAVMSG_LEN_BYTES) {
       /* get next 300 bit message since the buffer is empty*/
-      if (!get_l2c_message(au_msg_nxt)) {
+      if (!get_l2c_message(au_msg_nxt, rand(), rand(), q_tow_count)) {
         return -1;
       } else {
+        q_tow_count += CNAVMSG_TOW_COUNT_INC;
         u_idx = 0;
         t_msg_amount++;
         if (b_offset) {
@@ -179,6 +172,8 @@ s32 write_l2c_to_file(int i_fileno, u64 t_wanted_msg_amount)
   u8 au_msg_write[CNAVMSG_LEN_BYTES] = {0};
   FILE *pz_file = fdopen(i_fileno, "wb");
 
+  srand(time(NULL) * clock());
+
   if (NULL == pz_file) {
     printf("ERROR opening file\n");
     return -1;
diff --git a/tests/data/l2cbitstream/libl2cbitstream/l2cbitstream.h b/tests/data/l2cbitstream/libl2cbitstream/l2cbitstream.h
index 54ccd8c7..19492c2e 100644
--- a/tests/data/l2cbitstream/libl2cbitstream/l2cbitstream.h
+++ b/tests/data/l2cbitstream/libl2cbitstream/l2cbitstream.h
@@ -18,5 +18,5 @@
 #define CNAVMSG_LEN_BYTES 38
 
 u8 get_l2c_message_length(void);
-bool get_l2c_message(u8 *au_message);
+bool get_l2c_message(u8 *au_message, u8 prn, u8 msg_id, u32 tow);
 s32 write_l2c_to_file(int i_fileno, u64 t_msg_amount);