wmbusmeters/wmbus_im871a.cc

/*
 Copyright (C) 2017-2018 Fredrik Öhrström

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include"wmbus.h"
#include"wmbus_im871a.h"
#include"serial.h"

#include<assert.h>
#include<pthread.h>
#include<semaphore.h>
#include<unistd.h>

using namespace std;

enum FrameStatus { PartialFrame, FullFrame, ErrorInFrame };

struct WMBusIM871A : public WMBus {
    bool ping();
    uint32_t getDeviceId();
    LinkMode getLinkMode();
    void setLinkMode(LinkMode lm);
    void onTelegram(function<void(Telegram*)> cb);

    void processSerialData();
    SerialDevice *serial() { return serial_.get(); }
    void simulate() { }

    WMBusIM871A(unique_ptr<SerialDevice> serial, SerialCommunicationManager *manager);
    ~WMBusIM871A() { }

private:
    unique_ptr<SerialDevice> serial_;
    SerialCommunicationManager *manager_;
    vector<uchar> read_buffer_;
    pthread_mutex_t command_lock_ = PTHREAD_MUTEX_INITIALIZER;
    sem_t command_wait_;
    int sent_command_ {};
    int received_command_ {};
    vector<uchar> received_payload_;
    vector<function<void(Telegram*)>> telegram_listeners_;

    void waitForResponse();
    static FrameStatus checkFrame(vector<uchar> &data,
                           size_t *frame_length, int *endpoint_out, int *msgid_out,
                           int *payload_len_out, int *payload_offset);
    friend bool detectIM871A(string device, SerialCommunicationManager *manager);
    void handleDevMgmt(int msgid, vector<uchar> &payload);
    void handleRadioLink(int msgid, vector<uchar> &payload);
    void handleRadioLinkTest(int msgid, vector<uchar> &payload);
    void handleHWTest(int msgid, vector<uchar> &payload);
};

unique_ptr<WMBus> openIM871A(string device, SerialCommunicationManager *manager)
{
    auto serial = manager->createSerialDeviceTTY(device.c_str(), 57600);
    WMBusIM871A *imp = new WMBusIM871A(std::move(serial), manager);
    return unique_ptr<WMBus>(imp);
}

WMBusIM871A::WMBusIM871A(unique_ptr<SerialDevice> serial, SerialCommunicationManager *manager) :
    serial_(std::move(serial)), manager_(manager)
{
    sem_init(&command_wait_, 0, 0);
    manager_->listenTo(serial_.get(),call(this,processSerialData));
    serial_->open(true);
}

bool WMBusIM871A::ping() {
    pthread_mutex_lock(&command_lock_);

    vector<uchar> msg(4);
    msg[0] = IM871A_SERIAL_SOF;
    msg[1] = DEVMGMT_ID;
    msg[2] = DEVMGMT_MSG_PING_REQ;
    msg[3] = 0;

    sent_command_ = DEVMGMT_MSG_PING_REQ;
    verbose("(im871a) ping\n");
    serial()->send(msg);

    waitForResponse();

    pthread_mutex_unlock(&command_lock_);
    return true;
}

uint32_t WMBusIM871A::getDeviceId() {
    pthread_mutex_lock(&command_lock_);

    vector<uchar> msg(4);
    msg[0] = IM871A_SERIAL_SOF;
    msg[1] = DEVMGMT_ID;
    msg[2] = DEVMGMT_MSG_GET_DEVICEINFO_REQ;
    msg[3] = 0;

    sent_command_ = DEVMGMT_MSG_GET_DEVICEINFO_REQ;
    verbose("(im871a) get device info\n");
    serial()->send(msg);

    waitForResponse();

    uint32_t id = 0;
    if (received_command_ == DEVMGMT_MSG_GET_DEVICEINFO_RSP) {
        verbose("(im871a) device info: module Type %02x\n", received_payload_[0]);
        verbose("(im871a) device info: device Mode %02x\n", received_payload_[1]);
        verbose("(im871a) device info: firmware version %02x\n", received_payload_[2]);
        verbose("(im871a) device info: hci protocol version %02x\n", received_payload_[3]);
        id = received_payload_[4] << 24 |
            received_payload_[5] << 16 |
            received_payload_[6] << 8 |
            received_payload_[7];
        verbose("(im871a) devince info: id %08x\n", id);
    }

    pthread_mutex_unlock(&command_lock_);
    return id;
}

LinkMode WMBusIM871A::getLinkMode() {
    pthread_mutex_lock(&command_lock_);

    vector<uchar> msg(4);
    msg[0] = IM871A_SERIAL_SOF;
    msg[1] = DEVMGMT_ID;
    sent_command_ = msg[2] = DEVMGMT_MSG_GET_CONFIG_REQ;
    msg[3] = 0;

    verbose("(im871a) get config\n");
    serial()->send(msg);

    waitForResponse();
    LinkMode lm = UNKNOWN_LINKMODE;
    if (received_command_ == DEVMGMT_MSG_GET_CONFIG_RSP) {
        int iff1 = received_payload_[0];
        bool has_device_mode = (iff1&1)==1;
        bool has_link_mode = (iff1&2)==2;
        bool has_wmbus_c_field = (iff1&4)==4;
        bool has_wmbus_man_id = (iff1&8)==8;
        bool has_wmbus_device_id = (iff1&16)==16;
        bool has_wmbus_version = (iff1&32)==32;
        bool has_wmbus_device_type = (iff1&64)==64;
        bool has_radio_channel = (iff1&128)==128;

        int offset = 1;
        if (has_device_mode) {
            verbose("(im871a) config: device mode %02x\n", received_payload_[offset]);
            offset++;
        }
        if (has_link_mode) {
            verbose("(im871a) config: link mode %02x\n", received_payload_[offset]);
            if (received_payload_[offset] == im871a_C1a) {
                lm = LinkModeC1;
            }
            if (received_payload_[offset] == im871a_T1) {
                lm = LinkModeT1;
            }
            offset++;
        }
        if (has_wmbus_c_field) {
            verbose("(im871a) config: wmbus c-field %02x\n", received_payload_[offset]);
            offset++;
        }
        if (has_wmbus_man_id) {
            int flagid = 256*received_payload_[offset+1] +received_payload_[offset+0];
            string flag = manufacturerFlag(flagid);
            verbose("(im871a) config: wmbus mfg id %02x%02x (%s)\n", received_payload_[offset+1], received_payload_[offset+0],
                    flag.c_str());
            offset+=2;
        }
        if (has_wmbus_device_id) {
            verbose("(im871a) config: wmbus device id %02x%02x%02x%02x\n", received_payload_[offset+3], received_payload_[offset+2],
                    received_payload_[offset+1], received_payload_[offset+0]);
            offset+=4;
        }
        if (has_wmbus_version) {
            verbose("(im871a) config: wmbus version %02x\n", received_payload_[offset]);
            offset++;
        }
        if (has_wmbus_device_type) {
            verbose("(im871a) config: wmbus device type %02x\n", received_payload_[offset]);
            offset++;
        }
        if (has_radio_channel) {
            verbose("(im871a) config: radio channel %02x\n", received_payload_[offset]);
            offset++;
        }
        int iff2 = received_payload_[offset];
        offset++;
        bool has_radio_power_level = (iff2&1)==1;
        bool has_radio_data_rate = (iff2&2)==2;
        bool has_radio_rx_window = (iff2&4)==4;
        bool has_auto_power_saving = (iff2&8)==8;
        bool has_auto_rssi_attachment = (iff2&16)==16;
        bool has_auto_rx_timestamp_attachment = (iff2&32)==32;
        bool has_led_control = (iff2&64)==64;
        bool has_rtc_control = (iff2&128)==128;
        if (has_radio_power_level) {
            verbose("(im871a) config: radio power level %02x\n", received_payload_[offset]);
            offset++;
        }
        if (has_radio_data_rate) {
            verbose("(im871a) config: radio data rate %02x\n", received_payload_[offset]);
            offset++;
        }
        if (has_radio_rx_window) {
            verbose("(im871a) config: radio rx window %02x\n", received_payload_[offset]);
            offset++;
        }
        if (has_auto_power_saving) {
            verbose("(im871a) config: auto power saving %02x\n", received_payload_[offset]);
            offset++;
        }
        if (has_auto_rssi_attachment) {
            verbose("(im871a) config: auto RSSI attachment %02x\n", received_payload_[offset]);
            offset++;
        }
        if (has_auto_rx_timestamp_attachment) {
            verbose("(im871a) config: auto rx timestamp attachment %02x\n", received_payload_[offset]);
            offset++;
        }
        if (has_led_control) {
            verbose("(im871a) config: led control %02x\n", received_payload_[offset]);
            offset++;
        }
        if (has_rtc_control) {
            verbose("(im871a) config: rtc control %02x\n", received_payload_[offset]);
            offset++;
        }
    }

    pthread_mutex_unlock(&command_lock_);
    return lm;
}

void WMBusIM871A::setLinkMode(LinkMode lm)
{
    if (lm != LinkModeC1 && lm != LinkModeT1) {
        error("LinkMode %d is not implemented\n", (int)lm);
    }
    pthread_mutex_lock(&command_lock_);

    vector<uchar> msg(10);
    msg[0] = IM871A_SERIAL_SOF;
    msg[1] = DEVMGMT_ID;
    sent_command_ = msg[2] = DEVMGMT_MSG_SET_CONFIG_REQ;
    msg[3] = 6; // Len
    msg[4] = 0; // Temporary
    msg[5] = 2; // iff1 bits: Set Radio Mode
    if (lm == LinkModeC1) {
        msg[6] = (int)im871a_C1a;
    } else {
        msg[6] = (int)im871a_T1;
    }
    msg[7] = 16+32; // iff2 bits: Set rssi+timestamp
    msg[8] = 1;  // Enable rssi
    msg[9] = 1;  // Enable timestamp

    verbose("(im871a) set link mode %02x\n", msg[6]);
    serial()->send(msg);

    waitForResponse();
    pthread_mutex_unlock(&command_lock_);
}

void WMBusIM871A::onTelegram(function<void(Telegram*)> cb) {
    telegram_listeners_.push_back(cb);
}

void WMBusIM871A::waitForResponse() {
    while (manager_->isRunning()) {
        int rc = sem_wait(&command_wait_);
        if (rc==0) break;
        if (rc==-1) {
            if (errno==EINTR) continue;
            break;
        }
    }
}

FrameStatus WMBusIM871A::checkFrame(vector<uchar> &data,
                                    size_t *frame_length, int *endpoint_out, int *msgid_out,
                                    int *payload_len_out, int *payload_offset)
{
    if (data.size() == 0) return PartialFrame;
    if (data[0] != 0xa5) return ErrorInFrame;

    int ctrlbits = (data[1] & 0xf0) >> 4;
    if (ctrlbits & 1) return ErrorInFrame; // Bit 1 is reserved, we do not expect it....
    bool has_timestamp = ((ctrlbits&2)==2);
    bool has_rssi = ((ctrlbits&4)==4);
    bool has_crc16 = ((ctrlbits&8)==8);
    int endpoint = data[1] & 0x0f;
    if (endpoint != DEVMGMT_ID &&
        endpoint != RADIOLINK_ID &&
        endpoint != RADIOLINKTEST_ID &&
        endpoint != HWTEST_ID) return ErrorInFrame;
    *endpoint_out = endpoint;

    int msgid = data[2];
    if (endpoint == DEVMGMT_ID && (msgid<1 || msgid>0x27)) return ErrorInFrame;
    if (endpoint == RADIOLINK_ID && (msgid<1 || msgid>0x05)) return ErrorInFrame;
    if (endpoint == RADIOLINKTEST_ID && (msgid<1 || msgid>0x07)) return ErrorInFrame; // Are 5 and 6 disallowed?
    if (endpoint == HWTEST_ID && (msgid<1 || msgid>0x02)) return ErrorInFrame;
    *msgid_out = msgid;

    int payload_len = data[3];
    *payload_len_out = payload_len;
    *payload_offset = 4;

    *frame_length = *payload_offset+payload_len+(has_timestamp?4:0)+(has_rssi?1:0)+(has_crc16?2:0);
    if (data.size() < *frame_length) return PartialFrame;

    int i = *payload_offset + payload_len;
    if (has_timestamp) {
        uint32_t a = data[i];
        uint32_t b = data[i+1];
        uint32_t c = data[i+2];
        uint32_t d = data[i+3];

        uint32_t ts = a+b*256+c*256*256+d*256*256*256;
        debug("(im871a) timestamp %08x\n", ts);
        i += 4;
    }
    if (has_rssi) {
        uint32_t rssi = data[i];
        verbose("(im871a) rssi %02x\n", rssi);
        i++;
    }
    if (has_crc16) {
        uint32_t a = data[i];
        uint32_t b = data[i+1];
        uint32_t crc16 = a+b*256;
        i+=2;
        uint16_t gotcrc = ~crc16_CCITT(&data[1], i-1-2);
        bool crcok = crc16_CCITT_check(&data[1], i-1);
        debug("(im871a) got crc16 %04x expected %04x\n", crc16, gotcrc);
        if (!crcok) {
            warning("(im871a) warning: got wrong crc %04x expected %04x\n", gotcrc, crc16);
        }
    }

    return FullFrame;
}

void WMBusIM871A::processSerialData()
{
    vector<uchar> data;

    // Receive and accumulated serial data until a full frame has been received.
    serial_->receive(&data);

    read_buffer_.insert(read_buffer_.end(), data.begin(), data.end());

    size_t frame_length;
    int endpoint;
    int msgid;
    int payload_len, payload_offset;

    FrameStatus status = checkFrame(read_buffer_, &frame_length, &endpoint, &msgid, &payload_len, &payload_offset);

    if (status == ErrorInFrame) {
        verbose("(im871a) protocol error in message received!\n");
        string msg = bin2hex(read_buffer_);
        debug("(im871a) protocol error \"%s\"\n", msg.c_str());
        read_buffer_.clear();
    }
    else
    if (status == FullFrame) {

        vector<uchar> payload;
        if (payload_len > 0)
        {
            if (endpoint == RADIOLINK_ID &&
                msgid == RADIOLINK_MSG_WMBUSMSG_IND)
            {
                uchar l = payload_len;
                payload.insert(payload.begin(), &l, &l+1); // Re-insert the len byte.
            }
            // Insert the payload.
            payload.insert(payload.end(),
                           read_buffer_.begin()+payload_offset,
                           read_buffer_.begin()+payload_offset+payload_len);
        }

        read_buffer_.erase(read_buffer_.begin(), read_buffer_.begin()+frame_length);

        // We now have a proper message in payload. Let us trigger actions based on it.
        // It can be wmbus receiver-dongle messages or wmbus remote meter messages received over the radio.
        switch (endpoint) {
        case DEVMGMT_ID: handleDevMgmt(msgid, payload); break;
        case RADIOLINK_ID: handleRadioLink(msgid, payload); break;
        case RADIOLINKTEST_ID: handleRadioLinkTest(msgid, payload); break;
        case HWTEST_ID: handleHWTest(msgid, payload); break;
        }
    }
}

void WMBusIM871A::handleDevMgmt(int msgid, vector<uchar> &payload)
{
    switch (msgid) {
        case DEVMGMT_MSG_PING_RSP: // 0x02
            verbose("(im871a) pong\n");
            received_command_ = msgid;
            sem_post(&command_wait_);
            break;
        case DEVMGMT_MSG_SET_CONFIG_RSP: // 0x04
            verbose("(im871a) set config completed\n");
            received_command_ = msgid;
            received_payload_.clear();
            received_payload_.insert(received_payload_.end(), payload.begin(), payload.end());
            sem_post(&command_wait_);
            break;
        case DEVMGMT_MSG_GET_CONFIG_RSP: // 0x06
            verbose("(im871a) get config completed\n");
            received_command_ = msgid;
            received_payload_.clear();
            received_payload_.insert(received_payload_.end(), payload.begin(), payload.end());
            sem_post(&command_wait_);
            break;
        case DEVMGMT_MSG_GET_DEVICEINFO_RSP: // 0x10
            verbose("(im871a) device info completed\n");
            received_command_ = msgid;
            received_payload_.clear();
            received_payload_.insert(received_payload_.end(), payload.begin(), payload.end());
            sem_post(&command_wait_);
            break;
    default:
        verbose("(im871a) Unhandled device management message %d\n", msgid);
    }
}

void WMBusIM871A::handleRadioLink(int msgid, vector<uchar> &payload)
{
    switch (msgid) {
        case RADIOLINK_MSG_WMBUSMSG_IND: // 0x03
            {
                Telegram t;
                t.parse(payload);

                for (auto f : telegram_listeners_) {
                    if (f) f(&t);
                }
                if (isVerboseEnabled() && !t.handled) {
                    verbose("(im871a) telegram ignored by all configured meters!\n");
                }
            }
            break;
    default:
        verbose("(im871a) Unhandled radio link message %d\n", msgid);
    }
}

void WMBusIM871A::handleRadioLinkTest(int msgid, vector<uchar> &payload)
{
    switch (msgid) {
    default:
        verbose("(im871a) Unhandled radio link test message %d\n", msgid);
    }
}

void WMBusIM871A::handleHWTest(int msgid, vector<uchar> &payload)
{
    switch (msgid) {
    default:
        verbose("(im871a) Unhandled hw test message %d\n", msgid);
    }
}

bool detectIM871A(string device, SerialCommunicationManager *manager)
{
    // Talk to the device and expect a very specific answer.
    auto serial = manager->createSerialDeviceTTY(device.c_str(), 57600);
    bool ok = serial->open(false);
    if (!ok) return false;

    vector<uchar> data;
    // First clear out any data in the queue.
    serial->receive(&data);
    data.clear();

    vector<uchar> msg(4);
    msg[0] = IM871A_SERIAL_SOF;
    msg[1] = DEVMGMT_ID;
    msg[2] = DEVMGMT_MSG_PING_REQ;
    msg[3] = 0;

    verbose("(im871a) are you there?\n");
    serial->send(msg);
    // Wait for 100ms so that the USB stick have time to prepare a response.
    usleep(1000*100);
    serial->receive(&data);

    serial->close();

    string sent = bin2hex(msg);
    string recv = bin2hex(data);

    size_t frame_length;
    int endpoint, msgid, payload_len, payload_offset;
    FrameStatus status = WMBusIM871A::checkFrame(data,
                                                 &frame_length, &endpoint, &msgid,
                                                 &payload_len, &payload_offset);
    if (status != FullFrame ||
        endpoint != 1 ||
        msgid != 2)
    {
        return false;
    }
    return true;
}