toys/dbcc/source/signal.cpp

#include "dbcc/signal.h"

#include <sstream>
#include <limits>
#include <iterator>
#include <vector>       /**< std::vector */
#include <algorithm>

#include "dbcc/helper/signal_helper.h"

// FIXME: We ignore the properties and comment of Signal
namespace ad {
namespace dbcc {

static inline std::string &trim(std::string &str, const std::string &toTrim = " ")
{
    std::string::size_type pos = str.find_last_not_of(toTrim);

    if (pos == std::string::npos)
    {
        str.clear();
    }
    else
    {
        str.erase(pos + 1);
        str.erase(0, str.find_first_not_of(toTrim));
    }

    return str;
}

static inline std::vector<std::string> &split(const std::string &s, char delim, std::vector<std::string> &elems)
{
    std::stringstream ss(s);
    std::string item;

    while (std::getline(ss, item, delim))
    {
        elems.push_back(item);
    }

    return elems;
}

static inline std::vector<std::string> split(const std::string &s, char delim)
{
    std::vector<std::string> elems;
    split(s, delim, elems);
    return elems;
}

static inline bool consume_number(std::istream &in, double &num)
{
    int64_t temp = 0;

    in >> temp;

    char c = in.peek();

    if (c == '.')
    {
        double fraction = 0;
        in.ignore(1); // .
        in >> fraction;
        num = temp + fraction;
        return true;
    }

    num = temp;

    return false;
}

std::istream &operator>>(std::istream &in, Signal &sig)
{
    std::string preamble;

    in >> preamble;

    // Check if we are actually reading a signal otherwise fail the stream
    if (preamble != "SG_")
    {
        in.setstate(std::ios_base::failbit);
        return in;
    }

    sig.m_isFloat = false;
    // Parse the signal name
    in >> sig.m_name;

    std::string multi;
    in >> multi;

    // This case happens if there is not Multiplexor present
    if (multi == ":")
    {
        sig.m_multiplexor = Multiplexor::None;
    }
    else
    {
        if (multi == "M")
        {
            sig.m_multiplexor = Multiplexor::Multiplexor;
        }
        else
        {
            // The multiplexor looks like that 'm12' so we ignore the m and parse it as integer
            std::istringstream multistream(multi);
            multistream.ignore(1);
            uint16_t multiNum;
            multistream >> multiNum;
            sig.m_multiplexor = Multiplexor::Multiplexor;
            sig.m_multiplexedNumber = multiNum;
        }

        // Ignore the next character which is a ':'
        in >> multi;
    }

    in >> sig.m_startBit;
    in.ignore(1);
    in >> sig.m_length;
    in.ignore(1);

    int order;
    in >> order;

    if (order == 0)
    {
        sig.m_byteOrder = ByteOrder::Motorola;
    }
    else
    {
        sig.m_byteOrder = ByteOrder::Intel;
    }

    char sign;
    in >> sign;

    if (sign == '+')
    {
        sig.m_sign = Sign::Unsigned;
    }
    else
    {
        sig.m_sign = Sign::Signed;
    }

    bool isfloat = false;
    // (factor,offset) [max|min]
    in.ignore(std::numeric_limits<std::streamsize>::max(), '(');
    isfloat = consume_number(in, sig.m_factor) || isfloat;
    in.ignore(1); // ,
    isfloat = consume_number(in, sig.m_offset) || isfloat;
    in.ignore(1); // )
    in.ignore(std::numeric_limits<std::streamsize>::max(), '[');
    isfloat = consume_number(in, sig.m_minimum) || isfloat;
    in.ignore(1); // |
    isfloat = consume_number(in, sig.m_maximum) || isfloat;
    in.ignore(1); // ]

    sig.m_isFloat = isfloat;

    // Unit string
    std::stringstream unit;
    in.ignore(std::numeric_limits<std::streamsize>::max(), '\"');
    while (in.peek() == '\"')
    {
        unit.put(in.get());
    }

    if (in.eof() || !in)
    {
        in.setstate(std::ios_base::failbit);
        return in;
    }

    std::string unitstr = unit.str();
    sig.m_unit = trim(unitstr, "\"");

    std::string to;
    getline(in, to);

    if (!to.empty() && *to.rbegin() == '\r')
    {
        to.erase(to.length() - 1, 1);
    }

    if (!to.empty())
    {
        std::vector<std::string> toStrings = split(to, ',');
        std::move(toStrings.begin(), toStrings.end(), std::inserter(sig.m_to, sig.m_to.begin()));
    }

    return in;
}

Signal::~Signal()
{
}

bool Signal::compile()
{
    ad::dbcc::helper::SignalHelper sigHelper(*this);

    for (auto seg : sigHelper.segments(true))
    {
        CompiledSignalParameter param;

        if (seg.direction ==
            ad::dbcc::helper::SignalHelper::ShiftDirection::Left)
        {
            param.left = 1;
        }
        else
        {
            param.left = 0;
        }

        param.index = seg.index;
        param.shift = seg.shift;
        param.mask = seg.mask;

        m_params.emplace_back(param);
    }

    if (sign() == ad::dbcc::Sign::Signed)
    {
        uint32_t mask =
            ((1 << (sigHelper.typeLength() - length())) - 1);

        if (mask != 0)
        {
            mask <<= length();
            m_signedLengthMask = mask;
        }
    }

#if defined(DEBUG) && defined(VERBOSE)
    std::cout << *this << std::endl;
#endif /* DEBUG && VERBOSE */

    return true;
}

static uint8_t _packLeftShift(uint64_t value, uint32_t shift, uint32_t mask)
{
    return (uint8_t)((uint8_t)(value << shift) & mask);
}

static uint8_t _packRightShift(uint64_t value, uint32_t shift, uint32_t mask)
{
    return (uint8_t)((uint8_t)(value >> shift) & mask);
}

static int64_t _unpackLeftShift(uint8_t value, uint32_t shift, uint32_t mask)
{
    return static_cast<int64_t>(static_cast<int64_t>(value & mask) << shift);
}

static int64_t _unpackRightShift(uint8_t value, uint32_t shift, uint32_t mask)
{
    return static_cast<int64_t>(static_cast<int64_t>(value & mask) >> shift);
}

bool Signal::encode(const ParsedValue &pv, uint8_t *data, size_t /* length */)
{
    if (m_params.size() == 0)
    {
        compile();
    }

    if (pv.isInteger != isFloat())
    {
        return false;
    }

    int64_t temp = 0;

    if (pv.isInteger)
    {
        temp = static_cast<int64_t>((pv.i - offset()) / factor());
    }
    else
    {
        temp = static_cast<int64_t>((pv.f - offset()) / factor());
    }

    for (auto &p : m_params)
    {
        if (p.left)
        {
            data[p.index] |= _packRightShift(temp, p.shift, p.mask);
        }
        else
        {
            data[p.index] |= _packLeftShift(temp, p.shift, p.mask);
        }
    }

    return true;
}

bool Signal::decode(const uint8_t *data, size_t /* length */, ParsedValue &pv)
{
    if (m_params.size() == 0)
    {
        compile();
    }

    int64_t result = 0;

    pv.isInteger = !isFloat();

    for (auto &p : m_params)
    {
        if (p.left)
        {
            result |= _unpackLeftShift(data[p.index], p.shift, p.mask);
        }
        else
        {
            result |= _unpackRightShift(data[p.index], p.shift, p.mask);
        }
    }

    if (m_signedLengthMask != 0)
    {
        if ((result & (static_cast<int64_t>(1) << (length() - 1))) != 0)
        {
            result |= m_signedLengthMask;
        }
    }

    if (pv.isInteger)
    {
        pv.i = static_cast<int32_t>(result * factor() + offset());
    }
    else
    {
        pv.f = static_cast<float>(result * factor() + offset());
    }

    return true;
}

bool Signal::encode(double value, uint8_t *data, size_t /* length */)
{
    if (m_params.size() == 0)
    {
        compile();
    }

    int64_t temp = 0;

    if (!isFloat())
    {
        int32_t i = static_cast<int32_t>(value);
        temp = static_cast<int64_t>((i - offset()) / factor());
    }
    else
    {
        temp = static_cast<int64_t>((value - offset()) / factor());
    }

    for (auto &p : m_params)
    {
        if (p.left)
        {
            data[p.index] |= _packRightShift(temp, p.shift, p.mask);
        }
        else
        {
            data[p.index] |= _packLeftShift(temp, p.shift, p.mask);
        }
    }

    return true;
}

bool Signal::decode(const uint8_t *data, size_t /* length */, double &value)
{
    if (m_params.size() == 0)
    {
        compile();
    }

    int64_t result = 0;

    for (auto &p : m_params)
    {
        if (p.left)
        {
            result |= _unpackLeftShift(data[p.index], p.shift, p.mask);
        }
        else
        {
            result |= _unpackRightShift(data[p.index], p.shift, p.mask);
        }
    }

    if (m_signedLengthMask != 0)
    {
        if ((result & (static_cast<int64_t>(1) << (length() - 1))) != 0)
        {
            result |= m_signedLengthMask;
        }
    }

    if (!isFloat())
    {
        value = static_cast<double>(static_cast<int32_t>(result * factor() + offset()));
    }
    else
    {
        value = static_cast<double>(result * factor() + offset());
    }

    return true;
}

// Here I assume the layout:
//
//                        Bit
//
//           7   6   5   4   3   2   1   0
//         +---+---+---+---+---+---+---+---+
//       0 |<-x|<---------------------x|<--|
//         +---+---+---+---+---+---+---+---+
//       1 |-------------------------------|
//         +---+---+---+---+---+---+---+---+
//       2 |----------x|   |   |   |   |   |
//   B     +---+---+---+---+---+---+---+---+
//   y   3 |   |   |   |   |   |   |   |   |
//   t     +---+---+---+---+---+---+---+---+
//   e   4 |   |   |   |   |   |   |   |   |
//         +---+---+---+---+---+---+---+---+
//       5 |   |   |   |   |   |   |   |   |
//         +---+---+---+---+---+---+---+---+
//       6 |   |   |   |   |   |   |   |   |
//         +---+---+---+---+---+---+---+---+
//       7 |   |   |   |   |   |   |   |   |
//         +---+---+---+---+---+---+---+---+
//
// Reference: https://github.com/cantools/cantools#the-dump-subcommand
bool Signal::bitsLayout(LayoutInfo &info)
{
    info.bits.clear();
    info.byteLines.clear();
    info.byteLineRange.clear();

    if (m_byteOrder == ByteOrder::Intel) //< little-endian
    {
        int lineStart = m_startBit / 8;
        int remainder = 8 - m_startBit % 8;
        int lineNum = ((m_length - remainder + 7) / 8) + 1;
        int startBit = m_startBit;
        int endBit = startBit;
        if (remainder > m_length)
        {
            endBit += m_length;
        }
        else
        {
            endBit += remainder;
        }

        remainder = m_length;

        for (int line = lineStart; line < lineStart + lineNum; ++line)
        {
            for (int bitIdx = startBit; bitIdx < endBit; bitIdx++)
            {
                info.bits.push_back(bitIdx);
            }

            info.byteLines.push_back(line);
            info.byteLineRange.emplace_back(std::pair<int, int>{(9 - startBit % 8) - 1, (8 - (endBit - 1) % 8) - 1});

            remainder = remainder - (startBit - endBit);
            startBit = 8 * (line + 1);

            if (remainder >= 8)
            {
                endBit = startBit + 8;
            }
            else
            {
                endBit = startBit + remainder;
            }
        }
    }
    else    //< big-endian
    {
        int lineStart = m_startBit / 8;
        int remainder = (8 - (8 - (m_startBit + 1) % 8) % 8);
        int lineNum = ((m_length - remainder + 7) / 8) + 1;
        int startBit = m_startBit;
        int lineRemainder = (startBit % 8) + 1;
        int endBit = startBit;
        if (lineRemainder > m_length)
        {
            endBit -= m_length;
        }
        else
        {
            endBit -= lineRemainder;
        }

        remainder = m_length;

        for (int line = lineStart; line < lineStart + lineNum; ++line)
        {
            for (int bitIdx = startBit; bitIdx > endBit; bitIdx--)
            {
                info.bits.push_back(bitIdx);
            }

            info.byteLines.push_back(line);
            info.byteLineRange.emplace_back(std::pair<int, int>{(9 - (endBit + 1) % 8) - 1, (8 - startBit % 8) - 1});
            remainder = remainder - (startBit - endBit);
            startBit = 8 * (line + 2) - 1;
            if (remainder >= 8)
            {
                endBit = startBit - 8;
            }
            else
            {
                endBit = startBit - remainder;
            }
        }
    }

    return true;
}

std::ostream &operator<<(std::ostream &out, Signal::CompiledSignalParameter &p)
{
    out << "CompiledSignalParameter: { index: " << p.index << ", "
        << "shift: " << p.shift << ", "
        << "mask: 0x" << std::hex << p.mask << std::dec << ", "
        << "dir: " << (p.left == 1 ? "Left" : "Right")
        << " }";

    return out;
}

std::ostream &operator<<(std::ostream &out, Signal::LayoutInfo &p)
{
    out << "Bits: { ";
    for (auto bit : p.bits)
    {
        out << bit << ", ";
    }
    out << " }\n";

    for (size_t idx = 0; idx < p.byteLines.size(); idx++)
    {
        out << "Line(" << p.byteLines[idx] << "): [" << p.byteLineRange[idx].first << ", " << p.byteLineRange[idx].second << "]\n";
    }

    return out;
}

std::ostream &operator<<(std::ostream &out, Signal &cs)
{
    int i = 0;
    out << "Message: " << cs.getMessageId() << ", Signal: " << cs.name()
              << (cs.isFloat() ? ", float" : "") << std::endl;

    for (auto &p : cs.m_params)
    {
        out << "  param" << i << ": " << p << std::endl;
        i++;
    }

    if (cs.m_signedLengthMask != 0)
    {
        out << "  mask: 0x" << std::hex << cs.m_signedLengthMask << std::dec << std::endl;
    }

    out << "  factor: " << cs.factor() << ", offset: " << cs.offset();

    return out;
}

} // namespace dbcc
} // namespace ad