From e5b2286756b5a5637098cea740aa0426d43f14f2 Mon Sep 17 00:00:00 2001 From: Jake Dahl Date: Mon, 8 Oct 2018 17:36:10 -0600 Subject: [PATCH] added an SMBus base class --- src/lib/drivers/CMakeLists.txt | 1 + src/lib/drivers/smbus/CMakeLists.txt | 34 ++++++ src/lib/drivers/smbus/SMBus.cpp | 163 +++++++++++++++++++++++++++ src/lib/drivers/smbus/SMBus.hpp | 80 +++++++++++++ 4 files changed, 278 insertions(+) create mode 100644 src/lib/drivers/smbus/CMakeLists.txt create mode 100644 src/lib/drivers/smbus/SMBus.cpp create mode 100644 src/lib/drivers/smbus/SMBus.hpp diff --git a/src/lib/drivers/CMakeLists.txt b/src/lib/drivers/CMakeLists.txt index 688d2c2101..513f4c9c2e 100644 --- a/src/lib/drivers/CMakeLists.txt +++ b/src/lib/drivers/CMakeLists.txt @@ -34,3 +34,4 @@ add_subdirectory(airspeed) add_subdirectory(device) add_subdirectory(led) +add_subdirectory(smbus) diff --git a/src/lib/drivers/smbus/CMakeLists.txt b/src/lib/drivers/smbus/CMakeLists.txt new file mode 100644 index 0000000000..d3e1a1fcce --- /dev/null +++ b/src/lib/drivers/smbus/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2018 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(drivers__smbus SMBus.cpp) \ No newline at end of file diff --git a/src/lib/drivers/smbus/SMBus.cpp b/src/lib/drivers/smbus/SMBus.cpp new file mode 100644 index 0000000000..fdaecda156 --- /dev/null +++ b/src/lib/drivers/smbus/SMBus.cpp @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "SMBus.hpp" + +SMBus::SMBus(int bus_num, uint16_t address) : + I2C("BATT_SMBUS_I2C", nullptr, bus_num, address, 100000) +{ + init(); +} + +SMBus::~SMBus() +{ +} + +int SMBus::read_word(const uint8_t cmd_code, void *data) +{ + // 2 data bytes + pec byte + int result = transfer(&cmd_code, 1, (uint8_t *)data, 3); + + if (result == PX4_OK) { + // Check PEC. + uint8_t addr = get_device_address() << 1; + uint8_t full_data_packet[5]; + full_data_packet[0] = addr | 0x00; + full_data_packet[1] = cmd_code; + full_data_packet[2] = addr | 0x01; + memcpy(&full_data_packet[3], data, 2); + + uint8_t pec = get_pec(full_data_packet, sizeof(full_data_packet) / sizeof(full_data_packet[0])); + + if (pec == ((uint8_t *)data)[2]) { + return PX4_OK; + + } else { + return PX4_ERROR; + } + } + + return PX4_ERROR; +} + +int SMBus::block_read(const uint8_t cmd_code, void *data, const uint8_t length) +{ + unsigned byte_count = 0; + // Length of data (32max). byte_count(1), cmd_code(2), pec(1) + uint8_t rx_data[32 + 4]; + + transfer(&cmd_code, 1, (uint8_t *)rx_data, length + 2); + + byte_count = rx_data[0]; + + // addr1, addr2, byte_count, + memcpy(data, &rx_data[1], byte_count); + + // addr(wr), cmd_code, addr(r), byte_count, rx_data[] + uint8_t device_address = get_device_address(); + uint8_t full_data_packet[32 + 4] = {0}; + + full_data_packet[0] = (device_address << 1) | 0x00; + full_data_packet[1] = cmd_code; + full_data_packet[2] = (device_address << 1) | 0x01; + full_data_packet[3] = byte_count; + + memcpy(&full_data_packet[4], &rx_data[1], byte_count); + + uint8_t pec = get_pec(full_data_packet, byte_count + 4); + + // First byte is byte count, followed by data. + if (pec != ((uint8_t *)rx_data)[byte_count + 1]) { + PX4_INFO("bad PEC from block_read"); + return PX4_ERROR; + + } else { + return PX4_OK; + } +} + +int SMBus::block_write(const uint8_t cmd_code, void *data, const uint8_t byte_count) +{ + // cmd code, byte count, data[byte_count], pec + uint8_t buf[byte_count + 2]; + + buf[0] = cmd_code; + buf[1] = (uint8_t)byte_count; + memcpy(&buf[2], data, byte_count); + + uint8_t pec = get_pec(buf, sizeof(buf)); + buf[byte_count + 2] = pec; + + unsigned i = 0; + + // If block_write fails, try up to 10 times. + while (i < 10) { + if (PX4_OK != transfer((uint8_t *)buf, sizeof(buf), nullptr, 0)) { + i++; + PX4_WARN("block_write failed: %d", i); + usleep(100000); + + } else { + return PX4_OK; + } + } + + return PX4_ERROR; +} + +uint8_t SMBus::get_pec(uint8_t *buff, const uint8_t len) +{ + // Initialise CRC to zero. + uint8_t crc = 0; + uint8_t shift_register = 0; + bool invert_crc; + + // Calculate crc for each byte in the stream. + for (uint8_t i = 0; i < len; i++) { + // Load next data byte into the shift register + shift_register = buff[i]; + + // Calculate crc for each bit in the current byte. + for (uint8_t j = 0; j < 8; j++) { + invert_crc = (crc ^ shift_register) & 0x80; + crc <<= 1; + shift_register <<= 1; + + if (invert_crc) { + crc ^= SMBUS_PEC_POLYNOMIAL; + } + } + } + + return crc; +} \ No newline at end of file diff --git a/src/lib/drivers/smbus/SMBus.hpp b/src/lib/drivers/smbus/SMBus.hpp new file mode 100644 index 0000000000..e8fa196965 --- /dev/null +++ b/src/lib/drivers/smbus/SMBus.hpp @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#include + +#define SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC + +class SMBus : public device::I2C +{ +public: + SMBus(int bus_num, uint16_t address); + ~SMBus(); + + /** + * @brief Sends a block write command. + * @param cmd_code The command code. + * @param data The data to be written. + * @param length The number of bytes being written. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + int block_write(const uint8_t cmd_code, void *data, const uint8_t byte_count); + + /** + * @brief Sends a block read command. + * @param cmd_code The command code. + * @param data The returned data. + * @param length The number of bytes being read + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + int block_read(const uint8_t cmd_code, void *data, const uint8_t length); + + /** + * @brief Sends a read word command. + * @param cmd_code The command code. + * @param data The 2 bytes of returned data plus a 1 byte CRC if used. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + int read_word(const uint8_t cmd_code, void *data); + + /** + * @brief Calculates the PEC from the data. + * @param buffer The buffer that stores the data to perform the CRC over. + * @param length The number of bytes being written. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + uint8_t get_pec(uint8_t *buffer, uint8_t length); + +};