97 lines
3.7 KiB
C
97 lines
3.7 KiB
C
#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;
|
|
}
|