ptb/ptc.go

package main

import (
	"bufio"
	"bytes"
	"encoding/binary"
	"encoding/hex"
	"errors"
	"fmt"
	"github.com/TwiN/go-color"
	"github.com/albenik/go-serial/v2"
	"log"
	"math/bits"
	"os"
	"regexp"
	"runtime"
	"strconv"
	"strings"
	"sync"
	"time"

	"github.com/howeyc/crc16"
)

var SCSversion = map[string]string{
	"A": "PTC-II",
	"B": "PTC-IIpro",
	"C": "PTC-IIe",
	"D": "PTC-IIex",
	"E": "PTC-IIusb",
	"F": "PTC-IInet",
	"H": "DR-7800",
	"I": "DR-7400",
	"K": "DR-7000",
	"L": "PTC-IIIusb",
	"T": "PTC-IIItrx",
}

type pflags struct {
	exit      bool
	stopmodem bool
	//disconnected chan struct{}
	connected    chan struct{}
	closeWaiting chan struct{}
}

type pmux struct {
	device sync.Mutex
	pactor sync.Mutex
	write  sync.Mutex
	read   sync.Mutex
	close  sync.Mutex
	bufLen sync.Mutex
	//	sendbuf sync.Mutex
	//	recvbuf sync.Mutex
}

type Modem struct {
	devicePath string

	localAddr  string
	remoteAddr string

	state State

	device     *serial.Port
	mux        pmux
	wg         sync.WaitGroup
	flags      pflags
	goodChunks int
	//	recvBuf       bytes.Buffer
	//	cmdBuf        chan string
	//	sendBuf       bytes.Buffer
	packetcounter bool
	chanbusy      bool
	cmdlineinit   string
	initfile      string
	closeOnce     sync.Once
}

const (
	SerialTimeout = 1
	PactorChannel = 4
	MaxSendData   = 255
	MaxFrameNotTX = 2
)

// ToPactor states
const (
	Closed State = iota
	Ready
	Connecting
	Conntected
)

type State uint8

//var debugMux sync.Mutex

func debugEnabled() int {
	if value, ok := os.LookupEnv("PACTOR_DEBUG"); ok {
		level, err := strconv.Atoi(value)
		if err == nil {
			return level
		}
	}
	return 0
}

func writeDebug(message string, level int) {
	//debugMux.Lock()
	//	defer debugMux.Unlock()
	if debugEnabled() >= level {
		_, file, no, ok := runtime.Caller(1)
		if ok {
			log.Println(file + "#" + strconv.Itoa(no) + ": " + message)
		} else {
			log.Println(message)
		}
	}
	return
}

// OpenModem: Initialise the pactor modem and all variables. Switch the modem into hostmode.
//
// Will abort if modem reports failed link setup, Close() is called or timeout
// has occured (90 seconds)
func OpenModem(path string, baudRate int, myCall string, initfile string, cmdlineinit string) (p *Modem, err error) {
	writeDebug("Trying to open "+path+" with "+strconv.Itoa(baudRate)+" baud.", 0)
	p = &Modem{
		// Initialise variables
		devicePath: path,

		localAddr:  myCall,
		remoteAddr: "",

		state: Closed,

		device: nil,
		flags: pflags{
			exit:         false,
			stopmodem:    false,
			connected:    make(chan struct{}, 1),
			closeWaiting: make(chan struct{}, 1)},
		goodChunks: 0,
		//	cmdBuf:       make(chan string, 0),
		cmdlineinit: cmdlineinit,
		initfile:    initfile,
	}

	writeDebug("Initialising pactor modem", 1)
	if err := p.checkSerialDevice(); err != nil {
		writeDebug(err.Error(), 1)
		return nil, err
	}

	//Setup serial device
	if p.device, err = serial.Open(p.devicePath, serial.WithBaudrate(baudRate), serial.WithReadTimeout(SerialTimeout)); err != nil {
		writeDebug(err.Error(), 1)
		return nil, err
	}

	err = p.init()
	if err != nil {
		return nil, err
	}
	// run modem thread

	time.Sleep(3 * time.Second) // give the device a moment to settle, so no commands get lost
	return p, nil
}

func (p *Modem) setState(state State) {
	writeDebug("Setting state to: "+strconv.FormatUint(uint64(state), 10), 1)
	p.state = state
}

func (p *Modem) IsClosed() bool {
	if p == nil {
		return true
	}
	return p.state == Closed
}

