64b/67b Encoder

function [ enc ] = enc64b67b( bits )
%ENC64B66B 64b/66b encoder
%   Input: stream of bits to be encoded
%     Should be a multiple of 64 bits
%   Output: stream of 64b/67b encoded bits
%     Will be a multiple of 67 bits
% 
% Alex Forencich <alex@alexforencich.com>
 
% Sources:
% http://cp.literature.agilent.com/litweb/pdf/ads2008/numeric/ads2008/64b66bCoder.html
% http://grouper.ieee.org/groups/802/3/10G_study/public/jan00/walker_1_0100.pdf
% http://www.interlakenalliance.com/Interlaken_Protocol_Definition_v1.2.pdf
 
bits = bits(:)' > 0;
 
rc = size(bits);
 
m = mod(rc(2), 64);
 
if m > 0
    bits = [bits zeros(1,m)];
end
 
rc = size(bits);
 
bc = rc(2)/64;
 
bytes = reshape(bits, 64, bc)';
 
enc = zeros(bc, 67);
 
sreg = ones(1,58);
 
disp = 0;
 
for i = 1:bc
    val = bytes(i,:);
 
    d = 0;
    for j = 1:64
        v = xor(sreg(58),sreg(39));
        v = xor(v, val(j));
        sreg = circshift(sreg, [0 1]);
        sreg(1) = v;
        val(j) = v;
        d = d + (v*2)-1;
    end
 
    inv = sign(disp) == sign(d);
 
    if inv
        disp = disp - d;
        val = 1 - val;
    else
        disp = disp + d;
    end
 
    enc(i,:) = [inv 0 1 val];
end
 
enc = reshape(enc', 1, bc*67);
 
end