testbench/tcp/bds/util.go

package bds

import (
    "wb/lg"
)

func EscapeChars(src []byte) []byte {
    dstLen := len(src)
    for i:=1; i< len(src) -1;i++ {
        b := src[i]
        if b == startStopBit || b == escChar {
            dstLen += 1
        }
    }
    if dstLen == len(src) {
        return src[:]
    }
    dst := make([]byte, dstLen)
    dst[0] = 0x7e
    dst[dstLen -1] = 0x7e
    idx := 1
    for i:=1; i< len(src) -1;i++{
        b := src[i]
        switch b {
        case startStopBit:
            dst[idx] = escChar
            idx += 1
            dst[idx] = startStopAdd
        case escChar:
            dst[idx] = escChar
            idx += 1
            dst[idx] = escCharAdd
        default:
            dst[idx] = b
        }
        idx += 1
    }
    return dst
}

func RestoreEscChars(src []byte) []byte {
    dstLen := len(src)
    for _, b := range src {
        if b == escChar {
            dstLen -= 1
        }
    }
    dst := make([]byte, dstLen)
    idx := 0
    isEsc := false
    for _, b := range src {
        if isEsc {
            isEsc = false
            switch b {
            case escCharAdd:
                dst[idx] = escChar
            case startStopAdd:
                dst[idx] = startStopBit
            default:
                lg.Error("bds.restoreEscChars")
            }
            idx += 1
            continue
        }
        if b == escChar {
            isEsc = true
        } else {
            isEsc = false
            dst[idx] = b
            idx += 1
        }
    }
    return dst
}