int
wantsession(uint8_t *pkt)
{
int len;
uint8_t *s;
Pppoehdr *ph;
ph = (Pppoehdr*)(pkt+EtherHdrSz);
if(ph->code != CodeDiscSess)
return bad("not a session confirmation");
if(nhgets(ph->sessid) == 0x0000)
return bad("bad session id");
if(findtag(pkt, TagSrvName, &len, 0) == nil)
return bad("no service name");
if(findtag(pkt, TagSrvNameErr, &len, 0))
return bad("service name error");
if(findtag(pkt, TagAcSysErr, &len, 0))
return bad("ac system error");
/*
* rsc said: ``if there is no -S option given, the current code
* waits for an offer with service name == "".
* that's silly. it should take the first one it gets.''
*/
if(srvname[0] != '\0') {
if((s = findtag(pkt, TagSrvName, &len, 0)) == nil)
return bad("no matching service name");
if(len != strlen(srvname) || memcmp(s, srvname, len) != 0)
return bad("no matching service name");
}
sessid = nhgets(ph->sessid);
return 1;
}
/* execute a multicharacter command. */
void
exec_mca(void)
{
char *p, *fn, *patt;
*cp = '\0';
CMD_EXEC;
switch (mca) {
case A_F_SEARCH:
(void)search(1, cmdbuf, number, wsearch);
break;
case A_B_SEARCH:
(void)search(0, cmdbuf, number, wsearch);
break;
case A_EXAMINE:
for (p = cmdbuf; isspace(*p); ++p)
;
edit(glob(p));
break;
case A_TAGFILE:
for (p = cmdbuf; isspace(*p); ++p);
findtag(p, &fn, &patt);
if (fn == NULL)
break;
if (edit(fn))
(void)tagsearch(patt);
break;
}
}
/*
* Handler for -t option.
*/
void
opt_t(int type, char *s)
{
IFILE save_ifile;
off_t pos;
switch (type) {
case INIT:
tagoption = s;
/* Do the rest in main() */
break;
case TOGGLE:
if (secure) {
error("tags support is not available", NULL_PARG);
break;
}
findtag(skipsp(s));
save_ifile = save_curr_ifile();
/*
* Try to open the file containing the tag
* and search for the tag in that file.
*/
if (edit_tagfile() || (pos = tagsearch()) == -1) {
/* Failed: reopen the old file. */
reedit_ifile(save_ifile);
break;
}
unsave_ifile(save_ifile);
jump_loc(pos, jump_sline);
break;
}
}
int
SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler)
{
int ret;
int s[5];
int n = -1;
buf = findtag(buf, buflen, 'O');
if ((buf == nullptr) || (buflen < 12)) {
debug("output parser failed finding tag, ret: '%s'", buf);
return -1;
}
if ((ret = sscanf(buf, "O: %d %d %d %d %d %n",
&s[0], &s[1], &s[2], &s[3], &s[4], &n)) != 5) {
debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n);
return -1;
}
buf = skipline(buf, buflen);
if (buf == nullptr) {
debug("no line ending, line is incomplete");
return -1;
}
scaler.negative_scale = s[0] / 10000.0f;
scaler.positive_scale = s[1] / 10000.0f;
scaler.offset = s[2] / 10000.0f;
scaler.min_output = s[3] / 10000.0f;
scaler.max_output = s[4] / 10000.0f;
return 0;
}
TAG_SYMBOL *
defstruct (char *sname, int storage, int is_struct)
{
int itag ; /* index of tag in tag symbol table */
char nam[20]; /* Dummy name */
TAG_SYMBOL *tag = NULL;
if ( tagptr >= ENDTAG ) {
error(E_STROV) ;
}
if ( sname && sname[0] == 0 )
sname = NULL;
if ( sname && (tag = findtag(sname) ) ) {
if ( tag->weak == 0 ) {
if ( rcmatch('{') )
multidef();
else
return tag;
}
itag = tag - tagtab;
}
/* No tag defined for this, so leave it alone */
if ( tag == NULL ) {
tag = tagptr++;
itag = tag - tagtab ;
sprintf(nam,"0st%d",itag);
if ( sname == NULL )
sname = nam;
strcpy(tag->name,sname);
tag->size = 0;
tag->ptr = tag->end = membptr ; /* Set so no member searches done.. */
dummy_sym[NTYPE+1+itag] = addglb(nam,POINTER,STRUCT,0,STATIK,0,itag) ;
tag->weak = 1;
}
if ( rcmatch('{' ) ) {
/* increment tagptr to add tag to table */
tag->ptr = membptr;
tag->weak = 0;
needchar('{') ;
while ( dodeclare(storage, tag, is_struct) )
;
needchar('}') ;
tag->end = membptr ;
}
return tag ;
}
int
wantoffer(uint8_t *pkt)
{
int i, len;
uint8_t *s;
Etherhdr *eh;
Pppoehdr *ph;
eh = (Etherhdr*)pkt;
ph = (Pppoehdr*)(pkt+EtherHdrSz);
if(ph->code != CodeDiscOffer)
return bad("not an offer");
if(nhgets(ph->sessid) != 0x0000)
return bad("bad session id");
for(i=0;; i++){
if((s = findtag(pkt, TagSrvName, &len, i)) == nil)
return bad("no matching service name");
if(len == strlen(srvname) && memcmp(s, srvname, len) == 0)
break;
}
if((s = findtag(pkt, TagAcName, &len, 0)) == nil)
return bad("no ac name");
acname = copy(s, len);
if(wantac && strcmp(acname, wantac) != 0){
free(acname);
return bad("wrong ac name");
}
if(s = findtag(pkt, TagAcCookie, &len, 0)){
cookie = copy(s, len);
cookielen = len;
}
memmove(etherdst, eh->src, sizeof etherdst);
return 1;
}
int
SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group,
uint8_t &control_index)
{
unsigned u[2];
int s[5];
buf = findtag(buf, buflen, 'S');
if ((buf == nullptr) || (buflen < 16)) {
debug("control parser failed finding tag, ret: '%s'", buf);
return -1;
}
if (sscanf(buf, "S: %u %u %d %d %d %d %d",
&u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) {
debug("control parse failed on '%s'", buf);
return -1;
}
buf = skipline(buf, buflen);
if (buf == nullptr) {
debug("no line ending, line is incomplete");
return -1;
}
control_group = u[0];
control_index = u[1];
scaler.negative_scale = s[0] / 10000.0f;
scaler.positive_scale = s[1] / 10000.0f;
scaler.offset = s[2] / 10000.0f;
scaler.min_output = s[3] / 10000.0f;
scaler.max_output = s[4] / 10000.0f;
return 0;
}
HelicopterMixer *
HelicopterMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
{
mixer_heli_s mixer_info;
unsigned swash_plate_servo_count = 0;
unsigned u[5];
int s[5];
int used;
/* enforce that the mixer ends with space or a new line */
for (int i = buflen - 1; i >= 0; i--) {
if (buf[i] == '\0') {
continue;
}
/* require a space or newline at the end of the buffer, fail on printable chars */
if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
/* found a line ending or space, so no split symbols / numbers. good. */
break;
} else {
debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen - 1, buf[i]);
return nullptr;
}
}
if (sscanf(buf, "H: %u%n", &swash_plate_servo_count, &used) != 1) {
debug("helicopter parse failed on '%s'", buf);
return nullptr;
}
if (swash_plate_servo_count < 3 || swash_plate_servo_count > 4) {
debug("only supporting swash plate with 3 or 4 servos");
return nullptr;
}
if (used > (int)buflen) {
debug("OVERFLOW: helicopter spec used %d of %u", used, buflen);
return nullptr;
}
buf = skipline(buf, buflen);
if (buf == nullptr) {
debug("no line ending, line is incomplete");
return nullptr;
}
buf = findtag(buf, buflen, 'T');
if ((buf == nullptr) || (buflen < 12)) {
debug("control parser failed finding tag, ret: '%s'", buf);
return nullptr;
}
if (sscanf(buf, "T: %u %u %u %u %u",
&u[0], &u[1], &u[2], &u[3], &u[4]) != 5) {
debug("control parse failed on '%s'", buf);
return nullptr;
}
for (unsigned i = 0; i < HELI_CURVES_NR_POINTS; i++) {
mixer_info.throttle_curve[i] = ((float) u[i]) / 10000.0f;
}
buf = skipline(buf, buflen);
if (buf == nullptr) {
debug("no line ending, line is incomplete");
return nullptr;
}
buf = findtag(buf, buflen, 'P');
if ((buf == nullptr) || (buflen < 12)) {
debug("control parser failed finding tag, ret: '%s'", buf);
return nullptr;
}
if (sscanf(buf, "P: %d %d %d %d %d",
&s[0], &s[1], &s[2], &s[3], &s[4]) != 5) {
debug("control parse failed on '%s'", buf);
return nullptr;
}
for (unsigned i = 0; i < HELI_CURVES_NR_POINTS; i++) {
mixer_info.pitch_curve[i] = ((float) s[i]) / 10000.0f;
}
buf = skipline(buf, buflen);
if (buf == nullptr) {
debug("no line ending, line is incomplete");
return nullptr;
}
mixer_info.control_count = swash_plate_servo_count;
/* Now loop through the servos */
for (unsigned i = 0; i < mixer_info.control_count; i++) {
buf = findtag(buf, buflen, 'S');
if ((buf == nullptr) || (buflen < 12)) {
debug("control parser failed finding tag, ret: '%s'", buf);
return nullptr;
}
if (sscanf(buf, "S: %u %u %d %d %d %d",
&u[0],
&u[1],
&s[0],
&s[1],
&s[2],
&s[3]) != 6) {
debug("control parse failed on '%s'", buf);
return nullptr;
}
mixer_info.servos[i].angle = ((float) u[0]) * M_PI_F / 180.0f;
mixer_info.servos[i].arm_length = ((float) u[1]) / 10000.0f;
mixer_info.servos[i].scale = ((float) s[0]) / 10000.0f;
mixer_info.servos[i].offset = ((float) s[1]) / 10000.0f;
mixer_info.servos[i].min_output = ((float) s[2]) / 10000.0f;
mixer_info.servos[i].max_output = ((float) s[3]) / 10000.0f;
buf = skipline(buf, buflen);
if (buf == nullptr) {
debug("no line ending, line is incomplete");
return nullptr;
}
}
debug("remaining in buf: %d, first char: %c", buflen, buf[0]);
HelicopterMixer *hm = new HelicopterMixer(
control_cb,
cb_handle,
&mixer_info);
if (hm != nullptr) {
debug("loaded heli mixer with %d swash plate input(s)", mixer_info.control_count);
} else {
debug("could not allocate memory for mixer");
}
return hm;
}
