diff --git a/can/common.h b/can/common.h index 848c1bb..4d0e14c 100644 --- a/can/common.h +++ b/can/common.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include "common_dbc.h" @@ -55,5 +56,15 @@ public: void UpdateValid(uint64_t sec); void update_string(std::string data, bool sendcan); std::vector query_latest(); - +}; + +class CANPacker { +private: + const DBC *dbc = NULL; + std::map, Signal> signal_lookup; + std::map message_lookup; + +public: + CANPacker(const std::string& dbc_name); + uint64_t pack(uint32_t address, const std::vector &signals, int counter); }; diff --git a/can/common.pxd b/can/common.pxd index 366abf4..e07e0b3 100644 --- a/can/common.pxd +++ b/can/common.pxd @@ -9,8 +9,7 @@ from libcpp.unordered_set cimport unordered_set from libcpp cimport bool -cdef extern from "common.h": - +cdef extern from "common_dbc.h": ctypedef enum SignalType: DEFAULT, HONDA_CHECKSUM, @@ -64,6 +63,12 @@ cdef extern from "common.h": const char* name double value + cdef struct SignalPackValue: + const char * name + double value + + +cdef extern from "common.h": cdef const DBC* dbc_lookup(const string); cdef cppclass CANParser: @@ -71,3 +76,7 @@ cdef extern from "common.h": CANParser(int, string, vector[MessageParseOptions], vector[SignalParseOptions]) void update_string(string, bool) vector[SignalValue] query_latest() + + cdef cppclass CANPacker: + CANPacker(string) + uint64_t pack(uint32_t, vector[SignalPackValue], int counter) diff --git a/can/packer.cc b/can/packer.cc index c658e56..fb81952 100644 --- a/can/packer.cc +++ b/can/packer.cc @@ -8,132 +8,101 @@ #define WARN printf -namespace { +// this is the same as read_u64_le, but uses uint64_t as in/out +uint64_t ReverseBytes(uint64_t x) { + return ((x & 0xff00000000000000ull) >> 56) | + ((x & 0x00ff000000000000ull) >> 40) | + ((x & 0x0000ff0000000000ull) >> 24) | + ((x & 0x000000ff00000000ull) >> 8) | + ((x & 0x00000000ff000000ull) << 8) | + ((x & 0x0000000000ff0000ull) << 24) | + ((x & 0x000000000000ff00ull) << 40) | + ((x & 0x00000000000000ffull) << 56); +} - // this is the same as read_u64_le, but uses uint64_t as in/out - uint64_t ReverseBytes(uint64_t x) { - return ((x & 0xff00000000000000ull) >> 56) | - ((x & 0x00ff000000000000ull) >> 40) | - ((x & 0x0000ff0000000000ull) >> 24) | - ((x & 0x000000ff00000000ull) >> 8) | - ((x & 0x00000000ff000000ull) << 8) | - ((x & 0x0000000000ff0000ull) << 24) | - ((x & 0x000000000000ff00ull) << 40) | - ((x & 0x00000000000000ffull) << 56); +uint64_t set_value(uint64_t ret, Signal sig, int64_t ival){ + int shift = sig.is_little_endian? sig.b1 : sig.bo; + uint64_t mask = ((1ULL << sig.b2)-1) << shift; + uint64_t dat = (ival & ((1ULL << sig.b2)-1)) << shift; + if (sig.is_little_endian) { + dat = ReverseBytes(dat); + mask = ReverseBytes(mask); } + ret &= ~mask; + ret |= dat; + return ret; +} - uint64_t set_value(uint64_t ret, Signal sig, int64_t ival){ - int shift = sig.is_little_endian? sig.b1 : sig.bo; - uint64_t mask = ((1ULL << sig.b2)-1) << shift; - uint64_t dat = (ival & ((1ULL << sig.b2)-1)) << shift; - if (sig.is_little_endian) { - dat = ReverseBytes(dat); - mask = ReverseBytes(mask); +CANPacker::CANPacker(const std::string& dbc_name) { + dbc = dbc_lookup(dbc_name); + assert(dbc); + + for (int i=0; inum_msgs; i++) { + const Msg* msg = &dbc->msgs[i]; + message_lookup[msg->address] = *msg; + for (int j=0; jnum_sigs; j++) { + const Signal* sig = &msg->sigs[j]; + signal_lookup[std::make_pair(msg->address, std::string(sig->name))] = *sig; } - ret &= ~mask; - ret |= dat; - return ret; } + init_crc_lookup_tables(); +} - class CANPacker { - public: - CANPacker(const std::string& dbc_name) { - dbc = dbc_lookup(dbc_name); - assert(dbc); +uint64_t CANPacker::pack(uint32_t address, const std::vector &signals, int counter) { + uint64_t ret = 0; + for (const auto& sigval : signals) { + std::string name = std::string(sigval.name); + double value = sigval.value; - for (int i=0; inum_msgs; i++) { - const Msg* msg = &dbc->msgs[i]; - message_lookup[msg->address] = *msg; - for (int j=0; jnum_sigs; j++) { - const Signal* sig = &msg->sigs[j]; - signal_lookup[std::make_pair(msg->address, std::string(sig->name))] = *sig; - } - } - init_crc_lookup_tables(); + auto sig_it = signal_lookup.find(std::make_pair(address, name)); + if (sig_it == signal_lookup.end()) { + WARN("undefined signal %s - %d\n", name.c_str(), address); + continue; + } + auto sig = sig_it->second; + + int64_t ival = (int64_t)(round((value - sig.offset) / sig.factor)); + if (ival < 0) { + ival = (1ULL << sig.b2) + ival; } - uint64_t pack(uint32_t address, const std::vector &signals, int counter) { - uint64_t ret = 0; - for (const auto& sigval : signals) { - std::string name = std::string(sigval.name); - double value = sigval.value; - - auto sig_it = signal_lookup.find(std::make_pair(address, name)); - if (sig_it == signal_lookup.end()) { - WARN("undefined signal %s - %d\n", name.c_str(), address); - continue; - } - auto sig = sig_it->second; - - int64_t ival = (int64_t)(round((value - sig.offset) / sig.factor)); - if (ival < 0) { - ival = (1ULL << sig.b2) + ival; - } - - ret = set_value(ret, sig, ival); - } - - if (counter >= 0){ - auto sig_it = signal_lookup.find(std::make_pair(address, "COUNTER")); - if (sig_it == signal_lookup.end()) { - WARN("COUNTER not defined\n"); - return ret; - } - auto sig = sig_it->second; - - if ((sig.type != SignalType::HONDA_COUNTER) && (sig.type != SignalType::VOLKSWAGEN_COUNTER)) { - WARN("COUNTER signal type not valid\n"); - } - - ret = set_value(ret, sig, counter); - } - - auto sig_it_checksum = signal_lookup.find(std::make_pair(address, "CHECKSUM")); - if (sig_it_checksum != signal_lookup.end()) { - auto sig = sig_it_checksum->second; - if (sig.type == SignalType::HONDA_CHECKSUM) { - unsigned int chksm = honda_checksum(address, ret, message_lookup[address].size); - ret = set_value(ret, sig, chksm); - } else if (sig.type == SignalType::TOYOTA_CHECKSUM) { - unsigned int chksm = toyota_checksum(address, ret, message_lookup[address].size); - ret = set_value(ret, sig, chksm); - } else if (sig.type == SignalType::VOLKSWAGEN_CHECKSUM) { - // FIXME: Hackish fix for an endianness issue. The message is in reverse byte order - // until later in the pack process. Checksums can be run backwards, CRCs not so much. - // The correct fix is unclear but this works for the moment. - unsigned int chksm = volkswagen_crc(address, ReverseBytes(ret), message_lookup[address].size); - ret = set_value(ret, sig, chksm); - } else { - //WARN("CHECKSUM signal type not valid\n"); - } - } + ret = set_value(ret, sig, ival); + } + if (counter >= 0){ + auto sig_it = signal_lookup.find(std::make_pair(address, "COUNTER")); + if (sig_it == signal_lookup.end()) { + WARN("COUNTER not defined\n"); return ret; } + auto sig = sig_it->second; + if ((sig.type != SignalType::HONDA_COUNTER) && (sig.type != SignalType::VOLKSWAGEN_COUNTER)) { + WARN("COUNTER signal type not valid\n"); + } - private: - const DBC *dbc = NULL; - std::map, Signal> signal_lookup; - std::map message_lookup; - }; + ret = set_value(ret, sig, counter); + } -} - -extern "C" { - void* canpack_init(const char* dbc_name) { - CANPacker *ret = new CANPacker(std::string(dbc_name)); - return (void*)ret; - } - - uint64_t canpack_pack(void* inst, uint32_t address, size_t num_vals, const SignalPackValue *vals, int counter, bool checksum) { - CANPacker *cp = (CANPacker*)inst; - - return cp->pack(address, std::vector(vals, vals+num_vals), counter); - } - - uint64_t canpack_pack_vector(void* inst, uint32_t address, const std::vector &signals, int counter) { - CANPacker *cp = (CANPacker*)inst; - return cp->pack(address, signals, counter); - } + auto sig_it_checksum = signal_lookup.find(std::make_pair(address, "CHECKSUM")); + if (sig_it_checksum != signal_lookup.end()) { + auto sig = sig_it_checksum->second; + if (sig.type == SignalType::HONDA_CHECKSUM) { + unsigned int chksm = honda_checksum(address, ret, message_lookup[address].size); + ret = set_value(ret, sig, chksm); + } else if (sig.type == SignalType::TOYOTA_CHECKSUM) { + unsigned int chksm = toyota_checksum(address, ret, message_lookup[address].size); + ret = set_value(ret, sig, chksm); + } else if (sig.type == SignalType::VOLKSWAGEN_CHECKSUM) { + // FIXME: Hackish fix for an endianness issue. The message is in reverse byte order + // until later in the pack process. Checksums can be run backwards, CRCs not so much. + // The correct fix is unclear but this works for the moment. + unsigned int chksm = volkswagen_crc(address, ReverseBytes(ret), message_lookup[address].size); + ret = set_value(ret, sig, chksm); + } else { + //WARN("CHECKSUM signal type not valid\n"); + } + } + + return ret; } diff --git a/can/packer_pyx.pyx b/can/packer_pyx.pyx index 7869b35..00f83ec 100644 --- a/can/packer_pyx.pyx +++ b/can/packer_pyx.pyx @@ -7,78 +7,22 @@ from libcpp.map cimport map from libcpp.string cimport string from libcpp cimport bool from posix.dlfcn cimport dlopen, dlsym, RTLD_LAZY -import os -import subprocess -cdef struct SignalPackValue: - const char* name - double value - -ctypedef enum SignalType: - DEFAULT, - HONDA_CHECKSUM, - HONDA_COUNTER, - TOYOTA_CHECKSUM, - PEDAL_CHECKSUM, - PEDAL_COUNTER, - VOLKSWAGEN_CHECKSUM, - VOLKSWAGEN_COUNTER - -cdef struct Signal: - const char* name - int b1, b2, bo - bool is_signed - double factor, offset - SignalType type +from common cimport CANPacker as cpp_CANPacker +from common cimport dbc_lookup, SignalPackValue, DBC - -cdef struct Msg: - const char* name - uint32_t address - unsigned int size - size_t num_sigs - const Signal *sigs - -cdef struct Val: - const char* name - uint32_t address - const char* def_val - const Signal *sigs - -cdef struct DBC: - const char* name - size_t num_msgs - const Msg *msgs - const Val *vals - size_t num_vals - -ctypedef void * (*canpack_init_func)(const char* dbc_name) -ctypedef uint64_t (*canpack_pack_vector_func)(void* inst, uint32_t address, const vector[SignalPackValue] &signals, int counter) -ctypedef const DBC * (*dbc_lookup_func)(const char* dbc_name) - - -cdef class CANPacker(): - cdef void *packer - cdef const DBC *dbc - cdef map[string, (int, int)] name_to_address_and_size - cdef map[int, int] address_to_size - cdef canpack_init_func canpack_init - cdef canpack_pack_vector_func canpack_pack_vector - cdef dbc_lookup_func dbc_lookup +cdef class CANPacker: + cdef: + cpp_CANPacker *packer + const DBC *dbc + map[string, (int, int)] name_to_address_and_size + map[int, int] address_to_size def __init__(self, dbc_name): - can_dir = os.path.dirname(os.path.abspath(__file__)) - libdbc_fn = os.path.join(can_dir, "libdbc.so") - libdbc_fn = str(libdbc_fn).encode('utf8') + self.packer = new cpp_CANPacker(dbc_name) + self.dbc = dbc_lookup(dbc_name) - cdef void *libdbc = dlopen(libdbc_fn, RTLD_LAZY) - self.canpack_init = dlsym(libdbc, 'canpack_init') - self.canpack_pack_vector = dlsym(libdbc, 'canpack_pack_vector') - self.dbc_lookup = dlsym(libdbc, 'dbc_lookup') - - self.packer = self.canpack_init(dbc_name) - self.dbc = self.dbc_lookup(dbc_name) num_msgs = self.dbc[0].num_msgs for i in range(num_msgs): msg = self.dbc[0].msgs[i] @@ -99,7 +43,7 @@ cdef class CANPacker(): spv.value = value values_thing.push_back(spv) - return self.canpack_pack_vector(self.packer, addr, values_thing, counter) + return self.packer.pack(addr, values_thing, counter) cdef inline uint64_t ReverseBytes(self, uint64_t x): return (((x & 0xff00000000000000ull) >> 56) | diff --git a/can/packer_pyx_setup.py b/can/packer_pyx_setup.py index 82b10b0..5c2d54b 100644 --- a/can/packer_pyx_setup.py +++ b/can/packer_pyx_setup.py @@ -1,9 +1,36 @@ +import os +import subprocess from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module from Cython.Build import cythonize +from common.basedir import BASEDIR from common.cython_hacks import BuildExtWithoutPlatformSuffix -setup(name='CAN Packer API Implementation', +sourcefiles = ['packer_pyx.pyx'] +extra_compile_args = ["-std=c++11"] +ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() # pylint: disable=unexpected-keyword-arg + +if ARCH == "aarch64": + extra_compile_args += ["-Wno-deprecated-register"] + + +setup(name='CAN packer', cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, - ext_modules=cythonize(Extension("packer_pyx", ["packer_pyx.pyx"], language="c++", extra_compile_args=["-std=c++11"]))) + ext_modules=cythonize( + Extension( + "packer_pyx", + language="c++", + sources=sourcefiles, + extra_compile_args=extra_compile_args, + include_dirs=[ + BASEDIR, + os.path.join(BASEDIR, 'phonelibs', 'capnp-cpp/include'), + ], + extra_link_args=[ + os.path.join(BASEDIR, 'opendbc', 'can', 'libdbc.so'), + ], + ) + ), + nthreads=4, +)