static int cfusbl_receive(struct cflayer *layr, struct cfpkt *pkt)
{
u8 hpad;
/* Remove padding. */
cfpkt_extr_head(pkt, &hpad, 1);
cfpkt_extr_head(pkt, NULL, hpad);
return layr->up->receive(layr->up, pkt);
}
static int cfvei_transmit(struct cflayer *layr, struct cfpkt *pkt)
{
u8 tmp = 0;
struct caif_payload_info *info;
int ret;
struct cfsrvl *service = container_obj(layr);
if (!cfsrvl_ready(service, &ret))
return ret;
caif_assert(layr->dn != NULL);
caif_assert(layr->dn->transmit != NULL);
if (cfpkt_add_head(pkt, &tmp, 1) < 0) {
pr_err("CAIF: %s(): Packet is erroneous!\n", __func__);
return -EPROTO;
}
/* Add info-> for MUX-layer to route the packet out. */
info = cfpkt_info(pkt);
info->channel_id = service->layer.id;
info->hdr_len = 1;
info->dev_info = &service->dev_info;
ret = layr->dn->transmit(layr->dn, pkt);
if (ret < 0)
cfpkt_extr_head(pkt, &tmp, 1);
return ret;
}
static int cfvei_receive(struct cflayer *layr, struct cfpkt *pkt)
{
u8 cmd;
int ret;
caif_assert(layr->up != NULL);
caif_assert(layr->receive != NULL);
caif_assert(layr->ctrlcmd != NULL);
if (cfpkt_extr_head(pkt, &cmd, 1) < 0) {
pr_err("Packet is erroneous!\n");
cfpkt_destroy(pkt);
return -EPROTO;
}
switch (cmd) {
case VEI_PAYLOAD:
ret = layr->up->receive(layr->up, pkt);
return ret;
case VEI_FLOW_OFF:
layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_OFF_IND, 0);
cfpkt_destroy(pkt);
return 0;
case VEI_FLOW_ON:
layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_ON_IND, 0);
cfpkt_destroy(pkt);
return 0;
case VEI_SET_PIN: /* SET RS232 PIN */
cfpkt_destroy(pkt);
return 0;
default: /* SET RS232 PIN */
pr_warn("Unknown VEI control packet %d (0x%x)!\n", cmd, cmd);
cfpkt_destroy(pkt);
return -EPROTO;
}
}
static int cfdgml_transmit(struct cflayer *layr, struct cfpkt *pkt)
{
u32 zero = 0;
struct caif_payload_info *info;
struct cfsrvl *service = container_obj(layr);
int ret;
if (!cfsrvl_ready(service, &ret))
return ret;
cfpkt_add_head(pkt, &zero, 4);
/* Add info for MUX-layer to route the packet out. */
info = cfpkt_info(pkt);
info->channel_id = service->layer.id;
/* To optimize alignment, we add up the size of CAIF header
* before payload.
*/
info->hdr_len = 4;
info->dev_info = &service->dev_info;
ret = layr->dn->transmit(layr->dn, pkt);
if (ret < 0) {
u32 tmp32;
cfpkt_extr_head(pkt, &tmp32, 4);
}
return ret;
}
static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt)
{
u8 tmp = 0;
int ret;
struct cfsrvl *service = container_obj(layr);
caif_assert(layr->dn != NULL);
caif_assert(layr->dn->transmit != NULL);
if (!cfsrvl_ready(service, &ret))
return ret;
if (!cfpkt_getlen(pkt) > CAIF_MAX_PAYLOAD_SIZE) {
pr_err("CAIF: %s():Packet too large - size=%d\n",
__func__, cfpkt_getlen(pkt));
return -EOVERFLOW;
}
if (cfpkt_add_head(pkt, &tmp, 1) < 0) {
pr_err("CAIF: %s(): Packet is erroneous!\n", __func__);
return -EPROTO;
}
/* Add info for MUX-layer to route the packet out. */
cfpkt_info(pkt)->channel_id = service->layer.id;
/*
* To optimize alignment, we add up the size of CAIF header before
* payload.
*/
cfpkt_info(pkt)->hdr_len = 1;
cfpkt_info(pkt)->dev_info = &service->dev_info;
ret = layr->dn->transmit(layr->dn, pkt);
if (ret < 0)
cfpkt_extr_head(pkt, &tmp, 1);
return ret;
}
static int cfvidl_receive(struct cflayer *layr, struct cfpkt *pkt)
{
u32 videoheader;
if (cfpkt_extr_head(pkt, &videoheader, 4) < 0) {
pr_err("Packet is erroneous!\n");
cfpkt_destroy(pkt);
return -EPROTO;
}
return layr->up->receive(layr->up, pkt);
}
int cfpkt_peek_head(struct cfpkt *pkt, void *data, u16 len)
{
struct sk_buff *skb = pkt_to_skb(pkt);
if (skb_headlen(skb) >= len) {
memcpy(data, skb->data, len);
return 0;
}
return !cfpkt_extr_head(pkt, data, len) &&
!cfpkt_add_head(pkt, data, len);
}
static int cfdgml_receive(struct cflayer *layr, struct cfpkt *pkt)
{
u8 cmd = -1;
u8 dgmhdr[3];
int ret;
caif_assert(layr->up != NULL);
caif_assert(layr->receive != NULL);
caif_assert(layr->ctrlcmd != NULL);
if (cfpkt_extr_head(pkt, &cmd, 1) < 0) {
pr_err("CAIF: %s(): Packet is erroneous!\n", __func__);
cfpkt_destroy(pkt);
return -EPROTO;
}
if ((cmd & DGM_CMD_BIT) == 0) {
if (cfpkt_extr_head(pkt, &dgmhdr, 3) < 0) {
pr_err("CAIF: %s(): Packet is erroneous!\n", __func__);
cfpkt_destroy(pkt);
return -EPROTO;
}
ret = layr->up->receive(layr->up, pkt);
return ret;
}
switch (cmd) {
case DGM_FLOW_OFF: /* FLOW OFF */
layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_OFF_IND, 0);
cfpkt_destroy(pkt);
return 0;
case DGM_FLOW_ON: /* FLOW ON */
layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_ON_IND, 0);
cfpkt_destroy(pkt);
return 0;
default:
cfpkt_destroy(pkt);
pr_info("CAIF: %s(): Unknown datagram control %d (0x%x)\n",
__func__, cmd, cmd);
return -EPROTO;
}
}
static int cfvidl_transmit(struct cflayer *layr, struct cfpkt *pkt)
{
struct cfsrvl *service = container_obj(layr);
struct caif_payload_info *info;
u32 videoheader = 0;
int ret;
if (!cfsrvl_ready(service, &ret))
return ret;
cfpkt_add_head(pkt, &videoheader, 4);
/* Add info for MUX-layer to route the packet out */
info = cfpkt_info(pkt);
info->channel_id = service->layer.id;
info->dev_info = &service->dev_info;
ret = layr->dn->transmit(layr->dn, pkt);
if (ret < 0)
cfpkt_extr_head(pkt, &videoheader, 4);
return ret;
}
static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt)
{
u8 tmp;
bool segmented;
int ret;
caif_assert(layr->up != NULL);
caif_assert(layr->receive != NULL);
/*
* RFM is taking care of segmentation and stripping of
* segmentation bit.
*/
if (cfpkt_extr_head(pkt, &tmp, 1) < 0) {
pr_err("CAIF: %s(): Packet is erroneous!\n", __func__);
cfpkt_destroy(pkt);
return -EPROTO;
}
segmented = tmp & RFM_SEGMENTATION_BIT;
caif_assert(!segmented);
ret = layr->up->receive(layr->up, pkt);
return ret;
}
static int cfutill_receive(struct cflayer *layr, struct cfpkt *pkt)
{
u8 cmd = -1;
struct cfsrvl *service = container_obj(layr);
caif_assert(layr != NULL);
caif_assert(layr->up != NULL);
caif_assert(layr->up->receive != NULL);
caif_assert(layr->up->ctrlcmd != NULL);
if (cfpkt_extr_head(pkt, &cmd, 1) < 0) {
pr_err("Packet is erroneous!\n");
cfpkt_destroy(pkt);
return -EPROTO;
}
switch (cmd) {
case UTIL_PAYLOAD:
return layr->up->receive(layr->up, pkt);
case UTIL_FLOW_OFF:
layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_OFF_IND, 0);
cfpkt_destroy(pkt);
return 0;
case UTIL_FLOW_ON:
layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_ON_IND, 0);
cfpkt_destroy(pkt);
return 0;
case UTIL_REMOTE_SHUTDOWN: /* Remote Shutdown Request */
pr_err("REMOTE SHUTDOWN REQUEST RECEIVED\n");
layr->ctrlcmd(layr, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND, 0);
service->open = false;
cfpkt_destroy(pkt);
return 0;
default:
cfpkt_destroy(pkt);
pr_warn("Unknown service control %d (0x%x)\n", cmd, cmd);
return -EPROTO;
}
}