func (p *Modem) init() (err error) {
	writeDebug("Entering PACTOR init", 1)
	// Get modem out of CRC hostmode if it is still in it while starting.
	//p.stophostmode()
	//p._write(string([]byte{0xaa, 0xaa, 0x00, 0x01, 0x05, 0x4a, 0x48, 0x4f, 0x53, 0x54, 0x30, 0xfb, 0x3d}))
	//time.Sleep(100 * time.Millisecond)
	//_, _, _ = p._read(100)

	if _, _, err = p.writeAndGetResponse("", -1, false, 10240); err != nil {
		return err
	}

	// Make sure, modem is in main menu. Will respose with "ERROR:" when already in it -> Just discard answer!
	_, ans, err := p.writeAndGetResponse("Quit", -1, false, 1024)
	if err != nil {
		return err
	}

	if len(ans) < 2 {
		return errors.New("Modem does not react to Quit command. Please re-power your modem")
	}

	// Check if we can determine the modem's type
	_, ver, err := p.writeAndGetResponse("ver ##", -1, false, 1024)
	if err != nil {
		return err
	}
	re, err := regexp.Compile(`\w#1`)
	if err != nil {
		return errors.New("Cannot read SCS modem version string, did you configure the correct serial device? Error: " + err.Error())
	}
	version := strings.ReplaceAll(string(re.Find([]byte(ver))), "#1", "")
	modem, exists := SCSversion[version]

	if !exists {
		return errors.New("Found a modem type: " + ver + " which this driver doesn't support. Please contact the author.")
	}
	writeDebug("Found a "+modem+" modem at "+p.devicePath, 0)
	writeDebug("Running init commands", 1)
	ct := time.Now()
	commands := []string{"DD", "RESTART", "MYcall " + p.localAddr, "PTCH " + strconv.Itoa(PactorChannel),
		"MAXE 35", "REM 0", "CHOB 0", "PD 1",
		"ADDLF 0", "ARX 0", "BELL 0", "BC 0", "BKCHR 25", "CMSG 0", "LFIGNORE 0", "LISTEN 0", "MAIL 0", "REMOTE 0",
		"PDTIMER 5", "STATUS 1",
		"TONES 4", "MARK 1600", "SPACE 1400", "CWID 0", "CONType 3", "MODE 0",
		"DATE " + ct.Format("020106"), "TIME " + ct.Format("150405")}

	//run additional commands provided on the command line with "init"
	if p.cmdlineinit != "" {
		for _, command := range strings.Split(strings.TrimSuffix(p.cmdlineinit, "\n"), ";") {
			commands = append(commands, command)
		}
	}

	for _, cmd := range commands {
		var res string
		writeDebug("Sending command to modem: "+cmd, 0)
		_, res, err = p.writeAndGetResponse(cmd, -1, false, 1024)
		if err != nil {
			return err
		}
		if strings.Contains(res, "ERROR") {
			log.Print(`Command "` + cmd + `" not accepted: ` + res)
			return fmt.Errorf(`Command "` + cmd + `" not accepted: ` + res)
		} else {
			l := fmt.Sprintf("%s --> "+color.InGreen("OK\n"), cmd) // print some output
			log.Println(l)
			fmt.Println(l)
		}
	}

	// run additional commands stored in the init script file
	if p.initfile != "" {
		if err = p.runInitScript(p.initfile); err != nil {
			return err
		}
	}
	p.flags.stopmodem = false
	p.setState(Ready)
	go p.modemThread()
	//p.flags.exit = false
	return nil
}

// Send additional initialisation command stored in the InitScript
//
// Each command has to be on a new line
func (p *Modem) runInitScript(initScript string) error {
	if _, err := os.Stat(initScript); os.IsNotExist(err) {
		return fmt.Errorf("ERROR: PTC init script defined but not existent: %s", initScript)
	}

	file, err := os.Open(initScript)
	if err != nil {
		return err
	}
	defer file.Close()

	scanner := bufio.NewScanner(file)
	for scanner.Scan() {
		writeDebug("Sending command to modem: "+scanner.Text(), 0)
		if _, _, err = p.writeAndGetResponse(scanner.Text(), -1, false, 1024); err != nil {
			return err
		}
	}

	if err := scanner.Err(); err != nil {
		return err
	}

	return nil
}

