FrelonSerialLine.cpp 11 KB
Newer Older
1
2
#include "FrelonSerialLine.h"
#include "RegEx.h"
ahoms's avatar
ahoms committed
3
#include "MiscUtils.h"
4
#include <sstream>
5

6
using namespace lima;
7
8
9
using namespace lima::Frelon;
using namespace std;

10
11
12
13
const double SerialLine::TimeoutSingle    = 0.5;
const double SerialLine::TimeoutNormal    = 2.0;
const double SerialLine::TimeoutMultiLine = 3.0;
const double SerialLine::TimeoutReset     = 5.0;
14
15
16
17
18


SerialLine::SerialLine(Espia::SerialLine& espia_ser_line)
	: m_espia_ser_line(espia_ser_line)
{
19
20
21
22
23
	DEB_CONSTRUCTOR();
	ostringstream os;
	os << "Serial#" << espia_ser_line.getDev().getDevNb();
	DEB_SET_OBJ_NAME(os.str());

24
25
	m_espia_ser_line.setLineTerm("\r\n");
	m_espia_ser_line.setTimeout(TimeoutNormal);
26
27
28

	m_last_warn = 0;

29
30
31
32
	m_curr_op = None;
	m_curr_cache = false;
	m_cache_act = true;

33
	flush();
34
35
}

36
37
38
39
40
SerialLine::~SerialLine()
{
	DEB_DESTRUCTOR();
}

41
42
43
44
45
Espia::SerialLine& SerialLine::getEspiaSerialLine()
{
	return m_espia_ser_line;
}

46
void SerialLine::write(const string& buffer, bool no_wait)
47
{
48
49
	DEB_MEMBER_FUNCT();

50
51
	AutoMutex l = lock(AutoMutex::Locked);

52
53
	while (m_curr_op != None) {
		DEB_TRACE() << "Waiting end of current " << m_curr_op;
54
		m_cond.wait();
55
	}
56
57
58
59
60
61
62

	writeCmd(buffer, no_wait);

	l.leaveLocked();
}

void SerialLine::writeCmd(const string& buffer, bool no_wait)
63
{
64
	DEB_MEMBER_FUNCT();
65
	DEB_PARAM() << DEB_VAR1(no_wait);
66

ahoms's avatar
ahoms committed
67
	MsgPartStrMapType msg_parts;
68
69
70
	splitMsg(buffer, msg_parts);
	const string& cmd = msg_parts[MsgCmd];

71
72
	if (cmd == CmdStrMap[Reset]) {
		m_curr_op = DoReset;
73
		DEB_TRACE() << "DoReset: clearing reg cache";
74
75
		m_reg_cache.clear();
	} else {
ahoms's avatar
ahoms committed
76
		MultiLineCmdStrMapType::const_iterator it, end;
77
		end = MultiLineCmdStrMap.end();
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
		it = FindMapValue(MultiLineCmdStrMap, cmd);
		if (it != end)
			m_curr_op = MultiRead;
	}

	bool reg_found = false;
	if (m_curr_op == None) {
		RegStrMapType::const_iterator it, end = RegStrMap.end();
		it = FindMapValue(RegStrMap, cmd);
		reg_found = (it != end);
		if (reg_found)
			m_curr_reg = it->first;
	}

	m_curr_cache = false;
93
	if (reg_found && isRegCacheable(m_curr_reg)) {
94
95
		bool is_req = !msg_parts[MsgReq].empty();
		m_curr_op = is_req ? ReadReg : WriteReg;
96
97
98
99
100
101
102
103
104
105
106
107
		int cache_val;
		m_curr_cache = getRegCacheVal(m_curr_reg, cache_val);
		if (m_curr_cache) {
			ostringstream os;
			os << cache_val;
			const string& cache_str = os.str();
			if (is_req) {
				m_curr_resp = cache_str;
			} else {
				m_curr_resp = msg_parts[MsgVal];
				m_curr_cache = (m_curr_resp == cache_str);
			}
108
		}
109
110
111
		if (m_curr_cache) {
			DEB_TRACE() << "Skipping " << m_curr_op 
				    << ", already in cache";
112
			return;
113
		}
114
115
	}

116
117
118
	if (m_curr_op == None)
		m_curr_op = DoCmd;

119
	DEB_TRACE() << DEB_VAR1(m_curr_op);
120

121
122
	string sync = msg_parts[MsgSync].empty() ? ">" : "";
	string term = msg_parts[MsgTerm].empty() ? "\r\n" : "";
123
124
125
126
127
128
129
	string msg = sync + buffer + term;

	m_espia_ser_line.write(msg, no_wait);
}

