/**
 * Copyright (c) 2017 - 2017, Nordic Semiconductor ASA
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form, except as embedded into a Nordic
 *    Semiconductor ASA integrated circuit in a product or a software update for
 *    such product, must reproduce the above copyright notice, this list of
 *    conditions and the following disclaimer in the documentation and/or other
 *    materials provided with the distribution.
 *
 * 3. Neither the name of Nordic Semiconductor ASA nor the names of its
 *    contributors may be used to endorse or promote products derived from this
 *    software without specific prior written permission.
 *
 * 4. This software, with or without modification, must only be used with a
 *    Nordic Semiconductor ASA integrated circuit.
 *
 * 5. Any software provided in binary form under this license must not be reverse
 *    engineered, decompiled, modified and/or disassembled.
 *
 * THIS SOFTWARE IS PROVIDED BY NORDIC SEMICONDUCTOR ASA "AS IS" AND ANY EXPRESS
 * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL NORDIC SEMICONDUCTOR ASA OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include "dfu_ble_svc.h"
#include <string.h>
#include "nrf_error.h"
#include "crc16.h"

// #if defined ( __CC_ARM )
// static dfu_ble_peer_data_t m_peer_data __attribute__((section("NoInit"), zero_init));            /**< This variable should be placed in a non initialized RAM section in order to be valid upon soft reset from application into bootloader. */
// static uint16_t            m_peer_data_crc __attribute__((section("NoInit"), zero_init));        /**< CRC variable to ensure the integrity of the peer data provided. */
// #elif defined ( __GNUC__ )
// __attribute__((section(".noinit"))) static dfu_ble_peer_data_t m_peer_data;                      /**< This variable should be placed in a non initialized RAM section in order to be valid upon soft reset from application into bootloader. */
// __attribute__((section(".noinit"))) static uint16_t            m_peer_data_crc;                  /**< CRC variable to ensure the integrity of the peer data provided. */
// #elif defined ( __ICCARM__ )
// __no_init static dfu_ble_peer_data_t m_peer_data     @ 0x20003F80;                               /**< This variable should be placed in a non initialized RAM section in order to be valid upon soft reset from application into bootloader. */
// __no_init static uint16_t            m_peer_data_crc @ 0x20003F80 + sizeof(dfu_ble_peer_data_t); /**< CRC variable to ensure the integrity of the peer data provided. */
// #endif


// /**@brief Function for setting the peer data from application in bootloader before reset.
//  *
//  * @param[in] p_peer_data  Pointer to the peer data containing keys for the connection.
//  *
//  * @retval NRF_SUCCES      The data was set succesfully.
//  * @retval NRF_ERROR_NULL  If a null pointer was passed as argument.
//  */
// static uint32_t dfu_ble_peer_data_set(dfu_ble_peer_data_t * p_peer_data)
// {
//     if (p_peer_data == NULL)
//     {
//         return NRF_ERROR_NULL;
//     }

//     uint32_t src = (uint32_t)p_peer_data;
//     uint32_t dst = (uint32_t)&m_peer_data;
//     // Calculating length in order to check if destination is residing inside source.
//     // Source inside the the destination (calculation underflow) is safe a source is read before 
//     // written to destination so that when destination grows into source, the source data is no 
//     // longer needed.
//     uint32_t len = dst - src;

//     if (src == dst)
//     {
//         // Do nothing as source and destination are identical, just calculate crc below.
//     }
//     else if (len < sizeof(dfu_ble_peer_data_t))
//     {
//         uint32_t i = 0;

//         dst += sizeof(dfu_ble_peer_data_t);
//         src += sizeof(dfu_ble_peer_data_t);

//         // Copy byte wise backwards when facing overlapping structures.
//         while (i++ <= sizeof(dfu_ble_peer_data_t))
//         {
//             *((uint8_t *)dst--) = *((uint8_t *)src--);
//         }
//     }
//     else
//     {
//         memcpy((void *)dst, (void *)src, sizeof(dfu_ble_peer_data_t));
//     }