func (p *Modem) modemThread() {
	writeDebug("start modem thread", 1)

	_, _, _ = p.read(1024)
	_, _, _ = p.writeAndGetResponse("JHOST4", -1, false, 1024)
	_, _, _ = p.read(1024)
	// need to wait a second for WA8DED mode to "settle"
	time.Sleep(time.Second)
	const chunkSize = 1024
	p.wg.Add(1)
	for !p.flags.stopmodem {

		// TX data
		if p.getNumFramesNotTransmitted() < MaxFrameNotTX {
			//				data := p.getSendData()
			data, err := s.Data.Data.Dequeue(256)
			if err == nil {
				//data := []byte(d.(string))
				writeDebug("Write data ("+strconv.Itoa(len(data))+"): "+hex.EncodeToString(data), 2)
				// TODO: Catch errors!
				_, _, _ = p.writeAndGetResponse(string(data), PactorChannel, false, chunkSize)
				time.Sleep(1000 * time.Millisecond) // wait for the data to be filled into the buffer

				if s.VARAMode {
					s.Command.Response.Enqueue(fmt.Sprintf("BUFFER %d", s.Data.Data.GetLen()))
				}

			}
		}

		// TX commands
		cmd, err := s.Command.Cmd.Dequeue()
		if err == nil {
			writeDebug("Write command ("+strconv.Itoa(len(cmd))+"): "+cmd, 1)
			_, ans, err := p.writeAndGetResponse(cmd, PactorChannel, true, chunkSize)
			if err != nil {
				writeDebug("Error when sending Command: "+err.Error(), 0)
			}
			if strings.HasPrefix(cmd, "C ") || strings.HasPrefix(cmd, "c ") {

				// in VARA mode we need to know if the connection is outgoing or incoming so mark outgoing connects
				p.setState(Connecting)
			}
			if len(ans) > 2 {
				if !s.VARAMode {
					s.Command.Response.Enqueue(ans[2:])
				}
			}
			writeDebug("Answer from modem: "+ans, 1)
			// TODO: Catch errors!
		}

		// RX
		var res []byte

		if channels, err := p.getChannelsWithOutput(); err == nil {
			for _, c := range channels {
				_, data, _ := p.writeAndGetResponse("G", c, true, chunkSize)
				if _, res, err = p.checkResponse(data, c); err != nil {
					writeDebug("checkResponse returned: "+err.Error(), 1)
				} else {
					if res != nil {
						writeDebug("response: "+string(res)+"\n"+hex.Dump(res), 3)
						switch c {
						case PactorChannel:
							s.Data.Response.Enqueue(res)
							writeDebug("Written to sendbuf", 3)
						case 254: //Status update
							p.chanbusy = int(res[0])&112 == 112 // See PTC-IIIusb manual "STATUS" command
							writeDebug("PACTOR state: "+strconv.FormatInt(int64(res[0]), 2), 2)
						default:
							writeDebug("Channel "+strconv.Itoa(c)+": "+string(res), 1)
						}
					}
				}
			}
		}
	}
	time.Sleep(100 * time.Millisecond)

	_, _, err := p.writeAndGetResponse("JHOST0", PactorChannel, true, chunkSize)
	if err != nil {
		writeDebug("PACTOR ERROR Could not get modem out of the WA8DED mode: "+err.Error(), 0)
	}

	writeDebug("Modemthread is about to end", 1)

	time.Sleep(100 * time.Millisecond)
	p.wg.Done()
	//p.ModemClose()
	writeDebug("exit modem thread", 1)
}

func (p *Modem) Close() error {
	_, file, no, ok := runtime.Caller(1)
	if ok {
		writeDebug("PACTOR Close called from "+file+"#"+strconv.Itoa(no), 1)
	} else {
		writeDebug("PACTOR Close called", 1)
	}

	var err error
	p.closeOnce.Do(func() { err = p.close() })
	return err
}

func (p *Modem) stophostmode() {
	writeDebug("Stopping WA8DED hostmode", 0)
	var ok bool
	var err error

	// Send JHOST0 in CRC-enhanced WA8DED mode syntax

	for ok == false {
		buff := make([]byte, 100)
		p.device.Write([]byte{0xaa, 0xaa, 0x00, 0x01, 0x05, 0x4a, 0x48, 0x4f, 0x53, 0x54, 0x30, 0xfb, 0x3d})
		time.Sleep(100 * time.Millisecond)
		for {
			n, err := p.device.Read(buff)
			if err != nil {
				log.Fatal(err)
				break
			}
			if n == 0 {
				break
			}
			//fmt.Printf("%v", string(buff[:n]))
		}
		p.device.Write([]byte("\rRESTART\r"))
		time.Sleep(1000 * time.Millisecond)
		for {
			n, err := p.device.Read(buff)
			if err != nil {
				log.Fatal(err)
				break
			}
			if n == 0 {
				break
			}
			//fmt.Printf("%v", string(buff[:n]))
		}
		ok, err = regexp.Match("cmd:", buff)
		if err != nil {
			writeDebug("Error in stophostmode: "+err.Error(), 0)
		}
	}
}

