func_code, domain);
}
+/*
+ * Simple asn1 int writer/encoder helper function
+ * Returns number of bytes processed or < 0 on failure
+ * Only accepts int length values of 1, 2 or 4.
+ */
+static inline int asn1_int_encode(u8 *buf, size_t intlen, u32 u)
+{
+ switch (intlen) {
+ case 1:
+ *buf = (u8)u;
+ return 1;
+ case 2:
+ put_unaligned_be16((u16)u, buf);
+ return 2;
+ case 4:
+ put_unaligned_be32((u32)u, buf);
+ return 4;
+ default:
+ return -EINVAL;
+ }
+}
+
/*
* The request distributor calls this function if it picked the CEX4P
* device to handle a send_ep11_cprb request.
struct ap_message *ap_msg)
{
int rc;
- unsigned int lfmt;
struct ap_response_type *resp_type = &ap_msg->response;
struct {
struct type6_hdr hdr;
struct ep11_cprb cprbx;
- unsigned char pld_tag; /* fixed value 0x30 */
- unsigned char pld_lenfmt; /* payload length format */
} __packed * msg = ap_msg->msg;
- struct pld_hdr {
- unsigned char func_tag; /* fixed value 0x4 */
- unsigned char func_len; /* fixed value 0x4 */
- unsigned int func_val; /* function ID */
- unsigned char dom_tag; /* fixed value 0x4 */
- unsigned char dom_len; /* fixed value 0x4 */
- unsigned int dom_val; /* domain id */
- } __packed * payload_hdr = NULL;
/*
* The target domain field within the cprb body/payload block will be
* replaced by the usage domain for non-management commands only.
* Therefore we check the first bit of the 'flags' parameter for
* management command indication.
- * 0 - non management command
- * 1 - management command
*/
- if (!((msg->cprbx.flags & 0x80) == 0x80)) {
- msg->cprbx.target_id = (unsigned int)
- AP_QID_QUEUE(zq->queue->qid);
-
- if ((msg->pld_lenfmt & 0x80) == 0x80) { /*ext.len.fmt 2 or 3*/
- switch (msg->pld_lenfmt & 0x03) {
- case 1:
- lfmt = 2;
- break;
- case 2:
- lfmt = 3;
- break;
- default:
+ if (!(msg->cprbx.flags & 0x80)) {
+ int i, offs = 0;
+ size_t pld_len;
+ u8 *pld;
+ u32 u;
+
+ /* update target field in ep11_cprb */
+ msg->cprbx.target_id = (u32)AP_QID_QUEUE(zq->queue->qid);
+
+ /* ptr and length to payload */
+ pld = ap_msg->msg +
+ sizeof(struct type6_hdr) + sizeof(struct ep11_cprb);
+ pld_len = msg->cprbx.payload_len;
+ if (pld_len < MIN_EP11_PAYLOAD_SIZE)
+ return -EINVAL;
+
+ /*
+ * Parse the asn1 payload, at least we have
+ * pld tag (1 octet)
+ * payload length (1-5 octets)
+ * function tag (1 octet)
+ * function length (1-5 octets)
+ * function value (1-4 octets)
+ * ----- optional fields -----
+ * domain tag (1 octet)
+ * domain length (1-5 octets)
+ * domain value (1-4 octets)
+ * ... maybe much more data ...
+ */
+
+ /* payload tag */
+ if (pld[offs++] != 0x30)
+ return -EINVAL;
+ /* payload length field */
+ i = asn1_length_decode(pld + offs, pld_len - offs, &u);
+ if (i < 0)
+ return -EINVAL;
+ offs += i;
+ if (offs >= pld_len || u > pld_len - offs)
+ return -EINVAL;
+ /* function tag */
+ if (pld[offs++] != 0x04)
+ return -EINVAL;
+ /* function length */
+ if (offs >= pld_len)
+ return -EINVAL;
+ i = asn1_length_decode(pld + offs, pld_len - offs, &u);
+ if (i < 0)
+ return -EINVAL;
+ offs += i;
+ if (u > pld_len - offs)
+ return -EINVAL;
+ /* skip over the function value */
+ offs += u;
+ /* is there some payload left which could hold a domain value ? */
+ if (offs < pld_len && pld_len - offs >= 3) {
+ /* domain tag */
+ if (pld[offs++] != 0x04)
return -EINVAL;
- }
- } else {
- lfmt = 1; /* length format #1 */
+ /* domain length */
+ i = asn1_length_decode(pld + offs, pld_len - offs, &u);
+ if (i < 0)
+ return -EINVAL;
+ offs += i;
+ if (offs >= pld_len || u > pld_len - offs)
+ return -EINVAL;
+ /*
+ * pld[offs] is now at the start of the domain value
+ * with the value sprawled in u octets.
+ */
+ i = asn1_int_encode(pld + offs, u,
+ AP_QID_QUEUE(zq->queue->qid));
+ if (i < 0)
+ return -EINVAL;
+ offs += i;
}
- payload_hdr = (struct pld_hdr *)((&msg->pld_lenfmt) + lfmt);
- payload_hdr->dom_val = AP_QID_QUEUE(zq->queue->qid);
}
/*