diff --git a/include/patty/ax25/frame.h b/include/patty/ax25/frame.h index edc9025..f30c1a1 100644 --- a/include/patty/ax25/frame.h +++ b/include/patty/ax25/frame.h @@ -125,6 +125,10 @@ ssize_t patty_ax25_frame_encode(patty_ax25_frame *frame, void *buf, size_t len); +ssize_t patty_ax25_frame_encode_xid(patty_ax25_params *params, + void *buf, + size_t len); + ssize_t patty_ax25_frame_encode_reply_to(patty_ax25_frame *frame, patty_ax25_frame *reply, enum patty_ax25_frame_format format, diff --git a/src/frame.c b/src/frame.c index c197853..f49750b 100644 --- a/src/frame.c +++ b/src/frame.c @@ -518,6 +518,110 @@ error_toobig: return -1; } +static ssize_t encode_xid_param(void *data, + size_t offset, + enum patty_ax25_param_type type, + size_t bytes, + uint32_t value, + size_t len) { + uint8_t *buf = data; + + size_t start = offset, + i; + + if (offset + 2 + bytes > len || bytes > 4) { + goto error; + } + + buf[offset++] = (uint8_t)type; + buf[offset++] = (uint8_t)bytes; + + for (i=0; i> shift; + } + + return offset - start; + +error: + errno = EIO; + + return -1; +} + +static inline size_t needbytes(uint32_t value) { + if (!(value & 0xffffff00)) return 1; + if (!(value & 0xffff0000)) return 2; + if (!(value & 0xff000000)) return 3; + + return 4; +} + +ssize_t patty_ax25_frame_encode_xid(patty_ax25_params *params, + void *data, + size_t len) { + size_t offset = 0; + ssize_t encoded; + + struct { + uint8_t id; + size_t bytes; + uint32_t value; + } values[] = { + { PATTY_AX25_PARAM_CLASSES, 2, params->classes }, + { PATTY_AX25_PARAM_HDLC, 3, params->hdlc }, + { PATTY_AX25_PARAM_INFO_TX, 0, params->info_tx }, + { PATTY_AX25_PARAM_INFO_RX, 0, params->info_rx }, + { PATTY_AX25_PARAM_WINDOW_TX, 1, params->window_tx }, + { PATTY_AX25_PARAM_WINDOW_RX, 1, params->window_rx }, + { PATTY_AX25_PARAM_ACK, 0, params->ack }, + { PATTY_AX25_PARAM_RETRY, 0, params->retry }, + { 0, 0 } + }; + + int i; + + patty_ax25_frame_xid_group *group = (patty_ax25_frame_xid_group *) + ((char *)data + offset); + + memset(data, '\0', len); + + group->format = PATTY_AX25_FRAME_XID_GROUP_ISO8885; + group->type = PATTY_AX25_FRAME_XID_GROUP_PARAMS; + + offset += sizeof(*group); + + for (i=0; values[i].id; i++) { + size_t bytes = values[i].bytes? values[i].bytes: + needbytes(values[i].value); + + if (!(params->flags & (1 << values[i].id))) { + continue; + } + + if ((encoded = encode_xid_param(data, + offset, + values[i].id, + bytes, + values[i].value, + len)) < 0) { + goto error_encode_xid_param; + } else { + offset += encoded; + } + } + + group->len = htobe16(offset - sizeof(*group)); + + return offset; + +error_encode_xid_param: + errno = EOVERFLOW; + + return -1; +} + ssize_t patty_ax25_frame_encode_reply_to(patty_ax25_frame *frame, patty_ax25_frame *reply, enum patty_ax25_frame_format format,