// Wait for transmission to be finished
//
// BLOCKING until either all frames are transmitted and acknowledged or timeouts
// occurs
func (p *Modem) waitTransmissionFinish(t time.Duration) error {
	timeout := time.After(t)
	tick := time.Tick(500 * time.Millisecond)
	for {
		notAck := p.getNumFrameNotAck()
		notTrans := p.getNumFramesNotTransmitted()
		select {
		case <-timeout:
			if notAck != 0 && notTrans != 0 {
				return errors.New("Timeout: " + strconv.Itoa(notAck) + "frames not ack and " + strconv.Itoa(notTrans) + " frames not transmitted")
			} else if notAck != 0 {
				return errors.New("Timeout: " + strconv.Itoa(notAck) + "frames not ack")
			} else {
				return errors.New("Timeout: " + strconv.Itoa(notTrans) + " frames not transmitted")
			}

		case <-tick:
			if notAck == 0 && notTrans == 0 {
				return nil
			}
		}
	}
}

// Close current serial connection. Stop all threads and close all channels.
func (p *Modem) close() (err error) {

	_, file, no, ok := runtime.Caller(1)
	if ok {
		writeDebug("PACTOR close called from "+file+"#"+strconv.Itoa(no), 1)
	} else {
		writeDebug("PACTOR close called", 1)
	}

	//p.disconnect()
	writeDebug("signal modem thread to stop", 1)
	p.flags.stopmodem = true
	writeDebug("waiting for all threads to exit", 1)
	p.wg.Wait()
	writeDebug("modem thread stopped", 1)

	//p.hostmodeQuit()
	// will not close the serial port as we reuse it
	//p.device.Close()
	p.stophostmode()
	p.setState(Closed)
	writeDebug("PACTOR close() finished", 1)
	return nil
}

// Set specified flag (channel)
func setFlag(flag chan struct{}) {
	flag <- struct{}{}
}

// Get number of frames not yet transmitted by the modem
func (p *Modem) getNumFramesNotTransmitted() int {
	//return p.channelState.c
	return 0
}

// Get number of frames not acknowledged by the remote station
func (p *Modem) getNumFrameNotAck() int {
	//return p.channelState.d
	return 0
}

// Check if serial device is still available (e.g still connected)
func (p *Modem) checkSerialDevice() (err error) {
	if _, err := os.Stat(p.devicePath); os.IsNotExist(err) {
		return fmt.Errorf("ERROR: Device %s does not exist", p.devicePath)
	}
	return nil
}

// Poll channel 255 with "G" command to find what channels have output.
// Write them into the channels list.
func (p *Modem) getChannelsWithOutput() (channels []int, err error) {
	//Poll Channel 255 to find what channels have output
	n, chs, err := p.writeAndGetResponse("G", 255, true, 1024)
	if err != nil {
		return nil, err
	}

	channels = make([]int, 0)

	for i, ch := range []byte(chs)[:n-1] {
		if (i == 0 && ch == 255) || (i == 1 && ch == 1) {
			continue
		}
		channels = append(channels, int(ch)-1)
	}

	writeDebug("Channels with output: "+fmt.Sprintf("%#v", channels), 2)

	return
}