void SerialLine::read(string& buffer, int max_len, double timeout)
{
130
	DEB_MEMBER_FUNCT();
131
	m_espia_ser_line.read(buffer, max_len, timeout);
132
133
}

134
135
void SerialLine::readStr(string& buffer, int max_len, 
			 const string& term, double timeout)
136
{
137
	DEB_MEMBER_FUNCT();
138
	m_espia_ser_line.readStr(buffer, max_len, term, timeout);
139
140
}

141
void SerialLine::readLine(string& buffer, int max_len, double timeout)
142
{
143
	DEB_MEMBER_FUNCT();
144
	AutoMutex l = lock(AutoMutex::PrevLocked);
145

146
147
148
149
150
	readResp(buffer, max_len, timeout);
}

void SerialLine::readResp(string& buffer, int max_len, double timeout)
{
151
	DEB_MEMBER_FUNCT();
152
	DEB_PARAM() << DEB_VAR3(max_len, timeout, m_curr_op);
153
154
155

	if (m_curr_op == None) {
		DEB_FATAL() << "readLine without previous write!";
156
		throw LIMA_HW_EXC(Error, "readLine without previous write");
157
	}
158
159

	if ((m_curr_op == MultiRead) && (timeout == TimeoutDefault))
160
161
162
		readMultiLine(buffer, max_len);
	else
		readSingleLine(buffer, max_len, timeout);
163
164
165

	m_curr_op = None;
	m_cond.signal();
166
167
168
169
}

void SerialLine::readSingleLine(string& buffer, int max_len, double timeout)
{
170
171
	DEB_MEMBER_FUNCT();

172
173
	bool is_req = (m_curr_op == ReadReg);
	if (m_curr_cache) {
174
		DEB_TRACE() << "Using cache value";
175
176
177
178
179
180
181
		ostringstream os;
		os << "!OK";
		if (is_req)
			os << ":" << m_curr_resp;
		os << "\r\n";
		buffer = os.str();
		m_curr_fmt_resp = m_curr_resp;
182
		DEB_TRACE() << DEB_VAR1(m_curr_fmt_resp);
183
		return;
184
	}
185
186
187

	if ((m_curr_op == DoReset) && (timeout == TimeoutDefault))
		timeout = TimeoutReset;
188
	m_espia_ser_line.readLine(buffer, max_len, timeout);
189
190
191
192

	decodeFmtResp(buffer, m_curr_fmt_resp);
	
	bool reg_op = ((m_curr_op == WriteReg) || (m_curr_op == ReadReg));
193
	if (!reg_op || !isRegCacheable(m_curr_reg))
194
195
		return;

196
197
198
199
200
	const string& cache_str = is_req ? m_curr_fmt_resp : m_curr_resp;
	int cache_val;
	istringstream is(cache_str);
	is >> cache_val;
	m_reg_cache[m_curr_reg] = cache_val;
201
	DEB_TRACE() << "New " << DEB_VAR1(cache_val);
202
203
204
205
}

void SerialLine::readMultiLine(string& buffer, int max_len)
{
206
	DEB_MEMBER_FUNCT();
207
	DEB_PARAM() << DEB_VAR1(max_len);
208

209
210
211
212
213
214
215
216
	Timestamp timeout = Timestamp::now() + Timestamp(TimeoutMultiLine);

	buffer.clear();

	while (Timestamp::now() < timeout) {
		string ans;
		int len = max_len - buffer.size();
		try {
217
			DEB_TRACE() << "Atempting to read: " << DEB_VAR1(len);
218
219
220
221
222
223
224
225
			m_espia_ser_line.readLine(ans, len, TimeoutSingle);
			buffer += ans;
		} catch (Exception e) {
			if (!buffer.empty())
				break;
		}
	}

226
227
	if (buffer.empty()) {
		DEB_ERROR() << "Timeout reading multi-line";
228
		throw LIMA_HW_EXC(Error, "Timeout reading Frelon multi line");
229
	}
230
231
}

232
233
bool SerialLine::isRegCacheable(Reg reg)
{
234
235
236
237
	DEB_MEMBER_FUNCT();

	if (!m_cache_act) {
		DEB_TRACE() << "Reg cache is disabled";
238
		return false;
239
	}
240

241
242
	const RegListType& list = CacheableRegList;
	bool cacheable = (find(list.begin(), list.end(), reg) != list.end());
243
	DEB_RETURN() << DEB_VAR1(cacheable);
244
	return cacheable;
245
246
}