//     m_peer_data_crc = crc16_compute((uint8_t *)&m_peer_data, sizeof(m_peer_data), NULL);

//     return NRF_SUCCESS;
// }


/**@brief   Function for handling second stage of SuperVisor Calls (SVC).
 *
 * @details The function will use svc_num to call the corresponding SVC function.
 *
 * @param[in] svc_num    SVC number for function to be executed
 * @param[in] p_svc_args Argument list for the SVC.
 *
 * @return This function returns the error value of the SVC return. For further details, please
 *         refer to the details of the SVC implementation itself.
 *         @ref NRF_ERROR_SVC_HANDLER_MISSING is returned if no SVC handler is implemented for the
 *         provided svc_num.
 */
void C_SVC_Handler(uint8_t svc_num, uint32_t * p_svc_args)
{
    // switch (svc_num)
    // {
    //     case DFU_BLE_SVC_PEER_DATA_SET:
    //         p_svc_args[0] = dfu_ble_peer_data_set((dfu_ble_peer_data_t *)p_svc_args[0]);
    //         break;

    //     default:
            p_svc_args[0] = NRF_ERROR_SVC_HANDLER_MISSING;
    //         break;
    // }
}


/**@brief   Function for handling the first stage of SuperVisor Calls (SVC) in assembly.
 *
 * @details The function will use the link register (LR) to determine the stack (PSP or MSP) to be
 *          used and then decode the SVC number afterwards. After decoding the SVC number then
 *          @ref C_SVC_Handler is called for further processing of the SVC.
 */