// Do some checks on the returned data.
//
// Currently, only connection information (payload) messages are passed on
func (p *Modem) checkResponse(resp string, ch int) (n int, data []byte, err error) {
	if len(resp) < 3 {
		return 0, nil, fmt.Errorf("No data")
	}

	head := []byte(resp[:3])
	payload := []byte(resp[3:])
	length := int(head[2]) + 1
	pl := len(payload)
	if int(head[0]) != ch {
		writeDebug("WARNING: Returned data does not match polled channel\n"+hex.Dump([]byte(resp)), 1)
		return 0, nil, fmt.Errorf("Channel missmatch")
	}
	if int(head[1]) == 1 { //sucess,message follows
		writeDebug("*** SUCCESS: "+string(payload), 0)
	}
	if int(head[1]) == 2 {
		writeDebug("*** ERROR: "+string(payload), 0)
	}
	if int(head[1]) != 7 && int(head[1]) != 3 {
		if !s.VARAMode {
			s.Command.Response.Enqueue(fmt.Sprintf("%s\n", payload))
		}
		writeDebug("Message from Modem: "+string(payload), 0)
		return 0, nil, fmt.Errorf("Not a data response")
	}
	if int(head[1]) == 3 { //Link status
		if !s.VARAMode {
			s.Command.Response.Enqueue(fmt.Sprintf("%s\n", payload))
		}
		writeDebug("*** LINK STATUS: "+string(payload), 0)
		// there is no way to get the remote callsign from the WA8DED data, so we have to parse the link status :(
		re, err := regexp.Compile(` CONNECTED to \w{2,16}`)
		if err != nil {
			writeDebug("Cannot convert connect message to callsign: "+string(payload), 1)
		} else {
			ans := strings.ReplaceAll(string(re.Find(payload)), " CONNECTED to ", "")
			if len(ans) > 2 { //callsign consists of 3+ characters
				p.remoteAddr = ans
				writeDebug("PACTOR connection to: "+ans, 0)
				if s.VARAMode {
					if p.state == Connecting { //outgoing connection
						s.Command.Response.Enqueue(fmt.Sprintf("CONNECTED %s %s BW", p.localAddr, ans))
					} else { //incoming connection
						s.Command.Response.Enqueue(fmt.Sprintf("CONNECTED %s %s BW", ans, p.localAddr))
					}
				}
				p.setState(Conntected)
				return 0, nil, nil
			}
		}
		if strings.Contains(string(payload), "DISCONNECTED") {
			writeDebug("PACTOR DISCONNECTED", 0)
			p.setState(Ready)
			if s.VARAMode {
				s.Command.Response.Enqueue("DISCONNECTED")
			}
			// }
			//p.channelState.f = 0
			return 0, nil, nil
		}
		return 0, nil, fmt.Errorf("Link data")
	}
	if length != pl {
		writeDebug("WARNING: Data length "+strconv.Itoa(pl)+" does not match stated amount "+strconv.Itoa(length)+". After "+strconv.Itoa(p.goodChunks)+" good chunks.", 1)
		p.goodChunks = 0
		if pl < length {
			// TODO: search for propper go function
			for i := 0; i < (length - pl); i++ {
				payload = append(payload, 0x00)
			}
		} else {
			payload = payload[:length]
		}
	} else {
		p.goodChunks += 1
		writeDebug("Good chunk", 2)
	}
	return length, payload, nil
}

// Write and read response from pactor modem
//
// Can be used in both, normal and hostmode. If used in hostmode, provide
// channel (>=0). If used in normal mode, set channel to -1, isCommand is
// ignored.
func (p *Modem) writeAndGetResponse(msg string, ch int, isCommand bool, chunkSize int) (int, string, error) {
	writeDebug("wagr: Channel: "+strconv.Itoa(ch)+"; isCommand: "+strconv.FormatBool(isCommand)+"\n"+hex.Dump([]byte(msg)), 3)
	var err error

	var n int
	var str string
	if ch < 0 {
		err = p._write(msg + "\r")
		if err != nil {
			return 0, "", err
		}
		time.Sleep(500 * time.Millisecond)
		i := 0
		var tmp []byte
		for {
			n, tmp, err = p._read(chunkSize)
			str = string(tmp)
			if err == nil {
				break
			}
			i += 1
			if i > 9 {
				writeDebug("No successful read after 10 times!", 1)
				return 0, "", err
			}
		}
		writeDebug(fmt.Sprintf("response: %s\n%s", str, hex.Dump(tmp)), 2)
		return n, str, err
	} else {
		err = p.writeChannel(msg, ch, isCommand)
		if err != nil {
			return 0, "", err
		}
		if msg == "JHOST0" { //looks like the SCS modems do not answer to JHOST0, although the standard defines it
			return 0, "", nil
		}
		time.Sleep(50 * time.Millisecond)
		writeDebug("Decode WA8DED", 4)
		buf := []byte{}
		valid := false

		for valid == false {
			if bytes.Compare(buf, []byte{170, 170, 170, 85}) == 0 { // check for re-request 	#170#170#170#85
				// packet was re-requested!!
				writeDebug("Re-Request magic received", 3)
				buf = []byte{}                           //delete the re-request packet
				err = p.writeChannel(msg, ch, isCommand) // write command again
				if err != nil {
					return 0, "", err
				}
			}
			br, b, err := p._read(1)
			if err != nil {
				writeDebug("ERROR at _read: "+error.Error(err), 1)
			}
			writeDebug("Len: "+strconv.Itoa(len(buf))+"State: "+string(hex.Dump(buf)+"\n"), 4)
			if br > 0 {
				//we got some data
				buf = append(buf, b...)
				if len(buf) > 5 { // otherwise it's no enh. WA8DED: 2 magic bytes, 2 header bytes, 2 CRC bytes
					//unstuff (from 3rd byte on)
					t := bytes.Replace(buf[2:], []byte{170, 0}, []byte{170}, -1)
					buf = append([]byte{0xaa, 0xaa}, t...)
					if checkcrc(buf) {
						n = len(t) - 2
						str = string(t[:n])
						valid = true
					} else {
						writeDebug("(still) Invalid checksum", 4)
					}

				}
			}
		}

	}

	p.packetcounter = !(p.packetcounter)
	writeDebug("wagr: returning \n"+hex.Dump([]byte(str)), 3)
	return n, str, err
}

