summaryrefslogtreecommitdiff
path: root/mac/sar/src/sar_mf.c
blob: 6d1047ecb23ca9a9f3ec12dec0caa8b87b5dd188 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
/* Cesar project {{{
 *
 * Copyright (C) 2007 Spidcom
 *
 * <<<Licence>>>
 *
 * }}} */
/**
 * \file    sar_mf.c
 * \brief   the function to process a MF.
 * \ingroup mac/sar/src
 *
 */

#include "common/std.h"
#include "lib/read_word.h"
#include "mac/sar/inc/sar_mf.h"

/**
 * Get the type of the MF from the PB at the data adress plus the offset
 * 
 * \param  pb The first pb containing the MF.
 * \param  offset the offset to start reading
 * \return  MF type.
 */
uint mf_get_type (pb_t * pb, uint offset)
{
    dbg_assert (pb);
    dbg_assert (offset < BLK_SIZE);

    return read_u8_from_word (pb->data + offset) & 3;
}

/**
 * Test the existance of a MF at the address given by mf and return its length.
 * Will be used to determine if a complete MF is avaliable in the MFS.
 * Test if the MF start in the end of the PB by verifying the offset, if the MFH 
 * is in two PBs.
 * 
 * \param  pb The first pb containing the MF.
 * \param  offset the offset to start reading
 * \param  payload_only request to return only the payload length if true.
 *
 * \return  length of the payload with the header and icv
 */
uint mf_get_length (pb_t *pb, uint offset, bool payload_only)
{
    dbg_assert (pb);
    uint length;
    uint type;

    /* If the offset is in the last 6 bytes of the PB data. */
    if (offset == BLK_SIZE - 1 && pb->next)
    {
        length = read_u8_from_word (pb->data + offset) 
                  | (read_u8_from_word (pb->next->data) << 8);
        type = length & 0x3;
        length = length >> 2;
    }
    else if (offset == BLK_SIZE - 1 && !pb->next)
    {
        return 0;
    }
    else
    {
        length = read_u16_from_word (pb->data + offset);
        type = length & 0x3;
        length = length >> 2;
    }

    if (length < ETH_PACKET_MIN_SIZE || length
            > ETH_PACKET_MAX_SIZE + SAR_MF_ICV_SIZE)
        return 0;

    if (payload_only && (type > 1))
    {
        return length + 1 - SAR_MF_ATS_SIZE;
    }
    else if (payload_only)
    {
        return length + 1;
    }
    else
    {
        return length + 1 + SAR_MF_ICV_SIZE + SAR_MF_MFH_SIZE;
    }
}

/**
 * Calculate the number of PB used for the MF
 * 
 * Depends of the mf_get_length function
 * 
 * \param pb the first PB where the MF starts
 * \param offset the offset of the PB where the MF starts
 * \return the quantity of PBs 
 */
uint mf_get_quantity_of_pb (pb_t *pb, uint offset)
{
    uint number_of_pb;
    uint mf_length;

    if ((mf_length = mf_get_length (pb, offset, false)) == 0)
    {
        return 0;
    }

    number_of_pb = ((mf_length + offset) >> SAR_DIV_BY_BLK_SIZE) + 1;

    return number_of_pb;
}

/**
 * gets the ATS of a MF in the address pointed by the PB plus the offset
 * 
 * Depends of mf_get_type
 * 
 * \param pb the first PB where the MF starts
 * \param offset the offset of the PB where the MF starts 
 * \return  the ATS of the MF.
 */
u32 mf_get_ats (pb_t *pb, uint offset)
{
    u8 type;
    pb_t *pb_curr;

    type = mf_get_type (pb, offset);

    if (type < 0x2)
    {
        return 0;
    }

    offset += SAR_MF_MFH_SIZE;
    if (offset > BLK_SIZE && !pb->next)
    {
        return 0;
    }
    else if (offset > BLK_SIZE && pb->next)
    {
        pb_curr = pb->next;
        offset = offset % BLK_SIZE;
    }
    else
    {
        pb_curr = pb;
    }

    /* Normal case */
    if (offset < BLK_SIZE - SAR_MF_ATS_SIZE)
    {
        return read_u32_from_word (pb_curr->data + offset);
    }
    else if (pb->next)
    {
        uint ats1;
        uint ats2;

        switch (offset) {
            case BLK_SIZE - SAR_MF_ATS_SIZE + 1:
                ats2 = read_u8_from_word (pb_curr->next->data);
                ats1 = read_u24_from_word (pb_curr->data + offset);
                return (ats2 << 24) | ats1;
                break;
            case BLK_SIZE - SAR_MF_ATS_SIZE + 2:
                ats2 = read_u16_from_word (pb_curr->next->data);
                ats1 = read_u16_from_word (pb_curr->data + offset);
                return (ats2 << 16) | ats1;
            case BLK_SIZE - SAR_MF_ATS_SIZE + 3:
                ats2 = read_u24_from_word (pb_curr->next->data);
                ats1 = read_u8_from_word (pb_curr->data + offset);
                return (ats2 << 8) | ats1;
                break;
            default:
                return read_u32_from_word (pb_curr->data + offset);
        }
    }

    return 0;
}