diff options
| -rw-r--r-- | include/net/caif/cfpkt.h | 27 | ||||
| -rw-r--r-- | net/caif/cfctrl.c | 50 |
2 files changed, 49 insertions, 28 deletions
diff --git a/include/net/caif/cfpkt.h b/include/net/caif/cfpkt.h index fe328c52c46b..801489bb14c3 100644 --- a/include/net/caif/cfpkt.h +++ b/include/net/caif/cfpkt.h @@ -32,6 +32,33 @@ void cfpkt_destroy(struct cfpkt *pkt); */ int cfpkt_extr_head(struct cfpkt *pkt, void *data, u16 len); +static inline u8 cfpkt_extr_head_u8(struct cfpkt *pkt) +{ + u8 tmp; + + cfpkt_extr_head(pkt, &tmp, 1); + + return tmp; +} + +static inline u16 cfpkt_extr_head_u16(struct cfpkt *pkt) +{ + __le16 tmp; + + cfpkt_extr_head(pkt, &tmp, 2); + + return le16_to_cpu(tmp); +} + +static inline u32 cfpkt_extr_head_u32(struct cfpkt *pkt) +{ + __le32 tmp; + + cfpkt_extr_head(pkt, &tmp, 4); + + return le32_to_cpu(tmp); +} + /* * Peek header from packet. * Reads data from packet without changing packet. diff --git a/net/caif/cfctrl.c b/net/caif/cfctrl.c index 655ed7032150..a1e85f032108 100644 --- a/net/caif/cfctrl.c +++ b/net/caif/cfctrl.c @@ -352,15 +352,14 @@ static int cfctrl_recv(struct cflayer *layer, struct cfpkt *pkt) u8 cmdrsp; u8 cmd; int ret = -1; - u16 tmp16; u8 len; u8 param[255]; - u8 linkid; + u8 linkid = 0; struct cfctrl *cfctrl = container_obj(layer); struct cfctrl_request_info rsp, *req; - cfpkt_extr_head(pkt, &cmdrsp, 1); + cmdrsp = cfpkt_extr_head_u8(pkt); cmd = cmdrsp & CFCTRL_CMD_MASK; if (cmd != CFCTRL_CMD_LINK_ERR && CFCTRL_RSP_BIT != (CFCTRL_RSP_BIT & cmdrsp) @@ -378,13 +377,12 @@ static int cfctrl_recv(struct cflayer *layer, struct cfpkt *pkt) u8 physlinkid; u8 prio; u8 tmp; - u32 tmp32; u8 *cp; int i; struct cfctrl_link_param linkparam; memset(&linkparam, 0, sizeof(linkparam)); - cfpkt_extr_head(pkt, &tmp, 1); + tmp = cfpkt_extr_head_u8(pkt); serv = tmp & CFCTRL_SRV_MASK; linkparam.linktype = serv; @@ -392,13 +390,13 @@ static int cfctrl_recv(struct cflayer *layer, struct cfpkt *pkt) servtype = tmp >> 4; linkparam.chtype = servtype; - cfpkt_extr_head(pkt, &tmp, 1); + tmp = cfpkt_extr_head_u8(pkt); physlinkid = tmp & 0x07; prio = tmp >> 3; linkparam.priority = prio; linkparam.phyid = physlinkid; - cfpkt_extr_head(pkt, &endpoint, 1); + endpoint = cfpkt_extr_head_u8(pkt); linkparam.endpoint = endpoint & 0x03; switch (serv) { @@ -407,45 +405,43 @@ static int cfctrl_recv(struct cflayer *layer, struct cfpkt *pkt) if (CFCTRL_ERR_BIT & cmdrsp) break; /* Link ID */ - cfpkt_extr_head(pkt, &linkid, 1); + linkid = cfpkt_extr_head_u8(pkt); break; case CFCTRL_SRV_VIDEO: - cfpkt_extr_head(pkt, &tmp, 1); + tmp = cfpkt_extr_head_u8(pkt); linkparam.u.video.connid = tmp; if (CFCTRL_ERR_BIT & cmdrsp) break; /* Link ID */ - cfpkt_extr_head(pkt, &linkid, 1); + linkid = cfpkt_extr_head_u8(pkt); break; case CFCTRL_SRV_DATAGRAM: - cfpkt_extr_head(pkt, &tmp32, 4); linkparam.u.datagram.connid = - le32_to_cpu(tmp32); + cfpkt_extr_head_u32(pkt); if (CFCTRL_ERR_BIT & cmdrsp) break; /* Link ID */ - cfpkt_extr_head(pkt, &linkid, 1); + linkid = cfpkt_extr_head_u8(pkt); break; case CFCTRL_SRV_RFM: /* Construct a frame, convert * DatagramConnectionID * to network format long and copy it out... */ - cfpkt_extr_head(pkt, &tmp32, 4); linkparam.u.rfm.connid = - le32_to_cpu(tmp32); + cfpkt_extr_head_u32(pkt); cp = (u8 *) linkparam.u.rfm.volume; - for (cfpkt_extr_head(pkt, &tmp, 1); + for (tmp = cfpkt_extr_head_u8(pkt); cfpkt_more(pkt) && tmp != '\0'; - cfpkt_extr_head(pkt, &tmp, 1)) + tmp = cfpkt_extr_head_u8(pkt)) *cp++ = tmp; *cp = '\0'; if (CFCTRL_ERR_BIT & cmdrsp) break; /* Link ID */ - cfpkt_extr_head(pkt, &linkid, 1); + linkid = cfpkt_extr_head_u8(pkt); break; case CFCTRL_SRV_UTIL: @@ -454,13 +450,11 @@ static int cfctrl_recv(struct cflayer *layer, struct cfpkt *pkt) * to network format long and copy it out... */ /* Fifosize KB */ - cfpkt_extr_head(pkt, &tmp16, 2); linkparam.u.utility.fifosize_kb = - le16_to_cpu(tmp16); + cfpkt_extr_head_u16(pkt); /* Fifosize bufs */ - cfpkt_extr_head(pkt, &tmp16, 2); linkparam.u.utility.fifosize_bufs = - le16_to_cpu(tmp16); + cfpkt_extr_head_u16(pkt); /* name */ cp = (u8 *) linkparam.u.utility.name; caif_assert(sizeof(linkparam.u.utility.name) @@ -468,24 +462,24 @@ static int cfctrl_recv(struct cflayer *layer, struct cfpkt *pkt) for (i = 0; i < UTILITY_NAME_LENGTH && cfpkt_more(pkt); i++) { - cfpkt_extr_head(pkt, &tmp, 1); + tmp = cfpkt_extr_head_u8(pkt); *cp++ = tmp; } /* Length */ - cfpkt_extr_head(pkt, &len, 1); + len = cfpkt_extr_head_u8(pkt); linkparam.u.utility.paramlen = len; /* Param Data */ cp = linkparam.u.utility.params; while (cfpkt_more(pkt) && len--) { - cfpkt_extr_head(pkt, &tmp, 1); + tmp = cfpkt_extr_head_u8(pkt); *cp++ = tmp; } if (CFCTRL_ERR_BIT & cmdrsp) break; /* Link ID */ - cfpkt_extr_head(pkt, &linkid, 1); + linkid = cfpkt_extr_head_u8(pkt); /* Length */ - cfpkt_extr_head(pkt, &len, 1); + len = cfpkt_extr_head_u8(pkt); /* Param Data */ cfpkt_extr_head(pkt, ¶m, len); break; @@ -522,7 +516,7 @@ static int cfctrl_recv(struct cflayer *layer, struct cfpkt *pkt) } break; case CFCTRL_CMD_LINK_DESTROY: - cfpkt_extr_head(pkt, &linkid, 1); + linkid = cfpkt_extr_head_u8(pkt); cfctrl->res.linkdestroy_rsp(cfctrl->serv.layer.up, linkid); break; case CFCTRL_CMD_LINK_ERR: |