247
248
249
250
251
252
253
254
255
256
bool SerialLine::getRegCacheVal(Reg reg, int& val)
{
	DEB_MEMBER_FUNCT();
	RegValMapType::const_iterator it = m_reg_cache.find(reg);
	bool in_cache = (it != m_reg_cache.end());
	val = in_cache ? it->second : 0;
	DEB_RETURN() << DEB_VAR2(in_cache, val);
	return in_cache;
}

257
258
void SerialLine::flush()
{
259
	DEB_MEMBER_FUNCT();
260
	m_espia_ser_line.flush();
261
262
}

263
void SerialLine::getNbAvailBytes(int &avail)
264
{
265
	DEB_MEMBER_FUNCT();
266
	m_espia_ser_line.getNbAvailBytes(avail);
267
	DEB_RETURN() << DEB_VAR1(avail);
268
269
270
271
}

void SerialLine::setTimeout(double timeout)
{
272
	DEB_MEMBER_FUNCT();
273
	DEB_PARAM() << DEB_VAR1(timeout);
274
275
	m_espia_ser_line.setTimeout(timeout);
}
276

277
278
void SerialLine::getTimeout(double& timeout) const
{
279
	DEB_MEMBER_FUNCT();
280
	m_espia_ser_line.getTimeout(timeout);
281
	DEB_RETURN() << DEB_VAR1(timeout);
282
283
}

284

285
void SerialLine::splitMsg(const string& msg, 
ahoms's avatar
ahoms committed
286
			  MsgPartStrMapType& msg_parts) const
287
{
288
	DEB_MEMBER_FUNCT();
289
	DEB_PARAM() << DEB_VAR1(msg);
290

291
292
	msg_parts.clear();

293
	const static RegEx re("^(?P<sync>>)?"
ahoms's avatar
ahoms committed
294
			      "(?P<cmd>[A-Za-z]+)"
295
296
			      "((?P<req>\\?)|(?P<val>[0-9]+))?"
			      "(?P<term>[\r\n]+)?$");
297
298
299
300
301
302
303
304
305
306
307

	RegEx::FullNameMatchType match;
	if (!re.matchName(msg, match))
		throw LIMA_HW_EXC(InvalidValue, "Invalid Frelon message");

	typedef pair<MsgPart, string> KeyPair;
	static const KeyPair key_list[] = {
		KeyPair(MsgSync, "sync"), KeyPair(MsgCmd, "cmd"), 
		KeyPair(MsgVal,  "val"),  KeyPair(MsgReq, "req"),  
		KeyPair(MsgTerm, "term"),
	};
ahoms's avatar
ahoms committed
308
	const KeyPair *it, *end = C_LIST_END(key_list);
309
310
311
312
313
	for (it = key_list; it != end; ++it) {
		const MsgPart& key = it->first;
		const string&  grp = it->second;
		msg_parts[key] = string(match[grp].start, match[grp].end);
	}
314

315
316
	DEB_RETURN() << DEB_VAR2(msg_parts[MsgSync], msg_parts[MsgCmd]);
	DEB_RETURN() << DEB_VAR2(msg_parts[MsgReq], msg_parts[MsgVal]);
317
318
}

319
320
void SerialLine::decodeFmtResp(const string& ans, string& fmt_resp)
{
321
	DEB_MEMBER_FUNCT();
322
	DEB_PARAM() << DEB_VAR1(ans);
323

324
325
326
327
328
329
330
	fmt_resp.clear();

	const static RegEx re("!(OK(:(?P<resp>[^\r]+))?|"
			        "W\a?:(?P<warn>[^\r]+)|"
			        "E\a?:(?P<err>[^\r]+))\r\n");

	RegEx::FullNameMatchType match;
331
332
333
334
	if (!re.matchName(ans, match)) {
		DEB_ERROR() << "Unexpected Frelon answer";
		throw LIMA_HW_EXC(Error, "Unexpected Frelon answer");
	}
335
336
337
338
339

	RegEx::SingleMatchType& err = match["err"];
	if (err) {
		string err_str(err.start, err.end);
		string err_desc = string("Frelon Error: ") + err_str;
340
		DEB_ERROR() << err_desc;
341
342
343
344
345
		throw LIMA_HW_EXC(Error, err_desc);
	}

	RegEx::SingleMatchType& warn = match["warn"];
	if (warn) {
346
347
		string warn_str(warn.start, warn.end);
		DEB_WARNING() << "Camera warning: " << warn_str;
348
349
350
351
352
353
354
		istringstream is(warn_str);
		is >> m_last_warn;
		return;
	}

	RegEx::SingleMatchType& resp = match["resp"];
	fmt_resp = string(resp.start, resp.end);
355
	if (!fmt_resp.empty())
356
		DEB_RETURN() << DEB_VAR1(fmt_resp);
357
358
359
360
}

