159 lines
7.4 KiB
C
159 lines
7.4 KiB
C
#include "altera_stream.h"
|
|
#include "pcan_id.h"
|
|
#include <string.h>
|
|
|
|
typedef struct {
|
|
pcan_parser_t parser;
|
|
uint64_t indices[LAS_CAPACITY], next_index;
|
|
uint16_t samples[LAS_CAPACITY];
|
|
uint8_t breaks[LAS_CAPACITY];
|
|
uint32_t head, count, period, session, received, missing, duplicates, invalid, ignored;
|
|
uint8_t device, metadata, started;
|
|
} las_context;
|
|
static uint16_t r16(const uint8_t *p) { return (uint16_t)(p[0]|((uint16_t)p[1]<<8)); }
|
|
static uint32_t r32(const uint8_t *p) { return (uint32_t)r16(p)|((uint32_t)r16(p+2)<<16); }
|
|
static void w16(uint8_t *p,uint16_t v) { p[0]=(uint8_t)v;p[1]=(uint8_t)(v>>8); }
|
|
static void w32(uint8_t *p,uint32_t v) { w16(p,(uint16_t)v);w16(p+2,(uint16_t)(v>>16)); }
|
|
uint32_t las_device_type(void) { return LAS_DEVICE_TYPE; }
|
|
uint32_t las_device_id(void) { return LAS_DEVICE_ID; }
|
|
const char *las_device_name(void) { return "Altera Logic"; }
|
|
size_t las_context_size(void) { return sizeof(las_context); }
|
|
int las_init(void *ctx,uint32_t device) {
|
|
las_context *s=(las_context *)ctx;
|
|
if(!ctx || device>15) return 0;
|
|
memset(s,0,sizeof(*s));s->device=(uint8_t)device;pcan_parser_init(&s->parser);return 1;
|
|
}
|
|
static void append(las_context *s,uint64_t index,uint16_t value,uint8_t gap) {
|
|
s->indices[s->head]=index;s->samples[s->head]=value;s->breaks[s->head]=gap;
|
|
s->head=(s->head+1U)%LAS_CAPACITY;
|
|
if(s->count<LAS_CAPACITY) ++s->count;
|
|
}
|
|
void las_can(void *ctx,uint32_t raw,const uint8_t *p,size_t n,uint32_t extended,uint32_t remote) {
|
|
las_context *s=(las_context *)ctx;pcan_id_t id;uint32_t index,delta,period,session;
|
|
if(!extended || remote || raw>PCAN_ID_MASK) { ++s->ignored;return; }
|
|
pcan_id_unpack(raw,&id);
|
|
if(id.device_type!=LAS_DEVICE_TYPE || id.device_id!=s->device ||
|
|
id.route!=PCAN_ROUTE_FROM_DEVICE || id.msg_type!=PCAN_MSG_GAS ||
|
|
(id.msg_body!=LAS_GAS_META && id.msg_body!=LAS_GAS_DATA)) { ++s->ignored;return; }
|
|
if(n!=8 || !p) { ++s->invalid;return; }
|
|
if(id.msg_body==LAS_GAS_META) {
|
|
period=r32(p+4);session=r16(p+2);
|
|
if(r16(p)!=LAS_TAG || period==0) { ++s->invalid;return; }
|
|
if(s->metadata && session==s->session) {
|
|
if(period!=s->period) ++s->invalid;
|
|
return;
|
|
}
|
|
if(s->metadata && (uint16_t)(session-s->session)>=0x8000U) {
|
|
++s->duplicates;return;
|
|
}
|
|
s->metadata=1;s->session=session;s->period=period;
|
|
s->count=0;s->head=0;s->started=0;s->next_index=0;
|
|
return;
|
|
}
|
|
if(!s->metadata) { ++s->ignored;return; }
|
|
if(r16(p)!=s->session) { ++s->ignored;return; }
|
|
index=r32(p+4);
|
|
delta=0;
|
|
if(s->started) {
|
|
delta=index-(uint32_t)s->next_index;
|
|
if(delta>=0x80000000U) { ++s->duplicates;return; }
|
|
s->missing+=delta;
|
|
} else s->next_index=index;
|
|
s->next_index+=delta;
|
|
append(s,s->next_index,r16(p+2),(uint8_t)(!s->started || delta!=0));
|
|
s->next_index+=1;s->started=1;++s->received;
|
|
}
|
|
static void frame_callback(const pcan_frame_t *frame,void *user) {
|
|
if(frame->flags & (PCAN_FLAG_DIR|PCAN_FLAG_ERR)) {
|
|
++((las_context *)user)->ignored;return;
|
|
}
|
|
las_can(user,frame->id,frame->data,frame->dlc,
|
|
frame->flags&PCAN_FLAG_IDE,frame->flags&PCAN_FLAG_RTR);
|
|
}
|
|
void las_uart(void *ctx,const uint8_t *data,size_t size) {
|
|
las_context *s=(las_context *)ctx;
|
|
if(data) pcan_parser_feed(&s->parser,data,size,frame_callback,s);
|
|
}
|
|
uint32_t las_get(const void *ctx,uint32_t field) {
|
|
const las_context *s=(const las_context *)ctx;
|
|
switch(field) {
|
|
case LAS_COUNT:return s->count;case LAS_PERIOD_NS:return s->period;
|
|
case LAS_SESSION:return s->session;case LAS_RECEIVED:return s->received;
|
|
case LAS_MISSING:return s->missing;case LAS_DUPLICATES:return s->duplicates;
|
|
case LAS_INVALID:return s->invalid;case LAS_IGNORED:return s->ignored;
|
|
case LAS_CRC_ERRORS:return s->parser.stats.crc_errors;
|
|
case LAS_HAS_META:return s->metadata;default:return 0;
|
|
}
|
|
}
|
|
size_t las_snapshot(const void *ctx,uint64_t *indices,uint16_t *samples,uint8_t *breaks,size_t cap) {
|
|
const las_context *s=(const las_context *)ctx;uint32_t i,pos;
|
|
if(!indices || !samples || !breaks || cap<s->count) return 0;
|
|
pos=(s->head+LAS_CAPACITY-s->count)%LAS_CAPACITY;
|
|
for(i=0;i<s->count;++i) {
|
|
indices[i]=s->indices[pos];samples[i]=s->samples[pos];breaks[i]=s->breaks[pos];
|
|
pos=(pos+1U)%LAS_CAPACITY;
|
|
}
|
|
return s->count;
|
|
}
|
|
size_t las_trace(const void *ctx,uint32_t channel,uint64_t *indices,uint8_t *levels,
|
|
uint8_t *moves,size_t cap) {
|
|
const las_context *s=(const las_context *)ctx;uint32_t i,pos;
|
|
uint64_t previous_x=0;uint8_t previous_y=0;size_t n=0;
|
|
if(channel>=16 || !indices || !levels || !moves || cap<2U*s->count) return 0;
|
|
pos=(s->head+LAS_CAPACITY-s->count)%LAS_CAPACITY;
|
|
for(i=0;i<s->count;++i) {
|
|
uint64_t x=s->indices[pos];uint8_t y=(uint8_t)((s->samples[pos]>>channel)&1U);
|
|
if(!i) { indices[n]=x;levels[n]=y;moves[n++]=1; }
|
|
else if(s->breaks[pos]) {
|
|
indices[n]=previous_x;levels[n]=previous_y;moves[n++]=0;
|
|
indices[n]=x;levels[n]=y;moves[n++]=1;
|
|
} else if(y!=previous_y) {
|
|
indices[n]=x;levels[n]=previous_y;moves[n++]=0;
|
|
indices[n]=x;levels[n]=y;moves[n++]=0;
|
|
}
|
|
previous_x=x;previous_y=y;pos=(pos+1U)%LAS_CAPACITY;
|
|
}
|
|
if(s->count>1) { indices[n]=previous_x;levels[n]=previous_y;moves[n++]=0; }
|
|
return n;
|
|
}
|
|
static size_t packet(uint32_t device,uint16_t body,const uint8_t *data,uint32_t uart,
|
|
uint32_t *raw,uint8_t *out,size_t cap) {
|
|
pcan_id_t id;pcan_frame_t frame;
|
|
if(device>15 || !raw || !out || cap<(uart?PCAN_FRAME_MAX:8U)) return 0;
|
|
memset(&id,0,sizeof(id));id.device_type=LAS_DEVICE_TYPE;id.device_id=(uint8_t)device;
|
|
id.priority=PCAN_PRIORITY_STANDARD;id.route=PCAN_ROUTE_FROM_DEVICE;
|
|
id.msg_type=PCAN_MSG_GAS;id.msg_body=body;
|
|
memset(&frame,0,sizeof(frame));frame.id=pcan_id_pack(&id);
|
|
frame.flags=PCAN_FLAG_IDE;frame.dlc=8;memcpy(frame.data,data,8);*raw=frame.id;
|
|
if(uart) return pcan_frame_encode(&frame,out,cap);
|
|
memcpy(out,data,8);return 8;
|
|
}
|
|
size_t las_metadata(uint32_t device,uint32_t session,uint32_t period,uint32_t uart,
|
|
uint32_t *id,uint8_t *out,size_t cap) {
|
|
uint8_t data[8];if(session>65535 || !period) return 0;
|
|
w16(data,LAS_TAG);w16(data+2,(uint16_t)session);w32(data+4,period);
|
|
return packet(device,LAS_GAS_META,data,uart,id,out,cap);
|
|
}
|
|
size_t las_data(uint32_t device,uint32_t session,uint32_t index,uint32_t sample,uint32_t uart,
|
|
uint32_t *id,uint8_t *out,size_t cap) {
|
|
uint8_t data[8];if(session>65535 || sample>65535) return 0;
|
|
w16(data,(uint16_t)session);w16(data+2,(uint16_t)sample);w32(data+4,index);
|
|
return packet(device,LAS_GAS_DATA,data,uart,id,out,cap);
|
|
}
|
|
void las_demo_step(void *ctx,uint32_t count) {
|
|
las_context *s=(las_context *)ctx;uint32_t i,bit,idx,raw;uint8_t packet_data[PCAN_FRAME_MAX];size_t n;
|
|
if(!s->metadata) {
|
|
n=las_metadata(s->device,1,1000000,1,&raw,packet_data,sizeof(packet_data));
|
|
las_uart(s,packet_data,n);
|
|
}
|
|
if(count>4096) count=4096;
|
|
for(i=0;i<count;++i) {
|
|
uint16_t a=0;idx=(uint32_t)s->next_index;
|
|
for(bit=0;bit<16;++bit) {
|
|
if((idx/(5U+bit*7U))&1U) a|=(uint16_t)(1U<<bit);
|
|
}
|
|
n=las_data(s->device,s->session,idx,a,1,&raw,packet_data,sizeof(packet_data));
|
|
las_uart(s,packet_data,n);
|
|
}
|
|
}
|