skya_data_plane.c
drivers/src/skya_data_plane.c · 21 line(s) · c
#if !defined(_WIN32)
#define _POSIX_C_SOURCE 200809L /* clock_gettime and other POSIX.1-2008 symbols */
#endif
#include "../include/skya_data_plane.h"
#include <errno.h>
#include <pthread.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
typedef struct{skya_data_frame h;uint8_t*p;} slot;
struct skya_data_plane{pthread_mutex_t lock;pthread_cond_t ready;slot*s;size_t cap,head,tail,count;uint64_t next;int running;};
static uint64_t now_ns(void){struct timespec t;clock_gettime(CLOCK_MONOTONIC,&t);return(uint64_t)t.tv_sec*1000000000ull+(uint64_t)t.tv_nsec;}
static uint32_t crc32b(const void*d,size_t n){uint32_t c=0xffffffffu;const uint8_t*p=d;for(size_t i=0;i<n;i++){c^=p[i];for(unsigned b=0;b<8;b++)c=(c>>1)^(0xedb88320u&-(int)(c&1u));}return~c;}
int skya_data_plane_init(skya_data_plane**out,size_t cap){if(!out||cap<2)return-EINVAL;skya_data_plane*p=calloc(1,sizeof*p);if(!p)return-ENOMEM;p->s=calloc(cap,sizeof*p->s);if(!p->s){free(p);return-ENOMEM;}for(size_t i=0;i<cap;i++){p->s[i].p=malloc(SKYA_DATA_MAX_PAYLOAD);if(!p->s[i].p){for(size_t j=0;j<i;j++)free(p->s[j].p);free(p->s);free(p);return-ENOMEM;}}p->cap=cap;p->next=1;pthread_mutex_init(&p->lock,0);pthread_cond_init(&p->ready,0);*out=p;return 0;}
void skya_data_plane_destroy(skya_data_plane*p){if(!p)return;pthread_mutex_lock(&p->lock);p->running=0;pthread_cond_broadcast(&p->ready);pthread_mutex_unlock(&p->lock);for(size_t i=0;i<p->cap;i++)free(p->s[i].p);free(p->s);pthread_cond_destroy(&p->ready);pthread_mutex_destroy(&p->lock);free(p);}
int skya_data_plane_start(skya_data_plane*p){if(!p)return-EINVAL;pthread_mutex_lock(&p->lock);p->running=1;pthread_cond_broadcast(&p->ready);pthread_mutex_unlock(&p->lock);return 0;}
int skya_data_plane_stop(skya_data_plane*p){if(!p)return-EINVAL;pthread_mutex_lock(&p->lock);p->running=0;pthread_cond_broadcast(&p->ready);pthread_mutex_unlock(&p->lock);return 0;}
int skya_data_plane_send(skya_data_plane*p,skya_data_channel ch,uint16_t flags,const void*d,size_t n,uint64_t*seq){if(!p||(!d&&n)||n>SKYA_DATA_MAX_PAYLOAD||ch<SKYA_DATA_AUDIO_IN||ch>SKYA_DATA_FIRMWARE)return-EINVAL;pthread_mutex_lock(&p->lock);if(!p->running){pthread_mutex_unlock(&p->lock);return-ESHUTDOWN;}if(p->count==p->cap){pthread_mutex_unlock(&p->lock);return-EAGAIN;}slot*s=&p->s[p->tail];if(n)memcpy(s->p,d,n);s->h=(skya_data_frame){SKYA_DATA_MAGIC,1,flags,(uint32_t)ch,p->next++,now_ns(),(uint32_t)n,crc32b(d,n)};p->tail=(p->tail+1)%p->cap;p->count++;if(seq)*seq=s->h.sequence;pthread_cond_signal(&p->ready);pthread_mutex_unlock(&p->lock);return 0;}
int skya_data_plane_receive(skya_data_plane*p,skya_data_frame*h,void*d,size_t cap,size_t*n,int wait_ms){if(!p||!h||(!d&&cap)||!n)return-EINVAL;pthread_mutex_lock(&p->lock);while(!p->count&&p->running){if(wait_ms<0)pthread_cond_wait(&p->ready,&p->lock);else{struct timespec t;clock_gettime(CLOCK_REALTIME,&t);t.tv_sec+=wait_ms/1000;t.tv_nsec+=(long)(wait_ms%1000)*1000000L;if(t.tv_nsec>=1000000000L){t.tv_sec++;t.tv_nsec-=1000000000L;}if(pthread_cond_timedwait(&p->ready,&p->lock,&t)==ETIMEDOUT){pthread_mutex_unlock(&p->lock);return 1;}}}if(!p->count){pthread_mutex_unlock(&p->lock);return-ESHUTDOWN;}slot*s=&p->s[p->head];if(cap<s->h.length){pthread_mutex_unlock(&p->lock);return-ENOSPC;}if(s->h.magic!=SKYA_DATA_MAGIC||s->h.version!=1||crc32b(s->p,s->h.length)!=s->h.crc32){pthread_mutex_unlock(&p->lock);return-EBADMSG;}if(s->h.length)memcpy(d,s->p,s->h.length);*h=s->h;*n=s->h.length;p->head=(p->head+1)%p->cap;p->count--;pthread_mutex_unlock(&p->lock);return 0;}
size_t skya_data_plane_depth(const skya_data_plane*p){if(!p)return 0;pthread_mutex_lock((pthread_mutex_t*)&p->lock);size_t n=p->count;pthread_mutex_unlock((pthread_mutex_t*)&p->lock);return n;}