From ccaf53ec67f1c9641454e98e28fd3d98152ecf1c Mon Sep 17 00:00:00 2001 From: thesai <1021828630@qq.com> Date: Wed, 17 Jul 2024 15:44:47 +0800 Subject: [PATCH] =?UTF-8?q?[=E4=BF=AE=E6=94=B9]=E5=8C=97=E4=BA=AC12?= =?UTF-8?q?=E5=8F=B7=E7=BA=BF=E8=BF=9E=E9=94=81rsd=E3=80=81sse=E3=80=81ssr?= =?UTF-8?q?=E7=BC=96=E8=A7=A3=E7=A0=81=E7=AE=97=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- config/config.go | 42 +- third_party/axle_device/beijing12/service.go | 394 +++++++++++-------- third_party/example/rssp/main.go | 200 ---------- third_party/message/rssp_code.go | 23 +- 4 files changed, 262 insertions(+), 397 deletions(-) delete mode 100644 third_party/example/rssp/main.go diff --git a/config/config.go b/config/config.go index f3f87c7..e1e8ca0 100644 --- a/config/config.go +++ b/config/config.go @@ -176,23 +176,31 @@ type RsspAxleConfig struct { // RsspNetConfig 计轴通信配置 type RsspNetConfig struct { - RemoteIp string `json:"remoteIp" description:"远端IP"` - RemotePort int `json:"remotePort" description:"远端端口"` - LocalIp string `json:"localIp" description:"本地IP"` - LocalPort int `json:"localPort" description:"本地端口"` - RemoteAddr string `json:"sourceAddr" description:"联锁地址（16进制，2字节）"` - LocalAddr string `json:"targetAddr" description:"计轴地址（16进制，2字节）"` - Sid1 string `json:"sid1" description:"联锁SID_1（16进制，4字节）"` - Sid2 string `json:"sid2" description:"联锁SID_2（16进制，4字节）"` - LocalSid1 string `json:"remoteSid" description:"计轴SID_1（16进制，4字节）"` - LocalSid2 string `json:"remoteSid2" description:"计轴SID_2（16进制，4字节）"` - Sinit1 string `json:"sinit1" description:"SINIT_1（16进制，4字节）"` - Sinit2 string `json:"sinit2" description:"SINIT_2（16进制，4字节）"` - DataVer1 string `json:"dataVer1" description:"DATAVER_1（16进制，4字节）"` - DataVer2 string `json:"dataVer2" description:"DATAVER_2（16进制，4字节）"` - MaxDeviation int `json:"maxDeviation" description:"可容忍的最大时序偏差"` - WaitSSRTimeout int `json:"waitSSRTimeout" description:"等待SSR回应的定时器超时值（ms）"` - Period int `json:"period" description:"RSD发送周期（ms）"` + RemoteIp string `json:"remoteIp" description:"远端IP"` + RemotePort int `json:"remotePort" description:"远端端口"` + LocalPort int `json:"localPort" description:"本地端口"` + + RemoteAddr string `json:"sourceAddr" description:"联锁地址（16进制，2字节）"` + LocalAddr string `json:"targetAddr" description:"计轴地址（16进制，2字节）"` + + RemoteSid1 string `json:"remoteSid1" description:"联锁SID_1（16进制，4字节）"` + RemoteSid2 string `json:"remoteSid2" description:"联锁SID_2（16进制，4字节）"` + LocalSid1 string `json:"localSid1" description:"计轴SID_1（16进制，4字节）"` + LocalSid2 string `json:"localSid2" description:"计轴SID_2（16进制，4字节）"` + + RemoteSinit1 string `json:"remoteSinit1" description:"联锁SINIT_1（16进制，4字节）"` + RemoteSinit2 string `json:"remoteSinit2" description:"联锁SINIT_2（16进制，4字节）"` + LocalSinit1 string `json:"localSinit1" description:"计轴SINIT_1（16进制，4字节）"` + LocalSinit2 string `json:"localSinit2" description:"计轴SINIT_2（16进制，4字节）"` + + RemoteDataVer1 string `json:"remoteDataVer1" description:"联锁DATAVER_1（16进制，4字节）"` + RemoteDataVer2 string `json:"remoteDataVer2" description:"联锁DATAVER_2（16进制，4字节）"` + LocalDataVer1 string `json:"localDataVer1" description:"计轴DATAVER_1（16进制，4字节）"` + LocalDataVer2 string `json:"localDataVer2" description:"计轴DATAVER_2（16进制，4字节）"` + + MaxDeviation int `json:"maxDeviation" description:"可容忍的最大时序偏差"` + WaitSSRTimeout int `json:"waitSSRTimeout" description:"等待SSR回应的定时器超时值（ms）"` + Period int `json:"period" description:"RSD发送周期（ms）"` } /////////////////////////////////////////////////////////////////////////////////////// diff --git a/third_party/axle_device/beijing12/service.go b/third_party/axle_device/beijing12/service.go index 52134f3..d18eb0e 100644 --- a/third_party/axle_device/beijing12/service.go +++ b/third_party/axle_device/beijing12/service.go @@ -38,24 +38,30 @@ type serviceContext struct { cancelFunc context.CancelFunc ciSectionIndexConfigs []*proto.CiSectionCodePoint - remoteAddr uint16 //联锁地址 从配置中的16进制字符串转来的 - localAddr uint16 //计轴地址 从配置中的16进制字符串转来的 - sid1 uint32 //联锁SID1 从配置中的16进制字符串转来的 - sid2 uint32 //联锁SID2 从配置中的16进制字符串转来的 - localSid1 uint32 //计轴SID1 从配置中的16进制字符串转来的 - localSid2 uint32 //计轴SID2 从配置中的16进制字符串转来的 - sinit1 uint32 //SINT16 从配置中的16进制字符串转来的 - sinit2 uint32 //SINT26 从配置中的16进制字符串转来的 - dataver1 uint32 //DATAVER1 从配置中的16进制字符串转来的 - dataver2 uint32 //DATAVER2 从配置中的16进制字符串转来的 - msgChan <-chan []byte //消息队列 - seqNum uint32 //当前的序列号 - lastSeqParam1 uint32 //最近一次的有效时序参数 sid1^t1(n) - lastSeqParam2 uint32 //最近一次的有效时序参数 sid2^t2(n) - lfsr1 *lfsr //用来计算时间戳的lfsr - lfsr2 *lfsr //用来计算时间戳的lfsr - sseMsg *msg.SseMsg //发送出去的时序校验请求 - sseWaitTimer <-chan time.Time //sse超时定时器 + remoteAddr uint16 //联锁地址 从配置中的16进制字符串转来的 + localAddr uint16 //计轴地址 从配置中的16进制字符串转来的 + remoteSid1 uint32 //联锁SID1 从配置中的16进制字符串转来的 + remoteSid2 uint32 //联锁SID2 从配置中的16进制字符串转来的 + localSid1 uint32 //计轴SID1 从配置中的16进制字符串转来的 + localSid2 uint32 //计轴SID2 从配置中的16进制字符串转来的 + remoteSinit1 uint32 //联锁SINIT1 从配置中的16进制字符串转来的 + remoteSinit2 uint32 //联锁SINIT2 从配置中的16进制字符串转来的 + localSinit1 uint32 //计轴SINT1 从配置中的16进制字符串转来的 + localSinit2 uint32 //计轴SINT2 从配置中的16进制字符串转来的 + remoteDataVer1 uint32 //联锁DATAVER1 从配置中的16进制字符串转来的 + remoteDataVer2 uint32 //联锁DATAVER2 从配置中的16进制字符串转来的 + localDataVer1 uint32 //计轴DATAVER1 从配置中的16进制字符串转来的 + localDataVer2 uint32 //计轴DATAVER2 从配置中的16进制字符串转来的 + msgChan <-chan []byte //消息队列 + seqNum uint32 //当前的序列号 + lastSeqParam1 uint32 //最近一次的有效时序参数 remoteSinit1~+[remoteSid1^t1(n)] + lastSeqParam2 uint32 //最近一次的有效时序参数 remoteSinit2~+[remoteSid2^t2(n)] + lfsr1 *lfsr //用来计算时间戳的lfsr + lfsr2 *lfsr //用来计算时间戳的lfsr + sseMsg *msg.SseMsg //发送出去的时序校验请求 + sseWaitTimer <-chan time.Time //sse超时定时器 + precSinit1 uint32 // 用来从SSR消息的SeqInit中提取时序参数 + precSinit2 uint32 // 用来从SSR消息的SeqInit中提取时序参数 } func Start(simulation *memory.VerifySimulation) { @@ -81,73 +87,18 @@ func Start(simulation *memory.VerifySimulation) { logger().Warn(fmt.Sprintf("集中站[%s]无区段编码数据，服务不启动", rsspConfig.StationCode)) return } - sourceAddr, err := strconv.ParseUint(rsspConfig.NetAConfig.RemoteAddr, 16, 16) - if err != nil { - panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析源地址[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.RemoteAddr))) - } - targetAddr, err := strconv.ParseUint(rsspConfig.NetAConfig.LocalAddr, 16, 16) - if err != nil { - panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析目的地址[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.LocalAddr))) - } - sid1, err := strconv.ParseUint(rsspConfig.NetAConfig.Sid1, 16, 32) - if err != nil { - panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析联锁SID1[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.Sid1))) - } - sid2, err := strconv.ParseUint(rsspConfig.NetAConfig.Sid2, 16, 32) - if err != nil { - panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析联锁SID2[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.Sid2))) - } - localSid1, err := strconv.ParseUint(rsspConfig.NetAConfig.LocalSid1, 16, 32) - if err != nil { - panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析计轴SID1[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.LocalSid1))) - } - localSid2, err := strconv.ParseUint(rsspConfig.NetAConfig.LocalSid2, 16, 32) - if err != nil { - panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析计轴SID2[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.LocalSid2))) - } - sinit1, err := strconv.ParseUint(rsspConfig.NetAConfig.Sinit1, 16, 32) - if err != nil { - panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析SINT1[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.Sinit1))) - } - sinit2, err := strconv.ParseUint(rsspConfig.NetAConfig.Sinit2, 16, 32) - if err != nil { - panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析SINT2[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.Sinit2))) - } - dataver1, err := strconv.ParseUint(rsspConfig.NetAConfig.DataVer1, 16, 32) - if err != nil { - panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析DATAVER_1[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.DataVer1))) - } - dataver2, err := strconv.ParseUint(rsspConfig.NetAConfig.DataVer2, 16, 32) - if err != nil { - panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析DATAVER_2[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.DataVer2))) - } - //服务初始化及启动 + //初始化服务上下文 + serviceCtx := initServiceContext(rsspConfig, ref, simulation) + //准备启动服务 msgChan := make(chan []byte, 100) - serviceCtx := &serviceContext{ - sim: simulation, - config: rsspConfig, - ciSectionIndexConfigs: ref.SectionCodePoints, - remoteAddr: uint16(sourceAddr), - localAddr: uint16(targetAddr), - sid1: uint32(sid1), - sid2: uint32(sid2), - localSid1: uint32(localSid1), - localSid2: uint32(localSid2), - sinit1: uint32(sinit1), - sinit2: uint32(sinit2), - dataver1: uint32(dataver1), - dataver2: uint32(dataver2), - msgChan: msgChan, - lfsr1: &lfsr{value: uint32(localSid1), poly: msg.T_POLY_1}, - lfsr2: &lfsr{value: uint32(localSid2), poly: msg.T_POLY_2}, - } + serviceCtx.msgChan = msgChan netAConfig := rsspConfig.NetAConfig - server := udp.NewServer(fmt.Sprintf("%s:%d", netAConfig.LocalIp, netAConfig.LocalPort), func(b []byte) { + server := udp.NewServer(fmt.Sprintf(":%d", netAConfig.LocalPort), func(b []byte) { logger().Info(fmt.Sprintf("收到数据：%x", b)) msgChan <- b }) client := udp.NewClient(fmt.Sprintf("%s:%d", netAConfig.RemoteIp, netAConfig.RemotePort)) - err = server.Listen() + err := server.Listen() if err != nil { panic(sys_error.New(fmt.Sprintf("%s集中站[%s]服务启动失败", logTag, rsspConfig.StationCode))) } else { @@ -160,13 +111,103 @@ func Start(simulation *memory.VerifySimulation) { serviceCtx.runCollectTask(cancelCtx) serviceCtx.runHandleMsgTask(cancelCtx) contextMap[rsspConfig.StationCode] = serviceCtx +} - ////sse测试，测完删除 - //time.Sleep(1 * time.Second) - //serviceCtx.seqNum++ - //serviceCtx.lfsr1.add(0) - //serviceCtx.lfsr2.add(0) - //serviceCtx.startSeeProgress() +func initServiceContext(rsspConfig config.RsspAxleConfig, ref *proto.CentralizedStationRef, simulation *memory.VerifySimulation) *serviceContext { + sourceAddr, err := strconv.ParseUint(rsspConfig.NetAConfig.RemoteAddr, 16, 16) + if err != nil { + panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析源地址[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.RemoteAddr))) + } + targetAddr, err := strconv.ParseUint(rsspConfig.NetAConfig.LocalAddr, 16, 16) + if err != nil { + panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析目的地址[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.LocalAddr))) + } + remoteSid1, err := strconv.ParseUint(rsspConfig.NetAConfig.RemoteSid1, 16, 32) + if err != nil { + panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析联锁SID1[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.RemoteSid1))) + } + remoteSid2, err := strconv.ParseUint(rsspConfig.NetAConfig.RemoteSid2, 16, 32) + if err != nil { + panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析联锁SID2[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.RemoteSid2))) + } + localSid1, err := strconv.ParseUint(rsspConfig.NetAConfig.LocalSid1, 16, 32) + if err != nil { + panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析计轴SID1[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.LocalSid1))) + } + localSid2, err := strconv.ParseUint(rsspConfig.NetAConfig.LocalSid2, 16, 32) + if err != nil { + panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析计轴SID2[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.LocalSid2))) + } + remoteSinit1, err := strconv.ParseUint(rsspConfig.NetAConfig.RemoteSinit1, 16, 32) + if err != nil { + panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析联锁SINT1[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.RemoteSinit1))) + } + remoteSinit2, err := strconv.ParseUint(rsspConfig.NetAConfig.RemoteSinit2, 16, 32) + if err != nil { + panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析联锁SINT2[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.RemoteSinit2))) + } + localSinit1, err := strconv.ParseUint(rsspConfig.NetAConfig.LocalSinit1, 16, 32) + if err != nil { + panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析计轴SINT1[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.LocalSinit1))) + } + localSinit2, err := strconv.ParseUint(rsspConfig.NetAConfig.LocalSinit2, 16, 32) + if err != nil { + panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析计轴SINT2[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.LocalSinit2))) + } + remoteDataVer1, err := strconv.ParseUint(rsspConfig.NetAConfig.RemoteDataVer1, 16, 32) + if err != nil { + panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析联锁DATAVER_1[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.RemoteDataVer1))) + } + remoteDataVer2, err := strconv.ParseUint(rsspConfig.NetAConfig.RemoteDataVer2, 16, 32) + if err != nil { + panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析联锁DATAVER_2[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.RemoteDataVer2))) + } + localDataVer1, err := strconv.ParseUint(rsspConfig.NetAConfig.LocalDataVer1, 16, 32) + if err != nil { + panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析计轴DATAVER_1[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.LocalDataVer1))) + } + localDataVer2, err := strconv.ParseUint(rsspConfig.NetAConfig.LocalDataVer2, 16, 32) + if err != nil { + panic(sys_error.New(fmt.Sprintf("%s集中站[%s]解析计轴DATAVER_2[%s]出错", logTag, rsspConfig.StationCode, rsspConfig.NetAConfig.LocalDataVer2))) + } + //服务初始化及启动 + serviceCtx := &serviceContext{ + sim: simulation, + config: rsspConfig, + ciSectionIndexConfigs: ref.SectionCodePoints, + remoteAddr: uint16(sourceAddr), + localAddr: uint16(targetAddr), + + remoteSid1: uint32(remoteSid1), + remoteSid2: uint32(remoteSid2), + localSid1: uint32(localSid1), + localSid2: uint32(localSid2), + + remoteSinit1: uint32(remoteSinit1), + remoteSinit2: uint32(remoteSinit2), + localSinit1: uint32(localSinit1), + localSinit2: uint32(localSinit2), + + remoteDataVer1: uint32(remoteDataVer1), + remoteDataVer2: uint32(remoteDataVer2), + localDataVer1: uint32(localDataVer1), + localDataVer2: uint32(localDataVer2), + + lfsr1: &lfsr{value: uint32(localSid1), poly: msg.T_POLY_1}, + lfsr2: &lfsr{value: uint32(localSid2), poly: msg.T_POLY_2}, + + precSinit1: calculatePrecSinit(uint32(remoteSinit1), uint32(localSid1), uint32(remoteDataVer1)), + precSinit2: calculatePrecSinit(uint32(remoteSinit2), uint32(localSid2), uint32(remoteDataVer2)), + } + return serviceCtx +} + +func calculatePrecSinit(remoteSinit uint32, localSid uint32, remoteDataVer uint32) uint32 { + l1 := lfsr{value: remoteSinit, poly: msg.T_POLY_1} + l1.add(localSid ^ remoteDataVer) + valueTmp := l1.value + l1.load(0).post(valueTmp) + return l1.value } func Stop(simulation *memory.VerifySimulation) { @@ -217,6 +258,56 @@ func (s *serviceContext) runCollectTask(ctx context.Context) { }() } +func (s *serviceContext) collect() *msg.RsdMsgBuilder { + worldData := entity.GetWorldData(s.sim.World) + amdEntry := entity.FindAxleManageDevice(worldData, s.config.StationCode) + amd := component.AxleManageDeviceType.Get(amdEntry) + stateInfos := msg.StateInfos{} + for _, cfg := range s.ciSectionIndexConfigs { + sectionRuntime := amd.Adrs[cfg.SectionId] + sectionEntry, ok := entity.GetEntityByUid(s.sim.World, cfg.SectionId) + sectionState := component.PhysicalSectionStateType.Get(sectionEntry) + if !ok { + continue + } + stateInfos = append(stateInfos, &msg.StateInfo{ + CLR: !sectionState.Occ, + OCC: sectionState.Occ, + RAC: sectionRuntime.Rac, + RJO: sectionRuntime.Rjo, + RJT: sectionRuntime.Rjt, + }) + } + userData := []byte{0x00} //检查字节 + userData = append(userData, stateInfos.Encode()...) + //更新序列号及时间戳 + s.seqNum++ + s.lfsr1.add(0) + s.lfsr2.add(0) + //构建消息 + builder := &msg.RsdMsgBuilder{ + MsgHeader: msg.MsgHeader{ + ProtocolType: msg.ProtocolType_Sync, + MessageType: msg.MessageType_B, //从抓包数据里看到的 + SourceAddr: s.localAddr, + TargetAddr: s.remoteAddr, + }, + SeqNum: s.seqNum, + Svc1: s.calculateSvc1(userData), + Svc2: s.calculateSvc2(userData), + UserData: userData, + } + return builder +} + +func (s *serviceContext) calculateSvc1(userData []byte) uint32 { + return message.Rssp_I_Crc32C1(userData) ^ s.localSid1 ^ s.lfsr1.value ^ msg.SCW_1 +} + +func (s *serviceContext) calculateSvc2(userData []byte) uint32 { + return message.Rssp_I_Crc32C2(userData) ^ s.localSid2 ^ s.lfsr2.value ^ msg.SCW_2 +} + func (s *serviceContext) runHandleMsgTask(ctx context.Context) { go func() { defer func() { @@ -248,58 +339,8 @@ func (s *serviceContext) runHandleMsgTask(ctx context.Context) { }() } -func (s *serviceContext) collect() *msg.RsdMsgBuilder { - worldData := entity.GetWorldData(s.sim.World) - amdEntry := entity.FindAxleManageDevice(worldData, s.config.StationCode) - amd := component.AxleManageDeviceType.Get(amdEntry) - stateInfos := msg.StateInfos{} - for _, cfg := range s.ciSectionIndexConfigs { - sectionRuntime := amd.Adrs[cfg.SectionId] - sectionEntry, ok := entity.GetEntityByUid(s.sim.World, cfg.SectionId) - sectionState := component.PhysicalSectionStateType.Get(sectionEntry) - if !ok { - continue - } - stateInfos = append(stateInfos, &msg.StateInfo{ - CLR: !sectionState.Occ, - OCC: sectionState.Occ, - RAC: sectionRuntime.Rac, - RJO: sectionRuntime.Rjo, - RJT: sectionRuntime.Rjt, - }) - } - userData := []byte{0x00} //检查字节 - userData = append(userData, stateInfos.Encode()...) - //更新序列号及时间戳 - s.seqNum++ - s.lfsr1.add(0) - s.lfsr2.add(0) - //构建消息 - builder := &msg.RsdMsgBuilder{ - MsgHeader: msg.MsgHeader{ - ProtocolType: msg.ProtocolType_Sync, - MessageType: msg.MessageType_A, - SourceAddr: s.localAddr, - TargetAddr: s.remoteAddr, - }, - SeqNum: s.seqNum, - Svc1: s.calculateSvc1(userData), - Svc2: s.calculateSvc2(userData), - UserData: userData, - } - return builder -} - -func (s *serviceContext) calculateSvc1(userData []byte) uint32 { - return message.Rssp_I_Crc32C1(userData) ^ s.localSid1 ^ s.lfsr1.value ^ msg.SCW_1 -} - -func (s *serviceContext) calculateSvc2(userData []byte) uint32 { - return message.Rssp_I_Crc32C2(userData) ^ s.localSid2 ^ s.lfsr2.value ^ msg.SCW_2 -} - func (s *serviceContext) handleRsdMsg(data []byte) { - if s.sseMsg == nil { //正在时序校正过程中 + if s.sseMsg != nil { //正在时序校正过程中 return } rsdMsg := &msg.RsdMsg{} @@ -309,12 +350,13 @@ func (s *serviceContext) handleRsdMsg(data []byte) { return } //校验 - validateResult := s.validateRsdMsg(rsdMsg) + validateResult := s.validateRsdMsg(rsdMsg, data) if validateResult == 0 { return } else if validateResult == 2 { //开启时序校正流程 logger().Error("时序校验失败，开始时序校正") + s.startSeeProgress() return } //流程处理 @@ -350,19 +392,19 @@ func (s *serviceContext) handleSseMsg(data []byte) { logger().Error("SSE数据校验失败") return } - logger().Info("SSE数据通过校验") + logger().Info(fmt.Sprintf("SSE数据通过校验：%x", data)) //回复 ssrMsg := msg.SsrMsg{ MsgHeader: msg.MsgHeader{ ProtocolType: msg.ProtocolType_Sync, MessageType: msg.MessageType_SSR, - SourceAddr: s.remoteAddr, - TargetAddr: s.localAddr, + SourceAddr: s.localAddr, + TargetAddr: s.remoteAddr, }, SeqNumSsr: s.seqNum, SeqNumSse: sseMsg.SeqNum, SeqInit1: s.calculateSeqInit1(sseMsg.SeqEnq1), - SeqInit2: s.calculateSeqInit1(sseMsg.SeqEnq1), + SeqInit2: s.calculateSeqInit2(sseMsg.SeqEnq2), DataVer: 0x01, } ssrBytes := ssrMsg.Encode() @@ -372,8 +414,8 @@ func (s *serviceContext) handleSseMsg(data []byte) { } else { logger().Info(fmt.Sprintf("发送SSR数据：%x", ssrBytes)) //更新本地数据 - s.lastSeqParam1 = sseMsg.SeqEnq1 - s.lastSeqParam2 = sseMsg.SeqEnq2 + s.lastSeqParam1 = (&lfsr{value: s.remoteSinit1, poly: msg.T_POLY_1}).add(sseMsg.SeqEnq1) + s.lastSeqParam2 = (&lfsr{value: s.remoteSinit2, poly: msg.T_POLY_2}).add(sseMsg.SeqEnq2) } } @@ -394,9 +436,10 @@ func (s *serviceContext) handleSsrMsg(data []byte) { return } logger().Info("SSR数据通过校验") - //完成校正时序 + //完成校正时序（precSinit~+t_e == Sinit_r~+(sid_r^t_r)） + s.lastSeqParam1 = (&lfsr{value: s.precSinit1, poly: msg.T_POLY_1}).add(ssrMsg.SeqInit1 ^ (s.sseMsg.SeqEnq1 ^ s.localSid1)) + s.lastSeqParam2 = (&lfsr{value: s.precSinit2, poly: msg.T_POLY_2}).add(ssrMsg.SeqInit2 ^ (s.sseMsg.SeqEnq2 ^ s.localSid2)) s.sseMsg = nil - s.lastSeqParam1 = ssrMsg.SeqNumSsr } // 启动SSE流程 @@ -405,8 +448,8 @@ func (s *serviceContext) startSeeProgress() { MsgHeader: msg.MsgHeader{ ProtocolType: msg.ProtocolType_Sync, MessageType: msg.MessageType_SSE, - SourceAddr: s.remoteAddr, - TargetAddr: s.localAddr, + SourceAddr: s.localAddr, + TargetAddr: s.remoteAddr, }, SeqNum: s.seqNum, SeqEnq1: s.calculateSeqEnq1(), @@ -425,7 +468,7 @@ func (s *serviceContext) startSeeProgress() { // 校验RSD消息 // return 0-时序校验之外的失败 1-成功 2-时序异常 -func (s *serviceContext) validateRsdMsg(rsdMsg *msg.RsdMsg) int { +func (s *serviceContext) validateRsdMsg(rsdMsg *msg.RsdMsg, data []byte) int { if rsdMsg.SourceAddr != s.remoteAddr { logger().Error(fmt.Sprintf("源地址[%x]不正确[%s]", rsdMsg.SourceAddr, s.config.NetAConfig.RemoteAddr)) return 0 @@ -438,15 +481,15 @@ func (s *serviceContext) validateRsdMsg(rsdMsg *msg.RsdMsg) int { logger().Error(fmt.Sprintf("命令数据长度[%d]与配置长度[%d]不符", len(rsdMsg.UserData), len(s.ciSectionIndexConfigs))) return 0 } - if message.Rssp_I_Crc16(rsdMsg.UserData) != rsdMsg.Tail { + if message.Rssp_I_Crc16(data[:len(data)-2]) != rsdMsg.Tail { logger().Error(fmt.Sprintf("报文尾验证失败")) return 0 } - if s.validateSvc1(rsdMsg.Svc1, rsdMsg.UserData) { + if !s.validateSvc1(rsdMsg.Svc1, rsdMsg.UserData) { logger().Error(fmt.Sprintf("SVC1[%x]校验未通过", rsdMsg.Svc1)) return 2 } - if s.validateSvc2(rsdMsg.Svc2, rsdMsg.UserData) { + if !s.validateSvc2(rsdMsg.Svc2, rsdMsg.UserData) { logger().Error(fmt.Sprintf("SVC2[%x]校验未通过", rsdMsg.Svc2)) return 2 } @@ -459,37 +502,32 @@ func (s *serviceContext) validateSseMsg(sseMsg *msg.SseMsg) bool { func (s *serviceContext) validateSsrMsg(ssrMsg *msg.SsrMsg) bool { if s.sseMsg.SeqNum != ssrMsg.SeqNumSse { - logger().Error(fmt.Sprintf("SSR的Ne[%d]与请求方不符[%d]", ssrMsg.SeqNumSse, s.sseMsg.SeqNum)) + logger().Error(fmt.Sprintf("SSR的时序号[%d]与请求方不符[%d]", ssrMsg.SeqNumSse, s.sseMsg.SeqNum)) return false } return true } func (s *serviceContext) validateSvc1(svc uint32, userData []byte) bool { - return s.validateSvc(svc, msg.SCW_1, message.Rssp_I_Crc32C1(userData), s.sid1, s.sinit1, &s.lastSeqParam1) + return s.validateSvc(svc, msg.SCW_1, message.Rssp_I_Crc32C1(userData), s.remoteSid1, s.remoteSinit1, &s.lastSeqParam1, msg.T_POLY_1) } func (s *serviceContext) validateSvc2(svc uint32, userData []byte) bool { - return s.validateSvc(svc, msg.SCW_2, message.Rssp_I_Crc32C2(userData), s.sid2, s.sinit2, &s.lastSeqParam2) + return s.validateSvc(svc, msg.SCW_2, message.Rssp_I_Crc32C2(userData), s.remoteSid2, s.remoteSinit2, &s.lastSeqParam2, msg.T_POLY_2) } -func (s *serviceContext) validateSvc(svc uint32, scw uint32, crc1 uint32, sid uint32, sinit uint32, lastSeqParam *uint32) bool { +func (s *serviceContext) validateSvc(svc uint32, scw uint32, crc1 uint32, sid uint32, sinit uint32, lastSeqParam *uint32, tPoly uint32) bool { seqParam := crc1 ^ svc ^ scw - if *lastSeqParam == 0 { - *lastSeqParam = seqParam - return true - } - for i := 0; i < s.config.NetAConfig.MaxDeviation; i++ { - seqLfsr := lfsr{value: sinit} - seqLfsr.add(*lastSeqParam) - constLfsr := lfsr{value: sinit} + for i := 0; i <= s.config.NetAConfig.MaxDeviation; i++ { + seqLfsr := lfsr{value: *lastSeqParam, poly: tPoly} + constLfsr := lfsr{value: sinit, poly: tPoly} constLfsr.add(sid) for j := 0; j < i; j++ { seqLfsr.add(0) constLfsr.add(0) } - if seqLfsr.add(seqParam) == seqLfsr.add(sid) { - *lastSeqParam = seqParam + if seqLfsr.add(seqParam) == constLfsr.add(sid) { + *lastSeqParam = seqLfsr.load(sinit).add(seqParam) return true } } @@ -505,11 +543,11 @@ func (s *serviceContext) calculateSeqEnq2() uint32 { } func (s *serviceContext) calculateSeqInit1(seqEnq1 uint32) uint32 { - return seqEnq1 ^ s.sid1 ^ s.dataver1 ^ s.lfsr1.value + return seqEnq1 ^ s.localSid1 ^ s.localDataVer1 ^ s.lfsr1.value } func (s *serviceContext) calculateSeqInit2(seqEnq2 uint32) uint32 { - return seqEnq2 ^ s.sid2 ^ s.dataver2 ^ s.lfsr2.value + return seqEnq2 ^ s.localSid2 ^ s.localDataVer2 ^ s.lfsr2.value } type lfsr struct { @@ -517,6 +555,11 @@ type lfsr struct { poly uint32 //时间戳生成多项式 } +func (l *lfsr) load(value uint32) *lfsr { + l.value = value + return l +} + func (l *lfsr) add(x uint32) uint32 { l.value = l.value ^ x var carry bool @@ -530,6 +573,23 @@ func (l *lfsr) add(x uint32) uint32 { return l.value } +func (l *lfsr) post(x uint32) uint32 { + + var carry bool + for i := 0; i < 32; i++ { + carry = x&1 == 1 + if carry { + x ^= l.poly + } + x = x >> 1 + if carry { + x |= 0x80000000 + } + } + l.value ^= x + return l.value +} + func logger() *slog.Logger { loggerInit.Do(func() { privateLogger = slog.Default().With("tag", logTag) diff --git a/third_party/example/rssp/main.go b/third_party/example/rssp/main.go deleted file mode 100644 index 7286b60..0000000 --- a/third_party/example/rssp/main.go +++ /dev/null @@ -1,200 +0,0 @@ -package main - -import ( - "encoding/binary" - "fmt" - "joylink.club/bj-rtsts-server/third_party/axle_device/beijing12/msg" - "joylink.club/bj-rtsts-server/third_party/message" -) - -// 通过抓包数据测试svc校验逻辑 -func main() { - var scw1 uint32 = 0xAE390B5A - var sid1 uint32 = 0xa2bcfc8c - //var sid1 uint32 = 0x7665986c - //var sinit uint32 = 0xb763ec88 - var sinit uint32 = 0xc8b60868 - - var svcn_1 uint32 = 0xb4b6c3eb - svcn_1 = binary.LittleEndian.Uint32(binary.BigEndian.AppendUint32([]byte{}, svcn_1)) - var svcn uint32 = 0xb4c640a2 - svcn = binary.LittleEndian.Uint32(binary.BigEndian.AppendUint32([]byte{}, svcn)) - var userData []byte = []byte{0x80, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00} - crc1 := crc32Encode1(userData) - - m := func(svc1 uint32) uint32 { - return (svc1 ^ crc1) ^ scw1 - } - - rn_1 := m(svcn_1) - rn := m(svcn) - l1 := &Lfsr{ - value: sinit, - } - //l1_1 := &Lfsr{value: rn} - //rn = l1_1.add(0) - l1.add(rn_1) - l1.add(0) - l1.add(rn) - fmt.Printf("%x\n", l1.value) - - //sinit~ 0; i-- { - if (item^accum)&1 == 1 { - accum = (accum >> 1) ^ mask - } else { - accum >>= 1 - } - item >>= 1 - } - return accum -} - -const DIMTAB = 256 - -var crctab [2][DIMTAB]uint32 - -func onecrc(ch uint, item uint32, start uint32) uint32 { - accum := start - mask := maskcrc[ch] - for i := 8; i > 0; i-- { - if (item^accum)&1 != 0 { - accum = (accum >> 1) ^ mask - } else { - accum >>= 1 - } - item >>= 1 - } - return accum -} - -func mkcrctab(ch uint) { - for i := 0; i < DIMTAB; i++ { - crctab[ch][i] = onecrc(ch, uint32(i), 0) - } -} - -func Ncrc(ch uint, buf []byte, start uint32) uint32 { - crccode := start - for _, b := range buf { - crccode = crctab[ch][byte(crccode)^b] ^ ((crccode >> 8) & 0x00FFFFFF) - } - return crccode -} diff --git a/third_party/message/rssp_code.go b/third_party/message/rssp_code.go index a7fb857..466cdd5 100644 --- a/third_party/message/rssp_code.go +++ b/third_party/message/rssp_code.go @@ -20,22 +20,19 @@ func Rssp_I_Crc16(data []byte) uint16 { // 通道1的crc32 func Rssp_I_Crc32C1(data []byte) uint32 { - var result byte - for i, b := range data { - result = 0 - for i := 0; i < 8; i++ { - result <<= 1 - result |= b & 1 - b >>= 1 - } - data[i] = result - } - return uint32(RSSP_I_C1_CRC32.CalculateCRC(data)) + newData := reverseBitsInByte(data) + return uint32(RSSP_I_C1_CRC32.CalculateCRC(newData)) } // 通道2的crc32 func Rssp_I_Crc32C2(data []byte) uint32 { + newData := reverseBitsInByte(data) + return uint32(RSSP_I_C2_CRC32.CalculateCRC(newData)) +} + +func reverseBitsInByte(data []byte) []byte { var result byte + newData := make([]byte, len(data)) for i, b := range data { result = 0 for i := 0; i < 8; i++ { @@ -43,7 +40,7 @@ func Rssp_I_Crc32C2(data []byte) uint32 { result |= b & 1 b >>= 1 } - data[i] = result + newData[i] = result } - return uint32(RSSP_I_C2_CRC32.CalculateCRC(data)) + return newData }