// Flush waits for the last frames to be transmitted.
//
// Will throw error if remaining frames could not bet sent within 120s
func (p *Modem) Flush() (err error) {
	writeDebug("Flush called", 2)
	if err = p.waitTransmissionFinish(120 * time.Second); err != nil {
		writeDebug(err.Error(), 2)
	}
	return
}

// TxBufferLen returns the number of bytes in the out sendbuf queue.
func (p *Modem) TxBufferLen() int {
	writeDebug("TxBufferLen called ("+strconv.Itoa(s.Data.Data.GetLen())+" bytes remaining in sendbuf)", 2)
	return s.Data.Data.GetLen() + (p.getNumFramesNotTransmitted() * MaxSendData)
}

func (p *Modem) Busy() bool {
	return p.chanbusy
}

// Write to serial connection (thread safe)
//
// No other read/write operation allowed during this time
func (p *Modem) write(cmd string) error {
	p.mux.pactor.Lock()
	defer p.mux.pactor.Unlock()
	return p._write(cmd)
}

// Write to serial connection (NOT thread safe)
//
// If used, make shure to lock/unlock p.mux.pactor mutex!
func (p *Modem) _write(cmd string) error {
	if err := p.checkSerialDevice(); err != nil {
		writeDebug(err.Error(), 1)
		return err
	}

	writeDebug("write: \n"+hex.Dump([]byte(cmd)), 3)

	p.mux.device.Lock()
	defer p.mux.device.Unlock()
	out := cmd + "\r"
	for {
		status, err := p.device.GetModemStatusBits()
		if err != nil {
			writeDebug("GetModemStatusBits failed. cmd: "+cmd+" Error: "+err.Error(), 1)
			return err
		}
		if status.CTS {
			for {
				sent, err := p.device.Write([]byte(out))
				if err == nil {
					break
				} else {
					//					log.Errorf("ERROR while sending serial command: %s\n", out)
					writeDebug(err.Error(), 2)
					out = out[sent:]
				}
			}
			break
		}
	}

	return nil
}

// *** Helper functions for the CRC hostmode
// Although these functions are small, I prefer to keep their functionality
// separate. They follow the steps in the SCS documentation

// helper function: de-hexlify and write to debug channel
func printhex(s string) {
	t, _ := hex.DecodeString(s)
	writeDebug(string(t), 3)
}

// helper function: "unstuff" a string
func unstuff(s string) string {
	//Expect: the string contains aa aa at the beginning, that should NOT be
	//stuffed
	n, _ := hex.DecodeString(s[4:])
	n = bytes.Replace(n, []byte{170, 0}, []byte{170}, -1)
	var r []byte
	r = append([]byte{0xaa, 0xaa}, n...)
	re := hex.EncodeToString(r)
	return re
}

// helper function: "stuff" a string: replaces every #170 with #170#0
func stuff(s string) string {
	//Expect: the string contains aa aa at the beginning, that should NOT be
	//stuffed
	n, err := hex.DecodeString(s[4:])
	if err != nil {
		writeDebug("ERROR in Stuff: "+err.Error()+"\n"+string(hex.Dump([]byte(s))), 1)
	}

	n = bytes.Replace(n, []byte{170}, []byte{170, 0}, -1)
	var r []byte
	r = append([]byte{0xaa, 0xaa}, n...)
	re := hex.EncodeToString(r)
	return re
}

