| 提交 | 用户 | age | ||
| 483170 | 1 | /** |
| Q | 2 | ****************************************************************************** |
| 3 | * @file : ModbusRTU.c | |
| 4 | * @brief : Modbus RTU Protocol program body | |
| 5 | ****************************************************************************** | |
| 6 | */ | |
| 7 | #include "ModbusRTU.h" | |
| 8 | #include "functions.h" | |
| 9 | #include "string.h" | |
| 10 | #include "Kmachine.h" | |
| 11 | ||
| 12 | static inline unsigned char HiByte(uint16_t in) | |
| 13 | { | |
| 14 | return (in>>8)&0xff; | |
| 15 | } | |
| 16 | ||
| 17 | static inline unsigned char LoByte(uint16_t in) | |
| 18 | { | |
| 19 | return (in&0xff); | |
| 20 | } | |
| 21 | static inline uint16_t swaphilo(uint16_t in) | |
| 22 | { | |
| 23 | return ((in&0xff)<<8)|((in>>8)&0xff); | |
| 24 | } | |
| 25 | ||
| 26 | static inline uint16_t mkshort(uint8_t hi, uint8_t lo) | |
| 27 | { | |
| 28 | return ((uint16_t)hi<<8)|lo; | |
| 29 | } | |
| 30 | ||
| 31 | uint8_t MyAddr = 1; | |
| 32 | int ModBusCRC16(void * pBuf, int len1) | |
| 33 | { | |
| 34 | return 0; | |
| 35 | } | |
| 36 | ||
| 37 | //const uint16_t polynom = 0xA001; | |
| 38 | /* | |
| 39 | uint16_t crc16bitbybit(uint8_t *ptr, uint16_t len) | |
| 40 | { | |
| 41 | uint8_t i; | |
| 42 | uint16_t crc = 0xffff; | |
| 43 | ||
| 44 | if (len == 0) { | |
| 45 | len = 1; | |
| 46 | } | |
| 47 | while (len--) { | |
| 48 | crc ^= *ptr; | |
| 49 | for (i = 0; i<8; i++) | |
| 50 | { | |
| 51 | if (crc & 1) { | |
| 52 | crc >>= 1; | |
| 53 | crc ^= polynom; | |
| 54 | } | |
| 55 | else { | |
| 56 | crc >>= 1; | |
| 57 | } | |
| 58 | } | |
| 59 | ptr++; | |
| 60 | } | |
| 61 | return(crc); | |
| 62 | } | |
| 63 | */ | |
| 64 | const uint16_t crctalbeabs[] = { | |
| 65 | 0x0000, 0xCC01, 0xD801, 0x1400, 0xF001, 0x3C00, 0x2800, 0xE401, | |
| 66 | 0xA001, 0x6C00, 0x7800, 0xB401, 0x5000, 0x9C01, 0x8801, 0x4400 | |
| 67 | }; | |
| 68 | ||
| 69 | uint16_t crc16tablefast(const uint8_t *ptr, uint16_t len) | |
| 70 | { | |
| 71 | uint16_t crc = 0xffff; | |
| 72 | uint16_t i; | |
| 73 | uint8_t ch; | |
| 74 | ||
| 75 | for (i = 0; i < len; i++) { | |
| 76 | ch = *ptr++; | |
| 77 | crc = crctalbeabs[(ch ^ crc) & 15] ^ (crc >> 4); | |
| 78 | crc = crctalbeabs[((ch >> 4) ^ crc) & 15] ^ (crc >> 4); | |
| 79 | } | |
| 80 | ||
| 81 | return crc; | |
| 82 | } | |
| 83 | ||
| 84 | //********************************************************************** | |
| 85 | //****************** Modbus slave ************************************ | |
| 86 | ||
| 87 | int ModBusSlaveCheckPkg(int nChn, void *ptr, uint16_t len1) | |
| 88 | { | |
| 89 | if (len1 <=4) return -1; //包长 | |
| 90 | pModBusRTUReqPkg pPkg = (pModBusRTUReqPkg) ptr; | |
| 91 | if (pPkg->Dst >127) return -2; //地址码 | |
| 92 | if ((pPkg->Cmd&0x7f) > 0x1f) return -3; //功能码 | |
| 93 | uint16_t crc = crc16tablefast(ptr,len1); //CRC 校验 | |
| 94 | if (crc != 0 ) return 4; //CRC 校验错误 | |
| 95 | return S_OK; | |
| 96 | } | |
| 97 | ||
| 98 | int mkReqPkg(uint8_t *ptr, uint8_t opcode, uint8_t nsize, uint8_t *buf) | |
| 99 | { | |
| 100 | int len1=0; | |
| 101 | ||
| 102 | return len1; | |
| 103 | ||
| 104 | } | |
| 105 | /* | |
| 106 | static const unsigned short bitMasks[16]= | |
| 107 | { | |
| 108 | 0x1<<0, 0x1<<1, 0x1<<2, 0x1<<3, 0x1<<4, 0x1<<5, 0x1<<6, 0x1<<7, | |
| 109 | 0x1<<8, 0x1<<9, 0x1<<10, 0x1<<11, 0x1<<12, 0x1<<13, 0x1<<14, 0x1<<15, | |
| 110 | }; | |
| 111 | */ | |
| 112 | static inline void SetAddrBit(unsigned short * pW, unsigned char bitAddr) | |
| 113 | { | |
| 114 | (*pW)|=1<<(bitAddr&0xf); | |
| 115 | } | |
| 116 | ||
| 117 | static inline void ResetBit(unsigned short * pW, unsigned char bitAddr) | |
| 118 | { | |
| 119 | (*pW)&=~(1<<(bitAddr&0xf)); | |
| 120 | } | |
| 121 | ||
| 122 | static inline void SetBitValue(unsigned short * pW, unsigned char bitAddr, unsigned char Value) | |
| 123 | { | |
| 124 | if (Value) { SetAddrBit(pW, bitAddr);} | |
| 125 | else {ResetBit(pW, bitAddr);} | |
| 126 | } | |
| 127 | ||
| 128 | static inline unsigned char GetBitValue(unsigned short W, unsigned char bitAddr) | |
| 129 | { | |
| 130 | if (W&(1<<(bitAddr&0xf))) return 1; | |
| 131 | else return 0; | |
| 132 | } | |
| 133 | ||
| 134 | uint8_t Pkgbuf[128]; | |
| 135 | ||
| 136 | int ModBusSlaveParsePkg(int nChn, void *ptr, uint16_t len) | |
| 137 | { | |
| 138 | uchar * bptr = (uchar *) ptr; | |
| 139 | int err=ModBusSlaveCheckPkg(nChn, ptr, len); | |
| 140 | if (err) return -1; | |
| 141 | pModBusRTUReqPkg pPkg = (pModBusRTUReqPkg) ptr; | |
| 142 | uint8_t DstHost=pPkg->Dst; | |
| 143 | if (DstHost != MyAddr) return 0; | |
| 144 | uint8_t opcode = pPkg->Cmd; | |
| 145 | ushort Addr=mkshort(pPkg->AddrH,pPkg->AddrL); | |
| 146 | ushort nCount=swaphilo(pPkg->nCount); | |
| 147 | Pkgbuf[0]=MyAddr; | |
| 148 | Pkgbuf[1]=opcode; | |
| 149 | int len2=2; | |
| 150 | int needcrc=1; | |
| 151 | uint16_t crc; | |
| 152 | int nByteCount; | |
| 153 | unsigned short Addr1=(Addr&0xff0)>>4; | |
| 154 | unsigned char bitAddr=Addr&0xf; | |
| 155 | unsigned char thisValue; | |
| 156 | unsigned char bitmask=1; | |
| 157 | switch (opcode) | |
| 158 | { | |
| 159 | case None: //0 | |
| 160 | break; | |
| 161 | case ReadCoils: //1 bits | |
| 162 | // Fetch Datas; | |
| 163 | nByteCount=(nCount+7)/8; | |
| 164 | Pkgbuf[2]=nByteCount; | |
| 165 | for (int i=0;i<nByteCount;i++) | |
| 166 | { | |
| 167 | thisValue=0; | |
| 168 | bitmask=1; | |
| 169 | for (int j=0;j<8&&i*8+j<nCount;j++) | |
| 170 | { | |
| 8b51c7 | 171 | Addr1=(Addr&0xfff0)>>4; |
| 483170 | 172 | bitAddr=Addr&0xf; |
| 8b51c7 | 173 | if (Addr < 2048) { |
| Q | 174 | if (GetBitValue( KMem.WY[Addr1],bitAddr)) |
| 175 | { | |
| 176 | thisValue|=bitmask; | |
| 177 | } | |
| 178 | }else { | |
| 179 | if (GetBitValue( KMem.WR[Addr1-128],bitAddr)) | |
| 180 | { | |
| 181 | thisValue|=bitmask; | |
| 182 | } | |
| 483170 | 183 | } |
| Q | 184 | bitmask<<=1; |
| 185 | Addr++; | |
| 186 | } | |
| 187 | Pkgbuf[3+i]=thisValue; | |
| 188 | } | |
| 189 | len2+=1+nByteCount; | |
| 190 | break; | |
| 191 | case ReadInputs: //2 , bits | |
| 192 | // Fetch Datas; | |
| 193 | nByteCount=(nCount+7)/8; | |
| 194 | Pkgbuf[2]=nByteCount; | |
| 195 | for (int i=0;i<nByteCount;i++) | |
| 196 | { | |
| 197 | thisValue=0; | |
| 198 | bitmask=1; | |
| 199 | for (int j=0;j<8&&i*8+j<nCount;j++) | |
| 200 | { | |
| 201 | Addr1=(Addr&0xff0)>>4; | |
| 202 | bitAddr=Addr&0xf; | |
| 203 | if (GetBitValue( KMem.WX[Addr1],bitAddr)) | |
| 204 | { | |
| 205 | thisValue|=bitmask; | |
| 206 | } | |
| 207 | bitmask<<=1; | |
| 208 | Addr++; | |
| 209 | } | |
| 210 | Pkgbuf[3+i]=thisValue; | |
| 211 | } | |
| 212 | len2+=1+nByteCount; | |
| 213 | break; | |
| 214 | case ReadKeepRegs: //3 words | |
| 215 | // Fetch Datas; | |
| 216 | nByteCount=nCount*2; | |
| 217 | Pkgbuf[2]=nByteCount; | |
| 218 | for (int i=0;i<nCount;i++) | |
| 219 | { | |
| 220 | Pkgbuf[3+i*2]=(KMem.DT[Addr+i]>>8)&0xff; | |
| 221 | Pkgbuf[4+i*2]=(KMem.DT[Addr+i])&0xff; | |
| 222 | } | |
| 223 | len2+=1+nByteCount; | |
| 224 | break; | |
| 225 | case ReadInputRegs: //4 words | |
| 226 | // Fetch Datas; | |
| 227 | nByteCount=nCount*2; | |
| 228 | Pkgbuf[2]=nByteCount; | |
| 229 | for (int i=0;i<nCount;i++) | |
| 230 | { | |
| 231 | Pkgbuf[3+i*2]=HiByte(KMem.WX[Addr+i]); | |
| 232 | Pkgbuf[4+i*2]=LoByte(KMem.WX[Addr+i]); | |
| 233 | } | |
| 234 | len2+=1+nByteCount; | |
| 235 | break; | |
| 236 | case WriteCoil: //5 bit | |
| 237 | //Store Datas; | |
| 238 | //SetBitValue(&KMem.WR[Addr1],bitAddr,pPkg->nCount); | |
| 842bb6 | 239 | Addr1=(Addr&0xfff0)>>4; |
| Q | 240 | bitAddr=Addr&0xf; |
| 483170 | 241 | if (nCount == 0) // set to 0 |
| Q | 242 | { |
| 842bb6 | 243 | if (Addr < 2048) { ResetBit(&KMem.WY[Addr1],bitAddr);} |
| Q | 244 | else {ResetBit(&KMem.WR[Addr1-128],bitAddr);} |
| 483170 | 245 | }else if (nCount == 0xFF00) // set to 1 |
| Q | 246 | { |
| 842bb6 | 247 | if (Addr < 2048) { SetAddrBit(&KMem.WY[Addr1],bitAddr);} |
| Q | 248 | else {SetAddrBit(&KMem.WR[Addr1-128],bitAddr);} |
| 249 | ||
| 483170 | 250 | } |
| Q | 251 | memcpy(Pkgbuf,ptr,len); |
| 252 | needcrc=0; | |
| 253 | len2=len; | |
| 254 | break; | |
| 255 | case WriteReg: //6 word | |
| 256 | //Store Datas; | |
| 257 | KMem.DT[Addr]=mkshort(bptr[4],bptr[5]); | |
| 258 | memcpy(Pkgbuf,ptr,len); | |
| 259 | needcrc=0; | |
| 260 | len2=len; | |
| 261 | break; | |
| 262 | case WriteCoils: //0x0f, 15 bits | |
| 263 | //Store Datas; | |
| 264 | nByteCount=(nCount+7)/8; | |
| 265 | Pkgbuf[2]=nByteCount; | |
| 266 | for (int i=0;i<nByteCount;i++) | |
| 267 | { | |
| 268 | thisValue=bptr[7+i]; | |
| 269 | bitmask=1; | |
| 270 | for (int j=0;j<8&&i*8+j<nCount;j++) | |
| 271 | { | |
| 8b51c7 | 272 | Addr1=(Addr&0xfff0)>>4; |
| 483170 | 273 | bitAddr=Addr&0xf; |
| 8b51c7 | 274 | if (Addr < 2048) { |
| Q | 275 | SetBitValue( &KMem.WY[Addr1],bitAddr,bitmask&thisValue); |
| 276 | } else { | |
| 277 | SetBitValue( &KMem.WR[Addr1 - 128],bitAddr,bitmask&thisValue); | |
| 278 | } | |
| 483170 | 279 | bitmask<<=1; |
| Q | 280 | Addr++; |
| 281 | } | |
| 282 | } | |
| 283 | ||
| 284 | //makeReply | |
| 285 | Pkgbuf[2]=bptr[2]; //HiByte(Addr); | |
| 286 | Pkgbuf[3]=bptr[3]; //LoByte(Addr); | |
| 287 | Pkgbuf[4]=bptr[4]; //HiByte(nCount); | |
| 288 | Pkgbuf[5]=bptr[5]; //LoByte(nCount); | |
| 289 | len2+=4; | |
| 290 | break; | |
| 291 | case WriteRegs: //0x10, 16 words | |
| 292 | //Store Datas; | |
| 293 | for (int i=0;i<nCount;i++) | |
| 294 | { | |
| 295 | KMem.DT[Addr+i]=mkshort(bptr[7+i*2],bptr[8+i*2]); | |
| 296 | } | |
| 297 | //Make Reply | |
| 298 | Pkgbuf[2]=bptr[2]; //HiByte(Addr); | |
| 299 | Pkgbuf[3]=bptr[3]; //LoByte(Addr); | |
| 300 | Pkgbuf[4]=bptr[4]; //HiByte(nCount); | |
| 301 | Pkgbuf[5]=bptr[5]; //LoByte(nCount); | |
| 302 | len2+=4; | |
| 303 | break; | |
| 304 | ||
| 305 | default: //others | |
| 306 | break; | |
| 307 | } | |
| 308 | if (err==0) | |
| 309 | { | |
| 310 | if (needcrc) | |
| 311 | { | |
| 312 | crc=crc16tablefast(Pkgbuf,len2); | |
| 313 | Pkgbuf[len2]=LoByte(crc); | |
| 314 | Pkgbuf[len2+1]=HiByte(crc); | |
| 315 | len2+=2; | |
| 316 | } | |
| 317 | //SendPkg(Pkgbuf,len2); | |
| 318 | SendPacket(nChn, Pkgbuf, len2); | |
| 319 | } | |
| 320 | ||
| 321 | return 0; | |
| 322 | } | |
| 323 | ||
| 324 | ||
