111 lines
4.4 KiB
C
111 lines
4.4 KiB
C
/*
|
||
* Регистровый протокол УМП для UART/CAN с общей проверкой функций и диапазонов. Октеты
|
||
* извлекаются явно, чтобы формат оставался одинаковым на восьмибитных и C28x-платформах.
|
||
*/
|
||
|
||
#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 || n == 42 || n == 84)) ||
|
||
(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;
|
||
}
|