cfveil.c 2.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. /*
  3. * Copyright (C) ST-Ericsson AB 2010
  4. * Author: Sjur Brendeland
  5. */
  6. #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
  7. #include <linux/stddef.h>
  8. #include <linux/slab.h>
  9. #include <net/caif/caif_layer.h>
  10. #include <net/caif/cfsrvl.h>
  11. #include <net/caif/cfpkt.h>
  12. #define VEI_PAYLOAD 0x00
  13. #define VEI_CMD_BIT 0x80
  14. #define VEI_FLOW_OFF 0x81
  15. #define VEI_FLOW_ON 0x80
  16. #define VEI_SET_PIN 0x82
  17. #define container_obj(layr) container_of(layr, struct cfsrvl, layer)
  18. static int cfvei_receive(struct cflayer *layr, struct cfpkt *pkt);
  19. static int cfvei_transmit(struct cflayer *layr, struct cfpkt *pkt);
  20. struct cflayer *cfvei_create(u8 channel_id, struct dev_info *dev_info)
  21. {
  22. struct cfsrvl *vei = kzalloc(sizeof(struct cfsrvl), GFP_ATOMIC);
  23. if (!vei)
  24. return NULL;
  25. caif_assert(offsetof(struct cfsrvl, layer) == 0);
  26. cfsrvl_init(vei, channel_id, dev_info, true);
  27. vei->layer.receive = cfvei_receive;
  28. vei->layer.transmit = cfvei_transmit;
  29. snprintf(vei->layer.name, CAIF_LAYER_NAME_SZ, "vei%d", channel_id);
  30. return &vei->layer;
  31. }
  32. static int cfvei_receive(struct cflayer *layr, struct cfpkt *pkt)
  33. {
  34. u8 cmd;
  35. int ret;
  36. caif_assert(layr->up != NULL);
  37. caif_assert(layr->receive != NULL);
  38. caif_assert(layr->ctrlcmd != NULL);
  39. if (cfpkt_extr_head(pkt, &cmd, 1) < 0) {
  40. pr_err("Packet is erroneous!\n");
  41. cfpkt_destroy(pkt);
  42. return -EPROTO;
  43. }
  44. switch (cmd) {
  45. case VEI_PAYLOAD:
  46. ret = layr->up->receive(layr->up, pkt);
  47. return ret;
  48. case VEI_FLOW_OFF:
  49. layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_OFF_IND, 0);
  50. cfpkt_destroy(pkt);
  51. return 0;
  52. case VEI_FLOW_ON:
  53. layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_ON_IND, 0);
  54. cfpkt_destroy(pkt);
  55. return 0;
  56. case VEI_SET_PIN: /* SET RS232 PIN */
  57. cfpkt_destroy(pkt);
  58. return 0;
  59. default: /* SET RS232 PIN */
  60. pr_warn("Unknown VEI control packet %d (0x%x)!\n", cmd, cmd);
  61. cfpkt_destroy(pkt);
  62. return -EPROTO;
  63. }
  64. }
  65. static int cfvei_transmit(struct cflayer *layr, struct cfpkt *pkt)
  66. {
  67. u8 tmp = 0;
  68. struct caif_payload_info *info;
  69. int ret;
  70. struct cfsrvl *service = container_obj(layr);
  71. if (!cfsrvl_ready(service, &ret))
  72. goto err;
  73. caif_assert(layr->dn != NULL);
  74. caif_assert(layr->dn->transmit != NULL);
  75. if (cfpkt_add_head(pkt, &tmp, 1) < 0) {
  76. pr_err("Packet is erroneous!\n");
  77. ret = -EPROTO;
  78. goto err;
  79. }
  80. /* Add info-> for MUX-layer to route the packet out. */
  81. info = cfpkt_info(pkt);
  82. info->channel_id = service->layer.id;
  83. info->hdr_len = 1;
  84. info->dev_info = &service->dev_info;
  85. return layr->dn->transmit(layr->dn, pkt);
  86. err:
  87. cfpkt_destroy(pkt);
  88. return ret;
  89. }