void SerialLine::sendFmtCmd(const string& cmd, string& resp)
{
361
362
	DEB_MEMBER_FUNCT();

363
364
365
366
367
	AutoMutex l = lock(AutoMutex::Locked);

	m_curr_fmt_resp.clear();

	writeCmd(cmd);
368
	string ans;
369
370
371
	readResp(ans);

	resp = m_curr_fmt_resp;
372
373
374
375
}

int SerialLine::getLastWarning()
{
376
	DEB_MEMBER_FUNCT();
377
378
	int last_warn = m_last_warn;
	m_last_warn = 0;
379
	DEB_RETURN() << DEB_VAR1(last_warn);
380
381
	return last_warn;
}
382
383
384

void SerialLine::clearCache()
{
385
	DEB_MEMBER_FUNCT();
386
	AutoMutex l = lock(AutoMutex::Locked);
387
	DEB_TRACE() << "Clearing reg cache";
388
389
390
391
392
	m_reg_cache.clear();
}

void SerialLine::setCacheActive(bool cache_act)
{
393
	DEB_MEMBER_FUNCT();
394
	DEB_PARAM() << DEB_VAR2(cache_act, m_cache_act);
395
	AutoMutex l = lock(AutoMutex::Locked);
396
397
	if (cache_act && !m_cache_act) {
		DEB_TRACE() << "Clearing reg cache";
398
		m_reg_cache.clear();
399
	}
400
401
402
403
404
	m_cache_act = cache_act;
}

void SerialLine::getCacheActive(bool& cache_act)
{
405
	DEB_MEMBER_FUNCT();
406
407
	AutoMutex l = lock(AutoMutex::Locked);
	cache_act = m_cache_act;
408
	DEB_RETURN() << DEB_VAR1(cache_act);
409
}
410

411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
void SerialLine::writeRegister(Reg reg, int  val)
{
	DEB_MEMBER_FUNCT();
	const string& reg_str = RegStrMap[reg];
	DEB_PARAM() << DEB_VAR3(reg, reg_str, val);

	int cache_val;
	AutoMutex l = lock(AutoMutex::Locked);
	bool ok = (isRegCacheable(reg) && getRegCacheVal(reg, cache_val));
	l.unlock();
	
	if (ok && (cache_val == val)) {
		DEB_TRACE() << "Value already in cache";
	} else {
		if (reg_str.empty()) {
			DEB_ERROR() << "Invalid register reg=" << reg;
			throw LIMA_HW_EXC(InvalidValue, "Invalid register");
		}

		ostringstream cmd;
		cmd << reg_str << val;
		string resp;
		sendFmtCmd(cmd.str(), resp);
	}
}

void SerialLine::readRegister(Reg reg, int& val)
{
	DEB_MEMBER_FUNCT();
	const string& reg_str = RegStrMap[reg];
	DEB_PARAM() << DEB_VAR2(reg, reg_str);

	AutoMutex l = lock(AutoMutex::Locked);
	bool ok = (isRegCacheable(reg) && getRegCacheVal(reg, val));
	l.unlock();

	if (ok) {
		DEB_TRACE() << "Using cache value";
	} else {
		if (reg_str.empty()) {
			DEB_ERROR() << "Invalid register reg=" << reg;
			throw LIMA_HW_EXC(InvalidValue, "Invalid register");
		}
		
		string resp;
		sendFmtCmd(reg_str + "?", resp);
		istringstream is(resp);
		is >> val;
	}

	DEB_RETURN() << DEB_VAR1(val);
}

464
465
466
467
468
469
470
471
472
473
474
475
476
477
478

ostream& lima::Frelon::operator <<(ostream& os, SerialLine::RegOp op)
{
	const char *name = "Unknown";
	switch (op) {
	case SerialLine::None:      name = "None";      break;
	case SerialLine::DoCmd:     name = "DoCmd";     break;
	case SerialLine::ReadReg:   name = "ReadReg";   break;
	case SerialLine::WriteReg:  name = "WriteReg";  break;
	case SerialLine::DoReset:   name = "DoReset";   break;
	case SerialLine::MultiRead: name = "MultiRead"; break;
	}
	return os << name;
}