sb.c

Go to the documentation of this file.

 
#include "icarus/icarus.h"
#include "icarus/svc.h"
#include <string.h>
 
/* ============================================================================
 * ROUTE TABLE (DTCM)
 * ========================================================================= */
 
typedef struct {
    sb_msg_id_t msg_id;                      
    uint8_t     pipes[SB_MAX_SUBS_PER_MSG];  
    uint8_t     count;                        
    bool        used;                         
} sb_route_t;
 
DTCM_DATA_PRIV static sb_route_t routes[SB_MAX_ROUTES];
 
DTCM_DATA_PRIV static uint8_t route_used;
 
/* ============================================================================
 * INTERNAL HELPERS (ITCM, privileged)
 * ========================================================================= */
 
ITCM_FUNC static sb_route_t *find_route(sb_msg_id_t msg_id) {
    for (uint8_t i = 0u; i < route_used; i++) {
        if (routes[i].used && (routes[i].msg_id == msg_id)) {
            return &routes[i];
        }
    }
    return NULL;
}
 
ITCM_FUNC static sb_route_t *alloc_route(void) {
    if (route_used >= (uint8_t)SB_MAX_ROUTES) {
        return NULL;
    }
    sb_route_t *r = &routes[route_used++];
    (void)memset(r, 0, sizeof(*r));
    r->used = true;
    return r;
}
 
/* ============================================================================
 * PRIVILEGED IMPLEMENTATIONS
 * ========================================================================= */
 
ITCM_FUNC void __sb_init(void) {
    (void)memset(routes, 0, sizeof(routes));
    route_used = 0;
}
 
ITCM_FUNC bool __sb_subscribe(sb_msg_id_t msg_id, uint8_t pipe_idx) {
    sb_route_t *r = find_route(msg_id);
    if (r == NULL) {
        r = alloc_route();
        if (r == NULL) {
            return false;
        }
        r->msg_id = msg_id;
    }
 
    /* Subscriber limit */
    if (r->count >= (uint8_t)SB_MAX_SUBS_PER_MSG) {
        return false;
    }
 
    /* Reject duplicate */
    for (uint8_t i = 0u; i < r->count; i++) {
        if (r->pipes[i] == pipe_idx) {
            return false;
        }
    }
 
    r->pipes[r->count++] = pipe_idx;
    return true;
}
 
ITCM_FUNC bool __sb_unsubscribe(sb_msg_id_t msg_id, uint8_t pipe_idx) {
    sb_route_t *r = find_route(msg_id);
    if (r == NULL) {
        return false;
    }
 
    for (uint8_t i = 0u; i < r->count; i++) {
        if (r->pipes[i] == pipe_idx) {
            /* Compact: move last into this slot */
            r->pipes[i] = r->pipes[(uint8_t)(r->count - 1u)];
            r->count--;
            return true;
        }
    }
    return false;
}
 
ITCM_FUNC uint8_t __sb_publish(sb_msg_id_t msg_id, const uint8_t *data,
                                uint8_t len) {
    if ((data == NULL) || (len == 0u)) {
        return 0u;
    }
 
    sb_route_t *r = find_route(msg_id);
    if (r == NULL) {
        return 0u;
    }
 
    uint8_t delivered = 0u;
    for (uint8_t i = 0u; i < r->count; i++) {
        uint8_t pidx = r->pipes[i];
        if (__pipe_can_enqueue(pidx, len)) {
            __pipe_write_bytes(pidx, (uint8_t *)(uintptr_t)data, len);
            delivered++;
        }
    }
    return delivered;
}
 
ITCM_FUNC uint8_t __sb_subscriber_count(sb_msg_id_t msg_id) {
    sb_route_t *r = find_route(msg_id);
    return (r != NULL) ? r->count : 0u;
}
 
ITCM_FUNC uint8_t __sb_route_count(void) {
    return route_used;
}
 
/* ============================================================================
 * PUBLIC API (SVC call gates — privileged access to DTCM_PRIV data)
 * ========================================================================= */
 
void sb_init(void) {
#ifndef HOST_TEST
    __asm__ volatile ("svc %0\n" : : "I" (SVC_SB_INIT));
#else
    __sb_init();
#endif
}
 
bool sb_subscribe(sb_msg_id_t msg_id, uint8_t pipe_idx) {
#ifndef HOST_TEST
    uint32_t result;
    __asm__ volatile (
        "mov r0, %1\n"
        "mov r1, %2\n"
        "svc %3\n"
        "mov %0, r0\n"
        : "=r" (result)
        : "r" ((uint32_t)msg_id), "r" ((uint32_t)pipe_idx),
          "I" (SVC_SB_SUBSCRIBE)
        : "r0", "r1"
    );
    return (bool)result;
#else
    return __sb_subscribe(msg_id, pipe_idx);
#endif
}
 
bool sb_unsubscribe(sb_msg_id_t msg_id, uint8_t pipe_idx) {
#ifndef HOST_TEST
    uint32_t result;
    __asm__ volatile (
        "mov r0, %1\n"
        "mov r1, %2\n"
        "svc %3\n"
        "mov %0, r0\n"
        : "=r" (result)
        : "r" ((uint32_t)msg_id), "r" ((uint32_t)pipe_idx),
          "I" (SVC_SB_UNSUBSCRIBE)
        : "r0", "r1"
    );
    return (bool)result;
#else
    return __sb_unsubscribe(msg_id, pipe_idx);
#endif
}
 
uint8_t sb_publish(sb_msg_id_t msg_id, const uint8_t *data, uint8_t len) {
#ifndef HOST_TEST
    uint32_t result;
    __asm__ volatile (
        "mov r0, %1\n"
        "mov r1, %2\n"
        "mov r2, %3\n"
        "svc %4\n"
        "mov %0, r0\n"
        : "=r" (result)
        : "r" ((uint32_t)msg_id), "r" ((uint32_t)(uintptr_t)data),
          "r" ((uint32_t)len), "I" (SVC_SB_PUBLISH)
        : "r0", "r1", "r2"
    );
    return (uint8_t)result;
#else
    return __sb_publish(msg_id, data, len);
#endif
}
 
uint8_t sb_subscriber_count(sb_msg_id_t msg_id) {
#ifndef HOST_TEST
    uint32_t result;
    __asm__ volatile (
        "mov r0, %1\n"
        "svc %2\n"
        "mov %0, r0\n"
        : "=r" (result)
        : "r" ((uint32_t)msg_id), "I" (SVC_SB_SUBSCRIBER_COUNT)
        : "r0"
    );
    return (uint8_t)result;
#else
    return __sb_subscriber_count(msg_id);
#endif
}
 
uint8_t sb_route_count(void) {
#ifndef HOST_TEST
    uint32_t result;
    __asm__ volatile (
        "svc %1\n"
        "mov %0, r0\n"
        : "=r" (result)
        : "I" (SVC_SB_ROUTE_COUNT)
        : "r0"
    );
    return (uint8_t)result;
#else
    return __sb_route_count();
#endif
}