Transition edge sensors (TESs) have proven to be highly sensitive and versatile X-ray spectrometers. Upcoming missions, including Athena X-IFU, will rely on highly multiplexed focal planes where more than 32 TES pixels are read out using a single SQUID amplifier channel. We have implemented an error correction algorithm for code-division multiplexed TES signals that can compensate for potential failures of individual SQUID readout channels and that is both scalable and easily implemented in hardware. We present this algorithm for error correction and show results of laboratory tests to assess algorithm performance at recovering TES channels after a SQUID failure.