fix change freq bug

Signed-off-by: surenyi <surenyi82@163.com>

src/Makefile

@@ -48,9 +48,11 @@ $(PROG) : $(objs)
 
 pkg: $(PROG)
 	@ mkdir -p usr/lib/lua/5.3
+	@ mkdir -p usr/share/lua/5.3
 	@ mkdir -p usr/bin
 	@ mkdir -p etc/icd
 	@ cp $(LIBS_DIR)/bin/cjson.so usr/lib/lua/5.3
+	@ cp icd.lua usr/share/lua/5.3/
 	@ cp $(PROG) usr/bin
 	@ cp iphone.lua etc/icd
 	@ tar czf ../icd-rf-release.tar.gz etc usr

src/icd.lua

@@ -0,0 +1,258 @@
+constants = {
+    IP_INT  = "192.168.11.101",
+    IP_EXT  = "192.168.31.101",
+    IP_PRIV = "192.168.9.1",
+
+    MSG_MINLEN = 24,
+
+    DEV_ID_HOST    = 0x0222,
+    DEV_TERM_ID_1  = 0x0B02,
+    DEV_TERM_ID_2  = 0x0B03,
+    DEV_TERM_ID_3  = 0x0B04,
+    DEV_TERM_ID_4  = 0x0B05,
+    DEV_TERM_ID_5  = 0x0B20,
+    DEV_TERM_ID_6  = 0x0B21,
+    DEV_PTERM_ID_1 = 0x0210,
+    DEV_PTERM_ID_2 = 0x0310,
+    DEV_TERM_ALL   = 0xffff,
+
+    DEV_ID_STORE   = 0x0123,
+
+    SW_ID_HOST    = 0x0110,
+    SW_ID_TERM    = 0x1009,
+    SW_ID_PTERM   = 0x0402,
+    SW_ID_ADAPTER = 0x1001,
+
+    MSG_ID_IPHONE   = 0x1101,
+    MSG_ID_ALARM    = 0x1102,
+    MSG_ID_SELFTEST = 0x111E,
+    MSG_ID_CFREQ    = 0x1103,
+    MSG_ID_CFRES    = 0x1104,
+    MSG_ID_NTP      = 0x1125,
+    MSG_ID_NTP_STATUS = 0x1128,
+
+    CF_ID_VOL            = 0x3,
+    CF_ID_IPHONE_CHANNEL = 0x0B,
+    CF_ID_IPHONE_FREQ    = 0x0C,
+    CF_ID_STATION_ID     = 0x13,
+
+    GROUP_IP_ALARM = "225.0.0.22", -- internal or external net segment.
+    PORT_ALARM_IN  = 7001,
+    PORT_ALARM_OUT = 6001, -- iphone, alarm data , config response.
+    PORT_ST_IN     = 6002, -- selftest, config request.
+
+    GROUP_IP_ST     = "239.1.11.30", -- selftest [internal net segment]. 
+    PORT_ST_OUT     = 11006,
+
+    GROUP_IP_NTP = "239.1.11.30" ,-- ntp [external net segment]
+    PORT_NTP_IN  = 11004,
+    PORT_NTP_OUT = 11005,
+}
+
+local _seq = 0
+local _station = 0
+
+function set_station(sid)
+    _station = sid
+end
+
+function get_station()
+    return _station
+end
+
+function build_header(sdevid, sswid, ddevid, dswid, msgid, msglen)
+    msglen = msglen + 2 -- add last reserved 2 bytes
+
+    local seq = _seq
+    _seq = _seq + 1
+    if _seq > 255 then
+        _seq = 0
+    end
+
+    local sid = get_station()
+
+    local hdr = string.pack('HHHHHHIBBHHH', sid, sdevid, sswid, sid, 
+                ddevid, dswid, 0, 0, seq, msglen, msgid, 0);
+    assert(#hdr == 24)
+    return hdr
+end
+
+function build_packet(sdevid, sswid, ddevid, dswid, msgid, data)
+    local hdr = build_header(sdevid, sswid, ddevid, dswid, msgid, #data)
+    return hdr .. data
+end
+
+function build_alarm_text(termid, mode, data)
+    local ctx = string.pack('B', mode) .. data
+    return build_packet(0, 0, termid, constants.SW_ID_HOST, constants.MSG_ID_ALARM, ctx)
+end
+
+function build_alarm_data(termid, mode, data)
+    local ctx = string.pack('B', mode) .. data
+    return build_packet(constants.DEV_ID_HOST, constants.SW_ID_HOST,
+           termid, constants.SW_ID_TERM, constants.MSG_ID_ALARM, ctx)
+end
+
+function build_iphone_status(ch, cfreq, rxtx, freqs)
+    local m, d = math.modf(cfreq)
+    d = math.tointeger(d * 1000)
+    local data = string.pack('BHHB', ch, m, d, rxtx)
+
+    for _, v in ipairs(freqs) do
+        m, d = math.modf(v)
+        d = math.tointeger(d * 1000)
+        data = data .. string.pack('HH', m, d)
+    end
+    return build_packet(constants.DEV_ID_HOST, constants.SW_ID_HOST,
+        constants.DEV_TERM_ALL, constants.SW_ID_TERM, constants.MSG_ID_IPHONE, data)
+end
+
+function build_iphone_tx(termid, data)
+    return build_packet(constants.DEV_ID_HOST, constants.SW_ID_HOST,
+        termid, constants.SW_ID_TERM, constants.MSG_ID_IPHONE, data)
+end
+
+function build_iphone_rx(termid, seq, data)
+    return build_packet(termid, constants.SW_ID_TERM, constants.SW_ID_TERM,
+        constants.DEV_ID_HOST, constants.SW_ID_HOST, constants.MSG_ID_IPHONE, data)
+end
+
+function build_host_selftest(fault, fault_code, version, tdata)
+	local tmaps = {
+		constants.DEV_TERM_ID_1,
+		constants.DEV_TERM_ID_2,
+		constants.DEV_TERM_ID_3,
+		constants.DEV_TERM_ID_4,
+		constants.DEV_TERM_ID_5,
+		constants.DEV_TERM_ID_6,
+		constants.DEV_PTERM_ID_1,
+		constants.DEV_PTERM_ID_2,
+	}
+	local s = ""
+	for _, v in ipairs(tmaps) do
+		if tdata[v] ~= nil then
+			s = s .. tdata[v]
+		else
+			s = s .. string.rep("\x00", 10)
+		end
+	end
+
+    local ctx = '\x00' .. string.pack('BII', fault, fault_code, version) .. s
+    return build_packet(constants.DEV_ID_HOST, constants.SW_ID_HOST,
+        0, constants.SW_ID_ADAPTER, constants.MSG_ID_SELFTEST, ctx)
+end
+
+function build_term_selftest(termid, is_portable, fault, fault_code, version)
+    local ctx = '\x00' .. string.pack('BII', fault, fault_code, version)
+
+    local sw = constants.SW_ID_TERM
+    if is_portable then
+        sw = constants.SW_ID_PTERM
+    end
+
+    return build_packet(termid, sw, constants.DEV_ID_HOST, constants.SW_ID_HOST, constants.MSG_ID_SELFTEST, ctx)
+end
+
+function build_config_req(termid, mode, cfid, cflen, cdata)
+    local data = string.pack('BBB', mode, cfid, cflen)
+
+    if cflen > 0 then
+        data = data .. cdata
+    end
+    return build_packet(termid, constants.SW_ID_TERM, constants.DEV_ID_HOST, constants.SW_ID_HOST,
+        constants.MSG_ID_CFREQ, data)
+end
+
+function build_config_res(termid, succ, mode, cfid, cflen, cdata)
+    local data = string.pack('BBBB', mode, succ, cfid, cflen)
+
+    if cflen > 0 then
+        data = data .. cdata
+    end
+
+    return build_packet(constants.DEV_ID_HOST, constants.SW_ID_HOST,
+        termid, constants.SW_ID_TERM, constants.MSG_ID_CFRES, data)
+end
+
+function build_ntp_request()
+    return build_header(constants.DEV_ID_STORE, constants.SW_ID_ADAPTER, 0xff, 0xff, constants.MSG_ID_NTP, 0)
+end
+
+function build_ntp_status(status)
+    local data = string.pack('B', status)
+    return build_packet(constants.DEV_ID_HOST, constants.SW_ID_HOST, 
+        constants.DEV_ID_STORE, constants.SW_ID_ADAPTER, constants.MSG_ID_NTP_STATUS, data)
+end
+
+function parse_msg_id(pkt)
+    return string.unpack('H', pkt:sub(21))
+end
+
+function parse_src_dev(pkt)
+    return string.unpack('H', pkt:sub(3))
+end
+
+function parse_src_sw(pkt)
+    return string.unpack('H', pkt:sub(5))
+end
+
+function parse_dst_dev(pkt)
+    return string.unpack('H', pkt:sub(9))
+end
+
+function parse_dst_sw(pkt)
+    return string.unpack('H', pkt:sub(11))
+end
+
+function parse_alarm(pkt)
+    return pkt:byte(25), pkt:sub(26)
+end
+
+function parse_iphone_status(pkt) -- return rxtx, id, cfreq, freqs
+    if #pkt ~= 70 then
+        error("iphone status packet too short: ", #pkt)
+        return nil
+    end
+
+    local m, d = string.unpack('HH', pkt:sub(26))
+    local cfreq = m + d / 1000.0
+
+    local freqs = {}
+    for i = 31, 69, 4 do
+        m, d = string.unpack('HH', pkt:sub(i))
+        local cf = m + d / 1000.0
+        table.insert(freqs, cf)
+    end
+    return pkt:byte(30), pkt:byte(25), cfreq, freqs
+end
+
+function parse_iphone_data(pkt)
+    return pkt:sub(25)
+end
+
+function parse_config_req(pkt)
+    local cflen = pkt:byte(27)
+    if cflen > 0 then
+        return pkt:byte(25), pkt:byte(26), pkt:sub(28)
+    else
+        return pkt:byte(25), pkt:byte(26)
+    end
+end
+
+function parse_config_res(pkt)
+    local mode = pkt:byte(25)
+    local succ = pkt:byte(26)
+    local cfid = pkt:byte(27)
+    local cflen = pkt:byte(28)
+
+    if cflen > 0 then
+        return succ, mode, cfid, pkt:sub(29)
+    else
+        return succ, mode, cfid
+    end
+end
+
+function parse_ntp_status(pkt)
+    return pkt:byte(25)
+end
+

src/iphone.lua

@@ -3,21 +3,14 @@
 --
 -- put cjson.so to /usr/lib/lua/5.3/
 --
-local config_path = "/etc/icd/config.json"
-
-local constants = {
-	MSG_TYPE_INP = 0x1101,
-	MSG_TYPE_QCF = 0x1103,
-	MSG_TYPE_RCF = 0x1104,
-
-	CONFIG_TYPE_FREQ  =    0x0c,
-	CONFIG_TYPE_CHANNEL  = 0x0b,
+require("icd")
+cjson = require "cjson"
 
-	SLOT = 1,
+local config_path = "/etc/icd/config.json"
 
-	STATE_TX  = 0,
-	STATE_RX  = 1,
-}
+local STATE_TX  = 0
+local STATE_RX  = 1
+local SLOT = 1
 
 local options = {
 	channel = 6,
@@ -28,10 +21,6 @@ local options = {
 		421.125,
 		422.125,
 
---      423.125,
---		424.125,
---		425.125,
-
 		418.125,
 		419.125,
 		420.125,
@@ -41,13 +30,14 @@ local options = {
 }
 
 local phone = {
-	cjson = require "cjson",
-	state = constants.STATE_TX,
-	seq = 0
+	state = STATE_TX,
+	wait   = false,
+	cookie = 0.0,
+	seq    = 0
 }
 
 function save_cfg()
-	local ss = phone.cjson.encode(options)
+	local ss = cjson.encode(options)
 	local s = json_stringfy(ss)
 
 	s = s or ss
@@ -66,7 +56,7 @@ function load_cfg()
 	if f ~= nil then
 		local s = f:read('a')
 		f:close()
-		local n = phone.cjson.decode(s)
+		local n = cjson.decode(s)
 		if n ~= nil then
 			options = n
 			r = true
@@ -77,6 +67,13 @@ function load_cfg()
 	end
 end
 
+function sock_thread(s)
+	while true do
+		phone:on_udp(s)
+		s = coroutine.yield()
+	end
+end
+
 function dev_init(tty, ethx)
 	mcu.init()
 	xtty.init(tty)
@@ -88,8 +85,9 @@ function dev_init(tty, ethx)
 	end)
 	local eth = get_interface_address(ethx)
 	print(string.format("%s:\t%s", ethx, eth))
+	phone.sock_thread = coroutine.wrap(sock_thread)
 	phone.udp = udp.open(function (s)
-		phone:on_udp(s)
+		phone.sock_thread(s)
 	end)
 	phone.udp:bind("0.0.0.0", 6002)
 	phone.udp:add_multicast_membership("225.0.0.22", eth)
@@ -100,7 +98,7 @@ function dev_init(tty, ethx)
 		phone:on_timeout()
 	end)
 
-	phone.pcm = pcm_stream_open(constants.SLOT, function()
+	phone.pcm = pcm_stream_open(SLOT, function()
 		phone:pcm_start()
 	end,
 	function()
@@ -165,15 +163,19 @@ function phone:set_channel(id)
 end
 
 function phone:set_freq(id, freq)
-	self:set_channel(id)
-	xtty.set_freq(id, freq)
+	xtty.set_freq(freq, freq)
 	options.freqs[id] = freq
 	loginfo("set channel " .. id .. " freq to " .. freq)
 	msleep(50)
 end
 
 function phone:ingress(t, c, s_r, data)
-	local s = string.format("type %x, cmd %x, s_r %x : ", t, c, s_r)
+	local s = string.format("cmd %x, rwtype %x, s_r %x : ", c, t, s_r)
+	if self.wait then
+		if self.sock_thread ~= nil then
+			self.sock_thread(c, s_r, data)
+		end
+	end
 	loginfo(s)
 	logdump(data)
 end
@@ -184,12 +186,13 @@ function phone:build_header(t, tlen)
 end
 
 function phone:build_response(term, mlen)
-	local s = '\x00\x00\x22\x02\x10\x01\x00\x00' .. string.pack('H', term)  .. '\x09\x10\x00\x00\x00\x00\x00' .. string.pack('BHH', phone:get_seq(), mlen, constants.MSG_TYPE_RCF) .. '\x00\x00'
+	local s = '\x00\x00\x22\x02\x10\x01\x00\x00' .. string.pack('H', term)  .. '\x09\x10\x00\x00\x00\x00\x00' .. string.pack('BHH', phone:get_seq(), mlen, constants.MSG_ID_CFRES) .. '\x00\x00'
 	return s
 end
 
 function phone:on_pcma(pcma)
-	local s = self:build_header(constants.MSG_TYPE_INP, 162) .. pcma
+	local s = build_iphone_tx(constants.DEV_TERM_ALL, pcma)
+	--loginfo("recv pcma: " .. #pcma .. " bytes")
 	self:send(s)
 end
 
@@ -197,9 +200,28 @@ function phone:send(s)
 	self.udp:sendto("225.0.0.22", 6001, s)
 end
 
+function phone:yield(cmd)
+	self.wait   = true
+
+	self.cookie = now()
+	local c, s_r, dt = coroutine.yield()
+	while c ~= cmd do
+		c, s_r, dt = coroutine.yield()
+		if dt == "timeout" then
+			break
+		end
+	end
+
+	print("phone yield cmd : " .. c .. " status: " .. s_r)
+
+	self.wait = false
+
+	return s_r
+end
+
 function phone:set_config(s)
 	local mtype = s:byte(26)
-	if mtype == constants.CONFIG_TYPE_FREQ then
+	if mtype == constants.CF_ID_IPHONE_FREQ then
 		if s:byte(27) ~= 5 then
 			logerr("packet length error")
 			return
@@ -209,36 +231,57 @@ function phone:set_config(s)
 		local f = string.unpack("H", s:sub(31))
 
 		local freq = m + f / 1000.0
-		local rs = phone:build_response(string.unpack('H', s:sub(3)), 11)
-		rs = rs .. '\x01\x00' .. string.pack('B', mtype) .. '\x05' .. string.pack('BHH', id, m, f)
+
+		self:set_channel(id)
+
+		local s_r = self:yield(0x01)
+		if s_r == 0 then 
+			if freq < 400.0 then
+				self:to_xhf("vhf")
+			else
+				self:to_xhf("uhf")
+			end
+			self:set_freq(id, freq)
+			s_r = self:yield(0x0D)
+		end
+
+		local rs = string.pack('BHH', id, m, f)
+		local tid = parse_src_dev(s)
+		rs = build_config_res(tid, s_r, 1, constants.CF_ID_IPHONE_FREQ, #rs, rs)
 		loginfo(string.format("set channel [%d] freq to %f", id, freq))
 		self:send(rs)
-		self:set_freq(id, freq)
-	elseif mtype ==  constants.CONFIG_TYPE_CHANNEL then
+
+	elseif mtype ==  constants.CF_ID_IPHONE_CHANNEL then
 		if s:byte(27) ~= 1 then
 			logerr("packet length error")
 			return
 		end
 		local id = s:byte(28)
-		local rs = phone:build_response(string.unpack('H', s:sub(3)), 7)
-		rs = rs .. '\x01'
-		if options.freqs[id] ~= nil then
-			rs = rs .. '\x00'
+
+		local f = options.freqs[id]
+		if f < 400.0 then
+			self:to_xhf("vhf")
 		else
-			rs = rs .. '\x01'
+			self:to_xhf("uhf")
 		end
-		rs = rs .. string.pack('BBB', mtype, 1, id)
-		self:send(rs)
+
 		self:set_channel(id)
+
+		local s_r = self:yield(0x01)
+
+		local tid = parse_src_dev(s)
+
+		local rs = build_config_res(tid, s_r, 1,  constants.CF_ID_IPHONE_CHANNEL, 1, string.pack('B', id))
+		self:send(rs)
 	end
 	save_cfg()
 end
 
 function phone:get_config(s)
 	local mtype = s:byte(26)
-	if mtype == constants.CONFIG_TYPE_FREQ then
+	if mtype == constants.CF_ID_IPHONE_FREQ then
 		print("get freq: ")
-	elseif mtype == constants.CONFIG_TYPE_CHANNEL then
+	elseif mtype ==  constants.CF_ID_IPHONE_CHANNEL then
 		local rs = phone:build_response(string.unpack('H', s:sub(3)), 7)
 		rs = rs .. '\x00\x00' -- query success
 		rs = rs .. string.pack('BBB', mtype, 1, options.channel)
@@ -251,11 +294,11 @@ end
 function phone:on_udp(s)
 	--loginfo("recv: " .. #s .. " bytes")
 	--logdump(s)
-	if #s < 25 then -- too short, not for me.
+	if #s < constants.MSG_MINLEN then -- too short
 		return
 	end
-	local mtype = string.unpack('H', s:sub(21))
-	if mtype == constants.MSG_TYPE_INP then
+	local mtype = parse_msg_id(s)
+	if mtype == constants.MSG_ID_IPHONE then
 		local alen = string.unpack('H', s:sub(19)) - 2
 		local tlen = alen + 24
 		if tlen > #s then
@@ -263,7 +306,7 @@ function phone:on_udp(s)
 			return
 		end
 		self.pcm:put(s:sub(25))
-	elseif mtype == constants.MSG_TYPE_QCF then
+	elseif mtype == constants.MSG_ID_CFREQ then
 		if #s < 27 then
 			return
 		end
@@ -276,32 +319,36 @@ function phone:on_udp(s)
 	end
 end
 
-function phone:on_timeout()
-	local hdr = phone:build_header(constants.MSG_TYPE_INP, 0x30)
-	hdr = hdr .. '\x00\x00' .. string.pack('B', options.channel)
-	local m, f = math.modf(options.freqs[options.channel])
-	f = math.tointeger(f * 1000)
-	hdr = hdr .. string.pack('HHB', m, f, self.state)
-
-	for _, v in pairs(options.freqs) do
-		m, f = math.modf(v)
-		f = math.tointeger(f * 1000)
-		hdr = hdr .. string.pack('HH', m, f)
+function phone:resume()
+	if self.wait then
+		local cur = now()
+		local diff = cur - self.cookie
+
+		if diff >= 2000.0 then
+			if self.sock_thread ~= nil then
+				self.sock_thread(0, 1, "timeout")
+			end
+		end
 	end
-	self:send(hdr)
---	loginfo("len: " .. #hdr)
---	logdump(hdr)
+end
+
+function phone:on_timeout()
+	local ch = options.channel
+	local f = options.freqs[ch]
+	local s = build_iphone_status(ch, f, self.state, options.freqs)
+	self:send(s)
+	self:resume()
 end
 
 function phone:pcm_start()
 	mcu.ptt(true)
-	self.state = constants.STATE_RX
+	self.state = STATE_RX
 	loginfo("ptt on")
 end
 
 function phone:pcm_stop()
 	mcu.ptt(false)
-	self.state = constants.STATE_TX
+	self.state = STATE_TX
 	print("stop")
 end
 

tests/thread.lua

@@ -0,0 +1,16 @@
+function loop(n)
+	while true do
+		print("n = ", n)
+		n = coroutine.yield()
+	end
+end
+
+local f = coroutine.wrap(loop)
+
+f(1)
+f(2)
+f(3)
+f(4)
+f(5)
+f(6)
+