canopen.h

#pragma once
#include "can.h"
#include <utils/deadline.h>
#include <core/kern.h>
 
 
namespace CAN {
namespace Open {
enum class NMT : uint8_t {
    START = 1,          
    STOP = 2,           
    PREOP = 0x80,       
    RESET_APP = 0x81,   
    RESET_COMM = 0x82,  
};
enum class STATE : uint8_t {
    STOPPED = 4,        
    OPERATIONAL = 5,    
    PREOP = 0x7f,       
};
struct SDO {
    uint32_t data;  
    uint16_t ix;    
    uint8_t sub;    
    uint8_t cmd;    
    uint8_t nodeID; 
 
    Message toMessage() {
        return {
            .data = cmd | ix << 8 | sub << 24 | (uint64_t)data << 32,
            .id = (uint32_t)0x600 + nodeID,
            .opts = { .rtr = 0, .ide = 0, .dlc = 8},
        };
    }
    static SDO fromMessage(Message msg) {
        return {
            .data = (uint32_t)(msg.data >> 32),
            .ix = (uint16_t)(msg.data >> 8),
            .sub = (uint8_t)(msg.data >> 24),
            .cmd = (uint8_t)msg.data,
            .nodeID = (uint8_t)(msg.id % 0x80),
        };
    }
};
struct PDOMap {
    uint16_t ix;    
    uint8_t sub;    
    uint8_t len;    
    int32_t data;   
};
enum PDOType : uint8_t {
    SYNC,       
    CYCLIC=254, 
    CHANGE=255, 
};
struct RPDO {
    uint8_t N;              
    PDOType type;           
    uint32_t COB {};        
    Buffer<PDOMap> map;     
    Message toMessage() const {
        uint64_t data = 0;
        uint8_t shift = 0;
        for (auto &i : map) {
            data |= (i.data & (((uint64_t)1 << i.len)-1)) << shift;
            shift += i.len;
        }
        return {.data=data, .id = COB, .opts = {.dlc=(uint8_t)(shift / 8)}};
    }
};
struct TPDO {
    uint8_t N;              
    PDOType type;           
    uint32_t COB {};        
    uint32_t inhibit_time;  
    Buffer<PDOMap> map;     
    void receive(uint64_t d) {
        uint8_t shift = 0;
        for (auto &i : map) {
            i.data = d >> shift & (((uint64_t)1 << i.len) - 1);
            shift += i.len;
        }
    }
};
 
struct Dispatch : Sink<SDO>, Sink<Message> {
    enum LOGLEVEL { NONE, WARN, INFO} log;
    Dispatch(CAN &can, LOGLEVEL=NONE) : can(can) { }
    CAN &can;
    Sink<SDO> *ids[128] {};
    struct PDO {
        Sink<TPDO> *dev[8] {};
        TPDO *pdo[8] {};
        uint8_t n {};
    } pdo;
 
    void nmt(NMT command, uint8_t nodeid) {
        can.trypush({.data = (uint64_t)(nodeid & 0x7f) << 8 | (uint8_t)command,
                .id = 0, .opts = {.dlc = 2}});
    }
    void sync() {
        can.trypush({.id = 0x80, .opts = {.dlc = 0}});
    }
    void heartbeat(uint8_t id, STATE state) {
        can.trypush({.data = (uint8_t)state, .id = (uint32_t)0x700 + id, .opts = {.dlc = 1}});
    }
    void guard(uint8_t id) {
        can.trypush({.id = (uint32_t)0x700 + id, .opts = {.rtr = true}});
    }
    void process() {
        // TODO: what to do to CAN messages that _aren't_ CanOpen
        while (!can.empty()) {
            auto msg = can.pop();
            if (handlePDO(msg)) continue;
            if (handleSDO(msg)) continue;
            // don't know what to do with received message!
            if (log > NONE) k.log.warn("unhandled: [id: %x] [%02x %02x %02x %02x %02x %02x %02x %02x]\n",
                    msg.id,
                    (uint8_t)(msg.data),
                    (uint8_t)(msg.data>>8),
                    (uint8_t)(msg.data>>16),
                    (uint8_t)(msg.data>>24),
                    (uint8_t)(msg.data>>32),
                    (uint8_t)(msg.data>>40),
                    (uint8_t)(msg.data>>48),
                    (uint8_t)(msg.data>>56)
                    );
        }
    }
 
    bool full() override {
        return can.full();
    }
    using Sink<SDO>::push;
    void push(SDO &&rq) override {
        //assert(ids[rq.nodeID] != nullptr); //must register first!
        if (ids[rq.nodeID] == nullptr) {
            if (log > NONE) k.log.warn("ID [0x%x] NOT registered!\n", rq.nodeID);
            return;
        }
        can.push(rq.toMessage());
    }
    using Sink<Message>::push;
    void push(Message &&rq) override { can.push(rq); }
 
    void registerSDO(uint16_t nodeID, Sink<SDO> *dev) {
        assert(nodeID != 0);
        assert(ids[nodeID] == nullptr);
        ids[nodeID] = dev;
    }
    void registerPDO(TPDO *tpdo, Sink<TPDO> *dev) {
        for (uint8_t i = 0; i < pdo.n; i++) {
            if (pdo.pdo[i] == tpdo) {
                assert(pdo.dev[i] == dev);
                return;
            }
        }
        assert ( pdo.n < 8 );
        pdo.dev[pdo.n] = dev;
        pdo.pdo[pdo.n] = tpdo;
        pdo.n += 1;
    }
 
