Skip to content

Commit 670a339

Browse files
BsAtHomeBsAtHome
committed
Implement byte-sized Modbus integer types.
Fix pin name size inconsistency. Fix unsigned masking because the read data is always sign extended. Add missing error checks to map function calls.
1 parent a90a4e6 commit 670a339

3 files changed

Lines changed: 138 additions & 66 deletions

File tree

‎src/hal/drivers/mesa-hostmot2/hm2_modbus.c‎

Lines changed: 110 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -98,6 +98,8 @@ static const char *error_codes[] = {
9898
#define MBT_FEHGBADC 0x0b
9999
#define MBT_GHEFCDAB 0x0c
100100
#define MBT_HGFEDCBA 0x0d
101+
#define MBT_A 0x0e
102+
#define MBT_B 0x0f
101103

102104
#define MBT_U 0x00 // Which makes default U_AB zero
103105
#define MBT_S 0x10
@@ -106,10 +108,14 @@ static const char *error_codes[] = {
106108
#define MBT_X_MASK 0x0f
107109
#define MBT_T_MASK 0xf0
108110

109-
static inline bool mtypeiscompound(unsigned mtype) { return mtype >= MBT_ABCD; }
110-
static inline bool mtypeisvalid(unsigned mtype) { return (mtype & MBT_X_MASK) <= MBT_HGFEDCBA && (mtype & MBT_T_MASK) <= MBT_F; }
111+
static inline bool mtypeiscompound(unsigned mtype) { return mtype >= MBT_ABCD && mtype <= MBT_HGFEDCBA; }
111112
static inline unsigned mtypeformat(unsigned mtype) { return mtype & MBT_X_MASK; }
112113
static inline unsigned mtypetype(unsigned mtype) { return mtype & MBT_T_MASK; }
114+
static inline bool mtypeisvalid(unsigned mtype) {
115+
// excludes 8-bit floats
116+
return mtypetype(mtype) <= MBT_F &&
117+
!((mtypeformat(mtype) == MBT_A || mtypeformat(mtype) == MBT_B) && mtypetype(mtype) == MBT_F);
118+
}
113119
static inline unsigned mtypesize(unsigned mtype) {
114120
static const rtapi_u8 s[16] = {1, 1, 2, 2, 2, 2, 4, 4, 4, 4, 4, 4, 4, 4, 1, 1};
115121
return s[mtypeformat(mtype)];
@@ -1180,6 +1186,12 @@ static int map_u(hm2_modbus_cmd_t *cc, rtapi_u64 v, unsigned tidx)
11801186
mb_types64_u v64;
11811187
unsigned fmt = mtypeformat(cc->typeptr[tidx].mtype);
11821188
switch(fmt) {
1189+
case MBT_A:
1190+
case MBT_B:
1191+
if(haspinclamp(&cc->typeptr[tidx]) && v > RTAPI_UINT8_MAX) v = RTAPI_UINT8_MAX;
1192+
CHK_RV(ch_append16_sw(cc, (rtapi_u16)v & 0xff, fmt == MBT_B));
1193+
break;
1194+
11831195
case MBT_AB:
11841196
case MBT_BA:
11851197
if(haspinclamp(&cc->typeptr[tidx]) && v > RTAPI_UINT16_MAX) v = RTAPI_UINT16_MAX;
@@ -1217,6 +1229,13 @@ static int map_s(hm2_modbus_cmd_t *cc, rtapi_s64 v, unsigned tidx)
12171229
mb_types64_u v64;
12181230
unsigned fmt = mtypeformat(cc->typeptr[tidx].mtype);
12191231
switch(fmt) {
1232+
case MBT_A:
1233+
case MBT_B:
1234+
if(haspinclamp(&cc->typeptr[tidx]) && v > RTAPI_INT8_MAX) v = RTAPI_INT8_MAX;
1235+
if(haspinclamp(&cc->typeptr[tidx]) && v < RTAPI_INT8_MIN) v = RTAPI_INT8_MIN;
1236+
CHK_RV(ch_append16_sw(cc, (rtapi_u16)v & 0xff, fmt == MBT_B));
1237+
break;
1238+
12201239
case MBT_AB:
12211240
case MBT_BA:
12221241
if(haspinclamp(&cc->typeptr[tidx]) && v > RTAPI_INT16_MAX) v = RTAPI_INT16_MAX;
@@ -1257,6 +1276,11 @@ static int map_f(hm2_modbus_cmd_t *cc, double v, unsigned tidx)
12571276
rtapi_u16 w;
12581277
unsigned fmt = mtypeformat(cc->typeptr[tidx].mtype);
12591278
switch(fmt) {
1279+
case MBT_A:
1280+
case MBT_B:
1281+
// This should have been caught in reading the mbccb
1282+
return -EINVAL;
1283+
12601284
case MBT_AB:
12611285
case MBT_BA:
12621286
v64.f = v;
@@ -1382,67 +1406,67 @@ static int build_data_frame(hm2_modbus_inst_t *inst)
13821406
CHK_RV(ch_append16(cc, cc->cmd.caddr));
13831407
switch(cc->typeptr[0].htype) {
13841408
case HAL_BIT:
1385-
map_u(cc, hal->pins[p].pin->b ? 1 : 0, 0);
1409+
CHK_RV(map_u(cc, hal->pins[p].pin->b ? 1 : 0, 0));
13861410
break;
13871411
case HAL_U32:
13881412
switch(mtypetype(cc->typeptr[0].mtype)) {
1389-
case MBT_U: map_u(cc, hal->pins[p].pin->u, 0); break;
1390-
case MBT_S: map_s(cc, map_us(hal->pins[p].pin->u), 0); break;
1391-
case MBT_F: map_f(cc, map_uf(hal->pins[p].pin->u), 0); break;
1413+
case MBT_U: CHK_RV(map_u(cc, hal->pins[p].pin->u, 0)); break;
1414+
case MBT_S: CHK_RV(map_s(cc, map_us(hal->pins[p].pin->u), 0)); break;
1415+
case MBT_F: CHK_RV(map_f(cc, map_uf(hal->pins[p].pin->u), 0)); break;
13921416
}
13931417
break;
13941418
case HAL_U64:
13951419
switch(mtypetype(cc->typeptr[0].mtype)) {
1396-
case MBT_U: map_u(cc, hal->pins[p].pin->lu, 0); break;
1397-
case MBT_S: map_s(cc, map_us(hal->pins[p].pin->lu), 0); break;
1398-
case MBT_F: map_f(cc, map_uf(hal->pins[p].pin->lu), 0); break;
1420+
case MBT_U: CHK_RV(map_u(cc, hal->pins[p].pin->lu, 0)); break;
1421+
case MBT_S: CHK_RV(map_s(cc, map_us(hal->pins[p].pin->lu), 0)); break;
1422+
case MBT_F: CHK_RV(map_f(cc, map_uf(hal->pins[p].pin->lu), 0)); break;
13991423
}
14001424
break;
14011425
case HAL_S32:
14021426
if(!haspinscale(&cc->typeptr[0])) {
14031427
switch(mtypetype(cc->typeptr[0].mtype)) {
1404-
case MBT_U: map_u(cc, map_su(hal->pins[p].pin->s), 0); break;
1405-
case MBT_S: map_s(cc, hal->pins[p].pin->s, 0); break;
1406-
case MBT_F: map_f(cc, map_sf(hal->pins[p].pin->s), 0); break;
1428+
case MBT_U: CHK_RV(map_u(cc, map_su(hal->pins[p].pin->s), 0)); break;
1429+
case MBT_S: CHK_RV(map_s(cc, hal->pins[p].pin->s, 0)); break;
1430+
case MBT_F: CHK_RV(map_f(cc, map_sf(hal->pins[p].pin->s), 0)); break;
14071431
}
14081432
} else {
14091433
val64.f = (real_t)((rtapi_s64)hal->pins[p].pin->s - hal->pins[p].offset->s) * *(hal->pins[p].scale);
14101434
switch(mtypetype(cc->typeptr[0].mtype)) {
1411-
case MBT_U: map_u(cc, map_fu(val64.f), 0); break;
1412-
case MBT_S: map_s(cc, map_fs(val64.f), 0); break;
1413-
case MBT_F: map_f(cc, val64.f, 0); break;
1435+
case MBT_U: CHK_RV(map_u(cc, map_fu(val64.f), 0)); break;
1436+
case MBT_S: CHK_RV(map_s(cc, map_fs(val64.f), 0)); break;
1437+
case MBT_F: CHK_RV(map_f(cc, val64.f, 0)); break;
14141438
}
14151439
}
14161440
break;
14171441
case HAL_S64:
14181442
if(!haspinscale(&cc->typeptr[0])) {
14191443
switch(mtypetype(cc->typeptr[0].mtype)) {
1420-
case MBT_U: map_u(cc, map_su(hal->pins[p].pin->ls), 0); break;
1421-
case MBT_S: map_s(cc, hal->pins[p].pin->ls, 0); break;
1422-
case MBT_F: map_f(cc, map_sf(hal->pins[p].pin->ls), 0); break;
1444+
case MBT_U: CHK_RV(map_u(cc, map_su(hal->pins[p].pin->ls), 0)); break;
1445+
case MBT_S: CHK_RV(map_s(cc, hal->pins[p].pin->ls, 0)); break;
1446+
case MBT_F: CHK_RV(map_f(cc, map_sf(hal->pins[p].pin->ls), 0)); break;
14231447
}
14241448
} else {
14251449
val64.f = (real_t)(hal->pins[p].pin->ls - hal->pins[p].offset->ls) * *(hal->pins[p].scale);
14261450
switch(mtypetype(cc->typeptr[0].mtype)) {
1427-
case MBT_U: map_u(cc, map_fu(val64.f), 0); break;
1428-
case MBT_S: map_s(cc, map_fs(val64.f), 0); break;
1429-
case MBT_F: map_f(cc, val64.f, 0); break;
1451+
case MBT_U: CHK_RV(map_u(cc, map_fu(val64.f), 0)); break;
1452+
case MBT_S: CHK_RV(map_s(cc, map_fs(val64.f), 0)); break;
1453+
case MBT_F: CHK_RV(map_f(cc, val64.f, 0)); break;
14301454
}
14311455
}
14321456
break;
14331457
case HAL_FLOAT:
14341458
if(!haspinscale(&cc->typeptr[0])) {
14351459
switch(mtypetype(cc->typeptr[0].mtype)) {
1436-
case MBT_U: map_u(cc, map_fu(hal->pins[p].pin->f), 0); break;
1437-
case MBT_S: map_s(cc, map_fs(hal->pins[p].pin->f), 0); break;
1438-
case MBT_F: map_f(cc, hal->pins[p].pin->f, 0); break;
1460+
case MBT_U: CHK_RV(map_u(cc, map_fu(hal->pins[p].pin->f), 0)); break;
1461+
case MBT_S: CHK_RV(map_s(cc, map_fs(hal->pins[p].pin->f), 0)); break;
1462+
case MBT_F: CHK_RV(map_f(cc, hal->pins[p].pin->f, 0)); break;
14391463
}
14401464
} else {
14411465
val64.f = (hal->pins[p].pin->f - hal->pins[p].offset->f) * *(hal->pins[p].scale);
14421466
switch(mtypetype(cc->typeptr[0].mtype)) {
1443-
case MBT_U: map_u(cc, map_fu(val64.f), 0); break;
1444-
case MBT_S: map_s(cc, map_fs(val64.f), 0); break;
1445-
case MBT_F: map_f(cc, val64.f, 0); break;
1467+
case MBT_U: CHK_RV(map_u(cc, map_fu(val64.f), 0)); break;
1468+
case MBT_S: CHK_RV(map_s(cc, map_fs(val64.f), 0)); break;
1469+
case MBT_F: CHK_RV(map_f(cc, val64.f, 0)); break;
14461470
}
14471471
}
14481472
break;
@@ -1478,67 +1502,67 @@ static int build_data_frame(hm2_modbus_inst_t *inst)
14781502
}
14791503
switch(cc->typeptr[i].htype) {
14801504
case HAL_BIT:
1481-
map_u(cc, hal->pins[p].pin->b ? 1 : 0, i);
1505+
CHK_RV(map_u(cc, hal->pins[p].pin->b ? 1 : 0, i));
14821506
break;
14831507
case HAL_U32:
14841508
switch(mtypetype(cc->typeptr[i].mtype)) {
1485-
case MBT_U: map_u(cc, hal->pins[p].pin->u, i); break;
1486-
case MBT_S: map_s(cc, map_us(hal->pins[p].pin->u), i); break;
1487-
case MBT_F: map_f(cc, map_uf(hal->pins[p].pin->u), i); break;
1509+
case MBT_U: CHK_RV(map_u(cc, hal->pins[p].pin->u, i)); break;
1510+
case MBT_S: CHK_RV(map_s(cc, map_us(hal->pins[p].pin->u), i)); break;
1511+
case MBT_F: CHK_RV(map_f(cc, map_uf(hal->pins[p].pin->u), i)); break;
14881512
}
14891513
break;
14901514
case HAL_U64:
14911515
switch(mtypetype(cc->typeptr[i].mtype)) {
1492-
case MBT_U: map_u(cc, hal->pins[p].pin->lu, i); break;
1493-
case MBT_S: map_s(cc, map_us(hal->pins[p].pin->lu), i); break;
1494-
case MBT_F: map_f(cc, map_uf(hal->pins[p].pin->lu), i); break;
1516+
case MBT_U: CHK_RV(map_u(cc, hal->pins[p].pin->lu, i)); break;
1517+
case MBT_S: CHK_RV(map_s(cc, map_us(hal->pins[p].pin->lu), i)); break;
1518+
case MBT_F: CHK_RV(map_f(cc, map_uf(hal->pins[p].pin->lu), i)); break;
14951519
}
14961520
break;
14971521
case HAL_S32:
14981522
if(!haspinscale(&cc->typeptr[i])) {
14991523
switch(mtypetype(cc->typeptr[i].mtype)) {
1500-
case MBT_U: map_u(cc, map_su(hal->pins[p].pin->s), i); break;
1501-
case MBT_S: map_s(cc, hal->pins[p].pin->s, i); break;
1502-
case MBT_F: map_f(cc, map_sf(hal->pins[p].pin->s), i); break;
1524+
case MBT_U: CHK_RV(map_u(cc, map_su(hal->pins[p].pin->s), i)); break;
1525+
case MBT_S: CHK_RV(map_s(cc, hal->pins[p].pin->s, i)); break;
1526+
case MBT_F: CHK_RV(map_f(cc, map_sf(hal->pins[p].pin->s), i)); break;
15031527
}
15041528
} else {
15051529
val64.f = (real_t)((rtapi_s64)hal->pins[p].pin->s - hal->pins[p].offset->s) * *(hal->pins[p].scale);
15061530
switch(mtypetype(cc->typeptr[i].mtype)) {
1507-
case MBT_U: map_u(cc, map_fu(val64.f), i); break;
1508-
case MBT_S: map_s(cc, map_fs(val64.f), i); break;
1509-
case MBT_F: map_f(cc, val64.f, i); break;
1531+
case MBT_U: CHK_RV(map_u(cc, map_fu(val64.f), i)); break;
1532+
case MBT_S: CHK_RV(map_s(cc, map_fs(val64.f), i)); break;
1533+
case MBT_F: CHK_RV(map_f(cc, val64.f, i)); break;
15101534
}
15111535
}
15121536
break;
15131537
case HAL_S64:
15141538
if(!haspinscale(&cc->typeptr[i])) {
15151539
switch(mtypetype(cc->typeptr[i].mtype)) {
1516-
case MBT_U: map_u(cc, map_su(hal->pins[p].pin->ls), i); break;
1517-
case MBT_S: map_s(cc, hal->pins[p].pin->ls, i); break;
1518-
case MBT_F: map_f(cc, map_sf(hal->pins[p].pin->ls), i); break;
1540+
case MBT_U: CHK_RV(map_u(cc, map_su(hal->pins[p].pin->ls), i)); break;
1541+
case MBT_S: CHK_RV(map_s(cc, hal->pins[p].pin->ls, i)); break;
1542+
case MBT_F: CHK_RV(map_f(cc, map_sf(hal->pins[p].pin->ls), i)); break;
15191543
}
15201544
} else {
15211545
val64.f = (real_t)(hal->pins[p].pin->ls - hal->pins[p].offset->ls) * *(hal->pins[p].scale);
15221546
switch(mtypetype(cc->typeptr[i].mtype)) {
1523-
case MBT_U: map_u(cc, map_fu(val64.f), i); break;
1524-
case MBT_S: map_s(cc, map_fs(val64.f), i); break;
1525-
case MBT_F: map_f(cc, val64.f, i); break;
1547+
case MBT_U: CHK_RV(map_u(cc, map_fu(val64.f), i)); break;
1548+
case MBT_S: CHK_RV(map_s(cc, map_fs(val64.f), i)); break;
1549+
case MBT_F: CHK_RV(map_f(cc, val64.f, i)); break;
15261550
}
15271551
}
15281552
break;
15291553
case HAL_FLOAT:
15301554
if(!haspinscale(&cc->typeptr[i])) {
15311555
switch(mtypetype(cc->typeptr[i].mtype)) {
1532-
case MBT_U: map_u(cc, map_fu(hal->pins[p].pin->f), i); break;
1533-
case MBT_S: map_s(cc, map_fs(hal->pins[p].pin->f), i); break;
1534-
case MBT_F: map_f(cc, hal->pins[p].pin->f, i); break;
1556+
case MBT_U: CHK_RV(map_u(cc, map_fu(hal->pins[p].pin->f), i)); break;
1557+
case MBT_S: CHK_RV(map_s(cc, map_fs(hal->pins[p].pin->f), i)); break;
1558+
case MBT_F: CHK_RV(map_f(cc, hal->pins[p].pin->f, i)); break;
15351559
}
15361560
} else {
15371561
val64.f = (hal->pins[p].pin->f - hal->pins[p].offset->f) * *(hal->pins[p].scale);
15381562
switch(mtypetype(cc->typeptr[i].mtype)) {
1539-
case MBT_U: map_u(cc, map_fu(val64.f), i); break;
1540-
case MBT_S: map_s(cc, map_fs(val64.f), i); break;
1541-
case MBT_F: map_f(cc, val64.f, i); break;
1563+
case MBT_U: CHK_RV(map_u(cc, map_fu(val64.f), i)); break;
1564+
case MBT_S: CHK_RV(map_s(cc, map_fs(val64.f), i)); break;
1565+
case MBT_F: CHK_RV(map_f(cc, val64.f, i)); break;
15421566
}
15431567
}
15441568
break;
@@ -1606,6 +1630,25 @@ static inline mb_types64_u get64(rtapi_u8 *b, unsigned mtype)
16061630
return v;
16071631
}
16081632

1633+
static inline rtapi_u64 mask_mbtsize(unsigned mtype, rtapi_u64 v)
1634+
{
1635+
switch(mtypeformat(mtype)) {
1636+
case MBT_A:
1637+
case MBT_B:
1638+
return v & 0xff;
1639+
case MBT_AB:
1640+
case MBT_BA:
1641+
return v & 0xffff;
1642+
case MBT_ABCD:
1643+
case MBT_BADC:
1644+
case MBT_CDAB:
1645+
case MBT_DCBA:
1646+
return v & 0xffffffff;
1647+
default:
1648+
return v;
1649+
}
1650+
}
1651+
16091652
static inline rtapi_u32 unmap32_uu(const hm2_modbus_cmd_t *cc, rtapi_u64 v, unsigned tidx)
16101653
{
16111654
if(haspinclamp(&cc->typeptr[tidx]) && v > RTAPI_UINT32_MAX)
@@ -1790,6 +1833,12 @@ static int parse_data_frame(hm2_modbus_inst_t *inst)
17901833

17911834
// Read bytes according to the size of the mtype
17921835
switch(mtypeformat(cc->typeptr[i].mtype)) {
1836+
case MBT_A: // Always sign-extended
1837+
val64.s = (rtapi_s64)(rtapi_s8)bytes[pos+1];
1838+
break;
1839+
case MBT_B: // Always sign-extended
1840+
val64.s = (rtapi_s64)(rtapi_s8)bytes[pos];
1841+
break;
17931842
case MBT_AB: // Always sign-extended
17941843
val64.s = 256 * (rtapi_s64)(rtapi_s8)bytes[pos] + bytes[pos+1];
17951844
break;
@@ -1860,6 +1909,15 @@ static int parse_data_frame(hm2_modbus_inst_t *inst)
18601909
// val64.s is the signed value for MBT_S
18611910
// val64.f is the (promoted) fp value for MBT_F
18621911

1912+
// If the source is unsigned and the size smaller than the HAL
1913+
// target, then we need to mask the high bits.
1914+
// The reason is that we did an unconditional sign-extension above
1915+
// that will interfere with unsigned values with the high-bit set
1916+
// that are smaller than the HAL target.
1917+
if(MBT_U == mtypetype(cc->typeptr[i].mtype)) {
1918+
val64.u = mask_mbtsize(cc->typeptr[i].mtype, val64.u);
1919+
}
1920+
18631921
switch(cc->typeptr[i].htype) {
18641922
case HAL_BIT:
18651923
hal->pins[p].pin->b = 0 != val64.u; // Zero maps to false, anything else to true

‎src/hal/drivers/mesa-hostmot2/hm2_modbus.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@
2525
// Max one minute delay between init commands (in microseconds)
2626
#define MAXDELAY 60000000
2727
// Max chars for a name
28-
#define MAXPINNAME 24
28+
#define MAXPINNAME 32
2929
//
3030
// 16*4 byte structure
3131
// All values in Big-Endian

0 commit comments

Comments
 (0)