1
2
3
4#include "fm10k_tlv.h"
5
6
7
8
9
10
11
12
13s32 fm10k_tlv_msg_init(u32 *msg, u16 msg_id)
14{
15
16 if (!msg)
17 return FM10K_ERR_PARAM;
18
19 *msg = (FM10K_TLV_FLAGS_MSG << FM10K_TLV_FLAGS_SHIFT) | msg_id;
20
21 return 0;
22}
23
24
25
26
27
28
29
30
31
32
33
34static s32 fm10k_tlv_attr_put_null_string(u32 *msg, u16 attr_id,
35 const unsigned char *string)
36{
37 u32 attr_data = 0, len = 0;
38 u32 *attr;
39
40
41 if (!string || !msg)
42 return FM10K_ERR_PARAM;
43
44 attr = &msg[FM10K_TLV_DWORD_LEN(*msg)];
45
46
47 do {
48
49 if (len && !(len % 4)) {
50 attr[len / 4] = attr_data;
51 attr_data = 0;
52 }
53
54
55 attr_data |= (u32)(*string) << (8 * (len % 4));
56 len++;
57
58
59 } while (*(string++));
60
61
62 attr[(len + 3) / 4] = attr_data;
63
64
65 len <<= FM10K_TLV_LEN_SHIFT;
66 attr[0] = len | attr_id;
67
68
69 len += FM10K_TLV_HDR_LEN << FM10K_TLV_LEN_SHIFT;
70 *msg += FM10K_TLV_LEN_ALIGN(len);
71
72 return 0;
73}
74
75
76
77
78
79
80
81
82
83
84static s32 fm10k_tlv_attr_get_null_string(u32 *attr, unsigned char *string)
85{
86 u32 len;
87
88
89 if (!string || !attr)
90 return FM10K_ERR_PARAM;
91
92 len = *attr >> FM10K_TLV_LEN_SHIFT;
93 attr++;
94
95 while (len--)
96 string[len] = (u8)(attr[len / 4] >> (8 * (len % 4)));
97
98 return 0;
99}
100
101
102
103
104
105
106
107
108
109
110
111
112s32 fm10k_tlv_attr_put_mac_vlan(u32 *msg, u16 attr_id,
113 const u8 *mac_addr, u16 vlan)
114{
115 u32 len = ETH_ALEN << FM10K_TLV_LEN_SHIFT;
116 u32 *attr;
117
118
119 if (!msg || !mac_addr)
120 return FM10K_ERR_PARAM;
121
122 attr = &msg[FM10K_TLV_DWORD_LEN(*msg)];
123
124
125 attr[0] = len | attr_id;
126
127
128 attr[1] = le32_to_cpu(*(const __le32 *)&mac_addr[0]);
129 attr[2] = le16_to_cpu(*(const __le16 *)&mac_addr[4]);
130 attr[2] |= (u32)vlan << 16;
131
132
133 len += FM10K_TLV_HDR_LEN << FM10K_TLV_LEN_SHIFT;
134 *msg += FM10K_TLV_LEN_ALIGN(len);
135
136 return 0;
137}
138
139
140
141
142
143
144
145
146
147
148
149s32 fm10k_tlv_attr_get_mac_vlan(u32 *attr, u8 *mac_addr, u16 *vlan)
150{
151
152 if (!mac_addr || !attr)
153 return FM10K_ERR_PARAM;
154
155 *(__le32 *)&mac_addr[0] = cpu_to_le32(attr[1]);
156 *(__le16 *)&mac_addr[4] = cpu_to_le16((u16)(attr[2]));
157 *vlan = (u16)(attr[2] >> 16);
158
159 return 0;
160}
161
162
163
164
165
166
167
168
169
170
171
172s32 fm10k_tlv_attr_put_bool(u32 *msg, u16 attr_id)
173{
174
175 if (!msg)
176 return FM10K_ERR_PARAM;
177
178
179 msg[FM10K_TLV_DWORD_LEN(*msg)] = attr_id;
180
181
182 *msg += FM10K_TLV_HDR_LEN << FM10K_TLV_LEN_SHIFT;
183
184 return 0;
185}
186
187
188
189
190
191
192
193
194
195
196
197
198s32 fm10k_tlv_attr_put_value(u32 *msg, u16 attr_id, s64 value, u32 len)
199{
200 u32 *attr;
201
202
203 if (!msg || !len || len > 8 || (len & (len - 1)))
204 return FM10K_ERR_PARAM;
205
206 attr = &msg[FM10K_TLV_DWORD_LEN(*msg)];
207
208 if (len < 4) {
209 attr[1] = (u32)value & (BIT(8 * len) - 1);
210 } else {
211 attr[1] = (u32)value;
212 if (len > 4)
213 attr[2] = (u32)(value >> 32);
214 }
215
216
217 len <<= FM10K_TLV_LEN_SHIFT;
218 attr[0] = len | attr_id;
219
220
221 len += FM10K_TLV_HDR_LEN << FM10K_TLV_LEN_SHIFT;
222 *msg += FM10K_TLV_LEN_ALIGN(len);
223
224 return 0;
225}
226
227
228
229
230
231
232
233
234
235
236
237
238s32 fm10k_tlv_attr_get_value(u32 *attr, void *value, u32 len)
239{
240
241 if (!attr || !value)
242 return FM10K_ERR_PARAM;
243
244 if ((*attr >> FM10K_TLV_LEN_SHIFT) != len)
245 return FM10K_ERR_PARAM;
246
247 if (len == 8)
248 *(u64 *)value = ((u64)attr[2] << 32) | attr[1];
249 else if (len == 4)
250 *(u32 *)value = attr[1];
251 else if (len == 2)
252 *(u16 *)value = (u16)attr[1];
253 else
254 *(u8 *)value = (u8)attr[1];
255
256 return 0;
257}
258
259
260
261
262
263
264
265
266
267
268
269
270s32 fm10k_tlv_attr_put_le_struct(u32 *msg, u16 attr_id,
271 const void *le_struct, u32 len)
272{
273 const __le32 *le32_ptr = (const __le32 *)le_struct;
274 u32 *attr;
275 u32 i;
276
277
278 if (!msg || !len || (len % 4))
279 return FM10K_ERR_PARAM;
280
281 attr = &msg[FM10K_TLV_DWORD_LEN(*msg)];
282
283
284 for (i = 0; i < (len / 4); i++)
285 attr[i + 1] = le32_to_cpu(le32_ptr[i]);
286
287
288 len <<= FM10K_TLV_LEN_SHIFT;
289 attr[0] = len | attr_id;
290
291
292 len += FM10K_TLV_HDR_LEN << FM10K_TLV_LEN_SHIFT;
293 *msg += FM10K_TLV_LEN_ALIGN(len);
294
295 return 0;
296}
297
298
299
300
301
302
303
304
305
306
307
308
309s32 fm10k_tlv_attr_get_le_struct(u32 *attr, void *le_struct, u32 len)
310{
311 __le32 *le32_ptr = (__le32 *)le_struct;
312 u32 i;
313
314
315 if (!le_struct || !attr)
316 return FM10K_ERR_PARAM;
317
318 if ((*attr >> FM10K_TLV_LEN_SHIFT) != len)
319 return FM10K_ERR_PARAM;
320
321 attr++;
322
323 for (i = 0; len; i++, len -= 4)
324 le32_ptr[i] = cpu_to_le32(attr[i]);
325
326 return 0;
327}
328
329
330
331
332
333
334
335
336
337
338
339
340static u32 *fm10k_tlv_attr_nest_start(u32 *msg, u16 attr_id)
341{
342 u32 *attr;
343
344
345 if (!msg)
346 return NULL;
347
348 attr = &msg[FM10K_TLV_DWORD_LEN(*msg)];
349
350 attr[0] = attr_id;
351
352
353 return attr;
354}
355
356
357
358
359
360
361
362
363
364
365static s32 fm10k_tlv_attr_nest_stop(u32 *msg)
366{
367 u32 *attr;
368 u32 len;
369
370
371 if (!msg)
372 return FM10K_ERR_PARAM;
373
374
375 attr = &msg[FM10K_TLV_DWORD_LEN(*msg)];
376 len = (attr[0] >> FM10K_TLV_LEN_SHIFT) << FM10K_TLV_LEN_SHIFT;
377
378
379 if (len) {
380 len += FM10K_TLV_HDR_LEN << FM10K_TLV_LEN_SHIFT;
381 *msg += len;
382 }
383
384 return 0;
385}
386
387
388
389
390
391
392
393
394
395
396
397
398static s32 fm10k_tlv_attr_validate(u32 *attr,
399 const struct fm10k_tlv_attr *tlv_attr)
400{
401 u32 attr_id = *attr & FM10K_TLV_ID_MASK;
402 u16 len = *attr >> FM10K_TLV_LEN_SHIFT;
403
404
405 if (*attr & (FM10K_TLV_FLAGS_MSG << FM10K_TLV_FLAGS_SHIFT))
406 return FM10K_ERR_PARAM;
407
408
409 while (tlv_attr->id < attr_id)
410 tlv_attr++;
411
412
413 if (tlv_attr->id != attr_id)
414 return FM10K_NOT_IMPLEMENTED;
415
416
417 attr++;
418
419 switch (tlv_attr->type) {
420 case FM10K_TLV_NULL_STRING:
421 if (!len ||
422 (attr[(len - 1) / 4] & (0xFF << (8 * ((len - 1) % 4)))))
423 return FM10K_ERR_PARAM;
424 if (len > tlv_attr->len)
425 return FM10K_ERR_PARAM;
426 break;
427 case FM10K_TLV_MAC_ADDR:
428 if (len != ETH_ALEN)
429 return FM10K_ERR_PARAM;
430 break;
431 case FM10K_TLV_BOOL:
432 if (len)
433 return FM10K_ERR_PARAM;
434 break;
435 case FM10K_TLV_UNSIGNED:
436 case FM10K_TLV_SIGNED:
437 if (len != tlv_attr->len)
438 return FM10K_ERR_PARAM;
439 break;
440 case FM10K_TLV_LE_STRUCT:
441
442 if ((len % 4) || len != tlv_attr->len)
443 return FM10K_ERR_PARAM;
444 break;
445 case FM10K_TLV_NESTED:
446
447 if (len % 4)
448 return FM10K_ERR_PARAM;
449 break;
450 default:
451
452 return FM10K_ERR_PARAM;
453 }
454
455 return 0;
456}
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471static s32 fm10k_tlv_attr_parse(u32 *attr, u32 **results,
472 const struct fm10k_tlv_attr *tlv_attr)
473{
474 u32 i, attr_id, offset = 0;
475 s32 err = 0;
476 u16 len;
477
478
479 if (!attr || !results)
480 return FM10K_ERR_PARAM;
481
482
483 for (i = 0; i < FM10K_TLV_RESULTS_MAX; i++)
484 results[i] = NULL;
485
486
487 len = *attr >> FM10K_TLV_LEN_SHIFT;
488
489
490 if (!len)
491 return 0;
492
493
494 if (!tlv_attr) {
495 results[0] = attr;
496 return 0;
497 }
498
499
500 attr++;
501
502
503 while (offset < len) {
504 attr_id = *attr & FM10K_TLV_ID_MASK;
505
506 if (attr_id >= FM10K_TLV_RESULTS_MAX)
507 return FM10K_NOT_IMPLEMENTED;
508
509 err = fm10k_tlv_attr_validate(attr, tlv_attr);
510 if (err == FM10K_NOT_IMPLEMENTED)
511 ;
512 else if (err)
513 return err;
514 else
515 results[attr_id] = attr;
516
517
518 offset += FM10K_TLV_DWORD_LEN(*attr) * 4;
519
520
521 attr = &attr[FM10K_TLV_DWORD_LEN(*attr)];
522 }
523
524
525 if (offset != len)
526 return FM10K_ERR_PARAM;
527
528 return 0;
529}
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544s32 fm10k_tlv_msg_parse(struct fm10k_hw *hw, u32 *msg,
545 struct fm10k_mbx_info *mbx,
546 const struct fm10k_msg_data *data)
547{
548 u32 *results[FM10K_TLV_RESULTS_MAX];
549 u32 msg_id;
550 s32 err;
551
552
553 if (!msg || !data)
554 return FM10K_ERR_PARAM;
555
556
557 if (!(*msg & (FM10K_TLV_FLAGS_MSG << FM10K_TLV_FLAGS_SHIFT)))
558 return FM10K_ERR_PARAM;
559
560
561 msg_id = *msg & FM10K_TLV_ID_MASK;
562
563 while (data->id < msg_id)
564 data++;
565
566
567 if (data->id != msg_id) {
568 while (data->id != FM10K_TLV_ERROR)
569 data++;
570 }
571
572
573 err = fm10k_tlv_attr_parse(msg, results, data->attr);
574 if (err < 0)
575 return err;
576
577 return data->func(hw, results, mbx);
578}
579
580
581
582
583
584
585
586
587
588
589
590s32 fm10k_tlv_msg_error(struct fm10k_hw *hw, u32 **results,
591 struct fm10k_mbx_info *mbx)
592{
593 return FM10K_NOT_IMPLEMENTED;
594}
595
596static const unsigned char test_str[] = "fm10k";
597static const unsigned char test_mac[ETH_ALEN] = { 0x12, 0x34, 0x56,
598 0x78, 0x9a, 0xbc };
599static const u16 test_vlan = 0x0FED;
600static const u64 test_u64 = 0xfedcba9876543210ull;
601static const u32 test_u32 = 0x87654321;
602static const u16 test_u16 = 0x8765;
603static const u8 test_u8 = 0x87;
604static const s64 test_s64 = -0x123456789abcdef0ll;
605static const s32 test_s32 = -0x1235678;
606static const s16 test_s16 = -0x1234;
607static const s8 test_s8 = -0x12;
608static const __le32 test_le[2] = { cpu_to_le32(0x12345678),
609 cpu_to_le32(0x9abcdef0)};
610
611
612
613
614
615const struct fm10k_tlv_attr fm10k_tlv_msg_test_attr[] = {
616 FM10K_TLV_ATTR_NULL_STRING(FM10K_TEST_MSG_STRING, 80),
617 FM10K_TLV_ATTR_MAC_ADDR(FM10K_TEST_MSG_MAC_ADDR),
618 FM10K_TLV_ATTR_U8(FM10K_TEST_MSG_U8),
619 FM10K_TLV_ATTR_U16(FM10K_TEST_MSG_U16),
620 FM10K_TLV_ATTR_U32(FM10K_TEST_MSG_U32),
621 FM10K_TLV_ATTR_U64(FM10K_TEST_MSG_U64),
622 FM10K_TLV_ATTR_S8(FM10K_TEST_MSG_S8),
623 FM10K_TLV_ATTR_S16(FM10K_TEST_MSG_S16),
624 FM10K_TLV_ATTR_S32(FM10K_TEST_MSG_S32),
625 FM10K_TLV_ATTR_S64(FM10K_TEST_MSG_S64),
626 FM10K_TLV_ATTR_LE_STRUCT(FM10K_TEST_MSG_LE_STRUCT, 8),
627 FM10K_TLV_ATTR_NESTED(FM10K_TEST_MSG_NESTED),
628 FM10K_TLV_ATTR_S32(FM10K_TEST_MSG_RESULT),
629 FM10K_TLV_ATTR_LAST
630};
631
632
633
634
635
636
637
638
639static void fm10k_tlv_msg_test_generate_data(u32 *msg, u32 attr_flags)
640{
641 if (attr_flags & BIT(FM10K_TEST_MSG_STRING))
642 fm10k_tlv_attr_put_null_string(msg, FM10K_TEST_MSG_STRING,
643 test_str);
644 if (attr_flags & BIT(FM10K_TEST_MSG_MAC_ADDR))
645 fm10k_tlv_attr_put_mac_vlan(msg, FM10K_TEST_MSG_MAC_ADDR,
646 test_mac, test_vlan);
647 if (attr_flags & BIT(FM10K_TEST_MSG_U8))
648 fm10k_tlv_attr_put_u8(msg, FM10K_TEST_MSG_U8, test_u8);
649 if (attr_flags & BIT(FM10K_TEST_MSG_U16))
650 fm10k_tlv_attr_put_u16(msg, FM10K_TEST_MSG_U16, test_u16);
651 if (attr_flags & BIT(FM10K_TEST_MSG_U32))
652 fm10k_tlv_attr_put_u32(msg, FM10K_TEST_MSG_U32, test_u32);
653 if (attr_flags & BIT(FM10K_TEST_MSG_U64))
654 fm10k_tlv_attr_put_u64(msg, FM10K_TEST_MSG_U64, test_u64);
655 if (attr_flags & BIT(FM10K_TEST_MSG_S8))
656 fm10k_tlv_attr_put_s8(msg, FM10K_TEST_MSG_S8, test_s8);
657 if (attr_flags & BIT(FM10K_TEST_MSG_S16))
658 fm10k_tlv_attr_put_s16(msg, FM10K_TEST_MSG_S16, test_s16);
659 if (attr_flags & BIT(FM10K_TEST_MSG_S32))
660 fm10k_tlv_attr_put_s32(msg, FM10K_TEST_MSG_S32, test_s32);
661 if (attr_flags & BIT(FM10K_TEST_MSG_S64))
662 fm10k_tlv_attr_put_s64(msg, FM10K_TEST_MSG_S64, test_s64);
663 if (attr_flags & BIT(FM10K_TEST_MSG_LE_STRUCT))
664 fm10k_tlv_attr_put_le_struct(msg, FM10K_TEST_MSG_LE_STRUCT,
665 test_le, 8);
666}
667
668
669
670
671
672
673
674
675
676void fm10k_tlv_msg_test_create(u32 *msg, u32 attr_flags)
677{
678 u32 *nest = NULL;
679
680 fm10k_tlv_msg_init(msg, FM10K_TLV_MSG_ID_TEST);
681
682 fm10k_tlv_msg_test_generate_data(msg, attr_flags);
683
684
685 attr_flags >>= FM10K_TEST_MSG_NESTED;
686
687 if (attr_flags) {
688 nest = fm10k_tlv_attr_nest_start(msg, FM10K_TEST_MSG_NESTED);
689
690 fm10k_tlv_msg_test_generate_data(nest, attr_flags);
691
692 fm10k_tlv_attr_nest_stop(msg);
693 }
694}
695
696
697
698
699
700
701
702
703
704
705
706s32 fm10k_tlv_msg_test(struct fm10k_hw *hw, u32 **results,
707 struct fm10k_mbx_info *mbx)
708{
709 u32 *nest_results[FM10K_TLV_RESULTS_MAX];
710 unsigned char result_str[80];
711 unsigned char result_mac[ETH_ALEN];
712 s32 err = 0;
713 __le32 result_le[2];
714 u16 result_vlan;
715 u64 result_u64;
716 u32 result_u32;
717 u16 result_u16;
718 u8 result_u8;
719 s64 result_s64;
720 s32 result_s32;
721 s16 result_s16;
722 s8 result_s8;
723 u32 reply[3];
724
725
726 if (!!results[FM10K_TEST_MSG_RESULT])
727 return fm10k_tlv_attr_get_s32(results[FM10K_TEST_MSG_RESULT],
728 &mbx->test_result);
729
730parse_nested:
731 if (!!results[FM10K_TEST_MSG_STRING]) {
732 err = fm10k_tlv_attr_get_null_string(
733 results[FM10K_TEST_MSG_STRING],
734 result_str);
735 if (!err && memcmp(test_str, result_str, sizeof(test_str)))
736 err = FM10K_ERR_INVALID_VALUE;
737 if (err)
738 goto report_result;
739 }
740 if (!!results[FM10K_TEST_MSG_MAC_ADDR]) {
741 err = fm10k_tlv_attr_get_mac_vlan(
742 results[FM10K_TEST_MSG_MAC_ADDR],
743 result_mac, &result_vlan);
744 if (!err && !ether_addr_equal(test_mac, result_mac))
745 err = FM10K_ERR_INVALID_VALUE;
746 if (!err && test_vlan != result_vlan)
747 err = FM10K_ERR_INVALID_VALUE;
748 if (err)
749 goto report_result;
750 }
751 if (!!results[FM10K_TEST_MSG_U8]) {
752 err = fm10k_tlv_attr_get_u8(results[FM10K_TEST_MSG_U8],
753 &result_u8);
754 if (!err && test_u8 != result_u8)
755 err = FM10K_ERR_INVALID_VALUE;
756 if (err)
757 goto report_result;
758 }
759 if (!!results[FM10K_TEST_MSG_U16]) {
760 err = fm10k_tlv_attr_get_u16(results[FM10K_TEST_MSG_U16],
761 &result_u16);
762 if (!err && test_u16 != result_u16)
763 err = FM10K_ERR_INVALID_VALUE;
764 if (err)
765 goto report_result;
766 }
767 if (!!results[FM10K_TEST_MSG_U32]) {
768 err = fm10k_tlv_attr_get_u32(results[FM10K_TEST_MSG_U32],
769 &result_u32);
770 if (!err && test_u32 != result_u32)
771 err = FM10K_ERR_INVALID_VALUE;
772 if (err)
773 goto report_result;
774 }
775 if (!!results[FM10K_TEST_MSG_U64]) {
776 err = fm10k_tlv_attr_get_u64(results[FM10K_TEST_MSG_U64],
777 &result_u64);
778 if (!err && test_u64 != result_u64)
779 err = FM10K_ERR_INVALID_VALUE;
780 if (err)
781 goto report_result;
782 }
783 if (!!results[FM10K_TEST_MSG_S8]) {
784 err = fm10k_tlv_attr_get_s8(results[FM10K_TEST_MSG_S8],
785 &result_s8);
786 if (!err && test_s8 != result_s8)
787 err = FM10K_ERR_INVALID_VALUE;
788 if (err)
789 goto report_result;
790 }
791 if (!!results[FM10K_TEST_MSG_S16]) {
792 err = fm10k_tlv_attr_get_s16(results[FM10K_TEST_MSG_S16],
793 &result_s16);
794 if (!err && test_s16 != result_s16)
795 err = FM10K_ERR_INVALID_VALUE;
796 if (err)
797 goto report_result;
798 }
799 if (!!results[FM10K_TEST_MSG_S32]) {
800 err = fm10k_tlv_attr_get_s32(results[FM10K_TEST_MSG_S32],
801 &result_s32);
802 if (!err && test_s32 != result_s32)
803 err = FM10K_ERR_INVALID_VALUE;
804 if (err)
805 goto report_result;
806 }
807 if (!!results[FM10K_TEST_MSG_S64]) {
808 err = fm10k_tlv_attr_get_s64(results[FM10K_TEST_MSG_S64],
809 &result_s64);
810 if (!err && test_s64 != result_s64)
811 err = FM10K_ERR_INVALID_VALUE;
812 if (err)
813 goto report_result;
814 }
815 if (!!results[FM10K_TEST_MSG_LE_STRUCT]) {
816 err = fm10k_tlv_attr_get_le_struct(
817 results[FM10K_TEST_MSG_LE_STRUCT],
818 result_le,
819 sizeof(result_le));
820 if (!err && memcmp(test_le, result_le, sizeof(test_le)))
821 err = FM10K_ERR_INVALID_VALUE;
822 if (err)
823 goto report_result;
824 }
825
826 if (!!results[FM10K_TEST_MSG_NESTED]) {
827
828 memset(nest_results, 0, sizeof(nest_results));
829
830
831 err = fm10k_tlv_attr_parse(results[FM10K_TEST_MSG_NESTED],
832 nest_results,
833 fm10k_tlv_msg_test_attr);
834 if (err)
835 goto report_result;
836
837
838 results = nest_results;
839 goto parse_nested;
840 }
841
842report_result:
843
844 fm10k_tlv_msg_init(reply, FM10K_TLV_MSG_ID_TEST);
845 fm10k_tlv_attr_put_s32(reply, FM10K_TEST_MSG_RESULT, err);
846
847
848 return mbx->ops.enqueue_tx(hw, mbx, reply);
849}
850