Andrey/templates
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add shared emulator services, waveform generation and STM32 USB boot ports

Browse Source
This commit is contained in:
Andrey
2026-09-28 11:54:17 +03:00
parent 5260186c16
commit 79411dbc26
56 changed files with 3627 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
c/set-protocol/src/set_boot.c
+76
View File
@@ -0,0 +1,76 @@
#include "set_boot.h"
#include <string.h>
void setp_boot_init(setp_boot *b, const setp_boot_ops *ops, void *user,
uint32_t base, uint32_t capacity)
{
memset(b, 0, sizeof(*b)); b->ops=*ops; b->user=user;
b->base=base; b->capacity=capacity;
b->status.max_block_size=256;
}
static bool zero_hash(const uint8_t *p)
{
unsigned i; for(i=0;i<32;++i) if(p[i]) return false;
return true;
}
static uint16_t command(setp_boot *b, uint16_t type, const uint8_t *p, uint16_t n)
{
setp_fw_begin_t begin;
setp_fw_data_t data;
setp_fw_end_t end;
switch(type) {
case SETP_MSG_FW_BEGIN:
if(!setp_fw_begin_decode(p,n,&begin)) return SETP_STATUS_INVALID_LENGTH;
/* This backend implements CRC integrity only, no resume/signature/SHA. */
if(begin.slot || begin.flags!=SETP_FW_FLAG_ERASE_SLOT ||
begin.signature_length || begin.signing_key_id || !zero_hash(begin.sha256))
return SETP_STATUS_UNSUPPORTED;
if(begin.base_address!=b->base || begin.image_size<8 ||
begin.image_size>b->capacity || begin.block_size>256)
return SETP_STATUS_INVALID_ARGUMENT;
b->status.state=SETP_FW_FAILED;
if(!b->ops.invalidate(b->user) || !b->ops.erase(b->user,begin.image_size))
return SETP_STATUS_INTERNAL;
b->crc=begin.image_crc32; b->block_size=begin.block_size;
b->status.image_size=begin.image_size; b->status.next_offset=0;
b->status.state=SETP_FW_RECEIVING; return SETP_STATUS_OK;
case SETP_MSG_FW_DATA:
if(b->status.state!=SETP_FW_RECEIVING) return SETP_STATUS_WRONG_STATE;
if(!setp_fw_data_decode(p,n,&data)) return SETP_STATUS_CRC;
if(data.flags || data.data_length>b->block_size) return SETP_STATUS_INVALID_ARGUMENT;
if(data.offset!=b->status.next_offset) return SETP_STATUS_SEQUENCE;
if(data.data_length>b->status.image_size-b->status.next_offset)
return SETP_STATUS_NO_SPACE;
if(!b->ops.write(b->user,data.offset,data.data,data.data_length)) {
b->status.state=SETP_FW_FAILED; return SETP_STATUS_INTERNAL;
}
b->status.next_offset+=data.data_length; return SETP_STATUS_OK;
case SETP_MSG_FW_END:
if(b->status.state!=SETP_FW_RECEIVING) return SETP_STATUS_WRONG_STATE;
if(!setp_fw_end_decode(p,n,&end)) return SETP_STATUS_INVALID_LENGTH;
if(!zero_hash(end.sha256)) return SETP_STATUS_UNSUPPORTED;
if(end.image_size!=b->status.image_size || end.image_crc32!=b->crc ||
b->status.next_offset!=b->status.image_size) return SETP_STATUS_SEQUENCE;
b->status.state=SETP_FW_FAILED;
if(!b->ops.verify(b->user,end.image_size,b->crc)) return SETP_STATUS_VERIFY_FAILED;
if(!b->ops.commit(b->user,end.image_size,b->crc)) return SETP_STATUS_INTERNAL;
b->status.state=SETP_FW_READY; return SETP_STATUS_OK;
case SETP_MSG_FW_ABORT:
if(n) return SETP_STATUS_INVALID_LENGTH;
b->status.state=SETP_FW_IDLE; b->status.next_offset=0;
return SETP_STATUS_OK;
case SETP_MSG_FW_STATUS:
return n ? SETP_STATUS_INVALID_LENGTH : SETP_STATUS_OK;
case SETP_MSG_FW_ACTIVATE:
if(n) return SETP_STATUS_INVALID_LENGTH;
if(b->status.state!=SETP_FW_READY) return SETP_STATUS_WRONG_STATE;
b->ops.activate(b->user); return SETP_STATUS_OK;
default: return SETP_STATUS_UNSUPPORTED;
}
}
uint16_t setp_boot_command(setp_boot *b, uint16_t type, const uint8_t *p, uint16_t n)
{
uint16_t result=command(b,type,p,n);
if(type!=SETP_MSG_FW_STATUS) b->status.last_error=result;
return result;
}
c/set-protocol/src/set_emu_igbt.c
+142
View File
@@ -0,0 +1,142 @@
#include "set_emu_igbt.h"
#include <string.h>
static void output(set_igbt_emu *s)
{
unsigned ch;s->rx=0;
if(!s->enabled)return;
for(ch=0;ch<SET_IGBT_CHANNELS;++ch) {
set_igbt_channel *c=&s->channel[ch];uint8_t bit=(uint8_t)(1u<<ch),level;
if(!(c->flags&2))continue;
if(c->mode==1)level=0;
else if(c->mode==2)level=1;
else {level=(uint8_t)(c->mode==3?0:!!(s->logical_rx&bit));if(c->flags&4)level^=1;}
if(level)s->rx|=bit;
}
}
void set_igbt_init(set_igbt_emu *s)
{
unsigned ch;memset(s,0,sizeof(*s));s->enabled=1;s->delay_ms=1;s->width_ms=1;
for(ch=0;ch<SET_IGBT_CHANNELS;++ch) {
set_igbt_channel *c=&s->channel[ch];
c->flags=3;c->on_delay=c->off_delay=1;c->on_width=c->off_width=1;
c->tx_width=10;c->edges=3;
}
}
void set_igbt_tick(set_igbt_emu *s,uint8_t pins)
{
unsigned ch;uint8_t changed;
for(ch=0;ch<SET_IGBT_CHANNELS;++ch)
if(s->pulse[ch]&&--s->pulse[ch]==0)s->injected_tx&=(uint8_t)~(1u<<ch);
s->tx=s->input_source?s->injected_tx:pins;
for(ch=0;ch<SET_IGBT_CHANNELS;++ch)
if(!(s->channel[ch].flags&1))s->tx&=(uint8_t)~(1u<<ch);
changed=(uint8_t)(s->tx^s->previous_tx);s->previous_tx=s->tx;
if(!s->enabled){output(s);return;}
for(ch=0;ch<SET_IGBT_CHANNELS;++ch) {
set_igbt_channel *c=&s->channel[ch];uint8_t bit=(uint8_t)(1u<<ch);
unsigned on=!!(s->tx&bit);
if(s->active[ch]&&(!(s->fault&bit)||c->fault_width)&&--s->active[ch]==0)
s->logical_rx&=(uint8_t)~bit;
if(s->pending[ch]&&--s->pending[ch]==0) {
s->logical_rx|=bit;s->active[ch]=s->response_width[ch];
}
if(changed&bit) {
++s->event_count;++c->count;
if(!(c->flags&1)||!(c->flags&2)||c->mode||!(c->edges&(on?1u:2u)))continue;
s->pending[ch]=on?c->on_delay:c->off_delay;
s->response_width[ch]=(s->fault&bit)&&c->fault_width?c->fault_width:(on?c->on_width:c->off_width);
if(!s->pending[ch]){s->logical_rx|=bit;s->active[ch]=s->response_width[ch];}
}
}
output(s);
}
unsigned set_igbt_read(void *ctx,uint16_t off,uint16_t count,uint16_t *out)
{
set_igbt_emu *s=(set_igbt_emu *)ctx;
uint16_t w[SET_IGBT_WORDS]={0};unsigned i,ch;
if(!s||!out||!count||(uint32_t)off+count>SET_IGBT_WORDS)return 2;
w[0]=0x4947;w[1]=3;w[2]=s->enabled;w[3]=s->tx;w[4]=s->rx;
w[6]=s->fault;w[7]=s->delay_ms;w[8]=s->width_ms;w[9]=s->event_count;
w[10]=8;w[11]=1;w[12]=s->input_source;w[13]=s->injected_tx;
w[15]=SET_IGBT_CHANNEL_WORDS;
for(ch=0;ch<SET_IGBT_CHANNELS;++ch) {
set_igbt_channel *c=&s->channel[ch];unsigned b=16+ch*SET_IGBT_CHANNEL_WORDS;
w[b]=c->flags;w[b+1]=c->mode;w[b+2]=c->on_delay;w[b+3]=c->on_width;
w[b+4]=c->off_delay;w[b+5]=c->off_width;w[b+6]=c->tx_width;
w[b+7]=c->edges;w[b+8]=c->fault_width;w[b+9]=c->count;
w[b+10]=s->pending[ch];w[b+11]=s->active[ch];
}
for(i=0;i<count;++i)out[i]=w[off+i];
return 0;
}
unsigned set_igbt_write(void *ctx,uint16_t off,uint16_t value)
{
set_igbt_emu *s=(set_igbt_emu *)ctx;unsigned ch;uint8_t released;
if(!s)return 3;
if(off>=16) {
unsigned field=(off-16)%SET_IGBT_CHANNEL_WORDS;
set_igbt_channel *c;
ch=(off-16)/SET_IGBT_CHANNEL_WORDS;if(ch>=8)return 2;c=&s->channel[ch];
switch(field) {
case 0:if(value>7)return 3;c->flags=value;break;
case 1:if(value>3)return 3;c->mode=value;break;
case 2:if(value>1000)return 3;c->on_delay=value;break;
case 3:if(!value||value>1000)return 3;c->on_width=value;break;
case 4:if(value>1000)return 3;c->off_delay=value;break;
case 5:if(!value||value>1000)return 3;c->off_width=value;break;
case 6:if(!value||value>60000)return 3;c->tx_width=value;break;
case 7:if(value>3)return 3;c->edges=value;break;
case 8:if(value>60000)return 3;c->fault_width=value;break;
default:return 2;
}
/* Mode/enable changes discard pending responses. */
if(field<=1) {s->pending[ch]=s->active[ch]=0;s->logical_rx&=(uint8_t)~(1u<<ch);}
output(s);return 0;
}
switch(off) {
case 2:
if(value>1)return 3;
s->enabled=(uint8_t)value;
if(!value) {s->logical_rx=0;
for(ch=0;ch<8;++ch)if(s->pulse[ch])s->injected_tx&=(uint8_t)~(1u<<ch);
memset(s->pending,0,sizeof(s->pending));
memset(s->active,0,sizeof(s->active));memset(s->pulse,0,sizeof(s->pulse));}
break;
case 3:case 13:
if(value>255)return 3;
s->injected_tx=(uint8_t)value;memset(s->pulse,0,sizeof(s->pulse));break;
case 6:
if(value>255)return 3;
released=(uint8_t)(s->fault&~value);s->fault=(uint8_t)value;
s->logical_rx&=(uint8_t)~released;
for(ch=0;ch<8;++ch)if(released&(1u<<ch))s->active[ch]=0;
break;
case 7:
if(value>1000)return 3;
s->delay_ms=value;
for(ch=0;ch<8;++ch)s->channel[ch].on_delay=s->channel[ch].off_delay=value;
break;
case 8:
if(!value||value>255)return 3;
s->width_ms=value;
for(ch=0;ch<8;++ch)s->channel[ch].on_width=s->channel[ch].off_width=value;
break;
case 9:
if(value)return 3;
s->event_count=0;for(ch=0;ch<8;++ch)s->channel[ch].count=0;break;
case 12:
if(value>1)return 3;
s->input_source=(uint8_t)value;memset(s->pulse,0,sizeof(s->pulse));break;
case 14:
if(value>255)return 3;
if(!s->enabled||!s->input_source)return 6;
/* A pulse must start from low; retrigger without an edge is refused. */
if(value&(s->injected_tx|s->tx))return 6;
for(ch=0;ch<8;++ch)if((value&(1u<<ch))&&!(s->channel[ch].flags&1))return 6;
s->injected_tx|=(uint8_t)value;
for(ch=0;ch<8;++ch)if(value&(1u<<ch))s->pulse[ch]=(uint16_t)(s->channel[ch].tx_width+1u);
break;
default:return 2;
}
output(s);return 0;
}
c/set-protocol/src/set_emu_server.c
+194
View File
@@ -0,0 +1,194 @@
#include "set_emu_server.h"
#include "set_crc.h"
#include <string.h>
static const uint32_t baudrates[5]={9600,19200,38400,57600,115200};
uint32_t set_emu_server_baud(const set_emu_server *s) { return baudrates[s->config[2]]; }
static unsigned control_read(void *ctx,uint16_t off,uint16_t n,uint16_t *out)
{
set_emu_server *s=(set_emu_server *)ctx;uint16_t w[36]={0};unsigned i;
if(!((off==0&&n==4)||(off==16&&(n==8||n==10))||(off==32&&n==4)))return 2;
w[0]=0x454d;w[1]=1;w[2]=s->ump.enabled&1u;w[3]=s->ump.prev_command;
w[16]=0x5343;w[17]=3;
for(i=0;i<3;++i){w[18+i]=s->config[i];w[21+i]=s->staged[i];}
w[24]=s->config[3];w[25]=s->staged[3];
w[32]=0x4642;w[33]=1;w[34]=3;w[35]=SET_UMP_WORDS;
for(i=0;i<n;++i)out[i]=w[off+i];
return 0;
}
static unsigned control_write(void *ctx,uint16_t off,uint16_t v)
{
set_emu_server *s=(set_emu_server *)ctx;
if(off==2) {
if(v && s->config[3]!=0)return 6;
if(v>1)return 3;
s->ump.enabled=(uint8_t)((s->ump.enabled&~1u)|v);
if(!v)s->ump.scenario=0;
return 0;
}
if(off<21||off>25)return 2;
if(s->apply_delay)return 6;
if(off==24) {
if(v==0)memcpy(s->staged,s->config,sizeof(s->config));
else if(v==0xa55a) {
if(s->staged[0]==s->staged[1])return 3;
s->apply_delay=500;
} else return 3;
} else if(off==25) {
if(v>3 || (v==3&&!s->wave))return 3;
s->staged[3]=v;
} else {
if((off==21&&(!v||v>247))||(off==22&&(!v||v>255))||(off==23&&v>4))return 3;
s->staged[off-21]=v;
}
return 0;
}
static unsigned ump_read(void *ctx,uint16_t off,uint16_t n,uint16_t *out)
{
set_emu_server *s=(set_emu_server *)ctx;
return s->config[3]?6:set_ump_read(&s->ump,off,n,out);
}
static unsigned ump_write(void *ctx,uint16_t off,uint16_t v)
{
set_emu_server *s=(set_emu_server *)ctx;
return s->config[3]?6:set_ump_write(&s->ump,off,v);
}
static unsigned indexed_read(void *ctx,uint16_t off,uint16_t n,uint16_t *out)
{
set_emu_server *s=(set_emu_server *)ctx;
return s->config[3]?6:set_ump_indexed_read(&s->ump,off,n,out);
}
static unsigned tms_read(void *ctx,uint16_t off,uint16_t n,uint16_t *out)
{
set_emu_server *s=(set_emu_server *)ctx;
return set_tms_read(&s->tms,off,n,out);
}
static unsigned tms_write(void *ctx,uint16_t off,uint16_t v)
{
set_emu_server *s=(set_emu_server *)ctx;
return s->config[3]!=1?6:set_tms_write(&s->tms,off,v);
}
/* Status remains readable in every mode; only the selected service may write.
* Explicit stop commands remain available regardless of mode. */
static unsigned igbt_write(void *ctx,uint16_t off,uint16_t v)
{
set_emu_server *s=(set_emu_server *)ctx;
if(s->config[3]!=2 && !(off==2&&v==0))return 6;
return set_igbt_write(&s->igbt,off,v);
}
static unsigned igbt_read(void *ctx,uint16_t off,uint16_t n,uint16_t *out)
{
return set_igbt_read(&((set_emu_server *)ctx)->igbt,off,n,out);
}
static unsigned wave_write(void *ctx,uint16_t off,uint16_t v)
{
set_emu_server *s=(set_emu_server *)ctx;
if(s->config[3]!=3 && !(off==2&&v==0))return 6;
return set_wave_write(s->wave,off,v);
}
static unsigned wave_read(void *ctx,uint16_t off,uint16_t n,uint16_t *out)
{
return set_wave_read(((set_emu_server *)ctx)->wave,off,n,out);
}
static unsigned samples_write(void *ctx,uint16_t off,uint16_t v)
{
set_emu_server *s=(set_emu_server *)ctx;
return s->config[3]!=3?6:set_wave_samples_write(s->wave,off,v);
}
static unsigned samples_read(void *ctx,uint16_t off,uint16_t n,uint16_t *out)
{
return set_wave_samples_read(((set_emu_server *)ctx)->wave,off,n,out);
}
static void region(set_reg_region *r,uint16_t base,uint16_t words,uint16_t kind,
uint16_t version,void *ctx,set_reg_read_fn read,set_reg_write_fn write)
{
r->base=base;r->words=words;r->kind=kind;r->version=version;r->instance=0;
r->flags=(uint16_t)(SET_REG_READ|(write?SET_REG_WRITE:0));
r->context=ctx;r->read=read;r->write=write;
}
unsigned set_emu_server_init(set_emu_server *s,uint16_t *b0,uint16_t *b1,
uint16_t capacity,set_wave_state *wave,unsigned address,unsigned tms,
unsigned baud,unsigned can,unsigned enabled)
{
size_t n=0;
if(!s||!b0||!b1||b0==b1||capacity<2||capacity>819||
!address||address>247||!tms||tms>255||address==tms||baud>4||
!can||can>16||enabled>1)return 3;
if(wave&&(!wave->samples||wave->capacity<2||wave->capacity>SET_WAVE_MAX))return 3;
memset(s,0,sizeof(*s));s->wave=wave;
if(set_ump_init(&s->ump,b0,b1,capacity,(uint16_t)can,(uint8_t)enabled))return 3;
set_tms_init(&s->tms);set_igbt_init(&s->igbt);s->igbt.enabled=0;
s->config[0]=(uint16_t)address;s->config[1]=(uint16_t)tms;s->config[2]=(uint16_t)baud;
memcpy(s->staged,s->config,sizeof(s->config));
region(&s->regions[n++],0x1000,0x17c,SET_EMU_KIND_UMP,2,s,ump_read,ump_write);
region(&s->regions[n++],SET_EMU_CONTROL_BASE,36,SET_EMU_KIND_CONTROL,3,s,control_read,control_write);
region(&s->regions[n++],SET_EMU_IGBT_BASE,SET_IGBT_WORDS,SET_EMU_KIND_IGBT,3,s,igbt_read,igbt_write);
region(&s->regions[n++],SET_EMU_TMS_BASE,SET_TMS_WORDS,SET_EMU_KIND_TMS,1,s,tms_read,tms_write);
region(&s->regions[n++],SET_EMU_INDEXED_BASE,(uint16_t)(capacity*SET_UMP_WORDS),
SET_EMU_KIND_UMP_DATA,1,s,indexed_read,0);
if(wave) {
region(&s->regions[n++],SET_WAVE_BASE,9,SET_EMU_KIND_WAVE,3,s,wave_read,wave_write);
region(&s->regions[n++],SET_WAVE_DATA,(uint16_t)wave->capacity,
SET_EMU_KIND_WAVE_DATA,1,s,samples_read,samples_write);
}
return set_regmap_init(&s->map,s->regions,n);
}
void set_emu_server_tick(set_emu_server *s,uint8_t pins)
{
if(s->apply_delay&&--s->apply_delay==0) {
if(s->config[3]!=s->staged[3]) {
/* Stop every executor before selecting the new one. No implicit start. */
if(s->wave) {
if(s->wave->port.stop)s->wave->port.stop(s->wave->port.context);
s->wave->running=0;s->wave->ready=0;
}
set_igbt_init(&s->igbt);s->igbt.enabled=0;
set_tms_init(&s->tms);s->ump.state=3;s->ump.post=0;s->ump.scenario=0;
s->ump.prev_command=0;s->ump.prev_output=0;
}
memcpy(s->config,s->staged,sizeof(s->config));
}
if(s->config[3]==1)set_tms_tick(&s->tms);
else if(s->config[3]==0)set_ump_tick(&s->ump);
else if(s->config[3]==2)set_igbt_tick(&s->igbt,pins);
}
static int inactive_ump(const set_emu_server *s,uint16_t a)
{
return s->config[3]&&((a>=0x1000&&a<0x1200)||
(a>=SET_EMU_INDEXED_BASE&&a<(uint32_t)SET_EMU_INDEXED_BASE+s->ump.capacity*SET_UMP_WORDS));
}
size_t set_emu_server_rtu(set_emu_server *s,const uint8_t *r,size_t n,uint8_t *out,size_t cap)
{
if(!s||!r||!n)return 0;
if(r[0]==s->config[1])
return s->config[3]==1?set_tms_rtu(&s->tms,s->config[1],r,n,out,cap):0;
if(n==8&&inactive_ump(s,(uint16_t)(((unsigned)r[2]<<8)|r[3])))return 0;
return set_regmap_rtu(&s->map,s->config[0],r,n,out,cap);
}
size_t set_emu_server_can(set_emu_server *s,const uint8_t *r,size_t n,
uint16_t *out,size_t cap,unsigned *token,unsigned *error)
{
if(token)*token=0;
if(error)*error=0;
if(!s||!r||n!=8||inactive_ump(s,(uint16_t)(((unsigned)r[0]<<8)|r[1])))return 0;
return set_regmap_can(&s->map,r,n,out,cap,token,error);
}
size_t set_emu_stream_feed(set_emu_stream *st,set_emu_server *s,uint8_t byte,
uint32_t now,uint8_t *out,size_t cap)
{
size_t length,n;
if(!st||!s||!out||cap<253)return 0;
if((uint32_t)(now-st->last_byte)>1000u||st->used>=sizeof(st->data))st->used=0;
st->last_byte=now;st->data[st->used++]=byte;
while(st->used>=8) {
length=st->data[0]==s->config[1]&&st->data[1]==65?18u:8u;
if(st->used<length)return 0;
if(set_crc16_modbus(st->data,length)) {
--st->used;memmove(st->data,st->data+1,st->used);continue;
}
n=set_emu_server_rtu(s,st->data,length,out,cap);
st->used=(uint8_t)(st->used-length);
memmove(st->data,st->data+length,st->used);
if(n)return n;
}
return 0;
}
c/set-protocol/src/set_emu_tms.c
+96
View File
@@ -0,0 +1,96 @@
#include "set_emu_tms.h"
#include "set_crc.h"
#include <string.h>
static void analog(uint8_t *reply, unsigned index, int32_t value)
{
uint16_t word = (uint16_t)value;
reply[30 + index*2] = (uint8_t)word;
reply[31 + index*2] = (uint8_t)(word >> 8);
}
void set_tms_init(set_tms_emu *s) { memset(s,0,sizeof(*s)); }
void set_tms_tick(set_tms_emu *s)
{
int32_t wanted = s->running ? s->target : 0;
/* 10 Hz/s, signed hundredths of Hz. Independent of UMP recorder. */
if (++s->divider < 10) return;
s->divider = 0;
if (s->frequency < wanted) s->frequency += wanted-s->frequency < 10 ? wanted-s->frequency : 10;
if (s->frequency > wanted) s->frequency -= s->frequency-wanted < 10 ? s->frequency-wanted : 10;
}
static void apply(set_tms_emu *s)
{
unsigned i;uint16_t word=s->commands[0];
for(i=0;i<6;++i)s->controls[i]=(uint8_t)s->commands[4+i];
s->running=(uint8_t)((s->controls[0]&0x41)==0x41);
if((s->controls[0]&0x40) && (s->controls[2]&4))
s->target=word<0x8000?(int32_t)word:(int32_t)word-65536;
}
static void snapshot(set_tms_emu *s,uint8_t *reply)
{
memset(reply, 0, 101);
reply[0] = 0; reply[1] = 65;
/* BALZAM_7: byte10.Go; ready/Go LED; remote selector. */
reply[11] = s->running;
reply[14] = (uint8_t)(0x0b | (s->running << 5));
reply[15] = (uint8_t)(6 | ((s->controls[0] & 0x40) ? 0x20 : 0));
reply[16] = (uint8_t)(3 | ((s->controls[4] & 0xc0) >> 3));
analog(reply, 0, s->frequency); analog(reply, 1, 2000);
analog(reply, 4, s->frequency); analog(reply, 5, s->frequency);
analog(reply, 6, 540); analog(reply, 7, 540);
analog(reply, 8, s->frequency / 10); analog(reply, 9, s->frequency / 10);
analog(reply, 10, s->frequency); analog(reply, 11, s->frequency < 0 ? -1 : 1);
analog(reply, 12, 540); analog(reply, 13, 540);
analog(reply, 14, s->frequency); analog(reply, 15, s->frequency);
analog(reply, 25, 25); analog(reply, 26, 30);
analog(reply, 28, 400); analog(reply, 29, 400);
analog(reply, 30, (s->controls[4] & 0x40) ? 120 : 40);
analog(reply, 31, (s->controls[4] & 0x40) ? 120 : 40);
analog(reply, 32, (s->controls[4] & 0x80) ? 120 : 40);
analog(reply, 33, (s->controls[4] & 0x80) ? 120 : 40);
}
unsigned set_tms_read(void *ctx,uint16_t off,uint16_t count,uint16_t *out)
{
set_tms_emu *s=(set_tms_emu *)ctx;uint8_t r[101];unsigned i,a;
if(!s || !out || !count || (uint32_t)off+count>SET_TMS_WORDS)return 2;
snapshot(s,r);
for(i=0;i<count;++i) {
uint16_t v=0;a=off+i;
if(a==0)v=0x544d;
else if(a==1)v=1;
else if(a==2)v=s->running;
else if(a==3)v=(uint16_t)s->frequency;
else if(a==4)v=(uint16_t)s->target;
else if(a>=8 && a<18)v=s->commands[a-8];
else if(a>=32 && a<46)v=(uint16_t)(r[2+(a-32)*2]|((unsigned)r[3+(a-32)*2]<<8));
else if(a>=46)v=(uint16_t)(r[30+(a-46)*2]|((unsigned)r[31+(a-46)*2]<<8));
out[i]=v;
}
return 0;
}
unsigned set_tms_write(void *ctx,uint16_t off,uint16_t v)
{
set_tms_emu *s=(set_tms_emu *)ctx;
if(!s)return 3;
if(off>=8 && off<18) {
if(off>=12 && v>255)return 3;
s->commands[off-8]=v;return 0;
}
if(off==18) { if(v!=1)return 3;apply(s);return 0; }
return 2;
}
size_t set_tms_rtu(set_tms_emu *s,unsigned device,const uint8_t *r,size_t n,
uint8_t *out,size_t cap)
{
unsigned i;uint16_t crc;
if(!s || !r || !out || n!=18 || cap<101 || !device || device>255 ||
r[0]!=device || r[1]!=65 || set_crc16_modbus(r,n))return 0;
for(i=0;i<4;++i)s->commands[i]=(uint16_t)(r[2+2*i]|((unsigned)r[3+2*i]<<8));
for(i=0;i<6;++i)s->commands[4+i]=r[10+i];
apply(s);snapshot(s,out);out[0]=(uint8_t)device;
crc=set_crc16_modbus(out,98);out[98]=(uint8_t)crc;out[99]=(uint8_t)(crc>>8);
return 101;
}
c/set-protocol/src/set_emu_ump.c
+172
View File
@@ -0,0 +1,172 @@
/* UMP synthetic signals and recorder. Storage belongs to the caller. */
#include "set_emu_ump.h"
#include <string.h>
static int triangle(uint32_t t, unsigned cycle)
{
int x = (int)(t % cycle) * 480 / (int)cycle;
return x < 240 ? x - 120 : 360 - x;
}
static void signals(set_ump_emu *s, uint16_t *p)
{
unsigned enabled = (s->enabled & 1u) != 0;
unsigned t = enabled ? s->scenario % 6000u : 0, ch;
unsigned run = t >= 1000 && t < 5000, reset = t >= 5000 && t < 5300;
unsigned ramp = t < 1000 ? 0 : (t < 3500 ? t - 1000 : 2500);
unsigned current = run ? 60 + ramp * 100 / 2500 :
(t >= 5000 && t < 5300 ? 40 + (5300-t)*120/300 : 40);
memset(p, 0, SET_UMP_WORDS * sizeof(*p));
p[0] = (uint16_t)s->elapsed; p[1] = (uint16_t)(s->elapsed >> 16);
p[3] = (uint16_t)((run << 7) | (reset << 6));
p[4] = (uint16_t)((t >= 1200 && t < 5100) | (run << 1) | (reset << 2));
p[5] = (uint16_t)((t >= 1000 && t < 2000) | ((t >= 5000) << 1) | (run << 2));
p[6] = (uint16_t)((t >= 1000 && t < 2000) ? 2000-t : (t >= 5000 ? 6000-t : 0));
p[7] = (uint16_t)(enabled && t < 250 ? 250-t : 0);
p[8] = (uint16_t)(500 + (current-40)*3000/160); p[9] = (uint16_t)current;
for (ch = 0; ch < 4; ++ch) {
int v = enabled ? triangle(s->scenario + ch * 211u, 720 + ch * 280u) : 0;
p[10+ch] = (uint16_t)v;
p[14+ch] = (uint16_t)((t >= 5100 && t < 5400) ? 1u << ch : 0);
p[36+ch] = (uint16_t)(2048 + v * 4);
}
/* These fields are synthetic CAN diagnostics, not transport counters. */
p[18] = 0xE07F; p[19] = p[3]; p[20] = 6; p[21] = 16;
p[22] = enabled ? (uint16_t)(s->scenario/100) : 0;
p[24] = 6; p[25] = 16; p[26] = 500; p[27] = 3500;
p[28] = 2000; p[29] = 500; p[30] = 3500;
p[31] = (uint16_t)(run ? ramp : 0); p[32] = 2500;
p[33] = 60; p[34] = 160; p[35] = (uint16_t)(ramp*1000/2500);
}
static void capture(set_ump_emu *s, uint16_t reason, uint16_t *p)
{
if (reason != 5 && !s->post && (((p[3]&0x40) && !(s->prev_command&0x40)) ||
(!(p[3]&0x80) && (s->prev_command&0x80)))) {
if (s->used >= s->capacity/2) s->used = s->capacity/2-1;
s->post = 1000; s->dropped = 0; reason |= 0x100;
}
s->prev_command = p[3]; s->prev_input = p[4]; s->prev_output = p[5];
if (s->post && s->used >= s->capacity-1) { s->dropped = 1; return; }
p[2] = (uint16_t)(reason | ((reason == 5 && s->dropped) ? 0x200 : 0));
memcpy(s->banks[s->bank][s->next], p, SET_UMP_WORDS*sizeof(*p));
s->next = (uint16_t)((s->next+1)%s->capacity);
if (s->used < s->capacity) ++s->used;
++s->sequence;
}
unsigned set_ump_init(set_ump_emu *s,uint16_t *bank0,uint16_t *bank1,
uint16_t capacity,uint16_t can_mode,uint8_t enabled)
{
if(!s || !bank0 || !bank1 || bank0==bank1 || capacity<2 ||
capacity>1638 || !can_mode || can_mode>16)return 3;
memset(s,0,sizeof(*s));
s->banks[0]=(uint16_t (*)[SET_UMP_WORDS])bank0;
s->banks[1]=(uint16_t (*)[SET_UMP_WORDS])bank1;
s->capacity=capacity;s->can_mode=can_mode;s->enabled=enabled;
s->archive_bank=1;s->period=20;return 0;
}
void set_ump_tick(set_ump_emu *s)
{
uint16_t p[SET_UMP_WORDS],old_post;
if (!(s->enabled & 1u)) s->scenario = 0;
if (s->state != 1) return;
++s->elapsed;
if (s->enabled & 1u) ++s->scenario;
old_post = s->post;
signals(s,p);
if (++s->divider >= s->period) { s->divider = 0; capture(s,1,p); }
if (p[3] != s->prev_command || p[4] != s->prev_input || p[5] != s->prev_output) capture(s,3,p);
/* Count a full 1000 ms AFTER the trigger; never extend on new edges. */
if (old_post && --s->post == 0) {
capture(s,5,p);
s->archive_bank = s->bank; s->archive_count = s->used; s->archive_next = s->next;
++s->generation; s->bank ^= 1; s->used = s->next = 0;
}
}
static uint16_t record_word(set_ump_emu *s, unsigned archived, unsigned record, unsigned word)
{
unsigned n = archived ? s->archive_count : s->used;
unsigned end = archived ? s->archive_next : s->next;
if (record >= n) return 0;
return s->banks[archived ? s->archive_bank : s->bank][(end+s->capacity-n+record)%s->capacity][word];
}
static uint16_t read_word(set_ump_emu *s,uint16_t address)
{
unsigned offset,count=s->used<3?s->used:3;
uint32_t first=s->sequence-count;
switch(address) {
case 0x1000: return 0x554D; case 0x1001: return 2;
case 0x1002: return s->source ? 3 : s->state;
case 0x1003: return s->source ? s->archive_count : s->used;
case 0x1004: return SET_UMP_WORDS; case 0x1005: return s->capacity;
case 0x1006: return s->period; case 0x1007: return (uint16_t)s->elapsed;
case 0x1008: return (uint16_t)(s->elapsed>>16); case 0x1009: return 6;
case 0x100A: return s->can_mode; case 0x100B: return 1000;
case 0x100C: return s->generation; case 0x100D: return s->archive_count;
case 0x100E: return s->post; case 0x100F: return s->state;
}
if (address >= 0x1010u && address < 0x1010u+120) {
if (!s->source && s->state == 1) return 0;
offset = address-0x1010u;
return record_word(s,s->source, s->cursor+offset/SET_UMP_WORDS, offset%SET_UMP_WORDS);
}
offset = address-0x1100u;
if (offset == 0) return (uint16_t)first;
if (offset == 1) return (uint16_t)(first>>16);
if (offset == 2) return (uint16_t)count;
if (offset == 3) return s->generation;
offset -= 4;
return offset/SET_UMP_WORDS < count ? record_word(s,0,s->used-count+offset/SET_UMP_WORDS,offset%SET_UMP_WORDS) : 0;
}
unsigned set_ump_read(void *ctx,uint16_t off,uint16_t count,uint16_t *out)
{
set_ump_emu *s=(set_ump_emu *)ctx;unsigned i;
uint32_t end=(uint32_t)off+count;
if(!s || !out || !count || count>124)return 3;
if(!((off<16 && end<=16) || (off>=0x10 && end<=0x88) ||
(off==0x100 && count==124)))return 2;
for(i=0;i<count;++i)out[i]=read_word(s,(uint16_t)(0x1000u+off+i));
return 0;
}
unsigned set_ump_indexed_read(void *ctx,uint16_t off,uint16_t count,uint16_t *out)
{
set_ump_emu *s=(set_ump_emu *)ctx;unsigned i,records;
if(!s || !out || !count || count>120 || off%SET_UMP_WORDS || count%SET_UMP_WORDS)return 3;
records=s->source?s->archive_count:s->used;
if(off/SET_UMP_WORDS+count/SET_UMP_WORDS>records)return 2;
if(!s->source && s->state==1)return 6;
for(i=0;i<count;++i)out[i]=record_word(s,s->source,(off+i)/SET_UMP_WORDS,(off+i)%SET_UMP_WORDS);
return 0;
}
unsigned set_ump_write(void *ctx,uint16_t off,uint16_t value)
{
set_ump_emu *s=(set_ump_emu *)ctx;
uint16_t address=(uint16_t)(0x1000u+off),p[SET_UMP_WORDS];
if(!s || off>3)return 2;
if (address < 0x1000u || address > 0x1000u+3) return 2;
switch (address) {
case 0x1000u:
if (value == 0) { s->state = 3; s->post = 0; }
else if (value == 1 && s->state != 1) {
s->used = s->next = s->cursor = s->divider = s->post = s->source = s->dropped = 0;
s->scenario = 0; s->state = 1;
signals(s,p); s->prev_command = p[3]; capture(s,1,p);
} else return 3;
break;
case 0x1000u+1:
if (s->state == 1 || value < 1 || value > 1000) return 3;
s->period = value; break;
case 0x1000u+2:
if (value >= s->capacity) return 3;
s->cursor = value; break;
case 0x1000u+3:
if (value > 1) return 3;
s->source = value; break;
}
return 0;
}
c/set-protocol/src/set_regmap.c
+117
View File
@@ -0,0 +1,117 @@
#include "set_regmap.h"
#include "set_crc.h"
#include <string.h>
static uint16_t get16(const uint8_t *p) { return (uint16_t)(((unsigned)p[0]<<8)|p[1]); }
static void put16(uint8_t *p, unsigned v) { p[0]=(uint8_t)(v>>8); p[1]=(uint8_t)v; }
unsigned set_regmap_init(set_regmap *m, const set_reg_region *r, size_t count)
{
size_t i,j;
if (!m) return SET_REG_VALUE;
m->regions=0; m->count=0;
if ((!r && count) || count>64) return SET_REG_VALUE;
for (i=0;i<count;++i) {
uint32_t end=(uint32_t)r[i].base+r[i].words;
if (!r[i].words || end>65536u || r[i].base<8u+count*8u ||
!r[i].flags || (r[i].flags & ~3u) ||
((r[i].flags&SET_REG_READ) && !r[i].read) ||
((r[i].flags&SET_REG_WRITE) && !r[i].write)) return SET_REG_VALUE;
for (j=0;j<i;++j)
if (r[i].base<(uint32_t)r[j].base+r[j].words && r[j].base<end)
return SET_REG_ADDRESS;
}
m->regions=r; m->count=count; return 0;
}
static uint16_t directory(const set_regmap *m, unsigned a)
{
const set_reg_region *r;
if (a==0) return 0x5345; /* SE: service directory */
if (a==1) return 1;
if (a==2) return (uint16_t)m->count;
if (a==3) return 8;
if (a==4) return 8;
if (a==5) return SET_REG_MAX_WORDS;
if (a<8) return 0;
r=&m->regions[(a-8)/8];
switch ((a-8)%8) {
case 0:return r->kind; case 1:return r->version; case 2:return r->instance;
case 3:return r->base; case 4:return r->words; case 5:return r->flags;
default:return 0;
}
}
unsigned set_regmap_request(const set_regmap *m, unsigned fn, uint16_t a,
uint16_t v, uint16_t *out, size_t cap, size_t *count)
{
size_t i, n; unsigned error;
uint32_t end;
if (!count) return SET_REG_VALUE;
*count=0;
if (!m || (!m->regions && m->count) || !out) return SET_REG_VALUE;
if (fn!=3 && fn!=6) return SET_REG_FUNCTION;
n=fn==3?v:2;
if (!n || n>SET_REG_MAX_WORDS || cap<n) return SET_REG_VALUE;
end=(uint32_t)a+(fn==3?v:1u);
if (end>65536u) return SET_REG_ADDRESS;
if (a<8u+8u*m->count) {
if (fn!=3 || end>8u+8u*m->count) return SET_REG_ADDRESS;
for(i=0;i<n;++i) out[i]=directory(m,a+(unsigned)i);
*count=n; return 0;
}
for (i=0;i<m->count;++i) {
const set_reg_region *r=&m->regions[i];
if (a<r->base || a>=(uint32_t)r->base+r->words) continue;
if (end>(uint32_t)r->base+r->words) return SET_REG_ADDRESS;
if (fn==3) {
if (!(r->flags&SET_REG_READ)) return SET_REG_ADDRESS;
error=r->read(r->context,(uint16_t)(a-r->base),v,out);
} else {
if (!(r->flags&SET_REG_WRITE)) return SET_REG_ADDRESS;
error=r->write(r->context,(uint16_t)(a-r->base),v);
if (!error) { out[0]=a; out[1]=v; }
}
if (!error) *count=n;
return error;
}
return SET_REG_ADDRESS;
}
size_t set_regmap_rtu(const set_regmap *m, unsigned device, const uint8_t *req,
size_t length, uint8_t *out, size_t cap)
{
uint16_t words[SET_REG_MAX_WORDS],crc; size_t count=0,n,i;
unsigned error;
if (!req || !out || length!=8 || cap<253 || device<1 || device>247 ||
req[0]!=device || req[1]>127 || set_crc16_modbus(req,length)) return 0;
error=set_regmap_request(m,req[1],get16(req+2),get16(req+4),words,SET_REG_MAX_WORDS,&count);
out[0]=(uint8_t)device;out[1]=req[1];
if (error) { out[1]|=0x80;out[2]=(uint8_t)error;n=3; }
else if (req[1]==6) { put16(out+2,words[0]);put16(out+4,words[1]);n=6; }
else {
out[2]=(uint8_t)(2*count);
for(i=0;i<count;++i) put16(out+3+2*i,words[i]);
n=3+2*count;
}
crc=set_crc16_modbus(out,n);out[n++]=(uint8_t)crc;out[n++]=(uint8_t)(crc>>8);
return n;
}
size_t set_regmap_can(const set_regmap *m, const uint8_t *r, size_t length,
uint16_t *out, size_t cap, unsigned *token, unsigned *error)
{
size_t n=0;
if (token) *token=0;
if (error) *error=0;
if (!r || !out || !token || !error || length!=8 || cap<SET_REG_MAX_WORDS ||
get16(r+4)!=0x1f00 || !r[6]) return 0;
*token=r[6];
*error=set_regmap_request(m,r[7],get16(r),get16(r+2),out,cap,&n);
if (*error) { out[0]=get16(r);out[1]=get16(r+2);n=2; }
if (n&1u) out[n++]=0;
return n;
}
size_t set_regmap_can_fragment(const uint16_t *w,size_t n,size_t index,
unsigned token,unsigned error,uint8_t *out,size_t cap)
{
if (!w || !out || cap<8 || n<2 || n>SET_REG_MAX_WORDS || (n&1) ||
index>=n/2 || !token || token>255 || error>255) return 0;
put16(out,w[index*2]);put16(out+2,w[index*2+1]);
put16(out+4,0x1f00u+(unsigned)index);out[6]=(uint8_t)token;out[7]=(uint8_t)error;
return 8;
}
c/set-protocol/src/set_wavegen.c
+214
View File
@@ -0,0 +1,214 @@
#include "set_wavegen.h"
#include "set_crc.h"
#include <string.h>
static uint16_t read16(const uint8_t *p) {return (uint16_t)((p[0]<<8)|p[1]);}
static void write16(uint8_t *p,unsigned value) {p[0]=(uint8_t)(value>>8);p[1]=(uint8_t)value;}
static size_t seal(uint8_t *p,size_t n) {
uint16_t crc=set_crc16_modbus(p,n);p[n]=(uint8_t)crc;p[n+1]=(uint8_t)(crc>>8);return n+2;
}
void set_wave_init(set_wave_state *s,uint16_t *samples,size_t capacity,const set_wave_port *port) {
memset(s,0,sizeof(*s));s->samples=samples;s->capacity=capacity;s->rate=1000;
if(port) s->port=*port;
}
static unsigned write_register(set_wave_state *s,unsigned reg,unsigned value) {
if(reg==SET_WAVE_BASE+2 && value==0) {
if(s->port.stop) s->port.stop(s->port.context);
s->running=0; return 0;
}
if(s->running) return 6;
if(reg==SET_WAVE_BASE+2 && value==1) {
if(!s->ready||!s->port.start) return 3;
if(s->port.start(s->port.context,s->samples,s->count,s->rate)) return 4;
s->running=1;return 0;
}
if(reg==SET_WAVE_BASE+3) {
uint32_t rate=((uint32_t)s->rate_high<<16)|value;
if(rate<1||rate>SET_WAVE_MAX_RATE) return 3;
s->rate=rate;s->ready=0;return 0;
}
if(reg==SET_WAVE_BASE+8) {
if(value>(SET_WAVE_MAX_RATE>>16)) return 3;
s->rate_high=(uint16_t)value;s->ready=0;return 0;
}
if(reg==SET_WAVE_BASE+4) {
if(value<2||value>SET_WAVE_MAX||value>s->capacity) return 3;
s->count=(uint16_t)value;s->received=s->ready=0;return 0;
}
if(reg>=SET_WAVE_DATA&&reg<SET_WAVE_DATA+SET_WAVE_MAX) {
unsigned index=reg-SET_WAVE_DATA;
if(index>=s->count||index>s->received||value>4095||s->ready) return 3;
if(index<s->received) return s->samples[index]==value?0:3;
s->samples[index]=(uint16_t)value;++s->received;return 0;
}
if(reg==SET_WAVE_BASE+6) {
if(value!=0xA55A||s->count<2||s->received!=s->count) return 3;
s->ready=1;return 0;
}
return 2;
}
unsigned set_wave_read(void *ctx,uint16_t off,uint16_t count,uint16_t *out) {
set_wave_state *s=(set_wave_state *)ctx;uint16_t w[9];unsigned i;
if(!s||!s->samples||!out||!count||(uint32_t)off+count>9)return 2;
w[0]=0x5747;w[1]=3;w[2]=s->running;w[3]=(uint16_t)s->rate;
w[4]=s->count;w[5]=s->received;w[6]=s->ready;w[7]=(uint16_t)s->capacity;w[8]=(uint16_t)(s->rate>>16);
for(i=0;i<count;++i)out[i]=w[off+i];
return 0;
}
unsigned set_wave_write(void *ctx,uint16_t off,uint16_t value) {
set_wave_state *s=(set_wave_state *)ctx;
if(!s||!s->samples||off>=9)return 2;
return write_register(s,SET_WAVE_BASE+off,value);
}
unsigned set_wave_samples_read(void *ctx,uint16_t off,uint16_t count,uint16_t *out) {
set_wave_state *s=(set_wave_state *)ctx;unsigned i;
if(!s||!s->samples||!out||!count||(uint32_t)off+count>s->received)return 2;
for(i=0;i<count;++i)out[i]=s->samples[off+i];
return 0;
}
unsigned set_wave_samples_write(void *ctx,uint16_t off,uint16_t value) {
set_wave_state *s=(set_wave_state *)ctx;
if(!s||!s->samples||off>=SET_WAVE_MAX)return 2;
return write_register(s,SET_WAVE_DATA+off,value);
}
size_t set_wave_rtu(set_wave_state *s,unsigned device,const uint8_t *req,size_t n,uint8_t *out,size_t cap) {
unsigned reg,value,error=0,i;uint16_t status[8];size_t length=0;
if(!s||!s->samples||!req||!out||cap<21||n!=8||device<1||device>247
||req[0]!=device||set_crc16_modbus(req,n)) return 0;
reg=read16(req+2);value=read16(req+4);
if(!((reg>=SET_WAVE_BASE&&reg<SET_WAVE_BASE+9)||(reg>=SET_WAVE_DATA&&reg<SET_WAVE_DATA+SET_WAVE_MAX))) return 0;
out[0]=(uint8_t)device;out[1]=req[1];
if(req[1]==6) {error=write_register(s,reg,value);memcpy(out,req,6);length=6;}
else if(req[1]==3) {
(void)set_wave_read(s,0,8,status);
if(reg==SET_WAVE_BASE&&value==8) {
out[2]=16;for(i=0;i<8;++i) write16(out+3+2*i,status[i]);length=19;
} else if(reg==SET_WAVE_BASE+8&&value==1) {
out[2]=2;write16(out+3,s->rate>>16);length=5;
} else if(reg>=SET_WAVE_DATA&&value==1&&reg-SET_WAVE_DATA<s->received) {
out[2]=2;write16(out+3,s->samples[reg-SET_WAVE_DATA]);length=5;
} else error=2;
} else error=1;
if(error) {out[1]|=0x80;out[2]=(uint8_t)error;length=3;}
return seal(out,length);
}
size_t set_wave_request(unsigned device,unsigned op,unsigned index,unsigned value,uint8_t *out,size_t cap) {
unsigned reg=SET_WAVE_BASE,fn=6;
if(!out||cap<8||device<1||device>247) return 0;
switch(op) {
case 0: fn=3;value=8;break;
case 1: reg+=2;value=0;break;
case 2: reg+=3;if(value>65535)return 0;break;
case 3: reg+=4;if(value<2||value>SET_WAVE_MAX)return 0;break;
case 4: if(index>=SET_WAVE_MAX||value>4095)return 0;reg=SET_WAVE_DATA+index;break;
case 5: reg+=6;value=0xA55A;break;
case 6: reg+=2;value=1;break;
case 7: if(index>=SET_WAVE_MAX)return 0;reg=SET_WAVE_DATA+index;fn=3;value=1;break;
case 8: reg+=8;fn=3;value=1;break;
case 9: reg+=8;if(value>(SET_WAVE_MAX_RATE>>16))return 0;break;
default:return 0;
}
out[0]=(uint8_t)device;out[1]=(uint8_t)fn;write16(out+2,reg);write16(out+4,value);return seal(out,6);
}
int set_wave_response(const uint8_t *req,const uint8_t *reply,size_t size,uint16_t *words,size_t cap) {
size_t expected,i;
if(!req||!reply||!words||set_crc16_modbus(req,8))return -1;
if(size<2)return 0;
if(reply[0]!=req[0])return -1;
if(reply[1]==(req[1]|0x80)) {
if(size<5)return 0;
return size==5&&!set_crc16_modbus(reply,5)?-(100+(int)reply[2]):-1;
}
if(reply[1]!=req[1])return -1;
expected=req[1]==6?8:5+2*(size_t)read16(req+4);
if(size<expected)return 0;
if(size!=expected||set_crc16_modbus(reply,size))return -1;
if(req[1]==6) {if(memcmp(req,reply,8)||cap<1)return -1;words[0]=read16(req+4);return 1;}
if(reply[2]!=expected-5||cap<(expected-5)/2)return -1;
for(i=0;i<(expected-5)/2;++i)words[i]=read16(reply+3+2*i);
if(read16(req+2)==SET_WAVE_BASE&&(words[0]!=0x5747||(words[1]<1||words[1]>3)))return -2;
return (int)((expected-5)/2);
}
size_t set_wave_block_request(unsigned device,unsigned index,const uint16_t *values,
size_t count,uint8_t *out,size_t cap) {
size_t i,n=values?9+2*count:8;
if(!out||device<1||device>247||!count||count>SET_WAVE_BLOCK_MAX||
index>=SET_WAVE_MAX||count>SET_WAVE_MAX-index||cap<n)return 0;
if(values)for(i=0;i<count;++i)if(values[i]>4095)return 0;
out[0]=(uint8_t)device;out[1]=values?16:3;
write16(out+2,SET_WAVE_DATA+index);write16(out+4,(unsigned)count);
if(values) {
out[6]=(uint8_t)(count*2);
for(i=0;i<count;++i)write16(out+7+2*i,values[i]);
}
return seal(out,n-2);
}
size_t set_wave_block_rtu(set_wave_state *s,unsigned device,const uint8_t *req,
size_t n,uint8_t *out,size_t cap) {
unsigned reg,index,count,error=0,i;size_t length=3;
if(!s||!s->samples||!req||!out||cap<5||n<8||device<1||device>247||
req[0]!=device||set_crc16_modbus(req,n))return 0;
reg=read16(req+2);count=read16(req+4);
if(reg<SET_WAVE_DATA||reg>=SET_WAVE_DATA+SET_WAVE_MAX)return 0;
index=reg-SET_WAVE_DATA;out[0]=req[0];out[1]=req[1];
if(!count||count>SET_WAVE_BLOCK_MAX||count>SET_WAVE_MAX-index||
index+count>s->count||index+count>s->capacity)error=3;
else if(req[1]==16) {
if(n!=9+2*(size_t)count||req[6]!=2*count)error=3;
else if(s->running)error=6;
else if(s->ready||index>s->received)error=3;
else {
/* Preflight the whole block, including replayed prefix, before writing. */
for(i=0;i<count;++i) {
unsigned value=read16(req+7+2*i);
if(value>4095||(index+i<s->received&&s->samples[index+i]!=value)) {
error=3;break;
}
}
if(!error) {
if(cap<8)return 0;
for(i=0;i<count;++i)s->samples[index+i]=read16(req+7+2*i);
if(index+count>s->received)s->received=(uint16_t)(index+count);
memcpy(out,req,6);length=6;
}
}
} else if(req[1]==3) {
if(n!=8||index+count>s->received)error=3;
else {
if(cap<5+2*(size_t)count)return 0;
out[2]=(uint8_t)(2*count);
for(i=0;i<count;++i)write16(out+3+2*i,s->samples[index+i]);
length=3+2*(size_t)count;
}
} else error=1;
if(error) {out[1]|=0x80;out[2]=(uint8_t)error;length=3;}
return seal(out,length);
}
int set_wave_block_response(const uint8_t *req,size_t n,const uint8_t *reply,
size_t size,uint16_t *words,size_t cap) {
size_t expected,i;unsigned count;
if(!req||!reply||!words||n<8||set_crc16_modbus(req,n))return -1;
count=read16(req+4);
if(!count||count>SET_WAVE_BLOCK_MAX||read16(req+2)<SET_WAVE_DATA||
(unsigned)read16(req+2)+count>SET_WAVE_DATA+SET_WAVE_MAX)return -1;
if((req[1]==16&&(n!=9+2*(size_t)count||req[6]!=2*count))||
(req[1]==3&&n!=8)||(req[1]!=3&&req[1]!=16))return -1;
if(size<2)return 0;
if(reply[0]!=req[0])return -1;
if(reply[1]==(req[1]|0x80)) {
if(size<5)return 0;
return size==5&&!set_crc16_modbus(reply,size)?-(100+(int)reply[2]):-1;
}
if(reply[1]!=req[1])return -1;
expected=req[1]==16?8:5+2*(size_t)count;
if(size<expected)return 0;
if(size!=expected||set_crc16_modbus(reply,size))return -1;
if(req[1]==16) {
if(cap<1||memcmp(req,reply,6))return -1;
words[0]=(uint16_t)count;return 1;
}
if(cap<count||reply[2]!=2*count)return -1;
for(i=0;i<count;++i)words[i]=read16(reply+3+2*i);
return (int)count;
}
c/set-protocol/src/ump_protocol.c
+105
View File
@@ -0,0 +1,105 @@
#include "../include/ump_protocol.h"
static uint16_t get16(const ump_octet *p)
{ return (uint16_t)(((p[0]&255U)<<8)|(p[1]&255U)); }
static void put16(ump_octet *p, uint16_t v)
{ p[0] = v>>8; p[1] = v&255U; }
unsigned ump_validate(unsigned fn, uint16_t a, uint16_t n, int can)
{
if (fn == 6) return a >= UMP_BASE && a <= UMP_BASE+3 ? 0 : 2;
if (fn != 3) return 1;
if (can) return ((a == UMP_BASE && n == 16) ||
(a == UMP_DATA && (n == 40 || n == 80 || n == 120)) ||
(a == UMP_LIVE && n == 124)) ? 0 : 2;
if (!n || n > 125) return 2;
return ((a >= UMP_BASE && a < UMP_BASE+16 && n <= UMP_BASE+16-a) ||
(a >= UMP_DATA && a < UMP_DATA+120 && n <= UMP_DATA+120-a) ||
(a == UMP_LIVE && n == 124)) ? 0 : 2;
}
unsigned ump_prepare(const ump_backend *b, unsigned fn, uint16_t a,
uint16_t n, int can, uint16_t *out, unsigned *error)
{
unsigned i;
if (!out || !error) return 0;
*error = ump_validate(fn,a,n,can);
if (!*error) {
if (!b || (fn == 3 ? !b->read : !b->write)) *error = 3;
else if (fn == 3) {
for (i=0;i<n;i++) out[i] = b->read(b->context,(uint16_t)(a+i));
return n;
} else if (!b->write(b->context,a,n)) *error = 3;
}
out[0]=a; out[1]=n;
return 2;
}
uint16_t ump_crc(const ump_octet *p, size_t n)
{
uint16_t crc=0xFFFFU;
unsigned i;
while (n--) {
crc ^= *p++ & 255U;
for (i=0;i<8;i++) crc=(uint16_t)((crc>>1)^((crc&1U)?0xA001U:0));
}
return crc;
}
uint32_t ump_can_id(unsigned mode, unsigned kind)
{ return mode>=1 && mode<=16 && kind<=2 ? 0xBA0000UL+kind*16UL+mode-1 : 0; }
unsigned ump_can_request(const ump_backend *b, const ump_octet *r, size_t size,
uint16_t *out, unsigned *token, unsigned *error)
{
size_t i;
if (!r || !out || !token || !error || size!=8) return 0;
for (i=0;i<size;i++) if (r[i]>255) return 0;
if (get16(r+4)!=UMP_MARKER || !r[6]) return 0;
*token=r[6];
return ump_prepare(b,r[7],get16(r),get16(r+2),1,out,error);
}
unsigned ump_can_fragment(const uint16_t *words, unsigned count, unsigned index,
unsigned token, unsigned error, ump_octet *out)
{
if (!words || !out || count<2 || count>124 || (count&1) ||
index>=count/2 || !token || token>255 || error>255) return 0;
put16(out,words[index*2]); put16(out+2,words[index*2+1]);
put16(out+4,(uint16_t)(UMP_MARKER+index)); out[6]=(ump_octet)token; out[7]=(ump_octet)error;
return 8;
}
size_t ump_rtu(const ump_backend *b, unsigned device, const ump_octet *r,
size_t size, ump_octet *out, size_t capacity)
{
unsigned i;
if (!r || !out || size!=8 || capacity<UMP_RTU_CAPACITY ||
!device || device>247 || r[0]!=device) return 0;
for (i=0;i<8;i++) if (r[i]>255) return 0;
if (ump_crc(r,8)) return 0;
return ump_rtu_response(b,device,r[1],get16(r+2),get16(r+4),out,capacity);
}
size_t ump_rtu_response(const ump_backend *b, unsigned device, unsigned fn,
uint16_t address, uint16_t value, ump_octet *out, size_t capacity)
{
uint16_t words[UMP_MAX_WORDS];
unsigned count,error;
if (!out || capacity<UMP_RTU_CAPACITY || !device || device>247 || fn>127) return 0;
count=ump_prepare(b,fn,address,value,0,words,&error);
return ump_rtu_encode(device,fn,error,words,count,out,capacity);
}
size_t ump_rtu_encode(unsigned device, unsigned fn, unsigned error,
const uint16_t *words, unsigned count, ump_octet *out, size_t capacity)
{
unsigned i;
size_t n;
uint16_t crc;
if (!out || !device || device>247 || fn>127 || error>255) return 0;
if (!error && (!words || (fn!=3 && fn!=6) ||
(fn==3 && (!count || count>124)) || (fn==6 && count!=2))) return 0;
n=error ? 5 : (fn==6 ? 8 : 5+count*2);
if (capacity<n) return 0;
out[0]=(ump_octet)device; out[1]=(ump_octet)fn;
if (error) { out[1]|=128; out[2]=(ump_octet)error; n=3; }
else if (fn==6) { put16(out+2,words[0]); put16(out+4,words[1]); n=6; }
else {
out[2]=(ump_octet)(count*2);
for (i=0;i<count;i++) put16(out+3+i*2,words[i]);
n=3+count*2;
}
crc=ump_crc(out,n); out[n++]=crc&255U; out[n++]=crc>>8;
return n;
}