1#define LOG_TAG "NanohubHAL_Test"
2
3#include <cstddef>
4#include <cstdint>
5#include <functional>
6#include <iostream>
7#include <iomanip>
8#include <map>
9#include <memory>
10#include <cstddef>
11#include <cstdint>
12#include <mutex>
13#include <vector>
14
15#include <dlfcn.h>
16#include <signal.h>
17#include <unistd.h>
18
19#include <sys/endian.h>
20#include <utils/Log.h>
21
22#include <hardware/hardware.h>
23#include <hardware/context_hub.h>
24#include <nanohub/nanoapp.h>
25
26inline std::ostream &operator << (std::ostream &os, const hub_app_name_t &appId)
27{
28    char vendor[6];
29    __be64 beAppId = htobe64(appId.id);
30    uint32_t seqId = appId.id & NANOAPP_VENDOR_ALL_APPS;
31
32    std::ios::fmtflags f(os.flags());
33    memcpy(vendor, (void*)&beAppId, sizeof(vendor) - 1);
34    vendor[sizeof(vendor) - 1] = 0;
35    if (strlen(vendor) == 5)
36        os << vendor << ", " << std::hex << std::setw(6)  << seqId;
37    else
38        os << "#" << std::hex << appId.id;
39    os.flags(f);
40
41    return os;
42}
43
44void dumpBuffer(std::ostream &os, const char *pfx, const hub_app_name_t &appId, uint32_t evtId, const void *data, size_t len, int status)
45{
46    const uint8_t *p = static_cast<const uint8_t *>(data);
47    os << pfx << ": [ID=" << appId << "; SZ=" << std::dec << len;
48    if (evtId)
49        os << "; EVT=" << std::hex << evtId;
50    os << "]:" << std::hex;
51    for (size_t i = 0; i < len; ++i) {
52        os << " "  << std::setfill('0') << std::setw(2) << (unsigned int)p[i];
53    }
54    if (status) {
55        os << "; status=" << status << " [" << std::setfill('0') << std::setw(8) << status << "]";
56    }
57}
58
59class CHub
60{
61public:
62    class IClient {
63    public:
64        virtual void onMessage(const hub_message_t &msg) = 0;
65        virtual ~IClient(){}
66    };
67    class Client : IClient {
68        CHub *mParent;
69        const context_hub_t *mHub;
70        std::function<void(const hub_message_t &)> mHandler;
71
72    public:
73        explicit Client(const context_hub_t *hub, CHub *parent) {
74            mHub = hub;
75            mParent = parent;
76        }
77        ~Client() = default;
78
79        void setHandler(std::function<void(const hub_message_t &)> handler) {
80            mHandler = handler;
81        }
82        void onMessage(const hub_message_t &msg) {
83            if ((bool)mHandler == true) {
84                mHandler(msg);
85            }
86        }
87        void sendMessage(const hub_message_t &msg) {
88            mParent->sendMessage(mHub->hub_id, msg);
89        }
90        void sendToSystem(uint32_t typ, void *data, uint32_t len) {
91            mParent->sendMessage(mHub->hub_id, mHub->os_app_name, typ, data, len);
92        }
93        void sendToApp(hub_app_name_t app, void *data, uint32_t len) {
94            mParent->sendMessage(mHub->hub_id, app, 0, data, len);
95        }
96        const hub_app_name_t getSystemApp() const { return mHub->os_app_name; }
97    };
98private:
99    static int contextHubCallback(uint32_t id, const hub_message_t *msg, void *cookie)
100    {
101        CHub *hub = static_cast<CHub*>(cookie);
102        hub->onMessage(id, msg);
103        return 0;
104    }
105
106    CHub() {
107        hw_get_module(CONTEXT_HUB_MODULE_ID, (const hw_module_t **)&mMod);
108        if (!mMod)
109            return;
110        mMod->subscribe_messages(0, contextHubCallback, this);
111        mHubArraySize = mMod->get_hubs(mMod, &mHubArray);
112        for (size_t i = 0; i < mHubArraySize; ++i) {
113            auto item = &mHubArray[i];
114            mHubs[item->hub_id] = std::unique_ptr<Client>(new Client(item, this));
115        }
116    }
117
118    ~CHub() {
119        // destroy all clients first
120        mHubs.clear();
121        if (mMod != nullptr) {
122            // unregister from HAL services
123            mMod->subscribe_messages(0, nullptr, nullptr);
124            // there is no hw_put_module(); release HAL fd directly
125            dlclose(mMod->common.dso);
126            mMod = nullptr;
127        }
128    }
129
130    void onMessage(uint32_t hubId, const hub_message_t *msg) {
131        Client *cli = getClientById(hubId);
132        if (cli != nullptr && msg != nullptr) {
133            cli->onMessage(*msg);
134        }
135    }
136
137    int sendMessage(uint32_t id, const hub_message_t &msg) {
138        return  (mMod != nullptr) ? mMod->send_message(id, &msg) : 0;
139    }
140
141    int sendMessage(uint32_t id, hub_app_name_t app, uint32_t typ, void *data, uint32_t len) {
142        hub_message_t msg = {
143            .app_name = app,
144            .message_type = typ,
145            .message = data,
146            .message_len = len,
147        };
148        return sendMessage(id, msg);
149    }
150
151    Client *getClientById(size_t id) { return mHubs.count(id) ? mHubs[id].get() : nullptr; }
152
153    context_hub_module_t *mMod = nullptr;
154    const context_hub_t  *mHubArray = nullptr;
155    size_t                mHubArraySize = 0;
156    std::map <size_t, std::unique_ptr<Client> > mHubs;
157
158public:
159    static CHub *instantiate() {
160        static CHub instance;
161
162        return &instance;
163    }
164    Client *getClientByIndex(size_t idx) {
165        return idx < mHubArraySize && mHubArray != nullptr ?
166               getClientById(mHubArray[idx].hub_id) : nullptr;
167    }
168};
169
170class NanoClient
171{
172    CHub::Client *mClient;
173    std::ostream &log;
174    std::mutex lock;
175    void onMessage(const hub_message_t &msg){
176        std::lock_guard<std::mutex> _l(lock);
177        dumpBuffer(log, "Rx", msg.app_name, msg.message_type, msg.message, msg.message_len, 0);
178        log << std::endl;
179    }
180public:
181    NanoClient(int idx = 0) : log(std::clog) {
182        CHub *hub = CHub::instantiate();
183        mClient = hub->getClientByIndex(idx);
184        if (mClient)
185            mClient->setHandler(std::function<void(const hub_message_t&)>([this] (const hub_message_t&msg) { onMessage(msg); }));
186    }
187    void sendMessage(const hub_message_t &msg) { mClient->sendMessage(msg); }
188    void sendMessageToSystem(uint32_t cmd, void * data, size_t dataSize) {
189        hub_message_t msg;
190        msg.message = data;
191        msg.message_len = dataSize;
192        msg.message_type = cmd;
193        msg.app_name = mClient->getSystemApp();
194        {
195            std::lock_guard<std::mutex> _l(lock);
196            dumpBuffer(log, "TxCmd", msg.app_name, msg.message_type, msg.message, msg.message_len, 0);
197            log << std::endl;
198        }
199        sendMessage(msg);
200    }
201    void sendMessageToApp(const hub_app_name_t appName, void * data, size_t dataSize, uint32_t msg_type) {
202        hub_message_t msg;
203        msg.message = data;
204        msg.message_len = dataSize;
205        msg.message_type = msg_type;
206        msg.app_name = appName;
207        {
208            std::lock_guard<std::mutex> _l(lock);
209            dumpBuffer(log, "TxMsg", msg.app_name, msg.message_type, msg.message, msg.message_len, 0);
210            log << std::endl;
211        }
212        sendMessage(msg);
213    }
214};
215
216void sigint_handler(int)
217{
218    exit(0);
219}
220
221int main(int argc, char *argv[])
222{
223    int opt;
224    long cmd = 0;
225    unsigned long msg = 0;
226    uint64_t appId = 0;
227    const char *appFileName = NULL;
228    uint32_t fileSize = 0;
229
230    while((opt = getopt(argc, argv, "c:i:a:m:")) != -1) {
231        char *end = NULL;
232        switch(opt) {
233        case 'm':
234            msg = strtoul(optarg, &end, 16);
235            break;
236        case 'c':
237            cmd = strtol(optarg, &end, 10);
238            break;
239        case 'i':
240            appId = strtoull(optarg, &end, 16);
241            break;
242        case 'a':
243            appFileName = optarg;
244            break;
245        }
246        if (end && *end != '\0') {
247            std::clog << "Invalid argument: " << optarg << std::endl;
248            return 1;
249        }
250    }
251
252    NanoClient cli;
253
254    std::vector<uint8_t> data;
255    for (int i = optind; i < argc; ++i) {
256        char *end;
257        unsigned long v = strtoul(argv[i], &end, 16);
258        // ignore any garbage after parsed hex value;
259        // ignore the fact it may not fit 1 byte;
260        // we're not testing user's ability to pass valid data,
261        // we're testing the system ability to transfer data.
262        data.push_back(v);
263    }
264    if (msg != 0) {
265        // send APP message
266        const hub_app_name_t app_name = { .id = appId };
267        cli.sendMessageToApp(app_name, data.data(), data.size(), msg);
268    } else {
269        // send HAL command
270        switch(cmd) {
271        case CONTEXT_HUB_APPS_ENABLE:
272        {
273            apps_enable_request_t req;
274            req.app_name.id = appId;
275            cli.sendMessageToSystem(CONTEXT_HUB_APPS_ENABLE, &req, sizeof(req));
276        }
277        break;
278        case CONTEXT_HUB_APPS_DISABLE:
279        {
280            apps_disable_request_t req;
281            req.app_name.id = appId;
282            cli.sendMessageToSystem(CONTEXT_HUB_APPS_DISABLE, &req, sizeof(req));
283        }
284        break;
285        case CONTEXT_HUB_LOAD_APP:
286        {
287            load_app_request_t *req = NULL;
288            if (appFileName)
289                req = (load_app_request_t *)loadFile(appFileName, &fileSize);
290            if (!req || fileSize < sizeof(*req) || req->app_binary.magic != NANOAPP_MAGIC) {
291                std::clog << "Invalid nanoapp image: " <<
292                             (appFileName != nullptr ? appFileName : "<NULL>") << std::endl;
293                return 1;
294            }
295            cli.sendMessageToSystem(CONTEXT_HUB_LOAD_APP, req, fileSize);
296            free(req);
297        }
298        break;
299        case CONTEXT_HUB_UNLOAD_APP:
300        {
301            unload_app_request_t req;
302            req.app_name.id = appId;
303            cli.sendMessageToSystem(CONTEXT_HUB_UNLOAD_APP, &req, sizeof(req));
304        }
305        break;
306        case CONTEXT_HUB_QUERY_APPS:
307        {
308            query_apps_request_t req;
309            req.app_name.id = appId;
310            cli.sendMessageToSystem(CONTEXT_HUB_QUERY_APPS, &req, sizeof(req));
311        }
312        break;
313        case CONTEXT_HUB_QUERY_MEMORY:
314        default:
315            std::clog << "Unknown command: " << cmd << std::endl;
316            break;
317        }
318    }
319
320    signal(SIGINT, sigint_handler);
321    while(1) {
322        sleep(1);
323    }
324    return 0;
325}
326