    bool handlePDO(Message msg) {
        for (int i = 0; i < pdo.n; ++i) {
            auto tpdo = pdo.pdo[i];
            if (tpdo->COB == msg.id
                    && msg.opts.dlc) {
                tpdo->receive(msg.data);
                pdo.dev[i]->push(*tpdo);
                if (log > WARN) {
                    k.log.info("handled PDO id: %x\n", msg.id);
                    k.log.info("          data: %x\n", msg.data);
                    k.log.info("    TPDO data0: %x\n",tpdo->map[0].data);
                    k.log.info("    TPDO data1: %x\n",tpdo->map[1].data);
                }
                return true;
            }
        }
        return false;
    }
    bool handleSDO(Message msg) {
        uint16_t service = msg.id & ~0x7f;
        uint8_t id = msg.id & 0x7f;
        if (id == 0) return false;
        if (//service == 0x80 ||  // SYNC
            service == 0x580 || // SDO server-to-client
            //service == 0x600 || // SDO client-to-server
            //service == 0x700 || // NMT control
            false) {
            if (ids[id]) {
                ids[id]->push(SDO::fromMessage(msg));
            }
            if (log > WARN) k.log.info("handled SDO id: %x\n", msg.id);
            return true;
        }
        return false;
    }
};
 
struct Device : Sink<TPDO>, Sink<SDO> {
    Device(Dispatch &canopen, uint8_t id) : id(id), out(canopen)
    {
        assert(id > 0 && id < 128);
        canopen.registerSDO(id, this);
    }
    uint8_t const id {};    
    Dispatch &out;
 
    struct State {
        Queue<SDO> q{60};
        Deadline next{};
    } state;
    virtual void callback(SDO rq) { assert(false); };
    virtual void callback(TPDO rq) { assert(false); };
    void read(uint16_t ix, uint8_t sub) {
        pushorqueue({.ix=ix, .sub=sub, .cmd=0x40, .nodeID=id});
    }
    void w8(uint16_t ix, uint8_t sub, uint8_t val) {
        pushorqueue({.data=(uint32_t)val, .ix=ix, .sub=sub, .cmd=0x2f, .nodeID=id});
    }
    void w16(uint16_t ix, uint8_t sub, uint16_t val) {
        pushorqueue({.data=(uint32_t)val, .ix=ix, .sub=sub, .cmd=0x2b, .nodeID=id});
    }
    void w32(uint16_t ix, uint8_t sub, uint32_t val) {
        pushorqueue({.data=(uint32_t)val, .ix=ix, .sub=sub, .cmd=0x23, .nodeID=id});
    }
    void wPDO(const RPDO &rpdo) {
        (*(Sink<Message>*)&out).trypush(rpdo.toMessage());
    }
    void enablePDO(RPDO &pdo) {
        if (pdo.COB == 0) pdo.COB = id + 0x100 * (pdo.N+1);
        disablePDO(pdo);
        w8(0x1400+pdo.N-1, 0x2, pdo.type); // set transmission type
        w8(0x1600+pdo.N-1, 0x0, 0); // clear number of mapped objects
        for (size_t i = 0; i < pdo.map.len; ++i) {
            auto &d = pdo.map[i];
            w32(0x1600+pdo.N-1, i+1, d.ix << 16 | d.sub << 8 | d.len);
        }
        w8(0x1600+pdo.N-1, 0x0, pdo.map.len); // set number of mapped objects
        w32(0x1400+pdo.N-1, 0x1, 1<<30 | pdo.COB);
    }
    void disablePDO(RPDO &pdo) {
        w32(0x1400+pdo.N-1, 0x1, 1<<31); // clear
    }
    void enablePDO(TPDO &pdo) {
        if (pdo.COB == 0) pdo.COB = id + 0x80 + 0x100 * pdo.N;
        disablePDO(pdo);
        w8(0x1800+pdo.N-1, 0x2, pdo.type);
        w16(0x1800+pdo.N-1, 0x3, pdo.inhibit_time);
        /* w16(0x1800+pdo.N-1, 0x5, pdo.event_timer); */
        w8(0x1a00+pdo.N-1, 0x0, 0); // clear number of mapped objects
        for (size_t i = 0; i < pdo.map.len; ++i) {
            auto &d = pdo.map[i];
            w32(0x1a00+pdo.N-1, i+1, d.ix << 16 | d.sub << 8 | d.len);
        }
        w8(0x1a00+pdo.N-1, 0x0, pdo.map.len); // set number of mapped objects
        w32(0x1800+pdo.N-1, 0x1, 1<<30 | pdo.COB); // enable PDO
        out.registerPDO(&pdo, this);
    };
    void disablePDO(TPDO &pdo) {
        w32(0x1800+pdo.N-1, 0x1, 1<<31); // clear
    };
    bool full() override {
        return (*(Sink<Message>*)&out).full();
    }
    using Sink<SDO>::push;
    void push(SDO &&sdo) override {
        state.next = {0};
        callback(std::move(sdo));
        process();
    }
    using Sink<TPDO>::push;
    void push(TPDO &&pdo) override {
        callback(std::move(pdo));
        process();
    }
    void process() {
        if (state.q.empty()) return;
        if (state.next.when && !state.next(k.time)) return;
        out.push(state.q.pop());
        state.next = {k.time + 2};
    }
    void pushorqueue(SDO &&sdo) {
        if (state.next.when && !state.next(k.time)) {
            state.q.trypush(std::move(sdo));
        } else {
            (*(Sink<SDO>*)&out).trypush(std::move(sdo));
            state.next = {k.time + 2};
        }
    }
};
}
}