| 提交 | 用户 | 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 | ||
| 65 | ||
| 66 | //********************************************************************** | |
| 67 | //****************** Modbus slave ************************************ | |
| 68 | ||
| 69 | int ModBusSlaveCheckPkg(int nChn, void *ptr, uint16_t len1) | |
| 70 | { | |
| 71 | if (len1 <=4) return -1; //包长 | |
| 72 | pModBusRTUReqPkg pPkg = (pModBusRTUReqPkg) ptr; | |
| 73 | if (pPkg->Dst >127) return -2; //地址码 | |
| 74 | if ((pPkg->Cmd&0x7f) > 0x1f) return -3; //功能码 | |
| 75 | uint16_t crc = crc16tablefast(ptr,len1); //CRC 校验 | |
| 76 | if (crc != 0 ) return 4; //CRC 校验错误 | |
| 77 | return S_OK; | |
| 78 | } | |
| 79 | ||
| 80 | int mkReqPkg(uint8_t *ptr, uint8_t opcode, uint8_t nsize, uint8_t *buf) | |
| 81 | { | |
| 82 | int len1=0; | |
| 83 | ||
| 84 | return len1; | |
| 85 | ||
| 86 | } | |
| 87 | /* | |
| 88 | static const unsigned short bitMasks[16]= | |
| 89 | { | |
| 90 | 0x1<<0, 0x1<<1, 0x1<<2, 0x1<<3, 0x1<<4, 0x1<<5, 0x1<<6, 0x1<<7, | |
| 91 | 0x1<<8, 0x1<<9, 0x1<<10, 0x1<<11, 0x1<<12, 0x1<<13, 0x1<<14, 0x1<<15, | |
| 92 | }; | |
| 93 | */ | |
| 94 | static inline void SetAddrBit(unsigned short * pW, unsigned char bitAddr) | |
| 95 | { | |
| 96 | (*pW)|=1<<(bitAddr&0xf); | |
| 97 | } | |
| 98 | ||
| 99 | static inline void ResetBit(unsigned short * pW, unsigned char bitAddr) | |
| 100 | { | |
| 101 | (*pW)&=~(1<<(bitAddr&0xf)); | |
| 102 | } | |
| 103 | ||
| 104 | static inline void SetBitValue(unsigned short * pW, unsigned char bitAddr, unsigned char Value) | |
| 105 | { | |
| 106 | if (Value) { SetAddrBit(pW, bitAddr);} | |
| 107 | else {ResetBit(pW, bitAddr);} | |
| 108 | } | |
| 109 | ||
| 110 | static inline unsigned char GetBitValue(unsigned short W, unsigned char bitAddr) | |
| 111 | { | |
| 112 | if (W&(1<<(bitAddr&0xf))) return 1; | |
| 113 | else return 0; | |
| 114 | } | |
| 115 | ||
| 116 | uint8_t Pkgbuf[128]; | |
| 117 | ||
| 118 | int ModBusSlaveParsePkg(int nChn, void *ptr, uint16_t len) | |
| 119 | { | |
| 120 | uchar * bptr = (uchar *) ptr; | |
| 121 | int err=ModBusSlaveCheckPkg(nChn, ptr, len); | |
| 122 | if (err) return -1; | |
| 123 | pModBusRTUReqPkg pPkg = (pModBusRTUReqPkg) ptr; | |
| 124 | uint8_t DstHost=pPkg->Dst; | |
| 125 | if (DstHost != MyAddr) return 0; | |
| 126 | uint8_t opcode = pPkg->Cmd; | |
| 127 | ushort Addr=mkshort(pPkg->AddrH,pPkg->AddrL); | |
| 128 | ushort nCount=swaphilo(pPkg->nCount); | |
| 129 | Pkgbuf[0]=MyAddr; | |
| 130 | Pkgbuf[1]=opcode; | |
| 131 | int len2=2; | |
| 132 | int needcrc=1; | |
| 133 | uint16_t crc; | |
| 134 | int nByteCount; | |
| 135 | unsigned short Addr1=(Addr&0xff0)>>4; | |
| 136 | unsigned char bitAddr=Addr&0xf; | |
| 137 | unsigned char thisValue; | |
| 138 | unsigned char bitmask=1; | |
| 139 | switch (opcode) | |
| 140 | { | |
| 141 | case None: //0 | |
| 142 | break; | |
| 143 | case ReadCoils: //1 bits | |
| 144 | // Fetch Datas; | |
| 145 | nByteCount=(nCount+7)/8; | |
| 146 | Pkgbuf[2]=nByteCount; | |
| 147 | for (int i=0;i<nByteCount;i++) | |
| 148 | { | |
| 149 | thisValue=0; | |
| 150 | bitmask=1; | |
| 151 | for (int j=0;j<8&&i*8+j<nCount;j++) | |
| 152 | { | |
| 8b51c7 | 153 | Addr1=(Addr&0xfff0)>>4; |
| 483170 | 154 | bitAddr=Addr&0xf; |
| 8b51c7 | 155 | if (Addr < 2048) { |
| Q | 156 | if (GetBitValue( KMem.WY[Addr1],bitAddr)) |
| 157 | { | |
| 158 | thisValue|=bitmask; | |
| 159 | } | |
| 160 | }else { | |
| 161 | if (GetBitValue( KMem.WR[Addr1-128],bitAddr)) | |
| 162 | { | |
| 163 | thisValue|=bitmask; | |
| 164 | } | |
| 483170 | 165 | } |
| Q | 166 | bitmask<<=1; |
| 167 | Addr++; | |
| 168 | } | |
| 169 | Pkgbuf[3+i]=thisValue; | |
| 170 | } | |
| 171 | len2+=1+nByteCount; | |
| 172 | break; | |
| 173 | case ReadInputs: //2 , bits | |
| 174 | // Fetch Datas; | |
| 175 | nByteCount=(nCount+7)/8; | |
| 176 | Pkgbuf[2]=nByteCount; | |
| 177 | for (int i=0;i<nByteCount;i++) | |
| 178 | { | |
| 179 | thisValue=0; | |
| 180 | bitmask=1; | |
| 181 | for (int j=0;j<8&&i*8+j<nCount;j++) | |
| 182 | { | |
| 183 | Addr1=(Addr&0xff0)>>4; | |
| 184 | bitAddr=Addr&0xf; | |
| 185 | if (GetBitValue( KMem.WX[Addr1],bitAddr)) | |
| 186 | { | |
| 187 | thisValue|=bitmask; | |
| 188 | } | |
| 189 | bitmask<<=1; | |
| 190 | Addr++; | |
| 191 | } | |
| 192 | Pkgbuf[3+i]=thisValue; | |
| 193 | } | |
| 194 | len2+=1+nByteCount; | |
| 195 | break; | |
| 196 | case ReadKeepRegs: //3 words | |
| 197 | // Fetch Datas; | |
| 198 | nByteCount=nCount*2; | |
| 199 | Pkgbuf[2]=nByteCount; | |
| 200 | for (int i=0;i<nCount;i++) | |
| 201 | { | |
| 202 | Pkgbuf[3+i*2]=(KMem.DT[Addr+i]>>8)&0xff; | |
| 203 | Pkgbuf[4+i*2]=(KMem.DT[Addr+i])&0xff; | |
| 204 | } | |
| 205 | len2+=1+nByteCount; | |
| 206 | break; | |
| 207 | case ReadInputRegs: //4 words | |
| 208 | // Fetch Datas; | |
| 209 | nByteCount=nCount*2; | |
| 210 | Pkgbuf[2]=nByteCount; | |
| 211 | for (int i=0;i<nCount;i++) | |
| 212 | { | |
| 213 | Pkgbuf[3+i*2]=HiByte(KMem.WX[Addr+i]); | |
| 214 | Pkgbuf[4+i*2]=LoByte(KMem.WX[Addr+i]); | |
| 215 | } | |
| 216 | len2+=1+nByteCount; | |
| 217 | break; | |
| 218 | case WriteCoil: //5 bit | |
| 219 | //Store Datas; | |
| 220 | //SetBitValue(&KMem.WR[Addr1],bitAddr,pPkg->nCount); | |
| 842bb6 | 221 | Addr1=(Addr&0xfff0)>>4; |
| Q | 222 | bitAddr=Addr&0xf; |
| 483170 | 223 | if (nCount == 0) // set to 0 |
| Q | 224 | { |
| 842bb6 | 225 | if (Addr < 2048) { ResetBit(&KMem.WY[Addr1],bitAddr);} |
| Q | 226 | else {ResetBit(&KMem.WR[Addr1-128],bitAddr);} |
| 483170 | 227 | }else if (nCount == 0xFF00) // set to 1 |
| Q | 228 | { |
| 842bb6 | 229 | if (Addr < 2048) { SetAddrBit(&KMem.WY[Addr1],bitAddr);} |
| Q | 230 | else {SetAddrBit(&KMem.WR[Addr1-128],bitAddr);} |
| 231 | ||
| 483170 | 232 | } |
| Q | 233 | memcpy(Pkgbuf,ptr,len); |
| 234 | needcrc=0; | |
| 235 | len2=len; | |
| 236 | break; | |
| 237 | case WriteReg: //6 word | |
| 238 | //Store Datas; | |
| 239 | KMem.DT[Addr]=mkshort(bptr[4],bptr[5]); | |
| 240 | memcpy(Pkgbuf,ptr,len); | |
| 241 | needcrc=0; | |
| 242 | len2=len; | |
| 243 | break; | |
| 244 | case WriteCoils: //0x0f, 15 bits | |
| 245 | //Store Datas; | |
| 246 | nByteCount=(nCount+7)/8; | |
| 247 | Pkgbuf[2]=nByteCount; | |
| 248 | for (int i=0;i<nByteCount;i++) | |
| 249 | { | |
| 250 | thisValue=bptr[7+i]; | |
| 251 | bitmask=1; | |
| 252 | for (int j=0;j<8&&i*8+j<nCount;j++) | |
| 253 | { | |
| 8b51c7 | 254 | Addr1=(Addr&0xfff0)>>4; |
| 483170 | 255 | bitAddr=Addr&0xf; |
| 8b51c7 | 256 | if (Addr < 2048) { |
| Q | 257 | SetBitValue( &KMem.WY[Addr1],bitAddr,bitmask&thisValue); |
| 258 | } else { | |
| 259 | SetBitValue( &KMem.WR[Addr1 - 128],bitAddr,bitmask&thisValue); | |
| 260 | } | |
| 483170 | 261 | bitmask<<=1; |
| Q | 262 | Addr++; |
| 263 | } | |
| 264 | } | |
| 265 | ||
| 266 | //makeReply | |
| 267 | Pkgbuf[2]=bptr[2]; //HiByte(Addr); | |
| 268 | Pkgbuf[3]=bptr[3]; //LoByte(Addr); | |
| 269 | Pkgbuf[4]=bptr[4]; //HiByte(nCount); | |
| 270 | Pkgbuf[5]=bptr[5]; //LoByte(nCount); | |
| 271 | len2+=4; | |
| 272 | break; | |
| 273 | case WriteRegs: //0x10, 16 words | |
| 274 | //Store Datas; | |
| 275 | for (int i=0;i<nCount;i++) | |
| 276 | { | |
| 277 | KMem.DT[Addr+i]=mkshort(bptr[7+i*2],bptr[8+i*2]); | |
| 278 | } | |
| 279 | //Make Reply | |
| 280 | Pkgbuf[2]=bptr[2]; //HiByte(Addr); | |
| 281 | Pkgbuf[3]=bptr[3]; //LoByte(Addr); | |
| 282 | Pkgbuf[4]=bptr[4]; //HiByte(nCount); | |
| 283 | Pkgbuf[5]=bptr[5]; //LoByte(nCount); | |
| 284 | len2+=4; | |
| 285 | break; | |
| 286 | ||
| 287 | default: //others | |
| 288 | break; | |
| 289 | } | |
| 290 | if (err==0) | |
| 291 | { | |
| 292 | if (needcrc) | |
| 293 | { | |
| 294 | crc=crc16tablefast(Pkgbuf,len2); | |
| 295 | Pkgbuf[len2]=LoByte(crc); | |
| 296 | Pkgbuf[len2+1]=HiByte(crc); | |
| 297 | len2+=2; | |
| 298 | } | |
| 299 | //SendPkg(Pkgbuf,len2); | |
| 300 | SendPacket(nChn, Pkgbuf, len2); | |
| 301 | } | |
| 302 | ||
| 303 | return 0; | |
| 304 | } | |
| 305 | ||
| 306 | ||