#if defined ( __CC_ARM )
__asm void SVC_Handler(void)
{
EXC_RETURN_CMD_PSP  EQU 0xFFFFFFFD  ; EXC_RETURN using PSP for ARM Cortex. If Link register contains this value it indicates the PSP was used before the SVC, otherwise the MSP was used.

    IMPORT C_SVC_Handler
    LDR   R0, =EXC_RETURN_CMD_PSP   ; Load the EXC_RETURN into R0 to be able to compare against LR to determine stack pointer used. 
    CMP   R0, LR                    ; Compare the link register with R0. If equal then PSP was used, otherwise MSP was used before SVC.
    BNE   UseMSP                    ; Branch to code fetching SVC arguments using MSP.
    MRS   R1, PSP                   ; Move PSP into R1.
    B     Call_C_SVC_Handler        ; Branch to Call_C_SVC_Handler below.
UseMSP
    MRS   R1, MSP                   ; MSP was used, therefore Move MSP into R1.
Call_C_SVC_Handler
    LDR   R0, [R1, #24]             ; The arguments for the SVC was stacked. R1 contains Stack Pointer, the values stacked before SVC are R0, R1, R2, R3, R12, LR, PC (Return address), xPSR. 
                                    ; R1 contains current SP so the PC of the stacked frame is at SP + 6 words (24 bytes). We load the PC into R0.
    SUBS  R0, #2                    ; The PC before the SVC is in R0. We subtract 2 to get the address prior to the instruction executed where the SVC number is located.
    LDRB  R0, [R0]                  ; SVC instruction low octet: Load the byte at the address before the PC to fetch the SVC number.
    LDR   R2, =C_SVC_Handler        ; Load address of C implementation of SVC handler.
    BX    R2                        ; Branch to C implementation of SVC handler. R0 is now the SVC number, R1 is the StackPointer where the arguments (R0-R3) of the original SVC are located.
    ALIGN
}
#elif defined ( __GNUC__ )
void __attribute__ (( naked )) SVC_Handler(void)
{
    const uint32_t exc_return = 0xFFFFFFFD;      // EXC_RETURN using PSP for ARM Cortex. If Link register contains this value it indicates the PSP was used before the SVC, otherwise the MSP was used.
    
    __asm volatile(
        "cmp   lr, %0\t\n"                       // Compare the link register with argument 0 (%0), which is exc_return. If equal then PSP was used, otherwise MSP was used before SVC.
        "bne   UseMSP\t\n"                       // Branch to code fetching SVC arguments using MSP.
        "mrs   r1, psp\t\n"                      // Move PSP into R1.
        "b     Call_C_SVC_Handler\t\n"           // Branch to Call_C_SVC_Handler below.
        "UseMSP:  \t\n"                          //
        "mrs   r1, msp\t\n"                      // MSP was used, therefore Move MSP into R1.
        "Call_C_SVC_Handler:  \t\n"              //
        "ldr   r0, [r1, #24]\t\n"                // The arguments for the SVC was stacked. R1 contains Stack Pointer, the values stacked before SVC are R0, R1, R2, R3, R12, LR, PC (Return address), xPSR. 
                                                 // R1 contains current SP so the PC of the stacked frame is at SP + 6 words (24 bytes). We load the PC into R0.
        "sub   r0, r0, #2\t\n"                   // The PC before the SVC is in R0. We subtract 2 to get the address prior to the instruction executed where the SVC number is located.
        "ldrb  r0, [r0]\t\n"                     // SVC instruction low octet: Load the byte at the address before the PC to fetch the SVC number.
        "bx    %1\t\n"                           // Branch to C implementation of SVC handler, argument 1 (%1). R0 is now the SVC number, R1 is the StackPointer where the arguments (R0-R3) of the original SVC are located.
        ".align\t\n"
        :: "r" (exc_return), "r" (C_SVC_Handler) // Argument list for the gcc assembly. exc_return is %0, C_SVC_Handler is %1.
        : "r0", "r1"                             // List of register maintained manually.
    );
}
#elif defined ( __ICCARM__ )
void SVC_Handler(void)
{
    asm("movs  r0, #0x02\n"                    // Load 0x02 into R6 to prepare for exec return test.
        "mvns  r0, r0\n"                       // Invert R0 to obtain exec return code using PSP for ARM Cortex.
        "cmp   lr, r0\n"                       // Compare the link register with argument 0 (%0), which is exc_return. If equal then PSP was used, otherwise MSP was used before SVC.
        "bne.n UseMSP\n"                       // Branch to code fetching SVC arguments using MSP.
        "mrs   r1, psp\n"                      // Move PSP into R1.
        "b.n   Call_C_SVC_Handler\t\n"         // Branch to Call_C_SVC_Handler below.
        "UseMSP:  \n"                          //
        "mrs   r1, msp\n"                      // MSP was used, therefore Move MSP into R1.
        "Call_C_SVC_Handler:  \n"              //
        "ldr   r0, [r1, #24]\n"                // The arguments for the SVC was stacked. R1 contains Stack Pointer, the values stacked before SVC are R0, R1, R2, R3, R12, LR, PC (Return address), xPSR. 
                                               // R1 contains current SP so the PC of the stacked frame is at SP + 6 words (24 bytes). We load the PC into R0.
        "subs  r0, #0x02\n"                    // The PC before the SVC is in R0. We subtract 2 to get the address prior to the instruction executed where the SVC number is located.
        "ldrb  r0, [r0]\n"                     // SVC instruction low octet: Load the byte at the address before the PC to fetch the SVC number.
        "bx    %0\n"                           // Branch to C implementation of SVC handler, argument 1 (%1). R0 is now the SVC number, R1 is the StackPointer where the arguments (R0-R3) of the original SVC are located.
        :: "r" (C_SVC_Handler)                 // Argument list for the gcc assembly. C_SVC_Handler is %0.
        : "r0", "r1"                           // List of register maintained manually.
    );
}
#else
#error Compiler not supported.
#endif


// uint32_t dfu_ble_peer_data_get(dfu_ble_peer_data_t * p_peer_data)
// {
//     uint16_t crc;

//     if (p_peer_data == NULL)
//     {
//         return NRF_ERROR_NULL;
//     }

//     crc = crc16_compute((uint8_t *)&m_peer_data, sizeof(m_peer_data), NULL);
//     if (crc != m_peer_data_crc)
//     {
//         return NRF_ERROR_INVALID_DATA;
//     }

//     *p_peer_data = m_peer_data;

//     // corrupt CRC to invalidate shared information.
//     m_peer_data_crc++;

//     return NRF_SUCCESS;
// }