// helper function: calculates the CCITT-CRC16 checksum
func checksum(s string) uint16 {
	tochecksum, _ := hex.DecodeString(s[4:])
	chksum := bits.ReverseBytes16(crc16.ChecksumCCITT([]byte(tochecksum)))
	return chksum
}

// helper fuction: check the checksum by comparing
func checkcrc(s []byte) bool {
	tochecksum := s[2 : len(s)-2]
	chksum := bits.ReverseBytes16(crc16.ChecksumCCITT(tochecksum))
	pksum := s[len(s)-2:]
	return (binary.BigEndian.Uint16(pksum) == chksum)
}

// super helper fuction: convert an ordinary WA8DED message into a CRC-Hostmode message
func docrc(msg string) string {
	// step 1: add a #170170
	msg = fmt.Sprintf("%02x%02x%s", 170, 170, msg)
	// step 2: calculate and add the checksum
	chksum := checksum(msg)
	msg = fmt.Sprintf("%s%04x", msg, chksum)
	// step 3: add "stuff" bytes
	msg = stuff(msg)
	return msg

}

// Write channel to serial connection (NOT thread safe)
//
// If used, make shure to lock/unlock p.mux.pactor mutex!
func (p *Modem) writeChannel(msg string, ch int, isCommand bool) error {
	if err := p.checkSerialDevice(); err != nil {
		writeDebug(err.Error(), 1)
		return err
	}

	var d byte
	switch {
	case !p.packetcounter && !isCommand:
		d = byte(0x00)
	case !p.packetcounter && isCommand:
		d = byte(0x01)
	case p.packetcounter && !isCommand:
		d = byte(0x80)
	case p.packetcounter && isCommand:
		d = byte(0x81)
	}

	var o []byte = []byte{170, 170, byte(ch), byte(d), byte((len(msg) - 1))}
	o = append(o, []byte(msg)...)
	cksum := bits.ReverseBytes16(crc16.ChecksumCCITT(o[2:]))
	cksumb := make([]byte, 2)
	binary.BigEndian.PutUint16(cksumb, cksum)
	o = append(o, cksumb...)
	tostuff := o[2:]
	tostuff = bytes.Replace(tostuff, []byte{170}, []byte{170, 0}, -1)
	o = append([]byte{170, 170}, tostuff...)
	/*
		msg = fmt.Sprintf("%02x%02x%s", 170, 170, msg)
		// step 2: calculate and add the checksum
		chksum := checksum(msg)
		msg = fmt.Sprintf("%s%04x", msg, chksum)
		// step 3: add "stuff" bytes
		msg = stuff(msg)
	*/
	/*c := fmt.Sprintf("%02x", ch)
	l := fmt.Sprintf("%02x", (len(msg) - 1))
	s := hex.EncodeToString([]byte(msg))
	// add crc hostmode addons
	bs, _ := hex.DecodeString(docrc(fmt.Sprintf("%s%s%s%s", c, d, l, s)))*/
	writeDebug("write channel: \n"+hex.Dump(o), 2)

	if err := p.write(string(o)); err != nil {
		writeDebug(err.Error(), 2)
		return err
	}
	/*if !isCommand {
		p.cmdBuf <- "%Q"
	}*/
	return nil
}

// Read from serial connection (thread safe)
//
// No other read/write operation allowed during this time
func (p *Modem) read(chunkSize int) (int, []byte, error) {
	p.mux.pactor.Lock()
	defer p.mux.pactor.Unlock()
	return p._read(chunkSize)
}

// Read from serial connection (NOT thread safe)
//
// If used, make shure to lock/unlock p.mux.pactor mutex!
func (p *Modem) _read(chunkSize int) (int, []byte, error) {
	if err := p.checkSerialDevice(); err != nil {
		writeDebug(err.Error(), 1)
		return 0, []byte{}, err
	}

	buf := make([]byte, chunkSize)

	p.mux.device.Lock()
	defer p.mux.device.Unlock()
	n, err := p.device.Read(buf)
	if err != nil {
		writeDebug("Error received during read: "+err.Error(), 1)
		return 0, []byte{}, err
	}

	return n, buf[0:n], nil
}